[发明专利]一种改进RRT的机器人路径规划方法和系统

权利要求书

1.一种改进RRT的机器人路径规划方法，其特征在于，包括：

S1、将机器人活动的二维场景分为可通行区域和不可通行的障碍物，建立二维栅格图；获取机器人在二维场景中的起始位置Q_start和目的位置Q_goal；

如果从Q_start到Q_goal的直线路径没有碰触障碍物，则规划路径为从Q_start到Q_goal的直线段；否则，将Q_start作为RRT算法中随机扩展树的根节点，初始化目标方向权重k＝1，根据步骤S2-S7确定规划路径；

S2、在机器人运动区域生成随机位置点Q_rand；如果Q_start与Q_rand的连线穿过障碍物，则丢弃该Q_rand，并再次生成新的Q_rand，直至Q_start与Q_rand的连线没有穿过障碍物；

S3、遍历当前随机扩展树，查找与Q_rand距离最近的点Q_near；

S4、计算Q_near到障碍物的最小距离D_ob，计算斥力因子h：ρ为机器人运动步长；

计算Q_rand对Q_near的引力Q_goal对Q_near的引力障碍物对Q_near的斥力计算Q_near处受到的合力

其中

w为引力权重，是预设的(0,1]范围内的常数；Q_ob为距离Q_near最近的障碍物位置；|| ||为计算二维环境栅格图中两个位置点的距离；

以Q_near为父节点，根据合力生成新节点Q_new：

判断新节点Q_new和Q_near的连线是否与障碍物有碰撞，如没有碰撞则将Q_new添加到随机扩展树上，如果有碰撞跳转至S2重新生成随机位置点；

S5、优化起点Q_start到Q_new的路径；

S6、重复步骤S3-S5，当新节点Q_new进入目的位置Q_goal的邻域内或为Q_goal本身时，停止随机扩展树的扩展；对随机扩展树进行剪枝处理；

S7、从目的位置Q_goal开始依次回溯父节点直到起始点Q_start结束，找到一条从Q_start到Q_goal的无障碍路径。

2.根据权利要求1所述的机器人路径规划方法，其特征在于，所述步骤S4中还包括：根据F₁和F₂更新k：如果F₁＞F₂，k＝k+Δk；否则k＝k-Δk；Δk为预设的更新步长。