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

李德勝，孫一杰，張國(guó)良

(1.四川輕化工大學(xué) 自動(dòng)化與信息工程學(xué)院，四川 宜賓 644000；2.人工智能四川省重點(diǎn)實(shí)驗(yàn)室，四川 宜賓 644000)

0 引言

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

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

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

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

1 多層代價(jià)地圖

1.1 多層代價(jià)地圖的代價(jià)值

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

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

(1)

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

1.2 多層代價(jià)地圖的組成與更新

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

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

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

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

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

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

1.3 匹配性和實(shí)時(shí)性問(wèn)題

經(jīng)典障礙物層的代價(jià)地圖構(gòu)建過(guò)程存在一定的局限性。采取Bresenham 光線追蹤算法來(lái)對(duì)周圍環(huán)境空間存在的障礙物信息進(jìn)行標(biāo)記和清除。具體過(guò)程為激光雷達(dá)對(duì)周圍環(huán)境情況進(jìn)行數(shù)據(jù)探測(cè)，若探測(cè)到致命障礙物，就將其標(biāo)記并插入到代價(jià)地圖中；清除操作根據(jù)數(shù)據(jù)探測(cè)結(jié)果，從激光雷達(dá)的原點(diǎn)向四周進(jìn)行光線跟蹤，對(duì)代價(jià)地圖的障礙信息采取刪除處理。

當(dāng)所探測(cè)的周圍環(huán)境出現(xiàn)動(dòng)態(tài)障礙物時(shí)，即動(dòng)態(tài)障礙物處于運(yùn)動(dòng)的狀態(tài)(動(dòng)態(tài)障礙物的位置隨時(shí)都可能發(fā)生變化)，激光雷達(dá)只能處理當(dāng)前時(shí)刻動(dòng)態(tài)障礙物所存在的位置信息，而無(wú)法處理動(dòng)態(tài)障礙物在未來(lái)時(shí)刻的位置信息，進(jìn)而無(wú)法對(duì)動(dòng)態(tài)障礙物在未來(lái)時(shí)刻的位置信息進(jìn)行標(biāo)記。這將影響多層代價(jià)地圖的構(gòu)建過(guò)程，出現(xiàn)以下不理想的狀況。

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

(a) 時(shí)刻t

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

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

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

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

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

2.1 動(dòng)態(tài)障礙物的檢測(cè)

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

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

① 當(dāng)位移值小于v1·Δt時(shí)，表示幀B中的點(diǎn)云與幀A的點(diǎn)云為同一靜態(tài)障礙物，此時(shí)將其標(biāo)記為static(即靜態(tài)障礙物，性質(zhì)已知)；

② 當(dāng)位移值大于或者等于v1·Δt且小于或者等于v2·Δt時(shí)，表示幀B中的點(diǎn)云與幀A的點(diǎn)云來(lái)自同一動(dòng)態(tài)障礙物，此時(shí)將其標(biāo)記為dynamic(即動(dòng)態(tài)障礙物，性質(zhì)已知)；

③ 當(dāng)位移值大于v2·Δt時(shí)，表示幀B中的點(diǎn)云與幀A的點(diǎn)云為不同障礙物，此時(shí)將其標(biāo)記為new(即新增的障礙物，性質(zhì)未知)。

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

2.2 動(dòng)態(tài)障礙物的運(yùn)動(dòng)軌跡估計(jì)

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

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

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

(2)

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

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

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

(3)

式中，ω(t)為預(yù)測(cè)誤差矩陣；由于移動(dòng)機(jī)器人在二維空間中運(yùn)動(dòng)，則βk,t是普通參數(shù)矩陣。

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

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不與時(shí)間相關(guān)，βk,t=βk。利用最小二乘法擬合式(4)得到的加速度序列{ak(t),3≤t≤N} ，于是βk被估計(jì)為：

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

(6)

其解為：

(7)

在βk被估計(jì)出結(jié)果的前提下，動(dòng)態(tài)障礙物下一時(shí)刻的位置為：

(8)

2.3 障礙層的地圖構(gòu)建

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

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

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

(9)

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

3 仿真實(shí)驗(yàn)與結(jié)果驗(yàn)證

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

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

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

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

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

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

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

(b) 經(jīng)典算法

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

通過(guò)對(duì)比實(shí)驗(yàn)結(jié)果可以看出，優(yōu)化后的算法解決了經(jīng)典算法在多層代價(jià)地圖構(gòu)建過(guò)程中存在的匹配性和實(shí)時(shí)性問(wèn)題，實(shí)現(xiàn)了“運(yùn)動(dòng)即可標(biāo)記”的多層代價(jià)地圖。

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

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