基于VFH+算法的机器人实时地图创建与避障

王 韬，刘 金，马永起

(中国工程物理研究院计算机应用研究所，四川 绵阳 621900)

0 引言

智能移动机器人是一类能够通过传感器感知环境和自身状况，实现在有障碍物的环境中面向目标的自主运动，从而完成一定作业功能的机器人系统。避障算法的实时有效性极大地影响着移动机器人任务完成的效率。近年来，国内外学者在避障领域进行了深入研究，常用的方法包括:人工势场法［1-4］、VFH类算法(VFH［4-5］、VFH+［4，6］、VFH*［4，7-8］)、神经网络法［2，9-10］、遗传算法［9-10］和模糊逻辑法［2，11］等。

1989年，Borenstein和Koren在势场法和确定栅格的基础上，提出了虚拟力场法(VFF)，其路径安全，简单高效，但容易陷入局部极小值点，狭窄通道存在震荡。随后，Borenstein提出了VFH算法［5］来解决此问题，VFH算法通过2轮的环境数据压缩，选择活动窗口内局部极坐标直方图的波谷来完成机器人避障和控制，在阈值选取得当时不会陷入局部极小值，并允许机器人快速通过障碍物而不会出现不稳定现象，因而至今仍得到广泛应用。随后VFH+算法［6］将机器人尺寸以及运动特性对避障的影响考虑了进来。VFH*［8］将 VFH+与A* 算法结合，用 VFH+算法对运动轨迹进行未来几个阶段的提前计算形成路径节点树，并用A*算法对路径节点树进行搜索以找出代价最小的路径，选择该路径第一个节点方向作为目标方向。

但无论是原始VFH算法还是其改进算法，都存在以下问题:首先，上述方法的环境地图创建阶段，都直接将超声波传感器返回的距离信息映射到地图上，而忽略了由于机械震荡、传感器误差等所导致的错误距离信息等情况。其次，在可行区域的通行代价判定过程中，忽视了候选方向附近障碍物密度这一指标，容易导致在通用代价相近情况下选择次优方向的情况。最后，在陷入循环震荡后，缺乏一个循环跳出机制。

针对以上问题，本文在VFH+算法的基础上，对其进行了改进。在算法的实时地图创建阶段，将超声波传感器返回的距离信息进行软件滤波以增强实时环境地图的准确度。在算法目标方向决策阶段，改进了代价判定函数以便对通行代价进行更准确的标识，并设计了一个基于动态阈值的死循环跳出机制。

1 机器人避障原理

机器人在避障过程中状态如图1所示。

图1 避障过程机器人状态示意图

从原理上讲，避障的过程一般可分为环境地图创建、目标方向决策、运动执行3个步骤。其中，目标方向决策是整个系统的难点与核心，而环境地图创建又是目标方向决策的先决条件。

(1)环境地图创建。

机器人接收安装在其一周的距离传感器采集到的障碍物距离信息，并将障碍点映射到环境地图上，实时更新环境地图。

(2)目标方向决策。

在此阶段，参考更新后的环境地图，分析视野范围内(活动窗内)的障碍物情况并最终做出下一步目标运行方向的决策，并发送给运动执行单元。

(3)运动执行。

在完成目标方向决策后，机器人运动执行单元依据决策结果控制机器人的运动。

重复上述3个步骤直到机器人到达目标点为止。

2 避障算法环境地图创建阶段

避障算法决策的前提是对周围环境信息的掌握，实时准确的环境地图信息有助于提升算法效率、改善算法效果。本文和VFH等多类算法一样，采用超声波传感器和栅格地图结合的方式来创建和更新环境地图。其中，为避免错误数据影响环境地图的准确性，在超声检测阶段采用分组分段的工作方式，并引入软件滤波器来排除错误数据。

2.1 超声波分组检测

超声波传感器广泛地应用于机器人产品中，其耐污性高、环境适用性较强，而且随着超声波技术的发展，其精度和稳定度也越来越高。多个超声波传感器在安装距离较近并且同时工作的时候，因为模块频率接近的缘故，不可避免地会有错误接收的现象［12-13］。为了解决这一问题，本文将超声波传感器分为3组，分段进行工作，分组情况如图2所示。

图2 超声波分组示意图

超声波传感器分组分段工作，避免了临近传感器间的相互干扰，使得错误数据更少。虽然分组工作使得一轮检测时间延长为原来的3倍，但由于机器人决策和运动周期时间为检测周期时间的数十倍，因而不会对避障效率产生明显影响。

2.2 软件滤波

