熊光明, 于全富, 胡秀中, 周子杰, 許佳慧

(北京理工大學(xué) 機(jī)械與車輛學(xué)院，北京 100081)

0 引言

在美國(guó)國(guó)防高級(jí)計(jì)劃研究局(DARPA)的SUBT挑戰(zhàn)賽[1]和我國(guó)的跨越險(xiǎn)阻2021 地面無人系統(tǒng)挑戰(zhàn)賽[2]中，都首次要求無人平臺(tái)在多層建筑物內(nèi)完成自主任務(wù)。不同于一般單層環(huán)境，多層建筑具有多個(gè)相互連接的離散平面的空間特征。雖然許多應(yīng)用于單層連續(xù)平面環(huán)境的二維和三維運(yùn)動(dòng)規(guī)劃算法已經(jīng)被提出，但是無人平臺(tái)在多個(gè)離散平面之間的運(yùn)動(dòng)規(guī)劃問題仍有待解決。

運(yùn)動(dòng)規(guī)劃算法一般分為2D、2.5D 和3D 共 3 種類型。2D 算法是在平面地圖上進(jìn)行規(guī)劃的傳統(tǒng)方法。2.5D 算法在2D 算法的基礎(chǔ)上將3D 地圖映射到2D 平面上并進(jìn)行規(guī)劃，考慮到更多環(huán)境信息的同時(shí)保證了算法的計(jì)算效率。Carsten 等[3]將3D 環(huán)境投影到平面進(jìn)行柵格化，在每一個(gè)柵格上存儲(chǔ)一個(gè)地形對(duì)應(yīng)的高度值，在全局規(guī)劃算法中融合障礙物的高度信息進(jìn)行路徑規(guī)劃。Ishigami 等[4]將路徑長(zhǎng)度、俯仰和側(cè)傾程度等4 項(xiàng)指標(biāo)加權(quán)和作為Dijkstra 算法生成多條候選路徑，但是沒有考慮運(yùn)動(dòng)學(xué)約束。Lacroix 等[5]利用A*算法在搜索路徑時(shí)，采用圓弧路徑連接節(jié)點(diǎn)，得到相對(duì)平滑的路徑，但是無法保證路徑的曲率連續(xù)。Howard 等[6]利用運(yùn)動(dòng)學(xué)模型前向仿真生成基元路徑來進(jìn)一步提高路徑的連續(xù)性。在單層連續(xù)平面環(huán)境下，2D 和2.5D 規(guī)劃算法可以減少計(jì)算的復(fù)雜度，并同時(shí)保證規(guī)劃結(jié)果具有一定的準(zhǔn)確性。然而，2D 和2.5D規(guī)劃算法并不能直接應(yīng)用在多層建筑物環(huán)境中。

相對(duì)于2D 和2.5D 算法，3D 規(guī)劃算法可以在單層連續(xù)曲面上進(jìn)行規(guī)劃，但是由于實(shí)際三維環(huán)境的空間復(fù)雜性，難以應(yīng)用在多層建筑物等大規(guī)模場(chǎng)景。Colas 等[7]和Menna 等[8]將室內(nèi)環(huán)境使用三維柵格地圖建模，考慮到距離、坡度、凸起度作為代價(jià)值，最終在仿真環(huán)境下得到了樓梯處的一段可通行路徑。但是仿真環(huán)境只是一段樓梯區(qū)域，沒有包括各樓層環(huán)境信息，空間復(fù)雜度比較小。由于3D規(guī)劃算法的復(fù)雜度大于nlog(n)，n為算法處理的節(jié)點(diǎn)數(shù)量，隨著地圖空間的增大，節(jié)點(diǎn)數(shù)量的增多會(huì)引起路徑搜索時(shí)間的非線性激增[9]。此外，三維規(guī)劃常用的地圖表示方式主要為多邊形網(wǎng)格化曲面和三維柵格地圖。多邊形網(wǎng)格化曲面要經(jīng)過復(fù)雜的擬合重構(gòu)步驟，難以用于大規(guī)模地圖，而三維柵格地圖需要對(duì)大量的稠密點(diǎn)云信息進(jìn)行二次重構(gòu)才能用于規(guī)劃。加之，3D 規(guī)劃算法本身的復(fù)雜度以及需要在整個(gè)路徑長(zhǎng)度上進(jìn)行三維碰撞檢測(cè)。因此，3D 規(guī)劃算法在多層建筑物環(huán)境中需要占用大量的計(jì)算資源，難以實(shí)際應(yīng)用。

