基于三維激光雷達高精度地圖生成測繪背包研究

王志偉，薄孟德，王思元

（哈爾濱師范大學）

0 引言

高精度地圖（high－precision map），又稱三維地圖、自動駕駛圖，是指具有高精度、地圖元素更加詳細，屬性更加豐富、面向機器人（智能車）使用地圖.傳統電子地圖誤差可達10 m，三維信息一般不包含在電子地圖上；并且高精度地圖數據誤差通常在0.2 m 以內，并包含三維信息，不但能讓自動駕駛級別高車輛快速完成匹配定位，更能將車道等級信息提供給整個駕駛系統路徑規劃層，進而幫助車輛實現厘米等級路徑規劃和導航.國內百度智能汽車部、阿里旗下高德、四維圖新等各大地圖公司針對高速公路環境、城市重要路段高精度地圖制作，都有相關產品和方法，紛紛著手打造全國高精度地圖，搶占自動駕駛細分市場.但鑒于各大高校、封閉小區等封閉園區路段道路結構與主要道路存在較大差異，且相對來說，道路情況較為狹窄、結構較為復雜，不完全適用構建主要道路高精度地圖方法，因此構建高精度地圖這一特殊情況仍需繼續深入研究.高精度地圖制作過程主要包括數據采集、數據處理、語義特征提取、人工質量檢測、發布等過程，其中數據處理階段是將采集到地圖數據進行算法處理，從而獲得高精度初始地圖模板（通常以點云地圖形式存在），無語義信息，而高精度地圖精度則取決于這一關鍵環節，該文研究內容也主要以此為主.該文針對封閉園區環境高精度地圖構建，基于圖優化方法，結合多傳感器融合方案，研究設計了一套便攜式適配于封閉園區高精度地圖測繪背包，該測繪背包通過采集三維激光雷達點云數據和其他輔助傳感器數據，利用該文優化多傳感器融合算法對真實場景進行三維重建，形成高精度地圖，并把構建好高精度地圖在無人配送車上進行定位導航實驗，實驗結果證明：背包生成高精度地圖精度在0.1 m以內，可以很好完成地圖構建任務.

1 多傳感器融合方法

同步定位與地圖構建（Simultaneous Localization and Mapping，SLAM），是指移動機器人利用搭載傳感器確定自身位置及周圍環境過程，是實現自主導航重要前提.目前，移動機器人使用場景越來越復雜，這使得單一傳感器SLAM算法無法很好地在復雜使用環境下運行，魯棒性差，比如在光線不足、強光干擾、特征缺失、運動物體受到干擾場景下，視覺攝像機會出現跟蹤失效情況；2D激光雷達只能對某一平面2D信息進行掃描，不能對現場信息進行完整還原；慣性測量單元IMU存在零偏，采用積分方式求解位姿會產生累計誤差，GPS差分信號在建筑密集情況下狀態不佳，誤差精度較差.那么如何高效完成復雜場景下目標任務建圖導航就是當務之急了.Multi－Sensor融合SLAM根據不同傳感器特性成為潮流.該文旨在提高測繪背包在室外復雜環境下定位準確性和魯棒性，因此研究移動機器人常用可攜帶式傳感器.通過研究其傳感器模型及自身特性，在總結了各種傳感器優勢和缺點基礎上，研究將三維激光雷達、IMU 和GNSS－RTK 等傳感器進行多傳感器信息融合，以實現測繪背包在封閉園區未知環境下更加精確地定位建圖.

1.1 IMU外參標定

由加速度計和陀螺儀組成慣性測量單元（IMU）可以測量載體自身運動加速度和角速度，目前九軸IMU 還配備了磁力計，可以起到消除累積誤差作用.IMU是一種可以獲得高頻慣性測量值傳感器，這種傳感器成本低，體積小，精度高，在移動載體定位中經常使用.IMU 主要受到高斯白噪聲和BIAS 隨機游走兩種隨機誤差影響.高斯白噪聲是指在一段時間內，IMU 數據經歷平均值為0，方差為σ，高斯過程n（t），每個時刻彼此獨立：

其中，δ（）表示狄拉克函數.在實際環境中，IMU測量數據需要進行離散采樣，在只考慮高斯白噪聲積分時有式（3）：

協方差為：

L離散和連續高斯白噪聲轉換關系為：

1.2 IMU運動模型

IMU測量模型可以根據1.1 節分析誤差模型得到，只考慮高斯白噪聲和BIAS隨機游走：

其中，上標g為陀螺儀，上標a為加速度計，w 為世界坐標系，b為IMU 坐標系，gw為世界坐標系下重力，ω、a 分別為IMU 角速度和加速度真實值，～ω，～a分別為IMU 角速度和加速度測量值，用四元數q表示變換矩陣.由于位置對時間導數為速度，速度對時間導數為加速度，以四元數字表示旋轉，因此可以用IMU運動模型來表示：

1.3 IMU預積分

