胡玉文，龔建偉，姜 巖，熊光明

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

?

2015040

基于子地圖的智能車輛同步定位與地圖創(chuàng)建*

胡玉文，龔建偉，姜 巖，熊光明

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

為使智能車輛能在無法預(yù)先確定環(huán)境范圍的條件下創(chuàng)建環(huán)境柵格地圖并實時定位，提出了一種基于子地圖框架的同步定位與地圖創(chuàng)建方法。在子地圖中設(shè)置重合區(qū)域與切換區(qū)域，以避免相鄰子地圖間的連續(xù)切換。實驗結(jié)果表明，該方法可保證車輛在同步定位與地圖創(chuàng)建過程中地圖范圍的動態(tài)增長和子地圖間的穩(wěn)定切換，具有較高的實時性和定位精度。

智能車輛；同步定位與地圖創(chuàng)建；子地圖；柵格地圖

前言

同步定位與地圖創(chuàng)建(simultaneous localization and mapping, SLAM)是指在未知環(huán)境中創(chuàng)建環(huán)境地圖的同時進行定位的技術(shù)[1-2]。SLAM最早源于機器人研究領(lǐng)域，被譽為實現(xiàn)真正自主的關(guān)鍵技術(shù)，并衍生出采用不同技術(shù)實現(xiàn)SLAM的方案[3-5]。

在智能車輛研究中SLAM也是熱點。一方面精確定位有助于智能車輛的實時控制，另一方面實時生成的環(huán)境地圖，特別是柵格地圖的創(chuàng)建，有助于智能車輛進行全局和局部的路徑規(guī)劃。但當(dāng)無法預(yù)先確定行駛范圍或車輛進行長距離行駛時，在地圖創(chuàng)建過程中車輛位置可能超過柵格地圖所能表示的最大邊界，造成算法出現(xiàn)越界錯誤。

為解決這一問題，可以采用子地圖的方式構(gòu)建全局地圖[6]。全局地圖由相鄰的子地圖序列拼接組成，智能車輛通過新建或調(diào)用子地圖以維護一定范圍內(nèi)的環(huán)境信息。全局地圖范圍隨子地圖的動態(tài)創(chuàng)建而增加，在任意時刻只需對一張子地圖進行更新。采用子地圖時需要注意兩個問題：首先，在新建或切換子地圖后，新的子地圖中應(yīng)該保留部分已知地圖信息，以保證地圖信息的連續(xù)性和匹配定位結(jié)果的準(zhǔn)確性；其次要建立恰當(dāng)?shù)牡貓D切換機制，避免在兩個相鄰子地圖間連續(xù)切換的狀況發(fā)生。

針對以上問題，本文中提出了一種基于子地圖框架的粒子濾波SLAM方法，通過子地圖的方式實現(xiàn)在戶外大范圍環(huán)境中環(huán)境柵格地圖的創(chuàng)建以及地圖范圍的動態(tài)增長。通過對相鄰的兩張子地圖間設(shè)置重合部分，保證子地圖切換后新子地圖中保留了舊子地圖中的部分環(huán)境信息。并進一步對重合區(qū)域進行細分，設(shè)置切換區(qū)域，保證在子地圖的創(chuàng)建與調(diào)用過程中不會出現(xiàn)在兩張相鄰子地圖間連續(xù)切換。

1 基于粒子濾波的SLAM

基于粒子濾波的方法將SLAM過程分為地圖創(chuàng)建和定位兩個過程進行。在地圖創(chuàng)建過程中假設(shè)已知車輛位置，根據(jù)環(huán)境傳感器數(shù)據(jù)和相應(yīng)的模型對地圖狀態(tài)進行維護。在定位過程中對位置的估計采用粒子濾波方法，通過激光雷達數(shù)據(jù)與已知地圖進行地圖匹配，估計車輛位姿的后驗概率分布。

1.1 地圖創(chuàng)建

1.1.1 柵格地圖創(chuàng)建

本文中通過激光雷達建立環(huán)境柵格地圖[7]。在柵格地圖M中，環(huán)境被等分為二維柵格m。每個柵格都關(guān)聯(lián)一個數(shù)值p(m)(p(m)∈[0,1])描述了柵格m中存在障礙物的概率，即

