[发明专利]机器人工装安装偏差计算系统及其应用

权利要求书

1.一种机器人工装安装偏差计算系统，其特征在于它按照以下步骤顺序进行：

1)建立激光切割硬件设施

机器人激光切割机包括底座(11)，串联在其上连续的N个机械臂(12)，机械臂自由末端的抓取工装(13)以及激光器(14)；

2)基础变换矩阵的建立

建立激光坐标系(1)，该坐标系原点为激光在产品上的入射点，激光坐标系在机器人坐标系(2)下的位置及姿态可以用一个4×4的旋转变换矩阵Q_laser2base表示：

其中：

为激光坐标系(1)在机器人坐标系(2)下的位置，其中，p_x,p_y,p_z为该位置在x，y，z轴方向的分量；

为激光坐标系(1)在机器人坐标系(2)下的姿态，其中，n_x,n_y,n_z为该姿态的X轴方向的x，y，z轴方向的分量；o_x,o_y,o_z为该姿态的Y轴方向的x，y，z轴方向的分量；a_x,a_y,a_z为该姿态的Z轴方向的x，y，z轴方向的分量；

建立产品坐标系(3)，其相对于机械臂自由末端的TCP坐标系(5)的位置及姿态可以用一个4×4的旋转变换矩阵Q_plate2TCP表示，其中，TCP即为机器人机械臂自由末端的位置；

其中：

为产品坐标系(3)在TCP坐标系(5)下的位置，其中，p_x,p_y,p_z为该位置在x，y，z轴方向的分量；

为产品坐标系(3)在TCP坐标系(5)下的姿态；其中，n_x,n_y,n_z为该姿态的X轴方向的x，y，z轴方向的分量；o_x,o_y,o_z为该姿态的x，y，z轴方向的分量；a_x,a_y,a_z为该姿态的Z轴方向的x，y，z轴方向的分量；

3)切割变换矩阵的建立

获得待加工产品的三维数据，并从中提取出产品坐标系(3)下的激光入射点P1、P2、P3、……、Pn，以及对应的激光入射点法线方向N1、N2、N3、……、Nn；根据激光入射点和激光入射点法线方向建立局部坐标系(4)，以该局部坐标系的原点为激光入射点，坐标系相对于产品坐标系(3)的位置及姿态可以用一个4×4的旋转变换矩阵Q_mark2plate表示：

其中：

为每个入射点局部坐标系(4)在产品坐标系(3)下的位置，其中，p_x,p_y,p_z为该位置在x，y，z轴方向的分量；

为入射点局部坐标系(4)在产品坐标系(3)下的位姿态，其中，n_x,n_y,n_z为该姿态的X轴方向的x，y，z轴方向的分量；o_x,o_y,o_z为该姿态的Y轴方向的x，y，z轴方向的分量；a_x,a_y,a_z为该姿态的Z轴方向的x，y，z轴方向的分量，并且所述的Z轴方向即为入射点的法线方向；

4)计算理想状态下多轴机器人角度

利用机器人运动学方程计算当激光垂直入射到“O”型标记时多轴机器人的角度，根据机器人的D－H参数，计算对应的机械臂可到达角度{q_i,j}，其中，q_i,j表示产品的第i个激光入射点位置对应的机器人第j个机械臂的角度；

机器人运动方程为：

其中，j≥1；

其中为机器第j个机械臂相对于第j－1个机械臂的旋转矩阵：

其中，θ_j，α_j，d_j和a_j为机器人D-H参数，θ_j为机械臂之间的夹角，包含q_i,j；α_j为机械臂扭转角；d_j为机械臂之间的距离；a_j为机械臂长度；上述参数代入中即可计算每个激光入射点对应的机器人机械臂角度值q_i,j，i≥2；j≥1；5)工装安装偏差的计算

工装坐标系与机器人末端TCP坐标系并不重合，假设工装坐标系在机器人末端TCP坐标系下的位置及姿态用一个4×4的旋转变换矩阵Q_err表示：

当θ为小角度时，经计算，

其中x₀，y₀为中心平移的位置，θ为中心旋转的角度；

假设“O”型标记的位置为(p_xi，p_yi)，然后发现当机器人多轴角度设置为计算值时，激光入射到“×”型标记，测量得到“×”型标记的位置为(p_xi^,，p_yi^,)，经计算简化得下式：

已知(p_xi，p_yi)和(p_xi^,，p_yi^,)，最终拟合得到x₀，y₀以及θ。