基于视线导引法的AUV轨迹跟踪控制技术研究

郭亦平 王益民 任元洲

（中船重工第七〇七研究所九江分部，九江，332007）

随着AUV及相关技术的发展，AUV已经被用于执行扫雷、侦察、情报搜集及海洋探测等任务，未来还可作为水下武器平台、后勤支持等装备使用[1]。轨迹跟踪是AUV执行任务时最基本的运动模式之一，该控制方法已大量应用于航天、兵器、无人机（Unmanned aerial vehicle，UAV）、无人水面舰（Unmanned Surface vehicle，USV）等领域。陈霄、刘忠等针对欠驱动无人艇，研究了改进视线导引算法，实现了 USV的自适应滑模航迹跟踪控制[2]；Lionel Lapierre等基于LOS原理，提出了一种AUV反步自适应轨迹跟踪控制算法[3]；Morten Breivik 和 Thor I Fossen在文献[3]的基础上，改进了 LOS法，基于AUV动态特性提出了一种改进的反步自适应控制算法[4-5]。本文在学习借鉴上述研究成果的基础上，提出了一种基于 LOS法的运动控制算法与航向 MPC控制算法串接的AUV轨迹跟踪控制算法。

1 基于三次样条插值曲线的路径规划[5]

1.1 操纵任务概述

一条参数化轨迹定义为一被连续轨迹参数ϖ参数化的几何曲线η(ϖ)∈Rq,q≥1，ϖ为连续路径变量。对于定深运动的AUV而言，可使用一2-D描述：

式中，pn(ϖ)对ϖ的一阶导数和二阶导数分别记为p′和p′′。

轨迹跟踪问题的一个经常用到的解决方案是把其当做一个操纵问题的几何任务，由下面的定义给定。操纵问题包含两个任务：

（1）几何任务：对于任意连续函数ϖ(t)，驱使位置pn(t)收敛到一期望轨迹，

（2）动力学任务：根据下式使速度ϖ˙收敛到期望速度Ud

式中，[xd,yd]为目标轨迹的水平面坐标位置。动力学任务服从

式中(t)为沿轨迹切线的指令轨迹跟踪速度，而(t)表示沿轨迹法向的指令轨迹偏移速度。

1.2 使用3次样条插值法的路径产生方法

可使用样条插值法产生一曲线轨迹(xd(ϖ),yd(ϖ))以通过事先定义的航路点集合。轨迹(xd(ϖ),yd(ϖ))可通过选择=k,k∈R，以致ϖ=kt来得到，即在一段规划路径轨迹中，连续路径变量ϖ对时间的导数为常值。。

考虑下述三次多项式，使三次样条可内插通过航路点集合

式中，xd(ϖ)、yd(ϖ)是AUV计划航路点，而ϖ是由下式给定的轨迹变量

利用样条插值法的目的之一就是将上式轨迹变量函数用分段=k函数进行表述。

xd(ϖ)和yd(ϖ)对ϖ的偏导数分别为：

AUV规划轨迹的指向角即为

因此，AUV的期望速度可计算如下

上式中的未知参数a0、a1、a2、a3，b0、b1、b2、b3能够使用三次样条算法计算。

1.3 三次样条插值路径产生算法

通过航路点 (xk-1,yk-1)和(xk,yk)的轨迹必须满足

通过使下式成立，方可得到平滑曲线

对x和y，增加两份边界条件（速度和加速度）是可能的

对于n个航路点，可将未知参数收集为一个向量

因此，三次插值问题就可被写做如下线性方程：

式中

对于bk=[b3k，b2k，b1k，b0k]也可用相似方法求得。

1.4 本文算法设计

图1为Serret-Frenet 坐标系中AUV的轨迹跟踪运动描述，对于一个二维参数化路径{n}

通过指定一随轨迹移动的坐标系，基于船舶动态模型可设计该运动控制算法。该坐标系通常被选作Serret-Frenet 坐标系。在轨迹跟踪过程中，船速为U，设计运动控制算法的目的为：（1）调节 AUV与路径间的距离e和s到 0；（2）调节AUV的航速与轨迹变化速度差到0。

由图1可知，在轨迹跟踪过程中，实时轨迹位置与AUV间的距离为：

图1 Serret-Frenet 坐标系

为了调节AUV与规划轨迹间的距离e到0，可采用LOS法求得AUV的实时指令航向角为

式中，Δ＞0是一个常值参数，φd为式（7）所示的规划轨迹方向角。

为了克服洋流等常值干扰影响，可在上式基础上增加积分环节，实时指令航向角可修正为：

为了调节AUV切向跟踪偏差到0，可设计指令航速运动控制律为：

式中，χSF的求取就成为AUV轨迹跟踪的关键。由图1可知

