基于迭代的自主水下航行器三维曲线路径跟踪控制方法

摘要

本发明提供的是一种基于迭代的自主水下航行器三维曲线路径跟踪控制方法。步骤1.初始化；步骤2.计算初始时刻AUV当前位置与期望路径上“虚拟向导”点在AUV载体坐标系下的相对跟踪误差；步骤3.计算期望路径上“虚拟向导”点的期望移动速度、AUV运动学跟踪控制律；步骤4.在运动学等价控制律的基础上，采用迭代，推导欠驱动自主水下航行器AUV的三维路径跟踪的动力学控制律；步骤5.计算当前AUV位置ηn＝(x,y,z)与标定的转向点WPk＝(xk,yk,zk)之间的距离若小于设定的航迹切换半径R，则表示完成当前指定路径的跟踪任务停止航行或切换下一个期望航迹，否则继续步骤2。本发明能够提高AUV的路径跟踪精度。

说明书

技术领域

本发明涉及欠驱动自主水下航行器的三维空间运动控制技术领域。

背景技术

海底地形的勘探和测绘对深海资源的开发有着重要的意义，欠驱动自主水下航行器 AUV（Autonomous Underwater Vehicle）由于具有良好的机动性和续航能力，在海洋勘探开 发中扮演着重要的角色，随着AUV在海洋工程领域应用的不断深入，使得对AUV在水下 三维空间的运动控制技术的研究提出了新的挑战，考虑到受到航行经济性或负载能力制约， 通常执行机构配置为纵向尾部推进器、水平方向舵和垂直升降舵，AUV大多未配备横向和 垂向辅助推进器，使得控制输入的维数远小于运动自由度数，为欠驱动系统，无法设计光 滑时不变控制律实现反馈控制，同时由于受到复杂多变的海洋环境作用，使得AUV动力学 模型具有较高的非线性、不确定性和模型自身存在的耦合性，这也成为欠驱动AUV三维空 间跟踪控制器设计的难点。

目前，国内外针对欠驱动AUV的三维空间运动控制的研究较少，研究大多针对解耦的 水平面运动子系统和垂直面深度控制子系统分别设计控制器，进而实现对欠驱动自主水下 航行器在水下三维空间的运动控制，由于忽略了模型的耦合作用，设计的控制器无法实现 欠驱动AUV对空间任意平滑曲线的跟踪控制。这里讨论的跟踪控制问题具体为水下三维空 间的路径跟踪控制问题，水下三维空间路径的描述通过参数化方程进行描述，不同于三维 轨迹跟踪控制问题中的轨迹方程以时间作为参数，克服了传统轨迹跟踪控制问题中由于引 入具有同构动力学模型的“虚拟AUV”受到环境干扰作用引起闭环跟踪系统具有不稳定动 态，本发明中通过设计期望路径上“虚拟向导”的移动速度作为跟踪系统额外的控制输入， 由于“虚拟向导”只具有运动学特性而无具体动力学模型，因此状态不受外界扰动的影响， 能够保证跟踪系统的稳定性和动态性能。