除此之外，對(duì)多層建筑物內(nèi)運(yùn)動(dòng)規(guī)劃問題進(jìn)行降維處理的方案也被提出。Zhang 等[10]和Baltashov 等[11]將多層建筑內(nèi)3D 環(huán)境表示為多個(gè)2D 環(huán)境的組合來實(shí)現(xiàn)多樓層間的自主導(dǎo)航任務(wù)。但是，他們一方面沒有考慮到環(huán)境高度信息，另一方面全局規(guī)劃算法沒有考慮平臺(tái)特性，局部規(guī)劃算法無法很好地適應(yīng)多層建筑物環(huán)境空間狹小等特點(diǎn)，容易陷入局部最小。

針對(duì)上述情況，考慮到多層建筑物環(huán)境空間狹小特點(diǎn)，本文利用地圖的分層表示方法將多層建筑內(nèi)3D 運(yùn)動(dòng)規(guī)劃問題降解為2.5D，利用維諾路徑作為A*算法代價(jià)項(xiàng)，引入運(yùn)動(dòng)基元來實(shí)現(xiàn)路徑的運(yùn)動(dòng)學(xué)可行。通過履帶平臺(tái)運(yùn)動(dòng)學(xué)模型在動(dòng)力學(xué)窗口內(nèi)對(duì)其軌跡進(jìn)行預(yù)測(cè)，引入波陣值評(píng)估方法，避免陷入局部最小。利用有限狀態(tài)機(jī)建立起無人平臺(tái)模式之間的關(guān)聯(lián)關(guān)系，實(shí)現(xiàn)多樓層建筑物內(nèi)的自主導(dǎo)航任務(wù)。

1 多層建筑物內(nèi)運(yùn)動(dòng)規(guī)劃框架

考慮到無人平臺(tái)在多樓層建筑物內(nèi)進(jìn)行自主導(dǎo)航任務(wù)時(shí)需要完成上、下樓梯等操作，本文在履帶式差速底盤的基礎(chǔ)上進(jìn)行研究工作。

根據(jù)多樓層建筑物內(nèi)環(huán)境的特點(diǎn)可以將自主導(dǎo)航任務(wù)分為3 種類型，同樓層內(nèi)的運(yùn)動(dòng)規(guī)劃、上樓與下樓的運(yùn)動(dòng)規(guī)劃。針對(duì)于不同工況的特點(diǎn)，本文為無人平臺(tái)設(shè)計(jì)了3 種不同的工作模式，并利用有限狀態(tài)機(jī)決策模型(見圖1)實(shí)現(xiàn)多樓層間的運(yùn)動(dòng)規(guī)劃。多樓層間的運(yùn)動(dòng)規(guī)劃算法首先進(jìn)入同樓層規(guī)劃模式，導(dǎo)航平臺(tái)到達(dá)同層目標(biāo)點(diǎn)。若滿足條件1，即當(dāng)前樓層小于目標(biāo)樓層，則切換到上樓規(guī)劃模式。若滿足條件2，即當(dāng)前樓層大于目標(biāo)樓層，則切換到下樓模式。若滿足條件3，即當(dāng)前樓層等于目標(biāo)樓層，則再次進(jìn)入同層規(guī)劃模式，最終到達(dá)目標(biāo)點(diǎn)。

圖1 有限狀態(tài)機(jī)決策模型示意圖Fig. 1 Schematic diagram of Finite State Machine decision model

同樓層運(yùn)動(dòng)規(guī)劃算法如圖2 所示，由全局與局部規(guī)劃組成。全局規(guī)劃在全局地圖、維諾路徑、運(yùn)動(dòng)基元和定位信息的基礎(chǔ)上得到一條平滑、無碰撞的全局路徑發(fā)送到局部規(guī)劃。局部規(guī)劃結(jié)合全局路徑、局部地圖、波陣值、平臺(tái)運(yùn)動(dòng)學(xué)模型和定位信息得到最優(yōu)局部路徑和控制量，并發(fā)送到無人平臺(tái)。上、下樓模式在同層模式的基礎(chǔ)上對(duì)樓梯環(huán)境下進(jìn)行適配，并在局部地圖中濾除了樓梯臺(tái)階作為障礙物的環(huán)境信息。在上樓模式中，首先需要對(duì)地圖進(jìn)行處理，利用高度差值算法過濾掉臺(tái)階信息來實(shí)現(xiàn)在樓梯上的規(guī)劃，同時(shí)，需要增大轉(zhuǎn)向動(dòng)作的懲罰，避免在樓梯上進(jìn)行轉(zhuǎn)向操作，增大目標(biāo)速度來克服爬樓梯時(shí)的坡道阻力。與上樓模式相同，首先也需要過濾掉臺(tái)階的障礙物信息，增大轉(zhuǎn)向動(dòng)作的懲罰，減小目標(biāo)速度提高下樓梯時(shí)無人平臺(tái)姿態(tài)的穩(wěn)定性。

