CN106773713A - 针对欠驱动海洋航行器的高精度非线性路径跟踪控制方法 - Google Patents

Abstract

本发明提供了一种针对欠驱动海洋航行器的高精度非线性路径跟踪控制方法。本发明将航行器侧滑角的变化率看作一个不确定项，将动力学模型中参数不确定性、未建模动态及外部环境扰动当作集总不确定性，采用观测器对运动学不确定性和动力学不确定性进行实时观测；采用传统视线角制导方法计算期望视线角；设计基于观测器的非线性路径跟踪控制器，并且对运动学和动力学不确定性观测值进行补偿；并采用跟踪微分器对控制器进行简化，使得该控制器更加适合于工程应用。本发明消除了模型参数不确定性、未建模动态及外部环境扰动等对路径跟踪的影响，实现对航行器期望路径的精确跟踪控制。

Description

针对欠驱动海洋航行器的高精度非线性路径跟踪控制方法

技术领域

本发明涉及水下自主航行器(Autonomous Underwater Vehicle，AUV)、水面无人艇(Unmanned Surface Vehicle，USV)等欠驱动海洋水面或水下航行器的路径跟踪控制方法，尤其涉及一种多不确定性情况下的非线性路径跟踪控制方法。

背景技术

随着水面或水下自主航行器在海洋研究和开发领域的应用越来越广泛，其高精度路径跟踪控制已经成为当前的研究热点之一。路径跟踪是指控制航行器跟踪一条满足航向要求和性能约束，且与时间无关的期望轨迹。由于受到重量、可靠性、复杂性及效率等多方面的因素影响，目前大部分的海洋航行器属于欠驱动系统，加上其本身具有高度的非线性、耦合性以及加速度不可积的非完整约束等特性，给运动控制带来了较大的挑战。尤其在欠驱动航行器存在未建模动态、模型参数不确定性及风浪流等多不确定性干扰时，传统的控制方法往往不能保证路径跟踪控制的精度，有时甚至无法满足实际作业的需要。

发明内容

本发明的目的在于，在Serret-Frenet坐标系下构建欠驱动系统路径跟踪误差模型和基于多不确定性的动力学模型，设计一种针对欠驱动海洋航行器的高精度非线性路径跟踪控制方法，用于消除模型参数不确定性、未建模动态及外部环境扰动等对路径跟踪的影响，实现对期望路径的精确跟踪控制。

首先，本发明的非线性路径跟踪控制方法，将航行器侧滑角的变化率看作一个不确定项d_ψ，建立欠驱动海洋航行器水平面内包含多不确定性的路径跟踪误差模型和动力学模型，表示为如下三个子系统：

(1)位置子系统

(2)姿态子系统

(3)速度子系统

其中，s为期望路径上某点的横坐标；c_c为期望路径上该点处的曲率；(x_e,y_e,ψ_e)为航行器在Serret-Frenet坐标系下的位置和姿态误差；u、v和r分别为航行器质心在惯性坐标系下的纵向速度、横向速度及航向角速度；v_t为航行器的合成速度，m为航行器质量，I_z为航行器绕z轴的转动惯量；X_u,X_u|u|,Y_v,Y_v|v|,N_r,和N_r|r|为水动力参数；d_u,d_v和d_r为动力学模型集总不确定性，Δm,ΔI_z,ΔX_u,ΔX_u|u|,ΔY_v,ΔY_v|v|,ΔN_r,ΔN_r|r|,为动力学模型参数不确定项，Δτ_u,Δτ_v和Δτ_r为外部环境扰动不确定项，Δ_i(i＝u,v,r)为其它未建模动态；τ_u为外部控制输入力，τ_r为外部控制输入力矩。

本发明提供了一种高精度非线性路径跟踪控制方法，实现步骤如下：

步骤一：设计期望视线角ψ_los；

步骤二：设计观测器估计运动学和动力学不确定性；

对运动学不确定性项d_ψ和动力学不确定性项d_u,d_v和d_r，通过观测器进行实时在线观测，得到观测值d_ψ，d_u,d_v和d_r；

步骤三：设计基于观测器的控制器；

采用反步法(back-stepping)获取虚拟向导控制律虚拟航向角速度控制律r_d、输入力控制律τ_u及输入力矩控制律τ_r，并对步骤二观测的不确定性d_ψ、d_u、d_v和d_r进行实时补偿；

