移动机器人导航路径的Halton采样-先验RRT*规划*

陈 娟，梅占勇，马晓慧

(1.山西工程科技职业大学，太原 030031；2.成都理工大学计算机与网络安全学院，成都 610059)

0 引言

机器人可以在环境恶劣、危险等人类受限条件代替人类工作，不仅可以将人类从重复劳动中解放出来，而且提高了工作效率和工作质量，因此机器人在航空航天、医疗、服务、生产制造等行业得到越来越广泛应用[1]。路径规划是机器人实现自主导航的基础，也是完成其他任务的前提，行驶路径质量对机器人工作效率、工作质量、使用寿命、工作能耗等关键性能具有极大影响[2]。因此，机器人路径规划方法的研究具有重要的经济价值和使用价值。

移动机器人的工作环境是复杂多样且多变的，根据环境信息先验知识的多少，一般将路径规划分为全局规划和局部规划两类。从规划目标看，全局路径规划是在静态环境中规划出可行路径，且实现时间最少、路径最短、能耗最低等优化指标；而局部路径规划是在动态环境中规划出安全无碰、相对较优路径，更加强调安全性和灵活性[3]。本文研究全局路径优化问题，因此着重对全局路径优化方法进行分析。从整体规划思路讲，全局路径规划可以分为启发式方法、随机扩展方法、智能规划法两类。启发式方法思路是构造距离启发信息，使用类似贪婪思想优先选择最小代价的路径点，比如A*算法、D*算法等，A*算法在简单环境下搜索效率较高，但是在大规模复杂环境下规划效率低、实时性差；D*算法对环境多样性的适应能力较强，且能够在动态环境下应用，但是存在路径转弯多、不平滑的问题[4]。高鹏等[5]针对果园应用背景的机器人，使用启发函数减小A*算法的贪婪度，有效提高了路径质量。随机扩展方法思路是节点随机生长，当扩展至终点时得到了全局路径，比如RRT、RRT*算法等[6]，此类方法主要缺点是效率低、路径不平滑。叶鸿达等[7]提出了双向生长RRT*算法，并将该算法规划出的路径进行剪枝处理，有效减小了路径长度。智能规划方法是依赖智能算法的一类规划方法，比如神经网络、粒子群算法等，该类方法规划的路径质量主要依赖模型质量与算法性能[8]。随着机器人应用环境和场景的多样性，单一方法规划的路径质量难以满足指标要求，路径规划方法也会进行融合和多样化。

针对RRT、RRT*算法存在的效率低、路径不平滑等问题，本文在RRT*算法中加入了基于先验知识的目标偏置策略、基于Halton采样的候选点集策略、剪枝与平滑策略，从而提出了基于Halton采样-先验RRT*算法的路径规划方法，有效提高了算法的路径规划效率，同时减小了路径长度、提高了路径平滑性。

1 环境模型与算法分析

1.1 环境模型与碰撞检测

在路径规划研究中，一般将机器人视为一个质心，但是机器人本身是有尺寸的，这种简化方式会降低机器人行驶安全性。为了解决这一问题，需要将障碍物进行膨化处理，将机器人最大半径记为ϑ，则障碍物沿其最外侧轮廓向外膨胀ϑ，就实现了障碍物膨化处理。以二维空间三角形障碍物为例，膨化过程如图1所示，图中虚线三角形为障碍物自身尺寸，实线三角形为膨化后的障碍物尺寸。障碍物膨化处理本质是将机器人尺寸转移到障碍物上，从而将机器人视为一个质点。

通过计算法对机器人和障碍物的碰撞情况进行判断。首先将障碍物规则化为圆形或者多边形，以三角形为例对计算法进行介绍，如图2所示。

图1 障碍物膨化处理 图2 碰撞计算法

将机器人路径各段路径总合记为{Pathi}，则当下式存在解时说明机器人路径与三角形障碍物发生碰撞，当无解时说明机器人与三角形障碍物不发生碰撞。

(1)

1.2 RRT*算法原理及分析