在移动机器人实际运行过程中，因为机械震荡、传感器误差等情况的存在，不可避免会存在错误数据。为了避免错误数据对机器人实时地图的更新产生影响，一般会对超声波数据进行软件滤波［13-14］，本文加入了基于查考机器人上周期障碍和当前运动情况的软件滤波器。基本思路如下:

(1)得到预测距离。

本周期障碍点距离预测方法如式(1)所示:

(2)得到测量距离。

(3)判断是否偏差。

判断偏差值△是否小于阈值T，若小于阈值T，则接受本周期测量信息，进入下一周期。否则，本周期数据“挂起”，进入步骤(4)。

(4)处理“挂起”数据。

因数据待确认，所以该周期数据将延迟映射到地图上。此时本周期预测值和测量值同时参与下周期的数据预测阶段，如式(3)和式(4)所示:

添加障碍物或者转向过程中检测到的障碍，此时接受“挂起”数据;若预测值接近于，表示是错误值，放弃“挂起”数据。

若连续多次“挂起”，则应当考察是否传感器出现问题。可停止机器人运动，静止状态下多次测量检测数据是否一致。待调整完毕后继续测量。

2.3 数据映射

将经过软件滤波后得到的距离数据映射到环境地图上去，以便实时地更新环境地图。本文与VFH类等算法一致，忽略了超声波散出角的影响，只考虑传感器轴线方向，映射公式如式(5)所示:

其中，(xr，yr，θ)为机器人当前坐标和朝向角度，α 为传感器相对于机器人正向的角度，L为障碍点和机器人中心的距离，(xm，ym)为映射后的坐标点。map［xm］［ym］为地图上(xm，ym)坐标点的障碍物置信度。

3 避障算法目标方向决策阶段

目标方向决策是整个避障算法的难点与核心，直接影响避障的效率与效果。本文对VFH+算法进行改进:通过修改代价判定函数使得对通行代价进行更准确的标识;通过增加循环跳出机制使得当陷入局部死循环后，能增加走出死循环的可能性。

3.1 VFH+算法目标方向决策阶段流程

VFH+算法该阶段输入是环境栅格地图，经过4轮的数据压缩后输出本轮选择的目标前进方向。

(1)直方图创建:VFH+算法将活动窗(避障视野)分为72个扇区，统计各扇区内的障碍物复杂度。按照横轴为扇区编号，纵轴为复杂度的形式构建直方图。

(2)二值化直方图:使用高低2个阈值将直方图二值化。其中，高于高阈值部分置1，表示障碍，低于低阈值部分置0，表示通行。介于高低阈值之间部分设置为与上周期相同状态。

(3)掩膜直方图:根据机器人运动特性，将当前不可达到的区域在直方图中置1。

(4)代价计算:获取直方图中可通行扇区，并对其进行代价排序，选取通行代价最低的方向作为本轮选择的目标方向。

3.2 改进的代价函数

VFH+等一类算法的传统通用代价函数如式(6)所示，本文代价函数如式(7)所示。本文加入了“候选方向附近扇区环境复杂度”指标，其可以弥补在扇区通行性判定时二值化处理导致各扇区具体密度信息丢失这一不足，以便在出现前3项代价指标和相近的情况下对候选方向代价进行更准确的标识。

其中，θ为候选方向角度，θr为机器人当前朝向角度，θt为机器人与目标点的角度，θn-1为上轮目标行驶方向角度。Ss、Sl、Sr分别为θ所在扇区及其左右侧扇区。式(7)中4项系数的大小关系依次代表倾向于“接近目标方向”、“保持当前朝向”、“参考上轮决策”、“选择附近扇区复杂度低的候选方向”等策略的程度，本文分别取值为5、2、2.5、1。平均密度被乘以(c(θ，θt)+c(θ，θr)+c(θ，θn-1))的目的是将平均密度调整为与前3项同一量级。

3.3 局部死循环跳出机制

尽管VFH+等一类算法对阈值的敏感程度不高，但由于工作环境的不确定性，固定阈值仍旧会引起一些问题:比如在复杂环境中，较低的阈值会抛弃许多本可通行的候选方向导致机器人陷入局部死循环。本文提出一种基于考察机器人历史轨迹，线性改变阈值的方法来解决此问题。在多数情况下，采用原始阈值抉择;当陷入局部循环后，动态改变阈值以便走出困境，基本思路如下:

(1)保存机器人运动轨迹，在本轮二值化直方图之前，考察是否陷入局部循环(本文根据15个运行周期是否出现小范围内震荡作为判断依据)。

