海流干扰下的欠驱动AUV三维路径跟踪控制

姚绪梁¹,王晓伟^1,2,蒋晓刚²,王峰¹

(1. 哈尔滨工程大学 自动化学院,哈尔滨 150001;2. 九江职业技术学院 机械工程学院,江西 九江 332007)



摘要:

为解决存在海流干扰和模型不确定性情况下欠驱动自主水下航行器（AUV）的三维路径跟踪控制问题,首先在Serret-Frenet坐标系下,基于虚拟向导建立了跟踪误差模型. 然后,在运动学控制器中基于李雅普诺夫（Lyapunov）理论和反步法设计具有自适应律的虚拟向导和一种改进的积分视线法（ILOS）导引律,克服了海流的干扰并减少了超调. 应用反步自适应滑模控制（BASMC）理论设计动力学控制器,保证了系统的稳定性和鲁棒性. 最后,应用非线性级联理论证明了整个控制系统的闭环稳定性. 仿真结果表明:基于AUV与流体的相对速度来实现的控制律,便于工程应用. 该控制器能够有效克服海流和模型不确定的影响,实现对三维路径的跟踪.

关键词:  自主水下航行器  路径跟踪  滑模控制  Serret-Frenet坐标系  积分视线法

DOI：10.11918/j.issn.0367-6234.201709002

分类号:TP242.6

文献标识码:A

基金项目:国家自然科学基金(51279039)



Control for 3D path-following of underactuated autonomous underwater vehicle under current disturbance

YAO Xuliang¹,WANG Xiaowei^1,2,JIANG Xiaogang²,WANG Feng¹

(1.College of Automation, Harbin Engineering University, Harbin 150001, China; 2. College of Mechanical Engineering, Jiujiang Vocational and Technical College, Jiujiang 332007, Jiangxi, China)

Abstract:

To deal with the 3D path-following control problem of underactuated autonomous underwater vehicle (AUV) in the presence of current disturbance and model uncertainty, a tracking error model on the basis of virtual target in Serret-Frenet frame was established. Based on the Lyapunov theory and back-stepping method in kinematic controller, the virtual target with adaptive law and an improved integral line-of-sight (ILOS) guidance law were then developed, which can overcome the interference of current and reduce overshoot. Dynamic controller was designed based on the back-stepping adaptive sliding mode control (BASMC) theory, which guarantees the stability and robustness of the system. Finally, the closed loop stability of the system was demonstrated by the nonlinear cascade system theory. The simulation results demonstrate that the control law, which is implemented based on the relative velocity between AUV and fluid, is convenient for engineering application, and the controller can effectively overcome the influence of current and model uncertainty and realize 3D path-following.

Key words:  AUV  path-following  sliding mode control  Serret-Frenet frame  ILOS


姚绪梁, 王晓伟, 蒋晓刚, 王峰. 海流干扰下的欠驱动AUV三维路径跟踪控制[J]. 哈尔滨工业大学学报, 2019, 51(3): 37-45. DOI: 10.11918/j.issn.0367-6234.201709002.
YAO Xuliang, WANG Xiaowei, JIANG Xiaogang, WANG Feng. Control for 3D path-following of underactuated autonomous underwater vehicle under current disturbance[J]. Journal of Harbin Institute of Technology, 2019, 51(3): 37-45. DOI: 10.11918/j.issn.0367-6234.201709002.
基金项目 国家自然科学基金(51279039) 作者简介 姚绪梁(1969—)男，教授，博士生导师 通信作者 王晓伟，wangxiaowei@hrbeu.edu.cn 文章历史 收稿日期: 2017-09-01



Contents            -->Abstract            Full text            Figures/Tables            PDF


海流干扰下的欠驱动AUV三维路径跟踪控制
姚绪梁¹, 王晓伟^1,2, 蒋晓刚², 王峰¹    
1. 哈尔滨工程大学 自动化学院，哈尔滨 150001;
2. 九江职业技术学院 机械工程学院，江西 九江 332007

收稿日期: 2017-09-01
基金项目: 国家自然科学基金(51279039)
作者简介: 姚绪梁(1969—)男，教授，博士生导师
通信作者: 王晓伟，wangxiaowei@hrbeu.edu.cn


摘要: 为解决存在海流干扰和模型不确定性情况下欠驱动自主水下航行器(AUV)的三维路径跟踪控制问题，首先在Serret-Frenet坐标系下，基于虚拟向导建立了跟踪误差模型.然后，在运动学控制器中基于李雅普诺夫(Lyapunov)理论和反步法设计具有自适应律的虚拟向导和一种改进的积分视线法(ILOS)导引律，克服了海流的干扰并减少了超调.应用反步自适应滑模控制(BASMC)理论设计动力学控制器，保证了系统的稳定性和鲁棒性.最后，应用非线性级联理论证明了整个控制系统的闭环稳定性.仿真结果表明：基于AUV与流体的相对速度来实现的控制律，便于工程应用.该控制器能够有效克服海流和模型不确定的影响，实现对三维路径的跟踪.
关键词: 自主水下航行器    路径跟踪    滑模控制    Serret-Frenet坐标系    积分视线法    
Control for 3D path-following of underactuated autonomous underwater vehicle under current disturbance
YAO Xuliang¹, WANG Xiaowei^1,2, JIANG Xiaogang², WANG Feng¹    
1. College of Automation, Harbin Engineering University, Harbin 150001, China;
2. College of Mechanical Engineering, Jiujiang Vocational and Technical College, Jiujiang 332007, Jiangxi, China


Abstract: To deal with the 3D path-following control problem of underactuated autonomous underwater vehicle (AUV) in the presence of current disturbance and model uncertainty, a tracking error model on the basis of virtual target in Serret-Frenet frame was established. Based on the Lyapunov theory and back-stepping method in kinematic controller, the virtual target with adaptive law and an improved integral line-of-sight (ILOS) guidance law were then developed, which can overcome the interference of current and reduce overshoot. Dynamic controller was designed based on the back-stepping adaptive sliding mode control (BASMC) theory, which guarantees the stability and robustness of the system. Finally, the closed loop stability of the system was demonstrated by the nonlinear cascade system theory. The simulation results demonstrate that the control law, which is implemented based on the relative velocity between AUV and fluid, is convenient for engineering application, and the controller can effectively overcome the influence of current and model uncertainty and realize 3D path-following.
Keywords: AUV    path-following    sliding mode control    Serret-Frenet frame    ILOS    
自主水下航行器(AUV)在民用和军事等众多领域都具有很高的实用价值.由于AUV对路径和轨迹的精确跟踪能力是其完成各种作业任务和避障的关键技术，所以AUV的路径和轨迹跟踪控制问题一直是研究的热点.但是，目前仍存在一些困难使得AUV控制器的设计变得非常棘手^[1].例如，出于节能和提高可靠性等方面的考虑，大部分AUV都被设计为欠驱动的形式，一般只配置推进器、方向舵和水平舵，缺少横向和垂向的驱动，从而在横向和垂向运动受到限制.另外，AUV的工作环境中存在海浪、海流等干扰以及AUV六自由度运动存在非线性和耦合性、水动力参数存在不确定性，导致AUV的运动模型很难精确建模.

AUV的路径和轨迹跟踪控制一般分为运动学和动力学两个层面.文献[2-3]分别基于视线法(LOS)和向量场法(VF)导引律，设计了欠驱动船舶的路径跟踪控制器.文献[4]通过引入了趋近角和虚拟向导建立跟踪误差方程，然后基于李雅普诺夫(Lyapunov)理论和反步法设计了水平面路径跟踪控制器.但是当存在海流等干扰时，基于传统的LOS和VF导引律的路径跟踪会产生稳态误差.为了解决这个问题，文献[5]提出了一种积分视线法(ILOS)导引律，并得到了广泛的应用.针对欠驱动AUV的路径跟踪动力学控制器的设计，也有大量的研究.文献[6]应用ILOS导引律和PID控制器实现了海流干扰下欠驱动AUV的水平面路径跟踪控制.文献[7]采用反馈线性化方法设计了水平面路径跟踪控制器.针对AUV的垂直面路径跟踪问题，文献[8]采取解耦方式，分别采用S面控制方法和反步法设计了航速控制器和深度控制器.为了提高系统的鲁棒性，文献[9-10]分别针对水平面和垂直面路径跟踪设计了自适应控制器和滑模控制器.以上文献只是考虑了AUV在水平面或垂直面的路径跟踪问题.文献[11]应用线性稳定理论和反步法设计了欠驱动AUV的三维轨迹跟踪控制器，但未考虑参数的不确定性.文献[12]针对欠驱动AUV三维路径跟踪设计了鲁棒自适应控制器.文献[13]采用滤波反步法设计了三维路径跟踪控制器.但以上三维路径跟踪控制器都未考虑海流的影响.文献[14]针对欠驱动AUV模型不确定性和海流干扰，将三维路径跟踪分解为水平面和垂直面跟踪控制问题，设计了航向和纵倾鲁棒控制器，但仅实现了对三维直线路径的跟踪控制.

