動(dòng)態(tài)環(huán)境下基于IVS算法的多機(jī)器人路徑規(guī)劃*

董 海，關(guān)甜甜

(沈陽(yáng)大學(xué)a.應(yīng)用技術(shù)學(xué)院；b.機(jī)械工程學(xué)院，沈陽(yáng) 110044)

0 引言

隨著時(shí)代發(fā)展，很多領(lǐng)域開(kāi)始應(yīng)用移動(dòng)機(jī)器人技術(shù)，尤其是路徑規(guī)劃技術(shù)。在生產(chǎn)領(lǐng)域，用于物料搬運(yùn)的AGV，在快遞分揀中用于無(wú)人分揀小車(chē)和派送機(jī)器人等。按照生產(chǎn)需求，充分利用周?chē)h(huán)境，以感知、建模、規(guī)劃、執(zhí)行的標(biāo)準(zhǔn)路徑規(guī)劃方法執(zhí)行[1-2]。按照路徑規(guī)劃環(huán)境是否已知，可將其分為全局路徑規(guī)劃和局部路徑規(guī)劃；根據(jù)路徑規(guī)劃環(huán)境中是否存在動(dòng)態(tài)障礙物，可分為靜態(tài)路徑規(guī)劃和動(dòng)態(tài)路徑規(guī)劃[3-4]。

趙偉等[5]基于雙層優(yōu)化A*算法和動(dòng)態(tài)窗口法，提出改進(jìn)A*算法，通過(guò)一層和二層全局優(yōu)化降低機(jī)器人路徑的轉(zhuǎn)折次數(shù)，驗(yàn)證了該方法適用于動(dòng)態(tài)和靜態(tài)環(huán)境，實(shí)現(xiàn)有效避障和路徑規(guī)劃；XIONG等[6]針對(duì)動(dòng)態(tài)環(huán)境下蟻群優(yōu)化路徑規(guī)劃中存在的收斂速度慢、全局搜索能力差和未知?jiǎng)討B(tài)障礙物等問(wèn)題，提出了一種基于時(shí)間禁忌策略的改進(jìn)蟻群優(yōu)化算法，改善搜索弱點(diǎn)，應(yīng)用網(wǎng)格預(yù)測(cè)模型解決實(shí)時(shí)避障問(wèn)題；AJEIL等[7]提出一種修正頻率蝙蝠(MFB)算法，在環(huán)境中沒(méi)有障礙物時(shí)生成機(jī)器人路徑，當(dāng)檢測(cè)到障礙物時(shí)開(kāi)啟第二種模式進(jìn)行避障，驗(yàn)證提出的算法可以實(shí)現(xiàn)規(guī)劃的路徑更短，且過(guò)程中無(wú)碰撞； ZHANG等[8]提出了一種動(dòng)態(tài)環(huán)境下移動(dòng)機(jī)器人的預(yù)測(cè)路徑規(guī)劃算法，在預(yù)先搜索全局近似最優(yōu)路徑的前提下，根據(jù)移動(dòng)機(jī)器人自身和動(dòng)態(tài)障礙物的速度對(duì)其進(jìn)行簡(jiǎn)單的運(yùn)動(dòng)預(yù)測(cè)，仿真證明該算法使路徑規(guī)劃速度加快并降低路徑代價(jià)和碰撞概率。ELMI等[9]在動(dòng)態(tài)和未知環(huán)境中，提出了一種基于蚱蜢算法的新型路徑規(guī)劃方法，對(duì)比得到該方法在運(yùn)行時(shí)間、穩(wěn)定性和效率等方面具有良好的應(yīng)用前景。

