姜文飛， 張小俊， 王金剛

(河北工業(yè)大學(xué)機(jī)械工程學(xué)院， 天津 300132)

同時(shí)定位與建圖[1](simultaneous localization and mapping,SLAM)問題在智能車上表述為：在未知環(huán)境中移動(dòng)通過傳感器的觀測(cè)信息對(duì)汽車位姿進(jìn)行估計(jì)同時(shí)完成地圖創(chuàng)建的過程。李輝等[2]在VINS-Mono框架下改進(jìn)視覺慣導(dǎo)融合算法，提取灰度值陡變的像素點(diǎn)，并采用滑動(dòng)窗口的形式建立聯(lián)合殘差模型，提高了算法的定位精度。Chen等[3]將二維的激光雷達(dá)和深度相機(jī)安裝在移動(dòng)機(jī)器人上融合二者信息提高了定位與建圖的精度。 Ding等[4]采用一種混合光束平差算法，將雷達(dá)與相機(jī)建立的地圖進(jìn)行對(duì)齊融合，建圖的精度得到了提高但是降低了運(yùn)行效率。孫柏等[5]提出了通過高斯混合模型在前端匹配時(shí)加入色調(diào)到定向角點(diǎn)檢測(cè)和旋轉(zhuǎn)特征描述(oriented FAST and rotated BRIEF，ORB)特征匹配中，通過光束平差法進(jìn)行位姿優(yōu)化，提高了SLAM算法的精度。Chan等[6]引入了一種融合激光和視覺SLAM的新方法，提出軌跡匹配，減少了不必要的浪費(fèi)，減少了內(nèi)存損耗提高了運(yùn)行效率。張偉偉等[7]通過將視覺和激光點(diǎn)云數(shù)據(jù)融合，提出一種雙重閉環(huán)檢測(cè)給位子圖添加約束，提高定位的精度的同時(shí)增加了算法的復(fù)雜性。李陸君等[8]提出了一種將激光和視覺進(jìn)行基于貝葉斯的地圖層面的融合，建圖的效果得到了提升，但是耗費(fèi)大量運(yùn)算的時(shí)間。王消為等[9]提出了一種基于激光雷達(dá)與雙目視覺的移動(dòng)機(jī)器人SLAM算法，該算法通過改進(jìn)RBPF-SLAM算法，將雙目相機(jī)、激光雷達(dá)和里程計(jì)信息進(jìn)行融合，充分利用了視覺SLAM圖像采集精度高的優(yōu)勢(shì)，提高位姿估計(jì)的準(zhǔn)確性但是對(duì)光照忽明忽暗的場(chǎng)景容易失效。任明宇等[10]針對(duì)單一傳感器進(jìn)行定位于建圖精度低，建圖失敗的問題，提出了一種將通過粒子濾波估計(jì)的位姿和相機(jī)的數(shù)據(jù)進(jìn)行圖像配準(zhǔn)和點(diǎn)云拼接，采用Bayes方法更新地圖，但效果不明顯。靳東[11]提出了一種將視覺點(diǎn)云數(shù)據(jù)轉(zhuǎn)換到雷達(dá)坐標(biāo)系下用激光的點(diǎn)云配準(zhǔn)估計(jì)位姿，最后采用擴(kuò)展卡爾曼濾波實(shí)現(xiàn)位姿融合，視覺環(huán)境受限制較為嚴(yán)重。朱祖侖[12]提出了一種利用視覺詞袋模型來解決激光閉環(huán)檢測(cè)的累計(jì)誤差問題，降低了累計(jì)誤差，但是增加了內(nèi)存的損耗。Jiang等[13]提出一種基于低成本激光雷達(dá)和相機(jī)構(gòu)建2.5D地圖的SLAM框架，算法通過構(gòu)建激光和圖像匹配的聯(lián)合誤差函數(shù)來優(yōu)化機(jī)器人的位姿。周陽[14]將激光SLAM的位姿和視覺的位姿進(jìn)行時(shí)間戳匹配，再用線性插值的方式計(jì)算柵格地圖下的位姿，最后采用圖優(yōu)化來優(yōu)化位姿。鄭帆等[15]利用平約束的SE(3)參數(shù)化方法，將里程計(jì)與單目視覺SLAM進(jìn)行融合，引入了里程計(jì)的約束；潘陽杰[16]將慣性測(cè)量?jī)x(inertial measurement unit，IMU)優(yōu)化的里程計(jì)融合到雙目視覺SLAM算法，增強(qiáng)了系統(tǒng)的魯棒性，但優(yōu)化程度有限。

