為了解決激光雷達不均勻運動畸變問題，將視覺慣性里程表與激光雷達里程表相結(jié)合，構(gòu)建了三維地圖同時定位和繪圖(SLAM)方法。通過視覺估計和慣性測量單元IMU的預(yù)積分對預(yù)處理后的時間戳對齊數(shù)據(jù)進行初始化。通過約束滑動窗口優(yōu)化和視覺里程計的高頻位姿，將傳統(tǒng)的雷達均勻運動模型改進為多級均勻加速模型。從而減少點云失真。同時，利用Levenberg ~ Marquard LM方法對激光里程表進行優(yōu)化，提出了一種結(jié)合詞袋模型的環(huán)路檢測方法，最后構(gòu)建了三維地圖�；趯嵻囋囼灁�(shù)據(jù)，與LEGO￣LOAM(輕型和地面優(yōu)化激光雷達測程和可變地形地圖ping)方法的結(jié)果相比，平均誤差和中位數(shù)誤差分別提高了16%和23%。

０引言

隨著2020年2月《智能汽車創(chuàng)新發(fā)展戰(zhàn)略》的發(fā)布，汽車無人駕駛技術(shù)的研究已成為高校和產(chǎn)業(yè)界的熱門話題。同時定位與地圖繪制SLAM技術(shù)是無人駕駛技術(shù)的重要組成部分。當(dāng)無人車在全球定位系統(tǒng)GPS中出現(xiàn)故障時，SLAM技術(shù)可以在沒有先驗信息的情況下，依靠自身的傳感器獨立完成無人車的姿態(tài)估計和導(dǎo)航[1]。目前主流的SLAM方法根據(jù)傳感器類型可分為基于攝像機的視覺SLAM和基于雷達的激光SLAM[2]。近年來，由于慣性測量單元的集成(慣性測量單元IMU的視覺SLAM具有絕對尺度可估計且不受成像質(zhì)量影響的優(yōu)點，已逐漸成為該領(lǐng)域的研究熱點[3]。

視覺SLAM可進一步分為特征點法和光流法。特征點法通過特征點匹配跟蹤特征點，最后進行重投影光流規(guī)則基于恒灰度假設(shè)，將特征點法中描述子與特征點的匹配替換為光流跟蹤。在特征點法的SLAM方案中，ORB￣SLAM(定向FAST和旋轉(zhuǎn)BRIEF)最具代表性。ORB具有視點和光照的不變性，關(guān)鍵幀的提取和冗余幀的刪除也保證了BA(bundle adjustment)優(yōu)化的效率和準(zhǔn)確性[4-5]�？紤]到純視覺SLAM在旋轉(zhuǎn)過程中容易出現(xiàn)幀丟失，特別是純旋轉(zhuǎn)對噪聲比較敏感，因此，徐寬等[6]將IMU與視覺融合，利用高斯-牛頓方法對預(yù)集成的IMU信息和視覺信息進行優(yōu)化，再利用圖優(yōu)化方法對視覺重投影誤差和IMU殘差進行優(yōu)化，從而獲得更精確的位姿。VINS ~ MONO(一種魯棒通用的單目視覺慣性狀態(tài)估計器)在室外性能突出，它通過IMU+單目攝像機的方案來恢復(fù)目標(biāo)的尺度。由于采用光流跟蹤作為前端，ORB￣SLAM比使用描述子作為前端具有更強的魯棒性，并且ORB￣SLAM在高速運動時不易失跡[7]。

但純視覺SLAM需要光照條件適中，圖像特征鮮明，在室外難以構(gòu)建3D地圖SLAM可以構(gòu)建室外三維地圖，但在運動過程中容易產(chǎn)生不均勻的運動畸變，在退化場景中定位不準(zhǔn)確。因此，本文基于激光雷達采集的點云信息，提出了一種多傳感器集成的室外三維地圖構(gòu)建與定位方法。該方法首先計算視覺慣性里程計VIO，并輸出高頻位姿。然后利用激光測距技術(shù)ꎬLO(ꎬ)通過高頻位姿去除激光雷達的運動畸變，最后構(gòu)建三維地圖。

１算法框架

算法框架大致分為兩個模塊:視覺慣性里程表模塊、激光里程表模塊和映射模塊。視覺慣性里程表采用KLT(kanade-lucas-tomasi tracking)光流跟蹤相鄰兩幀，并使用IMU預(yù)積分作為相鄰兩幀運動的預(yù)測值。在初始化模塊中，視覺和IMU預(yù)積分松散耦合，求解相鄰幀之間的陀螺儀偏置、比例因子、重力方向和速度。采用滑動窗口法對基于視覺構(gòu)造和IMU構(gòu)造的殘差項進行優(yōu)化。輸出VIO計算出的高頻絕對位姿。通過兩個模塊之間的相機雷達聯(lián)合標(biāo)定得到外部參數(shù)矩陣，將相機坐標(biāo)系中的絕對位姿轉(zhuǎn)換為雷達坐標(biāo)系。

