Bug2算法的实现（RobotBASIC环境中仿真）

　　移动机器人智能的一个重要标志就是自主导航，而实现机器人自主导航有个基本要求——避障。之前简单介绍过Bug避障算法，但仅仅了解大致理论而不亲自动手实现一遍很难有深刻的印象，只能说似懂非懂。我不是天才，不能看几遍就理解理论中的奥妙，只能在别人大谈XX理论XX算法的时候，自己一个人苦逼的面对错误的程序问为什么...

　　下面开始动手来实现一下简单的Bug2避障算法。由于算法中涉及到机器人与外界环境的交互，因此需要选择一个仿真软件。常用的移动机器人仿真软件主要有Gazebo、V-rep、Webots、MRDS(microsoft robotics developer studio)等，这些软件基本都是三维环境下的模拟仿真，而目前大多数移动机器人均采用轮式结构，一般只在平面上运动。为了简化建模和编程方便，选择了一个平面环境下的机器人仿真软件——RobotBASIC。RobotBASIC的使用方法很简单，语法与标准的BASIC语言相似，它能使你简单、迅速地模拟多种类型的环境和情况。

　　在Bug2算法中机器人围绕障碍物行走需要机器人能感知到障碍物的存在，可以通过碰撞传感器或红外接近开关实现。RobotBASIC中的机器人身体上装有5个红外传感器，分别间隔45°安装，如下图所示。通过函数rFeel()可获得表示红外传感器状态的编码数字，如果该方向检测到障碍则对应的位置1。即如果只在正前方检测到障碍则rFeel()返回 0b00100 = 4. 与一般碰撞传感器相比，红外传感器探测物体效果更好，因为最好不与环境接触发生碰撞，哪怕是很小的碰撞也可能会对机器人造成影响。

　　RobotBASIC中的碰撞传感器有4个，分布在机身周围。前后的碰撞传感器分别形成130°圆弧，侧面两个形成50°圆弧。分布情况如下图所示：

有时机器人有必要对某物体的轮廓进行跟踪：

1. 当机器人沿预期路径运动时遇到障碍物，它可能会通过在障碍物周边移动，绕过这个障碍物。
2. 在办公室环境中，用于递送文件的机器人肯能会沿着墙壁在走廊里行走，依次拜访每个房间。
3. 一种使机器人在错综复杂的走廊中在一个方向（左或右）沿墙壁运动的策略。

　　机器人在遇到一个障碍物前一直向前运动。当遇到障碍物时，机器人将停止向前运动，并对墙面进行跟踪。为了理解机器人怎么对墙壁进行跟踪，想象你被蒙上眼睛时，站在离墙壁很近的地方，并沿着墙壁行走到达目的地。你可能会伸出一只手（如果墙在你的右边，伸出右手）来帮助你确定墙壁的位置。随着你和墙壁之间的距离增大，你的手最终将不能触摸到墙壁。然后你需要右转并向前运动以再次靠近墙壁。如果你发现自己与墙壁越来越近，则需要弯曲手臂。为了维持伸出手臂，你必须向远离墙壁的方向旋转，以避免撞到墙上。

　　下面根据Bug避障算法简介中的伪代码实现机器人在未知环境中的避障行走。可以在程序中设定不同的目标位置，并用鼠标在屏幕上选好一个机器人初始位置之后按下左键确定（为了模拟更加动态的未知环境，还可以在机器人移动过程中随时加入一个障碍物，但下面的程序还没有实现这一功能，有待优化）。

MainProgram:
    //-------------define variables here-----------
    target_x =  400
    target_y =  150
    HitPoint_x = 0
    HitPoint_y = 0
    dx0 = 0
    dy0 = 0
    
    RobotSize = 10
    delayTime = 2 
    TurnDir = 1  
    
    isFirstHit = false
    isReachTarget = false
    isAbleToTarget = true

    
    gosub DrawScene
    gosub InitializeRobot
   
    rInvisible Red,Gray
    LineWidth 2
    rPen Down,Red
   
    repeat 
        readmouse x,y,b
    until b = 2  //Click the right button to start movement
   
    //----------------while loop---------------
    while true
        gosub MoveLineToTarget 
        if isReachTarget then break
        
        gosub FollowWall
        if isReachTarget then break
        if (not isAbleToTarget) then break    
    wend           
