基于方向决策RRT算法的移动机器人路径规划

刘想德，何翔鹏，胡 勇，胡小林

(1. 重庆邮电大学信息无障碍工程与机器人技术研发中心，重庆 400065；2. 重庆工业大数据创新中心有限公司，重庆 400000)

1 引言

移动机器人路径规划[1]是实现机器人自主导航的关键技术之一，其目的是使机器人在障碍物约束环境中，通过路径长度、搜索时间，平滑度以及避障能力等性能指标，求解出一条从起始点到目标点的最优或近似最优路径。

传统的移动机器人路径规划算法包括dijkstra算法[2]、遗传算法[3]、A*算法[4]、粒子群算法[5]等，然而面对复杂约束的机器人应用场景，这些算法往往需要大量的计算力。而Lavalle提出的快速扩展随机树(Rapidly-Exploring Random Tree， RRT)算法，在状态空间中进行随机碰撞检测，将搜索空间逐渐扩展至可通行区域，进而搜索出一条从起始点到目标点的可行路径[6-8]，避免了对规划空间建模，运行速度快，被广泛应用于移动机器人路径规划中。

传统RRT算法应用于机器人路径规划时，由于扩展节点的随机性大，导致求解过程中存在冗余搜索，且生成的路径仅为可通行路径，路径长度以及平滑度并非最优解。针对这些问题，Jian-Quan L等人[9]采用双向RRT算法，从起始点和目标点同时进行扩展，进而提高求解效率。然而双向RRT算法的求解路径平滑度低，不符合机器人运动学规律。朱宏辉等人[10]通过选取最小成本的扩展节点，优化了RRT算法的路径长度，但牺牲了算法的收敛速度，导致搜索时间增加。贺伊琳等人[11]将目标偏向策略引入RRT算法，以固定概率将扩展节点向目标点延伸，减小了随机性，提高了算法的运行效率。值得注意的是，面对复杂场景，基于偏向策略的RRT算法极易陷入局部最小状态。此外，许多学者将RRT与其它智能算法混合，从而提高算法的求解效率。康摇等人[12]结合RRT和滚动窗口算法增强了算法搜索未知空间的能力。Zhou F等人[13]结合RRT和粒子群算法求解最优路径。刘恩海等人[14]提高了RRT算法运用在移动机器人时的避障能力，但是优化过程需要大量运算，导致算法运行效率降低。

本文提出一种基于方向决策的RRT算法，通过引入方向信息决策机制，引导新节点趋向目标点扩展，减小无效迭代次数，进而缩短路径搜索时间。同时，将一种启发式采样策略引入所提算法中，使搜索树能够快速跳出局部最小点，利用三次b样条曲线对规划路径进行平滑处理。最后，通过仿真，验证了本文算法的优越性。

2 双轮差速移动机器人运动模型

在轮式移动机器人中，双轮差动机器人由于结构紧凑、转弯灵活、易于控制等特点，被广泛应用于工业、搜救领域，其运动方式由旋转运动和平移运动组成[15]。

图1 移动机器人运动模型

图1中(a)表示双轮差动机器人旋转运动模型。其中，机器人左轮速度为vl，右轮速度为vr，旋转角度为θ，则机器人线速度和角速度可分别描述为

(1)

(2)

机器人运动半径为

(3)

机器人平移运动模型如图1中(b)所示。设t-1时刻机器人位姿Pt-1=(xt-1，yt-1，θt-1)，当Δt时间内机器人以线速度v和角速度w移动，则t时刻移动机器人的位姿可表示为

(4)

移动机器人所有时刻的位姿构成了机器人的实际运行路径。

3 传统RRT算法

传统RRT算法的移动机器人路径规划，采用状态空间内随机采样的方式扩展节点，将搜索树从初始点不断扩展，最终延伸至目标点。将机器人起始位置坐标作为根节点vinit，并在状态空间内随机生成一个随机节点vrand，基于与vrand距离最短原则，搜索树生成新节点vnear。为了形成碰撞判断机制，vnear与vrand的相连形成线段lnear，在lnear上扩展一个测试节点vnew，其扩展步长为给定常数，判断vnew以及lnear是否与障碍物碰撞，若存在碰撞，则放弃当前节点vnear，重新选择，并循环障碍物碰撞判断过程。通过不断构造新的子结点vnew，直到节点vnew与目标节点vgoal之间的距离小于一个扩展步长，且没有发生障碍物碰撞，则扩展随机树构建成功。搜索树的扩展过程如图2所示。

图2 传统RRT算法扩展过程

4 改进RRT算法

4.1 基于方向决策机制

本文改进的RRT算法，为搜索树上的节点引入方向变量orinode，首先确定根节点的orinode参数值为根节点vinit与目标节点vgoal的相对角度偏差值，可表示为

(5)

其中vinit和vgoal的直角坐标分别为(xinit，yinit)，(xgoal，ygoal)，Δy=(ygoal-yinit)，Δx=(xgoal-xinit)。

在搜索树中引入方向变量后，基于启发式采样策略求得随机节点vrand，在搜索树上检索与vrand距离最短的节点vnear。于此同时，获取方向变量orinear，以orinear为基准，在节点vnear添加三个探索式节点vins1、vins2、vins3，添加探索式结点示意图如图3所示。

图3 探索式结点示意图

三个探索结点坐标可分别表示为

(6)

(7)

然后，计算探索式节点与vgoal的距离d，表示为

(8)

