改進(jìn)動態(tài)規(guī)劃算法的移動機(jī)器人路徑規(guī)劃

周睿慜，李 輝

1.北京科技大學(xué) 自動化學(xué)院，北京 100083

2.北京科技大學(xué) 高級工程師學(xué)院，北京 100083

1 引言

隨著移動機(jī)器人在交通導(dǎo)航、移動倉庫、游戲設(shè)計(jì)、特種作業(yè)等領(lǐng)域應(yīng)用的快速增長，移動機(jī)器人如何選擇行走路徑，避免碰撞障礙物，快速到達(dá)指定目標(biāo)的路徑規(guī)劃問題成為移動機(jī)器人研究熱點(diǎn)之一。傳統(tǒng)尋優(yōu)算法在移動機(jī)器人路徑規(guī)劃求解中取得了較好的時間效率和路徑結(jié)果，如人工勢場法[1]、電勢場法[2]、A*算法[3]、D*算法[4]等。智能優(yōu)化算法在這個領(lǐng)域的應(yīng)用研究也較為廣泛，如粒子群算法[5]、遺傳算法[6]、蟻群算法[7-8]等，并通過不斷地融合改進(jìn)算法，求解結(jié)果越來越好[9-11]。這類算法依舊存在局部搜索能力不足，運(yùn)行時間長，參數(shù)設(shè)置依賴性高、微調(diào)環(huán)境穩(wěn)定性差的問題。動態(tài)規(guī)劃方法是解決多級決策問題的有效方法，在路徑規(guī)劃方面也取得較好的應(yīng)用成果。文獻(xiàn)[12]依據(jù)多機(jī)器人系統(tǒng)的動態(tài)特征，將圖論知識與動態(tài)規(guī)劃思想結(jié)合解決多機(jī)器人路徑規(guī)劃。文獻(xiàn)[13]將幾何逼近算法與動態(tài)規(guī)劃方法結(jié)合求解移動機(jī)器人路徑規(guī)劃。文獻(xiàn)[14]建立自適應(yīng)動態(tài)規(guī)劃方法實(shí)現(xiàn)輪式機(jī)器人非完整運(yùn)動規(guī)劃的最優(yōu)控制。文獻(xiàn)[15]將動態(tài)規(guī)劃方法和電勢理論結(jié)合規(guī)劃無人機(jī)三維航跡。由于移動機(jī)器人避障環(huán)境復(fù)雜，動態(tài)規(guī)劃方法需要與機(jī)器人運(yùn)動路徑特點(diǎn)和障礙物幾何特征相結(jié)合運(yùn)用。本文在格柵環(huán)境中分析移動機(jī)器人下降路徑搜索過程和上升路徑搜索過程的移動方向變化特點(diǎn)，分別建立兩個搜索過程的動態(tài)規(guī)劃模型，并提出了一種將兩者結(jié)合使用的改進(jìn)動態(tài)規(guī)劃算法。該算法能使移動機(jī)器人避開障礙，求得到達(dá)目標(biāo)的最優(yōu)路徑。路徑規(guī)劃仿真實(shí)驗(yàn)驗(yàn)證了改進(jìn)算法具有計(jì)算量小，運(yùn)行速度快的特點(diǎn)，并可以同時完成多條路徑規(guī)劃任務(wù)。

2 環(huán)境模型的建立