本文针对存在海流干扰和模型不确定性情况下欠驱动AUV的三维路径跟踪控制问题，基于Serret-Frenet坐标系引入了虚拟向导并建立了三维路径跟踪误差模型.设计了具有自适应律的虚拟向导并提出了一种改进的积分视线法(ILOS)导引律.基于反步自适应滑模控制(BASMC)理论和AUV与流体的相对速度设计动力学控制器，并应用sigmoid函数设计滑模项，保证了系统的稳定性和鲁棒性、有效地削弱了抖振，而且更便于工程应用.仿真结果验证了该控制器的有效性.

1 AUV六自由度运动方程本文研究的欠驱动AUV浮力与重力配置相等，尾部装有一个螺旋桨、一对水平舵、一对方向舵分别用来控制航速、纵倾和航向. AUV的运动及受力分析需要用到两种坐标系，分别是运动坐标系{B}:O-xyz和固定坐标系{I}:E-ξηζ.本文将运动坐标系的原点O选取在AUV的浮心处，各坐标轴均符合右手系. AUV在六自由度上受到的力和力矩如图 1所示，所有符号均采用国际拖拽水池会议(ITTC)所推荐的符号. AUV的运动学和动力学模型可以分别简化为

Fig. 1
图 1 三维路径跟踪示意图 Fig. 1 Schematic diagram of 3D path-following


$\mathit{\boldsymbol{\dot \eta }} = \mathit{\boldsymbol{J}}\left( \mathit{\boldsymbol{\eta }} \right){\mathit{\boldsymbol{v}}_r} + {\left[ {\begin{array}{*{20}{c}}{{v_\xi }}&{{v_\eta }}&{{v_\zeta }}&0&0&0\end{array}} \right]^{\rm{T}}},$ (1)

$\mathit{\boldsymbol{M}}{{\mathit{\boldsymbol{\dot v}}}_r} + \mathit{\boldsymbol{C}}\left( {{\mathit{\boldsymbol{v}}_r}} \right){\mathit{\boldsymbol{v}}_r} + \mathit{\boldsymbol{D}}\left( {{\mathit{\boldsymbol{v}}_r}} \right){\mathit{\boldsymbol{v}}_r} + \mathit{\boldsymbol{g}}\left( \mathit{\boldsymbol{\eta }} \right) = \tau + \mathit{\boldsymbol{d}}.$ (2)

式中：η =[ξ η ζ ? θ ψ]^T，其中ξ、η、ζ为AUV在固定坐标系中的坐标，?、θ、ψ分别为横摇角、纵摇角和艏摇角；J (η)为{B}到{I}的旋转变换矩阵，即

$\mathit{\boldsymbol{J}}\left( \mathit{\boldsymbol{\eta }} \right) = \left[ {\begin{array}{*{20}{c}}{{\mathit{\boldsymbol{J}}_1}\left( \mathit{\boldsymbol{\eta }} \right)}&{{{\bf{0}}_{3 \times 3}}}\\{{{\bf{0}}_{3 \times 3}}}&{{\mathit{\boldsymbol{J}}_2}\left( \mathit{\boldsymbol{\eta }} \right)}\end{array}} \right],$

${\mathit{\boldsymbol{J}}_1}\left( \mathit{\boldsymbol{\eta }} \right) = \left[ {\begin{array}{*{20}{c}}{\cos \psi \cos \theta }&{\cos \psi \sin \theta \sin \varphi - \sin \psi \cos \varphi }&{\cos \psi \sin \theta \cos \varphi + \sin \psi \sin \varphi }\\{\sin \psi \cos \theta }&{\sin \psi \sin \theta \sin \varphi + \cos \psi \cos \varphi }&{\sin \psi \sin \theta \cos \varphi - \cos \psi \sin \varphi }\\{ - \sin \theta }&{\cos \theta \sin \varphi }&{\cos \theta \cos \varphi }\end{array}} \right],$

${\mathit{\boldsymbol{J}}_2}\left( \mathit{\boldsymbol{\eta }} \right) = \left[ {\begin{array}{*{20}{c}}1&{\sin \varphi \tan \theta }&{\cos \varphi \tan \theta }\\0&{\cos \varphi }&{ - \sin \varphi }\\0&{\sin \varphi /\cos \theta }&{\cos \varphi /\cos \theta }\end{array}} \right];$

v_r= v_B- v_c^B=[u_r v_r w_r p q r]^T为AUV与流体的相对速度，其中u_r、v_r、w_r分别为纵向、横向和垂向相对速度；AUV的绝对速度定义为v_B = [u v w p q r]^T, 其中u、v、w分别为纵向、横向和垂向绝对速度，p、q、r分别为横摇、纵摇和艏摇角速度；v_c^B=[u_c v_c w_c 0 0 0]^T为海流在{B}中的速度分量；[u_c v_c w_c]^T= J₁^T(η) v_c^I, 其中v_c^I= [v_ξ v_η v_ζ]^T为海流在{I}中的速度分量，假设海流是无旋的且满足v_ξ+²v_η²+v_ζ² < V_max²；M、C (v_r)、D (v_r)分别为质量矩阵、哥氏力矩阵和阻尼矩阵；g (η)=[0 0 0 K_HS M_HS 0]^T为重力和浮力产生的恢复力矩，本文研究的AUV的重心位于浮心正下方，在横摇和纵摇两个自由度存在恢复力矩分别为K_HS=-z_gGcos θsin ?、M_HS=-z_gGsin θ, 其中z_g、G分别为AUV的稳心高和重力；τ = [X_τ 0 0 0 M_τ N_τ]^T为控制力和力矩，其中X_τ、M_τ、N_τ分别为螺旋桨、水平舵和方向舵产生的推力、纵倾力矩和艏摇力矩；d为模型不确定性.

为了便于动力学控制器的设计，可以将动力学模型(2)展开为如下微分方程组的形式：

