改進(jìn)A星算法移動(dòng)機(jī)器人路徑規(guī)劃

宋 宇， 王志明

(長(zhǎng)春工業(yè)大學(xué) 計(jì)算機(jī)科學(xué)與工程學(xué)院， 吉林 長(zhǎng)春 130012)

0 引 言

路徑規(guī)劃問(wèn)題在很多領(lǐng)域都具有廣泛的應(yīng)用，如機(jī)器人的自主無(wú)碰撞運(yùn)行、無(wú)人機(jī)的避障突防飛行等?，F(xiàn)有的路徑規(guī)劃方法包括蟻群算法、A星算法、D星算法、人工勢(shì)場(chǎng)算法、模糊邏輯算法、神經(jīng)網(wǎng)絡(luò)算法、強(qiáng)化學(xué)習(xí)算法等。其中，A星算法作為一種經(jīng)典路徑規(guī)劃算法，被廣泛應(yīng)用于路徑規(guī)劃問(wèn)題中，但由于在柵格環(huán)境下A星算法只選擇周?chē)鷸鸥竦闹行狞c(diǎn)加入open表中，導(dǎo)致路徑點(diǎn)的選擇局限于柵格中心點(diǎn)，路徑轉(zhuǎn)折角度固定為一些特定的離散值，從而產(chǎn)生了無(wú)效冗余轉(zhuǎn)折或找到的最終路徑不是最優(yōu)。文獻(xiàn)[1]提出了擴(kuò)展Moore型鄰居結(jié)構(gòu)A星算法；文獻(xiàn)[2]提出了結(jié)合跳點(diǎn)算法的A星算法；文獻(xiàn)[3]采用了二叉堆優(yōu)化查找open表中F值的方法，加快了A星算法的執(zhí)行速度；文獻(xiàn)[4]提出了去除冗余點(diǎn)的A星算法；文獻(xiàn)[5]采用A星算法初始化信息素，用蟻群算法進(jìn)行了機(jī)器人路徑規(guī)劃；文獻(xiàn)[6]用蟻群算法對(duì)Hopfield神經(jīng)網(wǎng)絡(luò)的參數(shù)進(jìn)行了優(yōu)化；文獻(xiàn)[7-8]采用了優(yōu)化算法進(jìn)行路徑規(guī)劃。

文中通過(guò)建立映射矩陣的方式提出了一種任意角度的A星算法，仿真實(shí)驗(yàn)表明，在柵格劃分粗糙的情況下，改進(jìn)算法效果顯著。

1 相關(guān)算法

1.1 A星算法

A星算法是在Dijstar算法的基礎(chǔ)上引入了啟發(fā)函數(shù)，加快了算法收斂速度。A星算法的估價(jià)函數(shù)為

f(x,y)=g(x,y)+h(x,y)

(1)

其中,g(x,y)為起始點(diǎn)到當(dāng)前考察點(diǎn)X(x,y)的實(shí)際距離值，h(x,y)為X(x,y)點(diǎn)到目標(biāo)點(diǎn)的估計(jì)距離值，距離值函數(shù)h(x,y)一般取歐幾里德距離(如式(2))或絕對(duì)值距離(如式(3))，gx、gy為目標(biāo)點(diǎn)的x坐標(biāo)與y坐標(biāo)。

(2)

h(x,y)=|x-gx|+|y-gy|

(3)

A星算法不斷將當(dāng)前位置周?chē)?個(gè)柵格的中心點(diǎn)加入open表，并不斷取open列表中F值最小的點(diǎn)作為當(dāng)前位置，直到當(dāng)前位置的臨近點(diǎn)出現(xiàn)目標(biāo)點(diǎn)或open表為空為止，A星算法的流程如下：

1)初始化open表、opencost表、close表為空，將起點(diǎn)坐標(biāo)加入open表，將起點(diǎn)的cost值0加入opencost表。

2)將open表中具有最小F值的點(diǎn)的坐標(biāo)作為當(dāng)前點(diǎn)，同時(shí)從open表與opencost表中刪除這個(gè)點(diǎn)的信息，并把當(dāng)前點(diǎn)的坐標(biāo)加入close表。

3)依次計(jì)算當(dāng)前點(diǎn)的鄰居?xùn)鸥裰行牡?個(gè)點(diǎn)的g值與f值，若這個(gè)點(diǎn)在close表中，則跳過(guò)這個(gè)點(diǎn)；若這個(gè)點(diǎn)既不在open表中，也不在close表中，則將這個(gè)點(diǎn)的坐標(biāo)加入open表，同時(shí)將這個(gè)點(diǎn)的g值加入opencost表，并將這個(gè)點(diǎn)的箭頭指向當(dāng)前點(diǎn)。若這個(gè)點(diǎn)已經(jīng)在open表中，則比較這個(gè)點(diǎn)之前的g值與通過(guò)當(dāng)前點(diǎn)計(jì)算得到的g值的大小，若前者大，則修改這個(gè)點(diǎn)的g值為當(dāng)前計(jì)算得到的g值，并將這個(gè)點(diǎn)的箭頭方向指向當(dāng)前點(diǎn)。

