一種結(jié)合改進(jìn)ORB視覺特征的智能汽車激光SLAM算法

李奎鎮(zhèn)，王金剛，張小俊

一種結(jié)合改進(jìn)ORB視覺特征的智能汽車激光SLAM算法

李奎鎮(zhèn)，王金剛，張小俊*

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

針對(duì)地下車庫場(chǎng)景中SLAM定位不準(zhǔn)的問題，文章提出一種基于視覺和雷達(dá)信息融合的SLAM算法，將三維ICP算法IMLS-ICP（Implicit Moving Least Square-Iterative Closest Point）進(jìn)行二維化，并融入入PL-ICP（Point to Line-Iterative Closest Point）和N-ICP（Normal-Iterative Closest Point）的內(nèi)容，使得IMLS-ICP更加適合于二維環(huán)境，獲得更好的點(diǎn)云匹配效果。然后，從圖像中提取ORB（Oriented FAST and Rotated BRIEF）特征點(diǎn)進(jìn)行特征匹配，并針對(duì)于地下車庫場(chǎng)景亮度不穩(wěn)定的特點(diǎn)，對(duì)圖像進(jìn)行了直方圖均衡化濾波處理，并用BRIEF（Binary Robust Independent Elementary Feature）描述子進(jìn)行約束，使得ORB特征點(diǎn)的提取與匹配更具精確性和魯棒性，并對(duì)得到的結(jié)果進(jìn)行了局部的BA（Bundle Adjustment）優(yōu)化。應(yīng)用加權(quán)最小二乘法進(jìn)行位姿融合。通過實(shí)驗(yàn)對(duì)位姿進(jìn)行了分析，與主流2DSLAM算法進(jìn)行對(duì)比，驗(yàn)證文章算法的有效性。

SLAM；IMLS-ICP；ORB；直方圖均衡化濾波；加權(quán)最小二乘法

前言

智能汽車SLAM問題可以描述為：智能汽車在未知環(huán)境中，在移動(dòng)過程中通過傳感器信息對(duì)智能汽車的位姿進(jìn)行估計(jì)，進(jìn)而建立增量式地圖，使智能汽車獲得了定位信息與高精度地圖。SLAM技術(shù)的發(fā)展會(huì)極大地影響智能汽車的發(fā)展，研究智能汽車SLAM具有一定的實(shí)際意義。

多位學(xué)者對(duì)SLAM進(jìn)行研究，武二永將Rao-Blackwel- lised粒子濾波器的理論融入SLAM算法中[1]，采用改進(jìn)的粒子分布預(yù)測(cè)函數(shù)，提高了粒子濾波器的性能。陳白帆[2]和周武[3]等人都是對(duì)FastSLAM進(jìn)行改進(jìn)，陳白帆將粒子群優(yōu)化加入更新預(yù)測(cè)粒子的過程中，使得粒子在實(shí)際姿態(tài)區(qū)域更集中，提高了定位精度；周武將遺傳算法與FastSLAM相結(jié)合，解決了重采樣樣本枯竭的問題，提高算法性能。朱磊[4]等人將人工魚群算法應(yīng)用到預(yù)測(cè)粒子上，提高了粒子的定位精度。浙江大學(xué)的彭勃[5]和李永佳[6]都對(duì)Harris特征進(jìn)行了分析，彭勃提出了一種基于RANSAC的視覺里程計(jì)方案，采用了Harris特征，提高了計(jì)算效率。李永佳將Harris特征與尺度不變特征相結(jié)合，構(gòu)建出視覺里程計(jì)方案，具有較好的計(jì)算精度。李宇波[7]對(duì)特征區(qū)域網(wǎng)格化并將特征點(diǎn)聚類，提高了視覺里程計(jì)的魯棒性，并通過實(shí)驗(yàn)平臺(tái)進(jìn)行了驗(yàn)證。北京理工大學(xué)的江燕華[8]，通過增加車輛俯仰和傾斜約束，提高了視覺里程計(jì)的精度，并進(jìn)行了實(shí)車驗(yàn)證。喬明起[9]將神經(jīng)網(wǎng)絡(luò)理論加入視覺里程計(jì)中，設(shè)計(jì)了一種基于光流信息的位姿估計(jì)模型，提高了系統(tǒng)性能，通過仿真實(shí)驗(yàn)驗(yàn)證了理論的有效性。Shao-Hung Chan等人對(duì)激光的Gmapping算法和視覺的ORB-SLAM2進(jìn)行融合[10]，通過雷達(dá)與相機(jī)的數(shù)據(jù)融合來提高了室內(nèi)環(huán)境下的定位精度。

