基于超聲波測距的TCP標(biāo)定方法研究

吳樂平 , 顧晟吉,程偉國

(1. 南京旭上數(shù)控技術(shù)有限公司，江蘇 南京 211167； 2. 南京機(jī)電職業(yè)技術(shù)學(xué)院，江蘇 南京 211135)

0 引言

隨著機(jī)器人在各行各業(yè)應(yīng)用的逐漸深入,對機(jī)器人性能的要求也在逐漸提高[1]。工具中心點(tool center point，TCP)標(biāo)定是機(jī)器人應(yīng)用中不可避免的重要環(huán)節(jié)之一，而且TCP標(biāo)定精度直接影響機(jī)器人的定位精度[2]。因此，就必須要研究一種快速、高精度的TCP標(biāo)定方法。

機(jī)器人TCP標(biāo)定是計算工具末端點相對于機(jī)器人末端坐標(biāo)系的位置辨識過程，該位置是一個未知的確定位置。文獻(xiàn)[3-4]分別采取SVD分解和投影約束的方法求解TCP參數(shù)，但都需要人工校準(zhǔn)，而且人工校準(zhǔn)帶來的誤差無法消除。文獻(xiàn)[5]提出一種基于雙目視覺的TCP標(biāo)定方法，不需要手動校準(zhǔn)，標(biāo)定精度高，但是雙目視覺系統(tǒng)的價格昂貴，不適合推廣。

本文提出一種基于超聲波測距的TCP標(biāo)定方法，只需要將機(jī)器人任意移動到3個不同位置，不需要操作人員人工校準(zhǔn)，實施簡單，標(biāo)定速度快，精度高，通用性強(qiáng)。

1 TCP標(biāo)定方法分析

為了方便操作人員使用，在機(jī)器人系統(tǒng)中定義了多種坐標(biāo)系，如圖1所示的坐標(biāo)系{B}為基坐標(biāo)系；坐標(biāo)系{R}為腕部坐標(biāo)系；坐標(biāo)系{T}為工具坐標(biāo)系。TCP標(biāo)定過程即獲得坐標(biāo)系{T}的原點在坐標(biāo)系{R}下坐標(biāo)值(xT,yT,zT)的過程。本文所述方法的標(biāo)定流程共分為以下5個步驟：

1)如圖2所示，首先在需要標(biāo)定的工具坐標(biāo)系原點安裝超聲波發(fā)送探頭S1，在矩形底座的3個頂點處安裝3個超聲波接收探頭R1、R2、R3，則R13⊥R12(Rij表示由Ri由指向Rj的向量)，并且已知|R12|=d1、|R13|=d2，R12、R13分別平行于基坐標(biāo)x、y軸。以R1為坐標(biāo)原點，R12、R13所在方向分別為x、y軸，建立如圖1所示的標(biāo)定坐標(biāo)系{C}；并設(shè)定采樣次數(shù)n，n為≥3的自然數(shù)。

圖1 機(jī)器人坐標(biāo)系示意圖

圖2 超聲波傳感器安裝示意圖

2) 將機(jī)器人移動到任意位置，探頭S1發(fā)送超聲波信號，同時開始計時。每個接收探頭接收到信號后，立即記錄從S1發(fā)送信號到接收所用時間，記ti為探頭Ri接收到S1信號所用的時間(i=1,2,3)，即傳輸時間，并記錄當(dāng)前周圍的環(huán)境溫度k和機(jī)器人當(dāng)前各關(guān)節(jié)角度值。

3)計算TCP點在坐標(biāo)系下{C}的坐標(biāo)值(x,y,z)。記li為探頭S1到探頭Ri之間的距離。則li=ti×(331.5+0.607k)，其中i=1,2,3,并且滿足條件：

4)重復(fù)步驟2)和步驟3)共n次，得到n組(x,y,z)(記為(xi,yi,zi),i=1,2,…,n)和與之相對應(yīng)的n組機(jī)器人各關(guān)節(jié)角度。

(1)

式中:

(2)

將式(2)代入式(1)，得

(3)

將式(3)中從第2行開始，每行減去第1行，整理得

(4)

記：

則式(4)可以表示為

解之得：

(xT,yT,zT)T=A+B

其中A+=(ATA)-1AT。

2 實驗分析

采用上述辦法，對NJ-XSM6機(jī)器人進(jìn)行安裝測試，取n=3,獲得3組數(shù)據(jù)如表1所示。jm為第m個關(guān)節(jié)的角度，單位為(°)。并且已知機(jī)器人D-H參數(shù)表如表2所示，由D-H參數(shù)表可獲得機(jī)器人正運(yùn)動學(xué)方程。

表1 3次測量結(jié)果記錄表

表2 機(jī)器人D-H參數(shù)表

將上述數(shù)值代入計算得：

(xT,yT,zT)=(50.03,0.03,199.98)

從設(shè)計圖樣中獲得x、y、z的尺寸分別為50，0，200。由此可見，本文所述標(biāo)定方法能夠滿足實際的工業(yè)需求。

3 結(jié)語

本文方法首先采用超聲波傳感器和溫度傳感器獲得距離數(shù)據(jù)，機(jī)器人關(guān)節(jié)角度可以直接從機(jī)器人系統(tǒng)中讀出。再經(jīng)過坐標(biāo)變換便可以直接獲得TCP參數(shù)。經(jīng)過驗證表明：該方法有效可行，有效避免人工校準(zhǔn)帶來的誤差，標(biāo)定精度高。