考虑速度传感器失效的无人潜航器轨迹跟踪控制方法

1.本发明涉及无人潜航器轨迹跟踪控制技术领域，具体涉及一种考虑速度传感器失效的无人潜航器轨迹跟踪控制方法。


背景技术：

2.近年来，欠驱动无人潜行器的运动控制已成为海洋工程的研究热点，无人潜航器在海洋学观测、海底资源勘探、水文测量和制图、海底救援等方面发挥着重要作用，具有广阔的应用前景。随着自动化程度的提高，在不依靠人工的情况下利用无人艇对无人潜航器进行布放及回收已经成为了新的研究热点。为了完成对无人潜航器的自动布放及回收，无人潜航器必须实现轨迹跟踪，尤其是高精度的水面轨迹跟踪。因此，对于无人潜航器布放回收过程中的轨迹跟踪的研究就变得愈发重要。但是，目前对于无人潜器在水面进行轨迹跟踪的研究相对缺乏，在现有技术中，对于自动布放回收过程中的无人潜航器的轨迹跟踪存在着无法精确估计航行过程中风、浪、流产生的影响或系统以外未建模部分产生的干扰，并未对干扰进行补偿、未考虑当无人潜航器完成水下作业后其速度传感器精度会受到极大影响甚至失效、没有根据无人潜航器自身的动力学性能对控制器的期望输出进行限制等缺点。特别是在海面风浪干扰复杂不确定的环境下这些缺点暴露得更加明显。
3.目前现有对无人潜航器布放回收过程中水面轨迹跟踪方法的研究主要有以下不足：
4.1)未考虑当无人潜航器完成水下作业后其速度传感器精度会受到极大影响甚至失效。比如论文《trajectory tracking control of an underactuated uuv using a novel nonlinear integral sliding mode surface》、《dynamical sliding mode control for the trajectory tracking of underactuated unmanned underwater vehicles》等论文中所提出的利用反步滑模法设计的轨迹跟踪控制方法均未考虑无人潜航器水下作业后传感器精度受到影响可能导致控制精度下降甚至失效。传统针对水面无人船设计的轨迹跟踪控制方法同样未考虑速度传感器精度对轨迹跟踪产生的影响，比如论文《欠驱动船舶航迹跟踪指令滤波滑模鲁棒控制》一文中考虑了对虚拟控制律进行限制，但没有考虑速度传感器失效产生的影响。
5.2)传统针对水面无人船设计的轨迹跟踪控制方法未考虑无人潜航器的动力学性能，没有对控制器的期望输出进行限制，并且没有解决对虚拟控制律计算高阶导数导致的计算复杂度增加的问题。比如论文《带非线性观测器的欠驱动船舶自适应动态面输出反馈轨迹跟踪控制》一文中所提出的控制方法就没有对控制器的期望输出加以限制，并且计算控制律的过程中没有避免对虚拟控制律的高阶求导所带来的的计算复杂度增加的问题。类似的，专利号cn113835338a、cn110134012a等专利中所提出的控制方法同样由于没有对期望输出进行限制无法适用于无人潜航器的水面轨迹跟踪控制。
6.3)没有观测航行过程中风、浪、流产生的影响或系统以外未建模部分产生的干扰，并未对干扰进行补偿。比如论文《基于命令滤波的水下机器人位置跟踪控制》一文中所提出
的控制方法就没有针对系统的各种扰动进行观测和补偿。


技术实现要素：

