文献阅读之机器人局部路径规划VFH+ VFH*算法

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

---

1. VFH+

VFH+是对VFH的改进，输入是局部栅格地图，输出下一步的运动方向。论文可以参考上一篇博客。相比VFH，主要的提升在于更平滑的轨迹和更好的可靠性，具体体现为：

• 将polar histogram转换为二值直方图binary polar histogram时，使用迟滞双阈值，去除抖动；
• 在计算polar obstacle density时考虑了机器人尺寸（半径），相当于做了障碍物膨胀，不再需要像VFH给低通加权平均方法调参；
• 考虑了机器人的运动学模型和当前运动状态，筛除了障碍物挡住机器人运动轨迹的sectors，建立masked polar histogram，相当于做了碰撞检测；
• 使用代价函数来选择最优前进方向。

VFH+主要有4个步骤，以机器为中心建立局部地图坐标系，将传感器观测到的信息更新到窗口ROI中，然后转换到3种直方图中：local gridmap ==> Primary polar histogram ==> Binary polar histogram ==> Masked polar histogram ==> Moving direction。在代码实现时，有很多地方可以利用缓存查表来减少计算量。

1. Primary polar histogram。将圆形的活动窗口中每个cell的obstacle vector转换为polar histogram中的polar obstacle density，计算density时不止考虑一个sector内的cells，还要考虑按机器人半径扩展cell所覆盖的cells。在设计density的计算公式时，要考虑栅格被占据概率、离robot中心的距离等因素的重要程度如何分配。
2. Binary polar histogram。利用迟滞双阈值，将Primary polar histogram转换为用0、1表示是否可行走的二值图。
3. Masked polar histogram。在Binary polar histogram的基础上，根据当前运动状态（v，w）预测轨迹（主要考虑转弯半径），搜索与该轨迹有重合的左右边界cell的角度（包含扩展的cell），确定可行sectors的左右边界，更新出Masked polar histogram。
4. 在可行sectors中，采样候选的运动方向。使用cost function在一些候选方向中选择最好的输出。代价函数主要考虑：1是候选方向与目标方向的差异；2是候选方向与轮子朝向的差异；3是候选方向与历史输出的差异；等等。

图1 膨胀障碍物栅格，考察其影响范围

 

图2 根据运动学模型做碰撞检测

VFH+在代码实现时比较简单，应用中主要有2个问题： 

1. 根据VFH+的原理和步骤可以看出，其只能输出下一步的运动方向，由当前运动姿态转换到该运动方向的线速度、角速度等动态信息，均无法给出。实际应用中，可以根据输出角度与当前朝向、目标朝向等的差异来确定速度，比如转角越大，角速度越大，线速度越小。但终归有失严谨，运动平滑性较差，更谈不上轨迹规划。因此，比较合适用来在障碍物较多的环境中指出脱困的方向，可以作为其他运动规划控制方法的一个预处理步骤和输入。
2. VFH+使用了障碍物栅格相对Robot中心的方位来直接限定运动方向，使用了障碍物栅格距Robot中心的距离来计算density，间接反映对运动的影响程度。但因为有Primary polar histogram向Binary polar histogram的二值化过程，受阈值影响极大，其实距离信息也基本损失了。这导致阈值难以调参，更重要的是难以接近靠近障碍物的目标点，因为目标点方向可能会被排除掉。虽然可以通过减小局部地图窗口范围缓解，但效果一般。

针对第2个问题，我采用了如下策略来解决，实测效果较好。

• 若障碍物位于Robot和目标点之间，按照VFH+避障。
• 若障碍物在Robot向目标点连线的延伸线上，且离目标点很近，按照VFH+计算density。当Robot离目标点很远时，density较小，Robot仍可以继续靠近目标点；逐渐变近时，density增大直到VFH+判断目标点不可达，向上通知全局规划器。
• 若障碍物在Robot向目标点连线的延伸线上，且离目标点很远，忽略其对Robot运动的影响。

图3 使Robot尽量接近目标点

可惜的是，ROS中并没有VFH算法的代码，这里推荐2个开源代码。

• https://github.com/AugustoPB/vfh_local_planner 实现了基本的VFH。
• http://playerstage.sourceforge.net/doc/Player-2.0.0/player/group__driver__vfh.html 实现了VFH+。

2. VFH*

对比VFH+，VFH*能更好的处理需要机器人大转向或停下来的场景，比如死胡同。其实就是在VFH+输出的基础上，像RRT那样，迭代运行VFH+来向前扩展多个节点，从而引入更接近全局的信息。论文中详细讲述了扩展步长、步数、不同代节点的代价函数设计、权重选取等调参细节。至关重要的参数是向前扩展的距离ng，如图4中所示，会直接影响到全局信息的比例、运算量的大小。

我认为避开大规模的障碍物、死胡同等，应该由全局规划器来做。调用规划算法的业务逻辑应该检测到环境变化、位姿偏离过大等异常，间歇性或周期性的调用全局规划器重新规划，而不是像VFH*这样。与其多轮次的去扩展，不如设计好全局规划、局部规划、状态监测、异常处理的关系。这是一个系统性的问题，而不是局部规划的问题。

图4 VFH*多轮扩展节点的示意图