基于主動視覺的手眼矩陣和光平面標(biāo)定方法

郭新年，白瑞林，王秀平

GUO Xinnian,BAI Ruilin,WANG Xiuping

江南大學(xué) 輕工過程先進(jìn)控制教育部重點實驗室 信息與控制實驗教學(xué)中心，江蘇 無錫214122

Information and Control Experiment Teaching Center, Key Laboratory of Advanced Process Control for Light Industry（Ministry of Education）,Jiangnan University,Wuxi,Jiangsu 214122,China

1 引言

視覺測量與控制在機(jī)器人領(lǐng)域占有重要的地位。結(jié)構(gòu)光視覺系統(tǒng)主要由激光投射器和攝像機(jī)組成，以其抗干擾能力強(qiáng)，實時性好，成本低等優(yōu)點，廣泛應(yīng)用于機(jī)器人焊接、裝配、物體三維形貌測量等。而實現(xiàn)機(jī)器人視覺測量與控制的關(guān)鍵部分是系統(tǒng)參數(shù)的標(biāo)定技術(shù)。

基于主動視覺的標(biāo)定方法通過精確控制攝像機(jī)運(yùn)動獲得多幅圖像，通過確立圖像與攝像機(jī)運(yùn)動之間的關(guān)系來進(jìn)行標(biāo)定。該方法在標(biāo)定過程中由于已知關(guān)于攝像機(jī)的運(yùn)動信息，攝像機(jī)的模型參數(shù)可以線性求解，且算法簡單，魯棒性較高，適用于視覺傳感器安裝在機(jī)器人末端的手眼系統(tǒng)的標(biāo)定。

基于主動視覺的標(biāo)定方法最早用于標(biāo)定攝像機(jī)內(nèi)參數(shù)，Ma[1]在1996 年提出的基于兩組三正交平移運(yùn)動的攝像機(jī)自定標(biāo)方法，通過控制攝像機(jī)做兩組三正交平移運(yùn)動，利用擴(kuò)展焦點（FOE）的性質(zhì)線性求解攝像機(jī)內(nèi)參數(shù)；其后，李華等[2]給出利用5 組平面正交運(yùn)動標(biāo)定攝像機(jī)所有的5 個內(nèi)參數(shù)的五參數(shù)模型的線性方法；同樣，主動視覺的標(biāo)定方法被引入手眼矩陣的標(biāo)定，楊廣林等[3]通過控制攝像機(jī)做兩次平移運(yùn)動和一次旋轉(zhuǎn)運(yùn)動，僅利用場景中兩個特征點，給出了機(jī)器人手眼矩陣標(biāo)定方法；王海霞等[4]等深入分析了eye-in-hand 系統(tǒng)，將攝像機(jī)內(nèi)參數(shù)和手眼矩陣旋轉(zhuǎn)部分看作一個“黑箱”直接求解，不需要分別確定系統(tǒng)各部分的每個參數(shù)，既降低了計算量又減少了誤差；Xu 等[5]設(shè)計了一種基于場景中單個特征點的手眼矩陣標(biāo)定方法，該方法以運(yùn)動前后特征點的相對位置表征攝像機(jī)坐標(biāo)系的平移，避免求解FOE 點的問題，達(dá)到了很高的精度。在光平面標(biāo)定方面，徐德等[6]提出基于主動視覺的標(biāo)定方法，該方法需要3 次及以上的攝像機(jī)姿態(tài)，且激光條打在的平面方程已知，還需用測量尺對激光條上特定點間的距離進(jìn)行測量，雖然達(dá)到了較高的標(biāo)定精度，但標(biāo)定過程復(fù)雜；陳天飛等[7]通過多組平移運(yùn)動，采用四參數(shù)平面模型，分別標(biāo)定光平面的法向量，和光平面的深度信息，達(dá)到了很高的標(biāo)定精度。

