【W(wǎng)ORD格式論文底稿】基于對(duì)偶四元數(shù)的未知幾何結(jié)構(gòu)的目標(biāo)的位姿確定1

1、基于對(duì)偶四元數(shù)的未知幾何結(jié)構(gòu)的目標(biāo)的位姿確定1馮國(guó)虎，吳文啟 國(guó)防科技大學(xué)機(jī)電工程與自動(dòng)化學(xué)院，長(zhǎng)沙410073 E-mail HYPERLINK mailto:yantai_fgh126 ：yantai_fgh126 摘要：為了解決攝影測(cè)量中目標(biāo)體坐標(biāo)和相機(jī)坐標(biāo)系之間的相對(duì)位置、姿態(tài)確定問(wèn)題，用 單攝像機(jī)作為觀測(cè)設(shè)備，用對(duì)偶四元數(shù)描述目標(biāo)體坐標(biāo)和相機(jī)坐標(biāo)系之間的變換，用透視投 影描述目標(biāo)直線在相機(jī)坐標(biāo)系中的投影變換，將表征目標(biāo)位置和姿態(tài)以及運(yùn)動(dòng)線速度和角速度加上目標(biāo)未知的直線特征作為狀態(tài)量，圖像二維坐標(biāo)作為觀測(cè)值，建立擴(kuò)展卡爾曼濾波器，得到目標(biāo)位置、姿態(tài)和運(yùn)動(dòng)的估計(jì)值。仿真說(shuō)明，估計(jì)值和真實(shí)

2、值是一致的，減小了對(duì)目標(biāo) 幾何尺寸準(zhǔn)確描述的要求，為攝影測(cè)量中的目標(biāo)定位找到了一個(gè)具有工程應(yīng)用的方法。 關(guān)鍵詞：?jiǎn)文恳曈X(jué)；對(duì)偶四元數(shù)；擴(kuò)展卡爾曼濾波；幾何特性；位姿確定中圖分類號(hào)：U61引言確定兩個(gè)坐標(biāo)系之間的相對(duì)位置、姿態(tài)以及相對(duì)運(yùn)動(dòng)在機(jī)器人、計(jì)算機(jī)視覺(jué)、攝影測(cè)量 等領(lǐng)域是一個(gè)重要問(wèn)題。為了解決這一問(wèn)題，在大多數(shù)實(shí)際應(yīng)用中利用單攝像機(jī)的連續(xù)圖像。 當(dāng)目標(biāo)物體幾何特征未知時(shí)，通常還要求估計(jì)物體的幾何尺寸。多使用單攝像機(jī)是因?yàn)闃?biāo)準(zhǔn) 攝像機(jī)本錢低，安裝和標(biāo)定簡(jiǎn)單，物理占用空間小，可靠性高，同時(shí)數(shù)字化和處理圖像的硬 件本錢低。通過(guò)圖像測(cè)量確定目標(biāo)位置和姿態(tài)存在一定困難，目標(biāo)物體的三維特征投影到二 維

3、圖像上是非線性過(guò)程。剛體運(yùn)動(dòng)變換的傳統(tǒng)算法是用四元數(shù)或方向余弦陣(DCM)表示旋轉(zhuǎn)，向量表示平移。這 種算法要求四元數(shù)更新速度必須足夠快以減小平移因素帶來(lái)的影響。與質(zhì)點(diǎn)運(yùn)動(dòng)的純位移不 同，剛體一般運(yùn)動(dòng)包括繞空間軸的旋轉(zhuǎn)和沿該軸的平移，兩者必須聯(lián)合起來(lái)分析。純四元數(shù) 在表示坐標(biāo)變換時(shí)會(huì)發(fā)生變換斷裂1，也就是不連續(xù)的情況，這是在剛體一般運(yùn)動(dòng)中一直困 擾我們的問(wèn)題。這一問(wèn)題因應(yīng)用對(duì)偶四元數(shù)得到圓滿解決。對(duì)偶四元數(shù)能夠同時(shí)描述物體的 轉(zhuǎn)動(dòng)和平移，是描述一般運(yùn)動(dòng)最為簡(jiǎn)潔完整的數(shù)學(xué)工具，利用較少的測(cè)量值圖像二維坐標(biāo) 等，在不破壞坐標(biāo)變換中位移和轉(zhuǎn)動(dòng)的完整性和融合性blend的根底上，可以同時(shí)獲 得相對(duì)位置

