Bug算法(Bug Algorithms)简介（Bug1 Bug2 Tangent Bug）

本文所有图片均来自以下这本书:

Principles of Robot Motion: Theory, Algorithms, and Implementations[M]. MIT Press, 2005.

在上一篇博客我也给出了下载链接。

本篇博客主要介绍一下BUG算法:

BUG算法（Bug Algorithms）是一种最简单的避障算法。其算法原理类似昆虫爬行的运动决策策略。在未遇到障碍物时，沿直线向目标运动；在遇到障碍物后，沿着障碍物边界绕行，并利用一定的判断准则离开障碍物继续直行。这种应激式的算法计算简便，不需要获知全局地图和障碍物形状，具备完备性。但是其生成的路径平滑性不够好，对机器人的各种微分约束适应性比较差。

Bug1算法

该算法的基本思想是在没有障碍物时，沿着直线向目标运动可以得到最短的路线。当传感器检测到障碍物时，机器人绕行障碍物直到能够继续沿直线项目标运动。BUG1算法只有两个行为：向目标直行和绕着障碍物的边界走。

如图2.1所示，假设机器人能够计算两点之间的距离，并且不考虑机器人的定位误差。起始点和目标点分别为qstartq_{start}qstart 和qgoalq_{goal}qgoal表示. 初始时刻 i=0i = 0i=0，令q0L=qstartq^L_0 = q_{start}q0L=qstart，并称连接qiLq^L_iqiL和 qgoalq_{goal}qgoal的线段为m−linem-linem−line. 在没有遇到障碍时，机器人沿着m−linem-linem−line朝目标qgoalq_{goal}qgoal直线移动. 如果遇到障碍，则称点q1Hq^H_1q1H为第一次遇到障碍时的撞击点（hit point）. 接着，机器人环绕障碍物移动直至返回 q1Hq^H_1q1H点。然后判断出障碍物周边上离目标最近的点，并移到这个点上，该点称为离开点（leave point），由 q1Lq^L_1q1L表示。从q1Lq^L_1q1L开始机器人再次沿直线驶向目标，如果这条线与当前障碍物相交，则不存在到达目标的路径（如图2.2所示）。 Bug1算法的效率很低，但可以保证机器人能到达任何可达的目标（概率完备）。

BUG1算法伪代码：

Bug2算法

Bug2算法也有两种运动：朝向目标的直行和沿边界绕行。与Bug1算法不同的是，Bug2算法中的直线m−linem-linem−line 是连接初始点和目标点的直线，在计算过程中保持不变。当机器人在点遇到障碍物时，机器人开始绕行障碍物，如果机器人在绕行过程中在距离目标更近的点再次遇到直线m−linem-linem−line ，那么就停止绕行，继续沿着直线 m−linem-linem−line 向目标直行。如此循环，直到机器人到达目标点 qgoalq_{goal}qgoal 。如果机器人在绕行过程中未遇到直线 m−linem-linem−line上与目标更近的qiLq^L_iqiL 点而回到了qiHq^H_iqiH 点，那么得出结论，机器人不能到达目标。

Bug2算法伪代码：

BUG1和BUG2算法提供了搜索问题的两种基本方法：比较保守的BUG1算法进行详细的搜索来获得最佳的离开点。这需要机器人环绕整个障碍物的边界。而BUG2算法使用一种投机的方法。机器人不环绕完整的障碍物，而选择第一个可用的点作为离开点。对于一般的环境，BUG2算法的效率更高；而对于复杂形状的障碍物，保守的BUG1算法性能更优。

Bug2算法在一般情况下具有很短的移动路径，然而这种策略并非完美。如图2.4所示的螺旋形障碍物，其边界与m-line多次相交，我们可以根据上述Bug2算法的伪代码确定其运动路径：