快速扩展随机树(rapidly-exploring random tree,RRT)算法以出发点为根节点，以随机生长的方式使节点逐步扩展到目标点，具有概率完备性的特点[9]。RRT算法的随机扩展方式，使算法的路径规划效率和质量略低。RRT*算法在原算法基础上增加了父节点重选择策略和重新布线策略[10]，一定程度上提高了路径质量。

(1)父节点重新选择策略。根据RRT算法原理扩展出新节点qnew，而后以qnew为圆心和设定半径画出一个圆，圆内的所有节点为qnew的可选父节点，从中选择出从起点到qnew长度最短的路径，对应节点为重新选择的父节点。如图3a所示，虚线圆内的节点为qnew的可选父节点，包括E、F、H、J共4个。

可选父节点对应的路径及长度如表1所示。

表1 可选父节点对应路径

由表1可知，将节点E作为父节点，可以得到从起点到当前节点qnew的最短路径，因此将E重置为父节点，得到新路径如图3b所示。

(a) 父节点重规划前 (b) 父节点重规划后

(2)重新布线策略也可理解为“qnew做谁父节点的问题”，该策略执行方法为以qnew为圆心和设定半径画出一个圆，圆内的所有节点为qnew的可选子节点，若将qnew作为父节点后的路径长度有所减小，则可以将qnew作为该节点的父节点。如图4a所示，虚线圆中节点D、F、H、J均可作为qnew的子节点。

图4a中路径在重新布线前后的路径长度比较如表2所示。

表2 重新布线路径对比

由表2可知，若将qnew作为父节点，节点F对应的路径长度减小，其余节点对应路径长度增大。因此，将节点F作为qnew的子节点，其余路径保持不变，得到重新布线后的路径如图4b所示。

(a) 重新布线前 (b) 重新布线后

由上述理论介绍和分析可知，RRT*算法的父节点重新选择和重新布线策略可以有效提高路径质量，减小路径长度。但是RRT*算法仍存在一些问题：①它是一种完全随机的路径规划方法，没有使用目标点位置、障碍物分布等先验知识；②规划后路径没有进行再次优化，路径存在冗余节点和不平滑等问题。

2 Halton采样-先验RRT*算法

2.1 基于先验知识的目标偏置策略

RRT*算法仍以随机扩展方式进行路径规划，使得算法的规划效率较低。为了提高算法的规划效率，本节提出了基于先验知识的目标偏置策略。其核心思想是设置一个控制概率σ和随机数p∈(0,1)，当p<σ时路径朝目标方向扩展，当p≥σ时路径朝随机方向扩展。具体分为以下3个方面：

(1)若p≥σ，则采样点xsample=xgoal，在xnearest→xgoal方向上扩展出新节点xnew，若xnew节点满足无碰约束，则将xnew添加到随机树节点中，如发生碰撞则进入随机采样策略；

(2)若p<σ，则采样点xsample=xrand，在xnearest→xrand方向上扩展出新节点xnew，若xnew节点满足无碰约束，则将xnew添加到随机树节点中，如发生碰撞则重新采样。

控制概率σ根据机器人周围的障碍物分布进行设置，当机器人周围障碍物较少时设置较小的σ，使随机树大概率向目标扩展，加快算法收敛；当机器人周围障碍物较多时设置较大的σ，使随机树大概率随机扩展，提高路径多样性。按照上述思想，控制概率σ设置为：

(2)

式中，σ∈[0.2,0.8]为控制概率；d为当前位置的障碍物分布；dmax、dmin为相应的最大值和最小值；θd为机器人一步范围内存在障碍物的累计角度范围，单位为 °，依赖机器人探测得到。

2.2 Halton采样与候选点集策略

在标准RRT*算法中，随机点qrandom的产生按照伪随机采样的方式产生，伪随机采样点容易产生聚集、不均匀、不合理等问题。相对而言，Halton采样具有空间分布均匀、覆盖性好等优点。Halton序列是基于质数产生的，以某一质数δ为例，Halton序列第m个数据产生方法为[11]：

