二維激光雷達的實時三維重建方法

陳恬宇,孫立雙,車德福

(1. 沈陽建筑大學,遼寧 沈陽 110168; 2. 東北大學,遼寧 沈陽 110819)

中國是斗輪堆取料機制造與應用大國,但目前各大煤炭港口、料場堆取料作業均為人工控制,普遍存在智能化程度偏低的問題,實現智慧料場無人化堆取料的關鍵是對料場料堆三維模型的實時顯示。獲取真實物體的三維模型,稱為三維重建[1]。通過圖片信息進行二維或三維重建是傳統的三維重建技術[2]。激光雷達測距技術的發展為三維空間數據的獲取提供了一種全新的技術方法,它可用于精準快速獲取物體二維或三維數據,同時實現目標物體或場景的三維重建[3]。

三維激光掃描儀是激光掃描建模技術的主要儀器,盡管通過三維激光掃描儀能夠精準快速地獲取三維點云信息,但由于三維激光掃描儀價格昂貴,并不能廣泛適用于實際應用,且對于精度要求不高的場景,性能過剩[4]。

在二維激光雷達的三維點云獲取的研究中,文獻[5—7]通過伺服電機驅動二維掃描儀的俯仰旋轉完成了三維掃描,已被廣泛應用于自動化機器人探測定位防撞系統和復雜地形檢測。國內外針對二維激光掃描儀的三維重建試驗及算法研究主要集中于獲取區域內三維點云數據[8-14],實現三維掃描功能,但在二維激光雷達的三維點云配準尚未有研究應用。

在點云的三維重建研究中,文獻[15]提出最近點迭代(iterative closest point,ICP)數據配準方法,即將當前數據與前一幀數據通過最近點迭代匹配,得到相對位置變化量。文獻[16]分別針對分散性、數據量大、隨機性、復雜度高提出了ICP算法實際使用問題。文獻[17]提出了基于單位四元數的ICP改進算法,引入四元數求解變換矩陣,提高配準時間與精度。文獻[18]提出了引入四元數作為空間旋轉變換的描述算子,實現了相鄰兩站點的高精度三維點云配準。

本文源于某單位基于多源時空數據獲取的臂式斗輪堆取料機無人值守智能平臺研發及應用項目,提出在二維激光雷達中基于空間坐標變換算法的三維點云數據算法,以期實現二維激光掃描儀的三維料堆掃描。同時,針對兩臺相對位置不變的二維激光掃描儀,提出在點云配準中基于四元數法的多組旋轉平移矩陣求均值解算法,對初步配準后的三維點云數據,基于四元數算法進一步優化配準,最后添加多個控制點,以此獲取并驗證點云配準精度。

1 基于空間坐標變換算法的三維點云數據生成算法

1.1 構建三維點云獲取流程

試驗器材采用兩臺SICK LMS511-10100 PRO二維激光雷達、兩臺飛越FY-SPT02中載云臺,模擬斗輪堆取料機模型,搭建點云采集設備平臺實物。如圖1所示。

圖1 點云采集試驗平臺實物

由兩臺二維激光雷達獲取二維平面數據,與云臺俯仰數據構成料機靜態行走下的三維點云數據;由料機大臂俯仰數據、料機水平回轉數據、基站和移動站差分定位的料機位置數據構成料機動態行走下的三維點云數據,如圖2所示。通過旋轉矩陣與歐拉角的空間坐標公式[19]變換獲得局部點云數據,即單側二維激光雷達下的三維點云數據,其流程如圖3所示。

圖2 三維點云獲取模型(側視圖)

1.2 基于空間坐標變換算法的三維點云數據生成算法步驟

以二維激光雷達中心、云臺中心分別構建兩三維坐標系。二維點云數據通過云臺的俯仰數據轉換為三維點云,由空間變換公式求得局部點云數據,即二維激光雷達坐標系下三維點云數據。

(1)二維激光雷達為原點構建坐標系,掃描面獲取二維點云數據(x,y)。

(2)以每幀掃描面二維點云數據構建n×4矩陣A,設點云坐標為(x1,y1),(x2,y2),…,(xn,yn),z軸為0。

(1)

(3)以云臺為旋轉中心為y軸,右手系繞y軸旋轉坐標變換得4×4變換矩陣B。

(2)

(4)以云臺中心點為原點建立云臺坐標系(Xc,Yc,Zc),以二維激光雷達中心為原點建立雷達坐標系(Xw,Yw,Zw),設云臺中心點與二維激光雷達中心點相對位置關系為Δx、Δy、Δz,歐拉角(roll、pitch、yaw),以此構建4×4轉換矩陣C。

