于曉天高秀花張 俊鄭冰環(huán)費雯凱(北京航天微系統(tǒng)研究所北京100094)

基于分層柵格地圖的移動機器人路徑規(guī)劃

于曉天，高秀花，張 俊，鄭冰環(huán)，費雯凱
(北京航天微系統(tǒng)研究所，北京100094)

針對傳統(tǒng)A?Star算法與模糊控制算法單獨應(yīng)用于移動機器人路徑規(guī)劃時各自的局限性，提出一種基于分層柵格地圖并將兩種算法融合的移動機器人路徑規(guī)劃新方法。融合后的新算法先利用A?Star算法在高層柵格地圖中整體規(guī)劃出一條概括性路徑，再利用模糊控制算法以概括性路徑中的點為導航點，在底層柵格地圖中進行局部規(guī)劃，從而得出最終的路徑。仿真結(jié)果表明，與傳統(tǒng)的A?Star算法與模糊控制算法相比較，新算法所規(guī)劃路徑距離較短且平滑可行，具有較高的品質(zhì)。

路徑規(guī)劃；A?Star算法；模糊控制算法；分層柵格地圖；融合算法

0 引言

近年來，移動機器人已經(jīng)被應(yīng)用到越來越多的領(lǐng)域當中，各式各樣的移動機器人呈井噴狀發(fā)展。盡管移動機器人的種類、數(shù)目繁多，但其在執(zhí)行任務(wù)時都需要解決一個問題：如何在有障礙物的環(huán)境中從給定起始點無碰地到達目標點。因而，路徑規(guī)劃一直是移動機器人研究方向上的一個熱點與難點。

從20世紀70年代開始，大量的學者投入到移動機器人路徑規(guī)劃的研究中，并且提出了很多優(yōu)秀的路徑規(guī)劃算法［1?5］，如Dijkstra算法、遺傳算法、A?Star算法、人工勢場法、蟻群算法、模糊控制算法等。然而，單一地使用一種路徑規(guī)劃算法進行路徑規(guī)劃往往有較大的局限性。因此，本文針對傳統(tǒng)的A?Star算法規(guī)劃路徑距離最短(前提為路徑存在)、但路徑折線多且邊沿可靠性差，而模糊控制算法規(guī)劃路徑相對較平滑、但路徑距離較長且邊沿可靠性不足的特點，基于分層柵格地圖，將兩種算法進行融合、優(yōu)勢互補，彌補了單一算法的一些缺點與不足，獲得了更為理想的路徑規(guī)劃效果。

1 分層柵格地圖的構(gòu)建

本文所提的融合算法是在分層柵格地圖的基礎(chǔ)上進行路徑規(guī)劃的，因而構(gòu)建分層柵格地圖是整個路徑規(guī)劃的首要工作。設(shè)定全局地圖為一正方形的迷宮地圖，根據(jù)融合算法的需要，全局地圖可分為高層柵格地圖和底層柵格地圖兩層。

首先，將正方形迷宮地圖均勻劃分為40行和40列，總共1600個柵格。每個柵格用深黑色或者白色標記，其中深黑色柵格表示有障礙物，白色柵格表示無障礙物，這樣便構(gòu)成了底層柵格地圖，如圖1所示。然后，在底層柵格地圖的基礎(chǔ)上，將N×N個底層柵格合并組成一個高層柵格，從而完成高層柵格地圖的構(gòu)建。在底層柵格個數(shù)一定的情況下，增大N值，則會降低高層柵格的個數(shù)。本文中N值取4，所構(gòu)建的高層柵格地圖如圖2所示。

2 基于高層柵格地圖的A-Star算法

2.1 A?Star算法基本原理

A?Star算法是一種啟發(fā)式搜索算法(Heuristic Search Algorithm)，通過不斷搜索逼近目的地的路徑來獲得移動機器人的移動路徑。而在啟發(fā)式搜索過程中，最重要的是啟發(fā)式函數(shù)，其表達式為：

