﻿ 水下航行器三维航迹反演滑模跟踪控制
 舰船科学技术  2019, Vol. 41 Issue (1): 66-70 PDF

1. 三峡大学 水电机械设备设计与维护湖北省重点实验室，湖北 宜昌 443002;
2. 中国船舶重工集团有限公司第七一〇研究所，湖北 宜昌 443003

3D trajectory-tracking control of autonomous underwater vehicles based on backstepping and sliding mode method
SUN Qiao-mei1, CHEN Jin-guo2, YU Wan1
1. Hubei Key Laboratory of Hydroelectric Machinery Design and Maintenance, China Three Gorges University, Yichang 443002, China;
2. The 710 Research Institute of CSIC, Yichang 443003, China
Abstract: A control algorithm is proposed for Autonomous Underwater Vehicles three-dimensional trajectory-tracking in this paper. The AUV path tracking controller is designed through the backstepping and variable structure sliding mode control law, considering the movement of AUV in three-dimensional space. The stability of the control system is analyzed using Lyapunov stability theory. As for environment disturbances, the proposed approach can achieve stability on three axes at the same time with smooth continuous outputs and the ability of restraining interference is enhanced. The simulation results show the effectiveness of the proposed control law.
Key words: trajectory tracking     backstepping     sliding mode control
0 引　言

AUV轨迹跟踪控制的目标是设计有效的控制律，使其从初始状态跟踪参考轨迹，并保证跟踪位置误差的全局一致渐进稳定[45]。目前研究成果采用的控制方法主要有传统PID控制方法、滑模控制方法、反演控制方法、神经网络法。由于传统PID参数需要适应模型参数的变化，而AUV动力学模型参数存在不确定性，因此很难满足需要。胡志强等[6]提出了USV系统航向在线自优化 PID（比例、积分、微分）控制算法。