$\left\{ \begin{array}{l}{{\dot u}_r} = {F_u}{u_r} + {F_X}{X_\tau } + {d_u},\\{{\dot v}_r} = {F_v}{v_r} + {d_v},\\{{\dot w}_r} = {F_w}{w_r} + {d_w},\\\dot p = {F_p}p + {d_p},\\\dot q = {F_q}q + {F_M}{M_{{\rm{HS}}}} + {F_M}{M_\tau } + {d_q},\\\dot r = {F_r}r + {F_N}{N_\tau } + {d_r}.\end{array} \right.$ (3)

式中：

${F_u} = \frac{{{X_{u\left| u \right|}}\left| {{u_r}} \right|}}{{m - {X_{\dot u}}}},{F_X} = \frac{1}{{m - {X_{\dot u}}}},{F_v} = \frac{{{Y_{v\left| v \right|}}\left| {{v_r}} \right|}}{{m - {Y_{\dot v}}}},$

${F_w} = \frac{{{Z_{w\left| w \right|}}\left| {{w_r}} \right|}}{{m - {Z_{\dot w}}}},{F_p} = \frac{{{K_{p\left| p \right|}}\left| p \right|}}{{{I_{xx}} - {K_{\dot p}}}},$

${F_q} = \frac{{{M_{q\left| q \right|}}\left| q \right| + {M_{uq}}{u_r}}}{{{I_{yy}} - {M_{\dot q}}}},{F_M} = \frac{1}{{{I_{yy}} - {M_{\dot q}}}},$

${F_r} = \frac{{{N_{r\left| r \right|}}\left| r \right| + {N_{ur}}{u_r}}}{{{I_{zz}} - {N_{\dot r}}}},{F_N} = \frac{1}{{{I_{zz}} - {N_{\dot r}}}},$

m为AUV的质量，I_xx、I_yy、I_zz为AUV的惯性矩，X_{.}、Y_{.}、Z_{.}、K_{.}、M_{.}、N_{.}为水动力系数，F_{.}采用名义值，模型不确定性包含在d_{.}中.

2 三维路径跟踪运动学误差模型欠驱动AUV三维路径跟踪原理见图 1~3，为了建立跟踪误差模型引入了{SF}: P-x_sfy_sfz_sf坐标系，{SF}坐标系的原点P即为期望路径上被AUV跟踪的虚拟向导.定义c_c、c_t分别为路径的曲率和挠率，从路径起点开始沿路径的广义弧长为s，虚拟向导的速度为u_F，{SF}坐标系的纵摇和艏摇角速度分别为q_F、r_F，则${\dot s}$=u_F, q_F=c_tu_F, r_F=c_cu_F.定义v_SF =[u_F ??? q_F ??? r_F]^T，忽略横摇的影响，定义P在{I}中的坐标为P_P=[ξ_P ??? η_P??? ζ_P??? θ_F??? ψ_F]^T，则${{\mathit{\boldsymbol{\dot P}}}_P}$= R_F^Iv_SF，其中R_F^I为{SF}坐标系到{I}坐标系的旋转变换矩阵为

Fig. 2
图 2 水平面路径跟踪示意图 Fig. 2 Horizontal plane of path-following


Fig. 3
图 3 垂直面路径跟踪示意图 Fig. 3 Vertical plane of path-following


$\mathit{\boldsymbol{R}}_F^I = \left[ {\begin{array}{*{20}{c}}{\cos {\psi _F}\cos {\theta _F}}&0&0\\{\sin {\psi _F}\cos {\theta _F}}&0&0\\{ - \sin {\theta _F}}&0&0\\0&1&0\\0&0&{1/\cos {\theta _F}}\end{array}} \right].$

为了便于跟踪误差模型的推导，以AUV的合成速度u_t为纵轴定义{W}坐标系，由{B}坐标系沿y轴旋转α再沿z轴旋转β，即为{W}坐标系.忽略横摇的影响，基于{W}定义P_O=[ξ_O??? η_O??? ζ_O??? θ_W??? ψ_W]^T为O在{I}中的位置和姿态，其中θ_W=θ+α，ψ_W=ψ+β，α、β分别为攻角和漂角，α=-tan^-1(w/u)，β=tan^-1(v/u).定义在{SF}中的跟踪误差为P_e=[x_ey_e?? z_e?? θ_e^W?? ψ_e^W]^T= R_I^F(P_O-P_P)，其中R_I^F是从{I}到{SF}的旋转变换矩阵，即

$\mathit{\boldsymbol{R}}_I^F = \left[ {\begin{array}{*{20}{c}}{{\mathit{\boldsymbol{R}}_1}}&{{{\bf{0}}_{3 \times 2}}}\\{{{\bf{0}}_{2 \times 3}}}&{{\mathit{\boldsymbol{R}}_2}}\end{array}} \right],$

${\mathit{\boldsymbol{R}}_1} = \left[ {\begin{array}{*{20}{c}}{\cos {\psi _F}\cos {\theta _F}}&{\sin {\psi _F}\cos {\theta _F}}&{ - \sin {\theta _F}}\\{ - \sin {\psi _F}}&{\cos {\psi _F}}&0\\{\cos {\psi _F}\sin {\theta _F}}&{\sin {\psi _F}\sin {\theta _F}}&{\cos {\theta _F}}\end{array}} \right],$

${\mathit{\boldsymbol{R}}_2} = \left[ {\begin{array}{*{20}{c}}1&0\\0&{\cos {\theta _F}}\end{array}} \right].$

矩阵R₁和R₂具有以下性质：

${{\mathit{\boldsymbol{\dot R}}}_1} = {\mathit{\boldsymbol{S}}_1}{\mathit{\boldsymbol{R}}_1},{{\mathit{\boldsymbol{\dot R}}}_2} = {\mathit{\boldsymbol{S}}_2}{\mathit{\boldsymbol{R}}_2},{\chi ^{\rm{T}}}{\mathit{\boldsymbol{S}}_1}\chi = 0,\chi \in {\mathit{\boldsymbol{R}}^3},$

${\mathit{\boldsymbol{S}}_1} = \left[ {\begin{array}{*{20}{c}}0&{{r_F}}&{ - {q_F}}\\{ - {r_F}}&0&{ - {r_F}\tan {\theta _F}}\\{{q_F}}&{{r_F}\tan {\theta _F}}&0\end{array}} \right],$

${\mathit{\boldsymbol{S}}_2} = \left[ {\begin{array}{*{20}{c}}0&0\\0&{ - {q_F}\tan {\theta _F}}\end{array}} \right].$

定义P_O1=[ξ_O η_O ζ_O]^T，P_P1=[ξ_P η_P ζ_P]^T，P_e1=[x_e y_e z_e]^T，则位置误差为P_e1=[x_e y_e z_e]^T= R₁(P_o1-P_P1)，对P_e1求导并左乘P_e1^T，得

$\begin{array}{l}\mathit{\boldsymbol{P}}_{e1}^{\rm{T}}{{\mathit{\boldsymbol{\dot P}}}_{e1}} = \mathit{\boldsymbol{P}}_{e1}^{\rm{T}}\left[ {{{\mathit{\boldsymbol{\dot R}}}_1}\left( {{\mathit{\boldsymbol{P}}_{O1}} - {\mathit{\boldsymbol{P}}_{P1}}} \right) + {\mathit{\boldsymbol{R}}_1}\left( {{{\mathit{\boldsymbol{\dot P}}}_{O1}} - {{\mathit{\boldsymbol{\dot P}}}_{P1}}} \right)} \right] = \\\;\;\;\;\;\;\;\;\;\;\;\;\mathit{\boldsymbol{P}}_{e1}^{\rm{T}}{\mathit{\boldsymbol{S}}_1}{\mathit{\boldsymbol{R}}_1}\left( {{\mathit{\boldsymbol{P}}_{O1}} - {\mathit{\boldsymbol{P}}_{P1}}} \right) + \mathit{\boldsymbol{P}}_{e1}^{\rm{T}}{\mathit{\boldsymbol{R}}_1}\left( {{{\mathit{\boldsymbol{\dot P}}}_{O1}} - {{\mathit{\boldsymbol{\dot P}}}_{P1}}} \right) = \\\;\;\;\;\;\;\;\;\;\;\;\;\mathit{\boldsymbol{P}}_{e1}^{\rm{T}}{\mathit{\boldsymbol{S}}_1}{\mathit{\boldsymbol{P}}_{e1}} + \mathit{\boldsymbol{P}}_{e1}^{\rm{T}}{\mathit{\boldsymbol{R}}_1}\left( {{{\mathit{\boldsymbol{\dot P}}}_{O1}} - {{\mathit{\boldsymbol{\dot P}}}_{P1}}} \right) = \\\;\;\;\;\;\;\;\;\;\;\;\;\mathit{\boldsymbol{P}}_{e1}^{\rm{T}}{\mathit{\boldsymbol{R}}_1}\left( {{{\mathit{\boldsymbol{\dot P}}}_{O1}} - {{\mathit{\boldsymbol{\dot P}}}_{P1}}} \right),\end{array}$

消去上式两边的P_e1^T,得${{\mathit{\boldsymbol{\dot P}}}_{e1}} = \mathit{\boldsymbol{R}}{_1}({{\mathit{\boldsymbol{\dot P}}}_o}_1 - \mathit{\boldsymbol{\dot P}}{_P}_1).$

根据关系式：

${\mathit{\boldsymbol{R}}_1}{{\mathit{\boldsymbol{\dot P}}}_{O1}} = \left[ {\begin{array}{*{20}{c}}{\cos \psi _e^W\cos \theta _e^W}&{ - \sin \psi _e^W}&{\cos \psi _e^W\sin \theta _e^W}\\{\sin \psi _e^W\cos \theta _e^W}&{\cos \psi _e^W}&{\sin \psi _e^W\sin \theta _e^W}\\{ - \sin \theta _e^W}&0&{\cos \theta _e^W}\end{array}} \right]\left[ {\begin{array}{*{20}{c}}{{u_t}}\\0\\0\end{array}} \right],$

${\mathit{\boldsymbol{R}}_1}{{\mathit{\boldsymbol{\dot P}}}_{P1}} = {\left[ {\begin{array}{*{20}{c}}{{u_F}}&0&0\end{array}} \right]^{\rm{T}}},$

可得

${{\mathit{\boldsymbol{\dot P}}}_{e1}} = \left[ {\begin{array}{*{20}{c}}{{{\dot x}_e}}\\{{{\dot y}_e}}\\{{{\dot z}_e}}\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}{{u_t}\cos \psi _e^W\cos \theta _e^W - {u_F}}\\{{u_t}\sin \psi _e^W\cos \theta _e^W}\\{ - {u_t}\sin \theta _e^W}\end{array}} \right].$ (4)

