基于改进粒子群算法的水下机器人路径规划研究

吕诗为，朱迎谷，卢倪斌，李忻阳，刘海瑞

（上海中车艾森迪海洋装备有限公司，上海 201306）

0 引言

近年来，随着国内外对海洋资源探索的不断深入和对水下作业需求的增长，水下机器人技术呈现出蓬勃发展态势。由于具有高度智能化和自动化的工作特点，水下机器人研究是未来深海探索及资源开发的新兴方向［1］。

路径规划是机器人研究领域的关键技术［2］，对水下机器人来说，路径规划是其能够实现自主作业的首要前提。在复杂的水下三维环境中，机器人不仅要识别障碍物并自主避障，还要考虑自身的能量限制、行动范围和移动距离等其他约束，因此水下机器人的路径规划是典型的非确定性多项式归约难题（nondeterministic polynomial hard problem，NP-Hard）［3］。

针对水下机器人路径规划问题的研究方法有很多［4］，比较典型的有人工势场法［5］、启发式搜索、智能优化算法等。人工势场法通过建立水下机器人与障碍物之间的虚拟势场，从而引导其规划出一条可行路径。王芳等［6］基于障碍物对栅格节点的不同影响，建立改进的人工势场算法，在一定程度上克服了原算法易出现局部极小点的问题。马小轩等［7］通过改进斥力场函数和设置子目标点的方式，进一步优化了算法的避障性能和目标不可达问题。车建涛等［8］利用启发式搜索中的Dijkstra算法，采用多边形拟合和分层多面体拟合的方法对障碍物建模，实现了二维和三维最优路径的规划。随着遗传算法（genetic algorithm，GA）［9］、蚁群算法（ant colony optimization，ACO）［10］等智能优化算法的广泛研究，越来越多的学者也将其应用在水下机器人路径寻优问题上。顾国昌等［11］提出了一种遗传模拟退火算法，该方法基于区域分层模型，解决了大范围海洋环境下的路径规划问题。刘雨青等［12］根据水下机器人的速度和受力情况建立了水下能耗模型，利用改进蚁群算法规划初始路径并用贝塞尔曲线改善路径的平滑性，所提方法具备一定的工程意义。

实际上，机器人在复杂的水下环境中运动受能源、压力、洋流等多方面条件制约。但在以往该领域的路径规划研究中，大多以避障和移动距离作为约束条件，但这些尚不能完全满足实际工况。本文基于改进的粒子群算法，从减少水下机器人能量消耗的角度出发，在避障和距离最短的优化基础上，将艏向角和纵倾角变化以及转向节点间距分布的均匀性纳入优化计算，并采用几何造型方法将计算后的初始路径进行平滑处理，从而实现三维环境下水下机器人的路径寻优。

1 问题建模

1.1 环境模型建立

作业级水下机器人通常被用于水下作业平台检修、管道巡检等，需要面临复杂的水下环境。因此合理的环境地图模型的建立可以帮助水下机器人更快地识别环境要素，是其完成路径规划的前提条件。

为了描述机器人水下作业环境，本文采用函数模拟法［13］建立地形模型。其中，模拟海底或湖泊水底的基准地形数学模型为

式中：x，y——水底平面上的横、纵坐标；H1——水底地形起伏高度；a，b，c，d，e，f，g——常数，可以控制地形特征。

模拟水下障碍的数学模型为

式中：q——障碍山峰个数；xr，yr——障碍山峰最高点处的横、纵坐标；hr——每个障碍山峰的最高值；xsr，ysr——控制山峰坡度的常数；H2——山峰表面任意一点的高度。

在路径规划过程中，判断路径是否与障碍干涉的依据是利用插值法计算路径插值节点（xi，yi）的高程值Hi，并代入到式（1）、式（2）中进行对比。

1.2 优化目标设计

在路径规划中，优化目标能直接影响路径的寻优效果。针对水下机器人的路径规划大多以距离最短作为主要优化目标。但是在实际工作环境中，机器人在水下的运动受多种因素制约，单一的距离最短并不能反映出路径的好坏。考虑水下机器人能量消耗的限制和路径的光滑程度，本文在距离最短以及自主避障的基础上，将机器人本体的运动姿态角度（即艏向角、纵倾角）变化以及路径节点距离分布纳入优化目标的约束中，以多个约束条件的均衡来综合地体现路径对机器人能量消耗的影响。

