﻿ 一种欠驱动无人水下航行器相对位置控制及其应用
 舰船科学技术  2023, Vol. 45 Issue (1): 108-113    DOI: 10.3404/j.issn.1672-7649.2023.01.019 PDF

A kind of under-actuated unmanned underwater vehicle relative position control and its application
ZHAO Xu, LIU Feng, PU Jun-tao, LI Jian-ping, LI Dong-qi
Kunming Branch of the 705 Research Institute, Kunming 650118, China
Abstract: Aiming at the requirement of under-actuated unmanned underwater vehicle to maintain the relative position of the target, a dual-loop position control algorithm is proposed that can stabilize relative position. The position control is decomposed into the position loop and the attitude/speed loop. The position loop outputs the virtual control. It is decoupled into required speed command and attitude command. The inner loop tracks the target speed and target attitude. The backstepping method is used to design the controller to realize the relative position maintenance of the under-actuated unmanned underwater vehicle. It is applied to plane trajectory tracking and formation position maintenance. The simulation results show that this method can effectively realize the plane trajectory tracking and formation position maintenance of the UUV, and ensure the stability of the UUV's relative position to the target. At the same time, due to the design of the guidance law in the inertial system, it is more concise and clearer than the double-loop control under the traditional system, and is simple to implement.
Key words: unmanned undersea vehicle     backstepping method     formation control     trajectory tracking
0 引　言

