基于空間轉(zhuǎn)換模型的工業(yè)機(jī)械臂手眼標(biāo)定方法研究

王任遠(yuǎn) 肖正航 吳杰 李沛

（北京空間機(jī)電研究所，北京 100094）

0 引言

近年來，超大口徑的空間遙感技術(shù)研究由大口徑單體反射鏡向空間分塊式反射鏡發(fā)展[1]，以詹姆斯·韋伯太空望遠(yuǎn)鏡（JWST）為代表的空間分塊可展開成像技術(shù)已開展工程研制，同屬分塊式反射鏡技術(shù)的在軌裝配技術(shù)復(fù)雜度更高，仍處于論證階段，需要在地面進(jìn)行大量的實(shí)驗(yàn)論證其可行性。

工業(yè)機(jī)械臂技術(shù)近年來取得長足發(fā)展，已經(jīng)廣泛應(yīng)用于制造業(yè)各個(gè)領(lǐng)域的裝配工藝環(huán)節(jié)，能夠?qū)崿F(xiàn)大質(zhì)量、大尺寸的零部件的裝配[2]。工業(yè)機(jī)械臂絕對定位精度低、重復(fù)定位精度高，利用其重復(fù)定位精度高的特點(diǎn)，批量的裝配應(yīng)用大多利用示教的方式[3-4]。而在單件裝配應(yīng)用場景下，如大尺寸飛機(jī)零件裝配[5-7]、大口徑分塊式反射鏡的裝配等場合，需要絕對定位精度高，一般的工業(yè)機(jī)械臂就不能滿足要求，就需要采用單目、雙目或多目相機(jī)組成攝影測量系統(tǒng)進(jìn)行實(shí)時(shí)位置引導(dǎo)[8-11]。裝配系統(tǒng)精度的提升是實(shí)時(shí)引導(dǎo)裝配中需要解決的一個(gè)關(guān)鍵問題，國內(nèi)外眾多學(xué)者對系統(tǒng)中單個(gè)誤差項(xiàng)的研究較多，但對于系統(tǒng)中各誤差的關(guān)系與綜合精度方面的研究較少。

如在機(jī)械臂絕對定位精度方面，通過建立工業(yè)機(jī)械臂的運(yùn)動(dòng)學(xué)模型，建立笛卡爾空間與關(guān)節(jié)空間的映射矩陣，是進(jìn)行誤差修正的前提；利用運(yùn)動(dòng)學(xué)模型推導(dǎo)誤差模型并設(shè)計(jì)相應(yīng)的補(bǔ)償算法和數(shù)學(xué)逼近法已經(jīng)是相當(dāng)成熟的方法[12-13]；引入優(yōu)化算法如LM算法優(yōu)化參數(shù)識(shí)別效果能顯著提高機(jī)械臂的絕對定位精度[14]；隨著智能算法的研究深入，神經(jīng)網(wǎng)絡(luò)算法與空間插值算法那也漸漸成為研究主流[15-17]。

在機(jī)械臂引導(dǎo)系統(tǒng)的研究上，大多基于攝影測量系統(tǒng)進(jìn)行設(shè)計(jì)。最基本的方法是基于圖像的點(diǎn)特征進(jìn)行識(shí)別并引導(dǎo)測量機(jī)械臂位置與姿態(tài)[18-19]；將攝影測量系統(tǒng)結(jié)合結(jié)構(gòu)光系統(tǒng)（如激光投影或條紋格柵式光源投影）可以有效提高引導(dǎo)系統(tǒng)的測量精度[20-22]。除了基于攝影測量系統(tǒng)，利用激光跟蹤儀等高精度測量設(shè)備對工業(yè)機(jī)械臂末端姿態(tài)進(jìn)行測量[23-24]，并搭建的閉環(huán)控制系統(tǒng)進(jìn)行位姿誤差補(bǔ)償也成為近期研究的熱點(diǎn)。而在綜合精度研究方面，文獻(xiàn)[25]也僅驗(yàn)證了機(jī)械臂運(yùn)動(dòng)學(xué)標(biāo)定與手眼標(biāo)定精度的相關(guān)性。

