基于Fast Marching方法的多目标点路径规划的研究

于晖+王永骥

摘要：目前，水下自主机器鱼已经被应用于对水域多个目标点依次进行水质监测，因此有必要研究多个目标点的路径规划。针对遍历多个目标点的路径规划问题，提出一种Multi-DirectionFastMarching（MDFM）方法和遗传算法相结合的路径规划方法。该方法首先使用MDFM方法对工作站和多个目标点两两之间进行路径规划，然后使用遗传算法规划出遍历所有点的最短路径，最后通过仿真实验验证算法的可行性。

关键词：机器鱼；路径规划；多目标点；Fast Marching

中图分类号：TP242.6 文献标识码：A

1引言

随着当今社会城镇化步伐的加快和区域经济的发展，水质污染状况日趋严重，给自然环境和人类身体健康造成严重的危害。对企业废水、城乡生活污水以及一些河流的定期水质监测成为我们当今环境保护的重要工作。目前我国的水质监测多采用人工操作和远程监测数据采集系统，监测范围难以满足水质监测的需要。随着仿生机器鱼技术的日趋成熟和性能的不断提高，其机动灵活，监测范围大，成本低，对水环境的影响小等优势使得将它用于水质污染监测有着得天独厚的条件。Roberr Bogue对用于环境监测的机器人的发展现状进行了综述，文章的讨论包括了水下机器人，陆地机器人和空中机器人。英国牛津大学研究了一种水下移动机器人网络，用来监测核废料蓄水池是否有核泄漏。2009年欧洲的SHOAL项目计划开发一种机器鱼编队用于在港口或其他水域进行水质监测和污染源搜索。目前SHOAL已经开发成功并且投产。在国内，华中科技大学研究了一种用于水质监测的自主式水下机器鱼（Autono-mousUnderwaterRoboticFish，AURF），可以在复杂的水下环境进行巡游，检测水质并绘制3D的水质分析图，并且可以对水域中的多个排污口依次进行水质监测。

在移动机器人相关技术的研究中，导航技术是其核心，而路径规划是自主式移动机器人导航的基本环节之一。在移动机器人的导航中，大多数方法都是使用栅格法对机器人工作环境进行分解。每个网格单元可以用二值信息来描述环境（障碍物或自由空间），也可以用一个相关的权值来表示穿过这个区域的代价值。A*算法是非常普及的基于栅格的路径规划方法。它在求解从起始位置到目标位置最短路径时进行启发式的搜索，所以它对从单个点到目标点进行路径规划是非常有效的。基于栅格法的搜索算法在离散的网格空间中一般会使用4元或者8元的后继节点，所以规划后得到的路径会出现急转弯，并且会限制移动机器人在运动的过程中只能以π/2或π/4的整数倍进行转向。当移动机器人沿着规划好的路径运动，它会停止，改变下一步的航向，然后再加速。这样会浪费移动机器人的燃料和时问。所以通过这些方法得到的路径是一条次优路径。Mihail提出一种向量A*算法，它是A*算法的一种扩展算法。向量A*算法的后继节点有16个，其规划后的路径可以让移动机器人的航向角分辨率达到22.5°，提高了算法的最优性和完备性。Wu针对无人机的运动规划提出了Multi-StepA*（MSA*）算法，不仅其规划出的路径提高了移动机器人航向角的分辨率，而且减小了算法的复杂度，提高了算法的运算效率。但是向量A*算法和MSA*算法得到的还是一组离散的解，其不能收敛到平滑连续解，并且规划后的路径仍然会产生急转弯。

目前对于移动机器人路径规划的研究都集中于两点之间最优路径搜索的情况，而用于多目标点路径规划的旅行商（TSP）问题是在已知两两点之问的最短路径和路径代价的基础上，规划出一条最短路径，其能够遍历所有的目标点一次，然后回到出发点。根据AURF进行多目标点水质监测的任务，我们对路径规划提出了如下要求：1）AURF从操作站出发，遍历所有的监测点并获取当地的水质数据后，返回操作站；2）工作环境已知，但操作站和所有监测点两两之间的最优路径和路径代价均未知，需要首先进行规划；3）得到遍历所有监测点一次并回到操作站的最短路径。与两点间路径规划和TSP问题相比，多目标点的路径规划问题更为复杂。本文提出了一种Multi-DirectionFastMarching（MDFM）方法可以同时对多个目标点两两之间进行路径规划，并结合遗传算法（GA）规划出遍历所有目标点的最短路径。

