鄒筠珍，趙 偉，許舒晨，孫永榮

(南京航空航天大學(xué)自動(dòng)化學(xué)院，南京 210016)

0 引言

隨著自動(dòng)駕駛汽車的發(fā)展，作為前沿發(fā)展技術(shù)之一的自定位技術(shù)成為一個(gè)熱門的研究方向，特別是在全球定位系統(tǒng)(Global Positioning System，GPS)拒止環(huán)境下的自定位算法是需要研究的重點(diǎn)。近年來(lái)，激光雷達(dá)里程計(jì)(LiDAR Odometry，LO)的應(yīng)用逐漸興起，相對(duì)于視覺(jué)定位來(lái)說(shuō)，激光雷達(dá)可以提取到深度信息，同時(shí)擺脫了對(duì)光照環(huán)境的依賴，在車輛定位的應(yīng)用方面具有一定的優(yōu)越性。

LO以激光雷達(dá)點(diǎn)云信息作為輸入，利用幀間匹配實(shí)現(xiàn)車輛定位，但是長(zhǎng)距離行駛產(chǎn)生累積漂移的問(wèn)題不容忽視。目前主要有三種解決方式：多源傳感器融合、同步構(gòu)建地圖以及利用已有地圖進(jìn)行定位校正。多源傳感器融合方式[1-2]雖然定位精度較高，但融合前標(biāo)定的傳感器外參容易受到機(jī)械變形及環(huán)境影響，數(shù)據(jù)處理耗時(shí)長(zhǎng)、操作復(fù)雜且成本昂貴[3]。而同步定位與建圖(Simultaneous Localization and Mapping，SLAM)方法[4-5]需要進(jìn)行全局的地圖構(gòu)建并利用回環(huán)檢測(cè)來(lái)消除誤差[6]，極大地限制了車輛的運(yùn)行軌跡。利用已有地圖進(jìn)行定位校正方式的低成本、易操作的特點(diǎn)，使得其在解決里程計(jì)累積漂移的問(wèn)題時(shí)更具有優(yōu)勢(shì)。已有地圖主要包括點(diǎn)云地圖和路網(wǎng)地圖等。點(diǎn)云地圖存儲(chǔ)需要較大的內(nèi)存，且沒(méi)有開(kāi)源的數(shù)據(jù)，需要提前構(gòu)建；相較而言，路網(wǎng)地圖例如開(kāi)放街道地圖(OpenStreetMap，OSM)，由于其開(kāi)源及輕量級(jí)的特點(diǎn)，能減少系統(tǒng)所需成本及內(nèi)存。文獻(xiàn)[7]和文獻(xiàn)[8]利用OSM路網(wǎng)及建筑信息構(gòu)建特征描述子，并利用激光點(diǎn)云信息進(jìn)行定位，但OSM建筑信息及描述子存儲(chǔ)增加了額外的內(nèi)存。文獻(xiàn)[9]僅利用OSM路網(wǎng)信息，將里程計(jì)與路網(wǎng)數(shù)據(jù)結(jié)合于統(tǒng)一的框架中，利用基于快速定向倒角匹配的蒙特卡羅定位對(duì)軌跡的每一幀進(jìn)行匹配，能在更低內(nèi)存的情況下達(dá)到類似的定位精度，但其在里程計(jì)軌跡失真的情況下，效果并不佳。文獻(xiàn)[10]利用軌跡與地圖進(jìn)行匹配，依據(jù)拐點(diǎn)通過(guò)距離閾值約束并選擇最佳路線，避免了里程計(jì)誤差發(fā)散，但是對(duì)于長(zhǎng)路徑來(lái)說(shuō)，窮盡搜索會(huì)變得極其耗時(shí)，而且將原有軌跡固連至路網(wǎng)上將會(huì)帶來(lái)額外的誤差。文獻(xiàn)[11]和文獻(xiàn)[12]在此基礎(chǔ)上考慮了路寬的影響，采用多源粒子濾波方法進(jìn)行軌跡匹配，相對(duì)于文獻(xiàn)[10]來(lái)說(shuō)誤差有了進(jìn)一步優(yōu)化，但是以事后推定的方式計(jì)算軌跡-路網(wǎng)匹配關(guān)系，不具有實(shí)時(shí)性。不過(guò)文獻(xiàn)[11]和文獻(xiàn)[12]利用拐點(diǎn)進(jìn)行濾波的思想，極大地節(jié)約了定位成本，且達(dá)到不錯(cuò)的定位效果，此思想為本文的算法提供了啟發(fā)。

