基于GNSS/INS與激光雷達的緩存池建圖

葉玨磊,周志峰,王立端,龐正雅

(1.上海工程技術大學機械與汽車工程學院,上海 201620;2.上海司南衛(wèi)星導航技術股份有限公司,上海 201801)

1 引 言

隨著各類傳感器技術的發(fā)展,人們對三維可視化技術的需求開始日益增長,如城市建圖、三維建模、環(huán)境感知等。對于自動駕駛車輛自主穩(wěn)定行駛時對外界環(huán)境模型的先驗信息不足的問題,激光雷達是必不可缺的范圍傳感器解決方案[1-2]。激光雷達可以提供高頻范圍測量,無論測量的距離如何誤差都相對恒定,其不易受天氣影響且對光照不敏感,返回的點云包括三維環(huán)境信息[3]。在激光雷達的唯一運動為旋轉激光束的情形下,點云的配準是簡單的。但當激光雷達本體開始移動時,精確的點云配準建圖就需要在激光雷達連續(xù)測距過程中了解激光雷達的姿態(tài)。

許多學者對這方面作了相關的研究,文獻[4]基于單個二維激光雷達通過使用SMACH庫對環(huán)境進行三維重建,使用ROS接口創(chuàng)建機器人環(huán)境的三維模型,無需先驗信息或人工干預。文獻[5]提出一種利用二維激光雷達在6自由度內移動時通過兩種并行算法進行測距和作圖的實時方法,通過激光雷達里程儀在高頻下處理估計速度,而激光雷達在低頻的速率下建圖。文獻[6]介紹了一種利用四臺二維激光雷達采集三維數(shù)據(jù)完成基于車輛的街道測繪系統(tǒng),在良好的GPS情況下預期精度可以達到30 mm。文獻[7]、[8]利用里程計如輪速編碼器和視覺里程表系統(tǒng)來配準激光雷達點云,但里程計集成了隨時間漂移的小增量運動,因此必然會產(chǎn)生漂移。文獻[9]通過結合視覺里程儀與三維激光雷達,利用視覺里程儀高頻估計運動,激光雷達低頻測程細化運動估計,并在掃描匹配時用線性運動模型對視覺里程計漂移引起的點云畸變進行校正。文獻[10]、[11]通過使用由二維激光雷達和IMU組成的手持測繪設備,利用地理結構與局部點云簇的幾何結構相匹配的方法進行點云配準。他們還使用多個二維激光雷達來繪制一個地下煤礦,該方法通過對分段數(shù)據(jù)進行批量優(yōu)化處理,利用分段間的邊界約束實現(xiàn)了軌跡的恢復,但該方法需要預先批處理,不適用于實時應用。

綜合以上所述,本文提出一種基于GNSS/INS與激光雷達傳感器的緩存池建圖算法,該算法首先采用一種基于GNSSINS的偏移量線性差值法與四元數(shù)球面插值法并行算法將激光雷達點云轉換到唯一坐標系下,糾正點云隨時間的漂移。根據(jù)歷史激光雷達數(shù)據(jù)幀生成的數(shù)據(jù)包,通過基于緩存池的建圖方法對周圍環(huán)境進行點云分割與拼接,對得到的子地圖進行體素濾波,最終得到豐富環(huán)境信息的三維重建地圖。本算法只使用到了激光雷達點云數(shù)據(jù)與GNSS/INS的偏移量與旋轉量,程序將執(zhí)行整個建圖過程并輸出三維建模結果。實驗結果表明,該方法在大多數(shù)情況下都是可靠的,可以對未知的外界環(huán)境進行三維模型的重建。

2 三維建圖方法

2.1 線性插值法與四元數(shù)插值法