(1)

式中：OCC表示柵格m被占據(jù)；UNKOWN表示柵格m狀態(tài)未知；EMP表示柵格m中不存在障礙物。初始時刻為對環(huán)境進行觀測時，地圖初始化為p(m)=0.5。

1.1.2 柵格地圖更新

(2)

式中：t為數(shù)據(jù)采集時刻；X表示車輛位姿；Z表示激光雷達測量數(shù)據(jù)。

式(2)表示智能車輛根據(jù)其在不同時刻位置上對環(huán)境的測量結(jié)果對地圖柵格中存在障礙物的概率進行更新的過程。由于式(2)的計算需要記錄大量的歷史數(shù)據(jù)，根據(jù)文獻[8]中的推導(dǎo)，對地圖柵格進行概率更新的遞歸公式為

(3)

式中：p(M|X1:t-1,Z1:t-1)為t-1時刻的地圖柵格概率；p(M|Xt,Zt)為t時刻激光雷達測量的柵格概率。使用激光雷達時可通過反式傳感器模型采用光線追蹤方法計算，如圖1(a)所示。圖1(b)中黑色為障礙物，灰色區(qū)域為雷達掃描范圍。圖1(c)中黑色表示柵格位置存在障礙物，白色表示柵格中不存在障礙物，灰色表示柵格狀態(tài)未知。

1.2 地圖創(chuàng)建

SLAM中的定位通過激光雷達數(shù)據(jù)與已知環(huán)境地圖間的匹配對車輛位姿進行修正，其過程可看作對車輛位姿最優(yōu)估計問題的求解，表示為

(4)

在獲得預(yù)測的粒子集分布以及粒子權(quán)重后，通過地圖匹配對粒子的權(quán)重進行更新。地圖匹配則通過將激光雷達的觀測數(shù)據(jù)以每個粒子所表示的位姿投影到地圖中進行。本文中采用end-point方法[11]

(5)

通過式(4)計算并歸一化后，可得到地圖匹配后的粒子權(quán)重，權(quán)重越大表示該粒子越接近真實車輛位姿。定位結(jié)果以權(quán)重最大的粒子所表示的位姿輸出。定位結(jié)束后通過對粒子重采樣避免樣本集的衰竭。

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

由于SLAM過程假設(shè)環(huán)境為靜態(tài)，而在真實環(huán)境中不可避免地存在動態(tài)目標(biāo)，如行人和車輛等。在基于概率方法的地圖更新過程中，動態(tài)目標(biāo)對于單個柵格的狀態(tài)影響時間極短，一般不會對其狀態(tài)(OCC/EMP/UNKOWN)造成影響。為進一步消除動態(tài)障礙物的影響，在地圖匹配過程中對激光雷達數(shù)據(jù)的來源進行了判斷：在匹配過程中，當(dāng)激光雷達的測量點所在柵格狀態(tài)為OCC時，標(biāo)記該測量來源于動態(tài)目標(biāo)。被標(biāo)記的測量點將不參與地圖的更新。

2 基于子地圖框架的全局地圖創(chuàng)建

任何形式的地圖都受其范圍限制，只能表達范圍內(nèi)的環(huán)境信息。當(dāng)車輛位置超出地圖邊界時，將會導(dǎo)致SLAM算法出現(xiàn)越界錯誤。因此在地圖創(chuàng)建過程中，采用子地圖的方式組建全局地圖，并對子地圖間的切換機制進行了研究。

2.1 全局地圖的子地圖框架

本文中坐標(biāo)系定義為：將車輛起點定義為全局地圖的原點，以正東方向為x軸正方向，正北方向為y軸正方向，建立全局地圖坐標(biāo)系。坐標(biāo)軸上的單位長度等于相鄰子地圖中心間的距離。子地圖如圖2所示。在子地圖框架下，全局柵格地圖M由子地圖序列{…,M(i,j),…}構(gòu)成。子地圖M(i,j)為柵格地圖中一系列相鄰柵格的集合，(i,j)表示子地圖中心在全局地圖坐標(biāo)系中的位置，為邊長是n個柵格的正方形，例如，車輛起點位置所在子地圖為M(0,0)。子地圖在全局地圖中等間距分布，相鄰子地圖中心沿x軸和y軸的柵格距離均為d。通過這樣的設(shè)置，每張子地圖都具有其唯一的編號即中心位置。這些編號可用于在地圖創(chuàng)建過程中對已生成子地圖的檢索，或通過編號所表示的相對位置關(guān)系重組全局柵格地圖。在地圖創(chuàng)建過程中任意時刻只須對當(dāng)前子地圖進行更新。

