記憶運動方向的機器人避障算法

魯統偉，林 芹，李 熹，鄒 旭

（1.武漢工程大學計算機科學與工程學院，湖北 武漢 430074；2.智能機器人湖北省重點實驗室，湖北 武漢 430074）

0 引 言

機器人同時定位與地圖創建即SLAM（Simultaneous Location and Mapping）指的是機器人在自身位置不確定的條件下，在完全未知的環境中創建地圖，同時利用地圖進行自主定位和導航.他需要將移動機器人的位置和環境特征坐標表示在一個狀態向量中，在機器人的步行過程中通過對環境特征的觀測做最有準則的估計和更新［1－2］.SLAM 中的問題可以歸結為以下四點：地圖表示、不確定信息處理、數據關聯、搜索規劃［3－4］.其中搜索規劃一直是移動機器人導航技術的重要環節.

所謂搜索規劃是指移動機器人在有障礙物的工作環境中尋找一條從給定起點到終點的適當的運動路徑，使機器人在運動過程中能安全、無碰撞的繞過所有障礙物［5－6］.

SLAM導航算法發展至今，已經有很多代表性算法［7－8］.如 Lumelsky和 Stepanov1980年首次提出的bug1、bug2算法，隨后提出的Alg1、Alg2算法，都是事先規定了某一個運動方向，簡單易懂，但是計算時間、路徑長度都很長［9］.Kamon和Rivlin在1997年提出的Distbug算法，由于不需要記住以前的點，因此內存要求比較少，路徑和Alg2相似，路徑長度和時間復雜度都比較長［9］.以及1994年Stentz提出的動態A＊算法，能夠產生很短的路徑，但是由于時間復雜度和空間復雜度都非常大，需要創建地圖.Tangentbug（切線局部規劃算法）算法是bug類算法中比較出色的一種［9］.相對于以上算法來說，它最突出的特色是可以根據掃描范圍生成LTG（Local Tangent Graph，局部切線圖），分析LTG智能選擇運動方向，計算簡單，實時性強，內存要求不高，能夠在較短的時間內根據局部信息獲得較短的路徑并具有全局收斂的特點［10］.

1 Tangentbug算法

Tangengbug算法是Kamon和Rivlin在1995年提出的，該算法已被證明能夠僅通過局部信息生成較短較優的路徑［8］.該算法假設機器人有一個距離傳感器，能夠掃描到半徑為R范圍為360°的區域，機器人利用傳感器收集局部地圖信息，生成局部切線圖，并利用局部切線圖選擇路徑.

Tangentbug算法流程圖如圖1所示，其中LTG（Local Tangent Graph）為局部切線圖，dmin為沿障礙物運動過程中到目標點T的最短距離，dleave為掃描范圍內到目標點T最近的距離，Hi即Hit點，為機器人與障礙物相遇的點.

圖1 Tangentbug流程圖Fig.1 Flow chart of Tangentbug

Tangentbug算法中最主要的兩個部分為生成LTG（局部切線圖）和運動方向選擇.它們貫穿算法的始終.

LTG（局部切線圖）是機器人在行走過程中通過傳感器獲得的局部地圖信息.機器人在運動過程中通過傳感器感知障礙物，并記錄以機器人local＿point（當前位置）為中心，傳感器掃描范圍R為半徑的圓與障礙物的交點Oi，連接Oi與local＿point和終點T，生成局部切線圖［11－12］.機器人通過切線圖中各點的信息，如該點到機器人當前位置和終點的距離和，計算出下一步的移動路徑.

運動方向確定：機器人在行走過程中始終通過傳感器掃描當前環境，并通過計算LTG來獲得運動方向next＿point（下一目標點）.機器人傳感器掃描范圍內無障礙物時，機器人計算掃描范圍上所有點到終點的距離即d（x，T），并找到最近點即機器人到終點的直線與掃描范圍的交點作為運動方向［13］.當掃描范圍內出現障礙物時，對LTG中每個交點Oj，計算距離和 Dist（Oj）＝d（x，Oj）＋d（Oj，T），當該等式最小時點Oj記做Omin，選擇Omin作為下一個運動方向點next＿point.

