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

郭新年，白瑞林，王秀平

GUO Xinnian,BAI Ruilin,WANG Xiuping

江南大學(xué) 輕工過程先進(jìn)控制教育部重點(diǎn)實(shí)驗(yàn)室 信息與控制實(shí)驗(yà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 引言

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

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

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

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

2 標(biāo)定原理

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

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

設(shè)P為空間中的一點(diǎn)，XW、XH和XC分別為點(diǎn)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òng)，平移后到達(dá)B，B在基坐標(biāo)系的位姿為T62，則兩末端坐標(biāo)系之間關(guān)系為T=(T61)-1T62，其中T的旋轉(zhuǎn)部分為E，平移運(yùn)動(dòng)為B。

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

機(jī)器人末端從A處平行移動(dòng)到B處，機(jī)器人末端平移量為B=k1b，P點(diǎn)在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)動(dòng)，同理可得：b2=Rc2。

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

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

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

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

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

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

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

以上方程兩兩相減可得：

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

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

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

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

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

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

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

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

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

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

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

因此，可求得：

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

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

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

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

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

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

3 實(shí)驗(yàn)與分析

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

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

3.1 特征點(diǎn)的選取和自動(dòng)匹配

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

圖5 特征點(diǎn)選取和匹配及FOE 點(diǎn)求解

3.2 實(shí)驗(yàn)結(jié)果對(duì)比

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

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

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

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

4 結(jié)論

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

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

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

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

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

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

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

[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ì)算機(jī)學(xué)報(bào)，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)動(dòng)的手眼系統(tǒng)結(jié)構(gòu)光參數(shù)標(biāo)定[J].儀器儀表學(xué)報(bào)，2005，26（11）：1101-1105.

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

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

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

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