一种基于改进RRT*算法的移动机器人路径规划方法与流程

本发明属于移动机器人路径规划技术领域，涉及一种机器人路径规划方法，尤其涉及一种基于改进rrt*算法的移动机器人路径规划方法。

背景技术

随着社会的发展，移动机器人在人类生活中应用越来越广泛。路径规划(机器人自主寻找一条从初始位置到目标位置的可行路径)是机器人完成各项工作的基础。目前，全局路径规划算法主要有dijkstra(迪杰斯特拉)算法、a*算法、蚁群算法等。但大多数算法应用于路径规划成功的关键在于对算法具体参数的设置，而且不适合在高维复杂空间中应用。在实际应用过程中，dijkstra算法搜索速率比较缓慢，复杂度较高，从而需要消耗较多的计算时间。a*算法对启发函数的选取要求较为严格，而且随着所研究环境规模的扩大，它的运算空间增长是指数级别的。蚁群算法又较容易陷入局部最小值，造成无法规划出可行的路径。

基于采样的快速扩展随机树(rrt)算法是用于路径规划的新方法之一，与其他应用于路径规划如蚁群算法、人工势场法和a*算法等传统方法相比，它具有概率完备性和收敛速度较快等显著优点，但由于它的搜索过程过于平均，所以计算到的最终路径通常都与最短路径有较大偏差。改进的rrt算法—rrt*算法，它在继承了rrt算法概率完备性的同时还具备渐进最优性，即得到的最终路径较为接近最短的可行路径。但是rrt*算法的不足之处在于它的渐进最优性是以减缓收敛速率和加大搜索时长为代价的。

因此，希望有一种基于改进rrt*算法的移动机器人路径规划方法可以改善现有技术的上述缺陷。

技术实现要素：

本发明的目的是提供一种基于改进rrt*算法的移动机器人路径规划方法，克服上述现有技术的缺点，降低搜索的随机性，提高了收敛速率，可以有效避免陷入局部最小值，使机器人成功避免障碍物快速到达目标点。

为实现上述目的，本发明采用的技术方案是：

一种基于改进rrt*算法的移动机器人路径规划方法，其特征在于包括以下步骤：

(1)通过移动机器人自带的激光雷达传感器、超声波传感器、红外传感器采集机器人工作环境信息，进行栅格地图建模，将每一个栅格标记为障碍区域或无障碍区域，并确定起始点和目标点；

(2)采用目标偏执策略在无障碍区域内生成随机采样点xrand，降低搜索的随机性，提高搜索效率；

(3)寻找与随机采样点xrand最近的节点xnearest；

(4)插入新节点xnew；

(5)遍历新节点邻域范围内点集，寻找最小代价值节点；

(6)对邻域节点进行重连操作；

(7)重复上述步骤，直到xnew落在目标节点xgoal的预设区域范围内，获得最优的规划路径；

(8)引进倒序试连法平滑策略对步骤(7)获得的路径进行路径平滑处理，获得机器人在实际环境下最佳运动路径。

进一步的，步骤(2)中，采用目标偏置策略在无障碍区域内生成随机采样点具体为：在每一次随机获取采样点时，先按照均匀概率分布随机获取一个概率值pg，当pg小于初始设定的阈值pmax，则选择目标点xgoal为采样点；否则，在全局区域随机获取采样点。

进一步的，步骤(4)中，插入新节点具体步骤为：

(4.1)当在无障碍区域时，在直线xrandxnearest上截取一点xnew，使||xnearest-xnew||等于搜索步长p，如果线段xnewxnearest没有接触到障碍区域，就把节点xnew作为新节点插入，否则舍弃xnew，返回步骤(2)，其中||xnearest-xnew||表示xnearest到xrand之间的欧式距离；

(4.2)当xgoal＝xrand且扩展新节点触碰到障碍物时，则使用规避步长延伸法插入新节点。