以上研究針對(duì)激光和視覺融合SLAM對(duì)環(huán)境光要求比較高，但僅僅用二維激光雷達(dá)在處理地下車庫場(chǎng)景時(shí)，由于點(diǎn)云信息的數(shù)量有限且場(chǎng)景存在移動(dòng)的物體，在位姿匹配的時(shí)候容易產(chǎn)生誤匹配而導(dǎo)致SLAM建圖的失敗，在此情景下，現(xiàn)提出一種改進(jìn)的適用于地下車庫場(chǎng)景的EKF算法，將深度RGB-D(red green blue-deep)相機(jī)和激光雷達(dá)信息融合，此外采用相機(jī)輔助激光進(jìn)行回環(huán)檢測(cè)。最后通過實(shí)驗(yàn)驗(yàn)證該算法提高智能車實(shí)時(shí)定位的精度和建圖的效果，以期為改進(jìn)的EKF算法提供現(xiàn)實(shí)可行性保障。

1 基于二維激光雷達(dá)的位姿估計(jì)

1.1 點(diǎn)云運(yùn)動(dòng)畸變?nèi)コ?/h3>

雷達(dá)運(yùn)動(dòng)畸變是由于傳感器運(yùn)動(dòng)而產(chǎn)生的，根源是由于每個(gè)激光點(diǎn)的基準(zhǔn)位置不同引起的，但是處理的時(shí)候往往都認(rèn)為所有的激光點(diǎn)處在同一位置，因此導(dǎo)致了畸變的產(chǎn)生如圖1所示，圖1(a)為理論的基準(zhǔn)位置，圖1(b)為實(shí)際的基準(zhǔn)位置。

采用里程計(jì)信息輔助雷達(dá)進(jìn)行運(yùn)動(dòng)畸變?nèi)コＲ阎?dāng)前幀激光的始終時(shí)刻ts、te, 以及兩個(gè)激光束間的時(shí)間間隔Δt,里程計(jì)信息按照時(shí)間先后存儲(chǔ)在一個(gè)隊(duì)列中，其中最早的里程計(jì)時(shí)間戳

圖1 二維雷達(dá)點(diǎn)云示意圖Fig.1 Schematic diagram of 2D radar point cloud

1.2 二維場(chǎng)景點(diǎn)云匹配

目前的配準(zhǔn)方法中IMLS-ICP算法由于初值不準(zhǔn)確從而影響點(diǎn)云匹配的精度，因此采用里程計(jì)信息來提供點(diǎn)云匹配更可靠的初始值，且為了提高運(yùn)行速率選擇具有良好曲率的點(diǎn)進(jìn)行匹配，使改進(jìn)后的IMLS-ICP點(diǎn)云匹配的激光點(diǎn)更加準(zhǔn)確，既能夠減少計(jì)算量同時(shí)又能減少激光點(diǎn)分布不均勻?qū)е碌挠?jì)算結(jié)果出現(xiàn)偏移；點(diǎn)云中隱藏著真實(shí)的曲面，最好的做法就是能從參考幀點(diǎn)云中將曲面重建出來，曲面重建的越準(zhǔn)確，匹配的精度就越高。

1.2.1 特征點(diǎn)的選取

在選取特征點(diǎn)時(shí)，選擇具有豐富特征的點(diǎn)，即結(jié)構(gòu)化的點(diǎn)：具有良好的曲率和法向量的定義；曲率越小的點(diǎn)越好，因?yàn)槟軌蛱峁┳銐虻募s束；選取代表性的點(diǎn)時(shí)候要注意均衡性；此外，由于是平面匹配所以角度都是可觀測(cè)的。只需要考慮x方向和y方向的可觀性，保證兩個(gè)方向約束一致；選點(diǎn)的時(shí)候盡量保證x方向和y方向上選取的點(diǎn)云數(shù)量盡可能相同。

