基于改進(jìn)A* 算法的智能倉(cāng)庫移動(dòng)機(jī)器人的路徑規(guī)劃

王海玲*

（廈門大學(xué)嘉庚學(xué)院，福建廈門，363105）

引言

機(jī)器人路徑規(guī)劃是滑動(dòng)機(jī)器人研究的熱點(diǎn)問題，可描述為在有障礙物的環(huán)境下，尋找一條從給定起點(diǎn)到指定終點(diǎn)的運(yùn)行路徑，在整個(gè)過程中必須安全、無碰撞地繞過所有障礙物，且盡量使所走路徑最短（或者所花費(fèi)時(shí)間最小、成本最少等）。目前，應(yīng)用于機(jī)器人運(yùn)動(dòng)規(guī)劃的方法有Dijkstra算法、A*算法等傳統(tǒng)算法及遺傳算法等智能算法。Dijkstra算法可保證搜索到全局最優(yōu)路徑，但是具有一定的盲目性，當(dāng)節(jié)點(diǎn)增多時(shí)，該算法所搜索到的節(jié)點(diǎn)很多，效率低。遺傳算法在尋找全局最優(yōu)解的過程中，是一個(gè)隨機(jī)搜索的過程，當(dāng)節(jié)點(diǎn)增多時(shí)，搜索時(shí)間較長(zhǎng)，且易出現(xiàn)早熟的現(xiàn)象。A*算法是一種啟發(fā)式搜索算法，搜索具有一定的方向性，實(shí)現(xiàn)簡(jiǎn)單，搜索效率高，已被廣泛應(yīng)用，詳見參考文獻(xiàn)[1-5]。顧辰[6]考慮到現(xiàn)實(shí)機(jī)器人的體積，采用基于優(yōu)先級(jí)的子節(jié)點(diǎn)生成策略得到了機(jī)器人行走時(shí)突遇未知障礙物時(shí)路徑規(guī)劃問題，但是并沒有考慮到轉(zhuǎn)彎角度和方向。李偉光等[7]有合轉(zhuǎn)彎因素，采用刪除邊的方法研究了AGV路徑規(guī)劃問題，但是計(jì)算實(shí)際路徑時(shí)還是用直線段代替。王殿君[8]考慮了旋轉(zhuǎn)方向和旋轉(zhuǎn)角度研究了室內(nèi)機(jī)器人路徑規(guī)劃問題，但是并沒有減少機(jī)器人的路徑長(zhǎng)度及轉(zhuǎn)折角度。王紅衛(wèi)等[9]提出了一種平滑 A*算法，減少了不必要的路徑節(jié)點(diǎn)并減少了路徑長(zhǎng)度和轉(zhuǎn)折角度，但是只是在原有的路徑點(diǎn)上處理，路徑長(zhǎng)度和轉(zhuǎn)折角度減少量有有。孫煒等[10]提出了另一種改進(jìn)的A*算法，進(jìn)一步減少滑動(dòng)機(jī)器人總的路徑長(zhǎng)度和轉(zhuǎn)折角度，但是沒有進(jìn)行平滑處理。本文在前面文獻(xiàn)基礎(chǔ)上，通過修改評(píng)價(jià)函數(shù)，既考慮轉(zhuǎn)彎因素又考慮轉(zhuǎn)折次數(shù)，使得規(guī)劃出的路徑更優(yōu)化、更平滑。

1 系統(tǒng)建模

眾所周知，滑動(dòng)機(jī)器人工作環(huán)境的信息建立是路徑規(guī)劃中十分重要的一步。圖1是智能倉(cāng)庫系統(tǒng)簡(jiǎn)圖（一個(gè)方格代表一個(gè)貨架，空白箭頭處表示機(jī)器人行駛路線）。本文采用棋盤式環(huán)境建模，首先將每一個(gè)貨架視為棋盤的一個(gè)方格，方格大小一致，將不規(guī)則區(qū)域填充為障礙物，使其成為標(biāo)準(zhǔn)的棋盤。然后以貨架、機(jī)器人所在空閑區(qū)域、障礙物為方格，構(gòu)造棋盤如圖2（以11行11列為例）。假設(shè)同一行或同一列的機(jī)器人，若他們之間沒有障礙物，則可按原計(jì)劃執(zhí)行的線路行走。若突遇障礙物，則會(huì)發(fā)生碰撞導(dǎo)致失去原來的行駛路線。于是產(chǎn)生如下問題：（1）突遇障礙物時(shí)如何行駛？（2）如何規(guī)劃一條從起點(diǎn)到終點(diǎn)的最優(yōu)路徑？