2 基于記憶運動方向的Tangentbug算法

按照Tangentbug算法中根據計算Omin（距離和最小點）來選擇下一個運動方向next＿point時，在某些環境特別是對稱環境中會出現路徑循環.如圖2所示，在點P1處，掃描計算得到的Omin為P2和P0，選擇P2作為next＿point時，運動到P2后掃描計算所得的下一個運動方向next＿point又為P1，這樣循環往復從而導致終點不可達.

圖2 路徑循環示例Fig.2 The example of cycle

為避免此類情況，提出基于記憶機器人運動方向的Tangentbug算法.該算法主要包括兩個部分：機器人直線運動和機器人繞障礙物移動［12－13］.該算法不僅通過計算Omin來確定next＿point，而且計算機器人與障礙物相遇方向hit＿direct和機器人運動方向move＿direct，并記住運動方向.在繞行中根據運動方向并結合計算Dist（距離和）來選擇next＿point，在直行和繞行轉換時候根據hit＿direct計算并更新機器人運動方向move＿direct.

Tangentbug算法和其他SLAM（同時定位與地圖創建）算法一樣將機器人比擬為一個質點，但是在實際比賽中是不可行的，因此將機器人設定為一個形狀的物體，并在算法中考慮到其形狀對是否遇見障礙物和next＿point的影響做出修改.

Tangentbug算法問題可以細分為以下幾個方面：1）機器人與障礙物相遇方向的計算；2）機器人運動方向的計算；3）確定機器人直線運動的方向；4）確定機器人從直線運動到沿障礙物邊界運動的切換點即Hit點；5）確定機器人繞障礙物邊界運動的方向；6）確定機器人離開障礙物開始直線運動的切換點即leave點；7）是否能夠到達終點.具體思路如下：

在掃描過程中，對掃描圓上每個點scan＿point［i］（scan＿point數組，用于存放掃描圓上的點信息）處，計算以下信息：該點像素值clr；該點到機器人位置local＿point和終點的距離和Dist（scan＿point［i］）；機器人運動方向move＿direct；機器人與障礙物的相遇方向hit＿direct；是否為Hit點（相遇點）；是否到達終點；是否為leave點（離開點）.并將掃描的點的信息存入scan＿point數組中.根據數組中點的顏色信息，分割數組：即將數組中的第一個顏色信息為障礙物的點i和最后一個顏色信息為障礙物的點j及其之間的點放入數組obstcl＿point（用于存放障礙物信息的障礙物數組），表示障礙物，其余點放入free＿point（用于存放自由點的數組）中，表示可行區域.

1）障礙物相遇方向的計算：當掃描范圍內出現障礙物時，對obstcl＿point（障礙物數組）中點的hit＿direct（相遇方向）進行統計，分別比較方向為上下左右的數目，選擇其中數目最大者作為障礙物相遇方向.其中每個點的上下左右方向則通過比較該點與起點的XY坐標軸的距離差subx（x方向距離差）和suby（y方向上距離差）來確定.當subx的絕對值大于suby時運動方向為上下方向，反之為左右方向.

2）運動方向的計算：move＿direct（運動方向）在沒有掃描到障礙物時僅通過比較與起點的XY坐標軸的距離差subx（x方向距離差）和suby（y方向上距離差）來確定.在掃描到障礙物時候，運動方向要根據障礙物相遇方向來確定，當障礙物相遇方向為上下時，即障礙物在機器人上下方向這時候運動方向僅比較subx來確定為左或右，反之當障礙物在機器人左右方向上，機器人的運動方向僅判斷是上或者下.

3）機器人直線運動：機器人直線運動可以分為兩種情形：一是在該點處機器人掃描范圍內無障礙物及該點為leave點（離開點）.此時將數組free＿point（空閑數組）中的數值按照插入排序法排序.從最小點開始循環判斷，從起點到出發到該點過程中是否會撞到障礙物，及該點的運動方向與上次直線運動的方向是否一致.找到符合的點即不會撞見障礙物且與上次直線運動方向一致作為next＿point（下一運動目標），并更新 move＿direct（運動方向）.二是在該點處機器人掃描范圍內出現障礙物，并能夠到達且未到達終點，分別對數組free＿point和obstcl＿point（障礙物數組）進行排序，并求出最小值，并將兩個數組的最小值進行比較.選擇途中不會撞見障礙物的且運動方向與上次直線運動方向一致的點作為next＿point并更新 move＿direct，此時 move＿direct應根據障礙物相遇方向重新計算.

