全驱动型无人水下航行器(unmanned underwater vehicle，UUV)因应用简便、性能优越，在水下勘探、布线甚至是各种水下复杂军事任务中发挥着越来越重要的作用[1]．UUV执行坐底作业、回收对接等任务需要良好的悬停能力，因此悬停是UUV的一种常见的运动模式．悬停控制是指在环境干扰下自动运动到并保持在期望的位置及姿态上[2]．但是UUV模型存在强耦合性、强非线性及时变未知环境干扰等问题，使得悬停控制系统的稳定性和精度均会受到影响[3]．此外由于水下复杂的环境，UUV的速度及干扰状态也难以准确测得[4]．因此，设计抗扰性能与鲁棒性强的悬停控制器对于UUV运动控制问题的研究十分重要．针对上述问题，研究者们已经做了大量工作．文献[5]将UUV的运动模型解耦成水平面与垂直面，针对UUV非线性强、模型参数获取困难的问题，采用了滑模控制的方法．文献[6]在UUV模型解耦的基础上，提出了对折型在线插值的模糊自适应控制器和二次分配的方法，解决考虑推进器约束的UUV定位控制．文献[7-8]针对模型的不确定性、海流、海浪、传输信号干扰等典型内外扰动下的UUV垂直面悬停控制，采用了自抗扰的控制方法．文献[9]基于垂直面的三自由度模型，采用反步法设计了具有PID控制器参数调节形式的垂直面悬停控制器．文献[10]针对未知定常海流干扰下的UUV垂直面悬停控制，提出了参数自适应的反步滑模控制方法．文献[11]考虑在回收过程中UUV会受到海流干扰和近壁面干扰的影响，以解耦后的模型为基础，设计了UUV回收时的自抗扰悬停控制器．综上所述，目前主流的UUV悬停控制方法是采用基于模型的鲁棒控制或观测器干扰补偿的策略来消除复杂海洋环境干扰的影响．为了设计方便，将模型解耦进行处理，对于采用扩张状态观测器(linear extended state observer，LESO)进行干扰补偿的级联控制策略，忽略了级联控制系统中观测器误差对控制器的影响．本研究针对具有模型不确定性、未知时变环境干扰和强耦合非线性的UUV，引入LESO对系统状态进行在线估计，并根据UUV的标称模型及LESO的输出，采用反步法设计控制器，引入动态面技术求解虚拟控制律的导数．控制器中系统的未知部分由LESO估计得到．LESO产生的估计误差由控制律中的自适应鲁棒项进行补偿，采用非线性Fal函数构造鲁棒项，未知参数由自适应律得到．然后根据李雅普诺夫稳定性原理证明LESO的收敛性和LESO输入的级联控制系统信号的一致渐进有界．最后以一艘小型全驱动型UUV模型进行仿真验证．1 UUV数学模型为描述UUV的位置和姿态等运动变量，定义所需坐标系．建立如图1所示的北东坐标系({n}坐标系，E-XYZ)和体坐标系({b}坐标系，B-XYZ)来描述UUV的运动控制问题[12]．10.13245/j.hust.210819.F001图1{n}坐标系与{b}坐标系低速航行下UUV的运动学和动力学模型可以描述为η˙=R(η)v;Mv˙+Dv+g(η)=u+RT(η)b, (1)式中：η∈R6为UUV在{n}坐标系的位置及姿态角；R(η)∈R6×6为{n}坐标系到{b}坐标系的坐标转换矩阵，具体表示见文献[5]；M=M0+ΔM∈R6×6为系统惯性矩阵，其中M0为标称的系统惯性矩阵，ΔM为模型参数和水动力导数引起的不确定惯性项；D=D0+ΔD∈R6×6为系统阻尼矩阵，其中D0为标称的系统阻尼矩阵，ΔD为模型参数摄动引起的不确定阻尼项；v∈R6为UUV在{b}坐标系下的速度；g(η) ∈R6为重力与浮力产生的力与力矩[13]；u=[ux,uy,uz,uk,um,un]T为UUV的控制输入，不同下标分别表示{b}坐标系下纵向、横向、垂向、横摇、纵摇和艏摇控制力(矩)；b∈R6为未知环境干扰．假设1 未知环境干扰b变化缓慢，且存在b*0，对于任意时间t0，满足||b||≤b*．假设2 参数引起的模型不确定项ΔM和ΔD满足：存在常数δ1,δ20，对于任意时间t0，||ΔM||≤δ1，||ΔD||≤δ2成立，其中||∙||为欧几里得范数．由于海洋环境及控制输入能量有界，因此上述假设合理．2 线性扩张状态观测器设计与分析观测器的目标是通过系统的输入输出数据，将系统的位置、速度和未知干扰状态在线估计出来．因此，定义干扰状态χ=[χx,χy,χz,χk,χm,χn]T，其中χ=M0-1(-ΔMv˙-ΔDv-g(η)+RT(η)b)表示由于UUV模型不确定性、时变未知环境干扰的未知状态，无法通过测量设备直接得到，因此须要设计合理的LESO估计．假设3 系统的扰动状态χ对所有自变量是连续可微的．假设4 存在δ30，对于任意时间t，使得χ关于时间的导数χ˙=h(η,v,t)满足||h(η,v,t)||≤δ3[14]．模型(1)重写为[14]：η˙=R(η)v;v˙=-M0-1D0v+χ+Bu;χ˙=h(η,v,t), (2)式中B=M0-1为控制输入矩阵．基于扩张状态的系统模型(2)，设计LESOη̂˙=R(η)v̂+K1(η-η̂);v̂˙=-M0-1D0v̂+χ̂+Bu+RT(η)K2(η-η̂);χ̂˙=RT(η)K3(η-η̂), (3)式中：η̂，v̂和χ̂分别为系统状态η，v和χ的估计；Kj (j=1,2,3)为观测器增益矩阵．根据式(2)和(3)定义观测误差e1=[η˜T,v˜T,χ˜T]T，其中η˜=η-η̂，v˜=v-v̂，χ˜=χ-χ̂，系统的误差动态方程为e˙1=A1e1+Bhh(η,v,t)，(4)式中：A1=-K106×6R(η)-RT(η)K2-M0-1D0I6×6-RT(η)K306×606×6，I为单位矩阵；Bh=06×606×6I6×6．定理1 假设1～4成立，且e1为系统(4)的解，观测器增益参数满足[14]K1K2K3．对于给定正定矩阵QT=Q，存在正定矩阵P，满足李雅普诺夫方程A1P+PA1T=-Q，并且存在一个连续的李雅普诺夫函数V1=0.5e1TPe1．(5)若对于常数μ10，ε10，有V˙1≤-μ1V1+ε1成立，则系统(4)的解是一致最终有界的，最终的界为2ε1/[μ1λmin(P)]，当μ1λmin(P)→∞时，||e1||→0，其中λmin为矩阵的最小特征值．该定理的证明过程参考文献[14]．3 鲁棒动态面控制器设计3.1　控制器设计将反步法与滑模控制方法相结合来设计鲁棒动态面控制器，实现在模型不确定及未知环境干扰下的UUV悬停控制．定义期望点为ηd，期望速度vd=0，LESO输入下的控制器的设计如下．步骤1 定义位移误差矩阵为z1，即z1=η̂-ηd．(6)为保证式(6)收敛，构造李雅普诺夫函数V21=z1Tz1/2．(7)式(7)关于时间的导数为V˙21=z1Tz˙1=z1T(R(η)(v̂-vd)+K1η˜)．(8)为了保证式(8)负定，引入速度的虚拟控制量α=-RT(η)(C1z1+D1z1)，(9)式中C1和D1为正定对角矩阵，D1=d1K1TK1用于镇定观测误差的影响，d1为正定对角矩阵．为了避免虚拟控制量(9)求导过程计算复杂的问题，引入一阶低通滤波器进行处理，即τ1α̂˙+α̂=α，式中：α̂为α的估计值；τ1为低通滤波器参数．定义低通滤波器滤波误差α˜=α-α̂步骤2 定义速度误差矩阵z2，即z2=v̂-α̂．构造系统的第二个李雅普诺夫函数V22=z2TM0z2/2+α˜Tα˜+V21/2．(10)对式(10)求导，代入式(8)可得V˙22=z1T(R(η)(α-α˜+z2)+K1η˜)+α˜T(α˙-α˜/τ1)+z2TM0(-M0-1D0v̂+χ̂+Bu+RT(η)K2η˜-α̂˙).因此基于LESO输入的鲁棒动态面控制律为u=D0v̂-M0χ̂+M0α̂˙-C2z2-θ̂f(z2/κ,σ,δ),式中：C2为正定对角矩阵；κ0；f(∙)为Fal函数，f(z2/κ,σ,δ)=[f1(z21/κ,σ,δ),f2(z22/κ,σ,δ)，⋯,f6(z26/κ,σ,δ)]T,fi(z2i/κ,σ,δ)的具体表达式为fiz2iκ,σ,δ=z2iκδσ-1    z2iκ≤δ;z2iκσsignz2iκ    z2iκδ,其中，z2i (i=1,2,⋯,6)为z2的第i个元素，σ和δ为正常数；θ̂=diag{θ̂11,θ̂22,θ̂33,θ̂44,θ̂55，θ̂66}为反映LESO观测误差边界θ的自适应参数，具体形式为θ̂˙ii=γ1[z2ifi(z2i/κ,σ,δ)-γ2θ̂ii]，其中γ1和γ2为正常数．3.2　稳定性分析定理2 对于存在模型不确定和时变位置环境干扰的UUV系统，在假设1～4及定理1成立的前提下，若设计的LESO输入下的鲁棒动态面控制器参数满足：λmin(C1)1；λmin(C2)1；λmin(I/τ1)1，则级联控制系统的所有误差信号e1，z1和z2一致最终有界．证明 根据式(5)、(7)和(10)，构造LESO输入下的鲁棒动态面控制系统的李雅普诺夫函数V2=V1+V22=e1TPe1/2+z1Tz1/2+z2TM0z2/2+α˜Tα˜/2+∑i=16θ˜ii2/(2γ1), (11)式中θ˜ii=θii-θ̂ii，θii为反映观测误差边界的参数．式(11)求导得V˙2=z1Tz˙1+z2TM0z˙2+∑i=16θ˜iiθ˜˙ii/γ1+α˜T(α˙-α˜/τ1)+V˙1. (12)应用Young不等式[15]，对于z1Tz˙1有如下不等式关系z1Tz˙1≤-z1T(C1+D1)z1+z1Tz1+z2Tz2/2+α˜Tα˜/2+z1TK1η˜.根据文献[16-17]，选取合适的正定矩阵Λ1，满足z1Tz˙1≤-z1T(C1-I)z1+z2Tz2/2+α˜Tα˜/2+η˜TΛ1η˜/4.对于z2TM0z˙2，满足如下关系z2TM0z˙2=-z2TC2z2-z2Tθ̂f(z2/κ,σ,δ)+z2TM0K2η˜,式中z2TM0K2η˜=θz2，满足θz2-z2Tθf(z2/κ,σ,δ)≤0.25∑i=16θii，(13)其中：当|z2i/κ|≤δ时，β=1；当|z2i/κ|δ时，β=α．则式(13)变为：z2TM0z˙2≤-z2TC2z2+0.25∑i=16θii+∑i=16θ˜ii(z2i)1+β；1γ1∑i=16θ˜iiθ˜˙ii≤∑i=16θ˜ii2+θ˙ii22γ1-θ˜ii(z2i)1+β.根据θii的定义可知，θii的大小和LESO观测误差有关[18]，存在δ40,δ50，满足：∑i=16θ˜ii2+θ˙ii22γ1≤δ422γ1；0.25∑i=16θii≤0.25δ5.对于α˜T(α˙-α˜/τ1)，由文献[19]可知，α˙为连续向量函数，且存在δ60，满足||α˙||≤δ6．α˜T(α˙-α˜/τ1)≤(-1/τ1+1/2)α˜Tα˜+δ62/2．定义ε2=0.25δ5+δ42/(2γ1)+δ62/2；μ2=min{λ(C1-I),λ(C2-I),λ(1/τ1-1),λ(Q1-0.25I),λ(Q2),λ(Q3)};μ3=max{λ(P),λ(M0),1}，式中：λ为矩阵的特征值；Qj (j=1,2,3)为Q=diag{Q1;Q2;Q3}的子矩阵．式(12)满足V˙2≤-2μ2V2/μ3+ε2，因此系统位置跟踪误差满足||z1||≤μ3ε2μ2+2V2(0)-μ3ε22μ2exp-μ3t2μ2，式中V2(0)为李雅普诺夫函数V2的初值．当t→∞时，满足||z1||≤μ3ε2/μ2，若增大观测器与控制律参数，即增大μ2，使位置、姿态跟踪误差可以收敛到任意小，则系统误差信号一致最终有界．定理2证毕．4 仿真结果与分析以一艘多推进器的全驱动UUV为研究对象，验证所提出的基于LESO的自适应鲁棒控制算法，仿真对象的M0和D0矩阵为：M0=156.660.000.000.000.000.000.00211.190.000.000.001.910.000.00299.580.00-14.930.000.000.000.002.980.000.000.000.00-37.160.0062.760.000.00-1.940.000.000.0030.02；D0=30.560.000.000.000.000.000.0037.470.000.000.00-2.790.000.00203.660.0090.090.000.000.000.000.000.000.000.000.00-114.770.0063.280.000.0035.180.000.000.0016.40．UUV的初始位置和姿态角η0=[100 m,100 m，500 m,0°,0°,60°]T，初始速度v0=[0 m/s，0 m/s,0 m/s,0 °/s,0 °/s,0 °/s]T，期望位置ηd=[100 m,100 m,500 m,0°,0°，60°]T，系统的输入以文献[20]的变预设参考轨迹的方法给出，采样步长为0.5 s，仿真时长为5 000个周期．假定零时刻遭遇{n}坐标系下0.1 m/s的平面流的定常干扰，流向角为北偏东20°，加入时变干扰b，并在1 000 s时垂向加入14 N的突变载荷(模拟坐底作业或者机械臂操作时的载荷)，b=cos(πt/800)[2 N,2 N,2 N,0 N•m,0 N•m,0 N•m]T.在1 300 s时加入模型变化干扰，ΔM=0.1M0，ΔD=0.1M0，控制输入力的上下限为±1 N/s，控制输入力矩的上下限为±1 (N•m)/s．在此基础上，设计包括本文方法在内的三种控制律，对UUV进行悬停运动仿真．a. 方法一uc1=D0v̂-M0χ̂+M0α̂˙-C2z2-θ̂f(z2/κ,α,δ).b. 方法二[7]uc2=-KPRT(η̂)(η̂-ηd)-Kd(v̂-vd)-M0χ̂.c. 方法三[6]uc3=-KPRT(η̂)(η̂-ηd)-Kd(v̂-vd)-KiRT(η̂)(ξ̂-ξd),式中：ξ̂˙=η̂；ξ˙d=η˙d；KP，Ki和Kd为增益参数矩阵．LESO增益参数为K1=diag{2,2,2,2,2,2};K2=diag{0.75,0.75,0.75,0.75,0.75,0.75};K3=diag{0.008,0.01,0.008,0.01,0.012,0.012}.方法二、方法三控制器参数为Kp=diag{15.67,21.12,19.96,2.98,12.5,20};Ki=diag{0.22,0.3,0.42,0.004,0.088,0.14};Kd=diag{62.67,84.5,120,1.2,25.1,12}.方法一控制器参数为C1=diag{1.2,1.8,1.8,1.2,1.8,1.8};C2=diag{7.68,7.68,15.36,7.68,7.68,7.68};D1=diag{0.6,0.6,0.6,0.6,0.6,0.6}.仿真结果如图2～7所示．图2和图3为{n}坐标系下的位置、姿态角误差仿真结果．其中：x˜，y˜和z˜分别表示北向、东向、垂向位置的控制误差；ϕ˜，θ˜和ψ˜分别表示横摇、纵摇、艏摇姿态的控制误差．三种控制方法都能最终收敛到期望位置和姿态上，同时保证了较小的超调量．当UUV在运动过程中存在因耦合产生的扰动及突变的环境扰动的影响时，方法一的控制误差比较平稳，方法二和方法三的控制都存在较大偏移，但方法二能较快稳定，方法三的误差存在更剧烈的误差震荡现象．因此，方法一的跟踪精度更高，对环境干扰及模型耦合具有更强的鲁棒性．10.13245/j.hust.210819.F002图2UUV悬停运动的位置误差仿真结果10.13245/j.hust.210819.F003图3UUV悬停运动的姿态误差仿真结果10.13245/j.hust.210819.F004图4UUV悬停运动控制力输出结果10.13245/j.hust.210819.F005图5UUV悬停运动控制力矩输出结果10.13245/j.hust.210819.F006图6LESO未知干扰力的估计结果10.13245/j.hust.210819.F007图7LESO未知干扰力矩的估计结果图4和图5分别为两种控制器的控制力与力矩输出结果．可以看出：三种控制器输出均满足变化率约束，但当1 000 s时，方法二和方法三纵向、横向、横摇、纵摇的控制输出变化会更剧烈．图6和图7分别为LESO对未知扰动估计的结果，可以看出：LESO能够比较好地估计出时变环境干扰、模型不确定性及模型耦合的总和，但是存在一定的相位滞后．根据对图2和图3的分析可知：方法一可以很好地抑制这种滞后所带来的观测误差，避免了位置及姿态结果产生波动．为了定量分析三种控制方法的优劣，用各自由度上控制偏差的最大范数max{||ηi(t)-ηdi(t)||}  (i=1,2,⋯,6)表示各自由度上与系统输入的最大偏离程度．||x˜||max，||y˜||max，||z˜||max，||ϕ˜||max，||θ˜||max和||ψ˜||max表示各自由度的最大误差范数，具体数据见表1．同另两种方法相比，方法一设计的控制器在时变环境干扰、模型不确定干扰下偏离期望位置、姿态最小，具有良好的位置及姿态保持性能．10.13245/j.hust.210819.T001表1控制结果最大偏差范数比较参数方法一方法二方法三||x˜||max/m0.0050.0410.110||y˜||max/m0.0220.1050.107||z˜||max/m0.2550.7030.759||ϕ˜||max/(°)0.0310.0660.067||θ˜||max/(°)7.81123.21023.803||ψ˜||max/(°)1.8132.2512.1125 结语基于六自由度的UUV模型，研究了时变环境干扰及模型不确定下的全驱动型UUV悬停控制问题，提出了基于LESO的鲁棒动态面控制器的解决方法．在利用LESO对系统的未知外界干扰及模型不确定性进行在线估计的基础上，根据标称模型及反步法设计了控制器，并利用动态面控制技术解决了虚拟控制律求导计算复杂的问题，在控制律中加入自适应鲁棒项，避免了级联控制系统中LESO观测误差对控制器的影响．以一艘全驱动配置的UUV模型的仿真结果验证了该方法的有效性，结果表明该方法对时变环境干扰、模型不确定性和观测误差具有更强的鲁棒性．