雙層優(yōu)化A*算法與動(dòng)態(tài)窗口法的動(dòng)態(tài)路徑規(guī)劃

趙 偉，吳子英

西安理工大學(xué) 機(jī)械與精密儀器工程學(xué)院，西安710048

移動(dòng)機(jī)器人的路徑規(guī)劃是求解機(jī)器人起始點(diǎn)與目標(biāo)點(diǎn)之間的位置序列[1]。在動(dòng)態(tài)環(huán)境中，移動(dòng)機(jī)器人既要做到實(shí)時(shí)感知周?chē)h(huán)境無(wú)碰撞抵達(dá)目標(biāo)點(diǎn)，還要做到路徑質(zhì)量盡可能高。這就要求動(dòng)態(tài)環(huán)境下移動(dòng)機(jī)器人的路徑規(guī)劃需要結(jié)合全局路徑規(guī)劃與局部路徑規(guī)劃。移動(dòng)機(jī)器人的路徑質(zhì)量主要涉及規(guī)劃時(shí)間、路徑長(zhǎng)度、折轉(zhuǎn)次數(shù)、軌跡平滑度、障礙物距離等方面。其中規(guī)劃時(shí)間與路徑長(zhǎng)度影響機(jī)器人的移動(dòng)效率，折轉(zhuǎn)次數(shù)與軌跡平滑度影響機(jī)器人運(yùn)動(dòng)連貫性，障礙物距離決定機(jī)器人移動(dòng)的安全性。根據(jù)對(duì)環(huán)境信息的掌握情況，移動(dòng)機(jī)器人的路徑規(guī)劃可以分為全局路徑規(guī)劃與局部路徑規(guī)劃，常用的局部路徑規(guī)劃算法為動(dòng)態(tài)窗口法[2]、粒子群算法[3]、遺傳算法[4]等。常用的全局路徑規(guī)劃算法有A*算法[5-6]、Dijkstra算法[7]、蟻群算法[8]等。

動(dòng)態(tài)窗口法常用于移動(dòng)機(jī)器人的局部路徑規(guī)劃，通過(guò)對(duì)機(jī)器人速度空間的約束來(lái)實(shí)現(xiàn)對(duì)局部環(huán)境避障計(jì)算。傳統(tǒng)的動(dòng)態(tài)窗口法存在兩個(gè)缺陷：面對(duì)諸如“凹”形與“C”形障礙物的時(shí)候，容易陷入局部最優(yōu)無(wú)法繼續(xù)前行；規(guī)劃路徑長(zhǎng)度較長(zhǎng)，無(wú)法做到全局路徑最優(yōu)。針對(duì)動(dòng)態(tài)窗口法的兩大缺陷，程傳奇等人構(gòu)造了顧及全局的最優(yōu)路徑評(píng)價(jià)函數(shù)，使得動(dòng)態(tài)窗口法在選取軌跡時(shí)更加側(cè)重全局最優(yōu)[9]；卞永明等人定義了新的轉(zhuǎn)點(diǎn)評(píng)價(jià)子函數(shù)，解決了動(dòng)態(tài)窗口法容易陷入“C”形障礙物且容易繞行濃密障礙物的問(wèn)題[10]；華洪等人受到動(dòng)態(tài)窗口算法的啟發(fā)提出了一種多重A*算法下的動(dòng)態(tài)避障策略，在局部路徑規(guī)劃中，基于云的協(xié)作定位系統(tǒng)，使用卷積層和ConLSTM層的組合算法確定動(dòng)態(tài)避障與局部路徑的軌跡關(guān)系，使得局部避障的轉(zhuǎn)角與規(guī)劃時(shí)間大大降低[11]。A*算法是一種有效的全局路徑規(guī)劃算法，其顯著的優(yōu)點(diǎn)在于計(jì)算時(shí)間快，求得的路徑長(zhǎng)度較短，但同時(shí)也存在著折轉(zhuǎn)次數(shù)較多、路線平滑度較低等問(wèn)題。針對(duì)A*算法的問(wèn)題，勞彩蓮等人先從A*規(guī)劃出的軌跡獲得一系列點(diǎn)，通過(guò)斜率判斷運(yùn)動(dòng)方向，將相同運(yùn)動(dòng)方向的中間點(diǎn)刪除，只留下關(guān)鍵轉(zhuǎn)折點(diǎn)，減少了路徑折轉(zhuǎn)次數(shù)[12]。陳嬌等人通過(guò)從A*規(guī)劃軌跡的起點(diǎn)開(kāi)始做直線，依次連接路徑點(diǎn)，判斷直線是否通過(guò)障礙物來(lái)確定關(guān)鍵轉(zhuǎn)折點(diǎn)，刪除冗余點(diǎn)，降低了路徑折轉(zhuǎn)次數(shù)[13]?；眲?chuàng)鋒等人將傳統(tǒng)A*算法的3×3搜索鄰域擴(kuò)展到7×7搜索鄰域，優(yōu)化了路徑的搜索角度，減少了路徑轉(zhuǎn)折以及避免了過(guò)大轉(zhuǎn)折角的出現(xiàn)[14]，但是較多的搜索方向增加了運(yùn)行內(nèi)存的消耗。

