大型自主水下机器人全局静态与局部动态融合路径规划方法

梁世勋，丁宁宁，刘 健

(1.中国科学院沈阳自动化研究所机器人学国家重点实验室，辽宁 沈阳 110016；2.中国科学院机器人与智能制造创新研究院，辽宁 沈阳 110169；3.中国科学院大学，北京 100049)

0 引 言

深入探索海洋需要越来越多的水下设备部署。当前以蓄电池为动力源的中小型自主水下机器人，续航能力普遍在10～40 h 之间。大型自主水下机器人可携带小型水下设备进行部署、回收；建立水下移动基站，为小型机器人补充能源；能够同时承担多种复合任务，水下设备的作业时间与作业范围能够大大增加。未来大规模水下机器人联网、协同对于高续航的大型自主水下机器人的安全性与自主性要求越来越高。机器人尺寸、重量的增加，其操纵性、机动性必然随之降低，灵活度势必减弱，这些问题使得大型机器人的环境模型更加复杂、机动性约束更加严格、路径选择更加困难，因此需要求解能力更强的全局静态与局部动态路径规划算法。

粒子群优化（PSO）优化算法具有算法简单、收敛迅速等优点，但各粒子间缺少信息交流，“搜索”能力与“收敛”能力难以完美平衡，局部最优解的“陷阱”难以完全避开。在利用PSO 算法规划全局路径时，大量文献在粒子群算法中，通过借鉴其他算法进行融合改进，并提高了粒子群算法的性能，取得良好效果。在全局静态路径规划研究中，文献[1]结合随机采样与均匀变异的方法来更新粒子，在高机动救援机器人的路径规划中生成了高质量的最优路径，但在保证收敛的前提下，其搜索能力仍需提高以保证机器人获取更大的安全活动空间；文献[2]提出了环境选择与匹配选择策略，在迭代过程中逐渐减少随机性，收敛速度大大提高，但其落入局部最优的可能性同样增大；文献[3]通过引入模拟退火算法，利用退火算法暂时接受少许劣质解的特性,优化全局搜索能力，提高局部搜索精度，但算法参数量的增加使得计算量、复杂性也同样增加；文献[4]通过差分进化过程，借鉴遗传算法的交叉变异过程，于粒子群算法中生成新的粒子，增加粒子群的信息交互能力，但其过多的变异增加了不必要的计算量；文献[5]在PSO 算法中引入同性因子，结合鱼群算法的“跳跃”过程提升了算法的求解能力，较好平衡了搜索与收敛能力，满足AUV 在复杂海域航行时的全局路径规划需求，但其过于依赖先验环境，对于实时环境信息融合海图后的动态环境模型缺少验证；在动态规划研究中，文献[6]提出基于改进的双向快速扩展随机搜索树（RRT）算法，优化搜索树之间的连接方式，并设计新的动态步长策略，能够较优规划局部避障路径，但其算法随机性强没有得到改善，路径的平滑连接依旧困难；文献[7]通过基于全局路径规划的引导，通过障碍物运动预测与滚动优化方式进行动态避障，但其对规划结果过于依赖障碍运动预测方式，且存在规划路径远离既定路线的可能；文献[8]改进了速度障碍法并用于动态避障,通过碰撞与避障的时间判定避障动作的始末时间,但其对速度条件过分依赖，如若障碍物速度过慢，可能发生碰撞危险。

基于上述问题，考虑实际海域环境，构建海域环境模型、目标函数以及优化约束条件，通过目标函数加权方式，将多目标函数转化为单目标函数，并通过调整加权权重突出机器人对各目标函数的重视程度；提高算法求解和收敛能力对加权后目标函数优化，得到最优或次优解，从而完成全局静态与局部动态相结合的路径规划任务。

1 基于改进PSO 算法的全局静态规划

1.1 障碍物处理与极坐标建立

