無人駕駛汽車三維同步定位與建圖精度評(píng)估

長(zhǎng)安大學(xué) 汽車學(xué)院 西安 710064

1 研究背景

三維同步定位與建圖(SLAM)由于定位精度高、累計(jì)誤差小等優(yōu)點(diǎn),成為國(guó)內(nèi)外研究的熱點(diǎn),并被廣泛應(yīng)用于無人駕駛汽車的地圖構(gòu)建與定位。開源機(jī)器人操作系統(tǒng)是一種基于消息傳遞通信的分布式多進(jìn)程框架[1],聚合了由全世界開發(fā)者實(shí)現(xiàn)的大量開源功能包,可以極大減小工作量,在各個(gè)領(lǐng)域得到大量運(yùn)用。基于開源機(jī)器人操作系統(tǒng),目前通過激光雷達(dá)進(jìn)行地圖構(gòu)建的主流方法為激光雷達(dá)測(cè)距與建圖(LOAM)算法[2]。LOAM算法將SLAM分為高頻運(yùn)動(dòng)估計(jì)和低頻環(huán)境建圖,提高了SLAM的準(zhǔn)確性。

評(píng)估激光雷達(dá)系統(tǒng)輸出的航跡位置精度,以全球定位系統(tǒng)(GPS)真值為標(biāo)準(zhǔn),將差分GPS的坐標(biāo)轉(zhuǎn)換至激光雷達(dá)的定位坐標(biāo)系下,剔除異常值,繪制出GPS與激光雷達(dá)的軌跡比較圖,并進(jìn)行誤差分析。

目前,相關(guān)研究多偏于理論,缺少對(duì)激光雷達(dá)定位精度的評(píng)估,而激光雷達(dá)在很大程度上決定了地圖構(gòu)建和定位的效果。筆者通過采用16線激光雷達(dá)實(shí)現(xiàn)無人駕駛汽車SLAM,并基于差分GPS評(píng)估其精度。

2 硬件設(shè)備

2.1 激光雷達(dá)

筆者采用的16線激光雷達(dá)是Velodyne公司出品的最小型三維激光雷達(dá)[3],保留了電機(jī)轉(zhuǎn)速可調(diào)節(jié)的功能,實(shí)時(shí)上傳周圍目標(biāo)距離和反射率測(cè)量值,具有100 m遠(yuǎn)程測(cè)量距離,安裝方便,質(zhì)量?jī)H830 g。這一激光雷達(dá)可以實(shí)現(xiàn)每秒30萬個(gè)點(diǎn)數(shù)據(jù)的輸出,具有±15°豎直視場(chǎng)、360°水平視場(chǎng)掃描功能,防塵、防濕氣等級(jí)為IP67,支持兩次回波接收,能夠測(cè)量第一次回波和最后一次回波的距離與反射強(qiáng)度,水平方向角度分辨率為0.1°～0.4°,測(cè)量頻率為5～20 Hz,具有很高的精度。

激光傳感器數(shù)據(jù)在開源機(jī)器人操作系統(tǒng)分布式架構(gòu)下封裝為sensor_msgs與PointCloud數(shù)據(jù)格式[4],由/laser_odom_to_init完成激光數(shù)據(jù)的發(fā)布工作,通過節(jié)點(diǎn)即可獲得激光數(shù)據(jù)。

2.2 差分GPS

高精度車載GPS是當(dāng)前無人駕駛汽車進(jìn)行全方位定位不可或缺的設(shè)備,但僅僅依靠車載GPS進(jìn)行定位,難以達(dá)到無人駕駛汽車的定位要求。在無人駕駛汽車領(lǐng)域,差分GPS應(yīng)運(yùn)而生。當(dāng)前,差分基站通過電臺(tái)或者4G網(wǎng)絡(luò)將定位信息傳輸至車載GPS定位端,用于提高車載GPS的定位精度[5]。應(yīng)用已知精確三維坐標(biāo)的差分GPS基準(zhǔn)臺(tái),求得偽距修正量或位置修正量,再將這個(gè)修正量實(shí)時(shí)或事后發(fā)送至GPS導(dǎo)航儀,對(duì)用戶的測(cè)量數(shù)據(jù)進(jìn)行修正,進(jìn)而提高GPS的定位精度。

3 LOAM算法