針對(duì)動(dòng)態(tài)環(huán)境中移動(dòng)機(jī)器人既要?jiǎng)討B(tài)避障又要做到全局最優(yōu)的路徑規(guī)劃需求，本文提出了雙層優(yōu)化A*算法與動(dòng)態(tài)窗口法融合的動(dòng)態(tài)路徑規(guī)劃算法。針對(duì)傳統(tǒng)動(dòng)態(tài)窗口法面對(duì)“凹”形與“C”形障礙物容易陷入局部最優(yōu)無(wú)法前行，以及規(guī)劃路徑無(wú)法做到全局最優(yōu)的缺陷，通過(guò)引入雙層優(yōu)化A*算法來(lái)使動(dòng)態(tài)窗口法的規(guī)劃軌跡更貼近全局最優(yōu)。首先通過(guò)對(duì)傳統(tǒng)A*算法的一層優(yōu)化獲得關(guān)鍵轉(zhuǎn)折點(diǎn)路徑；再通過(guò)二層優(yōu)化獲得轉(zhuǎn)折點(diǎn)數(shù)量更少的全局路徑規(guī)劃；最后通過(guò)設(shè)計(jì)評(píng)價(jià)函數(shù)，引入“路徑評(píng)價(jià)子函數(shù)”將雙層優(yōu)化A*算法與動(dòng)態(tài)窗口法結(jié)合為融合算法。分別在靜態(tài)環(huán)境與動(dòng)態(tài)環(huán)境中對(duì)融合算法進(jìn)行仿真實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果驗(yàn)證了本文算法對(duì)移動(dòng)機(jī)器人運(yùn)動(dòng)動(dòng)態(tài)避障與全局最優(yōu)的雙重要求。

1 全局路徑規(guī)劃

1.1 傳統(tǒng)A*算法

傳統(tǒng)A*算法是一種有效的全局路徑規(guī)劃算法，其路徑規(guī)劃運(yùn)算過(guò)程可以通過(guò)以下流程來(lái)表示：首先初始化地圖信息，包括起點(diǎn)、終點(diǎn)、障礙物等，創(chuàng)建openlist與closelist 兩個(gè)節(jié)點(diǎn)列表，并將起點(diǎn)放到openlist 中，設(shè)定起點(diǎn)為父節(jié)點(diǎn)。建立一個(gè)循環(huán)，首先判斷終點(diǎn)是否在closelist 表中，若終點(diǎn)不在closelist 中，則計(jì)算當(dāng)前父節(jié)點(diǎn)周?chē)I(lǐng)域的八個(gè)子節(jié)點(diǎn)：若子節(jié)點(diǎn)為障礙物無(wú)法達(dá)到，則忽略它；若子節(jié)點(diǎn)已經(jīng)在closelist中，忽略它；若子節(jié)點(diǎn)可以到達(dá)且不在openlist 中，則將子節(jié)點(diǎn)添加到openlist 中；若子節(jié)點(diǎn)已經(jīng)在openlist 中，檢查其節(jié)點(diǎn)的代價(jià)值是否比當(dāng)前節(jié)點(diǎn)??；之后對(duì)openlist 表中的節(jié)點(diǎn)按照代價(jià)值大小排序，將代價(jià)值最小的子節(jié)點(diǎn)從openlist表中刪除并添加到closelist中，之后跳轉(zhuǎn)其上，設(shè)定該點(diǎn)為父節(jié)點(diǎn)，繼續(xù)判斷終點(diǎn)是否存在于closelist 中重復(fù)上述循環(huán)。若openlist表為空表，則表示無(wú)路徑到達(dá)終點(diǎn)；若終點(diǎn)存在于closelist中，則表示找到路徑并結(jié)束循環(huán)，將closelist表中的坐標(biāo)點(diǎn)按照起點(diǎn)到終點(diǎn)的順序依次連接得到所求路徑。

傳統(tǒng)的A*算法的代價(jià)值由兩部分組成：子節(jié)點(diǎn)到起點(diǎn)的累積代價(jià)與子節(jié)點(diǎn)到終點(diǎn)的累積代價(jià)[15]。A*算法的代價(jià)值函數(shù)可以表示為：

式中，F(xiàn)(n)表示當(dāng)前子節(jié)點(diǎn)的總代價(jià)；G(n)表示子節(jié)點(diǎn)到起點(diǎn)的累積代價(jià)；H(n)表示子節(jié)點(diǎn)到終點(diǎn)的累積代價(jià)。

本文采用雙維靜態(tài)地圖模型，因此任意兩點(diǎn)(x1,y1)、(x2,y2)之間的代價(jià)值d采用歐式距離來(lái)計(jì)算：

