智能步长对RRT算法及其改进算法的影响

黄 元， 徐志博， 孙浩博， 岳一杰， 万芳新

(甘肃农业大学机电工程学院，甘肃 兰州 730070)

随着人工智能的发展，机器人被许多国家列为一项重点发展的新技术，并已被应用在诸多领域[1]。路径规划是机器人技术的重要组成部分，其算法的好坏会直接影响机器人的作业质量和效率。对于果园机器人，路径规划是实现其作业目的的基础。牛润新[2]等提出了一种基于激光雷达的树干检测算法，解决了丘陵山区果园坡度和杂草对果树检测精度的影响，为精确农业设备在丘陵山地果园的导航应用提供参考。为了解决导航机器人在温室果园环境中的快速工作问题，刘继展[3]等提出了一种基于电子罗盘和激光雷达航向信息融合的位姿检测方法，完成了自主导航系统的快速启动，实现了线角的优化。为了解决二维激光扫描仪在果园导航中感知信息少，不能有效处理复杂的三维果园场景的问题，刘伟洪[4]等提出了一种基于三维激光雷达的果园线对线导航方法，该系统可广泛应用于标准果园和复杂三维果园机械的自主导航，具有可靠的稳定性。为了提高操作设备在果园与林行之间的自主导航性能，刘明星[5]等提出了一种基于最小二乘法和支持向量机融合的树行识别与导航方法，该方案可有效提高果园作业机器的工作效率。

在移动机器人路径规划的算法研究中，RRT算法凭借其扩展随机性、灵活性得到广泛关注。Kuffner[6]等提出了改进型RRT-connect算法，其效率相较原始RRT算法提高显著，但目标函数较复杂。阮晓钢[7]等提出基于信息增益与RRT相结合的机器人环境探索策略，提高了RRT算法偏置率，但算法结构复杂，内存占用较大。司徒华杰[8]等提出了一种利用人工势场与RRT算法相结合算法，相比传统RRT算法，该算法具有更高的效率和更少的节点，但是其步长固定，在复杂环境有一定局限性。LaValle[9]等提出了双向搜索RRT算法，该算法提高了搜索速度，但是算法目标偏置率仍然较低。Hidalgo[10]等在双向RRT算法基础上提出四向RRT算法，但目标函数复杂，算法复杂度高，同时对硬件要求高。

本文在原始的RRT算法及其改进算法基础上，探索智能步长对各种算法下路径质量的影响，为后续果园作业机器人路径规划的研究提供理论依据。

1 算法介绍

1.1 RRT算法

RRT(快速探索随机树)是一种通用算法，可用于任意类型机器人。其工作原理如图1所示。以二维环境为例，其算法实现可分为3个步骤。(1)树的生成。首先在环境中将初始节点定义为xinit，然后在环境中得到随机节点xrand，如果xrand不在障碍物区域，则连接xinit和xrand得到一条连线L，如果L整个不在障碍物内，则沿着L，从xinit向xrand的方向移动一定的距离，得到一个新的点xnew，则xinit、xnew和它们之间的线段构成了一颗最简单的树。(2)树的扩展。继续重复(1)树的生成过程，在环境中取点，得到无障碍物区域的点xrand，然后在已经存在的树上找一个离xrand最近的点xnear，连接两个点，如果这条线没有与障碍物碰撞，则沿着这条线扩展得到xnew。(3)xnew节点的确定。从xnear到xrand移动一定的距离，得到新的点xnew，该点被添加到已经存在的树上规划。重复上述过程，直到目标点(或其附近的点)被添加到树上，这时就可以在树上找到一条从起点到目标点的路径。

图1 RRT算法扩展过程

1.2 RRT*算法

利用已知信息改进的RRT算法，被称为RRT*算法。RRT*算法在每次迭代后，都会在局部更新搜索树，以达到优化路径目的。如图2(a)所示，在新产生的节点xnew附近即定义的半径范围内寻找“近邻”，作为替换xnew父节点的备选。共分为3个步骤。(1)计算xnew“近邻”节点到起点的路径代价。(2)计算到每个“近邻”的路径代价。(3)将步骤1和步骤2中的路径代价相加，并将xnew连接到路径代价最小的一个“近邻”。

在为xnew重新选择父节点之后，为进一步使得随机树节点间连接的代价尽量小，需为随机树进行重新布线，以得到最优的xnew节点。重新布线过程如图2(b)所示，可分为2个步骤：第一步为xnew选择新的父节点，第二步借助最小路径代为xnew选择节点，从而达到优化的效果。

1.3 双向RRT*

为加快算法搜索速度及搜索效率，基于RRT*算法，LaValle提出了双向RRT*算法，算法伪码如下：

算法1：双向RRT*算法

1.V1{qinit}；E1；G1( )；

2.V2{qgoal}；E2；G2( )；i+0；

3.While i

4. qrandSample(i)；ii+1；

5. qnearstnearst(G1，qrand)；

6. qnewSteer(qnearst，qnew)；

7. If no_collision (q1，qnew)then

8. V1{qnew}；

9. (q1，qnew)}；

15. do

20. ；