P.Encarnacao等在论文《3D Path Following for Autonomous Underwater Vehicle》 （Proceedings of the 39th IEEE Conference on Decision and Control,IEEE Press,2000, Sydney.）利用正交投影的思想建立AUV在期望路径坐标系（Serret-Frenet）下的三维路径 跟踪误差模型，由于存在奇异值点，对AUV的初始位置有约束，无法实现AUV跟踪的全 局收敛，而本专利建立的AUV载体坐标系下的三维跟踪误差模型不存在奇异值问题，因此 能够保证AUV跟踪误差的全局收敛性；《基于自适应Backstepping的欠驱动AUV三维航 迹跟踪控制》（控制与决策，2012，第38卷第2期）依据视线法（line-of-sight,LOS）计算 期望跟踪视线角,基于自适应反步法设计跟踪控制器，针对离散的航迹点的跟踪控制，并未 给出三维航迹跟踪的误差方程，无法实现对三维空间光滑曲线的跟踪；且跟踪导引策略为 视线法（Line of Sight,LOS）而本专利采用的为虚拟向导策略（Virtual Guidance），通过跟 踪期望路径上的“虚拟向导”点实现AUV收敛于期望路径；文献《基于离散滑模预测的欠 驱动AUV三维航迹跟踪控制》（控制与决策,2011,第26卷第10期）中给出了在期望路径 上虚拟向导坐标系下的AUV三维路径跟踪误差方程的形式，将AUV假定为虚拟质点，假 设运动方向与合速度矢量方向一致，获得的三维路径跟踪误差方程需要AUV运动的侧漂角 和攻角精确可量测，这在实际应用中是存在困难，只能够通过对横向和垂向运动速度的测 量，进而计算出侧漂角和攻角，而由于沿三轴线速度的测量较困难，导致最终的控制器解 算存在潜在的中断可能。《基于非线性迭代滑模的欠驱动UUV三维航迹跟踪控制》（自动化 学报，2012，第38卷第2期）基于工程控制器解耦的思想设计非线性迭代滑模航迹跟踪控 制器，由于被控对象模型为六自由度耦合运动模型，因此针对纵向速度、艏向控制和纵倾 控制分别设计的解耦控制器只能通过鲁棒项抑制模型中的耦合作用，当模型各自由度之间 的耦合作用较明显时，控制器只能通过输出较高的控制器增益为代价消除耦合作用，引起 控制器输出饱和信号，解耦的控制器只能够保证三个独立控制子系统的渐近稳定性，而无 法保证整个控制系统的渐近稳定性，而本专利提出的三维航迹跟踪控制器能够保证整个系 统全局渐近稳定性；《自主式水下航行器三维路径跟踪的神经网络H_∞鲁棒自适应控制方法》 （控制理论与应用，2012，第29卷第3期）基于正交投影Serret-Frenet坐标系建立AUV 三维路径跟踪误差方程，运用H_∞鲁棒控制思想设计控制器，同时引入神经网络补偿模型不 确定性，但由于基于正交投影Serret-Frenet坐标系建立AUV三维路径跟踪误差模型存在奇 异值点，使得对AUV的初始条件有约束，即AUV初始位置必须位于跟踪曲线最小曲率半 径内，因此无法实现AUV跟踪的全局收敛性，而本专利建立在AUV载体坐标系下表示的 三维跟踪误差模型不存在奇异值问题，因此能够保证AUV跟踪误差的全局收敛性，此外本 专利采用迭代方法设计控制器不同于H_∞鲁棒控制设计思想。

以上文献中涉及的方法均针对建立在Serret-Frenet坐标系下误差方程进行控制器设计， 由于误差模型中的状态变量无法直接量测，导致控制系统对初始值精确性较为依赖，而本 专利针对AUV载体坐标系建立三维路径跟踪误差方程，基于迭代方法设计三维路径跟踪控 制器避免了采用传统反步法设计控制器时存在奇异值点，保证系统的全局收敛性。

发明内容

本发明的目的在于提供一种能提高路径跟踪精度的基于迭代的自主水下航行器三维路 径跟踪控制方法。

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

步骤1.初始化，给定三维空间期望跟踪路径参数化方程描述，给定AUV初始位置和 姿态信息，给定期望跟踪路径参数的初始值，给定期望路径上“虚拟向导”初始位置和初 始移动速度信息；

步骤2.计算初始时刻AUV当前位置与期望路径上“虚拟向导”点在AUV载体坐标 系下的相对跟踪误差；

步骤3.计算期望路径上“虚拟向导”点的期望移动速度、AUV运动学跟踪控制律（如 纵向移动速度、AUV的转艏角速度和纵倾角速度虚拟控制律）；

步骤4.在运动学等价控制律的基础上，采用迭代设计思想，推导欠驱动自主水下航行 器AUV的三维路径跟踪的动力学控制律，即根据AUV具体水动力数学模型解算出最终的 执行指令信号（如推进器推力、纵倾控制力矩和转艏控制力矩）；

步骤5.计算当前AUV位置ηⁿ＝(x,y,z)与标定的转向点WP_k＝(x_k,y_k,z_k)之间的距离 若小于设定的航迹切换半径R，则表示完成当前指定 路径的跟踪任务停止航行或切换下一个期望航迹，否则继续步骤2。