但是误差模型(4)中的u_t、θ_e^W、ψ_e^W不便于测量和应用，所以还需要作进一步处理.定义

$\mathit{\boldsymbol{P}}_{O2}^B = {\left[ {\begin{array}{*{20}{c}}\theta &\psi \end{array}} \right]^{\rm{T}}},$

$\mathit{\boldsymbol{P}}_{O2}^W = {\left[ {\begin{array}{*{20}{c}}{{\theta _W}}&{{\psi _W}}\end{array}} \right]^{\rm{T}}} = \mathit{\boldsymbol{P}}_{O2}^B + {\left[ {\begin{array}{*{20}{c}}\alpha &\beta \end{array}} \right]^{\rm{T}}},$

${\mathit{\boldsymbol{P}}_{P2}} = {\left[ {\begin{array}{*{20}{c}}{{\theta _F}}&{{\psi _F}}\end{array}} \right]^{\rm{T}}},$

姿态误差：

$\mathit{\boldsymbol{P}}_{e2}^B = {\left[ {\begin{array}{*{20}{c}}{\theta _e^B}&{\psi _e^B}\end{array}} \right]^{\rm{T}}} = {\mathit{\boldsymbol{R}}_2}\left( {\mathit{\boldsymbol{P}}_{O2}^B - {\mathit{\boldsymbol{P}}_{P2}}} \right),$

$\begin{array}{l}\mathit{\boldsymbol{P}}_{e2}^W = {\left[ {\begin{array}{*{20}{c}}{\theta _e^W}&{\psi _e^W}\end{array}} \right]^{\rm{T}}} = {\mathit{\boldsymbol{R}}_2}\left( {\mathit{\boldsymbol{P}}_{O2}^W - {\mathit{\boldsymbol{P}}_{P2}}} \right) = \mathit{\boldsymbol{P}}_{e2}^B + \\\;\;\;\;\;\;\;\;{\left[ {\begin{array}{*{20}{c}}\alpha &{\beta \cos {\theta _F}}\end{array}} \right]^{\rm{T}}}.\end{array}$

将${{\dot y}_e}$展开可得

${{\dot y}_e} = {u_t}{\rm{cos}}\;\theta _e^W{\rm{sin}}\;\psi _e^B{\rm{cos}}\;\left( {\beta {\rm{cos}}\;{\theta _F}} \right) + {u_t}{\rm{cos}}\;\theta _e^W\cdot{\rm{cos}}\;\psi _e^B{\rm{sin}}\;(\beta {\rm{cos}}\;{\theta _F})$, 因为β、θ_F比较小，所以cos (βcos θ_F)≈1, sin (βcos θ_F)≈β，不妨设u_t=k_uu_r, 0 < k_u < k_u, cos θ_e^W=k_θcos θ_e^B, 0 < k_θ < k_θ，0 < k_y=k_uk_θ < k_uk_θ，k_u、k_θ分别为k_u、k_θ的上界，则${{\dot y}_e}$可重写为${{\dot y}_e}$=k_yu_rcos θ_e^Bsin ψ_e^B+k_yu_rcos θ_e^Bcos ψ_e^Bβ.同理${{\dot z}_e}$可以重新整理为${{\dot z}_e}$=－k_uu_rsin θ_e^B－k_uu_rcos θ_e^Bα.用u_r、θ_e^B、ψ_e^B代替u_t、θ_e^W、ψ_e^W，则${{\dot x}_e}$可以重新整理为${{\dot x}_e}$=u_rcos ψ_e^Bcos θ_e^B－u_F+d_x，其中d_x为不确定项.综上所述，位置误差模型(4)最终重新整理为

${{\mathit{\boldsymbol{\dot P}}}_{e1}} = \left[ {\begin{array}{*{20}{c}}{{u_r}\cos \psi _e^B\cos \theta _e^B - {u_F} + {d_x}}\\{{k_y}{u_r}\cos \theta _e^B\sin \psi _e^B + {k_y}{u_r}\cos \theta _e^B\cos \psi _e^B\beta }\\{ - {k_u}{u_r}\sin \theta _e^B - {k_u}{u_r}\cos \theta _e^B\alpha }\end{array}} \right].$ (5)

对姿态误差P_e2^B求导，可得

$\begin{array}{l}\mathit{\boldsymbol{\dot P}}_{e2}^B = {{\mathit{\boldsymbol{\dot R}}}_2}\left( {\mathit{\boldsymbol{P}}_{O2}^B - {\mathit{\boldsymbol{P}}_{P2}}} \right) + {\mathit{\boldsymbol{R}}_2}\left( {\mathit{\boldsymbol{\dot P}}_{O2}^B - {{\mathit{\boldsymbol{\dot P}}}_{P2}}} \right) = \\\;\;\;\;\;\;\;\;{\mathit{\boldsymbol{S}}_2}\mathit{\boldsymbol{P}}_{e2}^B + {\mathit{\boldsymbol{R}}_2}\left( {\mathit{\boldsymbol{\dot P}}_{O2}^B - {{\mathit{\boldsymbol{\dot P}}}_{P2}}} \right) = \\\;\;\;\;\;\;\;\;\left[ {\begin{array}{*{20}{c}}{q - {q_F}}\\{ - {q_F}\psi _e^B\tan {\theta _F} + \frac{{\cos {\theta _F}}}{{\cos \theta }}r - {r_F}}\end{array}} \right].\end{array}$ (6)

3 三维路径跟踪控制器设计本文路径跟踪控制的目的是使AUV收敛到期望路径，并使AUV的纵向相对速度u_r保持恒定.所以控制目标为

$\left\{ \begin{array}{l}\mathop {\lim }\limits_{t \to \infty } {u_r}\left( t \right) = {u_{rd}};\\\mathop {\lim }\limits_{t \to \infty } {x_e}\left( t \right) = 0,\mathop {\lim }\limits_{t \to \infty } {y_e}\left( t \right) = 0,\mathop {\lim }\limits_{t \to \infty } {z_e}\left( t \right) = 0;\\\mathop {\lim }\limits_{t \to \infty } \theta _e^W\left( t \right) = 0,\mathop {\lim }\limits_{t \to \infty } \psi _e^W\left( t \right) = 0.\end{array} \right.$

3.1 运动学控制器的设计1) 镇定x_e.定义${{\tilde d}_x} = {d_x}{\rm{ - }}{{\hat d}_x}{\rm{, }}{{\hat d}_x}$为d_x的估计值.可以认为d_x为慢时变信号，则${{\dot {\tilde d}}_x} = - {{\dot {\hat d}}_x}\;$.选取Lyapunov函数为${V_1} = \frac{1}{2}x_e^2 + \frac{1}{{2{k_1}}}\tilde d_x^2$，对V₁求导得

${{\dot V}_1} = {x_e}\left( {{u_r}\cos \psi _e^B\cos \theta _e^B - {u_F} + {{\hat d}_x}} \right) + {{\tilde d}_x}\left( {{x_e} - \frac{1}{{{k_1}}}{{\dot {\hat d}}_x}} \right).$

设计虚拟向导和自适应控制律为

$\left\{ \begin{array}{l}{u_F} = {u_r}\cos \psi _e^B\cos \theta _e^B + {k_2}{x_e} + {{\hat d}_x},{k_2} > 0;\\{{\dot {\hat d}}_x} = {k_1}{x_e},{k_1} > 0.\end{array} \right.$ (7)

可得${{\dot V}_1}$=－k₂x_e²≤0.

2) 镇定y_e和z_e.要实现对y_e和z_e的镇定，需要选择合适的导引律，如果使用传统的LOS导引律

