一种基于动态约束采样RRT*-Connect算法的移动机器人路径规划方法、系统及处理器

本发明属于机器人路径规划领域，尤其涉及一种基于动态约束采样rrt＊-connect算法的移动机器人路径规划方法、系统及处理器。

背景技术：

1、智能机器人在移动过程中会遇到很多障碍物，如何在复杂障碍物环境下规划出一条无碰撞的安全路径是衡量智能机器人的性能指标之一。移动机器人的路径规划是指在各类障碍物环境下搜索到一条无碰撞、安全、可行的路径，目前在自动驾驶、自主探测等领域广泛应用。

2、快速搜索随机树rrt算法因为其结构简单、计算量少、适应性强等特点被广泛使用，但其本身也存在许多不足：(1)全局随机采样的机制导致搜索树扩展盲目性太大；(2)搜索树扩展步长固定，收敛条件不佳，效率低下；(3)搜索树随机扩展导致搜索树枝条繁多，节点冗余，路径质量不佳。rrt*-connect算法是rrt算法的改进算法，其使用双向搜索随机树进行快速搜索，相较于rrt算法搜索速度更快，效率更高，同时保留了rrt*算法的渐进最优特性。但其本身由于均匀随机采样的机制和采用的固定扩展步长，使其在狭窄通道障碍物环境和复杂障碍物环境下仍然具有随机性大、搜索效率不高、路径质量不佳等问题。因此，本发明针对rrt*-connect算法进行改进，提出一种基于动态约束采样的改进rrt*-connect算法。

技术实现思路

1、本发明的目的是提供一种基于动态约束采样rrt*-connect算法的移动机器人路径规划方法、系统及处理器。本发明提供的移动机器人路径规划方法可以解决原rrt*-connect算法路径规划时间较长、随机性大、路径不佳等问题。

2、本发明所采用的技术方案如下：

3、根据本发明的一个方面，提供一种基于改进rrt*-connect算法的移动机器人路径规划方法，包括以下步骤:

4、确定移动机器人的起点与终点，初始化状态空间与双向随机搜索树tree1和tree2，起点与终点分别为tree1和tree2的初始节点；

5、判断两棵搜索树的节点数量，节点少的随机搜索树开始搜索，初始默认tree1开始搜索。此处以tree1为节点少的搜索树为例进行扩展。

6、在扩展过程中，基于动态目标节点概率偏置策略与目标节点动态区域采样策略进行动态约束采样获得采样点qrand，以采样点qrand为扩展方向，离qrand最近的搜索树节点qnearest为扩展起点，基于自适应动态步长策略向扩展方向走一个自适应动态步长获得新节点qnew，判断扩展过程是否发生碰撞，若发生碰撞则重新采样，若未发生碰撞则进行父节点重选与重连后将新节点qnew加入搜索树；

7、生成新节点qnew后，采用贪婪策略，对向搜索树开始以离qnew最近的节点qnearest2为起点朝着新节点qnew贪婪直线扩展，直到碰到障碍物或两棵搜索树连接为止；

8、若两棵搜索树相连则通过路径回溯获取初始路径，若未相连则重新判断两棵搜索树的节点数量，选择节点少的搜索树继续重复搜索扩展过程，直至两棵搜索树相连为止。

9、获得初始路径后进行冗余点删除，使用cantmull-rom样条曲线平滑路径，减少因曲率突变带来的机器人抖动和急转弯问题。

10、作为本发明的优选方案，动态目标节点概率偏置策略为：

11、在搜索树扩展过程中，设置目标偏置概率p_goal(p_goal∈[0,1])，并设置随机概率p(p∈[0,1]),当p<p_goal时，将选择对向搜索树末梢点作为目标采样节点qrandnode，当p>p_goal时，将在对向搜索树除末梢节点外的节点集合中随机选择一节点作为目标采样节点qrandnode，以目标采样节点qrandnode作为搜索树扩展方向上的指引。

12、作为本发明的优选方案，目标节点动态区域采样策略为：

13、目标采样节点qrandnode对采样方向做指引后，以目标采样节点qrandnode和离该节点最近的对向搜索树节点qnearest为核心划分多级采样区域，以qnearest至qrandnode向量方向为区域划分方向，分别作二维栅格地图边界线的垂线进行多级采样区域划分。如附图2所示，本发明综合考虑算法运行效率和算法引导性，作3级采样区域划分，1级采样区域引导性最强，用于引导扩展，2级采样区域引导性稍弱，用于逃离1级采样区域内发生的小范围局部震荡，3级采样区域引导性最弱，用于逃离大范围局部震荡。初始由tree1先扩展，故以终点qgoal作为目标采样节点qrandnode，起点qstart作为qnearest。

