基于有限时间和状态观测器的双闭环AUV轨迹跟踪控制研究

马洪潮， 戴晓强， 曾庆军， 夏 楠， 郭雨青

(1.徐州徐工汽车制造有限公司，江苏 徐州 221000; 2.江苏科技大学电子信息学院，江苏 镇江 212000)

0 引言

海洋蕴藏着丰富的石油、天然气等发展经济所必须的不可再生能源。20世纪90年代以后，自主水下机器人(Autonomous Underwater Vehicle,AUV)应用于海洋资源勘探、低强度军事侦察等领域，由于AUV对轨迹和路径的精确跟踪的要求越来越高，对其研究也越来越多。对于AUV的轨迹运动控制问题，主要的困难在于绝大多数AUV属于欠驱动类型，不满足Brockett定理必要的条件，故无法通过持续的控制策略使AUV达到镇定。另外，自主水下机器人在实际任务操作中会受到未知干扰、模型的不确定、输入饱和、超调等因素的限制，使得AUV在进行三维轨迹跟踪时变得愈发困难。AUV的轨迹跟踪控制主要为运动学控制和动力学控制。文献[1]主要通过改进积分视线法来设计运动学控制器，通过反步自适应滑模方法来设计动力学控制器；文献[2]主要通过引用生物启发神经动态模型对期望速度进行滤波，利用积分滑模控制方法来设计动力学控制器，但不能有效抑制滑模固有的抖振问题。为了解决这些问题，文献[3-4]提出了非线性扰动观察滑模控制器，根据新的趋近律改进滑模来抑制抖振，并提出一种基于模型预测控制的新型双闭环三维轨迹跟踪方法，利用新型趋近律和双闭环控制器生成控制输入来抑制滑模抖振；而文献[5]考虑到外界环境干扰，使响应速度的误差收敛到零点上下。针对AUV模型的不确定性，文献[6]提出了基于干扰观测器的AUV深度自适应终端滑模控制，解决对外部环境干扰和系统扰动难以控制的问题，但未考虑系统的快速收敛性及系统的稳定性；文献[7-8]分别通过自适应神经网络和自适应滑模控制方法来解决模型不确定问题和外界干扰，但未考虑神经网络收敛速度慢和学习时间较长的问题。针对输入饱和性和模型的不确定问题，文献[9]提出了基于三维自适应轨迹跟踪控制，但并未解决外部环境干扰下及系统不确定带来的影响。

综上所述，为提高双闭环系统的收敛速度及克服系统的不确定性，本文引入有限时间的控制策略和降阶扩张状态观测器来设计一个高性能的轨迹跟踪控制器；所设计的自适应律的虚拟向导，更是减弱了外界洋流干扰，克服了超调，确保了该控制器对位置量、姿态角的快速收敛以及补偿混合不确定项。

1 控制问题描述

1.1 AUV数学模型

本文采用的AUV是自主研发的“探海Ⅱ型”AUV，其六自由度运动学和动力学方程如下。下文中未特别说明变量均视为标量。

1) 运动学模型为

(1)

式中：(x,y,z)是AUV在惯性坐标系的坐标位置；φ,ψ,θ分别为俯仰角、艏向角、横滚角；u,v,w,p，q,r在随体坐标系中分别表示纵向线速度、横向线速度、垂向线速度、横摇角速度、俯仰角速度、艏向角速度[10]。

2) 动力学模型为

(2)

(3)

综合式(2)及式(3),AUV动力学方程可简化表示为

(4)

其中

(5)

“探海Ⅱ型”AUV如图1所示。

图1 “探海Ⅱ型”AUVFig.1 “T-SEA Ⅱ” AUV

1.2 AUV 三维轨迹跟踪误差方程

图2为AUV空间曲线轨迹跟踪示意图，其中：I,B,F分别为固定坐标系、运动坐标系以及Serret-Frenet坐标系；AUV的重心与运动坐标系原点O重合，F坐标系原点为AUV期望轨迹中的任意一个虚拟参考点，相对于AUV的速度为Vp，原点P为水下机器人期望轨迹上的“虚拟向导”。

图2 虚拟向导轨迹示意图Fig.2 Schematic diagram of virtual guide trajectory

利用Serret-Frenet坐标系导出跟踪误差公式，根据三维轨迹跟踪示意图，将固定坐标系I绕η轴旋转ψ角度，再绕ζ轴旋转θ角度，然后平移固定坐标系中点E与运动坐标系P相重合[12]。Serret-Frenet坐标系框架相对于固定坐标系I的ψ,θ旋转的角度为

(6)

分别定义航向角γα和俯仰角γβ，即

(7)

考虑到AUV在水下实际运动中是受到未知环境影响的，AUV艏向角ψ和俯仰角θ可以表述为

(8)

综上所述，AUV运动方程在坐标系I下可以重新表示为

(9)

式中：U为AUV空间合成速度；ψω=ψ+β；θω=θ-α。

(10)

式中：ψe和θe分别表示坐标系H的艏向角误差和俯仰角误差;r/cosθ+β和q+α分别表示水下机器人的艏向和俯仰角速度，具体可表示为

(11)

综上所述，Serret-Frenet坐标系下的轨迹跟踪误差模型可具体表示为

(12)

2 AUV控制系统设计

2.1 位置控制器设计

首先定义李雅普诺夫函数Ve1为

(13)

则对式(13)的系统求导可得

(14)

定义虚拟控制变量α1为

(15)

系数k1,k2,k3均为大于零的常数，则

(16)

选取李雅普诺夫函数为Ve2为

(17)

则系统沿着式(14)的导数计算如下

(18)

定义虚拟控制输入α2，设e2=θ-α1。

α2=-k2e2+1+ue1

(19)

