﻿ 时变干扰下AUV三维轨迹跟踪反步滑模控制
 舰船科学技术  2022, Vol. 44 Issue (7): 82-87    DOI: 10.3404/j.issn.1672-7649.2022.07.016 PDF

1. 河北工业大学 机械工程学院，天津 300131;
2. 天津瀚海蓝帆海洋科技有限公司，天津 300457

Backstepping sliding mode control of auv three-dimensional trajectories tracking under time-varying interference
WU Jian-guo1, LIU Jie1, CHEN Kai2
1. School of Mechanical Engineering, Hebei University of Technology, Tianjin 300131, China;
2. Tianjin Hanhai Lanfan Marine Technology Co., Ltd., Tianjin 300457, China
Abstract: Aiming at the problem of three-dimensional tracking of autonomous underwater vehicles under environmental interference, a backstepping sliding mode controller based on nonlinear interference observer is designed. Construct a six degrees of freedom mathematical model based on the characteristics of the Yunfan AUV. NDO is designed to compensate the environmental interference. Finally, sliding mode control is introduced on the basis of backstepping method, and NDO is added to design backstepping sliding mode controller, and the stability of the system is proved by Lyapunov function. The simulation results show that the backstepping sliding mode controller based on NDO can meet the requirements of 3D trajectory tracking of AUV under the environmental disturbance, and has good robustness.
Key words: autonomous underwater vehicle     trajectory tracking     nonlinear disturbance observer     backstepping sliding mode control
0 引　言

AUV作为开发海洋资源的重要工具，具有机动性好、稳定性强、精确度高等一系列优点，已经在环境监测、资源勘探和水下救援等任务中发挥了重要作用[1]。但由于结构的限制，依靠尾部单推进器和十字舵来控制的小型欠驱动AUV，各自由度之间存在较强的耦合性，加之体积小，易受海浪、洋流的干扰。因此小型欠驱动AUV面对外界干扰和本身的局限性，如何进行精准的控制是水下机器人研究的主要方向之一。

1 AUV运动建模

 图 1 AUV的惯性坐标系与载体坐标系示意图 Fig. 1 Schematic diagram of AUV inertial coordinate system and carrier coordinate system