本发明相对现有技术具有如下的优点及效果：

1.基于AUV载体坐标系下建立三维路径跟踪误差方程，结合AUV的运动特性，避 免了在期望路径上虚拟向导坐标系下的AUV三维路径跟踪误差方程时，将AUV假定为虚 拟质点，假设运动方向与合速度矢量方向一致，获得的三维路径跟踪误差方程需要AUV运 动的侧漂角和攻角精确可量测，这在实际应用中是存在困难，只能够通过对横向和垂向运 动速度的测量，进而计算AUV的侧漂角和攻角，而由于沿三轴线速度的测量较困难，导致 最终的控制器解算存在潜在的中断可能的不足。

2.引入期望路径上“虚拟向导”点的移动速度作为额外的控制输入，保证实际应用中在 具有较大跟踪误差时，跟踪系统具有良好的动态性能，避免控制器输出较高的增益信号和 推力饱和现象；采用迭代设计思想，将AUV三维路径跟踪控制系统，分为运动学和动力学 两部分设计等价控制器，基于李雅普诺夫稳定性理论保证三维路径跟踪误差闭环系统的稳 定性，且控制器对海洋环境作用引起的模型参数不确定性具有一定鲁棒性。

附图说明

图1是本发明基于虚拟向导的AUV三维路径跟踪示意图。

图2是本发明AUV三维路径跟踪控制器解算流程图。

图3是本发明AUV三维路径跟踪非线性控制器框图。

图4～10为本发明设计AUV三维曲线路径跟踪控制仿真对比曲线。从图4中可以看出 本发明设计控制方法在AUV与期望跟踪三维路径初始距离较大时仍能实现精确跟踪控制， 图5和图6分别为AUV三维跟踪轨迹在水平面和垂直面得投影曲线，可以看出跟踪三个方 向上的跟踪误差逐渐减小，在图7中的AUV三维跟踪误差曲线最终收敛到零，验证了本发 明设计控制方法的有效性；图8～9为AUV状态变量的变化曲线；图10为AUV控制输入 响应曲线。

具体实施方式

下面举例对本发明做更详细的描述：

对于步骤1中给定期望跟踪路径Ω上的虚拟向导P在固定坐标系的坐标可以表示为 某一标量参数s∈R的函数为

${\eta }_d^n\left(s\right)={[x_d\left(s\right),y_d\left(s\right),z_d\left(s\right)]}^T---\left(1\right)$

为了保证被跟踪路径的光滑性，要求x_d(s)，y_d(s)，z_d(s)二阶偏导数存在。

定义虚拟向导点P的速度u_p方向为沿曲线路径的切线方向与固定坐标系水平轴的 夹角ψ_d为

ψ_d＝arctan(y′_d/x′_d)    (2)

速度向量u_p与固定坐标系垂直轴的夹角θ_d定义为

${\theta }_d={\tan }^{-1}\left(\frac{-z_d^{\prime }}{\sqrt{{\left(x_d^{\prime }\right)}^2+{\left(y_d^{\prime }\right)}^2}}\right)---\left(3\right)$

其中因此虚拟向导点P沿曲线路径的旋转角速度分别 可表示为

$q_d={\stackrel{·}{\theta }}_d---\left(4\right)$

$r_d={\stackrel{·}{\psi }}_d---\left(5\right)$

然后给定AUV在固定坐标系下的初始位置为ηⁿ＝[x,y,z]^T，AUV的初始艏向角和纵 倾角分别为ψ和θ，AUV纵向速度u、横向速度v和垂向速度w，艏摇角速度r和纵倾角速 度q。

至此完成了步骤1中的初始化设置。

步骤2中计算三维路径跟踪误差的具体过程如下：

图1为欠驱动AUV三维路径跟踪示意图，其中期望跟踪路径Ω上的虚拟向导点P在 固定坐标系{I}的位置向量定义为AUV在固定坐标系{I}下的位置向量定 义为ηⁿ＝[x,y,z]^T，ε＝[x_e,y_e,z_e]^T为相对于AUV载体坐标系{B}下跟踪误差向量，所以得 到跟踪误差可以表示为