其中，f(x)為從初始節(jié)點到目標節(jié)點的最小消耗，g(x)為初始節(jié)點到節(jié)點x的最優(yōu)路徑代價，h(x)為節(jié)點x到目標節(jié)點的估計消耗。在進行路徑規(guī)劃時，從起始點開始，在柵格地圖范圍內(nèi)，算法分別對當前點左上、上、右上、右、右下、下、左下、左8個方向上的非障礙物柵格進行擴展，每次選取拓展點中f(x)值最小的點為下一路徑點，依次循環(huán)，直到找到目標點。

2.2 改進的A?Star算法

傳統(tǒng)A?Star算法在范圍較小的環(huán)境地圖中，可以快捷、高效地找出一條最短路徑。但隨著環(huán)境范圍擴大，柵格數(shù)量增多，將會導致計算量迅速上升?？紤]到算法本身的效率以及下一步算法融合的要求，結(jié)合高層柵格地圖的特點，本文在傳統(tǒng)A?Star算法的基礎(chǔ)上，對原有算法做了相應(yīng)的改進［6］，下文將詳細介紹。

1)算法在搜索路徑過程中，加入了每個高層柵格被障礙物占有的概率，并將其定義為柵格內(nèi)障礙物的面積與柵格面積的比值，用Pi表示。柵格內(nèi)無障礙物時Pi＝0，全部被障礙物填充時Pi＝1，其他情況Pi取值0～1之間。同時，設(shè)定一個閾值Pt，高于這個閾值的柵格視為被障礙物占據(jù)，算法將其自動剔除，而低于閾值的柵格保留下來用于路徑的拓展。本文中，Pt設(shè)定為0.7。另外，對啟發(fā)式函數(shù)中各個參數(shù)的計算方法也做出了相應(yīng)改進，具體如下：

在式(2)、式(3)中，g(xpar)表示從初始節(jié)點移動到當前節(jié)點x的父節(jié)點xpar時所經(jīng)過的實際距離；g(x)中的dn(xpar，x)表示當前節(jié)點x與其父節(jié)點xpar之間的歐幾里得距離；h(x)中的dn(x，xgoal)表示當前節(jié)點x與目標節(jié)點xgoal之間的曼哈頓距離；而cm(x)表示在高層柵格地圖中，x節(jié)點被障礙物占據(jù)的綜合評判概率系數(shù)，其表達式如下：

由式(4)、式(5)可以看出，cm(x)由兩部分組成。一部分為x節(jié)點本身被障礙物占據(jù)的概率pi(x)，另一部分為x點周圍r個節(jié)點被障礙物占據(jù)的算術(shù)平均值cp(x)。其中，ω1、ω2為權(quán)值系數(shù)，本文將其設(shè)定為0.5。

最終，改進后的A?Star算法可利用式(1)～式(5)完成對高層柵格地圖中每個擴展節(jié)點的f(x)值的計算，然后按照A?Star算法本身的搜索規(guī)則，在高層柵格地圖中規(guī)劃出一條概括性的路徑，具體算法流程［7］如圖3所示。

3 基于底層柵格地圖的模糊控制算法

3.1 算法基本原理

模糊控制算法是一種模擬人的大腦對一些模糊事物進行識別和判決的人工智能算法。在路徑規(guī)劃中，模糊邏輯法模擬駕駛員的駕駛思想，將模糊控制本身具有的魯棒性與基于生理學的感知?動作行為結(jié)合起來，顯示出了很大的優(yōu)越性［8］。

本文設(shè)定3個互成銳角的虛擬測量傳感器作為測距系統(tǒng)，用于測量3個方向上各自與機器人距離最近的障礙物的距離，并根據(jù)實際應(yīng)用情況限定了最大測量距離。如圖4所示。記機器人與目標點之間的連線為Ln，圖4中D為位于Ln方向上的障礙物距離機器人的最短距離，DL、DR分別為與Ln呈±α夾角的兩方向上障礙物距離機器人的最短距離。在底層柵格地圖中，測量系統(tǒng)探測的障礙物即地圖中的黑色小柵格。算法開始運行時，機器人從起點出發(fā)，確定所要到達的目標點之后，首先通過3個測量傳感器獲取D、DL、DR的值，記R＝DL－DR，然后以D和R為兩個輸入量輸入到模糊控制器中，再經(jīng)過模糊化、模糊規(guī)則解算、去模糊等一系列處理，最后輸出機器人行走步長的比例因子Sd以及轉(zhuǎn)動角度的比例因子Sθ(逆時針為正方向)，進而更新機器人的位置和姿態(tài)，依次循環(huán)，直至到達目標點。本文中，α取30°且左偏為正。

