一種基于四面體邊長(zhǎng)和體積加權(quán)約束的單目視覺(jué)位姿測(cè)量方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設(shè)及一種單目視覺(jué)位姿測(cè)量方法，尤其設(shè)及一種基于四面體邊長(zhǎng)和體積加 權(quán)約束的單目視覺(jué)位姿測(cè)量方法。
【背景技術(shù)】
[0002] 目標(biāo)位姿測(cè)量在工業(yè)控制、航天操作等領(lǐng)域具有廣泛的應(yīng)用。其中基于視覺(jué)的目 標(biāo)位姿（位置、姿態(tài)）測(cè)量具有非接觸、設(shè)備成本低、監(jiān)視直觀等優(yōu)點(diǎn)。尤其在航天領(lǐng)域中， 利用視覺(jué)對(duì)飛行器位姿進(jìn)行那個(gè)位姿精確測(cè)量是多項(xiàng)空間任務(wù)的首要條件。
[0003] 視覺(jué)位姿測(cè)量可分為合作目標(biāo)測(cè)量和非合作目標(biāo)測(cè)量，合作目標(biāo)測(cè)量由于具有精 度高、測(cè)量技術(shù)成熟、可靠等優(yōu)勢(shì)，在空間領(lǐng)域廣泛采用。目前已采用過(guò)的合作標(biāo)志器形態(tài) 多種多樣，有利用特征點(diǎn)、特征線、特征圓等多種幾何特征的，通過(guò)對(duì)合作標(biāo)志器的特征識(shí) 別提取，從而利用提取的特征點(diǎn)坐標(biāo)重投影約束、特征直線斜率約束、特征圓的半徑約束等 解算出目標(biāo)位姿。
[0004] 根據(jù)秦麗娟等人的《基于矩形的=維物體位姿估計(jì)研究》（參見(jiàn)《計(jì)算機(jī)工程與 科學(xué)》，2009年31卷（4)49-51頁(yè)），利用了矩形對(duì)邊平行等條件構(gòu)造了位姿解析解算方 法，但未設(shè)及利用四面體邊長(zhǎng)和體積等幾何特征。根據(jù)王曉劍等人的《基于雙平行線特征 的位姿估計(jì)解析算法》（參見(jiàn)《儀器儀表學(xué)報(bào)》，2008年29卷（3) 600-604頁(yè)），利用了平行 四邊形的面積等特征構(gòu)造位姿解析解算方法，但同樣未設(shè)及四面體邊長(zhǎng)和體積特征。專利 CN201010563504. 6中提出了一種基于特征線的運(yùn)動(dòng)目標(biāo)位姿光學(xué)測(cè)量方法，方法中利用了 兩條相交直線上4個(gè)特征點(diǎn)不重合且不共線條件，解算目標(biāo)位置姿態(tài)，未設(shè)及利用四個(gè)點(diǎn) 構(gòu)成四面體的邊長(zhǎng)和體積特征。綜上所述，上述方法均未設(shè)及利用四面體邊長(zhǎng)和體積特征 加權(quán)關(guān)系構(gòu)造位姿迭代解算方法。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明要解決的技術(shù)問(wèn)題是：提供一種基于四面體邊長(zhǎng)和體積加權(quán)約束的單目視 覺(jué)位姿測(cè)量方法。該方法利用4個(gè)不共面特征點(diǎn)構(gòu)造了四面體，并利用四面體目標(biāo)在位姿 變化過(guò)程中其邊長(zhǎng)和體積保持不變的幾何特征建立了加權(quán)誤差約束關(guān)系，建立了對(duì)剛體形 狀約束，同時(shí)建立目標(biāo)位姿進(jìn)行約束，構(gòu)造了迭代解算位姿方法。通過(guò)對(duì)四面體邊長(zhǎng)和體積 的誤差約束迭代解算目標(biāo)位姿方法，有效利用了四面體目標(biāo)剛體結(jié)構(gòu)不變性特征，從而提 高了位姿解算結(jié)果準(zhǔn)確性。
[0006] 本發(fā)明采用的技術(shù)方案如下：一種基于四面體邊長(zhǎng)和體積加權(quán)約束的單目視覺(jué)位 姿測(cè)量方法，該方法包括如下步驟：
[0007] (1)對(duì)目標(biāo)成像，并獲取目標(biāo)圖像。首先設(shè)置目標(biāo)由4個(gè)不共面特征點(diǎn)構(gòu)成四面 體，已知4個(gè)目標(biāo)特征點(diǎn)在目標(biāo)坐標(biāo)系中3維坐標(biāo)集{Wi，胖2,胖3,WJ，獲取四面體邊長(zhǎng)參數(shù)L， 體積參數(shù)V，
【主權(quán)項(xiàng)】
1. 一種基于四面體邊長(zhǎng)和體積加權(quán)約束的單目視覺(jué)位姿測(cè)量方法，其特征在于該方法 包括如下步驟： 步驟（1)、對(duì)目標(biāo)成像，并獲取目標(biāo)圖像；首先設(shè)置目標(biāo)由4個(gè)不共面特征點(diǎn)構(gòu)成四面 體，已知4個(gè)目標(biāo)特征點(diǎn)在目標(biāo)坐標(biāo)系中3維坐標(biāo)集（W1, W2, W3, W4}，獲取四面體邊長(zhǎng)參數(shù)L， 體積參數(shù)V，
其中 i 乒 j 乒 g 乒 h,且 i, j, g, h e {1，2, 3, 4}; 步驟（2)、識(shí)別四面體目標(biāo)特征點(diǎn)；將步驟（1)獲取的圖像進(jìn)行識(shí)別并提取圖像中目標(biāo) 特征點(diǎn)坐標(biāo){aj (i = Ρ··4); 步驟（3)、建立四面體邊長(zhǎng)和體積加權(quán)約束關(guān)系；已知攝像機(jī)等效焦距為f，定義4個(gè)目 標(biāo)特征點(diǎn)在攝像機(jī)坐標(biāo)系下坐標(biāo)集為M1, A2, A3, AJ，定義特征點(diǎn)到攝像機(jī)光心p的距離為 Il1, 12, 13, 14};由步驟⑵獲取了目標(biāo)特征點(diǎn)在圖像平面成像點(diǎn)坐標(biāo)ai= (u i, Vi) (i = 1··· 3)，則成像點(diǎn)的3維坐標(biāo)表示為（ap f) = (Ui, Vi, f)，則：
應(yīng)用特征點(diǎn)構(gòu)造邊長(zhǎng)L和體積V作為對(duì)剛體目標(biāo)的約束，構(gòu)成了基于邊長(zhǎng)L和體積V 加權(quán)約束的誤差項(xiàng)Ew，1個(gè)四面體有6條邊，則共形成6組邊長(zhǎng)L誤差約束項(xiàng)，對(duì)應(yīng)形成6 組約束加權(quán)誤差約束項(xiàng)E w;
Elv= a eL+0 ev 其中i乒j乒g乒h,且i, j, g, h e {1，2, 3, 4} ;E為第k次迭代后由ejP e v開(kāi)多成的加 權(quán)誤差矩陣； 步驟（4)、基于四面體邊長(zhǎng)和體積約束關(guān)系解算位姿；首次迭代解算出{1J的修正量 X ：
JiX = Elv X = (J1tJ1)-1J1- 1Elv 其中1為雅可比（Jacobian)矩陣，在迭代中解算出{1 J的修正量X，從而最終解算出 準(zhǔn)確的{lj ; L(k+i)=L(k)_x 其中L(k)是第k次迭代后的{1J ;當(dāng)所有的邊長(zhǎng)和體積約束都能同時(shí)被滿足，誤差Ew 在迭代過(guò)程中將會(huì)逐漸收斂到零，且最終解算得到最優(yōu)的{1J ;另一方面，此非線性最優(yōu)化 問(wèn)題可以被認(rèn)為是局部線性的，反復(fù)迭代可以被解算得到{1J ;在{1J得到的基礎(chǔ)上，解算 得到目標(biāo)特征點(diǎn)在攝像機(jī)坐標(biāo)系中坐標(biāo){AJ，再由最小二乘平差方法，解算得到目標(biāo)姿態(tài) 角度9 = (Ax，Ay，Az)與目標(biāo)平移向星T= (tx，ty，tz)。
2.根據(jù)權(quán)利要求1所述的一種基于四面體邊長(zhǎng)和體積加權(quán)約束的單目視覺(jué)位姿測(cè)量 方法，其特征在于：所述步驟（3)中Ew為第k次迭代后由ejPev形成的加權(quán)誤差矩陣，具 體應(yīng)為：加權(quán)系數(shù)α和β滿足α + β = 1。
【專利摘要】本發(fā)明公開(kāi)了一種基于四面體邊長(zhǎng)和體積加權(quán)約束的單目視覺(jué)位姿測(cè)量方法，該方法的步驟包括：步驟(1)、獲取四面體目標(biāo)圖像；步驟(2)、提取圖像中四面體目標(biāo)特征點(diǎn)；步驟(3)、建立四面體體積和邊長(zhǎng)特征加權(quán)約束關(guān)系；步驟(4)、基于四面體體積邊長(zhǎng)加權(quán)約束關(guān)系解算位姿。該方法運(yùn)用圖像識(shí)別手段，以四面體體積和邊長(zhǎng)特征建立加權(quán)誤差約束關(guān)系，提高了目標(biāo)單目視覺(jué)位姿測(cè)量精度。
【IPC分類】G01C11-04
【公開(kāi)號(hào)】CN104880178
【申請(qǐng)?zhí)枴緾N201510290582
【發(fā)明人】趙汝進(jìn), 王進(jìn), 趙人杰, 王明富, 洪裕珍, 顏坤, 游迪
【申請(qǐng)人】中國(guó)科學(xué)院光電技術(shù)研究所
【公開(kāi)日】2015年9月2日
【申請(qǐng)日】2015年6月1日