步骤1：将m表达为δ进制数，为：

(3)

式中，I为mδ表达为δ进制的位数。

以在[0,1]×[0,1]平面上采样为例，使用随机法和Halton序列在此平面内各采样1000个点，其中Halton序列X轴和Y轴质数分别取为2和3，结果如图5所示。

(a) Halton采样 (b) 随机采样

对比图5a、图5b可知，Halton采样点在空间中的均匀性和覆盖性极好。而随机采样点存在聚集区域和稀疏区域，图5b中使用方框框出了一个聚集区域和两个稀疏区域。聚集区域的出现会使算法重复性搜索同一区域，降低算法效率；稀疏区域的出现使算法难以搜索到该区域，可能难以规划出最优路径。因此，本文使用Halton采样代替随机采样。

在标准RRT*算法中，随机点为惟一可选的扩展方向，为了筛选出最优扩展方向，采样阶段不再产生一个候选点，而是基于Halton序列产生一个候选点集，从候选点集中选择出采样点并扩展。如图6所示，圆圈中的点集为依据Halton序列产生一个候选点集。

图6 候选点集示意图

从候选点集中基于欧式距离和转弯角大小选择出最优采样点，评价函数Q构造为：

(4)

式中，C1=0.3为转角权值；C2=0.7为距离权值；λn为归一化的转角值；An为候选节点qn的转角，定义为矢量qparentqnear与qnearqn之间的夹角，其中qparent为qnear的父节点；Amax、Amin分别为转角最大值和最小值；Dn为归一化的距离；Sn为候选节点qn与目标节点qgoal的距离；Smax、Smin分别为距离最大值和最小值。

最优采样点确定方法为：若qnear与候选节点qn方向一步距离范围内存在障碍物，则评价函数Q直接设置为无穷大；若qnear与候选节点qn方向一步距离范围内不存在障碍物，则计算评价函数Q，选择出Qmin对应的节点为最优采样节点。

2.3 剪枝与平滑

基于RRT*算法规划出的路径存在冗余节点和路径不平滑的问题，因此在规划出初步路径后必须进行剪枝和平滑处理。

剪枝方法可以分为从根部剪起和从叶部剪起两类，根据任务特点，本文使用从叶部剪起的方法。具体操作为：首先，直接连接起点qstart和终点qend，若两者之间无障碍物则路径规划完毕；否则，将连接点前移，判断qstart与qend前一节点之间是否有障碍物，按照上述方法依次将连接点前移，直至找到一个非碰撞连接点；而后，将起点与该点连接，并将该点重置为新起点qstart，重复上述操作，直至与终点相连接，剪枝过程结束。

本文使用三次B样条曲线对剪枝路径进行平滑，N次B样条曲线多项式为：

(5)

式中，P(t)为N次B样条曲线；Pi为控制点；Fi,k(t)为基函数，其表达式为：

(6)

式中，t∈[0,1]；i∈(0,1,…,m)。

当N=3时，可得三次B样条曲线为：

(7)

式中，P0～P3为控制点。

3 仿真验证与分析

3.1 二维空间路径规划

使用仿真的方法对Halton采样-先验RRT*算法的路径规划性能进行验证，仿真环境为Win 10操作系统，处理器为Inter(R) Core(TM) i5-10210U CPU@1.6 GHz 2.11 GHz处理器，内存为16 GB。

首先在40 m×40 m规模的二维空间中进行性能比较，设置一个由不规则障碍物组成的狭窄通道环境，起点设置为(1 m，1 m)位置，终点设置在(40 m，40 m)位置。分别使用标准RRT*算法、文献[12]改进RRT*算法、本文Halton采样-先验RRT*算法规划出最优路径，3种算法各自独立运行50次，首先给出最优路径如图7所示。

(a) 标准RRT*算法 (b) 文献[12]改进RRT*算法

(c) Halton采样-先验RRT*算法