移動機(jī)器人路徑規(guī)劃是指機(jī)器人從現(xiàn)有位置出發(fā)自主規(guī)避環(huán)境中障礙物，到達(dá)指定目標(biāo)的最短或次短移動路徑。本文環(huán)境假設(shè)為不考慮障礙物的高度，且障礙物為靜態(tài)的二維有限空間，利用柵格法建立環(huán)境模型。首先以移動機(jī)器人的起始點(diǎn)和目標(biāo)點(diǎn)為對角線畫出矩形區(qū)域，作為移動機(jī)器人執(zhí)行任務(wù)區(qū)域。然后依據(jù)機(jī)器人半徑尺寸膨脹障礙物，對任務(wù)區(qū)域進(jìn)行均勻矩形網(wǎng)格劃分，每個矩形小格記為長寬均為1個柵格單位的矩形柵格。障礙物覆蓋區(qū)域柵格用黑色填充，代表不可通行。對于不規(guī)則障礙物的未完全覆蓋柵格也全部標(biāo)黑，不可通行。未覆蓋柵格用白色表示，代表可以通行。圖1 給出25×30 的柵格地圖。移動機(jī)器人看作質(zhì)點(diǎn)，位于地圖左上角的起點(diǎn)，目標(biāo)點(diǎn)位于地圖右下角。其他情況可以通過圖形的旋轉(zhuǎn)實(shí)現(xiàn)柵格地圖。

圖1 25×30柵格地圖

3 改進(jìn)動態(tài)規(guī)劃算法

3.1 下降路徑搜索動態(tài)規(guī)劃模型

選取S2中每個柵格為一個階段，柵格位置為狀態(tài)變量，移動機(jī)器人從當(dāng)前柵格位置向目標(biāo)柵格位置靠近過程中移動方向可選為右方、下方和斜下方，如圖2所示，新位置比原位置更接近目標(biāo)，這個過程稱為下降搜索過程。向右方和正下方移動時移動距離增加1柵格單位長度，向斜下方移動時移動距離增加 2 柵格單位長度。這表明每個階段向目標(biāo)靠近的決策路徑最多有三種，也表明下降路徑搜索過程是逐步靠近目標(biāo)的移動過程。

圖2 機(jī)器人下降路徑的限定移動方向

設(shè)指標(biāo)函數(shù)f(mi,j)(1 ≤i≤N,1 ≤j≤P)為下降路徑搜索動過程移動機(jī)器人從m1,1移動到mi,j的最短路徑長度，其取值由f(mi-1,j-1)、f(mi-1,j)、f(mi,j-1)以及移動路徑長度決定。具體定義為：

（1）當(dāng)mi,j∈S2，mi-1,j和mi,j-1至多一個屬于S1(1 ＜i≤N,1 ＜j≤P)時，令f(mi,j)由式（1）求得函數(shù)值，此時存在可移動方向到狀態(tài)mi,j，具體情況如圖3所示。

圖3 下降路徑機(jī)器人可移動到mi,j 位置的方向

（2）當(dāng)mi-1,1∈S1，mi,1∈S2(1 ＜i≤N)或mi-1,j∈S1，mi,j-1∈S1(1 ＜i≤N,1 ≤j≤P) 或m1,j-1∈S1，m1,j∈S2(1 ＜j≤P)時，令f(mi,j)等于下降路徑搜索不可到達(dá)標(biāo)記值C2（足夠大的常數(shù)值），表示向目標(biāo)靠近的下降路徑搜索過程中沒有路徑到達(dá)狀態(tài)mi,j，具體情況如圖4所示。

圖4 機(jī)器人無可移動到mi,j 位置路徑

為了算法判別方便，對S1中障礙塊的指標(biāo)值函數(shù)值定義為足夠大的常數(shù)值C1表示，即當(dāng)mi,j∈S1(1 ≤i≤N,1 ≤j≤P)時，令f(mi,j)等于障礙值C1。

3.2 上升路徑搜索動態(tài)規(guī)劃模型

將柵格地圖通過下降路徑搜索動態(tài)規(guī)劃求得移動機(jī)器人到達(dá)目標(biāo)的最優(yōu)路徑規(guī)劃，見圖5（a），稱為單一路徑。柵格環(huán)境需要在下降路徑動態(tài)規(guī)劃中加入上升路徑搜索過程才能避開障礙求得移動機(jī)器人到達(dá)目標(biāo)的最優(yōu)路徑規(guī)劃，見圖5（b），稱為混合路徑。上升路徑搜索使移動機(jī)器人遠(yuǎn)離目標(biāo)，但可以避開障礙，繼續(xù)完成下降路徑搜索動態(tài)規(guī)劃。接下來，為獲得最小上升路徑建立上升路徑搜索動態(tài)規(guī)劃模型。

