[发明专利]一种基于改进双向RRT机器人路径规划方法

权利要求书

1.一种基于改进双向RRT机器人路径规划方法，其特征在于：包括以下步骤，

S1：利用障碍物膨胀技术对障碍物进行膨胀，建立地图模型，同时在地图中确定机器人的起始位置和目标位置；

S2：两个快速探索随机树T_a，T_b分别从机器人的起始位置X_start和目标位置X_goal扩展到状态空间中利用中心圆采样策略进行采样，得到随机采样点X_rand-a、X_rand-b；

S3：得到随机采样点X_rand-a后，将树T_a中距离点X_rand-a最近的子节点作为最邻节点然后朝X_rand-a方向按照步长S生成新节点新节点和最邻节点做碰撞检测，检查两个节点、连接线是否在障碍物中，若不在障碍物区域，则视为通过碰撞检测；如果通过碰撞检测，并将新生成的节点添加到树T_a形成扩展树T_a-E；以T_a得到T_a-E方法对T_b进行操作得到T_b-E，如果扩展树T_b-E中的最邻节点找到T_a-E中新的节点且T_b-E中的与T_b-E中距离小于两棵扩展树连接的阈值，T_a-E与T_b-E将进行连接，规划的路径被找到；其中树T_b的扩展方式和T_a以相同的扩展方式在状态空间中进行扩展，直到路径被找到。

S4：对得到的路径，利用六次样条插值进行光滑处理，得到一条符合实际机器人运动的路径。

2.根据权利要求1所述的一种基于改进双向RRT机器人路径规划方法，其特征在于：所述步骤S1中的膨胀技术具体为障碍物依据机器人中心到机器人的边缘距离p，进行膨胀，障碍物初始状态、膨胀后的空间一起构成障碍物区域。

3.根据权利要求2所述的一种基于改进双向RRT机器人路径规划方法，其特征在于：所述步骤S2具体为，两个快速探索随机树T_a，T_b分别从起点X_start，目标点X_goal扩展到状态空间中，利用中心圆采样策略得到采样点X_rand-a、X_rand-b具体计算为：

式中：n为0到1范围内的随机数，m为中心圆系数，m＝1，R_w为地图100*100范围外接圆半径，x_start,y_start分别为起点X_start的横、纵坐标值，x_goal，y_goal为终点X_goal的横、纵坐标值，x_center，y_center为中心点的横、纵坐标值，x_rand，y_rand为中心圆采样点X_rand-a或X_rand-b的横、纵坐标值。

4.根据权利要求3所述的一种基于改进双向RRT机器人路径规划方法，其特征在于：所述步骤S3中树T_a中距离点X_rand-a最近的子节点作为最邻节点的公式为

其中，为树T_a中的子节点。