本文針對一種面向大口徑分塊式反射鏡的基于攝影測量系統(tǒng)引導(dǎo)的雙機(jī)械臂自動(dòng)裝配系統(tǒng)進(jìn)行系統(tǒng)建模與標(biāo)定。通過將機(jī)械臂運(yùn)動(dòng)學(xué)模型、相機(jī)成像模型與eye-to-hand形式的手眼模型作為整體進(jìn)行建模，分步對它們進(jìn)行參數(shù)標(biāo)定，為分塊式反射鏡在軌裝配技術(shù)工作進(jìn)行基礎(chǔ)技術(shù)研究與地面原理驗(yàn)證。

1 系統(tǒng)組成與系統(tǒng)空間模型建立

1.1 系統(tǒng)組成

裝配系統(tǒng)一般包括運(yùn)動(dòng)執(zhí)行子系統(tǒng)、引導(dǎo)測量子系統(tǒng)、計(jì)算機(jī)控制端和輔助裝配子系統(tǒng)幾部分組成，如圖1所示。本文研究的分塊式反射鏡主體長2.5m，高3.75m，待裝配的單塊分塊鏡聯(lián)合工裝質(zhì)量約88kg。

運(yùn)動(dòng)執(zhí)行子系統(tǒng)采用兩臺(tái)質(zhì)量150kg、最大工作范圍為3m的機(jī)械臂協(xié)同完成裝配?；陔p目視覺相機(jī)搭建了用于實(shí)現(xiàn)工件、機(jī)械臂末端的空間定位功能和對機(jī)械臂末端移動(dòng)位置實(shí)時(shí)測量和引導(dǎo)的引導(dǎo)測量子系統(tǒng)。

以圖1中右側(cè)工件裝配為例，進(jìn)行裝配時(shí)，機(jī)械臂A夾持待裝配工件，機(jī)械臂B夾持雙目視覺相機(jī)，通過雙目視覺相機(jī)對工件位置的測量，將測量信息發(fā)送位置指令至機(jī)械臂A，引導(dǎo)機(jī)械臂A完成裝配工作。

圖1 在軌組裝地面原理驗(yàn)證系統(tǒng)布局示意圖Fig.1 Layout of on-orbit assembly ground proof-of-principle system

1.2 系統(tǒng)空間模型的建立

系統(tǒng)空間模型包含三部分：相機(jī)成像模型、機(jī)械臂運(yùn)動(dòng)學(xué)模型和手眼關(guān)系模型。

1）相機(jī)成像模型如圖2所示，由世界坐標(biāo)系Ow-XwYwZw、相機(jī)坐標(biāo)系Oc-XcYcZc、焦平面坐標(biāo)系Omm-xy和圖像坐標(biāo)系Opixel-uv構(gòu)成。

相機(jī)把物理空間中的三維點(diǎn)投影到成像平面之上，該模型可用式（1）表示，空間中一點(diǎn)P在世界坐標(biāo)系下的坐標(biāo)與其在圖像坐標(biāo)系即ZC=–f焦平面上的投影點(diǎn)的坐標(biāo)間的映射關(guān)系為：

式中 (u,v)為點(diǎn)P在圖像坐標(biāo)系下的坐標(biāo)；M1矩陣包含4個(gè)參數(shù)分別表示x軸上的歸一化焦距、y軸上的歸一化焦距、光軸光心在圖像坐標(biāo)系u軸上的位置、光軸光心在圖像坐標(biāo)系v軸上的位置，它們由相機(jī)結(jié)構(gòu)決定，稱為內(nèi)參矩陣；M2矩陣包含2個(gè)參數(shù)R和t，它們分別表示相機(jī)坐標(biāo)系與世界坐標(biāo)系的旋轉(zhuǎn)矩陣與平移向量，稱為外參矩陣。

