何仲偉，張小俊，張明路

基于激光視覺融合的幀間匹配方法

何仲偉，張小俊，張明路

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

同步定位與地圖構(gòu)建（Simultaneous Location and Mapping, SLAM）是機(jī)器人在未知環(huán)境中實(shí)現(xiàn)自我導(dǎo)航能力的重要保證。目前SLAM算法使用的主傳感器基本是激光雷達(dá)或視覺相機(jī)。二者各具優(yōu)劣，激光雷達(dá)能更精確地進(jìn)行測距，視覺相機(jī)能反映環(huán)境豐富的紋理信息。與使用單一傳感器相比，將二者融合的SLAM算法能夠獲得更多環(huán)境信息，達(dá)到更好的定位和建圖效果。文章提出一種融合激光雷達(dá)和視覺相機(jī)的幀間匹配方法，通過在SLAM幀間匹配過程中加入地面約束以及視覺特征約束，提高幀間匹配過程精度，增強(qiáng)算法魯棒性，從而提升SLAM算法整體效果。文章最后利用采集的地下停車庫數(shù)據(jù)進(jìn)行結(jié)果驗(yàn)證，與開源算法A-LOAM進(jìn)行對(duì)比。結(jié)果表明，相比A-LOAM的幀間匹配方法，文章提出的方法相對(duì)位姿誤差提升約30%。

SLAM；多傳感器融合；地面提?。坏罱c(diǎn)；隨機(jī)一致性采樣；Fast特征；光流法；幀間匹配

引言

同步定位與地圖構(gòu)建（Simultaneous Location and Mapping, SLAM）能夠讓處于未知環(huán)境中的機(jī)器人進(jìn)行自身定位和獲取周遭信息，進(jìn)而實(shí)現(xiàn)路徑規(guī)劃和自主導(dǎo)航。SLAM算法中使用的主傳感器有激光雷達(dá)和視覺相機(jī)，其中激光雷達(dá)能夠獲取精確的環(huán)境距離信息，但因其具有一定的角分辨率，所以在空曠環(huán)境中獲取的數(shù)據(jù)較為稀疏；視覺相機(jī)能夠得到豐富的紋理信息，相比激光雷達(dá)數(shù)據(jù)更加連續(xù)致密，但對(duì)環(huán)境光照變化敏感[1]。因此，融合二者的SLAM系統(tǒng)更能適應(yīng)復(fù)雜多變的環(huán)境。

目前，已有多位學(xué)者對(duì)激光視覺融合SLAM算法進(jìn)行了研究。J. Zhang等利用視覺里程計(jì)估計(jì)機(jī)器的運(yùn)動(dòng)，高速率低精度地對(duì)準(zhǔn)點(diǎn)云，然后基于激光雷達(dá)來優(yōu)化運(yùn)動(dòng)估計(jì)和進(jìn)行高精度點(diǎn)云對(duì)準(zhǔn)[2]。Y. Xu等提出了一種基于EKF的RGB-D相機(jī)與激光雷達(dá)融合的SLAM方法用于處理相機(jī)匹配失敗，即當(dāng)相機(jī)匹配失敗時(shí)使用激光雷達(dá)對(duì)相機(jī)3D點(diǎn)云數(shù)據(jù)進(jìn)行補(bǔ)充并生成地圖[3]，該方法沒有真正地融合兩種傳感器數(shù)據(jù)，只是進(jìn)行了傳感器的切換。Y. Shin等提出了一種利用了雷達(dá)提供的稀疏深度，基于單目相機(jī)直接法的SLAM框架[4]，為了保證實(shí)時(shí)性，文章提出了利用滑動(dòng)窗口進(jìn)行追蹤，嚴(yán)格的位姿邊緣化和深度的大場景幀間匹配方法。歐陽毅利用激光數(shù)據(jù)為視覺圖像增加了深度信息，提高了圖像特征的提取效率，完成了對(duì)特征點(diǎn)的深度估計(jì)，且基于PnP算法完成了對(duì)機(jī)器人的運(yùn)動(dòng)估計(jì)，并采用光束平差法（Bundle Adjustment, BA）完成了局部優(yōu)化[5]。J.Graeter等開源了SLAM方案LIMO，利用單目相機(jī)和激光雷達(dá)來模擬超廣角深度相機(jī)，將提取到的視覺特征點(diǎn)轉(zhuǎn)換為三維數(shù)據(jù)點(diǎn)，利用PnP估計(jì)相機(jī)幀間運(yùn)動(dòng)，但整體采用VIO的策略[6]。

