多策略模式下RRT算法的优化*

笪 晨，宋天麟，施 维

(苏州大学应用技术学院，昆山 215300)

0 引言

随着时代的发展，机器人在人类的生活中扮演着非常重要的角色，机器人底盘的稳定、高效是移动机器人工作的基础。路径规划是指机器人在复杂环境中，根据自身传感器信息特征和环境地图，在一定时间内规划出来的一条无碰撞地从起点到终点的技术。

近年来，国内外许多学者都提出了路径优化策略，比如基于搜索方法的Dijkstra、A*算法[1]；基于采样的RRT算法；以及遗传、粒子群、蚁群等智能算法[2]。最近随着神经网络的兴起，有些学者采用残差卷积神经网络[3]和卷积评估[4]进行路径规划算法的研究；但是面对复杂的工作环境和快速导航的要求，传统的方法显得力不从心，而新方法处在实验阶段。RRT是一种基于随机采样的快速路径规划算法，具有良好的概率完备性和渐近最优性但是也存在搜索区域大，路径不平滑等缺点[5]。ADNAN等[6]结合地形粗糙度，找到最近邻的地形，基于欧氏距离进行导航，以此找到目标位置，但是移动机器人大多工作在平坦的环境，该算法局限性较大。刘恩海、陈侠等[7-10]针对RRT采样的随机性，结合人工势场法，处理目标点、最近点、障碍点之间引力与斥力的关系，为树的生长点明方向，但是人工势场法本身计算量较大，容易陷入局部最优解，继而影响引导点的选择。郗枫飞等[11]采用植物生长的方法，利用植物生长的三大原则设置增长引导点。臧强等[12]根据障碍物与扩展树之间的距离设置了动态步长来加快算法的收敛速度。上述改进的算法中，大多学者选择设置引导点和动态步长来加快算法的收敛速度，且取得了较好的结果。

本文在复杂的大环境下，基于前人的研究，分析原生RRT算法流程，提出了一种多策略模式下改进的RRT算法。优化场景地图，提出了目标点采样的新方法，设置动态步长以优化路径，最后通过仿真实验，验证M-RRT算法的有效性。

1 M-RRT算法

1.1 传统RRT算法

RRT算法是通过随机采样的方法构建一棵从起点Pinit到终点Pgoal的搜索树，随机获得生成点Prand，根据条件在生成树上寻找符合条件的Pnear，然后沿着Prand，Pnear的方向生长Step长度得到Pnew。

图1 标准RRT算法

图1为标准RRT算法的生成树，算法的随机性导致计算量增大，生长点缺乏方向；路径规划的长度与时间不稳定。图中浅灰色线框增大了搜索的时间，点A、B、C可以通过优化来缩短距离，固定步长的Step生长速度缓慢。因此本文提出了M-RRT算法来解决这些缺点，表1为该算法的伪代码。

表1 M-RRT算法

1.2 导航地图优化

在工程应用当中，工程师会对地图进行手动修改与优化，锐化障碍物边缘，连接断层区域，这样可以减少机器人导航的时间。复杂的环境中存在各种各样的物体，龙建全等[13]通过过滤掉较小的障碍物来寻找引导点，但是这样风险较大障碍物的选择没有科学标准。本文基于图像形态学处理的方法来处理障碍物，图2中安全区黑方块部分代表机器人，以半个单位进行膨胀，在四邻域上划定安全区域，保证在导航过程中机器人与障碍物之间的安全。理论上说机器人可以到达凸形障碍物周围的任何地方，但是对凹形障碍物有一定的选择性。对障碍物1和2进行膨胀后，对比发现凹形障碍物仍然存在可通过区域，所以需要进行重新标定。

膨胀后的障碍物仍然是不规则的，不利于算法的执行，需要获得它们的最小外接圆或外接矩形，为了得到最小的障碍物面积，防止对障碍物进行过处理，图3展示了外接圆和外接矩形的获取过程，浅灰色部分为障碍物的最小外接矩形。

图2 障碍物膨胀 图3 外接圆，矩形

通过预处理的方法，合理处理地图中所有的障碍物，为模拟植物生长提供较为规则的生长区域，防止光线进入凹槽中陷入“黑洞”，同时也为树的生长提供引导点。

1.3 M-RRT算法原理

1.3.1 模拟植物生长算法

科学家将生物群体及个体对环境的自适应模式作为算法设计的思想启发源。植物作为自然界最成功的物种，能够快速的适应周围环境，逐步形成了对光、温度、机械振动、水、重力、电信号和化学物质的感知能力和应对措施，这些因素都可以与工作环境中的参数相映射。

