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

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

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

?

2015040

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

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

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

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

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

前言

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

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

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

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

1 基于粒子濾波的SLAM

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

1.1 地圖創(chuàng)建

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

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

(1)

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

1.1.2 柵格地圖更新

(2)

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

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

(3)

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

1.2 地圖創(chuàng)建

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

(4)

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

(5)

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

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

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

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

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

2.1 全局地圖的子地圖框架

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

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

2.2 相鄰子地圖間的切換機(jī)制

子地圖切換的基本思路是：當(dāng)車輛進(jìn)入子地圖重合區(qū)域時(shí)，則切換至相應(yīng)的相鄰子地圖。但在算法測(cè)試過(guò)程中發(fā)現(xiàn)，這樣簡(jiǎn)單的設(shè)置容易導(dǎo)致在相鄰子地圖間連續(xù)切換。如圖3所示，令V表示車輛所在位置，且車輛保持向右的運(yùn)動(dòng)。子地圖中灰色為相鄰子地圖M(i-1,j)與M(i,j)間的重合區(qū)域。如果簡(jiǎn)單地以車輛進(jìn)入重合區(qū)域作為判斷是否進(jìn)行切換的標(biāo)志，當(dāng)車輛從M(i-1,j)進(jìn)入重合區(qū)域，則子地圖切換到M(i,j)。下一時(shí)刻，車輛位于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ū)時(shí)，不進(jìn)行切換操作。當(dāng)車輛進(jìn)入2區(qū)時(shí)，執(zhí)行切換操作并切換到M(i,j)。切換后車輛位于4區(qū)中，不是M(i,j)的切換區(qū)域，保證了在一次子地圖切換過(guò)程中的單向性。

2.3 子地圖的切換

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

(6)

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

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

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

3.1 實(shí)驗(yàn)平臺(tái)

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

3.2 校園環(huán)境實(shí)驗(yàn)

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

3.2.1 子地圖的生成

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

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

3.2.2 定位偏差分析

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

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

3.2.3 實(shí)時(shí)性分析

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

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

3.3 大范圍城市動(dòng)態(tài)環(huán)境實(shí)驗(yàn)

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

4 結(jié)論

提出一種基于子地圖框架的全局定位與柵格地圖創(chuàng)建方法，通過(guò)子地圖的形式解決了在車輛行駛過(guò)程中全局地圖范圍的動(dòng)態(tài)增長(zhǎng)。同時(shí)設(shè)計(jì)了子地圖間的切換機(jī)制，避免了在相鄰子地圖間連續(xù)切換的狀況出現(xiàn)。最后通過(guò)在不同環(huán)境范圍內(nèi)的地圖創(chuàng)建實(shí)驗(yàn)驗(yàn)證了該方案的可行性，能夠?qū)崿F(xiàn)在不預(yù)先設(shè)置行駛范圍的前提下創(chuàng)建全局柵格地圖，并具有較好的實(shí)時(shí)性。在以后的研究中應(yīng)進(jìn)一步研究地圖的組織與存儲(chǔ)形式，提高地圖調(diào)用的效率；同時(shí)須加強(qiáng)對(duì)環(huán)境中動(dòng)態(tài)障礙物的檢測(cè)和跟蹤，以保證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] 劉洞波,劉國(guó)榮,王迎旭,等.基于區(qū)間分析無(wú)跡粒子濾波的移動(dòng)機(jī)器人SLAM方法[J].農(nóng)業(yè)機(jī)械學(xué)報(bào),2012,43(10):155-160.

[4] 武二永,項(xiàng)志宇,沈敏一,等.大規(guī)模環(huán)境下基于激光雷達(dá)的機(jī)器人SLAM算法[J].浙江大學(xué)學(xué)報(bào)(工學(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] 胡士強(qiáng),敬忠良.粒子濾波算法綜述[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

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

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