圖1 智能倉(cāng)庫簡(jiǎn)圖

圖2 棋盤式地圖模型

假設(shè)如下：

（1）S，T分別為起點(diǎn)和終點(diǎn),i為中間節(jié)點(diǎn)（為障礙物），加粗邊框方格為障礙物所在方格，白色方格為無障礙物方格；

（2）障礙物邊界是在實(shí)際邊界的基礎(chǔ)上加一個(gè)滑動(dòng)機(jī)器人安全距離得到的，即把機(jī)器人看作一個(gè)質(zhì)點(diǎn)；

（3）機(jī)器人滑動(dòng)方向的任意的。

本文的目的是找到一條從起點(diǎn)S到終點(diǎn)T的最優(yōu)無碰撞路徑。

2 突遇障礙物時(shí)行駛路線規(guī)劃

按原計(jì)劃執(zhí)行的線路在i處突遇障礙物，傳統(tǒng)的 A*算法在生成與其相鄰的 8個(gè)子節(jié)點(diǎn)時(shí)并沒有考慮優(yōu)先行走順序，本文考慮優(yōu)先級(jí)的子節(jié)點(diǎn)生成策略，把所有與當(dāng)前節(jié)點(diǎn)i相鄰的水平、豎直、對(duì)角線的8個(gè)節(jié)點(diǎn)按照先水平，后豎直，再對(duì)角線分為三級(jí)（如圖2）。

（1）如A為障礙物節(jié)點(diǎn)，則不生成包含iF，iH的路徑；

（2）如B為障礙物節(jié)點(diǎn)，則不生成包含iE，iG的路徑；

（3）如C為障礙物節(jié)點(diǎn)，則不生成包含iE，iF的路徑；

（4）如D為障礙物節(jié)點(diǎn)，則不生成包含iG，iH的路徑。

在原始計(jì)劃S-O-i-T的路徑過程中，若到i為障礙物，認(rèn)為機(jī)器人當(dāng)前位置i及當(dāng)前朝下的位置A為

未知障礙物，則根據(jù)子節(jié)點(diǎn)優(yōu)先級(jí)，在B處優(yōu)先選擇E，再E到N，依次進(jìn)行，最后到終點(diǎn)T。

3 轉(zhuǎn)彎因素及轉(zhuǎn)折角度計(jì)算

第二部分知道，在i處突遇障礙物，在i的前面B處要迫使機(jī)器人改應(yīng)原來的行駛方向，按子節(jié)點(diǎn)優(yōu)先級(jí)可行駛至 N，但是在N的前方和右方都遇到障礙物，這時(shí)如何行駛？是沿著黑色折線行駛到M還是走弧線到M？如有是沿著弧線行駛，則轉(zhuǎn)彎的角度和旋轉(zhuǎn)方向如何？由于機(jī)器人在轉(zhuǎn)彎時(shí)要減速并調(diào)整姿態(tài)，在實(shí)際運(yùn)行中，轉(zhuǎn)折次數(shù)的多少、轉(zhuǎn)折角度的大小對(duì)規(guī)劃出來的路徑起著至關(guān)重要的作用，為此，引入轉(zhuǎn)彎因子及轉(zhuǎn)折角度等參數(shù)。

在圖2中，令NK、MK與水平線的夾角分別記為α1，α2，MK與 NK的延長(zhǎng)線夾角記為θ，由θ的值來判拓展節(jié)點(diǎn)是否發(fā)生轉(zhuǎn)向。

若θ=π，則不轉(zhuǎn)向，按原計(jì)劃的線路行駛；

若θ≠π，則發(fā)生轉(zhuǎn)向，存在拐點(diǎn)。把拐點(diǎn)及拐點(diǎn)的父節(jié)點(diǎn)提取出來，這樣就得到只剩起點(diǎn)、拐點(diǎn)、終點(diǎn)的曲線，而且旋轉(zhuǎn)的方向及旋轉(zhuǎn)角度大小可以由θ來決定。

設(shè) N、K、M 的坐標(biāo)依次為（x1,y1）, （x2,y2）, （x3,y3）,令θ=α1-α2，則

當(dāng)θ＞0, y3≥y2, y2≥y1時(shí)，右轉(zhuǎn)θ；

當(dāng)θ＞0, y3≤y2, y2≤y1時(shí)，右轉(zhuǎn)θ；

當(dāng)θ＞0, y3≥y2, y2≤y1時(shí)，左轉(zhuǎn)180°-θ；

當(dāng)θ＞0，y3≤y2, y2≥y1時(shí)，左轉(zhuǎn)180°-θ；

