移動機器人智能的一個重要標志就是自主導航，而實現機器人自主導航有個基本要求——避障。之前簡單介紹過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°圓弧。分布情況如下圖所示：
有時機器人有必要對某物體的輪廓進行跟蹤：
- 當機器人沿預期路徑運動時遇到障礙物，它可能會通過在障礙物周邊移動，繞過這個障礙物。
- 在辦公室環境中，用於遞送文件的機器人肯能會沿着牆壁在走廊里行走，依次拜訪每個房間。
- 一種使機器人在錯綜復雜的走廊中在一個方向（左或右）沿牆壁運動的策略。
機器人在遇到一個障礙物前一直向前運動。當遇到障礙物時，機器人將停止向前運動，並對牆面進行跟蹤。為了理解機器人怎么對牆壁進行跟蹤，想象你被蒙上眼睛時，站在離牆壁很近的地方，並沿着牆壁行走到達目的地。你可能會伸出一只手（如果牆在你的右邊，伸出右手）來幫助你確定牆壁的位置。隨着你和牆壁之間的距離增大，你的手最終將不能觸摸到牆壁。然后你需要右轉並向前運動以再次靠近牆壁。如果你發現自己與牆壁越來越近，則需要彎曲手臂。為了維持伸出手臂，你必須向遠離牆壁的方向旋轉，以避免撞到牆上。
下面根據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