【路径规划】VFH

简介

在没有全局地图或者是应对动态障碍物的情况下，往往是运用局部规划来进行绕障，在局部规划的经典算法中，比较常用的是 DWA 与 VFH，VFH 全名向量场直方图算法，不同于 DWA 的绕障思路。两者的绕障思路为：

DWA：对障碍物的位置进行测算，在满足运动约束的情况下，计算每一种速度矢量的运行轨迹，然后从运行轨迹中挑选出代价最小的轨迹(速度与当前速度最接近，距离障碍物不近，距离目标点方向的角度差较小等等代价)，之后选择此轨迹代表的速度矢量作为本周期的速度矢量目标，进行绕障

VFH：对障碍物的角度与距离进行测量，建立极性坐标系，环绕飞机一周的360度分为若干个区间，挑选出没有障碍物的区间，对区间进行二次处理，剔除掉不满足运动约束的区间，之后从剩余区间中计算出代价最小的区间，此区间的中间角度(或其他的代表角度)作为速度方向进行输出，对速度的大小并不修改，只输出目标速度方向

两种局部规划的方式各有优势，需要根据实际的应用场景来综合应用。

VFH的关键步骤

VFH的关键步骤为如下几步：

1）以飞机为中心建立极坐标系，更新障碍物数据，得到原始障碍物地图

2）对障碍物数据进行膨胀处理得到膨胀地图

3）将膨胀地图进行区间分割，分割为若干个角度区间

4）将距离数据转换为障碍物可信度数据，得到可信度区间地图

5）使用双阈值方式将可信度区间地图转换为二值化区间地图(二值化地图中为0表示无障碍物，1表示有障碍物)

6）二值化区间地图中为0的为可选区间，从可选区间中加入运动约束，去除无法到达的区间，得到有效区间

7）对每个有效区间进行代价计算，得到最终的最优角度作为速度矢量的方向

上述步骤中，关键在于地图的构建，地图构建的流程看下图会更加直观：

地图建立

原始障碍物极坐标地图

对于激光雷达来说，极坐标地图是可以直接建立的，激光雷达的数据拿到的就直接是角度与距离数据，vfh的仿真实验也是基于激光雷达的数据，不过对于使用视觉来识别障碍物的话，道理也是相通的。

障碍物膨胀地图

获取到原始的障碍物地图之后，还需要对障碍物进行膨胀，这样就可以将无人机看作一个质点，将无人机的外形进行简化，膨胀半径可以设置得较大，但最小也应该是无人机的尺寸数据，例如无人机的桨叶到中心的距离是0.5m，那么膨胀半径就至少需要0.5m，这样只要飞机能够跟上规划的轨迹与速度，就可以避免桨叶碰到障碍物边缘。

膨胀角度的计算

假设有一个障碍物，设定了膨胀半径r，找到障碍物的边缘，就可以计算出需要膨胀的角度α，示意图如下：

计算出了对于障碍物边缘，需要膨胀的角度，接下来就是用什么距离去填充，因为在局部规划中，是不知道全局地图的，因此在实际仿真中，我是先将见到的障碍物距离进行膨胀前移，例如检测到的距离是2m，膨胀半径是 0.5m，那么先对检测到的距离进行缩进膨胀，距离改为1.5m，之后假设此时刚好时障碍物边缘，对计算出的膨胀角度，都以1.5m进行填充。

角度补偿

由于飞机飞行的时候有一定的倾角，因此激光雷达测量出的距离，本身是有一定的角度影响的，导致测量到的距离是较大的，而实际障碍物要近，如下所示：

在仿真中使用的飞机模型的最大倾角是35度，按照正常的逻辑，每个激光雷达的距离都需要融合姿态进行角度补偿。

可信度地图

得到障碍物膨胀地图之后，需要将膨胀地图进行可信度的计算，可信度的计算可以理解为反向归一化，障碍物越近，说明此处有障碍物的可信度越高，距离越远，说明障碍物可信度越低。