$\theta _{ed}^B = {\tan ^{ - 1}}\left( {\frac{{{z_e}}}{{{\Delta _\theta }}}} \right),\psi _{ed}^B = - {\tan ^{ - 1}}\left( {\frac{{{y_e}}}{{{\Delta _\psi }}}} \right),{\Delta _\theta } > 0,{\Delta _\psi } > 0,$

其中θ_ed^B、ψ_ed^B分别为θ_e^B、ψ_e^B的期望值，将θ_e^B=θ_ed^B, ψ_e^B=ψ_ed^B代入误差模型(4)，可得

${{\dot y}_e} = - {u_t}\cos \theta _e^W\left[ {\frac{{{y_e}\cos \left( {\beta \cos {\theta _F}} \right)}}{{\sqrt {\Delta _\psi ^2 + y_e^2} }} - \frac{{{\Delta _\psi }\sin \left( {\beta \cos {\theta _F}} \right)}}{{\sqrt {\Delta _\psi ^2 + y_e^2} }}} \right],$

${{\dot z}_e} = - {u_t}\left[ {\frac{{{z_e}}}{{\sqrt {\Delta _\theta ^2 + z_e^2} }}\cos \alpha + \frac{{{\Delta _\theta }}}{{\sqrt {\Delta _\theta ^2 + z_e^2} }}\sin \alpha } \right].$

令${{\dot y}_e} = 0, {{\dot z}_e} = 0$，可得y_e和z_e的稳态值分别为

$\left| {{y_{ess}}} \right| = {\Delta _\psi }\left| {\tan \left( {\beta \cos {\theta _F}} \right)} \right| \le {\Delta _\psi }\left| {\tan \bar \beta } \right| \approx {\Delta _\psi }\left| {\bar \beta } \right|,$

$\left| {{z_{ess}}} \right| = {\Delta _\theta }\left| {\tan \alpha } \right| \le {\Delta _\theta }\left| {\tan \bar \alpha } \right| \approx {\Delta _\theta }\left| {\bar \alpha } \right|.$

式中α、β分别为α、β的上界.显然，如果α、β不为零，y_e、z_e会存在稳态误差.为了消除稳态误差，设计ILOS导引律为