图4 引导点获取策略

植物生长主要有3大原则：向光性、遮挡物影响、负向地。植物的顶端永远是向着光绕开障碍物背着地进行生长。对于导航系统而言，机器人永远从起点出发，向着终点避开障碍物进行移动。光照是影响植物生长的主要因素，图4中灰色枝芽代表当前点Current Point，太阳Goal表示终点，枝芽可以在Current Area区域内朝着任意方向生长。从当前点出发沿着太阳方向进行生长，灰色直线代表生长方向growth dir。面对图中复杂的障碍物，这肯定不是一个最好的选择。因此，选择从Goal处发射出多条光线，遇到障碍物进行反射，每次反射能量损耗20%，最后在反射光线的交汇处得到多个次级太阳点Sub Goal，将这些点作为树生长过程中的引导点。

1.3.2 顶芽生长点获取

植物从根节点B0开始生长，节点的生长与光照密切相关，如图5所示，当生长到B2点的时候，选择距离阳光最近的3个引导点G1、G2、G3。引导点对B2点在x方向与y方向影响如式(1)所示，g(Gi)表示当前引导点的光照强度值。

(1)

根据植物概率生长模型[15]，利用式(2)和式(3)考虑周围环境对生长点的影响，在每个引导点上定义适应度函数，gi为Gi点的能量；s为Gi与B2之间的障碍物影响；hi为终点与Gi的距离函数。B2的生长点选择与函数g、h、s相关。设定α、β、γ3个影响因子，fi表示各个引导点对B2的综合影响力。

(2)

h=h(dis(Goal,Gi))i=1,2,3
s=s(dis(Goal,Gi))i=1,2,3

(3)

1.4 动态步长生长

根据fi可以确定生长方向，在图5中B2、G2之间存在受障碍物的情形与G1、G3点的生长策略有所不同。

为了加快规划速度，式(4)提出了自适应步长的策略。首先判断B2与Gi之间是否存在无障碍物，无障碍物时设定奖励措施，直接将Gi选为生长点如图6所示；当存在障碍物时，根据B2与障碍物之间的距离设置惩罚措施，dis表示障碍物与B2的距离，比较dis与step的大小设置不同的策略，为了保证阳光的唯一性，每个引导点只能使用一次。

(4)

图5 引导策略 图6 生长点选择策略

1.5 航迹优化

M-RRT生长点的选择具备一定的随机性，导致规划的路径蜿蜒曲折，路径中存在很多冗余的节点需要优化。从起始点B0开始，依次判断与B2～Bn的连线，假设Bi与B0的连线第一次经过障碍物，那么将Bi-1定义为新的B1点，依次类推直至终点。这样能够剔除路径中多余的导航点，图7表示剪枝优化过程。为了消除导航过程中出现的拐点，以拐点Bi为圆心，生长步长为半径，在相邻节点的连线上获得两个固定点P0，P2，引入参数t，结合P1点，用二阶贝塞尔曲线化路径，如图8所示。

图7 剪枝优化 图8 贝塞尔曲线

2 M-RRT实验分析

2.1 环境准备

为了验证M-RRT算法，设置了20 m×20 m和50 m×50 m两种面积不同的地图，在上面分别放置简单和复杂的障碍物。在Python3.9开发环境，利用Dijkstra和A*算法在两种地图中进行采样测试，计算导航的路径长度和时间，以此作为纵向对比条件；其次将M-RRT算法与RRT和Informed RRT*算法作横向对比证明算法的真实、有效性。

2.2 采样测试

Dijkstra算法可以获得最优的导航路径，但是规划时间较长，只适用于小规模导航的场景；A*算法兼顾路径与时间，在实际生产当中应用广泛。首先对地图进行预处理，将机器人当成质点处理。图9为20 m×20 m地图在简单障碍物下的测试结果，图10为50 m×50 m地图在复杂障碍物下的测试结果，表2选取了100次规划结果的平均值，得到最优路径长度和规划时间，机器人半径选择0.1 m，每次搜寻的步长设置为0.4 m。

图9 20 m×20 m采样 图10 50 m×50 m采样

表2 Dijkstra，A*100次结果平均表

2.3 简单障碍物实验

2.3.1 实验准备

图11 20 m×20 m地图中的引导点

利用光线照射障碍物发生反射的原理，在图11中画出从光源处以30°发射出的4条光线，用4种不同的颜色表示，在障碍物区域反射10次，计算2条光线交叉处的坐标和能量，表3选取了部分引导点，接着在RRT算法中增加引导点，根据式(2)和式(3)来引导树的生长。

表3 部分引导点坐标能量表

