組合導航輔助的激光雷達－相機實時語義建圖?

張樂翔劉天弋張?zhí)嵘Ｐ◇K

(武漢大學衛(wèi)星導航定位技術研究中心,湖北 武漢430079)

機器人為了執(zhí)行一些復雜的任務,需要能夠利用環(huán)境中語義信息,構建包含語義信息的地圖稱為語義地圖。當前大多數激光雷達語義建圖都是使用64線或者128線激光雷達[1－2],然而64線和128線激光雷達價格過于昂貴,以Velodyne的產品為例,每臺高達幾十萬甚至上百萬人民幣。16線激光雷達則相對便宜得多,單臺采購價只有兩三萬人民幣,然而16線激光雷達由于其點云過于稀疏,難以提取其點云的語義特征。普通光學相機價格低廉且具有豐富的紋理數據,前人研究圖像語義信息提取也比較成熟[3－5],但其無法提供和激光雷達測距相當水平的高精度深度信息。因此,用圖像來輔助稀疏點云進行語義信息提取并構建語義地圖既可以保證地圖品質也可以降低成本。

文章[6]提出了一種相機輔助單線激光雷達語義建圖的方法,其只對二維點云進行聚類并與目標檢測框相匹配,該方法并不適用于多線激光雷達獲得的三維點云,且其不能實時生成語義點云地圖,限制了機器人根據實時的語義信息進行導航定位。文章[7]中提出了一種激光雷達－相機融合的方法,先將點云聚類并投影到圖像上,并根據聚類點云框和圖像目標檢測框的比例關系,來判斷點云和圖像是否成功匹配。該方法比較適合在開闊場景下使用,但當目標檢測框出現偏差以及目標物體前方有局部遮擋或后方有雜亂背景時則容易出現誤提取。

本文提出了一種組合導航輔助的激光雷達－相機語義建圖方案,利用GNSS/INS組合導航提供的位置和姿態(tài)信息輔助點云運動補償、異步采樣的點云－圖像配準以及構建地圖,使得16線激光雷達－相機系統(tǒng)能夠在保持較低成本的前提下準確、魯棒地提取出點云所對應的語義信息并實時構建語義地圖,為移動機器人利用語義信息進行導航和執(zhí)行任務奠定基礎。

1 系統(tǒng)設計

本文的移動機器人平臺是基于ROS(Robot Operating System)架構,以各個模塊/傳感器為節(jié)點,采用ROS消息的形式通信。

該機器人硬件平臺由英偉達公司生產的核心處理器Jetson AGX Xavier、自主研制的組合導航模塊[8]、Velodyne公司生產的VLP－16激光雷達、AVT Mako G－131彩色相機以及Autolabor底盤組成。VLP－16激光雷達內部的激光發(fā)射器和接收器根據設定的掃描頻率在水平方向旋轉,旋轉速度5 Hz～20 Hz,其水平視場(FOV)為360°,垂直視場為30°,測量距離范圍可以達到100 m。本文選取10 Hz掃描速率,對應的水平角分辨率為0.2°,垂直角分辨率為2°。AVT相機為分辨率1280×1204的RGB相機,其最高幀率可達62 Hz,鏡頭焦距為4 mm。為了與激光雷達掃描速率相一致,選取相機幀率為10 Hz。

核心處理器Xavier可通過串口接收機器人底盤的里程計編碼數據和自研組合導航模塊的GNSS(Global Navigation Satellite System)、IMU(Inertial Measurement Unit)數據,并通過4G網絡接收GNSS信號差分改正信號進行RTK(Real-time kinematic,實時動態(tài))定位。VLP－16激光雷達和AVT相機則通過千兆網線將圖像和點云數據傳輸至處理器Xavier進行融合。機器人系統(tǒng)數據傳輸結構如圖1所示。

多個傳感器之間時間和空間關系是多傳感器融合系統(tǒng)設計必須考慮的因素。本文利用組合導航模塊輸出的PPS(秒脈沖)來修正單片機本地晶振誤差漂移,并用單片機觸發(fā)其他傳感器采集或接收其他傳感器采集返回脈沖信號,從而將多個傳感器信號時間統(tǒng)一到衛(wèi)星時鐘基準下,確保其時間的一致性[8]。由于時間差異會導致所處的空間位置不同,對系統(tǒng)時間同步精度的要求主要取決于載體的速度,在機器人系統(tǒng)中其速度約為2 m/s,故時間同步精度在毫秒量級即可。對于VLP－16激光雷達,其有PPS接口,輸入PPS后可在激光數據對應的頭文件中解碼獲取衛(wèi)星時間系統(tǒng)下的點云采集時刻；對于相機,其在曝光時硬件會返回一個觸發(fā)脈沖,用組合導航模塊中衛(wèi)星時鐘基準下的單片機記錄觸發(fā)曝光脈沖時刻及曝光結束脈沖返回時刻。單片機記錄延時是微秒量級的,其記錄的時間的延時對于測量數據可以近似為無影響,由于本文實驗中相機曝光時長很短,在毫秒級別,因此將相機曝光中間時刻作為相機曝光時刻。