單線激光雷達(dá)在處理地下車庫場(chǎng)景時(shí)，由于點(diǎn)云信息不夠豐富且場(chǎng)景存在動(dòng)態(tài)物體，容易發(fā)生誤匹配，使得智能汽車定位發(fā)生偏移進(jìn)而使得SLAM過程失敗。采用視覺和雷達(dá)信息融合方法，將視覺傳感器信息嵌入到雷達(dá)SLAM框架中，讓車輛在地下車庫的場(chǎng)景實(shí)現(xiàn)精確的建圖和定位。

1 激光點(diǎn)云匹配算法研究

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

運(yùn)動(dòng)畸變是由于激光雷達(dá)的運(yùn)動(dòng)而產(chǎn)生的，旋轉(zhuǎn)一周的時(shí)間記為，激光收發(fā)元件在時(shí)間內(nèi)按照次數(shù)發(fā)送和接受激光束如圖1(a)，理論上運(yùn)動(dòng)的激光雷達(dá)的點(diǎn)云，認(rèn)為激光的一幀所有點(diǎn)的激光發(fā)射位置與激光接受位置是同一位置。實(shí)際情況如圖1(b)所示，激光雷達(dá)在運(yùn)動(dòng)過程中激光收發(fā)元件即旋轉(zhuǎn)也在平移，這樣導(dǎo)致激光雷達(dá)一幀點(diǎn)云數(shù)據(jù)的起始點(diǎn)和終止點(diǎn)不在同一處，會(huì)根據(jù)內(nèi)的移動(dòng)距離大小偏移，所獲取的點(diǎn)云數(shù)據(jù)會(huì)對(duì)后續(xù)計(jì)算產(chǎn)生較大誤差。

圖1 激光雷達(dá)點(diǎn)云示意圖

已知知激光幀開始時(shí)間t和終止時(shí)間t，兩個(gè)激光束間的時(shí)間為△，里程計(jì)數(shù)據(jù)存儲(chǔ)在一個(gè)隊(duì)列中，并按照時(shí)間排序，隊(duì)首的時(shí)間最早，隊(duì)尾的時(shí)間最晚。隊(duì)首的里程計(jì)數(shù)據(jù)的時(shí)間戳小于t，隊(duì)尾的里程計(jì)數(shù)據(jù)的時(shí)間戳大于t，保證了里程計(jì)數(shù)據(jù)的時(shí)間域要包含當(dāng)前幀的時(shí)間域。算法流程如下：

一幀激光數(shù)據(jù)的每個(gè)點(diǎn)都對(duì)應(yīng)了一個(gè)汽車位姿，對(duì)這些位姿進(jìn)行求解，即求解｛t，t+△，…，t}時(shí)刻的汽車位姿。首先求解t和t時(shí)刻對(duì)應(yīng)的位姿p和p。我們將里程計(jì)數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)同步即對(duì)齊時(shí)間戳，假設(shè)里程計(jì)數(shù)據(jù)隊(duì)列中，時(shí)刻t和t的數(shù)據(jù)分別為第數(shù)據(jù)和第數(shù)據(jù)：

p=[] （1）

p=[] （2）

里程計(jì)數(shù)據(jù)隊(duì)列中的時(shí)間戳和激光雷達(dá)數(shù)據(jù)的時(shí)間戳并不是一一對(duì)應(yīng)的，如果在雷達(dá)的數(shù)據(jù)的時(shí)間戳沒有里程計(jì)的數(shù)據(jù)，則進(jìn)行線性差值，假設(shè)在l，時(shí)刻有位姿且l<<，可以得到：

p=[] （3）

p=[] （4）

在當(dāng)前幀激光數(shù)據(jù)中，假設(shè)汽車在這段時(shí)間內(nèi)做勻加速運(yùn)動(dòng)，則汽車的位姿與時(shí)間構(gòu)成了二次函數(shù)，假設(shè)中間時(shí)刻為t，且l＜＜則：

