基于VFH*的水面无人艇局部避障方法

魏新勇，黄烨笙，洪晓斌

(华南理工大学机械与汽车工程学院，广东 广州 510641)

0 引言

水面无人艇是具备高度非线性动力学特征，能在无人干预的情况下在各种复杂未知的水面环境下执行任务的新型载体，目前在海域巡逻、水质监控和海底测绘等领域具有巨大的应用需求。由于水面无人艇作业环境复杂，因此具备避障规划能力不仅是无人艇安全航行的保障，也是无人艇执行各种作业的前提。无人艇避障规划包括全局避障规划和局部避障规划。全局避障规划一般采取静态的方法实现，通过已知的海域信息，在航行前规划出一条从航行起点到终点的无碰路径；局部避障规划则根据实时的艇载传感器信息获取无人艇位姿和探测范围内的障碍物分布情况，从而根据局部信息实时规划下一时刻的无碰运动。由于海域中存在众多不可预见的未知障碍物，因此根据实时的传感参量进行局部避障规划具有重要的研究价值。

目前无人艇局部避障规划算法领域有不少学者进行研究并取得了相应的成果。H Kim等[1]提出了一种基于Theta*算法的新方法来实时规划无人艇路径；Aaron等[2]提出了基于标准和规则的无人艇局部避障策略，该策略根据时变海域环境和目标的碰撞危险度来确定；唐平鹏等[3-4]采用分层策略将动态窗口引用到了无人艇局部危险避障中；吴博等[5]提出了一种基于速度障碍法并考虑风浪流影响下的无人艇操纵运动特性的自主避碰算法。然而上述方法多为非预测型方法，且假设障碍物的位置和运动信息完全可知，在未知复杂的海洋环境以及无人艇高速行驶下应用具有一定的局限性。

向量场直方图法（vector field histogram，VFH）[6-8]常用于无人载体在未知环境下的局部避障规划，通过测距传感器信息建立向量场直方图进行两级环境数据压缩，考虑无人载体前向运动代价进而得出当前最优可行方向，具有实时性高、避障可靠等优点，近年来得到了很好的应用和发展。如F.Fraundorfer等[9]结合双目视觉信息，采用VFH+算法实现无人机在三维环境下的导航规划；孙扬智等[10]提出了一种融合卡尔曼滤波的VFH算法，消除移动机器人在目标点附近的转向抖动，实现了机器人在野外自主作业的动态路径规划；徐玉华等[11]则基于激光测距仪提出一种自适应调节阈值的VFH算法来实现移动机器人的自主避障。然而，目前VFH算法主要在移动机器人和无人机领域研究较多，而在水面无人艇研究领域涉及很少，且无人艇与机器人和无人机相比具有较大的长宽比，容易造成障碍物位于传感器探测盲区，发生尾端碰撞等问题。

因此，本文针对无人艇在未知海洋环境下的局部避障问题，以激光雷达前后数据帧融合构建的无人艇环境模型为基础，采用具有前向预测功能的VFH*局部避障规划算法，实现无人艇在每一瞬时的状态感知和局部避障决策。

1 基于VFH*的水面无人艇局部避障方法

1.1 无人艇水面环境模型构建

水面无人艇局部避障规划的准确实现首先依赖于其环境感知能力的好坏，激光雷达作为一种基于TOF测距原理的高精度、高分辨率的外围传感器，可以实时获取水面无人艇前向区域内障碍物的相对距离与方位，易于构建无人艇环境模型。

激光雷达数据获取简单易用，但传感器的探测范围存在一定的盲区且每次只能获取一帧数据，如果仅根据当前帧的数据进行环境建模容易与已经避开的障碍物发生碰撞。如图1所示，激光雷达安装在无人艇前端，无人艇在行进的过程中已经探测到障碍物并且进行规避，但随后由于障碍物位于传感器探测的盲区且无人艇艇身未完全通过障碍物，因此在无人艇转向的过程中容易发生尾端碰撞。针对此问题，提出结合激光雷达前后帧数据融合进行无人艇水面环境模型构建。