真实海洋环境障碍大小形状各异，为缩短全局规划时间而简化障碍模型。模型简化前后满足以下三点：障碍物“阻挡”效果不发生改变；尽量减少可行路径损失以及方便计算，提高计算效率。分析海岸、船只、浮标、游鱼、暗礁以及漂浮物等动、静态障碍，选取矩形或圆形的障碍物包围方式。对于长宽比小于1.5 的障碍物采用圆形包围；长宽比大于1.5 的障碍物采用有向包围盒（OBB，即具有方向性的矩形）包围。考虑大型水下机器人航行状态调整缓慢，操纵性与机动性较弱，障碍模型外侧通过增加圆形直径或增加矩形边长的方式，膨胀一个安全阈值ε，模型与安全阈值范围内区域皆设为危险区域。安全阈值的取值与机器人尺寸正相关，与机动性能负相关。

以起始点为极点，起始点与终止点连线为极轴建立极坐标系，顺时针方向为负，逆时针方向为正。以极点为圆心，圆形障碍物的极径为半径建立路径同心圆。规划的路径用路径集合点P=P0,P1,P2,···,Pi,Pi+1表示。P0,Pi+1分 别代表起始点和终止点，i为路径节点的数量，与路径同心圆数量相同。路径节点位于路径同心圆上，路径节点信息用极径Pm和极角Pa表示。

矩形障碍物位置由4 个顶点确定，全部顶点建立路径同心圆会增加计算量，降低计算效率，选择2 个顶点建立路径同心圆既能较好反应障碍信息，又能减少计算量。根据矩形顶点所在位置的极径长短有2 种选取方式：极径最长和最短的2 个顶点；极径次长和次短的2 点。

如图1 所示，选取极径最长和最短的顶点会出现路径节点之间有障碍物“突出”现象。靠近障碍物调整航向角是对机器人回转性能的巨大挑战，在严格的机动性约束下大型水下机器人不具备此种回转条件的能力。因此，采取图2 所示的方式建立路径同心圆。

图1 极径最长和最短顶点建立路径同心圆Fig.1 Path concentric circle established by the longest and shortest vertices of polar radius

图2 极径次长和次短顶点建立路径同心圆Fig.2 Path concentric circle established by the second longest and the second shortest vertices of polar radius

由起始点、障碍物信息建立极坐标，采用弧形虚线表示路径同心圆，同心圆圆心为起始点；圆形、矩形实线表示障碍物；圆形、矩形虚线代表障碍物膨胀安全阈值 ε后的边界。极坐标系下全局静态环境信息如图3 所示，其中path1，path2 表示路径同心圆上存在的路径节点，在路径节点集合中将以P1，P2表示。

图3 极坐标系下的障碍物模型Fig.3 Obstacle model in the polar coordinate system

1.2 适应度函数

在自主水下机器人航行过程中，在线获取环境信息有限，由先验知识规划的路径信息十分重要。通常为得到全局最优路径，机器人会根据障碍物模型构建目标函数，将规划问题转化为多目标函数优化问题[9]。在全局路径规划中，需要在工作空间中搜索出一条从起始点到终止点的最优路径。最优路径不仅满足机器人自身速度、加速度以及角速度约束，同时还在满足安全条件下保证路径最短。执行层中的避障问题可以视为机器人在可行区域内的优化搜索，通过多目标函数优化求解找到可行区域的解。

建立路径长度函数J1计算机器人从起始点Start 到终止点End 的路径长度，如下式：

其中，L（.，.）为两路径点的欧式距离；

建立路径可行度函数J2，表示航行中路径可行程度，路径可行度函数J2如下式：

为防止出现路径点位于障碍内部或阈值区域内的情况，设立危险度函数D(i)，如下式：

式中：D(i)表示第i-1 个路径点与第i个路径点之间的危险度；OB表示危险区合集。若路径与危险区相交，则危险度为无穷大；若不相交，则危险度为0。

建立回转可行度函数J3，表示机器人航行过程中的回转角度可行性，如下式：

建立回转评估函数W(i)如下式：

其中：ω(i)代 表机器人于第i个路径点处回转角度；ωl(i)为机器人最大回转角度。由于自主水下机器人无法迅速调整首向，因此用J3约束机器人的回转角度。避免出现回转角度过大，机动性、操纵性不能满足的情况。

