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

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

0 引言

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

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

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

1 傳感器模型

1.1 單目相機(jī)

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

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

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

(1)

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

1.2 激光雷達(dá)

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

(2)

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

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

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

2.1 算法整體流程

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

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

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

在特征提取后進(jìn)行特征匹配，同時將激光雷達(dá)采集的點(diǎn)云數(shù)據(jù)投影在RGB圖上。在特征點(diǎn)周圍選取方形區(qū)域[9]，如圖3(a)所示，紅色點(diǎn)為視覺特征點(diǎn)，藍(lán)色點(diǎn)為激光點(diǎn)云。對方形區(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 幀間位姿估計算法

在估計幀與幀之間位姿階段，先對前一幀中的特征點(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種不同情況采取不同的位姿估計策略。

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

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

(3)

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

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

(4)

(5)

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

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

(6)

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

(7)

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

output：T1或T2或T3

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

begin

T1，T2，T3，m←0；

for對每個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)平臺

本文算法運(yùn)行的平臺是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臺Velodyne HDL-64E三維激光雷達(dá)，用于采集三維點(diǎn)云信息；2臺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í)時的激光點(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ù)集上的軌跡對比，通過使用evo工具進(jìn)行數(shù)據(jù)對齊。圖6(a)為本文算法估計軌跡、LIMO算法估計軌跡和真實(shí)軌跡的對比，容易看出，本文算法相較于LIMO算法更接近于真實(shí)軌跡。圖6(b)為本文算法和LIMO算法分別在x軸、y軸和z軸的數(shù)據(jù)對比，可以看出，兩種算法在x軸的估計值與真實(shí)軌跡較為符合，在y軸出現(xiàn)估計值與真實(shí)軌跡誤差逐漸變大的情況，但本文算法相較于LIMO算法有明顯提升。

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

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

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

通過對表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)部分誤差較小，常用平移部分誤差代替整體位姿誤差，因此兩種算法在同時考慮平移與旋轉(zhuǎn)APE的提升與平移部分APE的提升基本一致。

4 結(jié)束語

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