$ϵ=R_b^{\mathrm{nT}}{\eta }_e^n---\left(6\right)$

其中为AUV载体坐标系{B}到固定坐标系{I}的旋转矩阵，表示 矩阵的转秩。

$R_b^n=R_{y,\theta }{R}_{z,\psi }$

$=\left(\begin{array}{ccc}\cos \theta & 0& -\sin \theta \\ 0& 1& 0\\ \sin \theta & 0& \cos \theta \end{array}\right)\left(\begin{array}{ccc}\cos \psi & \sin \zeta & 0\\ -\sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right)---\left(7\right)$

对式(6)求导得

$\stackrel{·}{ϵ}={\stackrel{·}{R}}_b^{\mathrm{nT}}{\eta }_e^n+R_b^{\mathrm{nT}}{\stackrel{·}{\eta }}_e^n---\left(8\right)$

由于${\stackrel{·}{R}}_b^n=R_b^{n}S\left({\omega }_{\mathrm{nb}}^b\right),$其中

$S\left({\omega }_{\mathrm{nb}}^b\right)=\left(\begin{array}{ccc}0& -r& q\\ r& 0& 0\\ -q& 0& 0\end{array}\right)---\left(9\right)$

将上式代入式(8)得

$\stackrel{·}{ϵ}=S^T\left({\omega }_{\mathrm{nb}}^b\right)R_b^{\mathrm{NT}}{\eta }_e^n+R_b^{\mathrm{nT}}{\stackrel{·}{\eta }}_e^n---\left(10\right)$

考虑其中ν_b＝[u,v,w]^T为载体坐标系下的速度向量；ν_F＝[u_p,0,0]^T为期望路径坐标系{F}下参考点的速度向量，代入式(10)变为

$\stackrel{·}{ϵ}=S^T\left({\omega }_{\mathrm{nb}}^b\right)ϵ+R_b^{\mathrm{nT}}({\stackrel{·}{\eta }}^n-{\stackrel{·}{\eta }}_d^n)$

$=S^T\left({\omega }_{\mathrm{nb}}^b\right)ϵ+R_b^{\mathrm{nT}}{R}_b^n{v}_b-R_b^{\mathrm{nT}}{\stackrel{·}{\eta }}_d^n---\left(11\right)$

$=S^T\left({\omega }_{\mathrm{nb}}^b\right)ϵ+v_b-R_b^{\mathrm{nT}}{R}_F^n{v}_F$

展开得

$\left(\begin{array}{c}{\stackrel{·}{x}}_e\\ {\stackrel{·}{y}}_e\\ {\stackrel{·}{z}}_e\end{array}\right)=\left(\begin{array}{c}{\mathrm{ry}}_e-{\mathrm{qz}}_e\\ -r{x}_e\\ q{x}_e\end{array}\right)+\left(\begin{array}{c}u\\ v\\ w\end{array}\right)-R_F^{\mathrm{bT}}\left(\begin{array}{c}{u}_p\\ 0\\ 0\end{array}\right)---\left(12\right)$

其中

$R_F^b=\left(\begin{array}{ccc}\cos {\theta }_e\cos {\psi }_e& \cos {\theta }_e\sin {\psi }_e& -{\sin \theta }_e\\ -\sin {\psi }_e& \cos {\psi }_e& 0\\ \sin {\theta }_e\cos {\psi }_e& \sin {\theta }_e\sin {\psi }_e& \cos {\theta }_e\end{array}\right)---\left(13\right)$

整理得

$\left(\begin{array}{c}{\stackrel{·}{x}}_e={\mathrm{ry}}_e-{\mathrm{qz}}_e+u-u_p\cos {\psi }_e\cos {\theta }_e\\ {\stackrel{·}{y}}_e=-r{x}_e+u_p\sin {\psi }_e\cos {\theta }_e+v\\ {\stackrel{·}{z}}_e=q{z}_e-u_p\sin {\theta }_e+w\end{array}\right)---\left(14\right)$

其中ψ_e＝ψ-ψ_d，θ_e＝θ-θ_d

