基于單目相機(jī)與激光雷達(dá)融合的SLAM方法

齊繼超， 何 麗， 袁 亮， 冉 騰， 張建博

(新疆大學(xué)，烏魯木齊 830000)

0 引言

同時(shí)定位與地圖構(gòu)建(Simultaneous Localization and Mapping，SLAM)[1]是移動(dòng)機(jī)器人研究領(lǐng)域的熱點(diǎn)、難點(diǎn)，是移動(dòng)機(jī)器人在未知環(huán)境中實(shí)現(xiàn)自主導(dǎo)航的前提。激光雷達(dá)和視覺傳感器是SLAM中的兩種主流傳感器，近年來基于這兩種傳感器的SLAM算法得到了廣泛的研究和應(yīng)用[2]。

然而，單一傳感器的SLAM方法總是存在一定的缺陷，傳統(tǒng)單目視覺SLAM存在對(duì)光照變化敏感以及尺度漂移等問題[3-4]；激光SLAM在幾何結(jié)構(gòu)相似的環(huán)境中易失效并且回環(huán)檢測(cè)準(zhǔn)確度差[5-6]。因此，將單目視覺傳感器與激光傳感器融合的SLAM算法成為現(xiàn)階段研究的熱點(diǎn)方向。利用激光雷達(dá)準(zhǔn)確的三維點(diǎn)云信息，為單目相機(jī)采集的RGB圖像中對(duì)應(yīng)的像素賦予深度值，從而實(shí)現(xiàn)兩種傳感器的融合，通過這種融合方式可以提高單目視覺SLAM算法的準(zhǔn)確性。然而，現(xiàn)階段單目視覺與激光雷達(dá)基于數(shù)據(jù)層的融合往往因?yàn)榛謴?fù)特征點(diǎn)尺度算法的失效，用于定位的特征點(diǎn)不足，從而出現(xiàn)算法定位不精確以及魯棒性差的情況[7]。

本文有效地解決了在激光與單目視覺融合的SLAM算法中用于估計(jì)位姿的特征點(diǎn)不足導(dǎo)致位姿估計(jì)不準(zhǔn)確的問題；同時(shí)，選用兩種深度情況的特征點(diǎn)，通過對(duì)特征點(diǎn)深度信息的判斷提出一種多策略的視覺與激光融合的SLAM算法，提高了傳統(tǒng)融合算法的定位精度和魯棒性。

1 傳感器模型

1.1 單目相機(jī)

單目相機(jī)因結(jié)構(gòu)簡(jiǎn)單、成本低廉而備受研究者們的關(guān)注，單目SLAM是視覺SLAM的研究熱點(diǎn)。單目相機(jī)成像模型如圖1所示，像素點(diǎn)P在歸一化平面中可以得到像素坐標(biāo)系下的坐標(biāo)值，但像素P的空間位置可能出現(xiàn)在光心與歸一化平面連線的任意位置。因此，缺乏深度信息會(huì)造成當(dāng)機(jī)器人沿連線運(yùn)動(dòng)時(shí)，像素位置變化較小導(dǎo)致位姿估計(jì)不準(zhǔn)確。

圖1 單目相機(jī)成像模型Fig.1 Monocular camera imaging model

單目SLAM通常通過構(gòu)建對(duì)極幾何問題[8]來估計(jì)幀間位姿。I1和I2為連續(xù)的兩個(gè)圖像幀，O1和O2為兩幀的光心，p1和p2為兩幀中某一個(gè)對(duì)應(yīng)的特征點(diǎn)。則I1到I2間的旋轉(zhuǎn)矩陣R和位移向量t可表示為

(1)

式中，K為相機(jī)內(nèi)參。

1.2 激光雷達(dá)

激光雷達(dá)是通過發(fā)射激光束探測(cè)目標(biāo)的位置、速度等特征量的雷達(dá)系統(tǒng)，其測(cè)量模型為

(2)

式中：r是激光雷達(dá)到障礙物的距離；C是光速；t是所消耗的時(shí)間。

激光雷達(dá)發(fā)射持續(xù)不斷的激光束，激光束遇到物體時(shí)會(huì)發(fā)生反射，激光雷達(dá)接收到一部分反射的激光，通過測(cè)量激光發(fā)射和返回傳感器所消耗的時(shí)間可以計(jì)算物體到激光雷達(dá)的距離。本文通過激光雷達(dá)為視覺特征點(diǎn)提供深度信息。

2 單目相機(jī)融合激光雷達(dá)的里程計(jì)算法

2.1 算法整體流程

單目相機(jī)融合激光雷達(dá)的里程計(jì)算法的整體流程如圖2所示。首先對(duì)單目相機(jī)采集的RGB圖像進(jìn)行特征提取與匹配，然后通過激光點(diǎn)云的投影得到特征點(diǎn)深度值，最后對(duì)前一幀中特征點(diǎn)深度情況進(jìn)行判斷,進(jìn)而估計(jì)先驗(yàn)位姿。