步骤四：采用跟踪微分器对控制器进行简化，解决传统back-stepping控制方法中的“计算膨胀”问题。

所述的步骤三中，根据位置子系统，获取虚拟向导控制律为：

其中，k₂＞0，为控制增益；

根据姿态子系统，获得虚拟航向角速度控制律r_d和输入力矩控制律τ_r为：

其中，k₁,k₄＞0为控制增益；

根据速度子系统，获得输入力控制律τ_u为：

其中，k₃＞0为控制增益，u_d为恒定期望速度。

所述的步骤四中得到简化后的控制器可以表示为：

其中，r_c和为虚拟航向角速度控制律r_d通过跟踪微分器后得到的跟踪信号和微分信号。

本发明的优点和积极效果在于：

(1)本发明的非线性路径跟踪控制方法，将运动学跟踪误差模型中的未知参数-航行器侧滑角的变化率当作运动学不确定性，将动力学模型中的内部参数不确定性、外部环境扰动及未建模动态等当作动力学集总不确定性，并且采用观测器对上述运动学和动力学不确定性进行实时在线观测，实现对期望路径的精确跟踪控制；

(2)本发明的非线性路径跟踪控制方法，采用back-stepping控制方法设计运动学和动力学控制器，并且对观测到的多不确定性实时补偿；通过跟踪微分器对控制器进行简化，使得控制器更适合移植到实时运算平台，使得本路径跟踪控制方法及实现的控制器更加适用于工程实践；

(3)本发明的非线性路径跟踪控制方法，不依赖于控制对象精确的数学模型，在多种不确定性的影响下可以保持名义模型同样的性能，可以扩展到海洋航行器在多不确定性下的三维空间路径跟踪控制中。

附图说明

图1是海洋航行器水平面内的路径跟踪示意图；

图2是本发明基于观测器的非线性路径跟踪控制方法示意图；

图3是本发明的非线性路径跟踪控制方法和传统back-stepping控制器作用下路径跟踪对比图；

图4是本发明的非线性路径跟踪控制方法和传统back-stepping控制器作用下路径跟踪控制误差对比图；

图5是本发明的非线性路径跟踪控制方法和传统back-stepping控制器作用下航行器速度对比图。

具体实施方式

下面将结合附图对本发明的技术方案进行详细说明。

本发明提供了一种基于观测器的高精度非线性路径跟踪控制方法，以实现在多不确定性情况下的欠驱动海洋航行器高精度非线性路径跟踪。

如图1所示，海洋航行器坐标系定义及水平面内的路径跟踪示意图。{I},{B}和{SF}分别为惯性坐标系、载体坐标系及Serret-Frenet曲线坐标系。其中{I}和{B}坐标系遵循标准规范，{SF}的坐标原点为期望路径上的任意一点，其横轴X_F沿路径切线方向，纵轴Y_F为法线方向。

下面在上述坐标系下对多不确定性下的航行器水平面内的路径跟踪控制系统进行建模和分析。

首先，海洋航行器在惯性坐标系下的运动学方程可以表示为：

其中，x、y和r分别为航行器质心Q在惯性坐标系下的横坐标和纵坐标，ψ为航行器航向角；u、v和r分别为航行器在惯性坐标系下的纵向速度、横向速度及航向角速度。字符上加点表示求导操作。

其次，海洋航行器在Serret-Frenet曲线坐标系路径跟踪误差模型可以表示为：

其中，s为期望路径上某点P的横坐标，c_c(s)为期望路径上点P处的曲率，简写为c_c；(x_e,y_e,ψ_e)为航行器在Serret-Frenet坐标系下的位置和姿态误差，x_e和y_e分别为航行器质心Q在Serret-Frenet坐标系下的横坐标和纵坐标，姿态误差ψ_e＝ψ_w-ψ_F，ψ_w＝ψ+β为航行器的航迹角，ψ_F为惯性坐标系横轴和Serret-Frenet坐标系横轴之间的夹角；β为航行器的侧滑角，为侧滑角的导数，也就是侧滑角变化率；v_t为航行器的合成速度，

可以看出，上述模型中包含未知参数根据公式及动力学模型，取决于航行器纵向速度u、横向速度v、航向角速度r及模型参数和不确定性d_u、d_v。一种简单的方法是通过测量加速度和获得但是此方法会受到传感器测量噪声的影响。因此本发明将看作一个未知扰动进行处理，即设置不确定性参数进而运动学模型的第三个方程转化为：

