周睿慜，李 輝

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

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

1 引言

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

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

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

圖1 25×30柵格地圖

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

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

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

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

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

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

圖3 下降路徑機(jī)器人可移動(dòng)到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)時(shí)，令f(mi,j)等于下降路徑搜索不可到達(dá)標(biāo)記值C2（足夠大的常數(shù)值），表示向目標(biāo)靠近的下降路徑搜索過(guò)程中沒(méi)有路徑到達(dá)狀態(tài)mi,j，具體情況如圖4所示。

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

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

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

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

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

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

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

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

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

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

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

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

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

步驟2利用下降路徑搜索動(dòng)態(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ò)列利用上升路徑搜索動(dòng)態(tài)規(guī)劃求解各列元素的最優(yōu)路徑值M(i,j)(i=N-1,…,2,1) ，k=j+1，j=j+1，轉(zhuǎn)到步驟2。

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

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

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

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

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

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

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

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

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

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

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

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

5 結(jié)論

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

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