已知p，p，p，對(duì)數(shù)據(jù)進(jìn)行差值，差值方程為二次曲線方程：

將二次曲線用分段函數(shù)表示，如果分段數(shù)大于4，可以忽略誤差。在t和t時(shí)間段內(nèi)，提取個(gè)線性差值汽車位姿｛p，p+1，…，p+k-2，p}，假設(shè)p和p+1之間存在個(gè)汽車位姿｛p，p1，…，p（-2），p+1}則：

經(jīng)過差值計(jì)算得到一幀激光數(shù)據(jù)，包含個(gè)激光點(diǎn)與其對(duì)應(yīng)的位姿｛1，2，…，p}。將位姿坐標(biāo)轉(zhuǎn)換，統(tǒng)一到同一坐標(biāo)系中：

公式（10）中，x為轉(zhuǎn)化前坐標(biāo)系的坐標(biāo)，x’為轉(zhuǎn)換后坐標(biāo)系的坐標(biāo)。

將得到的位姿轉(zhuǎn)換為激光數(shù)據(jù)，并通過ROS話題機(jī)制進(jìn)行發(fā)布：

通過激光點(diǎn)云的運(yùn)動(dòng)畸變?nèi)コ行Ы档土擞捎诶走_(dá)運(yùn)動(dòng)引起的點(diǎn)云誤差。

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

雷達(dá)點(diǎn)云匹配采用IMLS-ICP匹配算法進(jìn)行匹配[11]。篩選具有代表性的點(diǎn)，減少計(jì)算量提升計(jì)算速度。在點(diǎn)云中抽象出曲線，合理地模擬出點(diǎn)云的狀態(tài)，進(jìn)行幀間匹配。相比于其他ICP匹配算法，IMLS-ICP匹配算法結(jié)合了N-ICP[12]和PL-ICP[13]。二維情況下有著更好的匹配效果。

通過KNN算法，應(yīng)用N-ICP理論的法向量提取方法切點(diǎn)對(duì)應(yīng)點(diǎn)法向量，假設(shè)點(diǎn)p在半徑內(nèi)所有點(diǎn)為V，計(jì)算V的均值和均方差矩陣U：

對(duì)U進(jìn)行特征值分解：

法向量為最小特征值的特征向量。其對(duì)應(yīng)去率為：

構(gòu)建點(diǎn)云集合p的曲線：

點(diǎn)x在曲線上的投影y為：

y=x?(x) n（19）

得到點(diǎn)x和點(diǎn)y的對(duì)應(yīng)關(guān)系，并通過PL-ICP求解方法進(jìn)行求解：

對(duì)二維下的IMLS-ICP進(jìn)行驗(yàn)證，采用雷達(dá)數(shù)據(jù)包作為驗(yàn)證的數(shù)據(jù)集，將IMLS-ICP與PL-ICP進(jìn)行比較，如圖2所示，圖中可以看出PL-ICP在第二次轉(zhuǎn)彎后發(fā)生了匹配失誤，誤差越來越大直接偏離路徑，而IMLS-ICP能夠較好的呈現(xiàn)路徑。但是累積誤差不斷累積，存在了較大的偏移。

2 基于直方圖濾波的ORB特征匹配

2.1 直方圖均衡化濾波

地下車庫場(chǎng)景下由于燈光存在使得亮度忽明忽暗，而亮度影響圖像的灰度。圖像的灰度分布主要集中于較窄的區(qū)域，造成圖像的對(duì)比度低，會(huì)使特征點(diǎn)提取很困難，進(jìn)而影響特征點(diǎn)匹配結(jié)果，為使在不同的亮度條件下都有比較好的效果，采用直方圖均衡化濾波，將原圖像的直方圖修改為在整個(gè)灰度區(qū)間內(nèi)大致均勻分布，對(duì)集中的灰度分布進(jìn)行平坦化，增強(qiáng)圖像的對(duì)比度。直方圖均衡化濾波算法的主要流程為：

（1）計(jì)算圖像的灰度直方圖：