IMU傳感器在IMU坐標系B 下采集三軸角速度ωB，在一定頻率下采集三軸線性加速度aB.但陀螺儀和加速度計會受到零均值高斯噪聲ηa和ηg，以及隨時間推移而變化零偏ba和bg影響.系統狀態估計可以通過積分IMU 測量值在系統初始狀態已知情況下傳遞出去.但這種方法以系統初始狀態為基礎，在非線性優化調整系統初始狀態情況下，需要重復積分過程.

1.4 激光雷達－IMU外參旋轉矩陣標定

激光雷達－IMU之間外參旋轉矩陣RCE對于整個系統魯棒性十分重要，過大偏差會導致系統初始化不成功而崩潰.但因為激光雷達－IMU 晚餐旋轉矩陣對精度敏感，所以容易被激勵計算.該文使用手眼標定方法，對齊IMU 和激光雷達觀測系統相互旋轉，從而標定激光雷達－IMU 外參旋轉矩陣，如圖1所示.

圖1 三維激光雷達與IMU手眼標定方法

2 高精度地圖生成算法研究

2.1 因子圖

因子圖是由表示優化變量變量節點，表示因子因子節點這兩種節點組成無向圖.貝葉斯網絡之所有可以轉換成一個因子圖，是因為最大后驗概率由許多項因子乘積而成.對于由J（x）矩陣在高斯牛頓法中所組成線性增量方程組，其求解方法為矩陣分解.J（x）矩陣是由誤差函數f（x）決定，而在SLAM 問題中J（x）矩陣是維數很高矩陣.但由于J（x）矩陣稀疏特性，利用QR 分解J（x）矩陣構成高維矩陣效率就會變得很高.在機器人運行過程中，因子圖一般是逐漸成長和增大.對于逐漸往外延伸一個因子圖，在機器人往前移動過程中，增加了一組節點，隨著觀測數據越來越多，增加了越來越多因子進來.每一次求解因子圖都是比上次求解因子圖大一些，而且大部分因子圖與之前因子圖基本上是一致，這是增量推理問題.如果從零開始求解優化問題，所求矩陣會越來越大，求解速度會越來越慢，而且絕大部分是重復性工作.所以這里采用增量QR 分解方法來構建因子圖，共采用4 個因子，分別是預積分因子、激光雷達因子、GPS 因子和回環檢測因子.該框架基于ROS 機器人操作系統，ROS作為點對點設計、多語言支持、架構精簡、集成度高、工具包豐富、免費開源全球最受歡迎開源機器人操作系統.圖2所示為整體軟件系統框架.

圖2 測繪背包軟件系統架構圖

2.2 激光運動畸變矯正

首先通過當前激光幀起止瞬間IMU 數據估計旋轉增量，IMU 里程計數據計算平移增量，然后對該幀激光各個瞬間激光點（利用相對于激光幀起始瞬間位姿增量，將當前激光點轉化為激光幀起始瞬間坐標系下，實現校正）進行運動畸變糾正；同時對當前幀激光位姿進行粗略初始化，利用IMU數據姿態角（RPY、ROLL、Pitch、YAW）和IMU 里程計數據位姿，這部分對IMU 精度要求更高，因此選擇了九軸IMU（帶磁力計）.此部分訂閱來自ImuPreintegration 原始IMU 數據、訂閱IMU里程計數據，表示每一時刻對應位姿；訂閱原始激光點云數據；發布當前幀激光運動畸變校正之后有效點云，用于RVIZ展示，發布當前幀激光運動畸變校正之后點云信息，包括點云數據、初始位姿、姿態角、有效點云數據等，發布給FeatureExtration用于特征提取.用關鍵幀和之前n＋1 個關鍵幀中特征集合構成地圖，進行匹配，構建點到線、點到面約束.

2.3 點云特征提取

假設一幀激光數據，是在一個很小時間段內獲取，但是由于這段時間內，激光雷達可能處于運動狀態，那么就可能導致運動畸變.點云特征提取模塊通過運動畸變校正后當前幀激光點云，對各點曲率進行計算，然后對角點和平面點進行提?。ㄅ袛嗲蚀笮。?從ImageProjection訂閱當前激光幀運動畸變修正后點云信息.發布當前激光幀提取特征后點云信息，包括歷史數據有：移動畸變校正、點云數據、初始位置、姿態角度、有效點云數據、角點云、平面點云等，發布給MapOptimization；放出目前激光畫幅抽取角點云進行RVIZ展示；用于RVIZ 展示當前激光幀提取平面點點云發布.

2.4 IMU預積分

這部分主要是接收激光里程計和IMU 里程計，根據前一刻激光里程計，以及從這一刻到這一刻IMU里程計變換增量，計算出這一刻IMU里程計；RVIZ顯示局部IMU里程計軌跡，利用激光里程計，以優化后狀態為基礎，對當前幀狀態（包括位姿、速度、偏置）進行優化，在兩幀激光里程計之間IMU 預積分量構建因子圖，對IMU預積分量進行優化，得到每一瞬間IMU 里程計.當關鍵幀集合為空時，用激光幀imu 原始數據角度數據初始化，并且把數據用lastImuTransformation保存.該功能模塊訂閱了激光里程計、IMU里程計、IMU原始數據、激光里程計，發布了IMU里程計軌跡，只在激光里程計最近一幀瞬間到當前瞬間之間顯示出軌跡.

