葛文雅， 李平

(華僑大學(xué) 信息科學(xué)與工程學(xué)院， 福建 廈門 361021)

路徑規(guī)劃是對(duì)初始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的可行路徑進(jìn)行規(guī)劃.路徑規(guī)劃的實(shí)現(xiàn)應(yīng)解決以下2個(gè)問(wèn)題：一是規(guī)劃的路徑能夠安全、有效地躲避障礙物，最終抵達(dá)目標(biāo)節(jié)點(diǎn)；二是有效減少移動(dòng)機(jī)器人的運(yùn)動(dòng)時(shí)間、優(yōu)化路徑軌跡、提高安全性、提升運(yùn)行效率等[1-4].根據(jù)環(huán)境為時(shí)變模型或時(shí)不變模型，路徑規(guī)劃可分為靜態(tài)路徑規(guī)劃和動(dòng)態(tài)路徑規(guī)劃[5].通常情況下，靜態(tài)路徑規(guī)劃是指移動(dòng)機(jī)器人在能夠直接了解全局環(huán)境信息的前提下，規(guī)劃一條安全可行的路徑.與靜態(tài)路徑規(guī)劃不同，動(dòng)態(tài)路徑規(guī)劃應(yīng)用于全局環(huán)境信息不可知或隨時(shí)間變化的情況，只能得到當(dāng)前周圍范圍內(nèi)的局部環(huán)境信息，并根據(jù)得到的信息完成局部路徑規(guī)劃，不斷調(diào)整移動(dòng)機(jī)器人的動(dòng)作，直到抵達(dá)目標(biāo)位置[6].移動(dòng)機(jī)器人的實(shí)際地圖信息多變且復(fù)雜，僅依靠某種算法進(jìn)行路徑規(guī)劃無(wú)法滿足應(yīng)用場(chǎng)景的要求，人與移動(dòng)機(jī)器人的安全無(wú)法得到保障[7-9].移動(dòng)機(jī)器人在規(guī)劃全局最優(yōu)路徑的同時(shí)，更要具備動(dòng)態(tài)避障的能力[10].因此，需對(duì)各類算法應(yīng)用時(shí)存在的不足進(jìn)行改進(jìn)，把各類算法進(jìn)行融合，這是目前路徑規(guī)劃的主要研究方向之一[11-12].

全局靜態(tài)路徑規(guī)劃算法能夠基于已知的靜態(tài)信息完成規(guī)劃，得到全局最優(yōu)路徑，但因缺少動(dòng)態(tài)障礙物信息，無(wú)法動(dòng)態(tài)避障.局部動(dòng)態(tài)路徑規(guī)劃算法能夠根據(jù)當(dāng)前輸入的傳感器信息規(guī)劃局部路徑，不斷更新輸入、輸出，實(shí)現(xiàn)動(dòng)態(tài)障礙物躲避，但因缺少全局信息，易陷入局部最優(yōu)[13]，導(dǎo)致目標(biāo)節(jié)點(diǎn)不可達(dá).兩種算法各有優(yōu)缺點(diǎn)，故一些學(xué)者將這兩種算法進(jìn)行結(jié)合.王志中[14]提出一種基于A*算法與改進(jìn)人工勢(shì)場(chǎng)法的融合算法，但人工勢(shì)場(chǎng)法規(guī)劃的路徑轉(zhuǎn)角較大，難以應(yīng)用于實(shí)際.動(dòng)態(tài)窗口法容易實(shí)現(xiàn)，且規(guī)劃路徑平滑，能夠?qū)崿F(xiàn)局部避障[15].王凡等[16]將動(dòng)態(tài)窗口法應(yīng)用于A*算法兩個(gè)路徑節(jié)點(diǎn)間的局部規(guī)劃，可得到更為平滑的路徑，但算法的運(yùn)行時(shí)間較長(zhǎng).Ji等[17]提出一種基于優(yōu)化A*算法和動(dòng)態(tài)窗口法的優(yōu)化算法，可提高搜索效率，但未考慮動(dòng)態(tài)障礙物的情況，且存在速度震蕩的問(wèn)題.Liu等[18]提出一種基于Jump-A*算法和動(dòng)態(tài)窗口法的融合算法，考慮了動(dòng)態(tài)障礙物的情況，可提高路徑的平滑度，但仍然存在移動(dòng)機(jī)器人速度震蕩及繞遠(yuǎn)的問(wèn)題.基于此，本文提出一種基于安全A*算法和雙速度模型動(dòng)態(tài)窗口法的移動(dòng)機(jī)器人全局動(dòng)態(tài)路徑規(guī)劃融合算法.

