基于球形標(biāo)靶的激光掃描儀位姿測(cè)量技術(shù)

萬(wàn)海霞 馬慧卿 王 真

中交水運(yùn)規(guī)劃設(shè)計(jì)院有限公司

1 引言

3D激光掃描儀通過(guò)向外部發(fā)射和接收激光束來(lái)獲得物體表面的點(diǎn)云數(shù)據(jù)，進(jìn)行物體的3D重構(gòu)，在高精度3D地形構(gòu)建、三維重建及變形監(jiān)測(cè)等領(lǐng)域得到了十分廣泛的應(yīng)用[1]。在3D重建領(lǐng)域通常采用多個(gè)標(biāo)定球完成不同坐標(biāo)系下點(diǎn)云數(shù)據(jù)的匹配，實(shí)現(xiàn)坐標(biāo)系之間的相互轉(zhuǎn)換。為了得到掃描對(duì)象的完整點(diǎn)云數(shù)據(jù)，通常需要在多個(gè)不同角度掃描測(cè)量目標(biāo)，由于在不同的位置掃描中三維激光位置發(fā)生變化，需要根據(jù)位置傳感器反饋結(jié)果來(lái)估算不同測(cè)量站的相對(duì)位姿，將得到的點(diǎn)云數(shù)據(jù)轉(zhuǎn)換到全局坐標(biāo)系下[2-3]。由于標(biāo)定球每個(gè)方向上的球心點(diǎn)是固定的，并且是唯一的，所以在匹配時(shí)通常采用3個(gè)以上標(biāo)定球來(lái)進(jìn)行[4]，如果要掃描的目標(biāo)周?chē)懈蓴_源以及掃描的目標(biāo)點(diǎn)云本身含有密集噪聲，會(huì)對(duì)最終的匹配精度有很大的影響。

目前點(diǎn)云數(shù)據(jù)匹配算法的研究很多，但標(biāo)定目標(biāo)在掃描后包含誤差的情況常被忽略[5]。最小二乘法是最常用的球心擬合算法，但是沒(méi)有充分考慮掃描時(shí)目標(biāo)本身的點(diǎn)云中含有粗糙度和噪聲的情況，采用最小二乘解算時(shí)噪聲對(duì)球心的計(jì)算結(jié)果有很大的影響。因此本研究在對(duì)球心計(jì)算前首先對(duì)球面周?chē)碾x散噪聲進(jìn)行濾波，基于去除噪聲后的點(diǎn)云數(shù)據(jù)對(duì)球心坐標(biāo)進(jìn)行擬合，并將最終計(jì)算結(jié)果作為匹配的關(guān)聯(lián)點(diǎn)?，F(xiàn)場(chǎng)測(cè)試證明，本系統(tǒng)在保證作業(yè)安全的同時(shí)，還具有控制精度高、穩(wěn)定性強(qiáng)、實(shí)時(shí)性好的特點(diǎn)。

2 基本原理

根據(jù)點(diǎn)云配準(zhǔn)的基本原理，想要求解同一個(gè)斗輪堆取料機(jī)在不同時(shí)刻的位姿變換，至少需要3個(gè)不共線(xiàn)的公共點(diǎn)。實(shí)際上掃描儀很難在料場(chǎng)中獲取不同時(shí)刻下的公共點(diǎn)。近年來(lái)一些文獻(xiàn)采用單個(gè)或者多個(gè)球形標(biāo)靶對(duì)激光或相機(jī)進(jìn)行內(nèi)參和外參的標(biāo)定工作[7-8]，本研究采用4個(gè)標(biāo)準(zhǔn)球形標(biāo)靶進(jìn)行標(biāo)定(見(jiàn)圖1)。

圖1 標(biāo)定球現(xiàn)場(chǎng)布置示意圖

2.1 坐標(biāo)系建立

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

圖2 各坐標(biāo)系的建立

2.2 建立球心擬合模型

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

圖3 球體表面特征點(diǎn)提取

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

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

(1)

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

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

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

(2)

對(duì)式(2)展開(kāi)可得：

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

(3)

v=Ax-L

(4)

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

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

(5)

