﻿ 基于改进蚁群算法的船舶路径规划及跟踪控制
 舰船科学技术  2022, Vol. 44 Issue (7): 68-73    DOI: 10.3404/j.issn.1672-7649.2022.07.013 PDF

Ship path planning and following control based on improved ant colony algorithm
LI Wei, ZHANG Jun, NING Jun, ZHOU Jing
Navigation College, Dalian Maritime University, Dalian 116026, China
Abstract: Aiming at the two problems of path planning and path tracking in the field of ship automation research, proposes a ship path planning method based on improved ant colony algorithm. In order to better solve the problems of the traditional ant colony algorithm, the artificial potential field method is used to improve the pheromone update strategy of the ant colony algorithm, and the ship path planning and ship path tracking control issues are coordinated, and the unknown uncertainty of the ship dynamics subsystem is introduced by the neural network. The items are approximated, and a sliding mode controller based on radial basis function neural network algorithm is designed,Simulation experiments verify the effectiveness of the algorithm proposed in this paper.
Key words: path planning     ant colony algorithm     closed loop control system     neural network
0 引　言

1 基于改进蚁群算法的船舶路径规划

1.1 改变信息素的更新策略

 ${\tau ''_{ij}}\left( h \right) = \left\{ \begin{array}{*{20}{l}} {\tau _{ij}}\left( h \right) + \delta {{\tau '}_{ij}}\left( h \right),&h \leqslant {h_a} ，\\ {\tau _{ij}}\left( h \right){\text{,}}&h > {h_a} 。\end{array} \right.$ (1)

 ${\tau ''_{ij}}\left( h \right) = \left\{ \begin{array}{*{20}{l}} {\tau _{\min }}{\text{,}}\qquad \;\;&{{\tau ''}_{ij}}\left( h \right) < {\tau _{\min }}，\\ {{\tau ''}_{ij}}\left( h \right),&{\tau _{\min }} < {{\tau ''}_{ij}}\left( h \right) < {\tau _{\max }} ，\\ {\tau _{\max }}{\text{,}}&{{\tau ''}_{ij}}\left( h \right) > {\tau _{\max }} 。\end{array} \right.$ (2)
1.2 改进信息素挥发系数

 $\lambda = 1 - {e^{\frac{{{N_c} - {N_{\max }}}}{{{N_{\max }}}}}} ，$ (3)
 $\rho ' = \left\{ \begin{array}{*{20}{l}} {\rho _{\min }}{\text{,}}\qquad &\rho < {\rho _{\min }} ，\\ \lambda \times \rho, &{\rho _{\min }} < \rho < {\rho _{\max }} ，\\ {\rho _{\max }},&\rho > {\rho _{\max }} 。\end{array} \right.$ (4)
 ${\tau ''_{ij}}\left( h \right) = \left\{ \begin{array}{*{20}{l}} {\tau _{\min }}{\text{ ,}}\quad &{{\tau ''}_{ij}}\left( h \right) < {\tau _{\min }} ，\\ {{\tau ''}_{ij}}\left( h \right), {\tau _{\min }} < {{\tau ''}_{ij}}\left( h \right) < {\tau _{\max }} ，\\ {\tau _{\max }}{\text{,}}&{{\tau ''}_{ij}}\left( h \right) > {\tau _{\max }} 。\end{array} \right.$ (5)

 ${\tau _{ij}}\left( {h + 1} \right) = \left( {1 - \rho '} \right){\tau ''_{ij}}\left( h \right) + \Delta {\tau _{ij}}\left( h \right) 。$ (6)
1.3 改进启发函数

 ${\eta _A}\left( t \right) = {a^{{F_{total}}(P)\cos \varphi }} 。$ (7)

 $\sigma = \frac{{{N_{\max }} - {N_c}}}{{{N_{\max }}}} ，$ (8)

 ${\eta '_{ij}}\left( t \right) = \frac{{{a^{{F_{total}}\left( P \right)\cos \varphi }}}}{{{d_{ij}}}} \times \frac{{{N_{\max }} - {N_c}}}{{{N_{\max }}}} 。$ (9)

 $P_{ij}^k(t) = \left\{ {\begin{array}{*{20}{l}} {\frac{{{{\left[ {{\tau _{ij}}(t)} \right]}^\alpha }{{\left[ {{{\eta '}_{ij}}(t)} \right]}^\beta }}}{{{{\displaystyle\sum\limits_{s \subset allowe{d_k}(t)} {\left[ {{\tau _{is}}(t)} \right]} }^\alpha }{{\left[ {{{\eta '}_{is}}(t)} \right]}^\beta }}}\;\;,\quad j \in allowe{d_k}(t)\quad } ，\\ {0,\quad \qquad \quad \quad \quad \quad \quad \quad \quad \;\,\quad j \notin allowe{d_k}(t)} 。\end{array}} \right.$ (10)

 $m = \left\{ {\begin{array}{*{20}{l}} m \times \dfrac{{{N_{\max }} - {N_c}}}{{{N_{\max }}}}，& m > {m_{\min }} ，\\ {m_{\min }},& m \leqslant {m_{\min }} 。\end{array}} \right.$ (11)

1.4 改进斥力势场函数

 ${U_{rep}}^\prime \left( P \right) = \left\{ \begin{array}{*{20}{l}} \dfrac{1}{2}{k_{rep}}{e^{ - d{{\left( {P,O} \right)}^2}}},\, & d\left( {P,O} \right) \leqslant {d_0} ，\\ 0 , & d\left( {P,O} \right) > {d_0} 。\end{array} \right.$ (12)

 $\begin{split} {U_{rep}}^{\prime \prime }\left( P \right) =& \xi {U_{rep}}^\prime \left( P \right) = \\ &\left\{ \begin{array}{*{20}{l}} \dfrac{1}{2}{k_{rep}}d{\left( {P,G} \right)^2}{e^{ - d{{\left( {P,O} \right)}^2}}}, & d\left( {P,O} \right) \leqslant {d_0} ，\\ 0{\text{,}} & d\left( {P,O} \right) > {d_0} 。\end{array} \right. \\[-25pt] \end{split}$ (13)

 $\begin{split} {F'_{rep}}\left( P \right) &= - \nabla {U_{rep}}^{\prime \prime }\left( P \right) =\\ &\left\{ \begin{array}{*{20}{l}} {k_{rep}}d{\left( {P,G} \right)^2}d\left( {P,O} \right){e^{ - d{{\left( {P,O} \right)}^2}}}{n_{OP}},& d\left( {P,O} \right) \leqslant {d_0} ，\\ 0,& d\left( {P,O} \right) > {d_0} 。\end{array} \right. \end{split}$ (14)

 ${F'_{total}}\left( P \right) = {F'_{rep}}\left( P \right) + {F_{att}}\left( P \right) 。$ (15)
2 控制律的设计 2.1 位置控制律的设计

 $\left\{ {\begin{array}{*{20}{c}} {{{\dot x}_e} = U\cos \theta - {{\dot x}_d}} ，\\ {{{\dot y}_e} = U\sin \theta - {{\dot y}_d}} ，\end{array}} \right.$ (16)

${y_e} = y - {y_d}$ 时，为了方便计算，可以取：

 $\left\{ {\begin{array}{*{20}{l}} {U\cos \theta }& = &{{u_1}} ，\\ {U\sin \theta }& = &{{u_2}} ，\end{array}} \right.$ (17)

 ${\dot s_1} = {\dot x_e} = {u_1} - {\dot x_d} ，$ (18)

 ${u_1} = {\dot x_d} - {k_1}{s_1} ，$ (19)

 ${\dot s_2} = {\dot y_e} = {u_2} - {\dot y_d} ，$ (20)

 ${u_2} = {\dot y_d} - {k_2}{s_2} ，$ (21)

 $\theta = \arctan \frac{{{u_2}}}{{{u_1}}} 。$ (22)

 ${\theta _d} = \arctan \frac{{{u_2}}}{{{u_1}}}，$ (23)

 $U = \frac{{{u_1}}}{{\cos {\theta _d}}} 。$ (24)
2.2 姿态控制律的设计

$\theta$ ${\theta _d}$ 有误差时，会造成控制律无法精确实现，只有通过更快的收敛算法，使 $\theta$ 快速跟踪 ${\theta _d}$ ，才能解决这个问题。将通过设计姿态控制律 $r$ ，实现 $\theta$ 跟踪 ${\theta _d}$ 。取 ${\theta _e} = \theta - {\theta _d}$ ，滑模函数为 ${s_3} = {\theta _e}$ ，可以得到：

 ${\dot s_3} = {\dot \theta _e} = r - {\dot \theta _d} ，$ (25)

 $\omega = {\dot \theta _d} - {k_3}{s_3} - {\eta _3}{{\rm{sgn}}} {s_3} 。$ (26)

2.3 闭环系统的设计

 图 1 具有双环的闭环系统结构 Fig. 1 Closed loop system structure with double loops
3 基于神经网络技术的船舶动力学滑膜控制

 $\left\{ {\begin{array}{l} {{m_u}\dot u = {f_u}\left( {u,\nu ,r} \right) + {\tau _u} + {\tau _{{\text{nw}}}}}，\\ {{m_\nu }\dot v = {f_\nu }\left( {u,\nu ,r} \right) + {\tau _{\nu w}}} ，\\ {{m_r}\dot r = {f_r}\left( {u,\nu ,r} \right) + {\tau _r} + {\tau _{nv}}} ，\end{array} } \right.$ (27)

 $\begin{split} {\dot u = \dot U\cos \left( \theta \right) - U\sin \left( \theta \right)} = { \dot U - 2\dot U{{\sin }^2}\left( {\dfrac{\theta }{2}} \right) - U\sin \left( \theta \right)} 。\end{split}$ (28)

 $\left\{ {\begin{array}{*{20}{l}} {{m_u}\dot U = {f_u}\left( {u,\nu ,r} \right) + {u_u} + {u_{dw}} + 2{m_u}\dot U{{\sin }^2}\left( {\dfrac{\theta }{2}} \right) + {m_u}U\sin \left( \theta \right)} ，\\ {{m_r}\dot r = {f_r}\left( {u,\nu ,r} \right) + {u_r} + {u_{rw}}} 。\end{array} }\right.$ (29)

 $\left\{\begin{array}{l}\underset{t\to \infty }{\mathrm{lim}}\left|{U}_{t}-U\right|\leqslant {\delta }_{1}\hfill，\\ \underset{t\to \infty }{\mathrm{lim}}\left|r-{r}_{r}\right|\leqslant {\delta }_{2}，\hfill \end{array}\right. \left\{\begin{array}{l}\underset{t\to \infty }{\mathrm{lim}}\left|{U}_{t}-U\right|\leqslant {\delta }_{1}，\hfill \\ \underset{t\to \infty }{\mathrm{lim}}\left|r-{r}_{r}\right|\leqslant {\delta }_{2}。\hfill \end{array}\right.$ (30)

 $\dot x = f(x) + u + d(t) 。$ (31)

 $\begin{gathered} {f_1} = {m_u}^{ - 1}({f_u}\left( {u,\nu ,r} \right) + 2{m_u}\dot U{\sin ^2}\left( {\frac{\theta }{2}} \right) + {m_u}U\sin \left( \theta \right)r) ，\hfill \\ {f_2} = {m_r}^{ - 1}{f_r}\left( {u,\nu ,r} \right) 。\hfill \\ \end{gathered}$ (32)

 $s = c \displaystyle\int_0^t e{{\rm{dt}}} + e(t) ，$ (33)

 $\begin{split} \dot s =& ce + \dot e = \dot x - {{\dot x}_d} + ce =\\ &u + d(t) + f(x) - {{\dot x}_d} + ce= u + F(x) - {{\dot x}_d} + c\dot e。\end{split}$ (34)

3.1 控制器的设计

 $F(x) - \hat F(x) = {W^{*{\rm{T}}}}h(x) + \varepsilon - {{\hat W}^{\rm{T}}}h(x) = - {{\tilde W}^{\rm{T}}}h(x) + \varepsilon ，$ (35)

 $V = \frac{1}{2}{s^2} + \frac{1}{{2\gamma }}{\tilde W^{\rm{T}}}\tilde W ，$ (36)

 $\dot V = s\dot s + \frac{1}{\gamma }{\tilde W^{\rm{T}}}\dot {\hat W} = s(ce + F(x) + u - {\dot x_d}) + \frac{1}{\gamma }{\tilde W^{\rm{T}}}\dot {\hat W} ，$ (37)

 $u = - ce - \hat F(x) + {\dot x_d} - \eta {sgn} (s) ，$ (38)

 $\begin{split} \dot V =& s(F(x) - \hat F(x) - \eta {sgn} (s)) + \frac{1}{\gamma }{{\tilde W}^{\rm{T}}}\dot {\hat W} = \\ & s( - {{\tilde W}^{\rm{T}}}h(x) + \varepsilon - \eta {sgn} (s)) + \frac{1}{\gamma }{{\tilde W}^{\rm{T}}}\dot {\hat W} = \\ &\varepsilon s - \eta \left| s \right| + {{\tilde W}^{\rm{T}}}\left(\frac{1}{\gamma }\dot {\hat W} - sh(x)\right) 。\end{split}$ (39)

$\eta > {\left| \varepsilon \right|_{\max }}$ ，自适应律为

 $\dot {\hat W} = \gamma sh(x) 。$ (40)

$\dot V \equiv 0$ 时， $s \equiv 0$ ，根据拉萨尔不变性原理，闭环系统趋于稳定，当 $t \to \infty$ 时， $s \to 0$

 图 2 控制系统 Fig. 2 Control system
3.2 仿真实验

 图 3 动力学控制力矩 Fig. 3 Dynamic control torque

 图 4 动力学系统控制力 Fig. 4 Control force of dynamic system

 图 5 合速度引导律 Fig. 5 Combined speed guidance rate

 图 6 系统不确定项 Fig. 6 System uncertainty

 图 7 姿态引导律 Fig. 7 Attitude guidance rate
4 结　语

 [1] NIE J, LIN X. Failos guidance law based adaptive fuzzy finite-time path following control for underactuated msv[J]. Ocean Engineering, 2019, 195: 106726. [2] ZHENG Z, XIE L. Finite-time path following control for a stratospheric airship with input saturation and error constraint[J]. International-Journal of Control, 2019, 92(2): 368-393. DOI:10.1080/00207179.2017.1357839 [3] 曾华宝. 基于终端滑模的欠驱动船舶路径跟踪自适应控制[D]. 大连: 大连海事大学, 2020. [4] HEALEY A J, LIENARD D. Multivariable sliding mode control for autonomous diving and steering of unmanned underwater vehicles[J]. Oceanic Engineering IEEE Journal of, 1993, 18(3): 327-339. DOI:10.1109/JOE.1993.236372 [5] FOSSEN T I, BREIVIK M, SKJETNE R. Line-of-sight path following of underactuated marine craft[J]. IFAC Proceedings Volumes, 2003, 36(21): 211-216. DOI:10.1016/S1474-6670(17)37809-6 [6] OH S R, SUN J. Path following of underactuated marine surface vessels using line-of-sight based model predictive control[J]. Ocean Engineering, 2010, 37(2-3): 289-295. DOI:10.1016/j.oceaneng.2009.10.004 [7] 朱骋, 庄佳园, 张磊, 等. 无人水面艇自适应路径跟踪算法[J]. 导航与控制, 2019, 18(1): 44-50. DOI:10.3969/j.issn.1674-5558.2019.01.006 [8] QIU B, WANG G, FAN Y, et al. Path following of underactuated unmanned surface vehicle based on trajectory linearization control with input saturation and external disturbances[J]. International Journal of Control Automation and Systems, 2020, 18(4): 1-12. [9] LIANG X, QU X, WAN L, et al. Three-dimensional path following of an underactuated auv based on fuzzy backstepping sliding mode control[J]. International Journal of Fuzzy Systems, 2017, 20: 640-649. [10] 沈智鹏, 景富盛. 欠驱动船舶路径跟踪神经元自适应迭代滑模控制[J]. 哈尔滨工程大学学报, 2019, 40(3): 489-494. [11] FOSSEN T I, LEKKAS A M. Direct and indirect adaptive integral line-of-sight path-following controllers for marine craft exposed to ocean currents[J]. International Journal of Adaptive Control and Signal Processing, 2017, 31(4): 445-463. DOI:10.1002/acs.2550 [12] FOSSEN T I, PETTERSEN K Y, GALEAZZI R. Line-of-sight path following for dubins paths with adaptive sideslip compensation of drift forces[J]. IEEE Transactions on Control Systems Technology, 2015, 23(2): 820-827. DOI:10.1109/TCST.2014.2338354 [13] SHEN C, SHI Y, BUCKHAM B. Integrated path planning and tracking control of an auv: a unified receding horizon optimization approach[J]. IEEE/ASME Transactions on Mechatronics, 2017, 22(99): 1163-1173. [14] 张力行. 欠驱动船舶路径规划及跟踪算法研究[D]. 上海: 上海交通大学, 2017. [15] 曾国奇, 赵民强, 刘方圆, 等. 基于网格PRM的无人机多约束航路规划[J]. 系统工程与电子技术, 2016, 38(10): 2310-2316. DOI:10.3969/j.issn.1001-506X.2016.10.13 [16] XUE Z, LIU J, W U Zhengxing, et al. Development and path planning of a novel unmanned surface vehicle system and its application to exploitation of qarhan salt lake[J]. Science China(Information Sciences), 2019, 62(8): 084202. DOI:10.1007/s11432-018-9723-5 [17] 刘陆. 欠驱动无人船的路径跟踪与协同控制[D]. 大连: 大连海事大学, 2018