本文采用改進(jìn)虛擬彈簧算法對(duì)機(jī)器人群的行進(jìn)路線(xiàn)進(jìn)行規(guī)劃。傳統(tǒng)虛擬彈簧算法，雖然環(huán)境適應(yīng)性好，但是規(guī)劃出的路線(xiàn)不平滑，易受環(huán)境影響，且環(huán)境越復(fù)雜路線(xiàn)越曲折。本文以最短路徑和最短時(shí)間為目標(biāo)，結(jié)合動(dòng)態(tài)環(huán)境的情況，綜合考慮車(chē)間可能出現(xiàn)障礙物的不確定情況，有效提高處理障礙物的效率，降低避障時(shí)間；同時(shí)，在MATLAB軟件進(jìn)行仿真，驗(yàn)證其有效解決局部最小值和GNRON問(wèn)題的能力，及具有良好的路徑規(guī)劃能力和避障功能。

1 模型建立

1.1 多機(jī)器人網(wǎng)格的邏輯拓?fù)?/h3>

假設(shè)每個(gè)機(jī)器人只接受相鄰機(jī)器人的消息，沒(méi)有任何通信延遲。其中，n個(gè)機(jī)器人都通過(guò)虛擬彈簧相互連接。圖1為K=4和K=6的拓?fù)渚W(wǎng)絡(luò)。本文以K=6的拓?fù)淠Ｐ蜑槔?，領(lǐng)導(dǎo)者機(jī)器人為第一層，是機(jī)器人(f1～f6)的父節(jié)點(diǎn)，每個(gè)機(jī)器人都有自己的子節(jié)點(diǎn)。

(a) K=4的拓?fù)渚W(wǎng)格 (b) K=6的拓?fù)渚W(wǎng)格

假設(shè)彈簧的自然長(zhǎng)度為l0，實(shí)際長(zhǎng)度為lr。機(jī)器人間的彈簧控制規(guī)則如下：

①當(dāng)l0≤lr時(shí)，機(jī)器人f1和f2都將受到Fp的力，且與彈簧變形方向相反；若l0>lr時(shí)，機(jī)器人f1和f2都將受到力Fs，方向與彈簧變形方向相反；②當(dāng)機(jī)器人f1的節(jié)點(diǎn)消失時(shí)，將機(jī)器人f1和f2之間的兩個(gè)彈簧視為一個(gè)被拉伸的彈簧，力的方向Fp與彈簧的變形方向相反。

1.2 多機(jī)器人的編隊(duì)控制

目前，常用的編隊(duì)控制[10]方法有3種：領(lǐng)導(dǎo)者-跟隨者[11]、基于行為的方法[12]和虛擬結(jié)構(gòu)法[13]。本文應(yīng)用領(lǐng)導(dǎo)者-跟隨者編隊(duì)法。機(jī)器人R1是領(lǐng)導(dǎo)者機(jī)器人，可以確定每個(gè)跟隨機(jī)器人Rfi(i=1,2,…,n)的運(yùn)動(dòng)。

空間中兩個(gè)機(jī)器人fi和fj用彈簧相連，它們之間的伸縮量為d，λ為剛度系數(shù)，彈簧受力表達(dá)式為：

F=λd

(1)

式中，依據(jù)經(jīng)驗(yàn)值將λ設(shè)置為1000。

在運(yùn)動(dòng)過(guò)程中，虛擬彈簧狀態(tài)Tij有松弛、壓縮和拉伸3種狀態(tài)，表示為：

(2)

式中，lrij為機(jī)器人fi和fj之間彈簧的物理長(zhǎng)度，根據(jù)胡克定律，機(jī)器人fi和fj之間的虛擬彈簧力表示為：

Fij=Tijk0|l0-lrij|

(3)

為避免式(3)中機(jī)器人存在碰撞的可能，定義ls為機(jī)器人的期望安全距離，則表達(dá)式為：

(4)

Fij是矢量，指向彈簧的變形方向，角βij是機(jī)器人fi和fj之間的角：

(5)

則每個(gè)機(jī)器人的控制率可以表示為：

(6)

距離估計(jì)函數(shù)運(yùn)用曼哈頓距離，表達(dá)式為：

D=abs(xij-xtarget)+abs(yij-ytarget)

(7)

式中，xij和yij分別表示機(jī)器人當(dāng)前位置的橫縱坐標(biāo)；xtarget和ytarget分別表示機(jī)器人目標(biāo)位置的橫縱坐標(biāo)。

