基于雙目立體視覺(jué)的目標(biāo)空間坐標(biāo)計(jì)算及姿態(tài)估計(jì)

黃青丹， 何彬彬， 宋浩永， 饒 銳， 趙寶玉， 王國(guó)庫(kù)

(1. 南方電網(wǎng)中低壓電氣設(shè)備質(zhì)量檢驗(yàn)測(cè)試重點(diǎn)實(shí)驗(yàn)室∥廣州供電局有限公司電力試驗(yàn)研究院， 廣州 510000；2. 河南中分儀器股份有限公司， 鄭州 450000)

在實(shí)際工程應(yīng)用中，經(jīng)常需要通過(guò)非接觸的方式實(shí)現(xiàn)目標(biāo)空間坐標(biāo)及姿態(tài)的測(cè)量，基于雙目立體視覺(jué)的測(cè)量方法具備效率高、系統(tǒng)結(jié)構(gòu)簡(jiǎn)單、成本低、非接觸式等特點(diǎn)，可以實(shí)現(xiàn)目標(biāo)空間三維信息的測(cè)量. 20世紀(jì)80年代，麻省理工人工智能實(shí)驗(yàn)室的MARR提出MARR視覺(jué)理論，推動(dòng)了機(jī)器視覺(jué)理論的快速發(fā)展[1]. 雙目立體視覺(jué)理論通過(guò)視差原理來(lái)還原空間物體的深度信息[2]，已被應(yīng)用于具體應(yīng)用場(chǎng)景，例如：在無(wú)人駕駛的起重機(jī)上安裝雙目立體視覺(jué)系統(tǒng)，為起重機(jī)吊運(yùn)目標(biāo)物體提供了具體的三維空間信息[3]；在月球漫游車(chē)模型上搭載雙目立體視覺(jué)系統(tǒng)[4]，對(duì)目標(biāo)進(jìn)行切割、抓取等操作；利用雙目立體視覺(jué)系統(tǒng)測(cè)量光學(xué)元件的位置姿態(tài)，可建立基于雙目立體視覺(jué)的孔徑測(cè)量系統(tǒng)[5]. 近些年來(lái)雙目立體視覺(jué)理論及技術(shù)得到了長(zhǎng)足發(fā)展，然而在實(shí)際應(yīng)用中仍然存在魯棒性差的問(wèn)題[6]. 需要針對(duì)不同的應(yīng)用背景，設(shè)計(jì)相應(yīng)的解決方案.

在電力儀表的全自動(dòng)化檢定領(lǐng)域，需要對(duì)儀表進(jìn)行自動(dòng)抓取與搬運(yùn)，其中亟需解決的問(wèn)題是對(duì)儀表空間位置及姿態(tài)識(shí)別計(jì)算的準(zhǔn)確性. 雖然基于雙目立體視覺(jué)與機(jī)械臂相配合的系統(tǒng)已在很多場(chǎng)合得到應(yīng)用，但目前應(yīng)用于儀表自動(dòng)識(shí)別并抓取的研究少有報(bào)道. 北京東方計(jì)量研究所目前公布了最新研制的數(shù)字式儀表檢定平臺(tái)，通過(guò)夾具固定儀表位置，根據(jù)儀表型號(hào)事先標(biāo)定相應(yīng)的空間位置；在變電站巡檢系統(tǒng)中，雖然通過(guò)模板匹配實(shí)現(xiàn)儀表平面內(nèi)位置的識(shí)別，但不能獲得其深度信息. 本文提出了一種實(shí)用化的目標(biāo)空間坐標(biāo)及姿態(tài)估計(jì)方法，并應(yīng)用于電力儀表的自動(dòng)抓取系統(tǒng)，實(shí)驗(yàn)結(jié)果表明，所提出的方法性能穩(wěn)定，能滿足實(shí)際應(yīng)用需求.

1 研究方法

1.1 雙目立體視覺(jué)成像模型

