相機(jī)與激光跟蹤儀相對(duì)位姿標(biāo)定方法的研究

范百興，楊聚慶，周維虎，李祥云

(1.信息工程大學(xué) 地理空間信息學(xué)院，河南 鄭州 450001；2.北京工業(yè)大學(xué) 應(yīng)用數(shù)理學(xué)院，北京 100124；3.中國(guó)科學(xué)院光電研究院，北京 100094)

隨著現(xiàn)代工業(yè)測(cè)量的需要，在航空、航天和裝配等領(lǐng)域不僅需要對(duì)空間目標(biāo)的位置進(jìn)行精確測(cè)量，需要姿態(tài)信息，即目標(biāo)的六自由度。長(zhǎng)期以來，六自由度測(cè)量的方式多依賴于光學(xué)測(cè)量傳感器、激光干涉儀[1]、多相機(jī)協(xié)作的視覺測(cè)量系統(tǒng)[2]，傳統(tǒng)的六自由度測(cè)量方式儀器組合多、難以實(shí)現(xiàn)跟蹤測(cè)量和使用復(fù)雜等缺點(diǎn)。

在現(xiàn)代精密工程測(cè)量中，激光跟蹤儀具有在單點(diǎn)測(cè)量中的精度高、動(dòng)態(tài)測(cè)量、實(shí)時(shí)快速等優(yōu)點(diǎn)，在數(shù)字化設(shè)計(jì)、制造和裝配檢測(cè)中的應(yīng)用越來越廣泛[3]；視覺測(cè)量具有無接觸、多目標(biāo)、操作簡(jiǎn)便等優(yōu)點(diǎn)，在研究物體的幾何尺寸以及在空間中的位姿中的應(yīng)用越來越廣泛[4]。近年來，由于兩者在測(cè)量中的優(yōu)勢(shì)，使兩者的聯(lián)合測(cè)量在六自由度精密測(cè)量中成為一種新的測(cè)量技術(shù)。在國(guó)外，leica T-mac是激光跟蹤儀測(cè)量技術(shù)與視覺測(cè)量技術(shù)相結(jié)合的目標(biāo)六自由度測(cè)量技術(shù)，實(shí)現(xiàn)目標(biāo)六自由度跟蹤測(cè)量[5]。而在國(guó)內(nèi)此技術(shù)的相關(guān)文獻(xiàn)較少，為了實(shí)現(xiàn)這種高精度、輕便的六自由度測(cè)量，必然存在著相機(jī)與激光跟蹤儀位姿模型的標(biāo)定。標(biāo)定中所用的算法有：基于Canny算子的標(biāo)志中心坐標(biāo)提取；基于4個(gè)非共線控制點(diǎn)的單像空間后方交會(huì)；坐標(biāo)系轉(zhuǎn)換。坐標(biāo)系轉(zhuǎn)換的方法有奇異值分解、四元數(shù)法及羅德里格矩陣法[6]。奇異值分解的方法比較依賴于矩陣的性質(zhì)，不一定得到旋轉(zhuǎn)矩陣，特殊性較強(qiáng)，四元數(shù)解算速度較快，但是精度不容易控制，而羅德里格矩陣在任意角度的姿態(tài)轉(zhuǎn)換解算中的嚴(yán)密性，在迭代中可以設(shè)置精度閾值，使用較為方便，因此本文在模型標(biāo)定中采用羅德里格矩陣的方法。

1 相機(jī)坐標(biāo)系下靶點(diǎn)坐標(biāo)的解算

本文首先利用Canny算子對(duì)含有靶點(diǎn)的圖像進(jìn)行邊緣檢測(cè)，通過標(biāo)志輪廓檢測(cè)、模式識(shí)別、最小二乘擬合橢圓中心的方法得到標(biāo)志中心坐標(biāo)[7]，再由內(nèi)方位元素得到靶點(diǎn)的像點(diǎn)坐標(biāo)。最后通過一種基于4個(gè)非共線控制點(diǎn)的單張像片空間后方交會(huì)算法可直接求得單張像片的外方位元素[8]。算法流程如圖1所示。

