基于速度矢量合成的AUV 在线路径规划研究*

徐炜翔 朱志宇

（江苏科技大学电子信息学院 镇江 212003）

1 引言

AUV（Autonomous Underwater Vehicle）作为一类表现优异的无人水下作业载具逐渐成为了研究的热点［1］。其自主性很大程度上体现在AUV 具有根据已有的环境地图信息和未知的传感器环境信息来自主规划任务路径和自主识别并躲避障碍物的能力。而AUV在线路径规划就是AUV路径规划任务中不可或缺的一环，作为对全局路径规划的补充，针对AUV 前进路径上出现的未知移动障碍物进行碰撞预测并提前采取改变航向航速等策略做出避碰动作，确认安全之后再回到已规划好的主路径上来［2］。因此，AUV 的在线路径规划对整体任务的安全性至关重要，研究AUV 的实时自主避碰策略具有重要意义［3］。

本文研究AUV 在水下未知环境中自主避碰问题，即根据如多波束声呐、双目摄像机等传感器实时数据，获取障碍物位置信息实时预测障碍物轨迹，提前采取适当策略进行自主避碰［4］。该问题的难点在于传感器探测范围有限导致的信息处理时间短和AUV 避碰动作受自身性能约束等因素限制无法实现快速制动与转向。国内AUV 路径规划技术在十几年的发展过程中，主要研究集中在静态环境或单一动态障碍物环境，对于复杂动态障碍物环境下的自主避碰研究较少［5］。

现有的AUV 动态避障方法主要有人工势场法、遗传算法、滚动栅格法、模糊神经网络、速度矢量法［6~9］等。例如，毛宇锋［10］等利用水下航行器与动态障碍物的相对位置与速度关系建立速度矢量坐标系，根据相对速度判断碰撞风险，对有碰撞风险的目标采取最小转向原则求得AUV 得安全避碰路径。严浙平［11］等将动态障碍物位置速度、AUV位置速度等信息进行二进制编码简化将二位信息简化为一维信息，将AUV 的下一步航向及速度作为输出采用遗传算法求得无碰路径。王秀芳［12］首先采用滚动栅格实时更新障碍物信息构建环境模型，然后采用生物启发模型将栅格地图与神经网络拓扑结构相结合，通过计算神经元活性值寻找安全路径。这些方法往往有一些缺陷，如人工势场法容易陷入局部最优，遗传算法等启发式算法搜索时间长无法满足实时性，滚动栅格法无法应用于复杂环境等。如何做到简化环境模型，实时处理信息及时反应做出避碰动作就成为一直以来研究的难点。

2 环境模型构建

环境模型包括前视声呐模型、AUV 模型、障碍物模型及相对坐标系模型，对这些因素分别进行精准的建模是完成路径规划的基础。

2.1 声呐模型

本文采用二维虚拟多波束声呐进行研究［13］，设定声呐的探测距离为150m，多波束数量为80 条，水平开角为120°，工作频率为2Hz。当障碍物进入声呐探测范围时，求出其与相应波束之间的交点坐标与AUV 坐标得到障碍物距离信息，若无交点则返回最大数值表示未探测到障碍物，之后将得到的信息传递到规划程序进行实时避碰。

2.2 AUV与障碍物模型

水下环境中的障碍物多种多样，大多形状难以具体描述。本文以障碍物最大边界为直径d作外接圆来表示障碍物的实际大小并对其边界作膨化处理，即在外接圆外增加一个安全阈值m，阈值m的大小由AUV 的最大直径确定。其构造如图所示。当存在多个障碍物距离较近时，将其视作一个大型障碍物处理。同理，AUV 也以最大直径作外接圆表示［14］。

2.3 相对坐标系

为了统一描述AUV 运动状态和障碍物运动状态，分别建立固定坐标系（即大地坐标系）与运动坐标系。固定坐标系取环境中任意一定点，两坐标轴互相垂直。运动坐标系取AUV 质心作为原点，横轴沿AUV 中线指向艇艏，纵轴垂直与横轴垂直。两坐标系平移原点重合后的旋转角为θ，则运动坐标系与大地坐标系的变换矩阵可表示为

3 障碍物轨迹预测

根据不同时段收集到的位置信息做到对不同障碍物精确可靠的运动轨迹预测是AUV 动态避碰的基础［15］。对于动态障碍物，需要预测目标的位置坐标、速度、航向、加速度等关键信息。在复杂海洋环境中，动态障碍物运动状态多种多样，可预测的障碍物运动轨迹一般为匀速直线运动或匀变速直线运动。因为卡尔曼滤波相较于普通低通滤波方法具有无延时和信号滞后的优点可以对目标状态进行实时状态估计而广泛运用于运动估计，且只需要输入当前由传感器采集到的测量值和上一个周期的估计值就能估计当前的状态，所以本文采用扩展卡尔曼滤波算法对移动障碍物运动状态作预测与估计。