4）Hit點的確定：Tangentbug算法和其他算法一樣，將機器人看作一個質點，但是在實際中，特別是避障比賽中，是不可以看成質點的，因此在仿真中假設機器人為一個長方體，在避障中主要考慮長和寬.在掃描過程中通過判斷長方形上離中心距離為L的八個方向上的點的像素值來判斷是否為障礙物.如圖3所示，當A點像素為RGB（169，169，169）則表示機器人遇見障礙物，機器人當前位置為Hit點（相遇點）.

圖3 機器人判斷Hit的八個方向Fig.3 The eight direction of robot to check Hit

5）繞行方向：繞行方向主要通過計算LTG（局部切線圖）中交點信息得到.在繞行時，通過傳感器掃描障礙物，生成LTG圖，并通過遍歷LTG圖中所有O點（切點）信息（包括該點的像素，該點的Dist值，該點是否為終點等），并將LTG圖中O點按照Dist（即該點到當前機器人位置與到終點的距離和）信息進行排序，按照Dist增大的方向遍歷數組，選擇滿足以下條件的點作為next＿point（下一運動目標），并更新 move＿direct（運動方向）：a.從起點運動到該點的過程中不會遇見障礙物.b.該點的運動方向與上次機器人的運動方向一致.c.該點在不是機器人第三次到達.

6）Leave點的確定：首先更新leave＿point（離開點）和min＿point（最小點，即繞行中遇見的點中距離和最小的點）：在繞行中計算Free＿point（空閑數組）和和LTG中的obstcl＿point（障礙物數組）的Dist（距離和）并排序，選擇滿足繞行方向的三個條件且Dist最小的點分別和min＿point和leave＿point比較，當free＿point小于 min＿point，則用最小值更新min＿point，同理obstcl＿point最小點小于leave＿point，那么更新leave＿point.比較min＿point和leave＿point，當滿足條件dleave（離開點的距離和）＜dmin（最小點的距離和）時跳轉到直線運動.

7）能否到達終點：在運動過程中用vector數組記住每個next＿point（下一運動目標），在存入前與數組中的點進行比較.如果出現一次重復，那么表示運動方向選擇可能錯誤，更改該點的運動方向為相反方向繼續掃描；如果重復兩次則表示機器人又一次回到該點，終點不可到達.

3 Tangentbug算法的仿真

3.1 環境仿真

智能機器人避障涉及到機器人結構技術、控制技術、傳感器技術、障礙物檢測與定位、路徑規劃、導航、和信息融合等等方面，是FIRA ROBO WORLD CUP（國際機器人足球聯盟世界杯）的常規比賽項目.在該比賽中，裁判在6×6m的場地中擺放用大于機器人的紙板代替的障礙物、終點和用白色膠帶代表的邊界，其中障礙物至少一個，形狀隨機且顏色和終點不同.機器人需要在裁判設計的避障環境中，通過傳感器掃描當前環境，根據里程計獲知自身坐標和方向，通過視覺處理獲知當前環境，并在最短時間內，避開障礙物到達終點.而對稱環境即存在一個或者多個障礙物相對于終點和起點的直線對稱，是比賽中常見的避障環境.

在仿真過程中，將機器人設定為3×4像素的長方形，場地范圍為400×400像素的正方形，避障邊界不超過場地范圍，掃描范圍假設為35像素.并用黑色代表邊界，灰色代表障礙物，底部圓點代表起點，頂部圓點代表終點.圖4為避障比賽中最常見的環境的仿真，且為對稱環境.

圖4 對稱避障環境示例Fig.4 The example of symmetric obstacle avoidance environment

3.2 處理過程