傳統(tǒng)A*算法在求解障礙物較多的地圖模型時(shí)，得到的路徑折轉(zhuǎn)點(diǎn)較多，頻繁的折轉(zhuǎn)使得機(jī)器人在實(shí)際尋路流程中經(jīng)常出現(xiàn)卡頓、卡死等狀況，較多的折轉(zhuǎn)點(diǎn)還使得機(jī)器人每次只能行駛較短的一段直線路段，無(wú)法用較大的加速通過(guò)，變相地增加了尋路耗時(shí)。為了解決A*算法轉(zhuǎn)折點(diǎn)較多、路徑長(zhǎng)度過(guò)長(zhǎng)等問(wèn)題，本文采用雙層全局優(yōu)化。首先通過(guò)第一層優(yōu)化來(lái)提取關(guān)鍵折轉(zhuǎn)點(diǎn)，刪除大量的冗余點(diǎn)；在第一層優(yōu)化的基礎(chǔ)上進(jìn)行第二層優(yōu)化，將路徑轉(zhuǎn)折點(diǎn)降到最低，得到折轉(zhuǎn)點(diǎn)數(shù)量最小的一條優(yōu)化路徑。如圖1 所示為傳統(tǒng)A*算法在場(chǎng)景1 中的尋路求解結(jié)果，其中黃色方格代表起點(diǎn)，藍(lán)色方格代表終點(diǎn)，黑色方格代表邊界與障礙物，紅色方格代表路徑轉(zhuǎn)折點(diǎn)，用黑色線段依次連接起點(diǎn)、路徑點(diǎn)、終點(diǎn)得到算法求得的路徑規(guī)劃。

圖1 傳統(tǒng)A*算法全局規(guī)劃Fig.1 Global planning of traditional A* algorithm

1.2 一層全局優(yōu)化

在傳統(tǒng)A*算法求得全局路徑規(guī)劃之后，首先提取路徑轉(zhuǎn)折拐點(diǎn)，對(duì)路徑轉(zhuǎn)折拐點(diǎn)進(jìn)行篩選剔除，刪除冗余拐點(diǎn)，具體篩選剔除策略如圖2所示。

圖2 一層優(yōu)化A*算法示意圖Fig.2 Schematic diagram of one-level optimization A* algorithm

（1）將傳統(tǒng)A*算法求得的路徑點(diǎn)坐標(biāo)Nodei(i=1,2,…)按照從起點(diǎn)到終點(diǎn)的順序依次存儲(chǔ)在坐標(biāo)集path中；將地圖的邊界與障礙物等不可到達(dá)區(qū)域存儲(chǔ)在集合Obstacle中。

（2）將起點(diǎn)與終點(diǎn)放置到坐標(biāo)集Fpo（First path optimization）中，并設(shè)定起點(diǎn)為當(dāng)前開(kāi)始點(diǎn)NodeS；之后按順序依次計(jì)算路徑點(diǎn)Nodei與開(kāi)始點(diǎn)NodeS兩點(diǎn)連線的斜率ki。

（3）若ki ＋1=ki，則表示點(diǎn)NodeS、Nodei與Nodei ＋1三點(diǎn)共線，忽略該路徑點(diǎn)Nodei，繼續(xù)計(jì)算ki ＋2；若ki ＋2=ki，則忽略路徑點(diǎn)Nodei ＋1，繼續(xù)計(jì)算ki ＋3…。

（4）若存在ki ＋j≠ki，其中j=1,2,…,則對(duì)路徑點(diǎn)Nodei ＋j進(jìn)行條件判斷：若從NodeS指向Nodei ＋j的連線不經(jīng)過(guò)Obstacle區(qū)域，則忽略該路徑點(diǎn)Nodei ＋j，繼續(xù)按照步驟（3）往下循環(huán)計(jì)算ki ＋j ＋1…；若從NodeS指向Nodei ＋j的連線經(jīng)過(guò)Obstacle區(qū)域，則路徑點(diǎn)Nodei ＋j－1為關(guān)鍵轉(zhuǎn)折點(diǎn)，將路徑點(diǎn)Nodei ＋j－1放置到坐標(biāo)集Fpo中，并將路徑點(diǎn)Nodei ＋j設(shè)置為新的當(dāng)前開(kāi)始點(diǎn)NodeS，將路徑點(diǎn)Nodei ＋j ＋1重置為Nodei，重復(fù)（2）—（3）—（4）的循環(huán)過(guò)程，直到抵到終點(diǎn)結(jié)束。

（5）將坐標(biāo)集Fpo中的路徑點(diǎn)按照從起點(diǎn)到終點(diǎn)的順序依次連接，得到第一層全局優(yōu)化的路徑規(guī)劃結(jié)果。

按照?qǐng)鼍? 的柵格地圖對(duì)傳統(tǒng)的A*算法進(jìn)行第一層路徑優(yōu)化，其優(yōu)化結(jié)果如圖3所示。通過(guò)對(duì)比可以看出，傳統(tǒng)A*算法求得的路徑轉(zhuǎn)折點(diǎn)數(shù)為7個(gè)，優(yōu)化后的路徑轉(zhuǎn)折點(diǎn)為3個(gè)，路徑轉(zhuǎn)折點(diǎn)數(shù)量有效地減少。

圖3 一層優(yōu)化A*算法路徑規(guī)劃Fig.3 Path planning of one-level optimization A* algorithm

1.3 二層全局優(yōu)化

通過(guò)第一層的全局優(yōu)化，A*算法求得的路徑中的拐點(diǎn)數(shù)有效地減少了，提高了路徑的運(yùn)動(dòng)連貫性且減短了路徑總長(zhǎng)。雙層全局優(yōu)化是在第一層全局優(yōu)化的基礎(chǔ)上進(jìn)一步的優(yōu)化，通過(guò)第二層的全局優(yōu)化，可以將A*算法求得的路徑轉(zhuǎn)折點(diǎn)進(jìn)一步降低。

