點(diǎn)云的剛體運(yùn)動(dòng)參數(shù)估計(jì)方法的比較

袁志聰，魯鐵定，鄧小淵

(1.東華理工大學(xué) 測(cè)繪工程學(xué)院，江西 南昌 330013；2.流域生態(tài)與地理環(huán)境監(jiān)測(cè)國(guó)家測(cè)繪地理信息局重點(diǎn)實(shí)驗(yàn)室，江西 南昌 330013)

近年來(lái)，隨著三維激光掃描技術(shù)的不斷發(fā)展，點(diǎn)云獲取的速度越來(lái)越快，如何高效地處理海量點(diǎn)云數(shù)據(jù)已成為科研領(lǐng)域的研究熱點(diǎn)。點(diǎn)云配準(zhǔn)是海量點(diǎn)云數(shù)據(jù)處理中的核心問(wèn)題；點(diǎn)云配準(zhǔn)就是要使所有來(lái)自兩個(gè)點(diǎn)云集合中的同名點(diǎn)對(duì)滿足同一剛體變換[1-2]，如何快速準(zhǔn)確地求解剛體變換參數(shù)成了點(diǎn)云配準(zhǔn)中的核心問(wèn)題。

傳統(tǒng)的坐標(biāo)轉(zhuǎn)換采用布爾沙七參數(shù)模型[3],由于布爾沙模型的旋轉(zhuǎn)矩陣是由坐標(biāo)軸之間的夾角表示的，其表示方式繁瑣，計(jì)算復(fù)雜，同時(shí)存在象限判斷難等問(wèn)題，文獻(xiàn)[4]提出基于羅德里格矩陣的求解方法，羅德里格矩陣由3個(gè)獨(dú)立參數(shù)組成旋轉(zhuǎn)矩陣，結(jié)合最小二乘法具有精度高，速度快等優(yōu)點(diǎn)。常用的剛體運(yùn)動(dòng)估計(jì)方法還有奇異值分解法；正交分解法[5]；四元數(shù)法[6-7]等。宋林霞[8]通過(guò)奇異值分解法求解旋轉(zhuǎn)矩陣，并提出一條相關(guān)引理，將距離優(yōu)化問(wèn)題轉(zhuǎn)為估計(jì)的最優(yōu)的旋轉(zhuǎn)矩陣R使得對(duì)于任意給定的方陣A都有tr(ATR)最大，對(duì)算法進(jìn)行優(yōu)化。邱志強(qiáng)[9]等提到基于三維特征點(diǎn)以及二維特征點(diǎn)的“8點(diǎn)算法”的剛體運(yùn)動(dòng)估計(jì)方法。官云蘭[10-11]應(yīng)用單位四元數(shù)進(jìn)行剛體運(yùn)動(dòng)估計(jì)，四元數(shù)法具有計(jì)算速度快、穩(wěn)定性好等優(yōu)點(diǎn)，代俁西[12]在基于對(duì)偶四元數(shù)的三維運(yùn)動(dòng)估計(jì)中介紹對(duì)偶四元數(shù)的估計(jì)方法。對(duì)偶四元數(shù)是對(duì)偶數(shù)和四元數(shù)的結(jié)合，具備其他方法所沒(méi)有的優(yōu)點(diǎn)，對(duì)偶四元數(shù)能同時(shí)求解旋轉(zhuǎn)矩陣和平移參數(shù)，計(jì)算過(guò)程中不會(huì)引入旋轉(zhuǎn)矩陣的誤差。

本文在現(xiàn)有基礎(chǔ)上，通過(guò)模擬數(shù)據(jù)和實(shí)例對(duì)奇異值分解法、正交分解法、單位四元數(shù)法和對(duì)偶四元數(shù)法4種估計(jì)方法作比較，分析4種方法的優(yōu)缺點(diǎn)，得出對(duì)偶四元數(shù)總體性能最優(yōu)。

1 剛體運(yùn)動(dòng)及參數(shù)求解方法

