一种用于水下无人航行器的局部避碰路径规划方法研究

李杨; 杜度; 于双宁; 毛柳伟; 徐雪峰; 徐野; LI Yang; DU Du; YU Shuangning; MAO Liuwei; XU Xuefeng; XU Ye

引 en

一种用于水下无人航行器的局部避碰路径规划方法研究

李杨¹

机构：

1. 海军研究院,北京 100161

×
，杜度¹

机构：

1. 海军研究院,北京 100161

×
，于双宁²

机构：

2. 天津航海仪器研究所九江分部,江西九江 332000

×
，毛柳伟¹

机构：

1. 海军研究院,北京 100161

×
，徐雪峰²

机构：

2. 天津航海仪器研究所九江分部,江西九江 332000

×
，徐野¹

机构：

1. 海军研究院,北京 100161

×

1. 海军研究院,北京 100161；
2. 天津航海仪器研究所九江分部,江西九江 332000；

Research on the Method of Local Collision Avoidance Path Planning for Unmanned Underwater Vehicle

LI Yang¹

Affiliation：

1. Naval Research Institute,Beijing 100161 ,China

×
， DU Du¹

Affiliation：

1. Naval Research Institute,Beijing 100161 ,China

×
， YU Shuangning²

Affiliation：

2. Jiujiang Branch of Tianjin Navigation Instrument Research Institute,Jiujiang 332000 ,China

×
， MAO Liuwei¹

Affiliation：

1. Naval Research Institute,Beijing 100161 ,China

×
， XU Xuefeng²

Affiliation：

2. Jiujiang Branch of Tianjin Navigation Instrument Research Institute,Jiujiang 332000 ,China

×
， XU Ye¹

Affiliation：

1. Naval Research Institute,Beijing 100161 ,China

×

1. Naval Research Institute,Beijing 100161 ,China；
2. Jiujiang Branch of Tianjin Navigation Instrument Research Institute,Jiujiang 332000 ,China；

作者简介:

李杨(1987-),男,博士,工程师,主要从事船舶导航与操纵控制技术研究。

中图分类号:V249.3

文献标识码:A

文章编号:2096-5753(2021)06-0520-05

DOI:10.19838/j.issn.2096-5753.2021.06.013

全文
评论
参考文献
出版信息

参考文献 1

倪生科，刘正江，蔡垚，等.基于遗传算法的船舶避碰决策辅助[J].上海海事大学学报，2017，38（1）：12-15.

查找原文

参考文献 2

吕红光，尹勇，尹建川，等.基于人工智能和软计算的船舶自动避碰决策算法[J].中国航海，2016，39（3）：12-15.

查找原文

参考文献 3

李春泽.基于多目标遗传算法的船舶自动避碰决策研究[D].大连：大连海事大学，2019.

查找原文

参考文献 4

ZENG X M，ITO M.Planning a collision avoidance model for ship using genetic algorithm[C]//2001 IEEE International Conference on Systems，Man and Cybernetics.IEEE，2002

查找原文

参考文献 5

TSOU M C，SUEH C K.The study of ship collision avoidance route planning by ant colony algorithm[J].Journal of Marine Science and Technology，2010，18（5）.

查找原文

参考文献 6

WOO J，KIM N.Collision avoidance for an unmanned surface vehicle using deep reinforcement learning[J].Ocean Engineering，2020，199c.

查找原文

参考文献 7

罗贤程.基于动态分阶势场法的船舶自动避碰研究[D].大连：大连海事大学，2019.

查找原文

参考文献 8

徐飞.基于改进人工势场法的机器人避障及路径规划研究[J].计算机科学，2016，43（12）：293-296.

查找原文

参考文献 9

吴博，熊勇，文元桥.基于速度障碍原理的无人艇自动避碰算法[J].大连海事大学学报，2014，40（2）：13-16.

查找原文

参考文献 10

谢礼飞.潜艇水下避碰辅助决策技术研究[D].哈尔滨：哈尔滨工程大学，2015.

查找原文

目录contents

摘要Abstract
关键词Keywords
0 引言
1 算法执行流程
2 具体实施
2.1 改进型人工势场法
2.2 速度障碍法
2.3 避碰危险度
2.4 根据危险度重新分配斥力
2.5 人工势场法与速度障碍法叠加
2.6 航路的柔顺性修正
3 仿真结果
4 结束语
参考文献

摘要

