基于改進ICP算法和CAD模型庫的單目視覺位姿測量

季 濤,李福東,楊月全,張?zhí)炱?/p>

(揚州大學信息工程學院,江蘇 揚州 225009)

基于視覺的位姿測量在工業(yè)自動化中應用廣泛,由于人們對機器人系統(tǒng)的智能化需求不斷提高,而位姿測量是實現(xiàn)機器人靈活有效運動的前提,故其精確度和實時性成為研究熱點．目前,基于單目視覺的位姿測量方法主要分為基于物體表面特征和基于模板匹配兩類．基于物體表面特征的方法通常使用點、線、圓等幾何特征進行匹配,有時須人為設(shè)定特征點．Loncomilla 等[1]通過識別對象局部不變特征的方法總結(jié)出機器人視覺中用于對象識別的系統(tǒng)．Liu等[2]對工件上的幾個特征點進行定向測量,利用微分變換估計初始迭代值,再采用不確定度分析法優(yōu)化攝像機姿態(tài)．基于模板匹配的方法則根據(jù)對象的CAD模型在各種姿態(tài)下的成像離線建立模板庫,然后對待測圖像進行處理并與模板庫里的圖像匹配來確定物體當前位姿．宋薇等[3]采用傾角分層方法將距離映射圖按邊緣傾角分層,結(jié)合模板探索點的傾角進行匹配．劉凌云等[4]首先選出模板庫里的候選圖,然后確定最佳匹配模板以加快匹配速度．Bratanic等[5]基于模板匹配的方法,改進相似性度量并引入光度立體視覺獲得表面法線實現(xiàn)位姿估計．Markus[6]結(jié)合尺度空間思想和方位圖的相似性實現(xiàn)三維物體的快速識別．由于基于物體表面特征的方法具有局限性,故本文擬采用基于模板匹配的方法,首先利用OpenGL繪制出物體的CAD模型,設(shè)置合適的采樣間距適當減少匹配庫容量,然后采用改進的Hausdorff距離匹配算法得到與待測圖像位姿最接近的一幅模板,匹配得到粗略位姿,最后使用改進的ICP算法對待測圖像的邊緣點集和物體三維邊緣點集進行迭代優(yōu)化得到精確位姿．

1 模板庫的建立

首先利用OpenGL繪制出物體的三維模型,以物體幾何中心為原點建立世界坐標系(Ow,Xw,Yw,Zw),再以其原點為球心建立地理坐標系,虛擬相機在球面上不同的觀察點處獲得對應的投影圖,如圖1所示．以相機的光軸為Zc軸建立相機坐標系(Oc,Xc,Yc,Zc),其光軸始終指向球心．虛擬相機在球面移動時,世界坐標系和相機坐標系的位姿關(guān)系可用參數(shù)經(jīng)度φ、緯度δ和半徑r表示出來．

圖1 虛擬相機投影示意圖Fig.1 Virtual camera projection

根據(jù)虛擬相機繞自身Zc軸轉(zhuǎn)動的角度θ及其在地理坐標系中的坐標,可得到世界坐標系和相機坐標系之間的轉(zhuǎn)換矩陣

(1)

其中R()表示旋轉(zhuǎn)量,其下標代表旋轉(zhuǎn)所圍繞的軸,T()表示平移量．根據(jù)實際相機的觀測范圍設(shè)置參數(shù)φ、δ和r的范圍和步長建立模板庫．若步長設(shè)置適中,則可較大程度地減少模板庫的容量,提高后續(xù)匹配速度．若步長設(shè)置得不夠小,匹配得到的只是粗略位姿,于是須采用ICP算法對粗略位姿進行迭代優(yōu)化得到精確位姿．根據(jù)式(1),欲得到世界坐標系與相機坐標系的相對位姿,只須確定參數(shù)φ、δ、r和θ的值．待測圖像與模板庫里的圖像配準后可得參數(shù)φ、δ、和r的值,參數(shù)θ的值則由圖像配準后的仿射變換求得．

2 改進的Hausdorff距離閾值法匹配

Hausdorff距離是描述兩組點集間相似程度的一種度量,2組點集間的Hausdorff距離越小說明兩者相似程度越高．對于2組有限點集A,B,定義Hausdorff距離[4]

H(A,B)=max{h(A,B),h(B,A)},

(2)

其中h(A,B)=maxa∈A(minb∈B(‖a-b‖)),h(B,A)=maxb∈B(mina∈A(‖a-b‖))．

由于待處理圖像中存在噪聲,所以提取的邊緣點中含出格點,即圖像噪聲導致的偽邊緣點．這里對Hausdorff距離進行改進,設(shè)置閾值β去除出格點,定義點集A到點集B的新有向距離

(3)

Hn(A,B)=max{hn(A,B),hn(B,A)}．

(4)

3 基于三維模型與改進ICP算法的位姿測量方案

受虛擬觀察點采樣間距的限制,匹配得到的位姿精度偏低,故筆者引入ICP算法進行精匹配[7]．由于ICP算法要求2個待匹配點集間的初始位姿不可相差太大[8-10],故選擇Hausdorff距離粗匹配法使得待匹配的2個點集滿足該條件．