4、和姿態(tài)，這是四元數(shù)或 DCM 方法等傳統(tǒng)算法難以做到的。在應(yīng)用對(duì)偶四元數(shù)確定目標(biāo)位置、姿態(tài)方面，James2提出觀測(cè)目標(biāo)直線的 line-point 坐 標(biāo)來(lái)確定物體坐標(biāo)系相對(duì)相機(jī)坐標(biāo)系的位置、姿態(tài)，而 Wu_xuedong3在此根底上對(duì)其中的 數(shù)值計(jì)算作了更為詳實(shí)的研究。這些研究的前提是目標(biāo)幾何尺寸準(zhǔn)確，如果目標(biāo)幾何尺 寸有誤那么難以準(zhǔn)確估計(jì)目標(biāo)的位置、姿態(tài)。實(shí)際情況通常是目標(biāo)局部幾何尺寸，其余尺 寸未知，在這種情況下如何準(zhǔn)確確定目標(biāo)的位置、姿態(tài)正是本文要解決的問(wèn)題。2根底知識(shí) 對(duì)偶四元數(shù)對(duì)偶四元數(shù)屬于幾何代數(shù)的一種，由實(shí)部和對(duì)偶局部組成，q = r + s ，r, s 為四元數(shù)，其中 0,

5、 2 = 0 。根據(jù) Kotelnikov 的轉(zhuǎn)移原理(Principle of Transference)4，對(duì)偶四元數(shù)完2*全繼承了四元數(shù)的性質(zhì)。對(duì)偶四元數(shù)范數(shù)定義為 q= qq，其中 q= r + s為共軛對(duì)偶1本課題得到高等學(xué)校博士學(xué)科點(diǎn)專項(xiàng)科研基金工程編號(hào) 20069998009的資助。四元數(shù)。對(duì)偶四元數(shù)可以同時(shí)描述剛體的轉(zhuǎn)動(dòng)和平移，q = q + t q ，其中 q, t 分別表示剛2體的純轉(zhuǎn)動(dòng)和純平移。 坐標(biāo)系轉(zhuǎn)換對(duì)偶四元數(shù)可實(shí)現(xiàn)坐標(biāo)系變換。如圖 1 所示，坐標(biāo)系 O 繞空間矢量 l 旋轉(zhuǎn) 角度，同時(shí)沿 l 平移 d 形成新坐標(biāo)系 N，該過(guò)程是剛體的一般空間運(yùn)動(dòng)，可用單位對(duì)偶四元

6、數(shù)表示：q = cos ( 2)sin ( 2)l1 其中 l = l + ( p l ) ， p 為坐標(biāo)系 O 的原點(diǎn)到旋轉(zhuǎn)軸上一點(diǎn)的向量。從圖 1 可以看出，只要給定對(duì)偶矢量 l 和對(duì)偶角 ，對(duì)偶四元數(shù)就可以非常簡(jiǎn)潔地表示坐標(biāo)系 O 到 N 的變換。圖 1 由坐標(biāo)系 O 變換到坐標(biāo)系 NFig.1 Transformation from coordinates O to N設(shè)坐標(biāo)系內(nèi)過(guò) p 點(diǎn)的 l 方向直線 Plu c ker 直線 l 可以由 (l, m) 表示，其中 m = p l 。 對(duì)偶四元數(shù)算子 q ( ) q* 可以表示空間矢量的任意變換。于是，同一 Plu c ker 直線