綜合上述分析，本文提出了一種基于路網(wǎng)節(jié)點(diǎn)的粒子濾波優(yōu)化算法，將LO及OSM數(shù)據(jù)作為輸入，并依據(jù)軌跡的拐點(diǎn)進(jìn)行粒子濾波，對(duì)LO的累積誤差實(shí)現(xiàn)有效的抑制，在滿足校正精度的同時(shí)避免頻繁使用濾波器，且可以滿足系統(tǒng)的實(shí)時(shí)性要求。

1 基于路網(wǎng)節(jié)點(diǎn)的粒子濾波系統(tǒng)框架設(shè)計(jì)

本文選用LeGO-LOAM的前端[13]作為L(zhǎng)O，平面點(diǎn)和邊緣點(diǎn)作為激光雷達(dá)點(diǎn)云圖像中顯著的特征點(diǎn)，可以通過(guò)曲率的大小對(duì)其進(jìn)行分別提取，并進(jìn)行特征匹配，實(shí)現(xiàn)實(shí)時(shí)定位。但是在沒(méi)有回環(huán)檢測(cè)等后端處理的情況下，不可避免地會(huì)出現(xiàn)誤差逐漸發(fā)散。為了補(bǔ)償LO隨著時(shí)間推移而積累的漂移，本文使用輕量級(jí)地圖OSM以及軌跡拐點(diǎn)信息對(duì)LO定位誤差進(jìn)行校正，基于路網(wǎng)節(jié)點(diǎn)的粒子濾波系統(tǒng)的結(jié)構(gòu)框架如圖1所示。

本系統(tǒng)主要包含三部分，分別是利用LO獲取位姿、拐點(diǎn)提取及匹配和利用粒子濾波器對(duì)拐點(diǎn)進(jìn)行定位校正。首先，將LO的位姿作為運(yùn)動(dòng)方程對(duì)粒子進(jìn)行預(yù)測(cè)；同時(shí)利用航向角信息對(duì)運(yùn)動(dòng)軌跡進(jìn)行拐點(diǎn)提取，并與OSM地圖節(jié)點(diǎn)進(jìn)行匹配；然后，當(dāng)拐點(diǎn)被提取出來(lái)后，根據(jù)其與地圖節(jié)點(diǎn)的匹配程度更新粒子權(quán)重實(shí)現(xiàn)濾波，獲得粒子的新位姿；最后將其均值作為車輛的真實(shí)位姿，實(shí)現(xiàn)誤差校正，其中拐點(diǎn)提取及匹配和利用粒子濾波器對(duì)拐點(diǎn)進(jìn)行定位校正是本文的研究重點(diǎn)。

2 基于路網(wǎng)節(jié)點(diǎn)的粒子濾波優(yōu)化算法研究

2.1 軌跡拐點(diǎn)提取及匹配

軌跡拐點(diǎn)是粒子進(jìn)行權(quán)重更新的依據(jù)，拐點(diǎn)提取與匹配的正確與否很大程度上決定了LO定位優(yōu)化的好壞。為了避免漏檢及錯(cuò)檢的情況發(fā)生，本節(jié)利用航向角變化速率、軌跡的直曲比及轉(zhuǎn)角變化約束進(jìn)行拐點(diǎn)提取，保證拐點(diǎn)提取的可靠性，并將提取的拐點(diǎn)與路網(wǎng)節(jié)點(diǎn)進(jìn)行匹配。拐點(diǎn)的提取主要包括粗篩和精篩兩個(gè)步驟。