激光雷達的數(shù)據(jù)采集頻率為10 Hz,而GNSS/INS接收機輸出數(shù)據(jù)的頻率為100 Hz。為了將激光雷達的點云數(shù)據(jù)轉換到對應的高斯投影坐標系下,必須將接收機與激光雷達的數(shù)據(jù)進行時間同步。但激光雷達與接收機的時間戳會有一個偏差,并不是完全一一對應的。例如:當激光雷達時間戳為51.2478 s時,GNSS/INS接收機數(shù)據(jù)的最近時間戳為51.2400 s與51.2500 s,兩者毫秒位對應的數(shù)據(jù)沒有匹配。對于十六線的激光雷達來說,探測距離有80～100 m,角度乘以目標物的距離就是目標物的幅長。即使激光雷達與接收機的真實姿態(tài)只相差略微,投射到的目標物幅長相對真實值會有較大的誤差。隨著時間推移,點云圖對真實環(huán)境描述的精度會越來越低。因而本文采用線性插值法與四元數(shù)球面插值法并行算法對慣導時間幀之間的數(shù)據(jù)進行推算,提高激光雷達點云在高斯投影坐標系下的位置精確度,解決建圖精度的問題。

對于旋轉偏量,若采用歐拉角法需結算三個微分方程,但當載體俯仰角為90°時將出現(xiàn)奇點,具有局限性；三角函數(shù)法需要解算六個微分方程,計算量大,不適合實時解算。四元數(shù)法與另外兩種相比較而言具有的優(yōu)勢:①只需求解四個微分方程；②四元數(shù)法可以避免在使用歐拉角法時出現(xiàn)的奇異值問題[12]；③與三角函數(shù)法相比,四元數(shù)法歪斜誤差為零,刻度誤差的推導也較簡單。由于線性插值法對于四元數(shù)是不適用的。因此本文對旋轉偏量采用四元數(shù)球面插值法,在兩個四元數(shù)之間平滑插值。如圖1所示,θ為單位向量P和Q之間的夾角,P和r之間的夾角為tθ,Q和r之間的夾角為(1-t)θ。 線性插值法與四元數(shù)球面插值法并行算法偽代碼如表1所示。

圖1 四元數(shù)球面插值法

插值的一般公式為:r=a(t)P+b(t)Q,在公式兩端分別點乘P與Q向量可得式(1):

(1)

利用式(1)求解出未知量a(t)與b(t),得式(2):

(2)

于是求得四元數(shù)球面線性插值公式為:

(3)

表1 線性插值法與四元數(shù)球面插值法并行算法偽代碼

2.2 基于緩存池模型的三維建圖算法

激光雷達點云數(shù)據(jù)量龐大,每秒鐘都會有十萬個點的數(shù)據(jù)上傳。若不及時處理這些數(shù)據(jù)會造成系統(tǒng)的卡頓與崩潰。基于以上問題本文采用緩存池分塊保存地圖。如圖2所示,在內存中設立n塊緩存池。右邊的緩存池為較老且信息豐富的緩存池。當小車在向前行駛過程中再也掃描不到緩存池1中的點云時。將緩存池1從內存中釋放掉,保存為子地圖存放在硬盤中,并將新建的緩存池new推入內存中,對緩存池new進行點云的建圖。

圖2 基于緩存池模型的三維建圖

這就保證了內存在任意時刻都只有n塊緩存池,不會發(fā)生內存的不夠或者崩潰現(xiàn)象。因為地圖內的點云信息是在高斯投影坐標系下的,因此掃描到的點云都是具有唯一性的。當小車行駛一圈再次來到相同的地點時,程序會把實時掃描到的相同點云信息代入地圖檢測是否這一塊已經(jīng)有先驗子地圖。如果發(fā)現(xiàn)存在對應的子地圖,算法將子地圖提出再次放入緩存池中,將掃到的實時點云添加到子地圖上的相應位置區(qū)內。這樣,無論在場景內跑多少圈,都可以在先驗地圖的基礎上對地圖進行優(yōu)化。如圖3所示為基于緩存池模型的三維建圖算法流程。

圖3 基于緩存池模型的三維建圖算法流程圖

