專利名稱：可變焦攝像機空間三維位姿的單視測量方法
技術領域：
本發明涉及一種可變焦攝像機空間三維位姿的單視測量方法，可實施在需要實現機器視覺導引的各系統如工業自動裝配機器人、水下機器人、無人飛行器、空間交互自動對接等，屬于自動化領域。
背景技術：
在實現無人機自主著艦、水下機器人、空間交互自動對接等智能機器系統中，實現機器系統的自主導引意義重大，其中導引控制信號的一部分主要來自于機器系統相對于合作目標的絕對位置和方向信息。在現有的導引方法中，以計算機視覺為理論基礎，具有大視場、非接觸及精度適中的視覺導引系統，尤其適合于估計機器系統相對于合作目標的位置姿態。其基本原理是，將攝像機裝載在機器系統上，由平面合作目標上的特征及特征在平面合作目標圖像上對應的信息，根據攝像機透視投影模型，適時估計出機器系統上攝像機相對于合作目標的位姿，由此可知，視覺導引的關鍵問題是實時獲得攝像機相對合作目標的位置姿態。在工程應用中，自動控制系統根據視覺導引系統獲得的導引信號，來控制機器系統相對于合作目標的相對運動。因此，為保證視覺導引系統中的攝像機與合作目標三維位置姿態估測的精確性，攝像機在采集圖像時，需要不斷地調整攝像機與合作目標的距離，改變攝像機的焦距，使得合作目標在成像畫面上含有3個正方形圖案且近乎充滿畫面，這就涉及到可變焦攝像機相對于合作目標的實時三維位置姿態的測量。
目前，一種被廣泛采用的非接觸式導引方法是基于特征點的攝像機三維位置姿態測量方法。2001年，sharp C S等人在“一種無人航空飛行器視覺著陸系統”(A vision system for landingan unmanned aerial vehicle)IEEE會議錄《機器人和自動化國際會議》，第21-26頁，2001年(Proceedings of 2001 IEEE，International Conference of Robotics and Automation，2001)提出了以5個小四邊形和一個大四邊形為合作目標的基于特征點的攝像機位置姿態測量方法。由于四邊形頂點的圖像特征容易受到光照、噪聲影響，使得提取精度下降，因此該方法的可靠性和使用范圍受到很大限制。鑒于此，《一種攝像機空間三維位置姿態測量方法》(申請號200510000274.1)提出了一種基于雙圓平面合作目標的攝像機三維位置姿態實時測量方法，但該方法只適用于定焦距攝像機的標定，其標定測量須要求攝像機或平面合作目標至少一方靜止，平面合作目標至少移動5個位置，拍攝5幅目標圖像，測量效率低；在基于單幅平面合作目標圖像的三維位姿的測量中，有人提出了《僅用一幅平面標定物圖像的攝像機標定方法》(申請號200510111518.3)，及《基于平行線的單視平面測量方法》(申請號01140336.5)，但這兩種方法都必須已知平面合作目標圖案的物理尺寸參數。而又有人提出的《基于單幅圖像的平面測量方法》(申請號01140335.7)，同樣只適用于定焦距攝像機，且無法確定攝像機的姿態和方位。
發明內容
本發明一種可變焦攝像機空間三維位姿的單視測量方法，其目的是采用參數未知的一組由3個正方形組成的圖案作為平面合作目標，只需拍攝一幅平面合作目標圖像，根據主點的查找表，來解決可變焦攝像機的實時三維位置姿態的測量問題。
本發明一種可變焦攝像機空間三維位姿的單視測量方法，其特征在于對畸變平面合作目標圖像的校正和攝像機相對于平面合作目標的標定兩個階段的處理，具體步驟如下1設定平面合作目標設定參數未知的一組由3個正方形組成的圖案作為平面合作目標。
2拍攝平面合作目標圖像在空間某一位置(拍攝時攝像機光軸與平面合作目標法線的夾角在20°至50°之間)，用調焦后的可變焦攝像機拍攝一幅含有3個正方形圖案且近乎充滿畫面的平面合作目標圖像。
3對平面合作目標圖像的處理流程，具體步驟如下3.1基于亞象素邊緣線點的平面合作目標圖像輪廓的快速畸變校正方法3.1.1基于邊緣線點的畸變參數標定提取平面合作目標圖像的亞象素邊緣線點，并將這些線點作為畸變參數標定的輸入，同時根據直線透視投影的不變性來估計鏡頭的一階徑向畸變參數；3.1.2基于平面合作目標圖像輪廓的幾何畸變校正利用攝像機畸變參數的最優值，針對于輪廓簡單的平面合作目標圖像，根據攝像機鏡頭的畸變模型及其反畸變模型，對畸變平面合作目標圖像的輪廓部分進行幾何校正。
3.2對平面合作目標圖像中方形P的角點自動排序復制原平面合作目標圖像，用Harris方法提取該平面合作目標圖像的角點，根據平面合作目標的特點，借助內部白色正方形A的四個角點，對黑色正方形P的四個角點進行自動排序。
3.3自動提取校正后平面合作目標圖像的亞象素邊緣直線參數的根據排序后的角點，在校正后的平面合作目標圖像輪廓上，分隔出進行亞像素邊緣提取的區域，然后采用精度為1/10像素的Hessian矩陣法提取這一區域的亞像素邊緣直線線點，擬合出所需直線的參數。
4基于主點查找表的可變焦攝像機空間三維位姿的單視測量方法4.1主點查找表的建立讓攝像機從最小焦距開始等步距變焦，直至變焦到最大，在每個焦距位置(用Tsai的方法或張正友的方法等)離線測得該焦距下攝像機的主點，建立主點查找表。
4.2可變焦攝像機空間三維位姿的單視測量方法利用主點查找表，根據變焦時間確定該焦距下的攝像機主點，由第3.3所得的邊緣直線參數以及與其對應的平面合作目標的直線參數，根據攝像機透視投影模型，通過線性變換求出攝像機的焦距和相對于平面合作目標的外部參數，完成攝像機相對于平面合作目標的三維位姿的單視測量。
本發明一種可變焦攝像機空間三維位姿的單視測量方法，其優點是與現有技術相比，本發明采用參數未知的一組由3個正方形組成的圖案作為平面合作目標，攝像機可實時變焦以保證調焦后拍攝的平面合作目標圖像含有3個正方形圖案且近乎充滿畫面，利用基于亞象素邊緣線點的快速畸變校正方法可對平面合作目標圖像輪廓進行快速幾何畸變校正。提取校正后平面合作目標圖像輪廓的亞像素邊緣直線參數，根據主點查找表，即可實現可變焦攝像機空間三維位姿的單視測量。
圖1為本發明所采用的平面合作目標。
圖2(a)為一幅平面合作目標圖像。
圖2(b)為圖2(a)所示平面合作目標圖像的輪廓。
圖2(c)為圖2(a)所示平面合作目標圖像的亞像素邊緣線點。
圖2(d)為校正后的無畸變平面合作目標圖像輪廓。
圖2(e)為平面合作目標圖像(圖2(a))的角點。
圖2(f)為排序后的角點在平面合作目標圖像中的順序。
圖3為校正圖像輪廓的流程圖。
圖4由平面合作目標圖像獲取攝像機三維位姿的示意圖。
圖5為實現可變焦攝像機三維位姿單視測量的總體流程圖。
圖中英文字母標注如下S為平面合作目標圖案中最外邊的白色正方形。
P為平面合作目標圖案中的黑色正方形。
A為平面合作目標圖案中位于黑色正方形P內部的白色正方形。
XW為世界坐標系的X軸。
YW為世界坐標系的Y軸。
OW為世界坐標系的原點。
p1、p2、p3、p4為方形P圖像角點排序后的順序。
Xu無畸變圖像坐標系的X軸。
Yu無畸變圖像坐標系的Y軸。
Xd畸變圖像坐標系的X軸。
Yd畸變圖像坐標系的Y軸。
xd畸變圖像輪廓中像點的橫坐標。
yd畸變圖像輪廓中像點的縱坐標。
xu根據畸變模型，與(xd，yd)對應的無畸變圖像像點的橫坐標。
yu根據畸變模型，與(xd，yd)對應的無畸變圖像像點的縱坐標。
xu+i為以(xu，yu)為中心5×5鄰域中第i行第j列像點的橫坐標。
yu+j為以(xu，yu)為中心5×5鄰域中第i行第j列像點的縱坐標。
xd′根據反畸變模型，與(xu+i，yu+j)對應的畸變圖像中像點的橫坐標。
yd′根據反畸變模型，與(xu+i，yu+j)對應的畸變圖像中像點的縱坐標。
Oc攝像機坐標系的原點。
Xc攝像機坐標系的X軸。
Yc攝像機坐標系的Y軸。
Zc攝像機坐標系的Z軸。
l1平面合作目標的直線1。
l2平面合作目標的直線2。
l3平面合作目標的直線3。
l4平面合作目標的直線4。
lm1經透視投影后，平面合作目標直線1在目標圖像上的投影。
lm2經透視投影后，平面合作目標直線2在目標圖像上的投影。
lm3經透視投影后，平面合作目標直線3在目標圖像上的投影。
lm4經透視投影后，平面合作目標直線4在目標圖像上的投影。
R攝像機相對于平面合作目標三維位姿的旋轉矩陣。
t攝像機相對于平面合作目標三維位姿的平移矢量。
具體實施例方式可變焦攝像機三維位姿單視測量的總體流程，如圖5所示。為了更好的理解本發明的技術方案，以下結合附圖和實施例作進一步的詳細描述。
本發明一種可變焦攝像機空間三維位姿的單視測量方法，包括以下步驟1.設定平面合作目標設定參數未知的一組由3個正方形組成的圖案作為平面合作目標，如圖1所示。該平面合作目標的特征圖案需滿足下列要求(1)相對于其周邊環境，該特征圖案應易于識別，并且構圖簡單以易于視覺系統能夠快速地進行圖像處理；(2)圖案包含了足夠的信息，如點、線或區域可使視覺系統利用這些信息計算出視覺導引所需的相對位姿信息；(3)特征圖案在空間上具有非對稱性，可提供對攝像機導引的指向要求。
最外邊的白色正方形S將黑色正方形P包圍起來并與之同中心(見圖1)，其功用可簡化平面合作目標特征圖案與周邊環境的圖像分割，平面合作目標特征圖案由黑色正方形P及白色正方形A組成。建立的世界坐標系OwXwYwZw定義為正方形P的中心為原點Ow，Xw軸水平沿正方形A方向延伸，Yw軸與Xw垂直，Zw軸由右手法則確定。
2.拍攝平面合作目標圖像在空間某一位置(拍攝時攝像機光軸與平面合作目標法線的夾角應在20°至50°之間)，用調焦后的可變焦攝像機拍攝平面合作目標的一幅含有3個正方形圖案且近乎充滿畫面的圖像，如圖2(a)所示。
3 對平面合作目標圖像的處理流程，具體步驟如下3.1 基于亞象素邊緣線點的平面合作目標圖像輪廓的快速畸變校正方法由于光學鏡頭畸變的存在，致使由圖2(a)所示的平面合作目標圖像存在幾何失真，因此根據直線透視投影的不變性，采用基于亞象素邊緣線點的快速畸變校正方法，對平面合作目標圖像進行幾何校正，具體分為以下兩步驟
3.1.1基于亞象素邊緣線點的畸變參數標定首先提取平面合作目標圖像的輪廓(圖2(b))及亞象素邊緣線點(圖2(c))所示，以每一條邊緣直線上像點的畸變圖像坐標Dij作為畸變參數標定的輸入，其中1≤i≤4，1≤j≤ni，第i條邊緣直線共有ni個共線點。若只考慮一階徑向畸變，攝像機的多項式畸變模型為xu=xd(1+k1rd2)yu=yd(1+k1rd2)---(1)]]>其中(xd，yd)為畸變圖像坐標，(xu，yu)為無畸變圖像坐標，rd=xd2+yd2.]]>根據式(1)可得到無畸變圖像坐標Uij，擬合無畸變圖像坐標得到直線Li，記點Uij到直線Li的距離為|Uij-Li|，建立鏡頭畸變參數標定的目標優化函數F(k1)=Σi=14Σj=1ni|Uij-Li|---(2)]]>采用Levenberg-Marquardt(列文伯格-馬夸爾特法)方法對式(2)表示的目標函數進行非線性優化，可以估計出鏡頭一階徑向畸變參數k1的值。
3.1.2基于平面合作目標圖像輪廓的幾何畸變校正在已知攝像機畸變參數最優值k1的情況下，畸變圖像的校正包括空間幾何變換和灰度校正，空間變換又包括直接變換和間接變換。針對如圖2(a)所示輪廓簡單的平面合作目標圖像，采用先直接后間接的空間變換方式來建立坐標映射關系。其基本原理是，將畸變圖像輪廓中的像點(xd，yd)作為輸入，根據畸變模型式(1)，計算出與之對應的無畸變圖像坐標(xu，yu)；但由于直接變換在桶形畸變圖像的拉伸放大區域存在空像點，因此在其反變換中，為填補這些空像點，需要由點(xu，yu)及其5×5鄰域中每一個像點(xu+i，yu+j)出發，根據反畸變模型，反算出所對應的畸變圖像坐標(xd′，yd′)，其中-2≤i≤2，-2≤j≤2。然后利用雙線性插值進行灰度校正，其校正流程如圖3所示，校正后的平面合作目標圖像輪廓如圖2(d)所示。在畸變校正過程中，反畸變模型是根據畸變模型經過變換建立的，具體變換如下首先令ru=xu2+yu2---(3)]]>
將式(1)代入式(3)，得ru=(1+k1*rd2)*xd2+yd2=(1+k1*rd2)*rd---(4)]]>則式(1)變換為xd=xu·rd/ruyd=yu·rd/ru---(5)]]>式(5)即為所求的反畸變模型。其中rd是一元三次方程k1rd3+rd-ru=0]]>的解。
3.2對平面合作目標圖像中方形P角點進行自動排序復制平面合作目標原圖像(圖2(a))，用Harris方法提取該圖像的角點，提取結果如圖2(e)所示，觀察平面合作目標圖案(圖2(f))可發現在方形P的四個角點中，角點p4與平面合作目標圖像中方形A最近，但是由于透視投影變換，無法保證角點p4在任何條件下都一定與方形A的中心距離最近，因此借助內部白色正方形A的四個角點，對黑色正方形P的四個角點進行排序，具體步驟如下距離新定義定義方形P圖像的任一角點(xip，yip)到連通域A的距離Di為到A中的每一個角點(xiA，yiA)距離中的最近距離，數學表達式如下Di=min((xiP-xjA)2+(yiP-yjA)2)]]>(j＝1…4)(6)(1)確定起始角點分別計算方形P圖像的每個角點與方形A圖像的四個角點的距離，記其中的最小值為該角點與方形A圖像的距離，再比較四個距離值，距離最小者即為角點p4與方形A圖像的距離D4，即D4＝minDi(i＝1，...，4)(7)由此，在任何透視投影變換的情況下，可唯一確定出方形P圖像的四個角點中的角點p4，并以之為起始角點。
(2)坐標轉換將探測出來的方形P圖像的四個角點的坐標從直角坐標系轉化為極坐標系表示。定義極坐標系為選取方形P圖像的四個角點的直角坐標中心O為極點，極軸選O到方形P圖像的角點p4間的連線，則方形P圖像的四個角點可表示為
XiP(ρi,θi)(i=1...4)---(8)]]>(3)確定角點排序將方形P圖像的四個角點XiP(ρi，θi)(i＝1...4在極坐標系中的角度θi歸一到0°～360°之間，并按角度增加的順序排列，也即按逆時針方向排列各角點，則可知θ＝0°，θ1＜θ2＜θ3。于是，方形P圖像的四個角點的排序不論在任何透視投影變換的情況下都可確定。圖2(f)表現了其中一種角點排序的結果。
3.3自動提取校正后平面合作目標圖像輪廓的亞象素邊緣直線參數根據排序后的角點，在校正后的平面合作目標圖像輪廓上，分隔出進行亞像素邊緣提取的區域，然后采用精度為1/10像素的Hessian矩陣法提取這一區域的亞像素邊緣直線線點，擬合出所需直線的參數。同時利用排序后的角點對方形P的4條直線進行排序。
4.基于主點查找表的可變焦攝像機空間三維位姿的單視測量方法4.1主點查找表的建立由于在攝像機空間三維位姿的單視測量中，將變焦過程中的主點變化視為已知，因此要預先建立主點的查找表。讓攝像機從最小焦距開始等時間間隔地移動鏡頭，實現攝像機的等步距變焦，在每個焦距位置(用Tsai的方法或張正友的方法等)離線測得該焦距下攝像機的主點，建立變焦時間與主點值之間的對應關系——主點查找表。
4.2可變焦攝像機空間三維位姿的單視測量方法本發明使用的攝像機模型為經典的透視投影模型(如圖4)，以鏡頭的光心為攝像機坐標系的原點，光軸為攝像機坐標系的Zc軸，使XcYc平面與圖像平面平行，以平面合作目標所在的平面為Zw＝0平面，則平面合作目標上的點(X,Y,0)到圖像點的投影關系為λuv1=HXY1---(9)]]>其中H為單應性矩陣
H=h11h12h13h21h22h23h31h32h33=fur11+u0r31fur12+u0r32futX+u0tZfur21+u0r31fur22+u0r31futY+u0tZr31r32tZ---(10)]]>fu、fv分別是平面合作目標圖像上u、v兩個方向的焦距，r11、r21、r31、r12、r22、r32是旋轉矩陣前兩列的元素，tX、tY、tZ是外部參數中的三個平移量。
根據式(9)所示的攝像機透視投影模型，利用第3.3所得的校正后平面合作目標圖像的四條邊緣直線參數lmiv＝aiu+bi，(i＝1，2，3，4)，以及與其對應的平面合作目標上黑色正方形P上的四條直線參數liY＝AiX+Bi，i＝1，2，3，4，其中，ai、bi分別為平面合作目標圖像邊緣直線的斜率及截距，Ai、Bi分別為平面合作目標中直線的斜率及截距，再通過對單應性矩陣H的線性變換，可得如式(11)所示的線性方程M·G＝B (11)其中令η＝h33，G＝[h11/η h12/η h13/η h21/η h22/η h23/η h31/η h32/η]T B＝
T，由于矩陣M是滿秩矩陣，因此方程(11)有唯一解，單應性矩陣H取決于系數η。將所求得的單應性矩陣H轉換成如式(12)所示的矩陣形式
10u000000000010u000000000010u001v000000000001v000000000001v0001000000000001000000000001·fur11fvr21r31fur12fvr22r32futxfvtytz=z---(12)]]>其中Z＝[ηg11，ηg12，ηg13，ηg21，ηg22，ηg23，ηg31，ηg32，η]T，由于等號左邊的系數矩陣是滿秩矩陣，所以fur11，fvr21，r31，fur12，fvr22，r32，futx，fvty，tz的解是解析線性的。
令fur11＝k1η，fvr21＝k2η，r31＝k3η，fur12＝k4η，fvr22＝k5η，r32＝k6η，futX＝k7η，fvty＝k8η，tz＝k9η則r11=k1ηfu,r21=k2ηfv,]]>r31＝k3ηr12=k4ηfu,r22=k5ηfv,]]>r32＝k6η根據旋轉矩陣的標準正交性質，即可得出fu，fv，η以及攝像機的外部參數。至此，可變焦攝像機的即時焦距和其相對于平面合作目標的三維位姿即可獲得。
權利要求
1.一種可變焦攝像機空間三維位姿的單視測量方法，其特征在于該方法包括如下步驟(1)設定平面合作目標設定參數未知的一組由3個正方形組成的圖案作為平面合作目標；(2)拍攝平面合作目標圖像在空間某一位置，用調焦后的可變焦攝像機拍攝一幅含有3個正方形圖案且近乎充滿畫面的平面合作目標圖像，拍攝時攝像機光軸與平面合作目標法線的夾角在20°至50°之間；(3)對平面合作目標圖像的處理流程，具體步驟如下(3.1)基于亞象素邊緣線點的平面合作目標圖像輪廓的快速畸變校正方法(3.1.1)基于邊緣線點的畸變參數標定提取平面合作目標圖像的亞象素邊緣線點，并將這些線點作為畸變參數標定的輸入，同時根據直線透視投影的不變性來估計鏡頭的一階徑向畸變參數；(3.1.2)基于平面合作目標圖像輪廓的幾何畸變校正利用攝像機畸變參數的最優值，針對于輪廓簡單的平面合作目標圖像，根據攝像機鏡頭的畸變模型及其反畸變模型，對平面合作目標圖像的輪廓部分進行幾何畸變校正；(3.2)平面合作目標圖像中方形P角點的自動排序復制平面合作目標原圖像，用Harris方法提取該圖像的角點，根據平面合作目標的特點，借助內部白色正方形A的四個角點，對黑色正方形P的四個角點進行自動排序；(3.3)自動提取校正后平面合作目標圖像輪廓的亞象素邊緣直線參數根據排序后的角點，在校正后平面合作目標圖像的輪廓上，分隔出進行亞像素邊緣提取的區域，采用精度為1/10像素的Hessian矩陣法提取這一區域的亞像素邊緣直線線點，擬合出所需直線的參數；(4)基于主點查找表的可變焦攝像機空間三維位姿的單視測量方法(4.1)主點查找表的建立讓攝像機從最小焦距開始等步距變焦，直至變焦到最大，在每個焦距位置離線測得該焦距下攝像機的主點，建立主點查找表；(4.2)可變焦攝像機空間三維位姿的單視測量方法利用主點查找表，根據變焦時間確定該焦距下的攝像機主點，由第3.3所得的邊緣直線參數以及與其對應的平面合作目標的直線參數，根據攝像機透視投影模型，通過線性變換求出攝像機的焦距及相對于平面合作目標的外部參數，完成攝像機相對于平面合作目標的三維位姿的單視測量。
全文摘要
本發明為一種可變焦攝像機空間三維位姿的單視測量方法，該方法是設定一組由3個正方形圖案組成的平面合作目標；在空間某一位置，拍攝一幅平面合作目標圖像；對平面合作目標圖像進行處理，處理流程是①由于光學鏡頭畸變的存在，致使所拍攝的平面合作目標圖像存在幾何失真，所以，首先采用基于亞象素邊緣線點的快速畸變校正方法來對平面合作目標圖像的輪廓進行幾何畸變校正。②然后，提取校正后的平面合作目標圖像輪廓的亞像素邊緣直線參數；采用主點查找表的方式，根據攝像機透視投影模型，通過線性變換求出攝像機的焦距及外部參數，完成可變焦攝像機相對于平面合作目標的三維位姿的單視測量。
文檔編號H04N5/225GK101038163SQ200710063678
公開日2007年9月19日 申請日期2007年2月7日 優先權日2007年2月7日
發明者王睿, 張廣軍, 王茂義, 李欣 申請人:北京航空航天大學