粒子群算法通过适应度函数来判别粒子优劣，是逼近最优解的决定性因素，适应度函数的选择将直接影响算法性能。在静态障碍环境中，寻求与障碍物不相交、路径长度最短且满足最大回转角限制的可行路径。

采用线性加权方法将J1，J2，J3转化为单目标函数，建立适应度函数如下式：

若路径符合航行条件，则适应度为路径总和；若路径不符合航行条件，则适应度为无穷大。为比较粒子优越性，制定如下粒子择优条件：

1）如Fit(i)＜Fit(j)，那么粒子i优于粒子j；

2）如Fit(i)=Fit(j),ω(i)＜ ω(j)，则粒子i优于粒子j。

1.3 改进粒子群算法

粒子群算法以其简单、参数少、快速收敛等特性被广泛应用在机器人各领域[10]。PSO 算法是一种随机搜索算法，该算法设计无质量的粒子群来模拟鸟群，鸟群中的粒子仅有2 个属性：速度 V和 位置 X，其基本更新公式[11]如下式：

学习因子c1，c2，惯性权重w等参数的选取和控制在一定程度上决定PSO 算法的性能。学习因子是粒子具备自我经验总结和学习能力的体现，c1保持较大，粒子搜索范围大但收敛慢；c2保持较大，能够学习群体经验使收敛迅速但搜索范围小[12]。算法需要在前期大范围搜索，后期快速收敛，因此需要在搜索前期c1较大，搜索后期c2较大，c1，c2由下式表示：

其中，run表 示当前迭代次数；runmax表示最大迭代次数；r1，r2为在0 和1 之间均匀分布的随机数。

惯性权重w影响全局与局部寻优能力，合适的w取值能够平衡全局与局部搜索能力，以实现算法初期搜索能力强，后期收敛快，以较少的迭代次数得到最优解。因此采用线型递减惯性权重如下式：

其中：wmax为最大惯性权重；wmin为最小惯性权重；run是当前迭代次数；runmax为迭代最大次数。

粒子最终收敛位置由群体最佳位置和个体历史最佳位置决定[13]。本文定义：如果粒子群经过若干代更新后，所有粒子都未发现比历史最优更优的值，则算法处于轻度收敛状态。引入遗传算法“变异”过程使得最佳位置发生“变异”。由于粒子群陷入局部“陷阱”是逐渐形成的，因此需要结合求解阶段对群体最佳位置进行变动，即采取不同的变异方式引导种群向有利方向继续搜索。

定义粒子k到当代最佳位置的维度距离：

其中：d为算法维度，在全局静态规划中为路径节点i；维度距离可将粒子聚集度反映到一维区间比较。如果算法适应度值较理想，且粒子聚集度较高，说明在收敛过程中尚未发现更优位置，此时变异应在小范围内进行，以免重复搜索过程；如若适应度不高，或者粒子聚集度较低，说明尚在搜索过程，且对空间搜索不充分，因此变异在大范围内进行，提高全局搜索能力，避免陷入局部极值。当算法处于轻度收敛状态时，变异后的最佳位置由原来最佳位置与变异范围共同决定，且变异范围根据收敛原因不断调整。

定义 γ (t)为最佳位置变异范围；

式中：rand() 表 示均匀分布在 (0，1)区间上的随机数；gb(t) 表示原来最佳位置。通过最佳适应度Fitbest来控制变异范围的大小；最佳位置越接近理想值，适应度值越小，位置变异程度也越小，反之则越大。

1.4 改进粒子群算法流程

全局路径规划的流程图如图 4 所示。分为如下步骤：

图4 静态环境改进PSO 算法流程图Fig.4 Flowchart of improved PSO algorithm in static obstacle environment

1）初始化种群，包括设置粒子的个数、维度，各个粒子的位置、速度，最大迭代次数runmax，初始化惯性权重w、学习因子c1，c2。

2）计算粒子适应度。

3）由粒子择优规则找出个体最优解和群体最优解。

4）更新学习因子与惯性权重，更新粒子的速度和位置。

5）经过指定代数更新后，如若粒子仍未产生新的群体最优解，则根据式（11）判断是否处于轻度收敛状态。若处于轻度收敛状态，则根据式（12）对最优解进行变异并转入第2 步；否则转入第6 步。