2）機(jī)械臂運(yùn)動(dòng)學(xué)模型采用Denavit-Havtenberg方法建立，簡稱D-H法。D-H法定義了機(jī)械臂的每個(gè)連桿坐標(biāo)系由4個(gè)連桿參數(shù)描述，連桿長度ai和連桿扭轉(zhuǎn)角αi用于描述連桿本身，連桿偏距di和關(guān)節(jié)角θi描述相鄰連桿之間的關(guān)系，i代表第i個(gè)連桿。ai為沿Xi軸，從Zi移動(dòng)到Zi+1的距離；αi為繞Xi軸，從Zi旋轉(zhuǎn)到Zi+1的角度；di為沿Zi軸，從Xi-1移動(dòng)到Xi的距離；θi為繞Zi軸，從Xi-1旋轉(zhuǎn)到Xi的角度。其矩陣表示形式如式（2）所示，其中矩陣D為平移矩陣，矩陣R為旋轉(zhuǎn)矩陣，其中矩陣用于表示在空間中的點(diǎn)從m坐標(biāo)系向n坐標(biāo)系的空間齊次坐標(biāo)變換，即將點(diǎn)從m坐標(biāo)系中的坐標(biāo)轉(zhuǎn)變成在n坐標(biāo)系中表示的坐標(biāo)變換矩陣。從連桿i向連桿i-1的坐標(biāo)變換矩陣為

通過4個(gè)連桿參數(shù)將所有的連桿表示出來即可表示機(jī)械臂機(jī)構(gòu)的位置關(guān)系與運(yùn)動(dòng)方式。本文采用的6自由度工業(yè)機(jī)械臂由6個(gè)連桿組成，且相互之間均為轉(zhuǎn)動(dòng)副連接。該機(jī)械臂的D-H模型參數(shù)如表1所示，機(jī)械臂運(yùn)動(dòng)學(xué)模型如圖3所示，其中連桿4與連桿5的原點(diǎn)重合。機(jī)械臂從機(jī)械臂末端中心點(diǎn)（下文簡稱TCP點(diǎn)），到機(jī)械臂基座B的空間轉(zhuǎn)換矩陣為

表1 工業(yè)機(jī)械臂D-H參數(shù)表Tab.1 Parameter table of Industrial manipulator by D-H method

3）Eye-to-hand形式的手眼位置關(guān)系如圖4所示，由機(jī)械臂基坐標(biāo)系OB、機(jī)械臂末端TCP點(diǎn)坐標(biāo)系OT、標(biāo)定坐標(biāo)系OL和相機(jī)坐標(biāo)系OC組成，可以得到手眼變換矩陣關(guān)系如式（4）所示：

圖3 工業(yè)機(jī)械臂運(yùn)動(dòng)學(xué)模型Fig.3 Kinematics model of industrial manipulator

4）基于手眼標(biāo)定關(guān)系的模型統(tǒng)一：手眼標(biāo)定過程中求解標(biāo)定靶球位姿的過程，實(shí)際上是一個(gè)計(jì)算相機(jī)坐標(biāo)系OC與世界坐標(biāo)系OW之間轉(zhuǎn)換關(guān)系的過程（將靶球坐標(biāo)系與世界坐標(biāo)系重合），因此在機(jī)械臂手眼關(guān)系中，標(biāo)定靶球的位姿矩陣就是相機(jī)模型的外參數(shù)M2矩陣，因此將式（4）代入式（1），可得

同時(shí)，將式（3）帶入式（5），得到整體建模的最終變換關(guān)系，

圖4 Eye-to-hand手眼坐標(biāo)關(guān)系圖Fig.4 The eye-to-hand coordinate diagram form

根據(jù)式（6）可知，標(biāo)定靶球坐標(biāo)系通過各轉(zhuǎn)換矩陣與像平面坐標(biāo)系建立了映射關(guān)系。由此，建立了基于機(jī)械臂運(yùn)動(dòng)學(xué)模型、相機(jī)成像模型與eye-to-hand形式的手眼關(guān)系模型。

其中，手眼標(biāo)定就是求解機(jī)械臂基坐標(biāo)系OB向相機(jī)坐標(biāo)系OC的空間齊次坐標(biāo)變換CBT的過程。根據(jù)式（4），任取位置i和位置i+1的轉(zhuǎn)換關(guān)系，聯(lián)立方程，如式（7）所示：

未經(jīng)運(yùn)動(dòng)學(xué)標(biāo)定的重載工業(yè)機(jī)械臂的絕對定位精度一般大于1mm[27]，不能滿足本項(xiàng)目需要。需要對機(jī)械臂進(jìn)行運(yùn)動(dòng)學(xué)標(biāo)定提高絕對定位精度，并通過高精度手眼標(biāo)定進(jìn)一步提高裝配精度，引導(dǎo)機(jī)械臂完成裝配工作。

