技術特征：

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

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

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

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

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

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

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

2.根據(jù)權利要求1所述的工業(yè)機器人動態(tài)六維參數(shù)測量方法，其特征是，步驟(1-1)包括如下步驟：

使機器人保持靜止狀態(tài)，記錄三向光纖陀螺儀的基準位姿，在測量坐標系下，此時機器人姿態(tài)坐標為(0，0，0)，同時記錄示教器上的機器人的初始姿態(tài)(a(0)，b(0)，c(0))；

設定姿態(tài)角為(a，b，c)，那么實時動態(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；

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

$T_1=\left[\begin{array}{ccc}cb\left(0\right)*cc\left(0\right)& sa\left(0\right)*sb\left(0\right)*cc\left(0\right)-ca\left(0\right)*sc\left(0\right)& ca\left(0\right)*sb\left(0\right)*cc\left(0\right)+sa\left(0\right)*sc\left(0\right)\\ cb\left(0\right)*sc\left(0\right)& sa\left(0\right)*sb\left(0\right)*sc\left(0\right)-ca\left(0\right)*cc\left(0\right)& ca\left(0\right)*sb\left(0\right)*sc\left(0\right)-sa\left(0\right)*cc\left(0\right)\\ -sb\left(0\right)& sa\left(0\right)*cb\left(0\right)& ca\left(0\right)*cb\left(0\right)\end{array}\right].$

3.根據(jù)權利要求1所述的工業(yè)機器人動態(tài)六維參數(shù)測量方法，其特征是，步驟(1-2)包括如下具體步驟：

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

$A=\left[\begin{array}{cccc}mx1& mx2& mx3& mx4\\ my1& my2& my3& my4\\ mz1& mz2& mz3& mz4\\ 1& 1& 1& 1\end{array}\right]$

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

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

4.根據(jù)權利要求1所述的工業(yè)機器人動態(tài)六維參數(shù)測量方法，其特征是，步驟(1-4)包括如下步驟：

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

5.根據(jù)權利要求1所述的工業(yè)機器人動態(tài)六維參數(shù)測量方法，其特征是，步驟(1-5)包括如下步驟：

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

$Ps\left(t\right)=\left[\begin{array}{ccc}cmb\left(t\right)*cmc\left(t\right)& sma\left(t\right)*smb\left(t\right)*cmc\left(t\right)-cma\left(t\right)*smc\left(t\right)& cma\left(t\right)*smb\left(t\right)*cmc\left(t\right)+sma\left(t\right)*smc\left(t\right)\\ cmb\left(t\right)*smc\left(t\right)& sma\left(t\right)*smb\left(t\right)*smc\left(t\right)-cma\left(t\right)*cmc\left(t\right)& cma\left(t\right)*smb\left(t\right)*smc\left(t\right)-sma\left(t\right)*smc\left(t\right)\\ -smb\left(t\right)& sma\left(t\right)*cmb\left(t\right)& cma\left(t\right)*cmb\left(t\right)\end{array}\right]$

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

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

利用公式c(t)＝atan(y1(t)/x1(t))計算角度c(t)，其中，atan為反正切函數(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)；

設定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))。