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

做个正直的人

这篇文章是我看论文《VFH+: Reliable Obstacle Avoidance for Fast Mobile Robots》（Iwan Ulrich and Johann Borenstein）的笔记，所以说是文章的翻译也可以。想看原paper的可以自己去找，看过VFH算法的话，这个VFH+算法还是非常好懂的。

我前面写过一篇讲VFH算法的文章，感兴趣的可以去看看。
VFH局部路径规划算法

我在讲VFH算法的最后，写了一点我对VFH算法的评价，主要有两点，一是VFH算法涉及的参数比较多，很麻烦；二是机器人总是会像个傻子一样在原地摆头，我设置了一个不灵敏区域稍微缓解了一下摆头的问题，但是这导致机器人对障碍物的灵敏性降低。

今天看了VFH+算法，才发现原来这些问题前人们早就研究过了。VFH+对VFH提出了几点修正，VFH还带了个正号，这是增强版的意思吗？hhhhhhhh…接下来进入正题。
VFH+算法同样分成四个部分，不过和VFH的四个部分可是不一样。

1、第一部分：映射到极坐标系

VFH算法的第一部分就是把局部地图从笛卡尔空间映射到极坐标系，VFH+算法的做法跟VFH一样，只不过VFH+不再像VFH那样采用正方形的工作空间（可以当成是局部地图），而是采用了圆形的工作空间，本来就该是圆形的好吧，VFH那是为了简便而已。这工作空间一旦变成圆形，就有了一个性质——机器人原地旋转感知到的局部地图是不变的，这称为旋转不变性。

VFH+对VFH的改进——考虑机器人的宽度&对障碍物进行膨胀

VFH并没有考虑机器人的宽度，而是采用一个低通滤波器进行补偿，这个滤波器的参数的设置还比较麻烦。
VFH+考虑到了机器人的宽度$r_r$ ，这里假定机器人是正圆盘形状。同时为了安全起见又设置了一个距离障碍物的安全距离$d_x$ ，基于此，我们就可以得到需要对障碍区进行膨胀的幅度$r_{r+x}=r_r+d_x$ 。这样我们就可以把机器人完全看做一个点来进行规划，是不是感觉事情变得简单了。

从上图可以看出，如果障起舞膨胀幅度为$r_{r+x}=r_r+d_x$ ，机器人距离障碍物的距离为
$d_{ij}$ ，此时这一个障碍物对应的张角为$2{\gamma }_{i,j}$ 。其中 γ i , j = a r c s i n ( r r + x / d i , j ) \gamma_{i,j}=arcsin(r