LOAM算法將激光雷達(dá)和慣性測(cè)量單元的信息作為輸入[6],分為高頻運(yùn)動(dòng)估計(jì)和低頻環(huán)境建圖兩個(gè)部分。高頻運(yùn)動(dòng)估計(jì)指在高頻率保真度下執(zhí)行雷達(dá)里程計(jì),消除運(yùn)動(dòng)畸變。在此過程中,慣性測(cè)量單元提供優(yōu)先運(yùn)動(dòng),并有助減輕總的高頻運(yùn)動(dòng)。低頻環(huán)境建圖時(shí),以同一個(gè)數(shù)量級(jí)的低頻率運(yùn)行,匹配和注冊(cè)點(diǎn)云信息。

圖1 LOAM算法框架

雷達(dá)里程計(jì)計(jì)算激光雷達(dá)在兩次連續(xù)掃描之間的運(yùn)動(dòng),去除Pk中的運(yùn)動(dòng)畸變,以10 Hz頻率運(yùn)行,稱為里程計(jì)算法。雷達(dá)建圖時(shí)進(jìn)一步處理輸出,對(duì)未失真的點(diǎn)云進(jìn)行匹配,并以1 Hz頻率輸出至地圖,稱為繪圖算法。對(duì)以上兩個(gè)輸出進(jìn)行姿勢(shì)變換集成,以10 Hz頻率輸出。

激光雷達(dá)獲取點(diǎn)云數(shù)據(jù),提取特征點(diǎn)。特征點(diǎn)選取邊緣點(diǎn)和平面點(diǎn),通過曲率計(jì)算進(jìn)行選取。曲率C計(jì)算式為:

(1)

圖2 點(diǎn)云投影

激光雷達(dá)運(yùn)動(dòng)在掃描期間以恒定的角度和線速度建模,通過一幀數(shù)據(jù)終止點(diǎn)相對(duì)于起始點(diǎn)的轉(zhuǎn)換矩陣,可以對(duì)這一幀數(shù)據(jù)中的任意點(diǎn)按照其獲得時(shí)相對(duì)于起始點(diǎn)的時(shí)間進(jìn)行插值。獲得每一個(gè)點(diǎn)的位姿。插值表達(dá)式為:

(2)

為了獲得這一幀數(shù)據(jù)中的點(diǎn)和上一幀數(shù)據(jù)中點(diǎn)的對(duì)應(yīng)關(guān)系,使用一個(gè)旋轉(zhuǎn)矩陣R和一個(gè)平移向量τ來表示[7]。

(3)

通過計(jì)算點(diǎn)到線和點(diǎn)到面的距離,可以計(jì)算出用于優(yōu)化的誤差函數(shù):

(4)

式中:d為距離。

(5)

式中:J為雅可比矩陣;λ為由Levenberg-Marquardt算法確定的因數(shù)[8]。

特征點(diǎn)到對(duì)應(yīng)邊或?qū)?yīng)面的距離越大,分配的權(quán)重就越小。如果距離大于閾值,那么將權(quán)重設(shè)置為0。

在算法中,將已經(jīng)消除畸變的點(diǎn)云先轉(zhuǎn)換至全局坐標(biāo)系,再與局部地圖進(jìn)行匹配。局部地圖指在已經(jīng)匹配好并且轉(zhuǎn)換至全局坐標(biāo)系下的前10 m3點(diǎn)云。特征點(diǎn)的數(shù)量是里程計(jì)算法的10倍,在地圖點(diǎn)云Qk中確定特征點(diǎn)的關(guān)聯(lián)時(shí),通過對(duì)特征點(diǎn)周圍點(diǎn)云簇進(jìn)行主成分分析[9],求點(diǎn)云簇的協(xié)方差矩陣特征值和特征向量,從而得到對(duì)應(yīng)邊和對(duì)應(yīng)面。

圖3 姿勢(shì)變換集成

在LOAM算法的兩種具體算法中,需要提取位于目標(biāo)邊緣和平面上的特征點(diǎn),并分別將特征點(diǎn)匹配至目標(biāo)邊緣直線段和平面。在里程計(jì)算法中,通過保證快速計(jì)算得到特征點(diǎn)的對(duì)應(yīng)關(guān)系。在繪圖算法中,通過保證精度得到特征點(diǎn)的對(duì)應(yīng)關(guān)系。