雙目視覺(jué)的基本原理是利用雙攝像頭模擬人眼的定位. 分析圖像中相同目標(biāo)成像的差異，可以得到目標(biāo)物的三維位置信息. 圖1模型代表了理想狀態(tài)的雙目立體視覺(jué)系統(tǒng)，系統(tǒng)成像共面的2個(gè)相機(jī)(C1、C2)具有一致的焦距且光軸平行[7]. 在左相機(jī)坐標(biāo)系中，空間點(diǎn)P的空間位置坐標(biāo)為(XC,YC,ZC)，而(uL,vL)、(uR,vR)表示圖像像素在左右圖像坐標(biāo)系中的坐標(biāo).

圖1 雙目立體視覺(jué)模型

根據(jù)幾何知識(shí)能夠推導(dǎo)特征點(diǎn)P和其在左相機(jī)圖像中投影點(diǎn)的位置關(guān)系：

(1)

其中，(u0,v0)是真實(shí)坐標(biāo)原點(diǎn)在像素坐標(biāo)系中的坐標(biāo)，f為相機(jī)焦距. 定義d=uL-uR作為視差，對(duì)于左相機(jī)的相機(jī)坐標(biāo)系，特征點(diǎn)P的三維坐標(biāo)為

(2)

然而，在實(shí)際工程的雙目視覺(jué)系統(tǒng)中，2個(gè)成像平面以及焦距難以保持一致，所以，如果知道2個(gè)相機(jī)的相對(duì)位置關(guān)系，可實(shí)施校正[8]，使校正模型滿足理想狀態(tài)下的成像要求.

1.2 雙目相機(jī)的標(biāo)定

基于二維棋盤(pán)格圖像校準(zhǔn)板的相機(jī)視覺(jué)算法標(biāo)定方法[9]結(jié)合了傳統(tǒng)校準(zhǔn)算法的優(yōu)點(diǎn)，具有良好的魯棒性和自校準(zhǔn)算法，可以滿足校準(zhǔn)對(duì)象的依賴性要求[10]. 不需要標(biāo)定板移動(dòng)的參數(shù)，僅需要標(biāo)定模板在2個(gè)方向上的移動(dòng)圖像即可進(jìn)行標(biāo)定. 該方法標(biāo)定精度高、成本低，已被廣泛用于桌面視覺(jué)系統(tǒng).

該標(biāo)定方法的基本步驟[11]：(1)將棋盤(pán)的圖像當(dāng)作標(biāo)定板，打印并將其附著在剛性平面上；(2)分別從多個(gè)角度拍攝標(biāo)定板；(3)提取每幅圖像中的特征點(diǎn)，同時(shí)得到特征點(diǎn)在2個(gè)關(guān)鍵坐標(biāo)系中的位置；(4)使用線性模型提取攝像頭的內(nèi)部參數(shù)以及每次圖像的外部參數(shù)；(5)借助畸變模型優(yōu)化相機(jī)內(nèi)外部參數(shù)，得到所有參數(shù).

本文利用MATLAB對(duì)雙目相機(jī)進(jìn)行標(biāo)定. 從多個(gè)角度用2個(gè)相機(jī)獲取20幅平面標(biāo)定板的雙圖像組(圖2展示20幅左相機(jī)圖像)，對(duì)獲得的圖像通過(guò)MATLAB的Stereo Calibration工具完成標(biāo)定.

圖2 標(biāo)定使用的左相機(jī)圖像

標(biāo)定得到平移向量T為[-71.409 21，1.071 99，-0.194 66]，2個(gè)相機(jī)的間距為72 mm，平移參數(shù)經(jīng)過(guò)標(biāo)定取得，結(jié)果為72.526 17 mm，大體上持平，同時(shí)，剩余維度的偏移很小. 所以，雙目相機(jī)標(biāo)定的結(jié)果可作為校正參數(shù).