由于模糊邏輯存在著對稱無法確定現(xiàn)象，因此當機器人面臨當前環(huán)境左右對稱時，無法確定行進方向或在兩個障礙物之間震蕩，陷入死鎖。本文采用預防死鎖機制［9］進行處理。從起始點開始，每次機器人到達新位置之后，先往目標方向進行探測，如果在此方向小范圍內(nèi)行動受阻，則認為機器人進入危險區(qū)域，啟動解鎖算法。之后，機器人立于原地分別轉(zhuǎn)向兩邊探測出口，然后選擇轉(zhuǎn)角比較小的一側(cè)，按照左正右負的規(guī)則標記機器人進入解鎖狀態(tài)。當處于解鎖狀態(tài)下，機器人到達新位置時仍然受阻，則加深此種狀態(tài)，否則減輕，直至解除危險標示。機器人解鎖時具體的探測過程如圖5所示。

圖5中，設(shè)定機器人位于A點，AB1為機器人按輸出轉(zhuǎn)角轉(zhuǎn)動后的前進方向。經(jīng)探測，AB1方向上障礙物距離小于固定步長，則機器人向左(假定)偏轉(zhuǎn)角度δ，方向為AB2，繼續(xù)探測，AB2方向上障礙物距離仍小于固定步長，于是再向左偏轉(zhuǎn)角度δ探測，依次下去，直到找到安全的前進方向ABi，然后機器人前進固定步長到C點，完成本次解鎖，本文中δ取15°。

3.2 模糊控制器設(shè)計

根據(jù)3.1節(jié)中算法的具體要求，設(shè)置D、R兩個變量為模糊控制器的輸入，兩個比例因子Sd、Sθ為輸出。將D均勻量化到 ［0，10］區(qū)間內(nèi)，論域為 ［1，3，5，7，9］。將左右探測器的距離差值R均勻量化到 ［－10，10］區(qū)間內(nèi)，R的論域為［－10，－7，－4，－1，0，1，4，7，10］。將輸出轉(zhuǎn)角比例因子Sθ均勻量化到 ［－2，2］區(qū)間內(nèi)，論域為 ［－1.5，－0.75，0，0.75，1.5］。將輸出步長比例因子Sd均勻量化到 ［0，2］區(qū)間內(nèi)，論域為［0，0.3，0.6，1，1.4，1.5，1.8］。定義各變量的模糊語言為D＝｛VS，S，M，B，VB｝，R＝｛RVB，RB，CE，LB，LVB｝，Sθ＝ ｛TRVB，TRN，Z，TLB，TLVB｝，Sd＝ ｛VS，S，M，B，VB｝，各字母含義為(V：Very，S：Small，M：Middle，B：Big，R：Right，Z：Zero，L：Left，T：Turn)。當位于機器人前進方向左(右)側(cè)的障礙物離機器人近些時，R為正(負)，機器人向左(右)轉(zhuǎn)為正(負)。取各個語言變量的隸屬度函數(shù)形狀為對稱的三角形且模糊分割完全是對稱的，D、R、Sθ及Sd的隸屬度函數(shù)形如圖6所示。

與此同時，根據(jù)人類駕駛汽車的相關(guān)經(jīng)驗設(shè)計了相應(yīng)的模糊規(guī)則庫，以供推理使用。設(shè)定模糊控制器的推理方法采用Mamdani推理法，清晰化方法采用加權(quán)平均法，至此，模糊控制器設(shè)計完成。最終，每當有一組D與R的值輸入，就可以通過模糊控制器處理并求得一組清晰值Sd、Sθ，再經(jīng)尺度變換即可輸出實際的控制量。

