基于球形標靶的激光掃描儀位姿測量技術

萬海霞 馬慧卿 王 真

中交水運規劃設計院有限公司

1 引言

3D激光掃描儀通過向外部發射和接收激光束來獲得物體表面的點云數據，進行物體的3D重構，在高精度3D地形構建、三維重建及變形監測等領域得到了十分廣泛的應用[1]。在3D重建領域通常采用多個標定球完成不同坐標系下點云數據的匹配，實現坐標系之間的相互轉換。為了得到掃描對象的完整點云數據，通常需要在多個不同角度掃描測量目標，由于在不同的位置掃描中三維激光位置發生變化，需要根據位置傳感器反饋結果來估算不同測量站的相對位姿，將得到的點云數據轉換到全局坐標系下[2-3]。由于標定球每個方向上的球心點是固定的，并且是唯一的，所以在匹配時通常采用3個以上標定球來進行[4]，如果要掃描的目標周圍有干擾源以及掃描的目標點云本身含有密集噪聲，會對最終的匹配精度有很大的影響。

目前點云數據匹配算法的研究很多，但標定目標在掃描后包含誤差的情況常被忽略[5]。最小二乘法是最常用的球心擬合算法，但是沒有充分考慮掃描時目標本身的點云中含有粗糙度和噪聲的情況，采用最小二乘解算時噪聲對球心的計算結果有很大的影響。因此本研究在對球心計算前首先對球面周圍的離散噪聲進行濾波，基于去除噪聲后的點云數據對球心坐標進行擬合，并將最終計算結果作為匹配的關聯點。現場測試證明，本系統在保證作業安全的同時，還具有控制精度高、穩定性強、實時性好的特點。

2 基本原理

根據點云配準的基本原理，想要求解同一個斗輪堆取料機在不同時刻的位姿變換，至少需要3個不共線的公共點。實際上掃描儀很難在料場中獲取不同時刻下的公共點。近年來一些文獻采用單個或者多個球形標靶對激光或相機進行內參和外參的標定工作[7-8]，本研究采用4個標準球形標靶進行標定(見圖1)。

圖1 標定球現場布置示意圖

2.1 坐標系建立

為了求解坐標系之間的轉換關系，需要根據堆取料機的的運動機構構建各個機構的坐標系，最終能使數據還原到全局坐標系下。子坐標系包括：激光掃描儀坐標系Claser；俯仰機構坐標系Cpitch；回轉機構坐標系Cyaw和軌道坐標系Crail(見圖2)。坐標系之間的變換關系是由旋轉矩陣和平移矩陣兩部分組成，這兩部分為主要的標定參數，設備在運動過程中的位姿數據利用設備自身的傳感器系統獲得，包括行進距離、回轉角度、俯仰角度，將位置反饋值和掃描數據組合起來進行標定。

圖2 各坐標系的建立

2.2 建立球心擬合模型

掃描儀在不同位姿(不同回轉角度、俯仰角度)下掃描球形標靶的表面，若標定球附近存在其他物體的干擾，或掃描標定球表面時點云含有大量噪聲，則會對擬合出來的球心坐標產生較大的偏差，并對點云匹配的精度產生較大的影響。所以需要對激光掃描儀獲取的點云數據去除冗余點和噪聲點，同時已知標定球的制造半徑，現場各個球之間的相對距離、球心到水平面的距離以及現場標定球大致的擺放位置等為約束條件，從掃描到的點云數據中分離出標定球的點云數據，分離結果見圖3。

圖3 球體表面特征點提取

三維球面球心的求取，至少需要采集球面上4個不同點的掃描數據方可計算出球心坐標。設擬合的球心坐標為(x0，y0，z0)，半徑為r，采用最小二乘法對分離出的標定球表面點云數據進行擬合，此方法能以更高的精度對球心進行擬合，球的方程如下。

(xi-x0)2+(yi-y0)2+(zi-z0)2=r2