在二维平面中，移动障碍物的运动轨迹模型可表示为

在建立障碍物的数学模型之后，就可以通过卡尔曼滤波对障碍物进行精确的估计和预测。标准卡尔曼滤波算法的适用前提必须是线性系统，当障碍物做非线性运动时，标准卡尔曼滤波算法不能将障碍物的加速运动看作随机噪声，所以当障碍物做变速运动时标准卡尔曼滤波算法会失效。此时采用扩展卡尔曼滤波算法进行状态估计［16］，扩展卡尔曼滤波算法通过雅克比矩阵将期望和方差线性化，使卡尔曼滤波适用于非线性系统问题的估计。

4 基于速度合成算法的动目标避碰

面对移动障碍物的威胁，通常有三种避碰策略［17］。第一种是保持AUV 航向不变通过改变AUV航速，即AUV 的加速或减速动作实现避碰。这种方法需要AUV 能够短时间内实现增速或减速，对于中小型AUV 其推进器推力往往不足以灵活调整航速，而且一旦减速之后再加速回到巡航速度能耗巨大。第二种是保持AUV 航速不变通过改变AUV航向，即调整AUV 前进方向的艏摇角度实现避碰。这种方法受限于AUV 自身转向性能影响难以实际运用。第三种综合了改变航向和改变航速来及时调整AUV 姿态实现避碰，这种方法能耗最小且更加灵活。本文采用第三种避碰策略实现AUV动态避碰。

4.1 障碍物威胁预测

通过声呐等传感器采集到障碍物信息时并对障碍物轨迹进行状态估计之后，首先要对障碍物威胁进行评估，判断障碍物与AUV 前进路径的最小距离是否小于安全距离从而决策是否采取避碰行为。

在大地坐标系中，假设AUV 的安全半径为m，障碍物的半径为n。AUV 的位置坐标和速度分别为Pauv(x1，y1) 、vauv(vx1，vy1) ，障碍物的位置坐标和速度分别为Pobs(x2，y2)，vobs(vx2，vy2)可 得AUV 与障碍物在水平方向与垂直方向距离差分别为(xd，yd)，水平方向与垂直方向速度差分别为(vxd，vyd)，即得到二者的相对距离与相对速度。通过式（3）可得AUV 与障碍物路径相交时所需时间t。

将t带入式（4）可得AUV与障碍物得最小距离Dt。

将Dt与安全距离m比较，若Dt小于m则进入下一步避碰程序，反之则AUV 按原速度与航向继续行进。

4.2 期望航向与航速求解

通过障碍物威胁预测模型可知，AUV 的避碰目的就是使AUV 与障碍物的相对速度在交会时间内所驶过的相对距离大于安全距离从而解除碰撞威胁。本文避碰策略为综合避碰策略，所以，AUV的避碰行为由航向更新机制和航速更新机制两部分共同组成。

1）航向更新机制

已知AUV 在无障碍环境中，水平坐标系中航向角角为θ，AUV 的速度矩阵vauv和障碍物的速度矩阵vobs，可得AUV 和障碍物相对速度vre=vauvvobs，其模值为 ||vre。根据相对速度可导出其与AUV速度夹角α的关系如下。

在水平坐标系中，AUV 与障碍物相对位置的改变可以用AUV 质点与障碍物质点的连线，AUV与障碍物膨化模型过连线的切线，两条线之间的夹角表示，交点的位置坐标可由式（6）推导。

在得到交点坐标后，AUV 与障碍物边界同侧下切线的夹角β可以由障碍物与交点连线角tan(Pobs，Pi)减去上切线与质点连线角tanδ=得到。此时，

而航向角更新系数即障碍物同侧切线角与相对速度角差的绝对值，即

根据AUV 速度正切值tanvauv和相对速度正切值tanvre的大小关系判断AUV航向变化的方向，并得到AUV的期望航向。

2）航速更新机制

根据式（5）可知，AUV 与障碍物相对速度的变化量为 ||vre×sinα，则更新后的航速为

AUV 在判定与障碍物存在碰撞风险之后即进入自主避碰程序，通过预测障碍物运动轨迹，实时调整航向、航速。在与障碍物距离到威胁范围之外后，重新根据目标点位置距离最短原则调整航向，并回复到巡航航速以节约能量消耗。