掃描仿真：機器人掃描器掃描以機器人為中心，掃描范圍R為掃描半徑的圓內的區域.仿真通過小角度逼近畫圓弧實現掃描，圓的中心為機器人當前位置，半徑為掃描范圍R.畫圓函數具體實現思路如圖5所示，從0°開始到360°結束，每隔θ度畫一條直線（圖中實線），長度為R＊sin（i＊θ）.

識別仿真：在仿真中通過獲取顏色GetPixel（）函數獲得當前點顏色，并判斷顏色值來識別當前環境，如GetPixel（）函數值為255時表示為終點.下一段路徑仿真：通過畫線實現.用藍色直線連接各個next＿point（下一運動目標）仿真機器人運動路徑.

圖5 掃描圓仿真Fig.5 Simulation of scan circle

3.3 仿真結果

仿真中分別對6種存在對稱障礙物環境進行實驗，實驗結果如圖6所示.

圖6 仿真結果示例Fig.6 The example of simulation result

4 結 語

以上針對Tangentbug算法在存在對稱障礙物環境中易產生循環路徑從而導致不可到達的情況進行改進，提出了基于記憶運動方向的Tangentbug算法.通過計算障礙物相遇方向來更新機器人運動方向，并在繞行中分別保持其運動方向，在直行和繞行轉換時更新運動方向的約束下選擇下一個運動點.并打破機器人看作質點的約束，將機器人簡化為一個長方形，從八個方向進行判斷是否遇見障礙物，從而有效避免了機器人與障礙物的碰撞.

從仿真結果可以看出機器人可以按照比賽要求從起點，無碰撞的到達終點.然而機器人避障導航是一項復雜的任務，盡管改進在多種存在對稱環境中進行了試驗，但是本算法在路徑長度、時間復雜度等方面仍需進一步深入研究.

［1］遲建男，徐心和.移動機器人即時定位與地圖創建問題研究［J］.機器人，2004，26（1）：92－96.

［2］張捍東，鄭睿，岑豫皖.移動機器人路徑規劃技術的現狀和展望［J］.系統仿真學報，2005，17（2）：439－443.

［3］劉祥，陳建新.一種基于有限視場的移動機器人避障路徑規劃算法［J］.空間控制技術與應用，2008，34（4）：11－16.

［4］羅榮華，洪炳榮.移動機器人同時定位與地圖創建研究進展［J］.機器人，2004，26（2）：182－186.

［5］王坤.液體狀態機在機器人足球路徑規劃中的應用［D］.武漢：武漢工程大學，2009.

［6］孔偉，張彥鐸.基于遺傳算法的自主機器人避障方法研究［J］.武漢工程大學學報，2008，30（3）：110－113.

［7］何俊學，李戰明.基于視覺的同時定位與地圖創建方法綜述［J］.計算機應用研究，2010，27（8）：2839－2843.

［8］石杏喜，趙春霞，郭劍輝.基于PF／CUPF／EKF的移動機器人SLAM 框架算法［J］.電子學報，2009，37（8）：1865－1868.

［9］James Ng.A Practical Comparison of Robot Planning Algorithm Given Only Local Information［D］.Australia：The University of Western Australia，2005.

［10］Yufka A，Parlaktuna O.Performance Com－parison of Bug Algorithms for Mobile robots［C］／／5th International Advanced Technologies Sympos－ium（IATS＇09）.Mustafa ACARER，Halil Ibrahim DEMIRCI，Cevdet GOLOGLU .Karabuk，Turkey：Karabuk University，2009：61－65.

［11］Choset H，Lynch K，Hutchinson S，et al.Principles of Robot Motion：Theory，Algorithms and Im－plementation［M］.Cambridge：The MIT Press，2005：11－18.

［12］趙祚喜，汪寧，張智剛，等.一種適用于非360°探測機器人的避障導航算法［J］.機械工程學報，2010，46（19）：44－52.

［13］Kamon I，Rivlin E，Rimon E.A New Range－sensor Based on Globally Convergent Navigation Algorithm for Mobile Robots［C］／／In Proceedings of the 1996IEEE International Conference on Robotics and Automation Minneapolis.M T J Tarn.Minneapolis，Minnesota：IEEE Conference Publications，1996（1）：429－435.