經(jīng)過圖像處理可以方便準(zhǔn)確的得到靶點(diǎn)的圖像像素坐標(biāo)(ui,vi)，再由相機(jī)內(nèi)部參數(shù)得到像點(diǎn)坐標(biāo)：

(1)

式中：u0，v0是相片尺寸長(zhǎng)和寬的一半，dx，dy像素大小。在像點(diǎn)坐標(biāo)的基礎(chǔ)上，除了利用相機(jī)內(nèi)參數(shù)的主距f和主點(diǎn)坐標(biāo)(x0,y0)，還有鏡頭徑向畸變(k1,k2,k3)、偏心畸變(p1,p2)及平面畸變(b1,b2)等7個(gè)內(nèi)參數(shù)可以修正像點(diǎn)坐標(biāo)，本文修正像點(diǎn)坐標(biāo)的10參數(shù)模型為

(2)

如圖2所示，物方點(diǎn)A，B，C在激光跟蹤儀下的坐標(biāo)為(XL1,YL1,ZL1)，(XL2,YL2,ZL2)，(XL3,YL3,ZL3)，到相機(jī)中心S的距離分別為AS，BS，CS，相應(yīng)像點(diǎn)坐標(biāo)分別為A′(x1,y1)，B′(x2,y2)，C′(x3,y3);ΔABC邊長(zhǎng)分別記為AB，AC，BC;∠ASB，∠BSC，∠ASC分別記為α，β，γ。

圖2 利用4個(gè)控制點(diǎn)計(jì)算外方位元素

根據(jù)圖中的幾何結(jié)構(gòu)及余弦定理可得

(3)

其中：

(4)

設(shè)AS∶BS∶CS=1∶n∶m，即BS=nAS,CS=mAS，將三者比例關(guān)系代入式(3)，消去AS，m整理得

w1n4+w2n3+w3n2+w4n+w5=0.

(5)

其中系數(shù)wi均可由物方點(diǎn)坐標(biāo)，α，β及γ求得。該一元四次方程可能有4個(gè)根，再加一個(gè)靶控制點(diǎn)D，選取兩組解中相同的根即為正確的n值。進(jìn)而可得

(6)

在相機(jī)坐標(biāo)系OCXCYCZC中，像點(diǎn)的坐標(biāo)為A′(x1,y1,-f)，B′(x2,y2,-f)，C′(x3,y3,-f)，由圖2知，根據(jù)AS與SA′、BS與SB′和CS與SC′之間的比例關(guān)系可得各靶點(diǎn)在坐標(biāo)系OCXCYCZC中的坐標(biāo)(XCi,YCi,ZCi)：

(7)

2 基于羅德里格矩陣的坐標(biāo)系轉(zhuǎn)換

當(dāng)已知3個(gè)靶點(diǎn)在相機(jī)坐標(biāo)系下和在激光跟蹤儀坐標(biāo)系下的坐標(biāo)時(shí)，根據(jù)公共點(diǎn)求得相機(jī)坐標(biāo)系和激光跟蹤儀坐標(biāo)系的位姿轉(zhuǎn)換矩陣。當(dāng)多于3個(gè)靶點(diǎn)時(shí)，公共點(diǎn)轉(zhuǎn)換一般需要先求初值再進(jìn)行平差迭代[9]，本文利用基于羅德里格矩陣的方法求解轉(zhuǎn)換參數(shù)。

設(shè)相機(jī)坐標(biāo)系和激光跟蹤儀坐標(biāo)系的位姿轉(zhuǎn)換矩陣為Tr,

.

(8)

由靶點(diǎn)分別在兩坐標(biāo)系中的坐標(biāo)可得

.

(9)

其中R,T分別是位姿轉(zhuǎn)換矩陣Tr中的旋轉(zhuǎn)矩陣和平移矢量。本文利用3個(gè)控制點(diǎn)求解旋轉(zhuǎn)矩陣和平移參數(shù)的初值。其中R用羅德里格矩陣表示

R=(I-S)-1(I-S).

(10)

其中I為單位陣，

.

(11)

,(i=1,2,3).

(12)

其中，

.

(13)

將式(13)代入式(12)得

.

(14)