5 仿真实验与结果分析

为验证本文算法的可行性，在Matlab2016b 环境下进行仿真试验。AUV 的基本参数如下，声呐探测范围为150m，AUV 安全直径3m，巡航速度为2.5m/s，最大航速为3.5m/s。安全距离为50m，威胁距离为5m。转向角最大/最小控制输入为aθmax=2°/s，aθmin=-2°/s。

5.1 障碍物轨迹估计试验

首先以二维平面空间中匀变速直线运动为例，采用扩展卡尔曼滤波算法进行预测，预测结果如图2所示。

图2 轨迹测量位置曲线与预测位置曲线

从仿真结果可以看出，扩展卡尔曼滤波算法很好的剔除了噪声，预测曲线十分理想。试验证明，扩展卡尔曼滤波算法能够很好地对变速目标进行状态估计和预测。

5.2 单个动目标避碰试验

第二项是在二维平面中对AUV 进行动态避碰仿真试验。AUV 的起点位置坐标为( 0 ，0 )，终点位置坐标为(1 00，100 )。障碍物直径5m ，从左上方往右下方运动，沿x轴速度为0.5m/s，沿y轴速度为-2m/s。规划结果如图3所示。

图3 单个动碍物避碰路径示意图

从图3 可以看出AUV 在动碍物在探测到进入威胁距离后改变航向开始进行自主避碰，在确认离开威胁距离后再次改变航向以最短路径向目标点前进。图4 是AUV 与障碍物距离曲线示意图。从图中可以看出，AUV 与障碍物最近点约为10m，满足AUV避碰要求。

图4 AUV与障碍物距离曲线

图5 是AUV 速度变化曲线，可以看出AUV 在进入自主避碰决策中，不仅改变了航向且同时调整了航速。经过一段时间的加速航行确认驶出威胁距离后降速回到正常巡航航速，整个加速过程在AUV 最大航速范围内。图6 反应的是AUV 与初始点到目标点连线的角度变化，可以看出两次调整航向都在AUV转向角输出控制范围内。

图5 AUV速度变化曲线

图6 AUV航向变化图

5.3 复杂动目标避碰试验

第三项是针对复杂环境下多个动态障碍物威胁时的AUV 自主避碰仿真试验，包括匀速运动障碍物和匀变速运动障碍物。AUV 起点位置坐标( 0 ，0 )，终点位置坐标( 2 50，250 )，共有4 个移动障碍物。第一个障碍物直径3m，从左上往右下方向移动，x 轴速度为0.5m/s，y 轴速度为-2m/s。第二个障碍物直径7m，从正下往正上方向移动，y 轴初速度为0，加速度为0.1m/s，y轴速度为-2m/s。第三个障碍物直径5m，从右下往左上方向移动，x 轴速度为-0.7m/s，y 轴速度为0.7m/s。第四个障碍物直径4m，从左往右移动，x 轴初速度为0，加速度为0.1m/s，y轴速度为0m/s。规划结果如图5所示。

从图7 可以看出AUV 在前进过程中多次改变航向，从整体视角来看，AUV 连续避碰效果良好。图8反映了AUV与各个障碍物的距离关系，可以看出AUV 通过避碰行为全程与障碍物保持在安全距离之内。

图7 多个动碍物避碰路径

图8 AUV与障碍物距离曲线

图9 和图10 分别反映了AUV 速度与航向的变化情况。从图中可以看出，航速最高为3.5m/s，并在离开威胁距离后迅速回到正常巡航状态。AUV航速与航向的调整是相对应的，通过混合调整的策略达到降低能耗提高灵活性的目的，仿真可知此方法是有效的。

图1 障碍物模型示意图

图9 AUV速度变化曲线

图10 AUV航向变化图

6 结语

本文研究了未知环境下针对动态障碍物的AUV 在线路径规划问题。首先，通过AUV 搭载的前视声呐等传感器收集到的障碍物信息进行建模，采用扩展卡尔曼滤波算法对移动障碍物进行状态估计，预测障碍物轨迹并判断碰撞威胁程度；然后，通过AUV 与运动障碍物之间的相对运动关系，基于速度合成法采用改变航速和改变航向的综合避碰策略，及时做出避障动作实现实时避碰。最后通过一系列仿真试验验证了该方法的有效性。本文存在两方面不足，一是没有拓展到复杂三维空间环境中，二是对于动态障碍物的模型没有考虑更多的可能性，如做弧形运动的障碍物等，后续还需要继续研究。