1.2.2 曲面重建

已知點(diǎn)云集合Pk中每一個(gè)點(diǎn)pi的法向量ni，其中i為表任意正整數(shù)變量，則Pk中隱藏的曲面為

(1)

(2)

1.2.3 匹配求解

當(dāng)前一幀中一點(diǎn)xi到曲面的距離為IPk(xi)，則點(diǎn)xi在曲面上的投影向量yi為

yi=xi-IPk(xi)ni

(3)

式(3)中：Pk中距離點(diǎn)xi最近的點(diǎn)的法向量為ni。利用式(3)尋找xi對(duì)應(yīng)的投影點(diǎn)yi。點(diǎn)xi和點(diǎn)yi為對(duì)應(yīng)的匹配點(diǎn)：

(4)

式(4)中：法向量ni是yi點(diǎn)的法向量；R為旋轉(zhuǎn)矩陣；t為平移向量。式(4)表示的是轉(zhuǎn)換后的xi點(diǎn)到投影點(diǎn)yi的距離(法方向上的距離)?？傊?，IMLS-ICP激光幀間匹配算法利用里程計(jì)的初始值，并且使用高斯擬合和最小二乘重建出一個(gè)隱含曲面，找到代表點(diǎn)的投影點(diǎn)，利用點(diǎn)到曲面的距離來構(gòu)建誤差的方程進(jìn)而求取雷達(dá)位姿。

2 基于RGB-D相機(jī)的位姿估計(jì)

2.1 相機(jī)標(biāo)定

張正友相機(jī)標(biāo)定因操作簡(jiǎn)單被廣泛應(yīng)用到相機(jī)標(biāo)定之中，相機(jī)在進(jìn)行外界信息獲取的時(shí)候存在著明顯的畸變，僅考慮徑向畸變，用函數(shù)來描述畸變方程。

(5)

式中：k1、k2為畸變系數(shù)；[x,y]T為歸一化平面的點(diǎn)坐標(biāo)；r為歸一化平面的點(diǎn)與坐標(biāo)系原點(diǎn)之間的距離；[xc,yc]T為畸變糾正后點(diǎn)的坐標(biāo)。對(duì)于畸變較小的圖像區(qū)域，k1起到主要作用；邊緣區(qū)域畸變較大，k2起主要作用。通過標(biāo)定將點(diǎn)轉(zhuǎn)換到像素坐標(biāo)系中。

(6)

式中：[u,v]T為轉(zhuǎn)換的像素坐標(biāo)；fx、fy為焦距f在x、y方向上的分量；cx、cy為對(duì)應(yīng)x、y方向上的偏移矯正參數(shù)。采用張正友標(biāo)定法對(duì)相機(jī)進(jìn)行標(biāo)定，得到相機(jī)的內(nèi)參矩陣參數(shù)如表1所示，相機(jī)的畸變系數(shù)如表2所示。

表1 相機(jī)內(nèi)參

表2 畸變系數(shù)

2.2 ORB特征提取與匹配

Rublee等[17]提出的關(guān)于加速角點(diǎn)檢測(cè)(feature from accelerated segment test，F(xiàn)AST)與局部圖像特征描述子(binary robust independent element feature，BRIEF)的ORB特征提取與匹配算法，與快速魯棒性特征(speeded UP robust features，SURF)、尺度特征點(diǎn)變換(scale invariant feature transform，SIFT)等算法對(duì)比特征提取的速度明顯更快，該算法采用灰度質(zhì)心法在FAST角點(diǎn)檢測(cè)基礎(chǔ)上用矩陣變換的方式給特征描述子加入方向信息，并且通過圖像金字塔給圖像加入尺度信息。

