機器人工具坐標系的快速標定方法

李廣云，羅豪龍，王 力

（戰(zhàn)略支援部隊信息工程大學 地理空間信息學院，河南 鄭州450001）

1 引 言

隨著制造業(yè)轉型升級和社會自動化水平的不斷提高，工業(yè)機器人在餐飲、電子、醫(yī)療和軍事等領域應用越來越廣泛［1-5］。眾所周知，工業(yè)機器人在工作之前需要為其末端安裝執(zhí)行機構即工具。工業(yè)機器人的工具好比人類的手，根據任務的不同，機器人末端安裝的工具也往往不相同。例如焊接機器人末端安裝焊槍，噴繪機器人末端安裝噴槍，搬運機器人末端安裝手爪等等［6-7］。同時為使機器人正常工作，還需要獲取機器人末端工具相對于機器人的位置和姿態(tài)，即進行工具坐標系標定。工具坐標系標定的精度關系著工業(yè)機器人的生產質量和工作效率；同時，機器人在實際工作中要求能夠快速地進行機器人工具坐標系標定［8-10］。因此，研究一種能夠快速、準確的工具坐標系標定方法對提高機器人的定位精度，拓展機器人的應用市場和提高機器人的智能化、柔性化水平具有重要意義。

目前，工業(yè)機器人的工具坐標系標定方法主要有以下兩種：一是不借助外界測量設備，基于機器人本體進行工具坐標系標定［11-13］。牛雪娟［14］基于奇異值分解的原理通過三點五步法實現(xiàn)了工具坐標系標定；劉成業(yè)［15］提出工具坐標系的位置和姿態(tài)分開標定的方法，利用四個點標定工具坐標系位置，三個點標定工具坐標系姿態(tài)。但上述方法存在的問題是標定精度往往不高且要求工具具有尖端點特征。二是基于外界測量設備（如激光跟蹤儀）進行工具坐標系標定［16-19］。齊立哲［20］利用距離約束原則，即空間中任意兩點在基坐標系下和跟蹤儀坐標系下保持距離不變進行工具坐標系標定，存在的問題是機器人的定位誤差會引入建立的約束方程中，且標定過程易受機器人空間運動范圍的限制；張博［21］提出控制機器人進行單軸運動，利用激光跟蹤儀測量的機器人運動數據擬合機器人各關節(jié)軸的運動軌跡進行工具坐標系的標定。該方法的缺點是，標定過程繁瑣且工作和計算量大，同時機器人的定位誤差會降低標定精度。綜上所述，傳統(tǒng)的工具坐標系標定方法不能兼顧工具坐標系的標定精度和標定效率。為了解決標定精度不高、操作繁瑣和標定效率較低等問題，本文通過分析機器人末端的法蘭盤結構，基于法蘭盤上各點的幾何關系，提出一種幾何法快速標定工具坐標系的方法。同時利用距離約束法進行了工具坐標系標定實驗并比較了二者的精度。

2 幾何法標定原理

幾何法標定原理是分開標定工具坐標系的位置和姿態(tài)。首先，分析法蘭盤上各點位置的幾何關系，利用激光跟蹤儀測量靶球在機器人法蘭盤上的多個位置進行工具坐標系位置標定；之后控制機器人沿工具坐標系的坐標軸方向運動進行工具坐標系姿態(tài)標定。

2.1 坐標系定義

工具坐標系標定過程中涉及到多個坐標系，首先對坐標系進行定義。機器人底座建立的坐標系稱為機器人基坐標系，基坐標系用｛B｝表示；世界坐標系一般默認與基坐標系重合，用｛W｝表示；在機器人末端法蘭建立的坐標系稱為法蘭末端坐標系，法蘭末端坐標系用｛E｝表示；機器人末端安裝的工具上建立的坐標系稱為工具坐標系，工具坐標系用｛T｝表示；激光跟蹤系統(tǒng)建立的坐標系為激光跟蹤儀坐標系，激光跟蹤儀坐標系用｛L｝表示［22］。實驗系統(tǒng)原理如圖1所示。工具坐標系標定的實質是獲取機器人法蘭末端坐標系到工具坐標系的位姿變換矩陣。

