一種融合視覺與IMU的車載激光雷達建圖與定位方法

林晨浩，彭育輝

(福州大學機械工程及自動化學院，福建 福州 350108)

0 引言

隨著2020年2月《智能汽車創(chuàng)新發(fā)展戰(zhàn)略》的發(fā)布，汽車無人駕駛技術(shù)的研究成為高校與工業(yè)界的熱點.其中同時定位與建圖 (simultaneous localization and mapping，SLAM)技術(shù)是無人駕駛技術(shù)中不可忽視的一環(huán).當無人車在全球定位系統(tǒng)(global positioning system，GPS)失效后，SLAM技術(shù)可依靠自身搭載的傳感器在沒有先驗信息的情況下，自主完成無人車的位姿估計和導航[1].目前主流的SLAM方法可根據(jù)傳感器的類型分為基于相機的視覺SLAM和基于雷達的激光SLAM[2].近年來，由于融合慣性測量單元(inertial measurement unit，IMU)的視覺SLAM具有可估計絕對尺度，不受成像質(zhì)量影響的優(yōu)點，逐漸成為該領(lǐng)域的研究熱點[3].

視覺SLAM又可進一步分為特征點法和光流法.特征點法通過特征點匹配來跟蹤特征點，最后用重投影誤差優(yōu)化R，t.光流法則是基于灰度不變假設(shè)，把特征點法中的描述子和特征點匹配換成光流跟蹤.在眾多特征點法的SLAM方案中，ORB-SLAM(oriented FAST and rotated BRIEF)最具代表性，ORB特征具有視點不變性和光照不變性，關(guān)鍵幀的提取和冗余幀的剔除也為BA(bundle adjustment)優(yōu)化的效率和精度提供了保證[4-5].考慮到純視覺SLAM在旋轉(zhuǎn)時比較容易丟幀，特別是純旋轉(zhuǎn)，對噪聲敏感，不具備尺度不變性.因此，徐寬等[6]將IMU與視覺進行融合，將預積分后的IMU信息和視覺信息應用高斯牛頓法優(yōu)化，再利用圖優(yōu)化的方法優(yōu)化視覺的重投影誤差和IMU的殘差，從而獲取更精確的位姿.目前的光流方案中，VINS-MONO(a robust and versatile monocular visual-inertial state estimator)在戶外的性能較為出眾，其通過IMU+單目相機的方案恢復出了物體的尺度，由于使用的光流追蹤作為前端，相較于將描述子作為前端的ORB-SLAM具有更強的魯棒性，在高速運動時不容易跟丟[7].

然而，純視覺SLAM需要適度的光照條件和明顯的圖像特征，且在室外很難構(gòu)建三維地圖.激光SLAM可構(gòu)建室外三維地圖，但運動中易產(chǎn)生非勻速運動畸變，且在退化場景中定位不準確.為此，基于激光雷達采集的點云信息，本文提出一種融合多傳感器的室外三維地圖構(gòu)建與定位方法.該方法首先計算視覺慣性里程計(visual-inertial odometry, VIO)并輸出高頻位姿，再通過高頻位姿去除激光雷達運動畸變并計算激光里程計(laser-odometry, LO)，最終實現(xiàn)三維地圖構(gòu)建.

1 算法框架

圖1 算法框架Fig.1 Algorithm framework

如圖1所示，算法框架大致分為兩個模塊： 視覺慣性里程計模塊，激光里程計與建圖模塊.視覺慣性里程計用KLT(kanade-lucas-tomasi tracking)光流追蹤相鄰兩幀并將IMU預積分作為相鄰兩幀圖像運動的預測值，同時在圖像中提取詞袋信息用于回環(huán)檢測.初始化模塊將視覺與IMU預積分進行松耦合求解出陀螺儀偏置、尺度因子、重力方向和相鄰兩幀間的速度.通過滑動窗口法優(yōu)化基于視覺構(gòu)造的殘差項和基于IMU構(gòu)造的殘差項，輸出VIO計算的高頻絕對位姿.兩模塊間通過相機雷達聯(lián)合標定得到外參矩陣將相機坐標系下的絕對位姿轉(zhuǎn)換到雷達坐標系下.

激光里程計與建圖模塊將點云分類成不同類型的聚類點，方便后續(xù)特征提取，再融合高頻VIO位姿將傳統(tǒng)的雷達勻速運動模型改進為多階段的勻加速模型.此時點云已融合相機與IMU的信息，將ICP(iterative closest point)幀間匹配后的點云用LM優(yōu)化得到兩幀點云間的位姿變換矩陣，并轉(zhuǎn)換到初始點云坐標系下，最后融合基于詞袋模型的回環(huán)檢測構(gòu)建三維地圖.

2 建圖與定位方法

2.1 相機、IMU數(shù)據(jù)預處理