1 安全A*算法

A*算法的路徑規(guī)劃存在路徑轉(zhuǎn)角過(guò)大，以及路徑和障礙物過(guò)近的問(wèn)題.因此，提出安全A*算法，主要從兩個(gè)方面進(jìn)行改進(jìn):1) 擴(kuò)展搜索鄰域；2) 改進(jìn)啟發(fā)式函數(shù)，加入一個(gè)安全項(xiàng)，增加樣本信息.

1.1 搜索鄰域的擴(kuò)展

針對(duì)A*算法路徑規(guī)劃存在路徑轉(zhuǎn)角過(guò)大的問(wèn)題，安全A*算法將節(jié)點(diǎn)n的搜索鄰域擴(kuò)展至24個(gè).搜索鄰域擴(kuò)展過(guò)程如下：對(duì)于節(jié)點(diǎn)n(x，y)，遍歷節(jié)點(diǎn)n臨近的24個(gè)節(jié)點(diǎn)(圖1，中間黑色區(qū)域?yàn)楣?jié)點(diǎn)n，周圍白色區(qū)域?yàn)楸闅v的臨近節(jié)點(diǎn)，箭頭方向?yàn)槁窂椒较?，以作為后續(xù)節(jié)點(diǎn)m(x′，y′)，m={(x′，y′)∣x′=[x-2，x+2]，y′=[y-2，y+2]}；如果后續(xù)節(jié)點(diǎn)m為障礙節(jié)點(diǎn)，或后續(xù)節(jié)點(diǎn)m在close表中，則跳過(guò)，選取下一個(gè)臨近點(diǎn)作為后續(xù)節(jié)點(diǎn)；否則，計(jì)算后續(xù)節(jié)點(diǎn)的評(píng)價(jià)函數(shù)f(m)，判斷m是否在open表中，若判斷失敗，將后續(xù)節(jié)點(diǎn)m放入open表，若判斷成功，則選取較小的f(m)，并更新后續(xù)節(jié)點(diǎn)m的實(shí)際代價(jià)函數(shù)、評(píng)價(jià)函數(shù)及其父節(jié)點(diǎn).

圖1 24鄰域的搜索方向

1.2 啟發(fā)式函數(shù)的改進(jìn)

擴(kuò)展節(jié)點(diǎn)的搜索鄰域可以減小路徑轉(zhuǎn)角，提高移動(dòng)機(jī)器人實(shí)際運(yùn)動(dòng)中的路徑安全性.然而，路徑和障礙物的距離仍然很近，路徑的安全性還有較大的提升空間.A*算法的核心在于啟發(fā)式函數(shù)h(n)的設(shè)計(jì)，安全A*算法采用A*算法對(duì)當(dāng)前軌跡節(jié)點(diǎn)進(jìn)行評(píng)估的思想，并增加了節(jié)點(diǎn)及一定范圍內(nèi)的障礙物信息，增大了路徑和障礙物的距離，從而具有更高的路徑安全性.

安全A*算法的評(píng)價(jià)函數(shù)f(n)為

f(n)=g(n)+h′(n)，h′(n)=h(n)+εL(n)，L(n)=k/s.

(1)

式(1)中：g(n)為實(shí)際代價(jià)函數(shù)，即當(dāng)前已走過(guò)的路徑距離；h′(n)為安全A*算法的啟發(fā)式函數(shù)；L(n)為節(jié)點(diǎn)n對(duì)應(yīng)的危險(xiǎn)評(píng)估值(安全項(xiàng))；k為評(píng)價(jià)范圍內(nèi)障礙物的個(gè)數(shù)；s為移動(dòng)機(jī)器人與障礙物的最小距離；ε為安全項(xiàng)L(n)的權(quán)重，文中設(shè)定為100.

1.3 安全A*算法的流程

open表存儲(chǔ)搜索過(guò)程中的待擴(kuò)展節(jié)點(diǎn)，并將這些節(jié)點(diǎn)按照評(píng)價(jià)函數(shù)值進(jìn)行升序排序；close表保存open表中評(píng)價(jià)函數(shù)值最小的后續(xù)節(jié)點(diǎn)；safe表保存后續(xù)節(jié)點(diǎn)的安全性指數(shù)S.在執(zhí)行路徑規(guī)劃時(shí)，安全A*算法主要通過(guò)open表和close表實(shí)現(xiàn)節(jié)點(diǎn)的擴(kuò)展和最優(yōu)節(jié)點(diǎn)的選取.

安全A*算法的流程有以下7個(gè)步驟.

步驟1將初始節(jié)點(diǎn)納入open表，將障礙節(jié)點(diǎn)納入close表.

