[发明专利]基于双向扩展随机树的智能车路径规划方法

权利要求书

1.基于双向扩展随机树的智能车路径规划方法，其特征在于，包括：

S1、采用Reeds-Shepp曲线连接智能车的起始点和目标点；

S2、当生成的曲线上存在障碍物时，从起始点向目标点构建扩展树tree1，从目标点向起始点构建扩展树tree2；

S3、在包含智能车起始点和目标点的地图上生成随机点q_rand1/q_rand2，并查找扩展树tree1/tree2距离随机点q_rand1/q_rand2最近的节点q_nearrest1/q_nearrest2；

S4、判断随机点q_rand1/q_rand2与节点q_nearrest1/q_nearrest2的连线上是否有障碍物；若有，则返回步骤S3，否则进入步骤S5；

S5、在随机点q_rand1/q_rand2与节点q_nearrest1/q_nearrest2的连线上取位于一个扩展步长长度处的节点q_new1/q_new2；

S6、判断节点q_nearrest1/q_nearrest2与节点q_new1/q_new2的连线上是否有障碍物；若有，则返回步骤S3，否则，进入步骤S7；

S7、选取以节点q_new1/q_new2所在区域内使得规划路径与障碍物之间的距离大于安全距离的节点更新节点q_new1/q_new2；

S8、根据智能车辆最大转向约束及节点q_new1/q_new2，选取节点q_new1/q_new2的父节点和子节点；

S9、判断扩展树tree1和扩展树tree2之间的距离是否小于设定阈值，若大于，则返回步骤S3，否则，进入步骤S10；

S10、根据扩展树tree1和扩展树tree2相遇的节点q_a和q_b，反向选取节点q_a和q_b分别到起始点和目标点的路径构成智能车规划路径；

选取以节点q_new1/q_new2所在区域内使得规划路径与障碍物之间的距离大于安全距离的节点更新节点q_new1/q_new2进一步包括：

S71、形成一块以节点q_new1/q_new2为中心，扩展步长为1/2边长的方形区域A1/A2，并同时初始化规划路径与障碍物之间的最小距离d_min为无穷大；

S72、选取方形区域A1/A2中未选取过的节点poses1/poses2，并判断节点poses1/poses2是否为障碍物；若是，执行步骤S72，否则进入步骤S73；

S73、以方形区域A1/A2中的每个节点poses1/poses2为中心，以扩展步长为边长，形成区域B1/B2；

S74、选取区域B1/B2中未选取过的节点poses11/poses21，并判断节点poses11/poses21是否为障碍物；若是，返回步骤S72，否则进入步骤S75；

S75、计算节点poses11/poses21到目标点的距离d，并判断距离d是否小于最小距离d_min；若是，进入步骤S76，否则返回步骤S72；

S76、采用距离d更新最小距离d_min，采用距离d对应的节点poses1/poses2更新节点q_new1/q_new2；

S77、判断方形区域A1/A2，是否存在未选取过得节点；若有，则执行S72；否则，输出节点q_new1/q_new2。