主動視覺平臺尤其是機(jī)器人的不斷使用，為基于主動視覺標(biāo)定提供了硬件前提。為解決現(xiàn)有基于主動視覺方法標(biāo)定光平面操作較復(fù)雜的問題，本文提出一種同時標(biāo)定手眼矩陣和光平面的方法，通過控制機(jī)器人做兩次相互正交的平移運(yùn)動，求解手眼矩陣的旋轉(zhuǎn)部分；而后通過兩次及以上帶旋轉(zhuǎn)運(yùn)動，求解手眼矩陣的平移部分，同時可得到特征點在機(jī)器人基坐標(biāo)系信息，進(jìn)而可得到光平面方程。

2 標(biāo)定原理

在內(nèi)參數(shù)已知的前提下，控制機(jī)器人進(jìn)行兩次相互正交的平移運(yùn)動，機(jī)器人末端平移信息由控制器讀出，結(jié)合由FOE 求出的攝像機(jī)的運(yùn)動方向，可求解手眼矩陣的旋轉(zhuǎn)部分[3]；進(jìn)行兩次及以上的帶旋轉(zhuǎn)運(yùn)動，求解手眼矩陣的平移部分和各特征點在機(jī)器人基坐標(biāo)系的深度值；無需求解外參數(shù)，直接通過機(jī)器人基坐標(biāo)系與末端坐標(biāo)系之間關(guān)系T6i和求得的手眼矩陣得到機(jī)器人基坐標(biāo)系與攝像機(jī)坐標(biāo)系的關(guān)系，利用對偶關(guān)系求取光平面方程[8]。

2.1 手眼系統(tǒng)中坐標(biāo)變換關(guān)系

結(jié)構(gòu)光手眼系統(tǒng)中機(jī)器人基坐標(biāo)系、機(jī)器人末端坐標(biāo)系和攝像機(jī)坐標(biāo)系之間的關(guān)系，如圖1 所示。

圖1 結(jié)構(gòu)光手眼系統(tǒng)關(guān)系圖

可得到機(jī)器人手眼系統(tǒng)中的坐標(biāo)變換關(guān)系如下[4]：

其中XW是特征點在機(jī)器人基坐標(biāo)系的坐標(biāo)，ε是特征點在攝像機(jī)坐標(biāo)系的深度值，RHW、tHW是機(jī)器人基坐標(biāo)系與末端坐標(biāo)系之間關(guān)系的旋轉(zhuǎn)與平移部分；R、t是機(jī)器人末端坐標(biāo)系與攝像機(jī)坐標(biāo)系之間的關(guān)系即手眼矩陣的旋轉(zhuǎn)與平移部分；(u,v,1)T是特征點圖像的齊次坐標(biāo)；K是攝像機(jī)內(nèi)參矩陣五參數(shù)模型。

2.2 手眼矩陣旋轉(zhuǎn)部分的標(biāo)定

設(shè)P為空間中的一點，XW、XH和XC分別為點P在機(jī)器人基坐標(biāo)系、機(jī)器人末端坐標(biāo)系和攝像機(jī)坐標(biāo)系下的坐標(biāo)，RHW和tHW表示機(jī)器人基坐標(biāo)系與機(jī)器人末端坐標(biāo)系之間的關(guān)系，P在各坐標(biāo)系的關(guān)系為：

如圖2 所示，機(jī)器人初始位姿A在基坐標(biāo)下的位姿為T61，控制機(jī)器人沿末端坐標(biāo)系平移運(yùn)動，平移后到達(dá)B，B在基坐標(biāo)系的位姿為T62，則兩末端坐標(biāo)系之間關(guān)系為T=(T61)-1T62，其中T的旋轉(zhuǎn)部分為E，平移運(yùn)動為B。

圖2 平移運(yùn)動示意圖

機(jī)器人末端從A處平行移動到B處，機(jī)器人末端平移量為B=k1b，P點在A處和B處機(jī)器人末端坐標(biāo)系下的坐標(biāo)分別為XH和XH1=XH+B，在攝像機(jī)坐標(biāo)系下的坐標(biāo)分別是XC和XC1，則有：

由上式得：

