CN106227223A - 一种基于动态滑模控制的uuv轨迹跟踪方法 - Google Patents

Abstract

本发明属于水下无人航行器的轨迹跟踪和动态滑模控制技术领域，具体涉及一种基于动态滑模控制的UUV轨迹跟踪方法。建立UUV水平面模型；通过坐标转换获得误差变量，并对误差变量求导得到误差变量的导数；构造李雅普诺夫函数并且定义虚拟速度控制变量将姿态跟踪转化为虚拟速度控制；稳定虚拟速度控制变量，利用滑模控制方法对系统参数不精确及外界时变扰动进行自适应估计，建立滑模动态函数；选取动态滑模控制律，实现UUV的轨迹跟踪。本专利通过反步法和自适应动态滑模控制技术的组合，解决了UUV平面轨迹跟踪控制问题在系统中可能存在建模不确定性、未知环境扰动的问题、系统的参数不确定性。

Description

一种基于动态滑模控制的UUV轨迹跟踪方法

技术领域

本发明属于水下无人航行器的轨迹跟踪和动态滑模控制技术领域，具体涉及一种基于动态滑模控制的UUV轨迹跟踪方法。

背景技术

水下无人航行器(UUV)的轨迹精确跟踪控制能力是实现水下勘探、救捞、避障等特种作业任务的技术基础。但是，UUV通常具有欠驱动、高度的非线性和耦合性以及加速度不可积的非完整约束等特性，特别是UUV水动力系数的不确定性及外界时变扰动，使UUV的轨迹跟踪控制更加难以实现。

UUV的轨迹跟踪要求控制律能够导引UUV同时满足空间时变轨迹跟踪及速度时变要求，对时间条件具有强约束。目前常用的UUV轨迹跟踪的控制方法比较常见的有反步控制、神经网络控制、状态反馈及参数映射技术等。但以上控制方法各有局限性。利用反步法可以解决平面轨迹规划和跟踪问题，但反步法完全依赖于系统的精确模型，在未知时变扰动及参数不确定情况下，控制器的鲁棒性及有效性尚未给出评价与分析。利用神经网络在线自适应学习算法构建UUV的逆动力模型，可以实现其在慢变海流干扰下的路径跟踪控制，但是由于神经网络的自适应学习过程须要耗用一定的时间，控制系统的实时性受到影响，只能克服定常或慢变的海流扰动，无法实现的UUV的实时轨迹跟踪。在反步法基础上可以利用状态反馈及参数映射技术设计轨迹跟踪控制器，但是由于涉及大量的参数估计律，导致在线计算量很大，而且这种映射算法使得所设计的控制律表达形式比较复杂。UUV的轨迹跟踪要求控制律能够导引UUV同时满足空间时变轨迹跟踪及速度时变要求，对时间条件具有强约束.本专利针对UUV轨迹跟踪控制问题，将姿态跟踪转化为虚拟速度控制，将反步技术和自适应滑模控制方法结合起来，设计UUV轨迹跟踪控制器，提高系统的鲁棒性和自适应能力，实现UUV的水平面精确轨迹跟踪。

针对上述研究存在的问题和UUV轨迹跟踪控制系统的特点，将反步技术和自适应滑模控制方法结合起来,提出了一种动态滑模控制方法。该控制方法的优点在于控制器对建模误差、环境干扰力等非匹配不确定性的影响不敏感，具有良好的自适应能力和鲁棒性。

发明内容

本发明的目的在于提供一种基于动态滑模控制的UUV轨迹跟踪方法。

本发明的目的是这样实现的：

本发明包括如下步骤：

(1)建立UUV水平面模型；

(2)通过坐标转换获得误差变量，并对误差变量求导得到误差变量的导数；

(3)构造李雅普诺夫函数并且定义虚拟速度控制变量将姿态跟踪转化为虚拟速度控制；

(4)稳定虚拟速度控制变量，利用滑模控制方法对系统参数不精确及外界时变扰动进行自适应估计，建立滑模动态函数；

(5)选取动态滑模控制律，实现UUV的轨迹跟踪。

所述步骤(1)UUV水平面模型如下：