步驟2選取open表中評(píng)價(jià)函數(shù)值最小的后續(xù)節(jié)點(diǎn)，將其納入close表，將后續(xù)節(jié)點(diǎn)的安全性指數(shù)納入safe表，并從open表刪除此節(jié)點(diǎn).

步驟3判斷該節(jié)點(diǎn)是否為目標(biāo)節(jié)點(diǎn)，若為目標(biāo)節(jié)點(diǎn)，則算法退出，返回當(dāng)前節(jié)點(diǎn)信息；若該節(jié)點(diǎn)非目標(biāo)節(jié)點(diǎn)，擴(kuò)展該節(jié)點(diǎn)的后續(xù)節(jié)點(diǎn)m.

步驟4在open表中建立從后續(xù)節(jié)點(diǎn)m返回n的指針，并計(jì)算f(m)，即

f(m)=g(m)+h′(m)，h′(m)=h(m)+εL(m)，L(m)=k/s.

(2)

步驟5判斷open表中是否存在后續(xù)節(jié)點(diǎn)m，若結(jié)果為假，則將節(jié)點(diǎn)m納入open表，并將節(jié)點(diǎn)m的安全性指數(shù)納入safe表；若結(jié)果為真，則比較不同前向指針的f(m)的大小，并選取較小的f(m).

步驟6更新g(m)，f(m)及后續(xù)節(jié)點(diǎn)m的前向指針.

步驟7按照評(píng)價(jià)函數(shù)值對(duì)open表重新進(jìn)行升序排序，并返回步驟2.

1.4 基于安全A*算法的全局路徑規(guī)劃

采用柵格法[19]構(gòu)建地圖模型，柵格法是移動(dòng)機(jī)器人環(huán)境建模的常用方法，它將移動(dòng)機(jī)器人的工作空間劃分為網(wǎng)格單元.柵格地圖模型，如圖2所示.圖2中：黑色網(wǎng)格為障礙物信息；白色格子為移動(dòng)機(jī)器人可移動(dòng)區(qū)域.

(a) 全局最優(yōu)路徑 (b) 安全性曲線

基于柵格地圖，采用安全A*算法進(jìn)行全局靜態(tài)規(guī)劃，可以得到全局最優(yōu)路徑及安全性曲線，如圖3所示.由圖3可知：全局最優(yōu)路徑平滑且距離障礙物較遠(yuǎn)；路徑的安全性指數(shù)始終保持在10，路徑安全性很高.

2 動(dòng)態(tài)窗口法

動(dòng)態(tài)窗口(DWA)法常用于存在動(dòng)態(tài)障礙環(huán)境的路徑規(guī)劃[20]，它基于采樣的當(dāng)前狀態(tài)計(jì)算下一時(shí)刻的合理軌跡，為每一軌跡附加評(píng)估值，并根據(jù)評(píng)估值選取該狀態(tài)的最優(yōu)預(yù)估軌跡[21-22].

2.1 移動(dòng)機(jī)器人運(yùn)動(dòng)模型

動(dòng)態(tài)窗口法需要在每個(gè)采樣時(shí)刻計(jì)算下一時(shí)刻所有的可能速度，通過(guò)運(yùn)動(dòng)模型預(yù)估下一時(shí)刻所有的可能路徑.因此，應(yīng)用動(dòng)態(tài)窗口法首先應(yīng)建立研究對(duì)象的運(yùn)動(dòng)模型，模型的準(zhǔn)確度會(huì)影響算法的實(shí)際控制效果.

假設(shè)采樣時(shí)間間隔(Δt)很短，移動(dòng)機(jī)器人的線速度(v)控制加速與減速，角速度(ω)負(fù)責(zé)轉(zhuǎn)向，記當(dāng)前時(shí)刻(t)移動(dòng)機(jī)器人位姿為(xt，yt，θt)，xt，yt分別為當(dāng)前時(shí)刻移動(dòng)機(jī)器人在x軸和y軸的位置，θt為當(dāng)前時(shí)刻移動(dòng)機(jī)器人與x軸的夾角；下一時(shí)刻的預(yù)估速度為(vt+1ωt+1)，vt+1，ωt+1分別為下一時(shí)刻移動(dòng)機(jī)器人的線速度和角速度.由于采樣時(shí)間間隔很短，故將移動(dòng)機(jī)器人運(yùn)動(dòng)的圓弧軌跡近似為直線，下一時(shí)刻的預(yù)估位姿記為(xt+1，yt+1，θt+1)，計(jì)算公式為

(3)

