雙目視覺測量系統(tǒng)位姿解算方法研究

李勤文，王志乾，楊文昌，沈鋮武

(1.中國科學院長春光學精密機械與物理研究所，吉林 長春 130033；2. 中國科學院大學，北京 10049)

1 引言

工業(yè)發(fā)展自動化、數(shù)字化水平的提高促進了自動對準技術(shù)的發(fā)展[1，2]，姿態(tài)測量技術(shù)作為自動對準系統(tǒng)的關(guān)鍵技術(shù)，主要用于定位被對準部件或直接測量和定位各種工藝設(shè)備，是自動調(diào)姿對準系統(tǒng)的重要組成部分[3]。基于點特征的單目視覺測量方法是三維姿態(tài)測量的基本方法，它通過n個滿足透視投影關(guān)系的三維特征點和它們的二維投影圖像點來計算位姿，又稱為“透視n點問題”(PnP問題)[4，5]。目前針對單目視覺的位姿解算方法已比較成熟，但在實際應(yīng)用中直接采用單目相機往往會由于視場及拍攝角度的限制而導(dǎo)致獲取的信息量少。近年來隨著相機價格的降低和計算機性能的提高，利用多個相機來擴大視場范圍和提高測量精度的方法得到了廣泛的研究應(yīng)用，如大型體育賽事，自動生產(chǎn)裝配和國防軍事領(lǐng)域等[6]。在利用多個相機拍攝的特征點信息對物體在空間中的位姿進行解算的過程中，由于目標的三維特征點和其對應(yīng)的圖像點之間不再滿足透視投影規(guī)則，傳統(tǒng)的PnP求解方法不再適用，因此要考慮多個相機拍攝信息的轉(zhuǎn)換問題[7，8]，將不同相機拍攝的信息轉(zhuǎn)換到統(tǒng)一的坐標系下進行表示，然后利用位姿解算算法的思想進行求解。

本文針對大部件雙目自動對準系統(tǒng)的位姿解算問題，利用單目視覺OI迭代算法[9]思想最小化四個共面特征點的目標空間共線性誤差求解旋轉(zhuǎn)矩陣和平移矩陣，針對算法在特征點數(shù)目較少時迭代誤差函數(shù)陷入局部極小值的問題，利用各坐標系之間的轉(zhuǎn)換關(guān)系構(gòu)造相機成像方程，求解迭代初始值進行迭代，并通過仿真驗證了本文方法的有效性。

2 自動對準測量系統(tǒng)概述

本文主要針對如圖1所示的自動對準系統(tǒng)布置雙目相機進行位姿測量，利用位姿解算方法計算固定部件在空間中的位姿，測量對準偏差，調(diào)整移動部件直到完成對準。在固定部件和移動部件上分別布設(shè)兩個合作靶標和兩個光學測量相機AA、CB，兩個靶標共面且平行于固定部件對接面，每個靶標上設(shè)置兩個合作特征點，為保證在最終對接階段兩相機依然能觀測到每個靶標上的全部特征點，兩個靶標安裝在固定部件對接面后方100mm處。兩個光學相機的位置關(guān)系及內(nèi)參數(shù)已標定。開始時人工粗對準至光學可測范圍內(nèi)，然后切換至自動控制。因為要討論近距離測量對準的情況，所以設(shè)定固定部件和移動部件之間僅有小范圍的對準偏差，即兩部件之間的旋轉(zhuǎn)角α′，β′，γ′∈[-5°，5°]，平移距離ΔX′，ΔY′∈[-200mm，200mm]，ΔZ′∈[0，200mm]。

由圖1，移動部件與固定部件之間的位姿偏差可以通過對準坐標系O′-X′Y′Z′到目標坐標系O-XYZ之間的轉(zhuǎn)換關(guān)系進行表示，因此測量的主要目的便是求解兩坐標系之間的轉(zhuǎn)換矩陣。通過將兩相機拍到的點轉(zhuǎn)換到基準相機CA坐標系進行位姿解算，可以求得CA坐標系與目標坐標系O-XYZ之間的轉(zhuǎn)換關(guān)系，再利用已知的相機CA坐標系到對準坐標系O′-X′Y′Z′之間的轉(zhuǎn)換關(guān)系可得到兩部件之間的位姿偏差。其中解算基準相機CA坐標系與目標坐標系O-XYZ之間的轉(zhuǎn)換關(guān)系是求解的關(guān)鍵，所以本文主要研究如何求得相機CA坐標系到目標坐標系O-XYZ之間的轉(zhuǎn)換關(guān)系，即兩坐標系之間的旋轉(zhuǎn)角α，β，γ和平移距離ΔX，ΔY，ΔZ。最后，利用相機CA坐標系到對準坐標系O′-X′Y′Z′之間的轉(zhuǎn)換關(guān)系便可得到兩部件之間的旋轉(zhuǎn)角偏差α′，β′，γ′和平移偏差ΔX′，ΔY′，ΔZ′。

