基于混合搜索快速步进算法的移动机器人路径规划研究

于晖

【摘 要】本文提出一种混合搜索快速步进算法用于解决移动机器人的路径规划问题。该算法在不损失计算精度的前提下，提高了快速步进算法的实时性。通过实验表明了该算法可以生成一条连续、平滑、最优的路径，并且运行速度非常快，可以进行在线规划。

【关键词】移动机器人；路径规划；快速步进法；混合搜索

【Abstract】In this paper，the Hybrid Search Fast Marching algorithm，which improves the real-time performance of Fast Marching algorithm without losing accuracy，was proposed to resolve mobile robot path planning problem.The simulation showed that the algorithm has the ability to find continuous smooth optimal paths，it rans fast， and it could work for online planning.

【Key words】Mobile robot；Path planning；Fast Marching method；Hybrid search

0 引言

所谓路径规划是指，在具有障碍物的环境中，移动机器人按照某一性能指标（如距离、时间、能量等）搜索一条从起始状态到目标状态的无碰、平滑的最优或近似最优路径[1]。路径规划是移动机器人导航中必不可少的一个重要环节。所规划的路径质量对移动机器人能够安全航行和成功完成任务起到了至关重要的作用。

在移动机器人的导航中，大多数方法都是使用栅格法[2]对机器人工作环境进行分解。每个网格单元可以用二值信息来描述环境（障碍物或自由空间），也可以用一个相关的权值来表示穿过这个区域的代价值。A*算法[3]及其扩展算法D*算法[4]，改进A*算法[5]和D*Lite算法[6]在移动机器人的路径规划中应用广泛，但是这些搜索算法在离散的网格空间中一般会使用4元或者8元的后继节点，这样会限制移动机器人在运动的过程中只能以？仔/2或？仔/4的整数倍进行转向。向量A*算法[7]和Multi-Step A*（MSA*）算法[8]提高了移动机器人航向角的分辨率，但是它们得到的还是一组离散的解，其不能收敛到平滑连续解，并且规划后的路径仍然会产生急转弯。所以通过这些方法得到的是一条次优路径。

与上述基于栅格的路径规划算法相比，虽然本文使用的快速步进（FM）方法也是在离散的网格单元上计算最优路径，但是它使用一阶数值近似来求解程函方程，所以能够得到一条平滑、连续、最优的路径解。

本文提出了一种混合搜索快速步进（HSFM）算法，其在不损失计算精度的前提下提高了FM算法的实时性。本文结构如下：首先介绍FM方法的原理。然后提出一种新的算法——基于混合搜索的快速步进算法。最后给出新算法在路径规划中的应用实例。

1 Fast Marching方法

FM方法是由Sethian首先提出的用来进行图像处理的一种解决波传播问题的水平集方法[9]。像大部分基于网格的搜索算法一样，FM算法的计算复杂度是O（N log（N）），其中N是工作空间中网格的数量。

FM算法是一种连续的广度优先算法。与传统的广度优先算法相同，FM算法的路径规划过程也分为两步：探索过程，即建立整个地图上每个网格顶点的距离函数值；开发过程，即通过所求解到的最优代价值，通过梯度下降法从目标点向起始点回溯形成最优路径。

在自然界中，有一种物理现象与FM算法的探索过程非常相似：光的传播。假设在起始点有一个向四周发射光波的光源，那么光从起始点向目标点传播的路径（即光线轨迹）可以认为是机器人路径规划生成的最优路径。因此我们可以根据光的传播现象来建立距离函数。光在传播的过程中，在某一个瞬时时刻所到达的所有点的轨迹称为波前（形成光波的等相面）计算初至时间T可以用来描述波传播过程中的波前位置。考虑一个二维的光波传播初至时间的问题：

t=T（x，y）

T（x，y）=0（1）

其中，T（x，y）表示在位置（x，y）的初至时间，（x0，y0）是初始位置。

将式（1）两边对t求导，可以得到：

1=·+·（2）

则我们可以将式（2）转换成初至函数T（x，y）的梯度T和机器人速度f两个向量的内积：

=1？圯T·F（x，y，n）=1（3）

其中，n=T/T表示T的等值面在所计算点的向外法向量，F称为速度函数，表示T（x，y）沿着梯度T方向扩散的速度。

假设移动机器人沿着波传播的方向（即T的方向）运动，并且速度大小不变，则F与位置和方向无关（即F（x，y，n）=F=f），并且所規划路径的长度最小问题可以等价于时间最小问题。波传播问题即可转化为求解程函方程：

T==（4）

T（x，y）=0

1.1 迎风策略

FM方法使用迎风策略来估计T（x，y）的梯度T的模：

Ti，j2≈maxDT，-DT，0+maxDT，-DT，0

DT和DT分别为在x方向上的前向和后向差分算子。在y方向上的前向和后向差分算子类似。则程函方程（式4）可以近似的表达为：

maxDT，-DT，0+maxDT，-DT，0=1/F

Sethian[9]已经证明了这种数值方法收敛于正确的连续解。

1.2 FM算法的实现

FM算法的核心思想是使用迎风值来系统地构建T的解，即前端的波只会由T值小的位置向T值更大的方向传播。FM算法将网格点分成三种类型：Dead类型表示此网格点的T值已经计算过并且确定；Open表示此网格点的T值是估计值，没有确定；Far表示此网格点的T值是未知的。Open类型的所有点被存储在一个称为窄带的优先级队列Q中，Q根据网格点的代价值T是按照升序排列。队列顶部的元素代价值最小，其对应的网格点称为trial。FM算法探索过程中的每一次迭代会将trial网格点由Open类型变成Dead类型，然后将它的邻接点的代价值更新，并且将类型为Far的邻接点变为Open。FM算法更新过程的细节请参考文献[9]。