即B=RC，其中B=k1b為機(jī)器人末端平移量，b為末端平移量的單位向量，C=k1c表示攝像機(jī)平移量，c1為攝像機(jī)平移向量的單位向量。

由于旋轉(zhuǎn)矩陣只改變向量的方向，不改變向量的大小，可以將C、B變換為單位向量后代人求解旋轉(zhuǎn)部分R，即：

機(jī)器人進(jìn)行一組正交平移運(yùn)動，同理可得：b2=Rc2。

假設(shè)運(yùn)動b3=b1×b2與b1、b2構(gòu)成三正交運(yùn)動，則c3=c1×c2與c1、c2也 構(gòu) 成 三 正 交 運(yùn) 動[3]，同 時 可 得：b3=Rc3。

b1、b2、b3三個列向量構(gòu)成矩陣P，c1、c2、c3構(gòu)成矩陣Q，即得P=RQ，可得手眼矩陣旋轉(zhuǎn)部分為R=PQ-1。

理論上，R、P和Q均為正交矩陣，但由于機(jī)器人與圖像誤差的存在，得到的b1、b2和c1、c2可能不相互正交，此時可將b1、b2和c1、c2施密特正交化后，再計算b3和c3，進(jìn)而求解手眼矩陣旋轉(zhuǎn)部分R。

2.3 手眼矩陣平移部分和光平面的標(biāo)定

2.3.1 手眼矩陣平移部分標(biāo)定

選取場景中不在同一直線上的3 個特征點，進(jìn)行兩次帶旋轉(zhuǎn)運(yùn)動，每次帶旋轉(zhuǎn)運(yùn)動后，取帶特征點的圖像和打開激光器后帶激光條紋的圖像。由結(jié)構(gòu)光手眼系統(tǒng)之間的關(guān)系（1）可得：

其中，RHWj、tHWj(j=1,2,3)是機(jī)器人在第j個位姿下基坐標(biāo)系與末端坐標(biāo)系之間關(guān)系的旋轉(zhuǎn)與平移部分，可以通過機(jī)器人控制器讀出；nij=(uij,vij,1)T(i,j=1,2,3)是第i個特征點在第j個位姿下特征點圖像的齊次坐標(biāo)。

以上方程兩兩相減可得：

其中Ai1=RHW1RK-1ni1，Bi1=RHW1-RHW2，Ci1=tHW2-tHW1，其余類推。將上式寫成矩陣形式：

通過最小二乘求解每個位姿下每個特征點的深度信息和手眼矩陣平移部分t。

2.3.2 光平面方程標(biāo)定

將求得的εij(i,j=1,2,3)帶入方程（7），可得每個特征點在機(jī)器人基坐標(biāo)系的3 個坐標(biāo)值，取均值作為該點坐標(biāo)。如圖3 所示，不在同一直線上的3 個點可構(gòu)成一平面，激光條打在3 個特征點構(gòu)成的平面上。

圖3 光平面深度信息確定示意圖

可得3 個特征點構(gòu)成的平面方程為：

在攝相機(jī)坐標(biāo)系中，設(shè)w代表光平面的法向量，wi、wj為第i、j個位姿下的特征點構(gòu)成平面的法向量，?i、?j為第i、j個位姿下的光條直線的方向向量。第i個位姿下圖像上的光條的方程?i（圖像坐標(biāo)系下）與?i（攝像機(jī)坐標(biāo)系下）可由下式得出：

投影空間中兩條激光交線及其相關(guān)聯(lián)的平面的相互關(guān)系的對偶性表示如圖4 所示。

圖4 兩條激光交線在投影空間的對偶表示

圖4 中，要標(biāo)定的光平面坐標(biāo)w在投影空間的對偶表示就是兩條激光交線的交點。由于誤差的影響，兩條激光交線在空間并不相交，因此用距離兩條激光線最近的點表示w。而距離兩條激光線最近的點位于兩條激光線的公垂線上。光平面坐標(biāo)為：

由于公垂線與兩條激光線分別垂直，因此內(nèi)積：(w1-w2,wi)=0，(w1-w2,wj)=0。