雷達坐標系轉換到云臺坐標系的過程分別由x、y、z軸旋轉,得出相對歐拉角,即從雷達坐標系到云臺坐標系的3個維度的旋轉矩陣為

(3)

(4)

(5)

因此,由雷達坐標系到云臺坐標系間的旋轉矩陣Mc為

Mc=RxRyRz

(6)

構建云臺坐標系(Xc,Yc,Zc)至雷達坐標系(Xw,Yw,Zw)轉換矩陣為

(7)

由矩陣Mc求得云臺坐標系至雷達坐標系轉換矩陣C為

(8)

(5)構建云臺坐標系下坐標變換矩陣D,即二維激光雷達坐標系下三維點云數據,為

D=ABC

(9)

1.3 基于空間坐標變換算法的三維數據生成算法試驗結果

為了規范點云配準精度,精準測量點云誤差,三維重建對象選用3個規則物體,測定試驗對象尺寸:1號長方體(18.89 cm×12.43 cm×4.38 cm),2號長方體(12.21 cm×7.72 cm×2.26 cm),3號四棱柱(取側面梯形面上底×下底×高為20.23 cm×24.63 cm×10.40 cm)。由空間坐標變換得到兩側三維點云數據,點云模型與實物模型一一對應。如圖4所示。

圖4 點云采集對象及其點云數據

2 基于四元數法的點云配準旋轉平移矩陣優化算法

點云配準算法是在一個旋轉平移矩陣初值已知的情形下(初值大概正確),進行運算得出比較準確的旋轉平移矩陣[20-22]。

由于兩臺二維激光雷達的相對位置不變,通過點云配準算法,多次迭代計算,得到兩組二維激光雷達下的三維點云數據的多組旋轉平移矩陣。對多組旋轉平移矩陣組求均值即可得到相對精度最高的旋轉平移矩陣P。不僅保證了點云配準精度,還簡化了點云配準迭代算法步驟,提高了配準速率。

2.1 多種旋轉平移矩陣均值算法對比

對點云配準所得多組旋轉平移矩陣求均值主要包括以下幾種算法。

(1)利用歐拉角表示旋轉,由歐拉角的3個參數相加后平均。

(2)由四元數表示旋轉,通過四元數運算[23]求得旋轉平移矩陣。其轉換原理即是三維空間x、y和z軸的旋轉,可由一個四維向量[θxyz]表示[24]。

(3)將旋轉矩陣映射至單位元的切空間求平均,再映射回旋轉矩陣。推導公式為

(10)

其中,xi為多組旋轉矩陣;exp為矩陣指數;ln為矩陣對數。

(4)利用平均的定義數值求解,在數學規定中,給定元素集{x0,x1,…,xn},存在空間中一點x,使該點至元素集中n個元素距離之和最小。因此推導公式為

(11)

式中,SO(3)為旋轉矩陣的特殊正交群,兩個SO(3)元素之間距離定義為

(12)

通過以上算法求解得到旋轉矩陣均值,對平移矩陣直接由平均定義相加取均值即可。

為了驗證以上4種算法的精確度,對單位三維旋轉矩陣繞某一定點旋轉,在給定范圍內隨機取樣,樣本數量為10組,3種線色矢量分別代表x、y、z軸的旋轉,如圖5所示。

圖5 三維旋轉矩陣隨機取樣矢量分布

圖6 旋轉平移矩陣均值算法對比試驗

2.2 點云配準旋轉平移矩陣優化試驗步驟

場景左側、右側分別為二維雷達激光雷達A、B,兩者相對位置不變,獲取左右兩側三維點云數據后,對其點云配準求得旋轉平移矩陣多次取樣。分別取多次點云A′與點云B′,求得旋轉平移矩陣,多次迭代后,求得精度最高的旋轉平移矩陣P。

(1)分別由激光雷達A與B獲取多組點云數據。

(2)對多組點云數據α分別配準,由點云B′配準至點云A′,得多組旋轉矩陣。

(3)基于四元數算法求相對精度最高旋轉平移矩陣P。

(4)由多幀點云數據應用旋轉平移矩陣P重新配準,得配準優化后多組點云數據β。

(5)對照試驗結果。

2.3 點云配準旋轉平移矩陣優化算法步驟

(1)四元數公式為

q=q0+q1i+q2j+q3k=[sv]

(13)

式中,q1、q2、q3均為實數,s=q0;v=[q1q2q3];i2=j2=k2=-1。i,j,k是一種旋轉狀態,-i,-j,-k分別代表i,j,k的反向旋轉。

(2)由Rodrigues公式通過四元數求旋轉矩陣式Rf。

(3)已知點云配準后旋轉平移矩陣組Rn,轉換為四元數組Fn。