式(3)中：vx，vy分別為vt+1投影在x軸和y軸上的線速度；ωt為當(dāng)前時(shí)刻的角速度.

2.2 移動(dòng)機(jī)器人速度模型

根據(jù)移動(dòng)機(jī)器人運(yùn)動(dòng)模型，可通過(guò)當(dāng)前位姿和施加的速度控制量預(yù)估下一時(shí)刻的運(yùn)動(dòng)軌跡.基于當(dāng)前時(shí)刻的速度(vt，ωt)，構(gòu)造下一時(shí)刻速度的采樣空間，理論上存在無(wú)數(shù)下一時(shí)刻的預(yù)估速度(vt+1，ωt+1).由于受限于移動(dòng)機(jī)器人速度極限、動(dòng)力學(xué)性能及環(huán)境等因素，移動(dòng)機(jī)器人的速度被約束在一定的范圍內(nèi).

1) 速度極限約束.在物理系統(tǒng)中，受限于移動(dòng)機(jī)器人本身的設(shè)計(jì)與結(jié)構(gòu)，移動(dòng)機(jī)器人的線速度與角速度都有上限和下限.

移動(dòng)機(jī)器人受速度極限約束所能達(dá)到的速度空間Vm為

Vm={(v，ω)∣v∈[vmin，vmax]，ω∈[ωmin，ωmax]}.

(4)

式(4)中：vmax，vmin分別為線速度的上限和下限；ωmax，ωmin分別為角速度的上限和下限.

2) 動(dòng)力學(xué)性能約束.每個(gè)移動(dòng)機(jī)器人的電機(jī)性能并非完全相同，導(dǎo)致移動(dòng)機(jī)器人的加速、減速也不相同，因此，移動(dòng)機(jī)器人在下一個(gè)時(shí)刻的速度會(huì)受到約束.

移動(dòng)機(jī)器人受動(dòng)力學(xué)性能約束所能達(dá)到的速度空間Vg為

Vg={(v，ω)∣v∈[vt-av，minΔt，vt+av，maxΔt]，ω∈[ωt-av，minΔt，ωt+av，maxΔt].

(5)

式(5)中：av，min，av，max分別為線加速度的最小值和最大值.

3) 最小制動(dòng)距離約束.計(jì)算路徑規(guī)劃預(yù)估速度的最小制動(dòng)距離，判斷其對(duì)障礙物的躲避能力，以確保移動(dòng)機(jī)器人移動(dòng)的安全性.

移動(dòng)機(jī)器人受最小制動(dòng)距離約束所能達(dá)到的速度空間Va為

(6)

式(6)中：dist(v，ω)為速度(v，ω)預(yù)測(cè)的機(jī)器人和最近障礙物之間的距離(距離評(píng)價(jià)函數(shù)).

由此可得，移動(dòng)機(jī)器人運(yùn)動(dòng)速度空間合集V為

V=Vm∩Vg∩Va.

(7)

2.3 評(píng)價(jià)函數(shù)

考慮如何施加合適的控制量，使得到的速度采樣空間能夠規(guī)劃出更優(yōu)的軌跡.所有的控制量對(duì)應(yīng)一個(gè)預(yù)估速度，經(jīng)計(jì)算可得對(duì)應(yīng)的預(yù)估軌跡，所有的預(yù)估軌跡組成的預(yù)估軌跡集即為一個(gè)動(dòng)態(tài)窗口，動(dòng)態(tài)窗口在每個(gè)采樣時(shí)間內(nèi)完成更新.評(píng)價(jià)函數(shù)是對(duì)動(dòng)態(tài)窗口內(nèi)的全部預(yù)估軌跡進(jìn)行評(píng)價(jià)，選擇評(píng)價(jià)值最高的預(yù)估軌跡作為下一時(shí)刻的目標(biāo)軌跡.評(píng)價(jià)函數(shù)主要從方位角、距離及速度3個(gè)方面進(jìn)行設(shè)計(jì).

1) 方位角評(píng)價(jià)函數(shù)heading(v，ω).方位角是指移動(dòng)機(jī)器人按照采樣得到的線速度和角速度運(yùn)動(dòng)到預(yù)估軌跡的末端時(shí)，移動(dòng)機(jī)器人的航向與移動(dòng)機(jī)器人中心指向目標(biāo)節(jié)點(diǎn)方向的角度差.角度差越大，方位角評(píng)價(jià)函數(shù)值越小.