圖5 運(yùn)動路徑劃分

選取S2中每個柵格為一個階段，柵格位置為狀態(tài)變量，此時每個柵格已求得下降路徑搜索動態(tài)規(guī)劃的指標(biāo)函數(shù)值。設(shè)指標(biāo)函數(shù)g(mi,j)(1 ≤i≤N,1 ≤j≤P)為上升路徑搜索動過程移動機(jī)器人從m1,1移動到mi,j的最短路徑長度，其取值由g(mi+1,j-1)、g(mi+1,j)、f(mi,j)以及移動路徑長度決定。具體定義為：

此時可通過上升的可移動方向到達(dá)狀態(tài)mi,j，具體情況如圖6所示。

圖6 上升路徑機(jī)器人可移動到mi,j 位置的方向

3.3 改進(jìn)動態(tài)規(guī)劃算法構(gòu)建

為獲得混合路徑移動機(jī)器人的最優(yōu)規(guī)劃路徑，需要將兩個基本動態(tài)規(guī)劃模型交互使用，形成改進(jìn)動態(tài)規(guī)劃算法。

改進(jìn)動態(tài)規(guī)劃算法基本思想是利用下降路徑搜索動態(tài)規(guī)劃模型逐列求解最優(yōu)路徑值，當(dāng)求得某一列元素的最優(yōu)路徑值均為障礙值C1或下降路徑搜索不可到達(dá)標(biāo)記值C2時，表明通過下降路徑搜索不能到達(dá)目標(biāo)，此時需要引入上升路徑搜索動態(tài)規(guī)劃模型求解，避開障礙。避障后可繼續(xù)進(jìn)行下降路徑搜索動態(tài)規(guī)劃求解。因此，改進(jìn)動態(tài)規(guī)劃算法是按下降路徑搜索動態(tài)規(guī)劃每列元素的最優(yōu)路徑值進(jìn)行檢測、依據(jù)障礙塊分布情況動態(tài)交互使用兩個基本動態(tài)規(guī)劃模型，最終求得移動過機(jī)器人到達(dá)目標(biāo)的最短路徑長度。

改進(jìn)動態(tài)規(guī)劃算法描述如下：

步驟1賦初值。列值j=1，階段劃分列值k=1，矩陣MN,P存放各階段的最優(yōu)路徑值，障礙值C1，下降路徑搜索不可到達(dá)標(biāo)記值C2。

步驟2利用下降路徑搜索動態(tài)規(guī)劃求解j列各元素的最優(yōu)路徑值M(i,j)(i=1,2,…,N)。

步驟3判別j列的M(i,j)值是否均為障礙值C1或下降路徑搜索不可到達(dá)標(biāo)記值C2。若是轉(zhuǎn)到步驟5，否則繼續(xù)。

步驟4若j=P，輸出M(N,P)最優(yōu)路徑值，結(jié)束算法，否則j=j+1，轉(zhuǎn)到步驟2。

步驟5從k列到j(luò)列利用上升路徑搜索動態(tài)規(guī)劃求解各列元素的最優(yōu)路徑值M(i,j)(i=N-1,…,2,1) ，k=j+1，j=j+1，轉(zhuǎn)到步驟2。

改進(jìn)動態(tài)規(guī)劃算法流程圖見圖7。算法占用存儲空間與柵格地圖的維度相同，上升路徑搜索動態(tài)規(guī)劃的最優(yōu)路徑值替換原位置存儲值，不增加新的存儲空間，尤其適合二維柵格地圖的路徑規(guī)劃問題。兩種基礎(chǔ)動態(tài)規(guī)劃算法的決策方向選取不超過三個，算法執(zhí)行的時間效率較好。

圖7 改進(jìn)動態(tài)規(guī)劃算法流程圖

4 仿真實(shí)驗(yàn)結(jié)果與分析