$\left\{\begin{array}{c}\stackrel{\·}{x}=cos\left(\mathrm{\ψ}\right)u-sin\left(\mathrm{\ψ}\right)v,\\ \stackrel{\·}{y}=\sin \left(\mathrm{\ψ}\right)u+cos\left(\mathrm{\ψ}\right)v,\\ \stackrel{\·}{\mathrm{\ψ}}=r,\\ \stackrel{\·}{u}=\frac{m_{22}}{m_{11}}vr-\frac{d_{11}}{m_{11}}u+\frac{{\mathrm{\τ}}_u+{\mathrm{\τ}}_{w1}}{m_{11}},\\ \stackrel{\·}{v}=-\frac{m_{11}}{m_{22}}ur-\frac{d_{22}}{m_{22}}v+\frac{{\mathrm{\τ}}_{w2}}{m_{22}},\\ \stackrel{\·}{r}=\frac{m_{11}-m_{22}}{m_{33}}uv-\frac{d_{33}}{m_{33}}r+\frac{{\mathrm{\τ}}_r+{\mathrm{\τ}}_{w3}}{m_{33}}\end{array}\right.$

其中：x，y为定义在固定坐标系下UUV的纵向位置矢量，横向位置矢量；ψ为UUV的航向角；u，v，r为UUV的纵向速度，横向速度和艏向角速度；τ_u和τ_r表示UUV的纵向推力和横向力矩；参数m_ii和d_ii是UUV的水动力参数。

所述步骤(2)的具体过程为：

给出误差变量x_e，y_e，ψ_e，其中x_e为纵向位置误差，y_e为横向位置误差，ψ_e艏向角误差；

UUV期望艏向角为：

${\mathrm{\ψ}}_d=arctan\left(\frac{{\stackrel{\·}{y}}_d}{{\stackrel{\·}{x}}_d}\right)$

则

$\left[\begin{array}{c}{x}_e\\ y_e\\ {\mathrm{\ψ}}_e\end{array}\right]=\left[\begin{array}{ccc}cos\left(\mathrm{\ψ}\right)& sin\left(\mathrm{\ψ}\right)& 0\\ -sin\left(\mathrm{\ψ}\right)& cos\left(\mathrm{\ψ}\right)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x-x_d\\ y-y_d\\ \mathrm{\ψ}-{\mathrm{\ψ}}_d\end{array}\right]$

对其求导可得：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_e=u-v_{p}cos\left({\mathrm{\ψ}}_e\right)+{\mathrm{ry}}_e,\\ {\stackrel{\·}{y}}_e=v+v_p\sin \left({\mathrm{\ψ}}_e\right)-{\mathrm{rx}}_e,\end{array}\right.$

其中r为艏向角速度，x_d为期望纵向位置变量，y_d为期望横向位置变量。

所述的步骤(3)的具体过程为：

选取李雅普诺夫函数

$V_1=\frac{1}{2}(x_e^2+y_e^2)$

对之求导得：

$\begin{array}{c}{\stackrel{\·}{V}}_1=x_e(u-v_{p}cos({\mathrm{\ψ}}_e)+{\mathrm{ry}}_e)+y_e(v+v_p\sin ({\mathrm{\ψ}}_e)-{\mathrm{rx}}_e)\\ =x_e(u-v_p\cos ({\mathrm{\ψ}}_e\left)\right)+y_e(v+v_p\sin ({\mathrm{\ψ}}_e\left)\right)\end{array}$

引入虚拟速度变量α_v，并令：

α_v＝v_psin(ψ_e)

变量ψ_e的控制转换为了虚拟速度变量α_v的控制。

所述的步骤(4)的具体过程为：

按下式计算动态滑模函数：

$\begin{array}{c}{S}_1=c_1{u}_e+{\stackrel{\·}{u}}_e+\frac{x_e}{m_{11}}-\frac{(F_1-{\hat{F}}_1)}{m_{11}}\\ =c_1{u}_e+\frac{({\stackrel{\·}{F}}_1+{\mathrm{\τ}}_u)+x_e}{m_{11}}\end{array}$

其中：

其中：c₁为正的常数，m₁₁、m₂₂为UUV的水动力系数，F₁和为需要估计的未知不确定项和它的估计值，τ_u为推进器推力，u_e为纵向速度误差；

$\begin{array}{c}{S}_2=c_2{r}_e+{\stackrel{\·}{r}}_e+\frac{m_{22}{\mathrm{\α}}_{ve}{v}_p\cos \left({\mathrm{\ψ}}_e\right)}{m_{33}}-\frac{(F_3-{\hat{F}}_3)}{m_{33}}\\ =c_2{r}_e+\frac{({\hat{F}}_3+{\mathrm{\τ}}_r)+m_{22}{\mathrm{\α}}_{ve}{v}_p\cos \left({\mathrm{\ψ}}_e\right)}{m_{33}}\\ =c_2{r}_e+\frac{({\hat{F}}_3+{\mathrm{\τ}}_r)}{m_{33}}+Q_2\end{array}$

其中Q₂＝m₂₂α_vev_p cos(ψ_e)/m₃₃

$\stackrel{\·}{\hat{F}}=r_e+c_2{S}_2{\mathrm{\τ}}_{w2}={10}^{-1}{m}_{22}+\mathrm{\λ}\left(sin\right(0.01t)-1)$

其中c₂为正的常数，m₂₂和m₃₃为水动力系数，F₃和为需要估计的未知不确定项和估计值，τ_r为推进器力矩；α_ve为虚拟速度误差，r_e为艏向角速度误差。

所述的步骤(5)中计算控制量并输出控制信号为：

$T_1=-c_1({\hat{F}}_1+{\mathrm{\τ}}_u)-\stackrel{\·}{\hat{F}}-{\stackrel{\·}{x}}_e-m_{11}{u}_e-k_{s1}\mathrm{sgn}\left(S_1\right)-w_{s1}{S}_1$

$T_2=-c_2({\hat{F}}_3+{\mathrm{\τ}}_r){\stackrel{\·}{\hat{F}}}_3-m_{33}{r}_e-m_{33}{\stackrel{\·}{Q}}_2-k_{s2}\mathrm{sgn}\left(S_2\right)-w_{s2}{S}_2$

其中c₁，k_s1，w_s1，c₂，k_s2，w_s2为正的常数，

本发明的有益效果在于：将UUV姿态跟踪控制转化为速度控制，能够有效避免传统反步法控制律设计存在的奇异值问题。本专利通过反步法和自适应动态滑模控制技术的组合，解决了UUV平面轨迹跟踪控制问题在系统中可能存在建模不确定性、未知环境扰动的问题、系统的参数不确定性。

附图说明

图1是动态滑模UUV控制器的控制结构框图。

图2是UUV的期望轨迹与实际轨迹示意图。

图3是无人水下航行器的艏向角误差示意图。

图4是无人水下航行器的纵向速度和横向速度示意图。

图5为无人水下航行器的滑模函数示意图。

图6为无人水下航行器的控制力示意图。

具体实施方式

下面结合附图对本发明做进一步描述。

本专利提出一种新型的UUV动态滑模轨迹跟踪控制设计方法，主要包括以下内容：

建立了水下无人航行器的数学模型。

通过坐标转换获得误差变量，并对误差变量求导得到误差变量的导数。

构造李雅普诺夫函数，采用虚拟速度来代替姿态误差的控制策略，定义虚拟速度控制变量将姿态跟踪转化为虚拟速度控制。

稳定虚拟速度控制变量，利用滑模控制方法对系统参数不精确及外界时变扰动进行自适应估计，建立滑模动态函数

选取动态滑模控制律，实现UUV的轨迹跟踪。

本专利以UUV为研究对象，提出了一种动态滑模控制方法，该方法将反步法和动态滑模技术相结合实现UUV的轨迹跟踪，具有良好的自适应能力和鲁棒性。

下面结合附图对本发明作进一步详细说明。

(1)考虑实际受控对象的特点，建立UUV的数学模型

UUV是一个空间的六自由度运动载体，为方便描述其运动，其数学模型通常建立在北东坐标系和运动坐标系下。在北东坐标系下定义其位置矢量，运动坐标系下定义其广义速度矢量。建立在两坐标系下的运动学和动力学水平面数学模型为：

$\left\{\begin{array}{c}\stackrel{\·}{x}=cos\left(\mathrm{\ψ}\right)u-sin\left(\mathrm{\ψ}\right)v,\\ \stackrel{\·}{y}=\sin \left(\mathrm{\ψ}\right)u+cos\left(\mathrm{\ψ}\right)v,\\ \stackrel{\·}{\mathrm{\ψ}}=r,\\ \stackrel{\·}{u}=\frac{m_{22}}{m_{11}}vr-\frac{d_{11}}{m_{11}}u+\frac{{\mathrm{\τ}}_u+{\mathrm{\τ}}_{w1}}{m_{11}},\\ \stackrel{\·}{v}=-\frac{m_{11}}{m_{22}}ur-\frac{d_{22}}{m_{22}}v+\frac{{\mathrm{\τ}}_{w2}}{m_{22}},\\ \stackrel{\·}{r}=\frac{m_{11}-m_{22}}{m_{33}}uv-\frac{d_{33}}{m_{33}}r+\frac{{\mathrm{\τ}}_r+{\mathrm{\τ}}_{w3}}{m_{33}}\end{array}\right.---\left(1\right)$

其中：x，y为定义在固定坐标系下UUV的纵向位置矢量，横向位置矢量，ψ为UUV的航向角u，v，r为UUV的纵向速度，横向速度和艏向角速度，τ_u和τ_r表示UUV的纵向推力和横向力矩，参数m_ii和d_ii是UUV的水动力参数。

(2)控制器的设计

步骤一、首先给出误差变量x_e，y_e，ψ_e，期望艏向角

则

其中x_e为纵向位置误差，y_e为横向位置误差，ψ_e艏向角误差，x_d为期望纵向位置变量，y_d为期望横向位置变量。

结合(1)可得

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_e=u-v_p\cos \left({\mathrm{\ψ}}_e\right)+{\mathrm{ry}}_e,\\ {\stackrel{\·}{y}}_e=v+v_p\sin \left({\mathrm{\ψ}}_e\right)-{\mathrm{rx}}_e,\end{array}\right.---\left(4\right)$

其中

考虑到(4)，选取李雅普诺夫函数为：

$V_1=\frac{1}{2}(x_e^2+y_e^2)---\left(5\right)$

对之求导并结合(4)可得

$\begin{array}{c}{\stackrel{\·}{V}}_1=x_e(u-v_{p}cos({\mathrm{\ψ}}_e)+{\mathrm{ry}}_e)+y_e(v+v_p\sin ({\mathrm{\ψ}}_e)-{\mathrm{rx}}_e)\\ =x_e(u-v_p\cos ({\mathrm{\ψ}}_e\left)\right)+y_e(v+v_p\sin ({\mathrm{\ψ}}_e\left)\right)\end{array}---\left(6\right)$

引入虚拟变量α_v，并另：

α_v＝v_psin(ψ_e) (7)

从虚拟变量公式中可以看出变量ψ_e的控制转换为了虚拟变量α_v的控制。为了保证是负的，u和α_v认为控制变量，它们的期望变量u_d和α_vd为：

u_d＝v_pcos(ψ_e)-k₁x_e/E (8)

α_vd＝-v-k₂y_e/E (9)

其中

步骤二、因为u_d和α_vd不是真的控制变量，引入虚拟误差变量u_e和α_ve：u_e＝u-u_d，α_ve＝α_v-α_vd (10)

那么

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_e=u_e-k_1{x}_e/E\\ {\stackrel{\·}{y}}_e={\mathrm{\α}}_{ve}-k_2{y}_e/E-{\mathrm{rx}}_e\end{array}\right.---\left(11\right)$

由此可得：

${\stackrel{\·}{V}}_1=-(k_1{x}_e^2+k_2{y}_e^2)/E+u_e{x}_e+{\mathrm{\α}}_{ve}{y}_e---\left(12\right)$

步骤三、稳定虚拟误差变量u_e

$\begin{array}{c}{\stackrel{\·}{u}}_e=\stackrel{\·}{u}-{\stackrel{\·}{u}}_d\\ =\frac{m_{22}vr-d_{11}u+{\mathrm{\τ}}_{w1}-m_{11}{\stackrel{\·}{u}}_d+{\mathrm{\τ}}_u}{m_{11}}\\ =\frac{F_1+{\mathrm{\τ}}_u}{m_{11}}\end{array}---\left(13\right)$

其中

选取李雅普诺夫函数

$V_2=V_1+\frac{1}{2}{m}_{11}{u}_e^2+\frac{1}{2}{(F_1-{\hat{F}}_1)}^2---\left(14\right)$

其中是F₁的估计值

选择动态滑模函数

$\begin{array}{c}{S}_1=c_1{u}_e+{\stackrel{\·}{u}}_e+\frac{x_e}{m_{11}}-\frac{(F_1-{\hat{F}}_1)}{m_{11}}\\ =c_1{u}_e+\frac{({\stackrel{\·}{F}}_1+{\mathrm{\τ}}_u)+x_e}{m_{11}}\end{array}---\left(15\right)$

式中c₁是正常量，则u_e的导数为：

${\stackrel{\·}{u}}_e=S_1-c_1{u}_e-\frac{x_e}{m_{11}}+\frac{(F_1-{\hat{F}}_1)}{m_{11}}---\left(16\right)$

则V₂的导数为：

$\begin{array}{c}{\stackrel{\·}{V}}_2=-(k_1{x}_e^2+k_2{y}_e^2)/E+{\mathrm{\α}}_{ve}{y}_e+m_{11}{u}_e{S}_1-c_1{m}_{11}{u}_e^2\\ +(u_e-{\stackrel{\·}{\hat{F}}}_1)(F_1-{\hat{F}}_1)\end{array}---\left(17\right)$

选那么则为：

${\stackrel{\·}{S}}_1=\frac{c_1(F_1+{\mathrm{\τ}}_u)+T_1+{\stackrel{\·}{\hat{F}}}_1+{\stackrel{\·}{x}}_e}{m_{11}}---\left(18\right)$

建立李雅普诺夫函数：

$V_3=V_2+\frac{1}{2}{m}_{11}{S}_1^2---\left(19\right)$

它的微分为：

$\begin{array}{c}{\stackrel{\·}{V}}_3=-(k_1{x}_e^2+k_2{y}_e^2)/E+{\mathrm{\α}}_{ve}{y}_e+m_{11}{u}_e{S}_1-c_1{m}_{11}{u}_e^2\\ +(u_e-{\stackrel{\·}{\hat{F}}}_1)(F_1-{\hat{F}}_1)+S_1\[c_1(F_1+{\mathrm{\τ}}_u)+T_1+{\stackrel{\·}{\hat{F}}}_1+{\stackrel{\·}{x}}_e\]\end{array}---\left(19\right)$

选择滑模控制率T₁为：

$T_1=-c_1({\hat{F}}_1+{\mathrm{\τ}}_u)-{\stackrel{\·}{\hat{F}}}_1-{\stackrel{\·}{x}}_e-m_{11}{u}_e-k_{s1}\mathrm{sgn}\left(S_1\right)-w_{s1}{S}_1---\left(21\right)$

其中k_s1和w_s1是正常量，由此可得：

$\begin{array}{c}{\stackrel{\·}{V}}_3=-(k_1{x}_e^2+k_2{y}_e^2)/E+{\mathrm{\α}}_{ve}{y}_e+m_{11}{u}_e{S}_1-c_1{m}_{11}{u}_e^2\\ +(u_e-{\stackrel{\·}{\hat{F}}}_1)(F_1-{\hat{F}}_1)+S_1\[c_1(F_1+{\mathrm{\τ}}_u)+T_1+{\stackrel{\·}{\hat{F}}}_1+{\stackrel{\·}{x}}_e\]\end{array}---\left(22\right)$

设计F₁的自适应控制律为：

${\stackrel{\·}{\hat{F}}}_1=u_e+c_1{S}_1---\left(23\right)$

则的V₃导数为：

${\stackrel{\·}{V}}_3=-(k_1{x}_e^2+k_2{y}_e^2)/E-c_1{m}_{11}{u}_e^2-k_{s1}\left|S_1\right|-w_{s1}{S}_1^2+{\mathrm{\α}}_{ve}{y}_e---\left(24\right)$

步骤四、稳定虚拟速度误差变量α_ve

$\begin{array}{c}{\stackrel{\·}{\mathrm{\α}}}_{ve}={\stackrel{\·}{\mathrm{\α}}}_v-{\stackrel{\·}{\mathrm{\α}}}_{vd}\\ ={\stackrel{\·}{v}}_p\sin \left({\mathrm{\ψ}}_e\right)+v_p\cos \left({\mathrm{\ψ}}_e\right)(r-{\stackrel{\·}{\mathrm{\ψ}}}_d)+\stackrel{\·}{v}+k_2(E^{-1}-y_e^2{E}^{-3}){\stackrel{\·}{y}}_e\\ -k_2{x}_e{y}_e{E}^{-3}{\stackrel{\·}{x}}_e\\ ={\stackrel{\·}{v}}_p\sin \left({\mathrm{\ψ}}_e\right)+v_p\cos \left({\mathrm{\ψ}}_e\right)(r-{\stackrel{\·}{\mathrm{\ψ}}}_d)+\frac{F_2}{m_{22}}+Q_1\end{array}---\left(25\right)$

其中F₂＝-m₁₁ur-d₂₂v+τ_w2，

为了使是负的,考虑变量r，设计它的期望变量r_d为：

$r_d={\stackrel{\·}{\mathrm{\ψ}}}_d+\frac{-{\stackrel{\·}{v}}_{p}sin\left({\mathrm{\ψ}}_e\right)-{\hat{F}}_2/m_{22}-Q_1-k_3{\mathrm{\α}}_{ve}-y_e/m_{22}}{v_{p}cos\left({\mathrm{\ψ}}_e\right)}---\left(26\right)$

其中是F₂估计量,k₃是正的常量

所以引入误差变量：

r_e＝r-r_d (27)

则α_ve的时间导数为：

${\stackrel{\·}{\mathrm{\α}}}_{ve}=r_e{v}_{p}cos\left({\mathrm{\ψ}}_e\right)+(F_2-{\hat{F}}_2)/m_{22}-k_3{\mathrm{\α}}_{ve}-y_e/m_{22}---\left(28\right)$

建立李雅普诺夫函数：

$V_4=V_3+\frac{1}{2}{m}_{22}{\mathrm{\α}}_{ve}^2+\frac{1}{2}{(F_2-{\hat{F}}_2)}^2---\left(29\right)$

它的时间导数为：

$\begin{array}{c}{\stackrel{\·}{V}}_4=-(k_1{x}_e^2+k_2{y}_e^2)/E-c_1{m}_{11}{u}_e^2-k_3{m}_{22}{\mathrm{\α}}_{ve}^2-k_{s1}\left|S_1\right|\\ -w_1{S}_1^2+({\mathrm{\α}}_{ve}-{\stackrel{\·}{\hat{F}}}_2)(F_2-\hat{F})+m_{22}{\mathrm{\α}}_{ve}{r}_e{v}_p\cos \left({\mathrm{\ψ}}_e\right)\end{array}---\left(30\right)$

设计F₂的自适应率为：

$\stackrel{\·}{\hat{F}}={\mathrm{\α}}_{ve}---\left(31\right)$

则V₄的导数为：

$\begin{array}{c}{\stackrel{\·}{V}}_4=-(k_1{x}_e^2+k_2{y}_e^2)/E-c_1{m}_{11}{u}_e^2-k_{s1}\left|S_1\right|-w_{s1}{S}_1^2-k_3{m}_{22}{\mathrm{\α}}_{ve}^2\\ +m_{22}{\mathrm{\α}}_{ve}{r}_e{v}_p\cos \left({\mathrm{\ψ}}_e\right)\end{array}---\left(32\right)$

步骤五、考虑r_e为辅助控制，因此接下来稳定误差变量r_e

$\begin{array}{c}{\stackrel{\·}{r}}_e=\stackrel{\·}{r}-{\stackrel{\·}{r}}_d\\ =\frac{(m_{11}-m_{22})uv-d_{33}r+{\mathrm{\τ}}_{w3}-m_{33}{\stackrel{\·}{r}}_d+{\mathrm{\τ}}_r}{m_{33}}\\ =\frac{(F_3+{\mathrm{\τ}}_r)}{m_{33}}\end{array}---\left(33\right)$

其中

建立如下李雅普诺夫函数：

$V_5=V_4+\frac{1}{2}{m}_{33}{r}_e^2+\frac{1}{2}{(F_3-{\hat{F}}_3)}^2---\left(34\right)$

其中是F₃的估计值

选择动态滑模函数：

$\begin{array}{c}{S}_2=c_2{r}_e+{\stackrel{\·}{r}}_e+\frac{m_{22}{\mathrm{\α}}_{ve}{v}_p\cos \left({\mathrm{\ψ}}_e\right)}{m_{33}}-\frac{(F_3-{\hat{F}}_3)}{m_{33}}\\ =c_2{r}_e+\frac{({\hat{F}}_3+{\mathrm{\τ}}_r)+m_{22}{\mathrm{\α}}_{ve}{v}_p\cos \left({\mathrm{\ψ}}_e\right)}{m_{33}}\\ =c_2{r}_e+\frac{({\hat{F}}_3+{\mathrm{\τ}}_r)}{m_{33}}+Q_2\end{array}---\left(35\right)$

其中Q₂＝(m₂₂α_vev_pcos(ψ_e)/m₃₃)，c₂是正常量

则

求V₅对时间的导数并且把(36)代入

$\begin{array}{c}{\stackrel{\·}{V}}_5=-(k_1{x}_e^2+k_2{y}_e^2)/E-c_1{m}_{11}{u}_e^2-k_{s1}\left|S_1\right|-w_{s1}{S}_1^2-k_3{m}_{22}{\mathrm{\α}}_{ve}^2\\ +m_{33}{r}_e{S}_2-c_2{m}_{33}{r}_e^2+(r_e-{\stackrel{\·}{\hat{F}}}_3)(F_3-{\hat{F}}_3)\end{array}---\left(37\right)$

求(35)对时间的导数，并且另则为

${\stackrel{\·}{S}}_2=\frac{c_2(F_3+{\mathrm{\τ}}_r)+T_2+{\stackrel{\·}{\hat{F}}}_3}{m_{33}}+{\stackrel{\·}{Q}}_2---\left(38\right)$

其中

选取李雅普诺夫函数

则它的导数为

$\begin{array}{c}{\stackrel{\·}{V}}_6=-(k_1{x}_e^2+k_2{y}_e^2)/E-c_1{m}_{11}{u}_e^2-k_{s1}\left|S_1\right|-w_{s1}{S}_1^2-k_3{m}_{22}{\mathrm{\α}}_{ve}^2\\ -c_2{m}_{33}{r}_e^2+m_{33}{r}_e{S}_2+(r_e-{\stackrel{\·}{\hat{F}}}_3)(F_3-{\hat{F}}_3)\\ +m_{33}{S}_2(\frac{c_2(F_3+{\mathrm{\τ}}_r)+T_2+{\stackrel{\·}{\hat{F}}}_3}{m_{33}}+{\stackrel{\·}{Q}}_2)\end{array}---\left(40\right)$

选择动态滑模控制率T₂为:

$T_2=-c_2({\hat{F}}_3+{\mathrm{\τ}}_r)-{\stackrel{\·}{\hat{F}}}_3-m_{33}{r}_e-m_{33}{\stackrel{\·}{Q}}_2-k_{s2}\mathrm{sgn}\left(S_2\right)-w_{s2}{S}_2---\left(41\right)$

其中k_s2和w_s2是正常量，把(41)代入(40)，则

$\begin{array}{c}{\stackrel{\·}{V}}_6=-(k_1{x}_e^2+k_2{y}_e^2)/E-c_1{m}_{11}{u}_e^2-k_{s1}\left|S_1\right|-w_{s1}{S}_1^2-k_3{m}_{22}{\mathrm{\α}}_{ve}^2\\ -c_2{m}_{33}{r}_e^2-k_{s2}\left|S_2\right|-w_{s2}{S}_2^2+(r_e-{\stackrel{\·}{\hat{F}}}_3+c_2{S}_2)(F_3-{\hat{F}}_3)\end{array}---\left(42\right)$

设计F₃的自适应率

${\stackrel{\·}{\hat{F}}}_3=r_e+c_2{S}_2---\left(43\right)$

则的V₆导数为

$\begin{array}{c}{\stackrel{\·}{V}}_6=-(k_1{x}_e^2+k_2{y}_e^2)/E-k_3{m}_{22}{\mathrm{\α}}_{ve}^2-c_1{m}_{11}{u}_e^2-c_2{m}_{33}{r}_e^2-k_{s1}\left|S_1\right|\\ -w_{s1}{S}_1^2-k_{s2}\left|S_2\right|-w_{s2}{S}_2^2\≤0\end{array}---\left(44\right)$

考虑UUV水平面模型(1)，存在滑模自适应律(23)、(31)、(43)和控制律(21)、(41)，能够保证最终跟踪上期望轨迹

仿真实验与验证

下面举例说明验证设计的UUV轨迹跟踪控制器的有效性。

UUV水平面模型参数为：

m＝185kg,I_z＝50kgm² X_u＝70kg/s,X_u|u|＝100kg/m,

Y_v＝100kg/s,Y_v|v|＝200kg/m,N_r＝50kgm²/s,N_r|r|＝100kgm²

d₁₁＝X_u+X_u|u||u|,d₂₂＝Y_v+Y_v|v||v|,d₃₃＝Nr+N_r|r_||r|,

控制器参数为

k₁＝1.7，k₂＝1，k₃＝0.1，c₁＝0.8，c₂＝1.3，k_s1＝k_s2＝0.5，w_s1＝w_s2＝1

$\left\{\begin{array}{c}{\mathrm{\τ}}_{w1}={10}^{-1}{m}_{11}+\mathrm{\λ}\left(\sin \right(0.01t)-1)\\ {\mathrm{\τ}}_{w2}={10}^{-1}{m}_{22}+\mathrm{\λ}\left(\sin \right(0.01t)-1)\\ {\mathrm{\τ}}_{w3}={10}^{-1}{m}_{33}+\mathrm{\λ}\left(\sin \right(0.01t)-1)\end{array}\right.$

期望路径为

$x_d=\left\{\begin{array}{cc}t& t<200\\ 200+100*cos(0.005*pi*(t-200\left)\right)& 200<t<400\\ 600-t& t\&GreaterEqual;400\end{array}\right.$

$y_d=\left\{\begin{array}{cc}100& t<200\\ 100*cos(0.005*pi*(t-200\left)\right)& 200<t<400\\ 300-t& t\&GreaterEqual;400\end{array}\right.$

${\mathrm{\&psi;}}_d=\left\{\begin{array}{cc}0& t<200\\ -0.005*pi*(t-200)& 200<t<400\\ -pi+atan\left(1\right)& t\&GreaterEqual;400\end{array}\right.$

图2给出了UUV轨迹跟踪仿真结果，显然可以看到，本专利所提出的方法不仅可以跟踪直线而且可以跟踪曲线轨迹。从图3、图4中可以看出，在从圆弧到直线的过渡时间为400秒时艏向角改变，UUV能够快速响应跟踪，说明UUV的动态响应是足够快的。图5为无人水下航行器的滑模函数示意图。图6给出了推进器控制力变化情况。本专利所提出的方法的效率和有效性充分体现了所设计控制器对系统模型不精确具有鲁棒性和自适应能力。

Claims (6)

1.一种基于动态滑模控制的UUV轨迹跟踪方法，其特征在于，包括如下步骤：

(1)建立UUV水平面模型；

(2)通过坐标转换获得误差变量，并对误差变量求导得到误差变量的导数；

(3)构造李雅普诺夫函数并且定义虚拟速度控制变量将姿态跟踪转化为虚拟速度控制；

(4)稳定虚拟速度控制变量，利用滑模控制方法对系统参数不精确及外界时变扰动进行自适应估计，建立滑模动态函数；

(5)选取动态滑模控制律，实现UUV的轨迹跟踪。

2.根据权利要求1所述的一种基于动态滑模控制的UUV轨迹跟踪方法，其特征在于，所述步骤(1)UUV水平面模型如下：

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{x}=cos\left(\mathrm{\&psi;}\right)u-sin\left(\mathrm{\&psi;}\right)v,\\ \stackrel{\&CenterDot;}{y}=\sin \left(\mathrm{\&psi;}\right)u+cos\left(\mathrm{\&psi;}\right)v,\\ \stackrel{\&CenterDot;}{\mathrm{\&psi;}}=r,\\ \stackrel{\&CenterDot;}{u}=\frac{m_{22}}{m_{11}}vr-\frac{d_{11}}{m_{11}}u+\frac{{\mathrm{\&tau;}}_u+{\mathrm{\&tau;}}_{w1}}{m_{11}},\\ \stackrel{\&CenterDot;}{v}=-\frac{m_{11}}{m_{22}}ur-\frac{d_{22}}{m_{22}}v+\frac{{\mathrm{\&tau;}}_{w2}}{m_{22}},\\ \stackrel{\&CenterDot;}{r}=\frac{m_{11}-m_{22}}{m_{33}}uv-\frac{d_{33}}{m_{33}}r+\frac{{\mathrm{\&tau;}}_r+{\mathrm{\&tau;}}_{w3}}{m_{33}}\end{array}\right.$

其中：x，y为定义在固定坐标系下UUV的纵向位置矢量，横向位置矢量；ψ为UUV的航向角；u，v，r为UUV的纵向速度，横向速度和艏向角速度；τ_u和τ_r表示UUV的纵向推力和横向力矩；参数m_ii和d_ii是UUV的水动力参数。

3.根据权利要求1所述的一种基于动态滑模控制的UUV轨迹跟踪方法，其特征在于，所述步骤(2)的具体过程为：

给出误差变量x_e，y_e，ψ_e，其中x_e为纵向位置误差，y_e为横向位置误差，ψ_e艏向角误差；

UUV期望艏向角为：

${\mathrm{\&psi;}}_d=arctan\left(\frac{{\stackrel{\&CenterDot;}{y}}_d}{{\stackrel{\&CenterDot;}{x}}_d}\right)$

则

$\left[\begin{array}{c}{x}_e\\ y_e\\ {\mathrm{\&psi;}}_e\end{array}\right]=\left[\begin{array}{ccc}cos\left(\mathrm{\&psi;}\right)& sin\left(\mathrm{\&psi;}\right)& 0\\ -sin\left(\mathrm{\&psi;}\right)& cos\left(\mathrm{\&psi;}\right)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x-x_d\\ y-y_d\\ \mathrm{\&psi;}-{\mathrm{\&psi;}}_d\end{array}\right]$

对其求导可得：

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_e=u-v_{p}cos\left({\mathrm{\&psi;}}_e\right)+r{y}_e,\\ {\stackrel{\&CenterDot;}{y}}_e=v+v_p\sin \left({\mathrm{\&psi;}}_e\right)-r{x}_e,\end{array}\right.$

其中r为艏向角速度，x_d为期望纵向位置变量，y_d为期望横向位置变量。

4.根据权利要求1所述的一种基于动态滑模控制的UUV轨迹跟踪方法，其特征在于，所述的步骤(3)的具体过程为：

选取李雅普诺夫函数

$V_1=\frac{1}{2}(x_e^2+y_e^2)$

对之求导得：

$\begin{array}{c}{\stackrel{\&CenterDot;}{V}}_1=x_e(u-v_{p}cos({\mathrm{\&psi;}}_e)+{\mathrm{ry}}_e)+y_e(v+v_p\sin ({\mathrm{\&psi;}}_e)-{\mathrm{rx}}_e)\\ =x_e(u-v_p\cos \left({\mathrm{\&psi;}}_e\right))+y_e(v+v_p\sin ({\mathrm{\&psi;}}_e))\end{array}$

引入虚拟速度变量α_v，并令：

α_v＝v_psin(ψ_e)

变量ψ_e的控制转换为了虚拟速度变量α_v的控制。

5.根据权利要求1所述的一种基于动态滑模控制的UUV轨迹跟踪方法，其特征在于，所述的步骤(4)的具体过程为：

按下式计算动态滑模函数：

$\begin{array}{c}{S}_1=c_1{u}_e+{\stackrel{\&CenterDot;}{u}}_e+\frac{x_e}{m_{11}}-\frac{(F_1-{\hat{F}}_1)}{m_{11}}\\ =c_1{u}_e+\frac{\left({\stackrel{\&CenterDot;}{F}}_1+{\mathrm{\&tau;}}_u\right)+x_e}{m_{11}}\end{array}$

其中：

其中：c₁为正的常数，m₁₁、m₂₂为UUV的水动力系数，F₁和为需要估计的未知不确定项和它的估计值，τ_u为推进器推力，u_e为纵向速度误差；

$\begin{array}{c}{S}_2=c_2{r}_e+{\stackrel{\&CenterDot;}{r}}_e+\frac{m_{22}{\mathrm{\&alpha;}}_{ve}{v}_{p}cos\left({\mathrm{\&psi;}}_e\right)}{m_{33}}-\frac{\left(F_3-{\hat{F}}_3\right)}{m_{33}}\\ =c_2{r}_e+\frac{\left({\hat{F}}_3+{\mathrm{\&tau;}}_r\right)+m_{22}{\mathrm{\&alpha;}}_{ve}{v}_p\cos \left({\mathrm{\&psi;}}_e\right)}{m_{33}}\\ =c_2{r}_e+\frac{\left({\hat{F}}_3+{\mathrm{\&tau;}}_r\right)}{m_{33}}+Q_2\end{array}$

其中

其中c₂为正的常数，m₂₂和m₃₃为水动力系数，F₃和为需要估计的未知不确定项和估计值，τ_r为推进器力矩；α_ve为虚拟速度误差，r_e为艏向角速度误差。

6.根据权利要求1所述的一种基于动态滑模控制的UUV轨迹跟踪方法，其特征在于，所述的步骤(5)中计算控制量并输出控制信号为：

$T_1=-c_1({\hat{F}}_1+{\mathrm{\&tau;}}_u)-{\stackrel{\&CenterDot;}{\hat{F}}}_1-{\stackrel{\&CenterDot;}{x}}_e-m_{11}{u}_e-k_{s1}\mathrm{sgn}\left(S_1\right)-w_{s1}{S}_1$

$T_2=-c_2({\hat{F}}_3+{\mathrm{\&tau;}}_r)-{\stackrel{\&CenterDot;}{\hat{F}}}_3-m_{33}{r}_e-m_{33}{\stackrel{\&CenterDot;}{Q}}_2-k_{s2}\mathrm{sgn}\left(S_2\right)-w_{s2}{S}_2$

其中c₁，k_s1，w_s1，c₂，k_s2，w_s2为正的常数，