由于FAST特征提取效率高，KLT光流追蹤不需要描述子，故選取此二者進行特征提取和光流追蹤.設(shè)Ix、Iy表示圖像中像素點亮度在x、y方向上的圖像梯度，It表示在t方向上的時間梯度，u、v為光流沿x、y軸的速度矢量.根據(jù)KLT光流原理構(gòu)建約束等式，應用最小二乘法可求得u、v，如下式.

(1)

在每張新的圖像中，現(xiàn)有的特征點被KLT算法跟蹤并檢測新的特征點.為保證特征點的均勻分布，將圖像劃分為18×10 個大小完全相同的子區(qū)域，每個子區(qū)域最多提取10個FAST角點，保持每張圖像存在50至200個FAST角點.室外場景相鄰兩幀圖像之間位移較大，且各像素的亮度值可能發(fā)生突變，對特征點的跟蹤造成不良影響.因此需要對特征點進行離群值剔除后再將其投影到單位球面上.離群值剔除使用RANSAC算法并融合卡爾曼濾波器，以實現(xiàn)在戶外動態(tài)場景下較為魯棒的光流追蹤.圖2展示了戶外場景未使用RANSAC算法和使用RANSAC算法的的特征點跟蹤，可以看到，使用RANSAC算法降低了誤跟蹤的情況.

圖2 RANSAC算法對特征點追蹤的影響Fig.2 The influence of RANSAC algorithm on feature point tracking

IMU響應快，不受成像質(zhì)量影響，可估計絕對尺度的特性，對室外表面無結(jié)構(gòu)的物體的視覺定位進行補充.如果在相機位姿估算時將IMU所有采樣時刻所對應的全部位姿插入幀間進行優(yōu)化，將會降低程序運行效率[8-9]，需進行IMU預積分處理，將高頻率輸出的加速度和角速度測量值轉(zhuǎn)化為單個觀測值，該測量值將在非線性迭代重新進行線性化，形成幀間狀態(tài)量的約束因子[7].連續(xù)時刻的IMU預積分見下式.

(2)

2.2 滑窗優(yōu)化

初始化模塊恢復單目相機的尺度，需對視覺信息和IMU信息進行松耦合.首先，用SFM求解滑動窗內(nèi)所有幀的位姿與所有路標點的三維位置，再將其與之前求得的IMU預積分值進行對齊，從而解出角速度偏置，重力方向、尺度因子和每一幀所對應的速度.隨著系統(tǒng)的運行，狀態(tài)變量的數(shù)目越來越多，使用滑動窗口法[10]優(yōu)化窗口內(nèi)的狀態(tài)變量.定義在i時刻窗口中的優(yōu)化向量xi如下式.

(3)

式中：Ri，pi為相機位姿的旋轉(zhuǎn)和平移部分；vi為相機在世界坐標系下的速度；abi、ωbi分別為IMU的加速度偏置和角速度偏置.

設(shè)在k時刻參與優(yōu)化滑窗中的所有幀的所有xi的集合為Xk，系統(tǒng)的所有觀測量為Zk. 結(jié)合貝葉斯公式，用最大后驗概率估計系統(tǒng)的狀態(tài)量，如下式

將該最大后驗問題轉(zhuǎn)化為優(yōu)化問題，定義優(yōu)化目標函數(shù)見下式.

(5)

2.3 點云數(shù)據(jù)預處理

點云預處理部分改進LEGO_LOAM方案，并將點云分為地面點、有效聚類點和離群點，分為兩步： 1) 地面點云提取，可將點云劃分為0.5 m×0.5 m的柵格，計算柵格內(nèi)的最高點和最低點的高度差，將高度差低于0.15 m的柵格歸類于地面點.2) 有效聚類點提取，在標記地面點后，某些小物體的擾動會對接下來的幀間配準環(huán)節(jié)造成影響.故對點云進行歐式聚類，將聚類點數(shù)少于30或在豎直方向上占據(jù)的線束小于3的點進行濾除.

2.4 高頻VIO位姿去除點云畸變

圖3 時間戳對齊Fig.3 Timestamp alignment

由于機械式激光雷達在掃描過程中存在點云的非勻速運動畸變[13]，為提升點云配準的精確度，使用VIO輸出的高頻位姿去除點云畸變.首先，對齊兩傳感器系統(tǒng)的時間戳，如圖3所示，定義tLq為雷達在第q次掃描時的時間戳，定義tV-Ik為VIO系統(tǒng)第k次位姿輸出時的時間戳，則通過下式實現(xiàn)時間對齊戳：

tmin=min{|tLq-tV-Ik|，|tLq-tV-Ik+1|}

(6)

將tV-Ik，tV-Ik+1，tV-Ik+2，tV-Ik+3時刻計算的四個絕對位姿表示為Tk，Tk+1，Tk+2，Tk+3，將tLq至tLq+1時間內(nèi)的位移分為二個勻加速階段，改進為雷達運動模型為多階段的勻加速模型，如下式.