1.1 剛體運(yùn)動(dòng)

(1)

式中:R為3×3正交單位矩陣，稱為旋轉(zhuǎn)矩陣;T為3維列向量，稱為平移向量。

剛體運(yùn)動(dòng)估計(jì)就是在目標(biāo)函數(shù)F(R,T)=min的情況下求解最優(yōu)的旋轉(zhuǎn)矩陣R,平移向量T，使得兩點(diǎn)集盡可能的重合。目標(biāo)函數(shù)具體形式為

(2)

1.2 剛體運(yùn)動(dòng)參數(shù)求解方法

1.2.1 單位四元數(shù)法

四元數(shù)是數(shù)學(xué)家Hamilton創(chuàng)造的；可以認(rèn)為是一個(gè)包含四個(gè)元素的列向量，可表示為

q=w+xi+yj+zk.

(3)

式中:w為標(biāo)量，(x,y,z)為四元數(shù)對(duì)應(yīng)的向量。四元數(shù)亦可寫(xiě)為q=(w,v)。

定義4階矩陣

(4)

設(shè)矩陣S最大特征值對(duì)應(yīng)的單位特征向量為q=(q0,q1,q2,q3)，則對(duì)應(yīng)的旋轉(zhuǎn)矩陣為

(5)

平移向量為

(6)

1.2.2 奇異值分解法(SVD)

首先求出運(yùn)動(dòng)前后兩點(diǎn)集的重心化坐標(biāo)

Pi=Pi-Pio,

(7)

(8)

R=VUT.

(9)

平移向量為

(10)

1.2.3 正交分解法(OD)

設(shè)矩陣HHT的特征值分別為λ1，λ2，λ3，對(duì)應(yīng)的特征向量分別為u1,u2,u3。旋轉(zhuǎn)矩陣可表示為

(11)

平移向量為

(12)

1.2.4 對(duì)偶四元數(shù)法(DQD)

對(duì)偶四元數(shù)是由Clifford提出來(lái)的,是對(duì)偶數(shù)和四元數(shù)的組合，可以理解為元素為四元數(shù)的對(duì)偶數(shù),同樣可理解為元素為對(duì)偶數(shù)的四元數(shù),對(duì)偶四元數(shù)的優(yōu)越性體現(xiàn)在它繼承了二者的共性，從而能夠統(tǒng)一表示旋轉(zhuǎn)和平移。對(duì)偶四元數(shù)可表示為

ε(q00,q01,q02,q03)T.

(13)

式中q，q0均為四元數(shù)，分別稱做對(duì)偶四元數(shù)的實(shí)部和對(duì)偶部，ε為對(duì)偶算符。

對(duì)偶四元數(shù)的空間變換如圖1所示。n是單位向量，是空間旋轉(zhuǎn)和平移的變換方向。傳統(tǒng)的坐標(biāo)轉(zhuǎn)換是先通過(guò)坐標(biāo)軸進(jìn)行旋轉(zhuǎn)變換，再通過(guò)平移向量t完成平移變換，形成新的坐標(biāo)系，對(duì)偶四元數(shù)空間變換首先將原始坐標(biāo)系沿著n方向平移距離d，然后圍繞過(guò)點(diǎn)p的直線n旋轉(zhuǎn)角度θ，即可得到轉(zhuǎn)換后的坐標(biāo)系。

圖1 對(duì)偶四元數(shù)空間變化

設(shè)點(diǎn)p=(x,y,z)T剛體運(yùn)動(dòng)后變?yōu)閜′=(x′,y′,z′)T，可用對(duì)偶四元數(shù)表示剛體運(yùn)動(dòng)，即

(14)

(15)

(16)

2 實(shí)例驗(yàn)證

2.1 模擬數(shù)據(jù)與結(jié)果分析

