[发明专利]一种考虑车头指向的两阶段多抽样的RRT路径规划方法

权利要求书

1.一种考虑车头指向的两阶段多抽样的RRT路径规划方法，其特征在于，包括以下步骤：

步骤1：初始化地图空间，输入无人车的起点P_init、终点P_goal以及环境中的障碍物分布信息；初始化随机树T、步长L、采样轮次k＝0及采样轮次上限M；设定安全距离SD；其中，以起点P_init作为随机树T的根节点；步长L为无人车每一步运动的长度；采样轮次k初始值为0，每确定一个随机采样点P_rand，采样轮次k的值都加1，当kM时，采样终止；

步骤2：确定随机采样点P_rand进行RRT树节点预扩展；

步骤2.1：设定在地图空间中任意位置随机取点作为一号临时采样点TP₁的概率为p₁₁；设定在地图空间中自起点至终点的线段上随机取点作为一号临时采样点TP₁的概率为p₁₂；执行第一阶段抽样，确定一号临时采样点TP₁；

步骤2.2：查找随机树T中离一号临时采样点TP₁最近的树节点P_near作为本次树节点扩展的原点；

步骤2.3：执行第二阶段抽样；以一号临时采样点TP₁为原点，以随机树T中离一号临时采样点TP₁最近的树节点P_near和一号临时采样点TP₁的连线为横轴，设定常数a，顺时针每隔a°确定一个候选临时采样点CTP_i，共确定360/a个候选临时采样点，构成集合CTP；

步骤2.4：从候选临时采样点集合CTP中，剔除掉碰撞障碍物的候选临时采样点，剩余的n个候选临时采样点的集合为S；

步骤2.5：从集合S中随机选择二号临时采样点TP₂；

步骤2.6：以随机树T中离一号临时采样点TP₁最近的树节点P_near为起点，向P_near和二号临时采样点TP₂的连线方向延伸步长L，此线段的终点为此两阶段所确定的随机采样点P_rand；

步骤2.7：随机树T向随机采样点P_rand进行预扩展；

步骤3：无人车从随机树T中离一号临时采样点TP₁最近的树节点P_near运行到随机采样点P_rand的过程中，判断无人车与障碍物是否无碰撞、无人车与障碍物是否满足安全距离SD、无人车转向角α是否满足α90°；若三个条件判断结果都为真，则执行步骤4；否则，执行步骤8；

无人车的转向角α具体为：以随机树T中离一号临时采样点TP₁最近的树节点P_near为原点向其前驱节点P_pioneer方向引出的射线与以树节点P_near为原点向随机采样点P_rand方向引出的射线，两条射线的夹角即为无人车转向角α；若树节点P_near为起点P_init，则以树节点P_near为原点向终点P_goal方向引出的射线与以树节点P_near为原点向随机采样点P_rand方向引出的射线，两条射线的夹角即为无人车转向角α；

步骤4：随机树T向随机采样点P_rand进行扩展，将树节点与对应的边添加到随机树T中，同时采样轮次k+1；

步骤5：判断终点P_goal是否包含在随机树T中；若终点P_goal包含在随机树T中，则执行步骤6；否则，执行步骤8；

步骤6：对步骤5输出的路径做平滑处理；

步骤7：判断平滑后的路径是否满足安全距离SD；若平滑后的路径满足安全距离SD，则跳执行步骤9；否则，执行步骤8；

步骤8：判断当前采样轮次k是否大于采样轮次上限M；若kM，则判断无符合要求的路径；否则，返回步骤2；

步骤9：输出满足要求的平滑路径，完成路径规划。