圖1 實驗系統(tǒng)原理Fig.1 Principle of experimental system

2.2 位置標定

工具坐標系的位置標定是求取工具坐標系的原點，即TCP（Tool Center Point）在法蘭末端坐標系下的位置。工具坐標系位置標定過程主要分為以下三步：（1）求取法蘭坐標系在跟蹤儀坐標系下的位置；（2）獲取TCP在跟蹤儀坐標系下的位置；（3）通過坐標系轉換得到TCP的位置（法蘭末端坐標系下）。工具坐標系位置標定的求解過程如圖2所示，第一、第二、第三平面分別代表本文后面定義的平面α1，α2和α3。

圖2 TCP標定流程Fig.2 Process of TCP calibration

工業(yè)機器人末端法蘭平面示意圖如圖3所示。法蘭盤平面上有六個大小相同的小孔，編號分別為1，2，…，6。法蘭盤的圓心O為法蘭坐標系的原點，小孔中心與法蘭盤圓心O的距離相等，且相鄰小孔的夾角為60°。法蘭盤圓心O到小孔7的圓心方向為法蘭坐標系的X軸正方向，過法蘭盤圓心O且垂直于法蘭盤平面的方向為法蘭坐標系的Z軸方向。定義法蘭盤所在的平面為α1。由于法蘭盤圓心O在實際的法蘭盤上是懸空的理想位置，無法直接測量，因此考慮利用激光跟蹤儀間接測量得到法蘭坐標系的位置。

圖3 法蘭盤示意圖Fig.3 Flange diagram

首先將跟蹤儀靶球依次放在1，3，5三個孔的位置處，假設3個位置處的靶球球心依次為O01，O02，O03，O01，O02，O03所 在 的 平 面 為α2。利用 跟 蹤 儀 測 得O01，O02，O03的 坐 標 依 次 為(x1，y1，z1)，(x2，y2，z2)，(x3，y3，z3)。由于3個小孔的圓心均勻分布在同一個圓上，故O01，O02，O03也 位 于 同 一 個 圓 上，假 設 圓 心 為O1(x4，y4，z4)。根 據 公 式（1）和 公 式（2）可 以 分 別 得 到O01，O02，O03所在的平面α2方程和圓心O1的位置。

整理得到，平面α2的一般方程為：

根據確定的α2方程和圓心O1的坐標(x4，y4，z4)，可以得到經過圓心O1且垂直于平面α2的法線方程l1為：

由于法蘭坐標系的Z軸與法線l1的方向重合，因此得到法蘭坐標系Z軸的單位方向向量為：

將靶球固定在法蘭盤平面任意位置處（如圖3中A點處），設此時靶球球心的位置為O04，過O04且平行于平面α2的平面為平面α3。根據平面α2方程和O04的位置，得到平面α3的方程為：

工具坐標系位置標定中各點的位置關系如圖5所示，且根據平面的定義可知α1∥α2∥α3。根據求得的平面α2方程和平面α3方程，計算平面α2與平面α3之間的距離d為：

圖5 距離約束法原理Fig.5 Principle of distance constraint method

由于平面α3與法蘭盤平面α1之間的距離為靶球半徑，且靶球半徑r為已知量。因此得到平面α2與法蘭平面α1之間的距離s為：

圖4 平面上各點的幾何關系Fig.4 Geometric relations of points on the plane

根據法線l1與平面α1的幾何關系可知，法蘭盤圓心O，即法蘭末端坐標系的原點即為法線l1與平面α1的交點。因此，根據上述求得的法線l1方程和圓心O1的坐標，以及平面α2與平面α1之間的距離s，可以得到法蘭坐標系原點O的位置為：

之后根據激光跟蹤儀與法蘭盤實際的相對位置關系，得到法蘭盤中心O的唯一解。

將靶球固定在小孔7位置處，激光跟蹤儀測得靶球球心的坐標O07(x7，y7，z7)。由于圓心O1與靶球球心O07連線的方向與法蘭坐標系的X軸方向平行，因此得到法蘭坐標系X軸的單位方向向量為：