雙層全局優(yōu)化的策略是將一層優(yōu)化后的路徑按照轉(zhuǎn)折點(diǎn)分成若干個(gè)相連的路徑段，通過(guò)判斷某一路徑段兩端的路徑段能否在非障礙物相交來(lái)優(yōu)化路徑，其具體策略如圖4、圖5所示。

圖4 二層優(yōu)化A*算法示意圖Fig.4 Schematic diagram of two-level optimization A* algorithm

圖5 雙層優(yōu)化A*算法示意圖Fig.5 Schematic diagram of bilevel optimization A* algorithm

（1）將一層優(yōu)化后的路徑按照從起點(diǎn)NodeS到終點(diǎn)NodeE的順序，以Fpo集合中的路徑轉(zhuǎn)折點(diǎn)Nodei為分割點(diǎn)，分割為若干個(gè)首位相連的路徑段Segmi(i=1,2,…)創(chuàng)建路徑坐標(biāo)集Spo（Second path optimization），將起點(diǎn)與終點(diǎn)放置到Spo中。

（2）按順序依次選定路徑段Segmi兩端的路徑段Segmi－1與Segmi ＋1，若Segmi－1不存在，則說(shuō)明路徑段Segmi的左端為起點(diǎn)，忽略Segmi，繼續(xù)選定Segmi ＋1兩端的路徑段；若Segmi ＋1不存在，則說(shuō)明Segmi右端點(diǎn)為終點(diǎn)，計(jì)算結(jié)束。

（3）將路徑段Segmi－1與路徑段Segmi ＋1無(wú)限延長(zhǎng)，若延長(zhǎng)后兩直線無(wú)交點(diǎn)，則忽略路徑段Segmi，將Nodei添加到集合Spo中，之后繼續(xù)計(jì)算Segmi ＋1的兩端直線是否相交；若延長(zhǎng)后兩直線有交點(diǎn)，設(shè)其交點(diǎn)為NodeC，并對(duì)交點(diǎn)NodeC進(jìn)行條件判斷。

（4）若交點(diǎn)NodeC存在于集合Obstacle中，即交點(diǎn)為障礙點(diǎn)，忽略路徑段Segmi，將Nodei添加到集合Spo中，繼續(xù)計(jì)算Segmi ＋1；若交點(diǎn)NodeC不存在于集合Obstacle中，且從交點(diǎn)NodeC到Segmi兩端點(diǎn)Nodei－1與Nodei的連線均不通過(guò)Obstacle區(qū)域，則認(rèn)為路徑段Segmi為可刪除路徑段，將NodeC添加到集合Spo中，之后選定計(jì)算Segmi ＋2…,重復(fù)（2）—（3）—（4）的循環(huán)過(guò)程，直到抵到終點(diǎn)結(jié)束。

（5）將坐標(biāo)集Spo中的路徑點(diǎn)按照從起點(diǎn)到終點(diǎn)的順序依次連接，得到雙層全局優(yōu)化的路徑規(guī)劃結(jié)果。

按照?qǐng)鼍? 的柵格地圖對(duì)傳統(tǒng)的A*算法進(jìn)行雙層路徑優(yōu)化，其優(yōu)化結(jié)果如圖6 所示。通過(guò)對(duì)比可以看出，傳統(tǒng)A*算法求得的路徑轉(zhuǎn)折點(diǎn)數(shù)為7個(gè)，第一層優(yōu)化后轉(zhuǎn)折點(diǎn)數(shù)為3個(gè)，雙層優(yōu)化后的路徑轉(zhuǎn)折點(diǎn)為2個(gè)，路徑轉(zhuǎn)折點(diǎn)數(shù)量進(jìn)一步地減少。

圖6 雙層優(yōu)化A*算法路徑規(guī)劃Fig.6 Path planning of bilevel optimization A* algorithm

2 局部路徑規(guī)劃

動(dòng)態(tài)窗口法常用于移動(dòng)機(jī)器人的局部路徑規(guī)劃，如圖7所示。傳統(tǒng)的動(dòng)態(tài)窗口法存在兩個(gè)缺陷：面對(duì)諸如“凹”形與“C”形障礙物時(shí)，容易陷入局部最優(yōu)無(wú)法繼續(xù)前行；規(guī)劃路徑長(zhǎng)度較長(zhǎng)，無(wú)法做到全局路徑最優(yōu)。

圖7 傳統(tǒng)動(dòng)態(tài)窗口法路徑規(guī)劃Fig.7 Path planning of traditional dynamic window method