1.3 儀表圖像的立體匹配

儀表圖像的立體匹配主要采用半全局立體匹配算法，通過(guò)在不同方向掃描線上一維的局部尋優(yōu)過(guò)程來(lái)逼近二維圖像的全局尋優(yōu)過(guò)程. 主要步驟為計(jì)算單個(gè)像素點(diǎn)的匹配代價(jià). 通過(guò)線上的平滑約束和多個(gè)方向上匹配成本的聚合階段，構(gòu)建逼近的全局能量函數(shù). 使用優(yōu)化方法最小化該能量函數(shù)來(lái)估計(jì)視差[12]. 半全局匹配方法在確保良好精度的前提下，具有相對(duì)低的時(shí)間復(fù)雜度，同時(shí)，并行處理單元可使算法的計(jì)算速度得到提高. 由于函數(shù)對(duì)相機(jī)采樣效果不敏感[13]，本文使用BT函數(shù)計(jì)算像素點(diǎn)的匹配成本. 實(shí)驗(yàn)中以儀表圖像為目標(biāo)，算法的結(jié)果視差圖通過(guò)實(shí)驗(yàn)得到(圖3).

圖3 立體匹配效果

在定義半全局匹配方法的能量函數(shù)時(shí)，增加額外的視差平滑約束：

(3)

式中，第一項(xiàng)為數(shù)據(jù)項(xiàng)，是基于視差的像索點(diǎn)匹配成本累計(jì)和；第二、三項(xiàng)為平滑項(xiàng)，P1、P2分別代表NP鄰域中視差變化的匹配點(diǎn)P的懲罰系數(shù)(滿足P1>P2). 通常，為了避免引起邊界模糊匹配，需要依據(jù)P′2=P2/(Irp-Irq)像素灰度的變化自適應(yīng)調(diào)節(jié)P2[14].

針對(duì)二維圖像上的全局最小NP完全問(wèn)題求解能量函數(shù)，需要使用多方向的一維平滑來(lái)逼近二維情況的平滑約束. 半全局匹配算法吸收了掃描線最優(yōu)算法的思想[15]：沿著8條不同的掃描線分別計(jì)算，然后聚合沿掃描線存儲(chǔ)的掃描成本. 以迭代的方式得到視差計(jì)算：采用右視差圖像糾正右圖像，進(jìn)而完成匹配并生成新的視差圖像. 還可以使用分層計(jì)算的方法：首先在半分辨率的情況下計(jì)算視差圖像作為初始值，然后使用所獲得的視差圖像執(zhí)行遞歸計(jì)算[16]. 以左圖像為基準(zhǔn)完成相應(yīng)的左圖像和右圖像的匹配，若2次匹配視差結(jié)果不相同則視為無(wú)效匹配. 與此同時(shí)，利用視覺(jué)系統(tǒng)的左相機(jī)收集圖像并進(jìn)行處理. 采用背景差分的方法在預(yù)先存儲(chǔ)了背景圖像的前提下分割采集到的圖像，可獲取儀表的輪廓. 圖4為儀表輪廓背景差分方法的流程圖.

圖4 背景差分法提取儀表輪廓的流程圖

Figure 4 The flow chart of instrument outline extraction with the background difference method

采用上述流程對(duì)采集的圖像進(jìn)行處理，去除處于圖像邊緣區(qū)域的輪廓，保留連通域中最大部分作為進(jìn)行儀表定位和抓取的工作區(qū)域，可得到儀表的輪廓信息，具體包含：儀表的輪廓大小、中心點(diǎn)坐標(biāo)和傾斜角度[7]. 然后，在原始圖像中顯示儀表的輪廓信息(圖5).

圖5 儀表輪廓的提取結(jié)果