利用Matlab隨機(jī)生成1 000個(gè)數(shù)據(jù)得到點(diǎn)云A，如圖2所示，給定初始旋轉(zhuǎn)矩陣和平移向量；對(duì)點(diǎn)云A進(jìn)行剛體運(yùn)動(dòng)變換，得到點(diǎn)云B，現(xiàn)通過(guò)4種方法分別求解剛體運(yùn)動(dòng)參數(shù)，把旋轉(zhuǎn)矩陣轉(zhuǎn)換為旋轉(zhuǎn)角，以求得的旋轉(zhuǎn)矩陣對(duì)應(yīng)的旋轉(zhuǎn)角與初始旋轉(zhuǎn)矩陣對(duì)應(yīng)的旋轉(zhuǎn)角之間的差作為旋轉(zhuǎn)角誤差；對(duì)應(yīng)的平移向量之間的差作為平移向量誤差。

已知兩點(diǎn)云的變換參數(shù)如表1所示。

表1 兩點(diǎn)云變換參數(shù)

圖2 兩點(diǎn)云相對(duì)位置

目標(biāo)點(diǎn)云不加噪聲情況下，4種方法求得的轉(zhuǎn)換參數(shù)如表2所示。

表2 不加噪聲的情況

續(xù)表2

下面將旋轉(zhuǎn)矩陣轉(zhuǎn)換為角度，計(jì)算不同噪聲水平下的參數(shù)誤差，算式為

(17)

(18)

加標(biāo)準(zhǔn)差為0.2的噪聲的參數(shù)誤差如表3所示。

表3 加標(biāo)準(zhǔn)差為0.2的噪聲計(jì)算2 000次的參數(shù)誤差

加標(biāo)準(zhǔn)差為0.4的噪聲的參數(shù)誤差如表4所示。

表4 加標(biāo)準(zhǔn)差為0.4的噪聲計(jì)算2 000次的參數(shù)誤差

加標(biāo)準(zhǔn)差為0.6的噪聲的參數(shù)誤差如表5所示。

表5 加標(biāo)準(zhǔn)差為0.6的噪聲計(jì)算2 000次的參數(shù)誤差

加標(biāo)準(zhǔn)差為0.8的噪聲的參數(shù)誤差如表6所示。

表6 加標(biāo)準(zhǔn)差為0.8的噪聲計(jì)算2 000次的參數(shù)誤差

加標(biāo)準(zhǔn)差為1.0的噪聲的參數(shù)誤差如表7所示。

表7 加標(biāo)準(zhǔn)差為1.0的噪聲計(jì)算2 000次的參數(shù)誤差

4種方法計(jì)算2 000次所耗時(shí)間如表8所示。

表8 計(jì)算2 000次耗時(shí)

噪聲標(biāo)準(zhǔn)差與角度誤差之間的關(guān)系如圖3所示。

圖3 噪聲標(biāo)準(zhǔn)差與角度誤差之間的關(guān)系

噪聲標(biāo)準(zhǔn)差與平移參數(shù)誤差之間的關(guān)系如圖4所示。

圖4 噪聲標(biāo)準(zhǔn)差與平移參數(shù)誤差之間的關(guān)系

由表2數(shù)據(jù)可知，在目標(biāo)點(diǎn)云不加噪聲的情況下，4種剛體運(yùn)動(dòng)參數(shù)求解方法具有相同的精度，且與初始旋轉(zhuǎn)矩陣；平移向量結(jié)果一致。由表3—表7數(shù)據(jù)可知，當(dāng)目標(biāo)點(diǎn)云加入隨機(jī)噪聲時(shí)，對(duì)于旋轉(zhuǎn)矩陣的估計(jì)，奇異值分解法、正交分解法、對(duì)偶四元數(shù)法三者具有相同的精度，均優(yōu)于四元數(shù)的求解精度，以表7數(shù)據(jù)為例，前3種方法的求解誤差為0.011 5，而四元數(shù)的求解誤差為0.013 0，圖3所示的折線圖可以明顯看出4種方法噪聲水平對(duì)角度估計(jì)誤差的影響。