圖2為地下車庫場(chǎng)景ORB特征匹配以及經(jīng)過篩選后圖像特征匹配的效果?？梢钥闯鼋?jīng)過BRIEF篩選后的特征匹配相對(duì)于原始ORB匹配明顯降低了誤匹配的點(diǎn)對(duì)數(shù)量，提高了匹配精度與特征匹配的效率。

圖2 ORB特征匹配Fig.2 ORB feature matching

由于地下車庫燈光忽明忽暗的情況，從圖2中可以明顯看出，有地面反射燈光的特征匹配從而影響了匹配的精度，提出了一種直方圖均衡化濾波的圖像預(yù)處理技術(shù)來消除因?yàn)闊艄夥瓷涠a(chǎn)生的誤匹配(紅色圓框)，處理后的特征匹配如圖3所示，通過將圖像的灰度區(qū)間均勻化改善了特征提取，提高了匹配的精度，進(jìn)而提高了定位與建圖的精度。

空間點(diǎn)的坐標(biāo)為Pi，通過相機(jī)模型轉(zhuǎn)換到相機(jī)坐標(biāo)系進(jìn)而轉(zhuǎn)換到像素坐標(biāo)系，像素坐標(biāo)系下的坐標(biāo)表示為pi=[ui,vi,1]T，那么有

pizi=K(RPi+t)=KTPi

(7)

式(7)中：K為相機(jī)的內(nèi)參；zi為像素點(diǎn)i到光心的距離；T為相機(jī)的外參。相機(jī)的位姿由旋轉(zhuǎn)矩陣R和平移向量t組成。坐標(biāo)點(diǎn)在實(shí)際轉(zhuǎn)換的過程中由于噪聲的存在會(huì)產(chǎn)生投影的誤差，如圖4所示，空間點(diǎn)P在第一張照片上的投影為p1，在第二張照片中的投影為p2，由于誤差的存在，P投影在p′2處。調(diào)整相機(jī)位姿使p′2與p2距離最小。

在進(jìn)行EPnP估計(jì)相機(jī)位姿時(shí)需要最小化重投影誤差[18]，此處采用最小二乘法對(duì)相機(jī)位姿估計(jì)值進(jìn)行非線性BA(bundle adjustment)優(yōu)化來尋找最好的相機(jī)位姿，計(jì)算公式為

圖3 濾波處理的ORB特征匹配Fig.3 ORB feature matching based on filtering

圖4 重投影誤差示意圖Fig.4 Schematic diagram of re projection error

(8)

式(8)中：T*為最優(yōu)相機(jī)位姿時(shí)的變換矩陣。

3 基于擴(kuò)展卡爾曼濾波的融合SLAM算法

3.1 聯(lián)合標(biāo)定數(shù)據(jù)關(guān)聯(lián)

為了降低RGB-D相機(jī)與2D激光雷達(dá)信息進(jìn)行融合的誤差，將兩種傳感器外參進(jìn)行標(biāo)定以便獲得相機(jī)坐標(biāo)系與雷達(dá)坐標(biāo)系的轉(zhuǎn)換關(guān)系，在相機(jī)和雷達(dá)坐標(biāo)系分別找到同一世界坐標(biāo)系下的特征點(diǎn)P的坐標(biāo)P=(x,y,z,1)T,在相機(jī)坐標(biāo)系的坐標(biāo)為Pk=(xk,yk,zk,1)T,在雷達(dá)坐標(biāo)系下的坐標(biāo)為Pr=(xr,yr,zr,1)T，相機(jī)圖像坐標(biāo)系下的坐標(biāo)為p=(u,v,1)T，圖5為標(biāo)定原理圖。

s為標(biāo)定板單位像素坐標(biāo)圖5 雷達(dá)與相機(jī)聯(lián)合標(biāo)定Fig.5 Joint calibration of radar and camera

雷達(dá)與相機(jī)坐標(biāo)系間的變換方程為

Pk=RPr+t

(9)

相機(jī)平面到像素平面的坐標(biāo)系變換方程為

p=KPk=K(RPr+t)

(10)