End
//==========================================================
DrawScene:
    ClearScr
    LineWidth 3 
    Data Wall_1; -100,450,  500,450,  500,360,  100,360,  100,450,  150,-400
    Data Wall_2; -250,250,  500,250,  500,50,   450,50,  450,200,  250,200, 250,250, 260,-240
    
    MPolygon Wall_1, Gray             //Draw obstacles
    MPolygon Wall_2, Gray
    Circle  600,400, 700,200, Black, Gray 
    
    Circle (target_x - 5),(target_y + 5), (target_x + 5),(target_y - 5), Red, Red  //Draw target
Return    
//==========================================================
InitializeRobot:
    repeat 
        readmouse x, y, b
    until b = 1  //Click the left button to locate the robot
    
    //Calculate the initial angle for robot to face the target
    dx0 = target_x - x
    dy0 = target_y - y 
    if dx0 = 0 AND dy0 = 0
        theta = 0
    else
        theta = PolarA(dx0, dy0) * 180 / pi() + 90 
    if theta > 180 then theta = theta - 360 
    if theta < -180 then theta = theta + 360
    
    rLocate x, y, theta, RobotSize          //Initialization

    GotoXY x,y
    lineto target_x, target_y, 1, Gray      //Draw m-line  
Return  
//==========================================================
MoveLineToTarget:
    dx = target_x - rGpsX()
    dy = target_y - rGpsY()
    if dx = 0 AND dy = 0  then return
    theta = PolarA(dx, dy) * 180 / pi() + 90 - rCompass()  
    if theta > 180 then theta = theta - 360 
    if theta < -180 then theta = theta + 360
    rTurn theta
    distance = Round(polarR(dx, dy))
    for i = 1 to distance
        if rBumper() & 4
           isFirstHit = true
           HitPoint_x = rGpsX()      //record the hit point position
           HitPoint_y = rGpsY()
           break
        endif
        
        if PolarR(target_x-rGpsX(),target_y-rGpsY()) < 5
           isReachTarget = true
           xyString 2,2,"Get to the target!"
           break
        endif
        
        rForward 1                  //move forward one pixel
        delay delayTime
    next
Return
//==========================================================
FollowWall:  
    TurnAmount = 5
    If TurnDir > 0 
        FN = 6     //right and right front
    Else
        FN  = 12   //front and left front
    Endif


    while true
         // reach the goal ? 
         condition1 = PolarR(target_x - rGpsX(),target_y - rGpsY()) < 5 
         if condition1 
              isReachTarget = true
              xyString 2, 2, "Get to the target!"
              break
         endif

         // re-encounter the hit point ? 
         condition2 = (PolarR(rGpsX()-HitPoint_x,rGpsY()-HitPoint_y) < 5) and (not isFirstHit)    
         if condition2        
              isAbleToTarget = false
              xyString 2, 2, "Cannot reach the target!"
              break
         else
              temp = rGpsY() - (dy0 / dx0 * (rGpsX() - target_x) + target_y)
              // re-counter the m-line ?
              condition3 = ((temp * temp) < 12) and (not isFirstHit)
              // closer to the goal?
              condition4 =  PolarR(target_x-rGpsX(),target_y-rGpsY()) < PolarR(target_x-HitPoint_x,target_y-HitPoint_y)
              if (condition3 and condition4) then  break          
         endif    


         While (rFeel() & FN) or (rBumper() & 4)
             rTurn -TurnDir
         Wend


         rForward 1  // forward always to prevent stall
   
         if PolarR(rGpsX()-HitPoint_x,rGpsY()-HitPoint_y) > 5
             isFirstHit = false
         endif

         While not rFeel()
              // turn back quickly to find wall again
              rTurn TurnAmount*TurnDir
              rForward 1
         Wend

         delay delayTime

    wend
Return
//==========================================================

　　由于真实的环境是动态的、未知的、复杂的，可能随时出现一个障碍物挡在机器人面前，也可能存在形状复杂的障碍物（如Bug避障算法简介中出现的螺旋形障碍物），或者一开始机器人就被封闭在障碍物的内部。因此，算法的鲁棒性非常重要，需要设计出一个合理地规则解决上述问题。上述算法的实现过程在大体上是对的，但在细节上还存在诸多问题有待优化。下面几幅图给出了不同位置和不同转向时的情况，假如算法考虑的不够全面就可能会出现在某些初始位置或者换个转向就失效的情况。

