專利名稱：一種空間平面物體位姿識別方法
技術領域：
本發(fā)明涉及圖像處理領域，特別是指一種空間平面物體位姿識別方法。

背景技術：
物體的位姿計算是機器視覺的基本任務之一，在視覺導航、機器人定位、物體識別、視覺監(jiān)控、工業(yè)測量等方面都有著廣泛而重要的應用。所謂位姿計算指的是，在攝像機已標定的條件下，通過空間三維基元如點或直線與圖像二維基元之間的對應關系，求得攝像機坐標系與物體坐標系之間的剛體變換關系，從而得到物體在攝像機坐標系中的位置和姿態(tài)，即通過對物體的位姿計算識別出物體在攝像機坐標系中的位置和姿態(tài)。
目前，常用的位姿計算方法主要分為兩大類一類是基于點的方法，這類方法可歸結為經(jīng)典的n點透視問題(PnP，perspective-n-point problem)，PnP的求解方法又可分為很多種，在已知3到5個三維點匹配的條件下，物體的位姿可以通過求解一個多項式方程組的方法得到；在已知6個三維點或者更多點匹配的條件下，物體的位姿可以通過求解線性或者非線性近似的方法得以解決。但是這類方法必須要在物點和像點匹配已知的條件下進行，如此計算物體位姿的限制條件比較苛刻。
另一類方法是基于幾何特征的方法，如這類方法中的某些方法是基于圓特征來求解空間平面物體的位姿，但是，這些方法只適合于幾何特征為圓的物體，適用的范圍受到限制，并且這類方法需要額外的點或者直線等特征來解決二義性問題。
另外，這兩類方法對于物體位姿的計算只能依據(jù)物體幾何特征的特殊點，如一個矩形的四個角點、一條直線的兩個端點等等。
由此可見，現(xiàn)有的一些空間平面物體的位姿計算必須在一些特定的條件下才能進行，適用的范圍受到了限制。


發(fā)明內(nèi)容
有鑒于此，本發(fā)明的主要目的在于提供一種空間平面物體位姿識別方法，能減弱對空間平面物體位姿計算的條件限制，進而擴展適用范圍。
為達到上述目的，本發(fā)明的技術方案是這樣實現(xiàn)的 本發(fā)明提供了一種空間平面物體位姿識別方法，建立基于弱透視模型的空間平面物體的物點與像點之間的變換關系，該方法包括 提取空間平面物體圖像的幾何特征的邊緣，得到邊緣像點集合，在空間平面物體的幾何特征上取點，得到物點集合，并根據(jù)所述物點集合和邊緣像點集合得到表征物點和像點變換關系的匹配矩陣； 給出物點與像點變換關系的初始變化估計； 根據(jù)所述物點集合、所述邊緣像點集合、所述初始變化估計和所述匹配矩陣，得到物點與像點的估計變換關系，并由所得到的估計變換關系和所述物點集合，得到與所述物點集合匹配的像點集合； 根據(jù)所述物點集合和像點集合得到空間平面物體的法向量在攝像機坐標系下的坐標和空間平面物體質(zhì)心在攝像機坐標系下的坐標； 根據(jù)所述空間平面物體的法向量得到空間平面物體在攝像機坐標系下的俯仰角和偏航角。
其中，步驟a中所述提取幾何特征的邊緣為利用Canny算子提取空間平面物體圖像的幾何特征的邊緣。
步驟b所述給出物點與像點變換關系的初始變化估計為利用隨機數(shù)發(fā)生器給出所述初始變化估計。
步驟c所述得到物點與像點的估計變換關系為利用確定退火算法得到物點與像點的估計變換關系。
步驟c進一步包括 c1、根據(jù)所述匹配矩陣的初始值設置確定退火算法的初始參數(shù)； c2、利用sinkhorn算法更新所述匹配矩陣； c3、根據(jù)更新后的匹配矩陣利用Gauss-Seidel迭代方法計算物點和像點的估計變換關系； c4、根據(jù)所述估計變換關系和所述物點集合，利用物點和像點的變換關系表達式得到與物點集合匹配的像點集合。
其中，步驟c2所述利用sinkhorn算法更新所述匹配矩陣包括 初始化所述匹配矩陣； 對所述匹配矩陣的每一行和每一列元素進行歸一化計算； 對所述匹配矩陣循環(huán)進行歸一化計算。
步驟d中所述得到法向量在攝像機坐標系下的坐標和空間平面物體質(zhì)心在攝像機坐標系下的坐標是利用迭代位姿算法得到空間平面物體的法向量在攝像機下的坐標和空間平面物體質(zhì)心在攝像機坐標系下的坐標。
步驟d進一步包括 d1、根據(jù)所述物點集合和所述像點集合，利用迭代位姿算法計算得到兩組位姿向量； d2、根據(jù)Tsai平面靶標標定的方法排除所述得到的兩組位姿向量中錯誤的一組位姿向量，得到正確的一組位姿向量； d3、根據(jù)所述正確的位姿向量得到空間平面物體的法向量在攝像機坐標系下的坐標和空間平面物體質(zhì)心在攝像機坐標系下的坐標。
本發(fā)明所提供的空間平面物體位姿識別方法，不需要任何物點與像點的先驗匹配信息，并且位姿的計算是基于平面點集，且與平面物體的幾何形狀無關，使平面物體位姿計算下的二義性問題通過Tsai平面靶標標定的方法得以解決。
由于不需要額外的特征信息，本發(fā)明的方法減弱了對空間平面物體位姿計算的條件限制，擴展了位姿計算的適用范圍，該方法在閉塞、混亂的環(huán)境下，以及存在不同級別的噪聲的情況下也能表現(xiàn)出較好的魯棒性，具有很好的應用價值。