最后，欠驱动海洋航行器的包含多不确定性的动力学模型可以表示为：

其中，m为航行器质量；I_z为航行器绕z轴的转动惯量，是指在{I}坐标系下；τ_u为外部控制输入力，调整海洋航行器的纵向速度；τ_r为外部控制输入力矩，调整海洋航行器的航向角；X_u,X_u|u|，Y_v,Y_v|v|,N_r,和N_r|r|为水动力参数；d_u,d_v和d_r为动力学模型的总不确定性，Δm,ΔI_z,ΔX_u,ΔX_u|u|,ΔY_v,ΔY_v|v|,ΔN_r,ΔN_r|r|,为动力学模型参数不确定项，Δτ_u,Δτ_v和Δτ_r为风、浪、流等外部环境扰动不确定项，Δ_i(i＝u,v,r)为其它未建模动态。公式中|.|表示求取绝对值，例如|u|为速度的绝对值。

为便于控制器设计，将上述模型转化为以下三个子系统：

(1)位置子系统

(2)姿态子系统

(3)速度子系统

其中，姿态子系统中，将未知参数看作不确定性项d_ψ来进行处理，是对现有系统的改进，是本发明的创新点之一，在下面步骤二中设计观测器对d_ψ进行观测。现有控制方法是通过动力学模型求解得到因此依赖于系统精确的数学模型。

本发明提供的基于观测器的高精度非线性路径跟踪控制方法包括四个步骤：根据视线角制导率计算视线角ψ_los；设计观测器估计运动学和动力学不确定性；基于观测器的控制器设计，包含设计虚拟向导控制律虚拟航向角速度控制律r_d、输入力控制律τ_u及输入力矩控制律τ_r；采用跟踪微分器对整个控制器进行简化。

如图2所示，对本发明的自适应解耦控制方法的四个步骤进行具体说明。

步骤一：设计视线角制导率。

为了降低算法的复杂度，本发明采用传统的视线角制导率计算期望视线角，如下：

其中，ψ_los为期望视线角，Δ为视线距离。

步骤二：运动学和动力学不确定性观测器设计。

为了实时估计海洋航行器受到的内外部扰动，可以采用扩张状态观测器、二阶滑模观测器等多种观测器，其中降阶扩展状态观测器可以表示为：

其中，d_ψ为运动学不确定性观测值，d_r和d_u为动力学不确定性观测值,ξ_i(i＝1,2,3,4)为观测器辅助变量，ω_i(i＝1,2,3,4)为观测器增益。需要指出的是，动力学不确定项d_v隐含在运动学不确定性d_ψ中，即

步骤三：基于观测器的控制器设计。

(1)根据位置子系统模型(5)，获取的虚拟向导控制律可以表示为：

其中，k₂＞0为设计的控制增益。

(2)进行姿态子系统控制律设计。

首先，进行运动学控制器设计。采用航向角速度r作为虚拟指令，为使得ψ_e＝ψ_los，ψ_e的期望动态特性可以表示为：

其中，是期望视线角的导数，k₁为控制增益。

将式(11)代入姿态子系统模型(6)可得虚拟向导控制律r_d为：

结合观测器(9a)，虚拟向导控制律r_d进一步表示为：

其次，采用back-stepping方法将上述设计的运动学控制器反推至动力学。实际航向角速度r的期望动态特性可以表示为：

其中，k₄为控制增益，将式(14)代入动力学模型，结合观测器(9b)可得输入力矩控制律τ_r为：

(3)进行速度子系统控制律设计。

纵向速度u的期望动态可以表示为：

其中，k₃为控制增益，u_d为恒定期望速度，因此将式(16)代入动力学模型，结合观测器(9c)可得输入力控制律τ_u为：

步骤四：采用跟踪微分器对整个控制器进行简化。

可以看出，步骤三设计的动力学控制器τ_r中包括表达式结合虚拟指令表达式(13)，可知动力学控制器非常复杂，且包含不确定性观测值d_ψ，本发明采用线性或者非线性跟踪微分器对运动学虚拟控制指令r_d进行数值求导，更适合于实际工程应用。

中科院系统所韩京清研究员给出的非线性跟踪微分器可以表示为：

其中，h为采样周期，q为加速因子，k为离散时间变量，fhan(·)函数的详细表达式可以参照相关文献(韩京清著,《自抗扰控制技术》[M].国防工业出版社,2008.p66-73)。

本发明简化后的控制器表示为：