1)拐點(diǎn)的粗篩：在拐彎處，航向角變化比較明顯，會(huì)出現(xiàn)連續(xù)段航向角變化速率較大的情況。故設(shè)置閾值θt與幀數(shù)m1，當(dāng)存在連續(xù)m1幀以上的航向角速率大于θt時(shí)，則認(rèn)為出現(xiàn)拐彎情況，以航向角速率最大的點(diǎn)作為拐點(diǎn)。為了避免出現(xiàn)拐彎處提取不完整，或者在同一個(gè)拐彎處提取出多個(gè)拐點(diǎn)的現(xiàn)象，如圖2(a)所示，本文對(duì)拐彎結(jié)束處的終止條件進(jìn)行優(yōu)化，認(rèn)為拐彎結(jié)束處需滿足連續(xù)若干幀(m2)對(duì)應(yīng)的角速率小于閾值θt。通過(guò)此法可以將拐彎處軌跡全部提取出，如圖2(b)所示。

(a) 原提取效果

2)拐點(diǎn)的精篩：通過(guò)航向角速率變化提取出的拐點(diǎn)并不可靠，在直線段行駛時(shí)也可能出現(xiàn)上述情況(例如：超車、急剎車等)，為了去除此類偽拐點(diǎn)，需要對(duì)其進(jìn)行精篩。本文采用軌跡的直曲比及轉(zhuǎn)角變化[14]進(jìn)行判別實(shí)現(xiàn)拐點(diǎn)的精篩。某次粗篩結(jié)果如圖3中黑框所示，設(shè)定直曲比閾值S1和角度閾值β1，只有當(dāng)同時(shí)滿足直曲比小于S1和轉(zhuǎn)角變化大于β1時(shí)，才認(rèn)為此為拐點(diǎn)，否則認(rèn)為是偽拐點(diǎn)進(jìn)行去除，從而達(dá)到精篩的目的，精篩結(jié)果如圖3中藍(lán)框所示。

圖3 精篩后提取出的拐點(diǎn)

提取出拐點(diǎn)之后，需要將此點(diǎn)與路網(wǎng)節(jié)點(diǎn)進(jìn)行匹配，遍歷路網(wǎng)節(jié)點(diǎn)，利用兩相鄰拐點(diǎn)間長(zhǎng)度誤差閾值LT及角度誤差閾值θT約束獲取匹配候選集P。

2.2 基于拐點(diǎn)的粒子濾波算法研究

為了解決LO誤差漂移問(wèn)題，本文基于粒子濾波器框架[15-16]，提出了一種新的粒子權(quán)重模型，并于拐點(diǎn)處更新粒子權(quán)重。每一次拐點(diǎn)濾波都會(huì)校正相應(yīng)位姿，進(jìn)而抑制LO誤差發(fā)散，具體的算法流程如下：

(1)粒子初始化

假設(shè)已知小車初始位姿，選定粒子數(shù)N，將粒子分布在初始位姿附近，其分布符合高斯正態(tài)分布，每個(gè)粒子代表小車可能的位姿，初始權(quán)重均為1/N。

(2)預(yù)測(cè)階段

根據(jù)運(yùn)動(dòng)模型，小車從k-1幀到k幀位姿變化可表示為

(1)

(2)

(3)

(4)

(3)測(cè)量階段及粒子權(quán)重更新

粒子濾波算法通過(guò)不斷更新粒子的位姿及權(quán)重來(lái)近似系統(tǒng)的真實(shí)后驗(yàn)概率，通過(guò)運(yùn)動(dòng)模型，粒子的位姿獲得更新，相應(yīng)權(quán)重由測(cè)量模型更新。與傳統(tǒng)的粒子濾波不同的是，本算法不對(duì)每個(gè)位置進(jìn)行測(cè)量及粒子權(quán)重更新，只對(duì)位于拐點(diǎn)處的粒子進(jìn)行處理，這樣在保證定位精度的同時(shí)，也可以提高效率，保證實(shí)時(shí)性。