圖1為本發(fā)明空間平面物體位姿識別方法的流程示意圖； 圖2為平面弱透視模型示意圖； 圖3為迭代位姿算法實現(xiàn)原理示意圖。

具體實施例方式 下面結合附圖和具體實施例對本發(fā)明的技術方案進一步詳細闡述。
本發(fā)明方案是在物點與像點的匹配未知的條件下進行的，適用于滿足弱透視的任何空間平面物體。如圖1所示，本發(fā)明空間平面物體位姿識別方法，主要包括以下步驟 步驟101，建立基于弱透視模型的空間平面物體的物點與像點之間的變換關系。
這里，建立基于弱透視的平面物點與像點的變換關系，變換{A，B}，其中A是2×2的仿射變換矩陣，B是2×1的向量，表示物體質(zhì)心的像點坐標。
通過建立物點與像點之間的變換關系，可以得到物點與像點的變換關系表達式。這樣，在物點已知的情況下，即可通過物點與像點的變換關系表達式得到與物點一一匹配的像點。
弱透視模型具體是這樣建立的 本發(fā)明適用于滿足弱透視的任何空間平面物體，所以，一般要求物體到攝像機的距離至少大于十倍的物體表面深度的變化，如果攝像機的視場比較小，而物體表面深度變化相對其到攝像機的距離很小的話，如物體到攝像機的距離是其表面深度變化的至少十倍，那么物體上各點的深度變化可以用固定的深度值z0近似，z0是物體質(zhì)心在光軸上的深度坐標。
假設攝像機的焦距為f，空間平面物體上任意一點P在攝像機坐標系下的坐標為(xc，yc，zc)T，(x，y)T代表點P在攝像機像平面上像點的坐標，則弱透視模型可以表示為公式(1) 可以表示為公式(1) 其中， 為放縮常數(shù)。
弱透視模型建立之后，對弱透視模型表達式，即公式(1)進行推導，可以得到物點與像點的變換關系表達式，具體方法如下 假如空間平面物體的幾何特征是圓，則設空間圓圓心在攝像機坐標系下的坐標為ow(xc0，yc0，zc0)，如圖2平面弱透視模型示意圖所示，以ow為原點，建立空間圓三維坐標系owxwywzw；設

為空間圓質(zhì)心，與空間圓圓心ow重合，以與像平面xoy平行的過空間圓質(zhì)心

的平面為

坐標面，

為坐標原點，建立三維坐標系

其中坐標軸