現(xiàn)場(chǎng)標(biāo)定時(shí)使堆取料機(jī)在多個(gè)位置下采集標(biāo)定球的表面數(shù)據(jù)，根據(jù)分離出的標(biāo)定球表面數(shù)據(jù)求解各標(biāo)定球的球心坐標(biāo)?，F(xiàn)場(chǎng)在40組不同位置下求解出各標(biāo)定球的球心坐標(biāo)，表1中列出了部分?jǐn)?shù)據(jù)。

表1 球心坐標(biāo)計(jì)算

2.3 建立標(biāo)定參數(shù)模型

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

Cij=

(6)

對(duì)式(6)展開(kāi)可得：

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

(7)

假定標(biāo)定中4個(gè)標(biāo)準(zhǔn)球的測(cè)量是等精度的，全局標(biāo)定最終表示為求解式(8)所示的優(yōu)化問(wèn)題，通過(guò)高斯牛頓(Gauss-Newton)、列文伯格-馬夸爾特(LM)算法可以對(duì)相關(guān)參數(shù)進(jìn)行優(yōu)化求解。

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

(8)

計(jì)算Fi,j關(guān)于k1、k2、k3的導(dǎo)數(shù)為：

(9)

計(jì)算Fi,j關(guān)于t1、t2、t3的公式為：

(10)

構(gòu)造矩陣式為：

(11)

并有

bi,j=-Fi,j

(12)

構(gòu)建X的估計(jì)表達(dá)式：

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

(13)

全局標(biāo)定包含3組剛體變換參數(shù)的求解，因此需至少3個(gè)不同的大機(jī)位姿才能完成標(biāo)定，且姿態(tài)的變化不可在同一平面內(nèi)，標(biāo)定球全局坐標(biāo)系下的坐標(biāo)見(jiàn)表2。

表2 標(biāo)定球全局坐標(biāo)值

2.4 測(cè)試結(jié)果

對(duì)40組描儀坐標(biāo)系下標(biāo)定球球心數(shù)據(jù)與姿態(tài)數(shù)據(jù)，結(jié)合全站儀所測(cè)量的全局坐標(biāo)系下目標(biāo)球形靶標(biāo)的坐標(biāo)值，采用LM算法進(jìn)行參數(shù)優(yōu)化求解其結(jié)果見(jiàn)表3。利用激光掃描儀在不同的位置對(duì)同一料堆進(jìn)行掃描，對(duì)標(biāo)定參數(shù)和設(shè)備當(dāng)前姿態(tài)數(shù)據(jù)進(jìn)行數(shù)據(jù)配準(zhǔn)，在最遠(yuǎn)距離100 m處，點(diǎn)云的變換后的誤差可以控制在100 mm以?xún)?nèi)。

表3 標(biāo)定結(jié)果

3 結(jié)語(yǔ)

針對(duì)三維激光掃描儀姿態(tài)測(cè)量模型參數(shù)估算，采用基于球形標(biāo)靶的姿態(tài)模型參數(shù)估算方法，將球形靶標(biāo)的已知特性作為約束條件，對(duì)球形標(biāo)靶表面點(diǎn)云數(shù)據(jù)進(jìn)行分離，篩選出有用的點(diǎn)進(jìn)行球心求解；通過(guò)最小二乘對(duì)篩選后的點(diǎn)云進(jìn)行擬合，發(fā)現(xiàn)半徑解算的結(jié)果基本與標(biāo)定球的制造參數(shù)一致，偏差控制在±10 mm。同時(shí)以此球心解算結(jié)果作為設(shè)備在不同位姿下的公共點(diǎn)，并且聯(lián)合設(shè)備位置數(shù)據(jù)采用LM優(yōu)化算法最小化目標(biāo)函數(shù)，完成全局標(biāo)定參數(shù)估算，對(duì)解算的結(jié)果進(jìn)行反推和多次的現(xiàn)場(chǎng)測(cè)量，在多個(gè)位姿下將目標(biāo)料堆三維數(shù)據(jù)變換到料場(chǎng)坐標(biāo)系下誤差可控制在100 mm以?xún)?nèi)，該標(biāo)定結(jié)果能滿(mǎn)足堆取料機(jī)無(wú)人化控制要求。