當(dāng)θ＜0，y3≥y2, y2≥y1時(shí)，左轉(zhuǎn)-θ；

當(dāng)θ＜0，y3≤y2, y2≤y1時(shí)，左轉(zhuǎn)-θ；

當(dāng)θ＜0，y3≥y2, y2≤y1時(shí)，左轉(zhuǎn)180°＋θ；

當(dāng)θ＜0，y3≤y2, y2≥y1時(shí)，左轉(zhuǎn)180°＋θ。

通過計(jì)算θ，即可判定在行駛過程中有沒有發(fā)生轉(zhuǎn)彎及轉(zhuǎn)彎的角度大小，進(jìn)而可以得到一條從起點(diǎn)到終點(diǎn)的考慮旋轉(zhuǎn)方向和角度的最優(yōu)路徑。

4 啟發(fā)函數(shù)的構(gòu)造

A*算法是一種典型的啟發(fā)式搜索算法，它從起始點(diǎn)開始，根據(jù)估計(jì)代價(jià)選擇價(jià)值最低的作為下一個(gè)拓展節(jié)點(diǎn)，直到將目標(biāo)點(diǎn)拓展進(jìn)來為止，獲得最終最優(yōu)路徑。因此 A*算法的關(guān)鍵在于選擇一個(gè)合適的啟發(fā)函數(shù)。傳統(tǒng)的A*算法的啟發(fā)函數(shù)一般為：

其中，f(i)是當(dāng)前節(jié)點(diǎn)i的估計(jì)函數(shù)，g(i)是起點(diǎn)到當(dāng)前節(jié)點(diǎn) i的實(shí)際費(fèi)用，這個(gè)費(fèi)用是可以算出來的。h(i)是從當(dāng)前節(jié)點(diǎn)到終點(diǎn)最小費(fèi)用的估計(jì)，一般采用歐幾里得距離、曼哈頓距離、切比雪夫距離等進(jìn)行計(jì)算，但是實(shí)際在選擇最優(yōu)路徑時(shí)還受到方向的影響，僅以距離作為約束條件顯示不是最優(yōu)的，為此我們加入方向因素來考慮，現(xiàn)修改啟發(fā)函數(shù)為：

g(i)為修改后的當(dāng)前節(jié)點(diǎn)的實(shí)際費(fèi)用，若θ=π,則沒有轉(zhuǎn)向；若θ≠π，則發(fā)生轉(zhuǎn)向。l,L分別為當(dāng)前節(jié)點(diǎn)和起始節(jié)點(diǎn)到終點(diǎn)的歐幾里得距離，l,L比值越接近 1，說明當(dāng)前接近越接近目標(biāo)點(diǎn)，α分別為當(dāng)前節(jié)點(diǎn)與后續(xù)節(jié)點(diǎn)的矢量和起始節(jié)點(diǎn)跟終點(diǎn)的矢量的夾角，θ為上文中的夾角，的比值越接近1，說明選取的節(jié)點(diǎn)路徑方向越接近終點(diǎn)方向。

5 算法流程

（1）初始化設(shè)置，設(shè)O為已拓展列表和C為待拓展列表；

（2）令S∈O，C=φ，檢查O是否為φ，若為φ，則搜索失敗，否則轉(zhuǎn)（3）；

（3）選O中最小的f(.)節(jié)點(diǎn)i作為拓展，從O中刪除，放入C中，并判斷i是否為目標(biāo)點(diǎn)，若是，則轉(zhuǎn)（5），否則轉(zhuǎn)（4）；

（4）判與i相鄰的節(jié)點(diǎn)j是否在O表中。作如下操作：若在，則判j的當(dāng)前g值是否比原來小，若是，則把i作為j的父節(jié)點(diǎn)，重新計(jì)算j的新的f,g,h；否則，不作任何操作。若不在，則把j添加到O中，并將i作為j的父節(jié)點(diǎn)，計(jì)算j的f,g,h值，轉(zhuǎn)（3）；

（5）從 j開始，用回溯法輸出起點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑。

6 有束語

（1）通過棋盤式的建模方式，構(gòu)建了倉(cāng)儲(chǔ)的環(huán)境模型；

（2）通過障礙物所在節(jié)點(diǎn)的優(yōu)先級(jí)，解決了機(jī)器人行走時(shí)突遇障礙物的行駛路線問題；

（3）通過考慮旋轉(zhuǎn)方向和旋轉(zhuǎn)角度，引入了新的評(píng)價(jià)函數(shù)計(jì)算方法，使規(guī)劃出的路徑更加合理、更加優(yōu)化。