因此，可求得：

將以上值分別帶入可得光平面方程坐標(biāo)w。經(jīng)兩次帶旋轉(zhuǎn)運(yùn)動后形成三個位姿下的光條圖像，兩兩組合求解光平面方程后求均值，可以減少誤差。

綜上所述，本文提出的基于主動視覺標(biāo)定手眼矩陣和光平面的方法，具體步驟如下：

（1）精確控制機(jī)器人進(jìn)行兩次相互正交的平移運(yùn)動，用2.2 節(jié)的方法求解R。

（2）通過給定3 個位姿T6j(j=1,2,3)實現(xiàn)兩次帶旋轉(zhuǎn)運(yùn)動；提取場景中特征點的圖像坐標(biāo)nij；并在每個位姿處分別打開激光器拍攝帶激光條圖像，提取光條直線，求解光條直線在圖像坐標(biāo)系的方向向量。

（3）根據(jù)nij和對應(yīng)的位姿T6j，依據(jù)文章2.3.1 小節(jié)的方法求解手眼矩陣平移部分t和特征點深度信息。

（4）通過內(nèi)參數(shù)K，求解光條直線在攝像機(jī)坐標(biāo)系的方向向量，結(jié)合每個位姿下特征點構(gòu)成的平面在攝像機(jī)坐標(biāo)系下的法向量，將位姿兩兩組合，根據(jù)2.3.2 小節(jié)的方法求解光平面方程。

3 實驗與分析

實驗使用DENSO VP-6242E/GM 六軸機(jī)器人，其重復(fù)定位精度0.02 mm，攝像機(jī)使用XINJE 智能攝像機(jī)，感光CCD 為640×480，使用Computar 工業(yè)鏡頭，焦距為8 mm；算法驗證基于Win7 系統(tǒng)，MATLAB 2011b平臺。

為方便與文獻(xiàn)[9]進(jìn)行結(jié)果比較，相機(jī)內(nèi)參數(shù)采用文獻(xiàn)[9]給出的張標(biāo)定方法。得到的內(nèi)參數(shù)K為：

3.1 特征點的選取和自動匹配

特征點使用激光打印機(jī)打印的非等腰直角三角形，特征點的求解采用文獻(xiàn)[10]的方法，精度可達(dá)到0.2 pixel。匹配方法根據(jù)三特征點之間距離不同進(jìn)行匹配，具體描述如下：角點提取后，求解特征角點兩兩之間的距離，選擇除距離最大的兩點外的一點作為編號1 點，另兩點中，離1 點距離遠(yuǎn)的為編號2 點，剩余的為編號3 點；取三點對應(yīng)連線的交點的均值作為FOE。為避免聚焦模糊問題，平移運(yùn)動的Z方向分量不超過10 cm；為提高FOE 精度，每次平移運(yùn)動的方向在XOY平面的分量可沿三角形的中線方向[4]；且平移過程中取30 幅圖片，提取對應(yīng)角點，通過最小二乘擬合成直線，優(yōu)化求解FOE點。如圖5 所示，（a）為特征角點的提取和自動匹配，（b）為運(yùn)動中取30 幅圖像優(yōu)化求解FOE 點。

圖5 特征點選取和匹配及FOE 點求解

3.2 實驗結(jié)果對比

本文標(biāo)定結(jié)果與文獻(xiàn)[9]標(biāo)定結(jié)果如表1，可見本文標(biāo)定結(jié)果與文獻(xiàn)[9]給出結(jié)果接近。

3.3 三維數(shù)據(jù)測試

由于機(jī)器人重復(fù)定位精度可達(dá)到0.02 mm，所以通過機(jī)器人對三維點的點觸得到的坐標(biāo)值，作為驗證結(jié)構(gòu)光手眼系統(tǒng)的標(biāo)定精度的基準(zhǔn)。在固定姿態(tài)下，控制機(jī)器人工件末端點觸標(biāo)定中的特征點1，得到工件末端坐標(biāo)系原點在機(jī)器人基坐標(biāo)系的修正值t0，實驗中t0=-[ ]15.86,11.18,277.32，而后以該姿態(tài)點觸測試點，機(jī)器人控制器讀出坐標(biāo)直接加上偏移值t0，即為該點在機(jī)器人基坐標(biāo)系的值。