圖1 自動對準雙目位姿測量系統(tǒng)

3 雙目位姿解算的迭代算法

為了便于分析，將雙目視覺測量系統(tǒng)簡化為圖2形式，選取相機CA為基準相機，P1i=(X1i，Y1i，Z1i)T，P2i=(X2i，Y2i，Z2i)T(i=1，2)分別表示相機CA、CB視場范圍內(nèi)的點在目標坐標系下的坐標，ATO表示要求的從目標坐標系O-XYZ到基準相機CA坐標系的轉(zhuǎn)換矩陣，它是由兩坐標系之間的旋轉(zhuǎn)矩陣R和平移矩陣t構(gòu)成的四階方陣。ATB表示已標定的從相機CB到相機CA坐標系的轉(zhuǎn)換矩陣，令p1i=(u1i，v1i，1)T，p2i=(u2i，v2i，1)T表示點的齊次圖像坐標，將兩個相機上點的圖像坐標轉(zhuǎn)換到它們的歸一化像平面上，得

圖2 自動對準雙目位姿測量系統(tǒng)示意圖

(1)

由于本文采用的是雙目相機，點的圖像坐標與目標坐標系下的坐標不再滿足單相機透視投影規(guī)則，因此不能直接采用單目視覺位姿解算的OI迭代算法，需要將相機坐標系下的點通過向量坐標變換轉(zhuǎn)換到基準相機坐標系下，如圖3所示，然后再套用OI算法進行解算。

圖3 相機CB到基準相機CA坐標變換示意圖

已知目標坐標系下的點P2i，將其轉(zhuǎn)換到相機CA坐標系下表示，得

Q2i=RP2i+t

(2)

=RP2i+t-AtB

(3)

(4)

(5)

RP2i+t-AtB=V2i(RP2i+t-AtB)

(6)

故對相機CB來說，目標空間共線性誤差為：

eB=(I-V2i)(RP2i+t-AtB)

(7)

由于選擇相機CA為基準相機，相機CA視場范圍內(nèi)的點滿足透視投影規(guī)則，可直接套用OI算法，為了與相機CB的相應(yīng)計算公式保持一致，將相機CA視場范圍內(nèi)點的視線投影矩陣及目標空間共線性誤差重寫為：

(8)

eA=(I-V1i)(RP1i+t-AtA)

(9)

因此，算法的目標函數(shù)為：

(10)

對于給定的R，平移矩陣t關(guān)于R的函數(shù)可以表示為

(11)

將式(11)代入式(10)并化簡，得

(12)

這是一個一元函數(shù)的最小化問題，給定旋轉(zhuǎn)矩陣R的初值R0，假設(shè)R的第k次估計值為Rk，則R的下一次估計值Rk+1為

(13)

(14)

(15)

Mk的SVD分解為(Uk，Σk，Vk)，為了保證求得的旋轉(zhuǎn)矩陣R不會出現(xiàn)det(Rk+1)=-1的情況，需要對傳統(tǒng)的SVD求解方法進行校正，校正后的解為[10]

Rk+1=Vkdiag(1，1，det(Vk(Uk)T))(Uk)T

(16)

得到Rk+1后，平移向量t的下一次估計值tk+1可以由式(11)求得，然后繼續(xù)進行迭代直至誤差E收斂，可以得到目標坐標系O-XYZ和相機CA坐標系之間的旋轉(zhuǎn)矩陣R和平移矩陣t，進而利用已知的相機CA坐標系和對準坐標系O′-X′Y′Z′之間的轉(zhuǎn)換關(guān)系求得固定部件與移動部件之間的對準偏差。

一般情況下，非線性優(yōu)化算法都不能保證誤差函數(shù)E一定收斂，取決于初值的選取。優(yōu)化算法有可能最終收斂到局部極小值，或者在有限的時間內(nèi)根本不收斂。本文介紹的OI迭代算法雖然在單目視覺解算時可保證全局收斂性，但應(yīng)用于雙目視覺解算時，在特征點數(shù)目較少時會出現(xiàn)迭代誤差E收斂到局部極小值的情況。為了解決這個問題，要考慮迭代初始值的選取問題，通過數(shù)值解算方法求出初始旋轉(zhuǎn)矩陣作為迭代初值進行迭代，保證解的準確性。

4 迭代初始值的求解

令P=(X，Y，Z，1)T表示目標坐標系下點的齊次坐標，Q=(x，y，z，1)T表示點在各自相機坐標系下的齊次坐標，p=(u，v，1)T表示點的齊次圖像坐標。各坐標之間的聯(lián)系可以表示為一系列的幾何變換，如圖4所示，其中KA，KB為已知的兩相機內(nèi)參數(shù)矩陣，BTA表示從相機CA到相機CB坐標系的轉(zhuǎn)換矩陣，且BTA=(ATB)-1。