对比图7中3种算法的规划结果可以看出，RRT*算法由于完全随机的扩展和生长方式，使得随机树几乎长满了所有无障碍物区域，才规划出一条相对较优的路径，这意味着标准RRT*算法的规划效率较低。相比较而言，文献[12]改进RRT*算法的扩展节点极大减少，这是因为文献[12]改进RRT*算法中使用了目标偏置策略，在目标牵引作用下使得随机树的扩展节点极大减少。本文Halton采样-先验RRT*算法规划路径的扩展节点最少，这是因为本文不仅使用了目标偏置策略，而且使用了候选点集策略，能够从点集中选择出最优采样点，极大减少了随机树扩展的节点数量。

为了定量地比较3种RRT*算法的路径规划性能，统计50次规划路径的节点扩展数量均值、算法耗时均值、路径长度均值，如表3所示。

表3 路径参数比较

由表3可知，Halton采样-先验RRT*算法的扩展节点数量最少，规划耗时最少，且路径长度均值最小。这是因为本文根据先验知识使用了目标偏置策略，使得树节点在扩展时的方向性和目的性较强；另外，本文还使用了候选点集策略，从候选点集中选择出最优采样点，能够有效减少树节点尝试扩展的数量，因此Halton采样-先验RRT*算法的扩展节点数量最少；也是因为其节点扩展的尝试次数较少，使得算法规划耗时远小于另外两种算法。最后在剪枝和平滑作用下，路径长度也小于另外两种RRT*算法。

3.2 三维空间路径规划

在同样的仿真环境下，设置一个20 m×20 m×20 m的三维地图，机器人起点坐标为(0，0，0)，重点坐标为(20 m，20 m，20 m)。三维环境中设置6个球形障碍物，障碍物半径及位置分布如表4所示。

表4 障碍物尺寸及位置

仍然使用3.1节的3种RRT*算法各自独立规划50次，得到的最优路径如图8所示。

(a) 标准RRT*算法 (b) 文献[12]改进RRT*算法

(c) Halton采样-先验RRT*算法

由图8可以直观看出：标准RRT*算法的扩展节点数最多，这导致标准RRT*算法的规划效率较低；文献[12]改进RRT*算法的扩展节点数量居中，Halton采样-先验RRT*算法规划过程中的扩展节点数最少，说明本文RRT*算法的规划效率最高。将3种RRT*算法在50次规划中的扩展节点数量、规划耗时、路径长度以图形形式给出，结果如图9所示。

(a) 扩展节点数量 (b) 规划耗时

(c) 路径长度

结合图8和图9可以看出：①在路径规划过程中，Halton采样-先验RRT*算法的节点扩展数量最少，且波动极小；②Halton采样-先验RRT*算法的路径规划耗时最小，且远远少于另外两种算法；③从路径长度的角度看，Halton采样-先验RRT*算法规划的路径最短、波动最小，且远小于另外两种算法。上述参数说明，Halton采样-先验RRT*算法的规划性能最佳，且波动最小。这是因为本文使用了目标偏置策略，使节点在扩展时的方向性和目的性较强；另外，本文的候选点集策略，可以从点集中选择出最优采样点，能够有效减少树节点尝试扩展的数量，因此Halton采样-先验RRT*算法的扩展节点数量最少、规划效率最高。最后在剪枝和平滑作用下，路径长度也小于另外两种RRT*算法。

在二维和三维环境下的路径规划结果表明，Halton采样-先验RRT*算法在机器人路径规划中可以减少路径长度、提高规划效率，因此具有一定的优越性。

4 结论

本文研究了移动机器人在二维和三维环境下的路径规划问题，在RRT*算法中添加了基于先验知识的目标偏置策略和基于Halton采样的候选点集策略，提出了基于Halton采样-先验RRT*的路径规划方法。经验证得出以下结论：①基于先验知识的目标偏置策略可以有效减少扩展节点数量，提高规划效率；②候选点集策略可以选取出最优采样点，因此可以减少节点扩展尝试次数；③剪枝和平滑策略可以有效减小路径长度。