$\left\{ \begin{array}{l}\theta _{ed}^B = {\tan ^{ - 1}}\left( {\frac{{{z_e} - {\alpha _c}}}{{{\Delta _\theta }}}} \right),\\\psi _{ed}^B = - {\tan ^{ - 1}}\left( {\frac{{{y_e} + {\beta _c}}}{{{\Delta _\psi }}}} \right).\end{array} \right.$ (8)

定义$\tilde \alpha = \alpha - \hat \alpha , \tilde \beta = \beta - \hat \beta , \hat \alpha 、\hat \beta $分别为α、β的估计值.选取Lyapunov函数

${V_2} = \frac{1}{2}z_e^2 + \frac{1}{2}y_e^2 + \frac{{{k_u}}}{{2{k_3}}}{{\tilde \alpha }^2} + \frac{{{k_y}}}{{2{k_4}}}{{\tilde \beta }^2},$

令u_r=u_rd, θ_e^B=θ_ed^B, ψ_e^B=ψ_ed^B，根据式(5)、(8)对V₂求导得

$\begin{array}{l}{{\dot V}_2} = {z_e}\left( { - \frac{{{k_u}{u_{rd}}\left( {{z_e} - {\alpha _c}} \right)}}{{\sqrt {\Delta _\theta ^2 + {{\left( {{z_e} - {\alpha _c}} \right)}^2}} }} - \frac{{{k_u}{u_{rd}}{\Delta _\theta }}}{{\sqrt {\Delta _\theta ^2 + {{\left( {{z_e} - {\alpha _c}} \right)}^2}} }}\dot \alpha } \right) - \\\;\;\;\;\;\;\tilde \alpha {k_u}\left( {\frac{1}{{{k_3}}}\dot {\hat \alpha} + \frac{{{u_{rd}}{\Delta _\theta }}}{{\sqrt {\Delta _\theta ^2 + {{\left( {{z_e} - {\alpha _c}} \right)}^2}} }}{z_e}} \right) + \\\;\;\;\;\;\;{y_e}\left( { - \frac{{{k_y}{u_{rd}}\cos \theta _e^B\left( {{y_e} + {\beta _c}} \right)}}{{\sqrt {\Delta _\psi ^2 + {{\left( {{y_e} + {\beta _c}} \right)}^2}} }} + \frac{{{k_y}{u_{rd}}\cos \theta _e^B{\Delta _\psi }}}{{\sqrt {\Delta _\psi ^2 + {{\left( {{y_e} + {\beta _c}} \right)}^2}} }}\hat \beta } \right) + \\\;\;\;\;\;\;\tilde \beta {k_y}\left( {\frac{{{u_{rd}}\cos \theta _e^B{\Delta _\psi }}}{{\sqrt {\Delta _\psi ^2 + {{\left( {{y_e} + {\beta _c}} \right)}^2}} }}{y_e} - \frac{1}{{{k_4}}}\dot {\hat \beta} } \right).\end{array}$

设计控制律和自适应律为

$\left\{ \begin{array}{l}{\alpha _c} = {\Delta _\theta }\hat \alpha ;\\\dot {\hat \alpha} = - {k_\alpha }\frac{{{k_3}{u_{rd}}{\Delta _\theta }}}{{\sqrt {\Delta _\theta ^2 + {{\left( {{z_e} - {\alpha _c}} \right)}^2}} }}{z_e},\\\;\;\;\;\;\;\;{k_3} > 0,{k_\alpha } = \left\{ {\begin{array}{*{20}{c}}\begin{array}{l}0,\\1,\end{array}&\begin{array}{l}\left| {{z_e}} \right| > {\Delta _z},\\\left| {{z_e}} \right| \le {\Delta _z};\end{array}\end{array}} \right.\\{\beta _c} = {\Delta _\psi }\hat \beta ;\\\dot {\hat \beta} = {k_\beta }\frac{{{k_4}{u_{rd}}\cos \theta _e^B{\Delta _\psi }}}{{\sqrt {\Delta _\psi ^2 + {{\left( {{y_e} + {\beta _c}} \right)}^2}} }}{y_e},\\\;\;\;\;\;\;\;{k_4} > 0,{k_\beta } = \left\{ {\begin{array}{*{20}{c}}\begin{array}{l}0,\\1,\end{array}&\begin{array}{l}\left| {{y_e}} \right| > {\Delta _y},\\\left| {{y_e}} \right| \le {\Delta _y}.\end{array}\end{array}} \right.\end{array} \right.$

式中：D_z、Δ_y为边界层厚度，当AUV位于边界层以外时k_α=0, k_β=0，式(8)退化为LOS导引律，这样可以减少超调，当AUV位于边界层以内时启动积分项消除稳态误差.只要设置Δ_y>| y_ess|，Δ_z> |z_ess |就可以保证AUV进入边界层以内，最终可得

${{\dot V}_2} = - \frac{{{k_u}{u_{rd}}}}{{\sqrt {\Delta _\theta ^2 + {{\left( {{z_e} - {\alpha _c}} \right)}^2}} }}z_e^2 - \frac{{{k_y}{u_{rd}}\cos \theta _e^B}}{{\sqrt {\Delta _\psi ^2 + {{\left( {{y_e} + {\beta _c}} \right)}^2}} }}y_e^2 \le 0.$

3.2 动力学控制器的设计1) 镇定u_re.定义误差变量为

$\left\{ \begin{array}{l}{u_{re}} = {u_r} - {u_{rd}},\\\tilde \psi _e^B = \psi _e^B - \psi _{ed}^B,\\\tilde \theta _e^B = \theta _e^B - \theta _{ed}^B.\end{array} \right.$ (9)

因为u_rd为常值，根据式(3)、(9)可得

${{\dot u}_{re}} = {{\dot u}_r} - {{\dot u}_{rd}} = {F_u}{u_r} + {F_X}{X_\tau } + {d_u}X.$

定义${{\tilde d}_u} = {d_u} - {{\hat d}_u}, {{\hat d}_u}、{d_u}$的估计值.选取Lyapunov函数${V_3} = \frac{1}{2}u_{re}^2 + \frac{1}{{2{k_5}}}\tilde d_u^2$，对V₃求导得

${{\dot V}_3} = {u_{re}}\left( {{F_u}{u_r} + {F_X}{X_\tau } + {{\hat d}_u}} \right) + {{\tilde d}_u}\left( {{u_{re}} - \frac{1}{{{k_5}}}{{\dot {\hat d}}_u}} \right).$

设计控制律和自适应律为

$\left\{ \begin{array}{l}{X_\tau } = \frac{1}{{{F_X}}}\left[ { - {F_u}{u_r} - {{\hat d}_u} - {k_6}{u_{re}} - {\varepsilon _1}{\rm{sig}}\left( {{u_{re}}} \right)} \right],\\{\rm{sig}}\left( {{u_{re}}} \right) = \frac{{1 - {e^{ - {k_7}{u_{re}}}}}}{{1 + {e^{ - {k_7}{u_{re}}}}}} = {\mathop{\rm sgn}} \left( {{u_{re}}} \right)\left| {\frac{{1 - {e^{ - {k_7}{u_{re}}}}}}{{1 + {e^{ - {k_7}{u_{re}}}}}}} \right|,\\{{\dot {\hat d}}_u} = {k_5}{u_{re}},{k_5} > 0,{k_6} > 0,{k_7} > 0,{\varepsilon _1} > 0.\end{array} \right.$ (10)

可得${{\dot V}_3} = - {k_6}u_{re}^2 - {\varepsilon _1}\left| {{u_{re}}} \right|\left| {\frac{{1 - {e^{ - {k_7}{u_{re}}}}}}{{1 + {e^{ - {k_7}{u_{re}}}}}}} \right| \le 0$.式(10)中sig和sgn分别代表Sigmoid函数和符号函数，用Sigmoid函数设计滑模项，可以削弱抖振.

2) 镇定$\tilde \psi _e^B$.选取Lyapunov函数${V_4} = \frac{1}{2}{\left( {\tilde \psi _e^B\;} \right)^2}$，根据式(6)对V₄求导得

${{\dot V}_4} = \tilde \psi _e^B\dot {\tilde \psi} _e^B = \tilde \psi _e^B\left( { - {q_F}\psi _e^B\tan {\theta _F} + \frac{{\cos {\theta _F}}}{{\cos \theta }}r - {r_F} - \dot \psi _{ed}^B} \right).$

设计r的期望值为

${r_d} = \frac{{\cos \theta }}{{\cos {\theta _F}}}\left( {\dot \psi _{ed}^B + {q_F}\psi _e^B\tan {\theta _F} + {r_F} - {k_8}\tilde \psi _e^B} \right),{k_8} > 0,$

定义误差变量r_e=r－r_d，得${{\dot V}_4} = - {k_8}{\left( {\tilde \psi _e^B\;} \right)^2} + \frac{{\cos \;{\theta _F}}}{{\cos \;\theta }}\tilde \psi _e^B{r_e}$.根据式(3)对r_e求导可得${{\dot r}_e} = \dot r - {{\dot r}_d} = {F_r}r + {F_N}{N_\tau } + {d_r} - {{\dot r}_d}$，定义${{\tilde d}_r} = {d_r} - {{\hat d}_r}, {{\hat d}_r}$为d_r的估计值.选取Lyapunov函数${V_5} = {V_4} + \frac{1}{2}r_e^2 + \frac{1}{{2{k_9}}}\tilde d_r^2$，对V₅求导得

$\begin{array}{l}{{\dot V}_5} = - {k_8}{\left( {\tilde \psi _e^B} \right)^2} + \frac{{\cos {\theta _F}}}{{\cos \theta }}\tilde \psi _e^B{r_e} + \\\;\;\;\;\;{r_e}\left( {{F_r}r + {F_N}{N_\tau } + {{\hat d}_r} - {{\dot r}_d}} \right) + {{\tilde d}_r}\left( {{r_e} - \frac{1}{{{k_9}}}{{\dot {\hat d}}_r}} \right),\end{array}$

设计控制律和自适应律为

$\left\{ \begin{array}{l}{N_\tau } = \frac{1}{{{F_N}}}\left[ {{{\dot r}_d} - {F_r}r - \frac{{\cos {\theta _F}}}{{\cos \theta }}\tilde \psi _e^B - {{\hat d}_r} - {k_{10}}{r_e} - {\varepsilon _2}{\rm{sig}}\left( {{r_e}} \right)} \right],\\{\rm{sig}}\left( {{r_e}} \right) = \frac{{1 - {e^{ - {k_7}{r_e}}}}}{{1 + {e^{ - {k_7}{r_e}}}}} = {\mathop{\rm sgn}} \left( {{r_e}} \right)\left| {\frac{{1 - {e^{ - {k_7}{r_e}}}}}{{1 + {e^{ - {k_7}{r_e}}}}}} \right|,\\{{\dot {\hat d}}_r} = {k_9}{r_e},{k_9} > 0,{k_{10}} > 0,{\varepsilon _2} > 0.\end{array} \right.$ (11)

可得

${{\dot V}_5} = - {k_8}{\left( {\tilde \psi _e^B} \right)^2} - {k_{10}}r_e^2 - {\varepsilon _2}\left| {{r_e}} \right|\left| {\frac{{1 - {e^{ - {k_7}{r_e}}}}}{{1 + {e^{ - {k_7}{r_e}}}}}} \right| \le 0.$

3) 镇定$\tilde \theta _e^B$.选取Lyapunov函数${V_6} = \frac{1}{2}{\left( {\tilde \theta _e^B} \right)^2}$，根据式(6)对V₆求导得${{\dot V}_6} = \tilde \theta _e^B\;\;\dot {\tilde \theta} _e^B = \tilde \theta _e^B\left( {q - {q_F} - \dot \theta _{ed}^B} \right)$.设计q的期望值为${q_d} = \dot \theta _{ed}^B + {q_F} - {k_{11}}\tilde \theta _e^B, {k_{11}} > 0$，定义误差变量q_e=q－q_d，可得${{\dot V}_6} = - {k_{11}}{\left( {\tilde \theta _e^B} \right)^2} + \tilde \theta _e^B{q_e}$.

根据式(3)对q_e求导，可得

${{\dot q}_e} = \dot q - {{\dot q}_d} = {F_q}q = {F_M}{M_{{\rm{HS}}}} + {F_M}{M_\tau } + {d_q} - {{\dot q}_d},$

定义${{\tilde d}_q} = {d_q} - {{\hat d}_q}, {{\hat d}_q}$为d_q的估计值.选取Lyapunov函数${V_{\rm{7}}}{\rm{ = }}{V_6} + \frac{1}{2}q_e^2 + \frac{1}{{2{k_{12}}}}\tilde d_q^2x$，对V₇求导得

$\begin{array}{*{20}{c}}{{{\dot V}_7} = - {k_{11}}{{\left( {\tilde \theta _e^B} \right)}^2} + \tilde \theta _e^B{q_e} + {{\tilde d}_q}\left( {{q_e} - \frac{1}{{{k_{12}}}}{{\dot {\hat d}}_q}} \right) + }\\{{q_e}\left( {{F_q}q + {F_M}{M_{HS}} + {F_M}{M_\tau } + {{\hat d}_q} - {{\dot q}_d}} \right).}\end{array}$

设计控制律和自适应律为

$\left\{ \begin{array}{l}{M_\tau } = \frac{1}{{{F_M}}}\left[ {{{\dot q}_d} - {F_q}q - {F_M}{M_{HS}} - \tilde \theta _e^B - {{\hat d}_q} - {k_{13}}{q_e} - } \right.\\\;\;\;\;\;\;\;\;\left. {{\varepsilon _3}{\rm{sig}}\left( {{q_e}} \right)} \right],\\{\rm{sig}}\left( {{q_e}} \right) = \frac{{1 - {e^{ - {k_7}{r_e}}}}}{{1 + {e^{ - {k_7}{r_e}}}}} = {\mathop{\rm sgn}} \left( {{q_e}} \right)\left| {\frac{{1 - {e^{ - {k_7}{r_e}}}}}{{1 + {e^{ - {k_7}{r_e}}}}}} \right|,\\{{\dot {\hat d}}_q} = {k_{12}}{q_e},\;\;\;\;{k_{12}} > 0,{k_{13}} > 0,{\varepsilon _3} > 0.\end{array} \right.$ (12)

可得

${{\dot V}_7} = - {k_{11}}{\left( {\tilde \theta _e^B} \right)^2} - {k_{13}}q_e^2 - {\varepsilon _3}\left| {{q_e}} \right|\left| {\frac{{1 - {e^{ - {k_7}{r_e}}}}}{{1 + {e^{ - {k_7}{r_e}}}}}} \right| \le 0.$

控制律(10)~(12)分别得到的是螺旋桨和舵产生的推力和力矩，螺旋桨转速n，水平舵角δ_S，方向舵角δ_R的水动力方程计算公式为

$n = \sqrt {\frac{{{X_\tau }}}{{{K_{\rm{T}}}\rho D_{\rm{p}}^4}}} ,{\delta _S} = \frac{{{M_\tau }}}{{{x_{{\rm{fin}}}}{k_{\rm{L}}}}},{\delta _R} = \frac{{{N_\tau }}}{{{x_{{\rm{fin}}}}{k_{\rm{L}}}}}.$

式中ρ、K_T、D_P、K_L、x_fin分别为流体密度、螺旋桨推力系数、螺旋桨直径、舵升力系数、舵力臂.

3.3 闭环系统的稳定性分析在前面运动学控制器设计中假设u_r=u_rd, θ_e^B=θ_ed^B, ψ_e^B=ψ_ed^B，但由于系统动力学特性的影响，u_r、θ_e^B、ψ_e^B达到期望值需要响应时间.接下来对运动学和动力学组成的闭环系统的稳定性进行分析.定义误差变量$\mathit{\boldsymbol{e}} = \left[ {{{\tilde d}_x}\;\;\;\tilde \alpha \;\;\;\;\tilde \beta \;\;\;{u_{re}}\;\;\;\;\tilde \theta _e^B\;\;\;\;\tilde \psi _e^B} \right]$，根据式(7)~(9)，误差方程(5)可以重新整理为

${{\mathit{\boldsymbol{\dot P}}}_{e1}} = \mathit{\boldsymbol{A}}{\mathit{\boldsymbol{P}}_{e1}} + \mathit{\boldsymbol{Be}}.$ (13)

式中：

$\mathit{\boldsymbol{A}} = \left[ {\begin{array}{*{20}{c}}{{A_{11}}}&0&0\\0&{{A_{22}}}&0\\0&0&{{A_{33}}}\end{array}} \right],$

$\mathit{\boldsymbol{B}} = \left[ {\begin{array}{*{20}{c}}{{\mathit{\boldsymbol{B}}_1}}\\{{\mathit{\boldsymbol{B}}_2}}\\{{\mathit{\boldsymbol{B}}_3}}\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}{{B_{11}}}&0&0&0&0&0\\0&0&{{B_{23}}}&{{B_{24}}}&0&{{B_{26}}}\\0&{{B_{32}}}&0&{{B_{34}}}&{{B_{35}}}&0\end{array}} \right],$

${A_{11}} = - {k_2},{A_{22}} = - \frac{{{k_y}{u_{rd}}\cos \theta _e^B}}{{\sqrt {\Delta _\psi ^2 + {{\left( {{y_e} + {\beta _c}} \right)}^2}} }},$

${A_{33}} = - \frac{{{k_u}{u_{rd}}}}{{\sqrt {\Delta _\theta ^2 + {{\left( {{z_e} - {\alpha _c}} \right)}^2}} }},$

${B_{11}} = 1,{B_{23}} = \frac{{{k_y}{u_{rd}}\cos \theta _e^B{\Delta _\psi }}}{{\sqrt {\Delta _\psi ^2 + {{\left( {{y_e} + {\beta _c}} \right)}^2}} }},$

${B_{24}} = {k_y}\cos \theta _e^B\sin \psi _e^B + {k_y}\cos \theta _e^B\cos \psi _e^B\beta ,$

$\begin{array}{l}{B_{26}} = \left( \begin{array}{l}{k_y}{u_{rd}}\cos \theta _e^B\sin \psi _{ed}^B + \\\;\;\;\;\;\;\;{k_y}{u_{rd}}\beta \cos \theta _e^B\cos \psi _{ed}^B\end{array} \right)\frac{{\left( {\cos \tilde \psi _e^B - 1} \right)}}{{\tilde \psi _e^B}} + \\\;\;\;\;\;\;\;\;\left( {{k_y}{u_{rd}}\cos \theta _e^B\cos \psi _{ed}^B - {k_y}{u_{rd}}\cos \theta _e^B\sin \psi _{ed}^B} \right)\\\;\;\;\;\;\;\;\;\frac{{\sin \tilde \psi _e^B}}{{\tilde \psi _e^B}},{B_{32}} = - \frac{{{k_u}{u_{rd}}{\Delta _\theta }}}{{\sqrt {\Delta _\theta ^2 + {{\left( {{z_e} - {\alpha _c}} \right)}^2}} }},\end{array}$

${B_{34}} = - \left( {{k_u}\sin \theta _e^B + {k_u}\cos \theta _e^B\alpha } \right),$

$\begin{array}{l}{B_{35}} = \left( {{k_u}{u_{rd}}\alpha \sin \theta _{ed}^B - {k_u}{u_{rd}}\cos \theta _{ed}^B} \right)\frac{{\sin \tilde \theta _e^B}}{{\tilde \theta _e^B}} - \\\;\;\;\;\;\left( {{k_u}{u_{rd}}\sin \theta _{ed}^B + {k_u}{u_{rd}}\alpha \cos \theta _{ed}^B} \right)\frac{{\left( {\cos \tilde \theta _e^B - 1} \right)}}{{\tilde \theta _e^B}}.\end{array}$

可以认为系统(13)是名义系统${{\mathit{\boldsymbol{\dot P}}}_{e1}}$= AP_e1通过B受e的干扰组成的级联系统.因为矩阵A是负定的，所以名义系统${{\mathit{\boldsymbol{\dot P}}}_{e1}}$= AP_e1是稳定的，误差变量e的稳定性在前面的控制器设计中也得到证明.因为k_u、k_y、u_rd、α、β均是有界的，根据

${\left\| {{\mathit{\boldsymbol{B}}_1}} \right\|_1} = \left| {{B_{11}}} \right| = 1,$

$\begin{array}{l}{\left\| {{\mathit{\boldsymbol{B}}_2}} \right\|_1} = \left| {{B_{23}}} \right| + \left| {{B_{24}}} \right| + \left| {{B_{26}}} \right| \le {k_y}{u_{rd}} + \\\;\;\;\;\;\;\;\;\;\;{k_y}\left( {1 + 2{u_{rd}}} \right)\left( {1 + \left| \beta \right|} \right),\end{array}$

$\begin{array}{l}{\left\| {{\mathit{\boldsymbol{B}}_3}} \right\|_1} = \left| {{B_{32}}} \right| + \left| {{B_{34}}} \right| + \left| {{B_{35}}} \right| \le {k_u}{u_{rd}} + \\\;\;\;\;\;\;\;\;\;\;{k_u}\left( {1 + 2{u_{rd}}} \right)\left( {1 + \left| \alpha \right|} \right).\end{array}$

可知B也是有界的.所以根据非线性级联系统理论可得系统(13)是渐进稳定的.

4 仿真结果与分析本文采用美国Hydroid公司的REMUS-100作为仿真模型，具体参数可详见文献[15].期望速度u_rd=1 m·s^-1，海流速度v_c^I= [0.2 m·s^-1 0.2 m·s^-1 0 m·s^-1]^T.控制器的设计参数分别为k₁=0.1, k₂=0.2, k₃=0.02, k₄=0.05, k₅=0.1, k₆=1, k₇=25, k₈=0.1, k₉=0.1, k₁₀=1, k₁₂=0.1, k₁₂=0.1, k₁₃=1, Δ_θ=6, Δ_ψ=6, Δ_z=2, Δ_y=2, ε₁=0.3, ε₂=0.1, ε₃=0.1.期望路径是基于表 1所列的路径点生成，第一段路径为螺旋线，半径和螺距分别为20、40 m，其余路径由直线和圆弧组成，圆弧半径为10 m.仿真采用了两种控制方法进行对比，方法1采用本文提出的控制方法，方法2的运动学控制器采用文献[5]提出了ILOS导引律，动力学控制器采用PID控制器.为了验证控制器的鲁棒性，仿真时段0~120 s水动力参数相对于名义参数增加20%，120~240 s采用名义水动力参数，240~350 s水动力参数相对于名义参数减小20%. AUV的初始状态为0，期望路径的初始位置和方向为P_P(0)=[0 ??-20 ??5 ??-tan^-1(1/π) ?? 0]^T.

表 1
表 1 路径点 Tab. 1 Waypoints 路径点ξ/mη/mζ/m

10-205

20-2045

380-2045

480045

540045

6402045

7802045



表 1 路径点 Tab. 1 Waypoints


仿真结果见图 4~8，图 4为三维路径跟踪曲线和在水平面、垂直面的投影，可以清晰的看出在存在海流和模型不确定性的情况下，两种控制方法都能实现路径的良好跟踪，但通过对比也可看出方法1的超调量要小的多；图 5为跟踪误差曲线，通过图 5(a)、5(b)可以看出位置误差和姿态误差虽然在路径切换时会出现一定的波动，但很快能够稳定下来并全部趋于零. 图 5(c)、5(d)分别为纵倾角和艏摇角误差曲线，纵倾角和艏摇角误差在初始阶段会迅速增大，从而可以有效的减少位置误差，随着位置误差趋于稳定，姿态误差也趋于稳定，应该注意因为要对漂角进行补偿克服海流的影响，艏摇角误差并没有趋于零.

Fig. 4
图 4 路径跟踪曲线 Fig. 4 Curves of path-following


Fig. 5
图 5 路径跟踪误差曲线 Fig. 5 Error curves of path-following


Fig. 6
图 6 攻角和漂角曲线 Fig. 6 Curves of attack and sideslip angles


Fig. 7
图 7 控制输入曲线 Fig. 7 Curves of control inputs


Fig. 8
图 8 速度和速度误差曲线 Fig. 8 Curves of velocities and velocity errors


图 6为攻角、漂角和它们估计值曲线，能够看出自适应律可以较好的实现对攻角、漂角的估计. 图 7为控制输入曲线，可以看到在路径切换时控制输入会产生波动，原因是路径切换时路径曲率不连续，但方法1的控制输入更加平稳一些，抖振也比较小. 图 8为速度和速度误差曲线，通过图 8(a)可以看到AUV的相对速度能很好的收敛到期望值，AUV的绝对速度由于受海流的影响，顺流时会增大，逆流时会减小，虚拟向导的速度能很好的收敛到AUV的绝对速度.通过图 8(b)、8(c)可以看到，纵倾角速度和艏摇角速度都能收敛到期望值.通过图 8(d)~8(f)可以看到纵向速度误差、纵倾角速度误差和艏摇角速度误差均收敛到零.

5 结论1) 基于Serret-Frenet坐标系引入了虚拟向导并建立了三维路径跟踪误差模型.

2) 在运动学控制中设计了具有自适应律的虚拟向导并提出了一种改进的积分视线法(ILOS)导引律.

3) 基于反步自适应滑模控制(BASMC)理论设计了动力学控制器，并应用Sigmoid函数设计滑模项，保证了系统的稳定性和鲁棒性、有效的削弱了抖振.

4) 应用非线性级联理论证明了整个控制系统的闭环稳定性，从而解决了存在海流干扰和模型不确定性情况下欠驱动AUV的三维路径跟踪控制问题.