21. else break；

24. If |V2| |V1| then swap(V1，V2)；

图2 RRT*算法

初始化时随机树T只包含一个节点，即根节点qinit。首先Sample函数从状态空间中随机选择一个采样点qrand(第4行)，然后Nearest函数从随机树中选择一个距离qrand最近的节点qnearst(第5行)，最后Extend函数通过从qnearst向qrand扩展一段距离，得到一个新的节点qnew(第8行)。如果qnew与障碍物发生碰撞，则Extend函数返回空，放弃这次生长，否则qnew将加入到随机树中。重复上述步骤直到qnearst和目标点qgoal距离小于一个阈值，则代表随机树到达了目标点，算法返回成功(第6～7行)。

1.4 智能步长算法

传统RRT算法在扩展时，如果使用固定步长进行工作，将可用节点相连，在无障碍区域产生节点较多。本文在RRT算法的两种改进算法中添加了智能步长，即在节点探索过程中根据是否遇到障碍物自动调节步长。该改进算法可以使节点在无障碍物的空旷区域加快搜索速度，扩大搜索区域，同时节点进入障碍物区域时可以保证节点有效扩展，提高扩展的效率。算法伪码如下：

算法2：智能步长算法

1.R

2.Safe_distance

3.Influence_area| (sx+R，sy+R)

4.IfSafe_distance>Influence_area：

5.d=step_size|α(α>1)

6.Else：

7.d=step_size( )

8.Returnd

其中R为安全距离，由半径r和障碍物影响范围的值构成的；(sx，sy)是圆心坐标，α、β分别为扩展因子和收缩因子。算法由3部分组分：第一部分初始化参数(第1～3行)，第1行生成安全距离R，第2行安全距离几何计算，第3行生成斥力影响范围；第二部分为影响范围判断(第4行)，判断安全距离与斥力影响范围几何关系；第三部分确定步长长度(第5～8行)，当安全距离大于斥力影响范围时，步长选择扩展因子(第4～5行)，同理当安全距离小于斥力影响范围时，步长选择收缩因子(第6～7行)。完成上述步骤结束算法得到步长d(第8行)。

2 算法仿真试验

2.1 仿真试验条件

为分析智能步长算法的有效性，进行算法仿真试验验证。本文用智能步长替换RRT算法及改进算法中的固定步长，在Pycharm软件中分别运行RRT算法、RRT*算法与双向RRT*算法。

试验设备如下。CPU：AMD Ryzen 5 4600H with Radeon Graphics (3.00 GHz)；运行内存16 G；显卡：NVIDIA GeForce GTX1650；使用python语言pygame模块，构建信息图，信息图的尺寸是(80×60)与(800×600)。将原始的RRT算法、RRT*算法、双向RRT*的算法在pycharm(2021)中做仿真分析。试验地图为二维场景图，并分为小型信息图与大型信息图。小型地图如图3(a)～3(d)所示，起点坐标(10，10)，终点坐标(50，50)；大型地图如图3(e)～3(f)所示，起点坐标(10，10)，终点坐标(790，590)。图3中，黑色区域表示障碍物，白色区域均表示算法可覆盖的区域，绿色线段表示扩展步长，红色有限线段表示算法规划出的路径。

2.2 仿真试验步骤

仿真试验验证可分为4个步骤。(1)在pycharm软件中生成地图信息，确定起点、终点和障碍物位置；(2)在步骤(1)生成的地图中分别运行原始的RRT算法、RRT*算法、双向RRT*算法，记录其运行时间、路径长度并保存路径规划图；(3)分别运行改进后的三种算法，记录其运行时间，路径长度并保存路径规划图；(4)整理步骤得到的数据，结合路径规划图，对比分析试验数据。

2.3 仿真试验结果分析

图3为仿真试验结果。由图3 (a)、图3(b)可以看出，步长改进算法对RRT算法提升不显著，但对RRT*算法、双向RRT*算法影响较大。

从图3(c)、图3(d)对比可知，传统的RRT*算法相较改进后的算法，重规划次数多，无用节点多，算法占用的内存空间较大；通过对比图3(e)与图3(f)可得出，智能步长改进的双向RRT*算法相较于原始得到的双向RRT*算法，探索范围更大，冗余节点数量少，路径较为平滑。

图3 算法运行结果

将RRT*算法、双向RRT*算法及其改进算法各运行500次，整理所需参数如表1所示。

表1 算法运行结果

通过表1可以得出，智能步长提高了RRT*算法、双向RRT*算法路径质量的部分指标。其中路径长度相较原始算法变化并不明显，但路径长度在近似相同的情况下算法运行时间分别缩短8.62%、7.96%，同时改进算法所得路径相较初始算法生成路径平滑度更高。

3 结论

本文提出的智能步长改进方案可适用于多种RRT改进算法，通过引入智能步长扩展的思想，加快了随机树生长速度，提高了算法的规划效率，大大减少了搜索路径的时间。通过与原始算法进行仿真试验对比，证明了本文提出的智能步长改进算法在路径规划效率和路径质量上更具优越性。研究对于移动机器人的运动规划具有一定的参考意义。