一種面向動態(tài)環(huán)境的多層代價地圖更新算法

李德勝，孫一杰，張國良

(1.四川輕化工大學 自動化與信息工程學院，四川 宜賓 644000；2.人工智能四川省重點實驗室，四川 宜賓 644000)

0 引言

導航技術是指移動機器人基于配置的激光雷達、相機和超聲波等傳感器對周圍狀態(tài)空間進行同時定位與地圖構建(Simultaneous Localization and Mapping，SLAM)，進而規(guī)劃出可安全執(zhí)行的路徑并有效地完成目標任務。移動機器人進行自主導航時主要使用的地圖包括拓撲地圖、幾何地圖、柵格地圖和語義地圖，這些地圖各有優(yōu)缺點和不同的應用對象[1-5]。當前，自主導航使用最多的是代價地圖，其中單層代價地圖存在有限的環(huán)境信息和固定的語義信息問題，限制了路徑規(guī)劃效率的提升[6]；而多層代價地圖將環(huán)境信息簡化成語義分離的層，擴展了地圖所包含的環(huán)境信息，但存在匹配性低、耗時長和膨脹效率低等問題[7]。

多層代價地圖在移動機器人導航領域具有重要的研究意義。Kong等[8]利用多層代價地圖，實現自動導引車在平面環(huán)境中自主導航。Coleto等[9]提出一種多層代價地圖架構，結合社會規(guī)范來生成對人類生活友好的運動軌跡。Groves等[10]設計了生成輻射代價圖，并與代價地圖進行信息融合，令機器人能探測到高輻射環(huán)境并自主離開。Chen等[11]提出新的深度強化學習算法，將局部占用率映射到代理層，提高機器人的決策能力。張福海等[12]優(yōu)化了代價地圖的生成方式，改善了代價地圖匹配性低和實時性弱的問題。Han等[13]利用“相遇時間”的定義，將動態(tài)對象轉換為虛擬靜態(tài)對象，進而改善開放動態(tài)環(huán)境下導航方案的魯棒性。Fang等[14]基于具有行人檢測和跟蹤功能的全局行人感知對群體互動和個人空間進行建模，生成多層動態(tài)代價地圖，為全局路徑規(guī)劃提供預測階段的社會約束信息。上述研究為地圖構建模型的設計貢獻了多種思路和應用案例，具有較好的啟發(fā)價值。但這些研究還需進一步加強對動態(tài)環(huán)境下動態(tài)障礙物的處理，提高地圖構建在處理環(huán)境信息方面的匹配性和實時性。

為獲得動態(tài)環(huán)境下動態(tài)障礙物的位置信息，當前國內外學者對動態(tài)障礙物的運動軌跡估計問題進行了研究[15-18]。胡玉可等[19]為了提高目標船舶預測的準確性，提出了基于循環(huán)神經網絡的預測算法。Kamil等[20]利用機器人的傳感系統(tǒng)信息，將決策過程與障礙物速度矢量的預測行為相結合，避免了機器人與障礙物發(fā)生碰撞。Lindqvist等[21]應用分類方案來區(qū)分不同種類的軌跡以預測未來障礙物位置。Kr?mer等[22]使用在線軌跡優(yōu)化來預測潛在的碰撞和任務變化，并相應地調整運動計劃。這些研究對動態(tài)障礙物運動估計問題進行了不同程度的分析和處理，但并未考慮將動態(tài)障礙物的信息融合到多層代價地圖。

為解決上述問題，提出了一種新的多層代價地圖更新算法，對地圖構建算法進行改進，在障礙物層添加對動態(tài)障礙物的檢測和運動軌跡估計，將下一幀動態(tài)障礙物信息轉換成當前幀的靜態(tài)障礙物信息并融合進多層代價地圖，從而提升代價地圖處理環(huán)境障礙物信息的效能。

1 多層代價地圖

1.1 多層代價地圖的代價值

依據Bresenham光線追蹤算法對環(huán)境信息進行處理，可得到占用一個字節(jié)的代價值。令μ表示代價值的比例因子(Cost Scaling Factor)，‖dij-o‖表示當前柵格與障礙物間的度量值，ri表示內切圓半徑大小，則柵格(i,j)的代價值c(i,j)為:

c(i,j)=exp(-1.0·μ·(‖dij-o‖-ri))·253。

(1)

