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

Abstract

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

Description

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

技术领域

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

背景技术

海底地形的勘探和测绘对深海资源的开发有着重要的意义，欠驱动自主水下航行器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的函数为

${\mathrm{\η}}_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定义为

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

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

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

$r_d={\stackrel{\·}{\mathrm{\ψ}}}_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}下跟踪误差向量，所以得到跟踪误差可以表示为

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

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

$R_b^n=R_{y,\mathrm{\θ}}{R}_{z,\mathrm{\ψ}}$

$=\left[\begin{array}{ccc}\cos \mathrm{\θ}& 0& -\sin \mathrm{\θ}\\ 0& 1& 0\\ \sin \mathrm{\θ}& 0& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\ψ}& \sin \mathrm{\ζ}& 0\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]---\left(7\right)$

对式(6)求导得

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

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

$S\left({\mathrm{\ω}}_{\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{\·}{\mathrm{\ϵ}}=S^T\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)R_b^{\mathrm{NT}}{\mathrm{\η}}_e^n+R_b^{\mathrm{nT}}{\stackrel{\·}{\mathrm{\η}}}_e^n---\left(10\right)$

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

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

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

$=S^T\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)\mathrm{\ϵ}+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 {\mathrm{\θ}}_e\cos {\mathrm{\ψ}}_e& \cos {\mathrm{\θ}}_e\sin {\mathrm{\ψ}}_e& -{\sin \mathrm{\θ}}_e\\ -\sin {\mathrm{\ψ}}_e& \cos {\mathrm{\ψ}}_e& 0\\ \sin {\mathrm{\θ}}_e\cos {\mathrm{\ψ}}_e& \sin {\mathrm{\θ}}_e\sin {\mathrm{\ψ}}_e& \cos {\mathrm{\θ}}_e\end{array}\right]---\left(13\right)$

整理得

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

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

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

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

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