2.3.2 实验结果

选取RRT、Informed RRT*、以及M-RRT算法进行对比，实验选取起点和终点为(2,3)、(18,15)搜索步长为0.4 m，迭代次数为3000次。

图12 M-RRT导航

图12为M-RRT的导航结果，图中灰色的*号表示引导点，黑色曲线为初始路径，浅灰色的线为剪枝优化后的结果，黑色虚线的为二阶贝塞尔曲线优化拐点后的路径。选取RRT系列3种的算法，进行100次实验，得到规划路径与时间的平均值，表4为3种算法的对比结果。

表4 20 m×20 m地图100次实验对比表

经过多次实验，测定式(2)中α=0.5，β=0.4，γ=0.1为最优值，为了防止陷入局部解，分别设定10%，15%，75%的概率指向终点、随机点和引导点。

2.4 复杂障碍物实验

图13为在复杂障碍物地图中设置的5条光线，计算得到引导点的能量与坐标。

选取RRT、Informed RRT*、以及M-RRT算法进行对比，实验选取起点和终点为(2,3)、(45,35) 搜索步长为1 m，迭代次数为3000次。

在50 m×50 m的地图中，存在多种不规则的障碍物中，图14为M-RRT算法的导航结果，黑色曲线为初始路径，浅灰色的线为优化后的结果，表5为3种算法的对比结果。

图13 50 m×50 m地图中的引导点 图14 M-RRT导航

表5 50 m×50 m地图下100次实验对比表

2.5 实验分析

为了验证M-RRT算法的效果，本文综合对比了Dijkstra、A*算法、RRT系列算法的实验数据。图15、图16为5种算法的路径规划时间和距离在两种地图下的对比。Dijkstra和A*算法在小地图中规划时间和距离有明显的优势，但是在大地图中，RRT系列算法的优势就更加明显。

图15 时间对比 图16 路径对比

表4和表5分析了RRT系列算法的实验结果，标准RRT算法路径的标准差最大，路径最长，Informed RRT*引入了生长区域，限制了树的生长方向，一定程度上减少了路径的标准差，但是增长区域本身的不确定性，导致规划时间的提高，出现规划失败的场景。在地图面积小，障碍物少的时候，引导点数量偏小；而在大地图中，障碍物多，引导点数量变多。在M-RRT算法中，引导点为光线反射的交点，远离障碍物，能够很好表示环境中障碍物的密集程度，在小地图导航的过程中，M-RRT算法没有明显的优势，甚至因为引导点数量偏少，导致出现了6%的规划失败率。

在大地图中，随着引导点数量的增多，引导点远离障碍物，可以减少迭代过程中碰撞检测的次数，以此减少规划的时间；利用引导点之间的连通性概率较大的特性，动态设置树的生长步长；最后通过剪枝优化和贝塞尔曲线得到光滑的导航路径。表5显示，M-RRT算法得到的路径较Dijkstra、Informed RRT*算法减少了6%和3%，时间比最快的RRT减少了53%，不仅如此，在大量的实验下，成功率达到了100%，规划结果得到的时间标准差和距离标准差更加小，算法更加稳定。所以本文提出的M-RRT导航算法能够在大地图、复杂地形下得到良好的导航效果。

3 结束语

本文首先分析了RRT算法存在的不足，设置引导点和动态步长的改进措施。利用植物生长的特性设置引导点，综合起点、引导点、终点的关系设置动态步长，利用图像处理的方式对地图进行预先处理。基于以上多种改进措施，提出了多策略模式下的改进RRT算法。M-RRT算法优势体现在于：

(1)面对工作场景中复杂的障碍物，利用OpenCV图像技术优化地图，得到障碍物的最小外接圆或外接矩形，将障碍物根据机器人的半径进行膨胀，将机器人作为质点，提升算法的计算速度。

(2)利用植物生长的向光性原则，根据光线的反射原理在地图中加入了引导点，综合生长点、引导点和终点的多种关系，定义了适应度函数，使得采样点更加的多样化。

(3)根据引导点与生长点的连线与障碍物之间的关系，设置了动态步长，同时设置多种判断策略，进行引导点与终点之间规划。

与常用的算法相比，M-RRT算法在小地图、低障碍物场景下，优势不大，但是在大地图、复杂障碍物的

场景下，不仅可以减少规划的时间与距离，还可以保证算法的稳定性。本文设计的光线反射程序，需要规则的障碍物边缘，势必会导致机器人工作区间的减少，导致路径不可达的现象，下一步将继续优化程序，对膨胀后的障碍物进行合理性标定，同时将继续优化极端条件下M-RRT算法的导航效果。