對得到的三維子地圖進行Voxel體素網(wǎng)格方法下采樣,減少點云的數(shù)量。在每個體素(即設置的三維立方體大小)內,用單個體素中所有點云的重心來近似顯示體素中其他點,這樣該體素內所有點就用一個重心點最終表示,既壓縮了點云數(shù)據(jù)量的大小,同時保持了激光雷達點云的形狀特征。將所有子地圖體素濾波后得到過濾后的點云子地圖,拼接成最后的三維地圖。

3 實驗設計與分析

為了驗證本文提出的算法的有效性,我們搭建一個實驗平臺和一個測試區(qū)域來記錄和測試數(shù)據(jù),如圖4所示。平臺由以下幾項組成:一臺GNSSINS接收機M600mini,由上海司南衛(wèi)星導航技術股份有限公司生產(chǎn),規(guī)格參數(shù)見表2；一臺16線激光雷達,由深圳速騰聚創(chuàng)公司所生產(chǎn),規(guī)格見表3,實驗中將激光雷達架高以防止激光雷達掃到太多車頂?shù)臄?shù)據(jù)失去對環(huán)境信息的獲取。實驗中,激光雷達數(shù)據(jù)處理算法運行在一臺2.5 GHz四核6GB內存的筆記本電腦上。在基于Linux下的機器人操作系統(tǒng)(ROS)上,編寫C++程序完成算法的實現(xiàn)。本次實驗場景設置在路面上動態(tài)物體較少的環(huán)境下,避免非靜態(tài)物體對靜態(tài)環(huán)境的干擾。

圖4 實驗平臺及實驗設備

表2 司南M600-mini接收機參數(shù) Tab.2 The parameters of Sino M600-mini

司南M600-mini接收機慣導部分司南M600-mini接收機GNSS部分加速度計陀螺儀HV測量量程±4 g±250°/sRTK精度10 mm+1 ppm20 mm+1 ppm穩(wěn)定性 35°/h40 mg靜態(tài)差分精度2.5 mm+1 ppm5.0 mm+1 ppm

表3 速騰聚創(chuàng)16線激光雷達參數(shù)

本次實驗設計在城市道路環(huán)境下行人車輛較少的情景下進行,在內存中設置了24塊緩存池以完成高精度建圖的要求。圖5為實驗車運動時激光雷達采集的原始數(shù)據(jù),這些原始點云都是單幀互無關聯(lián)的點云而且沒有豐富的外界環(huán)境信息,顯示不同的顏色為物體對激光雷達射束的不同的反射強度。圖6為采用基于緩存池模型的三維建圖算法后得到的三維地圖?？梢钥闯鼋?jīng)過本文算法處理后的三維地圖對地面、樹木,路面交通牌等標志的還原度較好,且地圖中的點云都帶有高斯投影坐標系的坐標屬性,能夠為下一步載體通過高精度地圖定位提供先驗信息。

圖5 原始點云數(shù)據(jù)

圖6 三維重建后的道路環(huán)境地圖

4 結束語

多線程激光雷達在自主機器人、智能駕駛領域的重要性正在與日俱增,是自動駕駛汽車感知的主要傳感器。為了提升智能導航載體對外界未知環(huán)境的感知能力。本文提出一種基于GNSS/INS與激光雷達傳感器的緩存池建圖算法,該算法首先采用一種基于GNSSINS的偏移量線性差值法與四元數(shù)球面插值法并行算法將激光雷達點云轉換到唯一坐標系下,糾正點云隨時間的漂移；通過基于緩存池的建圖方法對周圍環(huán)境進行點云分割與拼接,點云經(jīng)過體素濾波后得到豐富環(huán)境信息的三維重建地圖。實驗結果表明,使用該方法成功對汽車載體的周圍環(huán)境進行掃描匹配,可以實現(xiàn)對未知的外界環(huán)境進行三維的地圖重建。下一步的研究將著重于提升算法的實時性與基于三維地圖的應用。