圖2 同樓層運(yùn)動(dòng)規(guī)劃框架示意圖Fig. 2 Schematic diagram of motion planning framework

對(duì)于多層建筑物內(nèi)環(huán)境信息，本文采用基于分層的地圖表示方法，將問題進(jìn)行降解求解。分層地圖將多層建筑物室內(nèi)環(huán)境信息表示為多個(gè)2.5D 地圖和關(guān)鍵點(diǎn)，其中，2.5D 地圖是基于高度差信息建立的建筑物單層的柵格地圖，保存了樓層序號(hào)、單層內(nèi)的可通行區(qū)域和障礙物信息；關(guān)鍵點(diǎn)是保存在相應(yīng)地圖下的全局坐標(biāo)，為樓梯或建筑物入口的坐標(biāo)信息，代表著各個(gè)地圖之間的拓?fù)潢P(guān)系。關(guān)鍵點(diǎn)又分為兩種，一種是記錄進(jìn)入當(dāng)前層的入口信息，記作In，n為樓層數(shù)；另一種是出口信息，記作On。

圖3 展示了一個(gè)3 層建筑物環(huán)境地圖的表示方法的例子，圖中左側(cè)展示的是一個(gè)3 層建筑物的3D 結(jié)構(gòu)示意圖，利用分層表示方法可以表示為圖中右側(cè)的形式。其中，Map1、Map2 和Map3 分別表示建筑物1 層、2 層和3 層環(huán)境對(duì)應(yīng)的柵格地圖，綠色點(diǎn)表示進(jìn)入該層的關(guān)鍵點(diǎn)，紅色點(diǎn)表示離開該層的關(guān)鍵點(diǎn)。因此，利用柵格地圖和關(guān)鍵點(diǎn)信息就可以表示出一個(gè)多層建筑物內(nèi)的環(huán)境信息，為跨樓層的無人平臺(tái)運(yùn)動(dòng)規(guī)劃提供基礎(chǔ)。

圖3 多層建筑物環(huán)境地圖示意圖Fig. 3 Schematic diagram of the multi-storey building hierarchical map

2 基于維諾路徑和運(yùn)動(dòng)基元的全局規(guī)劃 算法

本節(jié)主要討論用于多層建筑物中的全局規(guī)劃算法?？紤]到多層建筑物環(huán)境具有空間狹小的特點(diǎn)，本文基于A*算法，考慮無人平臺(tái)運(yùn)動(dòng)學(xué)約束，并引入維諾路徑作為參考項(xiàng)，實(shí)現(xiàn)了在多層建筑物環(huán)境中規(guī)劃出平滑、無碰撞和運(yùn)動(dòng)學(xué)可行的全局 路徑。

2.1 維諾路徑

傳統(tǒng)A*算法[12-14]在搜索路徑時(shí)，只考慮路徑的長(zhǎng)度代價(jià)值，本文利用維諾圖中的維諾路徑來為A*算法提供新的代價(jià)項(xiàng)，從而規(guī)劃出遠(yuǎn)離障礙物的全局路徑，為平臺(tái)提供更安全的全局引導(dǎo)。

維諾圖[15-16]是由一組連接兩鄰近障礙物直線的垂直平分線組成的連續(xù)多邊形，這些多邊形的邊界稱做維諾路徑。典型室內(nèi)環(huán)境柵格地圖如圖4(a)所示，其對(duì)應(yīng)的維諾圖如圖4(b)所示，黑色線段即為維諾路徑。

圖4 室內(nèi)環(huán)境柵格地圖及其維諾圖Fig. 4 Grid map of an indoor environment and its Voronoi map

傳統(tǒng)A*算法利用節(jié)點(diǎn)的代價(jià)值來進(jìn)行搜索，總代價(jià)值F(k)可以表示為

式中：G(k)為從起點(diǎn)到節(jié)點(diǎn)k的路徑代價(jià)值；H(k)為節(jié)點(diǎn)k到目標(biāo)點(diǎn)的啟發(fā)值，用來估計(jì)當(dāng)前節(jié)點(diǎn)到目標(biāo)點(diǎn)的距離，為兩者之間的歐氏距離；Sk-1,k為節(jié)點(diǎn)k-1 與節(jié)點(diǎn)k之間的路程。

根據(jù)維諾路徑的特點(diǎn)，假設(shè)A*算法中一待擴(kuò)展節(jié)點(diǎn)p(k)以及維諾路徑V(x，y)，則其維諾路徑代價(jià)項(xiàng)可以表示為

式中：vnearest為維諾路徑上距離p(k)最近的點(diǎn)。因此，搜索過程中總的代價(jià)值可以表示為