上述算法雖進(jìn)行了激光雷達(dá)和視覺相機(jī)數(shù)據(jù)融合，但在解算過程僅采用單一傳感器進(jìn)行運(yùn)動(dòng)估計(jì)，而非采用緊耦合的融合方式。與這些方法不同，本文提出了一種融合激光雷達(dá)和相機(jī)的緊耦合的幀間配準(zhǔn)方法，將點(diǎn)云特征和視覺特征共同加入幀間配準(zhǔn)誤差方程進(jìn)行求解。同時(shí)為了增加程序魯棒性，在進(jìn)行點(diǎn)云預(yù)處理過程中，本文提出了一種基于隨機(jī)一致性采樣（Random Sample Consensus, RAN- SAC）的地面點(diǎn)云提取方法，并將地面點(diǎn)云作為一種約束加入幀間配準(zhǔn)誤差方程。改進(jìn)的幀間配準(zhǔn)算法流程如圖1所示。

圖1 算法流程圖

1 算法框架

該流程主要包括預(yù)處理部分和運(yùn)動(dòng)估計(jì)部分。點(diǎn)云預(yù)處理部分包括利用慣性導(dǎo)航單元（Inertial Measurement Unit, IMU）去除激光點(diǎn)云畸變、提取地面點(diǎn)云、依曲率提取邊角點(diǎn)云和平面點(diǎn)云。視覺預(yù)處理部分包括提取Fast特征和光流跟蹤。完成上述過程后，利用預(yù)處理得到的數(shù)據(jù)進(jìn)行緊耦合的運(yùn)動(dòng)估計(jì)。

1.1 點(diǎn)云預(yù)處理

1.1.1分割地面點(diǎn)云

地面提取算法流程如圖2所示，在實(shí)際場景中，地面并非完美平面而是具有一定的弧度，但為方便配準(zhǔn)方程的構(gòu)建，仍采用平面模型進(jìn)行地面提取。場景中的墻面也會(huì)對(duì)平面模型提取過程造成干擾，因此首先依據(jù)雷達(dá)實(shí)際安裝高度利用直通濾波提取候選點(diǎn)云。為增加提取速度，直通濾波通常區(qū)間長度通常較小。當(dāng)?shù)孛姹旧砭哂衅露然蚣す饫走_(dá)安裝誤差問題，將導(dǎo)致地面與激光雷達(dá)坐標(biāo)系的平面具有一定夾角，該情況使得候選點(diǎn)云中包含較大比重的非地面點(diǎn)云，影響地面提取效果。為解決上述問題，本文逐漸增加候選點(diǎn)云數(shù)量獲得多個(gè)平面模型，利用模型篩選單幀點(diǎn)云中符合該模型的內(nèi)點(diǎn)數(shù)據(jù)。若相鄰兩次內(nèi)點(diǎn)數(shù)據(jù)變化幅度不大，則認(rèn)為提取到了正確的地面模型。

平面提取過程利用的是隨機(jī)一致性采樣（Random Sample Consensus, RANSAC）算法，該算法是從一組包含異常數(shù)據(jù)的觀測數(shù)據(jù)集中估計(jì)其數(shù)學(xué)模型參數(shù)的迭代方法[8]，是一種不確定算法，通過重復(fù)選擇數(shù)據(jù)中的一組隨機(jī)子集來達(dá)成目標(biāo)。

圖2為地面點(diǎn)云提取效果，圖中顯示的是單幀點(diǎn)云數(shù)據(jù)，其中彩色點(diǎn)云為地面點(diǎn)云，可以看出該方法成功提取到大部分地面點(diǎn)，能夠?yàn)檫\(yùn)動(dòng)估計(jì)階段提供正確的地面特征。

圖2 地面點(diǎn)云提取效果

1.1.2提取點(diǎn)云特征

特征點(diǎn)云分為邊角點(diǎn)和平面點(diǎn)，分別計(jì)算每個(gè)點(diǎn)的曲率，式（1）為曲率計(jì)算公式。()代表第幀點(diǎn)云中第個(gè)數(shù)據(jù)點(diǎn)，代表()的鄰近點(diǎn)集。

依據(jù)曲率閾值將所有點(diǎn)分為邊角點(diǎn)和平面點(diǎn)。為使數(shù)據(jù)分布均勻，將點(diǎn)云劃分為多個(gè)子區(qū)域，每個(gè)子區(qū)域中提取固定數(shù)量的特征邊角點(diǎn)和特征平面點(diǎn)。若某個(gè)數(shù)據(jù)點(diǎn)被選為特征點(diǎn)，其鄰域內(nèi)的點(diǎn)則不予考慮，避免特征點(diǎn)過于集中，使運(yùn)動(dòng)估計(jì)過程陷入局部最優(yōu)的狀況。