如圖2所示，相鄰子地圖間均存在一定程度的重合區(qū)域。重合區(qū)域大小主要參考激光雷達的有效測量范圍。當(dāng)從子地圖M(i-1,j)切換到M(i,j)時，M(i,j)保留了重合區(qū)域中M(i-1,j)的環(huán)境信息，即柵格被占據(jù)的概率。這樣在地圖切換后仍保留了車輛當(dāng)前位置周圍的歷史環(huán)境信息，避免由于子地圖切換導(dǎo)致環(huán)境信息丟失而增大地圖匹配定位的誤差。

2.2 相鄰子地圖間的切換機制

子地圖切換的基本思路是：當(dāng)車輛進入子地圖重合區(qū)域時，則切換至相應(yīng)的相鄰子地圖。但在算法測試過程中發(fā)現(xiàn)，這樣簡單的設(shè)置容易導(dǎo)致在相鄰子地圖間連續(xù)切換。如圖3所示，令V表示車輛所在位置，且車輛保持向右的運動。子地圖中灰色為相鄰子地圖M(i-1,j)與M(i,j)間的重合區(qū)域。如果簡單地以車輛進入重合區(qū)域作為判斷是否進行切換的標(biāo)志，當(dāng)車輛從M(i-1,j)進入重合區(qū)域，則子地圖切換到M(i,j)。下一時刻，車輛位于M(i,j)中的重合區(qū)域，則繼續(xù)切換回M(i-1,j)。這樣就造成了在兩張子地圖間的相互切換，導(dǎo)致子地圖切換的失敗。

為避免這種情況的出現(xiàn)，須為相鄰子地圖分別設(shè)置用于判斷是否切換子地圖的切換區(qū)域，如圖4所示，同樣灰色為M(i-1,j)與M(i,j)的重合區(qū)域，其中1區(qū)與3區(qū)重合，2區(qū)與4區(qū)重合。將2區(qū)和3區(qū)分別設(shè)為M(i-1,j)和M(i,j)的切換區(qū)域。當(dāng)車輛位于1區(qū)時，不進行切換操作。當(dāng)車輛進入2區(qū)時，執(zhí)行切換操作并切換到M(i,j)。切換后車輛位于4區(qū)中，不是M(i,j)的切換區(qū)域，保證了在一次子地圖切換過程中的單向性。

2.3 子地圖的切換

由于車輛的行駛可以朝向任何方向，因此完整的子地圖切換機制還應(yīng)能夠確定所切換的新子地圖坐標(biāo)。本文中通過車輛位置相對子地圖中心的方向判斷應(yīng)創(chuàng)建或調(diào)用的子地圖坐標(biāo)。假設(shè)t時刻車輛相對起點位置為(x,y)，且車輛定位的坐標(biāo)系與坐標(biāo)軸方向與地圖坐標(biāo)系重合。令當(dāng)前子地圖為M(i,j)，且車輛已經(jīng)處于該子地圖的切換區(qū)域中，則下一時刻應(yīng)切換至的子地圖坐標(biāo)(l,k)為

(6)

式中：r為地圖柵格尺寸；n為以柵格數(shù)統(tǒng)計的子地圖邊長。

獲得子地圖坐標(biāo)(l,k)后，子地圖的操作包括創(chuàng)建和調(diào)用兩種方式。在地圖創(chuàng)建過程中，算法同時維護了子地圖坐標(biāo)列表listt={…,(i,j),…}。通過對listt進行搜索，如果(l,k)不存在列表中，則新建子地圖M(l,k)，同時將坐標(biāo)(l,k)加入坐標(biāo)列表中；如果(l,k)已存在，則調(diào)用對應(yīng)的子地圖信息。最后將M(i,j)與M(l,k)重合部分的信息復(fù)制到M(l,k)中對應(yīng)的區(qū)域。