其中，r_c和为虚拟参考指令r_d通过跟踪微分器后得到的跟踪信号和微分信号。

下面证明本发明方法的闭环系统稳定性：

定义闭环跟踪误差：E₂＝u＝u-u_d和E₃＝[x_e,y_e]^T。

首先，证明误差E₁在本发明涉及控制器作用下的有界性，考虑如下的Lyapunov函数：

沿方程(6)的轨迹，求公式(20)对时间的导数，可得:

将方程(9a),(9b),(13)和(15)代入上式(21)，可得：

其中，ε₁和ε₂为跟踪微分器的跟踪误差，d_ψ、d_r为观测器的观测误差。

进而采用杨氏不等式，可得：

其中，选择可以保证矩阵K为正定矩阵。因此，闭环系统误差E₁可以表示为：

其中，λ_min(K)为正定矩阵K的最小特征值，λ_max(G)为矩阵G的最大特征值。

其次，证明闭环误差E₂在本发明控制方法作用下的有界性，考虑如下的Lyapunov函数：

沿方程(7)的轨迹，求公式(25)对时间的导数，可得:

设观测器观测误差收敛，即d_u有界，因此当k₃足够大时，能够保证且使得闭环误差满足||E₂||≤d_u/k₃。

最后，证明误差E₃在本发明控制方法作用下的有界性，考虑如下的Lyapunov函数：

沿方程(5)的轨迹，求公式(27)对时间的导数，可得:

设y_e有界，且满足|y_e|≤y₀，y₀为一个正常数，闭环误差E₃可以表示为：

其中，λ_min(M)为矩阵M的最小特征值，λ_max(N)为矩阵N的最大特征值。

因此可以看出，闭环系统误差取决于设计的控制参数、跟踪微分器的跟踪误差和观测器的观测误差。以上，闭环系统的稳定性证明完毕。

为了验证上述提出的控制器的有效性，在MATLAB/Simulink环境下建立海洋航行器水平面内的路径跟踪控制系统的仿真模型，并对航行器在受多不确定性情况下的路径跟踪控制性能进行验证。航行器模型参数如表1所示：

表1航行器水动力参数

初始值设置为：x(0)＝30m,y(0)＝-20m,u(0)＝0.1m/s,v(0)＝0m/s,r(0)＝0rad/s,ψ(0)＝π/2rad,及s(0)＝0m；期望速度u_d＝1m/s；控制器参数k₁＝1,k₂＝0.1,k₃＝1,k₄＝10,ω₁＝5,ω₂＝20,ω₃＝20,ω₄＝20,h＝0.001s^-1,r＝1000m/s²。

假设期望路径的参数化曲线方程可以表示为式(30)：

其中，参数μ可以通过下式得到：期望路径参数如表2所示。

表2期望路径参数

路径参数	i＝0	i＝1	i＝2	i＝3	i＝4
						ai	0	0.87	-0.02	10-5	1.5×10-6
bi	0	0.5	-5×10-4	10-5	10-7

假设所有的水动力参数在名义值的基础上增加30％，航行器所受的外部环境扰动表示为：

本发明的基于观测器的非线性路径跟踪控制方法和传统back-stepping控制方法作用下路径跟踪曲线图如图3所示。从图3可以看出，本发明基于观测器的非线性路径跟踪控制方法和传统back-stepping控制方法都可以使得航行器渐进收敛于期望路径。但本发明方法精度更高，且没有受到多不确定性的影响。两种控制方法作用下，路径跟踪控制误差曲线如图4所示。由图4可知，本发明方法使得路径跟踪误差渐进收敛于零附近，而传统的back-stepping控制方法则明显受到多不确定性的影响而性能变差。

图5为本发明的非线性路径跟踪控制方法和传统back-stepping控制器作用下航行器速度对比曲线。可以看出，在本发明控制方法作用下，航行器纵向速度u(t)快速平稳收敛于期望速度u_d＝1m/s，而在传统的back-stepping控制方法作用下，u(t)随着航行器的不确定性在期望速度附近波动。另外，两种控制方法作用下，航行器横向速度v及航向角速度r均有界，但本发明方法更平滑。

通过和传统back-stepping控制器作用下的对比分析可知，在本发明的基于观测器的非线性路径跟踪控制方法作用下，航行器的路径跟踪控制精度明显提高，抗干扰能力显著增强。

Claims (3)

