文献阅读之机器人局部路径规划Vector Field Histogram算法(VFH)

大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。

---

VFH是机器人常用的局部规划算法，主要用于根据传感器观测数据计算接下来的无碰的接近目标点的运动方向。VFH有2个比较重要的变种：VFH+和VFH*，理解这3个算法，读3篇论文就够了。请参考下面的链接。

The Vector Field Histogram - Fast Obstacle Avoidance For Mobile Robots

VFH+: Reliable Obstacle Avoidance For Fast Mobile Robots

VFH*: Local Obstacle Avoidance With Look-Ahead Verification

1. Virtual Force Field (VFF)算法

VFF算法是VFH算法的前身，类似人工势场法。主要的步骤如下：

1. 根据测距传感器测量模型更新感兴趣的栅格。不同的传感器可以设计不同的测量模型和更新方法。
2. 设定活动窗口ROI，在里面计算障碍物排斥力的合力和前进目标的吸引力。排斥力和吸引力的合力就是运动方向。其中，排斥力的大小与栅格被障碍物占据的确定程度成正比，与距离成反比。

图1 传感器更新占据栅格地图

图2 活动窗口内的合力

VFF会有如下3个问题，主要原因是原理上的缺陷——大量障碍物点的数据被抽象成一个排斥力的大小和方向，造成了信息损失。

• 和人工势场法相同的局部极小值。
• 无法通过狭窄的障碍物间隙。
• 传感器测量的不确定性、坐标和占据值的离散变化，导致运动方向易抖动。

2. VFH算法

VFH算法的中心思想是将所有栅格的被障碍物占据情况、对运动方向的影响，通过统计直方图表示。这样的表示非常适合不准确的传感器数据，并适应多传感器读数的融合。VFH同样依赖测距传感器更新地图，对比VFF，优势是可以平滑的在障碍物密集的区域行驶，响应快，无需停止等待规划。可以输出速度、方向，运动速度受传感器观测和更新频率限制。如果陷入局部极小值，论文给出了方向偏离检测方法，可以调用全局规划脱困。

VFH有3层数据，顶层是根据传感器观测数据更新的二维栅格地图；中层是一维的极坐标直方图polar histogram，就是将VFF的活动窗口ROI按角度均分为多个sector，每个sector用polar obstacle density表示对应的角度范围内障碍物的占据情况（密度、数量、置信度等）；底层是VFH的输出。该算法的主要内容就是做这3层数据的转换，步骤如下：

1. 将ROI的每个栅格用obstacle vector表示，方向是cell到robot center的方向，大小与确定程度成正比、与到robot center的距离成反比。
2. 计算polar obstacle density，就是落在相同sector的obstacle vector的大小叠加。这里可以用加权平均来平滑。
3. 根据polar obstacle density的峰谷、density的阈值，挑选出与前往目标点的方向最吻合的谷。取谷的中间角度作为运动方向。可以通过全局规划设定自适应阈值。
4. 根据当前运动方向上的density大小，调整运动速度。输出结果。

至于VFH应用上的局限，我会在介绍VFH+时说明。

图3 二维栅格地图转换为一维极坐标直方图

 

图4 实际场景-栅格地图-活动窗口-直方图对应关系示例

 

图5 确定运动方向