2) 距離評(píng)價(jià)函數(shù)dist(v，ω).移動(dòng)機(jī)器人按照采樣得到的線速度和角速度運(yùn)動(dòng)到預(yù)估軌跡的末端時(shí)，移動(dòng)機(jī)器人和最近障礙物之間的距離為dist(v，ω).距離評(píng)價(jià)函數(shù)值越高，軌跡越安全，舍棄與障礙物相交的軌跡.

3) 速度評(píng)價(jià)函數(shù)vel(v，ω).采樣時(shí)刻移動(dòng)機(jī)器人的速度會(huì)影響移動(dòng)機(jī)器人的運(yùn)行時(shí)間與工作效率.在預(yù)估速度集中，速度評(píng)價(jià)函數(shù)值與速度的大小成正比.

總評(píng)價(jià)函數(shù)G(v，ω)為

G(v，ω)=αheading(v，ω)+βdist(v，ω)+γvel(v，ω).

(8)

式(8)中：α，β，γ分別為方位角評(píng)價(jià)函數(shù)、距離評(píng)價(jià)函數(shù)、速度評(píng)價(jià)函數(shù)的權(quán)重系數(shù).

2.4 基于安全A*算法和動(dòng)態(tài)窗口法的動(dòng)態(tài)路徑規(guī)劃

動(dòng)態(tài)窗口法根據(jù)得到的局部環(huán)境信息可以很好地躲避靜態(tài)或動(dòng)態(tài)的障礙物，得到一條不發(fā)生碰撞的最優(yōu)路徑.然而，動(dòng)態(tài)窗口法只根據(jù)局部環(huán)境信息進(jìn)行軌跡規(guī)劃，缺少全局環(huán)境信息的指引，很容易陷入局部最優(yōu)，甚至出現(xiàn)無(wú)法順利到達(dá)目標(biāo)位置等非常嚴(yán)重的問(wèn)題.

之前，學(xué)者提出基于A*算法和動(dòng)態(tài)窗口法的融合算法， 規(guī)劃時(shí)將A*算法規(guī)劃得到的全局路徑節(jié)點(diǎn)作為臨時(shí)目標(biāo)節(jié)點(diǎn)，到達(dá)路徑節(jié)點(diǎn)pi后，路徑節(jié)點(diǎn)pi+1將成為下一個(gè)臨時(shí)的目標(biāo)節(jié)點(diǎn)，直至到達(dá)目標(biāo)節(jié)點(diǎn).經(jīng)過(guò)實(shí)驗(yàn)仿真.該算法能使移動(dòng)機(jī)器人成功抵達(dá)目標(biāo)位置，但移動(dòng)機(jī)器人接近一個(gè)臨時(shí)目標(biāo)節(jié)點(diǎn)時(shí)，速度會(huì)急速下降，甚至直接降為0，導(dǎo)致移動(dòng)機(jī)器人整體運(yùn)行不夠平穩(wěn).

進(jìn)一步地，對(duì)基于安全A*算法和動(dòng)態(tài)窗口法的融合算法(初步融合算法)進(jìn)行仿真實(shí)驗(yàn)，可得初步融合算法的速度曲線，如圖4所示.圖4中：tp為路徑規(guī)劃時(shí)間.

圖4 初步融合算法的速度曲線

3 雙速度模型動(dòng)態(tài)窗口法

初步融合算法進(jìn)行規(guī)劃時(shí)，無(wú)法區(qū)別臨時(shí)目標(biāo)節(jié)點(diǎn)和全局目標(biāo)節(jié)點(diǎn)，以致無(wú)差別地給出執(zhí)行減速停止的指令.因此，需對(duì)動(dòng)態(tài)窗口法的速度模型進(jìn)行改進(jìn)，采用雙速度模型.

當(dāng)判定臨時(shí)目標(biāo)節(jié)點(diǎn)為全局目標(biāo)節(jié)點(diǎn)，或距離臨時(shí)目標(biāo)節(jié)點(diǎn)還有一定距離時(shí)，速度模型不變，仍采用式(4)～(6)的約束條件；當(dāng)判定臨時(shí)目標(biāo)節(jié)點(diǎn)不是全局目標(biāo)節(jié)點(diǎn)，且距離臨時(shí)目標(biāo)節(jié)點(diǎn)小于一定距離時(shí)，線速度保持不變(仍為上一時(shí)刻的線速度(vt-Δt))，角速度選取原則不變.

此時(shí)，雙速度模型如下.

(9)

(10)

(11)

由此可得雙速度模型的速度空間合集為

(12)

式(12)中：p為臨時(shí)目標(biāo)節(jié)點(diǎn)；G為全局目標(biāo)節(jié)點(diǎn)；distp為移動(dòng)機(jī)器人當(dāng)前位置和臨時(shí)目標(biāo)節(jié)點(diǎn)之間的距離；d為常數(shù)，文中取3.

