本發(fā)明涉及電子測量和數(shù)據(jù)處理技術(shù)領(lǐng)域，特別涉及一種測量精度高、動態(tài)性好的工業(yè)機(jī)器人動態(tài)六維參數(shù)測量方法。

背景技術(shù)：

工業(yè)機(jī)器人是現(xiàn)代化生產(chǎn)和流水線作業(yè)的核心設(shè)備。《GB/T12642-2013工業(yè)機(jī)器人性能規(guī)范及試驗(yàn)方法》對工業(yè)機(jī)器人的各項(xiàng)性能指標(biāo)進(jìn)行了明確詳細(xì)的闡述，規(guī)定了工業(yè)機(jī)器人14項(xiàng)需要進(jìn)行測量的性能指標(biāo)及其測試方法，對機(jī)器人末端位姿的測量是工業(yè)機(jī)器人性能測量的核心內(nèi)容，機(jī)器人的位姿是指工業(yè)機(jī)器人的TCP點(diǎn)相對基座坐標(biāo)系的位置(x、y、z)和以TCP點(diǎn)為原點(diǎn)的坐標(biāo)系相對基座坐標(biāo)系的空間姿態(tài)(a、b、c)，同時對位置和姿態(tài)進(jìn)行測量合稱為六維參數(shù)測量。

現(xiàn)有技術(shù)中包括基于攝像機(jī)的圖像法和基于多激光靶球的多靶球法。前者受限于圖像分辨率因素，難以做到高精度。多靶球方案基于激光跟蹤儀和多個靶球，在靜態(tài)時通過激光的切換實(shí)現(xiàn)姿態(tài)測量，但運(yùn)動時無法對多靶球進(jìn)行同一位姿下的測量，無法實(shí)現(xiàn)六維測量的動態(tài)性。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的發(fā)明目的是為了克服現(xiàn)有技術(shù)中的方法精度低或無法實(shí)現(xiàn)動態(tài)性的不足，提供了一種測量精度高、動態(tài)性好的工業(yè)機(jī)器人動態(tài)六維參數(shù)測量方法。

為了實(shí)現(xiàn)上述目的，本發(fā)明采用以下技術(shù)方案：

一種工業(yè)機(jī)器人動態(tài)六維參數(shù)測量方法，包括激光跟蹤儀，計算機(jī)，六維測量儀，設(shè)于機(jī)器人上的示教器，六維測量儀包括六維靶球和處理器，六維靶球包括激光靶球、三向光纖陀螺儀和三軸加速度計，激光靶球、三向光纖陀螺儀和三軸加速度計均與處理器電連接，計算機(jī)分別與激光跟蹤儀、示教器和處理器電連接；包括如下步驟：

(1-1)確定測量前的六維靶球基準(zhǔn)姿態(tài)，獲得機(jī)器人姿態(tài)和六維靶球的測量姿態(tài)轉(zhuǎn)換矩陣T₁；

(1-2)獲得測量坐標(biāo)系的位置轉(zhuǎn)換矩陣T₂；

(1-3)計算機(jī)獲得激光跟蹤儀輸出的實(shí)時位置信號(x(t)，y(t)，z(t))，三向光纖陀螺儀輸出的實(shí)時轉(zhuǎn)速信號(vx(t)，vy(t)，vz(t))，三軸加速度計輸出的加速度信號(ax(t)，ay(t)，az(t))；

(1-4)計算機(jī)將位置信號(x(t)，y(t)，z(t))從測量坐標(biāo)系轉(zhuǎn)換到機(jī)器人基座坐標(biāo)系下的位置數(shù)據(jù)Pr(t)；

(1-5)計算機(jī)將陀螺儀的3個轉(zhuǎn)速信號積分到角度信號，三個角度值即為測量坐標(biāo)系下的姿態(tài)數(shù)據(jù)，將姿態(tài)數(shù)據(jù)從測量坐標(biāo)系轉(zhuǎn)換到機(jī)器人基座坐標(biāo)系下的姿態(tài)數(shù)據(jù)；

