[发明专利]一种基于PMPSD的工业机器人绝对精度校准方法

权利要求书

1.一种基于PMPSD的工业机器人绝对精度校准方法，具体包括以下几个步骤：

第一步：建立机器人误差运动学模型；

第二步：建立空间多点虚拟约束，使用PSD装置进行数据采样；

第三步：利用空间矢量关系修正机器人末端激光器的位姿，并利用修正后的位姿对机器人关节转角进行修正，以取代从机器人示教器或从控制器得到的关节转角；

包括以下步骤：

步骤(1)，利用示教器控制方式，将激光束投射到PSD中心，记录此时的关节转角θ₁，然后改变机器人位姿，重新将激光束投射到PSD中心，记录此时的关节转角θ₂，这时便可得到两组激光束方程，可求得PSD中心点相对于机器人基坐标系的坐标为P_f(p_fx，p_fy，p_fz)，由于PSD为位置传感器，因此，当激光束再投射到PSD表面时，此时的斑点位置为P_s(p_sx，p_sy，p_sz)；

步骤(2)，设三维空间中任意一条激光束的直线方程表示为L＝(p_x，p_y，p_z，α，β，γ)，其中P_t(p_x，p_y，p_z)为激光器相对于机器人基坐标系的坐标，为在基坐标系下的激光束方向矢量；由于激光器以不同的位姿将激光束投射到PSD的表面，因此可以得到多组机器人关节转角，也就可以获得多组激光束在机器人基坐标系下的直线方程；PSD表面中心点坐标为P_c(p_cx，p_cy，p_cz)，激光束投射在PSD上的斑点位置为P_s(p_sx，p_sy，p_sz)，可得投射斑点到PSD中心点的方向矢量为

步骤(3)，已知激光束方向矢量及投射斑点至PSD中心点的方向矢量因此可以得到虚拟激光束方向矢量为

步骤(4)，利用已知激光器坐标P_t(p_x，p_y，p_z)及得到的虚拟激光束方向矢量再运用机器人运动学逆解求得多组关节转角值，并选取其中完全平方差之和最小的一组关节转角θ′，将关节转角θ′取代之前从机器人示教器或控制器取得的关节转角值θ；

第四步：构建模型约束目标函数；

第五步：利用最小化方法对模型约束目标函数进行优化，得到工业机器人的参数误差；使用最小化方法LM(Levenberg-Marquardt)算法不断进行迭代使得模型约束目标函数最小化，此时工业机器人的参数误差Δs将会被辨识出来，Δs＝[Δa₁ Δd₁ Δα₁ Δθ₁ … Δa_nΔd_n Δα_n Δθ_n]^T，Δs为m×1的矩阵，m为待辨识的参数个数，n为机器人关节数，具体步骤如下：

步骤(1)，初始化参数误差Δs，LM算法的初始化相关参数，迭代次数k；

Δs₀＝0，μ＝0.1，v＝2，k＝1 (2)

其中μ为阻尼因子，v为增长因子

步骤(2)，计算第k次迭代时的雅可比矩阵J(Δs_k)；

雅克比矩阵J(Δs_k)由Ω对Δs分量求解偏微分得到，其中Ω＝[Ω₁，Ω₂，…Ω_k×m]，其中，k为同一PSD装置位置处的数据采样次数，(ⁱx_j，ⁱy_j，ⁱz_j)为在位置i处时的第i个交点或者公垂线中点，为PSD装置在位置m处时的交点或者公垂线中点的平均值，m表示PSD装置放置的不同位置的个数；

步骤(3)，利用LM算法求解参数误差矩阵h_lm；

h_lm＝-(J(Δs_k)^TJ(Δs_k)+μ×I_m×m)^-1J(Δs_k)^TΩ (3)

其中h_lm表示为第k次迭代时参数误差的改变值，为m×1矩阵，Δs_k为第k次迭代时的参数误差，为m×1矩阵，μ为阻尼因子，m为待辨识的参数个数；

步骤(4)，计算第k次迭代时实际下降量^AD_k与预估下降量^PD_k的比值ρ；

^AD_k＝F(Δs_k)-F(Δs_k+h_lm) (4)

步骤(5)，更新迭代参数；

若ρ＞0：

Δs_k＝Δs_k+h_lm (7)

v＝2 (9)

否则：

μ＝μ×v (10)

v＝2×v (11)

重复步骤(2)至(5)，当||J(Δs_k)^TΩ||＜ε时，循环结束，得到最终的机器人参数误差Δs＝Δs_k，否则k＝k+1 ；

第六步：将参数误差对几何参数名义值进行修正，实现机器人的绝对精度校准，包括以下步骤：

辨识出的参数误差为Δs，s_g＝s_n+Δs，s_n为机器人几何参数名义值，s_g为机器人几何参数真实值。