1.2 提取視覺特征

采集單幀圖片中的FAST（Features from Accele- rated Segmentst）特征，利用LK光流法在幀間進(jìn)行特征像素追蹤。

LK光流[9]算法原理基于三個(gè)假設(shè)：（1）亮度恒定，即連續(xù)的前后幀圖片中相同物體的像素值大小不發(fā)生變化；（2）時(shí)間連續(xù)，即在連續(xù)前后幀的圖片中物體的運(yùn)動(dòng)狀態(tài)是緩慢變化的；（3）空間均勻，即圖片場景中某一像素點(diǎn)的運(yùn)動(dòng)狀態(tài)的變化和它相鄰區(qū)域像素點(diǎn)的運(yùn)動(dòng)狀態(tài)變化相同。

假設(shè)在時(shí)刻，圖像中（,）像素點(diǎn)亮度為(,,),經(jīng)過短時(shí)間?后，其像素坐標(biāo)變?yōu)?+?,+?)，亮度變?yōu)?+?,+?,+?)，根據(jù)亮度恒定假設(shè)，得到公式（2）。

(+?,+?,+?)=(,,) （2）

對(duì)上式進(jìn)行一階泰勒展開，由于?很小，忽略高階項(xiàng)，得到公式（3）。

(+?,+?,+?)

（3）

將公式（2）和公式（3）聯(lián)立，等式兩邊同時(shí)除以?，得到公式（4）。

根據(jù)光度不變假設(shè)，等號(hào)左側(cè)為零。根據(jù)空間均勻假設(shè)，利用特征像素的多個(gè)鄰近像素構(gòu)造超定方程，求方程的最小二乘解得到像素速率，實(shí)現(xiàn)特征點(diǎn)的追蹤。

1.3 構(gòu)建誤差方程

1.3.1特征點(diǎn)ICP約束

傳統(tǒng)ICP算法需已知兩幀點(diǎn)云中數(shù)據(jù)點(diǎn)的對(duì)應(yīng)關(guān)系，將前幀點(diǎn)云利用幀間位姿變換到后幀坐標(biāo)系下，以對(duì)應(yīng)兩點(diǎn)間的距離作為誤差項(xiàng)，迭代求解變換矩陣。

在實(shí)際場景中，沒有數(shù)據(jù)點(diǎn)之間的對(duì)應(yīng)關(guān)系，同時(shí)高線數(shù)激光雷達(dá)數(shù)據(jù)量龐大，用全部數(shù)據(jù)點(diǎn)構(gòu)建誤差方程進(jìn)行幀間匹配難以達(dá)到實(shí)時(shí)性要求。因此，該部分采用經(jīng)典激光SLAM算法LOAM中的特征點(diǎn)ICP匹配。將點(diǎn)云預(yù)處理階段的特征點(diǎn)利用幀間位姿變換到目標(biāo)坐標(biāo)系下，在目標(biāo)坐標(biāo)系中使用k-維樹[10]（k-dimensional tree, kd-tree）搜索邊角特征的兩個(gè)最近邊角點(diǎn)和平面特征的三個(gè)最近平面點(diǎn)，得到特征之間的匹配關(guān)系。以兩幀特征間的距離1(T+1,k)作為誤差項(xiàng)，變換矩陣T+1,k作為待優(yōu)化變量，進(jìn)行迭代優(yōu)化計(jì)算，見公式（5），其中為邊角特征到對(duì)應(yīng)邊角線的距離，為平面特征到對(duì)應(yīng)平面的距離。

1.3.2地面約束

在較為空曠的環(huán)境中，點(diǎn)云數(shù)量會(huì)大幅減少。此外，由激光雷達(dá)存在水平角分辨率，導(dǎo)致單線距離雷達(dá)較遠(yuǎn)數(shù)據(jù)點(diǎn)間有更大的間距，從而影響數(shù)據(jù)點(diǎn)曲率的計(jì)算，可能導(dǎo)致錯(cuò)誤的數(shù)據(jù)點(diǎn)分類，降低特征點(diǎn)ICP匹配的精度。相比提取到的特征點(diǎn)，地面點(diǎn)云在兩幀間連續(xù)，更具穩(wěn)定性。因此，將地面點(diǎn)云作為約束加入誤差方程，能降低錯(cuò)誤匹配特征點(diǎn)帶來的影響，使匹配算法更加精確、魯棒，見公式（6），式中J代表點(diǎn)到平面距離誤差，P代表第幀中提取的地面點(diǎn)云，F,k+1代表從+1幀地面點(diǎn)云中提取的平面模型。

