基于立體視覺的機(jī)器人位姿標(biāo)定技術(shù)研究

吳年祥 ，鄒華東

（1.中國地質(zhì)大學(xué)自動(dòng)化學(xué)院，湖北，武漢 430074；2.安徽國防科技職業(yè)學(xué)院機(jī)電工程系，安徽，六安 237001）

0 引言

隨著工業(yè)技術(shù)的迅速發(fā)展，機(jī)器人技術(shù)在機(jī)械、機(jī)電、汽車、電子等行業(yè)擔(dān)任著重要角色。在柔性自動(dòng)生產(chǎn)線中，機(jī)器人機(jī)械手的抓取、放置、裝配、焊接等應(yīng)用最廣泛。然而，產(chǎn)品競(jìng)爭(zhēng)日益激烈，產(chǎn)品質(zhì)量的不斷提高，要求工業(yè)機(jī)器人能在機(jī)電一體化等生產(chǎn)中完成三維空間精確操作，因此，基于視覺的機(jī)器人外部位姿標(biāo)定技術(shù)的研究顯得尤為重要。機(jī)器人外部位姿是指確定機(jī)器人世界坐標(biāo)系和基礎(chǔ)坐標(biāo)系及工件坐標(biāo)系之間的關(guān)系[1]。而在機(jī)器人基坐標(biāo)系固定情況下，通常將其與世界人坐標(biāo)系設(shè)置重合，從而確定機(jī)器人基坐標(biāo)系與工件坐標(biāo)系之間的坐標(biāo)變換關(guān)系即機(jī)器人外部位姿。本文通過分步標(biāo)定法之一的機(jī)器人外部位姿來確定機(jī)器人工件坐標(biāo)系與基坐標(biāo)系之間的坐標(biāo)變換關(guān)系，從而確定機(jī)器人工件坐標(biāo)系與立體視覺傳感器坐標(biāo)系之間的關(guān)系[2]。

1 機(jī)器人雙目立體視覺傳感器的三坐標(biāo)數(shù)學(xué)模型

機(jī)器人雙目立體視覺在計(jì)量測(cè)量技術(shù)領(lǐng)域的作用充分體現(xiàn)靈活的運(yùn)動(dòng)性、通用性。通過安裝在機(jī)器人末端手關(guān)節(jié)上的左右放置的雙目激光立體視覺傳感器獲取空間點(diǎn)的三維信息。其雙目立體視覺測(cè)量示意圖如圖1所示，數(shù)學(xué)模型如圖2所示。被測(cè)工件與末端關(guān)節(jié)左右相對(duì)固定的立體視覺傳感器構(gòu)成三角形，被測(cè)工件在兩臺(tái)攝像機(jī)上呈現(xiàn)立體圖像時(shí)，匹配相關(guān)的特征點(diǎn)，采用最小二乘法，計(jì)算左右兩幅圖像的視覺差異獲得空間特征點(diǎn)的三維坐標(biāo)位置。

圖1 雙目立體視覺傳感器測(cè)量示意圖、Fig.1 Schematic of binocular stereo vision sensor measurement

左、右兩個(gè)相對(duì)位置固定的視覺傳感器所處的空間關(guān)系如式（1）所示：

其中視覺傳感器的兩個(gè)外部參數(shù)即左、右相對(duì)位置攝像機(jī)坐標(biāo)系間的平移矢量與旋轉(zhuǎn)矩陣分別用Tab和Rab來表示。聯(lián)立式（1）中的(xb,yb,zb)三個(gè)方程，利用最小二乘算法求解該式，可以獲得視覺傳感器坐標(biāo)系下空間特征點(diǎn)的三維坐標(biāo)位置：

式（2）右邊的固定參數(shù)可以通過攝像機(jī)傳感器內(nèi)部與外部參數(shù)的標(biāo)定方法獲得[3-4]。因此，在雙目立體視覺傳感器下，以上數(shù)學(xué)模型的建立可以獲得空間被測(cè)點(diǎn)三維位置的信息(xa, ya, za)T。通過機(jī)器人手眼關(guān)系和靶標(biāo)法可以間接得出空間任意有效被測(cè)點(diǎn)時(shí)機(jī)器人工件坐標(biāo)系和視覺傳感器坐標(biāo)系之間的坐標(biāo)變換關(guān)系。

2 機(jī)器人外部位姿標(biāo)定方式

2.1 機(jī)器人位姿方程的建立與求解方法