研究水下无人航行器（UUV）的局部自动避碰路径规划，对人工势场法和速度障碍法进行改进，提出一种在三维空间重新分配斥力的方法，联合应用人工势场法和速度障碍法，建立人工模拟势场求得合力，通过合力的方向指引水下无人航行器航行，完成对动态静态障碍物的避让。仿真结果表明：该方法能有效在动静态混合环境下完成避碰，且平滑处理后的路径更符合水下无人航行器实际航行避碰操纵控制要求。

Abstract

By studying the local collision avoidance path planning of underwater unmanned vehicle，and improving the artifical potential field method and the speed obstacle method，a method to redistribute the repulsive force in three-dimensional space is proposed，which combines the artifical potential field method and the speed obstacle method. The artifical simulation potential field is established to obtain the resultant force，and the underwater unmanned vehicle is guided through the direction of the resultant force to complete the avoidance of dynamic and static obstacles. The simulation results show that this method can effectively complete collision avoidance in a mixed dynamic and static environment，and the smoothed path is more suitable to the actual navigation and collision avoidance requirements of unmanned underwater vehicles.

关键词

水下无人航行器（UUV）；自动避碰；速度障碍法；人工势场法

Keywords

unmanned underwater vehicle（UUV）； automatic collsion avoidance ； speed obstacle method ； artificial potential field method

0 引言
水下无人航行器（UUV）的局部自动避碰是指UUV在航行过程中，遇到岛屿、船只、浮标等动态静态障碍物时，能够自动识别避让障碍物，完成指定任务。基于神经网络法、遗传算法、蚁群算法、粒子群算法等的智能避碰算法更加灵活，可应用范围广^[1-6]，但在实时性要求较高的工作环境中，过大的运算量会使得运算时间较长，无法保证实时性的规划航路输出。
人工势场法、速度障碍法具有运算量小，输入信息简单，较易实现等优点，因此，合理选择并改进传统避碰算法能够优化UUV避碰航路规划。文献^[7]引入速度斥力势函数，构建的动态势场解决了在动态环境中避障时容易产生无法绕开障碍物和避让行动不合理等问题。但未能结合载体的操纵性能规划路径，可能会导致载体不能跟随航路。文献^[8]改进了人工势场法，给机器人一个外力以避免其在局部最小点处停止或者徘徊，确保机器人能够逃出最小值陷阱并顺利到达目标位置，解决了人工势场法容易陷入最小值陷阱问题，但未能解决避让动态障碍物问题。
针对水下三维空间的避碰环境，本文提出了一种联合使用人工势场法和速度障碍法的避碰算法，能避让动态与静态障碍物，生成避碰路径，并根据UUV的操纵性能进行路径的柔顺性处理。
1 算法执行流程
将UUV的起点记为p₀，在 p₀处根据探测到的环境信息判断障碍物是否在UUV的领域内，再根据计算危险度决定是否应用人工势场法和速度障碍法，之后根据重新分配的斥力计算合力，合力的方向为在当前路径节点处UUV的指令航向。局部避障路径通过连接路径节点产生，路径节点由合力 F 和前进步长获得。在 p₀处按合力 F 方向取合适的步长后到达 p₁，在 p₁位置应用上述方法计算合力，沿此合力方向前进相同步长，以此类推直到到达下一子目标点，顺次连接 p_i，得到局部避障路径。算法流程图如图1所示。
图1 算法流程图
Fig.1 Algorithm flowchart
2 具体实施
2.1 改进型人工势场法
定义引力势场为UUV与目标位置相对位置的函数：

U_{att} (ρ_{goal}) = \frac{1}{2} k_{a} ρ_{goal}^{2}

(1)

式中： $k_{a}$ 为正常数是引力势场系数； $ρ_{goal}$ 为UUV当前位置与目标点间的距离。
假设UUV所处航行环境中有 N 个碍航物，第 i 个碍航物与UUV间的斥力势函数为

U_{repi} = k_{r} \frac{1}{k_{s} (ρ_{i} - ρ_{0})}

(2)

式中： $k_{r}$ 为斥力势场强度系数，调节斥力大小； $k_{s}$ 为范围缩放系数，调节势场的有效作用范围； $ρ_{i}$ 为第 i 个碍航物与UUV间的距离； $ρ_{0}$ 为一正常数，表示碍航物的尺度范围。将引力势场与斥力势场叠加可得全局势场为

U = U_{a t t} + \sum_{i = 1}^{N} U_{r e p i}

(3)

引力势场的负梯度为UUV所受引力，方向由UUV指向目标点：

F_{a t t} (ρ_{g o a l}) = - \nabla U_{a t t} = - k_{a} ρ_{g o a l}

