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

李 杨，杜 度，于双宁，毛柳伟，徐雪峰，徐 野

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

0 引言

水下无人航行器（UUV）的局部自动避碰是指UUV在航行过程中，遇到岛屿、船只、浮标等动态静态障碍物时，能够自动识别避让障碍物，完成指定任务。基于神经网络法、遗传算法、蚁群算法、粒子群算法等的智能避碰算法更加灵活，可应用范围广[1-6]，但在实时性要求较高的工作环境中，过大的运算量会使得运算时间较长，无法保证实时性的规划航路输出。

人工势场法、速度障碍法具有运算量小，输入信息简单，较易实现等优点，因此，合理选择并改进传统避碰算法能够优化UUV避碰航路规划。文献[7]引入速度斥力势函数，构建的动态势场解决了在动态环境中避障时容易产生无法绕开障碍物和避让行动不合理等问题。但未能结合载体的操纵性能规划路径，可能会导致载体不能跟随航路。文献[8]改进了人工势场法，给机器人一个外力以避免其在局部最小点处停止或者徘徊，确保机器人能够逃出最小值陷阱并顺利到达目标位置，解决了人工势场法容易陷入最小值陷阱问题，但未能解决避让动态障碍物问题。

针对水下三维空间的避碰环境，本文提出了一种联合使用人工势场法和速度障碍法的避碰算法，能避让动态与静态障碍物，生成避碰路径，并根据UUV的操纵性能进行路径的柔顺性处理。

1 算法执行流程

将UUV 的起点记为p0，在p0处根据探测到的环境信息判断障碍物是否在UUV的领域内，再根据计算危险度决定是否应用人工势场法和速度障碍法，之后根据重新分配的斥力计算合力，合力的方向为在当前路径节点处UUV的指令航向。局部避障路径通过连接路径节点产生，路径节点由合力F和前进步长获得。在p0处按合力F方向取合适的步长后到达p1，在p1位置应用上述方法计算合力，沿此合力方向前进相同步长，以此类推直到到达下一子目标点，顺次连接pi，得到局部避障路径。算法流程图如图1所示。

图1 算法流程图Fig.1 Algorithm flowchart

2 具体实施

2.1 改进型人工势场法

定义引力势场为UUV与目标位置相对位置的函数：

式中：ka为正常数是引力势场系数；ρgoal为UUV当前位置与目标点间的距离。

假设UUV所处航行环境中有N个碍航物，第i个碍航物与UUV间的斥力势函数为

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

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

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

此斥力的构建不作为分段函数的形式设置，是为防止在进入分段函数设置的临界距离后突然增大的斥力使得航路急转，以免UUV无法跟随。

2.2 速度障碍法

人工势场法适用于对静态障碍物的避碰规划，但当UUV航行区域内有动态的障碍物时，人工势场法仅能根据动态障碍物的位置信息进行规划，无法根据动态障碍物的速度信息进行灵活处理，针对动态障碍物，可以利用速度障碍法进行避障规划。

应用速度障碍法时，首先进行碰撞区域的判断，将障碍物进行膨化处理后，建立UUV–障碍物（U–O）环境模型，确定碰撞区域，如图2所示，建立U–O坐标系。在t时刻，UUV向障碍物两侧切线方向发射射线L1和L2，L1和L2与x轴的夹角分别为θ1和θ2，水下无人航行器A和障碍物B的速度分别为Va和Vb，两者相对速度为VR=Va-Vb，VR与x轴的夹角为θa。如果θ1＜θa＜θ2，则认为UUV将驶向碰撞区域，需要进行规避，可根据UUV与障碍船的相对速度指向位于二者连线的左侧或右侧进行左转或右转操作[9]。

图2 速度障碍法碰撞区域示意图Fig.2 Schematic diagram of collision area

在人工势场法斥力与引力的基础上，在垂直UUV当前航向方向上叠加一个转向力FTrepi，在数值上设置为越靠近OA连线则转向力就越大：

设OA连线与x轴夹角为θ0，如果相对速度的指向与x轴夹角θ1＜θa＜θ0，FTurni应指向航向的右侧，如果相对速度的指向与x轴夹角θ0＜θa＜θ2，FTurni应指向航向的右侧。

合力F为来自目标引力、障碍物的斥力以及由速度障碍法计算的转向力的叠加。

2.3 避碰危险度

将水平面的最近会遇距离（DCPA）和垂直面的到达最近会遇点的时间（HCPA）作为影响碰撞危险度最大的因素，各个因素的模糊模型设置如下：

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

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

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

式中：Flevi为第i个障碍物提供的水平方向斥力；Fveri为第i个障碍物提供的竖直方向斥力。对于较大的危险度，说明距离较近，避让所需的机动越大，可能出现避让不及的情况，因此在另外一个方向上采取机动进行规避是较好的选择。

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

应用人工势场法叠加速度障碍法解决了UUV针对静态与动态障碍物的避碰规划，人工势场法中由目标点提供引力，障碍物提供斥力；速度障碍法中如果相对速度指向速度障碍区则会添加一个垂直当前航向的偏转力，将偏转力叠加进人工势场法中，求得合力方向即为 UUV的前进方向。

在避让静态障碍物时，应用速度障碍法可以有效避免陷入人工势场法的最小值陷阱问题，并能使路径更加光顺。在人工势场法中，由障碍物产生的斥力场随距离衰减很快，在控制 UUV进行避让时，会出现转向过急现象，如果调节参数减缓随距离衰减的程度，则会使斥力作用范围过大，减少可通行范围，造成空间浪费。因此在避让静态障碍物时，将障碍物看做静止的运动目标，相对速度变为 UUV自身的速度，应用速度障碍法，控制UUV提前转向。2种算法叠加使用，相互配合，应用速度障碍法防止转向过急，应用人工势场法控制最近接近距离，可同时保证安全性与可操控性。

2.6 航路的柔顺性修正

在航路的柔顺性方面，除了叠加人工势场法与速度障碍法防止转弯过急外，结合UUV的最小回转半径、最大潜浮速度等机动性能，得到单步最大转向角度γ和单步最大竖直方向变动深度h后进行约束：

式中：iα为pi点处规划前进方向在水平面上投影后与x轴的夹角；Hi为pi点处规划指令深度。

3 仿真结果

对本文提出的联合应用人工势场法和速度障碍法的避碰算法进行仿真实验验证，UUV出发点位置为（0，0，–100），终点位置为（10 000，10 000，–100），航速为 6 kn；动态障碍物出发点位置为（9 000，0，–100），终点为（0，9 000，–100），航速为6.35 kn；静态障碍物坐标为（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局部避碰路径规划中。