(2)若陷入局部循环，则将高低阈值各增加1个单位。但高低阈值均不能超过限定值。

(3)若未陷入局部循环，则将高低阈值均调整为初始值。

(4)在阈值增加过程中，如果达到限定值，意味着机器人处于较危险状态。此时应当降低速度并依靠其他方式走出困境(如结合全局规划算法、借助碰撞传感器等)。

4 实验结果与分析

实验平台由MFC开发，平台的系统结构如图3所示。

图3 实验平台系统结构示意图

按照模块的功能和任务，将系统分为3层。最底层是实时地图层，可进行避障地图的绘制、动态更改和实时更新，其中，障碍物位置、大小可任意设置。中间层为部件模拟层，模拟超声波检测和机器人运动执行2个模块。最上层是算法执行层，包括软件滤波器和目标方向决策，以动态链接库形式供实验平台调用。

在部件模拟中，超声波检测模块的模拟方法是:在环境地图上，从传感器当前位置按安装方向发出射线，沿着射线方向以5cm为单位向前逼近。检测当前到达的栅格地图是否被障碍占有，如果是，则返回传感器与该栅格的距离，否则，继续逼近直到传感器最大量程为止。运动执行模块的模拟方法是:根据算法的目标方向决策模块给出的结果数据，结合当前运动速度计算出机器人下个周期的位姿(位置和朝向)，并将位姿和运行轨迹绘制在地图上。

本文对改进点进行了单独实验和综合实验，以验证算法改进的有效性。

4.1 软件滤波器实验

为进行软件滤波器实验，在仿真平台上模拟超声波检测时，设置了干扰模型。干扰模型由随机数向量(r1，r2，r3，r4)表示，其中，r1决定是否产生严重错误干扰，实验中，严重错误发生概率设置为2%;r2表示是否产生普通干扰，概率为50%;r3表示干扰的方向，表示距离是增加还是减少;r4表示干扰的数值即距离。在测试过程中，出现连续错误概率、错误数据比例等2个因素会对错误排除率产生影响，它们各自独立。图4展示的是在固定一个因素时，另外一个影响因素对错误排除率的影响结果。

图4 排错率影响因素示意图

观察图4可知，当错误数据比例固定时，随着连续错误出现概率增大，错误排除率快速下降，因为在软件滤波器的第4步中，连续错误数据会引起多次“挂起”，在放弃挂起数据的同时因为缺失正确的预测数据会导致放弃部分正确数据;当出现连续错误概率固定时，随着错误数据所占比例的增加，纠错能力逐渐减弱，因为错误数据过多会使得正确数据也被当做错误处理。在实际机器人系统中，连续错误出现概率较低，只有当传感器故障或机械震动严重时才会出现较高情况。本文在滤波器测试时，设置连续错误出现概率为5%，测试数据总量设置为1000，在此设定下的滤波器测试结果如表1所示。

表1 软件滤波器测试数据

测试结果表明，当错误数据总量在测试数据总量中所占比低于10%时，排除错误效果良好;随着错误比例的增加，纠错能力逐渐减弱，并容易出现过多错误认定的情况。但由于单个障碍栅格对算法决策的影响较小且超声波置信度相对较高，故当错误比例低于16%时的误差在工程所允许范围之内。

4.2 改进代价函数实验

为测试改进代价函数的效果，在仿真时构造了一个最优和次优候选方向前3项代价值相当的情况，对比结果如图5所示。

图5 改进代价函数实验对比图

图5中，中间矩形和圆点表示障碍物，上方矩形代表机器人起始位置，下方三角代表机器人目标点，曲线表示是机器人的行驶轨迹。

图5(a)是试验中构造的测试环境，小圆点所代表的障碍物在当前时刻不足以使得该扇区被认定为“阻塞(1)”，该扇区被认定为“畅通(0)”，二值化后，扇区障碍物密度信息丢失。按照原始代价函数，此时会选择向左的方向前进，因为该方向略优于向右的方向，如图5(b)，但随着机器人的行进，该方向小圆点障碍物影响逐渐增大，会使得机器人朝该方向的前进受到阻碍。图5(c)使用了本文改进代价函数的方法，算法决策阶段会将二值化丢掉的扇区障碍物密度信息重新考虑，可以更加准确地标识通行代价，做出更优选择。

4.3 局部循环跳出机制实验

循环跳出机制的效果如图6所示。

图6 局部循环跳出实验对比图