式中：α和β為路程代價(jià)與維諾路徑代價(jià)項(xiàng)權(quán)重 系數(shù)。

2.2 運(yùn)動(dòng)基元

由于A*算法得到的路徑是沒有考慮到無人平臺(tái)運(yùn)動(dòng)學(xué)約束的，不利于對(duì)路徑進(jìn)行跟蹤，本文引入運(yùn)動(dòng)基元來求解運(yùn)動(dòng)學(xué)可行的全局路徑。

運(yùn)動(dòng)基元是路徑規(guī)劃問題中基本運(yùn)動(dòng)單元的簡(jiǎn)稱。常用的基元是固定無人平臺(tái)縱向速度，給定角速度預(yù)測(cè)無人平臺(tái)在有限時(shí)間內(nèi)行駛過的軌跡，通常給定不同的角速度生成多條軌跡。

根據(jù)傳統(tǒng)的基元設(shè)計(jì)思路，只通過增加運(yùn)動(dòng)基元數(shù)量來減少非直接可達(dá)區(qū)域的范圍，如圖5 所示，由于沒有充分利用履帶式平臺(tái)零半徑轉(zhuǎn)向的特點(diǎn)，導(dǎo)致規(guī)劃出的路徑更加復(fù)雜，同時(shí)也極大地增加了計(jì)算量。

圖5 傳統(tǒng)運(yùn)動(dòng)基元方案示意圖Fig. 5 Schematic diagram of traditional motion primitives

本文將無人平臺(tái)前向運(yùn)動(dòng)基元簡(jiǎn)化為7 個(gè)，如圖6 中彩色軌跡，并提出零半徑轉(zhuǎn)向基元的概念，如圖中箭頭，通過零半徑轉(zhuǎn)向基元與普通前向基元的組合可以實(shí)現(xiàn)更加靈活的全局路徑搜索。

圖6 零半徑運(yùn)動(dòng)基元方案示意圖Fig. 6 Schematic diagram of zero-radius motion primitives

全局路徑搜索如算法1 所示。算法輸入為起始點(diǎn)、目標(biāo)點(diǎn)和柵格地圖。首先，算法初始化兩個(gè)集合，分別用來存放待擴(kuò)展和已擴(kuò)展節(jié)點(diǎn)。在每次擴(kuò)展節(jié)點(diǎn)過程中，開集合中代價(jià)值最小的節(jié)點(diǎn)會(huì)被移入閉集合，而該節(jié)點(diǎn)的子節(jié)點(diǎn)會(huì)被添加到開集合當(dāng)中，如此迭代，直到擴(kuò)展到目標(biāo)節(jié)點(diǎn)，返回由節(jié)點(diǎn)構(gòu)成的全局路徑。

算法1：全局路徑搜索 輸入：目標(biāo)點(diǎn)坐標(biāo)ngoal，起始點(diǎn)坐標(biāo)nstart，全局柵格地圖Map 輸出：由節(jié)點(diǎn)構(gòu)成的全局路徑 1.Sopen←nstart2.Sclosed←Ф3. WhileSopen≠Ф4. Sort(Sopen) 5. ni←Sopen.PopTop() 6. if IsObstacles(ni,Map) 7. continue 8. else if Overlap(ni,ngoal) 9. return { nstart,…,ngoal} 10. else 11. Schildren←ExpandNode(ni，primitives) 12. for child inSchildren13. F(child)←H(each)+G(ni)+αD(child)+βS(ni，child) 14. if child ?Sopen15. Sopen←Sopen∪ {child } 16. else F(child)

2.3 路徑平滑

通過改進(jìn)基元A*算法直接得到的路徑仍然存在兩個(gè)問題，如圖7 所示，一方面是路徑上的路點(diǎn)為稀疏狀態(tài)，如紅色圓點(diǎn)所示；另一方面是由于零半徑基元的存在，路徑存在不光滑區(qū)段，如圖中間的路點(diǎn)處發(fā)生了零半徑轉(zhuǎn)向，不利于無人平臺(tái)實(shí)現(xiàn)平順的路徑跟蹤。

圖7 路徑平滑示意圖Fig. 7 Schematic diagram of path smoothing

基于以上兩點(diǎn)，本文首先過濾掉零半徑轉(zhuǎn)向后的位姿，再利用路徑平滑算法從稀疏路點(diǎn)來得到平滑路徑，如圖7 中綠色曲線所示，在平滑路徑的基礎(chǔ)上進(jìn)行插值，得到稠密路點(diǎn)，如黃色點(diǎn)所示。

考慮到全局路徑的導(dǎo)數(shù)與曲率連續(xù)，同時(shí)避免龍格現(xiàn)象，本文采用分段三次Hermite 插值[17-18]來對(duì)稀疏路點(diǎn)進(jìn)行處理。