圖2 整體算法流程Fig.2 Overall process of the algorithm

2.2 提取視覺特征點(diǎn)深度

在特征提取后進(jìn)行特征匹配，同時(shí)將激光雷達(dá)采集的點(diǎn)云數(shù)據(jù)投影在RGB圖上。在特征點(diǎn)周圍選取方形區(qū)域[9]，如圖3(a)所示，紅色點(diǎn)為視覺特征點(diǎn)，藍(lán)色點(diǎn)為激光點(diǎn)云。對(duì)方形區(qū)域內(nèi)的激光點(diǎn)按照深度進(jìn)行劃分，如圖3(b)所示，通過深度直方圖建立若干深度為d的區(qū)間，選取最靠近該特征點(diǎn)的深度區(qū)間中的點(diǎn)云并擬合成平面。最后將光心和特征點(diǎn)的連線與平面的交點(diǎn)到光心的距離記為該特征點(diǎn)的深度h。

圖3 提取特征點(diǎn)深度Fig.3 Extracting depth of feature points

2.3 幀間位姿估計(jì)算法

在估計(jì)幀與幀之間位姿階段，先對(duì)前一幀中的特征點(diǎn)進(jìn)行判斷，假設(shè)當(dāng)前幀為第k幀，則在第k-1幀中存在如圖4所示的3種不同情況。

圖4 第k-1幀中特征點(diǎn)深度值情況Fig.4 The depth value of feature points in the k-1 frame

根據(jù)第k-1幀中特征點(diǎn)的3種不同情況采取不同的位姿估計(jì)策略。

1) 第k-1幀中的特征點(diǎn)全部具有深度信息，如圖4(a)所示。

此時(shí),對(duì)兩幀中所有的特征點(diǎn)構(gòu)建PNP問題[10]求解位姿，即

(3)

2) 第k-1幀中的特征點(diǎn)不全具有深度信息，如圖4(b)所示。

此時(shí)，選用有深度與無深度兩種特征點(diǎn)進(jìn)行位姿的估計(jì)。對(duì)有深度的特征點(diǎn)采取構(gòu)建PNP問題的方式求解位姿T′2,方式如式(3)。取T′2中的位移部分t2作為該情況下所要估計(jì)位姿的位移向量；并對(duì)所有特征點(diǎn)構(gòu)建對(duì)極幾何問題求解T″2，取T″2中的旋轉(zhuǎn)部分R2作為所要估計(jì)位姿的旋轉(zhuǎn)矩陣，即

(4)

(5)

3) 第k-1幀中的特征點(diǎn)均不具有深度信息，如圖4(c)所示。

在第k-1幀中的特征點(diǎn)均不具有深度值的情況下采用改進(jìn)的對(duì)極幾何算法求取位姿。首先通過式(3)對(duì)兩幀內(nèi)所有的特征點(diǎn)構(gòu)建對(duì)極幾何問題求解位姿T′3，取T′3中的旋轉(zhuǎn)矩陣R3作為該情況下所要估計(jì)位姿的旋轉(zhuǎn)矩陣；構(gòu)建對(duì)極幾何問題求取位姿的方式在估計(jì)旋轉(zhuǎn)時(shí)表現(xiàn)良好，但對(duì)于平移部分估計(jì)效果較差，本文估計(jì)位移向量為

(6)

式中：t為T′3中的位移向量；d1為兩幀中特征點(diǎn)的實(shí)際距離。取t3和R3構(gòu)成該情況下所要估計(jì)的位姿T3

(7)

綜上，在對(duì)第k-1幀中的特征點(diǎn)深度值判斷后，分別采取對(duì)應(yīng)的3種策略估計(jì)位姿T1，T2和T3，算法對(duì)應(yīng)的偽代碼如下所示。

output：T1或T2或T3

3.方法落后，是影響風(fēng)險(xiǎn)管理審計(jì)的關(guān)鍵因素。風(fēng)險(xiǎn)管理審計(jì)還缺乏一套較為系統(tǒng)、科學(xué)的風(fēng)險(xiǎn)管理方法做指導(dǎo)。一方面，審計(jì)范圍主要停留在控制風(fēng)險(xiǎn)、確保審計(jì)質(zhì)量等基本層面，沒有按科學(xué)的風(fēng)險(xiǎn)管理過程進(jìn)行管理，成果應(yīng)用不充分；另一方面，沒有具體的、針對(duì)性強(qiáng)的風(fēng)險(xiǎn)管理策略。審計(jì)經(jīng)驗(yàn)不足，創(chuàng)新能力不強(qiáng)，導(dǎo)致不能恰當(dāng)?shù)仡A(yù)測(cè)風(fēng)險(xiǎn)、識(shí)別風(fēng)險(xiǎn)、評(píng)估風(fēng)險(xiǎn)和應(yīng)對(duì)風(fēng)險(xiǎn)，阻礙了內(nèi)部審計(jì)有效開展風(fēng)險(xiǎn)管理審計(jì)。