7、 l 在兩個(gè)坐標(biāo)系中分別表示為： l = l+ m OOOl = l+ m且有： NNNl + m= q + t q (l+ m) q + t q *2NN2 OO 2 式2完成了坐標(biāo)系變換，即同一矢量在不同坐標(biāo)系中的表示。 對(duì)偶四元數(shù)更新對(duì)偶四元數(shù)的動(dòng)力學(xué)方程5為q = 1 q2式中 為剛體運(yùn)動(dòng)的角速度斜對(duì)稱矩陣，當(dāng)剛體運(yùn)動(dòng)角速度為恒定值時(shí)，可解得1 3泰勒展開(kāi)近似得q (tk +1 ) = e 2q (tk ) 1 q (t1 ) = cos I +sin q (t) = Qq (t )4k + 2 2ktrank一般情況下式中 并非常值，可將更新算法中的時(shí)間系數(shù)選得足夠小，使得 在足夠小

8、的時(shí)間間隔內(nèi)可當(dāng)作常量來(lái)處理。3目標(biāo)位姿估計(jì) 系統(tǒng)模型狀態(tài)量 s 定義為Ts ( k ) = txt ytzq0q1q2q3VWl1mm1mlnmmnm5式中 ti 和 qi 分別是線性平移量和轉(zhuǎn)動(dòng)四元數(shù)，V ,W 分別是線速度和角速度，為簡(jiǎn)化問(wèn)題， 假設(shè)在充分小的時(shí)間間隔內(nèi)為勻速運(yùn)動(dòng)，即TTV (tk ) = V (tk 1 ) ,V = vxvyvz ,W (tk ) = W (tk 1 ) ,W = xyz lim , mim 分別是描述目標(biāo)直線的對(duì)偶四元數(shù)的實(shí)部和對(duì)偶部，n 為直線條數(shù)，目標(biāo)的幾何特 征不隨時(shí)間變化而改變，因此 lim , mim 為常量。系統(tǒng)方程sk = k 1 (

9、sk 1 ) + wk 1式中 w 為系統(tǒng)噪聲，假設(shè)為高斯白噪聲，其協(xié)方差矩陣為 Qk 1 ，狀態(tài)轉(zhuǎn)移函數(shù)為6Tk ( s ) = tx + vxt y + vytz + vzQtran q (tk )VWl1mm1mlnmmnm式中 Qtran 根據(jù)式4定義。 觀測(cè)方程觀測(cè)方程為zk = hk ( sk ) + k7式中 為觀測(cè)噪聲，假設(shè)為高斯白噪聲，其協(xié)方差矩陣為 Rk ，觀測(cè)量為目標(biāo)直線在像平面投影的 line-point 坐標(biāo)2，即zk =x1lpy1lpxnlpTynlp8圖 2 二維像平面上 line point 的定義Fig.2 The definition of line po

10、int on the 2D image plane目標(biāo)體坐標(biāo)中直線投影到相機(jī)坐標(biāo)中像平面上是一個(gè) 3-D 到 2-D 的非線性過(guò)程。為方便分析分兩步研究：首先是坐標(biāo)系變換，目標(biāo)直線由在體坐標(biāo)中表示變換成在相機(jī)坐標(biāo)系中 表示，這一個(gè)過(guò)程由對(duì)偶四元數(shù)完成；然后是投影變換，將空間直線投影到 2-D 像平面，這 一過(guò)程用透視投影描述。相機(jī)坐標(biāo)系中點(diǎn) ( xc , yc , zc ) 投影到像平面坐標(biāo) ( xi , yi ) 為記 222xi 0= xcf, yi 0zcf= yczc9ri = xi 0 + yi 0 ，考慮到透鏡畸變后像點(diǎn)坐標(biāo)為ii 0i 0 i 0 2kc (3) x y+ kc