為驗(yàn)證改進(jìn)動態(tài)規(guī)劃算法的快速有效性，使用PC機(jī)平臺的MATLAB軟件進(jìn)行編程實(shí)現(xiàn)，完成仿真實(shí)驗(yàn)，共進(jìn)行三組仿真對比實(shí)驗(yàn)。

實(shí)驗(yàn)1 利用格柵法建立25×30柵格地圖，分別取障礙物的覆蓋率為20%、30%、40%和50%作對比實(shí)驗(yàn)。圖8給出了路徑仿真結(jié)果，可以看出，不同覆蓋率下算法均能繞開障礙物找到較優(yōu)路徑且受環(huán)境變化影響小。從表1 的數(shù)據(jù)可知，本文算法能快速找到路徑，且覆蓋率越高，運(yùn)行時間越短。

圖8 不同障礙物覆蓋率的路徑仿真

表1 不同障礙物覆蓋率的路徑仿真結(jié)果對比

實(shí)驗(yàn)2 改進(jìn)動態(tài)規(guī)劃算法同文獻(xiàn)[11]中的改進(jìn)蟻群算法進(jìn)行仿真實(shí)驗(yàn)對比。文獻(xiàn)[11]提供了20×20格柵地圖和30×30 格柵地圖的求解算例。用本文算法和文獻(xiàn)中算法進(jìn)行仿真實(shí)驗(yàn)，移動機(jī)器人的運(yùn)動路徑仿真結(jié)果如圖9 所示。仿真結(jié)果表明兩種算法的最優(yōu)路徑長度均為30.384 8（20×20）和42.183 8（30×30），且30×30格柵地圖兩個算法的搜索路徑相同。文獻(xiàn)[11]算法的運(yùn)行時間為10.142 s（20×20）和42.183 8 s（30×30），本文算法的運(yùn)行時間為0.002 s（20×20）和0.001 5 s（30×30），可見本文算法能夠快速有效地找到一條較優(yōu)路徑，并且節(jié)省了運(yùn)行時間。通過算法結(jié)構(gòu)分析可知本文算法避免了多次柵格重新組合，對比迭代更新路徑長度的過程，每個柵格至多賦值兩次，因此時效性好。

圖9 本文算法與文獻(xiàn)中算法路徑仿真對比

實(shí)驗(yàn)3 利用格柵法建立40×40的柵格地圖，分別選取5 個不同的目標(biāo)點(diǎn)做仿真實(shí)驗(yàn)。圖10 給出了路徑仿真結(jié)果，表2給出改進(jìn)動態(tài)規(guī)劃算法求解不同目標(biāo)點(diǎn)的路徑長度。可以看出，不同目標(biāo)時算法均能繞開障礙物找到較優(yōu)路徑，且算法僅需要運(yùn)行一次，就能找出多個目標(biāo)點(diǎn)的規(guī)劃路徑，總用時為0.007 s。可見本文算法具有承襲性，每個格柵計(jì)算值可以用于多個目標(biāo)點(diǎn)的規(guī)劃路徑，且算法的時效性好。

圖10 多目標(biāo)點(diǎn)的路徑仿真

表2 多目標(biāo)點(diǎn)的路徑長度結(jié)果

5 結(jié)論

（1）首次運(yùn)用兩種動態(tài)規(guī)劃模型結(jié)合使用的方法提出解決移動機(jī)器人路徑規(guī)劃的改進(jìn)動態(tài)規(guī)劃算法。該算法節(jié)省反復(fù)搜索路徑的時間，為路徑規(guī)劃問題求解提供了新思路。

（2）由MATLAB仿真對比實(shí)驗(yàn)可知，本文算法可以同時求得多個目標(biāo)方向的搜索結(jié)果，具有承襲性，能快速求得規(guī)劃路徑。與改進(jìn)蟻群算法相比較在運(yùn)算時間和最短路徑尋優(yōu)效果上具一定的優(yōu)越性，在實(shí)際運(yùn)行過程中，受柵格地圖環(huán)境情況影響較小。