选取李雅普诺夫函数Ve3为

(20)

则系统沿着式(14)的导数计算如下

(21)

定义e3=q-α2，对状态量e3采用有限时间控制方法进行设计，得出τq控制器表达式为

(22)

状态量q的误差导数可表示为

(23)

假设系统的平衡点为原点，Ω为系统状态有限时间内收敛区域，Ω={e3:|e3|≤[(k4+l)/K]1/a}，K>0,0

证明过程如下。

选择李雅普诺夫函数

(24)

系统初始状态在Ω外，即e3∈R-Ω，由于V(e3)<0,则存在t1>0,使得e3(t)∈bdΩ，b为待设计参数。

将式(21)、式(22)、式(23)代入式(20)得

(25)

根据有限时间控制器设计判定以及不等式(23)的证明，所设计的式(21)控制器是有限时间稳定，使得系统的控制效果更好，抗扰动能力强。

2.2 姿态控制器设计

为解决外界多种环境干扰及输入饱和的问题，引入降阶扩张状态观测器、动态面控制技术、积分滑模及辅助动态系统。

引入线性降阶扩张状态观测器(ESO)为

(26)

引入动态面的思想简化控制器提高跟踪响应效率和高质量的控制输入。构造的数学表达式为

(27)

定义误差跟踪速度为

(ueqere)T=(u-v1q-qedr-red)T

(28)

式中：ue为镇定纵向速度误差;qe为纵倾速度误差；re为艏向速度误差;qed和red为所设计的虚拟控制律。

设计用于镇定纵向、俯仰、艏向速度的误差积分滑模面为

(29)

式中，λu，λq，λr为控制器待设计参数。

对式(29)求导可得

(30)

令式(30)为零，可求得控制律为

(31)

然而，AUV本身存在参数摄动项，又在洋流的干扰下，等效控制的控制效果并不能收敛逼近；引入指数趋近律进行补偿，可以有效抑制滑模带来的抖振问题，使AUV在大角度转弯时会产生连续输出信号，进而优化AUV的运动效果，进一步提高系统的鲁棒性。

选取如下趋近律

(32)

考虑到系统饱和对控制性能有较大影响，设计辅助动态系统为

(33)

式中:gu=1/m11,gq=1/m44,gr=1/m55;bi>0(i=u,q,r)；χi(i=u,q,r)为辅助动态系统的状态变量。

考虑到执行器的饱和问题及趋近律，则实际控制律可表示为

(34)

通过构造合理的李雅普诺夫函数来证明该控制器的稳定性，预选函数为

(35)

结合式(34)对式(35)求导得

(36)

进一步可简化为

。

(37)

由杨氏不等式得

(38)

式中，χi=τi，i=u,q,r。

3 仿真实验

为了验证控制算法的有效性和可行性，基于“探海II型”AUV数学模型进行仿真，对AUV的三维轨迹同步跟踪和镇定控制问题进行数值仿真。

图3所示为AUV三维轨迹。图3中，本文算法在拐点以及轨迹误差明显好于自适应滑模算法。

图3 AUV三维轨迹Fig.3 Three dimensional trajectory of AUV

图4、图5所示为AUV推力对比及深度控制对比情况。由图4、图5可知，在加入随机干扰以及在模型不确定性下，自适应滑模算法超调较大且易抖动，在受到较大干扰下，需要较长时间控制到稳定的期望状态；在有限时间力控制和降阶ESO滑模控制下超调较小，能够在短时间内调整至期望值并保持稳定。

图4 AUV推力对比图Fig.4 Comparison of AUV thrust

图5 AUV深度控制对比图Fig.5 Comparison of AUV depth control

图6所示为AUV姿态角对比情况。由图6可知，在利用自适应滑模算法进行姿态角控制时，收敛速度慢且姿态抖动较为严重；而在有限时间及降阶ESO滑模控制器的姿态控制下，响应速度较快且较好地抑制本身固有的抖振，能够快速平稳回到期望状态。

图6 AUV姿态角对比图Fig.6 Comparison of AUV attitude angle

4 湖泊试验

AUV湖泊试验于2021年7月在某湖完成，湖水平均深6 m，湖水流速3 kn(1 kn≈1.852 km)左右，下潜深度为1 m。设置使命为六方形，起点GPS经纬度坐标为(120.559 228°，31.892 370°)。使命任务的时长为14 min。图7为上位机使命规划图，图8～10分别为AUV湖试的三维轨迹、姿态角、深度的变化。

由图8～10可知，在设定使命后，AUV能够迅速到达期望的轨迹、姿态、深度及航向；因AUV搭载的捷联惯导，所记录的转向角到达最高360°后归零，故图9在400 s左右艏向角会从零开始，属正常现象。在AUV进行转向时，因实际水流速度发生较大变化，图10的深度控制曲线会出现轻微波动，但实际控制的平均误差在±0.1 m以内，航向角误差在3°内，达到指定的控制目标，能够应用于工程项目。

图7 上位机使命规划Fig.7 Mission planning of host computer

图8 AUV三维轨迹跟踪曲线

图9 AUV姿态角控制曲线Fig.9 AUV attitude angle control curve

图10 AUV深度控制曲线Fig.10 AUV depth control curve

5 结束语

本文考虑在未知环境下扰动和常规滑模控制器容易出现控制超调、收敛速度慢的情况，针对三维轨迹跟踪控制问题提出了有限时间控制以及降阶ESO动态积分滑模控制。引入动态辅助系统及具有自适应律的虚拟向导。仿真结果表明，该控制器能较好地结合降阶ESO和有限时间控制的优点，较好地抑制了抖振、减弱了外界干扰，同时具有控制器响应快、鲁棒性好的优点，更是克服了在拐点容易出现控制器超调的问题。