給定稀疏路點(diǎn)集合P(x，y，θ)，x和y為位置坐標(biāo)，θ為航向角，把節(jié)點(diǎn)兩兩分段，在每一個(gè)小段上作三次Hermite 插值，就可以得到一個(gè)分段三次Hermite 插值函數(shù)H(x)。

首先，構(gòu)造兩組基函數(shù)hi(x)，Hi(x) (i=0, 1, …,n)：

式中：li(x)為拉格朗日插值基函數(shù)，

因此，結(jié)合式(5)、式(6)和式(7)，可以得到分段三次Hermite 插值函數(shù)為

3 基于模型預(yù)測(cè)與波陣值的局部規(guī)劃算法

在全局路徑的基礎(chǔ)上，本文利用履帶式無人平臺(tái)運(yùn)動(dòng)學(xué)模型在動(dòng)力學(xué)窗口內(nèi)對(duì)其軌跡進(jìn)行預(yù)測(cè)與評(píng)估，并引入波陣值優(yōu)化距離評(píng)估方法，避免陷入局部最小。

3.1 運(yùn)動(dòng)學(xué)模型與軌跡預(yù)測(cè)

履帶式無人平臺(tái)兩側(cè)的獨(dú)立驅(qū)動(dòng)的履帶以不同速度進(jìn)行運(yùn)轉(zhuǎn)，由于兩側(cè)履帶的速率不同，此時(shí)在履帶平臺(tái)右側(cè)會(huì)形成一個(gè)轉(zhuǎn)動(dòng)中心，平臺(tái)模型會(huì)以圖8 所示的轉(zhuǎn)向半徑R，繞著轉(zhuǎn)動(dòng)中心O以ω的角速度進(jìn)行轉(zhuǎn)向。圖8 中，v1、v2為左側(cè)和右側(cè)履帶速度，B為履帶中心距。

圖8 履帶式無人平臺(tái)運(yùn)動(dòng)幾何關(guān)系示意圖Fig. 8 Schematic diagram of the kinematic geometric relationship of the tracked unmanned platform

根據(jù)幾何關(guān)系，可以計(jì)算出轉(zhuǎn)向半徑R：

同時(shí)，低速狀態(tài)下忽略滑轉(zhuǎn)與滑移，履帶式無人平臺(tái)運(yùn)動(dòng)學(xué)模型為

式中：vx和vy為平臺(tái)在x軸和y軸方向的速度；θ為平臺(tái)航向角。

由剛體運(yùn)動(dòng)學(xué)可以得知軌跡傳播方程：

式中：xt、yt、θt為t時(shí)刻平臺(tái)位姿；ωt為t時(shí)刻角速度。

基于運(yùn)動(dòng)學(xué)模型，結(jié)合式(9)、式(10)和 式(11)，可以得到軌跡預(yù)測(cè)方程：

在軌跡預(yù)測(cè)方程的基礎(chǔ)上，本文引用模型預(yù)測(cè)控制[19]的思想，如圖9 所示，根據(jù)當(dāng)前時(shí)刻t0無人平臺(tái)的運(yùn)動(dòng)狀態(tài)，在控制空間內(nèi)采樣得到一組控制量，以dt時(shí)間為步長(zhǎng)，來預(yù)測(cè)未來一段時(shí)間內(nèi)無人平臺(tái)的狀態(tài)，然后執(zhí)行最優(yōu)路徑對(duì)應(yīng)的控制量來驅(qū)動(dòng)無人平臺(tái)運(yùn)動(dòng)dt的時(shí)間長(zhǎng)度，來到t1時(shí)刻的狀態(tài)。以此類推，不斷進(jìn)行預(yù)測(cè)與執(zhí)行控制量，直到到達(dá)目標(biāo)點(diǎn)。

圖9 軌跡預(yù)測(cè)示意圖Fig. 9 Schematic diagram of trajectory prediction

3.2 基于動(dòng)力學(xué)窗口的控制空間采樣

在無人平臺(tái)運(yùn)動(dòng)學(xué)的基礎(chǔ)上進(jìn)行軌跡預(yù)測(cè)之前，需要對(duì)無人平臺(tái)的控制空間進(jìn)行采樣，由于無人平臺(tái)動(dòng)力學(xué)的約束，其采樣區(qū)間也被限制在動(dòng)力學(xué)窗口[20]之內(nèi)。

1)最大加速度與最大減速度約束

在無人平臺(tái)實(shí)際運(yùn)動(dòng)過程中，由于驅(qū)動(dòng)電機(jī)轉(zhuǎn)矩等的限制，其具有最大的加速度和減速度。假設(shè)最大加速度和減速度為aa和ad，最大角加速度和角減速度為αa和αd。