設(shè)置目標函數(shù)

(5)

1) 對于點集Bk中的每個點,在點集Ak中找到其最近點．為了剔除偽點對,以改進的Hn(A,B)為閾值,將距離大于該閾值的點對視為偽點對剔除．剔除偽點對后,點集Bk中剩余的點組成點集pk,在點集Ak中找到最近的點組成點集Qk;

2) 對點集Pk,Qk采用單位四元數(shù)法求得(H-1)k;

6) 如果|dk-dk+1|<ε(ε為設(shè)定閾值),則迭代停止;否則,轉(zhuǎn)步驟7);

7) 迭代更新:pk=pk+1;Qk=Qk+1,轉(zhuǎn)入步驟2)．

圖2 基于三維模型和ICP算法的單目視覺位姿測量流程圖Fig.2 Overall block diagram of monocular vision pose measurement

基于三維模型和改進ICP算法的單目位姿測量步驟如下: 利用OpenGL繪制目標物體的三維模型,以經(jīng)緯度和半徑為參數(shù)設(shè)置一系列虛擬觀察點,生成虛擬相機投影圖,提取圖像邊緣構(gòu)成模板庫;對待測圖像進行去噪處理后提取邊緣,以邊緣點集作為特征點集;使用改進的Hausdorff距離閾值法在模板庫里找出與待測圖像最近似的一幅圖像,根據(jù)式(1)計算粗略位姿;再利用相機內(nèi)參和外參矩陣乘積的廣義逆矩陣,將待測圖像二維點集轉(zhuǎn)換為三維點集,結(jié)合物體邊緣在世界坐標系下的三維點集采用ICP算法迭代優(yōu)化得到精確的位姿參數(shù)．其流程如圖2所示．

4 仿真分析

實驗采集數(shù)據(jù)所用相機型號為AVT Manta G125B,采集的圖片分辨率為1 292×964像素(其中焦距f=8.26 mm)．先采集多張標定板圖片,對相機標定得到相機內(nèi)參矩陣的值[11]．將如圖3(a)所示的三維物體擺放在固定位姿,提取當前位姿下所采集圖像的邊緣得到邊緣圖像,與模板庫里的圖像進行匹配得到粗略位姿,再采用ICP算法迭代得到精確位姿．

圖3 三維物體作平移和旋轉(zhuǎn)處理后的位姿圖Fig.3 A sketch of translational and rotational processing of three-dimensional objects

以初始位姿為基準,且其取值為[4.034 12.642 36.820 -2.144 0.440 0.464],前3個參數(shù)分別表示世界坐標系相對于相機坐標系沿X、Y、Z軸平移的距離,單位為cm;后3個參數(shù)分別表示世界坐標系相對于相機坐標系繞三軸旋轉(zhuǎn)的角度,單位為rad．在此基礎(chǔ)上對三維物體作圖3(b)～(e)所示的平移和旋轉(zhuǎn)處理,物體移動后的位姿可依據(jù)其轉(zhuǎn)換關(guān)系計算出期望的位姿,對比實際位姿與期望位姿的誤差驗證算法的有效性．

根據(jù)三維物體與相機的實際位姿關(guān)系離線建立模板庫,設(shè)置虛擬觀察點時的參數(shù)取值φ∈[50,110],δ∈[20,70],r∈[20,30],設(shè)定經(jīng)緯度的步長為2°,半徑的步長為2 cm,得到一系列不同位姿圖,模板庫里的圖片以參數(shù)φ,δ,r的值(r_φ_δ.bmp)命名,部分樣本如圖4所示．

圖4 模板庫部分樣本圖Fig.4 Sample diagram of partial template library

待檢測圖像與模板庫中圖像在提取邊緣后經(jīng)Hausdorff距離匹配算法得到匹配圖像,據(jù)式(1)計算出待測圖像中物體的粗略位姿,再采用ICP算法迭代優(yōu)化,計算所得實際位姿與期望位姿的誤差,結(jié)果如表1～2所示．由表1～2可知,平移處理和旋轉(zhuǎn)處理后位姿參數(shù)的最大誤差分別為0.293 cm,0.029 rad,均可滿足一般機械手抓取任務(wù)的要求[3]．

表1 平移處理后的位姿誤差Tab.1 Posture error after translational processing cm

表2 旋轉(zhuǎn)處理后的位姿誤差Tab.2 Posture error after rotation processing rad

5 結(jié)論

本文針對單目視覺位姿問題,提出了一種基于改進ICP算法和CAD模型庫的單目視覺位姿測量方法．在設(shè)置虛擬觀察點建立模板庫時,通過選擇合理的采樣間距減小模板庫容量,加快了后續(xù)匹配速度;應用改進的Hausdorff距離閾值法剔除圖像噪聲所致的出格邊緣點;設(shè)置閾值剔除偽點對,采用ICP算法迭代優(yōu)化得到精確的位姿參數(shù)．結(jié)果表明本文方案是有效的且具有良好的實際應用價值．