代價地圖中柵格單元存在的代價值為0～254，并且按代價值大小將柵格單元表示為已占用的、自由的和未知的3個狀態(tài)。由此，具有一定數量的柵格單元被標記已占用的，即被標記為costmap_2d::LETHAL_OBSTACLE代價；具有一定數量的柵格單元被標記未知的，即被標記為costmap_2d::NO_INFORMATION代價；剩下的柵格單元被標記自由的，即被標記為costmap_2d::FREE_SPACE代價。

1.2 多層代價地圖的組成與更新

多層代價地圖包括靜態(tài)地圖層、障礙物層和膨脹層，各個地圖層包含環(huán)境的不同方面。在代價地圖構建過程中，逐層更新當前地圖層數據，最終構建為后續(xù)路徑規(guī)劃任務使用的多層代價地圖，如圖1所示。

圖1 多層代價地圖的組成Fig.1 Composition of layered costmap

多層代價地圖通過維護一個有優(yōu)先級的地圖層列表，將靜態(tài)地圖層、障礙物層和膨脹層的數據信息依次更新到主代價地圖中。主代價地圖通過updateBounds函數和updateValues函數依次處理各個地圖層數據。updateBounds函數循環(huán)查詢每個地圖層，以確定所更新代價地圖的數量，然后地圖層間依次迭代，為每個當前地圖層提供歷史地圖層已更新的邊界框(最初是空白框)。updateValues函數在更新過程中，將連續(xù)地圖層的代價值依次更新到主代價圖邊界框中。

靜態(tài)地圖層：靜態(tài)地圖層接收到環(huán)境信息時，updateBounds函數將返回一個遍布整個代價地圖的邊界框，即使在不斷地迭代過程中，由于其處理的對象是靜態(tài)地圖，邊界框的狀態(tài)也不會發(fā)生變化。當移動機器人在使用多層代價地圖進行導航任務時，以及在執(zhí)行SLAM算法對環(huán)境信息進行獲取的過程中，代價地圖只需獨立地更新靜態(tài)地圖層信息，而不干涉障礙物層和膨脹層的地圖更新過程。

障礙物層：該地圖層通過激光雷達和RGB-D相機等高精度傳感器對環(huán)境信息進行采集，然后將環(huán)境信息轉換成代價地圖信息存儲到二維離散柵格中。高精度傳感器讀數之間的空間標記為自由移動區(qū)域，讀數所在的位置標記為已占用區(qū)域。在每個循環(huán)周期執(zhí)行updateBounds函數，不斷更新的傳感器數據被添加到障礙物層的代價地圖中，期間邊界框連續(xù)地進行擴展來跟蹤所更新的傳感器數據。此時，將障礙物層的代價值同處于代價地圖底層的代價值相融合的方式有最大值更新、覆蓋值更新和疊加值更新。默認方式是覆蓋值更新，即用傳感器所采集的數據覆蓋、更新靜態(tài)地圖層數據，期間如果靜態(tài)地圖層數據的期望信任性更強，則該地圖層中代價值的融合過程調整為只向主代價地圖添加致命障礙物所對應的地圖信息。

膨脹層：通過將代價地圖中的障礙物按一定安全距離進行膨脹處理，使得機器人在后續(xù)路徑規(guī)劃時最大限度地避開障礙物。在膨脹處理期間，updateBounds函數將增加對應的邊界框信息,以達到最近更新的致命障礙物都將被膨脹的目的，并且歷史更新過的邊界框范圍外的舊致命障礙物也同樣被膨脹；在不存儲局部代價地圖信息副本的情況下，updateValues直接更新主代價地圖信息。

1.3 匹配性和實時性問題

經典障礙物層的代價地圖構建過程存在一定的局限性。采取Bresenham 光線追蹤算法來對周圍環(huán)境空間存在的障礙物信息進行標記和清除。具體過程為激光雷達對周圍環(huán)境情況進行數據探測，若探測到致命障礙物，就將其標記并插入到代價地圖中；清除操作根據數據探測結果，從激光雷達的原點向四周進行光線跟蹤，對代價地圖的障礙信息采取刪除處理。

當所探測的周圍環(huán)境出現動態(tài)障礙物時，即動態(tài)障礙物處于運動的狀態(tài)(動態(tài)障礙物的位置隨時都可能發(fā)生變化)，激光雷達只能處理當前時刻動態(tài)障礙物所存在的位置信息，而無法處理動態(tài)障礙物在未來時刻的位置信息，進而無法對動態(tài)障礙物在未來時刻的位置信息進行標記。這將影響多層代價地圖的構建過程，出現以下不理想的狀況。