(1-6)對位置數(shù)據(jù)和姿態(tài)數(shù)據(jù)按時間戳進(jìn)行合并，獲得高精度、動態(tài)六維數(shù)據(jù)。

無論現(xiàn)有方法還是本發(fā)明方法，基本過程都包括測量坐標(biāo)系下六維參數(shù)測量和將測量坐標(biāo)系下參數(shù)通過轉(zhuǎn)換矩陣轉(zhuǎn)換到機(jī)器人基座坐標(biāo)系下。六維參數(shù)測量是指測量系統(tǒng)通過特定的傳感識別裝置，獲得機(jī)器人TCP點(diǎn)在空間中的位置以及TCP坐標(biāo)系的姿態(tài)，此時的位置數(shù)據(jù)和姿態(tài)數(shù)據(jù)都是基于測量坐標(biāo)系，是完成六維測量的基礎(chǔ)，對機(jī)器人六維測量，關(guān)注的焦點(diǎn)是測量精度、能否動態(tài)測量、動態(tài)測量的數(shù)據(jù)輸出率是多少，不同的測量方法差異極大。將測量數(shù)據(jù)從測量坐標(biāo)轉(zhuǎn)換到機(jī)器人坐標(biāo)本質(zhì)上是一組規(guī)則下的數(shù)學(xué)運(yùn)算，不同的測量方法實(shí)現(xiàn)原理基本相同，都是基于機(jī)器人運(yùn)動學(xué)的理論進(jìn)行數(shù)學(xué)變換來實(shí)現(xiàn)。

本發(fā)明采用的六維測量方案基于激光+光纖的方案，本發(fā)明數(shù)據(jù)運(yùn)算量更小，大幅降低對處理器要求；數(shù)據(jù)輸出率從幾百Hz提升到幾千Hz，支持更好的動態(tài)測量；激光和光纖技術(shù)都是高精度測量技術(shù)，保證了機(jī)器人測量的高精度。

作為優(yōu)選，步驟(1-1)包括如下步驟：

使機(jī)器人保持靜止?fàn)顟B(tài)，記錄三向光纖陀螺儀的基準(zhǔn)位姿，在測量坐標(biāo)系下，此時機(jī)器人姿態(tài)坐標(biāo)為(0，0，0)，同時記錄示教器上的機(jī)器人的初始姿態(tài)(a(0)，b(0)，c(0))；

設(shè)定姿態(tài)角為(a，b，c)，那么實(shí)時動態(tài)姿態(tài)角表示為(a(t)，b(t)，c(t))，按Rzyx的旋轉(zhuǎn)順序?qū)⒆藨B(tài)角(a，b，c)轉(zhuǎn)換為旋轉(zhuǎn)矩陣T

其中，c表示cos，s表示sin；

將機(jī)器人基座坐標(biāo)系下的姿態(tài)數(shù)據(jù)轉(zhuǎn)換為旋轉(zhuǎn)矩陣T₁：

作為優(yōu)選，步驟(1-2)包括如下具體步驟：

示教器控制機(jī)器人運(yùn)動4個點(diǎn)，激光跟蹤儀測得4點(diǎn)測量值分別為((mxi，myi，mzi))，i＝1，2，3，4，寫成矩陣形式

示教器讀得4點(diǎn)測量值分別為((rxi，ryi，rzi))，i＝1，2，3，4，寫成矩陣形式

利用公式T′＝B*A^-1計算測量坐標(biāo)系和機(jī)座坐標(biāo)系之間的轉(zhuǎn)換矩陣T′，其中，A^-1為矩陣A的逆。

作為優(yōu)選，步驟(1-4)包括如下步驟：

測量坐標(biāo)下測得的位置值為pm(t)＝(mx(t)，my(t)，mz(t))，利用公式將各坐標(biāo)值轉(zhuǎn)換成機(jī)器人基座坐標(biāo)系下的位置數(shù)據(jù)Pr(t)。

作為優(yōu)選，步驟(1-5)包括如下步驟：