實驗中三維測試結(jié)果如表2 所示，其中1～3 為標(biāo)定特征點的平面測試數(shù)據(jù)，最大誤差為±0.25 mm，4～9 為三維階梯測試數(shù)據(jù)，最大誤差±1.04 mm。

4 結(jié)論

為實現(xiàn)結(jié)構(gòu)光手眼系統(tǒng)的標(biāo)定，解決現(xiàn)有基于主動視覺方法標(biāo)定手眼矩陣和光平面操作較復(fù)雜的問題，提出一種基于主動視覺的標(biāo)定方法，該方法具有以下特點：

表1 本文標(biāo)定結(jié)果與文獻(xiàn)[9]結(jié)果比較

表2 平面和三維數(shù)據(jù)測試結(jié)果

（1）該標(biāo)定方法無需使用特定靶標(biāo)，通過精確控制機(jī)器人做兩次相互正交的平移運(yùn)動和兩次帶旋轉(zhuǎn)運(yùn)動即可實現(xiàn)手眼矩陣和光平面的標(biāo)定，本質(zhì)上屬于自標(biāo)定范疇。

（2）改進(jìn)利用對偶關(guān)系標(biāo)定光平面方程的算法，無需求解外參數(shù)。

（3）在特征點高精度選取后，采用平移過程中取多幅圖片擬合運(yùn)動的方法減少求解FOE 點誤差，適于工業(yè)現(xiàn)場使用。

實驗結(jié)果表明，本文方法魯棒性好，標(biāo)定得到的手眼矩陣和光平面方程穩(wěn)定，三維測試誤差為±1.04 mm，可滿足機(jī)器人焊接、抓取等工業(yè)現(xiàn)場的精度要求。

[1] Ma S D.A self-calibration technique for active vision systems[J].IEEE Trans on Robotics and Automation，1996，12（1）：114-120.

[2] 李華，吳福朝，胡占義.一種新的線性攝像機(jī)自標(biāo)定方法[J].計算機(jī)學(xué)報，2000，23（11）：1121-1129.

[3] 楊廣林，孔令富，王潔.一種新的機(jī)器人手眼關(guān)系標(biāo)定方法[J].機(jī)器人，2006，28（4）：400-405.

[4] 王海霞，王春艷，盧曉.手眼視覺系統(tǒng)的自標(biāo)定技術(shù)[C]//Proceedings of the 26th Chinese Control Conference，2007：212-215.

[5] Xu Haixia，Wang Yaonan，Chen Wei，et al.A self-calibration approach hand-eye relation using a single point[C]//Proceedings of the International Conference on Information and Automation，2008：413-418.

[6] 徐德，王麟琨，譚民.基于運(yùn)動的手眼系統(tǒng)結(jié)構(gòu)光參數(shù)標(biāo)定[J].儀器儀表學(xué)報，2005，26（11）：1101-1105.

[7] 陳天飛，馬孜，吳翔.基于主動視覺標(biāo)定線結(jié)構(gòu)光傳感器中的光平面[J].光學(xué)精密工程，2012，20（2）：256-262.

[8] 畢德學(xué)，劉方滔，薛強(qiáng)，等.基于激光交線的結(jié)構(gòu)光視覺傳感器現(xiàn)場標(biāo)定新方法[J].儀器儀表學(xué)報，2009，30（8）：1697-1701.

[9] 李龍，白瑞林，吉峰，等.結(jié)構(gòu)光視覺引導(dǎo)的軌跡跟蹤系統(tǒng)的標(biāo)定技術(shù)[J].計算機(jī)工程與應(yīng)用，2014，50（16）：259-264.

[10] 顧國慶，白瑞林，杜斌.基于曲率多尺度的高精度角點檢測[J].光學(xué)技術(shù)，2010，36（6）：918-922.