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

做个正直的人

本文章是我看《The Vector Field Histogram-Fast Obstacle Avoidance for Mobile Robots》论文时候的笔记，感兴趣的欢迎去看原论文。

VFH中有一些概念是从其他算法中借鉴过来的，并且VFH算法本身就是对VFF(Virtural Force Field)的一种改进，其中有很多的相似之处。所以按照原论文的思路，先介绍由CMU提出的Certainty Grid算法中的一些概念，再介绍一下势场法中的一些概念，接着介绍一下VFF算法，最后介绍最重要的VFH算法。

1、信度栅格（Certainty Grid）

CMU提出的算法中最重要的思想就是The Certainty Grid for Obstacle Representation，即用一个置信度来表示这一个位置存在障碍物的可能性有多大。至于中文名字该叫什么，难道翻译成“基于置信度栅格的障碍物表示法”？

具体来说就是，把整个地图用一幅二维的栅格（grid）地图来表示，比如大小是1024*1024。机器人的工作空间（就是局部地图）也用一个二维数组来表示，这个工作空间只包括以机器人为中心的一块正方形区域，比如30*30大小。每一个grid都有一个信度值（certainty value，CV）这一个CV值表示了这一个grid中存在障碍物的可能性有多大。CMU算法，使用一个概率函数来更新每一个CV值。比如通过超声波传感器来获取周边情况。

VFH算法中工作空间中的每一个unite不再叫做grid，而是叫做cell。

2、势场法（Potential Field Methods）

势场法我觉得肯定是一个/一群物理相当不错的人提出来的。我觉得这种方法有一种天然的美感，自然、不做作。

势场法比如人工势场法（APF），思路非常简单。可以这么理解，目标位置和障碍物会通过一种叫做场（就像磁场或者电场）的东西对我们机器人施加力的作用，所不同的是，目标位置对机器人施加的是吸引力 $F_{att}$ ，障碍物对机器人施加的是排斥力 $F_{rep}$ 。并且距离越远，力就越小。根据力的分析，我们的机器人总体上是受到一个合力 R 的作用，这个力就会不断的推着我们的机器人绕过障碍物到达目标位置。

3、VFH算法的前身——VFF（Virtual Force Field Method）

VFF算法使用一个二维的笛卡尔直方图栅格区域（Cartesian histogram grid，我能怎么翻译？真的翻译不出来呀！）C来表示整个区域，和CMU方法一样，VFF的每一个cell具有一个属性CV 即 $c_{i,j}$ 表示这一个cell存在障碍物的可能性有多大。和CMU方法不同的是，CV建立和更新的方法更加快速。
CMU是通过传感器数据来给这一个传感器范围内的每一个grid赋予概率值，VFF则是只更新每一个传感器范围内仅仅一个cell的值。打一个比方，我们拿着手电筒照射一堵墙，很明显我们的手电筒会在墙上形成一个一定大小的圆形光斑，对待这一个光斑CMU和VFF有不同的处理方法，CMU会给整个光斑区域进行更新，而VFF只会更新位于光斑中心位置的那一个点，那其他的点怎么办？不用担心，我们的机器人是在运动的呀，虽然只更新一个点，但是点动成线，扫过去不就连续更新了嘛？看下图就清楚了。

很明显，VFF的计算量大大减少，速度比较快，这也是VFF可以允许机器人进行快速、连续、光滑运动的原因。不过凡事有好就有坏，VFF此举导致丢失了很多的信息，从控制的角度来说，我们对系统信息知道的越少，就越是难以对系统进行控制，所以VFF带来了一些比较大的缺点，后面会提到。
此外，VFF还使用了势场法的思想。在机器人运动过程中，会维护一个特定大小的（比如30*30）、以机器人为中心的正方形区域，称为活跃窗口（active region），记为C*。C*中的每一个cell记为 $c_{i,j}$ 。其实理论上应该维护的是一个圆形，但是如果用圆形就会大大增大算法的计算量，所以就用了矩形，这样处理起来比较方便。

C*内的每一个$c_{i,j}$都会对机器人施加力（virtual force） $F_{i,j}$ ，这样就好象有一个力场一样推动机器人运动。$F_{i,j}$ 的大小和 $c_{i,j}$ 大小成正比，和 $d^x$ 成反比，其中d是该cell到机器人中心的距离。x一般取为2。

在每一次循环的时候，所有的 $F_{i,}$