(4)

斥力势函数的负梯度为作用于UUV的斥力，方向由碍航物指向UUV：

F_{repi} = - \nabla U_{repi} = \frac{k_{r} k_{s}}{{[k_{s} (ρ_{i} - ρ_{0})]}^{2}}

(5)

此斥力的构建不作为分段函数的形式设置，是为防止在进入分段函数设置的临界距离后突然增大的斥力使得航路急转，以免UUV无法跟随。
2.2 速度障碍法
人工势场法适用于对静态障碍物的避碰规划，但当UUV航行区域内有动态的障碍物时，人工势场法仅能根据动态障碍物的位置信息进行规划，无法根据动态障碍物的速度信息进行灵活处理，针对动态障碍物，可以利用速度障碍法进行避障规划。
应用速度障碍法时，首先进行碰撞区域的判断，将障碍物进行膨化处理后，建立UUV–障碍物（U–O）环境模型，确定碰撞区域，如图2所示，建立U–O坐标系。在 t 时刻，UUV向障碍物两侧切线方向发射射线 L₁ 和 L₂， L₁ 和 L₂ 与 x 轴的夹角分别为 $θ_{1}$ 和 $θ_{2}$ ，水下无人航行器A和障碍物B的速度分别为V_a 和V_b，两者相对速度为 $V_{R} = V_{a} - V_{b}$ ， V_R 与 x 轴的夹角为 $θ_{a}$ 。如果 $θ_{1} < θ_{a} < θ_{2}$ ，则认为UUV将驶向碰撞区域，需要进行规避，可根据UUV与障碍船的相对速度指向位于二者连线的左侧或右侧进行左转或右转操作^[9]。
图2 速度障碍法碰撞区域示意图
Fig.2 Schematic diagram of collision area
在人工势场法斥力与引力的基础上，在垂直UUV当前航向方向上叠加一个转向力 $F_{Turni}$ ，在数值上设置为越靠近 OA 连线则转向力就越大：

F_{Turni} = k_{s} F_{att} \frac{2 m i n \{(θ_{ai} - θ_{1}), (θ_{2} - θ_{ai})\}}{(θ_{2} - θ_{1})}

(6)

设 OA 连线与 x 轴夹角为 $θ_{0}$ ，如果相对速度的指向与 x 轴夹角 $θ_{1} < θ_{a} < θ_{0}$ ，F_Turni 应指向航向的右侧，如果相对速度的指向与 x 轴夹角 $θ_{0} < θ_{a} < θ_{2}$ ， F_Turni 应指向航向的右侧。
合力 F 为来自目标引力、障碍物的斥力以及由速度障碍法计算的转向力的叠加。

F = F_{a t t} + \sum_{i = 1}^{N} F_{r e p i} + \sum_{i = 1}^{N} F_{T u r n i}

(7)