3 基于子地圖的同步定位與地圖創(chuàng)建實驗

3.1 實驗平臺

如圖5所示，本文中的實驗平臺為基于豐田陸地巡洋艦改造的BIT號智能車輛，二維激光雷達使用德國施克公司生產(chǎn)的LMS291，具有80m/180°的掃描范圍和0.25°的角分辨率。并通過里程計和慣性導(dǎo)航設(shè)備進行航跡推算，獲得車輛行駛距離以及航向信息。

3.2 校園環(huán)境實驗

實驗中車輛進行同步定位與地圖創(chuàng)建。車輛運動的起、終點和行駛路線如圖6所示。整個過程中車輛行駛距離約為1.9km，地圖范圍約為370m×600m。場景中包含樹木和建筑物等靜態(tài)物體，以及少量行人和車輛等運動目標(biāo)。

3.2.1 子地圖的生成

圖7為在圖6所示場景中創(chuàng)建的子地圖序列，圖中坐標(biāo)為各個子地圖的坐標(biāo),箭頭表示在SLAM過程中子地圖的訪問順序，M(0,0)為起點地圖。在地圖創(chuàng)建過程中總共建立6張子地圖。由于實驗過程中起點與終點位置重合，M(0,0)在創(chuàng)建后重新調(diào)用了一次。實驗中當(dāng)對已存在的子地圖進行調(diào)用時，只根據(jù)SLAM的定位結(jié)果和傳感器數(shù)據(jù)對地圖進行更新，不另作路徑的閉環(huán)處理。

圖8為通過基于子地圖框架的同步定位與地圖創(chuàng)建過程后，由子地圖序列組建的全局環(huán)境柵格地圖。比較圖8與圖6可以看出，建立的全局地圖與實際實驗環(huán)境具有較好的一致性。

3.2.2 定位偏差分析

圖9中比較了航跡推算和基于子地圖的SLAM的定位結(jié)果。圖中坐標(biāo)系以正東方向為x軸正方向，正北方向為y軸正方向，以航跡推算的起點為原點。通過采用所提出的基于子地圖框架的同步定位與地圖創(chuàng)建方法，車輛行駛過程中航跡推算的定位結(jié)果得到了不斷修正，如圖9(a)所示。

圖9(b)以航跡推算定位結(jié)果為基準(zhǔn)，比較了航跡推算的定位結(jié)果與基于子地圖的SLAM定位在數(shù)據(jù)采集過程中沿坐標(biāo)軸方向的偏差dx和dy。由于研究中的航跡推算是根據(jù)車輛運動的距離和航向變化計算相鄰時刻間的直線距離，其累計誤差隨著車輛的行駛，特別是車輛的轉(zhuǎn)向動作而發(fā)生相對較大的變化。dx和dy的變化趨勢與之相同。在實驗最后運動路徑閉合處即起、終點，dx和dy分別為-1.2和-4.7m。

3.2.3 實時性分析

圖10反映出基于子地圖的SLAM的實時性。圖中橫坐標(biāo)表示車輛行駛過程中的各個時刻，縱坐標(biāo)表示采用基于子地圖的同步定位與地圖創(chuàng)建方法在每個定位周期中的時間消耗。實驗中采用的是大小為2500×2500個柵格的子地圖，采用VS2008軟件編寫算法?？梢钥闯觯趯嶒炦^程中，絕大多數(shù)定位周期均小于50ms。個別定位周期大于50ms，但也小于車輛航跡推算的定位周期100ms，如圖10中虛線所示。

由于軟件編寫采用單線程的方式，因此子地圖的切換過程與定位過程使用同一進程。大于100ms的6個定位周期分別對應(yīng)于在實驗過程中的6次子地圖切換，包含了子地圖調(diào)用、存儲和重合區(qū)域復(fù)制等操作。

3.3 大范圍城市動態(tài)環(huán)境實驗

本實驗為在較大范圍城市動態(tài)環(huán)境中同步定位與地圖創(chuàng)建實驗。車輛行駛距離約為4.4km,地圖范圍約為1km×1km。行駛環(huán)境中存在較多的車輛等動態(tài)目標(biāo)。其它條件與校園環(huán)境實驗相同，如圖11所示。在未改變程序和參數(shù)的條件下，采用子地圖框架的同步定位與地圖創(chuàng)建實現(xiàn)了地圖范圍的動態(tài)增長。