通過(guò)2.1節(jié)中的方法進(jìn)行拐點(diǎn)的識(shí)別及匹配，可獲得候選匹配集P。如圖4所示，P點(diǎn)集可能不止一個(gè)點(diǎn)，為了避免錯(cuò)誤的拐點(diǎn)數(shù)據(jù)關(guān)聯(lián)導(dǎo)致定位誤差加大，本文不直接進(jìn)行數(shù)據(jù)關(guān)聯(lián)，而是利用不同節(jié)點(diǎn)的相似度模型及測(cè)量值作為粒子權(quán)重的更新依據(jù)，提出了一種新的粒子權(quán)重模型如下

圖4 候選匹配集P

(5)

受到文獻(xiàn)[11]和文獻(xiàn)[12]的啟發(fā)，本文軌跡與道路候選段相似度模型ω1構(gòu)建如圖5所示。

圖5 相似度模型

軌跡的相似度模型主要基于長(zhǎng)度誤差及朝向角誤差，其中相對(duì)長(zhǎng)度誤差和朝向角誤差分別為

(6)

α=|αs-αr|

(7)

構(gòu)建的相似度模型為

(8)

其中，λ為歸一化權(quán)重因子，其值介于[0,1]之間，其大小表征了相似度模型對(duì)長(zhǎng)度相似度和角度相似度的依賴程度。通過(guò)此模型可以看出，當(dāng)軌跡與道路候選段相似度越高，ω1的值會(huì)越大。

基于測(cè)量值的概率密度函數(shù)ω2的構(gòu)建方式如下

(9)

d′的表達(dá)式為

(10)

其中，μd為0，d代表粒子與路網(wǎng)節(jié)點(diǎn)的距離，即測(cè)量值，如圖6所示，dth為設(shè)置的距離閾值，旨在考慮到路寬因素的影響。即認(rèn)為在一定范圍內(nèi)(d≤dth)，粒子的可靠性相同且最高，當(dāng)超過(guò)一定范圍時(shí)，認(rèn)為粒子與路網(wǎng)節(jié)點(diǎn)越遠(yuǎn)，粒子越不可靠。

圖6 粒子與節(jié)點(diǎn)分布

根據(jù)式(5)可以看出，粒子的權(quán)重是由相似度模型及測(cè)量值聯(lián)合決定的，當(dāng)LO獲得的軌跡與某一道路候選段相似度越高，則表明拐點(diǎn)與匹配集P中對(duì)應(yīng)節(jié)點(diǎn)關(guān)聯(lián)程度越大，故距離此節(jié)點(diǎn)測(cè)量值越小的粒子理論上應(yīng)該擁有更大的權(quán)重，而此次構(gòu)建的粒子權(quán)重模型很好地實(shí)現(xiàn)了這一點(diǎn)。

(4)重采樣

為了避免粒子退化，需要進(jìn)行重采樣操作，根據(jù)權(quán)重比例復(fù)制粒子，在這一過(guò)程中，權(quán)重較大的粒子將保留，權(quán)重小的粒子會(huì)舍棄。將得到的新粒子集代入狀態(tài)轉(zhuǎn)移方程中，對(duì)其進(jìn)行預(yù)測(cè)和測(cè)量，反復(fù)迭代，并將每次獲得的粒子集均值作為軌跡校正后的位姿輸出。在每確定一處拐點(diǎn)后，再依據(jù)路網(wǎng)中邊的朝向信息對(duì)粒子進(jìn)行方向約束，此優(yōu)化算法可以有效減小LO帶來(lái)的定位誤差，實(shí)現(xiàn)小車準(zhǔn)確定位。

3 實(shí)驗(yàn)結(jié)果與分析