判斷依據(jù)為彈簧的彈性勢(shì)能G，G越小，控制效果越好。能量函數(shù)可以表示為：

(8)

1.3 目標(biāo)約束采樣

本文結(jié)合目標(biāo)偏執(zhí)和位置約束兩個(gè)方法，以提高采樣的目標(biāo)導(dǎo)向性[14]。設(shè)置目標(biāo)偏執(zhí)概率ppz為0.1，其次根據(jù)均勻概率隨機(jī)產(chǎn)生一個(gè)概率值p，若該p

(9)

式中，Rr為隨機(jī)采樣函數(shù)。

若隨機(jī)生成采樣點(diǎn)，需要約束其位置。隨機(jī)生成的采樣點(diǎn)，對(duì)比其在X或Y方向比前一個(gè)采樣點(diǎn)更接近目標(biāo)點(diǎn)作為依據(jù)。采樣約束示意圖如圖2所示。

圖2 采樣約束示意圖

1.4 避障控制策略

融合多機(jī)器人避障規(guī)劃的姿態(tài)信息和誤差反饋跟蹤調(diào)節(jié)方法[15]，定義多機(jī)器人避障的表達(dá)式為：

(10)

式中，

p(k)=Rn

(11)

式中，H(k)為多機(jī)器人避障中的障礙物矩陣；p(k)為多機(jī)器人群繞坐標(biāo)系各軸的慣性協(xié)方差矩陣；若p(k)∈Q2(0,∞)，I、J、L、O為多移動(dòng)機(jī)器人質(zhì)心分布矩陣參數(shù)；n為控制約束的自變量維數(shù)，為正整數(shù)；ΔI1、ΔJ1為多移動(dòng)機(jī)器人避障隱態(tài)誤差增益。

基于式(10)，多機(jī)器人避障規(guī)劃的控制器表達(dá)為：

(12)

考慮動(dòng)態(tài)環(huán)境下的避障功能，引入動(dòng)態(tài)A*算法中的估計(jì)函數(shù)：

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

(13)

式中，f(x)為節(jié)點(diǎn)x的估價(jià)函數(shù)；g(x)為狀態(tài)空間中從初始節(jié)點(diǎn)到x節(jié)點(diǎn)的實(shí)際代價(jià)；h(x)為從x節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)最優(yōu)路徑的估計(jì)代價(jià)。

2 改進(jìn)虛擬彈簧算法(IVS算法)

當(dāng)多機(jī)器人距離障礙物足夠近時(shí)，障礙物產(chǎn)生排斥力Frep，排斥力與多機(jī)器人障礙物之間的距離成反比，并指向遠(yuǎn)離障礙物的方向。Fa是Fatt和Frep的合力，機(jī)器人群將朝著合力的方向運(yùn)動(dòng)，直到到達(dá)目標(biāo)。虛擬彈簧受力分析如圖3所示。虛擬彈簧受力分析表達(dá)式如式(14)所示。

圖3 作用在機(jī)器人群上的虛擬彈簧受力分析圖

(14)

為了避免碰撞，當(dāng)Frep

(15)

式中，q0為障礙物坐標(biāo)；f(q-q0)為機(jī)器人群與障礙物間的距離，迎面距離為障礙物的最大沖擊距離。

虛擬彈簧中儲(chǔ)存的彈性勢(shì)能表達(dá)為：

(16)

式中，Gg(q)為機(jī)器人群與目標(biāo)之間虛擬彈簧的勢(shì)能；G0(q)為機(jī)器人群與障礙物之間虛擬彈簧的勢(shì)能。

傳統(tǒng)虛擬彈簧算法的不足是易陷入局部最小值或者GNRON問(wèn)題，如圖4和圖5所示。

圖4 局部極小值時(shí)的路徑圖 圖5 GNRON時(shí)的路徑圖

2.1 基于IVS算法的多機(jī)器人路徑規(guī)劃

