﻿ 2种典型AUV编队控制算法比较与局部优化研究
 舰船科学技术  2023, Vol. 45 Issue (24): 102-107    DOI: 10.3404/j.issn.1672-7649.2023.24.019 PDF
2种典型AUV编队控制算法比较与局部优化研究

Comparison and local optimization of two typical AUV formation control algorithms
ZHENG Peng, CAO Yuan-shan, ZHANG Chao, WANG Jian, XU Ling-Ling
State Key Laboratory of Hydrodynamics, China Ship Scientific Research Center, Wuxi 214082, China
Abstract: In this paper, we investigate two common AUV formation control methods: leader-follower formation control method and path-following formation control method. we found an interesting but small problem of the two methods, the AUV changes course Angle too much near the waypoint point. Then we propose a local B-spline fitting optimization of the planned route to solve this problem. Numerical simulation results show that the proposed method can improve the change velocity and curvature of the following AUV's position in the leader-follower formation control during path switching, and weaken the difficulty of point following. At the same time, in the path following formation control, the B-spline basis function independent variable is used as a part of the synchronization control parameters of the path near the switch point, which can improve the synchronization speed of the formation on the path near the switch point.
Key words: leader-follower     path following     AUV formation control     local path B-spline optimization
0 引　言

1 问题描述与局部路径优化 1.1 路径切换附近的编队队形抖动问题

 图 1 坐标系与角度定义 Fig. 1 Coordinate system and angle definition

${\boldsymbol{P}} \;=\; \left[ {{P_1},{P_2}, \cdots {P_i},{P_{i + 1}},{P_{i + 2}}, \cdots {P_n}} \right],\;i = 1,2,...,n - 2$$\overrightarrow{{P}_{i}{P}_{i+1}}，\overrightarrow{{P}_{i+1}{P}_{i+2}}\left(i=1,2,\mathrm{..},n-2\right) 不平行。 在路径切换点 {P_{i + 1}} 前后，路径由 \overrightarrow {{P_i}{P_{i + 1}}} 切换至 \overrightarrow {{P_{i + 1}}{P_{i + 2}}} ，根据领航者AUV位置Pa距路径切换点的距离 \left\| {\overrightarrow {{P_a}{P_{i + 1}}} } \right\| 小于阈值 d\left( {R,\angle \left( {\overrightarrow {{P_i}{P_{i + 1}}} ,\overrightarrow {{P_{i + 1}}{P_{i + 2}}} } \right)} \right) 作为路径切换判断依据。其中，R为AUV的回转半径、 \angle \left( {\overrightarrow {{P_i}{P_{i + 1}}} ,\overrightarrow {{P_{i + 1}}{P_{i + 2}}} } \right) 为路径 \overrightarrow {{P_i}{P_{i + 1}}}$$ \overrightarrow {{P_{i + 1}}{P_{i + 2}}}$的夹角，记为角${\alpha _{i + 1}} \in ( - \text{π} ,0) \cup (0, \text{π} )$。跟随者AUV位置记为${P_f}$，其与领航者AUV的相对位置用$\left( {\rho ,\theta } \right)$表示，领航者的航向用${\varphi _a}$表示。则跟随者的目标跟踪点位置$p_f^d$为：

 $p_f^d = {p_a} + {\boldsymbol{{R}}^{bo}}\left( {{\varphi _a}} \right)\left[ \begin{gathered} \rho \cos \left( {\text{π} + \theta } \right) \\ \rho \sin \left( {\text{π} + \theta } \right) \\ \end{gathered} \right]。$ (1)

 $\varphi _a^d\left( {\angle \overrightarrow {{P_i}{P_{i + 1}}} ,{e_{{n}\left( {i \to i + 1} \right)}}} \right) = \angle \overrightarrow {{P_i}{P_{i + 1}}} + \arctan ( - \frac{{{e_{{n}\left( {{i} \to i + 1} \right)}}}}{\Delta })。$ (2)

 $\begin{split} &\varphi _a^d\left( {\angle \overrightarrow {{P_{i + 1}}{P_{i + 2}}} ,{e_{{n}\left( {i + 1 \to i + 2} \right)}}} \right) = \\& \angle \overrightarrow {{P_{i + 1}}{P_{i + 2}}} + \arctan ( - \frac{{{e_{{n}\left( {{i + 1} \to {i + 2}} \right)}}}}{\Delta })。\end{split}$ (3)

 $\delta (\varphi _a^d)\sim{\alpha _{i + 1}} = \angle \left( {\overrightarrow {{P_i}{P_{i + 1}}} ,\overrightarrow {{P_{i + 1}}{P_{i + 2}}} } \right) 。$ (4)

 $d \left( {\hat p_f^d - p_f^d} \right) = {\rho _k}\sqrt {2 - 2\cos {\alpha _{i + 1}}}。$ (5)