針對(duì)傳統(tǒng)動(dòng)態(tài)窗口法存在的問(wèn)題，需要通過(guò)引入全局規(guī)劃來(lái)引導(dǎo)機(jī)器人走出陷入局部最優(yōu)的“凹”形和“C”形區(qū)域且更加接近全局最優(yōu)。因此本文引入雙層優(yōu)化后的A*算法來(lái)與動(dòng)態(tài)窗口算法相融合。在雙層優(yōu)化A*算法求得全局規(guī)劃后，通過(guò)動(dòng)態(tài)窗口法來(lái)實(shí)現(xiàn)局部路徑規(guī)劃。雙層優(yōu)化后的A*算法可以求得一條節(jié)點(diǎn)較少、總路程較短的全局路徑規(guī)劃結(jié)果。這條路徑雖然已經(jīng)比傳統(tǒng)A*算法有所提升，但依然存在一些問(wèn)題：在拐點(diǎn)處路徑距離障礙物較近。距離障礙物較近的問(wèn)題，使得機(jī)器人在實(shí)際運(yùn)行中拐點(diǎn)處容易與障礙物發(fā)生碰撞，因此需要在動(dòng)態(tài)窗口法中對(duì)障礙物距離進(jìn)行約束來(lái)保證障礙物距離。動(dòng)態(tài)窗口法的避障策略其實(shí)是機(jī)器人運(yùn)動(dòng)的速度空間的帶約束的優(yōu)化問(wèn)題，通過(guò)捕捉預(yù)測(cè)機(jī)器人的實(shí)時(shí)運(yùn)動(dòng)狀態(tài)來(lái)預(yù)測(cè)行進(jìn)軌跡，再通過(guò)評(píng)價(jià)函數(shù)來(lái)選取一條總代價(jià)最小的軌跡作為實(shí)際路徑。

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

動(dòng)態(tài)窗口法實(shí)時(shí)預(yù)測(cè)時(shí)間間隔Δt內(nèi)機(jī)器人速度空間與狀態(tài)空間的變化，根據(jù)評(píng)價(jià)函數(shù)來(lái)確定最優(yōu)軌跡[16]。

移動(dòng)機(jī)器人的速度空間V=(νt,ωt)包含機(jī)器人的線速度νt與角速度ωt，機(jī)器人的狀態(tài)空間(xt,yt,yawt,νt,ωt)包含機(jī)器人的橫縱坐標(biāo)(xt,yt)、朝向yawt、線速度νt與角速度ωt，移動(dòng)機(jī)器人運(yùn)動(dòng)變化時(shí)間間隔為Δt。移動(dòng)機(jī)器人的運(yùn)動(dòng)模型可以表達(dá)為：

2.2 速度采樣

機(jī)器人的速度空間內(nèi)存在無(wú)窮組速度對(duì)(ν,ω)，但是在對(duì)機(jī)器人進(jìn)行實(shí)時(shí)速度采樣時(shí)，機(jī)器人在速度空間受到多方面的約束，其約束主要如下：

（1）最大、最小速度約束

移動(dòng)機(jī)器人的線速度與角速度是有限的，存在最大值與最小值，速度空間的最大值與最小值構(gòu)成動(dòng)態(tài)窗口算法的最大范圍Vs：

（2）最大加、減速度約束

移動(dòng)機(jī)器人的加速度受制于電機(jī)的輸出扭矩，存在最大加、減速度，即在速度采樣間隔Δt內(nèi)，速度的變化是有限的。在采樣間隔Δt內(nèi)允許的速度變化范圍Va：

（3）制動(dòng)距離約束

機(jī)器人的移動(dòng)過(guò)程中不允許與障礙物發(fā)生干涉碰撞，這就要求在最大減速度下，機(jī)器人在當(dāng)前速度狀態(tài)下抵到最近障礙物時(shí)速度必須可以減為0。安全制動(dòng)條件下的速度空間Vd：

機(jī)器人的速度需要同時(shí)滿足三個(gè)約束，即機(jī)器人允許的速度空間V為三個(gè)約束空間的交集：

將速度空間V按照速度的最大分辨率進(jìn)行離散化，得到速度的一系列離散采樣點(diǎn)。將每個(gè)速度采樣點(diǎn)代入評(píng)價(jià)函數(shù)中，根據(jù)評(píng)價(jià)函數(shù)選取最優(yōu)的軌跡點(diǎn)作為下一時(shí)刻的運(yùn)動(dòng)軌跡。

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

通過(guò)設(shè)計(jì)評(píng)價(jià)函數(shù)，在速度空間選取一條代價(jià)最小的軌跡作為移動(dòng)機(jī)器人的實(shí)際軌跡[16]，既要保證路徑與障礙物保持一定的距離，還要做到路徑盡可能地接近雙層優(yōu)化A*算法求得的全局路徑，即要求完成動(dòng)態(tài)避障的同時(shí)可以快速逼近終點(diǎn)。評(píng)價(jià)函數(shù)Cost(ν,ω) 的定義為：

式中，α、β、γ、σ為各項(xiàng)加權(quán)系數(shù)；Heading(ν,ω)為方位角評(píng)價(jià)子函數(shù)，當(dāng)前機(jī)器人的運(yùn)動(dòng)朝向角度與目標(biāo)點(diǎn)角度之差；Dist(ν,ω)為障礙物距離評(píng)價(jià)子函數(shù)，計(jì)算機(jī)器人預(yù)測(cè)點(diǎn)位置與最近障礙物之間的距離；Path(ν,ω)為路徑評(píng)價(jià)子函數(shù)，計(jì)算動(dòng)態(tài)窗口法預(yù)測(cè)的Δt之后的預(yù)測(cè)點(diǎn)與雙層優(yōu)化A*算法求得全局軌跡的最小距離；Goal(ν,ω)為代價(jià)評(píng)價(jià)子函數(shù)，計(jì)算預(yù)測(cè)點(diǎn)到目標(biāo)終點(diǎn)的代價(jià)值。