(5-1)利用矩陣T，將測得的測量坐標(biāo)系下的歐拉角形式姿態(tài)數(shù)據(jù)pose(t)＝(ma(t)，mb(t)，mc(t))轉(zhuǎn)換為旋轉(zhuǎn)矩陣形式姿態(tài)數(shù)據(jù)Ps(t)：

(5-2)利用公式Tr(t)＝T1*Ps(t)將姿態(tài)序列從測量坐標(biāo)系下轉(zhuǎn)換到機(jī)器人基座坐標(biāo)系下，此時姿態(tài)數(shù)據(jù)為旋轉(zhuǎn)矩陣形式；

(5-3)將旋轉(zhuǎn)矩陣形式姿態(tài)序列Tr(t)轉(zhuǎn)換為歐拉角形式，假設(shè)旋轉(zhuǎn)矩陣形式時，

利用公式c(t)＝atan(y1(t)/x1(t))計算角度c(t)，其中，atan為反正切函數(shù)；

設(shè)定cz(t)＝cos(c(t))，sz(t)＝sin(c(t))；

numb(t)＝-z1(t)，

denb(t)＝x1(t)*cz(t)+y1(t)*sz(t)，

利用公式b(t)＝atan(num(t)/den(t))計算角度b(t)；

設(shè)定numa(t)＝x3(t)*sz(t)-y3(t)*cz(t)

dena(t)＝y(tǒng)2(t)*cz(t)-x2(t)*sz(t)

利用公式a(t)＝atan(num(t)/den(t))計算角度a(t)；

得到歐拉角形式的姿態(tài)數(shù)據(jù)(a(t)，b(t)，c(t))。

因此，本發(fā)明具有如下有益效果：

<1>數(shù)據(jù)運(yùn)算量更小，大幅降低了對處理器要求。

<2>數(shù)據(jù)輸出率從幾百Hz提升到幾千Hz，支持更好的動態(tài)測量。

<3>激光和光纖技術(shù)都是高精度測量技術(shù)，保證了機(jī)器人測量的高精度。

附圖說明

圖1是本發(fā)明的一種流程圖。

具體實(shí)施方式

下面結(jié)合附圖和具體實(shí)施方式對本發(fā)明做進(jìn)一步的描述。

如圖1所示的實(shí)施例是一種工業(yè)機(jī)器人動態(tài)六維參數(shù)測量方法，包括激光跟蹤儀，計算機(jī)，六維測量儀，設(shè)于機(jī)器人上的示教器，六維測量儀包括六維靶球和處理器，六維靶球包括激光靶球、三向光纖陀螺儀和三軸加速度計，激光靶球、三向光纖陀螺儀和三軸加速度計均與處理器電連接，計算機(jī)分別與激光跟蹤儀、示教器和處理器電連接；包括如下步驟：

步驟100，確定測量前的六維靶球基準(zhǔn)姿態(tài)，獲得機(jī)器人姿態(tài)和六維靶球的測量姿態(tài)轉(zhuǎn)換矩陣T₁；

包括如下步驟：

使機(jī)器人保持靜止?fàn)顟B(tài)，記錄三向光纖陀螺儀的基準(zhǔn)位姿，在測量坐標(biāo)系下，此時機(jī)器人姿態(tài)坐標(biāo)為(0，0，0)，同時記錄示教器上的機(jī)器人的初始姿態(tài)(a(0)，b(0)，c(0))；

設(shè)定姿態(tài)角為(a，b，c)，那么實(shí)時動態(tài)姿態(tài)角表示為(a(t)，b(t)，c(t))，按Rzyx的旋轉(zhuǎn)順序?qū)⒆藨B(tài)角(a，b，c)轉(zhuǎn)換為旋轉(zhuǎn)矩陣T

其中，c表示cos，s表示sin；

將機(jī)器人基座坐標(biāo)系下的姿態(tài)數(shù)據(jù)轉(zhuǎn)換為旋轉(zhuǎn)矩陣T₁：

步驟200，獲得測量坐標(biāo)系的位置轉(zhuǎn)換矩陣T₂；

包括如下具體步驟：

