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

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

1 引 言

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

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

綜合以上所述,本文提出一種基于GNSS/INS與激光雷達(dá)傳感器的緩存池建圖算法,該算法首先采用一種基于GNSSINS的偏移量線性差值法與四元數(shù)球面插值法并行算法將激光雷達(dá)點(diǎn)云轉(zhuǎn)換到唯一坐標(biāo)系下,糾正點(diǎn)云隨時(shí)間的漂移。根據(jù)歷史激光雷達(dá)數(shù)據(jù)幀生成的數(shù)據(jù)包,通過基于緩存池的建圖方法對(duì)周圍環(huán)境進(jìn)行點(diǎn)云分割與拼接,對(duì)得到的子地圖進(jìn)行體素濾波,最終得到豐富環(huán)境信息的三維重建地圖。本算法只使用到了激光雷達(dá)點(diǎn)云數(shù)據(jù)與GNSS/INS的偏移量與旋轉(zhuǎn)量,程序?qū)?zhí)行整個(gè)建圖過程并輸出三維建模結(jié)果。實(shí)驗(yàn)結(jié)果表明,該方法在大多數(shù)情況下都是可靠的,可以對(duì)未知的外界環(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)換到對(duì)應(yīng)的高斯投影坐標(biāo)系下,必須將接收機(jī)與激光雷達(dá)的數(shù)據(jù)進(jìn)行時(shí)間同步。但激光雷達(dá)與接收機(jī)的時(shí)間戳?xí)幸粋€(gè)偏差,并不是完全一一對(duì)應(yīng)的。例如:當(dāng)激光雷達(dá)時(shí)間戳為51.2478 s時(shí),GNSS/INS接收機(jī)數(shù)據(jù)的最近時(shí)間戳為51.2400 s與51.2500 s,兩者毫秒位對(duì)應(yīng)的數(shù)據(jù)沒有匹配。對(duì)于十六線的激光雷達(dá)來說,探測(cè)距離有80～100 m,角度乘以目標(biāo)物的距離就是目標(biāo)物的幅長(zhǎng)。即使激光雷達(dá)與接收機(jī)的真實(shí)姿態(tài)只相差略微,投射到的目標(biāo)物幅長(zhǎng)相對(duì)真實(shí)值會(huì)有較大的誤差。隨著時(shí)間推移,點(diǎn)云圖對(duì)真實(shí)環(huán)境描述的精度會(huì)越來越低。因而本文采用線性插值法與四元數(shù)球面插值法并行算法對(duì)慣導(dǎo)時(shí)間幀之間的數(shù)據(jù)進(jìn)行推算,提高激光雷達(dá)點(diǎn)云在高斯投影坐標(biāo)系下的位置精確度,解決建圖精度的問題。

對(duì)于旋轉(zhuǎn)偏量,若采用歐拉角法需結(jié)算三個(gè)微分方程,但當(dāng)載體俯仰角為90°時(shí)將出現(xiàn)奇點(diǎn),具有局限性；三角函數(shù)法需要解算六個(gè)微分方程,計(jì)算量大,不適合實(shí)時(shí)解算。四元數(shù)法與另外兩種相比較而言具有的優(yōu)勢(shì):①只需求解四個(gè)微分方程；②四元數(shù)法可以避免在使用歐拉角法時(shí)出現(xiàn)的奇異值問題[12]；③與三角函數(shù)法相比,四元數(shù)法歪斜誤差為零,刻度誤差的推導(dǎo)也較簡(jiǎn)單。由于線性插值法對(duì)于四元數(shù)是不適用的。因此本文對(duì)旋轉(zhuǎn)偏量采用四元數(shù)球面插值法,在兩個(gè)四元數(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ù)量龐大,每秒鐘都會(huì)有十萬個(gè)點(diǎn)的數(shù)據(jù)上傳。若不及時(shí)處理這些數(shù)據(jù)會(huì)造成系統(tǒng)的卡頓與崩潰?；谝陨蠁栴}本文采用緩存池分塊保存地圖。如圖2所示,在內(nèi)存中設(shè)立n塊緩存池。右邊的緩存池為較老且信息豐富的緩存池。當(dāng)小車在向前行駛過程中再也掃描不到緩存池1中的點(diǎn)云時(shí)。將緩存池1從內(nèi)存中釋放掉,保存為子地圖存放在硬盤中,并將新建的緩存池new推入內(nèi)存中,對(duì)緩存池new進(jìn)行點(diǎn)云的建圖。

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

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

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

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

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

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

圖4 實(shí)驗(yàn)平臺(tái)及實(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部分加速度計(jì)陀螺儀HV測(cè)量量程±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è)計(jì)在城市道路環(huán)境下行人車輛較少的情景下進(jìn)行,在內(nèi)存中設(shè)置了24塊緩存池以完成高精度建圖的要求。圖5為實(shí)驗(yàn)車運(yùn)動(dòng)時(shí)激光雷達(dá)采集的原始數(shù)據(jù),這些原始點(diǎn)云都是單幀互無關(guān)聯(lián)的點(diǎn)云而且沒有豐富的外界環(huán)境信息,顯示不同的顏色為物體對(duì)激光雷達(dá)射束的不同的反射強(qiáng)度。圖6為采用基于緩存池模型的三維建圖算法后得到的三維地圖。可以看出經(jīng)過本文算法處理后的三維地圖對(duì)地面、樹木,路面交通牌等標(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òng)駕駛汽車感知的主要傳感器。為了提升智能導(dǎo)航載體對(duì)外界未知環(huán)境的感知能力。本文提出一種基于GNSS/INS與激光雷達(dá)傳感器的緩存池建圖算法,該算法首先采用一種基于GNSSINS的偏移量線性差值法與四元數(shù)球面插值法并行算法將激光雷達(dá)點(diǎn)云轉(zhuǎn)換到唯一坐標(biāo)系下,糾正點(diǎn)云隨時(shí)間的漂移；通過基于緩存池的建圖方法對(duì)周圍環(huán)境進(jìn)行點(diǎn)云分割與拼接,點(diǎn)云經(jīng)過體素濾波后得到豐富環(huán)境信息的三維重建地圖。實(shí)驗(yàn)結(jié)果表明,使用該方法成功對(duì)汽車載體的周圍環(huán)境進(jìn)行掃描匹配,可以實(shí)現(xiàn)對(duì)未知的外界環(huán)境進(jìn)行三維的地圖重建。下一步的研究將著重于提升算法的實(shí)時(shí)性與基于三維地圖的應(yīng)用。