进一步的，步骤(5)中，寻找最小代价值节点具体为：遍历节点xnew邻域范围内点集xnear，逐一比较从节点xi到xstart与xi到xnew的代价值之和cost||xnew-xi||+cost||xi-xstart||，其中cost||xm-xn||表示节点xm按照顺序路径到节点xn的路径长度，其中和值最小的点xi即为最小代价值节点xmin。

进一步的，步骤(6)中，对邻域节点进行重连操作具体步骤为：

(6.1)把节点xmin当成xnew的父节点，然后断开xnew与原父节点的连接，连接xnew与xmin；

(6.2)对于点集xnear中除却节点xmin之外的任一节点xi，将cost||xnew-xstart||与cost||xi-xnew||之和与cost||xi-xstart||做比较，如果前者较小，则断开xi与原先父节点的连接，将xnew作为其新的父节点，并且递归改变xi的子代节点的连接，反之则保持原先的连接关系不变。

进一步的，步骤(8)中，采用倒序试连法对步骤(7)获得的路径进行路径平滑处理具体包括以下步骤：

(8.1)将步骤(7)得到的最终路径上的所有节点依次标记为x0,x1,x2,···,xn-1,xn，其中x0与xn分别代表初始节点与目标节点，且令缓存点集x＝{x0}；

(8.2)尝试连接点xa初始值为起始点x0，另一尝试连接点xb初始化为目标节点xn，判断xa与xb二者之间的连线是否会触碰到障碍区域，如果会接触到障碍物则将xb重新赋值xn-1，再次对当前xa与xb之间的连线进行碰撞判断，如此反复，直至寻找到第一个节点xk(1≤k≤n)，n为节点总数，当xb＝xk时满足xa和xb之间的连线不会触碰到障碍物为止，且将此时节点xb加入到点集x中；

(8.3)将xa赋值为xb，将xb赋值为xn，重复步骤(8.2)的碰撞判断过程，并将满足非碰撞条件的新的节点xb加入到点集x中；

(8.4)重复步骤(8.3)，直到最终当xb＝xn时，满足xa与xb之间的连线不会触碰到障碍物，则将此时的xb加入到点集x中，然后依次连接点集x的节点，即为最终平滑的路径。

进一步的，步骤(4.2)中，规避步长延伸法的具体过程包括：

a、以xnearest为圆心、搜索步长p为半径作圆，寻找圆周上满足条件的点xt为扩展节点，即作为新节点插入；

b、若无法找到满足条件的点xt，则舍弃此次扩展操作，返回步骤(2)。

进一步的，步骤a中，圆周上的点xt满足的条件具体包括：

①点xt与点xnearest之间的连线不经过障碍区域；

②点xt与点xnearest之间的距离值为一个步长；

③在满足条件①和条件②的前提下，点xnearest与点xt的方向连线和点xnearest与点xgoal的方向连线的夹角θ最小，且为避免节点朝着目标点反向扩展，θ不超过90度。

进一步的，步骤(7)中，预设区域具体为：||xgoal-xnew||小于或等于搜索步长，并且线段xgoalxnew没有接触到障碍区域。

本发明有益效果是：

本发明的基于改进rrt*算法的移动机器人路径规划方法针对基本rrt*算法存在的问题从以下两点进行了改进：1)引入目标偏置策略，减少了采样点的随机性；2)在此基础上，提出了规避步长延伸法，使得随机树能够合理的远离障碍区域，避免陷入局部最小值。本发明的基于改进rrt*算法的移动机器人路径规划方法不仅保证了有效的避障能力，同时也提高了收敛速率，使机器人成功避开障碍物快速到达目标点。

附图说明

图1为rrt*算法扩展节点过程示意图；

图2为规避步长延伸法示意图；

图3为倒序试连法流程图；

图4为标准rrt*算法在矩形障碍环境下寻找最优路径结果示意图；

图5为本发明改进rrt*算法在矩形障碍环境下寻找最优路径结果示意图；

图6为标准rrt*算法在圆形障碍环境下寻找最优路径结果示意图；