由于小孔7位于小孔3和小孔4與圓心O的角平分線上，因此根據靶球在小孔3和小孔4處的位置也可以計算得到靶球在小孔7處的位置O07(x7，y7，z7)，進而計算法蘭坐標系的X軸方向向量。根據公式（5）和公式（10）得到的法蘭坐標系Z軸和X軸的單位方向向量，通過右手定則得到法蘭坐標系Y軸的單位方向向量為：

對上述得到的法蘭坐標系X軸、Y軸和Z軸的單位方向向量進行施密特正交化，得到法蘭末端坐標系的三軸單位向量（激光跟蹤儀坐標系下）為：

保持機器人不動，在機器人法蘭末端安裝工具，將靶球固定在工具合適位置。靶球球心認為是TCP，即工具坐標系的原點，激光跟蹤儀測量得到TCP的位置。根據公式（13）得到TCP在法蘭末端坐標系下的位置。

公式（13）中，ET P，LT P和O均為3×1向量，ET P為待求量。其中LT P表示TCP在跟蹤儀坐標系下的坐標，ET P表示TCP在法蘭末端坐標系下的坐標。由此，工具坐標系位置標定完成。同時，可以發(fā)現(xiàn)當利用激光跟蹤儀得到機器人法蘭末端的位置和姿態(tài)后，機器人末端任意更換新的工具時，只需要再測量1個點，即靶球在新工具上的位置，利用公式（13）就能實現(xiàn)新工具的TCP標定。

2.3 姿態(tài)標定

根據法蘭末端的位置和姿態(tài)和公式（13），可以得到機器人在任意位置處TCP在基坐標系下的位置為：

控制機器人運動到新的位置處，調整機器人的末端姿態(tài)使工具坐標系與機器人基坐標系保持平行，記錄此時法蘭末端的姿態(tài)同時根據公式（15）計算得到此時TCP的位置保持工具的姿態(tài)不變，使機器人沿工具坐標系的X軸運動一段距離，計算得到此時TCP的位置保持工具的姿態(tài)不變，使機器人沿工具坐標系的Z軸方向運動一段距離，計算得到此時TCP的位置

由于工具坐標系的坐標軸方向與基坐標的坐標軸方向平行，故工具坐標系的X軸、Z軸方向向量分別為：

根據右手原則確定工具坐標系的Y軸的單位方向向量為：

根據公式（12）對工具坐標系三軸方向向量進行施密特正交化得到工具坐標系的三軸正交單位向量px，py，pz。工具坐標系到基坐標系的姿態(tài)變換矩陣為：

得到工具坐標系到法蘭末端坐標系的姿態(tài)變換矩陣為：

結合公式（13）得到工具坐標系到法蘭末端坐標系的位姿變換矩陣為：

3 距離約束法標定原理

空間中任意一點在不同坐標系下的位置坐標可能不同，但空間中任意兩點在不同坐標系下的距離是保持不變的［23］（假設坐標系間的尺度因子為1）。通過測量空間中多個點在基坐標系和激光跟蹤儀坐標系下的位置，根據空間兩點距離不變這一約束條件建立方程組，通過最小二乘求解可以得到TCP在法蘭末端坐標系下的位置；根據2.2中姿態(tài)標定的方法進行工具坐標系姿態(tài)標定。

將靶球固定到機器人末端，操縱機器人運動到空間多個位置處（各點盡可能在空間分布均勻且各點間的距離盡可能大）。激光跟蹤儀測量靶球在各個位置處的坐標同時通過機器人得到法蘭末端在各個位置處的位姿矩陣假設第i個位置處，靶球在基坐標系下和跟蹤儀坐標系下的位置分別為以靶球到達的第1、i兩個位置為例，根據兩點在不同坐標系下的距離不變原則，建立約束方程為：

靶球球心在第i個位置處的坐標（基坐標系下）又可表示為：

結合公式（21）得到：

令

由公式（24）可以得到：