圖1 機器人系統(tǒng)數據傳輸結構

關于激光雷達、相機和組合導航系統(tǒng)間的空間標定,主要是要求解激光雷達、相機和IMU的外參,包括旋轉矩陣和平移向量。本文使用Autoware自動駕駛平臺的標定工具[9－10]來標定16線激光雷達和相機；激光和IMU的標定本文使用了文獻[11]的方法；相機和IMU間的標定本文使用了Kalibr標定工具[12]。

為了能夠利用語義信息進行導航和實時執(zhí)行任務,需要保證語義信息提取和建圖的實時性。本文在設計結構上采取了實時方案,如圖2所示。程序分為傳感器采集模塊、定位定姿模塊和激光雷達－相機融合模塊,保障了各個模塊工作的獨立性和運行效率。將不同的傳感器用不同的線程采集數據并放入全局緩沖區(qū),從緩沖區(qū)中按時間順序將同一時刻的GNSS、IMU、里程計數據輸入實時組合導航線程中進行融合定位解算,并將融合定位結果和緩沖區(qū)中的點云、圖像輸入到激光雷達－相機融合線程中進行融合。

在融合模塊中,為了在結構上減少算法耗時,將點云處理和圖像處理用獨立的兩個線程進行,如圖3所示。雖然激光雷達和相機的采集頻率同為10 Hz,但其開始采樣的時刻卻不能保證是同一時刻,故需要將點云對圖像進行配準,使點云和圖像為同一時刻的數據才能進行融合。而點云在采集過程中由于載體運動導致畸變,需要對點云進行矯正。在圖像數據到來后,將運動補償后的點云從其采樣時刻轉換到圖像數據采集時刻,并對投影到圖像部分的點云進行快速地面分割和點云聚類；同時,對圖像數據進行畸變校正和目標檢測；最后再將這兩個線程的輸出結果融合,輸出帶語義信息的點云簇。

本文的設計結構可以避免某些模塊因突發(fā)情況計算耗時過長而導致傳感器數據丟失或數據處理失敗,保證多傳感器融合的實時性和魯棒性。

圖2 系統(tǒng)算法結構圖

圖3 點云－圖像融合算法結構圖

2 關鍵技術

2.1 點云－圖像配準

受機器人載體運動特性影響,在進行點云、圖像融合時會面臨兩個問題:①激光雷達采樣間隔內掃描得到的一幀點云存在一定程度的運動畸變[13]；②點云和圖像的采樣時間不一致會導致點云不能準確地投影到圖像上。為了解決上述問題,本文提出了一種在不同采集時刻下基于組合導航輔助的點云－圖像精確配準方法。

組合導航算法輸出位置為IMU在當地導航坐標系(N系)下的緯度、經度和高程,先將其轉換成以起始點為原點的站心系(L系)位置和姿態(tài),表示為:

更新點云Plidar,tn轉換到tk時刻的激光雷達處

為了保持符號的簡潔,在不引起歧義的情況下,以后不區(qū)別齊次坐標與普通坐標符號,默認使用的是符合運算法則的那一種。則式(3)可以簡寫為

接收一幀點云數據是一個持續(xù)的過程,故在本文中將先接收到的點云轉換到當前點云幀起始時刻tstart,待全部數據到來之后再將其轉換到當前點云幀結束時刻tend,如圖4所示。

圖4 點云運動補償原理

同時,由于機器人載體運動的特性以及點云、圖像之間采樣時刻的不一致,會導致點云投影到圖像出現一定的偏差,特別是在機器人速度較快時,影響尤為明顯。譬如機器人以90°/s的角速度急轉彎時,點云－圖像配準角度誤差最大可達9°。令點云、圖像采樣時刻分別為tl和tc,將組合導航位姿線性內插得到對應時刻的和,則

再將點云投影到圖像上,即可完成點云－圖像的精確配準。

式中:u、v為點云投影在圖像上的像素坐標,K為相機的內參數矩陣,由相機內參標定得到。

2.2 目標語義點云提取

目標語義點云提取是語義建圖的難點,本文提出的語義點云思路如圖5所示。

圖5 點云語義信息提取原理圖

