本发明公开了一种工业机器人动态六维参数测量方法，包括激光跟踪仪，计算机，六维测量仪，设于机器人上的示教器，六维测量仪包括六维靶球和处理器，六维靶球包括激光靶球、三向光纤陀螺仪和三轴加速度计，激光靶球、三向光纤陀螺仪和三轴加速度计均与处理器电连接，计算机分别与激光跟踪仪、示教器和处理器电连接。本发明具有运算量更小，支持更好的动态测量测量精度高的特点。
1.一种工业机器人动态六维参数测量方法，其特征是，包括激光跟踪仪，计算机，六维测量仪，设于机器人上的示教器，六维测量仪包括六维靶球和处理器，六维靶球包括激光靶球、三向光纤陀螺仪和三轴加速度计，激光靶球、三向光纤陀螺仪和三轴加速度计均与处理器电连接，计算机分别与激光跟踪仪、示教器和处理器电连接；包括如下步骤：(1-1)确定测量前的六维靶球基准姿态，获得机器人姿态和六维靶球的测量姿态转换矩阵T1；
使机器人保持静止状态，记录三向光纤陀螺仪的基准位姿，在测量坐标系下，此时机器人姿态坐标为(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
将机器人基座坐标系下的姿态数据转换为旋转矩阵T1：(1-2)获得测量坐标系的位置转换矩阵T2；
示教器控制机器人运动4个点，激光跟踪仪测得4点测量值分别为((mxi，myi，mzi))，i＝1，2，3，4，写成矩阵形式示教器读得4点测量值分别为((rxi，ryi，rzi))，i＝1，2，3，4，写成矩阵形式利用公式T′＝B*A-1计算测量坐标系和机座坐标系之间的转换矩阵T′，其中，-1A 为矩阵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))；
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))。
工业机器人动态六维参数测量方法
技术领域
[0001] 本发明涉及电子测量和数据处理技术领域，特别涉及一种测量精度高、动态性好的工业机器人动态六维参数测量方法。
背景技术
[0002] 工业机器人是现代化生产和流水线作业的核心设备。《GB/T12642-2013工业机器人性能规范及试验方法》对工业机器人的各项性能指标进行了明确详细的阐述，规定了工业机器人14项需要进行测量的性能指标及其测试方法，对机器人末端位姿的测量是工业机器人性能测量的核心内容，机器人的位姿是指工业机器人的TCP点相对基座坐标系的位置(x、y、z)和以TCP点为原点的坐标系相对基座坐标系的空间姿态(a、b、c)，同时对位置和姿态进行测量合称为六维参数测量。
[0003] 现有技术中包括基于摄像机的图像法和基于多激光靶球的多靶球法。前者受限于图像分辨率因素，难以做到高精度。多靶球方案基于激光跟踪仪和多个靶球，在静态时通过激光的切换实现姿态测量，但运动时无法对多靶球进行同一位姿下的测量，无法实现六维测量的动态性。
发明内容
[0004] 本发明的发明目的是为了克服现有技术中的方法精度低或无法实现动态性的不足，提供了一种测量精度高、动态性好的工业机器人动态六维参数测量方法。
[0005] 为了实现上述目的，本发明采用以下技术方案：
[0006] 一种工业机器人动态六维参数测量方法，包括激光跟踪仪，计算机，六维测量仪，设于机器人上的示教器，六维测量仪包括六维靶球和处理器，六维靶球包括激光靶球、三向光纤陀螺仪和三轴加速度计，激光靶球、三向光纤陀螺仪和三轴加速度计均与处理器电连接，计算机分别与激光跟踪仪、示教器和处理器电连接；包括如下步骤：
[0007] (1-1)确定测量前的六维靶球基准姿态，获得机器人姿态和六维靶球的测量姿态转换矩阵T1；
[0008] (1-2)获得测量坐标系的位置转换矩阵T2；
[0009] (1-3)计算机获得激光跟踪仪输出的实时位置信号(x(t)，y(t)，z(t))，三向光纤陀螺仪输出的实时转速信号(vx(t)，vy(t)，vz(t))，三轴加速度计输出的加速度信号(ax(t)，ay(t)，az(t))；
[0010] (1-4)计算机将位置信号(x(t)，y(t)，z(t))从测量坐标系转换到机器人基座坐标系下的位置数据Pr(t)；
[0011] (1-5)计算机将陀螺仪的3个转速信号积分到角度信号，三个角度值即为测量坐标系下的姿态数据，将姿态数据从测量坐标系转换到机器人基座坐标系下的姿态数据；
[0012] (1-6)对位置数据和姿态数据按时间戳进行合并，获得高精度、动态六维数据。
[0013] 无论现有方法还是本发明方法，基本过程都包括测量坐标系下六维参数测量和将测量坐标系下参数通过转换矩阵转换到机器人基座坐标系下。六维参数测量是指测量系统通过特定的传感识别装置，获得机器人TCP点在空间中的位置以及TCP坐标系的姿态，此时的位置数据和姿态数据都是基于测量坐标系，是完成六维测量的基础，对机器人六维测量，关注的焦点是测量精度、能否动态测量、动态测量的数据输出率是多少，不同的测量方法差异极大。将测量数据从测量坐标转换到机器人坐标本质上是一组规则下的数学运算，不同的测量方法实现原理基本相同，都是基于机器人运动学的理论进行数学变换来实现。
[0014] 本发明采用的六维测量方案基于激光+光纤的方案，本发明数据运算量更小，大幅降低对处理器要求；数据输出率从几百Hz提升到几千Hz，支持更好的动态测量；激光和光纤技术都是高精度测量技术，保证了机器人测量的高精度。
[0015] 作为优选，步骤(1-1)包括如下步骤：
[0016] 使机器人保持静止状态，记录三向光纤陀螺仪的基准位姿，在测量坐标系下，此时机器人姿态坐标为(0，0，0)，同时记录示教器上的机器人的初始姿态(a(0)，b(0)，c(0))；
[0017] 设定姿态角为(a，b，c)，那么实时动态姿态角表示为(a(t)，b(t)，c(t))，按Rzyx的旋转顺序将姿态角(a，b，c)转换为旋转矩阵T
[0018] 其中，c表示cos，s表示sin；
[0019] 将机器人基座坐标系下的姿态数据转换为旋转矩阵T1：
[0020]
[0021] 作为优选，步骤(1-2)包括如下具体步骤：
[0022] 示教器控制机器人运动4个点，激光跟踪仪测得4点测量值分别为((mxi，myi，mzi))，i＝1，2，3，4，写成矩阵形式
[0023]
[0024] 示教器读得4点测量值分别为((rxi，ryi，rzi))，i＝1，2，3，4，写成矩阵形式[0025] 利用公式T′＝B*A-1计算测量坐标系和机座坐标系之间的转换矩阵T′，其中，A-1为矩阵A的逆。
[0026] 作为优选，步骤(1-4)包括如下步骤：
[0027] 测量坐标下测得的位置值为pm(t)＝(mx(t)，my(t)，mz(t))，利用公式将各坐标值转换成机器人基座坐标系下的位置数据Pr(t)。
[0028] 作为优选，步骤(1-5)包括如下步骤：
[0029] (5-1)利用矩阵T，将测得的测量坐标系下的欧拉角形式姿态数据pose(t)＝(ma(t)，mb(t)，mc(t))转换为旋转矩阵形式姿态数据Ps(t)：
[0030]
[0031] (5-2)利用公式Tr(t)＝T1*Ps(t)将姿态序列从测量坐标系下转换到机器人基座坐标系下，此时姿态数据为旋转矩阵形式；
[0032] (5-3)将旋转矩阵形式姿态序列Tr(t)转换为欧拉角形式，假设旋转矩阵形式时，[0033] 利用公式c(t)＝atan(y1(t)/x1(t))计算角度c(t)，其中，atan为反正切函数；
[0034] 设定cz(t)＝cos(c(t))，sz(t)＝sin(c(t))；
[0035] numb(t)＝-z1(t)，
[0036] denb(t)＝x1(t)*cz(t)+y1(t)*sz(t)，
[0037] 利用公式b(t)＝atan(num(t)/den(t))计算角度b(t)；
[0038] 设定numa(t)＝x3(t)*sz(t)-y3(t)*cz(t)
[0039] dena(t)＝y2(t)*cz(t)-x2(t)*sz(t)
[0040] 利用公式a(t)＝atan(num(t)/den(t))计算角度a(t)；
[0041] 得到欧拉角形式的姿态数据(a(t)，b(t)，c(t))。
[0042] 因此，本发明具有如下有益效果：
[0043] <1>数据运算量更小，大幅降低了对处理器要求。
[0044] <2>数据输出率从几百Hz提升到几千Hz，支持更好的动态测量。
[0045] <3>激光和光纤技术都是高精度测量技术，保证了机器人测量的高精度。
附图说明
[0046] 图1是本发明的一种流程图。
具体实施方式
[0047] 下面结合附图和具体实施方式对本发明做进一步的描述。
[0048] 如图1所示的实施例是一种工业机器人动态六维参数测量方法，包括激光跟踪仪，计算机，六维测量仪，设于机器人上的示教器，六维测量仪包括六维靶球和处理器，六维靶球包括激光靶球、三向光纤陀螺仪和三轴加速度计，激光靶球、三向光纤陀螺仪和三轴加速度计均与处理器电连接，计算机分别与激光跟踪仪、示教器和处理器电连接；包括如下步骤：
[0049] 步骤100，确定测量前的六维靶球基准姿态，获得机器人姿态和六维靶球的测量姿态转换矩阵T1；
[0050] 包括如下步骤：
[0051] 使机器人保持静止状态，记录三向光纤陀螺仪的基准位姿，在测量坐标系下，此时机器人姿态坐标为(0，0，0)，同时记录示教器上的机器人的初始姿态(a(0)，b(0)，c(0))；
[0052] 设定姿态角为(a，b，c)，那么实时动态姿态角表示为(a(t)，b(t)，c(t))，按Rzyx的旋转顺序将姿态角(a，b，c)转换为旋转矩阵T
[0053] 其中，c表示cos，s表示sin；
[0054] 将机器人基座坐标系下的姿态数据转换为旋转矩阵T1：
[0055]
[0056] 步骤200，获得测量坐标系的位置转换矩阵T2；
[0057] 包括如下具体步骤：
[0058] 示教器控制机器人运动4个点，激光跟踪仪测得4点测量值分别为((mxi，myi，mzi))，i＝1，2，3，4，写成矩阵形式
[0059]
[0060] 示教器读得4点测量值分别为((rxi，ryi，rzi))，i＝1，2，3，4，写成矩阵形式[0061] 利用公式T′＝B*A-1计算测量坐标系和机座坐标系之间的转换矩阵T′，其中，A-1为矩阵A的逆。
[0062] 步骤300，计算机获得激光跟踪仪输出的实时位置信号(x(t)，y(t)，z(t))，三向光纤陀螺仪输出的实时转速信号(vx(t)，vy(t)，vz(t))，三轴加速度计输出的加速度信号(ax(t)，ay(t)，az(t))；
[0063] 步骤400，计算机将位置信号(x(t)，y(t)，z(t))从测量坐标系转换到机器人基座坐标系下的位置数据Pr(t)；
[0064] 包括如下步骤：
[0065] 测量坐标下测得的位置值为pm(t)＝(mx(t)，my(t)，mz(t))，利用公式将各坐标值转换成机器人基座坐标系下的位置数据Pr(t)。
[0066] 步骤500，计算机将陀螺仪的3个转速信号积分到角度信号，三个角度值即为测量坐标系下的姿态数据，将姿态数据从测量坐标系转换到机器人基座坐标系下的姿态数据；
[0067] 包括如下步骤：
[0068] (5-1)利用矩阵T，将测得的测量坐标系下的欧拉角形式姿态数据pose(t)＝(ma(t)，mb(t)，mc(t))转换为旋转矩阵形式姿态数据Ps(t)：
[0069]
[0070] (5-2)利用公式Tr(t)＝T1*Ps(t)将姿态序列从测量坐标系下转换到机器人基座坐标系下，此时姿态数据为旋转矩阵形式；
[0071] (5-3)将旋转矩阵形式姿态序列Tr(t)转换为欧拉角形式，假设旋转矩阵形式时，[0072] 利用公式c(t)＝atan(y1(t)/x1(t))计算角度c(t)，其中，atan为反正切函数；
[0073] 设定cz(t)＝cos(c(t))，sz(t)＝sin(c(t))；
[0074] numb(t)＝-z1(t)，
[0075] denb(t)＝x1(t)*cz(t)+y1(t)*sz(t)，
[0076] 利用公式b(t)＝atan(num(t)/den(t))计算角度b(t)；
[0077] 设定numa(t)＝x3(t)*sz(t)-y3(t)*cz(t)
[0078] dena(t)＝y2(t)*cz(t)-x2(t)*sz(t)
[0079] 利用公式a(t)＝atan(num(t)/den(t))计算角度a(t)；
[0080] 得到欧拉角形式的姿态数据(a(t)，b(t)，c(t))。
[0081] 步骤600，对位置数据和姿态数据按时间戳进行合并，获得高精度、动态六维数据。
[0082] 应理解，本实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本申请所附权利要求书所限定的范围。