14、划分采样区域后，基于贪婪策略，设计采样优先级与扩展方式。设置1级、2级采样区域阈值k1与k2，初始以目标采样节点qrandnode作为采样点qrand进行扩展，若无法生成新节点qnew则降为1级采样区域采样，当区域采样次数k超过区域采样阈值k1时，强制降为2级采样区域，同理，当2级采样区域采样次数k超过区域阈值k2时，强制降为3级采样区域。通过设置区域阈值k1与k2，可有效避免局部震荡。

15、作为本发明的优选方案，自适应动态步长策略为：

16、依据地图中障碍物面积与地图的面积比、障碍物数量等因素分别乘以不同的权重系数计算得到全局环境系数ω1，基于地图尺寸数据与全局环境系数ω1获得初始步长stepinitial；引入人工势场法的斥力模型，基于斥力模型计算距采样点qrand最近节点qnearest所受到的障碍物斥力大小，依据qnearest所受到的斥力大小，可计算出qnearest的局部环境系数ω2，依据初始步长stepinitial与局部环境系数ω2得到自适应动态步长。

17、作为本发明的优选方案，冗余点删除和cantmull-rom曲线平滑为：

18、初始路径冗余点删除处理，首先从起点qstar开始与后面的节点依次相连，判断是否与障碍物碰撞，若未碰撞则继续与下一节点进行连线判断；如果与某一节点qk的连线与障碍物发生碰撞，则将qstar与该节点的前一节点qk-1相连并删除它们间的所有节点，同时将qk-1作为起点重复上述操作，直至连接终点。

19、冗余点删除后，采用cantmull-rom样条曲线对路径进行平滑处理。cantmull-rom样条曲线公式如下：

20、

21、其中，τ表示控制曲线的扭曲程度，t∈[0,1]，是插值变量，qi-2、qi-1、qi、qi+1是四个控制点。将路径结果依次取4个点代入公式中的qi-2、qi-1、qi、qi+1，即可进行路径点间的插值计算，曲线拟合。平滑后的路径曲率连续，能减少由于路径曲率突变而带来的移动机器人抖动和急转弯问题。

22、根据本发明的另一方面，提供了一种移动机器人路径规划系统：

23、初始化模块：初始化状态空间，初始化双向随机搜索树，确定移动机器人起点与终点。

24、初始路径搜索模块：在rrt*-connect算法的基础上，引入动态目标节点概率偏置和目标节点动态区域采样方法，以目标节点为搜索树扩展方向进行动态约束采样，引入自适应动态步长提升搜索树扩展效率，加速双搜索树连接速度，得到起点到终点的无碰撞初始路径。

25、路径优化模块：应用冗余点删除策略对初始路径进行冗余点删除，使用cantmull-rom样条曲线对删除冗余点后的路径进行平滑处理。

26、根据本发明的又一个方面，提供了一种处理器，其特征在于，所述处理器用于运行程序，可执行实现上述路径规划方法的步骤。

27、本发明的有益效果是：在rrt*-connect算法的基础上对其进行改进：(1)提出了动态目标节点概率偏置和目标节点动态区域采样结合的动态约束采样，通过目标节点动态偏置选择使双树扩展始终以对向搜索树节点为目标扩展方向，以目标节点及离该节点最近的对向搜索树节点为核心划分采样区域进行搜索树扩展方向约束，通过扩展过程中目标节点的不断变化动态变化采样区域，实现动态约束采样。仿真结果表明，动态约束采样有效降低了节点生成的随机性与盲目性，相较于原rrt*-connect算法，运行时间降低17％以上，总节点数降低26％以上，初始路径代价降低5.8％以上。(2)引入人工势场法的斥力思想提出了自适应步长设计，根据不同的地图环境自适应调整初始步长，通过受到的斥力大小判断周围环境障碍物的复杂程度，基于复杂程度动态调整步长。仿真结果表明，在方向引导不明确的情况下，自适应步长设计对部分环境提升不明显，而在结合动态约束采样后，相较于原rrt*-connect算法，运行时间降低27％以上，总节点数降低37％以上，有效提升了算法的搜索效率，减少了冗余枝条。(3)相较于传统的rrt*-connect算法采用了冗余点删除策略和cantmull-rom样条曲线平滑策略对路径进行了优化，仿真结果表明，优化后的路径长度降低了7％以上，有效降低了路径长度，路径更平滑。本发明在结合上述改进后，实验结果表明，总体相较于rrt*-connect算法，运行时间减少27％以上，路径长度缩减7％以上，总节点数降低37％以上，节点利用率达30％以上，有效降低了运行时间，减少了冗余枝条，提升了路径质量。