展開得到：

對于非線性方程f(x，y，z)=0，通過一階泰勒級數展開，忽略高階項整理得到：

其中，

上式經化簡得到：

其中，

機器人在空間運動i個位置時，可以得到i(i?1)/2條邊的距離。根據公式（21）可以建立i(i?1)/2個方程。為了使方程組有唯一解，機器人至少需要運動3個位置。為減少隨機誤差的影響，通常機器人的位置數要遠大于3。根據建立的方程組，通過最小二乘迭代解算未知數(x，y，z)，得到TCP在法蘭末端坐標下的位置。由此基于距離約束的工具坐標系位置標定完成。

3.2 姿態(tài)標定

姿態(tài)標定的方法與2.3中的方法相同，控制機器人運動到新的位置處，調整機器人的末端姿態(tài)使工具坐標系與機器人基坐標系保持平行，使機器人沿著工具坐標系的X軸和Z軸方向分別運動進行法蘭末端的姿態(tài)標定。

4 實驗與結果分析

4.1 幾何法實驗

本文中所使用的激光跟蹤儀型號為Leica AT 901-B，其在80 m范圍內的測量精度為±(15μm+6μm/m)，激光跟蹤儀實物如圖6所示。與激光跟蹤儀Leica AT 901-B配套的靶球型號為CCR1.5角偶棱鏡，其半徑為19.05 mm。本文中所使用的工業(yè)機器人為6自由度工業(yè)機器人ABB IRB2600，其負載為20 kg，工作范圍（半徑）1.65 m。

圖6 Leica AT 901-B激光跟蹤儀Fig.6 Leica AT 901-B laser tracker

實驗過程中，為減少隨機誤差對結果的影響，對每個位置處的點測量三次取平均值。首先將靶球依次放置在法蘭盤小孔1，3，5三個位置處，利用激光跟蹤儀測量靶球在每個位置處的坐標。計算得到平面α2方程和圓心O1的坐標，進而得到法蘭坐標系的Z軸方向向量。其次，將靶球固定在法蘭盤平面任意位置，激光跟蹤儀測量該點的位置，計算得到平面α2和平面α3的距離。之后，將靶球固定在第七個小孔處，利用激光跟蹤儀測量靶球的位置，通過計算得到法蘭末端坐標系在激光跟蹤儀坐標系下的位姿（位置和姿態(tài)）。

給機器人ABB IRB2600末端安裝工具，本文中機器人末端安裝的工具如圖7所示，該工具根據法蘭盤的結構設計制作，可直接安裝在機器人末端，工具末端凹槽由磁鐵制成用于吸附跟蹤儀靶球。將靶球固定到機器人末端工具上，利用激光跟蹤儀測量靶球球心的位置（即TCP）。計算得到TCP在法蘭末端坐標系下的位置為(0.025 6，?0.143 8，39.312 4)T，工 具 坐 標 系 的 位置標定完成。TCP位置標定過程中激光跟蹤儀測量的靶球位置數據如表1所示。

圖7 機器人末端工具Fig.7 Tool of robot end

圖8 幾何法TCP標定實驗Fig.8 T CP calibration experiment based on geometric method

表1 靶球位置Tab.1 T arget position （mm）

控制機器人運動到新的位置處，調整機器人的姿態(tài)使工具坐標系的三軸方向和基坐標系的三軸平行，之后控制機器人沿工具坐標系的X軸和Z軸方向各運動1個位置，計算得到工具坐標系到法蘭末端坐標系的旋轉矩陣。結合求得的TCP的位置得到工具坐標系到法蘭末端坐標系的轉換矩陣為：

4.2 距離約束法實驗

