1.一种改进RRT的机器人路径规划方法,其特征在于,包括:S1、将机器人活动的二维场景分为可通行区域和不可通行的障碍物,建立二维栅格图;
获取机器人在二维场景中的起始位置Qstart和目的位置Qgoal;
如果从Qstart到Qgoal的直线路径没有碰触障碍物,则规划路径为从Qstart到Qgoal的直线段;否则,将Qstart作为RRT算法中随机扩展树的根节点,初始化目标方向权重k=1,根据步骤S2‑S7确定规划路径;
S2、在机器人运动区域生成随机位置点Qrand;如果Qstart与Qrand的连线穿过障碍物,则丢弃该Qrand,并再次生成新的Qrand,直至Qstart与Qrand的连线没有穿过障碍物;
S3、遍历当前随机扩展树,查找与Qrand距离最近的点Qnear;
S4、计算Qnear到障碍物的最小距离Dob,计算斥力因子h: ρ为机器人运动步长;
计算Qrand对Qnear的引力 Qgoal对Qnear的引力 障碍物对Qnear的斥力 计算Qnear处受到的合力
其中
w为引力权重,是预设的(0,1]范围内的常数;Qob为距离Qnear最近的障碍物位置;|| ||为计算二维环境栅格图中两个位置点的距离;
以Qnear为父节点,根据合力 生成新节点Qnew:判断新节点Qnew和Qnear的连线是否与障碍物有碰撞,如没有碰撞则将Qnew添加到随机扩展树上,如果有碰撞跳转至S2重新生成随机位置点;
S5、优化起点Qstart到Qnew的路径;
S6、重复步骤S3‑S5,当新节点Qnew进入目的位置Qgoal的邻域内或为Qgoal本身时,停止随机扩展树的扩展;对随机扩展树进行剪枝处理;
S7、从目的位置Qgoal开始依次回溯父节点直到起始点Qstart结束,找到一条从Qstart到Qgoal的无障碍路径。
2.根据权利要求1所述的机器人路径规划方法,其特征在于,所述步骤S4中还包括:根据F1和F2更新k:如果F1>F2,k=k+Δk;否则k=k‑Δk;Δk为预设的更新步长。
3.根据权利要求1所述的机器人路径规划方法,其特征在于,所述步骤S5具体包括:S5.1、以Qnew为圆心,Rnear为半径计算邻域Vnear;Rnear为预设的布线长度;
S5.2、随机扩展树中位于邻域Vnear内的节点构成集合Snear,获取集合Snear中每个节点的第一外节点,组成集合Spnear;节点s的第一外节点为随机扩展树中与节点s间的距离小于第一距离R1的节点;s∈Snear;,Ssum=Snear∪Spnear;
S5.3、遍历集合Ssum中的节点,判断Qnew的父节点替换成Ssum中的节点后,是否会缩短Qstart到Qnew的路径,如果缩短则进行Qnew的父节点替换操作,反之不替换;
S5.4、寻找Qnew的第二外节点的集合Spnew,节点Qnew的第二外节点为随机扩展树中与Qnew间的距离小于第二距离R2的节点;
S5.5、遍历Snear中的节点,判断如果其父节点更换为Qnew与Spnew的并集内的节点后是否会减小Qstart到所述Snear中的节点的路径,如果路径有所减小,则进行所述Snear中的节点父节点的替换操作,反之不操作。
4.根据权利要求1所述的机器人路径规划方法,其特征在于,所述步骤S6中对随机扩展树进行剪枝处理具体为:
从随机扩展树的根节点Qstart开始,寻找能够无碰撞连接目的位置Qgoal的中间点P1,接着以P1为起点寻找能够无碰撞连接目的位置Qgoal的中间点P2,如此循环往复,直至找到Qgoal或进入目的位置Qgoal的邻域内;Qstart和所有中间点按序构成优化后的路径。
5.一种改进RRT的机器人路径规划系统,其特征在于,包括:二维场景建立模块,用于将机器人活动的二维场景分为可通行区域和不可通行的障碍物,建立二维栅格图;获取机器人在二维场景中的起始位置Qstart和目的位置Qgoal;
随机位置点Qrand生成模块,用于随机位置点Qrand;
最近点Qnear生成模块,用于遍历当前随机扩展树,查找与Qrand距离最近的点Qnear;
新节点Qnew生成模块,用于生成新节点Qnew,生成步骤为:计算Qnear到障碍物的最小距离Dob,计算斥力因子h: ρ为机器人运动步长;
计算Qrand对Qnear的引力 Qgoal对Qnear的引力 障碍物对Qnear的斥力 计算Qnear处受到的合力
其中
w为引力权重,是预设的(0,1]范围内的常数;Qob为距离Qnear最近的障碍物位置;|| ||为计算二维环境栅格图中两个位置点的距离;
以Qnear为父节点,根据合力 生成新节点Qnew:判断新节点Qnew和Qnear的连线是否与障碍物有碰撞,如没有碰撞则将Qnew添加到随机扩展树上,如果有碰撞则随机位置点Qrand生成模块重新生成随机位置点;
起点Qstart到Qnew的路径优化模块,用于优化起点Qstart到Qnew的路径;
停止扩展判断模块,用于判断是否停止随机扩展树的扩展;判断方法为:当新节点Qnew进入目的位置Qgoal的邻域内或为Qgoal本身时,停止随机扩展树的扩展;
剪枝处理模块,用于对随机扩展树进行剪枝处理;
路径生成模块,用于从目的位置Qgoal开始依次回溯父节点直到起始点Qstart结束,找到一条从Qstart到Qgoal的无障碍路径。
6.根据权利要求5所述的机器人路径规划系统,其特征在于,所述新节点Qnew生成模块还用于:根据F1和F2更新k:如果F1>F2,k=k+Δk;否则k=k‑Δk;Δk为预设的更新步长。
7.根据权利要求5所述的机器人路径规划系统,其特征在于,起点Qstart到Qnew的路径优化模块进行优化具体包括:
S5.1、以Qnew为圆心,Rnear为半径计算邻域Vnear;Rnear为预设的布线长度;
S5.2、随机扩展树中位于邻域Vnear内的节点构成集合Snear,获取集合Snear中每个节点的第一外节点,组成集合Spnear;节点s的第一外节点为随机扩展树中与节点s间的距离小于第一距离R1的节点;s∈Snear;,Ssum=Snear∪Spnear;
S5.3、遍历集合Ssum中的节点,判断Qnew的父节点替换成Ssum中的节点后,是否会缩短Qstart到Qnew的路径,如果缩短则进行Qnew的父节点替换操作,反之不替换;
S5.4、寻找Qnew的第二外节点的集合Spnew,节点Qnew的第二外节点为随机扩展树中与Qnew间的距离小于第二距离R2的节点;
S5.5、遍历Snear中的节点,判断如果其父节点更换为Qnew与Spnew的并集内的节点后是否会减小Qstart到所述Snear中的节点的路径,如果路径有所减小,则进行所述Snear中的节点父节点的替换操作,反之不操作。
8.根据权利要求5所述的机器人路径规划系统,其特征在于,剪枝处理模块对随机扩展树进行剪枝处理具体为:
从随机扩展树的根节点Qstart开始,寻找能够无碰撞连接目的位置Qgoal的中间点P1,接着以P1为起点寻找能够无碰撞连接目的位置Qgoal的中间点P2,如此循环往复,直至找到Qgoal或进入目的位置Qgoal的邻域内;Qstart和所有中间点按序构成优化后的路径。