本章設(shè)計(jì)了相關(guān)實(shí)驗(yàn)以驗(yàn)證本算法的有效性，選用公開(kāi)數(shù)據(jù)集KITTI[17]，激光雷達(dá)型號(hào)選用Velodyne HDL-64E，水平分辨率為0.2°，頻率為10Hz。其中序列00-10由GPS/IMU進(jìn)行標(biāo)定并作為真實(shí)位姿，本文選用序列00、02、05、08進(jìn)行實(shí)驗(yàn)，各序列的OSM地圖于OpenStreetMap官網(wǎng)下載，并進(jìn)行節(jié)點(diǎn)標(biāo)定。

在本次實(shí)驗(yàn)中，選取的角速率θt為0.01(°)/幀，直曲比S1為0.998，角度閾值β1為15°，角度閾值θT為25°，長(zhǎng)度閾值LT為0.3，選取高斯預(yù)測(cè)噪聲[vxk,vyk,vzk,vγk,vθk,vφk]T標(biāo)準(zhǔn)差為[0.2,0.2,0.2,0.005,0.0005,0.0005]T，均值為[0,0,0,0,0,0]T，歸一化權(quán)重因子λ為0.5，選取的粒子總數(shù)N為300個(gè)。

(1)算法耗時(shí)分析

不同的粒子數(shù)目對(duì)算法的準(zhǔn)確性和運(yùn)行效率存在影響。當(dāng)粒子數(shù)量越多，準(zhǔn)確性提高，算法耗時(shí)也會(huì)隨之增加。為了平衡兩者，需要通過(guò)實(shí)驗(yàn)確定最佳粒子數(shù)N。某次實(shí)驗(yàn)中粒子數(shù)量與均方根誤差(Root Mean Squared Error，RMSE)及算法耗時(shí)如圖7所示。其中，均方根誤差用來(lái)表征算法的準(zhǔn)確性，算法耗時(shí)選取本次實(shí)驗(yàn)中粒子濾波的最長(zhǎng)耗時(shí)。

(a) 算法誤差與粒子數(shù)關(guān)系

通過(guò)圖7可以看出，當(dāng)粒子數(shù)N小于300時(shí)，誤差變化較大，當(dāng)N大于300時(shí)，耗時(shí)增加明顯，故選取最佳粒子數(shù)N為300。此時(shí)，誤差為4.30m，粒子濾波最大耗時(shí)為12.9ms，只在LO運(yùn)行時(shí)間上增加了很小的成本，可以滿足系統(tǒng)的實(shí)時(shí)性要求。

(2)本算法與LO定位結(jié)果比較

由于高斯預(yù)測(cè)噪聲隨機(jī)，每次通過(guò)粒子濾波得到的軌跡略有不同，本次對(duì)每個(gè)KITTI序列進(jìn)行50次實(shí)驗(yàn)，計(jì)算每一次實(shí)驗(yàn)軌跡的均方根誤差，取50次實(shí)驗(yàn)的均值結(jié)果如表1所示。

表1 KITTI序列算法對(duì)比

通過(guò)表1的實(shí)驗(yàn)結(jié)果可以看出，本算法能有效抑制LO的誤差漂移，定位誤差均明顯減小，其優(yōu)化程度主要和拐點(diǎn)數(shù)目、路況、行駛距離和原LO誤差等有關(guān)。由于序列KITTI05的拐點(diǎn)數(shù)較少，其優(yōu)化程度較低，而序列KITTI08行駛距離較短，拐點(diǎn)多，優(yōu)化程度較高。

將算法前后的軌跡及誤差進(jìn)行對(duì)比分析，以序列KITTI00為例的濾波前后的結(jié)果如圖8所示。