如圖2所示，按照Shiu[5]手眼標(biāo)定算法以及機(jī)器人系統(tǒng)坐標(biāo)變換鏈的閉合特征，用矩陣A來表示工件坐標(biāo)和視覺傳感器件坐標(biāo)系之間的位置關(guān)系，矩陣T表示工件坐標(biāo)系和機(jī)器人基坐標(biāo)系之間的位置關(guān)系，未知位置關(guān)系X表示視覺傳感器坐標(biāo)系與機(jī)器人末端關(guān)節(jié)坐標(biāo)系之間的齊次坐標(biāo)變換矩陣，矩陣 H表示機(jī)器人基坐標(biāo)系相對(duì)于機(jī)器人末端關(guān)節(jié)的位姿。經(jīng)過機(jī)器人視覺傳感器坐標(biāo)系到末端關(guān)節(jié)坐標(biāo)系以及基坐標(biāo)系的齊次坐標(biāo)相關(guān)變換X·H，可以將任意空間位置點(diǎn)的坐標(biāo)統(tǒng)一轉(zhuǎn)換至基坐標(biāo)系下[6]。

圖2 機(jī)器人外部位姿標(biāo)定與視覺傳感器測(cè)量模型圖Fig.2 Bobot clibration of external pose and vision sensor measurement model diagram

由圖2可知，該位姿標(biāo)定坐標(biāo)變換鏈的閉合特征，可以得出: A = T · H · X （3）

