CN105807789A - 基于t-s模糊观测器补偿的uuv控制方法 - Google Patents

Abstract

基于T‑S模糊观测器补偿的UUV控制方法，涉及一种UUV控制方法。为了解决在有海流干扰时UUV跟踪航迹不精确的问题。包括：获取UUV下一步的期望航迹；姿态控制器根据期望轨迹进行跟踪误差，解算出下一步垂直舵与水平舵的舵角信息；T‑S模糊观测器根据海流干扰、当前UUV状态信息和航迹位置误差对UUV进行观测，估计出UUV下一步的状态信息；将UUV下一步的状态信息作为航速控制器的输入信号，获得推进器的下一步的推力；根据垂直舵与水平舵的舵角信息和推力，对UUV进行控制，获得UUV的运动状态，进而确定UUV的航迹，判断该航迹是否达到期望轨迹。本发明用于UUV跟踪水下线缆或管道、水下搜救、深海资源探测及地形探测。

Description

基于T-S模糊观测器补偿的UUV控制方法

技术领域

本发明涉及一种UUV控制方法，特别涉及一种基于T-S模糊观测器补偿的一种UUV控制方法。

背景技术

无人水下航行器(Underwater Unmanned Vehicle,UUV)在跟踪水下线缆或管道、水下搜救、深海资源探测及地形探测等方面有广泛应用，而精确的三维航迹跟踪是实现上述应用的重要前提。由于本发明研究的UUV具有欠驱动及加速度不可积的非完整约束条件，且受外界海流的影响，从而导致所建立的模型复杂。实际应用中所使用的UUV六自由度模型均以忽略高阶项及未建模动态为前提，致使当UUV工作于复杂环境下时，由于未建模动态及高阶项影响增大，实际跟踪精度变差。

1999年，Taniguchi T.等人将常规T-S模糊模型推广到更一般的情况，提出了T-S模糊广义系统模型。通过将T-S模糊系统与广义系统理论相结合，从而使得T-S系统可以借助于线性系统的分析和控制手段，为T-S模糊系统的应用开辟了一条崭新的途径。本发明通过建立UUV T-S模糊广义模型，引入T-S模糊观测器对UUV未建模动态进行在线观测，将观测结果反馈至控制器。该控制策略控制精度高，抗干扰性能强，具有非常好的使用价值。

文献《基于T-S模型的轮式移动机器人轨迹跟踪控制》(控制理论与应用，2007年12月，第24卷第6期)提出了在控制力受限情况下对外部干扰有一定抑制能力的轮式移动机器人(WMR)轨迹跟踪方法。利用轮式移动机器人本身运动特性和参考轨迹建立起跟踪误差系统，并将WMR实时的期望角度以及角度跟踪误差作为模糊推理的前件，对该系统作T-S模型描述，针对每个线性子系统，不考虑各个子系统的相互关系，单独设计满足控制输入约束以及对于外部干扰有一定抑制能力的状态反馈H_∞控制器，再利用PDC设计原则构建全局反馈控制器，并验证闭环系统的李雅普诺夫稳定性。

文献《基于T-S模糊模型的机器人轨迹跟踪控制》(现代电子技术，2014年4月，第37卷第8期)是针对机器人模型的非线性，通过扇区非线性方法建立T-S模糊模型的。文献考虑到与实际模型之间的差异和扰动的存在，设计了滑模控制器，并利用李雅普诺夫理论证明了其稳定性。设计方法简单，通用性强。

但采用现有这些基于T-S模糊模型的控制方法，控制有海流影响的UUV时，不能精确跟踪航迹。

发明内容

本发明的目的是为了解决现有UUV控制，在有海流干扰时，跟踪航迹不精确的问题，本发明提供一种基于T-S模糊观测器补偿的UUV控制方法。

本发明的基于T-S模糊观测器补偿的UUV控制方法，所述方法包括如下步骤：

步骤一：获取UUV下一步的期望航迹；

步骤二：姿态控制器根据期望轨迹进行跟踪误差，解算出下一步垂直舵与水平舵的舵角信息；

步骤三：T-S模糊观测器根据海流干扰、当前UUV状态信息和航迹位置误差对UUV进行观测，估计出UUV下一步的状态信息；

所述航迹位置误差为期望轨迹与T-S模糊观测器当前估计出的状态信息和UUV当前的状态信息的差值；

步骤四：将步骤三获得的UUV下一步的状态信息作为航速控制器的输入信号，获得推进器的下一步的推力；

步骤五：根据步骤二获得的垂直舵与水平舵的舵角信息和步骤三获得的推力，对UUV进行控制，并获得UUV的运动状态；

步骤六：根据步骤五获得的运动状态确定UUV的航迹，判断该航迹是否达到期望轨迹，若是，转入步骤一，若否，则转入步骤二。

所述步骤三中，T-S模糊观测器为：

