﻿ AUV水下空间运动自动控制仿真
 舰船科学技术  2023, Vol. 45 Issue (12): 57-62    DOI: 10.3404/j.issn.1672-7619.2023.12.011 PDF
AUV水下空间运动自动控制仿真

1. 中国船舶科学研究中心，江苏 无锡 214082;
2. 深海技术科学太湖实验室，江苏 无锡 214082;
3. 深海载人装备国家重点实验室，江苏 无锡 214082

Research on automatic control simulation of AUV underwater space motion
HU Zhong-hui1,2,3, SHEN Dan1,2,3, WANG Lei1,2,3, YANG Shen-shen1,2,3
1. China Ship Scientific Research Center, Wuxi 214082, China;
2. Taihu Laboratory of Deepsea Technological Science, Wuxi 214082, China;
3. State Key Laboratory of Deep-sea Manned Vehicles, Wuxi 214082, China
Abstract: The mathematical model of single plane motion is no longer applicable. Taking an AUV as the research object, based on the 6-DOF spatial motion mathematical model, the force analysis of AUV is carried out. And the incremental PID control method commonly used in engineering is applied to form the mathematical model of AUV underwater space motion automatic control simulation calculation. The space motion mathematical model and plane motion mathematical model are used to simulate the typical space motion. Trough comparative analysis, it can be seen that when AUV moves in space under water, the coupling effect between horizontal and vertical plane motion cannot be ignored. And this coupling effect will directly affect the control law of AUV and space motion. The research results can provide reference for the research, design and application of AUV automatic control of underwater space motion, and have certain engineering value.
Key words: AUV     motion model     automatic control     simulation
0 引　言

1 空间运动数学模型 1.1 坐标系

 图 1 坐标系 Fig. 1 Coordinate system