2Fast Marching方法

所谓路径规划是指，在具有障碍物的环境中，移动机器人按照某一性能指标（如距离、时间、能量等）搜索一条从起始状态到目标状态的无碰、平滑的最优或近似最优路径。基于栅格法的路径规划方法通常分为两步：探索过程，即建立整个地图上每个网格的最优代价值（距离函数）；开发过程，即通过所求解的最优代价值，从目标点向起始点回溯形成最优路径。

探索过程与光波的传播过程非常相似。假设在起始点有一个向四周发射光波的光源，光从光源到达目标点的路径可以认为是路径规划的最优路径，所以我们可以根据光的传播来建立距离函数。光在传播的过程中，在某一个瞬时时刻所到达的所有点的轨迹称为波前。计算初至时间T可以用来描述波传播过程中的波前位置。考虑一个二维的光波传播初至时间的问题：

t=T（x，y）

T（x₀，y₀）=0 （1）

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

将式（1）两边对t求导，则可以得到初至函数T（x，y）的梯度▽T和机器人速度f两个向量的内积：

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

Fast Marching（FM）方法是Sethian首先提出的用来进行图像处理的一种解决波传播问题的水平集方法。像大部分的栅格搜索算法一样，FM算法的计算复杂度是O（Nlog（N）），其中N是工作空间中网格的数量。在进行FM方法的整个过程中，速度函数F始终不会改变符号，即波前总是朝着一个方向在扩散，也就是说，当F>0时，波前总是朝外在扩散；反之亦然。这说明波前只会经过工作空间中的每个网格点一次。当没有环境力的作用下，移动机器人可以沿着波传播的方向（即▽T的方向）运动，所以F（x，y，n）=f（x，y）。本文假设AURF在执行任务的过程中速度大小不变，则F与位置和方向无关（即F（x，y，n）=F=f），并且所规划路径的长度最小问题可以等价于时间最小问题。波传播问题即可转化为求解程函方程：

2.1

迎风策略

FM算法使用一阶数值近似（前向差分算子和后向差分算子）来求解程函方程。假设一个方程T在网格点（i，j）的值为T_i，j=T（x_i，j），其中网格间距为h。在x方向上的前向差分算子为D_i，j^+x=（u_i+1-u_i，j）/h，后向差分算子为D_i，j^-x=（u_i，j-u_i-1，j）/h。在y方向上的前向和后向差分算子类似。

FM使用迎风策略来估计T（x，y）的梯度▽T

其中τ_i，j=τ（x_i，j）。

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

2.2Fast Marching算法的实现

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

3MDFM方法

目前，FM方法已经直接用于解决移动机器人的路径规划问题，但是这些问题都是两点之问的路径规划。而对于多点之问的路径规划问题，普通的Fast Marching算法需要逐一计算每对目标点之间的最优路径。本节提出一种新的算法，可以同时计算多目标点之间的最优路径，节约了计算时间。

考虑在区域Ω中，已知目标点集合O，解决多目标点路径规划问题的其中一个关键工作就是找到每对目标点之间的最优路径。在各向同性的工作环境中，任意两个目标点之间的往返路径（路径的轨迹和代价）相同。本节提出的Multi-DirectionFastMarching（MDFM），可以在各项同性的环境下快速找到每对目标点之间的最优路径。

目前两点之间最优路径搜索算法主要分为三类：前向路径搜索算法，指从起始点x_s创建搜索树，到达目标点结束搜索；后向路径搜索算法，指从目标点x_g创建搜索树，到达起始点结束搜索；双向路径搜索算法，指从起始点x_s和目标点x_g同时创建搜索树，两个搜索树相遇时搜索结束。双向Fast Marching（BDFM）方法是MDFM方法的一种特殊情况。MDFM算法的搜索方向从多个目标点开始，是一种同时在多个目标点对之间进行路径规划的算法。

3.1MDFM算法的执行过程

假设在Ω中有M（M≥2）个目标点（O₁，O₂，…，O_M）构成的集合O，每个目标点x_i（1≤i≤M）对应Ω上的一个网格点。在计算所有目标点对之问的最优路径和路径代价时，MDFM会在M个目标点上同时创建搜索树，并使用Fast Marching方法进行搜索。当所有的搜索树都相交时，搜索结束。

