立产品坐标系（3)，其相对于机械臂自由末端的TCP 坐标系（5)的位置及姿态可以用一个4X4的旋转变换矩阵Qpla_P表示，其中，TCP即为机 器人机械臂自由末端的位置；
[0057]
[0058] 3)切割变换矩阵的建立
[0059] 获得待加工产品的三维数据，并从中提取出产品坐标系（3)下的激光入射点 P2、P3、……、Pn，以及对应的激光入射点法线方向m……、Nn;根据激光入射点和激 光入射点法线方向建立局部坐标系（4)，以该局部坐标系的原点为激光入射点，坐标系相对 于产品坐标系（3)的位置及姿态可以用一个4X4的旋转变换矩阵Q_k2plate表示：
[0060] 对于切割点位置乃=A而言： Pr
[0061]
[0062] 4)计算理想状态下多轴机器人角度
[0063] 利用机器人运动学方程计算当激光垂直入射到"0"型标记时多轴机器人的角度， 根据机器人的D-Η参数，计算对应的机械臂可到达角度{qij，其中，qy表示产品的第i 个激光入射点位置对应的机器人第j个机械臂的角度；
[0064] 机器人运动方程为：
[0065]
[0066] ^ [0067]其中为机器第j各机械臂相对于第j一1个机械臂的旋转矩阵：
[0068]
[0069] 其中，θ』，apdjPa』为机器人D-Η参数，θ』为机械臂之间的夹角，包含υα』 为机械臂扭转角；士为机械臂之间的距离；&]为机械臂长度；上述参数代入式II中即可计算 每个激光切割点对应的机器人机械臂角度值qu，i多2 ;j多1 ;其中，当默认机器人每个轴 运动范围在-180deg到180deg时，根据机器人实际轴运动范围从中选取合适解，本实施例 选用的是ARB-120型机器人；
[0070] ARB-120型机器人D-Η参数
[0071]
[0073] 将上述参数代入gf的计算式中，求解出{q^};
[0074] 5)工装安装偏差的计算
[0075] 工装坐标系与机器人末端TCP坐标系并不重合，假设工装坐标系在机器人末端 TCP坐标系下的位置及姿杰用一个4X4的旋转变换矩阵表示：
[0076]
[0077] 当Θ为小角度时，sin ΘΘ;c〇sΘ1经计算得
其中 x〇，y()为中心平移的位置，9为中心旋转的角度；
[0078]
[0079] 假设"0"型标记的位置为（pXl，pyi)，然后发现当机器人多轴角度设置为计算值 时，激光入射到"X"型标记，测量得到"X"型标记的位置为（pXl'，pyi'），经计算简化得下 式：
[0080]
[0081] 已知（pxppyj和（pxj，pyj)，最终拟合得到x。，y。以及θ。
[0082] 上述机器人工件安装偏差计算系统应用于机器人工件偏差的矫正，在进行切割或 激光打标时能够更精准的定位工件的位置。
[0083] 以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限定，任 何熟悉本专业的技术人员可能利用上述技术内容作为启示加以变更或改型为等同变化的 等效实施例。但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例 所作出的简单修改，等同变化与改型，仍属于本发明权利要求的保护范围。
【主权项】
1. 一种机器人工装安装偏差计算系统，其特征在于它按照以下步骤顺序进行： 1) 建立激光切割硬件设施 机器人激光切割机包括底座（11)，串联在其上连续的N个机械臂（12)，机械臂自由末 端的抓取工装（13)以及激光器（14); 2) 基础变换矩阵的建立 建立激光坐标系（1)，该坐标系原点为激光在产品上的入射点，激光坐标系在机器人坐 标系⑵下的位置及姿态可以用一个4X4的旋转变换矩阵^^^表示：其中：为激光坐标系（1)在机器人坐标系（2)下的位置，其中，px，p y，？2为该位置 在X，y，z轴方向的分量；为激光坐标系（1)在机器人坐标系（2)下的姿态，其中，nx，n y，nz为该姿 态的X轴方向的X，y，z轴方向的分量；〇x，oy，Oz为该姿态的Y轴方向的X，y，z轴方向的分 量；ax，ay，az为该姿态的Z轴方向的X，y，z轴方向的分量； 建立产品坐标系（3)，其相对于机械臂自由末端的TCP坐标系（5)的位置及姿态可以用 一个4X4的旋转变换矩阵Qplate2rcp表示，其中，TCP即为机器人机械臂自由末端的位置；其中：为产品坐标系⑶在TCP坐标系（5)下的位置，其中，px，p y，pz为该位置在 X，y，z轴方向的分量；为产品坐标系（3)在TCP坐标系（5)下的姿态；其中，nx，n y，\为该姿态 的X轴方向的X，y，z轴方向的分量；〇x，oy，Oz为该姿态的X，y，z轴方向的分量；a x，ay，az为 该姿态的Z轴方向的x，y，z轴方向的分量； 3) 切割变换矩阵的建立 获得待加工产品的三维数据，并从中提取出产品坐标系（3)下的激光入射点Pp P2、 P3、……、Pn，以及对应的激光入射点法线方向N2、N3、……、义;根据激光入射点和激光 入射点法线方向建立局部坐标系（4)，以该局部坐标系的原点为激光入射点，坐标系相对于 产品坐标系（3)的位置及姿态可以用一个4X4的旋转变换矩阵Q niart2plate表示：其中：为每个入射点局部坐标系⑷在产品坐标系⑶下的位置，其中，Px，P y，Pz 为该位置在X，y，z轴方向的分量；为入射点局部坐标系（4)在产品坐标系（3)下的位姿态，其中，n x，ny，nz 为该姿态的X轴方向的X，y，z轴方向的分量；〇x，oy，oz为该姿态的Y轴方向的X，y，z轴方 向的分量；ax，ay，az为该姿态的Z轴方向的x，y，z轴方向的分量，并且所述的Z轴方向即为 入射点的法线方向； 4) 计算理想状态下多轴机器人角度 利用机器人运动学方程计算当激光垂直入射到"〇"型标记时多轴机器人的角度，根据 机器人的D - H参数，计算对应的机械臂可到达角度{qi」，其中，Ci1, ,表示产品的第i个激 光入射点位置对应的机器人第j个机械臂的角度； 机器人运动方程为：其中，j多1 ; 其中2^为机器第j各机械臂相对于第j 一 1个机械臂的旋转矩阵：其中，Θ j，a j，(Ij和a j为机器人D-H参数，Θ ;为机械臂之间的夹角，包含q j; α」为 机械臂扭转角；(!,为机械臂之间的距离；&]为机械臂长度；上述参数代入式II中即可计算每 个激光入射点对应的机器人机械臂角度值qu，i多2 ;j多1 ; 5) 工装安装偏差的计算 工装坐标系与机器人末端TCP坐标系并不重合，假设工装坐标系在机器人末端TCP坐 标系下的位置及姿态用一个4X4的旋转变换矩阵Qot表示：当Θ为小角度时，经计算，其中Χ(]，^为中心平移的位置，Θ为 中心旋转的角度；假设"〇"型标记的位置为（pXi，Pyi)，然后发现当机器人多轴角度设置为计算值时，激 光入射到" X "型标记，测量得到" X "型标记的位置为（pXl'，py/ )，经计算简化得下式：已知（Pxi, Pyi)和（px/，pyj )，最终拟合得到X。，y。以及Θ。2. 根据权利要求1所述的机器人工装安装偏差计算系统，其特征在于：所述的机械臂 个数N彡1。3. 根据权利要求2所述的机器人工装安装偏差计算系统，其特征在于：所述的N = 6。4. 一种如权利要求1 一 3中任一项所述的机器人工件安装偏差计算系统的应用，其特 征在于：所述的计算系统应用于机器人工件偏差的矫正。
【专利摘要】本发明公开了一种机器人工装安装偏差计算系统，它按照以下步骤顺序进行：1）建立激光切割硬件设施；2）基础变换矩阵的建立；3）切割变换矩阵的建立；4）计算理想状态下多轴机器人角度；5）工装安装偏差的计算；采用对机器人工件的安装进行偏差计算，及时的将位置有偏差的工件进行矫正。该计算系统应用于激光切割或激光打标过程中对于工件位置偏差的矫正，能够提高最终产品的合格率。本发明适用于机器人工件安装时偏差的计算，尤其适用于机器人激光切割或激光打标时工件的矫正。
【IPC分类】G06F17/50
【公开号】CN105426627
【申请号】CN201510907608
【发明人】姚峻峰, 查凯, 沈斌杰
【申请人】浙江隐齿丽医学技术有限公司
【公开日】2016年3月23日
【申请日】2015年12月9日