（3）種次號(hào)可以反映圖書的進(jìn)館時(shí)間，小號(hào)的圖書都是進(jìn)館比較早的書，可以了解圖書出版的先后時(shí)間順序，剔舊可以先從種次號(hào)數(shù)字較小的圖書開始剔，這樣圖書的剔舊工作也解決了。

（2）對(duì)原始圖像進(jìn)行累積直方圖計(jì)算：

（3）得到濾波后圖線的像素值：

公式（23）中，D是濾波后圖像的像素，（S）表示灰度的圖像的累積分布，為最大灰度級(jí)，最大值為255。

2.2 ORB特征匹配

假設(shè)空間中有個(gè)三維空間點(diǎn)P，其對(duì)應(yīng)投影p和深度信信息z，三維空間點(diǎn)對(duì)應(yīng)于世界坐標(biāo)系下的齊次坐標(biāo)為P= [X,Y,Z，1]，對(duì)應(yīng)投影U的像素齊次坐標(biāo)為[u,v，1]。未知量則是相機(jī)的位姿,。相應(yīng)地有：

實(shí)踐中，由于噪聲的原因，上式并不嚴(yán)格成立，即等式存在誤差。目標(biāo)就是最小化誤差的平方和：

這里的誤差項(xiàng)是觀測(cè)所得的像素坐標(biāo)和對(duì)應(yīng)空間點(diǎn)的三維位置根據(jù)當(dāng)前估計(jì)的位姿投影到像片上的位置之間的差異，即重投影誤差，如圖3所示，空間點(diǎn)P在第一張照片上的投影為1，在第二張照片中的投影為2，由于誤差的存在，投影在2’處。調(diào)整相機(jī)位姿，最小化虛線距離。忽略齊次坐標(biāo)的最后一維，這個(gè)誤差項(xiàng)是一個(gè)二維向量。此外，我們最小化的是所有點(diǎn)對(duì)重投影誤差的平方和，所以最后得到的是一個(gè)總體誤差最小的結(jié)果，而單個(gè)點(diǎn)的誤差并不會(huì)精確為零。并對(duì)最終位姿結(jié)果應(yīng)用BA優(yōu)化。

圖3 重投影誤差示意圖

圖4 地下車庫場(chǎng)景ORB特征匹配

如圖4和圖5所示，對(duì)地下車庫場(chǎng)景進(jìn)行ORB特征匹配。經(jīng)過BRIEF描述子篩選的特征點(diǎn)能夠更好地進(jìn)行ORB特征匹配，沒有出現(xiàn)誤匹配的情況。對(duì)于直方圖均衡化濾波的地下車庫場(chǎng)景，匹配的次數(shù)明顯增多，且均勻分布在圖像中，有了更多更精準(zhǔn)的ORB特征匹配。

圖5 直方圖均衡化濾波后的地下車庫場(chǎng)景ORB特征匹配

3 基于最小二乘法的信息融合算法

雷達(dá)位姿和相機(jī)位姿都是描述了汽車的運(yùn)動(dòng)軌跡，軌跡趨勢(shì)是大體一致的，兩種位姿具有線性相關(guān)性。最小二乘理論融合的優(yōu)點(diǎn)是速度快，非常適合實(shí)時(shí)環(huán)境。加入亮度權(quán)重，并對(duì)最小二乘計(jì)算的矯正矩陣進(jìn)行更新，以此實(shí)現(xiàn)滑動(dòng)，根據(jù)最近的數(shù)據(jù)來進(jìn)行計(jì)算，來進(jìn)行位姿融合。融合方式保留了激光的原有特性并加入了視覺的豐富信息協(xié)助定位，使得改進(jìn)的SLAM系統(tǒng)在不同亮度條件下的對(duì)稱的結(jié)構(gòu)化環(huán)境中能夠減少定位誤差。

用雷達(dá)位姿與相機(jī)位姿構(gòu)建直接線性方程：

公式（27）中，為矯正矩陣，為權(quán)重系數(shù)。

將公式27展開：

轉(zhuǎn)換為矩陣：

化簡(jiǎn)：

構(gòu)建超靜定方程，其中一共B組數(shù)據(jù)：

得到：

將C轉(zhuǎn)換為矯正矩陣。通過還進(jìn)行的更新，轉(zhuǎn)換為最終位姿：

p=Cp（34）

