基于子地圖的智能車輛同步定位與地圖創建*

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

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

?

2015040

基于子地圖的智能車輛同步定位與地圖創建*

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

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

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

智能車輛；同步定位與地圖創建；子地圖；柵格地圖

前言

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

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

為解決這一問題，可以采用子地圖的方式構建全局地圖[6]。全局地圖由相鄰的子地圖序列拼接組成，智能車輛通過新建或調用子地圖以維護一定范圍內的環境信息。全局地圖范圍隨子地圖的動態創建而增加，在任意時刻只需對一張子地圖進行更新。采用子地圖時需要注意兩個問題：首先，在新建或切換子地圖后，新的子地圖中應該保留部分已知地圖信息，以保證地圖信息的連續性和匹配定位結果的準確性；其次要建立恰當的地圖切換機制，避免在兩個相鄰子地圖間連續切換的狀況發生。

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

1 基于粒子濾波的SLAM

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

1.1 地圖創建

1.1.1 柵格地圖創建

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

(1)

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

1.1.2 柵格地圖更新

(2)

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

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

(3)

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

1.2 地圖創建

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

(4)

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

(5)

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

1.3 動態障礙物檢測

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

2 基于子地圖框架的全局地圖創建

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

2.1 全局地圖的子地圖框架

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

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

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

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

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

2.3 子地圖的切換

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

(6)

式中：r為地圖柵格尺寸；n為以柵格數統計的子地圖邊長。

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

3 基于子地圖的同步定位與地圖創建實驗

3.1 實驗平臺

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

3.2 校園環境實驗

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

3.2.1 子地圖的生成

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

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

3.2.2 定位偏差分析

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

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

3.2.3 實時性分析

圖10反映出基于子地圖的SLAM的實時性。圖中橫坐標表示車輛行駛過程中的各個時刻，縱坐標表示采用基于子地圖的同步定位與地圖創建方法在每個定位周期中的時間消耗。實驗中采用的是大小為2500×2500個柵格的子地圖，采用VS2008軟件編寫算法。可以看出，在實驗過程中，絕大多數定位周期均小于50ms。個別定位周期大于50ms，但也小于車輛航跡推算的定位周期100ms，如圖10中虛線所示。

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

3.3 大范圍城市動態環境實驗

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

4 結論

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

[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] 劉洞波,劉國榮,王迎旭,等.基于區間分析無跡粒子濾波的移動機器人SLAM方法[J].農業機械學報,2012,43(10):155-160.

[4] 武二永,項志宇,沈敏一,等.大規模環境下基于激光雷達的機器人SLAM算法[J].浙江大學學報(工學版),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

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

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