一种X型舵AUV鲁棒精确控制方法与流程

一种x型舵auv鲁棒精确控制方法
技术领域
1.本发明涉及auv控制技术领域，特别涉及一种x型舵auv鲁棒精确控制方法。


背景技术：

2.智能水下机器人(autonomous underwater vechicle，auv)凭借其无人自主工作特性，在科研、民用、军用领域受到了广泛关注，auv控制技术是实现其自主航行作业的核心技术和关键能力。配备x型艉舵的auv在安全性、水动力性能等方面优于传统十字舵auv。因此，x型舵auv控制受到了大量关注，具备广阔需求。
3.auv在航行中现有研究中面临如下问题：
4.1.大部分auv是一类欠驱动运动载体，需要运动学控制器生成期望艏向角和纵倾角，引导auv到达指定位置。但海流干扰造成的侧滑角容易导致auv运动学控制器存在静差，降低控制精度。
5.2.auv动力学模型具有高度非线性，包含各类水动力参数，模型难以精确获取，而且auv在运动过程中易受海流干扰，容易影响控制精度和鲁棒性。传统pid控制应用于具有高度非线性特性的auv上，容易引起超调、振荡等问题，同时也容易导致执行机构饱和，鲁棒性和精确性均有一定缺陷。滑模控制是一种鲁棒控制方法，能在一定程度上解决模型不确定及外界扰动等问题，但传统滑模控制同时也面临输出抖振问题，容易影响执行机构寿命。
6.3.x舵auv控制器输出是力矩形式，需要经过控制分配转化成各指令舵角。传统伪逆法实时性强，但未考虑舵角输出极限；而另一类基于非线性优化的方法，如sqp法等，虽然考虑输出限制，但算法复杂，占用了auv较大的计算资源，实时性较差。
7.因此，在现有auv航行控制的基础上，如何解决传统方法中由于auv海流干扰、模型不确定等原因造成的控制精度低、鲁棒性差、依赖模型参数等问题，成为本领域技术人员亟需解决的问题。


技术实现要素：