式(10)中：K為相機(jī)的內(nèi)參矩陣，由相機(jī)標(biāo)定得到。激光雷達(dá)的極坐標(biāo)系與直角坐標(biāo)系原點(diǎn)重合位于雷達(dá)中心，激光束極坐標(biāo)偏角值為θ，激光雷達(dá)極坐標(biāo)徑向測(cè)距值為r，則雷達(dá)的極坐標(biāo)與直角坐標(biāo)的轉(zhuǎn)化關(guān)系為

(11)

將相機(jī)的3D點(diǎn)云轉(zhuǎn)換為2D點(diǎn)云選擇了降維，則相機(jī)與激光雷達(dá)在豎直方向的參數(shù)不影響二者的融合。最終相機(jī)與雷達(dá)坐標(biāo)系間的變換關(guān)系為

p=K(RPr+t)(xkyk)T

(12)

3.2 擴(kuò)展卡爾曼濾波位姿融合

汽車在實(shí)際環(huán)境運(yùn)動(dòng)的過程中，兩種傳感器位姿估計(jì)可能出現(xiàn)一種傳感器位姿估計(jì)失效的情況。當(dāng)光線較暗不能滿足視覺信息獲取時(shí)，單獨(dú)依靠雷達(dá)點(diǎn)云信息完成位姿估計(jì)；當(dāng)光線較強(qiáng)，視覺特征點(diǎn)豐富時(shí)，默認(rèn)兩個(gè)傳感器數(shù)據(jù)都符合高斯分布，采用改進(jìn)的擴(kuò)展卡爾曼濾波算法對(duì)系統(tǒng)的狀態(tài)進(jìn)行預(yù)測(cè)，然后，根據(jù)光線強(qiáng)弱變化過程中視覺特征匹配點(diǎn)對(duì)的變化更新視覺信息的融合權(quán)重來完成觀測(cè)信息的更新，實(shí)現(xiàn)位姿的融合，系統(tǒng)的方程為非線性可微分函數(shù)。

xk=f(xk-1,uk)+wk，wk～N(0,Qk)

(13)

zk=h(xk)+vk，vk～N(0,Rk)

(14)

式中：xk為位姿狀態(tài)向量；zk為對(duì)應(yīng)圖像上的像素位置；f和h分別為系統(tǒng)的狀態(tài)模型和觀測(cè)模型；wk、vk為高斯白噪聲；Qk、Rk為對(duì)應(yīng)的白噪聲的協(xié)方差矩陣。

觀測(cè)模型是采用雷達(dá)和相機(jī)的觀測(cè)模型，將相機(jī)的位姿信息通過前面的雷達(dá)相機(jī)聯(lián)合標(biāo)定的轉(zhuǎn)換矩陣轉(zhuǎn)換到雷達(dá)的極坐標(biāo)系下，通過傳感器模型實(shí)現(xiàn)觀測(cè)信息更新，將k-1～k時(shí)間段內(nèi)的位姿的增量作為輸入，k-1和k分別代表相鄰的時(shí)間戳：

(15)

式(15)中：β為位姿估計(jì)距離的增量；δ為位姿估計(jì)的偏航角增量；θ為雷達(dá)極坐標(biāo)的偏角值。

對(duì)應(yīng)的雅克比矩陣為

(16)

預(yù)測(cè)方程為

(17)

(18)

傳統(tǒng)的EKF觀測(cè)方程是根據(jù)先驗(yàn)值來更新當(dāng)前的最優(yōu)估計(jì)，而改進(jìn)后的EKF算法是根據(jù)(k，k-1)兩個(gè)時(shí)刻的先驗(yàn)估計(jì)根據(jù)光照分配權(quán)重來更新k時(shí)刻的最優(yōu)估計(jì)。

更新過程的觀測(cè)方程為

(19)

(20)

K(k)=[λ1Kkλ2Kk-1]

(21)

(22)

(23)

改進(jìn)的EKF融合的流程圖如圖6所示。

圖6 改進(jìn)EKF融合的流程圖Fig.6 Improved EKF fusion flow chart