根據(jù)最終位姿與點(diǎn)云數(shù)據(jù)建立柵格地圖。

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

4.1 定位精度對(duì)比實(shí)驗(yàn)

本章主要利用奇瑞eQ1小螞蟻300汽車作為移動(dòng)平臺(tái)進(jìn)行實(shí)驗(yàn)，搭載了思嵐科技的RPLIDAR A1 360 度激光掃描測(cè)距雷達(dá)、樂視三合一體感攝像頭和里程計(jì)等傳感器，以太網(wǎng)、usb數(shù)據(jù)線、can總線等數(shù)據(jù)傳輸方式進(jìn)行數(shù)據(jù)傳輸。通過搭載的盈馳微型電腦進(jìn)行傳感器的數(shù)據(jù)處理和算法運(yùn)算，處理結(jié)果由副駕駛的顯示器進(jìn)行顯示，如圖6所示。

圖6 奇瑞eQ1小螞蟻300汽車

對(duì)兩種狀態(tài)下的位姿進(jìn)行對(duì)比，一種是只應(yīng)用雷達(dá)點(diǎn)云數(shù)據(jù)進(jìn)行匹配得到的位姿laser_pose，即原始SLAM位姿，另一種是在雷達(dá)點(diǎn)云數(shù)據(jù)匹配的基礎(chǔ)上加入了視覺定位和回環(huán)檢測(cè)得到的位姿fusion_pose，即改進(jìn)SLAM位姿。真實(shí)位姿用reference表示。實(shí)驗(yàn)結(jié)果如下圖7所示：

圖7 算法的軌跡對(duì)比圖

如圖8對(duì)laser_pose和fusion_pose進(jìn)行APE評(píng)測(cè)：針對(duì)真實(shí)位姿，原始SLAM位姿的ape誤差小于5 m，改進(jìn)SLAM位姿的ape誤差小于1 m。可以看出改進(jìn)SLAM系統(tǒng)性能明顯要高于原始SLAM。

圖8 fusion_pose與laser_pose之間的APE統(tǒng)

4.2 與主流開源2DSLAM對(duì)比實(shí)驗(yàn)

為了驗(yàn)證本算法的建圖質(zhì)量，與主流的開源2D激光SLAM進(jìn)行對(duì)比分析。主要對(duì)比的算法Gmapping、Hector和Cartographer。如圖9所示。

圖9 地圖對(duì)比

統(tǒng)一比較了四種算法的性能，如表1所示。

表1 算法性能對(duì)比

算法名稱幀間匹配方法優(yōu)化器大范圍精度使用里程計(jì) 本文算法IMLS-ICP+ORBg2o較好是 Gmapping粒子濾波無一般是 Hector勢(shì)能模型無較差否 Cartographer分支定界ceres較好是

5 結(jié)束語

針對(duì)單一傳感器在地下車庫場(chǎng)景中定位不準(zhǔn)的問題。將雷達(dá)位姿和深度相機(jī)位姿進(jìn)行了線性關(guān)聯(lián)，并采用加權(quán)最小二乘法去求解，并以一定的頻率對(duì)結(jié)果進(jìn)行更新。結(jié)合地下車庫場(chǎng)景的特殊性，提出一種基于直方圖濾波的ORB特征匹配算法。針對(duì)地下車庫場(chǎng)景亮度變化較大這一特點(diǎn)，對(duì)圖像進(jìn)行了直方圖均衡化濾波，增強(qiáng)了圖像在地下車庫場(chǎng)景的對(duì)比度，使得ORB特征點(diǎn)的提取匹配更加容易，對(duì)BRIEF進(jìn)行約束，濾掉了大量誤匹配，使得整個(gè)ORB特征匹配流程更魯棒性。對(duì)于獲得的位姿結(jié)果，進(jìn)行BA優(yōu)化，獲得更好的定位效果，提高了SLAM系統(tǒng)的有效性。

[1] 武二永,項(xiàng)志宇,沈敏一,等.大規(guī)模環(huán)境下基于激光雷達(dá)的機(jī)器人SLAM算法[J].浙江大學(xué)學(xué)報(bào)(工學(xué)版), 2007(12):1982-1986.

[2] 陳白帆,蔡自興,袁成.基于粒子群優(yōu)化的移動(dòng)機(jī)器人SLAM方法[J].機(jī)器人,2009,31(6): 513-517.