且方向一致。對于平面

上的任意點

設其在攝像機坐標系ocxcyczc下的坐標為(xc，yc，zc0)，在坐標系

下的坐標為

將帶入公式(1)，簡化公式(1)為公式(2) 其中，為放縮常數(shù)。
根據(jù)坐標系

與坐標系ocxcyczc之間的變換關系，可得到代入公式(2)，得到公式(3) 其中， 坐標系owxwywzw與

之間的旋轉(zhuǎn)變換關系可由公式(4)描述 其中，是正交矩陣。
空間圓平面xwowyw上的點有zw＝0，由公式(4)可得公式(5) 將公式(5)帶入公式(3)可得到物點與像點之間的變換關系，變換{A，B} 其中，A是2×2的仿射變換矩陣，B是2×1的向量，表示物體質(zhì)心的像點坐標，則從公式(6)可以得到空間圓質(zhì)心的像點坐標B＝s(xc0，yc0)T。
步驟102，提取空間平面物體圖像的幾何特征的邊緣，得到邊緣像點集合，在空間平面物體的幾何特征上取點，得到物點集合，并根據(jù)物點集合和邊緣像點集合得到表征物點和像點變換關系的匹配矩陣。
這里，如何提取空間平面物體圖像幾何特征的邊緣可以采用各種已有算法，下面以采用Canny算子提取的方法具體說明。利用Canny邊緣算子提取空間平面物體圖像幾何特征的邊緣，得到邊緣像點集合{pj}，1≤j≤g，g為邊緣像點的個數(shù)，且邊緣像點的個數(shù)由Canny算子決定；在物體平面幾何特征的邊緣上間隔取物點，得到物點集合{Pi}，1≤i≤h，h為物點的個數(shù)，h≥g，其中物點的取點采用現(xiàn)有技術且是隨機的，物點之間的間隔可以相等也可以不等，但要保證物點的個數(shù)大于等于邊緣像點的個數(shù)。
給定平面物點集合{Pi}，Pi＝(xwi，ywi)T，1≤i≤h；邊緣像點集合{pj}，pj＝(xj，yj)T，1≤j≤g，h≥g。公式(6)表示物點與像點間的變換關系，那么，物點集合{Pi}和邊緣像點集合{pj}的約束關系可以由公式(6)，即變換{A，B}表示。物點集合{Pi}中的每一個點最多只能匹配邊緣像點集合{pj}中的一個點，同時，邊緣像點集合{pj}中的每一個點也最多只能匹配物點集合{Pi}中的一個點，這是一個雙向約束優(yōu)化問題，可以用公式(7)表示
其中， 可以將公式(7)用矩陣Q表示為
根據(jù)物點集合與邊緣像點集合的雙向約束優(yōu)化，得到矩陣Qg×h；再根據(jù)矩陣Qg×h，將物點與像點的匹配問題轉(zhuǎn)化為尋求max qij，max qij表示矩陣Qg×h的第i行與第j列的最大值。如此，可以構造表征物點與像點匹配關系的g×h的匹配矩陣M匹配矩陣M由元素mij組成，匹配矩陣M中每一行和每一列元素，即mij的個數(shù)與矩陣Qg×h的每一行和每一列元素，即qij的個數(shù)相同。尋求矩陣Qg×h中每一行或者每一列中qij的最大值，即max qij，令矩陣M中與max qij對應位置的mij為1，即mij＝1，表示第i個像點與第j個物體點匹配，最終得到的匹配矩陣M，且滿足每一行與每一列只有一個值是1，其余值全是0。
這里，考慮到缺失點的現(xiàn)象，如一個立方體有8個角點，如果從立方體的正面進行拍攝，那么立方體的背面的四個角點就會被前面的四個角點遮住，那么在攝像機的坐標系中，立方體背面的四個角點就和前面的四個角點重合，則背面的四個角點就是缺失點?？紤]到缺失點給物體位姿計算帶來的不精確的問題，為匹配矩陣M增加一行和一列，則M變?yōu)?g+1)×(h+1)矩陣，如果mi，h+1＝1，1≤i≤g，則第i個圖像點無法和任何物體點進行匹配，同樣，mg+1，j＝1，1≤j≤h表示第j個物體點無法和任何圖像點進行匹配。這個離散問題可以通過引入一個控制變量β(β>0)轉(zhuǎn)化為連續(xù)問題，初始化匹配矩陣M，令 如此，可以保證m0ij>0。根據(jù)公式(9)來進行以后的確定退火算法，其中，α、γ、β是確定退火算法的參數(shù)值，γ是放縮常數(shù)系數(shù)，β用來模擬確定退火(deterministic annealing)算法中的溫度，其初始值很小，α是一個很小的常數(shù)，表示qij與0的接近程度。
步驟103，給出物點與像點變換關系的初始變化估計。
這里，可以利用隨機數(shù)發(fā)生器給出物點與像點變換關系的初始變化估計，變換{A0，B0}，A0是2×2矩陣，B0是2×1向量。
根據(jù)物點與像點的初始變化估計，變換{A0，B0}和已知的物點集合可以得到估計像點集合，然后由邊緣像點集合{pj}和估計像點集合可以計算出矩陣Qg×h的值，矩陣Qg×h的值用來作為進行確定退火算法時，設置參數(shù)β的依據(jù)。
較佳地，A0數(shù)值的量級設置為10左右，如此，物點與像點匹配時，所要優(yōu)化的全局函數(shù)很容易就可以收斂到全局最優(yōu)值，當全局函數(shù)收斂到全局最優(yōu)值時，與其所對應的變換{A，B}的值就是最終需要得到的估計變換關系{A′，B′}，此處在后面的步驟中加以說明。
根據(jù)初始變換{A0，B0}，由變換{A，B}，即公式(6)可以計算出估計像點集合{

}，其中1≤