4 基于安全A*算法和雙速度模型動(dòng)態(tài)窗口法的融合算法

基于安全A*算法和雙速度模型動(dòng)態(tài)窗口法，提出移動(dòng)機(jī)器人全局動(dòng)態(tài)路徑規(guī)劃融合算法(文中算法).采用基于柵格地圖的安全A*算法進(jìn)行全局靜態(tài)路徑規(guī)劃，可得全局最優(yōu)路徑；采用時(shí)間序列Bottom-Up算法對(duì)全局最優(yōu)路徑進(jìn)行特征點(diǎn)提取，以作為臨時(shí)目標(biāo)節(jié)點(diǎn)；在全局最優(yōu)路徑的信息引導(dǎo)下，采用雙速度模型動(dòng)態(tài)窗口法進(jìn)行動(dòng)態(tài)規(guī)劃與避障，躲避動(dòng)態(tài)障礙物后，采用避障重規(guī)劃?rùn)C(jī)制進(jìn)行路徑重規(guī)劃，確保規(guī)劃路徑的全局最優(yōu).

文中算法的思路框架，如圖5所示.

圖5 文中算法的思路框架

4.1 時(shí)間序列Bottom-Up算法

針對(duì)現(xiàn)有融合算法中臨時(shí)目標(biāo)節(jié)點(diǎn)過(guò)多導(dǎo)致效率較低、計(jì)算代價(jià)較大的問(wèn)題，采用時(shí)間序列Bottom-Up算法，對(duì)安全A*算法得到的全局最優(yōu)路徑節(jié)點(diǎn)進(jìn)行預(yù)處理，擬合分段得到特征路徑節(jié)點(diǎn)作為臨時(shí)目標(biāo)節(jié)點(diǎn)，在保留原路徑特征的前提下，減少全局路徑節(jié)點(diǎn)的數(shù)量.特征路徑節(jié)點(diǎn)的選取，如圖6所示.圖6中：藍(lán)色實(shí)線為安全A*算法得到的全局最優(yōu)路徑；藍(lán)色空心圓為安全A*算法得到的全局最優(yōu)路徑節(jié)點(diǎn)(80個(gè))；紅色實(shí)心圓為經(jīng)過(guò)時(shí)間序列Bottom-Up算法預(yù)處理后的特征路徑節(jié)點(diǎn)(7個(gè)).由圖6可知：臨時(shí)目標(biāo)節(jié)點(diǎn)的數(shù)量由80個(gè)減少為7個(gè)，有效降低了儲(chǔ)存和計(jì)算代價(jià)，提高了路徑規(guī)劃的效率.

(a) 安全A*算法 (b) 時(shí)間序列Bottom-Up算法

4.2 避障重規(guī)劃?rùn)C(jī)制

當(dāng)環(huán)境中出現(xiàn)未知障礙物和動(dòng)態(tài)障礙物時(shí)，初步融合算法根據(jù)當(dāng)前窗口不斷更新速度(v，ω)，使移動(dòng)機(jī)器人順利地躲避障礙物.由于脫離原全局最優(yōu)路徑已有一段時(shí)間，當(dāng)重新恢復(fù)至原來(lái)模式時(shí)，會(huì)出現(xiàn)移動(dòng)機(jī)器人繞遠(yuǎn)甚至繞圈的問(wèn)題(圖7的紅色路徑).這是由于避障模式結(jié)束后，原臨時(shí)目標(biāo)節(jié)點(diǎn)沒(méi)有改變，但此時(shí)移動(dòng)機(jī)器人很可能已經(jīng)走過(guò)原臨時(shí)目標(biāo)節(jié)點(diǎn)，且方位角無(wú)法立即改變.此時(shí)，如果不改變臨時(shí)目標(biāo)節(jié)點(diǎn)，移動(dòng)機(jī)器人則會(huì)繼續(xù)按照原臨時(shí)目標(biāo)節(jié)點(diǎn)進(jìn)行規(guī)劃，出現(xiàn)移動(dòng)機(jī)器人繞遠(yuǎn)甚至繞圈的問(wèn)題.為了解決此問(wèn)題，提出一種避障重規(guī)劃?rùn)C(jī)制.在路徑規(guī)劃的過(guò)程中，全程監(jiān)控臨時(shí)目標(biāo)節(jié)點(diǎn)、移動(dòng)機(jī)器人當(dāng)前位置及下一臨時(shí)目標(biāo)節(jié)點(diǎn)三者形成的夾角(θa).如果θa<90°，則臨時(shí)目標(biāo)節(jié)點(diǎn)保持不變，繼續(xù)進(jìn)行路徑規(guī)劃；如果θa≥90°，當(dāng)前規(guī)劃可能出現(xiàn)移動(dòng)機(jī)器人繞遠(yuǎn)甚至繞圈的問(wèn)題，則重新選擇下一特征節(jié)點(diǎn)作為臨時(shí)目標(biāo)節(jié)點(diǎn)進(jìn)行路徑規(guī)劃.夾角的不同情形，如圖8所示.