式中：vt為t時(shí)刻的平臺(tái)速度。

2)最大與最小速度約束

式中：vmin和vmax分別為平臺(tái)最小速度與最大速度；ωmin與ωmax分別為平臺(tái)最小角速度與最大角速度。

3)安全距離約束

在無人平臺(tái)當(dāng)前位置處，為避免與障礙物發(fā)生碰撞，無人平臺(tái)當(dāng)前速度對(duì)應(yīng)的剎車距離應(yīng)該小于距離最近障礙物的距離。因此可以得到

式中：dist為t時(shí)刻最近障礙物的距離。

3.3 基于波陣值的軌跡評(píng)估

履帶無人平臺(tái)在進(jìn)行軌跡預(yù)測(cè)之后，如圖10中紫色軌跡，可以得到軌跡的終點(diǎn)，如黃色圓點(diǎn)。由于定位誤差以及環(huán)境噪聲等的干擾，全局地圖中障礙物(灰色)會(huì)與實(shí)際障礙物(黑色)位置存在偏差。如果根據(jù)傳統(tǒng)軌跡評(píng)估的原則，中間軌跡距離局部目標(biāo)點(diǎn)(紅色圓點(diǎn))最近，會(huì)成為最優(yōu)路徑。然而，中間路徑會(huì)導(dǎo)致無人平臺(tái)陷入局部最小。

圖10 波陣值避免局部最小示意圖Fig. 10 Schematic diagram of Wavefront Value avoiding local minimum

本文引入混合A*算法[21]中的波陣值(Wavefront Value)來提高算法應(yīng)對(duì)障礙物信息偏差時(shí)的魯棒性。波陣值是一種考慮到障礙物信息后地圖上一點(diǎn)到目標(biāo)點(diǎn)的距離度量，可以利用算法2 計(jì)算得到。算法2 的輸入為局部目標(biāo)點(diǎn)和預(yù)測(cè)軌跡的端點(diǎn)集合，然后從目標(biāo)點(diǎn)出發(fā)，向鄰近的8 個(gè)柵格方向進(jìn)行反向搜索，若為障礙物則跳過，則直到搜索到所有預(yù)測(cè)軌跡端點(diǎn)時(shí)，過程中搜索路程長(zhǎng)度即為波陣值最為算法的輸出。

當(dāng)利用波陣值對(duì)軌跡進(jìn)行評(píng)估時(shí)，由于其具有考慮到環(huán)境中障礙物的特性，右側(cè)軌跡終點(diǎn)到達(dá)局部目標(biāo)點(diǎn)的波陣值最小，即藍(lán)色方塊的數(shù)量最小，為最優(yōu)軌跡，因此可以繞過障礙物，避免局部最小。

算法2：計(jì)算波陣值 輸入：局部目標(biāo)點(diǎn)坐標(biāo)g[x,y]，預(yù)測(cè)軌跡終點(diǎn)集合T[traj1，traj2, … , trajn]，局部柵格地圖Map 輸出：預(yù)測(cè)軌跡對(duì)應(yīng)波陣值集合W1.Goal←[g[0], g[1],0]2.S.Push(Goal) 3.W←[Finite]4. While S ≠Ф5. Current←S.PopTop() 6. for each in ExpandTable 7. Next←Current+ each 8. Next_Position←[Next[0]，Next[1]]9. Next_Wave←Next[2]10. If IsObstacles(Map，Next_Position) | IsExpanded(Next_Position) 11. continue 12. end 13. S.Push(Next) 14. fortrajinT15. iftraj= Next_Position16. W[GetIndex(traj)]←Next_Wave17. end 18. end 19. if IsAllFinite(W) 20. break 21. end 22. end 23. end 24. returnW

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

為評(píng)價(jià)本文提出的多層建筑物內(nèi)運(yùn)動(dòng)規(guī)劃框架的性能，針對(duì)全局與局部規(guī)劃模塊，在仿真環(huán)境中，采用本文方案和傳統(tǒng)運(yùn)動(dòng)規(guī)劃方法進(jìn)行對(duì)比實(shí)驗(yàn)，進(jìn)而通過實(shí)車實(shí)驗(yàn)來證明本文方案的可行性。

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

為測(cè)試全局和局部規(guī)劃算法的性能，本文在Vrep 中搭建圖11(a)所示虛擬環(huán)境作為算法的輸入，搭建圖11(b)所示無人履帶平臺(tái)模型作為算法的被控對(duì)象，配備有激光雷達(dá)和定位模塊。算法在Linux 環(huán)境和ROS 平臺(tái)下運(yùn)行，對(duì)虛擬環(huán)境的輸入信息進(jìn)行處理后，將得到的路徑下發(fā)給無人平臺(tái)模型，最后對(duì)結(jié)果進(jìn)行統(tǒng)計(jì)分析，實(shí)現(xiàn)對(duì)算法的仿真測(cè)試。

