﻿ 基于干扰观测器的X舵AUV零纵倾变深控制
 舰船科学技术  2022, Vol. 44 Issue (18): 73-77    DOI: 10.3404/j.issn.1672-7649.2022.18.016 PDF

1. 海军研究院，北京 100161;
2. 天津航海仪器研究所九江分部，天津 300131

Zero-pitch depth control of X-rudder AUV based on disturbance observer
LI Yang1, HAN Jun-qing2, MIAO Xu-hong1, XU Xue-feng2
1. Naval Research Institute, Beijing 100161, China;
2. Jiujiang Division, Tianjin Navigation Instruments Research Institute, Tianjin 300131, China
Abstract: To solve the zero-pitch depth control of X-rudder AUV, a sliding mode control method based on nonlinear disturbance observer was proposed. Firstly, based on the vertical plane model of AUV, nonlinear disturbance observer was designed for depth and pitch channels respectively, and the coupling disturbance and unmodeled dynamics was observed, and feedforward compensation was carried out for them. Secondly, a dual-channel coupling controller of depth and pitch was designed to control the depth and pitch at the same time, to realize the zero-pitch depth control of AUV. Finally, the X-rudder angle allocation algorithm was designed to realize the instruction transformation from cross rudder to X-rudder. Simulation results show that the proposed zero-pitch depth control algorithm can make the AUV to complete the high precision zero-pitch depth control effectively.
Key words: X-rudder AUV     depth control     nonlinear disturbance observer     sliding mode control     angle allocation
0 引　言