至此完成了计算AUV与期望路径上虚拟向导P之间的跟踪误差，下面设计过程给出如 何根据计算出的跟踪误差ε，计算控制信号

对于步骤2中给出的AUV三维路径跟踪误差变量，根据下式分别计算运动学虚拟控 制器

（1）期望路径上虚拟向导点P的期望移动速度计算公式：

$u_p(t,ϵ)=u_d(1-{\mathrm{\lambda e}}^{-c(t-t_0)})e^{-\gamma d_e}---\left(15\right)$

其中参数满足u_d＞0，调节因子0＜λ＜1，c＞0，γ＞0，为AUV载 体坐标系下的跟踪误差距离。

（2）AUV纵向速度运动学等价控制律为：

a_u＝u_pcosψ_ecosθ_e-k₁x_e    (16)

（3）AUV纵倾角速度运动学等价控制律为：

α_q＝q_d+(k₄z_eu_p-k₅sinθ_e)    (17)

（4）AUV艏摇角速度运动学等价控制律为：

α_r＝cosθ[r_d-(k₂cosθ_ey_eu_p +k₃sinψ_e)]    (18)

其中k₁＞0，k₂＞0，k₃＞0，k₄＞0，k₅＞0为控制器设计参数。

步骤3中设计AUV三维路径跟踪运动学虚拟控制器的具体过程如下：

由于运动学等价控制器与真实控制输入量存在偏差，因此定义偏差变量为

$\left(\begin{array}{c}{u}_e=u-{\alpha }_u\\ r_e=r-{\alpha }_r\\ q_e=q-{\alpha }_q\end{array}\right)---\left(19\right)$

对于给定航迹跟踪误差方程(14)，构造李雅普诺夫能量函数

$V_1=\frac{1}{2}(x_e^2+y_e^2+z_e^2)+\frac{1-\cos {\psi }_e}{k_2}+\frac{1-{\cos \theta }_e}{k_4}---\left(20\right)$

对式(20)求导，将式(16)～(18)和式(19)代入得

${\stackrel{·}{V}}_1=-k_1{x}_e^2-k_3{k}_2^{-1}{\sin }^2{\psi }_e-k_5{k}_4^{-1}{\sin }^2{\theta }_e$

$+y_{e}v+z_{e}w+x_e(r_e{y}_e+u_e-q_e{z}_e)-y_e{r}_e{x}_e$

$+z_e{q}_e{x}_e+\frac{r_e\sin {\psi }_e}{k_2\cos \theta }+\frac{q_e\sin {\theta }_e}{k_4}---\left(21\right)$

$=-k_1{x}_e^2-k_3{k}_2^{-1}{\sin }^2{\psi }_e-k_5{k}_4^{-1}{\sin }^2{\theta }_e$

$+y_{e}v+z_{e}w+x_e{u}_e+\frac{r_e\sin {\psi }_e}{k_2\cos \theta }+\frac{q_e\sin {\theta }_e}{k_4}$

步骤4中通过AUV数学模型解算控制输入指令的具体过程为：

根据AUV实测水动力系数，忽略横摇运动对模型的影响，得到AUV五自由度数学模 型如下

$\stackrel{·}{u}=\frac{m_2}{m_1}\mathrm{vr}-\frac{m_3}{m_1}\mathrm{wq}+\frac{d_1}{m_1}u+\frac{1}{m_1}{F}_u$

$\stackrel{·}{v}=-\frac{m_1}{m_2}\mathrm{ur}+\frac{d_2}{m_2}v$

$\stackrel{·}{w}=\frac{m_1}{m_3}\mathrm{uq}+\frac{d_3}{m_3}w+g_1---\left(22\right)$

$\stackrel{·}{q}=\frac{m_1-m_3}{m_4}\mathrm{uw}+\frac{d_4}{m_4}q-g_2+\frac{1}{m_4}{\tau }_q$

$\stackrel{·}{r}=\frac{m_1-m_2}{m_5}\mathrm{uv}+\frac{d_5}{m_5}r+\frac{1}{m_5}{\tau }_r$

其中