激光里程表和映射模塊將點云劃分為不同類型的聚類點，方便后續(xù)特征提取，然后融合高頻VIO位姿，將傳統(tǒng)的雷達均勻運動模型改進為多級均勻加速度模型。此時，點云融合了相機和IMU的信息。通過ICP(it￣r - nearest point)匹配，利用LM優(yōu)化兩幀點云之間的位姿變換矩陣，并將其轉(zhuǎn)換為初始點云坐標(biāo)系。最后，結(jié)合基于詞袋模型的循環(huán)檢測，構(gòu)建三維地圖。

2繪圖和定位方法

2.1攝像機和IMU數(shù)據(jù)預(yù)處理

由于FAST特征提取效率高，KLT光流跟蹤不需要描述，因此選擇兩者進行特征提取和光流跟蹤。設(shè)Ix、Iy分別表示像素亮度在x、y方向上的圖像梯度，表示t方向上的時間梯度u、v，分別表示光流在x、y軸上的速度矢量。根據(jù)KLT光流原理，構(gòu)造約束方程，利用最小二乘法得到u和v的方程:

在每張新圖像中，利用KLT算法跟蹤已有的特征點并檢測新的特征點。為了保證特征點的均勻分布，將圖像劃分為18×10大小相同的子區(qū)域，每個子區(qū)域最多提取10個FAST角點，每張圖像保持50 ~ 200個FAST角點。室外場景中相鄰兩幀之間的位移較大，且每個像素的亮度值可能會突然變化，這對特征點的跟蹤有不好的影響。因此，有必要去除特征點的異常值，然后將其投影到單位球上。異常值消除采用RANSAC算法，并結(jié)合卡爾曼濾波，在室外動態(tài)場景中實現(xiàn)更魯棒的光流跟蹤。圖2為未使用RANSAC算法和使用RANSAC算法的室外場景特征點跟蹤�？梢钥闯�，RANSAC算法的使用減少了誤跟蹤的情況。

（a）未使用RANSAC算法特征點追蹤

（b）使用RANSAC算法特征點追蹤

圖２ＲＡＮＳＡＣ算法對特征點追蹤的影響

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

式中:b為IMU坐標(biāo)系w為初始化時IMU所在坐標(biāo)系的原點，即世界坐標(biāo)系at和wt為IMU測量的加速度和角速度qb t k為時刻從IMU坐標(biāo)系到世界坐標(biāo)系的旋轉(zhuǎn)Ω為四元數(shù)右乘法。整合幀k和幀k+1之間的所有IMU數(shù)據(jù)。得到k+1幀的位置(P)、速度(v)和旋轉(zhuǎn)(Q)。這個PvQ被用作視覺估計的初始值，其中旋轉(zhuǎn)是四元數(shù)形式。

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

式中: Ｒｉｐｉ為相機位姿的旋轉(zhuǎn)和平移部分ｖｉ為相機在世界坐標(biāo)系下的速度ａｂｉ、 ωｂｉ分別為ＩＭＵ的加速度偏置和角速度偏置.

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

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

其中X * k為最大估計后驗值，r0為初始滑動窗口殘差，rIij為IMU觀測殘差，提出的標(biāo)定方法聯(lián)合標(biāo)定相機和激光雷達，確定相機和雷達的相對姿態(tài)。將優(yōu)化后的VIO姿態(tài)轉(zhuǎn)換為雷達坐標(biāo)系，再輸出到激光里程計模塊。同時，利用BRIEF描述符構(gòu)造的詞包DBoW2計算當(dāng)前幀與的相似度，進行循環(huán)檢測。

２.３點云數(shù)據(jù)預(yù)處理
點云預(yù)處理部分改進ＬＥＧＯ＿ＬＯＡＭ方案, 并將點云分為地面點、有效聚類點和離群點, 分為兩步:
１) 地面點云提取, 可將點云劃分為０.５ｍ×０.５ｍ的柵格, 計算柵格內(nèi)的最高點和最低點的高度差, 將高度差低于０.１５ｍ的柵格歸類于地面點.
２) 有效聚類點提取, 在標(biāo)記地面點后,某些小物體的擾動會對接下來的幀間配準(zhǔn)環(huán)節(jié)造成影響. 故對點云進行歐式聚類ꎬ 將聚類點數(shù)少于３０或在豎直方向上占據(jù)的線束小于３的點進行濾除
２.４高頻ＶＩＯ位姿去除點云畸變

由于機械式激光雷達在掃描過程中存在點云的非勻速運動畸變[１３] ,為提升點云配準(zhǔn)的精確度, 使用ＶＩＯ輸出的高頻位姿去除點云畸變. 首先,對齊兩傳感器系統(tǒng)的時間戳,如圖３所示,定義ｔＬｑ為雷達在第ｑ次掃描時的時間戳ꎬ 定義ｔＶ－Ｉｋ為ＶＩＯ系統(tǒng)第ｋ次位姿輸出時的時間戳ꎬ 則通過下式實現(xiàn)時間對齊戳:

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