當(dāng)評(píng)價(jià)函數(shù)Cost(ν,ω)最小時(shí)，即認(rèn)為預(yù)測(cè)點(diǎn)為最優(yōu)的軌跡點(diǎn)。各子函數(shù)的具體函數(shù)表達(dá)式如下：

各項(xiàng)加權(quán)系數(shù)α、β、γ、σ：加權(quán)系數(shù)決定了動(dòng)態(tài)窗口法在選擇最優(yōu)軌跡時(shí)不同子函數(shù)的優(yōu)先級(jí)程度。本算法的四個(gè)子函數(shù)中，加權(quán)系數(shù)的優(yōu)先級(jí)最高的為β，即優(yōu)先考慮障礙物距離。雙層的A*優(yōu)化算法雖然減少了轉(zhuǎn)折點(diǎn)的數(shù)量，但是在某些地圖場(chǎng)景中會(huì)增加路徑長(zhǎng)度，因此可以通過(guò)加權(quán)系數(shù)的關(guān)系對(duì)這一問(wèn)題進(jìn)行修復(fù)。令γ ＜σ，則算法會(huì)優(yōu)先選擇總代價(jià)較小的路徑。

方位角評(píng)價(jià)子函數(shù)Heading(ν,ω)：

式中，θg為機(jī)器人指向目標(biāo)點(diǎn)與水平線的夾角；θt為機(jī)器人行進(jìn)方向與水平線的夾角。θg與θt的差值越小說(shuō)明與目標(biāo)點(diǎn)的方位差越小。

障礙物距離評(píng)價(jià)子函數(shù)Dist(ν,ω)：

其中，R為機(jī)器人的實(shí)際底盤(pán)半徑，由于大部分機(jī)器人的底盤(pán)不完全是規(guī)則的圓形，且機(jī)器人的移動(dòng)與定位存在一定的誤差，為了保證機(jī)器人移動(dòng)的安全性，在機(jī)器人底盤(pán)半徑R基礎(chǔ)上增加0.2R的安全距離來(lái)保證其運(yùn)行的安全性。do為機(jī)器人到最近障礙物的距離。(xo,yo)為障礙物的坐標(biāo)。當(dāng)機(jī)器人到障礙物的距離大于機(jī)器人底盤(pán)半徑R時(shí)一般不會(huì)發(fā)生碰撞，此時(shí)子函數(shù)Dist(ν,ω)值為0，即不發(fā)生碰撞時(shí)不考慮障礙物距離評(píng)價(jià)；當(dāng)機(jī)器人到障礙物的距離小于或等于R時(shí)，機(jī)器人發(fā)生碰撞的可能性較大，此時(shí)Dist(ν,ω)值隨著距離的減小而增大，即距離越近發(fā)生碰撞的影響越大。

路徑評(píng)價(jià)子函數(shù)Path(ν,ω)：

其中，(xa,ya)為雙層優(yōu)化A*算法求得的全局規(guī)劃路徑的節(jié)點(diǎn)坐標(biāo)。Path(ν,ω)越小說(shuō)明越接近全局路徑規(guī)劃，反之則越偏離全局路徑規(guī)劃。

代價(jià)評(píng)價(jià)子函數(shù)Goal(ν,ω)：

其中，(xg,yg)為目標(biāo)終點(diǎn)的坐標(biāo)。Goal(ν,ω)越小，機(jī)器人移動(dòng)到目標(biāo)終點(diǎn)所需的代價(jià)越小，機(jī)器人越靠近目標(biāo)終點(diǎn)。

3 算法融合

移動(dòng)機(jī)器人在獲取到全局靜態(tài)環(huán)境信息之后，首先通過(guò)傳統(tǒng)的A*算法求得全局路徑規(guī)劃，在此全局規(guī)劃的基礎(chǔ)上進(jìn)行雙層路徑優(yōu)化。首先通過(guò)第一層路徑優(yōu)化降低轉(zhuǎn)折點(diǎn)數(shù)量，減短路徑長(zhǎng)度，再進(jìn)一步進(jìn)行二層優(yōu)化，使得路徑的轉(zhuǎn)折點(diǎn)進(jìn)一步降低，得到最終的全局路徑規(guī)劃。全局路徑規(guī)劃完成后開(kāi)始對(duì)機(jī)器人的速度進(jìn)行采樣，利用動(dòng)態(tài)窗口法進(jìn)行實(shí)時(shí)局部路徑規(guī)劃，達(dá)到局部路徑最優(yōu)與動(dòng)態(tài)避障。融合雙層優(yōu)化的A*全局路徑規(guī)劃與動(dòng)態(tài)窗口法的局部路徑規(guī)劃，保證了機(jī)器人路徑規(guī)劃的全局最優(yōu)性。用融合算法對(duì)上述地圖場(chǎng)景進(jìn)行路徑規(guī)劃的結(jié)果如圖8所示。

