記憶運(yùn)動(dòng)方向的機(jī)器人避障算法

魯統(tǒng)偉，林 芹，李 熹，鄒 旭

（1.武漢工程大學(xué)計(jì)算機(jī)科學(xué)與工程學(xué)院，湖北 武漢 430074；2.智能機(jī)器人湖北省重點(diǎn)實(shí)驗(yàn)室，湖北 武漢 430074）

0 引 言

機(jī)器人同時(shí)定位與地圖創(chuàng)建即SLAM（Simultaneous Location and Mapping）指的是機(jī)器人在自身位置不確定的條件下，在完全未知的環(huán)境中創(chuàng)建地圖，同時(shí)利用地圖進(jìn)行自主定位和導(dǎo)航.他需要將移動(dòng)機(jī)器人的位置和環(huán)境特征坐標(biāo)表示在一個(gè)狀態(tài)向量中，在機(jī)器人的步行過程中通過對(duì)環(huán)境特征的觀測(cè)做最有準(zhǔn)則的估計(jì)和更新［1－2］.SLAM 中的問題可以歸結(jié)為以下四點(diǎn)：地圖表示、不確定信息處理、數(shù)據(jù)關(guān)聯(lián)、搜索規(guī)劃［3－4］.其中搜索規(guī)劃一直是移動(dòng)機(jī)器人導(dǎo)航技術(shù)的重要環(huán)節(jié).

所謂搜索規(guī)劃是指移動(dòng)機(jī)器人在有障礙物的工作環(huán)境中尋找一條從給定起點(diǎn)到終點(diǎn)的適當(dāng)?shù)倪\(yùn)動(dòng)路徑，使機(jī)器人在運(yùn)動(dòng)過程中能安全、無碰撞的繞過所有障礙物［5－6］.

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

1 Tangentbug算法

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

Tangentbug算法流程圖如圖1所示，其中LTG（Local Tangent Graph）為局部切線圖，dmin為沿障礙物運(yùn)動(dòng)過程中到目標(biāo)點(diǎn)T的最短距離，dleave為掃描范圍內(nèi)到目標(biāo)點(diǎn)T最近的距離，Hi即Hit點(diǎn)，為機(jī)器人與障礙物相遇的點(diǎn).

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

Tangentbug算法中最主要的兩個(gè)部分為生成LTG（局部切線圖）和運(yùn)動(dòng)方向選擇.它們貫穿算法的始終.

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

運(yùn)動(dòng)方向確定：機(jī)器人在行走過程中始終通過傳感器掃描當(dāng)前環(huán)境，并通過計(jì)算LTG來獲得運(yùn)動(dòng)方向next＿point（下一目標(biāo)點(diǎn)）.機(jī)器人傳感器掃描范圍內(nèi)無障礙物時(shí)，機(jī)器人計(jì)算掃描范圍上所有點(diǎn)到終點(diǎn)的距離即d（x，T），并找到最近點(diǎn)即機(jī)器人到終點(diǎn)的直線與掃描范圍的交點(diǎn)作為運(yùn)動(dòng)方向［13］.當(dāng)掃描范圍內(nèi)出現(xiàn)障礙物時(shí)，對(duì)LTG中每個(gè)交點(diǎn)Oj，計(jì)算距離和 Dist（Oj）＝d（x，Oj）＋d（Oj，T），當(dāng)該等式最小時(shí)點(diǎn)Oj記做Omin，選擇Omin作為下一個(gè)運(yùn)動(dòng)方向點(diǎn)next＿point.

2 基于記憶運(yùn)動(dòng)方向的Tangentbug算法

按照Tangentbug算法中根據(jù)計(jì)算Omin（距離和最小點(diǎn)）來選擇下一個(gè)運(yùn)動(dòng)方向next＿point時(shí)，在某些環(huán)境特別是對(duì)稱環(huán)境中會(huì)出現(xiàn)路徑循環(huán).如圖2所示，在點(diǎn)P1處，掃描計(jì)算得到的Omin為P2和P0，選擇P2作為next＿point時(shí)，運(yùn)動(dòng)到P2后掃描計(jì)算所得的下一個(gè)運(yùn)動(dòng)方向next＿point又為P1，這樣循環(huán)往復(fù)從而導(dǎo)致終點(diǎn)不可達(dá).

圖2 路徑循環(huán)示例Fig.2 The example of cycle

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

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

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

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

1）障礙物相遇方向的計(jì)算：當(dāng)掃描范圍內(nèi)出現(xiàn)障礙物時(shí)，對(duì)obstcl＿point（障礙物數(shù)組）中點(diǎn)的hit＿direct（相遇方向）進(jìn)行統(tǒng)計(jì)，分別比較方向?yàn)樯舷伦笥业臄?shù)目，選擇其中數(shù)目最大者作為障礙物相遇方向.其中每個(gè)點(diǎn)的上下左右方向則通過比較該點(diǎn)與起點(diǎn)的XY坐標(biāo)軸的距離差subx（x方向距離差）和suby（y方向上距離差）來確定.當(dāng)subx的絕對(duì)值大于suby時(shí)運(yùn)動(dòng)方向?yàn)樯舷路较?，反之為左右方?