圖11 仿真環(huán)境示意圖Fig. 11 Schematic diagram of the simulation environment

全局規(guī)劃仿真結(jié)果如圖12 所示，綠色箭頭為目標(biāo)點(diǎn)，紅色扇形為算法擴(kuò)展結(jié)點(diǎn)集合。如表1 所示，從中可以看到，在擴(kuò)展節(jié)點(diǎn)數(shù)量增加3%、平均耗時(shí)增加6%的條件下，全局路徑長(zhǎng)度減少了31%，且距離障礙物更遠(yuǎn)、路徑更加安全。

表1 全局規(guī)劃仿真結(jié)果Table 1 Simulation results of the global planner

圖12 全局規(guī)劃仿真結(jié)果對(duì)比圖Fig. 12 Comparison of the global planner’s simulation results

局部規(guī)劃仿真實(shí)驗(yàn)通過偏移障礙物的位置來模擬障礙物位置偏差的情形。圓形障礙物在全局地圖中位置如圖13所示，由于定位和建圖誤差，實(shí)際障礙物在圓形障礙物右側(cè)，激光雷達(dá)掃描到其前部，在地圖中為半圓形。圖13中，藍(lán)色曲線為全局路徑，紅色矩形為無人平臺(tái)地面投影，綠色曲線為局部規(guī)劃的待選軌跡，紅色曲線為最優(yōu)軌跡。圖13(a)展示的是本文的局部規(guī)劃算法篩選出最優(yōu)軌跡并成功繞過了障礙物，圖13(b)中的傳統(tǒng)局部規(guī)劃算法在障礙物面前陷入局部最小。

圖13 局部規(guī)劃仿真對(duì)比圖(左為遇到障礙物，右為繞過障礙物)Fig. 13 Comparison of the local planner’s simulation results (left picture shows encountering obstacles, right picture shows bypassing obstacles)

4.2 實(shí)車實(shí)驗(yàn)

本文真實(shí)場(chǎng)景實(shí)驗(yàn)所使用的是履帶式無人平臺(tái)，如圖14 所示。本文依賴的傳感器有安裝于無人平臺(tái)頂部的16 線激光雷達(dá)和前部的32 線補(bǔ)盲雷達(dá)，承擔(dān)著感知與定位的任務(wù)。除傳感器外，無人平臺(tái)還配備有一臺(tái)主頻為3.6 Hz 的工控機(jī)，感知、定位與規(guī)劃程序都在該計(jì)算平臺(tái)上運(yùn)行。本文實(shí)驗(yàn)中感知模塊使用的是基于激光雷達(dá)的概率柵格地圖程序，定位模塊使用的是激光SLAM 程序，規(guī)劃模塊使用的是本文提出的多層建筑物運(yùn)動(dòng)規(guī)劃框架。

圖14 實(shí)驗(yàn)無人平臺(tái)外觀Fig. 14 Appearance of the unmanned platform in the experiment

首先，通過遙控?zé)o人平臺(tái)在多層建筑物內(nèi)進(jìn)行建圖，獲取建筑物的三維點(diǎn)云信息并以分層地圖的方式存儲(chǔ)下來。在此基礎(chǔ)上設(shè)置建筑物一層大廳內(nèi)一點(diǎn)為起點(diǎn)，3 層長(zhǎng)走廊內(nèi)一點(diǎn)為終點(diǎn)，如圖15所示，開始自主導(dǎo)航任務(wù)。

圖15 實(shí)驗(yàn)起始點(diǎn)與目標(biāo)點(diǎn)Fig. 15 Starting point and target point in the experiment

在1層環(huán)境中規(guī)劃結(jié)果如圖16所示，其中藍(lán)色曲線為全局路徑，綠色曲線為局部路徑，綠色箭頭為目標(biāo)點(diǎn)，可以看出，規(guī)劃出的全局路徑平滑連接到目標(biāo)點(diǎn)，且遠(yuǎn)離障礙物，無人平臺(tái)順利進(jìn)入狹長(zhǎng)走廊，如圖16(a)所示。由于室內(nèi)狹長(zhǎng)走廊的對(duì)稱性以及墻壁瓷磚反射效應(yīng)的影響，激光SLAM定位在此區(qū)域誤差增大，如圖16(b)紅色圓圈部分，這段直角轉(zhuǎn)彎區(qū)域墻壁發(fā)生了彎曲且長(zhǎng)度縮短，導(dǎo)致地圖中轉(zhuǎn)彎入口處相比實(shí)際的1.0 m寬度變大。在這種狹窄區(qū)域障礙物位置出現(xiàn)較大偏差的情形下，無人平臺(tái)避免了陷入局部最小值，成功通過該區(qū)域如圖16(b)所示。

