結構光視覺引導的軌跡跟蹤系統的標定技術

李 龍，白瑞林，吉 峰，郭新年

LILong1,BAIRuilin1,JIFeng2,GUO Xinnian1

1.江南大學 輕工過程先進控制教育部重點實驗室，江蘇 無錫 214122

2.無錫信捷電氣有限公司，江蘇 無錫 214072

1.Key Laboratory of Advanced Process Control for Light Industry（Ministry of Education）,Jiangnan University,Wuxi,Jiangsu 214122,China

2.Xinje Electronic Co.,Ltd,Wuxi,Jiangsu 214072,China

1 引言

視覺測量與控制在機器人領域占有重要的地位。在工業機器人領域，視覺主要用于目標和機器人末端位姿的測量以及對機器人末端位姿的控制，其典型應用包括焊接、噴漆、裝配、搬運等作業。視覺測量與控制的關鍵部分就是標定技術。

劉蘇宜等[1]提出了一種同時標定攝像機和機器人的方法。依據機器人手眼矩陣和機器人手爪對基坐標系位姿矩陣之間的特定關系，一次標定求解出攝像機參數和機器人手眼關系矩陣。謝凱等[2]將BP神經網絡與線性標定方法結合，不僅標定了結構光三維檢測CCD相機的內外參數，而且利用神經網絡的非線性逼近能力，克服了鏡頭徑向畸變、切向畸變等引起的系統非線性誤差；Jan Heller[3]等提出了一種手眼標定新方法。在使用校準靶標不可行的情況下，采用結構的運動方式進行手眼標定；賀俊吉等人[4]探討了被多種噪聲干擾的光條圖像的處理和精確提取光條中心的方法，給出了一種消除環境干擾的濾波模板并實驗驗證了其濾波效果，使光條中心的提取精度達到更高。

本文在充分分析結構光視覺引導系統的基礎上，提出了一種新的標定方法，采用平面靶標，充分利用實驗數據，簡化標定流程，控制機械臂以3個以上任意位姿拍攝固定靶標，即可實現相機標定、線激光光平面方程標定和手眼矩陣的標定，為精確跟蹤提供了充分條件。

2 標定原理

基于結構光視覺傳感器引導的機器人跟蹤系統的標定流程由相機標定、線激光光平面方程標定、手眼變換矩陣標定以及工件偏移標定四部分組成，控制機械臂以3個以上任意位姿拍攝固定靶標，利用得到的圓靶標圓心像素坐標以及其與世界坐標的匹配數據完成相機內外參計算，利用得到的外參和靶標圖片提取的線激光線方程標定出激光光平面方程[5]，利用每兩次機械臂移動中的外參變化和機械臂位姿變化構造AX=XB方程，并利用四元數法[6]求解手眼變換矩陣H既方程中的X。

2.1 靶標圓心點像素坐標與世界坐標匹配

自制靶標如圖1所示，其同時適用于相機標定和手眼變換矩陣標定過程中，并且其匹配方法簡單，匹配結果穩定，能有效簡化整個系統的標定復雜度。

圖1 自制靶標

具體匹配步驟如下：

（1）實時采集帶圓點靶標圖像，采用大津法對圖像二值化；對圖像進行閉運算，去除噪聲干擾；利用聚類算法，對圖像中目標進行分類。

（2）計算目標周長C和面積S，利用圓形度表達式e=C2/(2×π×S)，區分圓點與干擾。

（3）采用重心法求取每一個圓點的圓心坐標。

（4）靶標為自制靶標，相鄰兩行之間的行距為15 mm，同一行中相鄰的2圓心點距離為10 mm，在第一行的第一個點行方向上5 mm處有一個額外的圓，首先對所有圓心點進行排序，排序過程如下：

①找出所有有效圓點的圓心距離最小的兩個點，其中離所有圓心點均值中心遠的圓的定義為0點，另一個定義為1點。

②設置0，1點為已匹配點，在未匹配點中找與1點最近的點定義2點。

③設置2點為已匹配點，在未匹配點中找與2點最近的點定義3點，…以此類推直至把所有點都找全，完成所有圓心點的排序。

（5）根據上述圓靶標圖像中圓點排序，與給定圓靶標信息，完成圓靶標圖像中圓心點像素坐標與世界坐標的匹配。

2.2 結構光視覺傳感器標定

結構光視覺傳感器標定主要有相機標定和激光光平面方程標定兩部分組成，其中相機標定采用張正友標定法[7]，以不同位姿拍攝3幅以上靶標圖片，進行靶標匹配后，用張正友方法求取相機內參并同時求得相機外參。