故对χSF的求取可转为求AUV漂角问题。根据船舶操纵性可知，AUV的漂角定义为：

因此，可设计一水平面状态观测器对AUV的横荡速度v进行估计。观测器的设计可参照文献[6]。

2 带有约束的航向MPC控制器设计

AUV水平面运动线性方程可用状态空间方程描述如下：

式（25）和式（26）简记为

利用后向差分法离散化式（27）后，可得

由式（28）可得增量式状态方程和状态输出方程：

假设在k时刻的m个采样间隔后解算的预测指令舵角增量为

式中，L（m）T为拟合航向 MPC控制器的离散化Laguerre函数向量，η为Laguerre参数[7]。

假设在k时刻，式（29）的状态为x（k），则在此时刻的m个采样间隔的预测状态为

为解决航向预测控制律，设代价函数为

将式（30）代入上述代价函数后，可得

考虑到 AUV航行控制系统存在下述物理限制：（1）舵速限制：Δumin≤Δu≤Δumax；（2）舵角限制：umin≤u(k)≤umax，可用下述不等式描述舵速约束：

舵角约束可用下述不等式表示为：

航向MPC控制器的求解问题转换为二次规划问题：

该二次规划问题采用活动集法求解，可利用Hildreth二次规划方法求解。故航向MPC控制器可设计为：

上述二次规划问题中的AUV运动状态可利用状态观测器进行估计，可利用离散时间kalman估计理论设计[8]：

3 仿真分析

本文研究的AUV完整6自由度模型可参照文献[9]，该 AUV 是 The Naval Postgraduate School（NPS）设计的第二代AUV。NPS AUVⅡ的主尺度为：L=5.3 m，W=534 kN。其最高航速为4 kn。根据前文提出的AUV轨迹跟踪控制方法，设计如下轨迹跟踪试验：AUV初始位置为（–810，110）m，航向0°，航速3 kn，潜深50 m，航路点共计13个，设置为：(–800，100)m、(–500，1 800)m、(–200，2 000)m、(–50，1 500)m、(–200，1 000)m、(–250，100)m、(200，1 000)m、（500，300）m、（500，–250）m、（300，–200）m、（0，100）m、（–300，–600）m、（–600，–1 000）m。13个计划航路点见图2中A～M所示。三次样条插值轨迹产生方法中设置Ud(t)=2.5 kn、ω(tk)=0.001，计划航路见图2，轨迹跟踪变化曲线见图3，轨迹跟踪偏差变化曲线见图4，航速变化曲线见图5，指令航向和实际航向变化曲线见图6，操舵变化曲线见图7。

图2 计划航路

图3 轨迹跟踪变化曲线

图4 轨迹偏差变化曲线

图5 航速变化曲线

图6 航向角变化曲线

图7 舵角变化曲线

由图可以看出，利用三次样条插值法规划的轨迹连续平滑，有利于 AUV的轨迹跟踪控制。AUV可实时跟踪规划轨迹航行，表明提出的轨迹跟踪算法可以实现轨迹跟踪。在轨迹跟踪过程中，AUV的法向跟踪偏差最大不超过12 m，表明利用LOS导引法的运动控制算法合理可行。

轨迹跟踪过程中，轨迹纵向跟踪偏差有较大的负值，即AUV沿轨迹航行时可超过实时轨迹，而有时AUV也会落后于实时规划轨迹位置。由于AUV的航速控制算法采用了基于轨迹偏差的比例控制算法，前期逼近并追踪规划轨迹的过程中，航速调节过高，使得AUV超越了规划轨迹达190 m。此后，随着航速的不断调节，AUV与实时规划轨迹间的纵向距离逐渐缩小，并保持在可接受的范围内。这表明提出的航速控制算法虽可满足轨迹跟踪要求，但仍有较大的改进空间。仿真过程中，实际航向可实时跟踪指令航向角。

4 结论

本文针对AUV水下定深轨迹跟踪问题，首先基于航路点计划，利用三次样条插值方法规划了预定航行轨迹，随后基于LOS导引法，将轨迹跟踪问题转换为AUV的航速和航向跟踪问题，设计了一种航速、航向跟踪控制算法，然后，基于AUV水平面运动方程，设计了一种带有舵角舵速约束的离散时间航向模型预测控制方法，实现了AUV的轨迹跟踪控制。仿真分析表明：（1）提出的轨迹跟踪控制方法可实现AUV轨迹精确跟踪控制，控制效果好，操控品质高；（2）文中设计的LOS制导算法计算简洁，有较高的工程实用价值；（3）文中提出的基于 Laguerre函数的带有舵角舵速约束的航向MPC控制算法，利用Laguerre网络逼近方法和 Hildreth二次规划方法，大幅降低了一般MPC算法的沉重计算负担，有利于工程实现。