图1 无人艇尾端碰撞场景

首先通过三面角余弦公式计算两点之间的相对距离（其中R为地球半径）：

接着通过球面正弦公式计算两点之间的相对方位角：

根据相对距离和相对方位角求得平移坐标为

1.2 障碍物向量场直方图构建

VFH*局部避障规划算法采用栅格表示法将无人艇水面环境分解为二维直方图网格，每个网格的障碍物置信度通过激光雷达建立的环境模型信息不断更新，进而构建障碍物向量场直方图来表征无人艇的可行和不可行区域。

式中： (x0,y0)——无人艇几何中心的坐标；

(xi,yj) ——障碍物网格的坐标。

其中，di,j为 障碍物网格Ci,j到无人艇几何中心的距离；ci,j为概率函数，不同于传统的超声波传感器[12-13]需要对锥形区域内的障碍物网格置信度进行概率评估，激光雷达能够准确返回某一方位的距离测量值，因此，当障碍物网格Ci,j被障碍物覆盖时，ci,j的值设为1，反之，ci,j的值设为0。

其中：

图2 障碍物网格膨化处理

主直方图中每一个分区的障碍物密度定义为该分区最危险网格的障碍物向量强度。这是因为采用了激光雷达前后帧数据融合后，实际导入的环境模型中在径向上可能存在多个障碍物网格，采用传统的加权求和方式容易造成该方向上危险程度的误判。如图3所示的网格地图，无人艇此刻向左和向右转向避开正前方障碍物的概率是一致的，若采用加权求和的方式计算则导致无人艇更倾向于向左转向。

图3 无人艇网格地图

1.3 无人艇运动方向选取与前向预测寻优

向量场直方图的建立将无人艇周围环境划分可行区域和不可行区域。确定可行区域的左右边界kl和kr，如果左右边界的差值小于设定阈值Smax，那么此可行区域定义为窄谷区，选择中间方向作为候选方向：

图4 无人艇前向预测节点示意图

为了量化每一个初始候选方向的前进代价，定义搜索树上每一个节点的代价值由其代价函数和启发式函数构成，总的代价值为

由于无人艇在局部避障过程中需要以目标点为导向，因此还需满足以下条件：

2 实验结果与分析

2.1 仿真实验结果与分析

为了验证本文所述算法的局部避障性能，通过远程API函数建立Matlab与V-REP仿真平台之间的通信，进行无人艇局部避障性能的半实物仿真实验。

V-REP仿真平台具有丰富的传感器模块和执行机构模块，可以在Matlab窗口发送指令实时获取V-REP平台中无人艇载激光雷达、GPS和电子罗经等传感器信息，经过算法解算后将下一运动方向返回V-REP仿真平台的执行模块，控制无人艇在V-REP仿真平台下的运动。具体仿真环境如图5所示，可以在无人艇周围设置障碍物作为无人艇局部避障过程中的避碰对象，采用 SICK TiM310 Fast型号激光雷达作为障碍物测距传感器，安装在无人艇前端。

图5 V-REP仿真环境

图6是未结合激光雷达前后帧数据融合的无人艇局部避障仿真实验。如图6(a) 所示，在仿真初始阶段，无人艇能够顺利识别到障碍物并进行转向避碰，然而当障碍物处于激光雷达探测盲区时，无人艇受目标点导向的指引，持续左转，最终与障碍物发生碰撞，如图6(b)所示。

图6 未结合前后帧数据融合的仿真实验过程

图7 结合前后帧数据融合的仿真实验过程

图7是结合激光雷达前后帧数据融合的无人艇局部避障仿真实验，与前一仿真实验相似，在障碍物位于激光雷达探测盲区的情况下，无人艇在左转靠近目标点的过程中仍有可能与障碍物发生碰撞，但由于采用了激光雷达前后帧数据融合算法来建立无人艇环境模型，使得无人艇对之前探测过的障碍物仍具有一定的短期记忆功能，因此无人艇在当前时刻通过直行使整个艇体能过完全避开障碍物，然后再进行左转靠近目标点，如图7(b)所示。