示教器控制機(jī)器人運(yùn)動4個點(diǎn)，激光跟蹤儀測得4點(diǎn)測量值分別為((mxi，myi，mzi))，i＝1，2，3，4，寫成矩陣形式

示教器讀得4點(diǎn)測量值分別為((rxi，ryi，rzi))，i＝1，2，3，4，寫成矩陣形式

利用公式T′＝B*A^-1計算測量坐標(biāo)系和機(jī)座坐標(biāo)系之間的轉(zhuǎn)換矩陣T′，其中，A^-1為矩陣A的逆。

步驟300，計算機(jī)獲得激光跟蹤儀輸出的實(shí)時位置信號(x(t)，y(t)，z(t))，三向光纖陀螺儀輸出的實(shí)時轉(zhuǎn)速信號(vx(t)，vy(t)，vz(t))，三軸加速度計輸出的加速度信號(ax(t)，ay(t)，az(t))；

步驟400，計算機(jī)將位置信號(x(t)，y(t)，z(t))從測量坐標(biāo)系轉(zhuǎn)換到機(jī)器人基座坐標(biāo)系下的位置數(shù)據(jù)Pr(t)；

包括如下步驟：

測量坐標(biāo)下測得的位置值為pm(t)＝(mx(t)，my(t)，mz(t))，利用公式將各坐標(biāo)值轉(zhuǎn)換成機(jī)器人基座坐標(biāo)系下的位置數(shù)據(jù)Pr(t)。

步驟500，計算機(jī)將陀螺儀的3個轉(zhuǎn)速信號積分到角度信號，三個角度值即為測量坐標(biāo)系下的姿態(tài)數(shù)據(jù)，將姿態(tài)數(shù)據(jù)從測量坐標(biāo)系轉(zhuǎn)換到機(jī)器人基座坐標(biāo)系下的姿態(tài)數(shù)據(jù)；

包括如下步驟：

(5-1)利用矩陣T，將測得的測量坐標(biāo)系下的歐拉角形式姿態(tài)數(shù)據(jù)pose(t)＝(ma(t)，mb(t)，mc(t))轉(zhuǎn)換為旋轉(zhuǎn)矩陣形式姿態(tài)數(shù)據(jù)Ps(t)：

(5-2)利用公式Tr(t)＝T1*Ps(t)將姿態(tài)序列從測量坐標(biāo)系下轉(zhuǎn)換到機(jī)器人基座坐標(biāo)系下，此時姿態(tài)數(shù)據(jù)為旋轉(zhuǎn)矩陣形式；

(5-3)將旋轉(zhuǎn)矩陣形式姿態(tài)序列Tr(t)轉(zhuǎn)換為歐拉角形式，假設(shè)旋轉(zhuǎn)矩陣形式時，

利用公式c(t)＝atan(y1(t)/x1(t))計算角度c(t)，其中，atan為反正切函數(shù)；

設(shè)定cz(t)＝cos(c(t))，sz(t)＝sin(c(t))；

numb(t)＝-z1(t)，

denb(t)＝x1(t)*cz(t)+y1(t)*sz(t)，

利用公式b(t)＝atan(num(t)/den(t))計算角度b(t)；

設(shè)定numa(t)＝x3(t)*sz(t)-y3(t)*cz(t)

dena(t)＝y(tǒng)2(t)*cz(t)-x2(t)*sz(t)

利用公式a(t)＝atan(num(t)/den(t))計算角度a(t)；

得到歐拉角形式的姿態(tài)數(shù)據(jù)(a(t)，b(t)，c(t))。

步驟600，對位置數(shù)據(jù)和姿態(tài)數(shù)據(jù)按時間戳進(jìn)行合并，獲得高精度、動態(tài)六維數(shù)據(jù)。

應(yīng)理解，本實(shí)施例僅用于說明本發(fā)明而不用于限制本發(fā)明的范圍。此外應(yīng)理解，在閱讀了本發(fā)明講授的內(nèi)容之后，本領(lǐng)域技術(shù)人員可以對本發(fā)明作各種改動或修改，這些等價形式同樣落于本申請所附權(quán)利要求書所限定的范圍。