圖8 融合算法路徑規(guī)劃Fig.8 Path planning of fusion algorithm

融合算法的具體流程如圖9所示。

圖9 融合算法流程圖Fig.9 Flow chart of fusion algorithm

4 仿真實(shí)驗(yàn)

4.1 仿真環(huán)境描述

為了驗(yàn)證融合算法的有效性，本文采用Python3.6為實(shí)驗(yàn)仿真平臺(tái)。在Python3.6中進(jìn)行四種算法的路徑點(diǎn)計(jì)算，將輸出的路徑點(diǎn)坐標(biāo)導(dǎo)入到Matlab中進(jìn)行繪圖與路徑長(zhǎng)度計(jì)算。設(shè)置四種不同的二維靜態(tài)柵格地圖，其中地圖1、2 為非對(duì)稱(chēng)的隨機(jī)分布地圖，地圖3、4 為中心對(duì)稱(chēng)式分布地圖，在四組地圖中分別對(duì)傳統(tǒng)A*算法、一層優(yōu)化A*算法、二層優(yōu)化A*算法以及融合算法進(jìn)行仿真實(shí)驗(yàn)，對(duì)四種算法求得路徑進(jìn)行數(shù)據(jù)統(tǒng)計(jì)與比較。設(shè)置動(dòng)態(tài)障礙物的動(dòng)態(tài)地圖，在動(dòng)態(tài)環(huán)境中對(duì)雙層優(yōu)化A*算法、傳統(tǒng)動(dòng)態(tài)窗口法與融合算法進(jìn)行仿真實(shí)驗(yàn)與數(shù)據(jù)統(tǒng)計(jì)分析。

仿真環(huán)境為二維靜態(tài)柵格地圖，由25×25（不包含邊界）個(gè)相同大小的單元格組成。其中黑色方格代表障礙物，白色方格代表可行路徑點(diǎn)，黃色方格代表移動(dòng)機(jī)器人起點(diǎn)，紅色方格代表路徑轉(zhuǎn)折點(diǎn)，藍(lán)色方格代表目標(biāo)終點(diǎn)，黑色連續(xù)線條代表算法求得的路徑規(guī)劃，地圖模型采用相同的起點(diǎn)（1，24）與終點(diǎn)（24，1）。

融合算法中的動(dòng)態(tài)窗口法的相關(guān)參數(shù)設(shè)置為：起始坐標(biāo)（1，24），目標(biāo)點(diǎn)（24，1），初始方位角－π/2，初始線速度0 m/s，初始角速度0 rad/s，最大線速度1 m/s，最大角速度20 rad/s，最大線加速度0.2 m/s2，最大角加速度50 rad/s，線速度分辨率0.01 m/s，角速度分辨率1 rad/s，時(shí)間分辨率0.1 s，預(yù)測(cè)周期間隔2 s，動(dòng)態(tài)障礙物起始坐標(biāo)為[13，1]，按照3.6 m/s的速度沿Y軸正方向勻速移動(dòng)。

4.2 仿真結(jié)果

對(duì)傳統(tǒng)A*算法、一層優(yōu)化A*算法、二層優(yōu)化A*算法以及融合算法在四種不同的二維靜態(tài)網(wǎng)格地圖上進(jìn)行仿真實(shí)驗(yàn)，其仿真結(jié)果如圖10、圖11、圖12、圖13所示。

圖10 場(chǎng)景1仿真結(jié)果Fig.10 Simulation results of scenario 1

圖11 場(chǎng)景2仿真結(jié)果Fig.11 Simulation results of scenario 2

圖12 場(chǎng)景3仿真結(jié)果Fig.12 Simulation results of scenario 3

圖13 場(chǎng)景4仿真結(jié)果Fig.13 Simulation results of scenario 4

對(duì)雙層優(yōu)化A*算法、傳統(tǒng)動(dòng)態(tài)窗口法及融合算法在二維動(dòng)態(tài)網(wǎng)格地圖中進(jìn)行仿真實(shí)驗(yàn)，并在路徑規(guī)劃過(guò)程中捕捉四個(gè)時(shí)刻的規(guī)劃狀態(tài)，其仿真結(jié)果如圖14、圖15、圖16所示。

圖14 雙層優(yōu)化A*算法動(dòng)態(tài)仿真結(jié)果Fig.14 Dynamic simulation results of bilevel optimization A* algorithm

圖15 傳統(tǒng)動(dòng)態(tài)窗口法動(dòng)態(tài)仿真結(jié)果Fig.15 Dynamic simulation results of traditional dynamic window method

圖16 融合算法動(dòng)態(tài)仿真結(jié)果仿真結(jié)果Fig.16 Dynamic simulation results of fusion algorithm

對(duì)上述四組二維靜態(tài)環(huán)境地圖仿真結(jié)果進(jìn)行數(shù)據(jù)統(tǒng)計(jì)如下：