5) 控制器的设计是基于AUV与流体的相对速度，由于相对速度更便于测量，所以与基于绝对速度的控制方法相比，更便于工程应用.


参考文献
[1] 王芳, 万磊, 李晔, 等. 欠驱动AUV的运动控制技术综述[J]. 中国造船, 2010, 51(2): 227.
WANG Fang, WAN Lei, LI Ye, et al. A survey on development of motion control for underactuated AUV[J]. Shipbuilding of China, 2010, 51(2): 227. DOI:10.3969/j.issn.1000-4882.2010.02.030


[2] LEKKAS A M, FOSSEN T I. A time-varying lookahead distance guidance law for path following[J]. IFAC Proceedings Volumes, 2012, 45(27): 398. DOI:10.3182/20120919-3-IT-2046.00068


[3] XU H, SOARES C G. Vector field path following for surface marine vessel and parameter identification based on LS-SVM[J]. Ocean Engineering, 2016, 113: 151. DOI:10.1016/j.oceaneng.2015.12.037


[4] LAPIERRE L, SOETANTO D. Nonlinear path-following control of an AUV[J]. Ocean Engineering, 2007, 34(11): 1734.


[5] CAHARIJA W, PETTERSEN K Y, BIBULI M, et al. Integral line-of-sight guidance and control of underactuated marine vehicles: theory, simulations, and experiments[J]. IEEE Transactions on Control Systems Technology, 2016, 24(5): 1623. DOI:10.1109/TCST.2015.2504838


