舰船科学技术  2022, Vol. 44 Issue (9): 63-68    DOI: 10.3404/j.issn.1672-7649.2022.09.013 PDF

Research on path planning of unmanned boat routes based on improved artificial potential field algorithm
ZHANG Qi, TIAN Tian, LUAN Tian-yu, ZHANG Yi, ZHANG Ke-xin
CSSC Systems Engineering Research Institute, Beijing 100094, China
Abstract: The path planning technology of the unmanned ship is the key technology to ensure that the unmanned ship can effectively perform the task, independently and autonomously complete the path tracking and trajectory tracking, and automatically avoid the obstacles and optimize the navigation path during the navigation. For the actual process of the unmanned ship leaving the port, the main solution is to solve the target unreachability problem and local minima problem of the traditional artificial potential field method, a new artificial potential field function is proposed on the basis of Krogh algorithm and the escape force factor is added, the simulation experiment of the unmanned ship leaving the port verifies the effectiveness of the improved artificial potential field algorithm, and the comparison results with the traditional artificial potential field method show that the improved artificial potential field method. The comparison results show that the improved artificial potential field method is more flexible to avoid obstacles and the planned navigation trajectory is more in line with the actual navigation requirements.
Key words: artificial potential field     repulsive field function     factor of escape force     unmanned ship     path planning
0 引　言