由表1可以看出：雙層優(yōu)化A*算法提供了較高質(zhì)量的全局路徑規(guī)劃，其優(yōu)化主要表現(xiàn)在路徑轉(zhuǎn)折點(diǎn)數(shù)量與路徑長(zhǎng)度上。在四個(gè)不同場(chǎng)景的地圖中，一層優(yōu)化A*算法轉(zhuǎn)折點(diǎn)平均優(yōu)化率為34.3%，雙層A*優(yōu)化算法路徑轉(zhuǎn)折點(diǎn)平均優(yōu)化率為62.4%，優(yōu)化幅度較為明顯。在路徑長(zhǎng)度優(yōu)化比例上優(yōu)化幅度較小，其中一層優(yōu)化A*算法平均優(yōu)化率為3.1%，雙層優(yōu)化A*算法平均優(yōu)化率為－3.4%，融合算法平均優(yōu)化率為1.8%。可以看出雙層優(yōu)化A*算法在大幅降低路徑轉(zhuǎn)折點(diǎn)數(shù)量的同時(shí)，使得路徑長(zhǎng)度小幅度變長(zhǎng)，再通過(guò)融合算法的加權(quán)系數(shù)來(lái)修正路徑長(zhǎng)度，最后獲得的融合算法在較少轉(zhuǎn)折的基礎(chǔ)上路徑長(zhǎng)度再次減少。

表1 四組靜態(tài)地圖仿真結(jié)果Table 1 Four groups of map simulation results

由動(dòng)態(tài)仿真結(jié)果與表2可以看出：雙層優(yōu)化A*算法提供了轉(zhuǎn)折點(diǎn)較少、路徑長(zhǎng)度較短的高質(zhì)量全局路徑規(guī)劃，但是在t3時(shí)刻，機(jī)器人與障礙物的距離為－0.43 m，發(fā)生碰撞，碰撞點(diǎn)之后的路徑規(guī)劃也失去了參考價(jià)值；傳統(tǒng)動(dòng)態(tài)窗口法在t1時(shí)刻遇到第一個(gè)“L”形障礙物時(shí)，因?yàn)榫植孔顑?yōu)而選擇了較遠(yuǎn)的路程，同時(shí)在t4時(shí)刻面對(duì)“凹”形障礙物陷入局部最優(yōu)無(wú)法繼續(xù)前行抵達(dá)的終點(diǎn)；融合算法在t1時(shí)刻面對(duì)“L”形障礙物時(shí)選擇了更加貼近雙層優(yōu)化A*算法的最優(yōu)全局路徑規(guī)劃，在t2時(shí)刻融合算法的動(dòng)態(tài)窗口檢測(cè)到即將可能發(fā)生碰撞的動(dòng)態(tài)障礙物，在t3時(shí)刻機(jī)器人進(jìn)行了動(dòng)態(tài)避障，在t4時(shí)刻機(jī)器人完成動(dòng)態(tài)避障之后繼續(xù)按照雙層優(yōu)化A*算法的最優(yōu)全局規(guī)劃抵到終點(diǎn)?？梢钥闯鋈诤纤惴鎸?duì)動(dòng)態(tài)障礙物時(shí)，可以保證機(jī)器人與障礙物的安全距離并且實(shí)現(xiàn)實(shí)時(shí)動(dòng)態(tài)避障，同時(shí)路徑規(guī)劃貼近全局最優(yōu)，在完成動(dòng)態(tài)避障的同時(shí)僅比雙層優(yōu)化A*算法的最優(yōu)全局規(guī)劃多出了2%的路徑長(zhǎng)度，滿足了移動(dòng)機(jī)器人在動(dòng)態(tài)環(huán)境中既要實(shí)現(xiàn)動(dòng)態(tài)避障抵達(dá)終點(diǎn)，又要做到全局最優(yōu)的路徑規(guī)劃需求。

表2 動(dòng)態(tài)地圖仿真結(jié)果Table 2 Dynamic map simulation results

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

動(dòng)態(tài)環(huán)境中機(jī)器人既要安全規(guī)避動(dòng)態(tài)障礙物抵到終點(diǎn)，同時(shí)又需要全局規(guī)劃路徑質(zhì)量盡可能高，針對(duì)移動(dòng)機(jī)器人在動(dòng)態(tài)環(huán)境中的路徑規(guī)劃需求，本文進(jìn)行了以下研究：

（1）提出了雙層優(yōu)化A*算法與動(dòng)態(tài)窗口法結(jié)合的移動(dòng)機(jī)器人路徑規(guī)劃算法，實(shí)現(xiàn)了動(dòng)態(tài)避障與全局最優(yōu)的雙重要求。

（2）一層優(yōu)化A*算法采用節(jié)點(diǎn)斜率計(jì)算與篩除，有效減少了路徑轉(zhuǎn)折次數(shù)并小幅度降低了路徑長(zhǎng)度。

（3）二層優(yōu)化A*算法通過(guò)延長(zhǎng)路徑獲得交點(diǎn)的方法，大幅度降低了路徑轉(zhuǎn)折次數(shù)，將路徑轉(zhuǎn)折次數(shù)降到了最低，但是小幅度增加了路徑長(zhǎng)度。

（4）融合算法在動(dòng)態(tài)窗口法中通過(guò)引入“路徑規(guī)劃子函數(shù)”與設(shè)置障礙物安全距離，使得移動(dòng)機(jī)器人在面對(duì)諸“C”“凹”形障礙物時(shí)選擇全局最優(yōu)路徑，同時(shí)安全距離保證了機(jī)器人移動(dòng)的安全性。