由对比图可见，循环跳出机制是有效的。机器人进入“困境”后，由于阈值较低，将原本可通行区域认为“阻塞”，因而在一段时间内处于“震荡”状态。但图6(a)因不含局部循环跳出机制，陷入了死循环，而图6(b)则激活了局部循环跳出机制，通过动态阈值的增加，从而发现了可通行的区域并成功走出了“困境”。

4.4 综合实验

为验证算法改进的整体效果，将改进方法与原始VFH+算法进行了综合对比实验，实验效果对比图如图7所示，其中，使得机器人陷入局部循环的障碍物群是在其他障碍物避障完成后动态添加的，其余障碍物是固定设定的。

图7 与VFH+方法的对比

从实验对比图可以看出，在通常情况下，本文方法与原始VFH+算法效果基本一致，如对比图左上方的稀疏障碍群;当靠近地图中间较大的圆形障碍时，2个候选方向代价相近，本文改进方法选择附近障碍密度更低的右侧方向前进，轨迹平滑，而原始VFH+算法没有考虑附近扇区障碍物密度，选择了左侧方向行进，尽管也成功绕过障碍物，但其在小矩形障碍与圆形障碍间狭窄通道中轨迹不平滑，存在震荡。最后，在机器人绕开预设障碍物后，动态添加的障碍物群使得机器人陷入了局部循环，原始VFH+方法陷入了死循环，本文改进方法依靠局部循环跳出机制成功走出了障碍群，可见改进是有效的。

5 结束语

本文为提高机器人避障的效率，增强避障地图的准确性，对VFH+算法进行了改进。通过将超声波传感器分为3组，分段进行工作避免了相互之间的干扰。采用软件滤波器对超声波传感器返回的数据进行滤波，尽可能排除错误数据，从而增强了实时避障地图的准确性。同时，在原始通行代价判定函数中增加了“候选方向障碍物密度”一项指标，通过将二值化过程中丢失的障碍物密度信息重新考虑，增强了代价标识的准确度，能有助于发现更优方向。设计了基于考察历史轨迹的局部循环跳出机制，在确认陷入局部死循环后，在一定范围内动态地增加阈值，可以使得机器人发现一些原本可通行的通道，以便走出“困境”。在后续的研究中，计划尝试将本文等局部避障方法与一些全局路径规划方法结合，以便能在未知复杂环境中更好地完成任务。

［1］Khatib O.Real-time obstacle avoidance for manipulators and mobile robots［C］//Proc.of IEEE International Conference on Robotics and Automation.1985:500-505.

［2］朱大奇，颜明重.移动机器人路径规划技术综述［J］.控制与决策，2010，25(7):961-963.

［3］石为人，黄兴华，周伟.基于改进人工势场法的移动机器人路径规划［J］.计算机应用，2010，30(8):2021-2023.

［4］何武.室内服务器机器人的导航研究［D］.合肥:中国科学技术大学，2011.

［5］Borenstein J，Koren Y.The vector field histogram-Fast obstacle avoidance for mobile robots［J］.IEEE Journal of Robotics and Automation，1991，7(3):278-288.

［6］Ulrich I，Broenstein J.VFH+:Reliable obstacle avoidance for fast mobile robots［C］//Proc.of IEEE International Conference on Robotics＆ Automaiton.1998:1572-1577.

［7］许心德，关胜晓.未知环境下基于VFH*的机器人避障［J］.计算机仿真，2010，27(3):156-160.

［8］Ulrich I，Broenstein J.VFH*:Local obstacle avoidance for fast mobile robots［C］//Proc.of IEEE International Conference on Robotics ＆ Automaiton.2000:2505-2511.

［9］徐秀娜，赖汝.移动机器人路径规划技术的现状与发展［J］.计算机仿真，2006，23(10):1-4.

［10］刘玲，王耀南，况菲，等.基于神经网络和遗传算法的移动机器人路径规划［J］.计算机应用研究，2007，24(2):264-266.

［11］Malhotra R，Sarkar A.Development of a fuzzy logic based mobile robot for dynamic obstacle avoidance and goal acquisition in an unstructured environment［C］//Proc.of IEEE International Conference on Advanced Intelligent Mechatronics.2005:1198-1203.

［12］陈春林，陈宗海，卓睿.基于多超声波传感器的自主移动机器人探测系统［J］.测控技术，2004，23(6):11-13.

［13］黄庆成，洪炳镕，Javaid Khurshid，等.全自主足球机器人的超声波定位避障系统［J］.哈尔滨工业大学学报，2003，35(9):1077-1079.

［14］王军政，陈超，汪正军，等.基于Kalman滤波的量程自适应超声波测距［J］.北京理工大学学报，2011，31(3):283-288.