　　初始时刻机器人被封闭在障碍物内部，无法到达目标，此时算法应该做出合理地判断，停止循环，避免一直在内部转圈：

　　如下图所示，机器人跟踪墙壁时换了一个转向。之前设定的是靠右行走，现在靠左行走。如果算法鲁棒性不好，就可能会出现换个转向就失效的情况。

 　　编写这个程序的时候遇到了很多问题：

　　1. 在RobotBASIC软件中机器人移动的距离最小为1个像素点，因此很多情况下会出现舍入误差。比如判断点是否在直线上，两点是否重合的时候就不能用"="，而需要设定一个合理的阈值用"<"或">"判断，阈值过大过小都不好。

　　2. 直线检测时对公式 y=dy/dx*(x-x0)+y0进行变形（考虑到有可能机器人和目标点处于一条竖直线上，此时斜率为无穷大），判断y*dx - dy(x-x0)+y0*dx = 0?  理论上如果点在直线上该等式的值就为0，我想当然的以为当点接近直线时，该式子的值也会很小，因此设了一个不大的阈值来检测点是否在直线上。而一般情况下dx的值都会有好几百，两边同乘dx后误差被放的很大，远远超出了阈值。最后还是用y=dy/dx*(x-x0)+y0来进行判断了，一偷懒没有写if dx=0的情况，毕竟用鼠标去定位机器人的时候很难使其正好与目标处于一条铅垂线上...我又不严谨了O__O "…

　　3. 当机器人沿着直线撞到障碍物时，切换到FollowWall程序。在FollowWall中需要不断检测机器人是否再次遇到hit point，如果再次遇到则说明无路可走，要退出主程序。因此设置一个标识变量isFirstHit，在MoveLineToTarget子程序中碰到障碍物时，isFirstHit设为true，然后程序跳入FollowWall中。while循环中检测是否为再次碰到hit point，如果是第一次则机器人继续绕墙行走，理论上执行rForward 1之后就可以直接将变量isFirstHit设为false了。但实际上这样在机器人第一次撞墙后就停止不动，并跳出了主程序！仔细分析走一个像素点之后就立马将标志取反将会在第二次进入while循环的时候造成判断错误。因为我们比较当前点是否为hit point的时候使用了一个距离阈值：(PolarR(rGpsX()-HitPoint_x,rGpsY()-HitPoint_y) < 5) ，即当前点距hit point在5个像素范围内就认为这两个点重合。因此还没等机器人走出这一范围，if语句的条件就成立为真，接着就执行了break语句跳出了循环。为了解决这一问题，可以让机器人多走一段距离之后再对isFirstTrue赋值false。问题又来了，走多远再对其赋值？检测两点重合的阈值设多大？（有时设小了机器人会直接越过该点，导致其完全停不下来...）这些问题都需要仔细琢磨。

MoveLineToTarget:
    ...
    isFirstHit = true
    ....
Return

FollowWall:
    while true
         ...

         if (PolarR(rGpsX()-HitPoint_x,rGpsY()-HitPoint_y) < 5) and (not isFirstHit)     
             break

         rForward 1  // move forward
   
         if PolarR(rGpsX()-HitPoint_x,rGpsY()-HitPoint_y) > 5
             isFirstHit = false
         ...

    wend
Return

经验与总结：

　　调试往往需要花费远远超过你想象的时间——有时比编写代码时间还要长得多，因此一定要有耐心。在调试期间获得的信息无论对于修改程序中的错误，还是进一步开发这个或其他程序，甚至对提高你解决问题的能力，都是非常有价值的。随着经验的积累，你会发现在细心进行系统设计和谨慎编写代码的过程中多花一些时间，能够大大节省调试的时间。

　　对一台真实的机器人，我们很难设想各种问题产生的原因，所以也就很难对算法进行综合测试。一台能在几种有限环境中运行良好的机器人，往往面对不可测的情况时就会失败。有了机器人模拟器就能构建多种不同的环境，这有助于实现算法，减少失败的可能性。不过仅仅依靠调试器查找错误并不能代替全面的分析推理。

 

参考：

John Blankenship, Samuel Mishal. 《机器人编程设计与实现》 科学出版社

Robotics, vision and control fundamental algorithms in MATLAB.  Chapter 5 · Navigation  pp.90-91