由式(14)可知，n對(duì)公共點(diǎn)對(duì)應(yīng)3n個(gè)觀測(cè)方程，將其寫成矩陣形式為

(15)

其中

，

艾滋病可經(jīng)過母嬰傳播、性傳播以及血液傳播等，機(jī)體感染艾滋病病毒后引發(fā)病毒血癥，病毒在咨詢者肝細(xì)胞內(nèi)復(fù)制并引發(fā)病變，此外，艾滋病病毒表達(dá)產(chǎn)物存在肝臟毒性，會(huì)導(dǎo)致肝功能下降。若咨詢者艾滋病抗體實(shí)驗(yàn)室檢查結(jié)果為陽(yáng)性、近期有流行病史，則可診斷為艾滋病急性期[4] 。大部分艾滋病感染咨詢者在感染2個(gè)月后即可檢出艾滋病抗體，感染后3個(gè)月可全部檢出艾滋病抗體。當(dāng)前，我國(guó)主要通過疫苗預(yù)防艾滋病感染，同時(shí)加強(qiáng)艾滋病宣傳，及時(shí)發(fā)現(xiàn)艾滋病并采取病情控制措施，有助于使艾滋病毒損傷得到控制并可使病毒傳播得到減少[5] 。

(16)

當(dāng)n=3時(shí)，此時(shí)沒有多于觀測(cè)量，直接求解D，那么：

D=(AT·A)-1·(AT·l).

(17)

將解得的a，b，c，u，v，w作為D的初值D0，并將其余公共點(diǎn)代入式(15)組成觀測(cè)方程，

A·(D0+ΔD)=l.

(18)

進(jìn)而求改正值，

ΔD=(AT·A)-1·[AT·(l-A·D0)].

(19)

判斷此時(shí)改正值是否小于閾值ε，否則代回式(17)修正初值D0，并繼續(xù)求解修正值直到滿足閾值條件。最后，將經(jīng)過迭代后的D0代入式(10)、式(11)和式(13)可求得位姿變換矩陣R,T。

3 算例和分析

3.1 數(shù)據(jù)采集與預(yù)處理

測(cè)量過程使用的工業(yè)相機(jī)為basler ACA 2500-20 gm，鏡頭為computer m0814-MP，CCD成像個(gè)數(shù)為(2 592像素×2 048像素)，像素大小為 0.004 8 mm×0.004 8 mm。首先對(duì)相機(jī)內(nèi)參數(shù)進(jìn)行標(biāo)定，利用教研室標(biāo)定場(chǎng)，采用基于有限元模型的數(shù)字工業(yè)攝影測(cè)量相機(jī)二次檢校的方法檢校相機(jī)[10]，標(biāo)定結(jié)果為:x0=-0.128 630 6 mm，y0=0.202 353 3 mm，f=11.860 89 mm，k1=7.659 920×10-4，k2=-4.942 919-6，k3= 3.922 201-9，p1=-6.417 617-6，p2= -2.753 530-6，b1=-6.517 062-6，b2=-5.578 664-5；使用的激光跟蹤儀為L(zhǎng)eica AT901-B型,配合CCR1.5角隅棱鏡靶球,在2.5 m×5 m×10 m的工作范圍內(nèi)三維點(diǎn)坐標(biāo)測(cè)量精度為±(15 μm+6 μm/m)。

首先在試驗(yàn)場(chǎng)布置5個(gè)靶座，靶座上既可以放置角隅棱鏡靶球也可以放置人工標(biāo)志工裝，激光跟蹤儀所測(cè)靶球中心和視覺測(cè)量的標(biāo)志中心理論上為同一點(diǎn)；然后將人工標(biāo)志工裝放置于靶座上，用固定好的相機(jī)采集包含人工標(biāo)志的圖像，隨后撤掉人工標(biāo)志工裝并用激光跟蹤儀(配合靶球)測(cè)量靶座中心坐標(biāo)；最后用羅德里格矩陣進(jìn)行坐標(biāo)系轉(zhuǎn)換。

