一种基于IEKF的工业机器人绝对精度校准方法与流程

技术特征：

1.一种基于迭代扩展卡尔曼滤波的工业机器人绝对精度校准方法，具体包括以下几个步骤：

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

第二步：建立机器人参数误差模型；

第三步：在机器人笛卡尔空间中进行数据采样；

第四步：利用矢量积法构建参数矢量，计算不同数据采样下的参数雅克比矩阵Jj，j＝1,2…k，k为数据采样个数；

第五步：利用迭代扩展卡尔曼滤波算法对参数误差模型中的参数误差进行辨识，通过迭代计算，当满足一定的条件时，得到辨识出的参数误差△s，包括以下步骤：

(1)，计算第j点的观测向量Zj：

Zj＝△Pj＝Pm,j-Pt,j j＝1,2…k (1)

其中，Zj为3×1的矩阵；Pm,j为机器人末端j点的实际坐标；Pt,j为机器人末端j点的理论坐标；△Pj为机器人末端j点实际j点与理论j点位置之差；

(2)，初始化参数误差

其中，为m×1的矩阵，m为待辨识的参数个数；i＝1，j＝1，i为迭代计数，i＝1,2…w，w为最大迭代次数；j为数据采样点，j＝1,2…k；

(3)，状态一步预测：

其中，为第i次迭代的采样点j-1的状态估计；为第i次迭代的采样点j的状态预测；

(4)，预测误差方差阵：

其中，为第i次迭代的采样点j-1的误差方差阵估计；为第i次迭代的采样点j的误差方差阵预测，为m×m的矩阵，m为待辨识的参数个数；Q为过程噪声W的方差强度阵，通过对过程信号进行数理统计得出，在工业机器人中，取经验值Q＝10^-4Im×m，m为待辨识的参数个数；

(5)，更新滤波增益矩阵：

其中，为第i次迭代的采样点j-1的滤波增益矩阵，为m×3的矩阵，m为待辨识的参数个数；R为观测噪声V的方差强度阵，依据激光跟踪系统的测量精度得到，为3×3的矩阵；

(6)，更新状态估计：

其中，为观测新息，为状态新息，为第i次迭代的采样点j的状态估计；

(7)，估计误差方差阵：

(8)，更新观测向量

其中，为理论修正位置，为观测向量；

(9)，更新迭代次数i或j，若所有采样数据都已使用，则i递增，j从k到1；否则，使用下一个采样数据；

重复步骤(3)至(9)，令若dmin>d，则dmin＝d，否则，其中dmin为最小平均误差，当连续h次迭代都没有进行更新，迭代停止，则认为参数误差值已经收敛稳定，此时得到最佳的参数误差△s，h取4即可；

第六步：辨识出的参数误差为△s，

对几何参数名义值进行修正：sg＝sn+△s，

其中，sn为机器人几何参数名义值，sg为机器人几何参数真实值。