2.5 因子圖優化

此功能模塊主要包括SCAN－TO－MAP 匹配，局部關鍵幀map 特征點，提取當前幀特征點（平面點、角點），更新當前幀位置與姿態，實施SCAN－TO－MAP 迭代優化；關鍵幀因子圖優化（更新全部關鍵幀位姿，關鍵幀加入因子圖，加入激光里程計因子、GPS因子、閉環因子，執行因子圖優化）；做閉關式檢測，在歷史關鍵幀中尋找相近距離，相隔時間較遠幀設置為匹配幀，在匹配幀四周獲取局部關鍵幀MAP，實施SCAN－TOMAP 匹配，獲取最新位置與姿態變換，建立閉環式因子數據，并加入因子圖優化.這個功能模塊訂閱了目前激光幀點云信息（來自FeatureExtration），GPS里程計，以及由外部閉環檢測程序提供數據閉環，并將歷史上關鍵幀里程計進行了發布；局部關鍵幀地圖特征點云；激光里程計運動路線；服務保存地圖；局部關鍵幀MAP 匹配閉環；經過閉環優化后當前關鍵幀位置變換后特征點云；局部MAP 使用濾波器后平面點集合；歷史幀濾波后角點集、平點集；通過當前幀配準之后點云.

3 實驗平臺與硬件系統

為了滿足不同封閉園區環境，最終采用了搭載三維激光雷達、九軸IMU、GNSS－RTK 三種傳感器便攜式移動背包作為算法載體，為方便后續擴展而留有視覺外接接口，操作系統采用Linux系統，如圖3所示.RTK采用是電臺傳輸方案，在園區樓頂放置了一根能夠在4 km 內廣播信號4.5 m長天線，圖4 為在園區內收到衛星信號，實驗證明這個RTK差分信號在建筑群密度不是特別大情況下信號狀態良好.通過測繪背包生成兩個園區高精度地圖狀態良好，信息豐富，如圖5所示.

圖3 測繪背包硬件架構圖

圖4 封閉園區內各衛星信號圖

圖5 園區高精度地圖

實驗平臺選用是園區無人配送車和無人巡檢車，如圖6所示，作為封閉園區主要應用，這兩個實驗平臺分別采用Autoware、Apollo 自動駕駛系統.Autoware為世界上第一個開源自動駕駛框架，整體性能擁有強大魯棒性.Apollo 作為百度開發自動駕駛系統，是國內應用廣泛自動駕駛開源系統，在環境感知、路徑規劃、車輛操控等方面優勢較強.無人配送車采用是阿克曼模型作為地盤，無人巡檢車采用是差動運動模型，選用這兩個實驗平臺可以很好測試高精度地圖精度.對兩個不同園區進行高精度地圖制作，充分測試環境對移動背包影響，測試實驗證明，兩個園區高精度地圖在兩個不同實驗平臺上均可以很好地完成導航和避障工作.

圖6 無人巡檢車（左）和無人配送車（右）

4 結論

三維高精度地圖具備三維空間表達能力，可以更為直觀真實而精準地實現對現實物理世界數字化呈現和智慧化應用.在實現將現實物理世界映射到虛擬數字空間同時，三維高精度地圖可視化平臺將位置、時間、環境等動態信息進行整合，形成統一時空基準全場景化、多源多維多視角數字孿生世界，實現了既滿足道路交通地理信息系統基礎需求，又滿足道路交通地理信息系統基礎需求三維向多維升級應用.并且能夠支持智能交通領域前沿應用，如自動駕駛、車路協同、智能網聯等，通過對傳感器數據時間對齊，以因子圖優化方式，以激光雷達、IMU、GNSS 為傳感器，形成數據緊耦合融合，獲得高質量數據關鍵幀，完成真正意義上封閉園區數據采集.一個小型GNSS、IMU 和3D 激光雷達數據集已經建立起來，算法有效性已經在不同實驗平臺得到驗證.無人駕駛中建圖與定位存在許多不同算法，該文所研究只是SLAM 算法一個分支，屬于激光SLAM中一種算法.雖然該文在此基礎上做了一些改進與優化，但是仍然存在許多地方需要改進.首先已有很多SLAM算法將視覺SLAM和激光SLAM進行了融合，加入了視覺傳感器，將相機和激光雷達進行聯合標定，豐富了高精度地圖語義信息.如何在現有傳感器基礎上加上視覺傳感器，構建精度更高地圖生成算法是需要進一步研究內容，其次該文只是對激光雷達和IMU 數據進行緊耦合，對于GNSS 數據只是進行簡單位姿校正.而現在市面上已經存在成熟GNSS／IMU組合導航系統，是通過GNSS和IMU緊耦合或者松耦合實現.如何將其運用到SLAM中點云配準和圖優化，也是接下來需要考慮的問題.