1 欠驱动UUV相对位置控制的误差方程

 ${v_i} = {C_{ib}}({v_b}{\text{)}}。$ (1)

 $\left\{ \begin{gathered} \dot x = u\cos \psi ，\\ \dot z = - u\sin \psi 。\\ \end{gathered} \right.$ (2)

 $\left\{ {\begin{array}{*{20}{c}} {{e_x} = x - {x_d}(t)}，\\ {{e_z} = z - {z_d}(t)}。\end{array}} \right.$ (3)

 $\left\{ {\begin{array}{*{20}{c}} {{{\dot e}_x} = u\cos \psi - {{\dot x}_d}(t)}，\\ {{{\dot e}_z} = - u\sin \psi - {{\dot z}_d}(t)} 。\end{array}} \right.$ (4)

 $\left\{ \begin{gathered} (m + {\lambda _{11}})\dot u = {F_1}{\text{ + }}\frac{1}{2}\rho {u^2}S{C_x}，\\ ({J_y} + {\lambda _{55}})\dot q = \frac{1}{2}\rho {u^2}SL(m_y^{\bar q}\bar q + m_y^{{\delta _r}}{\delta _r})。\\ \end{gathered} \right.$ (5)

 ${\dot e_u} = \frac{{{F_1}{\text{ + }}\dfrac{1}{2}\rho {u^2}S{C_x}}}{{m + {\lambda _{11}}}} - {\dot u_d} ，$ (6)
 $\left\{ \begin{gathered} {{\dot e}_\psi } = q - {{\dot \psi }_d} ，\\ \dot q = \dfrac{{\dfrac{1}{2}\rho {u^2}SL(m_y^{\bar q}\bar q + m_y^{{\delta _r}}{\delta _r})}}{{{J_y} + {\lambda _{55}}}} 。\\ \end{gathered} \right.$ (7)

2 基于反步法的控制器设计

 图 1 控制框图 Fig. 1 The block diagram of control system
2.1 位置控制器设计

 $\left\{ {\begin{array}{*{20}{c}} {{{\dot e}_x} = {f_1} - {{\dot x}_d}(t)} ，\\ {{{\dot e}_z} = - {f_2} - {{\dot z}_d}(t)}。\end{array}} \right.$ (8)

 $V = \frac{1}{2}e_x^2 + \frac{1}{2}e_z^2 ，$ (9)

 $\dot V = e_x^{}({f_1} - {\dot x_d}) + e_z^{}( - {f_2} - {\dot z_d}) ，$ (10)

 $\left\{ \begin{gathered} {f_1} = - {k_1}{e_x} + {{\dot x}_d}，\\ {f_2} = {k_2}{e_z} - {{\dot z}_d} 。\\ \end{gathered} \right.$ (11)

 $\dot V = - {k_1}e_x^2 - {k_2}e_z^2 < 0 。$ (12)

 $\begin{gathered} {u_d} = \sqrt {f_1^2 + f_2^2}，\\ {\psi _d} = \arctan \Bigg(\frac{{{f_2}}}{{{f_1}}}\Bigg)。\\ \end{gathered}$ (13)
2.2 速度与航向控制器设计

 ${V_1} = \frac{1}{2}e_u^2 ，$ (14)

 ${\dot V_1} = {e_u}\left(\dfrac{{{F_1}{\text{ + }}\dfrac{1}{2}\rho {u^2}S{C_x}}}{{m + {\lambda _{11}}}} - {\dot u_d}\right)。$ (15)

 $\frac{{{F_1}{\text{ + }}\dfrac{1}{2}\rho {u^2}S{C_x}}}{{m + {\lambda _{11}}}} - {\dot u_d} = - {k_3}{e_u}，$ (16)

 ${\dot V_1} = - {k_3}e_u^2 < 0 ，$ (17)

 ${F_1} = (m + {\lambda _{11}})( - {k_3}{e_u} + {\dot u_d}) - \frac{1}{2}\rho {u^2}S{C_x}，$ (18)

 ${V_2} = \frac{1}{2}e_\psi ^2 ，$ (19)

 ${\dot V_2} = e_\psi ^{}(q - {\dot \psi _d}) ，$ (20)

 $q - {\dot \psi _d} = - {k_4}{e_\psi } ，$ (21)

 ${\dot e_q} = \dfrac{{\dfrac{1}{2}\rho {u^2}SL(m_y^{\bar q}\bar q + m_y^{{\delta _r}}{\delta _r})}}{{{J_y} + {\lambda _{55}}}} - {\dot q_r} ，$ (22)

 ${V_3} = {V_2} + \frac{1}{2}e_q^2 ，$ (23)

 $\begin{split} {{\dot V}_3} =& e_\psi ^{}({e_q} + {q_r} - {{\dot \psi }_d}) + {e_q}\left(\dfrac{{\dfrac{1}{2}\rho {u^2}SL(m_y^{\bar q}\bar q + m_y^{{\delta _r}}{\delta _r})}}{{{J_y} + {\lambda _{55}}}} - {{\dot q}_r}\right) = \\ &- {k_4}e_\psi ^2{\text{ + }}{e_q}\left(\dfrac{{\dfrac{1}{2}\rho {u^2}SL(m_y^{\bar q}\bar q + m_y^{{\delta _r}}{\delta _r})}}{{{J_y} + {\lambda _{55}}}} - {{\dot q}_r} + {e_\psi }\right)，\\[-25pt] \end{split}$ (24)

 ${\delta _r} = \dfrac{{\dfrac{{( - {k_5}{e_q} - {e_\psi } + {{\dot q}_r})({J_y} + {\lambda _{55}})}}{{\dfrac{1}{2}\rho {u^2}SL}} - m_y^{\bar q}\bar q}}{{m_y^{{\delta _r}}}}，$ (25)

 ${\dot V_3} = - {k_4}e_\psi ^2 - {k_5}e_q^2 < 0 ，$ (26)

3 仿真验证 3.1 UUV动力学模型

 $\left\{ \begin{gathered} (m + {\lambda _{11}})\dot u + mwq = {F_1}{\text{ + }}\frac{1}{2}\rho {V_s}^2S{C_x}，\\ (m + {\lambda _{33}})\dot w - muq = \frac{1}{2}\rho {V_s}^2S(C_z^{\bar q}\bar q + C_z^{{\delta _r}}{\delta _r} + C_z^\beta \beta ) ，\\ ({J_y} + {\lambda _{55}})\dot q = \frac{1}{2}\rho {V_s}^2SL(m_y^{\bar q}\bar q + m_y^{{\delta _r}}{\delta _r} + m_y^\beta \beta )。\\ \end{gathered} \right.$ (27)

3.2 轨迹跟踪仿真结果

 $\left\{ \begin{gathered} {x_1} = 100\sin (0.02t) ，\\ {z_1} = 3t 。\\ \end{gathered} \right.$ (28)

 图 2 轨迹跟踪结果图 Fig. 2 Trajectory tracking result

 图 3 位置误差 Fig. 3 Position error

 图 4 内环控制误差图 Fig. 4 Inner loop control error

3.3 队形保持仿真结果

 $\begin{split} & {x_2} = 3t，\\ & {z_2} = 2t。\end{split}$ (29)

 图 5 t=27 s时轨迹 Fig. 5 Trajectory at t=27 s

 图 6 t=87 s时轨迹 Fig. 6 Trajectory at t=87 s

 图 7 t=287 s时轨迹 Fig. 7 Trajectory at t=287 s

 图 8 跟随者1位置误差 Fig. 8 Position error of follower 1

 图 9 跟随者2位置误差 Fig. 9 Position error of follower 2
4 结　语

 [1] 潘光, 宋保维, 黄桥高, 等. 水下无人系统发展现状及其关键技术[J]. 水下无人系统学报, 2017, 25(1): 44-51. PAN Guang, SONG Bao-wei, HUANG Qiao-gao, et al. Development and key techniques of unmanned undersea system[J]. Journal of Unmanned Undersea Systems, 2017, 25(1): 44-51. [2] 严浙平, 李响, 宋育武, 等. 参数摄动下基于积分滑模的欠驱动UUV轨迹跟踪控制方法[J]. 水下无人系统学报, 2018, 26(3): 200-206. YAN Zhe-ping, LI Xiang, SONG Yu-wu, et al. Trajectory tracking control method for underactuated UUV using integral sliding mode under parameter perturbation[J]. Journal of Unmanned Undersea Systems, 2018, 26(3): 200-206. [3] 曹晓明, 魏勇, 衡辉, 等. 海流扰动下无人水下航行器的动态面反演轨迹跟踪控制[J]. 系统工程与电子技术, 2021, 43(6): 1664-1672. CAO Xiao-ming, WEI Yong, HENG Hui, et al. Dynamic surface backstepping trajectory tracking control of unmanned underwater vehicles with ocean current disturbances[J]. System Engineering and Electronics, 2021, 43(6): 1664-1672. DOI:10.12305/j.issn.1001-506X.2021.06.25 [4] 于浩淼. 非线性因素约束下欠驱动UUV轨迹跟踪控制方法研究[D]. 哈尔滨: 哈尔滨工程大学, 2016. [5] 严卫生. 鱼雷航行力学[M]. 西安: 西北工业大学出版社, 2005. [6] 武立军, 贺有智. 应用时标分离和动态逆方法设计飞行器的姿态控制系统[J]. 现代防御技术, 2007(4): 55-58. WU Li-jun, HE You-zhi. Design method to attitude control system of flying vehicle with dynamic inversion and time scale separation[J]. Modern Defence Technology, 2007(4): 55-58. DOI:10.3969/j.issn.1009-086X.2007.04.013 [7] JIANG Y, GUO C, YU H. Adaptive trajectory tracking control for an underactuated AUV based on command filtered backstepping[C]// Proceedings of the 37th Chinese Control Conference. 2018.