4 BOW詞袋模型及回環(huán)檢測(cè)

4.1 詞袋模型

視覺詞袋模型(bag of visual words,BoVW)是從每一幀任意圖像中提取局部特征來構(gòu)成BOW模型中的單詞，并統(tǒng)計(jì)該圖形中這些單詞出現(xiàn)的頻率。

其中BOW字典建立步驟如下：①?gòu)拿恳粠瑘D像中提取圖像的局部特征；②將提取的局部特征信息采用K-means++算法進(jìn)行聚類，并劃分成k個(gè)子空間；③將每個(gè)空間繼續(xù)進(jìn)行聚類； ④按照步驟②、③循環(huán)，得到K叉樹字典結(jié)構(gòu)模型，如圖7所示。

G為K叉樹的深度圖7 K叉樹字典示意圖Fig.7 K-tree dictionary diagram

詞典特征相似度計(jì)算相關(guān)的量為： TF-IDF(term frequency-inverse document frequency)[19-20]，或譯頻率-逆文檔頻率，TF思想是：圖像中出現(xiàn)較多的單詞的區(qū)分度也越高；而IDF則是：?jiǎn)卧~出現(xiàn)的頻率越低，區(qū)分度越高。其中IDF是某個(gè)葉子結(jié)點(diǎn)ωi中特征數(shù)量ni相對(duì)所有特征數(shù)量n的比例：

(24)

TF為一個(gè)特征在一幅圖中出現(xiàn)的頻率，像A中單詞ωi出現(xiàn)了ni次，而一共出現(xiàn)的單詞次數(shù)為n，則

(25)

單詞ωi的權(quán)重等于TF與IDF之積，則

ηi=TFiIDFi

(26)

對(duì)于圖像A中，它的特征點(diǎn)對(duì)應(yīng)到的單詞組成了一個(gè)視覺詞袋BOW，通過詞袋用單個(gè)向量vA描述圖像A：

vA={(ω1,η1),(ω2,η2),…，(ωn,ηn)}

(27)

4.2 激光視覺融合閉環(huán)檢測(cè)

2D激光slam的回環(huán)檢測(cè)由于對(duì)平面的依賴而存在特征的缺失，建圖的魯棒性低等問題。但是視覺信息可以根據(jù)圖像特征來判斷機(jī)器人是否回到原來的位置完成回環(huán)，提高了建圖的魯棒性。視覺信息的回環(huán)檢測(cè)首先計(jì)算兩個(gè)圖像的相似度；再通過回環(huán)候選幀和當(dāng)前幀的幾何約束來驗(yàn)證回環(huán)檢測(cè)。

外觀驗(yàn)證篩選關(guān)鍵幀：計(jì)算當(dāng)前幀與相鄰關(guān)鍵幀的視覺詞袋模型(BOW)匹配的最低得分；通過當(dāng)前幀BOW詞匯逆向索引找到相關(guān)聯(lián)的關(guān)鍵幀，并統(tǒng)計(jì)每個(gè)關(guān)鍵幀和當(dāng)前的關(guān)鍵幀的相似的詞匯量，通過設(shè)置閾值來選擇最終的閉環(huán)候選幀。

5 實(shí)驗(yàn)與分析

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

采用如圖8所示奇瑞eQ1小螞蟻300汽車作為移動(dòng)平臺(tái)進(jìn)行實(shí)驗(yàn)，搭載了思嵐科技的RPLIDAR A1 360 度激光掃描測(cè)距雷達(dá)、樂視三合一體感攝像頭和里程計(jì)等傳感器。通過搭載的盈馳微型電腦使用AMD Ryzen 5 4600H (3.00 GHz) CPU處理器和8 GB運(yùn)行內(nèi)存的上位機(jī)電腦進(jìn)行機(jī)器人控制及傳感器數(shù)據(jù)的處理，算法運(yùn)行在基于Ubuntu 16.04的機(jī)器人操作系統(tǒng)(robot operating system，ROS)下，并用圖形化窗口顯示構(gòu)建的地圖。

圖9為一個(gè)地下車庫作為實(shí)驗(yàn)場(chǎng)景。