6）判断迭代次数是否达到runmax，满足时候结束算法；否则，转第 2 步。

2 基于速度障碍法的局部动态规划

2.1 速度障碍法

机器人航行过程中，遭遇游鱼、漂浮物以及船舶等动态障碍时，可以改变首向绕过障碍，也可调整航速躲避障碍。当机器人遭遇动态障碍阻挡时，如需调整航速为Vnew，则要求Vnew尽可能接近最佳航速。

在局部动态避障的规划中，构建障碍物模型进行避障规划时，由导航精度带来的障碍物位置偏差会导致避障效果不理想，因此选建障碍物相对机器人的速度坐标系。利用机器人搭载的声呐系统采集数据构建障碍物相对机器人的地图系统，其中包括障碍物相对机器人方位Sobψ和距离Sob。以机器人声呐位置为原点(0,0)建立笛卡尔坐标系Os，机器人体坐标系 (Xa，Ya)，定义机器人航向为Y轴，水平处置于航向为X轴。假定在时间 Δt内，机器人的首向角 ψr，航速 υr为固定值。机器人航行过程中，笛卡尔坐标系Os随机器人位姿更新。当机器人以固定首向角 ψr以及航速 υr航行时，坐标系原点在X轴、Y轴的移动量表示为 (Δx，Δy)：

假定在时间 Δt内，障碍物的运动方向 ψo、航速υo为固定值。声呐测得障碍物位置相对于当前坐标系位置 (x1，y1)，障碍物新测得的位置 (x2，y2)。则上一时刻障碍物相对当前坐标系的位置为：

障碍物运动速度为：

障碍物运动方向为：

根据声呐图像的障碍物轮廓，对障碍物长宽比判别后进行包围处理。如图5 所示，机器人A与障碍物B均按照固定航向与航速运动。

图5 机器人与障碍物运动情况Fig.5 The motion of vehicle and obstacle

将机器人A视为质点，并结合机器人尺寸与安全阈值对障碍物B进行膨胀处理。计算A相对B的相对速度VBA=VA-VB，定义从一点P出发沿V方向的射线为：

当从A点发出沿VBA方向的射线 λBA与障碍物B相交，即射线 λBA落在障碍物B相对于机器人A的2 条切线夹角内时，二者将会发生碰撞，如图6 所示。

图6 碰撞条件判定Fig.6 Determination of collision condition

由速度障碍法，定义碰撞范围为：

如不改变机器人A的航行状态，机器人将在某一时刻t1与障碍物发生碰撞，为逃离碰撞范围，需要调整位置于安全区域。由碰撞范围可得：的补集即是安全航行区域[15]。

2.2 适应度函数

建立机器人-障碍物相对速度坐标系如图7 所示。

图7 机器人-障碍物相对速度坐标系Fig.7 Relative velocity coordinate system of vehicle-obstacle

设全局坐标系(X，Y)，机器人以速度VA、航向角α运动。机器人体坐标系(Xa，Ya)。O处为障碍物，(X，Y)坐标系中速度为VO，方向角 βO。LMO，LNO为机器人障碍物两侧切线；LO是以机器人为起点，到障碍物边缘任意点CO的 有向线段。θO是LO与 X 轴正向夹角，γAO为VAO与LO夹角。定义避碰角为机器人躲避障碍物，VAO的调整角度。Δγaolow，Δγaoup分别为VAO旋转到LMO，LNO的角度，设逆时针为正向。

机器人在时间区间 [tk，tk+Δt] 内 躲避某障碍物Oi，则在该段时间内VAOi应该偏离碰撞范围，即如下不等式之一成立：

假设 [tk，tk+Δt]区间内障碍物速度恒定，则

其中，ΔθOi=VAO=sinγAOi/LOi，文献[16]对上式有详尽推导证明。由式（2 2）可以看出避碰角由(ΔVA,VAΔα)确定。由此可知调整机器人的航速与航向是避障的2 种有效行为，且仅仅采取其中一种基本就能完成避障要求。针对子目标段的运动障碍物，粒子群维度数是固定的二维：机器人的航速和航向，且其优化调整变量即为这2 个值。