4 結(jié)論

提出一種基于子地圖框架的全局定位與柵格地圖創(chuàng)建方法，通過子地圖的形式解決了在車輛行駛過程中全局地圖范圍的動態(tài)增長。同時設(shè)計了子地圖間的切換機制，避免了在相鄰子地圖間連續(xù)切換的狀況出現(xiàn)。最后通過在不同環(huán)境范圍內(nèi)的地圖創(chuàng)建實驗驗證了該方案的可行性，能夠?qū)崿F(xiàn)在不預(yù)先設(shè)置行駛范圍的前提下創(chuàng)建全局柵格地圖，并具有較好的實時性。在以后的研究中應(yīng)進一步研究地圖的組織與存儲形式，提高地圖調(diào)用的效率；同時須加強對環(huán)境中動態(tài)障礙物的檢測和跟蹤，以保證SLAM環(huán)境靜止的假設(shè)。

[1]Durrant-WhyteH,BaileyT.SimultaneousLocalizationandMapping(SLAM):PartI[J].IEEERoboticsandAutomationMagazine,2006,13(2):99-108.

[2]BaileyT,Durrant-WhyteH.SimultaneousLocalizationandMapping(SLAM):PartII[J].IEEERoboticsandAutomationMagazine,2006,13(3):108-117.

[3] 劉洞波,劉國榮,王迎旭,等.基于區(qū)間分析無跡粒子濾波的移動機器人SLAM方法[J].農(nóng)業(yè)機械學(xué)報,2012,43(10):155-160.

[4] 武二永,項志宇,沈敏一,等.大規(guī)模環(huán)境下基于激光雷達的機器人SLAM算法[J].浙江大學(xué)學(xué)報(工學(xué)版),2007,41(12):1982-1986.

[5]BarkbyS,WilliamsSB,PizarroO,etal.AFeaturelessApproachtoEfficientBathymetricSLAMUsingDistributedParticleMapping[J].JournalofFieldRobotics,2011,28(1):19-39.

[6]KonradM,SzczotM,SchuleF,etal.GenericGridMappingforRoadCourseEstimation[C].IEEEIntelligentVehiclesSymposium,2011:851-856.

[7]ElfesA.UsingOccupancyGridsforMobileRobotPerceptionandNavigation[J].Computer,1989,22(6):46-57.

[8]WeissT,SchieleB,DietmayerK.RobustDrivingPathDetectioninUrbanandHighwayScenariosUsingaLaserScannerandOnlineOccupancyGrids[C].IEEEIntelligentVehiclesSymposium,2007:184-189.

[9] 胡士強,敬忠良.粒子濾波算法綜述[J].控制與決策,2005,20(4):361-365.

[10]ThrunS.ProbabilisticRobotics[J].CommunicationsoftheACM,2002,45(3):52-57.

[11]Trung-DungV,BurletJ,AycardO.Grid-basedLocalizationandLocalMappingwithMovingObjectDetectionandTracking[J].InformationFusion,2011,12(1):58-69.

A Sub-map-based Simultaneous Localization andMapping Technique for Intelligent Vehicles

Hu Yuwen, Gong Jianwei, Jiang Yan & Xiong Guangming

SchoolofMechanicalEngineering,BeijingInstituteofTechnology,Beijing100081

For enabling intelligent vehicle to create environmental grid map and achieve real-time positioning in a condition with unpredictable environment scope, a simultaneous localization and mapping (SLAM) technique base on sub-map frame is proposed.The overlapped zones and switching zones are set in sub-maps to avoid endless successive switching of adjacent sub-maps.The results of experiment show that the technique proposed can ensure the dynamic growth of map scope and stable switching between sub-maps in SLAM process of vehicle.

intelligent vehicle; SLAM; sub-map; grid maps

*國家自然科學(xué)基金(91120010)、教育部博士點基金(20121101120015)和北京理工大學(xué)基礎(chǔ)研究基金(20120342011)資助。

原稿收到日期為2013年4月6日，修改稿收到日期為2013年7月3日。