[6] FOSSEN T I, PETTERSEN K Y, GALEAZZI R. Line-of-sight path following for Dubins paths with adaptive sideslip compensation of drift forces[J]. IEEE Transactions on Control Systems Technology, 2015, 23(2): 820. DOI:10.1109/TCST.2014.2338354


[7] CAHARIJA W, PETTERSEN K Y, S?RENSEN A J, et al. Relative velocity control and integral line of sight for path following of autonomous surface vessels: merging intuition with theory[J]. Proceedings of the Institution of Mechanical Engineers, 2014, 228(2): 180. DOI:10.1177/0954407013502951


[8] 李岳明, 万磊, 孙玉山, 等. 水下机器人高度信息融合与欠驱动地形跟踪控制[J]. 控制理论与应用, 2013, 30(1): 118.
LI Yueming, WAN Lei, SUN Yushan, et al. Altitude information fusion and bottom-following control for underactuated autonomous underwater vehicle[J]. Control Theory & Applications, 2013, 30(1): 118.


[9] FOSSEN T I, LEKKAS A M. Direct and indirect adaptive integral line-of-sight path-following controllers for marine craft exposed to ocean currents[J]. International Journal of Adaptive Control and Signal Processing, 2017, 31(4): 445. DOI:10.1002/acs.2550


[10] 边信黔, 程相勤, 贾鹤鸣, 等. 基于迭代滑模增量反馈的欠驱动AUV地形跟踪控制[J]. 控制与决策, 2011, 26(2): 289.
BIAN Xinqian, CHENG Xiangqin, JIA Heming, et al. A bottom-following controller for underactuated AUV based on iterative sliding and increment feedback[J]. Control and Decision, 2011, 26(2): 289.


[11] LI Ye, WEI Cong, WU Qi, et al. Study of 3 dimension trajectory tracking of underactuated autonomous underwater vehicle[J]. Ocean Engineering, 2015, 105: 270. DOI:10.1016/j.oceaneng.2015.06.034


[12] 葛晖, 敬忠良, 高剑. 自主式水下航行器三维路径跟踪的神经网络H∞鲁棒自适应控制方法[J]. 控制理论与应用, 2012, 29(3): 317.
GE Hui, JING Zhongliang, GAO Jian. Neural network H-infinity robust adaptive control for autonomous underwater vehicle in 3-dimensional path following[J]. Control Theory & Applications, 2012, 29(3): 317.


[13] 王宏健, 陈子印, 贾鹤鸣, 等. 基于滤波反步法的欠驱动AUV三维路径跟踪控制[J]. 自动化学报, 2015, 41(3): 631.
WANG Hongjian, CHEN Ziyin, JIA Heming, et al. Three-dimensional path-following control of underactuated autonomous underwater vehicle with command filtered back-stepping[J]. Acta Automatica Sinica, 2015, 41(3): 631.


[14] 李娟, 边信黔, 熊华胜, 等. AUV的精确航迹跟踪系统的鲁棒控制[J]. 哈尔滨工业大学学报, 2013, 45(1): 112.
LI Juan, BIAN Xinqian, XIONG Huasheng, et al. Robust control research for AUV trajectory control system[J]. Journal of Harbin Institute of Technology, 2013, 45(1): 112.


[15] PRESTERO T. Verification of a six-degree of freedom simulation model for the REMUS autonomous underwater vehicle[D]. Cambridge: Massachusetts Institute of Technology, 2001 https://dspace.mit.edu/handle/1721.1/65068