1.2.1 约束条件

1.2.1.1 路径长度约束

路径长度是路径规划算法效果的直观体现，路径主要由各节点之间的线段Lj接连组成，使这些线段组成的最终路径最短是必不可少的优化方向。Lj具体可由如下公式表示：

式中：（xi，yi，zi）和（xi+1，yi+1，zi+1）分别表示一段路径的前后两个节点的坐标值。

1.2.1.2 节点距离约束

考虑路径的顺滑性，机器人转向节点的分布要尽量做到距离平均，这样一方面可使路径呈现比较合理的曲线，另一方面也可以将转弯角度的累积变化平均分散到每个节点，可避免出现某个转弯角度过大的情况，符合减少能耗的初衷。

1.2.1.3 姿态角度约束

水下机器人运动姿态角度变化对路径的光滑程度有着直接的影响，同时，角度变化大小也反映了推进器消耗能量的多少，若路径中各节点处转向角度过大则意味着推进器要消耗更多的能量。另外，转向角度的突然变大或变小对机器人自身的控制设计而言也是不合理的，一般不希望水下机器人在航行过程中做急转动作。

水下机器人的姿态角度由艏向角、纵倾角、横滚角决定，考虑到横滚运动对路径形状的影响不大，暂时对其忽略。这里用θi来表示路径中i点处的艏向角，用φi来表示路径中i点处的纵倾角。为了方便表示和计算，将路径向xOy平面投影得到艏向角、向yOz平面投影得到纵倾角，二者的表达式为

式中：Pi，XY ——路径中的节点在xOy平面的投影点；Pi，YZ——路径中的i节点在yOz平面的投影点。

1.2.1.4 避障约束处理

在算法寻优过程中，若出现i节点组成的路径与障碍物发生干涉的情况，则采用罚函数法处理，即在计算适应度函数时增加适当的惩罚项，以将此种情况在迭代计算中淘汰。

1.2.2 目标函数

针对以上几种约束情况，为达到优化目标约束条件之间的均衡，实现对水下机器人路径的寻优计算，本文采用线性加权的方式对各约束条件函数进行处理，得到最终的目标函数F表达式为

式中：λ1、λ2、λ3——相应约束的加权系数；P——与障碍干涉时的惩罚因子，通常为较大的常数；f1——路径长度；f2——节点距离分布；f3——姿态角度变化。

路径长度f1表示路径从起点到终点的总距离，主要由路径中若干个节点组成，结合式（3）其设计公式为

式中：α——路径中的节点数量。

节点距离f2考虑了路径中所有节点分布的均匀性，其设计公式为

式中：——路径中各段长度的平均值。

对于姿态角度f3的计算，在式（4）、式（5）中Pi，XY和Pi，YZ分别表示路径中的i节点在xOy平面和yOz平面的投影点，因此θi和φi是相应投影点与其前后两个节点形成的两个向量的夹角，故而得到姿态角度变化的计算公式为

2 改进粒子群算法

2.1 粒子群算法原理

粒子群算法（particle swarm optimization， PSO）［14］根据鸟群捕食行为，用一种无质量的粒子来模拟鸟群中的鸟，通过个体和群体之间的信息交流共享完成捕食（寻优）动作，是一种群智能优化算法。算法中，粒子仅有速度和位置两个属性。速度决定了粒子的搜索快慢，位置代表粒子当前相对于最优解的移动方向。在每一次迭代中，群体中的每个粒子都会找到自己当前的最优位置，并通过相互之间的比较决定出整个群体的最优位置，进而在后面的迭代中所有粒子都会根据自身的最优解和群体最优解调整自己的速度和位置。

假设整个种群中的粒子数量为N，搜索空间的维度为d，那么第m个粒子的位置可表示为Xm=（x1，m，x2，m，…，xd，m），第m个粒子的速度可以表示为Vm=（v1，m，v2，m，…，vd，m）， 其中m=1，2， …，N。在计算过程中，还要保存每个个体的最优解pbest以及群体最优解gbest。第m个粒子根据下面的公式更新自己的速度和位置：