4 算法融合

基于分層柵格地圖進行路徑規(guī)劃時，整體思想為：運用改進的A?Star算法在高層柵格地圖中規(guī)劃出一條概括性路徑，然后運用模糊控制算法，以概括性路徑中的節(jié)點為局部導航目標點，從起始點開始，在相鄰兩個導航目標點之間的地圖中進行局部路徑規(guī)劃，逐次到達每個導航目標點，最終形成一條從起始點到最終目標點，平滑、可行且距離較短的路徑。

然而，由于在高層柵格地圖中規(guī)劃的概括性路徑是一種基于概率性質(zhì)的規(guī)劃，而且改進的A?Star算法是沿著柵格邊緣進行路徑規(guī)劃的，因此概括性路徑給出的路徑點有可能是某個障礙物柵格的頂點，如圖7所示。A點為概括性路徑中的一個節(jié)點，B點為概括性路徑中與A點相鄰的下一個節(jié)點。此時若在底層柵格地圖中調(diào)用模糊控制算法到達A點后，B點即為下一個導航目標點，但從圖中顯然可以看出，到達A點后，機器人已經(jīng)進入了死區(qū)，這將導致路徑規(guī)劃失敗。為了避免此種情況的發(fā)生，在基于底層柵格地圖的路徑規(guī)劃中，對原有模糊控制算法做出以下3點改進：

1)基于高層柵格地圖進行路徑規(guī)劃得出概括性路徑后，在底層柵格地圖中對概括性路徑中的節(jié)點進行篩查，對包含在障礙物中的節(jié)點進行標記，即置Flag＝1。

2)每次給出機器人下一步的轉(zhuǎn)角θ及步長d后，機器人并不立即前進，而是按照給出的轉(zhuǎn)角θ先進行旋轉(zhuǎn)，然后在新的方向上測量與機器人距離最短的障礙物的距離dm。若dm＞d，則機器人按照步長d正常前進，否則，仿照算法預防死鎖機制中的探測過程，按照一定的方向(如向右)連續(xù)偏轉(zhuǎn)角度δ，直到找到安全的前進方向，繼續(xù)行進，實現(xiàn)二次轉(zhuǎn)動避障。

3)記機器人與局部目標點之間的距離為drg。當機器人到達一個新的位置后，若drg＜d且Flag＝1，則按照步驟2中的二次轉(zhuǎn)動避障方法尋找出安全前進方向并前進步長d。此時，認定機器人已經(jīng)到達該局部目標點，然后獲取下一局部目標點進行下一輪局部路徑規(guī)劃。

基于雙層柵格地圖的融合算法整體流程如圖8所示。

5 算法仿真

在Matlab r2014a的開發(fā)環(huán)境中，利用前文構(gòu)建的40×40的正方形迷宮地圖，分別對A?Star算法、模糊控制算法和本文所提出的融合算法進行仿真。仿真過程中，所有地圖的左上角為機器人的起始點，右下角為機器人的目標點，算法中的其他參數(shù)如前文中所述，仿真結(jié)果如圖9所示。

仿真結(jié)果表明，融合算法在高層柵格地圖中進行路徑規(guī)劃時，柵格的增大以及基于柵格障礙物占有率這一參數(shù)對A?Star算法啟發(fā)式函數(shù)的改進，使得所規(guī)劃路徑較傳統(tǒng)A?Star算法在距離上有所增大，但這樣卻避開了障礙物密集的區(qū)域，避免碰到局部 “陷阱”的情況，優(yōu)化了路徑規(guī)劃的時間。而且從所規(guī)劃路徑的長度、平滑性和邊緣可靠性3方面進行綜合評判，融合算法明顯優(yōu)于單獨的A?Star算法和模糊控制算法。

6 結(jié)論

