李明鑫，严华

（四川大学电子信息学院，成都 610041）

0 引言

路径规划一直以来都是移动智能机器人研究领域的研究热点，其任务是在地图信息已知或未知的环境中，依据时间最优、能耗最低、距离最短等一条或多条评价指标规划一条从起始位置到目标位置的无碰撞路径［1］。移动机器人广泛应用于家居、农业、医疗、工业以及军事等方面，而路径规划是移动机器人研究的重要一环，开展对路径规划的研究具有一定的现实意义。

数十年来，国内外学者对路径规划进行了大量研究，并提出了很多行之有效的路径规划算法。路径规划算法按算法策略可分为以下三类［2］：①启发式算法；②仿生算法；③基于概率的算法。启发式算法主要有Dijkstra 算法［3］、Greedy Best-First-Search 算法［4］和A*算法［5］。Dijkstra 算法是一种最短路径算法，但是搜索的区域较大，时间复杂度较高。Greedy Best-First-Search 算法搜索的区域较小，但可能被“欺骗”到“C 型”障碍物区域内部，且不能保证求得最短路径。A*算法结合了Dijkstra 和Greedy Best-First-Search 算法，在保证可以获得最短路径的同时具有较低的时间复杂度。仿生算法主要有遗传算法［6］（ge⁃netic algorithms）、粒子群算法（particle swarm opti⁃mization）和蚁群算法［7］（ant colony optimization）。遗传算法建立在自然选择和遗传学的基础上，吴晓涛和孙增圻［8］将遗传算法应用于路径规划问题。粒子群的概念来自于对简化的社会系统的模拟，该系统试图通过模拟一群鸟类的觅食运动进行寻优［9］，张万绪等人［10］将栅格法与粒子群优化结合用于路径规划算法。蚁群算法通过模仿蚁群根据其属地以及食物来源的信息寻找路径的行为进行寻优，朱庆保和张玉兰［11］将蚁群算法应用于机器人路径规划。基于概率的算法有概率路线图［12］（probabilistic roadmaps）算法和快速搜索随机树［13］（rapidly-exploring random tree）算法。概率路线图算法首先在自由空间随机取点并建图，将地图简化，然后使用A*等搜索算法进行路径规划。快速搜索随机树（RRT）算法是基于随机采样的路径规划算法，相对于其它算法，其优势在于可以将非完整约束考虑在算法内部，从而不必考虑复杂的运动学约束。但快速搜索随机树算法本身存在一定的缺陷，其搜索是“漫无目的”的，导致寻找可行解时需要迭代的次数较多。Goal-bias RRT［14］算法针对该问题进行了优化，在迭代求解过程中，有一定的概率引导快速搜索随机树向目标位置生长，使搜索具有了一定的“目的性”。但Goal-bias RRT 算法中提出的概率是固定值，适应性较差。针对上述问题，本文通过计算地图中障碍物的密度、方差等统计信息，对障碍物的分布情况进行量化，使用量化值设置上述概率，使该概率能适应不同地图中障碍物分布情况，更好地引导快速搜索随机树生长，达到减少RRT 算法迭代次数的目的。

1 算法描述

1.1 地图表示

为方便地表示地图，将二维平面划分为网格，如图1 所示。在图1 中，左上角为坐标原点，向右为s轴正方向，向下为t轴正方向。灰色区域为自由空间，黑色区域为障碍物。程序中使用二维数组A存储地图，0 表示自由空间，1 表示障碍物。使用网格表示地图，可以方便地判断某个坐标点是否处于障碍物区域。例如，网格大小为10×10，点的坐标为（67，78），可计算得到对应的网格坐标为（7，8），然后通过查询二维数组A即可判断该点是否处于障碍物中。同样大小的地图，网格越小可以将地图表示的越精细，随之而来的是计算量的增加。应根据实际需求进行权衡，设置网格大小。

图1 地图的表示

1.2 RRT和Goal-bias RRT算法描述

RRT 算法的任务是在给定的有限空间X中，找出一条从起点xinit⊂X到终点xgoal⊂X的路径，同时要求该路径避开障碍物Xobs⊂X。

RRT算法描述如下：

算法1RRT算法描述

GENERATE_RRT（xinit，K，Δt）

1T.init（xinit）；

2 fork=1 toKdo

3 if（||xnew-xgoal||

4 break；

5xrand←RANDOM_STATE（）；

6xnear←NEAREST_NEIGHBOR（xrand，T）；

7u←SELECT_INPUT（xrand，xnear）；

8 xnew←NEW_STATE（xnear，u，Δt）；

9 if（judge（xnew）==false）

10 continue；

11T.add_vertex（xnew）；

12T.add_edge（xnear，xnew，u）；

13 returnT

初始化时将xinit加入RRT 结果集T，经过最多K次迭代，若成功求解，则返回结果，否则求解失败。其中K是人为设定的预期最多的迭代次数，防止问题无解造成结果天然地不收敛，进而导致算法无法停止。在每次迭代中，首先验证是否求解完成，完成则停止迭代并返回结果，否则继续迭代。迭代时首先RANDOM_STATE()在给定的有限空间X中选择随机点xrand；然后NEAREST_NEIGHBOR(xrand,T) 从T中 选 择 距 离xrand最近的点xnear；然后SELECT_INPUT(xrand,xnear)和NEW_STATE(xnear,u,Δt) 产生一个新的点xnew；最终judge(xnew)判断xnew是否满足非完整约束以及xnear到xnew的路径上是否存在障碍物Xobs，若存在障碍物则放弃点xnew，否则将xnew以及xnear与xnew形成的边加入结果集T。