$u_p(t,\mathrm{\ϵ})=u_d(1-{\mathrm{\λe}}^{-c(t-t_0)})e^{-\mathrm{\γ}{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-{\mathrm{\α}}_u\\ r_e=r-{\mathrm{\α}}_r\\ q_e=q-{\mathrm{\α}}_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 {\mathrm{\ψ}}_e}{k_2}+\frac{1-{\cos \mathrm{\θ}}_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{\mathrm{\ψ}}_e-k_5{k}_4^{-1}{\sin }^2{\mathrm{\θ}}_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 {\mathrm{\ψ}}_e}{k_2\cos \mathrm{\θ}}+\frac{q_e\sin {\mathrm{\θ}}_e}{k_4}---\left(21\right)$

$=-k_1{x}_e^2-k_3{k}_2^{-1}{\sin }^2{\mathrm{\ψ}}_e-k_5{k}_4^{-1}{\sin }^2{\mathrm{\θ}}_e$

$+y_{e}v+z_{e}w+x_e{u}_e+\frac{r_e\sin {\mathrm{\ψ}}_e}{k_2\cos \mathrm{\θ}}+\frac{q_e\sin {\mathrm{\θ}}_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}{\mathrm{\τ}}_q$

$\stackrel{\·}{r}=\frac{m_1-m_2}{m_5}\mathrm{uv}+\frac{d_5}{m_5}r+\frac{1}{m_5}{\mathrm{\τ}}_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\\ {\mathrm{\τ}}_q=m_4({\stackrel{\·}{a}}_q-k_4^{-1}\sin {\mathrm{\θ}}_e-k_q{q}_e)-f_q\\ {\mathrm{\τ}}_r=m_5({\stackrel{\·}{a}}_r-k_2^{-1}{\cos }^{-1}\mathrm{\θ}\sin {\mathrm{\ψ}}_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{\mathrm{\ψ}}_e-k_5{k}_4^{-1}{\sin }^2{\mathrm{\θ}}_e$

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

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

整理得

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

$+y_{e}v+z_{e}w+r_e(\stackrel{\·}{r}-{\stackrel{\·}{\mathrm{\α}}}_r+k_2^{-1}{\cos }^{-1}\mathrm{\θ}\sin {\mathrm{\ψ}}_e)---\left(28\right)$

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

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

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

$-k_u{u}_e^2-k_q{q}_e^2-k_r{r}_e^2+y_{e}v+z_{e}w\≤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{\ωs}\right)\\ y_d\left(s\right)=A\sin \left(\mathrm{\ωs}\right)\\ z_d\left(s\right)=\mathrm{\ω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三维路径跟踪控制输入响应。

Claims (1)

1.一种基于迭代的自主水下航行器三维曲线路径跟踪控制方法，其特征是：

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

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

步骤3.计算期望跟踪路径上“虚拟向导”的期望移动速度、主水下航行器运动学跟踪控制律，包括纵向移动速度、主水下航行器的转艏角速度和纵倾角速度虚拟控制律；

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

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

给定期望跟踪路径上的虚拟向导在固定坐标系的坐标表示为某一标量参数s∈R的函数为

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

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

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

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

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

${\mathrm{\θ}}_d={\tan }^{-1}\left(\frac{-z_d^{\′}}{\sqrt{{\left(x_d^{\′}\right)}^2+{\left(y_d^{\′}\right)}^2}}\right)$

其中因此虚拟向导沿期望跟踪路径的旋转角速度分别表示为

$q_d={\stackrel{\·}{\mathrm{\θ}}}_d$

$r_d={\stackrel{\·}{\mathrm{\ψ}}}_d$

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

根据下式计算初始时刻主水下航行器当前位置与期望跟踪路径上“虚拟向导”在主水下航行器载体坐标系下的相对跟踪误差：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_e={\mathrm{ry}}_e-{\mathrm{qz}}_e+u-u_p\cos {\mathrm{\ψ}}_e\cos {\mathrm{\θ}}_e\\ {\stackrel{\·}{y}}_e=-{\mathrm{rx}}_e+u_p\sin {\mathrm{\ψ}}_e\cos {\mathrm{\θ}}_e+v\\ {\stackrel{\·}{z}}_e={\mathrm{qz}}_e-u_p\sin {\mathrm{\θ}}_e+w\end{array}\right.$

其中ψ_e＝ψ-ψ_d，θ_e＝θ-θ_d，ε＝[x_e,y_e,z_e]^T为AUV当前位置与期望跟踪路径上虚拟向导P之间的跟踪误差相对于主水下航行器载体坐标系{B}下三个坐标轴上的投影分量，主水下航行器纵向速度u、横向速度v和垂向速度w，艏摇角速度r和纵倾角速度q，u_p为虚拟向导的速度；

根据下式分别计算主水下航行器运动学跟踪控制律

(1)期望跟踪路径上虚拟向导的期望移动速度计算：

$u_p(t,\mathrm{\ϵ})=u_d(1-{\mathrm{\λe}}^{-c(t-t_0)})e^{-\mathrm{\γ}{d}_e};$

其中参数满足u_d>0，调节因子0<λ<1，c>0，γ>0，为主水下航行器载体坐标系下的跟踪误差距离；

(2)主水下航行器纵向速度运动学等价控制律为：

a_u＝u_pcosψ_ecosθ_e-k₁x_e；

(3)主水下航行器纵倾角速度运动学等价控制律为：

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

(4)主水下航行器艏摇角速度运动学等价控制律为：

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

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

根据主水下航行器具体水动力数学模型解算出最终的执行指令信号的具体过程为：

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

$\stackrel{\&CenterDot;}{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{\&CenterDot;}{v}=-\frac{m_1}{m_2}\mathrm{ur}+\frac{d_2}{m_2}v$

$\stackrel{\&CenterDot;}{w}=\frac{m_1}{m_2}\mathrm{uq}+\frac{d_3}{m_2}w+g_1$

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

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

其中

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

$m_4=I_y-M_{\stackrel{\&CenterDot;}{q}},m_5=I_z-N_{\stackrel{\&CenterDot;}{r}}$

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

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