建立机器人-目标区域相对速度坐标系如图8 所示，定义与机器人-障碍物相对速度坐标系相同。

图8 机器人-目标点相对速度坐标系Fig.8 Relative velocity coordinate system of vehicle-target

假设机器人子路径终点位于可行区域G内，CG为终止点，则期望VAG始 终指向CG，避碰角尽可能接近VAG与LG之间的夹角，保证目标函数式（23）极小：

为了减少偏航时间，使机器人尽快回归正常航线，建立最小时间函数。具体表示为在避障前航线方向上，当机器人航向分量在此方向速度最大时，能使偏航时间最短，则

利用线性加权方法分别将目标函数JG1，JG2归一化，转化为单目标函数，得到的单目标函数如下式：

其中，ϖ1，ϖ2分 别为JG1，JG2的权重。考虑到动态避障过程中，对回转角度要求更高，对航速变化要求较小，在选择参数的值时使 ϖ1＞ ϖ2，且ϖ1+ϖ2=1。

动态障碍环境下路径规划于路径节点间进行规划，由当前路径点向下一个路径点航行过程中进行避障操作，其算法具体流程如图9 所示。

图9 动态环境改进PSO 算法流程图Fig.9 Flowchart of improved PSO algorithm in dynamic obstacle environment

3 实验结果

根据建立的环境模型与适应度函数，分别采用遗传算法，传统粒子群算法和改进粒子群算法对机器人全局静态路径规划进行仿真实验。实验结果对比表明，本文的改进粒子群算法在全局静态规划中有更好的路径规划结果。最后，验证局部动态环境下路径规划效果。

为满足算法的稳定性并保证收敛，选取线性递减惯性权重，其中wmax=0.85，wmin=0.15；学习因子c2=c2=2 ；粒子数100；最大迭代次数runmax=100。利用传统 PSO 算法、遗传算法和改进 PSO 算法各实验20 次，全局静态规划结果如表1 所示，图10 为实验结果，图11 为目标函数值的迭代曲线图。

表1 实验结果Tab.1 The result of experiment

图10 同侧路径规划结果Fig.10 The result of path planing in the same side

图11 目标函数值的迭代曲线Fig.11 Iterative curve of objective function value

分析表1，由20 次实验结果所得路径长度平均值升序排序，分别为改进粒子群算法、遗传算法和粒子群算法。如图10 所示，同侧实验结果比较表明，3 种方法都能够完成全局静态路径规划任务，且机器人航行全程无危险；与遗传算法、传统粒子群算法相比，改进粒子群算法的规划效果更好，规划路径长度更短。

图11 为选取某次实验的函数值迭代曲线。结果表明，改进粒子群算法的收敛效果和求解能力优于传统 PSO算法和遗传算法，且算法的跳出局部最优解的能力优于二者。改进后的粒子群算法在前期搜索效果弱于传统粒子群算法和蚁群算法的情况下，能够利用粒子的“变异”加快搜索速度，快速逼近最优解。在后期收敛中，由于“变异”粒子的引入，能够避免陷入局部最优解。由以上分析可知，改进的粒子群算法在做机器人全局静态路径规划时，具有较强的搜索和收敛能力。

同时，本文验证了局部动态路径规划能力。图12为全局静态最优规划路径中，加入局部动态障碍后所得到的最终规划结果。结果表明，针对不同的局部动态障碍，机器人能够安全、高效的进行躲避，且避障动作小、路径损失少，能够在全局路径的基础上规划出最优避障路径。

图12 动态环境下规划结果Fig.12 The result of path planing in dynamic obstacle environment

4 结 语

本文依据大型自主水下机器人路径规划需求，在严格的机动性约束条件下构建了路径长度、路径可行度和回转可行度函数；通过引入最优位置变异过程增强了算法在全局静态路径规划过程中的求解能力；面对局部动态规划，引入速度障碍法以获得碰撞范围和安全区域。实验结果表明，全局静态与局部动态相融合的路径规划方法，在全局静态规划中路径更短、求解能力更强，局部动态规划能够规划出最优避障路径，高效且稳定。