基于A* 算法的機(jī)器人路徑規(guī)劃研究

(沈陽(yáng)理工大學(xué) 遼寧 沈陽(yáng) 110159)

一、引言

路徑規(guī)劃問(wèn)題成為機(jī)器人領(lǐng)域中一個(gè)重要的研究課題。研究者提出了多種算法，如Dijkstra 算法，蟻群算法，模擬退火算法、粒子群算法等。這些算法沒(méi)有將實(shí)際工作中的機(jī)器人與算法緊密的結(jié)合起來(lái)。

二、路徑規(guī)劃問(wèn)題描述

機(jī)器人路徑規(guī)劃問(wèn)題是指在工作區(qū)域中,機(jī)器人通過(guò)已知的環(huán)境信息或者對(duì)周圍環(huán)境進(jìn)行感知,按照預(yù)先設(shè)計(jì)的代價(jià)函數(shù)，通過(guò)一系列的算法，從起點(diǎn)開始找到一條代價(jià)最小的到終點(diǎn)的運(yùn)動(dòng)路徑,該路徑能避開工作空間中的障礙物。

根據(jù)機(jī)器人對(duì)環(huán)境信息的掌握程度可以分為全局規(guī)劃方法,局部規(guī)劃方法以及混合規(guī)劃方法三種。

(1)全局規(guī)劃方法必須知道整個(gè)環(huán)境地圖中障礙物分布的大體情況。該方法可以找到最佳路徑，但是考慮全局信息導(dǎo)致計(jì)算量大，效率低，不適用于未知環(huán)境或動(dòng)態(tài)環(huán)境。

(2)局部規(guī)劃方法中依靠機(jī)器人當(dāng)前得到的局部環(huán)境信息,根據(jù)制定的規(guī)則來(lái)實(shí)現(xiàn)避障導(dǎo)航,該方法可以滿足實(shí)時(shí)性要求，缺點(diǎn)是易陷入局部極值點(diǎn),機(jī)器人可能在中途停止，無(wú)法到達(dá)目標(biāo)點(diǎn)。

(3)混合型方法結(jié)合了全局規(guī)劃和局部規(guī)劃,具有良好的實(shí)用性。

三、基于改進(jìn)的A*算法的路徑規(guī)劃

(一)A*算法原理

A*算法基于啟發(fā)式搜索結(jié)構(gòu)，它是 Dijkstra 算法的擴(kuò)展，通過(guò)評(píng)估擴(kuò)展節(jié)點(diǎn)的代價(jià)值，并比較其大小加以選擇，使得在起點(diǎn)與終點(diǎn)之間尋找到一條代價(jià)最小的路徑。廣度優(yōu)先(BFS)和深度優(yōu)先(DFS)是簡(jiǎn)單的狀態(tài)空間搜索策略。BFS方法的原理是從初始節(jié)點(diǎn)逐層向下查找，將當(dāng)前節(jié)點(diǎn)按照可拓展的策略一層一層展開，直到找到目標(biāo)節(jié)點(diǎn)。該方法能找到最優(yōu)路徑，但節(jié)點(diǎn)展開的數(shù)量是和距離成級(jí)數(shù)增加的，不適用于大規(guī)模復(fù)雜的環(huán)境，效率低。DFS方法的原理是按照一定的順序先查找完一個(gè)分支，再查找另一個(gè)分支，直到找到目標(biāo)節(jié)點(diǎn)。

A*算法與BFS和DFS不同，就在于選擇下一步節(jié)點(diǎn)時(shí)，通過(guò)估值函數(shù)對(duì)節(jié)點(diǎn)進(jìn)行評(píng)估，通過(guò)排序并選擇代價(jià)最小的節(jié)點(diǎn)作為下一步搜索節(jié)點(diǎn)進(jìn)行拓展，如果存在一個(gè)以上代價(jià)最小的節(jié)點(diǎn)，選擇距離當(dāng)前節(jié)點(diǎn)最近的一個(gè)節(jié)點(diǎn)，該方法避免了搜索的盲目性，提高了算法的搜索效率。估值函數(shù)如公式(1)所示。

f(n)=g(n)+h(n)

(1)

其中，f(n)為節(jié)點(diǎn)n的估值函數(shù)，g(n)表示從初始節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)n的實(shí)際代價(jià)，h(n)表示從節(jié)點(diǎn)n行進(jìn)到目標(biāo)節(jié)點(diǎn)的估算代價(jià)。

估價(jià)函數(shù)h(n)的選取決定了是否能找到最優(yōu)路徑，如果估價(jià)值h(n)的小于或等于當(dāng)前節(jié)點(diǎn) n 到目標(biāo)節(jié)點(diǎn)的真實(shí)代價(jià)，將導(dǎo)致搜索范圍大，算法效率低，但能得到最優(yōu)路徑。如果估價(jià)值大于實(shí)際代價(jià)，算法的搜索范圍小，效率高，但不一定能找到最優(yōu)路徑。

A*算法 與廣度優(yōu)先和深度優(yōu)先的聯(lián)系在于，當(dāng)g(n)=0時(shí)，改算法雷石榆DFS，當(dāng)h(n)=0時(shí)，該算法類似于BFS；如果實(shí)際代價(jià)函數(shù) g(n)遠(yuǎn)大于估算代價(jià)函數(shù) h(n)，則啟發(fā)式函數(shù) h(n)沒(méi)有作用，A*算法退化成了 Dijkstra 算法。

本文中，將起始節(jié)點(diǎn)與當(dāng)前節(jié)點(diǎn)之間的距離作為實(shí)際代價(jià)函數(shù)，估計(jì)代價(jià)函數(shù) h(n)的采用曼哈頓方法。估計(jì)代價(jià)函數(shù)如公式(2)所示。