式中：Vm（t）——第m个粒子在第t次迭代时的速度；Xm（t）——粒子在第t次迭代时的位置；r1，r2——［0，1］范围内的随机数；c1，c2——认知系数及社会系数，二者代表了粒子向个体最优位置pm，best和群体最优位置gm，best逼近的趋势，也称加速项权重；w——惯性权重系数，代表了粒子的搜索性能，可体现粒子继承先前速度的能力。

2.2 惯性权重的自适应改进

在粒子的速度更新公式（10）中，惯性权重系数w决定了粒子的搜索能力，w越大则对全局空间的搜索能力越强，w越小则对局部区域的搜索能力越强。因此，w的选取决定了算法的求解性能。在大多应用场景中，w的取值是随着迭代次数线性变化的，而忽略了算法计算过程中的实时求解状况。随着迭代计算的进行，w也应该随着求解情况做出适应性的调整。这里引入一种自适应惯性权重改进方法［15］，以动态计算每次迭代时的w，从而适应当前的求解情况。该方法的计算依据是群体粒子最优适应度与每个粒子最优适应度平均值的比较，从而指引粒子向更优方向前进。自适应惯性权重更新公式如下：

式中：fbest——粒子群体的最优适应度值；fm，best——第t次迭代时每个粒子的最优适应度。

2.3 B样条曲线平滑处理

经过初始计算得到的路径只是由若干转向节点连接而成的分段折线，在转弯拐点处还存在尖角，这并不符合依靠推进器驱动的水下机器人的实际运动路径需求。因此还需要对该初始路径进行平滑处理，使最终的路线是一条平滑连续的曲线［16］。

在几何造型设计中，常用B 样条曲线法对转弯拐点进行平滑处理。根据曲线特征多边形的顶点数，有二次、三次、四次等阶次B 样条曲线形式；又根据待处理折线中的节点区间间距是否均匀，还分为均匀和非均匀B 样条曲线。控制点数量与样条曲线的数量决定了曲线的阶次，本文采用三次均匀B 样条曲线对路径中的拐点进行平滑处理。该曲线的数学表达式为

式中：k——B样条的次数；u——节点向量；n——样条曲线的数量；l——B 样条的序号；Nl，k（u）——k次B样条的基函数；Pl——路径中的控制点。

对于基函数的推导，一般遵循Cox-DeBoor［17-18］递归公式，这里不再赘述。经推导整理后的B样条曲线公式矩阵形式为

2.4 路径求解流程

改进后的粒子群算法（IPSO）求解路径的流程如图1所示，步骤如下：

图1 改进粒子群算法路径规划流程Fig. 1 Flowchart of path planning using the improved particle swarm optimization algorithm

1） 初始化地形参数a，b，c，d，e，f以及粒子群算法基本参数N，c1，c2，w，T，设置目标起点和终点位置；

2） 所有粒子的位置及速度随机初始化，以适当增强算法后续的计算效果；

3） 算法开始迭代，计算每一次迭代中全部粒子的速度Vm（t）和位置Xm（t）以及自适应惯性权重w（t）；

4） 计算当前迭代中每个粒子的适应度值fbest，并与粒子自身历史最优值pbest和群体最优值gbest进行比较，进而得到当前最优适应度的粒子，即当前代数中的最优路径；

5） 当前迭代中的粒子全部计算完毕后，更新每个粒子的最优位置以及群体最优值，得到当前全局最优粒子（即当前最优路径），并开始下一次迭代，重复步骤3）和步骤4），直至迭代完毕；

6） 迭代计算结束，得到全局最优初始路径（即最优粒子），再对该初始路径进行三次均匀B样条平滑处理；

7） 输出得到最终路径。

3 仿真实验与分析

为了验证本文所提方法的可行性与有效性，下面进行仿真实验，并将本文方法（IPSO）与标准粒子群算法（PSO）和在以往水下机器人路径规划中常用的蚁群算法（ACO）进行对比。3 种算法的参数设置如表1 所示，其中γ，β，ρ，Q为ACO算法的控制参数。

表1 算法参数设置Tab.1 Parameter settings of the algorithms

在仿真实验中，模拟场景为机器人入水后自主搜索水下作业目标。将水下环境的地形范围设置为500 m×500 m×500 m，障碍物数量设置为7 个。3 种算法对比过程中起止点均设置为［75，450，440］和［400，50，30］。结合以上参数，将3种算法各运行20次，并统计分析20次计算后的路径效果、求解精度以及收敛性等关键评价指标。

