基于改进RRT算法的移动机器人路径规划研究

杨军

(湖南交通工程学院,湖南 衡阳 421009)

0 引言

移动机器人是集环境感知、动态决策规划、运动行为控制等复杂功能于一体的机械装备,在民用和军用领域均具有广泛应用前景。路径规划是开发多功能移动机器人装备的核心技术,算法的目标是在机器人工作的复杂环境中为其规划出一条无碰撞的最短路径,这是学术界的广泛关注点[1]。杨恒等[2]对传统A*算法存在的路径规划效率低、拐点多等问题提出了联合改进A*算法和动态窗口法(Dynamic Window Approach,DWA)的路径规划方法,仿真试验指出该方法能够确保移动机器人实时避障,确保规划路径的安全性。杨博等[3]对传统遗传算法所规划的路径不够平滑、易陷入局部最优的问题,设计了新的适应度函数,同时采用混合选择策略改善算法,避免了传统算法存在的问题,通过与传统遗传算法的对比,所规划的路径在性能上更佳。邱添等[4]针对概率路线图法(Probabilistic Roadmap Method,PRM)进行路径规划存在的运行效率低、狭窄通道采样困难等问题,提出了栅格概率路径图法,仿真结果表明该方法对路径规划的效率高,提升了路径规划的质量。莫海宁等[5]分析了传统烟花算法的缺陷,将竞争学习和烟花算法相结合对机器人路径实施规划,得到的规划路径具有更高平滑度,能够更好地适应机器人的作业环境。快速扩展随机树(Rapidly-Exploring Random Trees,RRT)是基于概率采样的运动规划算法,其具有采样节点随机、增量式增长、搜索能力强等优点,是有效解决运动机器人路径规划的新方法[6]。实践表明,由于RRT算法缺乏路径导向性,使得实际路径规划效率不高、转折点多且距离长。在深入研究的基础上对RRT算法进行改进,提出了待扩展点的选择策略和扩展方向,降低了扩展点选择上的盲目性,同时提出了路径冗余点消除算法和路径平滑处理算法,使得规划的路径距离短、平顺性好,路径规划效率大大提升。最后将其应用于移动机器人路径规划仿真试验和实物试验中,验证算法的有效性。

1 RRT算法及改进

1.1 RRT算法

RRT算法采用递增式构造,由所搜索的空间随机抽取样本产生。在实际产生节点时倾向于未探测区域,不妨设qstart是路径规划的起始点,qrand是路径规划空间的随机采样点,qnear是最近邻点,qnew是扩展后生成的新节点,L是扩展的步长,随机树扩展示意如图1所示[7]。

图1 随机树扩展示意图

在图1中,路径规划的起始点qstart为根节点,在扩展树中寻找和随机采样点距离最近的点作为qnear。在随机采样点和最近邻点的连线上按照拓展步长来拓展新的节点。连接最近点和新节点,如果连线遇到障碍物,那么该条路径是不通的,舍弃扩展的新节点;如果连线没有遇到障碍物,那么该条路径是可行的,将该节点添加到随机树中,直到寻找到路径规划的目标点为止。

很明显,采样点的选择直接影响到算法的效率。尤其是在移动机器人的工作空间比较狭窄时,RRT进行路径规划的效率就会大大降低,同时由于在路径规划的过程中缺乏导向性,这也使得算法对最佳路径的搜索时间比较长,所规划的路径距离也更长。

1.2 RRT算法改进

1.2.1 待扩展节点选择改进

RRT中待扩展节点为随机树中和采样节点距离最近的点,而采样节点是随机的,这也使得扩展节点是随机的。在极端情况下,当扩展随机树接近目标点时,因扩展节点是随机的,从而使得随机树向四周等概率扩展,产生大量无效的节点。基于A*算法的思想,对待扩展节点的选择进行改进[8],定义随机树中任一节点qi和采样点qrand的距离为H(i),和目标点的距离为G(i),那么

F(i)=H(i)+G(i)。

(1)

将满足F(i)最小的节点作为扩展节点,记为qnear。很明显,采用这种方式来选择待扩展节点能够在很大程度上降低传统RRT算法在待扩展节点选择上的盲目性,使得路径规划的效率大大提升。

1.2.2 扩展方向选择

传统RRT的扩展方向为随机采样点qrand所在的位置方向,很明显扩展方向具有很大的随机性。基于人工势力场的思想,对节点的扩展方向进行改进[9]。由于路径规划的目标是寻找一条从起点到目标点的最佳路线,因此当待扩展的节点确定之后,优先考虑目标方向。如果目标方向拓展成功,那么直接生成新的节点;如果目标方向存在障碍物,那么以随机采样点方向作为拓展方向生成新的节点,同时对失败的待扩展节点进行标记。再次选择失败节点作为拓展节点时,直接按照随机采样点方向进行拓展。图2为目标方向存在障碍物示意图。

图2 目标方向存在障碍物示意图

1.2.3 冗余点消除

由于RRT算法在选择扩展点时的随机性导致所规划的路径中存在诸多的冗余点,这些冗余点使得移动机器人在实际运动时的波动比较大。如果两个不相邻的路径之间连接没有遇到障碍物,那么就可以将两个路径之间的点消除。图3为路径冗余点消除示意图。

