一種基于三角形邊長(zhǎng)、內(nèi)角、形狀因子加權(quán)約束的單目視覺位姿測(cè)量方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設(shè)及一種單目視覺位姿測(cè)量方法，尤其設(shè)及一種基于=角形邊長(zhǎng)、內(nèi)角、形 狀因子加權(quán)約束的單目視覺位姿測(cè)量方法。
【背景技術(shù)】
[0002] 目標(biāo)位姿測(cè)量在工業(yè)控制、航天操作等領(lǐng)域具有廣泛的應(yīng)用。其中基于視覺的目 標(biāo)位姿（位置、姿態(tài)）測(cè)量具有非接觸、設(shè)備成本低、監(jiān)視直觀等優(yōu)點(diǎn)。尤其在航天領(lǐng)域中， 利用視覺對(duì)飛行器位姿進(jìn)行那個(gè)位姿精確測(cè)量是多項(xiàng)空間任務(wù)的首要條件。
[0003] 視覺位姿測(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)重投影約束、特征直線斜率約束、特征圓的半徑約束等 建立對(duì)目標(biāo)形狀的約束關(guān)系，同時(shí)也建立起對(duì)目標(biāo)位姿的約束關(guān)系，達(dá)到解算出目標(biāo)位姿 的目的。
[0004] 根據(jù)秦麗娟等人的《基于矩形的=維物體位姿估計(jì)研究》（參見《計(jì)算機(jī)工程與科 學(xué)》，2009年31卷（4) 49-51頁(yè)），利用了矩形對(duì)邊平行等條件構(gòu)造了位姿解析解算方法，但 未設(shè)及利用=角形邊長(zhǎng)、內(nèi)角、形狀因子等幾何特征。根據(jù)王曉劍等人的《基于雙平行線特 征的位姿估計(jì)解析算法》（參見《儀器儀表學(xué)報(bào)》，2008年29卷（3)600-604頁(yè)），利用了平 行四邊形的面積等特征構(gòu)造位姿解析解算方法，但同樣未設(shè)及=角形邊長(zhǎng)、內(nèi)角、形狀因子 等特征。專利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)、內(nèi)角、形狀因子等特征。綜上所述，上述方法均未設(shè)及利用 =角形邊長(zhǎng)、內(nèi)角、形狀因子特征加權(quán)關(guān)系構(gòu)造位姿迭代解算方法。

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

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