4)跳到2)，直到open表為空，或者open表中出現(xiàn)目標(biāo)點(diǎn)為止。

1.2 粒子群算法

粒子群算法是一種模擬大自然群體尋優(yōu)的優(yōu)化算法，通過(guò)共享群體的適應(yīng)度值大小來(lái)加速極值尋優(yōu)過(guò)程。設(shè)D維粒子位置為xi=(xi1,xi2,…,xiD),粒子速度為vi=(vi1,vi2,…,viD)，種群中所有個(gè)體粒子目前為止適應(yīng)度最優(yōu)時(shí)的位置為p=(p1,p2,…,pD),到第k代為止種群中所有粒子的全局最優(yōu)適應(yīng)度值對(duì)應(yīng)的粒子位置為pbk。則在每次迭代中粒子的速度更新公式為

(4)

位置更新公式為

(5)

通過(guò)不斷迭代，最終算法保存了最優(yōu)位置，即最優(yōu)解。

2 改進(jìn)A星算法

2.1 粒子群搜索

使用粒子群算法搜索當(dāng)前柵格的鄰居?xùn)鸥駜?nèi)的最優(yōu)粒子位置，如圖1所示。

圖1 搜索坐標(biāo)邊界

最中間的格子代表當(dāng)前點(diǎn)所在的格子，首先在當(dāng)前點(diǎn)所在格子的8個(gè)鄰居格子內(nèi)的線段上搜索到8個(gè)最優(yōu)點(diǎn)，目標(biāo)函數(shù)為F值(當(dāng)前柵格的g值與當(dāng)前柵格內(nèi)的實(shí)際點(diǎn)到搜索柵格內(nèi)搜索點(diǎn)的距離與搜索點(diǎn)到目標(biāo)點(diǎn)的距離的和)，即搜索每個(gè)格子內(nèi)直線上的F值最小的點(diǎn)，使用粒子群算法搜索時(shí)，粒子位置搜索范圍限制在每個(gè)格子內(nèi)的線段上。例如，粒子群算法在每個(gè)柵格內(nèi)的給定范圍搜索后得到的點(diǎn)如圖2所示(黑色實(shí)心點(diǎn))。

圖2 搜索得到的點(diǎn)

將搜索到的8個(gè)最優(yōu)位置存入M矩陣中。同時(shí)將搜索到的點(diǎn)的g值與f值存入cost與F數(shù)組中。

2.2 鄰居格子屬于open表

如果鄰居?xùn)鸥裥蛱?hào)已經(jīng)在open表中，目標(biāo)函數(shù)為從當(dāng)前柵格的M矩陣(目前所有柵格內(nèi)的點(diǎn)的最優(yōu)位置組成的矩陣)存儲(chǔ)的當(dāng)前柵格內(nèi)的最優(yōu)點(diǎn)到搜索柵格內(nèi)點(diǎn)的距離與當(dāng)前點(diǎn)的g值的和，上下左右柵格的搜索范圍為柵格內(nèi)黑色線段，如圖3所示。

圖3 搜索坐標(biāo)邊界

4個(gè)對(duì)角柵格的搜索范圍為整個(gè)柵格之內(nèi)(即若鄰居?xùn)鸥裥蛱?hào)不在open表中，粒子群算法的搜索范圍見(jiàn)圖1，若柵格序號(hào)在open表中，粒子群算法的搜索范圍見(jiàn)圖3)。

比較被搜索柵格之前保存的g值與此次搜索到的點(diǎn)的目標(biāo)函數(shù)值，若前者大，則將此鄰居?xùn)鸥竦募^指向當(dāng)前柵格，并更新鄰居?xùn)鸥竦膅值與鄰居?xùn)鸥駜?nèi)M矩陣?yán)锎鎯?chǔ)的坐標(biāo)值。

2.3 設(shè)置安全距離

為了避免規(guī)劃出來(lái)的路徑穿過(guò)障礙物，設(shè)置粒子的坐標(biāo)距離最近障礙物中心坐標(biāo)的閾值為d，若粒子(搜索過(guò)程中點(diǎn)的坐標(biāo))的坐標(biāo)與所有障礙物中距離最近障礙物的中心點(diǎn)的坐標(biāo)距離小于d，則將粒子群算法的適應(yīng)度函數(shù)值額外增加100/distance，distance為機(jī)器人到障礙物中心的實(shí)際距離。若粒子的坐標(biāo)距離最近的障礙物距離大于閾值d，則不增加。

