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

摘要

本发明公开了一种机器人工装安装偏差计算系统，它按照以下步骤顺序进行：1）建立激光切割硬件设施；2）基础变换矩阵的建立；3）切割变换矩阵的建立；4）计算理想状态下多轴机器人角度；5）工装安装偏差的计算；采用对机器人工件的安装进行偏差计算，及时的将位置有偏差的工件进行矫正。该计算系统应用于激光切割或激光打标过程中对于工件位置偏差的矫正，能够提高最终产品的合格率。本发明适用于机器人工件安装时偏差的计算，尤其适用于机器人激光切割或激光打标时工件的矫正。

主权项

一种机器人工装安装偏差计算系统，其特征在于它按照以下步骤顺序进行：1)建立激光切割硬件设施机器人激光切割机包括底座(11)，串联在其上连续的N个机械臂(12)，机械臂自由末端的抓取工装(13)以及激光器(14)；2)基础变换矩阵的建立建立激光坐标系(1)，该坐标系原点为激光在产品上的入射点，激光坐标系在机器人坐标系(2)下的位置及姿态可以用一个4×4的旋转变换矩阵Q_laser2base表示：$Q_{laser2base}=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]$其中：$\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right]$为激光坐标系(1)在机器人坐标系(2)下的位置，其中，p_x,p_y,p_z为该位置在x，y，z轴方向的分量；$\left[\begin{array}{ccc}{n}_x& o_x& a_x\\ n_y& o_y& a_y\\ n_z& o_z& a_z\end{array}\right]$为激光坐标系(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即为机器人机械臂自由末端的位置；$Q_{plate2TCP}=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]$其中：$\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right]$为产品坐标系(3)在TCP坐标系(5)下的位置，其中，p_x,p_y,p_z为该位置在x，y，z轴方向的分量；$\left[\begin{array}{ccc}{n}_x& o_x& a_x\\ n_y& o_y& a_y\\ n_z& o_z& a_z\end{array}\right]$为产品坐标系(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)下的激光入射点P₁、P₂、P₃、……、P_n，以及对应的激光入射点法线方向N₁、N₂、N₃、……、N_n；根据激光入射点和激光入射点法线方向建立局部坐标系(4)，以该局部坐标系的原点为激光入射点，坐标系相对于产品坐标系(3)的位置及姿态可以用一个4×4的旋转变换矩阵Q_mark2plate表示：$Q_{mark2plate}=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]$其中：$\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right]$为每个入射点局部坐标系(4)在产品坐标系(3)下的位置，其中，p_x,p_y,p_z为该位置在x，y，z轴方向的分量；$\left[\begin{array}{ccc}{n}_x& o_x& a_x\\ n_y& o_y& a_y\\ n_z& o_z& a_z\end{array}\right]$为入射点局部坐标系(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个机械臂的角度；机器人运动方程为：$\begin{array}{c}{Q}_1^0\·Q_2^1\·Q_3^2......Q_j^{j-1}\·Q_{plate2TCP}\·Q_{mark2plate}=Q_{laser2base}\\ \⇒Q_1^0\·Q_2^1\·Q_3^2......Q_j^{j-1}=Q_{laser2base}\·inv\left(Q_{mark2plate}\right)\·inv\left(Q_{plate2TCP}\right)\end{array};$其中，j≥1；其中为机器第j各机械臂相对于第j－1个机械臂的旋转矩阵：$Q_j^{j-1}=\left[\begin{array}{cccc}{\mathrm{cos\θ}}_j& -{\mathrm{sin\θ}}_j\·{\mathrm{cos\α}}_j& {\mathrm{sin\θ}}_j\·{\mathrm{sin\α}}_j& a_j\·{\mathrm{cos\θ}}_j\\ {\mathrm{sin\θ}}_j& {\mathrm{cos\θ}}_j\·{\mathrm{cos\α}}_j& -{\mathrm{cos\θ}}_j\·{\mathrm{sin\α}}_j& a_j\·{\mathrm{sin\θ}}_j\\ 0& {\mathrm{sin\α}}_j& {\mathrm{cos\α}}_j& d_j\\ 0& 0& 0& 1\end{array}\right]$其中，θ_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表示：$Q_{err}=\left[\begin{array}{cccc}cos\mathrm{\θ}& -sin\mathrm{\θ}& 0& x_0\\ sin\mathrm{\θ}& \cos \mathrm{\θ}& 0& y_0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$当θ为小角度时，经计算，$Q_{err}=\left[\begin{array}{cccc}1& -\mathrm{\θ}& 0& x_0\\ \mathrm{\θ}& 1& 0& y_0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$其中x₀，y₀为中心平移的位置，θ为中心旋转的角度；$Q_1^0\·Q_2^1\·Q_3^2......Q_j^{j-1}\·Q_{err}\·Q_{plate2TCP}\·Q_{mark2plate}=Q_{laser2base}$假设“O”型标记的位置为(px_i，py_i)，然后发现当机器人多轴角度设置为计算值时，激光入射到“×”型标记，测量得到“×”型标记的位置为(px_i’，py_i’)，经计算简化得下式：$\left[\begin{array}{ccc}1& -\mathrm{\θ}& x_0\\ \mathrm{\θ}& 1& y_0\end{array}\right]\left[\begin{array}{c}{p}_{x_i}\\ p_{y_i}\\ 1\end{array}\right]=\left[\begin{array}{c}{p}_{{x_i}^{\′}}\\ p_{{y_i}^{\′}}\end{array}\right]$已知(px_i，py_i)和(px_i’，py_i’)，最终拟合得到x₀，y₀以及θ。