基于混合势场法的无人潜航器路径规划及编队方法研究

王健，张华，徐令令，曹园山，陈伟，顾媛媛

(中国船舶科学研究中心，江苏 无锡 214082)

0 引 言

水下无人潜航器（UUV）是一种无人化能够自主在水下作业完成特定功能的水下无人装备[1]。UUV的精确航迹跟踪能力对于UUV 来说是一项很关键的行为能力，它是完成UUV 作业任务（如海底地形调查，远程航行等）的一种重要技术手段。

对于一般UUV 来说，通常采用螺旋桨进行驱动，以尾舵来控制航向。在进行路径跟踪时需要通过尾舵同时进行航向以及航迹控制，这种单尾舵的控制方式属于欠驱动的方式，需要设计合理的潜航器航行控制算法满足潜航器这种欠驱动的方式。

1 混合势场法路径跟踪及避障架构

对于路径规划方法，通常是针对从起始点航行到目标点，自主寻找到一条最优的无障碍航线，一般应用方法有A*算法、蚁群算法[2]、粒子群算法[3]、势场法[4]等。而水下潜航器一般在实际作业时都有一条预规划的航线，潜航器在一般情况下需要按照预设航线运动，在遇到障碍等特殊情况需要进行规避，规避动作也必须在预设航线的趋势上进行规避。

本文提出一种混合势场架构，将潜航器避障算法与路径跟踪算法进行混合，合并为一种统一的广义势场。该种方法将避障、航向保持、路径跟踪等多驱动问题进行统一，输出为单舵控制的方式，适合欠驱动潜航器。混合势场法控制架构如图1 所示。针对潜航器单驱动问题，算法最终的输出层必须为单目标的方式，以目标航向的形式输送给潜航器执行层。潜航器的目标航向来源于潜航器本体所受各抽象力的合力，主要有引力合力和斥力合力。潜航器的引力合力来源于潜航器的航线引力以及航向引力，潜航器的斥力来源于各障碍斥力。

图 1 混合势场法控制架构图Fig. 1Control architecture of hybrid potential field method

2 混合势场法的无人潜航器路径规划方法

2.1 势场法下路径跟踪方法

目前势场法的理论应用很广，所谓人工势场法是将实体环境中的UUV 以及障碍物都抽象为三维空间中的微粒，在微粒间构建虚拟力。障碍物形成斥力、目标点形成引力，通过多种力的合力实现UUV 的正确运动方向[5]。

图 2 势场法路径跟踪图示Fig. 2Diagram of path tracking based on potential field method

参照势场法理论[6]，潜航器在进行水下路径跟踪时，在潜航器周围构建虚拟力场，如图2 所示。由潜航器偏离航线产生的航线偏差构建对潜航器的吸引力吸引力的方向由潜航器指向预设定的航线。同时潜航器预设定的保持航向对潜航器亦产生吸引力航器，实吸际引目力标的航方向向由由吸设引定力航线的A 点指合向力B方点向。决潜定。对于吸引力的构建方法有多种，本文采用距离值构建法。距离构建法中，建立引力势能函数见式（1），

其中： Ka为 标量控制增益； Rgk为航行器和目标中心点之间的距离；dgoal为引力影响半径，在该值范围内引力影响消失。由此产生的吸引力为吸引势能场的负的梯度，具有如下形式：

则潜航器目标航向 ψd，为航线航向 αk与合力夹角Ψr的合力值：

2.2 混合势场法下路径跟踪及避障方法

一般在研究UUV 避障中，通常需要一条从起始点到目标点的无障碍路径。而通常一般UUV 在进行作业时需要按照实际的路径进行作业，在遇到障碍后进行自主规避，规避完成后回归预设定的路线。