图7为本发明改进rrt*算法在矩形障碍环境下寻找最优路径结果示意图；

图8为标准rrt*算法在混合障碍环境下寻找最优路径结果示意图；

图9为本发明改进rrt*算法在混合障碍环境下寻找最优路径结果示意图；

图10为本发明改进rrt*算法在矩形障碍环境下平滑路径图；

图11为本发明改进rrt*算法在圆形障碍环境下平滑路径图；

图12为本发明改进rrt*算法在混合障碍环境下平滑路径图；

图13为标准rrt*算法和本发明改进rrt*算法迭代次数对比图；

图14为标准rrt*算法和本发明改进rrt*算法最终路径长度对比图。

具体实施方式

为使本发明实施的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行更加详细的描述。在本发明实施例中，路径规划方法包括以下步骤：

(1)通过移动机器人自带的激光雷达传感器、超声波传感器、红外传感器采集机器人工作环境信息，进行栅格地图建模，将每一个栅格标记为障碍区域或无障碍区域，并确定起始点和目标点；图4至图12中，黑色部分为障碍区域，白色部分为无障碍区域；s点为起始点，g点为目标点。

(2)采用目标偏执策略在无障碍区域内生成随机采样点，降低搜索的随机性，提高搜索效率；

(3)寻找与随机采样点xrand最近的节点xnearest；

(4)插入新节点xnew；

(5)遍历新节点邻域范围内点集，寻找最小代价值节点；

(6)对邻域节点进行重连操作。

(7)重复上述步骤，直到xnew落在目标节点xgoal的预设区域范围内，获得最优的规划路径。

(8)引进倒序试连法平滑策略对步骤(7)获得的路径进行路径平滑处理，获得机器人在实际环境下最佳运动路径。

所述步骤(2)中，采用目标偏置策略在无障碍区域内生成随机采样点具体为：在每一次随机获取采样点时，先按照均匀概率分布随机获取一个概率值pg，当pg小于初始设定的阈值pmax，则选择目标点xgoal为采样点；否则，在全局区域随机获取采样点。

所述步骤(4)中，插入新节点具体步骤为：

(4.1)当在无障碍区域时，在直线xrandxnearest上截取一点xnew，使||xnearest-xnew||(表示xnearest到xrand之间的欧式距离)等于搜索步长p。如果线段xnewxnearest没有接触到障碍区域，就把节点xnew作为新节点插入，否则舍弃xnew，返回步骤(2)。

(4.2)当xgoal＝xrand且扩展新节点触碰到障碍物时则使用规避步长延伸法插入新节点。

所述步骤(5)中，寻找最小代价值节点具体为：遍历节点xnew邻域范围内点集xnear，逐一比较从节点xi到xstart与xi到xnew的代价值之和cost||xnew-xi||+cost||xi-xstart||(其中cost||xm-xn||表示节点xm按照顺序路径到节点xn的路径长度)，其和值最小的点xi即为最小代价值节点xmin。

所述步骤(6)中，对邻域节点进行重连操作具体为：

(6.1)把节点xmin当成xnew的父节点，然后断开xnew与原父节点的连接，连接xnew与xmin。

(6.2)对于点集xnear中除却节点xmin之外的任一节点xi，将cost||xnew-xstart||与cost||xi-xnew||之和与cost||xi-xstart||做比较。如果前者较小，则断开xi与原先父节点的连接，将xnew作为其新的父节点，并且递归改变xi的子代节点的连接，反之则保持原先的连接关系不变。

所述步骤(4.2)中，使用规避步长延伸法插入新节点具体为：

a、以xnearest为圆心、搜索步长p为半径作圆，寻找圆周上满足条件的点xt为扩展节点，即作为新节点插入；

b、若无法找到满足条件的点xt，则舍弃此次扩展操作，返回步骤(2)；

所述步骤a中，扩展节点xt需要满足的条件具体为：

①点xt与点xnearest之间的连线不经过障碍区域；

②点xt与点xnearest之间的距离值为一个步长；