當q0≠0,1+r11+r22+r33>0時

(14)

(15)

(16)

(17)

當q0趨近于0,1+r11+r22+r33趨近于-1時,rmax=r11、rmax=r22、rmax=r333種情況由四元數算法求解。

(4)由四元數加法公式求得四元數組Fn之和,公式為

Fn=s1+s2+v1+v2+…

(18)

(5)求四元數組均值并轉換為旋轉平移矩陣即相對精度最高旋轉平移矩陣,并進行后續試驗。

2.4 試驗結果和分析

2.4.1 點云配準算法用時對比

點云配準通常采用的方法是最近點迭代(ICP)算法,但最近點迭代算法迭代次數過多且與運行時間過長。目前國內外引入基于KD-tree搜索法的ICP算法,顯著提高運行時間降低迭代次數。為驗證本文算法有效性,通過最近點迭代算法,基于KD-tree搜索法ICP算法,對同一三維點云集進行點云配準試驗[25-26],其收斂速度對比見表1。可以看出,基于四元數法的配準優化算法節省了旋轉平移矩陣迭代所耗時長,為點云的配準節省了大量時間。

表1 3種算法收斂速度對比

2.4.2 點云配準算法圖像對比

在點云配準圖像中,通過與傳統ICP配準影像對比,圖7(a)傳統配準算法中3個規則物體發生位置偏移,這是由于圖像特征匹配較差造成的,算法修正后,圖7(b)局部紋理更加清晰,物體姿態正確,點云配準正確率大幅度提高。

圖7 點云配準算法圖像對比

2.4.3 點云配準算法精度測量

選取3個規則物體為試驗對象,將三維點云數據導入Matlab,由pcread函數讀取點云數據。選取對應點坐標,由歐式距離公式計算點云對應點距離與實際測量距離對比,從而獲取點云精度,見表2。本文算法中點云精度為2 cm,滿足本文課題要求。在給定相同條件下,基于四元數法的點云配準旋轉平移矩陣優化算法具有良好的點云精度。

2.4.4 點云配準優化算法應用

采用臂式斗輪堆取料機模型模擬料機運轉,二維激光雷達實測小型料堆,并通過全站儀測定標定點坐標與二維激光雷達點云測定標定點坐標對比試驗,獲取本文算法點云配準精度,如圖8所示。

全站儀是目前應用最廣泛的測量設備,單點測量精度高于SICK LMS511-10100 PRO,能夠快速準確測得全局坐標系。本文采用圓形反射片中心孔作為標定點,對反射片中心點坐標通過全站儀與二維激光雷達分別測量,如圖9所示。

圖9 全站儀與激光雷達工作原理示意圖

(1)選取4個不同直線,不同平面分布的標定點分別放置圓形反射片。

(2)使用全站儀進行坐標測量,以自身的坐標系構建全局坐標系;使用二維激光雷達掃描得到點云數據,進行坐標測量時使用自身局部坐標系,分別測得坐標數據。

(3)將全站儀測得坐標數據與點云測得坐標數據導入同一坐標系中,計算反射片之間相互夾角關系與距離關系;根據在空間中的位置關系確定掃描儀的反射片坐標與全站儀的反射片坐標之間的對應關系,獲得兩個坐標系之間的轉換矩陣,從而將全部掃描數據轉換至全局坐標系,求得兩坐標差,見表3。

表3 各標靶全站儀坐標與點云坐標誤差統計 m

由以上試驗可得,本文算法下二維激光雷達測得點云配準數據與全站儀數據誤差控制在2 cm以內。目前市面常用三維激光掃描儀(如Velodyne HDL-32E等)20 m內測距檢測精度均在為±2 cm,完全滿足試驗項目要求。

3 結 語

本文采用C#語言,結合Visual Studio 2019,Matlab等開發工具進行系統開發,建立了基于二維激光雷達的三維點云重建系統,實現了以下主要功能:

(1)通過基于空間坐標變換算法的三維點云數據生成算法實現了由二維激光掃描儀對三維點云的實時獲取。在滿足規定項目使用要求下,極大降低了生產成本。

(2)通過基于四元數法的點云配準旋轉平移矩陣算法,將二維激光掃描儀的三維點云精度控制在2 cm內,且當前幀配準速度達0.36 s,算法滿足當前項目工程上對于三維點云的獲取及三維點云重建精度與速率的要求,在實際工程應用中具有廣泛的前景。

在實際研究過程中,本文采用兩臺相對位置相同的二維激光雷達,進行水平安裝,因而不存在傾斜誤差。但在實際應用中,由于激光雷達采集目的不同導致兩臺激光雷達相對位置發生更替時,該算法可能不再適用。