1.2 基于B样条曲线的局部路径重规划

AUV在路径$\overrightarrow {{P_i}{P_{i + 1}}}$上时，路径同步控制参数可取为：

 $\begin{gathered} {\lambda ^a} = \left\| {\overrightarrow {{P_a}{P_{i + 1}}} } \right\|/\left\| {\overrightarrow {{P_i}{P_{i + 1}}} } \right\|，\\ {\lambda ^f} = \left\| {\overrightarrow {{P_f}P_{i + 1}^f} } \right\|/\left\| {\overrightarrow {P_i^fP_{i + 1}^f} } \right\|。\\ \end{gathered}$ (16)

 $\begin{gathered} {\lambda ^a} = \left\| {\overrightarrow {{P_a}{P_{i + 1}}} } \right\|/\left\| {\overrightarrow {{P_i}{P_{i + 1}}} } \right\| + {t^a} ，\\ {\lambda ^f} = \left\| {\overrightarrow {{P_f}P_{i + 1}^f} } \right\|/\left\| {\overrightarrow {P_i^fP_{i + 1}^f} } \right\| + {t^f} 。\\ \end{gathered}$ (17)

2.3 航速调整策略

 $\hat u = \left\{ \begin{gathered} u, \left\| {p_f^d - {P_f}} \right\| \leqslant {\delta _1}，\\ u + {{{k}}_1}\frac{{\left\| {p_f^d - {P_f}} \right\|}}{\delta }, \left\| {p_f^d - {P_f}} \right\| > {\delta _1} 。\\ \end{gathered} \right.$ (18)

 $\hat u = \left\{ \begin{gathered} u, \left\| {{\lambda ^a} - {\lambda ^f}} \right\| \leqslant {\delta _2}，\\ u + {k_2}\frac{{\left\| {{\lambda ^a} - {\lambda ^f}} \right\|}}{{{\delta _2}}}, \left\| {{\lambda ^a} - {\lambda ^f}} \right\| > {\delta _2} 。\\ \end{gathered} \right.$ (19)

3 数值仿真 3.1 领航AUV路径跟随优化前后对比

NPS AUV在2 kn航速下完成给定路径的跟随任务，设计航行路径为（单位：m）：

 $\begin{gathered} {P_1} = \left( {250,0} \right),{P_2} = \left( {350,0} \right),{P_3} = \left( {350,500} \right), \\ {P_4} = \left( {550,500} \right),{P_5} = \left( {550,0} \right),{P_6} = \left( {800,250} \right), \\ {P_7} = \left( {800,850} \right),{P_8} = \left( {550,600} \right),{P_9} = \left( {300,600} \right), \\ {P_{10}} = \left( {550,850} \right),{P_{11}} = \left( {300,850} \right),{P_{12}} = \left( {50,600} \right), \\ {P_{13}} = \left( { - 100,600} \right),{P_{14}} = \left( { - 100,0} \right),{P_{15}} = \left( { - 500,0} \right)。\\ \end{gathered}$

 ${K_p} = 0.5,{K_d} = 0.5。$

 图 3 优化前后路径跟随任务航行轨迹对比 Fig. 3 Trajectory comparison of the path following tasks

 图 4 优化前后路径跟随任务控制航向角对比 Fig. 4 Course angle comparison of the path following task
3.2 基于领航跟随的AUV编队控制仿真

 图 5 优化前后跟随AUV的目标跟随点轨迹对比 Fig. 5 The target tracking point trajectory comparison of the following AUV before and after optimization

 图 6 基于领航者跟随者控制的AUV编队轨迹 Fig. 6 AUV formation trajectory based on leader-follower formation control method

 图 7 跟随AUV与领航AUV距离时间历程 Fig. 7 Tracking distance between leader and follower
3.3 基于路径跟随的AUV编队控制仿真

 图 8 基于路径跟随控制的AUV编队轨迹 Fig. 8 AUV formation trajectory based on path follow formation control method

 图 9 领航AUV与跟随AUV的航速调整时间历程 Fig. 9 Speed adjustment time history of the leader AUV and the follower AUV
3.4 2种编队控制方法的控制效果比较

4 结　语

 [1] EDWARDS D B, BEAN T, ODELL D, et al. A leader-follower algorithm for multiple AUV formations[C]//Autonomous Underwater Vehicles, Sebasco, Maine, The United State, 2004. IEEE, 2004: 501–509. [2] CALADO P, SOUSA J. Leader-follower control of underwater vehicles over acoustic communications[C]//OCEANS, Santander, Spain, 2011. IEEE, 2011: 501–506. [3] 姜成林, 徐会希. 面向复杂地形海洋勘探的Multi-AUV编队协同控制策略[J]. 舰船科学技术, 2021, 43(3): 93-100. JIANG Cheng-lin, XU Hui-xi. Multi-autonomous underwater vehicles formation control and strategy for complex terrain oceanographic exploration[J]. Ship Science and Technology, 2021, 43(3): 93-100. DOI:10.3404/j.issn.1672-7649.2021.02.020 [4] 袁健, 唐功友. 采用一致性算法与虚拟结构的多自主水下航行器编队控制[J]. 智能系统学报, 2011, 6(3): 248-253. YUAN Jian, TANG Gong-you. Formation control of autonomous underwater vehicles with consensus algorithms and virtual structure[J]. CAAI Transactions on Intelligent Systems, 2011, 6(3): 248-253. DOI:10.3969/j.issn.1673-4785.2011.03.009 [5] 潘无为, 姜大鹏, 庞永杰, 等. 人工势场和虚拟结构相结合的多水下机器人编队控制[J]. 兵工学报. 2017(2): 326–334. PAN Wu-wei, JIANG Da-peng, PANG Yong-jie. et al. A multi-AUV formation algorithm combining artificial potential field and virtual structure[J]. Acta Armamentarii, 2017(2): 326–334. [6] 王银涛, 严卫生. 多自主水下航行器系统一致性编队跟踪控制[J]. 控制理论与应用. 2013, 30(3): 379–384. WANG Yin-tao, YAN Wei-sheng. Consensus formation tracking control of multiple autonomous underwater vehicle systems[J]. Control Theory & Applications, 2013, 30(3): 379–384. [7] 何斌. 多AUV编队控制与协同搜索技术研究[D]. 哈尔滨: 哈尔滨工程大学, 2017. [8] GHABCHELLO R. Coordinated path following of multiple autonomous vehicles[D]. Lisbon: Ph. D Thesis of Technical University of Lisbon, 2007: 23–64. [9] 边信黔, 牟春晖, 严浙平. 多UUV沿多条给定路径运动的协调编队控制[J]. 哈尔滨工业大学学报. 2013, 45(1): 106–111. BIAN Xin-qian, MU Chun-hui, YAN Zhe-ping. Coordinated control for multi-UUV formation motion on a set of given paths[J]. Journal of Harbin Institute of Technology, 2013, 45(1): 106–111. [10] 马艳彤, 郑荣, 韩晓军. 面向海底光学探测使命的自治水下机器人水平路径跟随控制[J]. 兵工学报, 2017, 38(6): 1147-1153. MA Yan-tong, ZHENG Rong, HAN Xiao-jun. Horizontal trajectory tracking control of autonomous underwater vehicle based on seabed optical detection mission[J]. Acta Armamentarii, 2017, 38(6): 1147-1153. DOI:10.3969/j.issn.1000-1093.2017.06.014 [11] 赵宁宁, 徐德民, 高剑, 等. 基于Serret-Frenet坐标系的多AUV编队路径跟踪控制[J]. 鱼雷技术, 2015, 23(1): 35-39. ZHAO Ning-ning, XU De-min, GAO Jian, et al. Formation path following control of multiple AUVs based on serret-frenet coordinate system[J]. Torpedo Technology, 2015, 23(1): 35-39. [12] HEALEY A J, MARCO D B, MCGHEE R B, et al. Tactical / execution level coordination for hover control of the nps auv ii using onboard sonar servoing[J]. Proceedings of the IEEE Symposium on Autonomous Underwater Vehicle Technology, 1994.