2 系統(tǒng)標(biāo)定與裝配實(shí)驗(yàn)

2.1 機(jī)械臂運(yùn)動(dòng)學(xué)標(biāo)定

利用遞推最小二乘法對機(jī)械臂末端進(jìn)行運(yùn)動(dòng)學(xué)標(biāo)定，式（4）的函數(shù)形式如式（8）所示：

而機(jī)械臂在實(shí)際制造過程中因加工、裝配等各個(gè)環(huán)節(jié)使得各參數(shù)產(chǎn)生了一定的誤差，各誤差積累影響了末端TCP點(diǎn)的實(shí)際位置。設(shè)各物理參量與理論值的微小偏差為dα、da、dθ、dd。因此TCP的實(shí)際位置為：

根據(jù)機(jī)器人微分運(yùn)動(dòng)學(xué)，可得到TCP與基坐標(biāo)理論位姿與實(shí)際位姿的絕對定位精度誤差ΔP關(guān)系：

其中J為誤差參數(shù)矩陣，δΔ 為包含所有的誤差項(xiàng)。

通過構(gòu)建超定方程組通過可遞推最小二乘法對誤差參數(shù)進(jìn)行辨識(shí)與方程組求最小二乘解，形式如式（12）所示。

實(shí)驗(yàn)采用Leica AT401激光跟蹤儀對機(jī)械臂TCP的實(shí)際位置進(jìn)行測量，本次實(shí)驗(yàn)在機(jī)械臂工作空間設(shè)置40個(gè)樣本點(diǎn)用于識(shí)別誤差參數(shù)，采樣點(diǎn)盡可能覆蓋機(jī)械臂工作范圍，分布如圖5所示。

每達(dá)到一個(gè)樣本點(diǎn)時(shí)使用激光跟蹤儀測量靶標(biāo)的實(shí)際位置，根據(jù)式（12）計(jì)算出各關(guān)節(jié)參數(shù)的誤差值，最終通過將誤差值預(yù)補(bǔ)償至機(jī)械臂運(yùn)動(dòng)學(xué)模型的方式提高機(jī)械臂末端的絕對定位精度。辨識(shí)得到誤差參數(shù)矩陣J如表2所示。重復(fù)實(shí)驗(yàn)，測得補(bǔ)償后的機(jī)械臂末端TCP位置。補(bǔ)償前后40個(gè)點(diǎn)與示教器顯示點(diǎn)的誤差如圖6所示。

圖5 40個(gè)采樣點(diǎn)在空間中的分布圖Fig.5 The distribution map of 40 sampling points in space

表2 誤差參數(shù)辨識(shí)矩陣Tab.2 The error parameter identification matrix

圖6 標(biāo)定前后位置誤差對比Fig.6 Comparison of position errors before and after calibration

根據(jù)GB/T 12642–2001“工業(yè)機(jī)器人性能規(guī)范及其試驗(yàn)方法”中關(guān)于位置準(zhǔn)確度（即機(jī)器人的絕對定位精度）的評(píng)定方法，點(diǎn)在空間中的理論位置(x,y,z)與實(shí)際位置(xr,yr,zr)的距離即定位誤差

對測量結(jié)果進(jìn)行分析可知，最大定位誤差由補(bǔ)償前的1.81mm降低至0.84mm，最小定位誤差補(bǔ)償后降至0.35mm。平均絕對定位精度由補(bǔ)償前的1.19mm提高至0.55mm，提高了53.78%，機(jī)械臂絕對定位精度的提高有助于手眼標(biāo)定精度的提高。

剔除少部分壞點(diǎn)后，對數(shù)據(jù)分析可知，當(dāng)機(jī)械臂末端遠(yuǎn)離機(jī)械臂基座時(shí)，其絕對定位精度逐漸下降。這主要是因?yàn)槟┒酥剌d的串聯(lián)機(jī)器人為懸臂梁結(jié)構(gòu)，隨著末端與基座的遠(yuǎn)離，臂長增加，彎矩增加，末端絕對定位精度降低。由此可知，將機(jī)械臂對稱布置，每個(gè)機(jī)械臂負(fù)責(zé)臨近區(qū)域的裝配是一種絕對定位精度更高，更合理的布局形式。