將跟蹤儀靶球固定在機器人ABB IRB2600末端，靶球的球心即為TCP?？刂茩C器人運動到空間任意25個位置處。利用激光跟蹤儀測量靶球在25個位置處的坐標，同時記錄機器人法蘭末端在25個位置處的位置和姿態(tài)，距離約束法的工具坐標系標定實驗如圖9所示。標定過程中記錄的數據如表2所示，其中靶球位置數據由激光跟蹤儀測量得到，法蘭末端的位姿數據（基坐標系下）由機器人直接得到，法蘭末端的姿態(tài)用四元數Q1，Q2，Q3和Q4表示。根據實驗數據計算得到TCP在基坐標系下的位置為(?0.114 6，?0.209 9，39.993 2)T。之后根據2.3中的姿態(tài)標定方法控制機器人沿工具坐標系的X軸和Z軸方向分別運動，計算工具坐標系在法蘭末端坐標系下的變換矩陣。最后得到工具坐標系到法蘭末端坐標系下的轉換矩陣為：

表2 TCP標定數據Tab.2 Data of TCP calibration （mm）

圖9 距離約束法TCP標定實驗Fig.9 TCP calibration experiment based on distance constraint

4.3 精度對比

為對比兩種工具坐標系標定方法的標定精度，控制機器人運動到空間任意20個位置處，根據兩種方法得到的工具坐標系標定結果分別計算靶球球心在20個位置處的坐標（基坐標系下），并將其作為測量值。激光跟蹤儀測得的靶球在20個位置處的坐標作為真實值，通過公共點轉換得到兩種工具坐標系標定方法的誤差。圖10表示兩種標定方法下機器人在空間任意20個位置處的誤差。表3表示兩種標定方法下機器人在基坐標系三個軸向上的均方根誤差。

圖10 工具坐標系標定誤差Fig.10 Calibration error of tool coordinate system

從圖中可以看出，兩種方法的精度轉換誤差都在1.2 mm以內。靶球在不同位置處的誤差有所不同但相差不大，符合機器人在不同位置時的定位誤差不同的特點。同時，兩種標定方法在任意位置處的誤差相差不大，說明兩種方法的標定精度相當。

從表3中可以看出：幾何法和距離約束法的標定誤差在三軸方向上有所不同，但是位置均方根誤差基本相同。再次驗證了兩種方法的標定精度基本一致。

表3 兩種標定方法的RMSTab.3 RMS of two calibration methods（mm）

經過分析發(fā)現(xiàn)，本文提出的幾何法工具坐標系標定誤差來源主要有以下兩個方面：一是法蘭盤定位孔徑本身存在一定的誤差，6個定位孔的大小并不能保證完全地相同，同時6個未定位孔的位置并不能保證完全對稱，該誤差主要是由于生產加工機器人過程中產生的系統(tǒng)誤差引起的；另一方面是利用多個點進行圓心擬合以及平面方程擬合時會存在一定的誤差，導致計算得到的法蘭盤中心和實際的法蘭盤中心存在一定的偏差，從而導致工具坐標系標定結果存在一定偏差。通過結果發(fā)現(xiàn)，幾何法與距離約束法的標定結果基本相同，原因可能在于激光跟蹤儀本身的測量精度較高，因此工具坐標系標定的誤差主要是由于工業(yè)機器人本身所引起的，因此兩種方法得到的工具坐標系標定精度基本相同。

5 結 論

本文通過分析工業(yè)機器人的法蘭盤結構，基于法蘭盤上各點的幾何關系，提出一種基于幾何法原理機器人工具坐標系快速標定方法。該方法不需要控制運動，只需要利用激光跟蹤儀測量機器人末端的6個點，就能實現(xiàn)工具坐標系的位置標定，標定過程簡單且不受機器人運動范圍的限制。之后，任意更換機器人法蘭末端的工具只需加測一點就可以實現(xiàn)工具坐標系的位置標定。而傳統(tǒng)的標定方法在更換機器人末端工具后，要重新進行工具坐標系標定。本文提出的標定方法標定效率大大提高，在實際機器人工具坐標系標定中更具適用性。同時，和基于距離約束的工具坐標系標定方法進行精度對比分析發(fā)現(xiàn)二者的精度基本一致。實驗結果證明：基于幾何法的工具坐標系標定后機器人的定位均方根誤差為0.692 mm，說明該方法具有較高的精度，能夠滿足機器人高精度作業(yè)任務的要求。