由于機(jī)器人群在轉(zhuǎn)向上的時(shí)間能量耗費(fèi)比直線(xiàn)行駛要多，所以在傳統(tǒng)虛擬彈簧算法基礎(chǔ)上加入轉(zhuǎn)向代價(jià)，估價(jià)函數(shù)表達(dá)為：

f′(x)=g(x)+h(x)+ε0

(17)

式中，ε0為轉(zhuǎn)向代價(jià)，其值為：

(18)

假設(shè)空間中的目標(biāo)與障礙物分布圖如圖6所示。機(jī)器人群連接起點(diǎn)O和目標(biāo)點(diǎn)A形成OA，當(dāng)OA不穿過(guò)任何障礙物，機(jī)器人群路徑規(guī)劃圖如圖7所示。

圖6 目標(biāo)與障礙物之間分布圖 圖7 機(jī)器人群路徑規(guī)劃圖

當(dāng)OA穿過(guò)障礙物時(shí)，機(jī)器人群以當(dāng)前位置為中心旋轉(zhuǎn)，直到OA不穿過(guò)任何障礙物為止。旋轉(zhuǎn)后的矢量記為OB，軸向旋轉(zhuǎn)的轉(zhuǎn)角為γ，搜索點(diǎn)為T(mén)1，該點(diǎn)可滿(mǎn)足以下方程：

BT1·OB=0

(19)

|BT1|=d

(20)

(21)

根據(jù)式(19)～式(21)可得T1唯一的位置，視為虛擬目標(biāo)，OT1為第一子路徑，當(dāng)機(jī)器人群到達(dá)T1，若T1A通過(guò)障礙物，按照得到T1的方式，可得其它虛擬目標(biāo)T2,T3,…,Tn，直到TnA不跨越任何障礙物，若機(jī)器人群當(dāng)前位置O與所有虛擬目標(biāo)點(diǎn)連接，最終目標(biāo)與有向線(xiàn)段連接，即為機(jī)器人群從局部極小點(diǎn)到最終目標(biāo)的期望最優(yōu)路徑。本文基于MATLAB進(jìn)行仿真，結(jié)果如圖8所示。為評(píng)估算法的可靠性，計(jì)算彈性勢(shì)能Eg(q)如圖9所示。

圖8 機(jī)器人實(shí)際移動(dòng)路徑圖 圖9 機(jī)器人間彈性勢(shì)能Eg(q)圖

從機(jī)器人群軌跡可看出，實(shí)際路徑與圖7規(guī)劃的路徑非常接近，成功到達(dá)目標(biāo)。圖9顯示了機(jī)器人群到達(dá)各個(gè)虛擬目標(biāo)和最終目標(biāo)的彈性勢(shì)能情況，該圖可匹配圖8中的軌跡。

2.2 多機(jī)器人編隊(duì)路徑規(guī)劃

在多機(jī)器人系統(tǒng)中，機(jī)器人群的目標(biāo)還包括編隊(duì)[16]。在動(dòng)態(tài)環(huán)境下，機(jī)器人群實(shí)時(shí)更新附近障礙物情況以此判斷哪個(gè)方向位置為最佳。采用IVS算法的多機(jī)器人動(dòng)態(tài)路徑規(guī)劃的流程圖如圖10所示。

圖10 多機(jī)器人動(dòng)態(tài)路徑規(guī)劃流程圖

3 仿真分析

為評(píng)估所提的策略和算法的性能，設(shè)置機(jī)器人群步長(zhǎng)為0.06，仿真中所有參數(shù)均視為無(wú)量綱數(shù)。

3.1 多機(jī)器人編隊(duì)路徑規(guī)劃

機(jī)器人群之間虛擬彈簧的松弛長(zhǎng)度l0=3，剛度因子k0=300。障礙物位置均為隨機(jī)分布，障礙物的虛擬彈簧松弛長(zhǎng)度β=1000，剛度因子kr=600，領(lǐng)導(dǎo)者機(jī)器人的目標(biāo)位置坐標(biāo)為(68,68)，剛度因子kg=6。6個(gè)機(jī)器人穿越并避開(kāi)隨機(jī)障礙物的路線(xiàn)和彈性勢(shì)能圖如圖11所示。