2.2 手眼標(biāo)定與實(shí)驗(yàn)

在完成機(jī)械臂運(yùn)動(dòng)學(xué)標(biāo)定的基礎(chǔ)上，進(jìn)行手眼標(biāo)定。手眼標(biāo)定的過程如圖7（a）所示，在保持基坐標(biāo)系與相機(jī)坐標(biāo)系位置不變的情況下，控制機(jī)械臂末端帶動(dòng)標(biāo)定靶標(biāo)運(yùn)動(dòng)，在n個(gè)位置測量靶標(biāo)和讀取相應(yīng)的機(jī)械臂關(guān)節(jié)參數(shù)。

手眼標(biāo)定過程中，機(jī)械臂夾持靶球應(yīng)進(jìn)行至少兩組旋轉(zhuǎn)軸不平行的運(yùn)動(dòng)[6]，通過求解機(jī)械臂空間基坐標(biāo)系OB與相機(jī)坐標(biāo)系OC的變換關(guān)系。本文采用經(jīng)典的Tsai兩步法，結(jié)合機(jī)械臂運(yùn)動(dòng)學(xué)標(biāo)定后的運(yùn)動(dòng)學(xué)模型與相機(jī)模型，完成手眼標(biāo)定，如圖7（b）所示。

為驗(yàn)證手眼標(biāo)定有效性，設(shè)計(jì)軸孔對插實(shí)驗(yàn)驗(yàn)證標(biāo)定后的系統(tǒng)工作精度。

圖7 手眼標(biāo)定過程和靶標(biāo)球Fig.7 Hand eye calibration process and the target ball

軸孔對插實(shí)驗(yàn)如圖8所示，在機(jī)械臂末端執(zhí)行器安裝實(shí)測尺寸直徑為7.92mm的定位軸，在工作平臺(tái)放置一塊預(yù)先鉆孔的工裝板，孔的大小按照直徑8.1mm~9mm依次排列。采用Leica AT402激光跟蹤儀測量孔的位置和完成軸孔裝配后軸的位置，選取2個(gè)裝配位置進(jìn)行測量。

圖8 攝影測量系統(tǒng)引導(dǎo)的孔軸對插實(shí)驗(yàn)Fig.8 The experiment of hole axis pair insertion guided by the photogrammetry system

實(shí)驗(yàn)結(jié)果如表3所示，可以成功完成實(shí)測直徑為8.34 mm和8.38 mm的孔軸裝配工作。

表3 孔軸位置測量坐標(biāo)值Tab.3 Coordinate values of hole axis position measurement

完成高精度手眼標(biāo)定后，機(jī)械臂系統(tǒng)在攝影測量系統(tǒng)引導(dǎo)下，在XOY平面裝配精度由章節(jié)2.1中測量的最大定位誤差1.81mm大幅降低至0.29mm，提高了83.98%。標(biāo)定前機(jī)械臂工作的定位誤差大于1mm，無法滿足任務(wù)需求，在完成手眼標(biāo)定后，可以將裝配系統(tǒng)用于大尺寸分塊式反射鏡裝配的地面原理驗(yàn)證任務(wù)。

4 結(jié)束語

本文通過相機(jī)模型、機(jī)械臂運(yùn)動(dòng)學(xué)模型和手眼關(guān)系模型建立了基于裝配系統(tǒng)的空間轉(zhuǎn)換模型，通過遞推最小二乘參數(shù)辨識(shí)方法將機(jī)械臂的平均絕對定位精度由補(bǔ)償前的1.19mm提高至0.55mm，提高了53.78%；在此基礎(chǔ)上，利用空間轉(zhuǎn)換模型完成了高精度手眼標(biāo)定，并設(shè)計(jì)軸孔對插實(shí)驗(yàn)驗(yàn)證標(biāo)定后的裝配系統(tǒng)精度，平均定位誤差由1.81mm降低至0.29mm。可應(yīng)用于大尺寸分塊式反射鏡系統(tǒng)的視覺引導(dǎo)裝配，為空間在軌裝配工作進(jìn)行技術(shù)研究與地面原理驗(yàn)證。