1 基于人工势场法路径规划算法

 图 1 无人船驶经港口示意图 Fig. 1 Schematic diagram of the unmanned ship sailing through the port

 ${U_{\text{a}}}{\text{ = }}\frac{1}{2}\eta [{(\chi - {\chi _g})^2} + {(y - {y_g})^2}] 。$ (1)

 ${F_a} = - \nabla ({U_a}) = \left[ { - \eta (x - {x_g}), - \eta (y - {y_g})} \right]，$ (2)

 $\left| {{F_a}} \right| = \eta \sqrt {{{(x - {x_g})}^2} + {{(y - {y_g})}^2}}。$ (3)

 ${U_r} = \left\{ {\begin{array}{*{20}{l}} \dfrac{1}{2}\xi \left(\dfrac{1}{\rho } - \dfrac{1}{{{\rho _d}}}\right),&\rho < {\rho _d} ，\\ 0,&\rho \geqslant {\rho _d} 。\end{array}} \right.$ (4)

 ${F_r} = - \nabla ({U_r}) = \frac{\xi }{{{\rho ^2}}}\left(\frac{1}{\rho } - \frac{1}{{{\rho _d}}}\right)\left[ {\begin{array}{*{20}{c}} {\dfrac{{\partial {U_r}}}{{\partial x}}}&{\dfrac{{\partial {U_r}}}{{\partial y}}} \end{array}} \right] ，$ (5)

 $\left|{F}_{r}\right|=\left\{\begin{array}{*{20}{l}}\dfrac{\xi }{{\rho }^{2}}\left(\dfrac{1}{\rho }-\dfrac{1}{{\rho }_{d}}\right),&\rho < {\rho }_{d}，\\ 0\text{，}&\rho \geqslant {\rho }_{d}。\end{array}\right.$ (6)

 $\phi {\text{ = }}\arccos \left(\frac{{x - {x_d}}}{{\sqrt {{{(x - {x_d})}^2} + {{(y - {y_d})}^2}} }}\right)，$ (7)

 $\begin{array}{l}\left|{F}_{rx}\right|=\left\{\begin{array}{*{20}{l}}\left|{F}_{r}\right|·\mathrm{cos}\varphi ,&\rho < {\rho }_{d}，\\ 0,&\rho \geqslant {\rho }_{d}，\end{array}\right.\\ \left|{F}_{ry}\right|=\left\{\begin{array}{*{20}{l}}\left|{F}_{r}\right|·\mathrm{sin}\varphi ,&\rho < {\rho }_{d}，\\ 0,&\rho \geqslant {\rho }_{d}。\end{array}\right.\end{array}$ (8)

2 改进的人工势场路径规划技术 2.1 斥力场函数优化

 ${U_r} = \left\{ {\begin{array}{*{20}{l}} \dfrac{1}{2}\xi {{(\dfrac{1}{\rho } - \dfrac{1}{{{\rho _o}}})}^2}\left[ {{{(x - {x_g})}^2} + {{(y - {y_g})}^2}} \right],&\rho < {\rho _o} ，\\ 0,&\rho \geqslant {\rho _o} 。\end{array}} \right.$ (9)

 $\begin{split} {F_{rx}} = & - {\nabla _x}({U_r}) = - \xi (x - {x_g}){\left(\frac{1}{\rho } - \frac{1}{{{\rho _o}}}\right)^2} +\\ &\xi \left[ {{{(x - {x_g})}^2} + {{(y - {y_g})}^2}} \right]\left(\frac{1}{\rho } - \frac{1}{{{\rho _o}}}\right)\frac{{x - {x_o}}}{{{\rho ^3}}} ，\\ {F_{ry}} = & - {\nabla _y}({U_r}) = - \xi (y - {y_g}){\left(\frac{1}{\rho } - \frac{1}{{{\rho _o}}}\right)^2} +\\ &\xi \left[ {{{(x - {x_g})}^2} + {{(y - {y_g})}^2}} \right]\left(\frac{1}{\rho } - \frac{1}{{{\rho _o}}}\right)\frac{{y - {y_o}}}{{{\rho ^3}}} ，\\ \left| {\overrightarrow {{F_r}} } \right| = &\sqrt {{F^2}_{rx} + {F^2}_{ry}} 。\end{split}$ (10)

 ${U_r} = \left\{ {\begin{array}{*{20}{l}} \dfrac{1}{2}\xi {{\left(\dfrac{1}{\rho } - \dfrac{1}{{{\rho _o}}}\right)}^2}\left[ {1 - \exp \left( - \dfrac{{\left[ {{{(x - {x_g})}^2} + {{(y - {y_g})}^2}} \right]}}{{{R^2}}}\right)} \right],\\ \rho < {\rho _o} ，\\ 0,\rho \geqslant {\rho _o}。\\ \end{array}} \right.$ (11)

 $\begin{split} {F_{rx}} =& - {\nabla _x}({U_r}) = - \xi (x - {x_g}){\left(\frac{1}{\rho } - \frac{1}{{{\rho _o}}}\right)^2}\times\\ &\exp \left( - \frac{{\left[ {{{(x - {x_g})}^2} + {{(y - {y_g})}^2}} \right]}}{{{R^2}}}\right)+\\ &\xi \left[ {1 - \exp \left( - \frac{{\left[ {{{(x - {x_g})}^2} + {{(y - {y_g})}^2}} \right]}}{{{R^2}}}\right)} \right]\left(\frac{1}{\rho } - \frac{1}{{{\rho _o}}}\right)\frac{{x - {x_o}}}{{{\rho ^3}}} ，\\ {F_{ry}} =& - {\nabla _y}({U_r}) = - \xi (y - {y_g}){\left(\frac{1}{\rho } - \frac{1}{{{\rho _o}}}\right)^2}\times\\ &\exp \left( - \frac{{\left[ {{{(x - {x_g})}^2} + {{(y - {y_g})}^2}} \right]}}{{{R^2}}}\right) +\\ &\xi \left[ {1 - \exp \left( - \frac{{\left[ {{{(x - {x_g})}^2} + {{(y - {y_g})}^2}} \right]}}{{{R^2}}}\right)} \right]\left(\frac{1}{\rho } - \frac{1}{{{\rho _o}}}\right)\frac{{y - {y_o}}}{{{\rho ^3}}}，\\ \left| {\overrightarrow {{F_r}} } \right| =& \sqrt {{F^2}_{rx} + {F^2}_{ry}} 。\\[-15pt] \end{split}$ (12)

2.2 逃逸算法优化

 $\begin{gathered} \left| {{F_{rx}}_e} \right| = \left\{ {\begin{array}{*{20}{c}} \alpha \left| {{F_{rx}}} \right|*\cos \theta ,&\rho < {\rho _o}，\\ 0,&\rho \geqslant {\rho _o} ，\end{array}} \right. \hfill \\ \left| {{F_{rye}}} \right| = \left\{ {\begin{array}{*{20}{c}} \beta \left| {{F_{ry}}} \right|*\sin \theta ,&\rho < {\rho _o}，\\ 0,&\rho \geqslant {\rho _o} 。\end{array}} \right. \hfill \\ \end{gathered}$ (13)

 图 2 改进算法的无人船路径规划流程图 Fig. 2 Flow chart of unmanned ship path planning with improved algorithm

3 仿真验证

 图 3 Krogh改进的人工势场算法路径规划航进曲线 Fig. 3 Artificial potential field path planning algorithm under Krogh improvement

 图 4 Krogh改进的人工势场算法路径规划过程合力曲线 Fig. 4 Krogh's improved artificial potential field path planning process ensemble curve

 图 5 本文改进算法的无人船路径规划航进曲线 Fig. 5 Improved path planning algorithm for unmanned ships in this paper

 图 6 本文改进的人工势场算法路径规划过程合力曲线 Fig. 6 The improved artificial potential field path planning process ensemble curve in this paper

 图 7 Krogh改进算法的逃离局部极小值过程 Fig. 7 Escape from local minima process under Krogh's improved algorithm

 图 8 Krogh改进算法的逃离局部极小值过程排斥力曲线 Fig. 8 Exclusion force curves for escape from local minimal value processes under Krogh's improved algorithm

 图 9 本文改进算法的逃离局部极小值过程 Fig. 9 The escape from local minima process under the improved algorithm in this paper

 图 10 本文改进算法的逃离局部极小值过程排斥力曲线 Fig. 10 The repulsive force curve of escape from local minimal value process under the improved algorithm in this paper

 图 11 传统人工势场算法出海港过程仿真轨迹 Fig. 11 Traditional artificial potential field algorithm out of the harbor process simulation trajectory

 图 12 本文改进人工势场算法出海港过程仿真轨迹 Fig. 12 This paper improves the artificial potential field algorithm out of the harbor process simulation trajectory
4 结　论

 [1] 栾天宇. 无人船自动驾驶系统的设计与实现[D]. 北京: 北京理工大学, 2018. [2] 陈永泽, 舒军勇, 王真亮, 等. 基于GPS定位的无人艇自主导航[J]. 重庆理工大学学报, 2016, 30(8): 117-121. [3] 田勇. 水面无人艇运动控制系统设计与实现[D]. 大连: 大连海事大学, 2016. [4] 石祥. 水面无人船控制系统设计与研究[D]. 上海: 上海海洋大学, 2015. [5] ADVISOR P, SHOCKEY I. A study of the feasibility of autonomous surface vehicles[J]. Journal of Marine Science and Application, 2013, 1-4. [6] 李明. 智能车辆避障路径规划及横向控制研究[D]. 大连: 大连理工大学, 2013. [7] LI Ronghui, QI Yunpeng, LI Tieshanet al. On path following of underactuated surface ships[M]. Norway, 2009: 751-756. [8] Fossen Thor I. , Guidance and control of ocean vehicles[M]: Norway, 1994 [9] 廖煜雷. 无人艇的非线性运动控制方法研究[D]. 哈尔滨: 哈尔滨工程大学, 2012. [10] 邵峰. 双体双推进无人船路径跟踪控制研究[D]. 青岛: 青岛大学, 2015. [11] 张新慈. 基于ROS的无人水面艇运动控制系统的设计[D]. 南京: 江苏科技大学, 2015. [12] Bertram Volker. Unmanned surface vehicles–a survey[J]. Skibsteknisk Selskab, 2008, 1-4. [13] 苏金涛. 无人水面艇路径规划[J]. 指挥控制与仿真, 2015(6): 36-40. DOI:10.3969/j.issn.1673-3819.2015.06.008 [14] 卢艳爽. 水面无人艇路径规划算法研究[D]. 哈尔滨: 哈尔滨工程大学, 2010.