≤h，h為物點的個數(shù)。采用現(xiàn)有技術中矩陣的構造方法，由邊緣像點集合{pj}構造矩陣U和V 由估計像點集合{

}構造矩陣U′和V′ 將公式(10)和公式(11)代入公式(7)可以計算出矩陣Qg×h。Qg×h的值用來決定β的值，此處在下面的步驟中進行說明。
步驟104、根據(jù)物點集合、邊緣像點集合、物點和像點變換關系的初始變化估計、以及匹配矩陣，得到物點與像點的估計變換關系，并由估計變換關系和物點集合，得到與物點集合匹配的像點集合。
該步驟的關鍵是計算出物點與像點的估計變換關系，該估計變換關系即是物點與像點的實際變換關系，由此便可得到與物點一一匹配的實際像點，進而得到實際像點集合。其中，物點與像點的估計變換關系通過確定退火算法求得，具體方法如下 依據(jù)公式(9)1≤i≤g，1≤j≤h進行確定退火算法，確定退火算法所要求解的參數(shù)是物點和像點的變換關系，變換{A，B}，通過計算可以得到物點和像點的估計變換關系，變換{A′，B′}，根據(jù)變換{A′，B′}可以得到與物點集合{Pi}匹配的像點集合{pi}。在每一次確定退火算法過程中，通過計算可以得到一個匹配矩陣M的值，根據(jù)匹配矩陣M的值可以得到一個新的物點和像點的變換關系，變換{Ai+1，Bi+1}，當變換{Ai+1，Bi+1}使全局優(yōu)化函數(shù)達到全局最優(yōu)值，即滿足確定退火的迭代終止條件時，結束確定退火循環(huán)，則該變換{Ai+1，Bi+1}即為物點和像點的估計變換關系，變換{A′，B′}。具體方法為 首先，需要設置確定退火算法的各參數(shù)值，即對公式(9)中的各個參數(shù)值進行設置。
確定退火算法各個參數(shù)值的設置是這樣的α是個很小的值，一般可設置為α＝10-5；γ對退火過程中的迭代影響不大，可以設置為1；β對迭代影響較大，T表示確定退火算法的溫度參數(shù)，則β可用來模擬確定退火算法的溫度，β的初始值β0的設定應參考矩陣Qg×h元素的數(shù)量級，Qg×h的值在步驟103中通過估計像點集合{

}和像點集合{pj}可以計算出來，如果Qg×h中大部分元素的數(shù)量級是105，那么β0應該設置為β0＝10-5，β0設置的太高或者太低，容易使迭代陷入局部極小值，β的終止值βfinal一般設置為0.5，β的更新倍數(shù)βupdate一般設置為1.05，βi為每一次退火過程中的β值，βi+1＝βi×βupdate；delta表示估計的像點與提取的真實像點歐式距離的平均值，tol1是一個很小的數(shù)值，和噪聲水平有關，noiseStd表示噪聲的標準差，在實際圖像處理中，噪聲的標準差是未知的，tol1的值一般設置為0.5。
將確定退火算法的各個參數(shù)值設置好后，利用sinkhorn算法更新匹配矩陣M，具體的方法是 1、初始化匹配矩陣M，令1≤i≤g，1≤j≤h，給mi，h+1，1≤i≤g+1分配一個很小的常數(shù)，如10-3，給mg+1，j，1≤j≤h+1分配一個很小的常數(shù)，如10-3； 2、對矩陣M的每一行元素和每一列元素進行歸一化計算，對每一行元素使用公式(12)進行歸一化計算 其中，l表示進行歸一化計算的次數(shù)。
對每一列元素使用公式(13)進行歸一化計算 其中l(wèi)表示進行歸一化計算的次數(shù)。
3、對匹配矩陣M循環(huán)進行歸一化計算； 這里，循環(huán)歸一化計算的終止條件是tol2小于一個給定的值，如0.005，對匹配矩陣M進行完一次歸一化處理以后得到一個新的匹配矩陣M，將其代入中，得到一個tol2，如果tol2<0.005，則終止對匹配矩陣M的歸一化計算；或者循環(huán)歸一化計算的次數(shù)大于設定的最大循環(huán)次數(shù)，如80次，則終止對匹配矩陣M的歸一化計算。
通過sinkhorn算法對匹配矩陣M每進行循環(huán)歸一化計算后，就會得到新的匹配矩陣M，根據(jù)計算得到的匹配矩陣M計算新的物點和像點的變換關系{Ai+1，Bi+1}，令 其中，a11、a12、a21、a22是矩陣A中的元素，b1、b2是矩陣B中的元素，將全局優(yōu)化函數(shù)帶入公式(14)，然后將公式(14)展開后可以得到線性方程組CX＝N，其中C是一個6×6的矩陣 X＝(a11，a12，a21，a22，b1，b2)T， N是一個6×1的矩陣 需要指出的是，在構造線性方程組CX＝N時，可以針對具體的問題做一個技術上的處理。當通過計算可以求得物體幾何特征中心的像點坐標時，則可以將該像點坐標近似代替物體質(zhì)心的像點坐標，由公式(6)可知，B表示物體質(zhì)心的像點坐標。例如，當空間平面物體的幾何特征是圓形時，該空間圓的成像為橢圓形，那么可以利用Canny算子提取得到橢圓的邊緣像點集合，然后利用橢圓擬合的方法得到橢圓中心的像點坐標。由于橢圓中心像點和空間圓質(zhì)心像點之間距離的畸變誤差只有幾微米到幾十微米，所以橢圓中心的像點坐標可以近似代替空間圓質(zhì)心的像點坐標，則公式(6)中B就成為了已知量，如此，在確定退火算法過程中需要求解的未知量只是2×2的矩陣A。
當B由物體幾何特征中心的像點坐標近似代替后，公式(6)可以表示為 這時，確定退火算法過程中需要優(yōu)化的全局函數(shù)為其中則構造線性方程組CX＝N，其中 X＝(a11 a12 a21 a22)T，則N是一個4×1的矩陣 然后，根據(jù)Gauss-Seidel迭代方法對線性方程組CX＝N求解，就可以得到新的變換{Ai+1，Bi+1}。
在執(zhí)行完一次退火算法后，更新β、βi+1＝βi×βupdate，再次執(zhí)行確定退火算法，得到新的匹配矩陣M和變換{Ai+1，Bi+1}，如此循環(huán)執(zhí)行確定退火算法，直到滿足確定退火的迭代終止條件，即βfinal＝0.5，確定退火算法達到迭代終止條件后，退出確定退火算法。
需要指出的是，每次確定退火算法過程結束后都會得到一個新的變換{Ai+1，Bi+1}，當變換{Ai+1，Bi+1}使全局優(yōu)化函數(shù)達到全局最優(yōu)值，即滿足確定退火的迭代終止條件時，結束確定退火循環(huán)，則該變換{Ai+1，Bi+1}即為物點和像點的估計變換關系，變換{A′，B′}。
將估計變換關系，變換{A′，B′}和物點集合{Pi}帶入公式(6)，通過計算可以得到與物點集合匹配的像點集合{pi}。
步驟105、根據(jù)物點集合和像點集合，得到空間平面物體的法向量在攝像機坐標系下的坐標和空間平面物體質(zhì)心在攝像機坐標系下的坐標。
這里，根據(jù)物點集合{Pi}和像點集合{pi}，利用迭代位姿算法計算得到兩組位姿向量I和J，利用I和J可以得到空間平面物體的法向量在攝像機坐標系下的坐標kT＝iT×jT，還可以得到空間平面物體質(zhì)心在攝像機坐標系下的坐標T。具體的算法如下 如圖3所示，ocxcyczc是攝像機坐標系，任意選取一個物點M0，以M0為物坐標系的坐標原點ow，建立物坐標系owxwywzw，K平面是過M0與像平面π平行的平面，點Pi是物點Mi正投影到K平面上的交點，pi是物點Pi的像點，mi是物點Mi的像點，m0是物點M0的像點，i、j、k分別是攝像機坐標系的xc軸、yc軸、zc軸在物坐標系下的方向矢量，由此可以得到 其中，(xci，yci，zci)T表示物點Mi在攝像機坐標系下的坐標，(xwi，ywi，zwi)T表示物點Mi在物體坐標系下的坐標，M0＝(xc0，yc0，zc0)T。
由于 k＝i×j (17) 其中，m0是參考點M0的像點，如此可以得知空間物體的姿態(tài)完全由i，j，zc0，m0＝(xc0，yc0)T決定。
因為 (18) 所以 其中， 公式(19)是一個線性方程組，未知量是向量I、J的坐標，已知量是M0、Mi的物坐標，以及真實的像坐標m0、mi。
對于n個物點M1，M2，Mi，...，Mn，有 AI＝x′ AJ＝y(tǒng)′ (20) 其中， 如果有至少4個非共面點(M0，M1，M2，M3)，那么矩陣的A的秩是3，所以矩陣方程組(20)的最小二乘解是 I0＝Bx′ J0＝By′(21) 其中，B＝(ATA)-1AT。依據(jù)公式(19)和(20)列出方程組，并設置初始ε＝0，再根據(jù)公式(21)進行計算，可以得到I0和J0。
共面點構成的矩陣A的秩是2，這時需要額外的約束條件，方程組(20)的真實解由公式(22)描述 I＝I0+λu J＝J0+μu(22) 對于真實姿態(tài)向量I，J，有兩條約束條件 I·J＝0 (23) ‖I‖＝‖J‖(24) 整理后，得到兩個λ和μ的約束條件 λμ＝-I0·J0 λ2-μ2＝J20-I20(25) 根據(jù)公式(23)和(24)計算λ和μ，進而得到兩組姿態(tài)向量I和J。定義復數(shù)C C＝λ+iμ (26) 所以 C2＝J20-I20-2iI0·J0 (27)將C2寫成極坐標形式 C2＝[R，Θ](28) R＝((J20-I20)2+4(I0·J0)2)1/2 (29) (30) 如果J20-I20＝0，R＝|2I0·J0｜ 那么，對于復數(shù)C有兩個根，C＝[ρ，θ]，C＝[ρ，θ+π]，其中， 所以， λ＝ρcosθ，μ＝ρsinθ(32) λ＝-ρcosθ，μ＝-ρsinθ 這樣，就可以得到兩組姿態(tài)向量 將I0和J0帶入公式(33)和(34)，得到姿態(tài)向量I，J。由于姿態(tài)向量I，J有兩組，所以得到的旋轉(zhuǎn)矩陣R也有兩組，為R1和R2，但是其中一組并不可行，是錯誤的旋轉(zhuǎn)矩陣。也就是說，利用得到的旋轉(zhuǎn)矩陣R計算物點質(zhì)心在攝像機坐標系下的坐標T，必須保證所有物點的zci>0，如果兩組旋轉(zhuǎn)矩陣都滿足zci>0，則可以根據(jù)Tsai的攝像機單目平面靶標標定的方法排除錯誤的旋轉(zhuǎn)矩陣R，由錯誤的旋轉(zhuǎn)矩陣R計算得到的有效焦距f<0，由正確的旋轉(zhuǎn)矩陣R計算得到的有效焦距f>0，具體方法是求解線性方程組 wi＝r31xwi+r32ywi+r9·0(36) 其中，2≤i≤h，h表示物點的個數(shù)。
由此便可以得到正確的旋轉(zhuǎn)矩陣，正確的旋轉(zhuǎn)矩陣的第三列即是空間平面物體的法向量NormVect在攝像機坐標系下的坐標。得到正確的法向量NormVect后，對正確的I，J單位化，可以得到旋轉(zhuǎn)矩陣R的iT，jT，進而得到kT＝iT×jT，即空間平面物體的法向量NormVect在攝像機坐標系下的坐標。
得到正確的一組姿態(tài)向量I，J后，由公式(19)可知則可以得到空間平面物體的質(zhì)心在攝像機坐標系下zc0的坐標 或 公式(37)帶入公式(16)可以得到空間平面物體的質(zhì)心在攝像機坐標系下xc0、yc0的坐標 公式(37)和(38)即表示空間平面物體的質(zhì)心在攝像機坐標系下的坐標，可以用唯一的平移向量T＝(xc0，yc0，zc0)T表示。
步驟106、根據(jù)空間平面物體的法向量，得到空間平面物體在攝像機坐標系下的俯仰角和偏航角。
該步驟采用現(xiàn)有技術實現(xiàn)，具體的將法向量NormVect單位化，得到俯仰角偏航角其中俯仰角pitch的取值范圍為
，偏航角yaw的取值范圍是
。
下面通過一個半實物仿真實驗來說明本發(fā)明的方案，該半實物仿真實驗中的圖像是實驗室開發(fā)的半實物平臺生成的虛擬圖像，圖像是由虛擬的圓生成的，圓半徑2米，圓心距離虛擬攝像機約300公里，攝像機焦距為66.885米，生成的圖像像素大小為1024×768，圓的圖像大約占整個像平面80×80的像素，圖像上加均值為0，標準差為4個像素的高斯噪聲。在虛擬相機中，設定圓的法向量NormVect和圓心在攝像機下的坐標為 俯仰角為56.774°，偏航角為118.187°。
首先，利用Canny算子提取圓的邊緣，得到100個邊緣像點{pj}，在物體圓上均勻間隔取100個物點{Pi}，計算與物點{Pi}一一匹配的像點{pi}，然后根據(jù)得到的{Pi}和{Pi}，利用迭代位姿算法進行位姿計算，得到圓在虛擬相機下的法向量和平移向量 計算出的虛擬圓的法向量的夾角絕對誤差為AngleErr＝0.678。
計算出的虛擬圓的圓心距離光心的距離的相對誤差為DistErr＝1.046％。
進而，得到的俯仰角為57.452°，偏航角為118.188°，俯仰角誤差為0.678°，偏航角誤差為0.001°。
以上所述，僅為本發(fā)明的較佳實施例而已，并非用于限定本發(fā)明的保護范圍。
權利要求
1、一種空間平面物體位姿識別方法，建立基于弱透視模型的空間平面物體的物點與像點之間的變換關系；其特征在于，該方法包括:
a、提取空間平面物體圖像的幾何特征的邊緣，得到邊緣像點集合，在空間平面物體的幾何特征上取點，得到物點集合，并根據(jù)所述物點集合和邊緣像點集合得到表征物點和像點變換關系的匹配矩陣；
b、給出物點與像點變換關系的初始變化估計；
c、根據(jù)所述物點集合、所述邊緣像點集合、所述初始變化估計和所述匹配矩陣，得到物點與像點的估計變換關系，并由所得到的估計變換關系和所述物點集合，得到與所述物點集合匹配的像點集合；
d、根據(jù)所述物點集合和像點集合得到空間平面物體的法向量在攝像機坐標系下的坐標和空間平面物體質(zhì)心在攝像機坐標系下的坐標；
e、根據(jù)所述空間平面物體的法向量得到空間平面物體在攝像機坐標系下的俯仰角和偏航角。
2、根據(jù)權利要求1所述空間平面物體位姿識別方法，其特征在于，步驟a中所述提取幾何特征的邊緣為:利用Canny算子提取空間平面物體圖像的幾何特征的邊緣。
3、根據(jù)權利要求1所述空間平面物體位姿識別方法，其特征在于，步驟b所述給出物點與像點變換關系的初始變化估計為:利用隨機數(shù)發(fā)生器給出所述初始變化估計。
4、根據(jù)權利要求1所述空間平面物體位姿識別方法，其特征在于，步驟c所述得到物點與像點的估計變換關系為:利用確定退火算法得到物點與像點的估計變換關系。
5、根據(jù)權利要求1或4所述空間平面物體位姿識別方法，其特征在于，步驟c進一步包括:
c1、根據(jù)所述匹配矩陣的初始值設置確定退火算法的初始參數(shù)；
c2、利用sinkhorn算法更新所述匹配矩陣；
c3、根據(jù)更新后的匹配矩陣利用Gauss-Seidel迭代方法計算物點和像點的估計變換關系；
c4、根據(jù)所述估計變換關系和所述物點集合，利用物點和像點的變換關系表達式得到與物點集合匹配的像點集合。
6、根據(jù)權利要求5所述空間平面物體位姿識別方法，其特征在于，步驟c2所述利用sinkhorn算法更新所述匹配矩陣包括:
初始化所述匹配矩陣；
對所述匹配矩陣的每一行和每一列元素進行歸一化計算；
對所述匹配矩陣循環(huán)進行歸一化計算。
7、根據(jù)權利要求1所述空間平面物體位姿識別方法，其特征在于，步驟d中所述得到法向量在攝像機坐標系下的坐標和空間平面物體質(zhì)心在攝像機坐標系下的坐標是:利用迭代位姿算法得到空間平面物體的法向量在攝像機下的坐標和空間平面物體質(zhì)心在攝像機坐標系下的坐標。
8、根據(jù)權利要求1或7所述空間平面物體位姿識別方法，其特征在于，步驟d進一步包括:
d1、根據(jù)所述物點集合和所述像點集合，利用迭代位姿算法計算得到兩組位姿向量；
d2、根據(jù)Tsai平面靶標標定的方法排除所述得到的兩組位姿向量中錯誤的一組位姿向量，得到正確的一組位姿向量；
d3、根據(jù)所述正確的位姿向量得到空間平面物體的法向量在攝像機坐標系下的坐標和空間平面物體質(zhì)心在攝像機坐標系下的坐標。
全文摘要
本發(fā)明公開了一種空間平面物體位姿識別方法，不需要任何物點與像點的先驗匹配信息，并且位姿的計算是基于平面點集，且與平面物體的幾何形狀無關；平面物體位姿計算下的二義性問題通過Tsai平面靶標標定的方法得以解決。由于不需要額外的特征信息，本發(fā)明的方法減弱了對空間平面物體位姿計算的條件限制，擴展了位姿計算的適用范圍，該方法在閉塞、混亂的環(huán)境下，以及存在不同級別的噪聲的情況下也能表現(xiàn)出較好的魯棒性，具有很好的應用價值。
文檔編號G06K9/00GK101377812SQ20081016778
公開日2009年3月4日 申請日期2008年10月7日 優(yōu)先權日2008年7月11日
發(fā)明者張廣軍, 巍 王, 魏振忠, 征 趙, 孫軍華 申請人:北京航空航天大學