在拍攝每一幅靶標圖片的位姿后，打開線激光，拍攝一幅帶激光光條的靶標圖片，提取光條，細化光條，通過Hough變換求出光條線方程。結合上一步得到的外參矩陣利用投影空間中兩條激光交線及其相關聯的平面的對偶關系就可以得到線激光光平面在相機坐標系下的平面方程。

基于交線圖像的激光平面標定的原理如下，圓靶標平面在世界坐標系下的平面方程可以表示為(π1，x)=0，式中：π1=[0，0，1，0]T，利用第一步得到的外參數矩陣RT，計算得到在相機坐標系下圓靶標平面方程為(RT-Tπ1，x′)=0 ；其中的 x 與 x′分別為靶標平面在世界坐標系和相機坐標系下的平面法向坐標向量。

在相機坐標系中，設 w、wi、wj、λi、λj分別代表激光平面法向坐標向量、第i個平面靶標的法向坐標向量、第 j個平面靶標的法向坐標向量、第i個平面靶標內的激光交線圖像的坐標向量、第 j個平面靶標內的激光交線圖像的坐標向量。注意第i個平面靶標圖像上的激光交線圖像的方程 λi（像素坐標系下）與 λi（相機坐標系下）可以直接計算出來：

其中A為相機內參矩陣。

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

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

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

由于公垂線與兩條激光線分別垂直，因此內積：

因此，可求得：

式中：D是一個2×2矩陣，b是一個二維向量，定義如下：

將式（7）（8）的值帶入式（2）和（3），就會求得激光平面方程坐標w。

為了提高標定的精度，通常情況下選擇多幅位于不同位姿的平面靶標圖像進行標定。其中任意兩幅就標定了一個光平面方程坐標。因此n個平面靶標圖像，則可以標定個光平面方程標定參數。對每項排序，去除最大和最小值并求剩下的平均值來減少標定的光平面參數的誤差。

2.3 手眼變換矩陣標定

本跟蹤系統采用Eye-in-Hand系統，即視覺傳感器固定在機械臂末端，相機坐標系相對于機械臂末端坐標系的關系是固定的。因此，對于Eye-in-Hand系統，手眼標定時求取的是相機坐標系相對于機械臂末端坐標系的關系。

對于手眼關系矩陣，直接測量幾乎是不可能的，如圖3，通過改變機械臂位姿，根據坐標變換鏈的封閉性可得：

其中TA1，TA2表示機器人變換位姿前后機械臂末端坐標系相對于機械臂基坐標系的位姿變換矩陣；TB1，TB2表示機器人變換位姿前后標靶所在世界坐標系相對于相機坐標系的齊次變換矩陣。

圖3 手眼關系圖

2.3.1 四元數與旋轉矩陣變換

采用四元數可以描述通用旋轉變換[8]。四元數是實數和復數以及三維空間點矢量的擴展，由實數部分s∈R和矢量部分v=(x，y，z)∈R3組成。四元數表示形式為：

式中，i2=j2=k2=ijk=-1，ij=k，jk=i，ki=j，ji=-k，kj=-i，ik=-j。當||q||=1時，稱q為單位四元數。四元數可以描述三維剛體的姿態和旋轉變換如圖4，其理論基礎是三維空間中繞單位軸n旋轉2θ角可以用四元數q=[s，v]=[cosθ，sinθn]來描述。

圖4 四元數描述三維旋轉示意圖

與單位四元數q=[s，(a，b，c)]對應的姿態矩陣為：

式中：

2.3.2 利用四元數法求解AX=XB

齊次變換矩陣A和B為已知參數，這里采用兩步法求解該方程，把上式齊次矩陣分解成旋轉和平移兩部分：

展開得：

令 qA=[a0，aT]T，qx=[x0，xT]T，qB=[b0，bT]T為 RA，Rx，RB對應的四元數，方程RARx=RxRB可以表示為：

利用四元數乘法展開上式得：

因為

得到a0=b0，由上式可得：

令 z=x/x0，則

實驗過程中可以利用等式a0=b0檢查測試數據的準確性，并進行錯誤數據的剔除。

式（21）為形如Gz=c的形式，唯一的未確定z至少需要機械臂做兩次運動。且兩次運動的旋轉軸不能平行。對于多次運動，使用最小二乘法確定z之后利用約束條件||qx||=1，即可得到qx。將得到的旋轉矩陣代入式（16）得到平移向量tx。

2.4 工件修正

實驗采用的是DENSO 6軸機械臂，通過發送六軸位姿來控制機械臂，在焊縫跟蹤實驗中，機械臂末端需要夾持焊槍，需要對末端工件即焊槍偏移進行修正，機械臂位姿參數可以分兩部分，旋轉部分和平移部分，所提取的焊縫特征點三維數據最后穩定的保存格式是在機械臂基坐標系下的坐標，工件的偏移修正必須轉換到相應旋轉位姿下的機械臂基坐標系的偏移向量。