在进行可信度计算时，将地图进行一定的简化，将地图分做若干个区间，例如分为12个区间，每个区间就是 360/12 = 30 度 的角度，将一个区间 30 度之内的障碍物距离取平均值作为此区间的障碍物距离，之后进行可信度的计算。

按照正常的逻辑，当然可信度越高，表示有障碍物的概率越大，因此与膨胀地图的距离是成反比的，利用最大测量距离进行反向归一化即可：

可信度 = (最大测量距离 - 障碍物距离) / 最大测量距离

可信度二值化地图

在得到可信度地图之后，可信度都为 [0,1] 区间的数，那么需要对可信度地图进行二值化，0代表没有障碍物，1代表有障碍物，在进行二值化时，不能简单的设置一个阈值，高于此阈值就认为是有障碍，低于阈值就是无障碍，这样的做法会带来一个问题，也是局部规划的通病：局部规划是不知道全局地图的，因此很有可能当前计算处理的最优角度与上次计算出来的最优角度相差很大，之后就会陷入方向跳变过大的陷阱。

例如最开始飞机比较远的时候，路线是往一边飞：

因为此时障碍物的间隙占用的角度比较小，但当飞机飞了一段实际之后，这个障碍物间隙越来越大了，就可以飞行了，就会跳转飞行角度：

当然，以上只是一个示意，就是说，局部规划中，会出现方向频繁跳转的问题，一方面可以调节代价系数，另一方面，在 vfh 中使用了双阈值对其进行优化。

vfh的双阈值

vfh 设置了双阈值 Tmax 与 Tmin，可信度高于 Tmax 的置1，低于 Tmin 的置0，中间的，维持上一次的状态，它的含义就是：高于一定的值，是必然有障碍物的，低于一定的值，是必然没有障碍物的，处于中间的值，作为一个过渡区，相当于待定状态，维持上一次的值，有可能是一闪而过的障碍物，也有可能是一闪而过的空隙。

这样做相当于是设置了一个缓冲区，对障碍物进行考核待定，这样子对于保持飞机方向不反复横跳有一定的作用。

可选区间

一般，二值化地图中，为0的，视作可选区间，也就是待定区。

有效区间

得到了可选区间之后，并不是每个可选区间都是可以用于实际飞行的，需要考虑飞机当前能不能飞到区间，因此需要增加一些运动约束，将不能飞到的区间去除掉，剩余的区间就是有效区间。

刹车距离

运动约束可以通过刹车距离来进行约束，根据当前速度来计算如果刹车到 0 的距离，可以参考 px4 中的根据当前距离来计算当前最大速度的方法，只是反过来计算距离而已。

假设当前速度正在以最大加速度 amax 加速，此时是最恶劣的情况，刹车距离为：

计算出当前合速度的刹车距离之后，开始计算可选区间是否是有效的，假如可选区间是 5，当前的飞机速度是在 3 区间，那么如果 5 是有效区间的话，就需要经过 4 区间，读取 4 区间的障碍物距离。根据当前区间的中间角度与当前速度方向的夹角，再对当前的刹车距离进行角度的分解，得到当前区间的分解刹车距离，判断 4 区间的障碍物距离是否大于刹车距离，如果大于，则说明飞机可以正常经过，如果小于，则说明飞机可能会发生碰撞，那么 5 区间就不是有效区间。

代价函数

得到有效区间之后，需要从有效区间中，挑选出最优的角度，作为接下来的飞行方向，此时需要代价函数来挑选，每个区间选择中间角度作为候选角度，按照三个方面的条件来计算代价，一般的考虑思路为：

1）候选方向与目标点方向的偏差

2）候选方向与当前速度方向的偏差

3）候选方向与上次速度方向的偏差

将三个条件各增加一个系数，构成代价函数：

g = k1 * (theta - goal_theta) + k2 * (theta - vel_theta) + k3 * (theta - vel_theta_last)