相機與激光跟蹤儀相對位姿標定方法的研究

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

(1.信息工程大學 地理空間信息學院，河南 鄭州 450001；2.北京工業大學 應用數理學院，北京 100124；3.中國科學院光電研究院，北京 100094)

隨著現代工業測量的需要，在航空、航天和裝配等領域不僅需要對空間目標的位置進行精確測量，需要姿態信息，即目標的六自由度。長期以來，六自由度測量的方式多依賴于光學測量傳感器、激光干涉儀[1]、多相機協作的視覺測量系統[2]，傳統的六自由度測量方式儀器組合多、難以實現跟蹤測量和使用復雜等缺點。

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

1 相機坐標系下靶點坐標的解算

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

經過圖像處理可以方便準確的得到靶點的圖像像素坐標(ui,vi)，再由相機內部參數得到像點坐標：

(1)

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

(2)

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

圖2 利用4個控制點計算外方位元素

根據圖中的幾何結構及余弦定理可得

(3)

其中：

(4)

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

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

(5)

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

(6)

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

(7)

2 基于羅德里格矩陣的坐標系轉換

當已知3個靶點在相機坐標系下和在激光跟蹤儀坐標系下的坐標時，根據公共點求得相機坐標系和激光跟蹤儀坐標系的位姿轉換矩陣。當多于3個靶點時，公共點轉換一般需要先求初值再進行平差迭代[9]，本文利用基于羅德里格矩陣的方法求解轉換參數。

設相機坐標系和激光跟蹤儀坐標系的位姿轉換矩陣為Tr,

.

(8)

由靶點分別在兩坐標系中的坐標可得

.

(9)

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

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

(10)

其中I為單位陣，

.

(11)

,(i=1,2,3).

(12)

其中，

.

(13)

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

.

(14)

由式(14)可知，n對公共點對應3n個觀測方程，將其寫成矩陣形式為

(15)

其中

，

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

(16)

當n=3時，此時沒有多于觀測量，直接求解D，那么：

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

(17)

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

A·(D0+ΔD)=l.

(18)

進而求改正值，

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

(19)

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

3 算例和分析

3.1 數據采集與預處理

測量過程使用的工業相機為basler ACA 2500-20 gm，鏡頭為computer m0814-MP，CCD成像個數為(2 592像素×2 048像素)，像素大小為 0.004 8 mm×0.004 8 mm。首先對相機內參數進行標定，利用教研室標定場，采用基于有限元模型的數字工業攝影測量相機二次檢校的方法檢校相機[10]，標定結果為: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；使用的激光跟蹤儀為Leica AT901-B型,配合CCR1.5角隅棱鏡靶球,在2.5 m×5 m×10 m的工作范圍內三維點坐標測量精度為±(15 μm+6 μm/m)。

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

對采集到圖像首先利用Opencv中Canny算子對標志進行檢測邊緣，并用橢圓擬合算法求出圓形標志的中心坐標，此時坐標為圖像像素坐標系下，需由式(1)轉換為像平面坐標系下，結果如表1所示。

表1 橢圓擬合得到的標志中心坐標 mm

利用激光跟蹤儀和靶球分別測量各個靶座上的中心坐標，得到坐標結果如表2所示。

表2 跟蹤儀坐標系下各靶座中心坐標 mm

通過測量和預處理，表1為經過預處理后的數據，表2為激光跟蹤儀直接測量所得到的數據。在所采集數據基礎上即可進行坐標系轉換和精度分析。

3.2 坐標系轉換和精度分析

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

表3 靶點在相機坐標系下的坐標 mm

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

(20)

為了驗證坐標系轉換精度，本文利用矩陣T及共線方程反算像點坐標，將結果與橢圓擬合所得結果進行比較，比較結果如表4所示，

(21)

表4僅僅為相機在某一個位姿下的標定精度，為了說明算法的普適性，將相機調整位姿繼續拍攝兩張照片，標定結果如表5和表6所示。

綜合表4、表5和表6得計算結果，位姿標定模

表4 坐標反算結果與橢圓擬合坐標對比表 mm

表5 相機位姿一次調整后的標定結果 mm

表6 相機位姿二次調整的標定結果 mm

4 結 論

在六自由度精密測量中，針對激光跟蹤儀與視覺測量聯合應用中的相機與激光跟蹤儀相對位姿的標定問題，本文利用基于羅德里格矩陣的方法標定了兩者之間的相對位姿以及利用共線方程反算標志中心坐標驗證了轉換精度。實驗結果證明，所得相機坐標系與激光跟蹤儀相對位姿模型總體精度保持在0.01 mm以下，基本滿足了激光跟蹤儀與相機聯合精密測量中的需要。