8.鉴于上述问题，本发明提出了一种至少解决上述部分技术问题的x型舵auv鲁棒精确控制方法，该方法可有效降低模型不确定性、外界干扰对auv控制的影响。
9.本发明实施例提供一种x型舵auv鲁棒精确控制方法，包括：
10.获取auv艇体的位置、姿态、线速度和角速度，建立运动学模型、动力学模型以及x分配模型，构成x舵auv数学模型；
11.分别获取水平面和垂直面的当前auv所处位置以及航路信息，建立水平面运动学控制器，设计垂直面运动学控制律；
12.设计非线性干扰观测器对所述x舵auv数学模型和垂直面运动学控制律的未建模动态进行估计和补偿，生成auv高阶滑模控制器；
13.基于阻尼矩阵对所述auv高阶滑模控制器解算的控制输出进行x舵的控制分配。
14.进一步地，所述运动学模型为：
[0015][0016][0017][0018][0019][0020][0021]
上式中，向量[x,y,z]表示大地坐标系下auv艇体的位置；欧拉角向量[φ,θ,ψ]表示艇体姿态；[u,v,w]表示艇体坐标系下艇体的线速度，[p,q,r]表示艇体坐标系下艇体的角速度。
[0022]
进一步地，所述动力学模型为：
[0023][0024][0025][0026][0027][0028][0029]
上式中，m表示auv质量；fr表示动力学模型已知项；dr表示模型不确定项。
[0030]
进一步地，所述x分配模型为：
[0031][0032]
上式中，k
δi
、m
δi
、n
δi
，i＝1,
…
,4分别为4个舵在横倾的水动力系数、纵倾的水动力系数、以及回转自由度的水动力系数；δi，i＝1,
…
,4为x舵的4个舵角。
[0033]
进一步地，通过如下方式建立水平面运动学控制器：
[0034]
分别获取水平面的当前auv所处位置点以及当前航段的起始航路点、目标航路点和下一航路点的二维坐标；
[0035]
计算第一直线航迹段的航迹角；所述第一直线航迹段由所述水平面的当前航段的起始航路点和目标航路点组成；
[0036]
从所述水平面的当前auv所处位置点作垂线到所述第一直线航迹段，根据所述第一直线航迹段的航迹角，得到auv水平面的航迹偏差；
[0037]
消除所述auv水平面的航迹偏差，计算干扰估计和侧滑角估计值，生成水平面运动学控制器。
[0038]
进一步地，通过如下公式计算所述第一直线航迹段的航迹角：
[0039]
ψk＝atan2(y
k+1-yk,x
k+1-xk)
[0040]
上式中，(xk,yk)表示当前航段的目标航路点的水平面二维坐标；(x
k+1
,y
k+1
)表示当前航段的下一航路点的水平面二维坐标。
[0041]
进一步地，所述auv水平面的航迹偏差通过如下公式计算：
[0042]
e＝-(x-x
k-1
)sin(ψk)+(y-y
k-1
)cos(ψk)
[0043]
上式中，(x
k-1
,y
k-1
)表示当前航段的起始航路点的水平面二维坐标；(x,y)表示水平面的当前auv所处位置点；ψk表示第一直线航迹段的航迹角。
[0044]
进一步地，所述消除auv水平面的航迹偏差，计算干扰估计，包括：
[0045]
对所述auv水平面的航迹偏差进行求导：
[0046][0047]
上式中，v表示auv的水平面航行速度；β表示海流干扰造成的侧滑角；ψk表示第一直线航迹段的航迹角；
[0048]
假设g＝vβcos(ψ-ψk)；
[0049]
通过下式对g进行干扰估计：
[0050][0051]
上式中，为g的估计值；kg为观测器的参数；pg为观测器的辅助变量；v表示auv的水平面航行速度；e表示auv水平面的航迹偏差；ψk表示第一直线航迹段的航迹角。
[0052]
进一步地，通过如下公式计算侧滑角估计值：
[0053][0054]
上式中，为g的估计值；v表示auv的水平面航行速度；ψk表示第一直线航迹段的航迹角。
[0055]
进一步地，通过如下方式设计所述垂直面运动学控制律：
[0056]
分别获取垂直面的当前auv所处位置点，以及当前航段的起始航路点和目标航路点的二维坐标；
[0057]
计算第二直线航迹段的潜浮角；所述第二直线航迹段由所述垂直面的当前航段的起始航路点和目标航路点组成；
[0058]
从所述垂直面的当前auv所处位置点作垂线到所述第二直线航迹段，根据所述第二直线航迹段的潜浮角，得到auv垂直面的航迹偏差；
[0059]
消除所述auv垂直面的航迹偏差，引入积分作用的垂直面运动学控制律。
[0060]
进一步地，所述积分作用的垂直面运动学控制律为：
[0061][0062]
上式中，θd为垂直面的期望艏向角，θk为第二直线航迹段的潜浮角，eh为auv垂直面的航迹偏差，δ为前视距离，ki为积分参数。
[0063]
进一步地，所述auv高阶滑模控制器为：
[0064][0065][0066][0067][0068]
上式中，k1、k2为控制器增益；su、s
θ
、s
ψ
为滑模面；sgn为符号函数；eu、e
θ
、e
ψ
为误差，为动力学模型中gu、g
p
、gq、gr的估计值；表示观测器估计的模型不确定性与海流共同造成的干扰。
[0069]
进一步地，基于阻尼矩阵对所述auv高阶滑模控制器解算的控制输出进行x舵的控制分配，为：
[0070]
y＝(w-1bt
b)-1
w-1bt
τ
[0071]
上式中，τ＝[τ
p
,τq,τr]
t
为auv高阶滑模控制器解算的控制输出；w为阻尼矩阵，w＝diag(w1,w2,w3,w4)，wi,i∈{1,2,3,4}；b为x舵控制分配矩阵。
[0072]
进一步地，考虑舵角输出极限，wi满足：
[0073][0074]
上式中，ws为常数；ui,i∈{1,2,3,4}为auv高阶滑模控制器的舵角输出；δ
min,i
为对应舵角的最小极限；δ
max,i
为对应舵角的最大极限；为预设大于0的常数。
[0075]
本发明实施例提供的上述技术方案的有益效果至少包括：
[0076]
本发明实施例提供的一种x型舵auv鲁棒精确控制方法，包括：获取auv艇体的位置、姿态、线速度和角速度，建立运动学模型、动力学模型以及x分配模型，构成x舵auv数学模型；分别获取水平面和垂直面的当前auv所处位置以及航路信息，建立水平面运动学控制器，设计垂直面运动学控制律；设计非线性干扰观测器对x舵auv数学模型和垂直面运动学控制律的未建模动态进行估计和补偿，生成auv高阶滑模控制器；基于阻尼矩阵对auv高阶滑模控制器解算的控制输出进行x舵的控制分配。该控制方法可有效降低模型不确定性、外界干扰对auv控制的影响。
[0077]
本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变
得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。
[0078]
下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。
附图说明
[0079]
附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：
[0080]
图1为本发明实施例提供的x型舵auv鲁棒精确控制方法流程图；
[0081]
图2为本发明实施例提供的x舵auv控制器结构图；
[0082]
图3为本发明实施例提供的视线导航法示意图；
[0083]
图4为本发明实施例提供的auv动力学控制器结构图；
[0084]
图5为本发明实施例提供的auv三维路径跟踪控制仿真效果图；
[0085]
图6为本发明实施例提供的auv水平面轨迹控制仿真效果图；
[0086]
图7为本发明实施例提供的auv垂直面轨迹控制仿真效果图。
具体实施方式
[0087]
下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。
[0088]
本发明实施例提供一种x型舵auv鲁棒精确控制方法，参照图1所示，包括：
[0089]
获取auv艇体的位置、姿态、线速度和角速度，建立运动学模型、动力学模型以及x分配模型，构成x舵auv数学模型；
[0090]
分别获取水平面和垂直面的当前auv所处位置以及航路信息，建立水平面运动学控制器，设计垂直面运动学控制律；
[0091]
设计非线性干扰观测器对x舵auv数学模型和垂直面运动学控制律的未建模动态进行估计和补偿，生成auv高阶滑模控制器；
[0092]
基于阻尼矩阵对auv高阶滑模控制器解算的控制输出进行x舵的控制分配。
[0093]
该控制方法可有效降低模型不确定性、外界干扰对auv控制的影响。
[0094]
下面具体对该x型舵auv鲁棒精确控制方法进行详细阐述：
[0095]
步骤1.构建x舵auv数学模型：
[0096]
x舵auv数学模型包括auv运动学模型、动力学模型以及x分配模型。运动学模型描述了auv大地坐标系下位姿一阶微分与艇体坐标系下广义速度的关系，具体如下式所示：
[0097][0098][0099][0100]
[0101][0102][0103]
auv运动状态采用sname体系推荐形式进行描述构建，其中，定义向量[x,y,z]表示大地坐标系下艇体的位置，定义欧拉角向量[φ,θ,ψ]描述艇体姿态，[u,v,w]表示艇体坐标系下艇体的线速度，[p,q,r]表示艇体坐标系下艇体的角速度。具体参数含义如表1所示：
[0104]
表1 sname符号定义
[0105][0106][0107]
auv动力学模型描述了auv在力/力矩影响下的运动状态，艇体坐标系下的海流速度可表示为：
[0108]
uc＝uccos(θ)cos(ψ
c-ψ)
[0109][0110][0111]
其中，uc、ψc分别表示大地坐标系下的海流速度大小及方向。
[0112]
艇体坐标系下的相对速度为：
[0113]
ur＝u-uc[0114]
vr＝v-vc[0115]
wr＝w-wc[0116]
假设auv在水中受零浮力，考虑海流影响的auv纵向动力学模型可以描述为：
[0117][0118]
其中，m为auv质量，c
mn
为水动力系数，c∈[x,y,z,k,m,n]为auv六自由度对应的水动力，下角标mn代表对应的运动，m,n∈[u,v,w,p,q,r....]。
[0119]
同理，auv剩余5自由度动力学模型，可以写为已知模型与干扰、未知模型解耦的形式，可描述为：
[0120][0121][0122][0123]
其中，fo代表动力学模型已知项，do代表动力学模型不确定项，τo为对应自由度的控制力/力矩，auv推进器和x形舵产生。其中，o∈[u,v,w,p,q,r]。其中fu，fv，fw，f
p
，fq，dv，dw，d
p
，dq，τ
p
，τq，τr，gu，g
p
，gq，gr这些参数仅需给出g项四式，剩余几项可从考虑海流影响的auv纵向动力学模型式子中轻易推导得出。此外，有：
[0124][0125][0126]
其中，i
x
、iy、iz为绕auv对应坐标轴的惯性矩，τ
p
、τq和τr均由x舵产生。
[0127]
auv的x舵分配模型描述了舵角与输出力矩之间的关系，如下式所示：
[0128][0129]
其中：k
δi
、m
δi
、n
δi
，i＝1,
…
,4为4个舵在横倾、纵倾与回转自由度的水动力系数，δi为x舵的4个舵角。
[0130]
步骤2.构建x舵auv控制器：
[0131]
x舵auv控制器由基于侧滑角补偿的运动学控制器、基于高阶滑模的动力学控制器以及非线性干扰观测器构成，x舵auv控制器结构如图2所示。
[0132]
具体地，auv的水平面运动学控制器采用视线导航法，其示意图如图3所示。图中p
k-1
(x
k-1
,y
k-1
)、pk(xk,yk)分别为当前航段的起始航路点与目标航路点水平面二维坐标，p
k+1
(x
k+1
,y
k+1
)为下一航路点的水平面二维坐标，(x
t
,y
t
)为auv当前所处位置，则由航迹点p
k-1
(x
k-1
,y
k-1
)与pk(xk,yk)组成的直线航迹段的航迹角ψk的计算如下式所示：
[0133]
ψk＝atan2(y
k+1-yk,x
k+1-xk)
ꢀꢀ
(6)
[0134]
由auv当前位置点作垂线到目标直线航迹段，则可得出auv距离航迹的航迹偏差e，
其计算公式如下式所示：
[0135]
e＝-(x-x
k-1
)sin(ψk)+(y-y
k-1
)cos(ψk)
ꢀꢀ
(7)
[0136]
为了使auv航迹跟踪的航迹偏差渐进收敛到零，根据传统的直线los设计算法，计算出航向指令如下式所示：
[0137][0138]
式中，δ为前视距离。当auv航行过程中受到海流干扰时，由于干扰造成的侧滑角存在，传统的los不能输出正确的航向指令，直线航迹跟踪稳定后会有静差存在。本实施例针对此问题对los算法进行改进，设计侧滑角观测器对los算法进行补偿，以达到航迹控制无静差的目的。侧滑角观测器设计过程如下：
[0139]
首先，对航迹偏差求导，可得到下式：
[0140][0141]
式中：v为auv的水平面航行速度，β为海流干扰造成的侧滑角；ψk表示sname体系下的艏向角。由于航行中的auv侧滑角很小，因此可将上式(9)转化为下式所示：
[0142][0143]
假设g＝vβcos(ψ-ψk)，可根据上式(10)设计g的干扰估计算法如下式所示：
[0144][0145]
式中：为g的估计值，kg为观测器的参数，pg为观测器的辅助变量。
[0146]
进而根据公式(10)和公式(11)计算出侧滑角估计值：
[0147][0148]
将观测的侧滑角采用修正los算法，考虑到auv欠驱动性质，考虑侧滑角补偿的水平面运动学控制器如下式所示：
[0149][0150]
其中，ψd为水平面的期望艏向角，ψk为直线航迹段的航迹角，e为航迹偏差，δ为前视距离，为侧滑角估计值。
[0151]
在垂直面上，令pk(yk,zk)、p
k+1
(y
k+1
,z
k+1
)为分别表示当前航段的起始航路点与目标航路点东向位移与深度的二维坐标，(y,z)为auv当前所处位置，则由航迹点pk(yk,zk)、p
k+1
(y
k+1
,z
k+1
)组成的直线航迹段的潜浮角可计算为下式所示：
[0152]
θk＝atan2(z
k+1-zk,y
k+1-yk)
ꢀꢀ
(14)
[0153]
由auv当前位置点作垂线到目标航迹段，则可得出auv距离航迹的航迹偏差eh，其计算公式如下式所示：
[0154]eh
＝-(y-yk)sin(θk)+(z-zk)cos(θk)
ꢀꢀ
(15)
[0155]
为了使auv垂直面航迹偏差收敛到零，设计指令纵倾角的生成算法如下式所示：
[0156][0157]
式中：δ为前视距离。
[0158]
为了消除垂直面不均衡量产生攻角造成的垂直面航迹控制静差，在上式(16)的基础上加入积分作用，通过积分累计来消除垂直面航迹控制静差，引入积分作用的垂直面运动学控制律如下式所示：
[0159][0160]
式中：θd为垂直面的期望艏向角，θk为直线航迹段的潜浮角，eh为垂直面航迹偏差，δ为前视距离，ki为积分参数。
[0161]
之后，基于高阶滑模算法与非线性干扰观测器设计auv的反馈控制律。
[0162]
如图4所示，以auv横摇镇定控制为例，定义系统的跟踪误差为：
[0163][0164]
其中，为期望横倾角，设为0。设计线性滑模面为：
[0165][0166]
上式中，c为控制常数。
[0167]
对上式求导，有：
[0168][0169]
根据(1)，有：
[0170][0171][0172]
设计高阶滑模趋近律为：
[0173][0174]
其中，k1、k2为增益，sgn()为符号函数。将式(4)、(20)、(21)代入式(22)中，得到横摇自由度高阶滑模控制律为：
[0175][0176]
考虑到auv横摇角度较小，式(1)可改写为：
[0177][0178][0179]
类似于式(18)-(23)过程，可得到auv纵摇、艏摇自由度的高阶滑模控制律为：
[0180][0181][0182]
其中，e
θ
＝θ-θd，e
ψ
＝ψ-ψd，
[0183]
auv还要实现速度控制，设计滑模面为：
[0184][0185]
其中，eu＝u-ud为速度误差。类似于式(20)-(23)过程，可得面向auv速度的高阶滑模控制律为：
[0186][0187]
其中，eu＝u-ud。因为控制律(23)、(25)、(27)中包含未建模动态，因此设计非线性干扰观测器对上述未建模动态进行估计和补偿，对应四个自由度上观测器形式为：
[0188][0189]
式(28)中，l为观测器增益，ξu、ξ
p
和ξq为辅助变量。结合非线性干扰观测器的auv高阶滑模控制器形式为：
[0190][0191][0192][0193][0194]
上式中，k1、k2为控制器增益，su、s
θ
、s
ψ
为滑模面，sgn为符号函数，eu、e
θ
、e
ψ
为误差，为动力学模型中gu、g
p
、gq、gr的估计值，可按常数处理；为观测器估计的模型不确定性与海流共同造成的干扰，其中观测器指式(28)的干扰观测器；θd为垂直面的期望艏向角；ψd为水平面的期望艏向角。
[0195]
控制律(29)中仅需要模型(1)中的已知量，未知量和外界干扰由干扰观测器式(28)补偿。如果动力学模型完全未知，式(28)和(29)构成的干扰观测器和高阶滑模控制器还可以进一步写成：
[0196][0197][0198][0199][0200][0201]
其中，l为干扰观测器增益，ξu、ξ
p
、ξq、ξr为辅助变量。
[0202]
从而在动力学模型完全未知的情况下完成auv控制。
[0203]
步骤3.构建x舵auv控制分配策略：
[0204]
高阶滑模控制器(31)解算的控制输出[τu,τ
p
,τq,τr]中，τu由auv主推进器直接产生，τ＝[τ
p
,τq,τr]
t
由auv的x舵产生，需要根据控制分配策略计算x舵各舵角。
[0205]
根据式(5)，有：
[0206]
τ＝bδ
ꢀꢀ
(10)
[0207]
其中，δ＝[δ1,δ2,δ3,δ4]
t
。
[0208]
本实施例提出一种改进伪逆算法，基于阻尼矩阵实现x舵控制分配，能够在快速计算指令舵角的同时，满足舵角限制和快速响应的要求，有：
[0209]
y＝(w-1bt
b)-1
w-1bt
τ
ꢀꢀ
(11)
[0210]
其中，y为算法输出的指令舵角，τ＝[τ
p
,τq,τr]
t
为控制器输出，w＝diag(w1,w2,w3,w4)为阻尼矩阵，b为x舵控制分配矩阵，wi,i∈{1,2,3,4}为大于0常数，阻尼矩阵中wi数值越大，对应舵角输出限制作用越强，考虑到舵角输出极限，wi满足：
[0211][0212]
其中，ws为一较大常数，ui,i∈{1,2,3,4}为控制分配算法(指指式(33))舵角输出，δ
min,i
和δ
max,i
为对应舵角的最小和最大极限，为大于0常数。
[0213]
按照步骤1至步骤3对本实施例中的auv控制算法进行了仿真，仿真环境中假设auv受到正弦形式的海流干扰，动力学模型未知，auv从点[-8,102,0]出发，遍历6个路径点[500,300,15]，[1000,300,30]，[1500,0,45]，[1000,-300,30]，[500,-300,15]，[80,0,0]，期望航速为3节，根据所提出的控制算法完成了鲁棒精确的三维路径跟踪。三维路径跟踪结果如图5所示，auv水平面轨迹如图6所示，auv垂直面轨迹如图7所示。
[0214]
本实施例提供的x型舵auv鲁棒精确控制方法，可有效解决传统方法中由于auv海流干扰、模型不确定等造成的控制精度低、鲁棒性差、依赖模型参数等问题。首先，基于观测器设计了一种具备侧滑角补偿的运动学控制器，降低了运动学控制器的控制误差；其次，考虑到传统滑模控制带来的执行机构抖振问题，结合高阶滑模控制器与非线性干扰力观测器，设计了动力学控制器，适合于外界干扰及模型不确定条件下的auv控制；最后，结合阻尼矩阵，提出了一种改进自适应x舵控制分配方法，兼顾x舵舵角输出限制与计算实时性。
[0215]
显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。