11、( 4) (r 2 + 2x2 ) xi = (1 + kc (1) r 2 + kc ( 2) r 4 ) xi 0 + i 0 i 0ii 0 ii yi yi 0 kc (3) ( r 2 + 2 y 2 ) + 2kc ( 4) x y 式中畸變系數(shù) kc 通過(guò)相機(jī)標(biāo)定獲得?？臻g向量在像平面上的投影位于空間直線和相機(jī)原點(diǎn)確定的平面上，該平面方程為mx x + my y + mz z = 0該平面交像平面于 z = f ，得到投影直線方程mx xi + my yi = mz f10那么投影直線的方向向量為Tli = mym2 + m2mx0m2 + m2 xyxy投影直線用對(duì)偶四元數(shù)表示

12、為 Li = li + mi ， mi = pi li 其中 pi 為投影直線上的點(diǎn)，滿足mi =fxym2 + m2m ，根據(jù) line point 的定義，有x = l m = fmx mz, y = l m = fmy mz11lpiy izm2 + m2lpix izm2 + m2xyxy 擴(kuò)展卡爾曼濾波由式6和7知，系統(tǒng)動(dòng)態(tài)過(guò)程和測(cè)量方程都是非線性的，采用擴(kuò)展卡爾曼濾波EKF 進(jìn)行狀態(tài)估計(jì)，濾波過(guò)程需要對(duì)狀態(tài)轉(zhuǎn)移函數(shù)和觀測(cè)函數(shù)線性化。狀態(tài)轉(zhuǎn)移函數(shù)線性化 k ( s k( ) = k ( s )ss = s k ( ) It0 I0000 0Qtran0Qw000 00Iv0000 =

13、 000I00012k 0000Il ,m00 00000% 0 000000Il ,mQtran q (tk )式中 Q =觀測(cè)函數(shù)線性化 =kH k ( s k( ) = h ( s )ss = s k ( ) nm nm 式中 H k 為 2in 矩陣，i 為觀測(cè)目標(biāo)直線條數(shù)，n 為狀態(tài)量維數(shù)xlpx=lpmx + xlpmy + xlp mzsimxsimysimzsi13ylpy=lpmx + ylpmy + ylp mzsi對(duì)式11求 m 的偏導(dǎo)，得mxsimysimzsixfmfm2mlp = z 2 x z xxyxym(m2 + m2 )(m2 + m2 )2xlpm= 2f

14、m x my mz(m2 + m2 )2yxyxlpm=fmx(m2 + m2 )zxyylpm= 2fm x my mz(m2 + m2 )2xxyyfmfm2m= 2lp z m(m2 + m2 )(y z22 )2yxymx + myylpmfmy= (m2 + m2 )zxy對(duì)偶四元數(shù)描述直線由目標(biāo)體坐標(biāo)系到相機(jī)坐標(biāo)系的變換為l + m =( r + s ) (lm + mm ) (r* + s* )式中直線在相機(jī)坐標(biāo)系中的對(duì)偶部 m 為*1* *1*m = rmm r寫成四元數(shù)矩陣聯(lián)乘形式，有+rlm r t2+trlm r2+1 + +m = M r M r*mm + M t +

15、M2 t* M r M * lmr14對(duì)式14求狀態(tài)量 s 的偏導(dǎo)，有+ + m = 1 M t + M t* M M lti2tirr* m + + m M r M r* =1 + M r M * r=qqmm + M t + M t* qlmii2 im = 0, m = 0vimi+T mT+m = M r M r* , l= M t + M t* M r M r* mm 將對(duì)式11和14的偏導(dǎo)代入式13，即可得到觀測(cè)矩陣 H k 的解析表達(dá)式。4實(shí)驗(yàn)及結(jié)果分析以目標(biāo)直線投影的 line-point 為觀測(cè)量，通過(guò) EKF 估計(jì)目標(biāo)位姿、速度的流程如圖 3 所 示。如果目標(biāo)幾何特征，即

16、lim , mim ，狀態(tài)量相應(yīng)降維s ( k ) = txt ytzq0q1q2Tq3VW 15狀態(tài)轉(zhuǎn)移函數(shù)和觀測(cè)函數(shù)也作相應(yīng)簡(jiǎn)化。如果目標(biāo)幾何特征完全未知，那么對(duì)應(yīng)狀態(tài)量維數(shù)很 大，難以通過(guò)單純的 EKF 獲得準(zhǔn)確的目標(biāo)位姿、速度信息。通常情況下，目標(biāo)幾何特征是 局部的，可以利用的尺寸和相應(yīng)圖像測(cè)量，結(jié)合透視投影估算目標(biāo)未知尺寸，作為 相應(yīng)狀態(tài)量的初始值，通過(guò) EKF 獲得目標(biāo)位姿、速度信息。以平面四邊形為目標(biāo)進(jìn)行研究，體坐標(biāo)選擇和目標(biāo)直線標(biāo)注如圖 4 所示。根據(jù)文獻(xiàn)6 的結(jié)論：如果平行四邊形的兩組邊長(zhǎng)均未知，那么無(wú)法確定平移向量的大??；當(dāng)知道平行四邊 形任一組邊長(zhǎng)時(shí)，旋轉(zhuǎn)矩陣和平移向量均

17、可以唯一確定。假設(shè)直線 2，4 之間的距離，那么 l2 m , m2 m 和 l4 m , m4 m ，直線 1，3 平行于 X b 軸，l1m , l3m ， m1m , m3m 未知。由直線2，4 之間的幾何尺寸和直線 2，4 和 1，3 在像平面上投影之間距離，根據(jù)透視投影可以算 出直線 1，3 之間的幾何尺寸，因?yàn)槟繕?biāo)體坐標(biāo)和相機(jī)坐標(biāo)系之間存在旋轉(zhuǎn)，計(jì)算出的尺寸 存在誤差。為了方便比擬，在相同條件下對(duì)同一目標(biāo)的位姿、速度用不同方法進(jìn)行估計(jì)。目標(biāo)幾何尺寸為 200cm80 cm，直線 1，3 之間尺寸估算誤差 10。初始狀態(tài)量，誤差 均方差和系統(tǒng)噪聲如表 1 和 2 所示。假設(shè)相機(jī)是理想

