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

０引言

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

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

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

１算法框架

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

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

2繪圖和定位方法

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

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

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

（a）未使用RANSAC算法特征點(diǎn)追蹤

（b）使用RANSAC算法特征點(diǎn)追蹤

圖２ＲＡＮＳＡＣ算法對(duì)特征點(diǎn)追蹤的影響

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

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

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

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

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

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

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

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

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

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

２.５點(diǎn)云特征點(diǎn)的提取與匹配

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

３實(shí)際場(chǎng)景驗(yàn)證

圖5 實(shí)驗(yàn)路線

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

表１實(shí)車驗(yàn)證結(jié)果對(duì)比

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

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

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

參考文獻(xiàn):

[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十四講:從理論到實(shí)踐[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áng)，李建生等。基于非線性優(yōu)化的單目視覺/慣性組合導(dǎo)航算法[J]。慣性技術(shù)學(xué)報(bào)，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.