机器人避障规划算法之VFH系列算法研究现状

机器人路径规划算法可以分为全局路径规划与局部路径规划，全局规划算法一般需要地图信息作为先验知识，而局部规划算法利用传感器探测环境信息避开障碍物。常用的全局算法算法有可视图法，栅格地图法，智能算法等。常见的局部规划算法有人工势场，BUG算法，VFH算法，DWA算法，TEB算法等。本文针对VFH算法的研究现状做个简单概述。

​ 图1 VFH算法发展历程图

1 VFF算法

​ 该算法是一种虚拟力场法，针对传感器感知周围环境，生成笛卡尔坐标障碍图，整个地图划分成栅格，每个栅格都有一个确定的障碍物确定值CV值（certainty value）。通过公式计算：R=${\sum }_{i,j}^n(F_{rpu}+F_{attr})$,其中为障碍物栅格$F_{rpu(i,j)}$为对机器人的排斥力，F_{attr}为目标点对机器人的吸引力，R为机器人的移动方向和速度。

缺点：无法通过门道，两侧的排斥力会产生将机器人推开的力；当机器人从一个栅格单元到另一个栅格单元，R值剧烈可能变化，导致航向剧变；狭窄走廊环境下，运动振荡不稳定。

2 VFH算法

​ 针对VFF算法诸多不足，分析其原因，主要还是因为VFF算法是直接通过二维度笛卡尔坐标系计算出候选运行角度，数据缩减过快。所以VFH算法引入两个阶段的数据缩减(笛卡尔坐标障碍图->极坐标直方图->候选方向)。整个的规划流程如下：

（1）传感器获取信息

（2）更新笛卡尔坐标障碍图

（3）创建极坐标直方图

（4）确定自由扇区和转向方向

（5）计算速度

（6）给出电机控制命令

其中（1）（2）和VFF算法是一致的。第三步需要根据机器人当前位置建立极坐标图，以逆时针方向为0-360°（需要根据自身传感器扫描范围）。设置分辨率为${\alpha }_r$，所以扇区数量$n=\frac{360}{{\alpha }_r}$（例如我设置的分辨率为5°，所以就有72个扇区）。
栅格障碍物强度值：$m_{i,j}={c_{i,j}^{\ast }}^2(a-b{d}_{i,j})$,其中$c_{i,j}^{\ast }$代表活动区域内的一个栅格的CV值（例如传感器的扫描距离为$d_s$，那么活动区域就是以机器人为圆心，$d_s$为半径的区域），$d_{i,j}$为机器人与栅格的距离，a,b为常数，且满足
$a-b{d}_s=0$。

计算栅格归属扇区：

其中k表示当前栅格所在扇区，$(x_o,y_o)$表示机器人所在位置，$(x_i,y_i)$表示当前栅格所在位置。

扇区k的障碍物强度值：$h_k={\sum }_{i,j}{m}_{i,j}$ （针对扇区内的所有栅格累加和）

对求得的障碍物强度值做一个平滑处理，低通滤波：

（这里的 l 代表k扇区前面和后面的扇区，我这里选择的 l=5）
第四步：确定自由扇区和选择方向，设定阈值$d_{th}$，小于$d_{th}$的扇区为自由扇区。将可通行扇区分为：窄谷和宽谷，通过设定$S_{max}$，连续$S_{max}$个扇区为自由扇区是宽谷，反之为窄谷。
宽谷 if S>=$S_{max}$

窄谷 if S>=$S_{max}$

选择方向：选择距离目标方向最近的宽谷为候选方向。
计算速度就不过多阐述了，可以参考一下后面的论文，大体原则就是根据$h_k$的值调整车速，例如当$h_k$值很大时，说明这个方向有障碍物且距离障碍物越来越近了。

3 VFH+算法

​ 该算法相对于VFH算法有四点改进如下：

①VFH算法并没有考虑机器人本生的尺寸，VFH+算法对机器人宽度进行了宽度补偿，对于每一个栅格向外膨胀$R_{r+s}=R_r+d_{safe}$，其中$R_r$为机器人半径，$d_{safe}$为机器人安全运行距离。经过这样的一个处理，机器人可以被看成一个质点。
②VFH算法不适合狭窄通道环境，VFH+提出了双阈值滞后系统