對(duì)于平移向量的求解，通過(guò)表3—表7數(shù)據(jù)可得出對(duì)偶四元數(shù)法精度最高，穩(wěn)定性最好；體現(xiàn)了對(duì)偶數(shù)和四元數(shù)的優(yōu)良性質(zhì)。以表7數(shù)據(jù)為例，當(dāng)噪聲標(biāo)準(zhǔn)差達(dá)到1.0時(shí)，對(duì)偶四元數(shù)的參數(shù)誤差為0.057 5，四元數(shù)參數(shù)誤差0.141 0，奇異值分解法和正交分解法具有相同的精度，參數(shù)誤差均為0.132 3。由圖4可得出平移參數(shù)求解誤差與噪聲標(biāo)準(zhǔn)差之間的關(guān)系。對(duì)偶四元數(shù)精度最高的原因在于求解變換參數(shù)時(shí)，旋轉(zhuǎn)矩陣和平移向量同時(shí)求解，平移向量不會(huì)引入旋轉(zhuǎn)矩陣所帶來(lái)的誤差。

通過(guò)實(shí)驗(yàn)數(shù)據(jù)分析，可知：

1)在目標(biāo)點(diǎn)云不含任何誤差時(shí)，4種剛體運(yùn)動(dòng)參數(shù)求解方法具有相同的求解精度，均滿足實(shí)際工程的需求。

2)在誤差水平相同的情況下，奇異值分解法，正交分解法、對(duì)偶四元數(shù)法對(duì)于旋轉(zhuǎn)矩陣的估計(jì)具有相同的精度，優(yōu)于四元數(shù)的求解。對(duì)于平移參數(shù)的估計(jì)，對(duì)偶四元數(shù)求解精度最高，奇異值分解法與正交分解法具有相同的精度，優(yōu)于四元數(shù)求解精度。

3)在計(jì)算速度方面，由快到慢依次為奇異值分解法、四元數(shù)法、正交分解法、對(duì)偶四元數(shù)法。

4)在綜合性能方面，對(duì)偶四元數(shù)綜合性能最優(yōu)。

2.2 實(shí)測(cè)數(shù)據(jù)分析

以實(shí)測(cè)的點(diǎn)云數(shù)據(jù)為例，如圖5所示，圖中兩平面點(diǎn)云是不同視角下獲得的點(diǎn)云數(shù)據(jù)，已知兩者之間的變換參數(shù)，現(xiàn)分別通過(guò)4種方法求解剛體變換參數(shù)。

圖5 兩平面點(diǎn)云相對(duì)位置

兩平面點(diǎn)云的已知變換參數(shù)如表9所示。

表9 兩點(diǎn)云變換參數(shù)

4種方法求得的轉(zhuǎn)換參數(shù)如表10所示。

表10 4種方法求得的轉(zhuǎn)換參數(shù)

通過(guò)實(shí)測(cè)數(shù)據(jù)求解，由表10數(shù)據(jù)可知，對(duì)偶四元素總體性能優(yōu)于其他三種算法，由圖6可知，經(jīng)過(guò)對(duì)偶四元數(shù)求解的剛體變換參數(shù)；兩點(diǎn)云很好的重合在了一起。對(duì)偶四元數(shù)法滿足實(shí)際工程的需求。

對(duì)偶四元數(shù)配準(zhǔn)后兩點(diǎn)云的相對(duì)位置如圖6所示。

圖6 對(duì)偶四元數(shù)配準(zhǔn)后

3 結(jié) 論