3.1 路径可视化效果分析

图2 中（a）、（b）、（c）分别展示了3 种算法路径规划三维立体图，图3 展示了3 种算法对应路径的俯视图，三角符号表示路径中的转向节点。从图中可以看出，3 种算法相较之下，ACO 的路径效果最差，具体表现为距离过长、角度变化太大以及转向节点间距分布不合理，与理想效果还有一定差距。IPSO 与PSO 两者从路径距离与下降过程的平缓性来说都比较好，但IPSO显示了更强的避障性能以及更优的路线选择。

图2 不同算法路径三维立体图对比Fig.2 Comparison of 3D views of different algorithm paths

图3 不同算法路径俯视图对比Fig. 3 Comparison of top view of different algorithm paths

3.2 能耗分析及求解精度对比

在3 种算法的优化计算对比过程中随机选择一次，其综合能耗随迭代次数的收敛曲线如图4所示。此外，3种算法20次计算后得到的路径总长度、节点距离差异以及姿态角度变化各自的平均值如表2 所示，这些参数也综合影响着机器人在水下的能耗，数值越小代表搜索得到的路径越良好，消耗的能量也就越少。

表2 求解精度对比Tab. 2 Comparison of solution accuracy

图4 算法收敛过程对比Fig. 4 Comparison of algorithm convergence processes

由图4 不难看出，ACO 在迭代过程中过早地陷入了局部最优，导致对问题解的搜索陷入停滞，而PSO和IPSO 还能继续保持向最优解的搜索趋势。另外，IPSO在保持与原PSO收敛速度相当的前提下，在最优解的求解能力和求解质量上有了更进一步的提升。最终IPSO 规划出的路径所消耗的总体能量，相较于ACO的减少了56.6%，相较于PSO的减少了19.3%。

表2列出了各个算法在路径总长度、转向节点分布均匀性及姿态变化方面的目标函数差异，分别由式（7）～式（9）计算得到。其中在路径长度上，IPSO的长度适应度值为736.157 2 m，为三者中最小，分别减少了23.5%和1.9%，说明其能够找到更短的路径；在节点分布方面，ACO 的适应度值为346.375 3 m，远大于PSO 及IPSO，表明其转向节点分布不均匀，效果最差，PSO较之有所改善，但IPSO 的数值最小，比前两者分别下降了94.8%和50.3%，表明效果最好，节点分布更均匀；姿态角度方面，适应度值越小则表明转向角度变化得越小，PSO 的适应度值为239.683 3°，比ACO 的628.377 8°已大幅改善，但IPSO 更小的适应度值185.841 3°显然更具优势。

3.3 算法稳定性分析

前文算例均默认路径的中间转向节点为5 个，为了测试算法在更复杂计算情况下的表现，将路径中间节点数量依次增加，并根据式（6）对比目标函数的计算结果，如表3所列。从表中可以看出，随着节点的增加，各个算法的适应度求解数值都有不同程度的增长。其中ACO 的变化范围最大，表明了求解问题越复杂，性能越差；PSO算法性能较为稳定，没有出现适应度求解数值大幅度变化的情况；IPSO 的效果最好，相比ACO它能够将求解结果浮动减少50%～60%，对比PSO其也能有10%左右程度的改善。显然IPSO的求解结果优于其他算法的，这也体现了该方法在多维度计算情况下较好的稳定性。

表3 不同节点数量适应度求解结果对比Tab. 3 Comparison of fitness solution results of different numbers of node

4 结束语

针对水下机器人的路径规划问题，本文基于函数模拟法构建了水下环境，从能耗的角度分析设计了多种约束条件下的优化目标，并对粒子群算法中的惯性权重引入自适应改进方法，对算法求解的初始路径采用B样条方法进行平滑处理。仿真实验结果显示，本文所提基于改进粒子群算法，在稳定性等方面表现良好，其中在总体能耗表现上相较于标准的粒子群算法降低了56.6%，相较于传统蚁群算法降低了19.3%。这表明其能合理有效地辅助水下机器人规划路径，已达到了减少机器人能量消耗的目标，进而验证了本文方法的有效性和准确性。本文的研究适用于对静态环境进行路径规划，而对于实时的动态环境情况还需要更多考量。后续的研究将考虑水下动态障碍物对路径的影响，从而向着工程化进一步迈进。