1 模型描述

 $\left\{ \begin{gathered} \dot w = {f_w} + {b_{wb}}{\delta _b} + {b_{ws}}{\delta _s} + {d_w} \;, \\ \dot q = {f_q} + {b_{qb}}{\delta _b} + {b_{qs}}{\delta _s} + {d_q} \;, \\ \dot z = - u\sin \theta + w\cos \theta \;, \\ \dot \theta = q \;。\\ \end{gathered} \right.$ (1)

 $\begin{split} {f_w} = \frac{{{a_3}{f_3} + {a_2}{f_5}}}{{{a_1}{a_3} - {a_2}{a_4}}}\;,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {f_q} = \frac{{{a_4}{f_3} + {a_1}{f_5}}}{{{a_1}{a_3} - {a_2}{a_4}}} \;,\\ {b_{wb}} = \frac{{{a_3}{b_{b3}} + {a_2}{b_{b5}}}}{{{a_1}{a_3} - {a_2}{a_4}}}\;,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {b_{ws}} = \frac{{{a_3}{b_{s3}} + {a_2}{b_{s5}}}}{{{a_1}{a_3} - {a_2}{a_4}}}{\kern 1pt}\;, \\ {b_{qb}} = \frac{{{a_4}{b_{b3}} + {a_1}{b_{b5}}}}{{{a_1}{a_3} - {a_2}{a_4}}}\;,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {b_{qs}} = \frac{{{a_4}{b_{s3}} + {a_1}{b_{s5}}}}{{{a_1}{a_3} - {a_2}{a_4}}}\;。\end{split}$ (2)

 $\begin{split} {a_1} =& m - \frac{1}{2}\rho {L^3}{Z_{\dot w}}\;,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {a_2} = \frac{1}{2}\rho {L^4}{Z_{\dot q}} \;,\\ {a_3} =& {I_y} - \frac{1}{2}\rho {L^5}{M_{\dot q}}\;,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {a_4} = \frac{1}{2}\rho {L^4}{M_{\dot w}} \;,\\ {f_3} = &\frac{1}{2}\rho {L^3}\left( {{Z_q}uq + {Z_{w\left| q \right|}}w\left| q \right|} \right) + \frac{1}{2}\rho {L^2}\left( {{Z_u}{u^2} + {Z_w}uw} \right) + \\ &\frac{1}{2}\rho {L^2}\left( {{Z_{\left| w \right|}}u\left| w \right| + {Z_{w\left| w \right|}}w\left| w \right|} \right) + muq\;。\end{split}$
 $\begin{split} {f_5} =& \frac{1}{2}\rho {L^4}\left( {{M_q}uq + {M_{\left| w \right|q}}\left| w \right|q} \right) + \frac{1}{2}\rho {L^3}\left( {{M_u}{u^2} + {M_w}uw} \right)+ \\ &\frac{1}{2}\rho {L^3}{M_{w\left| w \right|}}w\left| w \right| - B{z_B}\sin \left( \theta \right) \;,\\ {b_{s3}} =& \frac{1}{2}\rho {L^3}{Z_{\left| q \right|{\delta _s}}}u\left| q \right| + \frac{1}{2}\rho {L^2}{Z_{{\delta _s}}}{u^2} \;,\\ {b_{s5}} =& \frac{1}{2}\rho {L^4}{M_{\left| q \right|{\delta _s}}}u\left| q \right| + \frac{1}{2}\rho {L^3}{M_{{\delta _s}}}{u^2} \;,\\ {b_{b3}} =& \frac{1}{2}\rho {L^2}{Z_{{\delta _b}}}{u^2}{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {b_{b5}} = \frac{1}{2}\rho {L^3}{M_{{\delta _b}}}{u^2}\;。\end{split}$

2 控制器设计

 图 1 控制器结构框图 Fig. 1 Block diagram of controller
2.1 非线性干扰观测器设计

AUV在变深航行过程中，为了保持艇内人员的舒适性，纵倾角一般很小。因此可将式(1)转化为：

 $\left\{ \begin{gathered} \ddot z = {f_z} + {b_{wb}}{\delta _b} + {b_{ws}}{\delta _s} + {d_w}\;, \\ \dot q = {f_q} + {b_{qb}}{\delta _b} + {b_{qs}}{\delta _s} + {d_q} \;, \\ \dot z = - u\theta + w ，\; \\ \dot \theta = q\;。\\ \end{gathered} \right.$ (3)

 $\left\{ \begin{gathered} {{\dot p}_z} = - {\beta _1}{p_z} - \beta _1^2\dot z - {\beta _1}\left( {{f_z} + {b_{wb}}{\delta _b} + {b_{ws}}{\delta _s}} \right)\;, \\ {{\hat d}_w} = {\beta _1}\dot z + {p_z} \;。\\ \end{gathered} \right.$ (4)
 $\left\{ \begin{gathered} {{\dot p}_q} = - {\beta _2}{p_q} - \beta _2^2q - {\beta _2}\left( {{f_q} + {b_{qb}}{\delta _b} + {b_{qs}}{\delta _s}} \right) \;, \\ {{\hat d}_q} = {\beta _2}q + {p_q} \;。\\ \end{gathered} \right.$ (5)

 $\begin{split} {{\dot {\tilde d}}_w} =& {{\dot d}_w} - {{\dot {\hat d}}_w} = - {\beta _1}\ddot z - {{\dot p}_z}= \\ &{\beta _1}\left( {{f_z} + {b_{wb}}{\delta _b} + {b_{ws}}{\delta _s} + {d_w}} \right) + \\ &{\beta _1}{p_z} + \beta _1^2\dot z + {\beta _1}\left( {{f_z} + {b_{wb}}{\delta _b} + {b_{ws}}{\delta _s}} \right)= \\ &- {\beta _1}{d_w} + {\beta _1}{p_z} + \beta _1^2\dot z= \\ &- {\beta _1}{d_w} + {\beta _1}\left( {{{\hat d}_w} - {\beta _1}\dot z} \right) = - {\beta _1}{{\tilde d}_w}\;。\end{split}$ (6)

2.2 滑模控制器设计

 $\begin{split} {u_z} = {b_{wb}}{\delta _b} + {b_{ws}}{\delta _s} \;,\\ {u_q} = {b_{qb}}{\delta _b} + {b_{qs}}{\delta _s} \;。\end{split}$ (7)

 $\left\{ \begin{gathered} \ddot z = {f_z} + {u_z} + {{\hat d}_w} \;, \\ \dot q = {f_q} + {u_q} + {{\hat d}_q} \;, \\ \dot z = - u\theta + w \;, \\ \dot \theta = q \;。\\ \end{gathered} \right.$ (8)

 ${e}_{z}=z-{z}_{c}\text{，}{\dot{e}}_{z}=\dot{z}-{\dot{z}}_{c} \;。$ (9)

 ${s_z} = {\dot e_z} + {c_z}{e_z} \;。$ (10)

 $\begin{gathered} {{\dot s}_z} = {{\ddot e}_z} + {c_z}{{\dot e}_z} = {f_z} + {u_z} + {{\hat d}_w} - {{\ddot z}_c} + {c_z}{{\dot e}_z} \;。\end{gathered}$ (11)

 ${u_{zeq}} = {\ddot z_c} - {\hat d_w} - {f_z} - {c_z}{\dot e_z} \;。$ (12)

 ${u_{zsmc}} = - {k_{z1}}{s_z} - {k_{z2}}{{\rm{sgn}}} \left( {{s_z}} \right) \;。$ (13)

 $\begin{split} {u_z} = &{u_{zeq}} + {u_{zsmc}} = {{\ddot z}_c} - {{\hat d}_w} - {f_z} -\\ &{c_z}{{\dot e}_z} - {k_{z1}}{s_z} - {k_{z2}}{{\rm{sgn}}} \left( {{s_z}} \right) \;。\end{split}$ (14)

 $sat\left( {{s_z}} \right) = \left\{ \begin{array}{ll} {{\rm{sgn}}} \left( {{s_z}} \right)，& {s_z} > {\Delta _z}，\\ {{s_z}} / {\Delta _z}，& {s_z} \leqslant {\Delta _z}。\end{array} \right.$ (15)

 ${u_z} = {\ddot z_c} - {\hat d_w} - {f_z} - {c_z}{\dot e_z} - {k_{z1}}{s_z} - {k_{z2}}sat\left( {{s_z}} \right)\;。$ (16)

 ${u_q} = - {\hat d_q} - {f_q} - {c_q}{\dot e_q} - {k_{q1}}{s_q} - {k_{q2}}sat\left( {{s_q}} \right)\;。$ (17)

2.3 舵角分配算法设计

 图 2 X舵布局 Fig. 2 Layout of the X-rudder

 $\left\{ \begin{gathered} {\delta _r} = \frac{1}{4}\left( {{\delta _1} + {\delta _2} + {\delta _3} + {\delta _4}} \right) \;，\\ {\delta _s} = \frac{1}{4}\left( {{\delta _1} - {\delta _2} + {\delta _3} - {\delta _4}} \right)\;。\\ \end{gathered} \right.$ (18)

 ${\tau _s} = B{\tau _x}\;。$ (19)

 $B = \left[ {\begin{array}{*{20}{c}} {0.25}&{0.25}&{0.25}&{0.25} \\ {0.25}&{ - 0.25}&{0.25}&{ - 0.25} \end{array}} \right] \;。$ (20)

 ${\tau _x} = {B^{\rm{T}}}{\left( {B{B^{\rm{T}}}} \right)^{ - 1}}{\tau _s}\;。$ (21)
3 仿真验证

 图 3 深度响应曲线 Fig. 3 The response curve of depth

 图 7 舵角2响应曲线 Fig. 7 The response curve of rudder 2

 图 8 舵角3响应曲线 Fig. 8 The response curve of rudder 3

 图 9 舵角4响应曲线 Fig. 9 The response curve of rudder 4

 图 4 深度曲线局部放大图 Fig. 4 The detail view of depth curve

 图 5 纵倾响应曲线 Fig. 5 The response curve of pitch

 图 6 舵角1响应曲线 Fig. 6 The response curve of rudder 1

 图 10 首舵响应曲线 Fig. 10 The response curve of bow rudder
4 结　语

 [1] 黄琰, 李岩, 俞建成, 等. AUV智能化现状与发展趋势[J]. 机器人, 2020, 42(2): 215-231. HUANG Yan, LI Yan, YU Jian-cheng, et al. State of the art and development trends of AUV intelligence[J]. Robot, 2020, 42(2): 215-231. DOI:10.13973/j.cnki.robot.190392 [2] 王小平. X舵AUV控制分配优化与容错控制方法研究[D]. 哈尔滨: 哈尔滨工程大学, 2020. [3] 李岳明, 王小平, 张军军, 等. 基于改进二次规划算法的X舵智能水下机器人控制分配[J]. 上海交通大学学报, 2020, 54(5): 524-531. LI Yue-ming, WANG Xiao-ping, ZHANG Jun-jun, et al. X-rudder AUV control allocation based on improved quadratic programming algorithm[J]. Journal of Shanghai Jiao Tong University, 2020, 54(5): 524-531. DOI:10.16183/j.cnki.jsjtu.2020.05.010 [4] 杨德成, 郭亦平, 任元洲, 等. 多约束模型预测控制在AUV深度控制中的应用[J]. 舰船科学技术, 2021, 43(3): 120-124. YANG De-cheng, GUO Yi-ping, REN Yuan-zhou, et al. Application research of multi-constrained model predictive control in AUV depth control[J]. Ship Science and Technology, 2021, 43(3): 120-124. DOI:10.3404/j.issn.1672-7649.2021.03.023 [5] 霍江航, 姜向远, 栾义忠, 等. 基于L1自适应理论的AUV深度控制器设计[J]. 中国舰船研究, 2021, 16(5): 150-157. HUO Jiang-hang, JIANG Xian-gyuan, LUAN Yi-zhong, et al. Design of AUV depth controller based on L1 adaptive theory[J]. Chinese Journal of Ship Research, 2021, 16(5): 150-157. DOI:10.19693/j.issn.1673-3185.02114 [6] 绕志荣, 董绍江, 王军, 等. 基于干扰观测器的AUV深度自适应终端滑模控制[J]. 北京化工大学学报(自然科学版), 2021, 48(1): 103-107. RAO Zhi-rong, DONG Shao-jiang, WANG Jun, et al. Adaptive terminal sliding mode control of AUV depth based on a disturbance observer[J]. Journal of Beijing University of Chemical Technology(Natural Science), 2021, 48(1): 103-107. DOI:10.13543/j.bhxbzr.2021.01.014 [7] FOSSEN T. I. Guidance and control of ocean vehicles[M]. Chicester: John Wiley & Sons, 1994. [8] WANG Wen-jin, XIA Ying-kai, CHEN Ying, et al. motion control methods for x-rudder underwater vehicles: model based sliding mode and non-model based iterative sliding mode. [J] Ocean Engineering, 2020, 216: 1–16. [9] PRESTERO T. Verification of a six-degree of freedom simulation model for the REMUS autonomous underwater vehicle[D]. Woods Hole Ocenographic Institution, Departments of Ocean and Mechanical Engineering, MIT, MA, 2001.