本文通過(guò)模擬仿真實(shí)驗(yàn)與實(shí)際算例對(duì)4種基于點(diǎn)云的剛體運(yùn)動(dòng)參數(shù)求解方法做了比較，得出以下結(jié)論：1)在目標(biāo)點(diǎn)云不含任何誤差時(shí)，4種剛體運(yùn)動(dòng)參數(shù)求解方法具有相同的求解精度，均滿足實(shí)際工程的需求。2)在誤差水平相同的情況下，奇異值分解法、正交分解法、對(duì)偶四元數(shù)法對(duì)于旋轉(zhuǎn)矩陣的估計(jì)具有相同的精度，優(yōu)于四元數(shù)的求解。對(duì)于平移參數(shù)的估計(jì)，對(duì)偶四元數(shù)求解精度最高，奇異值分解法與正交分解法具有相同的精度，優(yōu)于四元數(shù)求解精度。3)在計(jì)算速度方面，由快到慢依次為奇異值分解法、四元數(shù)法、正交分解法、對(duì)偶四元數(shù)法。4)在綜合性能方面，對(duì)偶四元數(shù)綜合性能最優(yōu)。相比其他3種方法，對(duì)偶四元數(shù)能夠同時(shí)求解旋轉(zhuǎn)參數(shù)和平移參數(shù)，平移參數(shù)不存在旋轉(zhuǎn)矩陣誤差累積的問(wèn)題。在實(shí)際問(wèn)題中，應(yīng)優(yōu)先使用對(duì)偶四元數(shù)求解。

本文探討當(dāng)樣本數(shù)據(jù)存在偶然誤差時(shí)，點(diǎn)云剛體運(yùn)動(dòng)參數(shù)估計(jì)的方法，發(fā)現(xiàn)基于對(duì)偶四元數(shù)的估計(jì)方法具有一定的優(yōu)勢(shì)；但是當(dāng)樣本數(shù)據(jù)中存在粗差時(shí)，本文并沒(méi)有做研究，研究具有良好抗差性的點(diǎn)云剛體運(yùn)動(dòng)參數(shù)估計(jì)方法是下一步的重點(diǎn)。

[1] 吳嘉嘉.貝葉斯剛性點(diǎn)云配準(zhǔn)的研究[D].蘭州：蘭州大學(xué),2013.

[2] 林文惠.剛體運(yùn)動(dòng)學(xué)的幾何討論[J].力學(xué)與實(shí)踐,2005,27(5):71-72.

[3] 陳宇,白征東,羅騰.基于改進(jìn)的布爾沙模型的坐標(biāo)轉(zhuǎn)換方法[J].大地測(cè)量與地球動(dòng)力學(xué),2010,30(3):71-73.

[4] 田茂,花向紅,丁鴿,等.基于羅德里格矩陣的混合最小二乘方法在三維激光中的應(yīng)用[J].測(cè)繪地理信息,2014,39(2): 18-21.

[5] HORN B K P,HILDEN H M,NEGAHDARIPOUR S.Closed-form solution of absolute orientation using orthonormal matrices[J].JOSA A,1988,5(7): 1127-1135.

[6] WALKER M W,SHAO L,VOLZ R A.Estimating 3-D location parameters using dual numberquaternions[J].CVGIP: image understanding,1991,54(3): 358-367.

[7] 李志偉,李克昭,趙磊杰,等.基于單位四元數(shù)的任意旋轉(zhuǎn)角度的三維坐標(biāo)轉(zhuǎn)換[J].大地測(cè)量與地球動(dòng)力學(xué),2017,37(1): 81-85.

[8] 宋林霞.三維點(diǎn)云配準(zhǔn)方法的研究[D].濟(jì)南：濟(jì)南大學(xué)，2013.

[9] 邱志強(qiáng),陸宏偉,于起峰.基于圖像的三維剛體運(yùn)動(dòng)估計(jì)算法比較[J].光學(xué)技術(shù),2004,30(1):109-112.

[10] 官云蘭,程效軍,周世健,等.基于單位四元數(shù)的空間后方交會(huì)解算[J].測(cè)繪學(xué)報(bào),2008,37(1): 30-35.

[11] 官云蘭.地面三維激光掃描數(shù)據(jù)處理中的若干問(wèn)題研究 [D].上海: 同濟(jì)大學(xué),2008.

[12] 代俁西.基于對(duì)偶四元數(shù)的三維運(yùn)動(dòng)估計(jì)研究 [D].南京：南京航空航天大學(xué),2016.