(1)

式中，xi、yi、zi為采集的球體表面坐標值；x0、y0、z0為球心坐標的最小二乘估值；r為球半徑。式(1)是一個關于a、b、c的非線性方程組。已知球表面任意2點(xi,yi,zi)和(xi+1,yi+1,zi+1)滿足：

(xi-x0)2+(yi-y0)2+(zi-z0)2

=(xi+1-x0)2+(yi+1-y0)2+(zi+1-z0)2

(2)

對式(2)展開可得：

基于交通載荷作用下頂管頂力計算優化分析………………………………………王曉凡，張玉廷，白延杰（6.37）

(3)

v=Ax-L

(4)

基于最小二乘原理可以擬合出球心坐標X(x0,y0,z0)的估值：

X=(AT×A)-1×AT×L

(5)

現場標定時使堆取料機在多個位置下采集標定球的表面數據，根據分離出的標定球表面數據求解各標定球的球心坐標。現場在40組不同位置下求解出各標定球的球心坐標，表1中列出了部分數據。

表1 球心坐標計算

2.3 建立標定參數模型

采用Rodrigues羅德里格斯公式將旋轉矩陣R表示為R(k,θ)，k為旋轉軸，θ為相應部件的運動角度值，式(6)表示球形靶標球心坐標值從激光掃描坐標系到局部料場坐標系下的坐標值，其中Cij表示局部料場坐標系下球形靶標的球心坐標值，t1、t2、t3為矩陣的平移量，k1為回轉軸，k2為俯仰軸，k3為激光與設備安裝位置的固有偏差,μi,j為設備在第i個位置下掃描儀坐標系下第j個球的坐標值[9-10]。

Cij=

(6)

對式(6)展開可得：

Ci,j=R1,j[R2,j(R3,jμi,j+t3)+t2]+t1

(7)

假定標定中4個標準球的測量是等精度的，全局標定最終表示為求解式(8)所示的優化問題，通過高斯牛頓(Gauss-Newton)、列文伯格-馬夸爾特(LM)算法可以對相關參數進行優化求解。

Fi,j(θ1,θ2,θ3,k1,k2,k3,t1,t2,t3)

(8)

計算Fi,j關于k1、k2、k3的導數為：

(9)

計算Fi,j關于t1、t2、t3的公式為：

(10)

構造矩陣式為：

(11)

并有

bi,j=-Fi,j

(12)

構建X的估計表達式：

X=(AT×A)-1×AT×T

(13)

全局標定包含3組剛體變換參數的求解，因此需至少3個不同的大機位姿才能完成標定，且姿態的變化不可在同一平面內，標定球全局坐標系下的坐標見表2。

表2 標定球全局坐標值

2.4 測試結果

對40組描儀坐標系下標定球球心數據與姿態數據，結合全站儀所測量的全局坐標系下目標球形靶標的坐標值，采用LM算法進行參數優化求解其結果見表3。利用激光掃描儀在不同的位置對同一料堆進行掃描，對標定參數和設備當前姿態數據進行數據配準，在最遠距離100 m處，點云的變換后的誤差可以控制在100 mm以內。

表3 標定結果

3 結語

針對三維激光掃描儀姿態測量模型參數估算，采用基于球形標靶的姿態模型參數估算方法，將球形靶標的已知特性作為約束條件，對球形標靶表面點云數據進行分離，篩選出有用的點進行球心求解；通過最小二乘對篩選后的點云進行擬合，發現半徑解算的結果基本與標定球的制造參數一致，偏差控制在±10 mm。同時以此球心解算結果作為設備在不同位姿下的公共點，并且聯合設備位置數據采用LM優化算法最小化目標函數，完成全局標定參數估算，對解算的結果進行反推和多次的現場測量，在多個位姿下將目標料堆三維數據變換到料場坐標系下誤差可控制在100 mm以內，該標定結果能滿足堆取料機無人化控制要求。