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

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2 基于維諾路徑和運動基元的全局規(guī)劃 算法

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

2.1 維諾路徑

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

維諾圖[15-16]是由一組連接兩鄰近障礙物直線的垂直平分線組成的連續(xù)多邊形，這些多邊形的邊界稱做維諾路徑。典型室內(nèi)環(huán)境柵格地圖如圖4(a)所示，其對應(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é)點的代價值來進行搜索，總代價值F(k)可以表示為

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

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

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

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

2.2 運動基元

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

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

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

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

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

圖6 零半徑運動基元方案示意圖Fig. 6 Schematic diagram of zero-radius motion primitives

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

算法1：全局路徑搜索 輸入：目標點坐標ngoal，起始點坐標nstart，全局柵格地圖Map 輸出：由節(jié)點構(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 路徑平滑

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

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

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

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

給定稀疏路點集合P(x，y，θ)，x和y為位置坐標，θ為航向角，把節(jié)點兩兩分段，在每一個小段上作三次Hermite 插值，就可以得到一個分段三次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ù)測與波陣值的局部規(guī)劃算法

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

3.1 運動學(xué)模型與軌跡預(yù)測

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

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

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

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

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

由剛體運動學(xué)可以得知軌跡傳播方程：

式中：xt、yt、θt為t時刻平臺位姿；ωt為t時刻角速度。

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

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

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

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

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

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

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

式中：vt為t時刻的平臺速度。

2)最大與最小速度約束

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

3)安全距離約束

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

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

3.3 基于波陣值的軌跡評估

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

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

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

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

算法2：計算波陣值 輸入：局部目標點坐標g[x,y]，預(yù)測軌跡終點集合T[traj1，traj2, … , trajn]，局部柵格地圖Map 輸出：預(yù)測軌跡對應(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 實驗與結(jié)果分析

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

4.1 仿真實驗

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

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

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

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

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

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

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

4.2 實車實驗

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

圖14 實驗無人平臺外觀Fig. 14 Appearance of the unmanned platform in the experiment

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

圖15 實驗起始點與目標點Fig. 15 Starting point and target point in the experiment

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

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

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

圖17 樓梯區(qū)域?qū)嶒灲Y(jié)果Fig. 17 Experimental results of the stair area

圖18 轉(zhuǎn)臺區(qū)域?qū)嶒灲Y(jié)果Fig. 18 Experimental results of the platform area

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

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

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

表2 實車實驗結(jié)果Table 2 Results of the experiment

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

5 結(jié)論

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

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

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

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