1.一种高精度非线性路径跟踪控制方法，用于在多不确定性影响下的水面或水下自主式欠驱动海洋航行器，其特征在于，所述方法将航行器的侧滑角变化率看作一个不确定项d_ψ，建立欠驱动海洋航行器水平面内包含多不确定性的路径跟踪误差模型和动力学模型，表示为如下三个子系统：

(1)位置子系统

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_e=-\stackrel{\·}{s}(1-c_c{y}_e)+v_t{\mathrm{cos\ψ}}_e\\ {\stackrel{\·}{y}}_e=-c_c\stackrel{\·}{s}{x}_e+v_t{\mathrm{sin\ψ}}_e\end{array}\right.---\left(1\right)$

(2)姿态子系统

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ψ}}}_e=r+d_{\mathrm{\ψ}}-c_c\stackrel{\·}{s}\\ \stackrel{\·}{r}=\frac{Y_{\stackrel{\·}{v}}-X_{\stackrel{\·}{u}}}{I_z-N_{\stackrel{\·}{r}}}uv-\frac{N_r}{I_z-N_{\stackrel{\·}{r}}}r-\frac{N_{r\left|r\right|}}{I_z-N_{\stackrel{\·}{r}}}r\left|r\right|+\frac{{\mathrm{\τ}}_r}{I_z-N_{\stackrel{\·}{r}}}+d_r({\mathrm{\ΔI}}_z,{\mathrm{\ΔX}}_{\stackrel{\·}{u}},{\mathrm{\ΔY}}_{\stackrel{\·}{v}},{\mathrm{\ΔN}}_{\stackrel{\·}{r}},{\mathrm{\ΔN}}_r,{\mathrm{\ΔN}}_{r\left|r\right|},{\mathrm{\Δ\τ}}_r,{\mathrm{\Δ}}_r)\end{array}\right.---\left(2\right)$

(3)速度子系统