$\left\{\begin{array}{c}\stackrel{\·}{\hat{x}}=T^{-1}\underset{i=1}{\overset{n}{\Σ}}{w}_i\left(z\right)(A_i\hat{x}+M_i({\hat{A}}_{\mathrm{\δ}i}\hat{x}+{\hat{B}}_{\mathrm{\δ}i}u+u)+D_i\mathrm{\τ}+L_i(y-\hat{y}\left)\right)\\ \hat{y}=C\hat{x}\end{array}\right.$

所述T-S模糊观测器是针对UUV近水面航行建立，忽略UUV的横倾角；

其中，选取未建模动态的观测更新律为：

n为T-S模糊观测器建立规则的数量，i＝1,2,...,n；

为UUV下一步在{B}下的线速度和角速度矢量；

y为UUV当前状态输出；

为UUV下一步的状态输出；

w_i(z)为相应规则的归一化权值函数，z＝[u,v,w,q,r,(v²+w²)²]^T，u、v和w为UUV线速度矢量在艇体坐标系{B}下的三个分量；p、q和r分别为UUV角速度矢量在艇体坐标系{B}下的三个分量，p＝0；

A_i为相应规则所对应的UUV动力学模型的非线性项线性化的常量矩阵；

D_i相应规则所对应的输入矩阵；

M_i为相应规则的外界干扰的增益矩阵；

τ为控制力或控制力矩；

L_i为相应规则所对应的增益矩阵；

T为UUV的系统惯性矩阵，含附加质量，

m₁₁为ξ方向的惯性质量，m₂₂为η方向的惯性质量，m₃₃为ζ方向的惯性质量，m₄₄为η方向的惯性矩，m₅₅为ζ方向的惯性矩；

外界干扰力或外界干扰力矩τ_d＝A_δix+B_δiu+a_i，A_δi为相应规则所对应的状态干扰系数矩阵，B_δi为相应规则所对应的输入干扰系数矩阵，a_i为相应规则所对应的外界干扰系数矩阵；

e_y为状态观测误差；

P为变换矩阵；

为C的伪逆；

C＝I为线性化后的常矩阵；

˙表示求导。

所述T-S模糊观测器的获取方法包括:

步骤A：建立UUV动力学模型；

步骤B：根据动力学模型，获得UUV动力学方程式，并依据模糊规则将UUV动力学方程式转换为T-S模糊模型为：

$\left\{\begin{array}{c}\stackrel{\·}{x}=T^{-1}\underset{i=1}{\overset{n}{\Σ}}{w}_i\left(z\right)(A_{i}x+M_i{\mathrm{\τ}}_d+D_i\mathrm{\τ})\\ y=Cx\end{array}\right.;$

其中，x＝[u,v,w,q,r]^T∈R^5×1表示UUV当前在艇体坐标系{B}下的线速度和角速度矢量，τ_d为外界干扰力或外界干扰力矩；A_i∈R^5×5,M_i∈R^5×5,D_i∈R^5×5；

步骤C：将UUV所受τ_d表示为τ_d＝A_δx+B_δu+a，将其带入步骤二中T-S模糊模型得出含有未建模动态的T-S模糊模型为：

$\left\{\begin{array}{c}\stackrel{\·}{x}=T^{-1}\underset{i=1}{\overset{n}{\Σ}}{w}_i\left(z\right)(A_{i}x+M_i(A_{\mathrm{\δ}i}x+B_{\mathrm{\δ}i}u+a_i)+D_i\mathrm{\τ})\\ y=Cx\end{array}\right.,$

其中，A_δ为模糊前的状态干扰系数矩阵，B_δ为模糊前的输入干扰系数矩阵，a为模糊前的外界干扰系数矩阵；A_δi∈R^5×5，B_δi∈R^5×5，a_i∈R^5×5；

步骤D：根据步骤C得出的T-S模糊模型得到模糊观测器：

$\left\{\begin{array}{c}\stackrel{\·}{\hat{x}}=T^{-1}\underset{i=1}{\overset{n}{\Σ}}{w}_i\left(z\right)(A_i\hat{x}+M_i({\hat{A}}_{\mathrm{\δ}i}\hat{x}+{\hat{B}}_{\mathrm{\δ}i}u+u)+D_i\mathrm{\τ}+L_i(y-\hat{y}\left)\right)\\ \hat{y}=C\hat{x}\end{array}\right.,$

其中，选取未建模动态的观测更新律为：

所述动力学模型：

$\left\{\begin{array}{c}{m}_{11}\stackrel{\·}{u}=m_{vr}vr+m_{wq}wq+d_u+X_{porp}+w_1\\ m_{22}\stackrel{\·}{v}=m_{ur}ur+d_v+w_2\\ m_{33}\stackrel{\·}{w}=m_{uq}uq+d_w+w_3\\ m_{44}\stackrel{\·}{q}=M_{prop}+d_q+w_4\\ m_{55}\stackrel{\·}{r}=N_{prop}+d_r+w_5\end{array}\right.$

式中m_vr、m_wq、m_ur和m_uq均是水动力项系数，分别为m_vr＝1/2ρL³X＇_wv、m_wq＝1/2ρL³X＇_wq、m_ur＝1/2ρL³Y′_r和m_uq＝1/2ρL³Z′_q,L为UUV长度，ρ为水密度，X′_vr为v和r的耦合系数，X′_wq为w和q的耦合系数，Y′_r为艏项角速度系数，Z′_q为纵倾角速度系数，d_u、d_v、d_w、d_q和d_r分别为在五个自由度上的非线性水动力阻尼项，X_porp、M_porp和N_porp分别表示推进器推力、水平舵作用力矩和垂直舵作用力矩，w₁、w₂、w₃、w₄和w₅分别为在五个自由度上的外界干扰力或外界干扰力矩，所述外界干扰力或外界干扰力矩不且为τ_d减去输入不确定性和状态不确定性。

本发明的有益效果在于，通过建立UUV的T-S模糊模型估计出UUV的高阶未建模项，设计的T-S模糊观测器使所估计出的UUV状态信息更加接近实际情况，最终使得UUV在复杂环境下能够进行精确航迹跟踪。

附图说明

图1为具体实施方式中UUV垂直面坐标系示意图。

图2为具体实施方式中UUV水平面坐标系示意图。

图3为艇体坐标系{B}的原理示意图。

图4为具体实施方式中基于T-S模糊观测器补偿的UUV控制方法的流程示意图。

具体实施方式

本实施方式所述的基于T-S模糊观测器补偿的UUV控制方法，本实施方式研究的UUV具有欠驱动及加速度不可积的非完整约束条件，且受外界海流影响，导致所建立的模型复杂。该UUV模型在尾部安装水平推进器提供纵向推力，垂直舵实现航向控制，水平舵实现纵倾或深度控制。为便于分析，对该UUV模型进一步做出如下假设：UUV航速为非负值，即不考虑倒行时的航迹控制；忽略横滚运动的影响；所考虑的UUV对象左右对称。

在UUV运动分析及操纵性试验中，通常分为水平面与垂直面两方面来进行。其中水平面分析主要用于考察UUV航速及回转运动稳定性，而垂直面分析则主要考察UUV定深、变深性能。图1与图2描述了UUV三维空间运动分析时所用到的坐标系。其中，E_ξηζ为大地固定坐标系，以E为大地固定坐标系的中心，ξ、η和ζ分别表示大地固定坐标系下的三个轴对应的坐标。B_XYZ为UUV艇体坐标系，如图3所示，以B为艇体坐标系的中心，p、q和r分别为UUV角速度矢量在艇体坐标系下的三个分量；G为UUV重心，在坐标系{B}下其坐标为R_G＝(x_G,0,0)^T；UUV的合成速度U＝[u v w]^T，x和γ分别为UUV潜浮角与航迹角；α和β分别为UUV冲角与漂角；ψ为艏向角；由于本实施方式考虑的是近水面UUV航行时，所以忽略横倾角与纵倾角θ，即

UUV艇体坐标系B_XYZ相关参数含义如表1所示。

表1UUV运动变量

根据前述假设，有则欠驱动UUV三维运动学模型为：

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& 1/cos\mathrm{\θ}\end{array}\right]\left[\begin{array}{c}q\\ r\end{array}\right]+\left[\begin{array}{c}\stackrel{\·}{\mathrm{\α}}\\ \stackrel{\·}{\mathrm{\β}}\end{array}\right]---\left(1\right)$

其中，θ和ψ分别为UUV初始状态的艏向角和纵倾角，α和β分别为UUV初始状态的冲角与漂角；

假定坐标系{E}与坐标系{B}原点重合，动力学方程可描述为：

$\left\{\begin{array}{c}{m}_{11}\stackrel{\·}{u}=m_{vr}vr+m_{wq}wq+d_u+Xprop+w_1\\ m_{22}\stackrel{\·}{v}=m_{ur}ur+d_v+w_2\\ m_{33}\stackrel{\·}{w}=m_{uq}uq+d_w+w_3\\ m_{44}\stackrel{\·}{q}=M_{prop}+d_q+w_4\\ m_{55}\stackrel{\·}{r}=N_{prop}+d_r+w_5\end{array}\right.---\left(2\right)$

式中m_vr、m_wq、m_uV和m_uq均是水动力项系数，分别为m_vr＝1/2ρL³X＇_wr、m_wq＝1/2ρL³X＇_wq、m_ur＝1/2ρL³Y′_r和m_uq＝1/2ρL³Z′_q,L为UUV长度，ρ为水密度，X′_vr为v和r的耦合系数，X′_wq为w和q的耦合系数，Y_r′为艏项角速度系数，Z′_q为纵倾角速度系数，d_u、d_v、d_w、d_q和d_r分别为在五个自由度上的非线性水动力阻尼项，X_porp、M_porp和N_porp分别表示推进器推力、水平舵作用力矩和垂直舵作用力矩，w₁、w₂、w₃、w₄和w₅分别为在五个自由度上的外界干扰力或外界干扰力矩，所述w₁、w₂、w₃、w₄和w₅为不包括输入不确定性和状态不确定性的外界干扰力或外界干扰力矩。

将UUV动力学方程式(2)表示为矩阵向量形式：

$\begin{array}{c}T\stackrel{\·}{x}=A\left(x\right)x+{\mathrm{M\τ}}_d+D\mathrm{\τ}\\ y=Cx\end{array}---\left(3\right)$

式中，x＝[u,v,w,q,r]^T∈R^5×1表示UUV在{B}下的线速度和角速度矢量，τ为控制力或控制力矩；τ_d为外界干扰力或外界干扰力矩；M为外界干扰的增益矩阵，D为输入矩阵，y为UUV当前状态输出；C＝I为线性化后的常矩阵；˙表示求导；

可将式(3)中的非线性项A(x)表示为：

$\begin{array}{c}A\left(x\right)=\\ \left[\begin{array}{ccccc}{X}_{uu}u& X_{vv}u& X_{ww}w& (-m+X_{wq})w+X_{qq}q& (m+X_{vr})v+X_{rr}r\\ 0& Y_v& Y_{vw}u& Y_{vq}v& -mu+Y_r+Y_{wr}w+Y_{qr}q\\ 0& Z_{vv}u& Z_w& mu+Z_q& Z_{vr}r+Z_{rr}r\\ 0& M_{v}u& M_w+M_{w\left|w\right|}{(v^2+w^2)}^{\mathrm{1/2}}& M_{q\left|q\right|}q+M_{wq}{(v^2+w^2)}^{\mathrm{1/2}}& (J_y-J_z)q+J_{xy}q+J_{zx}r+M_{rr}r+M_{vr}v\\ 0& N_v& N_{vw}v+N_{v\left|v\right|}{(v^2+w^2)}^{\mathrm{1/2}}& -J_{xy}q-J_{zx}x+N_{vq}v& N_r+N_{wr}w+N_{q}q+N_{r\left|r\right|}\left|r\right|+N_{\left|v\right|r}{(v^2+w^2)}^{\mathrm{1/2}}\end{array}\right]\end{array}---\left(4\right)$

为所对应的水动力系数；m为UUV的质量。

A(x)的每一个元素都可由集合{u,v,w,q,r,(v²+w²)^1/2}中的元素线性表示。从而由模糊推理系统邻域非线性逼近原理，可知：

$\begin{array}{c}{R}_i:Ifuis{N}_i^{u}andvis{N}_i^v...and{(v^2+w^2)}^{1/2}is{N}_i^{vw},then\\ \stackrel{\·}{x}=T^{-1}(A_{i}x+M_i{\mathrm{\τ}}_d+D_i\mathrm{\τ})\\ y=Cx\end{array}---\left(5\right)$

式中i＝1,2,...,n，n为T-S模糊观测器所建立规则的数量，N为模糊集合，A_i∈R^5×5,M_i∈R^5×5,D_i∈R^5×5。可将式(3)改写成为T-S模糊模型：

$\begin{array}{c}\stackrel{\·}{x}=T^{-1}\underset{i=1}{\overset{n}{\Σ}}{w}_i\left(z\right)(A_{i}x+M_i{\mathrm{\τ}}_d+D_i\mathrm{\τ})\\ y=Cx\end{array}---\left(6\right)$

式中z＝{u,v,w,q,r,(v²+w²)¹²}，w(z)为相应的归一化权值函数，其计算过程如下:

$\begin{array}{c}{w}_i\left(z\right)=\frac{h_i\left(z\right)}{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{h}_i\left(z\right)}\\ h_i\left(z\right)=h_{iu}\left(u\right)h_{iv}\left(v\right)h_{iw}\left(w\right)h_{iq}\left(q\right)h_{ir}\left(r\right)h_{ivw}\left(vw\right)\end{array}---\left(7\right)$

式中，h_ix(x)为第i条规则时，分量x所具有的权值，具体值根据所选取的隶属度函数计算。

当UUV在实际航行时，由于受外界不确定扰动的影响，其未建模动态是时变的，因此利用该观测器实时对UUV的高阶未建模动态进行在线估计。

设UUV所受干扰力或干扰力矩τ_d由三部分组成：状态不确定性所引入的干扰、输入不确定所引入的干扰及外界直接作用在UUV上的干扰力或干扰力矩。从而可将τ_d表述为：

τ_d＝A_δx+B_δu+a (8)

式中，其中，A_δ为模糊前的状态干扰系数矩阵，B_δ为模糊前的输入干扰系数矩阵，a为模糊前的外界干扰系数矩阵；A_δ∈R^5×5，B_δ∈R^5×5，a∈R^5×5均未知。将(8)式代入(5)式，可得含有未建模动态的T-S模糊模型为：

$\begin{array}{c}\stackrel{\·}{x}=T^{-1}\underset{i=1}{\overset{n}{\Σ}}{w}_i\left(z\right)(A_{i}x+M_i(A_{\mathrm{\δ}i}x+B_{\mathrm{\δ}i}u+a_i)+D_i\mathrm{\τ})\\ y=Cx\end{array}---\left(9\right)$

基于T-S模糊模型式(8)，所设计的自适应观测器如下：

$\left\{\begin{array}{c}\stackrel{\·}{\hat{x}}=T^{-1}\underset{i=1}{\overset{n}{\Σ}}{w}_i\left(z\right)(A_i\hat{x}+M_i({\hat{A}}_{\mathrm{\δ}i}\hat{x}+{\hat{B}}_{\mathrm{\δ}i}u+u)+D_i\mathrm{\τ}+L_i(y-\hat{y}))\\ \hat{y}=C\hat{x}\end{array}\right.---\left(10\right)$

式中，L_i为相应规则所对应的增益矩阵；i＝1,2,...,n。选取未建模动态的观测更新律为：

A_i为相应规则所对应的UUV动力学模型的非线性项线性化的常量矩阵；

D_i相应规则所对应的输入矩阵；

M_i为相应规则的外界干扰的增益矩阵；

T为UUV的系统惯性矩阵，含附加质量，

m₁₁为ξ方向的惯性质量，m₂₂为η方向的惯性质量，m₃₃为ζ方向的惯性质量，m₄₄为η方向的惯性矩，m₅₅为ζ方向的惯性矩；

外界干扰力或外界干扰力矩τ_d＝A_δix+B_δiu+a_i，A_δi为相应规则所对应的状态干扰系数矩阵，B_δi为相应规则所对应的输入干扰系数矩阵，a_i为相应规则所对应的外界干扰系数矩阵；

e_y为状态观测误差；

P为变换矩阵；

为C的伪逆；

设为估计误差，则状态观测误差e_y＝Ce，结合式(8)、(9)，可得误差更新律为：

$\begin{array}{c}\stackrel{\·}{e}=\underset{i=1}{\overset{n}{\Σ}}{w}_i\left(z\right)\[(A_i-L_{i}C+A_{\mathrm{\δ}i})e+M_i({\stackrel{\‾}{A}}_{\mathrm{\δ}i}\hat{x}+{\stackrel{\‾}{B}}_{\mathrm{\δ}i}u+\stackrel{\‾}{{\mathrm{\θ}}_i})\]\\ e_y=Ce\end{array}---\left(12\right)$

其中

证明：由式(12)，取Lyapunov函数为如下形式：

定理：如果存在有P＝P^T>0，Q＝Q^T>0,L_i,i＝1,2,...,n且满足

$\begin{array}{c}\begin{array}{cc}P(A_i-L_{i}C)+(A_i^T-C^T{L}_i^T)P^T<-2Q& i=1,2,...,n\end{array}\\ u_{\max }\&le;\frac{{\mathrm{\&lambda;}}_{min}\left(Q\right)}{{\mathrm{\&lambda;}}_{max}\left(P\right)}\end{array}---\left(13\right)$

未建模动态观测更新律应用式(11)，则式(12)渐近趋于0。式中max||A_δi||≤u_max为未建模动态的界。

$V=e^{T}Pe+\underset{i=1}{\overset{n}{\&Sigma;}}tr\left({\stackrel{\&OverBar;}{A}}_{\mathrm{\&delta;}i}^T{\stackrel{\&OverBar;}{A}}_{\mathrm{\&delta;}i}\right)+\underset{i=1}{\overset{n}{\&Sigma;}}tr\left({\stackrel{\&OverBar;}{B}}_{\mathrm{\&delta;}i}^T{\stackrel{\&OverBar;}{B}}_{\mathrm{\&delta;}i}\right)+\underset{i=1}{\overset{n}{\&Sigma;}}\left({\stackrel{\&OverBar;}{\mathrm{\&theta;}}}_i^T\stackrel{\&OverBar;}{{\mathrm{\&theta;}}_i}\right)---\left(14\right)$

对上式进行求导，有：

$\begin{array}{c}\stackrel{\&CenterDot;}{V}=\underset{i=1}{\overset{n}{\&Sigma;}}{w}_i\left(z\right)e^T\&lsqb;{(A_i-L_{i}C+A_{\mathrm{\&delta;}i})}^{T}P+P(A_i-L_{i}C+A_{\mathrm{\&delta;}i})\&rsqb;e\\ +2e^{T}P\underset{i=1}{\overset{n}{\&Sigma;}}{w}_i\left(z\right)M_i{\stackrel{\&OverBar;}{A}}_{\mathrm{\&delta;}i}\hat{x}+2e^{T}P\underset{i=1}{\overset{n}{\&Sigma;}}{w}_i\left(z\right)M_i{\stackrel{\&OverBar;}{B}}_{\mathrm{\&delta;}i}u\\ +2e^{T}P\underset{i=1}{\overset{n}{\&Sigma;}}{w}_i\left(z\right)M_i\stackrel{\&OverBar;}{{\mathrm{\&theta;}}_i}-2\underset{i=1}{\overset{n}{\&Sigma;}}tr\left({\stackrel{\&CenterDot;}{\hat{A}}}_{\mathrm{\&delta;}i}^T{\stackrel{\&OverBar;}{A}}_{\mathrm{\&delta;}i}\right)-2\underset{i=1}{\overset{n}{\&Sigma;}}tr\left({\stackrel{\&CenterDot;}{\hat{B}}}_{\mathrm{\&delta;}i}^T{\stackrel{\&OverBar;}{B}}_{\mathrm{\&delta;}i}\right)-2\underset{i=1}{\overset{n}{\&Sigma;}}\left({\stackrel{\&CenterDot;}{\widehat{\mathrm{\&theta;}}}}_i^T\stackrel{\&OverBar;}{{\mathrm{\&theta;}}_i}\right)\\ =\underset{i=1}{\overset{n}{\&Sigma;}}{w}_i\left(z\right)e^T{G}_{i}e+2\underset{i=1}{\overset{n}{\&Sigma;}}tr\left(e^T{\mathrm{Pw}}_i\right(z)M_i{\stackrel{\&OverBar;}{A}}_{\mathrm{\&delta;}i}\hat{x})-\underset{i=1}{\overset{n}{\&Sigma;}}tr\left({\stackrel{\&CenterDot;}{\hat{A}}}_{\mathrm{\&delta;}i}^T{\stackrel{\&OverBar;}{A}}_{\mathrm{\&delta;}i}\right)\\ +2\underset{i=1}{\overset{n}{\&Sigma;}}tr\left(e^T{\mathrm{Pw}}_i\right(z)M_i{\stackrel{\&OverBar;}{B}}_{\mathrm{\&delta;}i}u)-\underset{i=1}{\overset{n}{\&Sigma;}}tr\left({\stackrel{\&CenterDot;}{\hat{B}}}_{\mathrm{\&delta;}i}^T{\stackrel{\&OverBar;}{B}}_{\mathrm{\&delta;}i}\right)+2\underset{i=1}{\overset{n}{\&Sigma;}}\left(e^T{\mathrm{Pw}}_i\right(z)M_i\stackrel{\&OverBar;}{{\mathrm{\&theta;}}_i}-{\stackrel{\&CenterDot;}{\widehat{\mathrm{\&theta;}}}}_i^T\stackrel{\&OverBar;}{{\mathrm{\&theta;}}_i})\\ =\underset{i=1}{\overset{n}{\&Sigma;}}{w}_i\left(z\right)e^T{G}_{i}e+2\underset{i=1}{\overset{n}{\&Sigma;}}tr\left(\right(e^T{\mathrm{Pw}}_i\left(z\right)M_i\hat{x}-{\stackrel{\&CenterDot;}{\hat{A}}}_{\mathrm{\&delta;}i}^T\left){\stackrel{\&OverBar;}{A}}_{\mathrm{\&delta;}i}\right)\\ +2\underset{i=1}{\overset{n}{\&Sigma;}}tr\left(\right(e^T{\mathrm{Pw}}_i\left(z\right)M_{i}u-{\stackrel{\&CenterDot;}{\hat{B}}}_{\mathrm{\&delta;}i}^T\left){\stackrel{\&OverBar;}{B}}_{\mathrm{\&delta;}i}\right)+2\underset{i=1}{\overset{n}{\&Sigma;}}\left(e^T{\mathrm{Pw}}_i\right(z)M_i\left({\stackrel{\&CenterDot;}{\widehat{\mathrm{\&theta;}}}}_i^T\right)\stackrel{\&OverBar;}{{\mathrm{\&theta;}}_i}\end{array}---\left(15\right)$

式中G_i＝(A_i-L_iC+A_δi)^TP+P(A_i-L_iC+A_δi)，i＝1,2,...,n。

由式(12)可知G_i＜0，i＝1,2,...,n。因此，要使得只需

$\begin{array}{c}tr\left(\right(e^T{\mathrm{Pw}}_i\left(z\right)M_i\hat{x}-{\stackrel{\&CenterDot;}{\hat{A}}}_{\mathrm{\&delta;}i}^T\left){\stackrel{\&OverBar;}{A}}_{\mathrm{\&delta;}i}\right)=0\\ tr\left(\right(e^T{\mathrm{Pw}}_i\left(z\right)M_{i}u-{\stackrel{\&CenterDot;}{\hat{B}}}_{\mathrm{\&delta;}i}^T\left){\stackrel{\&OverBar;}{B}}_{\mathrm{\&delta;}i}\right)=0\\ e^T{\mathrm{Pw}}_i\left(z\right)M_i-{\stackrel{\&CenterDot;}{\widehat{\mathrm{\&theta;}}}}_i^T=0\end{array}---\left(16\right)$

上述条件即为未建模动态更新律式(10)。此时系统渐近稳定。

结合图4，基于T-S模糊观测器补偿的UUV控制方法，包括如下步骤：

步骤一：获取UUV下一步的期望航迹

步骤二：姿态控制器根据期望轨迹进行跟踪误差，解算出下一步垂直舵与水平舵的舵角信息，获得水平舵作用力矩M_porp和垂直舵作用力矩N_porp；

步骤三：T-S模糊观测器根据海流干扰、当前UUV状态信息和航迹位置误差对UUV进行观测，估计出UUV下一步的状态信息；

所述航迹位置误差为期望轨迹与T-S模糊观测器当前估计出的状态信息和UUV当前的状态信息的差值；

图3中M为艇体坐标系{B}下的线速度[u v w]^T到固定坐标系{E}下的位移[X Y Z]^T的转换矩阵，将步骤六中UUV实际运动状态转换为UUV在惯性坐标系下的位置作为求航迹位置误差时所需的航迹位置误差。

步骤四：将步骤三获得的UUV下一步的状态信息作为航速控制器的输入信号，获得推进器的下一步的推力X_porp；

步骤五：根据获得的垂直舵水平舵作用力矩M_porp、垂直舵作用力矩N_porp和推力X_porp作用到UUV执行机构上，执行机构控制UUV得到UUV实际运动状态[u v w q r]^T；

步骤六：根据步骤五获得的实际运动状态确定UUV的航迹，判断该航迹是否达到期望轨迹，若是，转入步骤一，若否，则转入步骤二。

若航迹达到期望轨迹则达到航迹点的精确跟踪，结束本次跟踪继续进行下一步跟踪；否则返回姿态控制器重新解算，直到达到航迹准确跟踪。

Claims (4)

1.一种基于T-S模糊观测器补偿的UUV控制方法，其特征在于，所述方法包括如下步骤：

步骤一：获取UUV下一步的期望航迹；

步骤二：姿态控制器根据期望轨迹进行跟踪误差，解算出下一步垂直舵与水平舵的舵角信息；

步骤三：T-S模糊观测器根据海流干扰、当前UUV状态信息和航迹位置误差对UUV进行观测，估计出UUV下一步的状态信息；

所述航迹位置误差为期望轨迹与T-S模糊观测器当前估计出的状态信息和UUV当前的状态信息的差值；

步骤四：将步骤三获得的UUV下一步的状态信息作为航速控制器的输入信号，获得推进器的下一步的推力；

步骤五：根据步骤二获得的垂直舵与水平舵的舵角信息和步骤三获得的推力，对UUV进行控制，并获得UUV的运动状态；

步骤六：根据步骤五获得的运动状态确定UUV的航迹，判断该航迹是否达到期望轨迹，若是，转入步骤一，若否，则转入步骤二。

2.根据权利要求1所述一种基于T-S模糊观测器补偿的UUV控制方法，其特征在于，所述步骤三中，T-S模糊观测器为：

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{\hat{x}}=T^{-1}\underset{i=1}{\overset{n}{\&Sigma;}}{w}_i\left(z\right)(A_i\hat{x}+M_i({\hat{A}}_{\mathrm{\&delta;}i}\hat{x}+{\hat{B}}_{\mathrm{\&delta;}i}u+u)+D_i\mathrm{\&tau;}+L_i(y-\hat{y}\left)\right)\\ \hat{y}=C\hat{x}\end{array}\right.$

所述T-S模糊观测器是针对UUV近水面航行建立，忽略UUV的横倾角；

其中，选取未建模动态的观测更新律为：

n为T-S模糊观测器建立规则的数量，i＝1,2,...,n；

为UUV下一步在{B}下的线速度和角速度矢量；

y为UUV当前状态输出；

为UUV下一步的状态输出；

w_i(z)为相应规则的归一化权值函数，z＝[u,v,w,q,r,(v²+w²)²]^T，u、v和w为UUV线速度矢量在艇体坐标系{B}下的三个分量；p、q和r分别为UUV角速度矢量在艇体坐标系{B}下的三个分量，p＝0；

A_i为相应规则所对应的UUV动力学模型的非线性项线性化的常量矩阵；

D_i相应规则所对应的输入矩阵；

M_i为相应规则的外界干扰的增益矩阵；

τ为控制力或控制力矩；

L_i为相应规则所对应的增益矩阵；

T为UUV的系统惯性矩阵，含附加质量，

m₁₁为ξ方向的惯性质量，m₂₂为η方向的惯性质量，m₃₃为ζ方向的惯性质量，m₄₄为η方向的惯性矩，m₅₅为ζ方向的惯性矩；

外界干扰力或外界干扰力矩τ_d＝A_δix+B_δiu+a_i，A_δi为相应规则所对应的状态干扰系数矩阵，B_δi为相应规则所对应的输入干扰系数矩阵，a_i为相应规则所对应的外界干扰系数矩阵；

e_y为状态观测误差；

P为变换矩阵；

为C的伪逆；

C＝I为线性化后的常矩阵；

˙表示求导。

3.根据权利要求2所述一种基于T-S模糊观测器补偿的UUV控制方法，其特征在于，所述T-S模糊观测器的获取方法包括:

步骤A：建立UUV动力学模型；

步骤B：根据动力学模型，获得UUV动力学方程式，并依据模糊规则将UUV动力学方程式转换为T-S模糊模型为：

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{x}=T^{-1}\underset{i=1}{\overset{n}{\&Sigma;}}{w}_i\left(z\right)(A_{i}x+M_i{\mathrm{\&tau;}}_d+D_i\mathrm{\&tau;})\\ y=Cx\end{array}\right.;$

其中，x＝[u,v,w,q,r]^T∈R^5×1表示UUV当前在艇体坐标系{B}下的线速度和角速度矢量，τ_d为外界干扰力或外界干扰力矩；A_i∈R^5×5,M_i∈R^5×5,D_i∈R^5×5；

步骤C：将UUV所受τ_d表示为τ_d＝A_δx+B_δu+a，将其带入步骤二中T-S模糊模型得出含有未建模动态的T-S模糊模型为：

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{x}=T^{-1}\underset{i=1}{\overset{n}{\&Sigma;}}{w}_i\left(z\right)(A_{i}x+M_i(A_{\mathrm{\&delta;}i}x+B_{\mathrm{\&delta;}i}u+a_i)+D_i\mathrm{\&tau;})\\ y=Cx\end{array}\right.,$

其中，A_δ为模糊前的状态干扰系数矩阵，B_δ为模糊前的输入干扰系数矩阵，a为模糊前的外界干扰系数矩阵；A_δi∈R^5×5，B_δi∈R^5×5，a_i∈R^5×5；

步骤D：根据步骤C得出的T-S模糊模型得到模糊观测器：

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{\hat{x}}=T^{-1}\underset{i=1}{\overset{n}{\&Sigma;}}{w}_i\left(z\right)(A_i\hat{x}+M_i({\hat{A}}_{\mathrm{\&delta;}i}\hat{x}+{\hat{B}}_{\mathrm{\&delta;}i}u+u)+D_i\mathrm{\&tau;}+L_i(y-\hat{y}\left)\right)\\ \hat{y}=C\hat{x}\end{array}\right.,$

其中，选取未建模动态的观测更新律为：

4.根据权利要求3所述的基于T-S模糊观测器补偿的UUV控制方法，其特征在于，所述动力学模型：

$\left\{\begin{array}{c}{m}_{11}\stackrel{\&CenterDot;}{u}=m_{vr}vr+m_{wq}wq+d_u+X_{porp}+w_1\\ m_{22}\stackrel{\&CenterDot;}{v}=m_{ur}ur+d_v+w_2\\ m_{33}\stackrel{\&CenterDot;}{w}=m_{uq}uq+d_w+w_3\\ m_{44}\stackrel{\&CenterDot;}{q}=M_{prop}+d_q+w_4\\ m_{55}\stackrel{\&CenterDot;}{r}=N_{prop}+d_r+w_5\end{array}\right.$

式中m_vr、m_wq、m_ur和m_uq均是水动力项系数，分别为m_vr＝1/2ρL³X＇_vr、m_wq＝1/2ρL³X′_wq、m_ur＝1/2ρL³Y′_r和m_uq＝1/2ρL³Z′_q,L为UUV长度，ρ为水密度，X＇_vr为v和r的耦合系数，X′_wq为w和q的耦合系数，Y′_r为艏项角速度系数，Z′_q为纵倾角速度系数，d_u、d_v、d_w、d_q和d_r分别为在五个自由度上的非线性水动力阻尼项，X_porp、M_porp和N_porp分别表示推进器推力、水平舵作用力矩和垂直舵作用力矩，w₁、w₂、w₃、w₄和w₅分别为在五个自由度上的外界干扰力或外界干扰力矩，所述外界干扰力或外界干扰力矩不且为τ_d减去输入不确定性和状态不确定性。