$m_1=m-X_{\stackrel{·}{u}},$$m_2=m-Y_{\stackrel{·}{v}},$$m_3=m-Z_{\stackrel{·}{w}}$

$m_4=I_y-M_{\stackrel{·}{q}},$$m_5=I_z-N_r$

g₁＝(W-B)cosθ，g₂＝(z_gW-z_bB)sinθ

                                    (23)

d₁＝X_u+X_u|u||u|，d₂＝Y_v+Y_v|v||v|

d₃＝Z_w+Z_w|w||w|，d₄＝M_q+M_q|q||q|

d₅＝N_r+N_r|r||r|

其中，状态变量u，v，w，q和r分别表示载体坐标系{B}下AUV的纵向速度、横向速度、 垂向速度、纵倾角速度和艏摇角速度；m和m_(·)分别表示AUV质量和由流体作用产生的附 加质量，I_y为AUV绕y轴的转动惯量，I_z为AUV绕z轴的转动惯量，X_(·),Y_(·),Z_(·),M_(·)和N_(·)为粘性流体水动力系数；z_g和z_b分别为载体坐标下垂直轴上重心和浮心的坐标位置，W和 B分别表示AUV受到的重力和浮力，d_(·)为非线性阻尼水动力项，控制输入F_u、τ_q和τ_r分 别表示AUV推进器推力、纵倾控制力矩和转艏控制力矩。

这里结合式(23)设计AUV三维路径跟踪动力学控制器为

$\left(\begin{array}{c}{F}_u=m_1({\stackrel{·}{a}}_u-x_e-k_u{u}_e)-f_u\\ {\tau }_q=m_4({\stackrel{·}{a}}_q-k_4^{-1}\sin {\theta }_e-k_q{q}_e)-f_q\\ {\tau }_r=m_5({\stackrel{·}{a}}_r-k_2^{-1}{\cos }^{-1}\theta \sin {\psi }_e)-f_r\end{array}\right)---\left(24\right)$

其中

$\left(\begin{array}{c}{f}_u=m_2\mathrm{vr}-m_3\mathrm{wq}+d_{1}u\\ f_q=(m_1-m_3)\mathrm{uw}+d_{4}q-g_2\\ f_r=(m_1-m_2)\mathrm{uv}+d_{5}r\end{array}\right)---\left(25\right)$

这里变量u_e＝u-α_u，r_e＝r-α_r，q_e＝q-α_q定义为运动学控制器输出的实际值和期望 值的偏差，增益系数k_u＞0，k_q＞0，k_r＞0；通过李雅普诺夫稳定性理论证明，AUV三维 路径跟踪控制律式(24)能够保证跟踪误差闭环系统的渐近稳定性。

步骤4中设计AUV三维路径跟踪动力学等价控制器的具体过程为

结合式(20)构造李雅普诺夫能量函数为

$V_2=V_1+\frac{1}{2}(u_e^2+r_e^2+q_e^2)---\left(26\right)$

对上式两边求导，将式(21)代入得

${\stackrel{·}{V}}_2=-k_1{x}_e^2-k_3{k}_2^{-1}{\sin }^2{\psi }_e-k_5{k}_4^{-1}{\sin }^2{\theta }_e$

$+y_{e}v+z_{e}w+x_e{u}_e+\frac{r_e\sin {\psi }_e}{k_2\cos \theta }+\frac{q_e\sin {\theta }_e}{k_4}---\left(27\right)$

$+u_e(\stackrel{·}{u}-{\stackrel{·}{\alpha }}_u)+r_e(\stackrel{·}{r}-{\stackrel{·}{\alpha }}_r)+q_e(\stackrel{·}{q}-{\stackrel{·}{\alpha }}_q)$

整理得

${\stackrel{·}{V}}_2=-k_1{x}_e^2-k_3{k}_2^{-1}{\sin }^2{\psi }_e-k_5{k}_4^{-1}{\sin }^2{\theta }_e$

$+y_{e}v+z_{e}w+r_e(\stackrel{·}{r}-{\stackrel{·}{\alpha }}_r+k_2^{-1}{\cos }^{-1}\theta \sin {\psi }_e)---\left(28\right)$