$\left\{\begin{array}{c}\stackrel{\·}{u}=\frac{m-Y_{\stackrel{\·}{v}}}{m-X_{\stackrel{\·}{u}}}vr-\frac{X_u}{m-X_{\stackrel{\·}{u}}}u-\frac{X_{u\left|u\right|}}{m-X_{\stackrel{\·}{u}}}u\left|u\right|+\frac{{\mathrm{\τ}}_u}{m-X_{\stackrel{\·}{u}}}+d_u(\mathrm{\Δ}m,{\mathrm{\ΔX}}_{\stackrel{\·}{u}},{\mathrm{\ΔY}}_{\stackrel{\·}{v}},{\mathrm{\ΔX}}_u,{\mathrm{\ΔX}}_{u\left|u\right|},{\mathrm{\Δ\τ}}_u,{\mathrm{\Δ}}_u)\\ \stackrel{\·}{v}=-\frac{m-X_{\stackrel{\·}{u}}}{m-Y_{\stackrel{\·}{v}}}ur-\frac{Y_v}{m-Y_{\stackrel{\·}{v}}}v-\frac{Y_{v\left|v\right|}}{m-Y_{\stackrel{\·}{v}}}v\left|v\right|+d_v(\mathrm{\Δ}m,{\mathrm{\ΔX}}_{\stackrel{\·}{u}},{\mathrm{\ΔY}}_{\stackrel{\·}{v}},{\mathrm{\ΔY}}_v,{\mathrm{\ΔY}}_{v\left|v\right|},{\mathrm{\Δ\τ}}_v,{\mathrm{\Δ}}_v)\end{array}\right.---\left(3\right)$

其中，s为期望路径上某点的横坐标；c_c为期望路径上该点处的曲率；(x_e,y_e,ψ_e)为航行器在Serret-Frenet坐标系下的位置和姿态误差；u、v和r分别为航行器质心在惯性坐标系下的纵向速度、横向速度及航向角速度；v_t为航行器的合成速度，m为航行器质量，I_z为航行器绕z轴的转动惯量；X_u,X_u|u|,Y_v,Y_v|v|，N_r，和N_r|r|为水动力参数；d_u，d_v和d_r为动力学模型总不确定性项，Δm,ΔI_z,ΔX_u,ΔX_u|u|,ΔY_v,ΔY_v|v|,ΔN_r,ΔN_r|r|,为动力学模型参数不确定项，Δτ_u,Δτ_v和Δτ_r为外部环境扰动不确定项，Δ_i(i＝u,v,r)为其它未建模动态；τ_u为外部控制输入力，τ_r为外部控制输入力矩；

所述方法实现步骤包括：

步骤一：设计期望视线角ψ_los；

步骤二：设计观测器估计运动学和动力学不确定性；

对运动学不确定性项d_ψ和动力学不确定性项d_u、d_v和d_r，通过观测器进行实时在线观测，得到观测值d_ψ,d_u,d_v和d_r；

步骤三：设计基于观测器的控制器；

采用反步法获取虚拟向导控制律虚拟航向角速度控制律r_d、输入力控制律τ_u及输入力矩控制律τ_r，并对步骤二观测的不确定性d_ψ、d_u、d_v和d_r进行实时补偿；

步骤四：采用跟踪微分器对控制器进行简化。

2.根据权利要求1所述的非线性路径跟踪控制方法，其特征在于，所述的步骤三中，根据位置子系统，获取虚拟向导控制律为：

$\stackrel{\·}{s}=k_2{x}_e+v_t{\mathrm{cos\ψ}}_e---\left(4\right)$

其中，k₂＞0，为控制增益；

根据姿态子系统，获得虚拟航向角速度控制律r_d和输入力矩控制律τ_r为：

$\left\{\begin{array}{c}{r}_d=c_c\stackrel{\·}{s}+{\stackrel{\·}{\mathrm{\ψ}}}_{los}-d_{\mathrm{\ψ}}-k_1({\mathrm{\ψ}}_e-{\mathrm{\ψ}}_{los})\\ {\mathrm{\τ}}_r=(I_z-N_{\stackrel{\·}{r}})\[{\stackrel{\·}{r}}_d-\frac{Y_{\stackrel{\·}{v}}-X_{\stackrel{\·}{u}}}{I_z-N_{\stackrel{\·}{r}}}uv+\frac{N_r}{I_z-N_{\stackrel{\·}{r}}}r+\frac{N_{r\left|r\right|}}{I_z-N_{\stackrel{\·}{r}}}r\left|r\right|-d_r-k_4(r-r_d)\]\end{array}\right.---\left(5\right)$

其中，k₁,k₄＞0为控制增益；

根据速度子系统，获得输入力控制律τ_u为：

${\mathrm{\τ}}_u=(m-X_{\stackrel{\·}{u}})\[-\frac{m-Y_{\stackrel{\·}{v}}}{m-X_{\stackrel{\·}{u}}}vr+\frac{X_u}{m-X_{\stackrel{\·}{u}}}u+\frac{X_{u\left|u\right|}}{m-X_{\stackrel{\·}{u}}}u\left|u\right|-d_u-k_3(u-u_d)\]---\left(6\right)$

其中，k₃＞0为控制增益，u_d为恒定期望速度。

3.根据权利要求1或2所述的非线性路径跟踪控制方法，其特征在于，所述的步骤四中，简化后的控制器表示为：

$\left\{\begin{array}{c}{r}_d=c_c\stackrel{\·}{s}+{\stackrel{\·}{\mathrm{\ψ}}}_{los}-d_{\mathrm{\ψ}}-k_1({\mathrm{\ψ}}_e-{\mathrm{\ψ}}_{los})\\ \stackrel{\·}{s}=k_2{x}_e+v_t{\mathrm{cos\ψ}}_e\\ {\mathrm{\τ}}_r=(I_z-N_{\stackrel{\·}{r}})\[\stackrel{\·}{r}-\frac{Y_{\stackrel{\·}{v}}-X_{\stackrel{\·}{u}}}{I_z-N_{\stackrel{\·}{r}}}uv+\frac{N_r}{I_z-N_{\stackrel{\·}{r}}}r+\frac{N_{r\left|r\right|}}{I_z-N_{\stackrel{\·}{r}}}r\left|r\right|-d_r-k_4(r-r_c)\]\\ {\mathrm{\τ}}_u=(m-X_{\stackrel{\·}{u}})\[-\frac{m-Y_{\stackrel{\·}{v}}}{m-X_{\stackrel{\·}{u}}}vr+\frac{X_u}{m-X_{\stackrel{\·}{u}}}u+\frac{X_{u\left|u\right|}}{m-X_{\stackrel{\·}{u}}}u\left|u\right|-d_u-k_3(u-u_d)\]\end{array}\right.---\left(7\right)$

其中，r_c和为虚拟航向角速度控制律r_d通过跟踪微分器后得到的跟踪信号和微分信号。