針對傳統(tǒng)A?Star算法與模糊控制算法各自的特點，提出了一種基于分層柵格地圖并將兩種算法融合的移動機器人路徑規(guī)劃新方法。傳統(tǒng)A?Star算法規(guī)劃路徑距離最短(前提為路徑存在)，但路徑都為折線且常沿障礙物邊緣前進，不一定可行；而模糊控制算法規(guī)劃路徑相對較平滑，但路徑距離較長，且邊沿可靠性不足。融合后的新算法利用A?Star算法在高層柵格地圖中整體規(guī)劃出一條概括性路徑，再利用模糊控制算法以概括性路徑中的點為導航點，在底層柵格地圖中進行局部規(guī)劃，從而得出最終的路徑。仿真結(jié)果表明，與傳統(tǒng)的A?Star算法與模糊控制算法相比較，基于分層柵格地圖，融合后的新算法規(guī)劃路徑距離較短且平滑可行，具有更高的品質(zhì)。

[1] Dijkstra E W.A note on two problems in connection with graphs[J].Numerische Mathematik，1959，1(1):269?271. [2] Agirrebeitia J，Aviles R.A new APF strategy for path planning in environments with obstacles[J].Mechanism and machine theory，2005，40(6):645?658.

[3] Mohanta J C，Parhi D R，Patel S K.Path planning strategy for autonomous mobile robot navigation using Petri?GA optimization[J].Computers and Electrical Engi?neering，2011，37(6):1058?1070.

[4] 曾明如，徐小勇，劉亮，等.改進的勢場蟻群算法的移動機器人路徑規(guī)劃[J].計算機應(yīng)用工程，2015，51 (22):33?37. ZENG Ming?ru，XU Xiao?yong，LIU Liang，etal. Improved ant colony optimization with potential field heu?ristic for robot path planning[J].Computer Engineering and Applications，2015，51(22):33?37.

[5] Perez D A.Fuzzy logic based speed planning for autono?mous navigation under velocity field control[C].IEEE In?tetnational Conference on Mechatronics，2009:14?17.

[6] 秦玉鑫，王紅旗，杜翠杰.基于雙層A?算法的移動機器人路徑規(guī)劃[J].制造業(yè)自動化，2014，36(12): 21?25. QIN Yu?xin，WANG Hong?qi，DU Cui?jie.A path planning approach to moving robot based on double layer A?algorithm[J].Manufacturing Automation，2014，36 (12):21?25.

[7] 時也.基于A?Star算法與模糊控制融合的移動機器人路徑規(guī)劃[D].武漢科技大學，2012. SHI Ye.Mobile robot path planning based on fusion of A?Star algorithm and fuzzy control[D].Wuhan University of Science and Technology，2012.

[8] Xu W L，Tso S K.Sensor?based fuzzy reactive navigation of a mobile robot through local target swithching[J].IEEE Transactions on Systems，Man and Cybemetics，1999，29 (3):451?459.

[9] 李鵬，溫素芳.基于模糊控制的路徑規(guī)劃算法的實現(xiàn)[J].杭州電子科技大學學報，2007，27(6):82?86. LI Peng，WEN Su?fang.Realization of the path planning algorithm based on fuzzy control[J].Journal of Hangzhou Dianzi Univercity，2007，27(6):82?86.

Path Planning of Mobile Robot Based on Hierarchical Grid Map

YU Xiao?tian，GAO Xiu?hua，ZHANG Jun，ZHENG Bing?huan，F(xiàn)EI Wen?kai
(Beijing Aerospace Institute of Microsystems，Beijing 100094)

According to the limitation of the traditional A?Star algorithm and the fuzzy control algorithm applied to the path planning of mobile robot，this paper presents a new method based on the hierarchical grid map and the fusion of these two kinds of algorithm.The fusion algorithm plans a general path in the top grid map by using the A?Star algorithm.Then based on the navigation point in the general path，the fusion algorithm concludes the final path in the bottom gird map by u?sing the fuzzy control algorithm.The simulation results show the path planned by fusion algorithm has more excellent char?acter than the path planned by the other two algorithms.

path planning；A?Star algorithm；fuzzy control algorithm；hierarchical map；fusion algorithm

TP24

A

1674?5558(2017)01?01291

10.3969／j.issn.1674?5558.2017.02.006

于曉天，男，碩士，研究方向為機電控制。

2016?07?07