結(jié)合視差可計(jì)算儀表中心點(diǎn)的三維坐標(biāo). 僅獲得儀表中心點(diǎn)的三維坐標(biāo)仍然不能描述儀表在空間中的姿態(tài)，本文通過(guò)增加儀表在水平面的傾斜角度，結(jié)合儀表的中心點(diǎn)三維坐標(biāo)，可以完整地描述儀表處于水平面時(shí)的空間姿態(tài).

1.4 實(shí)驗(yàn)設(shè)置

本文將所設(shè)計(jì)的雙目立體視覺(jué)系統(tǒng)應(yīng)用于電力儀表的自動(dòng)抓取，實(shí)現(xiàn)儀表空間坐標(biāo)計(jì)算及姿態(tài)估計(jì)，系統(tǒng)實(shí)物如圖6所示，機(jī)械臂末端安裝夾爪及雙目立體視覺(jué)系統(tǒng)，通過(guò)雙目立體視覺(jué)系統(tǒng)識(shí)別儀表空間坐標(biāo)及姿態(tài)，進(jìn)而實(shí)現(xiàn)對(duì)儀表的自動(dòng)抓取.

圖6 電力儀表抓取系統(tǒng)的實(shí)物圖

雙目視覺(jué)系統(tǒng)首先采集儀表圖像，然后提取特征點(diǎn)，進(jìn)而實(shí)現(xiàn)立體匹配. 儀表特征點(diǎn)集中在儀表表盤(pán)目標(biāo)區(qū)域，且儀表外殼區(qū)域特征點(diǎn)的分布沒(méi)有統(tǒng)計(jì)意義. 所以，儀表中心點(diǎn)的三維坐標(biāo)能夠通過(guò)表盤(pán)區(qū)域中匹配點(diǎn)的視差得到. 通過(guò)查閱儀表規(guī)格知道表盤(pán)區(qū)域較外殼突出16 mm，那么計(jì)算所有匹配點(diǎn)的視差得到坐標(biāo)Z最大的點(diǎn)，則判斷為表盤(pán)區(qū)域的點(diǎn). 為了得到儀表中心點(diǎn)坐標(biāo)，可以通過(guò)這個(gè)點(diǎn)的視差得到，對(duì)Z進(jìn)行補(bǔ)償能夠得到中心點(diǎn)的三維坐標(biāo). 使用精度為0.01 mm的手動(dòng)三維坐標(biāo)測(cè)量?jī)x測(cè)量坐標(biāo)，將其作為標(biāo)定的世界坐標(biāo)，視為坐標(biāo)真值，與雙目視覺(jué)系統(tǒng)匹配得到的三維坐標(biāo)對(duì)比，進(jìn)行坐標(biāo)誤差計(jì)算.

2 結(jié)果與討論

2.1 匹配誤差分析

系統(tǒng)與目標(biāo)儀表檢測(cè)區(qū)域的相對(duì)距離為400 mm，考慮不同儀表的不同姿態(tài)，進(jìn)行10次實(shí)驗(yàn)，實(shí)驗(yàn)所得儀表中心點(diǎn)的三維坐標(biāo)、坐標(biāo)真值以及兩者間的誤差如表1所示.

表1 匹配結(jié)果及誤差

當(dāng)雙目立體視覺(jué)系統(tǒng)與儀表的距離分別為380、400、420、440、460 mm時(shí)，計(jì)算儀表中心點(diǎn)三維坐標(biāo)中Z的絕對(duì)誤差如圖7所示，其平均相對(duì)誤差不超過(guò)1.84%. 根據(jù)趨勢(shì)，圖中Z的絕對(duì)誤差隨著儀表深度距離的增加而增大，當(dāng)雙目立體視覺(jué)系統(tǒng)與儀表的距離增加到460 mm時(shí)，誤差達(dá)到最大(13.96 mm)，此時(shí)已不滿足系統(tǒng)的誤差要求. 而在實(shí)際操作情況中，在保證檢測(cè)速度的前提下，通過(guò)轉(zhuǎn)動(dòng)儀表檢測(cè)平臺(tái)，設(shè)置實(shí)驗(yàn)將雙目立體視覺(jué)系統(tǒng)的相對(duì)距離設(shè)置為 400 mm，即相機(jī)與不同儀表的距離為400 mm，對(duì)儀表中心點(diǎn)三維坐標(biāo)進(jìn)行立體匹配. 經(jīng)過(guò)大量的測(cè)試實(shí)驗(yàn)，結(jié)果表明：本文設(shè)計(jì)的雙目立體視覺(jué)系統(tǒng)運(yùn)行穩(wěn)定、魯棒性好、系統(tǒng)誤差滿足要求，可實(shí)現(xiàn)對(duì)儀表空間坐標(biāo)的計(jì)算及姿態(tài)估計(jì)，進(jìn)而實(shí)現(xiàn)對(duì)儀表的準(zhǔn)確抓取.