7.为解决现有技术中在无人潜航器布放回收过程中的轨迹跟踪控制存在的问题，本发明考虑了无人潜航器在无法测量速度向量的情况下的轨迹跟踪问题，同时考虑了系统内外部及未建模部分产生的干扰，为防止控制器的期望输出超出无人潜航器驱动器推力上限，通过对虚拟控制律的约束限制了控制器的输出，并提供相应的控制方法。
8.为实现上述目的，本发明通过如下技术方案实现：
9.考虑速度传感器失效的无人潜航器轨迹跟踪控制方法，其特征在于，包括以下步骤：
10.s1、建立无人潜航器水面航行数学模型，设定期望轨迹数学模型。
11.s2、设计基于位置信号的速度向量观测器，在此基础上设计纵向力矩和横向力矩方向上的干扰观测器，并通过lyapunov定理验证其有效性。
12.s3、基于实时位置误差，设计纵向速度和横向速度的虚拟控制律，在虚拟控制律的基础上导入指令滤波器对虚拟控制律的幅值及速度进行约束，并解决求取实际控制律时需要对虚拟控制律进行高阶求导产生计算复杂度增加的问题，并通过lyapunov定理验证其有效性。
13.s4、基于虚拟控制律及指令滤波器的输出值设计无人潜航器轨迹跟踪滑模控制器，同时基于干扰观测器所观察到的系统扰动估计值对纵向力矩和横向力矩的扰动进行补偿，并通过lyapunov定理验证其有效性。
14.s5、仿真验证所提出方法的有效性。
15.进一步地，所述步骤s1具体包括：
16.分别定义北东坐标系oxyz和附体坐标系obxbybzb两个坐标系，将北东坐标系xoy设为惯性坐标系，取地球上任意一点o作为坐标系的原点，ox指向正北，oy指向正东，oz垂直于ox、oy所在平面竖直向下。将附体坐标系obxbybzb作为惯性坐标系，假设无人潜航器体身左右对称，取其中心点为惯性坐标系的原点ob，obxb轴沿艇身指向艇艏方向，obyb垂直指向右舷,obzb垂直于艇身向下。对无人潜航器在水面航行过程进行动力学建模，得到包含纵荡(surge)、横荡(sway)、垂荡(heave)、横摇(roll)、纵摇(pitch)、艏摇(yaw)六自由度模型。忽略高于二阶的水动力阻力项及垂荡(heave)、纵摇(pitch)、横摇(roll)运动，建立运动学和动力学模型为：
[0017][0018]
式中，η＝[x,y,ψ]
t
，x,y表示无人潜航器运动位置矢量，ψ∈[0,2π]表示艏向角，j(ψ)是北东坐标系与附体坐标系的转换矩阵，v＝[u,v,r]
t
表示附体坐标系下无人潜航器运动的速度向量，u、v、r分别表示纵荡、横荡、艏摇速度。表示包含附加质
量的惯性矩阵，c(v)表示斜对称矩阵，表示阻尼矩阵，τ＝[τu,τv,τr]
t
表示力矩控制输入向量，τu、τv、τr分别表示纵荡、横荡、艏摇控制力，τd＝[τ
du
,τ
dv
,τ
dr
]
t
系统扰动力矩向量，τ
du
、τ
dv
、τ
dr
分别表示纵荡、横荡、艏摇方向上的扰动力矩。
[0019]
进一步地，所述步骤s2具体包括：
[0020]
首先设计基于位置信号的速度向量观测器，由于速度向量v不可测量，因此消除模型中带有速度向量v的c(v)v项，导入变量矩阵p(η,v)：
[0021]
p(η,v)＝h(η)v (2)
[0022]
式中，h(η)3×3为速度向量v到p(η,v)的映射矩阵。
[0023]
对式(2)求导，得：
[0024][0025]
为了消除式(3)中带有速度向量v的项，导入变量y：
[0026][0027]
为了使y＝0从而消除式(3)中含有速度向量v的项，令：
[0028][0029]
于是有：
[0030]
p＝y-h(η)m-1
dh-1
(η)p h(η)m-1
(τ τd) (6)
[0031]
进而设计速度观测器为：
[0032][0033]
式中，为观测器对η、p的观测值，k1、k2为增益设计矩阵，为观测器的观测误差。
[0034]
为了证明所设计的速度向量观测器的有效性，定义观测器的观测误差为：
[0035][0036]
式中，为观测器的观测误差，为观测器观测值的位置误差。
[0037]
定义lyapunov函数：
[0038][0039]
式中，x1、x2为正定设计矩阵。对式(9)进行求导，并由式(8)得：
[0040][0041]
并且令：
[0042]
x1j(ψ)h-1
(η)＝x2k
2 (11)
[0043]
则有：
[0044][0045]
由lyapunov定理的定义可知，式(7)所示的观测器是渐近收敛的。
[0046]
在设计速度观测器基础上，对于未知扰动部分设计扰动观测器，对纵荡及艏摇方向的扰动进行在线估计，根据式(6)得：
[0047][0048]
式中，为扰动观测器对系统未建模部分及内外部未知扰动τd的观测值，k0∈r3×3为由正定参数组成的设计矩阵。
[0049]
在实际情况中，无法通过观测值直接获得，因此导入辅助变量进而设计扰动观测器为：
[0050][0051]
式中，为扰动观测器对系统未建模部分及内外部未知扰动τd的观测值，k0∈r3×3为由正定参数组成的设计矩阵。
[0052]
为了证明所设计的力矩扰动观测器的有效性，定义lyapunov函数：
[0053][0054]
式中，τd为系统扰动的真实值，为参数据估计误差。
[0055]
对式(15)求导可得：
[0056][0057]
由此可知式(14)所示的干扰观测器的观测误差在有限时间内渐进收敛于零。
[0058]
进一步地，所述步骤s3具体包括：
[0059]
基于实时位置误差，设计纵向速度和横向速度的虚拟控制律。首先，定义实际位置与期望轨迹位置之间的误差为：
[0060][0061]
式中，xd为期望轨迹的横坐标，yd为期望轨迹的纵坐标，x为实际跟踪位置的横坐
标，y为实际跟踪位置的纵坐标。
[0062]
求取式(17)一阶导，并结合无人潜航器运动学和动力学模型式(1)得：
[0063][0064]
为了使位置跟踪误差收敛于零，设计纵向虚拟控制律a
ud
和横向虚拟率a
vd
为：
[0065][0066]
式中，其中，k＞0，c＞0为设计常数。
[0067]
为防止虚拟控制的输出超出无人潜航器推进器所能提供力矩的最大值，并解决在求取实际控制律的过程中需要对虚拟控制律高阶求导而产生计算复杂度增加问题，使用指令滤波器对虚拟控制律的速度和幅值进行限制，需要将纵向虚拟控制律a
ud
和横向虚拟率a
vd
作为指令滤波器的输入，指令滤波器的输出及用于实际控制律的求取。
[0068]
指令滤波器的定义：
[0069]
为解决求取实际控制律可能会由于需要对虚拟控制律求导而产生的微分复杂性膨胀问题，为减少反推计算的工作量，并且为防止输入饱和导致控制律失效的问题，需要对虚拟控制律的幅值和速度进行限制；同一阶滤波器相比，指令滤波器还能处理中间状态变量的约束。因此导入指令滤波器对虚拟控制律进行处理。
[0070]
定义指令滤波器的状态空间表达式为：
[0071][0072]
式中，x
id
为该滤波器的输入，z1为该滤波器的输出，ζ和ω分别是指令滤波器的阻尼和带宽。当不考虑幅值和速率的的作用时，x
id
和z1之间的线性关系为：
[0073][0074]
当输入x
id
有界时，则输出z1、同样有界，并在ω比较大的情况下，|z
1-x
id
|趋于零。
[0075]
为证明虚拟控制律的有效性，定义lyapunov函数：
[0076][0077]
由式(19)得：
[0078][0079]
将式(22)代入式(18)有：
[0080]
[0081]
当(u-a
ud
)与(v-a
vd
)趋于0时：
[0082][0083]
对式(22)求导，并结合式(25)得：
[0084][0085]
由此可知，式(19)所示的虚拟控制律可使位置误差xe、ye渐进收敛于零。因此虚拟控制律也可视为使无人潜航器达到目标位置的速度期望。
[0086]
进一步地，所述步骤s4具体包括：
[0087]
设计纵向推力控制律及横向推力控制律；首先设计纵向推力控制律，设纵向速度与期望纵向速度的误差ue为：
[0088][0089]
式中，为速度观测器对纵向速度u的观测值，a
ud
为纵向速度的虚拟控制律。
[0090]
设计关于纵向速度误差的积分型滑模面s1：
[0091][0092]
式中，μ1＞0为常数。
[0093]
为了进一步求取纵向推力控制律，对式(28)进行求导，并结合式(1)得：
[0094][0095]
式中，为观测器对速度向量u、v、r的估计值，τu为纵向推力的实际控制律，为虚拟控制律a
ud
经过指令滤波器的输出值，m
11
、m
22
、d
11
分别为式(1)中附加质量的惯性矩阵m及阻尼矩阵d中的系数。
[0096]
进而，由式(29)及式(14)得纵向推力控制律τu为：
[0097][0098]
式中，λ1＞0；为式(7)中干扰观测器对纵向扰动的观测补偿值。
[0099]
为证明所设计纵向推力控制律τu的有效性，定义lyapunov函数：
[0100][0101]
对式(31)求导并将式(29)和式(30)代入得：
[0102][0103]
由于当t
→
∞时，因此有：
[0104][0105]
因此信号s1一致最终有界。
[0106]
其次设计横向推力控制律，设横向速度与期望横向速度的误差为：
[0107][0108]
由于无人潜航器的动力系统为欠驱动系统，没有横向方向的推力输出，要求出转向力矩τr，设计如下线性滑模面：
[0109][0110]
式中，μ2＞0为常数。
[0111]
为了进一步求取横向推力控制律，对式(35)关于时间求导得：
[0112][0113]
式中，为虚拟控制律a
vd
经过指令滤波器的一阶导输出值，
[0114]
为虚拟控制律a
vd
经过指令滤波器的二阶导输出值。同时结合式(1)有：
[0115][0116]
式中，τr为横向方向力矩的实际控制律。结合式(37)与式(1)，式(36)可表示为：
[0117][0118]
由式(38)结合式(14)得：
[0119][0120]
式中，λ2＞0；为干扰观测器对横向扰动的观测补偿值。
[0121]
为了证明所设计的横向方向力矩控制律τr的有效性，定义lyapunov函数：
[0122][0123]
对式(40)求导并将式(38)和式(39)代入得：
[0124][0125]
由于当t
→
∞时，因此有：
[0126][0127]
因此信号s2一致最终有界。
[0128]
与现有技术相比，本发明具有的有益效果为：
[0129]
1、本发明充分考虑了无人潜航器在执行完毕水下任务后回收过程中，速度传感器可能受到压强及潮湿环境影响导致观测精度降低，导致控制律控制精度降低甚至失效的问题，专门设计了基于位置向量的速度观测器。保证控制效果不受压强及潮湿环境的影响。
[0130]
2、充分考虑了系统内外部扰动及系统未建模部分扰动对无人潜航器布放回收过程的影响，基于速度观测器模型，设计了纵向力矩及横向力矩的扰动观测器，并在实际控制中基于扰动观测器的观测值对控制律进行补偿。
[0131]
3、考虑到无人潜航器的驱动器性能，利用指令滤波器对控制律的期望输出进行限制，与现有技术相比，本发明所设计控制律在跟踪上期望轨迹所用的的时间更长，但优点在于避免了因为输出力矩超过驱动器的最大输出性能导致控制律失效。
附图说明
[0132]
为了更清楚地说明现有技术中的技术方案和本发明的实施例，下面将对现有技术和实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
[0133]
图1是本发明的系统结构图；
[0134]
图2是本发明中无人潜航器运动学和动力学建模示意图；
[0135]
图3是本发明轨迹跟踪效果示意图；
[0136]
图4是本发明期望纵向位置响应与实际纵向位置响应对比图；
[0137]
图5是本发明期望纵向位置响应与实际纵向位置响应对比图；
[0138]
图6是本发明观测器对纵向速度观测值与纵向速度实际值对比图；
[0139]
图7是本发明观测器对横向速度观测值与横向速度实际值对比图；
[0140]
图8是本发明观测器对艏向角速度观测值与艏向角速度实际值对比图；
[0141]
图9是本发明纵向虚拟控制律的变化曲线；
[0142]
图10是本发明横向虚拟控制律的变化曲线；
[0143]
图11是本发明纵向速度控制律输入变化曲线；
[0144]
图12是本发明横向速度控制律输入变化曲线。
具体实施方式
[0145]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
[0146]
如图1所示，本发明提供了考虑速度传感器失效的无人潜航器轨迹跟踪控制方法，
包括如下步骤：
[0147]
s1、建立无人潜航器水面航行数学模型，设定期望轨迹数学模型。
[0148]
s2、设计基于位置信号的速度向量观测器，在此基础上设计纵向力矩和横向力矩方向上的干扰观测器，并通过lyapunov定理验证其有效性。
[0149]
s3、基于实时位置误差，设计纵向速度和横向速度的虚拟控制律，在虚拟控制律的基础上导入指令滤波器对虚拟控制律的幅值及速度进行约束，并解决求取实际控制律时需要对虚拟控制律进行高阶求导产生计算复杂度增加的问题，并通过lyapunov定理验证其有效性。
[0150]
s4、基于虚拟控制律及指令滤波器的输出值设计无人潜航器轨迹跟踪滑模控制器，同时基于干扰观测器所观察到的系统扰动估计值对纵向力矩和横向力矩的扰动进行补偿，并通过lyapunov定理验证其有效性。
[0151]
s5、仿真验证所提出方法的有效性。
[0152]
所述步骤s1具体包括：
[0153]
分别定义北东坐标系oxyz和附体坐标系obxbybzb两个坐标系，将北东坐标系xoy设为惯性坐标系，取地球上任意一点o作为坐标系的原点，ox指向正北，oy指向正东，oz垂直于ox、oy所在平面竖直向下。将附体坐标系obxbybzb作为惯性坐标系，假设无人潜航器体身左右对称，取其中心点为惯性坐标系的原点ob，obxb轴沿艇身指向艇艏方向，obyb垂直指向右舷,obzb垂直于艇身向下。对无人潜航器在水面航行过程进行动力学建模，得到包含纵荡(surge)、横荡(sway)、垂荡(heave)、横摇(roll)、纵摇(pitch)、艏摇(yaw)六自由度模型如图2所示。忽略高于二阶的水动力阻力项及垂荡(heave)、纵摇(pitch)、横摇(roll)运动，建立运动学和动力学模型为：
[0154][0155]
式中，η＝[x,y,ψ]
t
，x,y表示无人潜航器运动位置矢量，ψ∈[0,2π]表示艏向角，j(ψ)是北东坐标系与附体坐标系的转换矩阵，v＝[u,v,r]
t
表示附体坐标系下无人潜航器运动的速度向量，u、v、r分别表示纵荡、横荡、艏摇速度。表示包含附加质量的惯性矩阵，c(v)表示斜对称矩阵，表示阻尼矩阵，τ＝[τu,τv,τr]
t
表示力矩控制输入向量，τu、τv、τr分别表示纵荡、横荡、艏摇控制力，τd＝[τ
du
,τ
dv
,τ
dr
]
t
系统扰动力矩向量，τ
du
、τ
dv
、τ
dr
分别表示纵荡、横荡、艏摇方向上的扰动力矩。
[0156]
所述步骤s2具体包括：
[0157]
首先设计基于位置信号的速度向量观测器，由于速度向量v不可测量，因此消除模型中带有速度向量v的c(v)v项，导入变量矩阵p(η,v)：
[0158]
p(η,v)＝h(η)v (2)
[0159]
式中，h(η)3×3为速度向量v到p(η,v)的映射矩阵。
[0160]
对式(2)求导，得：
[0161][0162]
为了消除式(3)中带有速度向量v的项，导入变量y：
[0163][0164]
为了使y＝0从而消除式(3)中含有速度向量v的项，令：
[0165][0166]
于是有：
[0167]
p＝y-h(η)m-1
dh-1
(η)p h(η)m-1
(τ τd) (6)
[0168]
进而设计速度观测器为：
[0169][0170]
式中，为观测器对η、p的观测值，k1、k2为增益设计矩阵，为观测器的观测误差。
[0171]
为了证明所设计的速度向量观测器的有效性，定义观测器的观测误差为：
[0172][0173]
式中，为观测器的观测误差，为观测器观测值的位置误差。
[0174]
定义lyapunov函数：
[0175][0176]
式中，x1、x2为正定设计矩阵。对式(9)进行求导，并由式(8)得：
[0177][0178]
并且令：
[0179]
x1j(ψ)h-1
(η)＝x2k
2 (11)
[0180]
则有：
[0181][0182]
由lyapunov定理的定义可知，式(7)所示的观测器是渐近收敛的。
[0183]
在设计速度观测器基础上，对于未知扰动部分设计扰动观测器，对纵荡及艏摇方向的扰动进行在线估计，根据式(6)得：
[0184][0185]
式中，为扰动观测器对系统未建模部分及内外部未知扰动τd的观测值，k0∈r3×3为由正定参数组成的设计矩阵。
[0186]
在实际情况中，无法通过观测值直接获得，因此导入辅助变量进而设计扰动观测器为：
[0187][0188]
式中，为扰动观测器对系统未建模部分及内外部未知扰动τd的观测值，k0∈r3×3为由正定参数组成的设计矩阵。
[0189]
为了证明所设计的力矩扰动观测器的有效性，定义lyapunov函数：
[0190][0191]
式中，τd为系统扰动的真实值，为参数据估计误差。
[0192]
对式(15)求导可得：
[0193][0194]
由此可知式(14)所示的干扰观测器的观测误差在有限时间内渐进收敛于零。
[0195]
所述步骤s3具体包括：
[0196]
基于实时位置误差，设计纵向速度和横向速度的虚拟控制律。首先，定义实际位置与期望轨迹位置之间的误差为：
[0197][0198]
式中，xd为期望轨迹的横坐标，yd为期望轨迹的纵坐标，x为实际跟踪位置的横坐标，y为实际跟踪位置的纵坐标。
[0199]
求取式(17)一阶导，并结合无人潜航器运动学和动力学模型式(1)得：
[0200][0201]
为了使位置跟踪误差收敛于零，设计纵向虚拟控制律a
ud
和横向虚拟率a
vd
为：
[0202]
[0203]
式中，其中，k＞0，c＞0为设计常数。
[0204]
为防止虚拟控制的输出超出无人潜航器推进器所能提供力矩的最大值，并解决在求取实际控制律的过程中需要对虚拟控制律高阶求导而产生计算复杂度增加问题，使用指令滤波器对虚拟控制律的速度和幅值进行限制，需要将纵向虚拟控制律a
ud
和横向虚拟率a
vd
作为指令滤波器的输入，指令滤波器的输出及
[0205]
用于实际控制律的求取。
[0206]
指令滤波器的定义：
[0207]
为解决求取实际控制律可能会由于需要对虚拟控制律求导而产生的微分复杂性膨胀问题，为减少反推计算的工作量，并且为防止输入饱和导致控制律失效的问题，需要对虚拟控制律的幅值和速度进行限制；同一阶滤波器相比，指令滤波器还能处理中间状态变量的约束。因此导入指令滤波器对虚拟控制律进行处理。
[0208]
定义指令滤波器的状态空间表达式为：
[0209][0210]
式中，x
id
为该滤波器的输入，z1为该滤波器的输出，ζ和ω分别是指令滤波器的阻尼和带宽。当不考虑幅值和速率的的作用时，x
id
和z1之间的线性关系为：
[0211][0212]
当输入x
id
有界时，则输出z1、同样有界，并在ω比较大的情况下，|z
1-x
id
|趋于零。
[0213]
为证明虚拟控制律的有效性，定义lyapunov函数：
[0214][0215]
由式(19)得：
[0216][0217]
将式(22)代入式(18)有：
[0218][0219]
当(u-a
ud
)与(v-a
vd
)趋于0时：
[0220][0221]
对式(22)求导，并结合式(25)得：
[0222][0223]
由此可知，式(19)所示的虚拟控制律可使位置误差xe、ye渐进收敛于零。因此虚拟
控制律也可视为使无人潜航器达到目标位置的速度期望。
[0224]
所述步骤s4具体包括：
[0225]
设计纵向推力控制律及横向推力控制律；首先设计纵向推力控制律，设纵向速度与期望纵向速度的误差ue为：
[0226][0227]
式中，为速度观测器对纵向速度u的观测值，a
ud
为纵向速度的虚拟控制律。
[0228]
设计关于纵向速度误差的积分型滑模面s1：
[0229][0230]
式中，μ1＞0为常数。
[0231]
为了进一步求取纵向推力控制律，对式(28)进行求导，并结合式(1)得：
[0232][0233]
式中，为观测器对速度向量u、v、r的估计值，τu为纵向推力的实际控制律，为虚拟控制律a
ud
经过指令滤波器的输出值，m
11
、m
22
、d
11
分别为式(1)中附加质量的惯性矩阵m及阻尼矩阵d中的系数。
[0234]
进而，由式(29)及式(14)得纵向推力控制律τu为：
[0235][0236]
式中，λ1＞0；为式(7)中干扰观测器对纵向扰动的观测补偿值。
[0237]
为证明所设计纵向推力控制律τu的有效性，定义lyapunov函数：
[0238][0239]
对式(31)求导并将式(29)和式(30)代入得：
[0240][0241]
由于当t
→
∞时，因此有：
[0242][0243]
因此信号s1一致最终有界。
[0244]
其次设计横向推力控制律，设横向速度与期望横向速度的误差为：
[0245][0246]
由于无人潜航器的动力系统为欠驱动系统，没有横向方向的推力输出，要求出转向力矩τr，设计如下线性滑模面：
[0247][0248]
式中，μ2＞0为常数。
[0249]
为了进一步求取横向推力控制律，对式(35)关于时间求导得：
[0250][0251]
式中，为虚拟控制律a
vd
经过指令滤波器的一阶导输出值，
[0252]
为虚拟控制律a
vd
经过指令滤波器的二阶导输出值。同时结合式(1)有：
[0253][0254]
式中，τr为横向方向力矩的实际控制律。结合式(37)与式(1)，式(36)可表示为：
[0255][0256]
由式(38)结合式(14)得：
[0257][0258]
式中，λ2＞0；为干扰观测器对横向扰动的观测补偿值。
[0259]
为了证明所设计的横向方向力矩控制律τr的有效性，定义lyapunov函数：
[0260][0261]
对式(40)求导并将式(38)和式(39)代入得：
[0262][0263]
由于当t
→
∞时，因此有：
[0264][0265]
因此信号s2一致最终有界。
[0266]
所述步骤s5中，对无人潜航器布放回收过程的轨迹跟踪控制进行仿真验证，验证方法的有效性，具体包括：
[0267]
定义参考轨迹如下：
[0268]
[0269]
设定纵向及横向未建模部分及系统内外部扰动τ
du
和τ
dr
为：
[0270][0271]
设定无人潜航器动力学方程m,c,d矩阵为：
[0272][0273]
仿真相关参数μ1＝1，μ2＝0.02，λ1＝1，λ2＝1，ζ1＝0.9，ζ2＝0.9，ω1＝20rad/s，ω2＝20rad/s，设计矩阵k0、k1、k2设为：
[0274]
k0＝[2,0,2]
t
,
[0275]
无人潜航器初始位置[x(0),y(0),ψ(0)]
t
＝[0m,0m,0.5rad]
t
，初始状态[u(0),v(0),r(0)]
t
＝[3m/s,0m/s,0rad/s]
t
。
[0276]
期望轨迹跟踪路径与实际轨迹跟踪路径对比如图3所示，结果表明本发明所提出的控制方法可以很好地让无人潜航器在期望路径行驶，并一直保持稳定状态，具有很好的性能。
[0277]
无人潜航器位置跟踪变化曲线如图4-图5所示，在83s时，纵向位置跟踪上期望路径，在189s时，横向位置跟踪上期望路径。
[0278]
观测器对无人潜航器状态的观测值如图6-图8所示，表明速度向量能够被快速准确地观测。
[0279]
横向及纵向虚拟控制律的输出如图9-图10所示，纵向及横向的实际控制律的输出如图11-图12所示，可以看出控制律曲线较为光滑，具有很好的鲁棒性。
[0280]
从图9-图10所示的纵向及横向实际控制律的变化曲线可以看出，从80s-120s区间有较为强烈的抖振现象，结合式(30)与式(39)并结合图9-图10可以看出，抖动现象来自于虚拟控制律从80s-120s区间的强烈抖动。结合式(17)-式(19)虚拟控制律的推导过程可以看出，虚拟控制律的本质实质上表示的是无人潜航器从当前的实际位置到期望位置的过程中纵向速度和横向速度的期望值，因此虚拟控制律与当前的纵向横向实际位置和期望的纵向横向位置高度相关，换句话说，造成虚拟控制律抖动的本质原因是式(17)所示的实际位置与期望轨迹位置之间的误差值造成的。结合图4-图5所示的纵向位置与横向位置的关系不难看出，在第80s时，实际纵向位置与期望纵向位置重合，但此时系统没有完全稳定，因为实际横向位置与期望横向位置没有重合，在此过程中，横向实际控制律一直起作用，不断调整横向速度的大小，但由于无人潜航器是欠驱动系统，纵向速度会受到横向速度分量的影响，这时对于纵向推力控制律而言必须应对来自横向速度的分量影响，而纵向推力控制律又是基于速度差的积分滑模面设计的，因此纵向推力控制律为了保持稳定在滑模面附近来回切换，导致图11所示纵向控制律输出的抖振。结合图4和图6可以看出，纵向推力控制律通过滑模面附近来回切换很好的保证了从83秒开始纵向实际位置精确地跟踪上纵向期望位置并且不受横向推力的影响而偏移。同理，对于横向推力控制律而言，在80s时刻之前，由于
纵向速度没有被约束，横向推力控制律可以自由地控制调整无人潜航器的横向速度，进而使无人潜航器的横向实际位置跟踪上期望实际位置，但在80s时刻之后，由于纵向速度受到推力纵向控制律的约束，并且无人潜航器是欠驱动系统，横向速度的变化必然受到纵向速度分量的影响，这部分影响来自于纵向控制律在其滑模面附近切换过程中的干扰，由于在80s之后纵向推力控制律以抖振的形式保证纵向跟踪效果，因此这部分干扰也以抖振的形式影响横向速度，在80s-120s过程中，横向推力控制律在保证实际横向位置达到期望横向位置的过程中同时要应对纵向速度的分量带来的扰动，横向推力控制律为应对这些扰动产生抖振，当实际横向位置跟踪上期望横向位置的过程中，横向推力控制律输出逐渐减小，对纵向速度的影响也同样减小，因此横向推力控制律和纵向推力控制律的抖振会同步逐渐减小，直到式(17)中的ye＝0，式(34)中ve＝0，进而式(35)中s2＝0，系统整体达到稳定状态横向控制律中抖振完全消失。
[0281]
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。