MDFM算法的执行过程如下：

初始化：将每一个目标点对应的搜索树初始化，而这些目标点为对应搜索树所构建的搜索树的起始点。

开始循环：

比较每个搜索树上Fast Marching方法的tri-al节点的估计值，选择估计值为最小的trial节点所对应的搜索树继续运行，而其它的搜索树则挂起等待。

当目标点O_i对应的Fast Marching方法FMi构建的搜索树tree_i与另一个目标点O_i对应的Fast Marching方法FM_j构建的搜索树tree_i相交于节点x′，则O_i与O_j之间的路径代价为x′在FM_i和FM_i距离函数值之和。

当FMi构建的搜索树与所有其它的搜索树都相交，则下一次循环时终止FM_i的所有操作。

当所有的搜索树都相互相交时，终止循环。

当目标点Q_i与目标点Q_j的搜索树相交于网格点x′，则Q_i和O_j之间的路径代价值D=T_i（x′）+T_i（x′）（T_i（x′）表示x′在FM_i的距离函数值），最优路径可以通过x′分别向O_i和O_j回溯得到。

因为Fas tMarching方法的计算复杂度为O（Nlog（N）），所以MDFM算法的计算复杂度为O（M/2·N·logN），其中M是目标点总数，N是网格上的网格点总数。如果使用Fast Marching方法对单个目标点进行路径规划，那么需要使用M-1次才能够得到每对目标点的最优路径，这种方法的总的计算复杂度为O（（M-1）·N·logN）。显然，当M≥2时，MDFM算法比后一种方法的运行速度要快，并且随着M的增大，MDFM算法的相对计算时间越小。

3.2MDFM算法的应用

MDFM算法分别被应用在两个目标点和多个目标点的实例中。

1）两个目标点：如图1所示，从Start Pointl和Start Point2产生一个个前向波不断向外扩散，在同一水平集上的点代价值相等。当扩散到x′处时，两个起始点的前向波相交，算法结束。最后返回两目标点之间路径代价，并形成最优路径。

2）多个目标点：如图2所示，从多个目标点同时产生前向波不断向外扩散，当Start Pointi的前向波与其它几个目标点的前向波都相交时，StartPointi停止算法迭代，不再向外扩散。当所有目标点的前向波都两两相交时，算法结束。返回两两目标点之间的路径代价，并形成最优路径。

4多目标点的路径规划

通过上一节MDFM方法得到的结果，我们可以将多目标点的路径规划问题转换为TSP问题。TSP问题的历史很久，最早的描述是1759年欧拉研究的骑士周游问题。目前求解TSP问题的算法主要有经典的确定性算法，包括动态规划算法，分支界限法等，和现代流行的智能算法，包括遗传算法，粒子群算法，蚁群算法和模拟退火算法等。由于这些算法各有优缺点，许多学者通过将各种算法相结合进行混合优化，取长补短。由于遗传算法结构简单，易于实现，能够并行化，并且具有强鲁棒性和全局搜索能力，本文选用遗传算法来搜索能够遍历多目标点的最优路径。

遗传算法是计算机科学人工智能领域中用于解决最优化的一种搜索启发式算法，是进化算法的一种。这种启发式通常用来生成有用的解决方案来优化和搜索问题。由于篇幅的限制，遗传算法的详细介绍请参考文献。

4.1实验的建立

在工作空间C（如图7所示）中，AURF需要从工作站出发，遍历所有10个目标点并进行水质监测，完成任务后返回工作站。多目标路径规划的任务就是找到一条能够遍历所有目标点并返回工作站的最短路径。

4.2实验结果

通过使用MDFM和GA结合的方法，我们得到了一条无碰平滑最优的路径，能够遍历所有10个目标点并返回工作站（如图3）。在一个分辨率为400*400的工作空问中，整个算法所使用的时间为8.857s。

5结论

本文提出了一种MDFM方法和GA相结合的方法，用来解决多目标点路径规划的问题。其中MDFM方法是FM方法的一种扩展，它继承了FM方法计算快速，所求的解可以收敛到连续解等优点，并且在处理多目标点两两之间最优路径和路径代价时计算时间复杂度小。因此在AURF执行任务的过程中，使用MDFM方法也可以进行在线路径的重规划。