對(duì)采集到圖像首先利用Opencv中Canny算子對(duì)標(biāo)志進(jìn)行檢測(cè)邊緣，并用橢圓擬合算法求出圓形標(biāo)志的中心坐標(biāo)，此時(shí)坐標(biāo)為圖像像素坐標(biāo)系下，需由式(1)轉(zhuǎn)換為像平面坐標(biāo)系下，結(jié)果如表1所示。

表1 橢圓擬合得到的標(biāo)志中心坐標(biāo) mm

利用激光跟蹤儀和靶球分別測(cè)量各個(gè)靶座上的中心坐標(biāo)，得到坐標(biāo)結(jié)果如表2所示。

表2 跟蹤儀坐標(biāo)系下各靶座中心坐標(biāo) mm

通過測(cè)量和預(yù)處理，表1為經(jīng)過預(yù)處理后的數(shù)據(jù)，表2為激光跟蹤儀直接測(cè)量所得到的數(shù)據(jù)。在所采集數(shù)據(jù)基礎(chǔ)上即可進(jìn)行坐標(biāo)系轉(zhuǎn)換和精度分析。

3.2 坐標(biāo)系轉(zhuǎn)換和精度分析

在對(duì)應(yīng)的5組坐標(biāo)中選取4組作為求解坐標(biāo)系轉(zhuǎn)換參數(shù)的數(shù)據(jù)，為了保證4點(diǎn)非共線以及各靶點(diǎn)與相機(jī)中心S的連線之間的夾角盡量大，實(shí)驗(yàn)所用點(diǎn)號(hào)分別為1、2、3以及4，根據(jù)相機(jī)坐標(biāo)系下靶點(diǎn)坐標(biāo)的解算理論，可能會(huì)解出兩組相近的n值，利用兩組n值分別求出坐標(biāo)系轉(zhuǎn)換參數(shù)，進(jìn)而利用共線方程反算像點(diǎn)坐標(biāo)，反算結(jié)果與表1進(jìn)行比較，結(jié)果相近的為正解，結(jié)果如表3所示。

表3 靶點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo) mm

利用表2和表3中4組對(duì)應(yīng)的公共點(diǎn)，用羅德里格矩陣進(jìn)行坐標(biāo)系轉(zhuǎn)換，先用其中3組數(shù)據(jù)解算轉(zhuǎn)換參數(shù)的初值，利用初值和全部數(shù)據(jù)進(jìn)行最小二乘迭代，直到平差精度滿足設(shè)定閾值為止。最終所得跟蹤儀與相機(jī)相對(duì)位姿模型為

(20)

為了驗(yàn)證坐標(biāo)系轉(zhuǎn)換精度，本文利用矩陣T及共線方程反算像點(diǎn)坐標(biāo)，將結(jié)果與橢圓擬合所得結(jié)果進(jìn)行比較，比較結(jié)果如表4所示，

(21)

表4僅僅為相機(jī)在某一個(gè)位姿下的標(biāo)定精度，為了說明算法的普適性，將相機(jī)調(diào)整位姿繼續(xù)拍攝兩張照片，標(biāo)定結(jié)果如表5和表6所示。

綜合表4、表5和表6得計(jì)算結(jié)果，位姿標(biāo)定模

表4 坐標(biāo)反算結(jié)果與橢圓擬合坐標(biāo)對(duì)比表 mm

表5 相機(jī)位姿一次調(diào)整后的標(biāo)定結(jié)果 mm

表6 相機(jī)位姿二次調(diào)整的標(biāo)定結(jié)果 mm

4 結(jié) 論

在六自由度精密測(cè)量中，針對(duì)激光跟蹤儀與視覺測(cè)量聯(lián)合應(yīng)用中的相機(jī)與激光跟蹤儀相對(duì)位姿的標(biāo)定問題，本文利用基于羅德里格矩陣的方法標(biāo)定了兩者之間的相對(duì)位姿以及利用共線方程反算標(biāo)志中心坐標(biāo)驗(yàn)證了轉(zhuǎn)換精度。實(shí)驗(yàn)結(jié)果證明，所得相機(jī)坐標(biāo)系與激光跟蹤儀相對(duì)位姿模型總體精度保持在0.01 mm以下，基本滿足了激光跟蹤儀與相機(jī)聯(lián)合精密測(cè)量中的需要。