18、模型，焦距為 12mm，像點(diǎn)定位誤差mm，那么觀測(cè)噪聲矩陣 R 的對(duì)角線元素為mm2。仿真總時(shí)間 30s，濾波周期 s。表 1 真實(shí)初始狀態(tài)量和估計(jì)初始狀態(tài)量Tab.1 True and assumed initial state狀態(tài)量平移量m四元數(shù)線速度m/sec角速度rad/sec真實(shí)初始量10 10 10001 0 0 05 5 00 0 0.05估計(jì)初始量0 0 9900.9998 0.01 0.01 0.010 0 00 0 0表 2 誤差協(xié)方差初值和系統(tǒng)噪聲矩陣的對(duì)角線元素Tab.2 The diagonal elements of initial error covariance

19、 and process noise matrix噪聲均方差平移量m2四元數(shù)線速度m/sec2角速度rad/sec2誤差均方差初值1000.01100系統(tǒng)噪聲10-510-510-510-6圖 3 目標(biāo)位姿、速度估計(jì)流程示意圖圖 4 體坐標(biāo)和目標(biāo)直線標(biāo)注Fig.3 Functional block diagram of the pose andFig.4 The object reference frame and motion estimationlines label on the object文獻(xiàn)2中的方法仿真結(jié)果如圖 5 所示圖 5 文獻(xiàn)2中的方法對(duì)目標(biāo)位姿、速度信息的估計(jì)Fig.5

20、Simulation results of pose and motion estimation using Jamess2method從圖 5 可以看出，當(dāng)用有誤差的估計(jì)值代替未知的目標(biāo)幾何尺寸時(shí)，目標(biāo)的位姿、速度估計(jì) 也存在誤差。要準(zhǔn)確對(duì)目標(biāo)定位，必須事先知道目標(biāo)幾何尺寸的準(zhǔn)確值，這在攝影測(cè)量中難以滿足。本文方法的仿真結(jié)果如圖 6 所示圖 6 本文方法對(duì)目標(biāo)位姿、速度信息的估計(jì)Fig.6 Simulation results of pose and motion estimation using our method比擬圖 5 和 6 發(fā)現(xiàn)在目標(biāo)幾何尺寸估計(jì)誤差相同的條件下，相對(duì)于文獻(xiàn)2中