首先,將原始圖像數據和原始點云數據根據上述方法進行精確配準得到像素坐標表示的點云,如圖6(a)。其次,使用RANSAC算法對配準好的點云進行快速的地面分割[14],將非地面點云進行聚類,排除離散點干擾,如圖6(b)。再次,利用YOLOv3神經網絡[5]獲取圖像中目標檢測框的像素坐標,如圖6(c)。最后,篩選出檢測框內的帶有目標語義信息的點云,如圖6(d)。

圖6 點云語義提取步驟

利用VLP－16激光雷達的特性本文采用Igor Bogoslavskyi等人提出的快速分割算法[15],對除去地面的點云分割聚類,其原理如圖7所示。

圖7 點云快速分割算法原理

式中:d1為較長的激光束,d2為較短的激光束,α為兩條激光束間夾角,對于Velodyne16線激光雷達在其采樣頻率為10 Hz時,垂直方向夾角為2°,水平方向夾角為0.2°。若計算得到的β角度大于一定閾值,我們則認為其不是同一個類別的物體。

從目標檢測框中提取對應目標的語義點云存在以下三個難點:①目標檢測框一般比目標物體更大,所以不單單包含目標語義點云還會包含其背景的點云,如圖8(a)；②圖像目標檢測結果以及激光雷達和相機的外參標定存在一定程度上的誤差,導致檢測框不能完全包含目標物體的全部點云,如圖8(b)；③被檢測物體前方有部分遮擋,如圖8(c)。

圖8 難以提取語義點云的情況

圖8中距離較近的點云對應為圖8(a)、圖8(b)中行人和圖8(c)中電線桿,較遠的點云對應圖8(a)、圖8(b)中房屋和圖8(c)中行人,而我們需要的只是類別為行人的點云,必須排除其他物體點云的干擾。為了準確提取目標的語義點云即需要排除背景點云和前方障礙物點云的干擾,并把檢測框外的目標點云恢復,本文提出了一種從圖像目標檢測框中提取出對應語義信息點云簇的方法,分為背景點云去除、框內點云判斷和語義點云生長三個步驟。

第一步背景點云去除目的為去除聚類后背景點云對檢測框內點云提取的干擾,如圖8。將上述分割好且?guī)в胁煌悇e標簽和像素坐標的點云Pl中的每個點根據是否在某個特定語義的目標檢測框內外分類,令pli∈Pl,若pli在目標檢測框內則將其存入代表語義目標點云的Pi,否則存入代表背景點云的Po。對背景點云Po針對其類別對應的點云點數進行排序,將背景點云Po中點數最多的numback個類別的標簽存在Lo中,其中numback為

式中:numlabel為點云聚類得到的總類別數,numbox為當前幀點云對應的目標檢測框個數,s為一根據實際環(huán)境決定的固定常數,本文中取5。

第二步框內點云判斷是為了找到檢測框對應的語義點云聚類的類別。對語義目標點云Pi按點云點數對其類別進行排序,得到該目標檢測框中點數前二的標簽lmax1、lmax2且保證lmax1、lmax2?Lo。計算得到lmax1、lmax2對應的點云點數nmax1、nmax2和平均距離dmax1、dmax2,根據點數和距離判斷得到最優(yōu)的類別標簽lres。

第三步點云生長是為了恢復出檢測框外的語義點云,從而完整地提取該語義信息對應的語義點云。從Pl中提取出所有標簽為lres的點云,即可完整的恢復出該目標檢測框語義信息對應的點云Pres。其偽代碼如圖9所示。

圖9 目標檢測框提取點云偽代碼

3 實驗

在校園環(huán)境基于機器人平臺采集數據,并在嵌入式處理器Jetson AGX Xavier上對本文方法進行測試。Xavier為8核ARM v8.2、64位的CPU,包含64個Tensor核的512核Volta GPU。以下測試結果如未特殊說明,則均為上述條件和場景下進行的。為了驗證語義點云提取的準確性和語義建圖的實時性和建圖效果,進行了如下實驗測試。

3.1 語義點云提取評估

在移動機器人平臺上進行測試評估,數據一語義目標均為靜止情況,數據二部分語義目標有遮擋情況,如圖8(c),數據三語義目標也是運動的情況。定義語義點云類別提取成功率為正確提取目標檢測框中點云類別次數占該目標檢測框出現總次數的百分比,表1給出了三組數據中語義點云類別提取成功率。

表1 目標檢測框中語義點云類別提取成功率

由此可見,在語義目標有遮擋情況下本文方法相較于論文[7]方法優(yōu)勢明顯,其他情況時也有所改善,總體來看行人和汽車語義點云類別提取成功率分別提高了11.30%和2.38%,均達到90%以上。