其中，探索节点的直角坐标为(xins，yins)，三个探索式节点vins1、vins2、vins3与vgoal的距离分别表示为d1、d2、d3(d1

(9)

使用三个探索节点与搜索树中Vnear节点进行碰撞检测，则反向扩展节点生成vnew，此时vnew的状态如下

(10)

将扩展后的vnew节点加入搜索树。重复执行以上过程，直至vnew与vgoal之间的距离小于一个扩展步长，且无碰撞发生。即可以得到一条从起始点到目标点之间的较优路径规划结果。

图4所示为引入方向变量的RRT算法扩展过程，虚线表示该探索式节点与障碍物发生碰撞，红色结点表示节点vnew，黄色线段为最终求解路径。

图4 改进RRT算法扩展过程

4.2 启发式采样策略

传统RRT算法在移动机器人的路径规划中，存在以下缺点：

1)由于在状态空间中随机选取采样点，求解过程中存在较多冗余扩展，导致算法求解效率低下。

2)面对多障碍物约束环境，特别是狭窄通道时，传统RRT算法的搜索树扩展需要进行大量运算，才能实现路径规划。

本节介绍一种启发式采样策略，保证搜索树向目标节点扩展的同时，也增强了算法对复杂环境的适应性。具体步骤如下：

定义变量vnear′和vnear″分别记录随机树搜索过程中最后两次的扩展节点，定义一个概率阈值变量p。随机生成一个概率值q。如果vnear′等于vnear″，则说明随机树可能陷入局部最小点，则比较p与q的值，然后执行随机采样过程，若vnear′≠vnear″，则判断当前最近点vnear′与vgoal，vnear″与vgoal之间是否发生碰撞，若未发生碰撞，则直接将vrand设定为vgoal，反之则执行随机采样。

融入启发式采样策略后系统整体流程图如图5所示。

图5 启发式采样策略流程图

4.3 三次B样条曲线拟合

利用RRT算法求解移动机器人路径规划，虽然可以得到一条无碰撞发生的可通行路径，但生成的路径存在多个折点，导致机器人在移动过程中出现卡顿感。

为了优化生成路径，利用三次B样条插值拟合法对RRT生成的路径进行平滑处理，提升路径的连续性和平滑性。

B样条曲线定义为给定m+n+1个平面内控制点Pi(i=0，1，…，m+n)，称n次参数曲线段，可表示为：

(11)

这些曲线段的总和称为n次B样条曲线。其中Gi，n(t)为基函数，定义为：

(12)

当式(12)中n取3时，即为3次B样条，3次B样条算法的基函数为

(13)

3次B样条算法曲线方程为：

(14)

利用3次B样条算法对改进RRT进行路径平滑，图5中(a)， (b)分别为3次B样条处理前后的得到的路径。

可见经平滑处理后的路径连续性和平滑性得到了优化。

图6 规划路径平滑前后对比图

5 实验结果与分析

利用Matlab2016b在两种环境下对比传统RRT算法、基于目标偏向RRT算法和改进RRT算法的性能。

环境1下实验结果如图7(a)，(b)，(c)所示，环境2下实验结果如图7(d)，(e)，(f)所示。环境地图尺寸均为800m×800m，黑色区域表示障碍物，扩展步长S为25m，概率阈值p为0.3。环境1为简单场景，机器人起始点坐标为(50m，50m)点，目标点坐标为(750m，750m)点。环境2为较复杂场景，机器人起始点坐标为(50m，750m)点，目标点坐标为(750m，750m)点。蓝色粗线条为不同算法下机器人的求解规划路径。

图7 算法路径规划结果对比图

从图7中可以看到，在环境1中，传统的RRT算法由于缺乏导向性，仅通过随机采样的方式拓展节点，导致出现大量无效拓展，且求解路径非最优解。而基于目标偏向的RRT算法虽然可以得到一条较优路径，但是当目标点和当前位置的连接线上存在障碍物时，会出现较多无效扩展，从图8中可以看到，在较复杂场景中，传统RRT算法和目标偏向性RRT算法除上述缺点之外，若环境中障碍物较为密集时，会产生更多的无效扩展，而改进RRT算法，能够在进行较少迭代后跳出了这片区域。

图8 算法迭代次数对比图

图8(a)、(b)是以上三种算法的迭代次数对比图，图中可以看出，在两种环境下，本文改进RRT算法均能在较少迭代次数内收敛。

考虑到RRT算法是一种随机性算法，为了验证实验结果具有普遍适应性，分别在两种环境下进行30次试验，得到的平均数据记录于表1表示。

表1 不同环境地图下算法实验结果对比

在场景1中，相比于传统RRT算法，改进RRT算法的迭代次数减少62.6%，求解时间缩短86.9%，路径长度缩短23.1%；相比目标偏向性RRT算法，改进RRT算法的迭代次数减少24.1%，求解时间缩短48.2%，路径长度缩短18.1%。场景2中，相比传统RRT算法，改进RRT算法的迭代次数减少39.5%，求解时间缩短85.4%，路径长度缩短8.4%；相比基于目标偏向性的RRT算法，迭代次数减少50.6%，求解时间缩短79.9%，路径长度缩短9.4%。

在不同复杂程度的场景中，分析三种RRT算法的运行结果，可以看出OIS-RRT算法能以更少的迭代次数，更快的收敛速度求解得到一条更优路径。

6 结论

为了解决传统RRT算法应用于移动机器人路径规划时扩展随机性强，算法运行效率低，生成路径质量非最优的问题，本文提出一种基于方向决策RRT算法，通过方向变量，引导节点快速向目标点扩展；采用启发式采样策略提升算法在复杂环境下的适应性和避障能力；利用三次B样条曲线优化求解路径，提高了路径质量。通过仿真，验证了本文改进RRT算法能够有效缩短移动机器人求解路径规划的时间，并提升求解路径质量。