4 試驗(yàn)分析

4.1 試驗(yàn)平臺(tái)及架構(gòu)

試驗(yàn)平臺(tái)為一輛黃色園區(qū)車,其實(shí)物如圖4所示。激光雷達(dá)安裝在車前方,差分GPS安裝在車頂。Linux嵌入式平臺(tái)是一套免費(fèi)使用和自由傳播的類Unix操作系統(tǒng)[10]。通過無線保真與計(jì)算機(jī)進(jìn)行通信,計(jì)算機(jī)端運(yùn)算能力強(qiáng),可進(jìn)行數(shù)據(jù)量較大的運(yùn)算。Linux嵌入式平臺(tái)由于運(yùn)算能力有限,只能運(yùn)行基本的功能包。筆者采用如圖5所示GPS數(shù)據(jù)采集和繪圖軟件,繪制GPS路網(wǎng)文件和無人駕駛汽車實(shí)時(shí)路徑。當(dāng)前幀GPS 數(shù)據(jù)顯示當(dāng)前通過通用串行總線接口截取到的原始數(shù)據(jù),當(dāng)前GPS位置顯示當(dāng)前無人駕駛汽車定位的經(jīng)緯度,在GPS設(shè)備和測(cè)試通信區(qū)域,可以通過按鈕打開GPS設(shè)備。測(cè)試按鈕用于測(cè)試當(dāng)前GPS數(shù)據(jù)采集和繪圖軟件與主控制軟件是否通信正常。

圖4 試驗(yàn)平臺(tái)實(shí)物

圖5 GPS數(shù)據(jù)采集和繪圖軟件

4.2 試驗(yàn)環(huán)境

試驗(yàn)以圖6所示環(huán)境地圖為試驗(yàn)場(chǎng)景,其中黑色曲線為GPS軌跡,由車載GPS得到。將GPS定位數(shù)據(jù)與激光雷達(dá)獲得的傳感器點(diǎn)云數(shù)據(jù)記錄在文件中,采用LOAM算法進(jìn)行定位與建圖,對(duì)定位的精度進(jìn)行評(píng)估。

4.3 試驗(yàn)結(jié)果

在激光雷達(dá)的測(cè)量數(shù)據(jù)中,如果有非常明顯的異常值,可以對(duì)該值進(jìn)行處理。在試驗(yàn)中,為方便研究,將GPS數(shù)據(jù)值進(jìn)行有效提取,使GPS數(shù)據(jù)與激光雷達(dá)數(shù)據(jù)長(zhǎng)度保持一致,兩組數(shù)據(jù)的一次差值如果大于60 cm,那么認(rèn)為是異常值。通過MATLAB軟件進(jìn)行繪圖,對(duì)差分GPS和激光雷達(dá)的曲線進(jìn)行誤差分析,如圖7、圖8所示。

圖6 試驗(yàn)場(chǎng)景

圖7 差分GPS與激光雷達(dá)軌跡對(duì)比

圖8 激光雷達(dá)誤差分布

由圖7和圖8可以看出,基于LOAM算法的激光雷達(dá)定位精度較高,雖存在一定誤差,但能將誤差控制在30 cm內(nèi),累積誤差不隨無人駕駛汽車行駛距離的增大而增大。

在不同車速和轉(zhuǎn)向時(shí),LOAM算法能很好地消除位姿估計(jì)誤差。隨著無人駕駛汽車的行駛,位姿估計(jì)累積誤差可以得到有效控制。彎道誤差波動(dòng)較大,主要原因是無人駕駛汽車姿態(tài)瞬時(shí)改變量較大,從而造成障礙物匹配不佳。

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

筆者應(yīng)用16線激光雷達(dá)和差分GPS進(jìn)行SLAM試驗(yàn),對(duì)比基于LOAM算法的激光雷達(dá)定位和基于差分GPS的定位,確認(rèn)激光雷達(dá)定位曲線與差分GPS軌跡基本保持一致。通過圖形直觀展示了激光雷達(dá)定位誤差分布的特點(diǎn)和變化規(guī)律,對(duì)影響定位誤差的主要因素進(jìn)行了分析,對(duì)LOAM算法中的應(yīng)用和SLAM的精度評(píng)估具有參考價(jià)值。