圖16 1 層樓內(nèi)實(shí)驗(yàn)結(jié)果(左為規(guī)劃路徑，右為實(shí)景)Fig. 16 Experimental results of the first floor (the left is the planned path, and the right is the real scene)

在此基礎(chǔ)上，無人平臺(tái)開始在樓梯上進(jìn)行自主導(dǎo)航任務(wù)。圖17 和圖18 所示為無人平臺(tái)在樓梯和轉(zhuǎn)臺(tái)上規(guī)劃出的路徑與實(shí)驗(yàn)場(chǎng)景，從中可以看出通過高度差將樓梯信息過濾后，無人平臺(tái)在樓梯模式中可以規(guī)劃出更加平直的路徑。

圖17 樓梯區(qū)域?qū)嶒?yàn)結(jié)果Fig. 17 Experimental results of the stair area

圖18 轉(zhuǎn)臺(tái)區(qū)域?qū)嶒?yàn)結(jié)果Fig. 18 Experimental results of the platform area

圖19為在3層時(shí)的實(shí)驗(yàn)結(jié)果。圖19(a)中無人平臺(tái)跟蹤全局路徑成功通過1 m寬的直角轉(zhuǎn)彎，進(jìn)入狹長(zhǎng)走廊。在走廊中有一扇打開的門，可通行空間寬度為0.90 m，而平臺(tái)寬度為0.80 m，剩余空間狹窄。如圖19(b)所示，無人平臺(tái)成功穿越狹窄區(qū)域。最后，如圖19(c)所示，無人平臺(tái)到達(dá)目 標(biāo)點(diǎn)。

圖19 第3 層樓實(shí)驗(yàn)結(jié)果(左為規(guī)劃路徑，右為實(shí)景)Fig.19 Experimental results on the third floor (the left is the planned path, right is the real scene)

實(shí)車實(shí)驗(yàn)結(jié)果見表2，無人平臺(tái)經(jīng)過第1 層區(qū)域、樓梯區(qū)域和第3 層區(qū)域3 個(gè)階段，其中第1 層起始區(qū)域空間相對(duì)寬闊，平均速度也最高；樓梯區(qū)域由兩層共6 段樓梯組成，路徑長(zhǎng)度最長(zhǎng)，同時(shí)為保證樓梯上平臺(tái)運(yùn)行的安全，最大速度也最小，導(dǎo)致花費(fèi)時(shí)間最長(zhǎng)；第3 層區(qū)域空間狹窄，平臺(tái)處于低速狀態(tài)更多，因此平均速度下降。在實(shí)驗(yàn)過程中，算法的頻率能夠穩(wěn)定在10 Hz，并完成多層建筑物內(nèi)的自主導(dǎo)航任務(wù)。

表2 實(shí)車實(shí)驗(yàn)結(jié)果Table 2 Results of the experiment

應(yīng)用本文運(yùn)動(dòng)規(guī)劃框架的北理精靈隊(duì)在跨越險(xiǎn)阻2021 地面無人系統(tǒng)挑戰(zhàn)賽中取得了第4 名的 成績(jī)。

5 結(jié)論

本文針對(duì)多層建筑物內(nèi)無人平臺(tái)的自主導(dǎo)航問題，提出了一個(gè)針對(duì)于多層建筑環(huán)境優(yōu)化的運(yùn)動(dòng)規(guī)劃框架。通過仿真與實(shí)車實(shí)驗(yàn)驗(yàn)證，得到以下主要結(jié)論：

1)在全局規(guī)劃層面，提出零半徑轉(zhuǎn)向基元并引入維諾路徑，全局規(guī)劃算法在擴(kuò)展節(jié)點(diǎn)數(shù)量增加3%、平均耗時(shí)增加6%的條件下，全局路徑長(zhǎng)度減少了31%，且距離障礙物更遠(yuǎn)，路徑更加安全，提高了在狹窄區(qū)域規(guī)劃路徑的靈活性。

2)在局部規(guī)劃層面，引入波陣值作為評(píng)估項(xiàng)，局部規(guī)劃算法相比于傳統(tǒng)算法避免了陷入局部最小值，提高了算法的魯棒性。

3)經(jīng)過實(shí)車實(shí)驗(yàn)，基于本文運(yùn)動(dòng)規(guī)劃框架的無人平臺(tái)在多層建筑物內(nèi)完成了自主導(dǎo)航任務(wù)，證明了方案的可行性。