1.2 数学模型 1.2.1 空间运动模型

 $\left\{ \begin{gathered} m(\dot u + qw - rv - {x_G}\left( {{q^2} + {r^2}} \right) + \\ {y_G}\left( {pq - \dot r} \right) + {z_G}\left( {pr + \dot q} \right)) = X，\\ m(\dot v + ru - pw - {y_G}\left( {{r^2} + {p^2}} \right)+ \\ {z_G}\left( {qr - \dot p} \right) + {x_G}\left( {pq + \dot r} \right)) = Y ，\\ m(\dot w + pv - qu - {z_G}\left( {{p^2} + {q^2}} \right) + \\ {x_G}\left( {pr - \dot q} \right) + {y_G}\left( {qr + \dot p} \right)) = Z，\\ {I_x}\dot p + ({I_z} - {I_y})qr - \left( {\dot r + pq} \right){I_{xz}} + \\ \left( {{r^2} - {q^2}} \right){I_{yz}} + \left( {pr - \dot q} \right){I_{xy}}+ \\ m\left[ {{y_G}\left( {\dot w - uq + vp} \right) - {z_G}\left( {\dot v - wp + ur} \right)} \right] = K，\\ {I_y}\dot q + ({I_x} - {I_z})rp - \left( {\dot p + qr} \right){I_{xy}} + \\ \left( {{p^2} - {r^2}} \right){I_{xz}} + \left( {pq - \dot r} \right){I_{yz}} + \\ m\left[ {{z_G}\left( {\dot u - vr + wq} \right) - {x_G}\left( {\dot u - vr + wq} \right)} \right] = M，\\ {I_z}\dot r + ({I_y} - {I_x})pq - \left( {\dot q + rp} \right){I_{yz}} + \\ \left( {{q^2} - {p^2}} \right){I_{xy}} + \left( {rq - \dot p} \right){I_{zx}} + \\ m\left[ {{x_G}\left( {\dot v - wp + ur} \right) - {y_G}\left( {\dot w - uq + vp} \right)} \right] = N。\\ \end{gathered} \right.$ (1)

1.2.2 受力分析

 图 2 某AUV外观图 Fig. 2 Appearance of an AUV

AUV的主要参数如表1所示。

 $\begin{split}X=&\dfrac{\rho }{2}{L}^{4}\left[{{X}^{\prime }}_{qq}{q}^{2}+{{X}^{\prime }}_{rr}{r}^{2}+{{X}^{\prime }}_{rp}rp\right]+\\ &\dfrac{\rho }{2}{L}^{3}\left[{{X}^{\prime }}_{\dot{u}}\dot{u} + {{X}^{\prime }}_{vr}vr+{{X}^{\prime }}_{wq}wq\right] + \dfrac{\rho }{2}{L}^{2}\left[{{X}^{\prime }}_{vv}{v}^{2} + {{X}^{\prime }}_{ww}{w}^{2}\right] +\\ &\dfrac{\rho }{2}{L}^{2}\left[{{X}^{\prime }}_{{\delta }_{r}{\delta }_{r}}{u}^{2}{\delta }_{r}{}^{2}+{{X}^{\prime }}_{{\delta }_{s}{\delta }_{s}}{u}^{2}{\delta }_{s}{}^{2}\right]-\left(W-B\right)\mathrm{sin}\theta +{X}_{T}，\end{split}$ (2)
 $\begin{split}Y=&\dfrac{\rho }{2}{L}^{4}\left[{{Y}^{\prime }}_{\dot{r}}\dot{r}+{{Y}^{\prime }}_{\dot{p}}\dot{p}+{{Y}^{\prime }}_{p\left|p\right|}p\left|p\right|+{{Y}^{\prime }}_{pq}pq\right]+\\ &\dfrac{\rho }{2}{L}^{3}\left[{{Y}^{\prime }}_{\dot{v}}\dot{v}+{{Y}^{\prime }}_{r}ur+{{Y}^{\prime }}_{wp}wp+{{Y}^{\prime }}_{p}up\right]+\\ &\dfrac{\rho }{2}{L}^{2}\left[{{Y}^{\prime }}_{*}{u}^{2}+{{Y}^{\prime }}_{v}uv+{{Y}^{\prime }}_{v\left|v\right|}v\left|{\left({v}^{2}+{w}^{2}\right)}^{1/2}\right|\right]+\\ &\dfrac{\rho }{2}{L}^{2}\left[{{Y}^{\prime }}_{{\delta }_{r}}{u}^{2}{\delta }_{r}\right]+\left(W-B\right)\mathrm{cos}\theta \mathrm{sin}\phi ，\end{split}$ (3)
 $\begin{split}Z=&\dfrac{\rho }{2}{L}^{4}\left[{{Z}^{\prime }}_{\dot{q}}\dot{q}+{{Z}^{\prime }}_{pp}{p}^{2}+{{Z}^{\prime }}_{rr}{r}^{2}+{{Z}^{\prime }}_{rp}rp\right]+\dfrac{\rho }{2}{L}^{3}\left[{{Z}^{\prime }}_{\dot{w}}\dot{w}+\right.\\ &\left.{{Z}^{\prime }}_{q}uq+{{Z}^{\prime }}_{vr}vr+{{Z}^{\prime }}_{vp}vp\right]+\dfrac{\rho }{2}{L}^{2}\left[{{Z}^{\prime }}_{*}{u}^{2}+{{Z}^{\prime }}_{w}^{}uw+\right.\\ &\left.{{Z}^{\prime }}_{vv}^{}{v}^{2}\right]+ \dfrac{\rho }{2}{L}^{2}\left[{Z}_{{\delta }_{s}}{u}^{2}{\delta }_{s}\right]+\left(W-B\right)\mathrm{cos}\theta \mathrm{cos}\phi ，\\[-15pt]\end{split}$ (4)
 $\begin{split}K=&\dfrac{\rho }{2}{L}^{5}\left[{{K}^{\prime }}_{\dot{p}}^{}\dot{p}+{{K}^{\prime }}_{\dot{r}}^{}\dot{r}+{{K}^{\prime }}_{qr}^{}qr+{{K}^{\prime }}_{pq}^{}pq\right]+\\ &\dfrac{\rho }{2}{L}^{4}\left[{{K}^{\prime }}_{\dot{v}}^{}\dot{v}+{{K}^{\prime }}_{p}^{}up+{{K}^{\prime }}_{r}^{}ur+{{K}^{\prime }}_{vq}^{}vq+\right.\\ &\left.{{K}^{\prime }}_{wp}^{}wp+{{K}^{\prime }}_{wr}^{}wr\right]+\\ &\dfrac{\rho }{2}{L}^{3}\left[{{K}^{\prime }}_{*}{u}^{2}+{{K}^{\prime }}_{v}uv+{{K}^{\prime }}_{vw}vw\right]+\\ &\dfrac{\rho }{2}{L}^{3}\left[{{K}^{\prime }}_{{\delta }_{r}}{u}^{2}{\delta }_{r}\right]+\left({y}_{G}W-{y}_{B}B\right)\mathrm{cos}\theta \mathrm{cos}\phi -\\ &\left({z}_{G}W-{z}_{B}B\right)\mathrm{cos}\theta \mathrm{sin}\phi ，\end{split}$ (5)
 $\begin{split} M =& \dfrac{\rho}{2}{L^5}\left[ M'_{\dot q} \dot q + M'_{pp}{p^2} + M'_{rr}{r^2} + M'_{rp}rp \right] + \\ & \dfrac{\rho}{2}{L^4}\left[ M'_{\dot w}\dot w + M'_{vr}vr + M'_{vp}vp + M'_quq \right] + \\ & \dfrac{\rho }{2}{L^3}\left[ {{{M'}_*}{u^2} + {{M'}_w}uw + {{M'}_{vv}}{v^2}} \right] + \\ & \dfrac{\rho }{2}{L^3}\left[ {{{M'}_{{\delta _s}}}{u^2}{\delta _s}} \right] - ({x_G}W - {x_B}B)\cos \theta \cos \varphi - \\ & ({z_G}W - {z_B}B)\sin \theta，\end{split}$ (6)
 $\begin{split} N = &\dfrac{\rho }{2}{L^5}[N'_{\dot r}\dot r + N'_{pq}pq + N'_{qr}qr]+ \\ & \dfrac{\rho }{2}{L^4}\left[ N'_{\dot v}\dot v + N'_{wr}wr + N'_{wp}wp + N'_{vq}vq + {{N'}_p}up + N'_rur \right] + \\ &\dfrac{\rho }{2}{L^3}\left[ {N'_*{u^2} + N'_vuv + N'_{vw}vw} \right] + \dfrac{\rho }{2}{L^3}\left[ {{{N'}_{{\delta _r}}}{u^2}{\delta _r}} \right] +\\ & \left( {{x_G}W - {x_B}B} \right)\cos \theta \sin \varphi + \left( {{y_G}W - {y_B}B} \right)\sin \theta 。\\[-10pt] \end{split}$ (7)

AUV在固定坐标系中的位置参数可表示为：

 $\left\{ \begin{array}{l} \dot \varphi = p + q\tan \theta \sin \varphi + r\tan \theta \cos \varphi ，\\ \dot \theta = q\cos \varphi - r\sin \varphi ，\\ \dot \psi = \left( {q\sin \varphi + r\cos \varphi } \right)/\cos \theta，\\ {{\dot \xi }_G} = u\cos \psi \cos \theta + v\left( \cos \psi \sin \theta \sin \varphi - \sin \psi \cos \varphi \right) + \\ \qquad w\left( {\cos \psi \sin \theta \cos \phi + \sin \psi \sin \varphi } \right) ，\\ \dot \eta {}_G = u\sin \psi \cos \theta + v\left( \sin \psi \sin \theta \sin \varphi + \cos \psi \cos \varphi \right) + \\ \qquad w\left( {\sin \psi \sin \theta \cos \varphi - \cos \psi \sin \varphi } \right)，\\ {{\dot \zeta }_G} = - u\sin \theta + v\cos \theta \sin \varphi + w\cos \theta \cos \varphi 。\\ \end{array} \right.$ (8)
2 自动控制模型

 $\Delta u(k) = {K_P}\Delta e(k) + {K_I}e(k)+ {K_D}[\Delta e(k) - \Delta e(k - 1)]。$ (9)

 $\Delta e(k) = e(k) - e(k - 1) 。$ (10)

 图 3 自动航向保持结构 Fig. 3 Automatic heading angle-holding structure

 图 4 自动深度保持结构 Fig. 4 Automatic depth-holding structure
3 空间运动仿真计算

3.1 Z形操舵运动仿真计算

Z形操舵运动采用10°/10°操舵方法，采用空间运动数学模型和水平面运动数学模型分别进行仿真计算。其中，采用空间运动数学模型进行仿真计算时，对深度采用增量式PID控制方法进行控制。仿真计算过程中，AUV的航行速度为1.5 m/s，初始深度30 m，采样间隔为0.5 s，结果如图5所示。

 图 5 10°/10°Z形操舵特征曲线图 Fig. 5 Characteristic curves of Z-shaped steering motion

 图 6 初转期对比图 Fig. 6 Comparison of initial turnaround periods

 图 8 周期时间对比图 Fig. 8 Comparison of cycle times

 图 7 超越时间对比图 Fig. 7 Comparison of beyond times

 图 9 运动轨迹对比图 Fig. 9 Comparison of movement trajectories

 图 10 深度变化图 Fig. 10 Depth with time

 图 11 水平舵舵角变化图 Fig. 11 Horizontal rudder angle with time

3.2 航向与深度保持仿真计算

AUV在实际航行过程中，航向与深度往往需要同时保持，这也是AUV空间机动能力的体现。本文分别采用空间运动数学模型和平面运动数学模型进行航向与深度保持仿真计算，计算类型划分如表2所示。

 图 12 航向角控制对比图 Fig. 12 Comparison of heading angle control

 图 15 水平舵舵角变化对比图 Fig. 15 Comparison of horizontal rudder angle

 图 13 垂直舵舵角变化对比图 Fig. 13 Comparison of vertical rudder angle

 图 14 深度控制对比图 Fig. 14 Comparison of depth control
4 结　语

 [1] 吴有生, 赵羿羽, 郎舒妍, 等. 智能无人潜水器技术发展研究[J]. 中国工程科学, 2020, 22(6): 26-31. [2] 罗建超, 朱心科. 基于改进粒子群优化算法的自主水下航行器深度控制[J]. 舰船科学技术, 2022, 44(8): 64-68. LUO Jianchao, ZHU Xinke. Depth control of autonomous underwater vehicle based on improved particle swarm optimization algorithm[J]. Ship Science and Technology, 2022, 44(8): 64-68. DOI:10.3404/j.issn.1672-7649.2022.08.013 [3] 雷江航, 姜向远, 栾义忠, 等. 基于L1自适应理论的AUV深度控制器设计[J]. 中国舰船研究, 2021, 16(5): 150-157. LEI Jianghang, JIANG Xiangyuan, LUAN Yizhong, 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 [4] 饶志荣, 董绍江, 王军, 等. 基于干扰观测器的AUV深度自适应终端滑模控制[J]. 北京化工大学学报(自然科学版), 2021, 48(1): 103-110. DOI:10.13543/j.bhxbzr.2021.01.014 [5] 李泽宇, 刘卫东, 申高展, 等. 水下航行器空间机动滑模变结构控制仿真[C]// 2015年中国西部声学学术交流会论文集, 2015: 4. [6] 梅新华, 周祥龙. 基于BP神经网络的双舵型AUV深度控制算法应用[J]. 水雷战与舰船防护, 2017, 25(3): 79-82. [7] 胡坤, 何斌, 张建华, 等. 波浪环境下潜艇近水面操纵特性仿真分析[J]. 舰船科学技术, 2017, 44(2): 6-11. HU Kun, HE Bin, ZHANG Jian-hua, et al. Simulation analysis of submarine maneuvering characteristics near water surface in wave environment[J]. Ship Science and Technology, 2017, 44(2): 6-11. DOI:10.3404/j.issn.1672-7619.2017.02.002 [8] 聂为彪, 钱治强, 吴铭, 等. 水下航行器横向操纵性运动预报设计与仿真[J]. 舰船科学技术, 2021, 43(S1): 22-26. NIE Weibiao, QIAN Zhiqiang, WU Ming, et al. Analysis on navigation scheme and key technology of the underwater vehicle[J]. Ship Science and Technology, 2021, 43(S1): 22-26. [9] 于浩洋, 李普强. 无缆自主式水下机器人航向的模糊控制[J]. 黑龙江工程学院学报, 2017, 31(5): 33-36. [10] 陈恳, 刘涛, 杜新光, 等. 基于自适应神经网络滑模控制的常压潜水装具航向控制器设计[J]. 中国造船, 2018, 59(1): 187-196. CHEN Ken, LIU Tao, DU Xinguang, et al. Design of course controller for atmospheric diving suit(ads) based on sliding mode control with adaptive neural network[J]. Shipbuilding of China, 2018, 59(1): 187-196. [11] 施生达. 潜艇操纵性[M]. 北京: 国防工业出版社, 1995. [12] 魏延辉. UVMS系统控制技术[M]. 哈尔滨: 哈尔滨工程大学出版社, 2017.