h(n)=|nx-px|+|ny-py|

(2)

其中，nx、ny為當(dāng)前節(jié)點(diǎn)的橫、縱坐標(biāo)，px、py為目標(biāo)節(jié)點(diǎn)橫、縱坐標(biāo)。

(二)改進(jìn)的A*算法

在 A*算法中，若搜索方向?yàn)?8 方向搜索，則路徑中存在的轉(zhuǎn)彎方向角只有 45°。因?yàn)檗D(zhuǎn)向存在時(shí)間，機(jī)器人路徑規(guī)劃整個(gè)系統(tǒng)中的最短路徑不僅僅式路徑的長(zhǎng)度。改進(jìn)的策略是在原有的 A*算法評(píng)價(jià)函數(shù)上加一項(xiàng)t(n)表示轉(zhuǎn)向過(guò)程所需要的時(shí)間，根據(jù)新的評(píng)價(jià)函數(shù)判斷路徑中的單位柵格代價(jià)。改進(jìn) A*算法的評(píng)價(jià)函數(shù)如式(3)所示。

f(n)=g(n)+h(n)+t(n)

(3)

改進(jìn) A*算法的評(píng)價(jià)函數(shù)之后，在原有的評(píng)價(jià)函數(shù)中增加了新的約束條件，即機(jī)器人自身特性所限制的運(yùn)動(dòng)轉(zhuǎn)向時(shí)間。整個(gè)路徑的評(píng)價(jià)因素不再僅是距離的長(zhǎng)短，而是整個(gè)系統(tǒng)運(yùn)行的時(shí)間。

得到規(guī)劃后初始路徑為F，遍歷F內(nèi)的所有節(jié)點(diǎn)，當(dāng)兩個(gè)節(jié)點(diǎn)之間的連線中沒(méi)有障礙物節(jié)點(diǎn)時(shí)，將連線中的節(jié)點(diǎn)設(shè)置為空結(jié)點(diǎn)，最終刪除集合 F內(nèi)的所有空節(jié)點(diǎn)，將非空節(jié)點(diǎn)依次連接得到最終的路徑。

(三)改進(jìn)的A*算法流程

A*算法需要維護(hù)開表和閉表。開表存放拓展節(jié)點(diǎn)的可通行的相鄰節(jié)點(diǎn)，閉表存放已訪問(wèn)過(guò)的節(jié)點(diǎn)。選擇開表中總代價(jià) f 最小的那個(gè)節(jié)點(diǎn)作為拓展節(jié)點(diǎn)，不斷的向周圍拓展。算法的具體流程如下：

(1)初始化開表和閉表，將起始節(jié)點(diǎn)添加到開表中。

(2)將開表中f 最小的節(jié)點(diǎn)作為拓展節(jié)點(diǎn)。

(3)按照如下方式處理該節(jié)點(diǎn)相鄰的8個(gè)方向的每個(gè)節(jié)點(diǎn)。①若該鄰節(jié)點(diǎn)為障礙物，或者在閉表中存在，則不進(jìn)行任何操作。②若開表中沒(méi)有該鄰節(jié)點(diǎn)，就將其加入到開表中，并計(jì)算出該節(jié)點(diǎn)的實(shí)際代價(jià) g 和估計(jì)代價(jià) h。③若開表中已經(jīng)有了該相鄰節(jié)點(diǎn)，則比較當(dāng)前節(jié)點(diǎn)到達(dá)該相鄰節(jié)點(diǎn)的估計(jì)代價(jià) h 與原來(lái)的估計(jì)代價(jià) h 之間的大小。如果當(dāng)前節(jié)點(diǎn)的估計(jì)代價(jià) h 小的話，那么設(shè)置當(dāng)前節(jié)點(diǎn)為該相鄰節(jié)點(diǎn)的父節(jié)點(diǎn)；否則，不進(jìn)行任何操作。

(4)將當(dāng)前節(jié)點(diǎn)從開表中刪除，加入到閉表中。

(5)判斷開表是否為空以及閉表中是否包含有目標(biāo)結(jié)點(diǎn)。如果開表不為空并且閉表中不包含目標(biāo)結(jié)點(diǎn)，則返回到步驟(2)；如果開表不為空且閉表中包含有目標(biāo)結(jié)點(diǎn)，執(zhí)行步驟(6)；如果開表為空，停止搜索。

(6)從目標(biāo)結(jié)點(diǎn)沿著父節(jié)點(diǎn)遍歷到起始節(jié)點(diǎn)，得到初始規(guī)劃路徑F。

(7)遍歷初始路徑F內(nèi)的所有節(jié)點(diǎn)，若某個(gè)兩個(gè)節(jié)點(diǎn)之間沒(méi)有障礙物節(jié)點(diǎn)時(shí)，將中間節(jié)點(diǎn)置為空結(jié)點(diǎn)。

(8)將 F 中的所有空節(jié)點(diǎn)刪除，依次連接剩余節(jié)點(diǎn)，得到最終的路徑 Q。

四、實(shí)驗(yàn)結(jié)果及分析

本文提出的改進(jìn)的 A*算法，對(duì)A*算法的評(píng)價(jià)函數(shù)改進(jìn)，考慮了轉(zhuǎn)向時(shí)間對(duì)整體路徑規(guī)劃的影響，并對(duì)規(guī)劃后的路徑進(jìn)行了平滑處理，與傳統(tǒng) A*算法相比，采用改進(jìn)的 A*算法規(guī)劃出的路徑長(zhǎng)度和路徑的轉(zhuǎn)折角度有所減小，使得規(guī)劃出的路徑更優(yōu)。