[发明专利]工业机器人动态六维参数测量方法

权利要求书

1.一种工业机器人动态六维参数测量方法，其特征是，包括激光跟踪仪，计算机，六维测量仪，设于机器人上的示教器，六维测量仪包括六维靶球和处理器，六维靶球包括激光靶球、三向光纤陀螺仪和三轴加速度计，激光靶球、三向光纤陀螺仪和三轴加速度计均与处理器电连接，计算机分别与激光跟踪仪、示教器和处理器电连接；包括如下步骤：

(1-1)确定测量前的六维靶球基准姿态，获得机器人姿态和六维靶球的测量姿态转换矩阵T₁；

使机器人保持静止状态，记录三向光纤陀螺仪的基准位姿，在测量坐标系下，此时机器人姿态坐标为(0，0，0)，同时记录示教器上的机器人的初始姿态(a(0)，b(0)，c(0))；

设定姿态角为(a，b，c)，那么实时动态姿态角表示为(a(t)，b(t)，c(t))，按Rzyx的旋转顺序将姿态角(a，b，c)转换为旋转矩阵T

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

将机器人基座坐标系下的姿态数据转换为旋转矩阵T₁：

(1-2)获得测量坐标系的位置转换矩阵T₂；

示教器控制机器人运动4个点，激光跟踪仪测得4点测量值分别为((mxi，myi，mzi))，i＝1，2，3，4，写成矩阵形式

示教器读得4点测量值分别为((rxi，ryi，rzi))，i＝1，2，3，4，写成矩阵形式

利用公式T′＝B*A^-1计算测量坐标系和机座坐标系之间的转换矩阵T′，其中，A^-1为矩阵A的逆；

(1-3)计算机获得激光跟踪仪输出的实时位置信号(x(t)，y(t)，z(t))，三向光纤陀螺仪输出的实时转速信号(vx(t)，vy(t)，vz(t))，三轴加速度计输出的加速度信号(ax(t)，ay(t)，az(t))；

(1-4)计算机将位置信号(x(t)，y(t)，z(t))从测量坐标系转换到机器人基座坐标系下的位置数据Pr(t)；

(1-5)计算机将陀螺仪的3个转速信号积分到角度信号，三个角度值即为测量坐标系下的姿态数据，将姿态数据从测量坐标系转换到机器人基座坐标系下的姿态数据；

(1-6)对位置数据和姿态数据按时间戳进行合并，获得高精度、动态六维数据。

2.根据权利要求1所述的工业机器人动态六维参数测量方法，其特征是，步骤(1-4)包括如下步骤：

测量坐标下测得的位置值为pm(t)＝(mx(t)，my(t)，mz(t))，利用公式将各坐标值转换成机器人基座坐标系下的位置数据Pr(t)。

3.根据权利要求1所述的工业机器人动态六维参数测量方法，其特征是，步骤(1-5)包括如下步骤：

(4-1)利用矩阵T，将测得的测量坐标系下的欧拉角形式姿态数据pose(t)＝(ma(t)，mb(t)，mc(t))转换为旋转矩阵形式姿态数据Ps(t)：

(4-2)利用公式Tr(t)＝T1*Ps(t)将姿态序列从测量坐标系下转换到机器人基座坐标系下，此时姿态数据为旋转矩阵形式；

(4-3)将旋转矩阵形式姿态序列Tr(t)转换为欧拉角形式，假设旋转矩阵形式时，

利用公式c(t)＝atan(y1(t)/x1(t))计算角度c(t)，其中，atan为反正切函数；

设定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)＝y2(t)*cz(t)-x2(t)*sz(t)

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

得到欧拉角形式的姿态数据(a(t)，b(t)，c(t))。