２.５點云特征點的提取與匹配

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

３實際場景驗證

圖5 實驗路線

將本文方法標(biāo)記為Ａ法ＬＥＧＯ￣ＬＯＡＭ法標(biāo)記為Ｂ法. 表１為Ａ法和Ｂ法的對比兩者均為真值即ＧＰＳ數(shù)據(jù)進行對比. 表１中Ｍａｘ為最大誤差Ｍｅａｎ為均平均誤差Ｍｅｄｉａｎ為誤差中位數(shù) Ｍｉｎ為最小誤差ＲＭＳＥ為均方根誤差ＳＳＥ為誤差平方和ＳＴＤ為標(biāo)準(zhǔn)差.

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

表１中由序１至４的對照組可以看出，在各類型路線的各項誤差上，Ａ法比Ｂ法具有更小的誤差.在大場景地圖下，最大誤差減小了２６％，平均誤差減小了１６％，誤差中位數(shù)減�。玻常� 最小誤差減小８８％均方根誤差減�。玻埃フ`差平方和減�。常叮� 標(biāo)準(zhǔn)差減小３０％. 圖７(ａ)、７(ｂ)為大場景地圖下Ａ法與真值的軌跡對比和誤差分析圖７(ｃ)、７(ｄ)為大場景地圖下Ｂ法與真值的軌跡對比和誤差分析Ａ法相較于Ｂ法在大場景建圖下各方面誤差都得到有效的縮小.

圖7 兩種方法所建的軌跡與真值軌跡的誤差對比

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

參考文獻:

[1] HENING S IPPOLITO C A KRISHNAKUMAR K S et al. 3D LiDAR SLAM integration with GPS/INS for UAVs in urban areas GPS ~ degraded environments[C] / / AIAA Information Systems ~ AIAA Infotech@Aerospace. Grapevine: AIAA 2017: 0448.

[2] SHIN Y S PARK Y S KIM A. Direct visual SLAM using sparse depth for camera￣lidar system[C] / / International Conference on Robotics and Automation (ICRA). Brisbane: IEEE 2018:1-8.

[3] NUTZI G WEISS S SCARAMUZZA D et al. Fusion of IMU and vision for absolute scale estimation in monocular SLAM[J].Journal of Intelligent & Robotic Systems201161(1/2/3/4): 287-299.

[4] MUR￣ARTAL R MONTIEL J M M TARDOS J D ORB￣SLAM: a versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics 2015 31(5): 1147-1163.

[5]高翔，張濤，劉毅等。視覺SLAM十四講:從理論到實踐[M]。北京:電子工業(yè)出版社2017。(中文)

[6]徐凱�；贗MU信息融合的雙目視覺SLAM研究[D]。哈爾濱:哈爾濱工業(yè)大學(xué)2018。(中文)

[7] QIN T LI P SHEN S. Vins￣mono: a robust and versatile monocular visual￣inertial state estimator[J]. IEEE Transactions on Robotics 2018 34(4): 1004-1020.

[8] INDELMAN V WILLIAMS S KAESS M et al. Factor graph based incremental smoothing in inertial navigation systems[C] / /15th International Conference on Information Fusion. Singapore: IEEEꎬ 2012:2154-2161.

[9]程傳奇，郝向陽，李建生等�；诜蔷€性優(yōu)化的單目視覺/慣性組合導(dǎo)航算法[J]。慣性技術(shù)學(xué)報，2017,25(5):643-649。

[10] HINZMANN T SCHNEIDER T DYMCZYK M et al. Monocular visual￣inertial SLAM for fixed￣wing UAVs using sliding window based nonlinear optimization[J]. Lecture Notes in Computer Science 2016 10072: 569-581.

[11] UNNIKRISHNAN R HEBERT M. Fast extrinsic calibration of a laser rangefinder to a camera: CMU￣RI￣ TR-05-09 [R].Pittsburgh: Robotics Institute Carnegie Mellon University 2005.

[12] KASSIR A PEYNOT T. Reliable automatic camera￣laser calibration[C] / / Proceedings of the 2010 Australasian Conference on Robotics & Automation. Australasia: ARAA 2010: 1-10.

[13] ZHANG Jꎬ SINGH S. LOAM: lidar odometry and mapping in real￣time[C] / / Robotics: Science and Systems. Berkeley: CA 2014 2:9.

[14] SHAN Tꎬ ENGLOT B. Lego￣loam: lightweight and ground￣optimized lidar odometry and mapping on variable terrain[C] / /

IEEE/RSJ International Conference on Intelligent Robots and Systems. Madrid: IEEEꎬ 2018: 4758-4765.