[3] 周武,趙春霞.一種基于遺傳算法的FastSLAM 2.0算法[J].機(jī)器人, 2009,31(1): 25-32.

[4] 朱磊,樊繼壯,趙杰,等.未知環(huán)境下的移動(dòng)機(jī)器人SLAM方法[J].華中科技大學(xué)學(xué)報(bào)(自然科學(xué)版), 2011,39(7):9-13.

[5] 彭勃.立體視覺里程計(jì)關(guān)鍵技術(shù)與應(yīng)用研究[D].杭州:浙江大學(xué), 2008.

[6] 李永佳.基于視覺的機(jī)器人定位研究[D].杭州:浙江大學(xué),2011.

[7] 李宇波.戶外環(huán)境下移動(dòng)機(jī)器人視覺里程計(jì)技術(shù)研究[D].長(zhǎng)沙:國(guó)防科學(xué)技術(shù)大學(xué),2012.

[8] 江燕華.車輛運(yùn)動(dòng)特性約束的智能車輛視覺里程計(jì)系統(tǒng)研究[D].北京:北京理工大學(xué),2014.

[9] 喬明起.基于深度神經(jīng)網(wǎng)絡(luò)的視覺位姿估計(jì)方法研究[D].合肥:中國(guó)科學(xué)技術(shù)大學(xué),2018.

[10] Shao-Hung Chan, Ping-Tsang Wu,Li-Chen Fu.Robust 2D Indoor Localization Through Laser SLAM and Visual SLAM Fusion[C]// Miyazaki, Japan,Japan:IEEE International Conference on Systems, Man,and Cybernetics (SMC),2018.

[11] Deschaud J E.IMLS-SLAM:scan-to-model matching based on 3D data[C]//2018:2480-2485.

[12] SERAFIN J,GRISETTI G. NICP:Dense Normal Based Point Cloud Registration[C]//IEEE/RSJ International Conference on Intelligent Robots & Systems. IEEE, 2015:742-749.

[13] ANDREA CENSI.An ICP variant using a point-to-line met ric [C]//2008 IEEE International Conference on Robotics and Auto- mation (ICRA 2008), vol.1.2008:19-25.

A Laser SLAM Algorithm Combining ORB Characteristics

LI Kuizhen, WANG Jin'gang, ZHANG Xiaojun*

（School of Mechanical Engineering, Hebei University of Technology, Tianjin 300130）

To solve the problem of inaccurate localization of SLAM in underground garage scenes, a new SLAM algorithm based on vision and radar information fusion is proposed, which uses the three-dimensional ICP algorithm IMLS-ICP (Implicit Moving Least Square-Iterative Closest Point) to work out the two-dimensional expression.At the same time, it integrates PL-ICP (Point to line-Iterative Closest Point) and N-ICP (normal-Iterative Closest Point) expression to make IMLS-ICP more suitable for two-dimensional environment and obtain better Point cloud matching efect.ORB(Oriented Fast and Rotated Brief) feature points were extracted from the image for feature matching. In view of the unstable brightness of the scene of underground garage, histogram equalization filtering was carried out on the image. Binary Robust Independent Elementary feature (Brief) descriptor was used for constraint to make the extraction and matching of ORB feature points more accurate and Robust.Local BA (Bundle Adjustment) optimization was carried out for the results obtained.Weighted least square method was used for pose fusion and raster map was built.The pose is analyzed through experiments and compared with the mainstream 2DSLAM algorithm to verify the effectiveness of the proposed algorithm.

SLAM; IMLS-ICP; ORB; Histogram equalization filtering; Weighted least squares

10.16638/j.cnki.1671-7988.2021.021.008

U495

A

1671-7988(2021)21-31-06

U495

A

1671-7988(2021)21-31-06

李奎鎮(zhèn)（1995.11—），男，碩士研究生，河北工業(yè)大學(xué)機(jī)械工程學(xué)院學(xué)生，主要研究方向?yàn)镾LAM。

張小俊，河北工業(yè)大學(xué)機(jī)械工程學(xué)院。

天津市新一代人工智能科技重大專項(xiàng)（編號(hào)：18ZXZNGX00230）。