1 AUV数学模型

 $\left\{ \begin{array}{l} {\dot{ \eta }} = {{J}}\left( {{\eta }} \right){{\nu }},\\ {{M\dot \nu }} + {{C}}\left( {{\nu }} \right){{\nu }} + {{D}}\left( {{\nu }} \right) + {{g}}\left( {{\eta }} \right) = {{\tau }} + {{d}}{\text{。}} \end{array} \right.$ (1)

 ${{{M}}^*}\left( {{\eta }} \right){\ddot{ \eta }} + {{{C}}^*}\left( {{{\nu }}, {{\eta }}} \right){\dot{ \eta }} + {{{D}}^*}\left( {{{\nu }}, {{\eta }}} \right){\dot{ \eta }} + {{{g}}^*}\left( {{\eta }} \right) = {{{J}}^{ - {\rm T}}}\left( {{\eta }} \right){{\tau }},$ (2)

 ${{{M}}^*}\left( {{\eta }} \right) = {{{J}}^{ - {\rm T}}}\left( {{\eta }} \right){{M}}{{{J}}^{ - 1}}\left( {{\eta }} \right),$
 ${{{C}}^*}\left( {{{\nu }}, {{\eta }}} \right) = {{{J}}^{ - {\rm T}}}\left( {{\eta }} \right)\left[{{{C}}\left( {{\nu }} \right) + {{M}}{{{J}}^{-1}}\left( {{\eta }} \right){\dot{ J}}\left( {{\eta }} \right)} \right]{{{J}}^{ - 1}}\left( {{\eta }} \right),$
 ${{{g}}^*}\left( {{\eta }} \right) = {{{J}}^{ - {\rm T}}}\left( {{\eta }} \right){{g}}\left( {{\eta }} \right),$
 ${{{D}}^*}\left( {{{\nu }}, {{\eta }}} \right) = {{{J}}^{ - {\rm T}}}\left( {{\eta }} \right){{D}}\left( {{\nu }} \right){{{J}}^{ - 1}}\left( {{\eta }} \right){\text{。}}$
2 轨迹跟踪控制器设计

1）反演控制律

 ${{{V}}_1} = \frac{1}{2}{{e}}_1^{\rm T}{{{e}}_1},$ (3)

 ${{\dot{ V}}_1} = {{e}}_1^{\rm T}{{\dot{ e}}_1} = {{e}}_1^{\rm T}\left( {{\dot{ \eta }} - {{{\dot{ \eta }}}_r}} \right){\text{。}}$ (4)

 ${\dot V_1} = {{e}}_1^{\rm T}\left( { - {{{\varsigma }}_{\text{1}}}{{{e}}_1} + {{{e}}_2}} \right) = - {{e}}_1^{\rm T}{\varsigma _1}{{{e}}_1} + {{e}}_1^{\rm T}{{{e}}_2}{\text{。}}$ (5)

 $\begin{split} {{{\dot{ e}}}_2} & = {\ddot{ \eta }} + {{{\varsigma }}_{\rm{1}}}{{{\dot{ e}}}_1} - {{{\ddot{ \eta }}}_r}=\\ & {{{M}}^{* - 1}}[{{{J}}^{ - {\rm T}}}\left( {{{\tau }} + {{d}}} \right) - {{{C}}^*}{\dot{ \eta }}-\\ & {{{D}}^*}{\dot{ \eta }} - {{{g}}^*}]\; - {{{\varsigma }}_{\rm{1}}}{{{\dot{ e}}}_1} - {{{\ddot{ \eta }}}_r}{\text{。}} \end{split}$ (6)

2）滑模项的推导

 ${{{V}}_2} = {{{V}}_1} + \frac{1}{2}{{{e}}_2}^{\rm T}{{{\varsigma }}_2}{{{M}}^*}{{{e}}_2},$ (7)

 $\begin{split} {{{\dot{ V}}}_2} =& {{{\dot{ V}}}_1} + {{{e}}_2}^{\rm T}{{{\varsigma }}_2}{{{M}}^*}{{{\dot{ e}}}_2} = - {{e}}_1^{\rm T}{\varsigma _1}{{{e}}_1} + {{e}}_1^{\rm T}{{{e}}_2}+\\ & {{{e}}_2}^{\rm T}{{{\varsigma }}_2}\left[ {{{J}}^{ - {\rm T}}}\left( {{{\tau }} + {{d}}} \right) - {{{C}}^*}{\dot{ \eta }} - {{{D}}^*}{\dot{ \eta }} \right.-\\ & \left. {{{g}}^*} - {{{M}}^*}{{{\varsigma }}_{\rm{1}}}{{{\dot{ e}}}_1} - {{{M}}^*}{{{\ddot{ \eta }}}_r} \right]{\text{。}} \end{split}$ (8)

 $\begin{split} {{\tau }} =& {{J}}[{{{C}}^*}{\dot{ \eta }} + {{{D}}^*}{\dot{ \eta }} + {{{g}}^*} + {{{M}}^*}{{{\varsigma }}_{\rm{1}}}{{{\dot{ e}}}_1}+\\ & {{{M}}^*}{{{\ddot{ \eta }}}_r} - {{{e}}_2} - {{{\varsigma }}_2}^{ - 1}{{{e}}_1} - \lambda {\mathop{\rm sgn}} {\left( {{{e}}_2^{\rm T}{{{\varsigma }}_2}} \right)^{\rm T}}]\text{，} \end{split}$ (9)

 $\begin{split} &{{{\dot{ V}}}_2} = - {{e}}_1^{\rm T}\varsigma {{{e}}_1} - {{{e}}_2}^{\rm T}{{{\varsigma }}_2}{{{e}}_2} - {{{e}}_2}^{\rm T}{{{\varsigma }}_2}\left[ {\lambda {\mathop{\rm sgn}} {{\left( {{{e}}_2^{\rm T}{{{\varsigma }}_2}} \right)}^{\rm T}} - }\right.\\ &\leqslant \left.{{{J}}^{ - {\rm T}}}{{d}} \right]{{e}}_1^{\rm T}\varsigma {{{e}}_1} - {{{e}}_2}^{\rm T}{{{\varsigma }}_2}{{{e}}_2} - \left| {{{{e}}_2}^{\rm T}{{{\varsigma }}_2}} \right|\left( {\lambda - \left| {{{{J}}^{ - {\rm T}}}{{d}}} \right|} \right){\text{。}}\;\;\;\; \end{split}$ (10)

3 仿真验证

AUV主要参数如表1所示。

 $\begin{split} &{{d}}\left( t \right) = [10\sin \left( {0.1t - {\text{π}} /4} \right),12\sin \left( {0.1t - {\text{π}} /6} \right),\\ &5\sin \left( {0.1t \!\!-\!\! {\text{π}} /3} \right),\;5\sin \left( {0.1t \!\!-\!\! {\text{π}} /6} \right),5\sin \left( {0.1t \!\!-\!\! {\text{π}} /4} \right)]{\text{。}} \end{split}$ (11)

1）给定期望航迹为一光滑三维航迹：

 $\begin{split} &\left( {x,y,z} \right) = (\sin (0.02{\text{π}} t),\cos (0.01{\text{π}} t),\;\\ & \sin (0.01{\text{π}} t) + 2\cos (0.01{\text{π}} t)){\text{。}} \end{split}$ (12)

 图 1 反演滑模控制AUV的xyz轴航迹跟踪曲线 Fig. 1 AUV trajectory tracking based on backstepping SMC

 图 2 PID控制AUV的xyz轴航迹跟踪曲线 Fig. 2 AUV trajectory tracking based on PID

 图 3 AUV空间航迹跟踪曲线 Fig. 3 AUV main parameters

 图 4 xyz轴航迹跟踪误差曲线 Fig. 4 AUV 3D Trajectory

2）设定AUV航迹制导器生成的期望航迹点为

 $\left( {{x_i}, {y_i}, {z_i}} \right) = \left( {R \times \cos \left( {i \times 15^\circ } \right), R \times \sin \left( {i \times 15^\circ } \right), \left( {i + 1} \right) \times d} \right),$

 图 5 反演滑模控制AUV的xyz轴航迹跟踪曲线 Fig. 5 AUV trajectory tracking based on backstepping SMC

 图 6 PID控制AUV的xyz轴航迹跟踪曲线 Fig. 6 AUV trajectory tracking based on PID

 图 7 AUV空间航迹跟踪曲线 Fig. 7 AUV 3D helix trajectory

 图 8 xyz轴航迹误差曲线 Fig. 8 Tracking control error
4 结　语

 [1] 张利军, 齐雪, 赵杰梅, 等. 垂直面欠驱动自治水下机器人定深问题的自适应输出反馈控制[J]. 控制理论与应用, 2012, 29(10): 1371-1376. [2] 俞建成, 张艾群, 王晓辉, 等. 基于模糊神经网络水下机器人直接自适应控制[J]. 自动化学报, 2007, 33(8): 840-846. [3] LEE K W, KEUM W, SINGH S N, et al. Multi input submarine control via L1 adaptive feedback despite uncertainties[J]. Journal of System and Control Engineers, 2014, 228(5): 330-347. [4] 朱大奇, 杨蕊蕊. 生物启发神经动力学模型的自治水下机器人反演跟踪控制[J]. 控制理论与应用, 2012, 29(10): 1309-1316. [5] 周超, 曹志强, 王硕. 微小型仿生机器鱼设计与实时路径规划[J]. 自动化学报, 2008, 34(7): 772-777. [6] 胡志强, 周焕银, 林扬, 等. 基于在线自优化PID算法的USV系统航向控制[J]. 机器人, 2013, 35(3): 263-268. [7] BAGHERI A, MOGHADDAM J J. Simulation and tracking control based on neural-network strategy and sliding-mode control for underwater remotely operated vehicle[J]. Neurocomputing, 2009, 72(8): 1934-1950. [8] WALLACE M B, MAX S D, EDWIN K. Depth control of remotely operated underwater vehicles using an adaptive fuzzy sliding mode controller[J]. Robotics and Autonomous Systems, 2008, 56(8): 670-677. DOI:10.1016/j.robot.2007.11.004 [9] 魏延辉, 周卫祥, 贾献强, 等. AUV模型解耦水平运动多控制器联合控制[J]. 华中科技大学学报(自然科学版), 2016, 44(4): 37-42. [10] CONTE G, DE CAPUA G P, SCARADOZZI D. Designing the NGC system of a small ASV for tracking underwater targets[J]. Robotics and Autonomous Systems, 76 (2016) 46–57. [11] ZOOL H I, MOHDB M M, VINA W E, et al. A robust dynamic region- based control scheme for an autonomous underwater vehicle [J]. Ocean Engineering, 111 (2016) 155–165. [12] GAO Fu-dong, PAN Cun-yun, HAN Yan-yan, et al. Nonliear traiectory tracking control of a new autonomous underwater vehicle in complex sea conditions[J]. Journal of Central South University, 2012, 19(7): 1859-1868. DOI:10.1007/s11771-012-1220-1 [13] HNAGIL J, MINSUNG K, SON CHEOL Y. Second order sliding mode controller for autonomous underwater vehicle in the presence of unknown disturbances[J]. Nonlinear Dynamics, 2014, 78(1): 183-196. DOI:10.1007/s11071-014-1431-0 [14] 贾鹤鸣, 张利军, 齐 雪, 等. 基于神经网络的水下机器人三维航迹跟踪控制[J]. 控制理论与应用, 2012, 29(7): 877-883. [15] LIU Yan-cheng, LIU Si-yuan, WANG Ning. Fully-tuned fuzzy neural network based robust adaptive tracking control of unmanned underwater vehicle with thruster dynamics[J]. Neurocomputing, 2016, 196: 1–13. [16] JON E R, ASGEIR J S. Model-based output feedback control of slender-body underactuated AUVs: theory and experiments[J]. IEEE Transactions on Control Systems Technology, 2008, 16(5): 930-946. DOI:10.1109/TCST.2007.916347 [17] LIONEL L, BRUNO J. Robust nonlinear path-following control of AUV[J]. IEEE Journal of Oceanic Engineering, 2008, 33(2): 89-102. DOI:10.1109/JOE.2008.923554