在建立势场法路径跟踪方法后，在势场法全面的算法架构下，进行自主避障只需要遵循势场法的理念在潜航器本体上添加虚拟的障碍物斥力使水下无人潜航器在任务空间中远离障碍物，即可实现自主避障[7]。建立潜航器障碍斥力场同样采用距离值的方法进行构建。在建立排斥势能场时，同时考虑了障碍物对航行器的作用以及目标对航行器的作用，可用来解决目标在障碍物影响范围内的情况。建立的排斥势能函数为如下式：

排斥势能场负的梯度，即由排斥势能场产生的排斥力，潜航器障碍物斥力函数见式（8），斥力的方向由障碍物指向潜航器本体。

其中：rat为当前点和吸引目标距离； rre为当前点和障碍距离， R rei=rre2； Po为障碍物影响距离； m为斥力增益系数。

在增加相关斥力场后，潜航器的受力以及运动图解如图3 所示。图中航线以及航向的引力合力FNatt对潜航器产生吸引力，障碍物对潜航器产生斥力FRep，2 种力的综合作用产生力 FNSJ，力FNSJ将指引潜航器产生航向偏转作用，指引潜航器绕开障碍物进行规避运动。

图 3 混合势场法避障受力示意图Fig. 3Force diagram of obstacle avoidance with mixed potential field method

3 实航测试

采用中国船舶科学研究中心研制的“海翔-H”混合动力水下无人潜航器进行试验，“海翔-H”无人潜航器具备螺旋桨以及尾舵可用于进行有动力推进航行。

3.1 无障碍路径跟踪测试

进行无障碍路径跟踪测试，给潜航器预设计一条1 km 长的弧形折线预设轨迹，如图4 虚线所示。潜航器从图中起点开始运动，到达终点停止，图中实线为潜航器实际轨迹，从图中可以看出潜航器能够很好的沿预设定路线进行运动，在受到实际环境的风浪流干扰后偶尔会偏离预设定的航线但很快能够自主回归，轨迹跟踪误差小于5 m。

图 4 轨迹跟踪路线Fig. 4Trajectory tracking

3.2 有障碍路径跟踪测试

在进行潜航器避障试验时，分别设计潜航器单障碍避障、双障碍避障、多障碍连续避障工况，考验方法的可靠性。水下单障碍避障如图5 所示，主要是考验算法控制器在路径跟踪以及障碍规避、回航的能力；双障碍避障如图6 所示，主要是考验算法控制器在按照预设定的航线运动过程中受到来自双重障碍的夹击作用，滑翔器是否能够从两障碍物中间找到最优路径通过，不能出现堵死的状况；多障碍连续避障如图7 所示，主要是考验算法控制器在航行路线有多个障碍，进行连续避障、而不发散的能力。

图 5 单障碍避障路线Fig. 5Single obstacle avoidance route

图 6 双障碍避障路线Fig. 6Double obstacle avoidance route

图中虚线为预设运动轨迹，圆圈为7 m 直径圆形障碍，实线为实际轨迹。从图中可以看出，在未接近障碍前潜航器按照设定路线航行，靠近障碍后潜航器开始自主的偏离航线进行自主规避，规避完成后回到预设定路线。同时在双障碍夹击作用能够找到最优路径通过，在多个障碍连续作用能够连续避障不发散。

4 结 语

本文从应用实际出发，针对水下无人潜航器避障、航向保持、航线跟踪等多输出、单尾舵输入欠驱动控制问题，提出一种统一架构的混合势场法。该方法巧妙利用虚拟力的理念，在避障控制、航向保持控制、航线跟踪控制等全部建立虚拟力场，通过多种力场的综合作用生成单一合力场指引尾舵操纵。同时本文还利用“海翔-H”水下无人潜航器进行实际水域的航线跟踪、单障碍避障、双障碍避障、多障碍连续避障测试。结果表明，混合势场法能够很好的兼顾潜航器实际作业时的航向保持、航线跟踪、避障等工况，具备良好的鲁棒性。另外，通过本文建立起的统一势场法增加虚拟力还可以很快的拓展到无人潜航器的编队控制中。

图 7 多障碍避障路线Fig. 7Obstacle avoidance route