2.4 改進(jìn)算法流程

1)以起點(diǎn)為當(dāng)前點(diǎn)，以2.1所述方法搜索起點(diǎn)的鄰居?xùn)鸥駜?nèi)的8個(gè)最優(yōu)位置，以及這8個(gè)位置的g值(即從起點(diǎn)到這8個(gè)位置的距離)與f值。將起點(diǎn)序號(hào)加入close表，將8個(gè)鄰居?xùn)鸥竦男蛱?hào)加入open表。將搜索到的8個(gè)柵格內(nèi)對(duì)應(yīng)的8個(gè)最優(yōu)位置存入矩陣M中。將搜索得到的8個(gè)g值與f值存入cost數(shù)組與F數(shù)組中。

2)從F數(shù)組中選擇f值最小的柵格為當(dāng)前柵格，同時(shí)將此柵格序號(hào)從open表移入close表。

3)以當(dāng)前柵格為中心，依次考察當(dāng)前柵格的8個(gè)鄰居?xùn)鸥?。若鄰居?xùn)鸥裥蛱?hào)在close表中，則跳過(guò)此柵格；若鄰居?xùn)鸥裨趏pen表中，則以2.2所述方法更新鄰居?xùn)鸥竦膅值和f值，以及最優(yōu)點(diǎn)位置；若鄰居?xùn)鸥窦炔辉趏pen表中，也不在close表中，則以2.1所述方法更新g、f值與最優(yōu)位置,同時(shí)將該柵格序號(hào)加入open表。

4)若終點(diǎn)不在open表中，跳到2)，若終點(diǎn)在open表中，則算法結(jié)束。

3 仿 真

文中將粒子群算法迭代次數(shù)設(shè)置為10次，種群個(gè)數(shù)設(shè)置為10個(gè)。粒子群算法的速度最大、最小值不設(shè)限，粒子群搜索算法中的最大速度設(shè)置值為0.03，最小速度值為-0.03，閾值距離d設(shè)置為1.6。粒子群算法全局最優(yōu)項(xiàng)前面的系數(shù)為2，局部最優(yōu)項(xiàng)前面的系數(shù)為1.5。在Matlab2014a環(huán)境下進(jìn)行了仿真。文中將平均轉(zhuǎn)折角度定義為總的轉(zhuǎn)折角度(每次轉(zhuǎn)折時(shí)角度改變量的和)除以轉(zhuǎn)折點(diǎn)的個(gè)數(shù)。由傳統(tǒng)A星算法得到的路徑和改進(jìn)A星算法得到的路徑分別如圖4和圖5所示。

圖4 A星算法路徑

圖5 改進(jìn)算法路徑

由圖4和圖5可知，改進(jìn)算法找到的最優(yōu)路徑點(diǎn)較傳統(tǒng)A星算法找到的路徑點(diǎn)準(zhǔn)確。

算法長(zhǎng)度與轉(zhuǎn)折角對(duì)比見(jiàn)表1。

表1 算法長(zhǎng)度與轉(zhuǎn)折角對(duì)比

障礙物較多情況下傳統(tǒng)A星算法規(guī)劃得到的路徑和障礙物較多情況下改進(jìn)算法規(guī)劃得到的路徑分別如圖6和圖7所示。

圖6 障礙物較多情況下A星算法路徑

圖7 障礙物較多情況下改進(jìn)算法路徑

障礙物較多情況下算法長(zhǎng)度與轉(zhuǎn)折角對(duì)比見(jiàn)表2。

表2 障礙物較多情況下算法長(zhǎng)度與轉(zhuǎn)折角對(duì)比

由表1和表2可以看出，由文中算法得出的路徑長(zhǎng)度較短且路徑較平滑，更符合實(shí)際機(jī)器人的運(yùn)動(dòng)規(guī)律。另一方面，從圖6和圖7也可以看出，由于文中算法加入了對(duì)安全距離的考慮，得出的路徑盡量遠(yuǎn)離障礙物，而不是緊貼障礙物的尖點(diǎn)，故文中算法得出的路徑較安全。

4 結(jié) 語(yǔ)

通過(guò)設(shè)置柵格內(nèi)搜索到的點(diǎn)與柵格中心點(diǎn)的對(duì)應(yīng)關(guān)系以及搜索方式得到了柵格內(nèi)的最優(yōu)點(diǎn)，而不是直接選擇中心點(diǎn)作為備選點(diǎn)，通過(guò)設(shè)置安全距離使得生成的路徑不經(jīng)過(guò)障礙物。仿真結(jié)果表明，改進(jìn)算法得出的路徑長(zhǎng)度較短且路徑較平滑。