圖4 坐標變換過程

基準相機CA視場范圍內(nèi)的點P11，P12在目標坐標系下的坐標與它們的圖像坐標滿足單相機透視成像規(guī)則，即

(17)

其中z1i為標量系數(shù)，i=1，2。為了建立兩相機成像之間的聯(lián)系，將相機CB視場范圍內(nèi)的點P21，P22先轉(zhuǎn)換到基準相機CA坐標系下，再利用已標定的兩相機之間的坐標轉(zhuǎn)換關(guān)系在CB坐標系下進行成像，最后利用已知的相機CB的內(nèi)參數(shù)矩陣KB建立點在相機坐標系下的坐標與圖像坐標之間的轉(zhuǎn)換，代數(shù)表達式為

(18)

要求的變換矩陣

(19)

共有六個未知數(shù)(α，β，γ，ΔX，ΔY，ΔZ)，式(17)，式(18)可分別提供三個等式方程，方程中含有未知的標量系數(shù)zji(j=1，2)，從Pji中任選三個點(如P11，P12，P21)列寫成像方程，得

(20)

其中BRA，BtA表示已標定的相機CA坐標系到相機CB坐標系的旋轉(zhuǎn)矩陣和平移矩陣。將式(20)展開可得由九個未知數(shù)(α，β，γ，ΔX，ΔY，ΔZ，z11，z12，z21)表示的九個方程，利用MATLAB求解方程，并利用測量范圍約束去掉不符合條件的值，可以求得旋轉(zhuǎn)矩陣R和平移矩陣t。由于三點位姿解算的解不唯一，可能出現(xiàn)1，2，3或4個解[11]，若在測量范圍內(nèi)出現(xiàn)多個解，選取令迭代誤差函數(shù)E最小的R，t作為迭代初值進行迭代，求得相機CA坐標系和目標坐標系之間的轉(zhuǎn)換關(guān)系A(chǔ)TO，進而利用已知的相機CA坐標系和對準坐標系之間的轉(zhuǎn)換關(guān)系O′TA求得固定部件與移動部件之間的對準偏差O′TO，O′TO可轉(zhuǎn)換為兩部件之間的旋轉(zhuǎn)角α′，β′，γ′和平移距離ΔX′，ΔY′，ΔZ′。

5 仿真研究

為了驗證算法的正確性，建立MATLAB仿真對自動對準系統(tǒng)的雙目姿態(tài)估計過程進行了仿真。仿真相機采用Nikon D700相機的相關(guān)參數(shù)，相機焦距為35mm， 分辨率為4256×2832，傳感器尺寸為36×23.9mm。兩相機之間的位置關(guān)系已知，選取相機CA為基準相機，目標坐標系和基準相機坐標系之間的實際旋轉(zhuǎn)角度和平移距離在各自的測量范圍內(nèi)隨機產(chǎn)生。

為了驗證算法的正確性，本文對比分析了理想情況下直接使用OI迭代算法思想對四個共面特征點進行位姿求解的結(jié)果和使用本文初始化后的算法進行位姿解算結(jié)果，如圖5、圖6所示。其中圖5為固定平移距離取值的情況下，在旋轉(zhuǎn)角α′，β′，γ′的取值范圍[-5。，5。]內(nèi)以1。為步長遍歷所有點得到的平移距離解算誤差；圖6為固定旋轉(zhuǎn)角取值的情況下，在平移距離ΔX′，ΔY′，ΔZ′的取值范圍內(nèi)以50mm為步長遍歷所有點得到的平移距離解算誤差。可以看出，直接使用迭代算法在某些點處會由于迭代誤差函數(shù)陷入局部極小值而使得平移距離解算誤差達到103mm，使用本文初始化后的算法求得的平移距離解算誤差小于10-9mm，證明了本文方法的有效性。

圖5 角度變化時的平移距離解算誤差

圖6 位移變化時的平移距離解算誤差

6 結(jié)論

通過上述仿真證明，本文針對自動對準系統(tǒng)所采用的雙目視覺位姿測量解算方法是有效的。該方法利用單目視覺位姿解算的OI迭代算法思想進行位姿解算，針對算法在特征點數(shù)目較少時迭代誤差陷入局部極小值的問題，利用各坐標系之間的轉(zhuǎn)換關(guān)系求解迭代初值，保證解的正確性。總之，本文針對大部件自動對準系統(tǒng)近距離觀測所采用的雙目視覺位姿測量解算方法可以有效解決大部件的空間定位問題，具有很強的應(yīng)用前景和實用價值。