具體修正方法如下：

（1）在靶標所在世界坐標系下定義一個預設點，通過之前計算得到的相機內參，外參，手眼變換矩陣以及當前機械臂位姿矩陣，計算得到預設點在機械臂基坐標下的坐標值x。

（2）控制機械臂點觸預設點，讀取當前機械臂位姿，通過當前機械臂位姿得到機械臂末端坐標系與機械臂基坐標系的轉換矩陣T6。

（4）使用時根據焊縫特征點在圖像中所處的位置以及焊縫特征點所經過曲線切線方向，調整機械臂位姿的旋轉部分，可以得到當前旋轉位姿下機械臂末端坐標系與機械臂基坐標旋轉變換矩陣T6'。

（5）x1=R6'*x′，通過上式計算得到工件在機械臂當前旋轉姿態下的基坐標偏移量x1，R6'為T6'中的旋轉部分。

3 實驗測試與分析

本實驗標定過程理論測試操作在Win7系統的MATLAB 2009a平臺上完成和實現，實時跟蹤在VS2010平臺上完成，computar工業鏡頭，12 mm焦距，感光元件CCD 640×480。

標定實驗流程如圖5所示。

圖5 標定流程圖

3.1 匹配測試

測試相機從各個角度拍攝靶標得到的匹配結果如圖6所示，其中圖（a）~（d）為從各個角度拍攝的靶標圖片，（a1）~（d1）為對應的匹配結果圖，其中藍點表示各個圓心點，紅線表示圓心點排序示意。實驗結果表明在相機光軸與靶標平面法向向量夾角小于45°時匹配結果魯棒性強。

3.2 平面數據測試

使用在計算手眼矩陣時得到的機械臂基坐標與由靶標定義的世界坐標系的轉換矩陣。測試靶標上的點在機械臂基坐標系下的坐標計算值與實際值的誤差。實驗數據如表1。

其中機械臂末端旋轉矩陣表示方式為ZYX，工件偏差T7=[-42.41，0.82，350.33]，當前旋轉位姿參數為 Rx=177.36，Ry=0.26，Rz=75.18，測試最大誤差±0.28 mm。

3.3 三維數據測試

圖6 匹配結果圖

結合標定好的相機內參，線激光光平面方程，手眼矩陣，工件偏移參數，對三維階梯形數據進行測試，測試結果如表2，其中最大誤差為±1.16 mm。

多次實驗結果表明采用光軸繞靶標平面法向量轉動，所得到的實驗數據精度較高。

4 結論

為實現工業現場三位特征點的跟蹤，提出一種基于結構光視覺，六軸機械臂的整體標定方法，主要特點是：

（1）采用自制圓靶標，簡化圓心點提取和特征點匹配算法，提高匹配算法的魯棒性，極大減少運算時間。

（2）改進利用對偶關系求取線激光光平面方程的算法，避免錯誤數據對標定結果的影響。

（3）利用四元數求解AX=XB方程，并創新采用四元數錯誤數據剔除算法，使得標定結果受誤差干擾大幅度減小。

（4）針對焊接工件的不確定性，通過簡單的點觸方法，修正在基坐標下的工件偏移。

實際測試表明：該方法魯棒性好，能得到穩定的匹配結果，標定得到的相機內參，手眼矩陣和線激光光平面方程穩定，平面跟蹤可達到±0.3 mm精度，三維跟蹤可達到±1.2 mm精度，滿足工業現場應用要求。

表1 平面測試數據

表2 三維測試數據

[1]劉蘇宜，王國榮，石永華.激光視覺機器人焊接中攝像機和手眼的同時標定[J].華南理工大學學報，2008，36（2）：75-77.

[2]謝凱，劉宛予.結構光三維視覺檢測系統的標定方法研究[J].光電子·激光，2007，18（3）：369-371.

[3]Heller J，Havlena M.Structure-from-motion based hand-eye calibration using L∞minimization[C]//Computer Vision and Pattern Recognition（CVPR），2011：3497-3503.

[4]賀俊吉，張廣軍.結構光三維視覺檢測中光條圖像處理方法研究[J].北京航空航天大學學報，2003，29（7）：593-597.

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

[6]李愛國.結構光視覺測量機器人標定技術研究[D].大連：大連海事大學，2009.

[7]孟海崗.基于平面約束的CCD相機標定方法改進[D].長春：吉林大學，2009.

[8] 劉俊峰.三維轉動的四元數表示[J].大學物理，2004，23（4）：39-43.