2.3 避碰危险度
将水平面的最近会遇距离（DCPA）和垂直面的到达最近会遇点的时间（HCPA）作为影响碰撞危险度最大的因素，各个因素的模糊模型设置如下：
$\begin{matrix} U (D C P A) = \\ \{\begin{matrix} 1 & | D C P A | ⩽ d_{1} \\ \frac{1}{2} - \frac{1}{2} s i n [\frac{π}{d_{2} - d_{1}} (| D C P A | - \frac{d_{1} + d_{2}}{2})] & d_{1} < | D C P A | ⩽ d_{2} \\ 0 & | D C P A | > d_{2} \end{matrix} \end{matrix}$

U (H C P A) = \{\begin{matrix} \frac{1}{h_{2} - | H C P A |} & | H C P A | ⩽ h \\ h_{2} - h_{1} & h_{1} < | H C P A | ⩽ h_{2} \\ 0 & | H C P A | > h_{2} \end{matrix}

(8)

其中 U(DCPA) 称为水平面上的空间碰撞危险度（SCR），主要受最近会遇距离、相对方位、UUV操纵性、能见度、航行水域情况等因素影响； U(HCPA) 为垂直面上的碰撞危险度模型。d₁ 为水平面上的安全会遇领域， $K_{1} \cdot S D A$ 为水平面上的UUV安全通过距离， h₁ 为垂直面上的安全会遇领域， $h_{2} = 2 h_{1}$ 。UUV只有水平面和垂直面的碰撞危险度同时不为0时，才存在一定的碰撞危险，其余情况下不存在碰撞危险^[10]。所以UUV总的危险度为

\begin{matrix} U_{A} = \\ \{\begin{matrix} m i n {U (D C P A), U (H C P A)} & , & U (D C P A) \cdot U (H C P A) \neq 0 \\ 0 & , & 其他 \end{matrix} \end{matrix}

(9)

2.4 根据危险度重新分配斥力
UUV水下航路规划区别于水面船，水下的航路是三维的，受到的障碍物斥力也是三维空间内的一个矢量。将UUV的水下机动分解为水平面上的转向机动与竖直方向上的俯仰机动，显然UUV在水平面和竖直方向上的机动性和UUV自身空间尺度差别很大，由这个三维空间的斥力指引UUV机动不合适，需要对斥力进行重新分配，本文提出了一种重新分配斥力的方法。由斥力建模公式决定的斥力分布随距离衰减是非常明显的，因此，距离UUV较远的障碍物对UUV的影响可以忽略， U(HCPA) 与U(DCPA) 都等于0时，斥力的分配可以沿势场梯度方向。对于距离较近 U(HCPA) 与 U(DCPA) 都等于1时，按照比重相同进行分配。 U(HCPA) 与U(DCPA) 相差较大时，按较大危险度分配斥力较小原则分配斥力：

\begin{matrix} F_{levi} = F_{repi} \cdot \sqrt{\frac{U (H C P A)}{U (H C P A) + U (D C P A)}} \\ F_{veri} = F_{repi} \cdot \sqrt{\frac{U (D C P A)}{U (H C P A) + U (D C P A)}} \end{matrix}, U (H C P A) \cdot U (D C P A) \neq 0

(10)

式中： F_levi 为第 i 个障碍物提供的水平方向斥力； F_veri为第 i 个障碍物提供的竖直方向斥力。对于较大的危险度，说明距离较近，避让所需的机动越大，可能出现避让不及的情况，因此在另外一个方向上采取机动进行规避是较好的选择。
2.5 人工势场法与速度障碍法叠加
应用人工势场法叠加速度障碍法解决了UUV针对静态与动态障碍物的避碰规划，人工势场法中由目标点提供引力，障碍物提供斥力；速度障碍法中如果相对速度指向速度障碍区则会添加一个垂直当前航向的偏转力，将偏转力叠加进人工势场法中，求得合力方向即为UUV的前进方向。
在避让静态障碍物时，应用速度障碍法可以有效避免陷入人工势场法的最小值陷阱问题，并能使路径更加光顺。在人工势场法中，由障碍物产生的斥力场随距离衰减很快，在控制UUV进行避让时，会出现转向过急现象，如果调节参数减缓随距离衰减的程度，则会使斥力作用范围过大，减少可通行范围，造成空间浪费。因此在避让静态障碍物时，将障碍物看做静止的运动目标，相对速度变为UUV自身的速度，应用速度障碍法，控制UUV提前转向。2种算法叠加使用，相互配合，应用速度障碍法防止转向过急，应用人工势场法控制最近接近距离，可同时保证安全性与可操控性。
2.6 航路的柔顺性修正
在航路的柔顺性方面，除了叠加人工势场法与速度障碍法防止转弯过急外，结合UUV的最小回转半径、最大潜浮速度等机动性能，得到单步最大转向角度 $γ$ 和单步最大竖直方向变动深度 h 后进行约束：

\begin{matrix} α_{i + 1} = \{\begin{matrix} α_{i + 1} & , |α_{i + 1} - α_{i}| \leq γ \\ α_{i} + γ & , α_{i + 1} - α_{i} > γ \\ α_{i} - γ & , α_{i + 1} - α_{i} < - γ \end{matrix} \\ H_{i + 1} = \{\begin{matrix} H_{i + 1}, |H_{i + 1} - H_{i}| \leq h \\ H_{i} + h, H_{i + 1} - H_{i} > h \\ H_{i} - h, H_{i + 1} - H_{i} < - h \end{matrix} \end{matrix}

(11)

式中： $α_{i}$ 为 p_i点处规划前进方向在水平面上投影后与 x 轴的夹角； H_i 为p_i点处规划指令深度。
3 仿真结果
对本文提出的联合应用人工势场法和速度障碍法的避碰算法进行仿真实验验证，UUV出发点位置为（0，0，–100），终点位置为（10 000，10 000， –100），航速为6kn；动态障碍物出发点位置为（9 000，0，–100），终点为（0，9 000，–100），航速为6.35kn；静态障碍物坐标为（3 000，2 500， –50），（6 000，2 500，0），（6 000，6 000，–100），仿真结果如图3所示。
图3 仿真路径结果图
Fig.3 Simulation path result
4 结束语
仿真结果表明，叠加应用人工势场法和速度障碍法可以应用于同时存在动态与静态障碍物的环境中，有效解决人工势场法中部分极小值问题，并且能使路径更为柔顺，保证UUV能跟随，算法运算量小、实时性高，适合应用于UUV局部避碰路径规划中。
参考文献
- [1] 倪生科，刘正江，蔡垚，等.基于遗传算法的船舶避碰决策辅助[J].上海海事大学学报，2017，38（1）：12-15.
- [2] 吕红光，尹勇，尹建川，等.基于人工智能和软计算的船舶自动避碰决策算法[J].中国航海，2016，39（3）：12-15.
- [3] 李春泽.基于多目标遗传算法的船舶自动避碰决策研究[D].大连：大连海事大学，2019.
- [4] ZENG X M，ITO M.Planning a collision avoidance model for ship using genetic algorithm[C]//2001 IEEE International Conference on Systems，Man and Cybernetics.IEEE，2002
- [5] TSOU M C，SUEH C K.The study of ship collision avoidance route planning by ant colony algorithm[J].Journal of Marine Science and Technology，2010，18（5）.
- [6] WOO J，KIM N.Collision avoidance for an unmanned surface vehicle using deep reinforcement learning[J].Ocean Engineering，2020，199c.
- [7] 罗贤程.基于动态分阶势场法的船舶自动避碰研究[D].大连：大连海事大学，2019.
- [8] 徐飞.基于改进人工势场法的机器人避障及路径规划研究[J].计算机科学，2016，43（12）：293-296.
- [9] 吴博，熊勇，文元桥.基于速度障碍原理的无人艇自动避碰算法[J].大连海事大学学报，2014，40（2）：13-16.
- [10] 谢礼飞.潜艇水下避碰辅助决策技术研究[D].哈尔滨：哈尔滨工程大学，2015.

基本信息

中图分类号: V249.3
文献标识码: A
DOI: 10.19838/j.issn.2096-5753.2021.06.013
文章编号: 2096-5753(2021)06-0520-05

基金信息

引用信息

稿件历史

收稿日期: 2021-10-19

参考文献

[1] 倪生科，刘正江，蔡垚，等.基于遗传算法的船舶避碰决策辅助[J].上海海事大学学报，2017，38（1）：12-15.
[2] 吕红光，尹勇，尹建川，等.基于人工智能和软计算的船舶自动避碰决策算法[J].中国航海，2016，39（3）：12-15.
[3] 李春泽.基于多目标遗传算法的船舶自动避碰决策研究[D].大连：大连海事大学，2019.
[4] ZENG X M，ITO M.Planning a collision avoidance model for ship using genetic algorithm[C]//2001 IEEE International Conference on Systems，Man and Cybernetics.IEEE，2002
[5] TSOU M C，SUEH C K.The study of ship collision avoidance route planning by ant colony algorithm[J].Journal of Marine Science and Technology，2010，18（5）.
[6] WOO J，KIM N.Collision avoidance for an unmanned surface vehicle using deep reinforcement learning[J].Ocean Engineering，2020，199c.
[7] 罗贤程.基于动态分阶势场法的船舶自动避碰研究[D].大连：大连海事大学，2019.
[8] 徐飞.基于改进人工势场法的机器人避障及路径规划研究[J].计算机科学，2016，43（12）：293-296.
[9] 吴博，熊勇，文元桥.基于速度障碍原理的无人艇自动避碰算法[J].大连海事大学学报，2014，40（2）：13-16.
[10] 谢礼飞.潜艇水下避碰辅助决策技术研究[D].哈尔滨：哈尔滨工程大学，2015.

首页

本刊简介

编委会

收录情况

期刊订阅

征稿简则

联系我们

分享给微信好友或者朋友圈

使用微信“扫一扫”功能。

一种用于水下无人航行器的局部避碰路径规划方法研究

Research on the Method of Local Collision Avoidance Path Planning for Unmanned Underwater Vehicle

摘要

Abstract

关键词

Keywords

0 引言

1 算法执行流程

2 具体实施

2.1 改进型人工势场法

(1)

(2)

(3)

(4)

(5)

2.2 速度障碍法

(6)

(7)

2.3 避碰危险度

(8)

(9)

2.4 根据危险度重新分配斥力

(10)

2.5 人工势场法与速度障碍法叠加

2.6 航路的柔顺性修正

(11)

3 仿真结果

4 结束语

参考文献

基本信息

基金信息

引用信息

稿件历史

参考文献