表2給出了在進行點云－圖像配準和目標語義點云提取改進前后的語義點云提取準確率對比。在圖像目標檢測的結果為正確的前提下,令從目標檢測框中正確提取得到的語義點云數為Numac,激光雷達掃到目標物體的真實點云數為Numobj,即單幀點云語義目標提取準確率為Numseg/Numobj的百分比。值得注意的是,若出現檢測框中點云被錯誤提取,則計Numac為0。

表2 語義點云提取準確率

表2可以看出,語義點云提取準確率在數據二和數據三得到較大改善,總體準確率提高了約20%,均達到85%以上。其準確率提高的原因為對檢測框內語義類別提取和點云－圖像配準更加準確,具體場景如圖10所示。

圖10 語義點云提取結果圖

圖10右邊表示的為當目標檢測結果不夠準確和有較多背景干擾的情況；左邊表示的為前方有障礙物干擾的情況。對于這幾種特殊情況,本文提出的方法能很好的提取目標語義點云。

3.2 實時性測試

分別在電腦和嵌入式處理器Xavier測試算法耗時,電腦配置為4核3.4G Hz i5－7500 CPU,NVIDIA GeForce GTX 1050的GPU。整體算法耗時如圖11所示。

圖11 算法耗時測試

各個模塊及總耗時如表3所示。其中組合導航部分包括GNSS、INS以及里程計的融合定位定姿,點云運動補償部分包含點云運動畸變補償和點云時刻投影到圖像時刻,點云預處理部分包含點云投影成圖像、點云地面分割以及聚類,融合建圖部分包含從目標檢測框中提取語義點云和構建語義地圖?？偤臅r為從接收到點云開始到生成語義點云地圖所花費的時間,其中包含了各個模塊處理時間、各個傳感器數據時序不一致導致的等待時間以及數據傳輸的時間。

表3 各個模塊用時 單位:ms

由于本文采用了圖2、圖3的算法設計模式,可以在進行圖像數據處理的同時進行點圖配準和地面分割、點云聚類等點云處理,對于電腦來說提速13.0%,對Xavier處理器來說提速14.1%。在圖11中有少數情況處理耗時超過100 ms,但由于模塊間緩沖區(qū)的設計可以保證數據不會丟失。該結果表明本文所設計的語義建圖算法無論在電腦還是嵌入式處理器Xavier上都能保證在激光雷達采集周期內計算完成。

3.3 移動機器人語義建圖

為了驗證語義建圖的性能,本文在開闊環(huán)境下進行實時語義建圖,如圖12所示。

圖12 語義建圖測試場地

圖13和圖14表示改進前后語義建圖的整體效果對比,圖15、圖16分別代表圖13、圖14中處的具體場景對比效果,上方為改進前的效果,下方為改進后的效果,箭頭指向為改進效果比較明顯的部分。處表示的為地面點云,、處表示的是行人,處表示的是汽車。

圖13 改進前移動機器人平臺實時語義建圖效果

圖14 改進后移動機器人平臺實時語義建圖效果

圖15 改進前后處語義建圖效果對比

圖16 改進前后處語義建圖效果對比

對比圖13～圖16可以發(fā)現在處改進后對行人的語義點云提取準確率明顯提高。結果表明,在進行點云－圖像配準和目標語義點云提取改進后,語義點云提取的準確率得到提高,構建的語義地圖更加準確。

4 結論

本文提出了一種成本低、魯棒性強、實時性好的激光雷達－相機實時語義建圖的方案。該方案通過組合導航的位姿信息將16線稀疏激光點云和相機圖像配準融合,準確地從目標檢測框中提取語義點云,并在移動機器人平臺上實時構建點云地圖。實驗結果表明,本文提出的方法能很好的融合稀疏點云和相機圖像,在移動載體上對復雜環(huán)境下行人和汽車的點云類別提取成功率分別提高了12.51%和2.38%,均達到90%以上；改進后點云提取準確率提高了約20%,均達到85%以上,使系統(tǒng)語義點云提取的魯棒性得到提高。在電腦和嵌入式處理器Xavier上耗時分別為55.5 ms和37.8 ms,均能達到10 Hz頻率的實時處理。

本文提出的方法可以應用于剔除建圖中動態(tài)物體,包括周圍的車輛、行人以及控制機器人建圖的操作人員等；也可以利用語義信息進行導航定位,或在場景中搜尋某個特定語義目標執(zhí)行特定任務。本文提出的低成本、魯棒性強、實時性好的語義建圖方案為機器人根據語義點云來進行導航定位和實施多種智能任務奠定了基礎。