由圖11a中可得，機(jī)器人群路徑規(guī)劃過(guò)程中沒(méi)有發(fā)生任何碰撞。如圖11b中的彈性勢(shì)能走向可得，該圖與軌跡匹配，驗(yàn)證底層控制效果良好。

(a) 6個(gè)機(jī)器人穿越并避開(kāi)隨機(jī)障礙物路線(xiàn)圖 (b) 6個(gè)機(jī)器人之間的彈性勢(shì)能圖

領(lǐng)導(dǎo)者機(jī)器人和跟隨者機(jī)器人穿越并避開(kāi)隨機(jī)障礙物的初始狀態(tài)如表1所示。領(lǐng)導(dǎo)者機(jī)器人和跟隨者機(jī)器人穿越并避開(kāi)固定障礙物的初始狀態(tài)如表2所示。

表1 領(lǐng)導(dǎo)者機(jī)器人和跟隨者機(jī)器人穿越并避開(kāi)隨機(jī)障礙物的初始狀態(tài)

表2 領(lǐng)導(dǎo)者機(jī)器人和跟隨者機(jī)器人穿過(guò)并避開(kāi)固定障礙物的初始狀態(tài)

設(shè)置l0、k0、β=1000、kr同上。領(lǐng)導(dǎo)者機(jī)器人的目標(biāo)位置坐標(biāo)為(30,65)，剛度因子kg=6。6個(gè)機(jī)器人穿越并避開(kāi)固定障礙物的路線(xiàn)和彈性勢(shì)能圖如圖12所示。

(a) 6個(gè)機(jī)器人穿越并避開(kāi)固定障礙物路線(xiàn)圖 (b) 6個(gè)機(jī)器人之間的彈性勢(shì)能圖

從圖12a中可以明顯看出，機(jī)器人群保持原有隊(duì)形，成功到達(dá)目標(biāo)，且無(wú)碰撞。圖12b驗(yàn)證了多機(jī)器人在編隊(duì)控制方面表現(xiàn)出了良好的性能。

3.2 算法比較

3.2.1 路徑平滑比較

程傳奇、王殿君等[17-18]基于傳統(tǒng)A*算法提出動(dòng)態(tài)A*算法，適用于典型的動(dòng)態(tài)路徑規(guī)劃。本文模擬車(chē)間的動(dòng)態(tài)環(huán)境，將IVS算法與傳統(tǒng)的虛擬彈簧算法和動(dòng)態(tài)A*算法進(jìn)行比較。

參數(shù)θ、§、γ的選取為平滑處理路徑節(jié)點(diǎn)的重要環(huán)節(jié)。經(jīng)過(guò)多次仿真實(shí)驗(yàn)的結(jié)果得出，θ=125°，§=56°，γ=2時(shí)路徑平滑處理效果最好。路徑平滑處理的參考標(biāo)準(zhǔn)有3種：累計(jì)轉(zhuǎn)角、平均轉(zhuǎn)角和路徑長(zhǎng)度。采用3種算法生成的路徑圖如圖13所示。不同算法路徑平滑處理對(duì)比表如表3所示。

圖13 3種算法生成的路徑對(duì)比圖

表3 不同算法路徑平滑處理對(duì)比表

由表3可得，IVS算法相比于傳統(tǒng)VS算法和動(dòng)態(tài)A*算法，累計(jì)轉(zhuǎn)角分別降低了79.07%和25.63%，平均轉(zhuǎn)角分別降低了41.83%和29.14%，路徑長(zhǎng)度分別降低了61.76%和18.69%，采用IVS 算法規(guī)劃處的路徑更加平滑。

3.2.2 最短時(shí)間和最短路徑比較

為適應(yīng)更多障礙物環(huán)境，本文覆蓋率分別在8.6%、17.2%和28.4%的環(huán)境下仿真比較，如表4所示。