通過先求解工件坐標(biāo)和視覺傳感器坐標(biāo)系之間的位置矩陣關(guān)系A(chǔ)，可以求解矩陣T。由式（4）齊次矩陣坐標(biāo)方程的變換，可以將經(jīng)過標(biāo)定后的視覺傳感器特征點(diǎn)的空間三維坐標(biāo)值轉(zhuǎn)移到工件坐標(biāo)系下。如果空間有若干個(gè)點(diǎn) Pi(i ≥ 4 )，經(jīng)過其空間點(diǎn)的齊次坐標(biāo)相關(guān)變換H·X，可以由在其工件坐標(biāo)系下的坐標(biāo)位置(轉(zhuǎn)換為基坐標(biāo)系下的坐標(biāo)位置(。

式（5）中就是所求機(jī)器人外部位姿T，即工件坐標(biāo)系和機(jī)器人基坐標(biāo)系之間的位置關(guān)系，而 T矩陣中的旋轉(zhuǎn)子矩陣依據(jù)正交約束方式建立方程為非線性，求解困難。為簡(jiǎn)化方程，使用坐標(biāo)空間多點(diǎn)采集方法，確立線性方程求解。

對(duì)于一個(gè)空間點(diǎn)可以列出三個(gè)位置方向方程，假設(shè)有四個(gè)已知點(diǎn)的坐標(biāo)值 ()分別代入式（5），可以得出矩陣A中的12個(gè)不相關(guān)的未知量方程：

將式（7-10）合并轉(zhuǎn)換成矩陣形式，可以得到式（19）：

將式（19）的四元一次方程組利用高斯消去法（Gaussian Elimination）可以直接求解得到未知數(shù)：r1、r2、r3、p1。同理，將式（11-14）和式（15-18）合并轉(zhuǎn)換成矩陣形式，可以得到式（20）、（21）：

同理，再通過高斯消去法求解 r4、r5、r6、p2和r7、r8、r9、p3矩陣。進(jìn)而求得機(jī)器人外部外姿工件坐標(biāo)系與機(jī)器人基坐標(biāo)系之間的位置關(guān)系矩陣T。

在實(shí)際應(yīng)用當(dāng)中，考慮到各種坐標(biāo)系下的坐標(biāo)值存在噪聲誤差，為了進(jìn)一步提高精度，可進(jìn)一步采用最小二乘法擬合，將方程組（19-21）簡(jiǎn)化成D C Y= · 矩陣方程形式：

式（22）為每一個(gè)方程組所對(duì)應(yīng)的未知量向量，也同樣可以得出機(jī)器人的外部位姿。

2.2 機(jī)器人外部位姿的工具與標(biāo)定方式

通過以下靶標(biāo)法來確定機(jī)器人在某一位姿時(shí)工件坐標(biāo)系和機(jī)器人視覺傳感器坐標(biāo)系之間齊次坐標(biāo)變換矩陣A。如圖3所示，首先讓機(jī)器人固定在某一靜態(tài)位姿；其次利用CMM（三坐標(biāo)測(cè)量機(jī)）標(biāo)定一個(gè)定位夾，可以得到其夾具坐標(biāo)系和靶標(biāo)坐標(biāo)系之間的精確位置關(guān)系；最后將圓形平面靶標(biāo)定位安裝到工件坐標(biāo)系范圍內(nèi)，并且讓其工件定位基準(zhǔn)和夾具定位基準(zhǔn)重合，從而保證它們之間的坐標(biāo)系重合。

圖3 靶標(biāo)法示意圖Fig.3 Sketch of bar method

在測(cè)量當(dāng)中，圓形平面的靶標(biāo)配合一維精密導(dǎo)軌和組合量塊的使用，使靶標(biāo)能夠在視覺傳感器的可視范圍內(nèi)移動(dòng)，從而可以采集更多的空間已知坐標(biāo)點(diǎn)。經(jīng)過已標(biāo)定的工件坐標(biāo)系和靶標(biāo)坐標(biāo)系的關(guān)系，從而得出視覺傳感器所采集到的空間特征點(diǎn)在工件坐標(biāo)系下的坐標(biāo)值也是已知的，再利用2.1機(jī)器人位姿方程的建立與求解方式可以得出齊次坐標(biāo)變換矩陣A。綜上所述，可知該靶標(biāo)法利用了機(jī)器人在某一靜態(tài)位姿時(shí)的相關(guān)值進(jìn)行了機(jī)器人在任意狀態(tài)時(shí)都固定不變的機(jī)器人外部位姿的求解[7]。

3 模擬研究實(shí)驗(yàn)

本實(shí)驗(yàn)采用模擬實(shí)驗(yàn)方法來初步驗(yàn)證基本立體視覺的機(jī)器人位姿標(biāo)定程序設(shè)計(jì)算法的測(cè)量精度。

假設(shè)機(jī)器人實(shí)際外部位姿其中的一組數(shù)據(jù)矩陣為：

并假定，位于工件坐標(biāo)系下四個(gè)位置點(diǎn)p1、p2、p3、p4的坐標(biāo)值分別為(1.5,2.0,3.3)T，(30.2,-5.7,8.9)T，(102.4,70.7,59.8)T，(-40.6,89.9,-50.4)T，分別代入式（5），通過矩陣整理和運(yùn)算，可以得出在機(jī)器人基礎(chǔ)坐標(biāo)系下四個(gè)位置點(diǎn)的坐標(biāo)值分別為：

圖4 外部位姿標(biāo)定求解Fig.4 Solution of equations calibration of external pose

如圖5所示。

圖5 外部位姿模擬的實(shí)驗(yàn)結(jié)果Fig.5 The experimental results of external pose simulation

從假設(shè)的真值式（23）與計(jì)算結(jié)果式（24）可以看出，其數(shù)據(jù)誤差在10～5數(shù)量級(jí)上。 同理，若在數(shù)據(jù)不存在噪聲因素的情況下，進(jìn)行大量仿真實(shí)驗(yàn)進(jìn)行推論，其系統(tǒng)精度級(jí)別高。

4 結(jié)論

本文建立雙目立體視覺傳感器三坐標(biāo)的數(shù)學(xué)模型，得出空間特征點(diǎn)的三維坐標(biāo)。通過機(jī)器人外部位姿工具與靶標(biāo)法利用了機(jī)器人在某一靜態(tài)位姿時(shí)的相關(guān)值進(jìn)行了機(jī)器人在任意狀態(tài)時(shí)都固定不變的機(jī)器人外部位姿的求解。仿真實(shí)驗(yàn)表明，在沒有噪聲因素的環(huán)境中，此方法可以獲得機(jī)器人外部位姿的精確求解。

[1] 周富強(qiáng).CCD攝像機(jī)快速標(biāo)定技術(shù)[J].光學(xué)精密工程,2002, 8(1):97-99.

[2] 楊濤.基于合作靶標(biāo)的激光掃描車身坐標(biāo)測(cè)量關(guān)鍵技術(shù)研究[D]．天津:天津大學(xué),2009:6-7.

[3] 楊劍,呂乃光.加權(quán)最小二乘算法在機(jī)器視覺系統(tǒng)中的應(yīng)用[J].光學(xué)精密工程, 2009, 17(8):1871-1873.

[4] 王福斌,劉杰.挖掘機(jī)器人雙目視覺系統(tǒng)標(biāo)定方法與立體匹配[J].機(jī)械制造與自動(dòng)化,2012,41(3) :157-160.

[5] Shiu Y C, Ahmad S. Calibration of wrist-mounted robotic sensors by solving homogeneous transform equations of the form AX= XB[J]. Robotics and Automation, IEEE Transactions on, 1989, 5(1): 16-29.

[6] 鄭俊, 邾繼貴.基于雙目立體視覺的機(jī)器人測(cè)量技術(shù)研究[J].計(jì)量技術(shù), 2005, 17(4):15-17.

[7] 鄭俊.基于立體視覺的通用測(cè)量機(jī)器人標(biāo)定模式研究[D]. 天津:天津大學(xué),2005:25-29.