(a) θa<90° (b) θa≥90°

4.3 文中算法流程

動(dòng)態(tài)窗口法根據(jù)實(shí)時(shí)獲取的局部環(huán)境信息，實(shí)現(xiàn)路徑規(guī)劃和動(dòng)態(tài)避障，但由于忽略全局信息，導(dǎo)致易陷入局部最優(yōu)、目標(biāo)節(jié)點(diǎn)不可達(dá)等問(wèn)題.因此，文中算法首先通過(guò)安全A*算法進(jìn)行全局靜態(tài)路徑規(guī)劃；然后，將規(guī)劃得到最優(yōu)路徑上的節(jié)點(diǎn)作為臨時(shí)目標(biāo)節(jié)點(diǎn)，兩個(gè)臨時(shí)目標(biāo)節(jié)點(diǎn)之間采用動(dòng)態(tài)窗口法實(shí)現(xiàn)動(dòng)態(tài)規(guī)劃，融合了靜態(tài)規(guī)劃的全局信息和動(dòng)態(tài)規(guī)劃的動(dòng)態(tài)避障.若簡(jiǎn)單進(jìn)行兩種方法的融合，則會(huì)出現(xiàn)移動(dòng)機(jī)器人運(yùn)動(dòng)不平滑、速度震蕩、移動(dòng)機(jī)器人繞遠(yuǎn)且效率低等問(wèn)題.針對(duì)以上問(wèn)題使用時(shí)間序列Bottom-Up算法對(duì)安全A*算法得到的全局最優(yōu)路徑進(jìn)行預(yù)處理，提取全局最優(yōu)路徑的特征節(jié)點(diǎn)作為融合算法的臨時(shí)目標(biāo)節(jié)點(diǎn)，并對(duì)動(dòng)態(tài)窗口法的速度模型進(jìn)行改進(jìn)，基于全局最優(yōu)路徑信息，采用雙速度模型動(dòng)態(tài)窗口法進(jìn)行動(dòng)態(tài)規(guī)劃，動(dòng)態(tài)避障后采用避障重規(guī)劃?rùn)C(jī)制，避免出現(xiàn)移動(dòng)機(jī)器人繞遠(yuǎn)甚至繞圈的問(wèn)題.文中算法流程圖，如圖9所示.

圖9 文中算法流程圖

5 仿真結(jié)果與分析

仿真實(shí)驗(yàn)的主要參數(shù)設(shè)置如下：vmax=1.0 m·s-1；ωmax=20 rad·s-1；Δt=0.1 s；α=0.05；β=0.5；γ=0.1；av，max=0.2 m·s-2；角加速度最大值aω，max=50 rad·s-2.

兩種算法的路徑軌跡，如圖10所示.由圖10可知：初步融合算法和文中算法都能在靜態(tài)環(huán)境下根據(jù)全局信息的指引進(jìn)行全局路徑規(guī)劃；文中算法路徑更加平滑.

(a) 初步融合算法 (b) 文中算法

兩種算法的速度曲線，如圖11所示.由圖11可知：初步融合算法的移動(dòng)機(jī)器人多次加速、減速，速度曲線震蕩劇烈，導(dǎo)致移動(dòng)機(jī)器人運(yùn)行不穩(wěn)定，且規(guī)劃時(shí)間較長(zhǎng)，效率不高；文中算法的速度曲線平滑，移動(dòng)機(jī)器人運(yùn)行平穩(wěn)，規(guī)劃時(shí)間較短.

(a) 初步融合算法 (b) 文中算法

因此，文中算法可解決移動(dòng)機(jī)器人減速導(dǎo)致的運(yùn)行不平穩(wěn)、速度震蕩等問(wèn)題，提高了移動(dòng)機(jī)器人的運(yùn)行效率.

算法性能指標(biāo)的對(duì)比，如表1所示.表1中：l為路徑長(zhǎng)度.由表1可知：算法改進(jìn)前后，路徑長(zhǎng)度幾乎沒(méi)有變化，說(shuō)明文中算法仍然可以較好地在最優(yōu)路徑信息的指引下進(jìn)行規(guī)劃，以保證搜索路徑的全局最優(yōu)性和安全性；路徑規(guī)劃時(shí)間從202.24 s縮短到108.85 s，文中算法的規(guī)劃效率提高了46.18%.