$+u_e(\stackrel{·}{u}-{\stackrel{·}{\alpha }}_u+x_e)+q_e(\stackrel{·}{q}-{\stackrel{·}{\alpha }}_q+k_4^{-1}\sin {\theta }_e)$

将式(22)和式(24)代入得，式(28)变为

${\stackrel{·}{V}}_2=-k_1{x}_e^2-k_3{k}_2^{-1}{\sin }^2{\psi }_e-k_5{k}_4^{-1}{\sin }^2{\theta }_e$(29)

$-k_u{u}_e^2-k_q{q}_e^2-k_r{r}_e^2+y_{e}v+z_{e}w\le 0$

由于式(29)中AUV横向运动速度v和垂向运动速度w均为较小有界值，且由AUV运 动特性可知存在|v|＜u_max，|w|＜u_max，u_max为AUV纵向速度上界，所以当且仅当 (x_e,y_e,z_e,ψ_e,θ_e,u_e,r_e,q_e)＝0时由LaSalle不变原理可得，闭环跟踪误差系统渐近稳 定，通过调节控制器增益系数k₁，k₂，k₃，k₄，k₅和k_u，k_q，k_r保证系统的动态性能。

步骤5的具体过程为：

计算当前AUV位置ηⁿ＝(x,y,z)与标定的转向点WP_k＝(x_k,y_k,z_k)之间的距离 若小于设定的航迹切换半径R，则表示完成当前指定 路径的跟踪任务停止航行或切换下一个期望航迹，否则继续步骤2。

仿真验证与分析

下面举例说明，为验证发明设计的AUV三维航迹跟踪控制器的有效性，针对规划的三 维曲线路径进行仿真实验，并与传统PID控制仿真结果进行对比分析：

按照发明内容中步骤1所述，首先给出期望跟踪三维曲线的参数化描述

$\left(\begin{array}{c}{x}_d\left(s\right)=A\cos \left(\mathrm{\omega s}\right)\\ y_d\left(s\right)=A\sin \left(\mathrm{\omega s}\right)\\ z_d\left(s\right)=\mathrm{\omega s}\end{array}\right)---\left(30\right)$

其中参数A＝20，ω＝0.02π

给定期望跟踪三维曲线上“虚拟向导”初始位置信息

$\left(\begin{array}{c}{x}_d\left(0\right)=20\\ y_d\left(0\right)=0\\ z_d\left(0\right)=0\end{array}\right)---\left(31\right)$

给定期望跟踪三维曲线上“虚拟向导”的初始速度变量参数u_d＝1（m/s），增益参数 λ＝0.5，c＝1，γ＝1，控制器参数k₁＝50，k₂＝10，k₃＝100，k₄＝20，k₅＝100；k_u＝1， k_r＝5，k_q＝10；采用MATLAB数值仿真平台，按照步骤2～4解算AUV三维路径跟踪控 制输入得到最终的仿真曲线。

仿真分析

图4～图10给出AUV三维曲线路径跟踪控制仿真结果。图4为AUV三维螺旋下潜路 径跟踪轨迹，图5和图6分别为AUV三维路径跟踪轨迹在水平面和垂直面得投影曲线，从 中可以看出由于AUV的三维运动各自由度具有耦合作用，采用传统的PID控制器时控制 器参数不易调节，控制效果较差，无法实现对三维路径的精确跟踪，而本发明基于精确模 型设计的非线性控制器能够很好实现跟踪控制，提高了路径跟踪精度。图7为AUV三维路 径跟踪控制中跟踪误差曲线，与传统的PID控制器相比，可以看出本文设计的三维路径控 制器提高了路径跟踪的精度，缩短了AUV的冗余航程，具有更加稳定的控制能力保证AUV 较快的跟踪并收敛到期望路径，使得跟踪误差最终收敛到零，表明了控制器的跟踪精度和 响应速度。图8和图9分别为AUV三维路径跟踪控制过程中各状态变量包括线速度和姿态 角的变化曲线，可以看出AUV在沿螺旋线下潜过程中横向速度和垂向速度相比于纵向速度 较小，且为有界值，在控制器设计时可以忽略。图10为AUV三维路径跟踪控制输入响应。