③在满足条件①和条件②的前提下，点xnearest与点xt的方向连线和点xnearest与点xgoal的方向连线的夹角θ最小，且为避免节点朝着目标点反向扩展，θ不超过90度。

重复步骤(1)至步骤(7)，直到xnew落在目标节点xgoal的预设区域范围内。

本发明改进后的rrt*算法伪代码如表1所示：

表1改进后的rrt*算法伪代码表

本发明为解决所生成路径存在大量不必要冗余转折节点问题，引进倒序试连法平滑策略对步骤(7)获得的路径进行路径平滑处理，如图3所示，具体包括以下步骤：

(8.1)将步骤(7)得到的最终路径上的所有节点依次标记为x0,x1,x2,···,xn-1,xn，n为节点总数，其中x0与xn分别代表初始节点与目标节点，且令缓存点集x＝{x0}。

(8.2)尝试连接点xa初始值为起始点x0，另一尝试连接点xb初始化为目标节点xn，判断xa与xb二者之间的连线是否会触碰到障碍区域，如果会接触到障碍物则将xb重新赋值xn-1,再次对当前xa与xb之间的连线进行碰撞判断，如此反复，直至寻找到第一个节点xk(1≤k≤n)，当xb＝xk时满足xa和xb之间的连线不会触碰到障碍物为止，且将此时节点xb加入到点集x中；

(8.3)将xa赋值为xb，将xb赋值为xn，重复步骤(8.2)的碰撞判断过程，并将满足非碰撞条件的新的节点xb加入到点集x中；

(8.4)重复步骤(8.3)，直到最终当xb＝xn时，满足xa与xb之间的连线不会触碰到障碍物，则将此时的xb加入到点集x中，然后依次连接点集x的节点，即为最终平滑的路径；

为验证本发明改进rrt*算法的有效性和可行性，在64位windows7的联想m490笔记本(intelcore处理器，主频2.5ghz，内存4gb)硬件环境下，利用matlab2009a进行模拟仿真，全局状态区域为1000×1000的二维平面，起始点s(300,100)，目标点g(900,800)。针对仿真环境，选取搜索步长为p＝25，邻域半径为r＝50，概率阈值pmax＝0.2。

首先在矩形障碍环境下使用标准rrt*算法和本发明改进rrt*算法对机器人路径进行规划，实验结果如图4和图5所示。

为验证本发明改进rrt*算法在不同环境下的适应性，在圆形障碍环境下和混合型障碍环境下分别使用标准rrt*算法和本发明改进rrt*算法对机器人路径进行规划，实验结果如图6至图9所示。

为验证本发明改进rrt*算法引进倒序试连法平滑策略的有效性和可行性，在矩形障碍环境、圆形障碍环境和混合型障碍环境下分别对本发明改进rrt*算法规划好的最优路径进行路径平滑处理，实验结果如图10至图12所示。图10至图12分别与图6至图9一一对应。

如图10至图12可以看出，本发明改进rrt*算法引进倒序试连法平滑策略后，冗余转折节点大幅度减少，路径变的更为平滑，减少了机器人在移动过程中的换向动作，使得机器人可以平稳的达到目标点。

为验证本发明改进rrt*算法的稳定性，多次运行标准rrt*算法与本发明改进算法进行路径规划，记录每次运行结果，统计迭代次数和路径长度得到的实验数据对比图如图13和图14所示。

如图13和图14可以看出，本发明改进rrt*算法与标准rrt*算法相比较，所需迭代次数和规划路径长度均有明显的减少。

根据图13和图14数据计算可得表2。

表2两种算法实验结果对比分析表

注：i表示平均迭代次数，l表示平均路径长度

从表2可以看出，本发明改进rrt*算法的有效性和可行性。在两种不同障碍区域下，改进rrt*算法与原始rrt*算法相比，平均迭代次数降低了30％到40％，平均规划路径长度减少了5％左右。因此验证了本发明改进rrt*算法比标准rrt*算法规划出来的路径更优，耗费时间更少。