AUV的数学模型可表示为：

 $\left\{ {\begin{array}{*{20}{l}} {\dot \eta {\text{ = }}R\left( \eta \right)v} ，\\ {M\dot v + C\left( v \right)v + D\left( v \right)v + g\left( \eta \right) = \tau + {\tau _d}} 。\end{array}} \right.$ (1)

 $\left\{ {\begin{array}{*{20}{l}} {\dot x = u\cos \psi \cos \theta - v\sin \psi + w\sin \theta \cos \psi } ，\\ {\dot y = u\sin \psi \cos \theta + v\cos \psi + w\sin \theta \sin \psi } ，\\ {\dot z = - u\sin \theta + w\cos \theta } ，\\ {\dot \theta = q} ，\\ {\dot \psi = r\sec \theta } 。\end{array}} \right.$ (2)

AUV的动力学模型可表示为：

 $\left\{ {\begin{array}{*{20}{l}} {{m_{11}}\dot u = {m_{22}}vr - {m_{33}}wq - {X_u}u - {X_{u\left| u \right|}}u\left| u \right| + {\tau _u} + {\tau _{d1}}} ，\\ {{m_{22}}\dot v = - m{}_{11}ur - {Y_v}v - {Y_{v\left| v \right|}}v\left| v \right| + {\tau _{d2}}} ，\\ {{m_{33}}\dot w = {m_{11}}uq - {Z_w}w - {Z_{w\left| w \right|}}w\left| w \right| + {\tau _{d3}}} ，\\ \begin{gathered} {m_{55}}\dot q = \left( {{m_{33}} - {m_{11}}} \right)uw - {M_q}q - {M_{q\left| q \right|}}q\left| q \right| + {\tau _q} + {\tau _{d5}} ，\hfill \\ \quad \quad \;\; - \overline {B{G_z}} W\sin \theta ，\hfill \\ \end{gathered} \\ {{m_{66}}\dot r = \left( {{m_{11}} - {m_{22}}} \right)uv - {N_r}r - {N_{r\left| r \right|}}r\left| r \right| + {\tau _r} + {\tau _{d6}}} 。\end{array}} \right.$ (3)

2 反步滑模控制

2.1 NDO的设计

 $\left\{ {\begin{array}{*{20}{l}} {d = z + p(v)}，\\ \begin{gathered} \dot z = - Lz - L(p(v) - (C\left( v \right) + D\left( v \right))v + \tau - g(\eta )) 。\end{gathered} \end{array}} \right.$ (4)

 $L = \frac{{\partial p(v)}}{{\partial v}} = {\rm{diag}}({l_1},{l_2},{l_3},{l_4},{l_5},{l_6})。$ (5)

 $p(v) = LMv ，$ (6)

 $\dot {\tilde d} = {\dot \tau _d} - \dot d = {\dot \tau _d} - L\tilde d 。$ (7)

 $\dot{ \tilde d} = {\dot \tau _d} - \dot d = - L\tilde d ，$ (8)

 $\dot V = \dot{ \tilde d}\tilde d = - L{\tilde d^2} \leqslant 0 ，$ (9)

 $\tilde d\left( t \right) = \tilde d\left( 0 \right){e^{ - Lt}} ，$ (10)

 ${u_d} = Md。$ (11)
2.2 传统反步法控制

 $\left\{ {\begin{array}{*{20}{l}} {\dot \eta = R\left( \eta \right)v} ，\\ {M\dot v = \tau - \left( {C\left( v \right) + D\left( v \right)} \right)v - g\left( \eta \right)} 。\end{array}} \right.$ (12)

 ${\dot V_1} = {e_1}{\dot e_1} = {e_1}\left( {\dot \eta - {{\dot \eta }_d}} \right) = {e_1}\left( {R\left( \eta \right)v - {{\dot \eta }_d}} \right)，$ (13)

 ${\dot V_1} = {e_1}\left( {{e_2} + {{\dot \eta }_d} - k{e_1} - {{\dot \eta }_d}} \right) = {e_1}{e_2} - {k_1}e_1^2，$ (14)

 $\begin{gathered} {{\dot V}_2} = {{\dot V}_1} + {e_2}{{\dot e}_2} =\hfill \\ \quad \;\; {e_1}{e_2} - {k_1}e_1^2 + {e_2}(\dot R\left( \eta \right)v + R\left( \eta \right){M^{ - 1}}\tau - \hfill \\ \quad \;\; R\left( \eta \right){M^{ - 1}}\left( {\left( {C\left( v \right) + D\left( v \right)} \right)v + g\left( \eta \right)} \right) + {k_1}{{\dot e}_1} - {{\ddot \eta }_d}) 。\end{gathered}$ (15)

 $\begin{split} \tau =& \left( {C\left( v \right) + D\left( v \right)} \right)v + g\left( \eta \right) + \\ & MR{\left( \eta \right)^{ - 1}}\left( { - {k_1}{{\dot e}_1} + {{\ddot \eta }_d} - {k_2}{e_2} - {e_1} - \dot R\left( \eta \right)v} \right) ，\end{split}$ (16)

 ${\dot V_2} = - {k_1}e_1^2 - {k_2}e_2^2 \leqslant 0。$ (17)

${\dot V_2}$ 是负半定的，因此，使用式(16)可以使非线性系统(12)渐进稳定。

2.3 反步滑模控制

 $\left\{ {\begin{array}{*{20}{l}} {\dot \eta = R\left( \eta \right)v} ，\\ {M\dot v = \tau - \left( {C\left( v \right) + D\left( v \right)} \right)v - g\left( \eta \right) + \zeta } 。\end{array}} \right.$ (18)

 ${\dot V_3} = {e_3}{\dot e_3} = {e_3}\left( {R\left( \eta \right)v - {{\dot \eta }_d}} \right) ，$ (19)

 $\dot s = \dot R\left( \eta \right)v + R\left( \eta \right)\dot v - {\ddot \eta _d} + c{\dot e_3} ，$ (20)

 ${V_4} = {V_3} + \frac{1}{2}{s^2} + \frac{1}{2}{\zeta ^2}，$ (21)

 $\begin{split} &{{\dot V}_4} = {{\dot V}_3} + s\dot s + \zeta \dot \zeta = \hfill \\ & \quad s\left( {\dot R\left( \eta \right)v + R\left( \eta \right)\dot v - {{\ddot \eta }_d} + c{{\dot e}_3}} \right) - L{{\tilde d}^2} +\hfill \\ & \quad \;\; {e_3}\left( {R\left( \eta \right)v - {{\dot \eta }_d}} \right)= \hfill \\ & \quad s\left( {R\left( \eta \right){M^{ - 1}}\left( {\tau - C\left( v \right)v - D\left( v \right)v - g\left( \eta \right) + \zeta } \right)} \right. - \hfill \\ & \quad \;\;\left. { {{\ddot \eta }_d} + c{{\dot e}_3} + \dot R\left( \eta \right)v} \right) - L{{\tilde d}^2} + {e_3}\left( {R\left( \eta \right)v - {{\dot \eta }_d}} \right) = \hfill \\ & \quad s\left( {R\left( \eta \right){M^{ - 1}}\left( {\tau - C\left( v \right)v - D\left( v \right)v - g\left( \eta \right) + \zeta } \right)} \right.- \hfill \\ & \quad \;\;\left. { {{\ddot \eta }_d} + c{{\dot e}_3} + \dot R\left( \eta \right)v} \right) - L{{\tilde d}^2} + {e_3}\left( {s - c{e_3}} \right)，\end{split}$ (22)

 $\begin{split} \tau =& MR{\left( \eta \right)^{ - 1}}\left( { - c{{\dot e}_3} + {{\ddot \eta }_d} - {k_3}s - {e_3} - \dot R\left( \eta \right)v} \right) + \\ & \left( {C\left( v \right) + D\left( v \right)} \right)v - {\rm{a}}{{\rm{sgn}}} (s) + g\left( \eta \right) - \zeta ，\end{split}$ (23)

 ${\dot V_4} = - L{\tilde d^2} - ce_3^2 - {k_3}{s^2} - {\rm{a}}{{\rm{sgn}}} (s) \leqslant 0 。$ (24)

${\dot V_4}$ 是负半定的，因此，使用式(23)可以使非线性系统(18)渐进稳定。

3 仿真结果与分析

 图 2 期望轨迹和有无NDO实际轨迹 Fig. 2 Expected trajectory and actual trajectory with and without NDO

 图 3 X轴方向干扰力 Fig. 3 X-axis interference force

 图 4 Y轴方向干扰力 Fig. 4 Y-axis interference force

 图 5 Z轴方向干扰力 Fig. 5 Z-axis interference force

 图 6 坐标轴方向跟踪误差 Fig. 6 Tracking error of coordinate axis direction

 图 7 期望轨迹与实际轨迹 Fig. 7 Expected trajectory and actual trajectory

 图 8 期望与反步滑模控制垂直面轨迹 Fig. 8 Expectation and backstepping sliding mode control of vertical plane trajectory

 图 9 期望与PID控制垂直面轨迹 Fig. 9 Expectation and PID control of vertical plane trajectory

 图 10 期望与反步滑模控制水平面轨迹 Fig. 10 Expectation and backstepping sliding mode control of horizontal plane trajectory

 图 11 期望与PID控制水平面轨迹 Fig. 11 Expectation and PID control of horizontal plane trajectory

 图 12 坐标轴方向跟踪误差 Fig. 12 Tracking error of coordinate axis direction

4 结　语

 [1] 黄琰, 李岩, 俞建成, 等. AUV智能化现状与发展趋势[J]. 机器人, 2020, 42(2): 215-231. HUANG Y, LI Y, YU J C, et al. Status and development trend ofAUV intelligence[J]. Robot, 2020, 42(2): 215-231. [2] 刘丽萍, 王红燕. 基于海流观测的欠驱动AUV自适应反演滑模轨迹跟踪[J]. 天津大学学报(自然科学与工程技术版), 2020, 53(7): 745-753. LIU L P, WANG H Y. Adaptive inverse sliding mode trajectory tracking of underactuated AUV based on ocean current observation[J]. Journal of Tianjin University (natural science and engineering), 2020, 53(7): 745-753. [3] 杨泽文, 贾鹤鸣, 宋文龙, 等. 欠驱动AUV水平面轨迹跟踪的反步控制研究[J]. 机电工程, 2017, 34(11): 1338-1342. DOI:10.3969/j.issn.1001-4551.2017.11.022 [4] 严浙平, 杨泽文, 贾鹤鸣, 等. 时变干扰下欠驱动AUV水平面轨迹跟踪的反步滑模控制[J]. 宇航总体技术, 2017, 1(4): 1-7. YAN Z P, YANG Z W, JIA H M, et al. Backstepping sliding mode co-ntrol for horizontal trajectory tracking of underactuated AUV under time-varying interference[J]. Astronautical General Technique, 2017, 1(4): 1-7. [5] 钟雨轩, 翁磊, 梁旭. 非完全对称欠驱动无人艇的自适应滑模轨迹跟踪控制[J]. 舰船科学技术, 2020, 42(17): 92-98. ZHONG Y X, WENG L, LIANG X. Adaptive sliding mode trajectory tracking control of a nonsymmetric underactuated unmanned ship[J]. Ship Science and Technology, 2020, 42(17): 92-98. [6] RAMEZANI-AL M R, SERESHKI Z T. A novel adaptive sliding mode controller design for tracking problem of an AUV in the horizontal plane[J]. International Journal of Dynamics and Control, 2019, 7(2): 679-689. DOI:10.1007/s40435-018-0457-4 [7] 王洪斌, 苏博, 王跃灵, 等. 基于粒子群优化的AUV定深跟踪有限时间控制[C]// 第37届中国控制会议论文集, 2018. WANG H B, SU B, WANG Y L, et al. AUV fixed-depth tracking finit-e time control based on particle swarm optimization[C]//Proceedingsof the 37th Chinese Control Conference, 2018. [8] 林荣鹏, 贾鹤鸣, 吴慧, 等. 基于反步法的AUV深度控制研究[J]. 科技创新与生产力, 2018, 4(5): 48-51. LIN R P, JIA H M, WU H, et al. Research on AUV depth control b-ased on backstepping method[J]. Science and Technology Innovationand Productivity, 2018, 4(5): 48-51. DOI:10.3969/j.issn.1674-9146.2018.05.048 [9] 饶志荣, 董绍江, 王军, 等. 基于干扰观测器的AUV深度自适应终端滑模控制[J]. 北京化工大学学报(自然科学版), 2021, 48(1): 103-110. [10] ZHANG G C, HUANG H, QIN H D, et al. A novel adaptive second o-rder sliding mode path following control for a portable AUV[J]. Ocean Engineering, 2018, 151(151): 82-92. [11] 王金强, 王聪, 魏英杰, 等. 欠驱动AUV自适应神经网络反步滑模跟踪控制[J]. 华中科技大学学报(自然科学版), 2019, 47(12): 12-17. [12] 徐健, 汪慢, 乔磊, 等. 欠驱动UUV三维轨迹跟踪的反步动态滑模控制[J]. 华中科技大学学报(自然科学版), 2015, 43(8): 107-113. XU J, WANG M, QIAO L, et al. Backstepping dynamic sliding mode control for underactuated UUV 3D trajectory tracking[J]. Journal of Huazhong University of Science and Technology (Natural Science Edition), 2015, 43(8): 107-113. [13] 周丽, 姜长生, 都延丽. 一种基于反步法的鲁棒自适应终端滑模控制[J]. 控制理论与应用, 2009, 26(6): 678-682. ZHOU L, JIANG C S, DU Y L. A robust adaptive terminal sliding mode control based on backstepping method[J]. Control Theory and Applications, 2009, 26(6): 678-682.