2）運(yùn)動(dòng)方向的計(jì)算：move＿direct（運(yùn)動(dòng)方向）在沒有掃描到障礙物時(shí)僅通過比較與起點(diǎn)的XY坐標(biāo)軸的距離差subx（x方向距離差）和suby（y方向上距離差）來確定.在掃描到障礙物時(shí)候，運(yùn)動(dòng)方向要根據(jù)障礙物相遇方向來確定，當(dāng)障礙物相遇方向?yàn)樯舷聲r(shí)，即障礙物在機(jī)器人上下方向這時(shí)候運(yùn)動(dòng)方向僅比較subx來確定為左或右，反之當(dāng)障礙物在機(jī)器人左右方向上，機(jī)器人的運(yùn)動(dòng)方向僅判斷是上或者下.

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

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

圖3 機(jī)器人判斷Hit的八個(gè)方向Fig.3 The eight direction of robot to check Hit

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

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

7）能否到達(dá)終點(diǎn)：在運(yùn)動(dòng)過程中用vector數(shù)組記住每個(gè)next＿point（下一運(yùn)動(dòng)目標(biāo)），在存入前與數(shù)組中的點(diǎn)進(jìn)行比較.如果出現(xiàn)一次重復(fù)，那么表示運(yùn)動(dòng)方向選擇可能錯(cuò)誤，更改該點(diǎn)的運(yùn)動(dòng)方向?yàn)橄喾捶较蚶^續(xù)掃描；如果重復(fù)兩次則表示機(jī)器人又一次回到該點(diǎn)，終點(diǎn)不可到達(dá).

3 Tangentbug算法的仿真

3.1 環(huán)境仿真

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

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

圖4 對(duì)稱避障環(huán)境示例Fig.4 The example of symmetric obstacle avoidance environment

3.2 處理過程

掃描仿真：機(jī)器人掃描器掃描以機(jī)器人為中心，掃描范圍R為掃描半徑的圓內(nèi)的區(qū)域.仿真通過小角度逼近畫圓弧實(shí)現(xiàn)掃描，圓的中心為機(jī)器人當(dāng)前位置，半徑為掃描范圍R.畫圓函數(shù)具體實(shí)現(xiàn)思路如圖5所示，從0°開始到360°結(jié)束，每隔θ度畫一條直線（圖中實(shí)線），長(zhǎng)度為R＊sin（i＊θ）.

識(shí)別仿真：在仿真中通過獲取顏色GetPixel（）函數(shù)獲得當(dāng)前點(diǎn)顏色，并判斷顏色值來識(shí)別當(dāng)前環(huán)境，如GetPixel（）函數(shù)值為255時(shí)表示為終點(diǎn).下一段路徑仿真：通過畫線實(shí)現(xiàn).用藍(lán)色直線連接各個(gè)next＿point（下一運(yùn)動(dòng)目標(biāo)）仿真機(jī)器人運(yùn)動(dòng)路徑.

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

3.3 仿真結(jié)果

仿真中分別對(duì)6種存在對(duì)稱障礙物環(huán)境進(jìn)行實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如圖6所示.

圖6 仿真結(jié)果示例Fig.6 The example of simulation result

4 結(jié) 語

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

從仿真結(jié)果可以看出機(jī)器人可以按照比賽要求從起點(diǎn)，無碰撞的到達(dá)終點(diǎn).然而機(jī)器人避障導(dǎo)航是一項(xiàng)復(fù)雜的任務(wù)，盡管改進(jìn)在多種存在對(duì)稱環(huán)境中進(jìn)行了試驗(yàn)，但是本算法在路徑長(zhǎng)度、時(shí)間復(fù)雜度等方面仍需進(jìn)一步深入研究.

［1］遲建男，徐心和.移動(dòng)機(jī)器人即時(shí)定位與地圖創(chuàng)建問題研究［J］.機(jī)器人，2004，26（1）：92－96.

［2］張捍東，鄭睿，岑豫皖.移動(dòng)機(jī)器人路徑規(guī)劃技術(shù)的現(xiàn)狀和展望［J］.系統(tǒng)仿真學(xué)報(bào)，2005，17（2）：439－443.

［3］劉祥，陳建新.一種基于有限視場(chǎng)的移動(dòng)機(jī)器人避障路徑規(guī)劃算法［J］.空間控制技術(shù)與應(yīng)用，2008，34（4）：11－16.

［4］羅榮華，洪炳榮.移動(dòng)機(jī)器人同時(shí)定位與地圖創(chuàng)建研究進(jìn)展［J］.機(jī)器人，2004，26（2）：182－186.

［5］王坤.液體狀態(tài)機(jī)在機(jī)器人足球路徑規(guī)劃中的應(yīng)用［D］.武漢：武漢工程大學(xué)，2009.

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

［7］何俊學(xué)，李戰(zhàn)明.基于視覺的同時(shí)定位與地圖創(chuàng)建方法綜述［J］.計(jì)算機(jī)應(yīng)用研究，2010，27（8）：2839－2843.

［8］石杏喜，趙春霞，郭劍輝.基于PF／CUPF／EKF的移動(dòng)機(jī)器人SLAM 框架算法［J］.電子學(xué)報(bào)，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°探測(cè)機(jī)器人的避障導(dǎo)航算法［J］.機(jī)械工程學(xué)報(bào)，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.