qstartq_{start}qstart→qgoalq_{goal}qgoal，遇到障碍物，到达撞击点q1Hq^H_1q1H；
q1Hq^H_1q1H开始环绕障碍物，直到与m−linem-linem−line相交到达m点（此时进行判断：没有到达目标；没有再次遇到q1Hq^H_1q1H；相比q1Hq^H_1q1H点m点离目标更近；继续朝目标前进不会碰到障碍），则q1L=m，i=2q^L_1=m，i=2q1L=m，i=2。机器人从q1Lq^L_1q1L沿着m−linem-linem−line继续朝目标前进；
再次遇到障碍物，到达撞击点q2Hq^H_2q2H，然后沿着障碍物边界移动，直到再次与m−linem-linem−line相交到达m点（此时进行判断：没有到达目标；没有再次遇到q2Hq^H_2q2H；但继续朝目标前进会碰到障碍），由于不满足离开点的条件，则继续环绕；
机器人环绕边界到达q1Hq^H_1q1H点，与m−linem-linem−line相交（此时进行判断：没有到达目标；没有再次遇到q2Hq^H_2q2H；但继续朝目标前进会碰到障碍），由于不满足离开点的条件，则继续环绕目标；
机器人继续环绕边界到达q1Hq^H_1q1H点，与m−linem-linem−line相交（此时进行判断：没有到达目标；没有再次遇到q2Hq^H_2q2H；继续朝目标前进不会碰到障碍；但此时相比q2Hq^H_2q2H机器人离目标位置更远），因此也不满足离开点条件，则继续环绕；
机器人环绕边界，与m−linem-linem−line相交到达m点（此时进行判断：没有到达目标；没有再次遇到q2Hq^H_2q2H；继续朝目标前进不会碰到障碍；但此时相比q2Hq^H_2q2H机器人离目标位置更远），因此也不满足离开点条件，则继续环绕；
机器人环绕边界，与m−linem-linem−line相交到达m点（此时进行判断：没有到达目标；没有再次遇到qH2；继续朝目标前进不会碰到障碍；相比q2Hq^H_2q2H机器人离目标位置更近），满足离开点条件，则q2L=m，i=3q^L_2=m，i=3q2L=m，i=3；
机器人从q2Hq^H_2q2H沿着m−linem-linem−line继续朝目标前进，到达目标位置。

Tangent Bug算法

TangentBUG算法是对BUG2算法的改进。它利用机器人上距离传感器的读数对障碍物做出提前规避，可以获得更短更平滑的机器人路径。假设机器人上安装有360°激光雷达(或者红外距离传感器),那么我们可以测得每束光线到达障碍物的距离.下图中xxx代表机器人的位置,细线代表发出的光线,粗线代表了光线被遮挡(说明机器人无法到达这些位置).

我们用 OiO_iOi标记光线与障碍物相交的边界点:

与其他的Bug算法一样，Tangent Bug算法也有两种行为：直行（motion-to-go）和环绕障碍物（boundary-following）。
算法过程：

机器人直接沿着目标方向按直线行走,直到激光雷达检测到了障碍物。
用虚线的圆表示激光雷达的检测范围。
标记出 OiO_iOi,然后机器人向着启发距离最小的 OiO_iOi前进。

关于OiO_iOi的选择

启发距离是人为规定的,比如我们可以用 d(x,Oi)+d(Oi,qgoal)d(x,O_i)+d(O_i,q_{goal})d(x,Oi)+d(Oi,qgoal)来表示.不同的目标位置会导致机器人对每一步 OiO_iOi的选择不同,比如下面这幅图中,左图机器人选择了 O2O_2O2,而右图机器人选择了 O4O_4O4 .

当然OiO_iOi的值是实时更新的,这将导致最后机器人靠近障碍物时行走的轨迹是一条曲线而不是直线

在机器人运动过程中，探索距离不再减小时，就停止向目标运动行为，切换到环绕边界行为。此时，机器人找到了探测距离的一个极小值，并可计算已探测的障碍物边界与目标qgoalq_{goal}qgoal 的最近距离dfollowedd_{followed}dfollowed 。机器人按照原来的方向环绕障碍物运动，同时机器人更新当前探测的障碍物边界与目标的最近距离dreachd_{reach}dreach 。当发现dreach<dfollowedd_{reach}<d_{followed}dreach<dfollowed 时，机器人停止障碍物环绕行为，继续向目标运动。

如上图所示，当机器人探索到障碍物上的 MMM点后，探索距离就不再减小，即 MMM 点是机器人探索距离在障碍物边界上的局部极小点。机器人开始沿着障碍物边界进行环绕，图中虚线路径就是机器人环绕障碍物时所走的路径。当机器人探测到与目标距离相比 MMM点更近的点时，重新开始接近目标的运动。

Tangent Bug算法伪代码如下：

激光雷达半径对算法的影响

使用Tanget Bug可以有效的提升整体效率,这是激光雷达测量半径为RRR 时形成的轨迹,当测量半径退化为0时(变成了接触传感器),Tangent Bug算法就退化成了Bug2算法：

总结

Bug算法是最早被提出来的路径规划算法，很符合直觉，尤其是在完全一无所知的环境下，Bug算法往往能够取得非常不错的效果！