2(T1,k)=min‖J(P,F+1)‖ （6）

1.3.3視覺特征約束

由于激光雷達(dá)角分辨率的緣故，實(shí)際點(diǎn)云并不連續(xù)，隨著雷達(dá)線數(shù)降低越發(fā)稀疏。而相機(jī)圖像攜帶的場景信息更加連續(xù)致密，大部分像素?zé)o法利用外參和激光數(shù)據(jù)獲得深度。因此，本文利用對(duì)極約束和兩幀間特征像素的對(duì)應(yīng)關(guān)系解算出不含尺度信息的位姿變換，將該變換中的旋轉(zhuǎn)加入誤差方程中進(jìn)行約束，加快結(jié)果收斂速度。公式（7）中T1,k和T1,k分別代表視覺特征獲得的幀間旋轉(zhuǎn)和待求的角度旋轉(zhuǎn)，J(P,P)代表兩種旋轉(zhuǎn)間對(duì)應(yīng)歐拉角的差值。

3(T1,k)min‖J(T1,k,T1,k)（7）

聯(lián)立以上各式，構(gòu)建幀間位姿變換誤差方程，待求變量為T1,k，利用高斯牛頓法對(duì)其進(jìn)行迭代求解，完成緊耦合的運(yùn)動(dòng)估計(jì)過程。相比于A-LOAM中單純利用特征點(diǎn)作為運(yùn)動(dòng)估計(jì)的約束，本文算法額外添加了地面約束和視覺旋轉(zhuǎn)約束，能夠?qū)πD(zhuǎn)和軸向位移做出更嚴(yán)格的限制，從而在有限的迭代輪數(shù)中獲得更精確的前端配準(zhǔn)結(jié)果。

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

本文的實(shí)驗(yàn)平臺(tái)為Inter Core i5-4210H, 2.90 GHz，4 GB內(nèi)存，操作系統(tǒng)Ubuntu 16.04。以上述算法替換A-LOAM中的前端配準(zhǔn)模塊得到改進(jìn)算法，完成智能車位姿估計(jì)并構(gòu)建地下停車庫點(diǎn)云地圖。利用SLAM領(lǐng)域開源工具EVO繪制智能車軌跡，計(jì)算智能車的絕對(duì)位姿誤差和相對(duì)位姿誤差，并與改進(jìn)前的算法A-LOAM進(jìn)行精度對(duì)比。

圖4為本文算法與A-LOAM的軌跡對(duì)比圖，其中深色線條為A-LOAM算法軌跡，淺色線條為改進(jìn)后算法軌跡。從圖中可以看出在路徑前半部分兩種算法得到的結(jié)果相差無幾。但在后半程中，A-LOAM算法出現(xiàn)了明顯的漂移，而本文算法表現(xiàn)相對(duì)更好。

圖4 軌跡對(duì)比圖

表1為兩種算法相對(duì)位姿誤差的結(jié)果對(duì)比，該項(xiàng)指標(biāo)與絕對(duì)誤差不同，其計(jì)算兩幀間的位姿變換并與真值進(jìn)行結(jié)果對(duì)比。即便某次幀間變換誤差過大，并不影響后續(xù)誤差的計(jì)算，能夠?qū)θ治蛔斯烙?jì)誤差作出更為公正的判斷，評(píng)價(jià)結(jié)果更全面。因此，本文將相對(duì)位姿誤差作為算法精度的判定標(biāo)準(zhǔn)。該項(xiàng)指標(biāo)中，最大值下降約59.67%，平均值下降約29.09%，中位數(shù)下降約27.66%，標(biāo)準(zhǔn)差下降約31.71%，均方誤差下降約30.43%。

表1 相對(duì)位姿誤差

絕對(duì)誤差A(yù)-LOAM本文算法 最大值0.041 90.016 9 平均值0.005 50.003 9 中位數(shù)0.004 70.003 4 標(biāo)準(zhǔn)差0.004 10.002 8 均方誤差0.006 90.004 8

SLAM算法框架的前端部分實(shí)時(shí)性要求較高且需要進(jìn)行傳感器數(shù)據(jù)預(yù)處理，較長時(shí)間的解算過程將會(huì)影響后續(xù)傳感器數(shù)據(jù)的讀取，因此前端配準(zhǔn)精度一般較低。而算法框架的后端優(yōu)化部分在前端配準(zhǔn)結(jié)果的基礎(chǔ)上進(jìn)行點(diǎn)云幀到局部點(diǎn)云地圖的高精度位姿估計(jì)，更為準(zhǔn)確的前端結(jié)果能有效加快算法的收斂速度，從而在相同的迭代次數(shù)下獲得更準(zhǔn)確的結(jié)果。