動態(tài)環(huán)境與激光雷達如圖2所示，圖中表示了在不同時刻下動態(tài)環(huán)境中障礙物所存在的位置，其中A為靜態(tài)障礙物，B與B′為動態(tài)障礙物。

(a) 時刻t

(b) 時刻 t+1圖2 動態(tài)環(huán)境與激光雷達Fig.2 Dynamic environment and lidar

如圖2(a)所示，在時刻t下，激光雷達跟蹤探測到當前周圍環(huán)境存在的靜態(tài)障礙物A并將其標記在代價地圖中，此時靜態(tài)障礙物A的標記過程并不會影響到代價地圖構建過程的匹配性和實時性；而對于動態(tài)障礙物B的處理過程，能被激光雷達探測到當前時刻下的位置信息并同樣被標記在代價地圖中。

如圖2(b)所示，在時刻t+1下，激光雷達跟蹤探測到當前周圍環(huán)境存在的靜態(tài)障礙物A，此時靜態(tài)障礙物A已被標記，并不會發(fā)生變化，因而不會影響到代價地圖構建過程的匹配性和實時性。對于在時刻t+1下，動態(tài)障礙物B′已運動到其他位置，而代價地圖中存儲的環(huán)境信息仍是時刻t下的標記信息，代價地圖中存在的標記障礙物信息不能實時地對在時刻t+1下動態(tài)障礙物B′的環(huán)境信息進行更新，并且存在代價地圖所標記存儲的環(huán)境信息并不完全與真實環(huán)境情況相一致，即障礙物動態(tài)變化時多層代價地圖并不能及時地更新代價地圖信息并且匹配性較差。

由此可見，在環(huán)境中存在動態(tài)障礙物的情況下，經典代價地圖構建地圖算法缺少對動態(tài)障礙物的檢測和估計處理，導致所構建的地圖并不能實時地處理激光雷達跟蹤探測到的動態(tài)障礙物信息，并且代價地圖與環(huán)境信息的匹配性較低。為提高多層代價地圖所構建地圖的匹配性和實時性，以有利于導航規(guī)劃過程中安全性和準確性的提高，有必要對動態(tài)障礙物進行運動檢測，對其運動軌跡進行估計，并將動態(tài)障礙物的運動估計信息融合到多層代價地圖中。

2 融合動態(tài)障礙物信息的障礙物層地圖

2.1 動態(tài)障礙物的檢測

在室內動態(tài)環(huán)境下，移動機器人附近分布著多個靜態(tài)或動態(tài)的障礙物。考慮環(huán)境中障礙物的歷史位移信息，即考慮若干個連續(xù)相鄰幀的點云坐標值，移動機器人可檢測出障礙物的狀態(tài)(靜止或運動)信息。因此，在進行多層代價地圖構建之前，對激光雷達處理的相鄰2幀數據進行距離值匹配，即可判斷障礙物是靜態(tài)還是動態(tài)。

令激光雷達連續(xù)2幀數據的前一幀為A，后一幀為B，幀A與幀B的間隔時間為Δt，動態(tài)障礙物的速度為v1≤vd≤v2，則Δt時間內動態(tài)障礙物的位移為v1·Δt≤xd≤v2·Δt。計算幀A與幀B在相同索引值的點云位移值，進行距離值匹配：

① 當位移值小于v1·Δt時，表示幀B中的點云與幀A的點云為同一靜態(tài)障礙物，此時將其標記為static(即靜態(tài)障礙物，性質已知)；

② 當位移值大于或者等于v1·Δt且小于或者等于v2·Δt時，表示幀B中的點云與幀A的點云來自同一動態(tài)障礙物，此時將其標記為dynamic(即動態(tài)障礙物，性質已知)；

③ 當位移值大于v2·Δt時，表示幀B中的點云與幀A的點云為不同障礙物，此時將其標記為new(即新增的障礙物，性質未知)。

初始情況下，幀A中未傳入激光雷達數據，在幀B中將點云信息全部標記為new。在初始幀之后的激光雷達點云信息則采取距離值匹配進行處理，處理過程結束后可檢測出障礙物的狀態(tài)信息，從而為后續(xù)動態(tài)障礙物的運動軌跡預測提供計算依據。

2.2 動態(tài)障礙物的運動軌跡估計

移動機器人借助激光雷達實時感知周圍障礙物信息，連續(xù)多幀的激光雷達數據所對應的障礙物的位置信息構成了障礙物的狀態(tài)信息。令R表示配置于移動機器人本體的激光雷達，O表示激光雷達感知到的動態(tài)障礙物，動態(tài)障礙物如圖3所示。激光雷達隨移動機器人本體從位置P以速度V向前運動到位置P′時，障礙物O從位置o以速度v向前運動到可能出現的位置o″。

