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

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

1 引 言

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

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

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

2 三維建圖方法

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

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

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

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

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

(1)

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

(2)

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

(3)

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

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

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

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

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

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

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

3 實(shí)驗(yàn)設(shè)計與分析

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

圖4 實(shí)驗(yàn)平臺及實(shí)驗(yàn)設(shè)備

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

司南M600-mini接收機(jī)慣導(dǎo)部分司南M600-mini接收機(jī)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線激光雷達(dá)參數(shù)

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

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

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

4 結(jié)束語

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