RRT 算法中RANDOM_STATE（）函数是从有限空间X 中随机选取一个点xrand作为每次迭代的目标点，该搜索算法是漫无目的的，效率较低。为了解决该问题，LaValle 和Kuffner 提出了Goalbias RRT 算法，对RANDOM_STATE（）函数进行了优化，通过设定一个概率值p，每次迭代以概率p选中xgoal⊂X作为该次迭代的目标点，以概率1-p选择随机点作为该次迭代的目标点，增加了算法一定的目的性。

1.3 基于地图统计信息的优化RRT算法描述

Goal-bias RRT 算法未考虑障碍物的分布情况，而是设定一个固定的概率值p，不能适应任意的地图。基于地图统计信息的优化RRT 算法对RANDOM_STATE（）函数进行了进一步优化，通过计算障碍物的密度以及均值和方差，进而按照一定地策略利用上述统计信息计算出一个引导概率值p，一定程度上增加了对不同地图的适应性。为了使得概率值p反映对路径求解有较大影响的区域的障碍物的分布情况，计算障碍物密度以及均值和方差时，只考虑以xinit和xgoal为对角线的矩形内部的障碍物，而认为该区域以外的障碍物对路径求解影响是较小的。

障碍物密度反映了障碍物的数量，障碍物越多则从xinit到xgoal前进时遇到障碍物的概率越大，故障碍物的密度与p值负相关。对于二维空间X，分为两个维度独立地考察障碍物的均值和方差。若s 轴和t 轴的障碍物分布的均值越接近xinit和xgoal在两个轴上的中点，从xinit到xgoal前进时遇到障碍物的概率越大，故障碍物均值与中点的接近程度与p值负相关。障碍物的方差越大，说明障碍物分布越散乱，阻碍xinit到xgoal的路径的概率越大，故障碍物的方差与p值负相关。具体计算方法见式（1）—（5）。

令denseCnt表示障碍物的格子数，totalCnt表示总体的格子数，则障碍物密度为：

对于每个维度上障碍物均值与xinit和xgoal中点的接近程度，以s 轴为例，首先求得以xinit和xgoal为对角线的矩形以内的障碍物在s轴的均值为aves，则在s 轴上障碍物的均值与中点的接近程度可以由式（2）衡量。

计算可知，式（2）的最大值在aves=(xinits+xgoals)/2 处取得，即最大值在中点处取得，而越偏离中点则值越小，故CloseWithCenter可以正确反映均值与中点的接近程度。t轴同理。

使用方差计算公式分别计算两个维度的方差Vars和Vart即可。

对于CloseWithCenter和Var，由于其取值会大于1，为使其值分布到0-1 之间，需要使用式（3）和（4）进行归一化处理。以s轴为例，t轴同理。

在上文的分析中表明，障碍物密度、均值与中点的接近程度和方差三项指标均与引导概率p呈负相关。但上述三项指标与p的相关性不同，故需要增加系数以区分相关性。实验表明，使用式（5）计算引导概率p效果较好。

求得引导概率p后，将算法1 中算法的步骤5使用算法2代替，其中random（）函数产生一个0～1之间的随机数。

算法2基于地图统计信息的优化RRT 算法中选择目标点的算法

2 实验验证

2.1 实验环境

在Linux 平台使用Qt5.5 编写了交互式GUI 程序进行实验验证，该程序可以方便地绘制地图以及使用提出的算法、RRT算法和Goal-bias RRT 算法进行路径规划并显示迭代次数和路径长度。使用10幅地图进行实验，记录了本文提出的算法求得的各项统计信息以及引导概率，并对比了本文提出的算法、RRT算法和Goal-bias RRT 算法的迭代次数和路径长度。

实验所使用的地图如图2 所示，地图的大小为900×600，网格大小为30×30。第一行从左到右编号为1—5，第二行从左到右编号为6—10。图中左上角红色的点为起点xinit，右下角绿色的点为终点xgoal，黑色为障碍物Xobs。

图2 实验使用地图

地图大小为900×600，网格大小为30×30。第一行从左到右编号为1—5，第二行从左到右编号为6—10

2.2 统计信息和引导概率

表1 显示了本文提出的算法对图2 中的10 幅地图求得的各项统计信息以及引导概率。

从表1 中可以看出，地图中障碍物分布情况越复杂，求得的引导概率值p越小，且p值之间差异较大，较为均匀地分布在0～1 之间，符合1.3中的理论分析。

表1 对10幅地图求得的统计信息以及引导概率

2.3 迭代次数和路径长度

使用图2中的10幅地图分别做20次实验，使用本文提出的算法、RRT算法和Goal-bias RRT 算法进行路径规划。对于三种算法，同一幅地图保证实验中起点xinit和终点xgoal保持不变，记录迭代次数和路径长度。实验结果的算术平均值见表2。

从表2 可以看出，在迭代次数方面，本文提出的算法相对于RRT 算法有较大的优势，相对于Goal-bias RRT 算法也有一定的优势；在路径长度方面，本文提出的算法相对于RRT 算法和Goalbias RRT 算法都有所缩短。迭代次数的减小使得路径规划算法时间复杂度有所降低，路径长度的减小使得机器人移动的时间有所减少。

表2 迭代次数和路径长度结果对比

3 结语

本文提出了一种基于地图统计信息的优化RRT 算法，该算法通过计算地图中障碍物的统计信息，量化地图中障碍物的分布情况，进而得到能够适应不同地图的搜索引导概率，以决定每步迭代向终点方向步进的概率。从实验结果来看，在迭代次数和路径长度方面，本文提出的算法要优于RRT算法和Goal-bias RRT算法。