由圖8(a)可以看出，經(jīng)過(guò)本算法校正后的軌跡分布在路網(wǎng)周圍，與真實(shí)軌跡有較高的重合度，定位軌跡的誤差存在明顯的改善；由誤差曲線圖8(b)可以看出，LO的誤差會(huì)隨著時(shí)間逐漸發(fā)散，而本算法由于拐點(diǎn)的約束，每經(jīng)過(guò)一處拐點(diǎn)，誤差都會(huì)得到校正，最終輸出誤差持續(xù)較低，顯著地解決了LO的累積誤差過(guò)大的問(wèn)題。

(a) 序列KITTI00濾波軌跡對(duì)比圖

(3)與其他粒子濾波算法比較

為了說(shuō)明本算法的定位效果，選取文獻(xiàn)[11]中算法進(jìn)行對(duì)比。文獻(xiàn)[11]提出了一種拐點(diǎn)濾波(Turning Point Filtering，TPF)算法，通過(guò)相鄰點(diǎn)關(guān)系約束拐點(diǎn)與路網(wǎng)節(jié)點(diǎn)的匹配關(guān)系，并使用多源粒子濾波算法實(shí)現(xiàn)里程計(jì)誤差校正。選用同樣的參數(shù)進(jìn)行50次實(shí)驗(yàn)，得到的誤差結(jié)果如表2所示。

表2 KITTI序列優(yōu)化算法對(duì)比

可以看出，本算法與TPF算法得到的誤差結(jié)果相差不大，但是由于TPF算法是通過(guò)事后推定的方式計(jì)算軌跡-路網(wǎng)匹配關(guān)系，優(yōu)化定位誤差，不具有實(shí)時(shí)性；而本算法是基于實(shí)時(shí)定位的，相對(duì)于TPF算法來(lái)說(shuō)，本算法具有更廣的應(yīng)用空間。某次實(shí)驗(yàn)結(jié)果如圖9所示。

(a) 序列KITTI00軌跡對(duì)比圖

4 總結(jié)

本文針對(duì)GPS拒止環(huán)境下車輛自定位問(wèn)題，提出了一種基于路網(wǎng)節(jié)點(diǎn)的粒子濾波優(yōu)化算法，該算法基于粒子濾波框架，在運(yùn)動(dòng)過(guò)程中對(duì)粒子進(jìn)行狀態(tài)預(yù)測(cè)，并于拐點(diǎn)處對(duì)粒子權(quán)重進(jìn)行更新以校正位姿，消除激光雷達(dá)帶來(lái)的累積誤差。通過(guò)對(duì)KITTI數(shù)據(jù)集的實(shí)驗(yàn)可以看出，本算法具有顯著的優(yōu)化效果，可以避免LO在長(zhǎng)距離定位中帶來(lái)的漂移。本文的主要結(jié)論如下：

1)對(duì)于拐點(diǎn)的提取，采取兩次篩選，利用航向角變化速率、軌跡的直曲比及轉(zhuǎn)角變化約束進(jìn)行拐點(diǎn)提取，避免出現(xiàn)拐彎處提取不完整，對(duì)直線段的偽拐點(diǎn)進(jìn)行了較好的去除。

2)基于不同節(jié)點(diǎn)的相似度模型及測(cè)量值，設(shè)計(jì)粒子濾波的權(quán)重模型，此模型可以針對(duì)候選匹配集為多個(gè)節(jié)點(diǎn)的情況，避免拐點(diǎn)與節(jié)點(diǎn)的錯(cuò)誤關(guān)聯(lián)導(dǎo)致定位誤差加大。

3)在LO的基礎(chǔ)上，借助OSM地圖，對(duì)其進(jìn)行基于軌跡拐點(diǎn)的粒子濾波，消除激光雷達(dá)帶來(lái)的累積誤差。整個(gè)過(guò)程中沒(méi)有對(duì)每一幀進(jìn)行粒子濾波，節(jié)省了短時(shí)間內(nèi)幾乎無(wú)法獲得額外有效信息帶來(lái)的運(yùn)行成本，同時(shí)保證了校正的模糊性，具有較好的定位精度和實(shí)時(shí)性。