(7)

通過兩階段的位移和速度，插值計算點云的速度、位移和歐拉角，消除雷達非勻速運動產(chǎn)生的畸變.

2.5 點云特征點的提取與匹配

圖4 雷達掃描中的平行點與斷點Fig.4 Paralel points and breakpoints in lidar scanning

點云特征點主要包含兩類： 平面特征點和邊緣特征點.如圖4所示，由于斷點曲率較大，平行點曲率較小，會分別被誤當做邊緣點和平面點提取[14].因此，在進行特征提取前，必須對斷面上的斷點和與激光線方向相平行的平行點進行去除處理.定義點云粗糙度ck, i，Sk, i為在k時刻距點云pk, i最近的前后五個點的集合，通過對ck, i的大小來對邊緣點和平面點進行閾值分割，如下式:

(8)

幀間匹配環(huán)節(jié)的根本在于求解兩幀之間位姿變化的最小二乘問題，將匹配點之間的距離范數(shù)和作為誤差函數(shù)f(Pk-1,Pk)，如下式:

(9)

(10)

(11)

(12)

回環(huán)模塊采用點云近鄰搜索算法，以激光雷達當前位姿為搜索點，查找半徑范圍為5 m內(nèi)的若干個位姿，為提高搜索精度，同步DBoW2檢測結(jié)果，增加回環(huán)檢測時間上的約束.

3 實際場景驗證

本文數(shù)據(jù)集源自于福州大學旗山校園區(qū)域，實驗車輛為自主研制的無人駕駛方程式賽車測試平臺，如圖5所示.激光雷達為Pandar 40P，攝像頭為MYNT-1000 D(內(nèi)置IMU)，工控機為Nuvo-7160 GC，操作系統(tǒng)為ubuntu16.04.實驗路線位于福州大學旗山校區(qū)北區(qū)，分別對應大場景、方型、Z型、P型地圖，如圖6所示.

圖5 實驗路線Fig.5 Experimental route

圖6 實驗平臺Fig.6 Experimental facilities

將本文方法標記為A法，LEGO-LOAM法標記為B法.表1為A法和B法的對比，兩者均為真值，即GPS數(shù)據(jù)進行對比.表1中，Max為最大誤差；Mean為均平均誤差；Median為誤差中位數(shù)；Min為最小誤差；RMSE為均方根誤差；SSE為誤差平方和；STD為標準差.

表1 實車驗證結(jié)果對比

表1中，由序1至4的對照組可以看出，在各類型路線的各項誤差上，A法比B法具有更小的誤差.在大場景地圖下，最大誤差減小了26%，平均誤差減小了16%，誤差中位數(shù)減小23%，最小誤差減小88%，均方根誤差減小20%，誤差平方和減小36%，標準差減小30%.圖7(a)、7(b)為大場景地圖下A法與真值的軌跡對比和誤差分析，圖7(c)、7(d)為大場景地圖下B法與真值的軌跡對比和誤差分析，A法相較于B法，在大場景建圖下各方面誤差都得到有效的縮小.

圖7 兩種方法所建的軌跡與真值軌跡的誤差對比Fig.7 Error comparison between the trajectory built by the two methods and the true data trajectory

圖8(a)為兩種方法建圖的軌跡與真值軌跡的對比，圖8(b)、8(c)、8(d)為局部放大圖，圖8中虛線為GPS真值，藍線為A法所建軌跡，可見，A法所建的車輛軌跡比B法更貼近于GPS所輸出的真值.綠線為B法所建軌跡.圖9是本文方法所建的福州大學旗山校區(qū)北區(qū)的三維地圖，可見各部分回環(huán)情況良好.

圖8 軌跡局部放大圖Fig.8 Partial enlarged drawing of route

圖9 福州大學旗山校區(qū)北區(qū)三維地圖Fig.9 Three dimensional map of north district of Qishan campus of Fuzhou University

4 結(jié)語

室外激光雷達三維建圖存在點云非勻速運動畸變，在傳統(tǒng)激光SLAM的方法上融合了視覺慣性里程計，將雷達勻速運動模型改進為多階段的勻加速運動模型，并在回環(huán)檢測模塊引入詞袋模型.通過本文方法和LEGO-LOAM方法在建圖絕對位姿誤差上進行對比，本文方法在平均誤差和誤差中位數(shù)上分別提升了16%和23%，建圖精度有較大提升.由此可見，多階段勻加速的雷達運動模型在長時間建圖下能有效減小里程計累計誤差.提出的雙重回環(huán)檢測在回環(huán)時刻精確度上相較傳統(tǒng)方法具有更強的時間約束, 能滿足戶外三維建圖的需求.