图8为无人艇在两次仿真过程中输出的避障方向。可以看出，在未结合无人艇前后帧数据融合的仿真过程中，算法在6.7 s至12.6 s的时间段内持续输出向左的避碰方向，最终导致无人艇在转向过程中与障碍物发生碰撞；而在结合无人艇前后帧数据融合的仿真过程中，算法在7.3 s至7.7 s的过程中输出直行的避碰方向，使得无人艇在转向过程中能够短暂直行从而使艇体完全避开障碍物。

图8 无人艇避障方向

图9(a) 是算法未结合前向预测功能的无人艇局部避障仿真结果图，此时预测步长算法仅仅根据当前获取到的传感器信息进行局部寻优，导致无人艇在第一次避障时获得的局部最优转向（左转59°）并不是全局最优转向，使得无人艇在局部避障过程中陷入局部最小值，最终获得的避障轨迹稍显冗长。图9(b)是算法结合前向预测功能的无人艇局部避障仿真结果图，此时预测步长由于考虑了无人艇沿每个初始候选方向前进的代价，无人艇在第一次避障时选择右转64°作为当前的避障角度，使得无人艇能够跳出第一次避障时的局部最小值，从而能够获得较短的避障轨迹。

图9 无人艇局部避障仿真结果图

2.2 实物实验结果与分析

本文采用的实验平台是一艘艇长2m的全电力推进无人艇，如图10所示。环境感知传感器包括激光雷达和网络摄像头，通过无线路由模块返回岸基控制端；而位姿测量传感器GPS和电子罗经，则通过射频模块进行传输。艇载激光雷达选用SICK LMS200，理论最远探测范围为80m，最大探测角度范围为180°。艇载网络摄像头可以实时获取水面图像，从而监控无人艇行进过程。

图10 水面无人艇实验平台

为了验证本文所述算法的有效性，在水池内人为设置由泡沫箱组成的障碍物群，通过在障碍物群两侧预设无人艇起始点和目标点，使无人艇能够在航行过程中通过障碍物群。实艇实验环境设置如表1所示。无人艇启动后，可以在岸基控制端实时监控无人艇航行情况及其对障碍物群的避碰行为。

表1 实船实验各关键点经纬度

图11所示为水面无人艇在一个完整避障过程的轨迹示意图，其中绿色原点为无人艇起始点，红色圆点为无人艇需要到达的目标点，黄色圆点为水面障碍物。从实验过程中可以看到，无人艇在启动初始阶段，在不存在与障碍物碰撞危险的情况下，通过一系列的右转动作从而使无人艇的艏向能够对准目标点；直行一段距离之后，无人艇与障碍物1相遇，如图12所示，红色箭头表示当前时刻的可行方向，算法此时输出左转的可行方向控制无人艇成功避开障碍物；而后，无人艇在继续直行的过程中与障碍物2相遇，如图13所示，算法此时输出右转的避障方向控制无人艇成功避开障碍物；最后无人艇逐渐逼近目标点并最终成功到达。

3 结束语

图11 无人艇局部避障轨迹示意图

图12 无人艇与障碍物1会遇

图13 无人艇与障碍物2会遇

本文提出一种基于VFH*的水面无人艇局部避障方法。通过激光雷达前后帧数据融合建立水面无人艇环境模型，使得无人艇对已探测过的障碍物具有一定的短期记忆功能，弥补传感器探测盲区；并以此水面环境模型为基础，构建障碍物向量场直方图，选取候选方向以及前向预测寻优，最终获得无人艇在当前时刻的最优避障角度。V-REP平台仿真实验表明，所提出的算法能结合激光雷达数据得到合理的局部避障规划策略，避免无人艇在局部避障过程中出现尾端碰撞或陷入局部最小值的问题；通过水池实艇实验有效验证了算法在障碍物群中的危险规避能力和目标导向性，满足水面无人艇在实际环境中的局部避障需求。