2 HSFM算法

对于移动机器人路径规划来说，计算时间和路径的最优性都是我们需要考虑的评价标准。本小节提出了HSFM算法，能够从一个离散的环境下得到一条连续平滑的最优路径，并且在不损失计算精度的前提下提高了FM算法的实时性。

2.1 启发式方法

启发式方法可以为网格搜索算法的每個节点提供较低的分叉率，因此使用启发式方法的网格搜索算法拥有较佳的计算能力。按照窄带优先队列Q优先级的分配方法，我们将网格搜索算法分为三类：广度优先搜索算法、启发式搜索算法和混合搜索算法[10]。

FM算法与广度优先搜索算法的优先级分配原则相同，算法不借助任何启发信息。FM*[11]算法与启发式搜索算法的优先级分配原则相同，FM*算法将欧几里德启发信息加入评价函数，从而可以缩短搜索过程的时间。

2.2 HSFM算法

HSFM算法与混合搜索算法的优先级分配原则相同，即将Q中e（x）最小的元素设为优先级最高，其中e（x）=？琢v（x）+（1-？琢）h（x，x），v（x）为距离函数在位置x的估计值，h（x，xgoal）为欧几里德启发函数，？琢是一个常数（0.5？燮？琢？燮1）。FM算法和FM*算法都是HSFM算法的一种特殊情况。当？琢=0.5时，HSFM算法就是FM*算法。当？琢=1时，HSFM算法就是FM算法。

为了得到不同？琢值对HSFM算法的计算时间和计算精度的影响，我们适用蒙特卡罗模拟法，对不同？琢值的HSFM算法在1000组随机产生的三维环境下进行仿真实验。每组随机产生的仿真模型包含了地形图、运动物体、障碍物等环境信息。起始点和目标点也是随机的。三维环境的分辨率为200×200×40。

仿真结果如表1所示，其中FMn代表？琢=0.01×n时的HSFM算法。可见FM55的平均路径代价近似与FM算法（即FM100）相等，而平均计算时间却减少38%。本论文后面部分，HSFM算法指？琢=0.55的HSFM算法。所以我们得到以下结论：HSFM算法在不损失FM算法计算精度的前提下，大大提高了实时性，可以在环境发生改变或面对移动障碍物时进行在线规划。

3 HSFM算法应用实例

我们首先给出了HSFM算法和FM算法在二维环境下的比较，如图所示。网格的分辨率为400×400。FM算法的计算时间为382ms，HSFM算法计算的路径代价为FM算法的1.0086倍，而计算时间为207ms，几乎为FM算法的一半。

最后给出了HSFM算法在路径重规划方面的应用实例。在仿真实验中，假设自主水下机器人（AUV）的活动区域为5km×5km×40m，每个网格单元的大小为25m×25m×2m。AUV沿初始航路航行40min17s时，发现右侧新出现一个AUV，其运动轨迹与初始路径在110min左右有重叠区域，也就是说我们的AUV与新出现的AUV有发生碰撞的风险。需要使用HSFM算法对AUV的路径进行重新规划。规划出的新路径如图2所示，计算时间为0.7628s。可见 HSFM算法是理想的在线路径规划算法。

图2 HSFM算法在路径重规划中的应用

【参考文献】

[1]戴博，肖晓明，蔡自兴.移动机器人路径规划技术的研究现状与展望[J].控制工程，2005（3）：198-202.

[2]Metea M，Tsai J.Route planning for intelligent autonomous land vehicles using hierarchical terrain representation[C]// Robotics and Automation Proceedings 1987 IEEE International Conference on：IEEE；1987：1947-52.

[3]LaValle SM.Planning algorithms[M].Cambridge university press，2006.

[4]Stentz A.The focussed D* algorithm for real-time replanning[C]//IJCAI；1995， 1652-9.

[5]李季，孙秀霞.基于改进A-Star算法的无人机航迹规划算法研究[J].兵工学报，2008（7）：788-92.

[6]Koenig S，Likhachev M.Fast replanning for navigation in unknown terrain[J]. Robotics，IEEE Transactions on，2005，21（3）：354-63.

[7]Pivtoraiko M，Kelly A.Generating near minimal spanning control sets for constrained motion planning in discrete state spaces[C]// Intelligent Robots and Systems，2005（IROS 2005） 2005 IEEE/RSJ International Conference on：IEEE； 2005，3231-7.

[8]Wu P-Y，Campbell D，Merz T.Multi-objective four-dimensional vehicle motion planning in large dynamic environments[J].Systems， Man， and Cybernetics，Part B： Cybernetics，IEEE Transactions on，2011，41（3）：621-34.

[9]Sethian J A.Level set methods and fast marching methods：evolving interfaces in computational geometry，uid mechanics，computer vision，and materials science[M] （volume 3）.U.K.：Cambridge university press，1999.

[10]LaValle S M.Planning algorithms，1st ed.U.K.：Cambridge university press， 2006.

[11]Petres C，Pailhas Y，Patron P，et al.Path planning for autonomous underwater vehicles[J].IEEE Transactions on Robotics，2007，23（2）：331-341.

[责任编辑：田吉捷]