VFH*避障/局部路径规划算法
做个正直的人
VFH*是VFH、VFH+两个算法的持续改进,对这两个算法不熟悉的同学,不建议直接看这一篇文章。可以去看我写的两篇介绍VFH、VFH+算法的文章。
先说一下这三个算法的关系。VFH(Vector Field Histogram)的前身是VFF(Virtual Force Field),VFH+是对VFH的改进,VFH*又是对VFH+的改进。也就是下面这样的关系
V F F > > V F H > > V F H + > > V F H ∗ VFF>>VFH>>VFH+>>VFH* VFF>>VFH>>VFH+>>VFH∗
话不多说,直接上干货。
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 d_s ds(这是探查阶段的第一个重要参数),探查出来的新节点标记为projected node,然后计算该节点处的代价和启发函数值。然后,在新节点处继续探查。一直重复 n g n_g ng次(这是VFH*算法的第二个重要的参数)。
在探查阶段完成之后,在搜索树中搜索那一条代价最短的路径。这一条路径VFH*算法规划出的避障路径。
2.1.1 VFH*的参数
总的说来,VFH*算法有三个主要的参数, d s d_s ds、 n g n_g ng、 d t d_t dt,并且这三者之间满足如下关系:
d t = n g ∗ d s d_t=n_g*d_s dt=ng∗ds
其中, d s d_s ds每一次探查的距离,建议设置