VFH*避障/局部路径规划算法

做个正直的人

VFH*是VFH、VFH+两个算法的持续改进，对这两个算法不熟悉的同学，不建议直接看这一篇文章。可以去看我写的两篇介绍VFH、VFH+算法的文章。

先说一下这三个算法的关系。VFH（Vector Field Histogram）的前身是VFF（Virtual Force Field），VFH+是对VFH的改进，VFH*又是对VFH+的改进。也就是下面这样的关系
$$VFF>>VFH>>VFH+>>VFH\ast$$
话不多说，直接上干货。

1、VFH+存在的问题——dead-end

VFH*之所以要改进VFH+，这是因为VFH+在某些情况下会出现很不希望出现的表现。请看下图，机器人检测到障碍物时，VFH+算法这个时候可以计算出两个可行的前进方向：A和B。VFH+这个时候是有很大的概率会选择A方向前进的。然而，很明显，如果机器人真的沿着A方向前进，那么很快机器人就会发现前方是一个dead-end（死胡同），机器人就不得不改变方向甚至原路返回后沿着B方向前进。

那我们就发现，这种情况下，机器人对待dead-end的表现是很差劲的。那我们就来分析一下，是什么导致机器人难以应对dead-end的呢？

不难发现，VFH+采用一个圆（实际上是传感器的数据，比如声呐）来检测障碍物，但是这一个圆的半径总归是有限的，这就导致机器人实际上能看到的视野也是十分有限的，那已知信息很少，自然控制效果也就比较差了。那么我们该采取什么手段来解决问题呢？

我们再来分析一下，VFH+之所以如此，是因为VFH+没有考虑如果真的沿着选择的方向前进后会遇到什么。如果他考虑了，那就有可能及早发现dead-end，从而尽早选择别的路。这就是VFH*算法。

2、VFH*算法

2.1 VFH*算法概述

VFH*所做的改进其实非常少，只有一步，那就是纳入了前瞻距离（Look-ahead），即通过提前探查一下如果真的沿着某一个方向前进会不会遇到dead-end，根据探查的结果，来选择更好的前进方向。

不过，虽然只是改进了一步，所做的工作却不少。下面我们来具体的描述一下这一个过程。

VFH*首先也是构建极坐标直方图，然后根据直方图中的openings来确定所有候选的前进方向。只不过VFH*在此时并不急于确定最终的前进方向，而是要沿着每一个方向进行探查。探查的过程类似于A*算法中的expansion操作。

假如说，机器人确定了N个候选的前进方向，那么以机器人的当前位置为根节点，沿着这N个前进方向进行探查。每一次探查的距离设置为$d_s$(这是探查阶段的第一个重要参数)，探查出来的新节点标记为projected node，然后计算该节点处的代价和启发函数值。然后，在新节点处继续探查。一直重复$n_g$次(这是VFH*算法的第二个重要的参数)。

在探查阶段完成之后，在搜索树中搜索那一条代价最短的路径。这一条路径VFH*算法规划出的避障路径。

2.1.1 VFH*的参数

总的说来，VFH*算法有三个主要的参数，$d_s$、$n_g$、$d_t$，并且这三者之间满足如下关系：
$$d_t=n_g\ast d_s$$

其中，$d_s$每一次探查的距离࿰