圖7 不同距離下三維坐標(biāo)Z的誤差

Figure 7 The errors inZof three-dimensional coordinates at different distances

2.2 與其他方法的對(duì)比及分析

目標(biāo)空間位置識(shí)別的準(zhǔn)確性主要取決于采用的立體匹配算法，將本文提出的方法與基于尺度不變特征變換(SIFT)特征算子的立體匹配方法進(jìn)行比較. 基于SIFT特征算子的立體匹配是典型的基于特征的匹配算法，僅需要對(duì)圖像中的一部分像素點(diǎn)計(jì)算視差，因此其計(jì)算量較小. 雙目立體視覺(jué)系統(tǒng)與儀表的深度距離為400 mm時(shí)采集10個(gè)不同姿態(tài)的儀表圖像，不同方法的平均坐標(biāo)誤差及計(jì)算時(shí)間如表2所示. 雖然所提出的方法耗時(shí)比SIFT特征匹配方法的秏時(shí)略長(zhǎng)，但準(zhǔn)確性顯著優(yōu)于SIFT特征匹配方法，這是因?yàn)榛赟IFT特征的匹配方法屬于稀疏匹配，匹配到的特征點(diǎn)位置具有隨機(jī)性，但是對(duì)儀表中心點(diǎn)坐標(biāo)計(jì)算的精度不如本文所提出的方法.

表2 本文方法與SIFT特征匹配方法對(duì)比

Table 2 the comparison of the proposed method and the SIFT method

匹配方法平均坐標(biāo)絕對(duì)誤差/mm平均計(jì)算時(shí)間/s本文方法6.1568.101SIFT特征匹配方法8.4128.056

3 結(jié)論

提出了基于雙目立體視覺(jué)的目標(biāo)空間坐標(biāo)計(jì)算及姿態(tài)估計(jì)方法，并應(yīng)用于電力儀表抓取系統(tǒng). 首先在完成雙目系統(tǒng)標(biāo)定后，通過(guò)對(duì)左右相機(jī)采集的圖像經(jīng)過(guò)半全局立體匹配算法計(jì)算獲得目標(biāo)視差圖；然后通過(guò)背景差分法獲得儀表輪廓信息，包括儀表輪廓大小、輪廓中心點(diǎn)坐標(biāo)以及相對(duì)垂直方向的傾斜角；最后通過(guò)儀表中心點(diǎn)的像素坐標(biāo)和儀表的視差圖計(jì)算出儀表中心點(diǎn)在空間中的三維坐標(biāo)，將該坐標(biāo)與儀表的傾斜角度結(jié)合起來(lái)，可以描述儀表在空間中的任意水平姿態(tài). 實(shí)際測(cè)試結(jié)果表明：所設(shè)計(jì)的雙目視覺(jué)系統(tǒng)性能穩(wěn)定，可以實(shí)現(xiàn)儀表空間坐標(biāo)的計(jì)算及姿態(tài)估計(jì)，為機(jī)械臂抓取儀表奠定了基礎(chǔ)，具有重要的應(yīng)用價(jià)值.