其中n代表时刻，n-1是前一时刻
③考虑机器人运动学和动力学：由于转弯半径的影响，导致一些本来可以通行的扇区变成了不可通行扇区。
④提出了选择方向时的代价函数

其中$u_1,u_2,u_3$为常数，$▽(c,r_t)$为候选方向和目标方向之间的夹角；$▽(c,\frac{{\theta }_n}{\alpha })$为候选方向和当前运动之间的夹角；$▽(c,k_{d,n-1})$为候选方向与前一次运动方向的夹角。最后选择代价最小的候选方向作为行进方向。

4 VFH*算法

​ VFH+算法还是存在局部规划算法存在的问题，因为只拥有地图的部分信息，可能会陷入局部死区。VFH*算法通过预测机器人未来前进几步的轨迹并评估其后果来做出当前的方向选择。所以VFH*算法的步骤如下：

（1）建立极坐标图

（2）确定候选方向

（3）计算后续节点的投影位置和方向

（4）确定到达这些节点的成本

（5）确定启发函数，通过启发函数选择方向

其中（1）（2）步和VFH+算法是一样的。后面三步主要是基于一个对未来的预测，预测图形像一个树状结构，在第一次可能有三个候选方向，VFH+算法是直接选择一个代价最小的方向行驶。而VFH*算法对这三个方向都进行投影预测，计算新的投影位置和方向，构建新的直方图，继续投影预测。进行的轮次也就是树的深度，记为$n_g$；每一次的投影步长为$d_s$；所以总的投影距离$d_t=d_s\ast n_g$。最后选择一个累计成本最小的作为行驶方向。

5 针对阈值改进的VFH算法

​ 在构建完成向量场直方图后，阈值不一样，可选择的候选方向就有很大区别。若是阈值选择过大，可通行扇区变多，在距离障碍物很近时才做出反应，若是速度很快的话，可能会导致碰撞；若是阈值选择过小，则可能忽略一些可以通行的方向。针对VFH算法阈值敏感问题，很多学者都提出了自适应阈值。

6 基于A*引导域的VFH算法

​ 虽然VFH*算法融合了A*的一些思想，但是对于复杂环境下的路径规划还是会存在死区现象。这也是局部避障算法的固有缺陷，若是能对地图有一个大概的了解（先建图），利用全局路径规划A*算法规划出一条粗糙的路径（低分辨率路径），在VFH算法运行过程中用这条粗糙的路径做引导。

7 利用全局路径信息引导VFH局部路径规划算法

​ 先利用SLAM建图，利用建好的地图做全局路径规划（RRT，A*,人工势场法等），让后让机器人沿着规划好的路径运行，若是碰到动态障碍物，利用VFH算法加临时全局路径点引导避障。

参考文献

[1] Borenstein J, Koren Y. The vector field histogram-fast obstacle avoidance for mobile robots[J]. IEEE transactions on robotics and automation, 1991, 7(3): 278-288.

[2] Ulrich I, Borenstein J. VFH+: Reliable obstacle avoidance for fast mobile robots[C]//Proceedings. 1998 IEEE international conference on robotics and automation (Cat. No. 98CH36146). IEEE, 1998, 2: 1572-1577.

[3] Ulrich I, Borenstein J. VFH*: Local Obstacle Avoidance with Lookahead Verification In IEEE int. conf. on Robotics and Automation[J]. San Francisco, USA, 2000.

[4] 庄宇辉, 赵成萍, 严华. 一种针对VFH系列算法阈值敏感问题的改进策略[J]. 四川大学学报(自然科学版), 2018, 55(05):95-102.

[5] Chen W, Wang N, Liu X, et al. VFH* Based Local Path Planning for Mobile Robot[C]//2019 2nd China Symposium on Cognitive Computing and Hybrid Intelligence (CCHI). IEEE, 2019: 18-23.

[6] 周俊, 庄宇辉, 严华. 基于记忆矩阵A~*引导域的VFH算法改进策略[J]. 四川大学学报(自然科学版), 2020(4).