3 結(jié)語

本文提出了一種基于激光視覺融合的SLAM前端幀間配準(zhǔn)方法，通過耦合多種數(shù)據(jù)，添加約束使配準(zhǔn)結(jié)果更加精確。實(shí)驗(yàn)結(jié)果證明，改進(jìn)算法相比改進(jìn)前精度提升約30%。同時(shí)，添加的視覺信息能夠保證在一些退化環(huán)境中仍得到正確的旋轉(zhuǎn)約束，提升了前端幀間配準(zhǔn)算法的魯棒性。

[1] 王錦凱,賈旭.視覺與激光融合SLAM研究綜述[J].遼寧工業(yè)大學(xué)學(xué)報(bào)(自然科學(xué)版),2020,40(06):356-361.

[2] J.Zhang,S.Singh.Visual-lidar odometry and mapping: Low- drift, robust, and fast[C].2015 IEEE International Conference on Robotics and Automation (ICRA),2015,2174-2181.

[3] Y. Xu, Y.Ou and T. Xu. SLAM of Robot based on the Fusion of Vision and LIDAR[C].2018 IEEE International Confe- rence on Cyborg and Bionic Systems (CBS), Shenzhen,2018, pp.121-126.

[4] Y.Shin,Y.S.Park and A.Kim.Direct Visual SLAM Using Sparse Depth for Camera-LiDAR System[C].2018 IEEE Internati- onal Conference on Robotics and Automation (ICRA), Brisb- ane,QLD, 2018, pp. 5144-5151.

[5] 歐陽毅.基于激光雷達(dá)與視覺融合的環(huán)境感知與自主定位系統(tǒng)[D].哈爾濱:哈爾濱工業(yè)大學(xué),2019.

[6] Graeter J, Wilczynski A, Lauer M. LIMO: Lidar-Monocular Visual Odometry[C], IEEE/RSJ International Conference on Interlligent Robots and Systems(IROS), 2018 7872-7879.

[7] J.Zhang, S. Singh. LOAM: Lidar Odometry and Mapping in Real-time[C]. Robotics:Science and Systems Conference.2014.

[8] Fischler M A,Bolles R C.Random Sample Consensus:A Para- digm for Model Fitting with Applications to Image Analysis and Automated Cartography-ScienceDirect[J].Readings in Computer Vision, 1987:726-740.

[9] Lucas B D,Kanade T.An Iterative Image Registration Techniquewith an Application to Stereo Vision[C].Proceedings of the 7th International Joint Conference on Artificial Intelligence. Morgan Karfmann Publishers Inc.1997.

[10] Bentley J L,Friedman J H.Data structures for range searching [J]. ACM Computing Survyes,1979,11(4):397-409.

Frame-matching Method Based on Laser and Vision Fusion

HE Zhongwei, ZHANG Xiaojun, ZHANG Minglu

( School of Mechanical Engineering of Hebei University of Technology, Tianjin 300132 )

Simultaneous location and mapping (SLAM) is an important guarantee for robot to realize self navigation ability in unknown environment. At present, the main sensors used in SLAM algorithm are lidar or vision camera. Lidar can range more accurately, and visual camera can reflect the rich texture information of the environment. Compared with using a single sensor, SLAM algorithm can obtain more environmental information and achieve better positioning and mapping effect. In this paper, an inter frame matching method combining lidar and vision camera is proposed. By adding ground constraints and visual feature constraints in the process of slam inter frame matching, the accuracy of inter frame matching process is improved, the robustness of algorithm is enhanced, and the overall effect of SLAM algorithm is improved. Finally, the paper uses the collected underground parking data to verify the results, and compares with the open source algorithm A-LOAM. The results show that the relative pose error of the proposed method is improved by about 30% compared with A-LOAM.

SLAM; Laser and vision fusion; Ground extraction; ICP; RANSAC; FAST; Optical flow method; Inter frame match

A

1671-7988(2022)01-19-05

TP 24

A

1671-7988(2022)01-19-05

CLC NO.:TP 24

何仲偉，男，碩士研究生，河北工業(yè)大學(xué)機(jī)械工程學(xué)院，主要研究方向?yàn)橹悄苘嚟h(huán)境感知。

10.16638/j.cnki.1671-7988.2022.001.005