表1 算法性能指標(biāo)的對(duì)比

為了驗(yàn)證文中算法的動(dòng)態(tài)避障性能，在環(huán)境中增加3個(gè)持續(xù)來(lái)回運(yùn)動(dòng)的障礙物.文中算法的動(dòng)態(tài)避障規(guī)劃，如圖12所示.圖12中：紅色圓形為動(dòng)態(tài)障礙物，按照黑色虛線來(lái)回移動(dòng)；藍(lán)色實(shí)線為文中算法進(jìn)行全局動(dòng)態(tài)規(guī)劃時(shí)遇見(jiàn)動(dòng)態(tài)障礙物進(jìn)行避障的實(shí)際規(guī)劃路徑(動(dòng)態(tài)避障實(shí)際軌跡)；紅色虛線為文中算法在沒(méi)有動(dòng)態(tài)障礙物時(shí)進(jìn)行全局動(dòng)態(tài)規(guī)劃的路徑(靜態(tài)全局規(guī)劃軌跡).

由圖12可知：文中算法能夠躲避動(dòng)態(tài)障礙物，且未出現(xiàn)初步融合算法中移動(dòng)機(jī)器人繞遠(yuǎn)甚至繞圈的問(wèn)題，可成功地完成全局動(dòng)態(tài)路徑規(guī)劃任務(wù).

(a) 避障前(1號(hào)動(dòng)態(tài)障礙物) (b) 避障后(1號(hào)動(dòng)態(tài)障礙物)

文中算法全局動(dòng)態(tài)規(guī)劃路徑與速度曲線，如圖13所示.由圖13可知：文中算法可在全局最優(yōu)路徑信息的指引下很好地完成全局動(dòng)態(tài)路徑規(guī)劃，躲避動(dòng)態(tài)障礙物，保證路徑的平滑性和安全性；文中算法動(dòng)態(tài)避障規(guī)劃時(shí)對(duì)應(yīng)的移動(dòng)機(jī)器人速度曲線較為平滑，有少數(shù)減速；文中算法速度平穩(wěn)，遇到動(dòng)態(tài)障礙物時(shí)能夠適當(dāng)減速，可保證移動(dòng)機(jī)器人整體運(yùn)行的安全性.

(a) 全局動(dòng)態(tài)規(guī)劃路徑 (b) 速度曲線

由仿真實(shí)驗(yàn)可知，文中算法能夠很好地解決A*算法和動(dòng)態(tài)窗口法融合過(guò)程中移動(dòng)機(jī)器人運(yùn)動(dòng)速度曲線不平滑、速度震蕩及移動(dòng)機(jī)器人繞遠(yuǎn)導(dǎo)致規(guī)劃效率低的問(wèn)題；文中算法遇到未知障礙物時(shí)也能表現(xiàn)出很好的性能.因此，文中算法能夠高效地完成全局動(dòng)態(tài)規(guī)劃任務(wù)，并保證路徑的安全性和最優(yōu)性.

6 結(jié)束語(yǔ)

針對(duì)移動(dòng)機(jī)器人全局動(dòng)態(tài)路徑規(guī)劃效率較低的問(wèn)題，提出一種基于安全A*算法與雙速度模型動(dòng)態(tài)窗口法的全局動(dòng)態(tài)路徑規(guī)劃融合算法.通過(guò)時(shí)間序列Bottom-Up算法對(duì)安全A*算法規(guī)劃的全局最優(yōu)路徑進(jìn)行預(yù)處理，將全局最優(yōu)路徑的特征節(jié)點(diǎn)作為臨時(shí)目標(biāo)節(jié)點(diǎn)，減少臨時(shí)目標(biāo)節(jié)點(diǎn)的數(shù)量；使用改進(jìn)后的雙速度模型動(dòng)態(tài)窗口法，基于全局最優(yōu)信息進(jìn)行動(dòng)態(tài)規(guī)劃，使移動(dòng)機(jī)器人的運(yùn)行速度平穩(wěn)；通過(guò)避障重規(guī)劃?rùn)C(jī)制，解決動(dòng)態(tài)避障后的路徑重規(guī)劃問(wèn)題，避免移動(dòng)機(jī)器人出現(xiàn)繞遠(yuǎn)甚至繞圈的問(wèn)題.仿真結(jié)果表明，文中算法的規(guī)劃效率提高了46.18%，可保證路徑的安全性和最優(yōu)性.