雙目視覺測量系統位姿解算方法研究

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

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

1 引言

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

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

2 自動對準測量系統概述

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

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

圖1 自動對準雙目位姿測量系統

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

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

圖2 自動對準雙目位姿測量系統示意圖

(1)

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

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

已知目標坐標系下的點P2i，將其轉換到相機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視場范圍內的點滿足透視投影規則，可直接套用OI算法，為了與相機CB的相應計算公式保持一致，將相機CA視場范圍內點的視線投影矩陣及目標空間共線性誤差重寫為：

(8)

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

(9)

因此，算法的目標函數為：

(10)

對于給定的R，平移矩陣t關于R的函數可以表示為

(11)

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

(12)

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

(13)

(14)

(15)

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

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

(16)

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

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

4 迭代初始值的求解

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

圖4 坐標變換過程

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

(17)

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

(18)

要求的變換矩陣

(19)

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

(20)

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

5 仿真研究

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

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

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

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

6 結論

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