|
一種融合視覺與ＩＭＵ的車載激光雷達建圖與定位方法為了解決激光雷達不均勻運動畸變問題，將視覺慣性里程表與激光雷達里程表相結合，構建了三維地圖同時定位和繪圖(SLAM)方法。通過視覺估計和慣性測量單元IMU的預積分對預處理后的時間戳對齊數據進行初始化。通過約束滑動窗口優化和視覺里程計的高頻位姿，將傳統的雷達均勻運動模型改進為多級均勻加速模型。從而減少點云失真。同時，利用Levenberg ~ Marquard LM方法對激光里程表進行優化，提出了一種結合詞袋模型的環路檢測方法，最后構建了三維地圖�；趯嵻囋囼灁祿cLEGO￣LOAM(輕型和地面優化激光雷達測程和可變地形地圖ping)方法的結果相比，平均誤差和中位數誤差分別提高了16%和23%。 ０ 引言 隨著2020年2月《智能汽車創新發展戰略》的發布，汽車無人駕駛技術的研究已成為高校和產業界的熱門話題。同時定位與地圖繪制SLAM技術是無人駕駛技術的重要組成部分。當無人車在全球定位系統GPS中出現故障時，SLAM技術可以在沒有先驗信息的情況下，依靠自身的傳感器獨立完成無人車的姿態估計和導航[1]。目前主流的SLAM方法根據傳感器類型可分為基于攝像機的視覺SLAM和基于雷達的激光SLAM[2]。近年來，由于慣性測量單元的集成(慣性測量單元IMU的視覺SLAM具有絕對尺度可估計且不受成像質量影響的優點，已逐漸成為該領域的研究熱點[3]。 視覺SLAM可進一步分為特征點法和光流法。特征點法通過特征點匹配跟蹤特征點，最后進行重投影光流規則基于恒灰度假設，將特征點法中描述子與特征點的匹配替換為光流跟蹤。在特征點法的SLAM方案中，ORB￣SLAM(定向FAST和旋轉BRIEF)最具代表性。ORB具有視點和光照的不變性，關鍵幀的提取和冗余幀的刪除也保證了BA(bundle adjustment)優化的效率和準確性[4-5]�？紤]到純視覺SLAM在旋轉過程中容易出現幀丟失，特別是純旋轉對噪聲比較敏感，因此，徐寬等[6]將IMU與視覺融合，利用高斯-牛頓方法對預集成的IMU信息和視覺信息進行優化，再利用圖優化方法對視覺重投影誤差和IMU殘差進行優化，從而獲得更精確的位姿。VINS ~ MONO(一種魯棒通用的單目視覺慣性狀態估計器)在室外性能突出，它通過IMU+單目攝像機的方案來恢復目標的尺度。由于采用光流跟蹤作為前端，ORB￣SLAM比使用描述子作為前端具有更強的魯棒性，并且ORB￣SLAM在高速運動時不易失跡[7]。 但純視覺SLAM需要光照條件適中，圖像特征鮮明，在室外難以構建3D地圖SLAM可以構建室外三維地圖，但在運動過程中容易產生不均勻的運動畸變，在退化場景中定位不準確。因此，本文基于激光雷達采集的點云信息，提出了一種多傳感器集成的室外三維地圖構建與定位方法。該方法首先計算視覺慣性里程計VIO，并輸出高頻位姿。然后利用激光測距技術ꎬLO(ꎬ)通過高頻位姿去除激光雷達的運動畸變，最后構建三維地圖。 １ 算法框架 算法框架大致分為兩個模塊:視覺慣性里程表模塊、激光里程表模塊和映射模塊。視覺慣性里程表采用KLT(kanade-lucas-tomasi tracking)光流跟蹤相鄰兩幀，并使用IMU預積分作為相鄰兩幀運動的預測值。在初始化模塊中，視覺和IMU預積分松散耦合，求解相鄰幀之間的陀螺儀偏置、比例因子、重力方向和速度。采用滑動窗口法對基于視覺構造和IMU構造的殘差項進行優化。輸出VIO計算出的高頻絕對位姿。通過兩個模塊之間的相機雷達聯合標定得到外部參數矩陣，將相機坐標系中的絕對位姿轉換為雷達坐標系。 激光里程表和映射模塊將點云劃分為不同類型的聚類點，方便后續特征提取，然后融合高頻VIO位姿，將傳統的雷達均勻運動模型改進為多級均勻加速度模型。此時，點云融合了相機和IMU的信息。通過ICP(it￣r - nearest point)匹配，利用LM優化兩幀點云之間的位姿變換矩陣，并將其轉換為初始點云坐標系。最后，結合基于詞袋模型的循環檢測，構建三維地圖。 2繪圖和定位方法 2.1攝像機和IMU數據預處理 由于FAST特征提取效率高，KLT光流跟蹤不需要描述，因此選擇兩者進行特征提取和光流跟蹤。設Ix、Iy分別表示像素亮度在x、y方向上的圖像梯度，表示t方向上的時間梯度u、v，分別表示光流在x、y軸上的速度矢量。根據KLT光流原理，構造約束方程，利用最小二乘法得到u和v的方程:
在每張新圖像中，利用KLT算法跟蹤已有的特征點并檢測新的特征點。為了保證特征點的均勻分布，將圖像劃分為18×10大小相同的子區域，每個子區域最多提取10個FAST角點，每張圖像保持50 ~ 200個FAST角點。室外場景中相鄰兩幀之間的位移較大，且每個像素的亮度值可能會突然變化，這對特征點的跟蹤有不好的影響。因此，有必要去除特征點的異常值，然后將其投影到單位球上。異常值消除采用RANSAC算法，并結合卡爾曼濾波，在室外動態場景中實現更魯棒的光流跟蹤。圖2為未使用RANSAC算法和使用RANSAC算法的室外場景特征點跟蹤�？梢钥闯�，RANSAC算法的使用減少了誤跟蹤的情況。
（a）未使用RANSAC算法特征點追蹤
（b）使用RANSAC算法特征點追蹤 圖２ ＲＡＮＳＡＣ 算法對特征點追蹤的影響 IMU響應速度快，不受成像質量的影響，可以估計室外表面無結構物體視覺定位的絕對尺度特征補充。在攝像機姿態估計時，如果在幀間插入IMU所有采樣次數對應的所有位姿進行優化，會降低程序運行效率[8-9]，并且需要對IMU進行預積分處理，將高頻輸出的加速度和角速度的實測值轉換為單個觀測值。將測量值在非線性迭代中再次線性化，形成幀間狀態量的約束因子[7]。連續時間的IMU預積分如下式所示。
式中:b為IMU坐標系w為初始化時IMU所在坐標系的原點，即世界坐標系at和wt為IMU測量的加速度和角速度qb t k為時刻從IMU坐標系到世界坐標系的旋轉Ω為四元數右乘法。整合幀k和幀k+1之間的所有IMU數據。得到k+1幀的位置(P)、速度(v)和旋轉(Q)。這個PvQ被用作視覺估計的初始值，其中旋轉是四元數形式。 ２.２ 滑窗優化
式中: Ｒｉ ｐｉ 為相機位姿的旋轉和平移部分 ｖｉ 為相機在世界坐標系下的速度 ａｂｉ、 ωｂｉ 分別為ＩＭＵ的加速度偏置和角速度偏置. 設在 ｋ 時刻參與優化滑窗中的所有幀的所有 ｘｉ 的集合為 Ｘｋ 系統的所有觀測量為 Ｚｋ. 結合貝葉斯公式，用最大后驗概率估計系統的狀態量，如下式：
將該最大后驗問題轉化為優化問題，定義優化目標函數見下式.
其中X * k為最大估計后驗值，r0為初始滑動窗口殘差，rIij為IMU觀測殘差，提出的標定方法聯合標定相機和激光雷達，確定相機和雷達的相對姿態。將優化后的VIO姿態轉換為雷達坐標系，再輸出到激光里程計模塊。同時，利用BRIEF描述符構造的詞包DBoW2計算當前幀與的相似度，進行循環檢測。 ２.３ 點云數據預處理 由于機械式激光雷達在掃描過程中存在點云的非勻速運動畸變[１３] ,為提升點云配準的精確度, 使用 ＶＩＯ 輸出的高頻位姿去除點云畸變. 首先,對齊兩傳感器系統的時間戳,如圖 ３ 所示,定義 ｔＬｑ為雷達在第 ｑ 次掃描時的時間戳ꎬ 定義 ｔＶ－Ｉｋ為 ＶＩＯ 系統第 ｋ 次位姿輸出時的時間戳ꎬ 則通過下式實現時間對齊戳:
通過兩階段的位移和速度,插值計算點云的速度、 位移和歐拉角,消除雷達非勻速運動產生的畸變. ２.５ 點云特征點的提取與匹配 點云特征點主要包含兩類: 平面特征點和邊緣特征點. 如圖 ４ 所示, 由于斷點曲率較大,平行點曲率較小,會分別被誤當做邊緣點和平面點提取[１４] .因此,在進行特征提取前,必須對斷面上的斷點和與激光線方向相平行的平行點進行去除處理. 定義點云粗糙度為在 ｋ 時刻距點云最近的前后五個點的集合,通過對 ｃｋꎬ ｉ的大小來對邊緣點和平面點進行閾值分割,如下式:
３ 實際場景驗證
圖5 實驗路線 將本文方法標記為 Ａ 法ＬＥＧＯ￣ＬＯＡＭ 法標記為 Ｂ 法. 表 １ 為 Ａ 法和 Ｂ 法的對比兩者均為真值即ＧＰＳ 數據進行對比. 表 １ 中Ｍａｘ 為最大誤差Ｍｅａｎ 為均平均誤差Ｍｅｄｉａｎ 為誤差中位數 Ｍｉｎ 為最小誤差ＲＭＳＥ 為均方根誤差ＳＳＥ 為誤差平方和ＳＴＤ 為標準差.
表 １ 實車驗證結果對比 表 １ 中由序 １ 至 ４ 的對照組可以看出，在各類型路線的各項誤差上，Ａ 法比 Ｂ 法具有更小的誤差.在大場景地圖下，最大誤差減小了２６％，平均誤差減小了 １６％， 誤差中位數減小２３％ 最小誤差減�。福福ゾ礁`差減�。玻埃フ`差平方和減�。常叮� 標準差減小３０％. 圖 ７(ａ)、 ７(ｂ)為大場景地圖下 Ａ 法與真值的軌跡對比和誤差分析圖 ７(ｃ)、 ７(ｄ)為大場景地圖下 Ｂ 法與真值的軌跡對比和誤差分析Ａ 法相較于 Ｂ 法在大場景建圖下各方面誤差都得到有效的縮小.
圖7 兩種方法所建的軌跡與真值軌跡的誤差對比 ４ 結語 參考文獻: [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]。北京:電子工業出版社2017。(中文) [6]徐凱�；贗MU信息融合的雙目視覺SLAM研究[D]。哈爾濱:哈爾濱工業大學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]程傳奇，郝向陽，李建生等。基于非線性優化的單目視覺/慣性組合導航算法[J]。慣性技術學報，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. IMU姿態傳感器 |