begin

T1，T2，T3，m←0；

for對(duì)每個(gè)i≤N執(zhí)行

ifi有深度then

m=m+1；

end

end

ifm=0

執(zhí)行式(4)，(6)，(7)得到位姿T3；

else ifm=N

執(zhí)行式(3)得到位姿T1；

else

執(zhí)行式(3)～(5)得到位姿T2；

end

returnT1或T2或T3；

end。

3 實(shí)驗(yàn)及分析

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

本文算法運(yùn)行的平臺(tái)是DELL G3筆記本電腦，CPU為i5-9300,8 GiB內(nèi)存，64位Ubuntu系統(tǒng)，本文采用KITTI[11]數(shù)據(jù)集Sequence 01和Sequence 04進(jìn)行實(shí)驗(yàn)，驗(yàn)證算法的有效性和性能。該數(shù)據(jù)集包含：1臺(tái)Velodyne HDL-64E三維激光雷達(dá)，用于采集三維點(diǎn)云信息；2臺(tái)1.4 Megapixels:Point Grey Flea 2(FL2-14S3C-C)彩色相機(jī)，用于采集彩色RGB圖像。

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

實(shí)驗(yàn)運(yùn)行的過程中，在可視化界面Rviz下可以得到實(shí)時(shí)的激光點(diǎn)云與視覺特征點(diǎn)，如圖5所示。

圖5 當(dāng)前幀視覺特征點(diǎn)與激光點(diǎn)云Fig.5 Current-frame visual feature point with laser point cloud

圖6所示為本文算法與LIMO算法[9]在Sequence 01數(shù)據(jù)集上的軌跡對(duì)比，通過使用evo工具進(jìn)行數(shù)據(jù)對(duì)齊。圖6(a)為本文算法估計(jì)軌跡、LIMO算法估計(jì)軌跡和真實(shí)軌跡的對(duì)比，容易看出，本文算法相較于LIMO算法更接近于真實(shí)軌跡。圖6(b)為本文算法和LIMO算法分別在x軸、y軸和z軸的數(shù)據(jù)對(duì)比，可以看出，兩種算法在x軸的估計(jì)值與真實(shí)軌跡較為符合，在y軸出現(xiàn)估計(jì)值與真實(shí)軌跡誤差逐漸變大的情況，但本文算法相較于LIMO算法有明顯提升。

圖6 本文算法與LIMO算法軌跡對(duì)比Fig.6 The trajectory of our algorithm and LIMO algorithm

為進(jìn)一步評(píng)估本文算法的性能，本文采用evo工具中提供的絕對(duì)位姿誤差(Absolute Pose Error，APE)對(duì)比算法的定位精度，驗(yàn)證本文算法對(duì)于定位精度的準(zhǔn)確性與魯棒性的提升，對(duì)兩種算法在Sequence 01與Sequence 04數(shù)據(jù)集上分別進(jìn)行10次實(shí)驗(yàn)，取兩種算法在10次實(shí)驗(yàn)中旋轉(zhuǎn)部分APE、平移部分APE與同時(shí)考慮旋轉(zhuǎn)和平移APE的平均值，統(tǒng)計(jì)結(jié)果如表1所示。

表1 算法絕對(duì)位姿誤差比較Table 1 Comparison of absolute pose errors of the algorithms

通過對(duì)表1中數(shù)據(jù)分析可得，本文算法相較于LIMO算法在Sequence 01數(shù)據(jù)集的平移部分誤差降低了9.657%，在旋轉(zhuǎn)部分降低了15.581%；相較于LIMO算法在Sequence 04數(shù)據(jù)集的平移部分誤差降低了8.198%，在旋轉(zhuǎn)部分降低了13.300%；由于旋轉(zhuǎn)部分誤差較小，常用平移部分誤差代替整體位姿誤差，因此兩種算法在同時(shí)考慮平移與旋轉(zhuǎn)APE的提升與平移部分APE的提升基本一致。

4 結(jié)束語

本文對(duì)單目SLAM與激光SLAM融合的前端里程計(jì)進(jìn)行設(shè)計(jì)。在里程計(jì)執(zhí)行前，先對(duì)上一幀中特征點(diǎn)的深度值進(jìn)行判斷，然后根據(jù)3種判斷結(jié)果提出各自對(duì)應(yīng)的位姿估計(jì)方式，解決了激光與視覺融合SLAM中特征點(diǎn)深度值的缺失導(dǎo)致位姿估計(jì)不準(zhǔn)確的問題。最終經(jīng)過在KITTI數(shù)據(jù)集上進(jìn)行反復(fù)實(shí)驗(yàn)驗(yàn)證了算法在定位準(zhǔn)確性和魯棒性上均有所提升。