[發(fā)明專利]單相機(jī)測(cè)量物體三維位置與姿態(tài)的方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201010184619.4
• 申請(qǐng)日: 2010-05-27
• 公開（公告）號(hào): CN101839692A
• 公開（公告）日: 2010-09-22
• 發(fā)明（設(shè)計(jì)）人: 薛建儒;王飛;田麗華;杜少毅;平林江;穆帆
• 申請(qǐng)（專利權(quán)）人: 西安交通大學(xué)
• 主分類號(hào): G01B11/00
• 分類號(hào): G01B11/00;G01B11/26;G01B11/16;G01C11/04
• 代理公司: 西安通大專利代理有限責(zé)任公司 61200
• 代理人: 陸萬壽

1.單相機(jī)測(cè)量物體三維位置與姿態(tài)的方法，其特征在于：包括相機(jī)標(biāo)定，目標(biāo)標(biāo)記，特征點(diǎn)的檢測(cè)，特征點(diǎn)與標(biāo)記點(diǎn)的匹配和目標(biāo)三維位置與姿態(tài)信息的計(jì)算五個(gè)步驟；

步驟1：相機(jī)標(biāo)定，標(biāo)定相機(jī)的內(nèi)外參數(shù)，建立視覺測(cè)量系統(tǒng)中三維世界坐標(biāo)和二維圖像坐標(biāo)的映射關(guān)系，所述相機(jī)的內(nèi)參數(shù)是指相機(jī)的焦比、光心及成像坐標(biāo)系兩軸間的夾角；所述相機(jī)的外參數(shù)是指旋轉(zhuǎn)矩陣和平移向量；

步驟2：目標(biāo)標(biāo)記，將標(biāo)記點(diǎn)標(biāo)記在目標(biāo)上，以便準(zhǔn)確快速的檢測(cè)和識(shí)別；

步驟3：特征點(diǎn)的檢測(cè)，對(duì)相機(jī)采集的圖像進(jìn)行圖像處理，獲得圖像中的所有特征點(diǎn)，計(jì)算各特征點(diǎn)在圖像中的位置，所述圖像的特征點(diǎn)是成像后待測(cè)目標(biāo)上的標(biāo)記點(diǎn)；

步驟4：特征點(diǎn)與標(biāo)記點(diǎn)的匹配，在檢測(cè)出的所有特征點(diǎn)中確定用于匹配的特征點(diǎn)，將其與目標(biāo)上標(biāo)記點(diǎn)進(jìn)行匹配，獲得二維坐標(biāo)與三維坐標(biāo)的對(duì)應(yīng)關(guān)系；

步驟5：目標(biāo)三維位置與姿態(tài)信息的計(jì)算，根據(jù)特征點(diǎn)與標(biāo)記點(diǎn)的對(duì)應(yīng)關(guān)系，計(jì)算目標(biāo)的三維位置與姿態(tài)信息，所述的三維位置與姿態(tài)信息是指目標(biāo)在三維世界坐標(biāo)系中的三維坐標(biāo)和其相對(duì)于坐標(biāo)系的偏轉(zhuǎn)角度。

2.根據(jù)權(quán)利要求1所述的單相機(jī)測(cè)量物體三維位置與姿態(tài)的方法，其特征在于：所述的步驟1包括以下具體步驟，

步驟1.1：建立二維坐標(biāo)和三維坐標(biāo)之間的映射關(guān)系，令圖像點(diǎn)p＝(u，v，1)^T是空間一點(diǎn)^CP＝(x，y，z，1)^T在圖像平面的齊次坐標(biāo)，則規(guī)一化圖像坐標(biāo)的齊次坐標(biāo)為：

p^=u^v^1=x/zy/z1---(1)]]>

物理圖像坐標(biāo)系與歸一化圖像坐標(biāo)系之間的關(guān)系為：

p=Ap^,]]>其中A=defαcu00βv0001---(2)]]>

(u₀，v₀，α，β，c)統(tǒng)稱為相機(jī)的內(nèi)部參數(shù)，其中(α，β)為相機(jī)的焦比，c反應(yīng)了u軸和v軸夾角斜度，C₀＝(u₀，v₀)^T是相機(jī)的光心，

圖像點(diǎn)p在物理圖像坐標(biāo)系中的齊次坐標(biāo)與空間點(diǎn)^CP在相機(jī)坐標(biāo)系中的齊次坐標(biāo)之間的關(guān)系為：

p=1zA0(PC)---(3)]]>

相機(jī)坐標(biāo)系(C)在世界坐標(biāo)系(W)＝(O_w，X，Y，Z)中的位置記為：

PC1=RWCOWC0T1P1]]>

代入(3)式得

p=1zHP,]]>其中H=defARt---(4)]]>

其中，是旋轉(zhuǎn)矩陣，t＝^CO_W是平移向量，它們是相機(jī)的外參數(shù)；

步驟1.2：對(duì)選定的相機(jī)進(jìn)行標(biāo)定，獲得相機(jī)的內(nèi)外參數(shù)；采用基于平面單應(yīng)矩陣的標(biāo)定方法，利用不同視點(diǎn)間的平面匹配來計(jì)算相機(jī)參數(shù)，最少需要3個(gè)不同視點(diǎn)的圖像完成標(biāo)定；記像平面二維坐標(biāo)點(diǎn)p＝(u，v)^T，世界坐標(biāo)P＝(X，Y，Z)^T，由公式(4)可知，從三維到二維的映射可以表示為：

p=1zARtP,]]>其中A=defαcu00βv0001---(5)]]>

令標(biāo)板所在的世界坐標(biāo)Z＝0，則上式可寫為：

uv1=1zAr1r2r3tXY01=1zAr1r2tXY1---(6)]]>

將上式重新記為：

p=1zHp,]]>其中H=defAr1r2t---(7)]]>

H矩陣即確定了三維空間坐標(biāo)到二維像平面坐標(biāo)的映射關(guān)系，求解出H矩陣就完成了相機(jī)的標(biāo)定；

步驟1.3：考慮到測(cè)量與計(jì)算誤差，定義誤差函數(shù)如下：

Σi=1nΣj=1m||pij-Pij(A,R,t)||2---(8)]]>

使上式最小即可求得最大似然估計(jì)下的最優(yōu)參數(shù)。