CN105929842A - 一种基于动态速度调节的欠驱动uuv平面轨迹跟踪控制方法 - Google Patents

Abstract

一种基于动态速度调节的欠驱动UUV平面轨迹跟踪控制方法，涉及欠驱动水下无人航行器的运动控制技术。本发明是为了实现欠驱动UUV平面轨迹的精确跟踪控制。包括以下步骤：步骤一：UUV根据当前任务获取位置、姿态信息；步骤二：利用欠驱动UUV的数学模型得出位置、姿态误差变量；步骤三：采用定义虚拟速度误差变量的方法，计算出虚拟控制律；步骤四：结合生物启发模型对速度误差进行动态调节；步骤五：推导出动态速度调节控制器产生的控制信号，实现欠驱动UUV平面轨迹跟踪控制。本发明方法能够对欠驱动UUV的速度进行动态调节，同时避免了传统反步法中首向角误差等于90°时的奇异值，实现了在外界常值扰动下对圆形轨迹的跟踪。

Description

一种基于动态速度调节的欠驱动UUV平面轨迹跟踪控制方法

技术领域

本发明涉及欠驱动水下无人航行器的运动控制技术，特别是涉及一种基于动态速度调节的欠驱动UUV平面轨迹跟踪控制技术。

背景技术

水下无人航行器(Unmanned underwater vehicle，UUV)的轨迹跟踪控制能力是实现水下地形测绘、救生、勘探等任务的技术基础。轨迹跟踪要求控制律能够引导UUV的实际轨迹收敛到具有时变特性的参考轨迹，加之欠驱动特性、非线性和耦合性等，使得欠驱动UUV轨迹跟踪控制问题变的具有挑战性。与路径跟踪相比，轨迹跟踪更加强调时间与空间上的要求，也就是期望的位置受时间的约束，符合工程上的实际应用，具有极大的应用价值。轨迹跟踪控制问题一直是国内外研究的热点。

与其它控制方法相比，反步法采用了基于系统分层递推的设计思想，为欠驱动UUV轨迹跟踪控制系统的设计提供了有效的手段。然而，在设计中间分层系统时，必须要求逐步的计算出中间虚拟控制量的导数，然后逐级的引入后续的子系统，实现对前级子系统的镇定，利用这种迭代的思想获得最终的实际控制输入。可见当中间的虚拟变量选取不恰当的时候会增加计算量，并且虚拟速度控制都和误差相关，当误差增大时，速度会产生较大的跳变，可能会超出控制力矩的输出范围，导致控制失效。

为了克服传统反步法中的计算量大的缺点，文献《A biologically inspiredapproach to tracking control of underactuated surface vessels subject tounknown dynamics》(Expert Systems with Applications.2015,第42卷第4期)利用生物模型的微分输出去逼近虚拟控制律的导数；文献《基于滤波反步法的欠驱动AUV三维路径跟踪控制》(自动化学报.2015,第41卷第3期)和文献《基于滤波反步法的无人直升机轨迹跟踪控制》(控制与决策.2012,第27卷第4期)利用二阶滤波过程获得了虚拟控制量的导数，简化了控制器的设计过程；为了避免控制设计中的奇异值问题，文献《欠驱动无人水下航行器三维轨迹跟踪的反步控制》(控制理论与应用.2014,第31卷第11期)提出了定义虚拟速度误差变量的反步控制方法。目前还没有相关文献讨论基于动态速度调节的欠驱动UUV平面轨迹跟踪控制设计。

发明内容

本发明是为了实现欠驱动UUV平面轨迹的精确跟踪控制，从而提供一种基于动态速度调节的欠驱动UUV平面轨迹跟踪控制方法。

一种基于动态速度调节的欠驱动UUV平面轨迹跟踪控制方法，它包括以下步骤：

步骤一、根据当前任务，将期望轨迹位置信息给定UUV，并通过所搭载的导航设备和传感器采集数据，获得当前位置P(t)＝[x(t),y(t)]^T信息、姿态ψ(t)信息、速度V(t)＝[u_r(t),v_r(t),r(t)]^T信息；u_r(t)为UUV的纵向速度，v_r(t)为UUV的横向运动速度，r(t)代表UUV的艏摇角速度；

步骤二、利用步骤一中获得的当前位置P(t)＝[x(t),y(t)]^T信息、姿态ψ(t)信息、速度V(t)＝[u_r(t),v_r(t),r(t)]^T信息，通过欠驱动UUV的数学模型和轨迹跟踪位置误差模型，计算出实际轨迹与期望轨迹之间的位置误差和期望姿态，将地面坐标信息转换为船体坐标信息；

步骤三、基于步骤二中计算出的位置误差，采用定义虚拟速度误差变量的方法，将姿态跟踪控制转化为速度控制，计算纵向速度虚拟控制律u_d和艏摇角速度虚拟控制律r_d；