表4 不同算法下機(jī)器人路徑規(guī)劃的路徑長(zhǎng)度與時(shí)間對(duì)比表

由表4可得，IVS算法相比于傳統(tǒng)VS算法和動(dòng)態(tài)A*算法，在障礙物覆蓋率為8.6%時(shí)，路徑規(guī)劃的時(shí)間和路徑長(zhǎng)度分別減少了38.1%和18.75%；在障礙物覆蓋率為17.2%時(shí)，路徑規(guī)劃的時(shí)間和路徑長(zhǎng)度分別減少了33.33%和18.18%；在障礙物覆蓋率為28.4%時(shí)，路徑規(guī)劃的時(shí)間和路徑長(zhǎng)度分別減少了23.43%和10.91%。

3種算法的路徑規(guī)劃時(shí)間和路徑長(zhǎng)度對(duì)比圖如圖14和圖15所示。

圖14 3種算法的路徑規(guī)劃時(shí)間隨環(huán)境變化折線(xiàn)圖 圖15 3種算法的路徑規(guī)劃長(zhǎng)度隨環(huán)境變化折線(xiàn)圖

由圖14得出，隨著環(huán)境大小的增大，應(yīng)用本文算法使得時(shí)間增長(zhǎng)的幅度明顯低于其他兩種算法?？勺C，本文提出的算法更優(yōu)。

圖15可得，隨著環(huán)境大小的增加，應(yīng)用本文算法使得路徑長(zhǎng)度增長(zhǎng)的幅度更小，可證，IVS算法在機(jī)器人行走路程上短于采用其他兩種算法的多機(jī)器人行走路程，驗(yàn)證本文算法在路徑規(guī)劃上的搜索效率高和收斂速度快的特點(diǎn)。

3.3 動(dòng)態(tài)環(huán)境仿真

在仿真環(huán)境下，以(4,0)為起點(diǎn)，(35,45)為目標(biāo)點(diǎn)進(jìn)行動(dòng)態(tài)環(huán)境下得路徑規(guī)劃能力驗(yàn)證，在路徑中選取兩個(gè)局部子目標(biāo)點(diǎn)，設(shè)置為P1，P2，如圖16所示。本文設(shè)定障礙物的行駛速度小于機(jī)器人群的行駛速度。

圖16 局部子目標(biāo)點(diǎn)位置圖

圖16中，路徑被分為目標(biāo)點(diǎn)-P1；P1-P2和P2-目標(biāo)點(diǎn)。機(jī)器人群在P1-P2路徑內(nèi)，檢測(cè)到前方有障礙物出現(xiàn)(黑色方塊)，若按原路線(xiàn)行駛，將會(huì)在灰色方塊位置發(fā)生碰撞，如圖17所示。由于障礙物和機(jī)器人群的行動(dòng)軌跡交于一點(diǎn)，需要對(duì)P1-P2段路徑進(jìn)行局部規(guī)劃，如圖18所示。

圖17 機(jī)器人位于P1點(diǎn) 圖18 機(jī)器人位于P2點(diǎn)

P1-P2段原路徑用虛線(xiàn)表示，其他兩段路徑用實(shí)線(xiàn)表示。由圖17和圖18可證，應(yīng)用IVS算法可實(shí)現(xiàn)動(dòng)態(tài)環(huán)境下的避障，且避障能力充足。

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

(1)提出IVS算法，可規(guī)劃出最優(yōu)路徑，且具有搜索效率高、避障能力強(qiáng)的特點(diǎn)。

(2)采用IVS算法可以有效解決局部最小值和GNRON問(wèn)題，且實(shí)現(xiàn)實(shí)時(shí)避障。

(3)將IVS算法、傳統(tǒng)VS算法與動(dòng)態(tài) A*算法對(duì)比，驗(yàn)證應(yīng)用IVS算法得到的路徑更加平滑，能夠適應(yīng)不同障礙物密度環(huán)境，且有效避開(kāi)動(dòng)態(tài)障礙物。