21、的方法， 本文方法提高了目標(biāo)位姿、速度估計(jì)精度，從而減少了對(duì)目標(biāo)幾何尺寸準(zhǔn)確描述的要求。對(duì)目標(biāo)未知幾何尺寸的估計(jì)量如圖 7 所示，從圖 7 可以看出，目標(biāo)尺寸的估計(jì)量向真實(shí) 值趨近，但沒(méi)有收斂于真實(shí)值。計(jì)算可觀性矩陣相應(yīng)的奇異值，發(fā)現(xiàn)這些奇異值很小，說(shuō)明 這些狀態(tài)量的可觀測(cè)度低，解釋了尺寸估計(jì)值沒(méi)有收斂于真實(shí)值的原因。5總結(jié)圖 7 對(duì)目標(biāo)未知幾何尺寸的估計(jì)Fig.7 The estimation of objects unknown geometric description本文解決了在目標(biāo)幾何尺寸不是完全情況下對(duì)目標(biāo)位置、姿態(tài)準(zhǔn)確估計(jì)問(wèn)題。對(duì)本 文方法的仿真結(jié)果說(shuō)明目標(biāo)位姿的估計(jì)值迅速收斂于

22、真實(shí)值，相對(duì)于 James2的方法，本文 方法提高了目標(biāo)位姿估計(jì)的精度，降低了對(duì)目標(biāo)幾何尺寸準(zhǔn)確描述的要求，符合工程應(yīng)用的 實(shí)際情況。仿真發(fā)現(xiàn)目標(biāo)未知的幾何尺寸沒(méi)有收斂于真實(shí)值，為提高目標(biāo)位置、姿態(tài)估計(jì)的精度，需要給出準(zhǔn)確的初值。大量仿真說(shuō)明，目標(biāo)未知尺寸估計(jì)值誤差在 5之內(nèi)時(shí)，目標(biāo)位置、姿態(tài)估計(jì)誤差在 3之內(nèi)。如何結(jié)合其他方法將目標(biāo)未知尺寸估計(jì)值誤差控制在 5之內(nèi)是 下一步研究方向。同時(shí)，相機(jī)的真實(shí)模型以及系統(tǒng)噪聲和觀測(cè)噪聲真實(shí)特征的準(zhǔn)確描述也是 下一步研究方向。參考文獻(xiàn)1 S C. Dual quaternions for rigid transformation blendingC/ p

23、roceedings of the Symposium on Interactive 3D Graphics and Games, 2007.2 S GJ. Pose and motion estimation from vision using dual quaternion-based extended kalman filteringDD. Knoxville: Tennessee, 1997.3 Xue-dong W. An application of unscented Kalman filter for pose and motion estimation based on mo

24、nocular visionC/ proceedings of the IEEE International Sysmposium on Industrial Electronics, 2006.4 P KA. Screw calculus and some applications to geometry and mechanicsJ. Annuals of Imperial University ofKazan, 1895.5 武元新. 對(duì)偶四元數(shù)導(dǎo)航算法和非線性高斯濾波研究DD. 長(zhǎng)沙: 國(guó)防科學(xué)技術(shù)大學(xué), 2005.6 孫風(fēng)梅, 王衛(wèi)寧. 基于單個(gè)平行四邊形單幅圖像的物體定位J. 自動(dòng)

25、化學(xué)報(bào), 2006,32(5): 746-752.Pose Estimation with Unknown Object Geometry based-onDual QuaternionFeng Guohu, Wu WenqiCollege of Mechatro-Eletronic and Automation, NUDT, Changsha (410073)AbstractDetermination of relative three-dimensional position and orientation between object reference frameand camera reference frame is an important problem in photogrammetry. A s