步骤四、结合步骤二中获得的实际轨迹与期望轨迹之间的位置误差和期望姿态和步骤三获得的纵向速度虚拟控制律u_d和艏摇角速度虚拟控制律r_d，计算得出纵向速度误差e_u和角速度误差e_r，并将该两个误差分别通入生物启发模型，通过设置模型中的参数，完成对速度误差的动态调节；

步骤五、根据给定的数学模型和步骤四中的动态调节输出量，推导欠驱动UUV平面轨迹跟踪的动态速度调节控制器，包括：纵向控制力矩τ_u的控制信号、艏摇控制力矩τ_r的控制信号，计算界扰动自适应控制律，实现在外界常值扰动下对欠驱动UUV平面轨迹跟踪控制。

步骤二中，欠驱动UUV的数学模型包括运动学模型和动力学模型、轨迹跟踪误差模型、位置误差、期望姿态的表达式分别为：

运动学模型：

$\left\{\begin{array}{c}\stackrel{\·}{x}=u_{r}cos\mathrm{\ψ}-v_{r}sin\mathrm{\ψ}\\ \stackrel{\·}{y}=u_r\sin \mathrm{\ψ}+v_r\cos \mathrm{\ψ}\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.---\left(1\right)$

动力学模型：

$\left\{\begin{array}{c}{\stackrel{\·}{u}}_r=\frac{m_{22}}{m_{11}}{v}_{r}r-\frac{X_u}{m_{11}}{u}_r-\frac{X_{u\left|u\right|}}{m_{11}}{u}_r\left|u_r\right|+\frac{{\mathrm{\τ}}_u+{\mathrm{\ω}}_u}{m_{11}}\\ {\stackrel{\·}{v}}_r=-\frac{m_{11}}{m_{22}}{u}_{r}r-\frac{Y_v}{m_{22}}{v}_r-\frac{Y_{v\left|v\right|}}{m_{22}}{v}_r\left|v_r\right|+\frac{{\mathrm{\ω}}_v}{m_{22}}\\ \stackrel{\·}{r}=\frac{(m_{11}-m_{22})}{m_{33}}{u}_r{v}_r-\frac{N_r}{m_{33}}r-\frac{N_{r\left|r\right|}}{m_{33}}r\left|r\right|+\frac{{\mathrm{\τ}}_r+{\mathrm{\ω}}_r}{m_{33}}\end{array}\right.---\left(2\right)$

其中，m为航行器的总质量；ω_u、ω_v、ω_r为外界扰动在UUV运动系各自由度的分量，X_u,X_u|u|,Y_v,Y_v|v|,N_r,N_r|r|为水动力参数；

位置误差：

$\left\{\begin{array}{c}{x}_e=x-x_d\\ y_e=y-y_d\end{array}\right.---\left(3\right)$

式中：(x,y)为实际轨迹位置信息；(x_d,y_d)为期望轨迹位置信息；

轨迹跟踪位置误差模型：

$\left\{\begin{array}{c}{\stackrel{\·}{e}}_x=u_r-v_{p}cos\left({\mathrm{\ψ}}_e\right)+{\mathrm{re}}_y\\ {\stackrel{\·}{e}}_y=v_r+v_p\sin \left({\mathrm{\ψ}}_e\right)-{\mathrm{re}}_x\end{array}\right.---\left(4\right)$

其中，(e_x，e_y)为船体坐标系下的位置误差变量；

期望姿态：

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

步骤三中，虚拟速度误差变量α、纵向速度虚拟控制律u_d、艏摇角速度虚拟控制律r_d的表达式分别为：

虚拟速度误差变量：

α＝v_p sin(ψ_e) (6)

纵向速度虚拟控制律：

$u_d=v_{p}cos\left({\mathrm{\ψ}}_e\right)-\frac{k_1{e}_x}{e}---\left(7\right)$

其中，k₁、k₂为正常数，

艏向角速度虚拟控制律：

$r_d={\stackrel{\·}{\mathrm{\ψ}}}_d+\frac{(-k_3{e}_{\mathrm{\α}}-e_y+{\stackrel{\·}{\mathrm{\α}}}_d-{\stackrel{\·}{v}}_{p}sin({\mathrm{\ψ}}_e\left)\right)}{v_p}---\left(8\right)$

其中，k₃为正常数。

步骤四中，纵向速度误差e_u、角速度误差e_r、生物启发模型的表达式分别为：

纵向速度误差：

e_u＝u-u_d (9)

角速度误差：

e_r＝r-r_d (10)

生物启发模型：

${\stackrel{\·}{S}}_u=-A_1{S}_u+(B_1-S_u)f\left(e_u\right)-(D_1+S_u)g\left(e_u\right)---\left(11\right)$

其中：S_u为速度动态模型的输出；参数A₁是正常数，为动态纵向速度误差输出的衰减率；正常数B₁和D₁分别为动态纵向速度误差输出的上限和下限；

函数f(e_u)＝max{e_u,0}，g(e_u)＝max{-e_u,0}；

${\stackrel{\·}{S}}_r=-A_2{S}_r+(B_2-S_r)f\left(e_r\right)-(D_2+S_r)g\left(e_r\right)---\left(12\right)$

其中：S_r为速度动态模型的输出；参数A₂是正常数，为动态角速度误差输出的衰减率；正常数B₂和D₂分别为动态角速度误差输出的上限和下限；函数f(e_r)＝max{e_r,0}，g(e_r)＝max{-e_r,0}。

步骤五中，纵向控制力矩τ_u的控制信号、艏摇控制力矩τ_r的控制信号的表达式分别为：

纵向控制力矩：

${\mathrm{\τ}}_u=(-m_{22}{v}_{r}r+X_u{u}_r+X_{u\left|u\right|}{u}_r|u_r|-{\widehat{\mathrm{\ω}}}_u)+m_{11}({\stackrel{\·}{u}}_d-e_x-S_u)---\left(13\right)$

其中，为对当前外界扰动的估计值，

艏摇控制力矩：

${\mathrm{\τ}}_r=(-(m_{11}-m_{22})u_r{v}_r+N_{r}r+N_{r\left|r\right|}r|r|-{\widehat{\mathrm{\ω}}}_r)+m_{33}({\stackrel{\·}{r}}_d-v_{p}cos({\mathrm{\ψ}}_e)e_{\mathrm{\α}}-S_r)---\left(14\right)$

其中，为对当前外界扰动的估计值，

所述的界扰动自适应控制律为：

$\left\{\begin{array}{c}{\stackrel{\·}{\widetilde{\mathrm{\ω}}}}_u=-{\widetilde{\mathrm{\ω}}}_u-\frac{e_u}{k_4}\\ {\stackrel{\·}{\widetilde{\mathrm{\ω}}}}_r=-{\widetilde{\mathrm{\ω}}}_r-\frac{e_r}{k_5}\end{array}\right.---\left(15\right)$

其中：k₄、k₅为正常数。

本发明针对欠驱动无人水下航行器平面轨迹跟踪控制中的位置、速度、姿态等随时间变化的要求，定义了虚拟速度误差控制变量的反步控制设计方法，有别于传统定义虚拟速度误差变量的方法，有效的避免了控制律中出现奇异值的问题，并利用生物启发模型来调节所设计的速度误差，实现了对速度进行一定范围内的动态调节的目的，该设计在保证避免奇异值的同时，提高了控制器的性能。同时设计了欠驱动UUV平面动态速度调节轨迹跟踪控制器，并基于李雅普诺夫理论证明了该控制系统误差在外界常值扰动下最终一致有界。仿真研究中考虑到了艏向角误差为±90°的情况，仿真结果表明，本发明所提出的控制方法收敛、有效，可以实现欠驱动UUV平面轨迹的精确跟踪控制。

本发明方法的有益效果：

1、定义了虚拟速度误差变量，将姿态跟踪控制转化为速度控制，简化了计算过程；

2、设计了虚拟控制律，避免了基于视线法设计虚拟控制量时，艏向角误差为±90°时的奇异值问题；

3、将纵向速度误差和艏向角速度误差通入到生物启发模型，完成了对速度的动态调节增加了系统的控制性能；

4、在外界常值扰动下可以实现对欠驱动UUV平面轨迹的精确跟踪控制。

附图说明

图1是欠驱动水下无人航行器平面轨迹跟踪控制流程示意图；

图2是欠驱动UUV平面轨迹跟踪示意图；

图3是欠驱动UUV平面轨迹跟踪控制系统结构图；

图4是欠驱动UUV圆形轨迹跟踪效果仿真示意图；

图5是欠驱动UUV圆形轨迹跟踪位置误差仿真示意图；

图6是欠驱动UUV圆形轨迹跟踪艏向角误差和总位置误差仿真示意图；

图7是欠驱动UUV圆形轨迹跟踪速度与角速度响应曲线仿真示意图；

图8是欠驱动UUV圆形轨迹跟踪控制输入响应曲线仿真示意图；

图9是欠驱动UUV圆形轨迹跟踪角速度误差与其动态速度调节输出变化曲线仿真示意图；

图10是欠驱动UUV圆形轨迹跟踪纵向速度误差与其动态速度调节输出变化曲线仿真示意图；

具体实施方式

具体实施方式一、一种基于动态速度调节的欠驱动UUV平面轨迹跟踪控制方法，

步骤一中，UUV根据当前任务，将期望轨迹位置信息给定UUV，并通过所搭载的导航设备和传感器采集数据，获得当前的位置P(t)＝[x(t),y(t)]^T信息、姿态ψ(t)信息、速度V(t)＝[u_r(t),v_r(t),r(t)]^T信息。

然后UUV获取在地面坐标系下的初始位置P(t)＝[x(t),y(t)]^T，UUV初始的艏向角ψ(t)，UUV的初始纵向速度u_r(t)、横向速度v_r(t)和艏摇角速度r(t)，并给定期望轨迹位置P_d(t)＝[x_d(t),y_d(t)]^T。

步骤二中，利用步骤1中的信息，通过欠驱动UUV的数学模型和轨迹跟踪位置误差模型，计算出实际轨迹与期望轨迹之间的位置误差和期望姿态，将地面坐标信息转换为船体坐标信息。

所涉及的欠驱动UUV的数学模型包括运动学模型和动力学模型表达式为：

运动学模型：

$\left\{\begin{array}{c}\stackrel{\·}{x}=u_{r}cos\mathrm{\ψ}-v_{r}sin\mathrm{\ψ}\\ \stackrel{\·}{y}=u_r\sin \mathrm{\ψ}+v_r\cos \mathrm{\ψ}\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.---\left(1\right)$

动力学模型：

$\left\{\begin{array}{c}{\stackrel{\·}{u}}_r=\frac{m_{22}}{m_{11}}{v}_{r}r-\frac{X_u}{m_{11}}{u}_r-\frac{X_{u\left|u\right|}}{m_{11}}{u}_r\left|u_r\right|+\frac{{\mathrm{\τ}}_u+{\mathrm{\ω}}_u}{m_{11}}\\ {\stackrel{\·}{v}}_r=-\frac{m_{11}}{m_{22}}{u}_{r}r-\frac{Y_v}{m_{22}}{v}_r-\frac{Y_{v\left|v\right|}}{m_{22}}{v}_r\left|v_r\right|+\frac{{\mathrm{\ω}}_v}{m_{22}}\\ \stackrel{\·}{r}=\frac{(m_{11}-m_{22})}{m_{33}}{u}_r{v}_r-\frac{N_r}{m_{33}}r-\frac{N_{r\left|r\right|}}{m_{33}}r\left|r\right|+\frac{{\mathrm{\τ}}_r+{\mathrm{\ω}}_r}{m_{33}}\end{array}\right.---\left(2\right)$

其中，ω_u、ω_v、ω_r为外界扰动在UUV运动系各自由度的分量，X_u,X_u|u|,Y_v,Y_v|v|,N_r,N_r|r|为水动力参数。

根据给定的期望轨迹位置信息可以得到UUV的期望姿态：

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

姿态误差方程为：

ψ_e＝ψ-ψ_d (4)

所述的将地面坐标信息转换为船体坐标的过程如下：

首先根据期望轨迹获得地面坐标系下的位置误差为：

$\left\{\begin{array}{c}{x}_e=x-x_d\\ y_e=y-y_d\end{array}\right.---\left(5\right)$

再经过坐标转换后得到新的船体坐标系下的位置误差为：

$\left[\begin{array}{c}{e}_x\\ e_y\end{array}\right]=\left[\begin{array}{cc}cos\mathrm{\ψ}& sin\mathrm{\ψ}\\ -sin\mathrm{\ψ}& \cos \mathrm{\ψ}\end{array}\right]\left[\begin{array}{c}{x}_e\\ y_e\end{array}\right]---\left(6\right)$

轨迹跟踪位置误差模型：

$\left\{\begin{array}{c}{\stackrel{\·}{e}}_x=u_r-v_{p}cos\left({\mathrm{\ψ}}_e\right)+{\mathrm{re}}_y\\ {\stackrel{\·}{e}}_y=v_r+v_p\sin \left({\mathrm{\ψ}}_e\right)-{\mathrm{re}}_x\end{array}\right.---\left(7\right)$

其中，至此完成了欠驱动UUV的初始化设置。

步骤三中按下式分别给出或计算出所定义的虚拟速度误差变量、纵向速度虚拟控制律u_d和艏向角速度虚拟控制律r_d。

对于位置误差(6)构造李雅普诺夫能量函数：

$V_1\left(t\right)=\frac{1}{2}(e_x^2+e_y^2)---\left(8\right)$

定义虚拟速度误差变量：

α＝v_psin(ψ_e) (9)

所设计的虚拟控制律α_d：

${\mathrm{\α}}_d=-v_r-\frac{k_2{e}_y}{e}---\left(10\right)$

纵向速度虚拟控制律：

$u_d=v_{p}cos\left({\mathrm{\ψ}}_e\right)-\frac{k_1{e}_x}{e}---\left(11\right)$

其中，k₁、k₂为正常数，

给出误差变量分别为：

$\left\{\begin{array}{c}{e}_u=u_r-u_d\\ e_{\mathrm{\α}}=\mathrm{\α}-{\mathrm{\α}}_d\end{array}\right.---\left(12\right)$

结合式(9)至(12)对式(8)求导可得出：

$\begin{array}{c}{\stackrel{\·}{V}}_1\left(t\right)=e_x{\stackrel{\·}{e}}_x+e_y{\stackrel{\·}{e}}_y\\ =e_x(u_r-v_p\cos ({\mathrm{\ψ}}_e))+e_y(v_r+v_p\sin ({\mathrm{\ψ}}_e))\\ =-(k_1{e}_x^2+k_2{e}_y^2)/e+e_u{e}_x+e_{\mathrm{\α}}{e}_y\end{array}---\left(13\right)$

然后对虚拟控制量e_α构造李雅普诺夫能量函数：

$V_2\left(t\right)=V_1\left(t\right)+\frac{1}{2}{e}_{\mathrm{\α}}^2---\left(14\right)$

所设计的艏向角速度虚拟控制律为：

$r_d={\stackrel{\·}{\mathrm{\ψ}}}_d+\frac{(-k_3{e}_{\mathrm{\α}}-e_y+{\stackrel{\·}{\mathrm{\α}}}_d-{\stackrel{\·}{v}}_{p}sin({\mathrm{\ψ}}_e\left)\right)}{v_p}---\left(15\right)$

给出误差变量为：

e_r＝r-r_d (16)

结合式(15)，(16)对式(14)求导可得出：

${\stackrel{\·}{V}}_2\left(t\right)=-(k_1{e}_x^2+k_2{e}_y^2)/e-k_3{e}_{\mathrm{\α}}^2+v_p{e}_{\mathrm{\α}}\mathrm{\δ}+v_p{e}_{\mathrm{\α}}{e}_{r}cos\left({\mathrm{\ψ}}_e\right)+e_u{e}_x---\left(17\right)$

其中：

步骤四中将步骤三中计算得出的纵向速度误差e_u和角速度误差e_r分别通入生物启发模型，可得到如下的方程：

${\stackrel{\·}{S}}_u=-A_1{S}_u+(B_1-S_u)f\left(e_u\right)-(D_1+S_u)g\left(e_u\right)---\left(18\right)$

其中：S_u为速度动态模型的输出；参数A₁是正常数，为动态纵向速度误差输出的衰减率；正常数B₁和D₁分别为动态纵向速度误差输出的上限和下限；函数f(e_u)＝max{e_u,0}，g(e_u)＝max{-e_u,0}。

${\stackrel{\·}{S}}_r=-A_2{S}_r+(B_2-S_r)f\left(e_r\right)-(D_2+S_r)g\left(e_r\right)---\left(19\right)$

其中：S_r为速度动态模型的输出；参数A₂是正常数，为动态角速度误差输出的衰减率；正常数B₂和D₂分别为动态角速度误差输出的上限和下限；函数f(e_r)＝max{e_r,0}，g(e_r)＝max{-e_r,0}。

最终得到了模型的输出S_u和S_r。

步骤五中推导的欠驱动UUV平面轨迹跟踪的动态速度调节控制器，具体的计算和推到过程如下：

对于速度误差e_u和e_r，构造李雅普诺夫能量函数：

$V_3\left(t\right)=V_2\left(t\right)+\frac{1}{2}{e}_u^2+\frac{1}{2}{e}_r^2---\left(20\right)$

结合式(17)，对式(20)求导可得：

$\begin{array}{c}{\stackrel{\·}{V}}_3\left(t\right)={\stackrel{\·}{V}}_2\left(t\right)+e_r{\stackrel{\·}{e}}_r+e_u{\stackrel{\·}{e}}_u\\ =-(k_1{e}_x^2+k_2{e}_y^2)/e-k_3{e}_{\mathrm{\α}}^2\\ +e_u({\stackrel{\·}{e}}_u+e_x)+v_p{e}_{\mathrm{\α}}\mathrm{\δ}+e_r\left({\stackrel{\·}{e}}_r+v_p{e}_{\mathrm{\α}}\cos \right({\mathrm{\ψ}}_e))\end{array}---\left(21\right)$

控制输入τ_u和τ_r设计为：

${\mathrm{\τ}}_u=(-m_{22}{v}_{r}r+X_u{u}_r+X_{u\left|u\right|}{u}_r|u_r|-{\widehat{\mathrm{\ω}}}_u)+m_{11}({\stackrel{\·}{u}}_d-e_x-S_u)---\left(22\right)$

${\mathrm{\τ}}_r=(-(m_{11}-m_{22})u_r{v}_r+N_{r}r+N_{r\left|r\right|}r|r|-{\widehat{\mathrm{\ω}}}_r)+m_{33}({\stackrel{\·}{r}}_d-v_{p}cos({\mathrm{\ψ}}_e)e_{\mathrm{\α}}-S_r)---\left(23\right)$

其中和为对当前环境扰动的估计值，

结合(21)至(23)式，可得到：

$\begin{array}{c}{\stackrel{\·}{V}}_3\left(t\right)=-(k_1{e}_x^2+k_2{e}_y^2)/e-k_3{e}_{\mathrm{\α}}^2-e_u{S}_u-e_r{S}_r\\ +e_u{\widetilde{\mathrm{\ω}}}_u+e_r{\widetilde{\mathrm{\ω}}}_r+v_p{e}_{\mathrm{\α}}\mathrm{\δ}\end{array}---\left(24\right)$

对于模型输出量S_u和S_r，构造李雅普诺夫能量函数：

$V_4\left(t\right)=V_3\left(t\right)+\frac{1}{2B_1}{S}_u^2+\frac{1}{2B_2}{S}_r^2---\left(25\right)$

结合(18)和(28)式，对(25)式求导可得：

$\begin{array}{c}{\stackrel{\·}{V}}_4\left(t\right)=-(k_1{e}_x^2+k_2{e}_y^2)/e-k_3{e}_{\mathrm{\α}}^2-A_u{S}_u^2-A_r{S}_r^2\\ +v_p{e}_{\mathrm{\α}}\mathrm{\δ}+e_u{\widetilde{\mathrm{\ω}}}_u+e_r{\widetilde{\mathrm{\ω}}}_r+B_u{S}_u+B_r{S}_r\end{array}---\left(26\right)$

其中：

$A_r=B_2^{-1}\[A_2+f\left(e_r\right)+g\left(e_r\right)\];$

$B_r=B_2^{-1}\[B_{2}f\left(e_r\right)-D_{2}g\left(e_r\right)-B_2{e}_r\];$

$A_u=B_1^{-1}\[A_1+f\left(e_u\right)+g\left(e_u\right)\];$

$B_u=B_1^{-1}\[B_{1}f\left(e_u\right)-D_{1}g\left(e_u\right)-B_1{e}_u\];$

当取D₁＝B₁且D₂＝B₂时由函数f(e_i)和g(e_i)的性质可知B_u＝0，B_r＝0，A_u≥0，A_r≥0。

式(26)可改写为：

${\stackrel{\·}{V}}_4\left(t\right)=-(k_1{e}_x^2+k_2{e}_y^2)/e-k_3{e}_{\mathrm{\α}}^2-A_u{S}_u^2-A_r{S}_r^2+v_p{e}_{\mathrm{\α}}\mathrm{\δ}+e_u{\widetilde{\mathrm{\ω}}}_u+e_r{\widetilde{\mathrm{\ω}}}_r---\left(27\right)$

对于外界干扰量，构造李雅普诺夫能量函数：

$V_5=V_4+\frac{1}{2}(k_4{{\widetilde{\mathrm{\ω}}}_u}^2+k_5{{\widetilde{\mathrm{\ω}}}_r}^2)---\left(28\right)$

其中：k₄、k₅为正常数。

设计外界扰动自适应控制律：

$\left\{\begin{array}{c}{\stackrel{\·}{\widetilde{\mathrm{\ω}}}}_u=-{\widetilde{\mathrm{\ω}}}_u-\frac{e_u}{k_4}\\ {\stackrel{\·}{\widetilde{\mathrm{\ω}}}}_r=-{\widetilde{\mathrm{\ω}}}_r-\frac{e_r}{k_5}\end{array}\right.---\left(29\right)$

结合(29)式，并对(30)式求导可得：

${\stackrel{\·}{V}}_5=-(k_1{e}_x^2+k_2{e}_y^2)/e-k_3{e}_{\mathrm{\α}}^2-A_u{S}_u^2-A_r{S}_r^2-k_4{{\widetilde{\mathrm{\ω}}}_u}^2-k_5{{\widetilde{\mathrm{\ω}}}_r}^2+v_p{e}_{\mathrm{\α}}\mathrm{\δ}\≤0---\left(30\right)$

参数k₁,k₂,k₃,k₄,k₅均是正常数，A_i≥0(i＝u,r)。由UUV的运动特性可知存在 其中均为已知的上界，可得到v_pe_αδ是有界的，所以当且仅当时由LaSalle不变定理可得，闭环跟踪误差系统渐近稳定，通过调节控制器增益系数k₁,k₂,k₃,k₄,k₅,A_u,A_r可保证系统的动态特性。

本发明采用基于动态速度调节的反步法设计中间的虚拟控制量，将姿态跟踪控制转化为速度控制，虽然仍需要逐步的计算出中间的虚拟控制导数的解析形式，但其避免了控制律在艏向角误差为±90°时的奇异值，完成了对速度的动态调节，保证了系统的控制性能。

仿真实验验证与分析：

为了验证所设计控制器的有效性，及动态速度调节的效果，对圆形曲线进行跟踪，为了证明所设计控制器能够有效的解决了当艏向角误差ψ_e＝π/2时的有效性，设计起始艏向ψ＝π/2，起始位置(x,y)＝(1,45)，起始误差为(x_e,y_e)＝(1,-5)，控制增益为k₁＝2,k₂＝k₃＝1,k₄＝k₅＝1，初始速度u_d＝0.1m/s，生物模型参数A₁＝12，B₁＝D₁＝8，A₂＝17.5，B₂＝D₂＝10。定常外界扰动ω_u＝0.2N，ω_v＝0.05N，ω_r＝0.2Nm，仿真是将从0-500s，实验的结果如图4至图10所示。

圆形轨迹为：

$\left\{\begin{array}{c}{x}_d=50sin\left(0.02t\right)\\ y_d=50\cos \left(0.02t\right)\end{array}\right.---\left(31\right)$

图4为欠驱动UUV圆形曲线跟踪的轨迹，看出本发明方法对圆形曲线的总体跟踪效果，在初始艏向角速度误差等于90°时，该控制系统仍然可以跟踪上期望的轨迹，避免了奇异值的出现，证明了本发明方法的有效性。

图5是位置跟踪误差曲线，从中可以看出位置跟踪误差最终足够小，且最终趋于零。

图6是欠驱动UUV圆形轨迹跟踪艏向角误差和总位置误差图，从放大的图像中可以看出首向角的误差值为π/2，此外位列更好的反应总体的跟踪效果，我们还定义了表示的是期望轨迹和实际轨迹的总体误差值。

图7为欠驱动UUV速度与角速度响应曲线，从中可以看出当速度稳定后各项指标都趋于常值。

图8展示出了欠驱动UUV圆形轨迹跟踪真实的控制输入响应。

图9、图10分别为角速度误差和纵向速度误差分别与其通入生物启发模型后的输出，可以看出其动态速度调节的效果，通过调节后其误差值明显的变小，从而达到了对速度进行动态调节的目的，提高了轨迹跟踪的精度。

Claims (5)

1.一种基于动态速度调节的欠驱动UUV平面轨迹跟踪控制方法，其特征是：它包括以下步骤：

步骤一、根据当前任务，将期望轨迹位置信息给定UUV，并通过所搭载的导航设备和传感器采集数据，获得当前位置P(t)＝[x(t),y(t)]^T信息、姿态ψ(t)信息、速度V(t)＝[u_r(t),v_r(t),r(t)]^T信息；u_r(t)为UUV的纵向速度，v_r(t)为UUV的横向运动速度，r(t)代表UUV的艏摇角速度；

步骤二、利用步骤一中获得的当前位置P(t)＝[x(t),y(t)]^T信息、姿态ψ(t)信息、速度V(t)＝[u_r(t),v_r(t),r(t)]^T信息，通过欠驱动UUV的数学模型和轨迹跟踪位置误差模型，计算出实际轨迹与期望轨迹之间的位置误差和期望姿态，将地面坐标信息转换为船体坐标信息；

步骤三、基于步骤二中计算出的位置误差，采用定义虚拟速度误差变量的方法，将姿态跟踪控制转化为速度控制，计算纵向速度虚拟控制律u_d和艏摇角速度虚拟控制律r_d；

步骤四、结合步骤二中获得的实际轨迹与期望轨迹之间的位置误差和期望姿态和步骤三获得的纵向速度虚拟控制律u_d和艏摇角速度虚拟控制律r_d，计算得出纵向速度误差e_u和角速度误差e_r，并将该两个误差分别通入生物启发模型，通过设置模型中的参数，完成对速度误差的动态调节；

步骤五、根据给定的数学模型和步骤四中的动态调节输出量，推导欠驱动UUV平面轨迹跟踪的动态速度调节控制器，包括：纵向控制力矩τ_u的控制信号、艏摇控制力矩τ_r的控制信号，计算界扰动自适应控制律，实现在外界常值扰动下对欠驱动UUV平面轨迹跟踪控制。

2.根据权利要求1所述的一种基于动态速度调节的欠驱动UUV平面轨迹跟踪控制方法，其特征在于步骤二中，欠驱动UUV的数学模型包括运动学模型和动力学模型、轨迹跟踪误差模型、位置误差、期望姿态的表达式分别为：

运动学模型：

$\left\{\begin{array}{c}\stackrel{\·}{x}=u_{r}cos\mathrm{\ψ}-v_{r}sin\mathrm{\ψ}\\ \stackrel{\·}{y}=u_{r}sin\mathrm{\ψ}+v_{r}cos\mathrm{\ψ}\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.---\left(1\right)$

动力学模型：

$\left\{\begin{array}{c}{\stackrel{\·}{u}}_r=\frac{m_{22}}{m_{11}}{v}_{r}r-\frac{X_u}{m_{11}}{u}_r-\frac{X_{u\left|u\right|}}{m_{11}}{u}_r\left|u_r\right|+\frac{{\mathrm{\τ}}_u+{\mathrm{\ω}}_u}{m_{11}}\\ {\stackrel{\·}{v}}_r=-\frac{m_{11}}{m_{22}}{u}_{r}r-\frac{Y_v}{m_{22}}{v}_r-\frac{Y_{v\left|v\right|}}{m_{22}}{v}_r\left|v_r\right|+\frac{{\mathrm{\ω}}_v}{m_{22}}\\ \stackrel{\·}{r}=\frac{(m_{11}-m_{22})}{m_{33}}{u}_r{v}_r-\frac{N_r}{m_{33}}r-\frac{N_{r\left|r\right|}}{m_{33}}r\left|r\right|+\frac{{\mathrm{\τ}}_r+{\mathrm{\ω}}_r}{m_{33}}\end{array}\right.---\left(2\right)$

其中，m为航行器的总质量；ω_u、ω_v、ω_r为外界扰动在UUV运动系各自由度的分量，X_u,X_u|u|,Y_v,Y_v|v|,N_r,N_r|r|为水动力参数；

位置误差：

$\left\{\begin{array}{c}{x}_e=x-x_d\\ y_e=y-y_d\end{array}\right.---\left(3\right)$

式中：(x,y)为实际轨迹位置信息；(x_d,y_d)为期望轨迹位置信息；

轨迹跟踪位置误差模型：

$\left\{\begin{array}{c}{\stackrel{\·}{e}}_x=u_r-v_p\cos \left({\mathrm{\ψ}}_e\right)+{\mathrm{re}}_y\\ {\stackrel{\·}{e}}_y=v_r+v_p\sin \left({\mathrm{\ψ}}_e\right)-{\mathrm{re}}_x\end{array}\right.---\left(4\right)$

其中，(e_x，e_y)为船体坐标系下的位置误差变量；

期望姿态：

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

3.根据权利要求1所述的一种基于动态速度调节的欠驱动UUV平面轨迹跟踪控制方法，其特征在于步骤三中，虚拟速度误差变量α、纵向速度虚拟控制律u_d、艏摇角速度虚拟控制律r_d的表达式分别为：

虚拟速度误差变量：

α＝v_psin(ψ_e) (6)

纵向速度虚拟控制律：

$u_d=v_{p}cos\left({\mathrm{\ψ}}_e\right)-\frac{k_1{e}_x}{e}---\left(7\right)$

其中，k₁、k₂为正常数，

艏向角速度虚拟控制律：

$r_d={\stackrel{\·}{\mathrm{\ψ}}}_d+\frac{(-k_3{e}_{\mathrm{\α}}-e_y+{\stackrel{\·}{\mathrm{\α}}}_d-{\stackrel{\·}{v}}_{p}sin({\mathrm{\ψ}}_e\left)\right)}{v_p}---\left(8\right)$

其中，k₃为正常数。

4.根据权利要求1所述的一种基于动态速度调节的欠驱动UUV平面轨迹跟踪控制方法，其特征在于步骤四中，纵向速度误差e_u、角速度误差e_r、生物启发模型的表达式分别为：

纵向速度误差：

e_u＝u-u_d (9)

角速度误差：

e_r＝r-r_d (10)

生物启发模型：

${\stackrel{\·}{S}}_u=-A_1{S}_u+(B_1-S_u)f\left(e_u\right)-(D_1+S_u)g\left(e_u\right)---\left(11\right)$

其中：S_u为速度动态模型的输出；参数A₁是正常数，为动态纵向速度误差输出的衰减率；正常数B₁和D₁分别为动态纵向速度误差输出的上限和下限；

函数f(e_u)＝max{e_u,0}，g(e_u)＝max{-e_u,0}；

${\stackrel{\·}{S}}_r=-A_2{S}_r+(B_2-S_r)f\left(e_r\right)-(D_2+S_r)g\left(e_r\right)---\left(12\right)$

其中：S_r为速度动态模型的输出；参数A₂是正常数，为动态角速度误差输出的衰减率；正常数B₂和D₂分别为动态角速度误差输出的上限和下限；函数f(e_r)＝max{e_r,0}，g(e_r)＝max{-e_r,0}。

5.根据权利要求1所述的一种基于动态速度调节的欠驱动UUV平面轨迹跟踪控制方法，其特征在于步骤五中，纵向控制力矩τ_u的控制信号、艏摇控制力矩τ_r的控制信号的表达式分别为：

纵向控制力矩：

${\mathrm{\τ}}_u=(-m_{22}{v}_{r}r+X_u{u}_r+X_{u\left|u\right|}{u}_r|u_r|-{\widehat{\mathrm{\ω}}}_u)+m_{11}({\stackrel{\·}{u}}_d-e_x-S_u)---\left(13\right)$

其中，为对当前外界扰动的估计值，

艏摇控制力矩：

${\mathrm{\τ}}_r=(-(m_{11}-m_{22})u_r{v}_r+N_{r}r+N_{r\left|r\right|}r|r|-{\widehat{\mathrm{\ω}}}_r)+m_{33}({\stackrel{\·}{r}}_d-v_{p}cos({\mathrm{\ψ}}_e)e_{\mathrm{\α}}-S_r)---\left(14\right)$

其中，为对当前外界扰动的估计值，

所述的界扰动自适应控制律为：

$\left\{\begin{array}{c}{\stackrel{\·}{\widetilde{\mathrm{\ω}}}}_u=-{\widetilde{\mathrm{\ω}}}_u-\frac{e_u}{k_4}\\ {\stackrel{\·}{\widetilde{\mathrm{\ω}}}}_r=-{\widetilde{\mathrm{\ω}}}_r-\frac{e_r}{k_5}\end{array}\right.---\left(15\right)$

其中：k₄、k₅为正常数。