图3 路径冗余点消除示意图

路径冗余点消除就是从起始点开始逐个进行碰撞检测,按照图3的方式消除路径上的冗余点。路径冗余点的消除不仅使得所规划的路径长度大大缩短,同时也使得所规划的路径更加平顺。不妨设初始路径点序列为{q1,q2,…,qn},其中q1为起始点,qn为目标点。构建路径冗余点消除之后的路径点序列为φ,初始状态φ为空集。

1)将起始点q1添加到序列集合φ中,同时令j=1,i=n;

2)检测qj和qi之间是否有障碍物,如果有障碍物,那么继续下一步,如果没有障碍物,那么直接进入步骤4;

3)如果i=j+1,那么j=j+1,i=n,转入步骤2,反之,i=i-1,转入步骤2;

4)将点qi添加到序列集合φ中,令j=i,i=n;

5)如果j=n,那么路径上的所有冗余点被消除,反之,继续转入步骤2。

1.2.4 路径平滑处理

消除规划路径冗余点之后,其轨迹上依旧会存在比较多的拐点,这使得移动机器人在实际的转向时存在角度过大的问题。采用B样条曲线对路径进行平滑处理,从而使得所规划的路径更加平滑[10]。三次样条曲线的数学表达式为

(2)

式中Vi+j为基函数控制点,Gj,3(t)为基函数,其数学表达式为

(3)

设经过冗余点消除后的节点序列集合为{φ1,φ2,…,φm},其中φ1为起始点,φm为目标点。各点为基函数的控制点,可以得到路径上点φi和φi+3之间的轨迹,其数学表达式为

(4)

2 仿真分析

为了更好地验证改进RRT算法在机器人路径规划方面的优良性能,在MATLAB中构建简单地图环境和复杂地图环境,地图的大小为800×800,移动机器人的初始点坐标为(100,700),目标点坐标为(700,100),具体如图4所示。

图4 构造的简单和复杂地图环境

分别采用传统RRT算法和改进RRT算法对简单地图环境进行路径规划,结果如图5所示。

图5 路径规划结果对比(简单地图环境)

由图5可知,相对于RRT算法,改进RRT算法随机树路径扩展分支明显减少,这使得算法的运行效率大大提升。在距离障碍物比较远的区域,改进RRT算法能够更加快速地向目标区域生长,加快了路径规划的速度。另外,改进RRT采用B样条函数进行路径平滑,很明显改进RRT算法所规划的路径没有了比较明显的拐点,而在RRT算法所规划的路径中存在比较多的明显拐点。

分别采用传统RRT算法和改进RRT算法对复杂地图环境进行路径规划,结果如图6所示。

图6 路径规划结果对比(复杂地图环境)

对比图5和图6可知,不论是简单地图环境还是复杂地图环境,改进RRT算法随机树路径扩展分支明显减少,同时所规划的路径也更加平滑。

从算法规划路径的用时、扩展节点数、规划路径长度3个角度进行对比,对比结果见表1。

表1 算法路径规划性能对比

由表1可知,改进RRT不论是从用时、扩展节点数还是规划路径长度均明显优于传统RRT。相对于简单地图,改进RRT相对于RRT在移动机器人路径规划上表现出了更加优良的性能。移动机器人实际作业环境比较复杂,障碍物形状多种多样,传统RRT所规划的路径平顺性比较差,移动机器人在实际的移动过程中受惯性的影响,其往往无法在拐点处沿着路径移动,很容易和邻近的障碍物发生碰撞。改进RRT算法所规划的路径更加平顺,避免了移动机器人在实际的移动过程中走曲折的路线,降低了在移动过程中与周边障碍物发生碰撞的风险。

3 试验验证

为了验证改进RRT算法在真实移动机器人上的可行性,选择四轮独立驱动的移动机器人作为试验对象,其外形如图7所示。

图7 试验所用移动机器人

在移动机器人上安装了激光雷达、编码器和摄像头等,控制系统由试验台上的电脑和NPX控制板组成,在电脑上运行机器人操作系统,其负责机器人路径规划。NPX控制板通过Wi-Fi与电脑之间进行通信,获取电脑发送的路径规划信息。移动机器人在运动的过程中通过激光雷达、编码器和摄像头等设备来获取外界的信息。将改进RRT算法应用于图7所示的机器人中,对不同起始点下的移动路径进行规划,结果如图8所示。

图8 移动机器人路径规划结果

由图8可知,不论是移动机器人最终的目的地在何处,改进RRT算法均可以规划出最佳路径。

4 结论

路径规划是移动机器人开发的核心技术之一,针对传统RRT算法存在的路径规划效率低、规划路径拐点多的问题,对RRT进行改进。通过引入A*算法和人工势力场思想来选择待扩展节点和扩展方向,同时对所规划的路径进行冗余点消除和平滑处理。将改进的RRT算法应用于简单地图环境和复杂地图环境的仿真中,验证了改进RRT算法在路径规划上的有效性,同时通过实物试验,对改进RRT算法在真实移动机器人上的可行性进行了验证。改进RRT算法对移动机器人的开发具有一定的参考价值。