圖3 動態(tài)障礙物Fig.3 Dynamic obstacles

令激光雷達所探測到的動態(tài)障礙物Ok在任意時刻的位置為pk(t)，其中p=(xy)T。根據自回歸(Autoregressive)模型建立動態(tài)障礙物的運動軌跡數學模型。已知動態(tài)障礙物連續(xù)時刻的位置序列值{pk(t),t=1,2,3,…}，則n階自回歸模型為：

(2)

式中，e(t)為預測誤差矩陣；{αi,1

考慮到動態(tài)障礙物Ok的加速度變化較小(且激光雷達采樣間隔時間很短)，用一階自回歸模型表示Ok的加速度ak(t)為：

ak(t)=βk,tak(t-1)+ω(t)，

(3)

式中，ω(t)為預測誤差矩陣；由于移動機器人在二維空間中運動，則βk,t是普通參數矩陣。

假設βk,t與時間不相關，則位置pk(t)、速度vk(t)和加速度ak(t)相互間的關系為：

ak(t)=vk(t)-vk(t-1)=|pk(t)-pk(t-1)|-|pk(t-1)-pk(t-2)|=pk(t)-2pk(t-1)+pk(t-2)，

(4)

于是,可求解出：

pk(t)=(2I+βk,t)pk(t-1)-(2βk,t+I)pk(t-2)+βk,tpk(t-3)+ω(t)。

(5)

由于βk,t不與時間相關，βk,t=βk。利用最小二乘法擬合式(4)得到的加速度序列{ak(t),3≤t≤N} ，于是βk被估計為：

[ak(i)-βkak(i-1)]，

(6)

其解為：

(7)

在βk被估計出結果的前提下，動態(tài)障礙物下一時刻的位置為：

(8)

2.3 障礙層的地圖構建

障礙層onInitialize函數初始化地圖信息，創(chuàng)建觀察緩沖區(qū)，對“LaserScan”“PointCloud”和“PointCloud2”類型的數據分別調用相應的回調函數來處理各自對應的激光雷達數據，處理結束后被添加到標記觀察緩沖區(qū)。障礙層updateBounds函數更新邊界，獲取標記觀察和清除觀察數據，并更新當前狀態(tài)信息；調用raytraceFreespace函數清理出激光雷達到障礙物間的自由活動區(qū)域，處理越界的障礙物點云數據后將新觀測到障礙物信息添加到觀察緩沖區(qū)；調用updateFootprint函數更新機器人輪廓，以及updateCosts函數采用最大值更新或覆蓋更新將障礙物信息更新到mastermap。

經典算法在調用回調函數處理激光雷達數據的過程中，并未對動態(tài)環(huán)境的運動物體進行特殊處理，使得對運動物體的觀測信息存在延時，導致地圖信息更新不及時。為使障礙物層能處理運動物體，對激光雷達數據進行動態(tài)障礙物的檢測，若激光雷達數據檢測為動態(tài)障礙物，則根據歷史雷達數據進行運動軌跡估計，便可在當前幀的點云數據中融合下一幀點云的運動估計數據，進而將下一幀動態(tài)障礙物信息轉換成當前幀的靜態(tài)障礙物信息并融合進多層代價地圖；若激光雷達數據檢測為靜態(tài)障礙物，采用經典的障礙層構建算法進行障礙層地圖的構建。

動態(tài)障礙物的信息融合進多層代價地圖的方式采用疊加值更新法，具體為：

(9)

式中，cno為未知狀態(tài)下的代價值；cins為障礙物位于移動機器人底盤內切圓內的代價值；cstat(i,j)為靜態(tài)地圖層中柵格坐標(i,j)對應的代價值；cdyn(i,j)為柵格坐標(i,j)處動態(tài)障礙物所對應的代價值；cmas(i,j)為總的多層代價地圖中柵格坐標(i,j)對應的代價值。

3 仿真實驗與結果驗證

為實現所提出的面向動態(tài)環(huán)境的多層代價地圖構建算法，在ROS Navigation中costmap_2d功能包的obstacle_layer插件增加動態(tài)障礙物的檢測、運動預測和融合算法進行實驗測試。采用ROS Navigation 導航功能框架，在Gazebo中搭建動態(tài)仿真環(huán)境，如圖4所示。已建好的靜態(tài)代價地圖如圖5所示，在動態(tài)環(huán)境(動態(tài)障礙物速度為0.05 m/s≤vd≤0.15 m/s)下進行實驗。所使用的導航功能框架運行在Ubuntu16.04操作系統(tǒng)，ROS版本為Kinetic，CPU的主頻2.2 GHz，運行內存8 GB。

圖4 Gazebo動態(tài)仿真環(huán)境Fig.4 Gazebo dynamic simulation environment

圖5 靜態(tài)代價地圖Fig.5 Static costmap

保證移動機器人和激光雷達在位姿恒定不變的前提下，動態(tài)障礙物在距離移動機器人6 m的位置(位于激光雷達掃描的范圍以外)以0.1 m/s的速度向機器人底盤中心運動，激光雷達跟蹤觀察代價地圖中動態(tài)障礙物的信息變化。Gazebo環(huán)境情況及代價地圖的信息，如圖6所示(從左至右分別為Gazebo環(huán)境模型、經典算法構建的多層代價地圖和本文算法構建的多層代價地圖)。此時代價地圖并未出現動態(tài)障礙物的信息，只存在有3個靜態(tài)障礙物和1個移動機器人模型。

圖6 Gazebo仿真環(huán)境與多層代價地圖的初始狀態(tài)Fig.6 Gazebo simulation environment and the initial state of layered costmaps

當動態(tài)障礙物進入激光雷達掃描范圍并逐漸接近移動機器人底盤中心時，Gazebo仿真環(huán)境如圖7(a)所示。在該仿真環(huán)境下，經典算法構建的多層代價地圖，如圖7(b)所示；本文算法構建的多層代價地圖，如圖7(c)所示。圖7(b)的紅色線圈出的區(qū)域(被藍色區(qū)域環(huán)繞的紅色標記點對應環(huán)境中障礙物的點云點)為未優(yōu)化的ROS Navigation 導航功能框架對當前激光雷達掃描周期內的障礙物信息進行更新，并將更新的障礙物信息標記在多層代價地圖中。由于動態(tài)障礙物在未來時刻的運動軌跡信息并未融合到激光雷達掃描周期內的點云集中，經典算法未對動態(tài)障礙物的運動軌跡進行特殊處理。圖7(c)的紅色線圈出的區(qū)域(被藍色區(qū)域環(huán)繞的紅色標記點對應環(huán)境中障礙物的點云點)為優(yōu)化后的ROS Navigation 導航功能框架將未來時刻的動態(tài)障礙物位置信息進行估計后,融合到當前激光雷達掃描周期內的障礙物信息中進行更新，并將其標記在多層代價地圖中。優(yōu)化的算法增加對未來時刻動態(tài)障礙物運動軌跡信息處理，使得多層代價地圖中的標記信息在處理了當前激光雷達掃描周期內的障礙物信息的基礎上，包含了后續(xù)激光雷達掃描周期中將處理的障礙物信息(即圖7(c)的結果在圖7(b)結果的基礎上增加了動態(tài)障礙物運動軌跡估計信息所對應的紅色標記點)。

(a) Gazebo仿真環(huán)境

(b) 經典算法

(c) 本文算法圖7 Gazebo仿真環(huán)境與多層代價地圖對比Fig.7 Comparison of Gazebo simulation environment and layered costmap

通過對比實驗結果可以看出，優(yōu)化后的算法解決了經典算法在多層代價地圖構建過程中存在的匹配性和實時性問題，實現了“運動即可標記”的多層代價地圖。

4 結束語

針對經典多層代價地圖處理動態(tài)環(huán)境的實時性和匹配性問題，本文提出了一種面向動態(tài)環(huán)境的多層代價地圖更新算法，對激光雷達的點云進行距離值匹配，以此從獲取到的環(huán)境信息中檢測出動態(tài)障礙物，接著采用自回歸模型以改變對激光雷達信息的處理方式，使得構建的代價地圖可融合下一幀動態(tài)障礙物的位置信息。研究障礙物層的地圖構建過程，改善了經典地圖構建算法在處理動態(tài)環(huán)境存在的匹配性和實時性問題。根據具有差異性的代價地圖實驗結果，驗證了本文所提出的優(yōu)化方案在更新動態(tài)環(huán)境的地圖信息方面確有明顯改善。后續(xù)工作將在面向動態(tài)環(huán)境結合本算法進行代價地圖構建的基礎上，探索移動機器人導航中更高效的導航規(guī)劃算法。