基于主動視覺的手眼矩陣和光平面標定方法

郭新年，白瑞林，王秀平

GUO Xinnian,BAI Ruilin,WANG Xiuping

江南大學 輕工過程先進控制教育部重點實驗室 信息與控制實驗教學中心，江蘇 無錫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 引言

視覺測量與控制在機器人領域占有重要的地位。結構光視覺系統主要由激光投射器和攝像機組成，以其抗干擾能力強，實時性好，成本低等優點，廣泛應用于機器人焊接、裝配、物體三維形貌測量等。而實現機器人視覺測量與控制的關鍵部分是系統參數的標定技術。

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

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

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

2 標定原理

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

2.1 手眼系統中坐標變換關系

結構光手眼系統中機器人基坐標系、機器人末端坐標系和攝像機坐標系之間的關系，如圖1 所示。

圖1 結構光手眼系統關系圖

可得到機器人手眼系統中的坐標變換關系如下[4]：

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

2.2 手眼矩陣旋轉部分的標定

設P為空間中的一點，XW、XH和XC分別為點P在機器人基坐標系、機器人末端坐標系和攝像機坐標系下的坐標，RHW和tHW表示機器人基坐標系與機器人末端坐標系之間的關系，P在各坐標系的關系為：

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

圖2 平移運動示意圖

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

由上式得：

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

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

機器人進行一組正交平移運動，同理可得：b2=Rc2。

假設運動b3=b1×b2與b1、b2構成三正交運動，則c3=c1×c2與c1、c2也 構 成 三 正 交 運 動[3]，同 時 可 得：b3=Rc3。

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

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

2.3 手眼矩陣平移部分和光平面的標定

2.3.1 手眼矩陣平移部分標定

選取場景中不在同一直線上的3 個特征點，進行兩次帶旋轉運動，每次帶旋轉運動后，取帶特征點的圖像和打開激光器后帶激光條紋的圖像。由結構光手眼系統之間的關系（1）可得：

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

以上方程兩兩相減可得：

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

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

2.3.2 光平面方程標定

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

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

可得3 個特征點構成的平面方程為：

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

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

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

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

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

因此，可求得：

將以上值分別帶入可得光平面方程坐標w。經兩次帶旋轉運動后形成三個位姿下的光條圖像，兩兩組合求解光平面方程后求均值，可以減少誤差。

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

（1）精確控制機器人進行兩次相互正交的平移運動，用2.2 節的方法求解R。

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

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

（4）通過內參數K，求解光條直線在攝像機坐標系的方向向量，結合每個位姿下特征點構成的平面在攝像機坐標系下的法向量，將位姿兩兩組合，根據2.3.2 小節的方法求解光平面方程。

3 實驗與分析

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

為方便與文獻[9]進行結果比較，相機內參數采用文獻[9]給出的張標定方法。得到的內參數K為：

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

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

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

3.2 實驗結果對比

本文標定結果與文獻[9]標定結果如表1，可見本文標定結果與文獻[9]給出結果接近。

3.3 三維數據測試

由于機器人重復定位精度可達到0.02 mm，所以通過機器人對三維點的點觸得到的坐標值，作為驗證結構光手眼系統的標定精度的基準。在固定姿態下，控制機器人工件末端點觸標定中的特征點1，得到工件末端坐標系原點在機器人基坐標系的修正值t0，實驗中t0=-[ ]15.86,11.18,277.32，而后以該姿態點觸測試點，機器人控制器讀出坐標直接加上偏移值t0，即為該點在機器人基坐標系的值。

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

4 結論

為實現結構光手眼系統的標定，解決現有基于主動視覺方法標定手眼矩陣和光平面操作較復雜的問題，提出一種基于主動視覺的標定方法，該方法具有以下特點：

表1 本文標定結果與文獻[9]結果比較

表2 平面和三維數據測試結果

（1）該標定方法無需使用特定靶標，通過精確控制機器人做兩次相互正交的平移運動和兩次帶旋轉運動即可實現手眼矩陣和光平面的標定，本質上屬于自標定范疇。

（2）改進利用對偶關系標定光平面方程的算法，無需求解外參數。

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

實驗結果表明，本文方法魯棒性好，標定得到的手眼矩陣和光平面方程穩定，三維測試誤差為±1.04 mm，可滿足機器人焊接、抓取等工業現場的精度要求。

[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].計算機學報，2000，23（11）：1121-1129.

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

[4] 王海霞，王春艷，盧曉.手眼視覺系統的自標定技術[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] 徐德，王麟琨，譚民.基于運動的手眼系統結構光參數標定[J].儀器儀表學報，2005，26（11）：1101-1105.

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

[8] 畢德學，劉方滔，薛強，等.基于激光交線的結構光視覺傳感器現場標定新方法[J].儀器儀表學報，2009，30（8）：1697-1701.

[9] 李龍，白瑞林，吉峰，等.結構光視覺引導的軌跡跟蹤系統的標定技術[J].計算機工程與應用，2014，50（16）：259-264.

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