5.2 改進(jìn)算法定位與建圖精度實(shí)驗(yàn)對(duì)比

對(duì)單獨(dú)使用激光算法，傳統(tǒng)EKF融合算法以及改進(jìn)的融合算法進(jìn)行定位精度進(jìn)行對(duì)比實(shí)驗(yàn)，軌跡對(duì)比實(shí)驗(yàn)結(jié)果如圖10所示，以黑色原點(diǎn)為起點(diǎn)，運(yùn)行一周。

從圖10軌跡的對(duì)比圖可以看出，改進(jìn)的融合算法的軌跡相比較單獨(dú)使用激光算法以及傳統(tǒng)EKF融合算法的軌跡更加接近實(shí)際軌跡。

圖8 奇瑞eQ1小螞蟻300汽車Fig.8 Chery eQ1 Little Ant 300 car

圖9 實(shí)驗(yàn)場(chǎng)地Fig.9 Experimental site

圖10 軌跡對(duì)比圖Fig.10 Trajectory comparison chart

將兩種算法分別用evo誤差評(píng)測(cè)工具進(jìn)行絕對(duì)位姿誤差(absolute pose error，APE)評(píng)測(cè)和相對(duì)位姿誤差(relative pose error，RPE)評(píng)測(cè)，實(shí)驗(yàn)結(jié)果如圖11和圖12所示。

t為數(shù)據(jù)包運(yùn)行的時(shí)間戳圖11 APE絕對(duì)誤差Fig.11 Absolute trajectory error of APE

表2將3種算法產(chǎn)生的誤差進(jìn)行了對(duì)比分析，實(shí)驗(yàn)結(jié)果表明，改進(jìn)的融合算法的定位精度明顯優(yōu)于單獨(dú)使用激光算法和EKF融合算法。從表2中誤差分析的結(jié)果可以看出，改進(jìn)融合方法建圖相比較激光算法和傳統(tǒng)EKF算法的均方根誤差分別降低了85%和50%。因此，使用相機(jī)與雷達(dá)信息融合新算法明顯提高了汽車實(shí)時(shí)定位的精度和準(zhǔn)確性，降低了位姿估計(jì)的誤差。為更直觀地反映建圖效果，將3種算法進(jìn)行二維柵格地圖構(gòu)建，得到的柵格地圖如圖13所示，對(duì)柵格建圖的質(zhì)量進(jìn)行對(duì)比分析。

t為數(shù)據(jù)包運(yùn)行的時(shí)間戳圖12 RPE相對(duì)誤差Fig.12 Relative trajectory error of RPE

表2 3種算法誤差對(duì)比

圖13 二維柵格地圖Fig.13 2D grid map

從圖13中3種算法二維柵格地圖紅框中車輛障礙物顯示情況可以看出，圖13(c)改進(jìn)的融合方法建圖的精度明顯優(yōu)于圖13(a)單獨(dú)使用激光建圖和圖13(b)傳統(tǒng)EKF算法建圖，改進(jìn)EKF算法以及采用視覺詞袋模型來輔助激光進(jìn)行回環(huán)檢測(cè)來完成建圖，明顯提高了建圖的精度與準(zhǔn)確性。

6 結(jié)論

提出了一種融合視覺信息的激光SLAM 建圖方法解決了基于激光的SLAM建圖中定位不準(zhǔn)確及建圖效果差的問題，使用里程計(jì)信息來提供激光匹配的初值提高了激光點(diǎn)云匹配的精度；采用視覺信息來補(bǔ)償激光信息提高建圖的精度，視覺的ORB特征的BOW詞袋模型來補(bǔ)助進(jìn)行回環(huán)檢測(cè)，降低了累計(jì)誤差。提出的改進(jìn)的擴(kuò)展卡爾曼濾波算法能夠快速，準(zhǔn)確的完成SLAM建圖，通過結(jié)果對(duì)比可以看出改進(jìn)的基于激光視覺信息融合的EKF建圖方法更加準(zhǔn)確，魯棒性也更高。