CN110561424A - 基于多传感器混合滤波器的在线机器人运动学校准方法 - Google Patents

Abstract

本发明提供基于多传感器混合滤波器的在线机器人运动学校准方法，包括如下步骤：S1、用惯性传感器和位置传感器分别测量机器人末端效应器的朝向和位置；S2、使用卡尔曼滤波对机器人末端的姿态进行估计；S3、使用粒子滤波对机器人末端的位置进行估计；S4、利用混合传感器收集机器人的姿势，使用扩展卡尔曼滤波计算运动学参数的微分误差从而优化运动误差。本发明实现了在不停止机器人的情况下快速在线纠正机器人参数，从而大大提高了机器人的操作精度和效率。

Description

基于多传感器混合滤波器的在线机器人运动学校准方法

技术领域

本发明属于机器人运动领域，特别涉及基于多传感器混合滤波器的在线机器人运动学校准方法。

背景技术

根据生产过程中的固有公差，在机器人的运动学参数方面，测量值与实际值之间产生偏差。当使用标称运动学参数来估计机器人的位置和方向时，由于运动学误差将产生机器人末端执行器误差。为了实现校准任务，现在有各种各样的测量技术，如昂贵的定制功能，坐标测量机和激光跟踪干涉仪***。

由于传感器的发展，传统的离线机器人运动校准方法的过程转换为在线模式。传统的基于视觉的机器人校准技术，基于激光的机器人校准技术等，通过增加额外的功能实现位置和方向误差的在线测量。还有另一种更方便的方式。即安装非接触式位置和方位测量传感器，例如方位测量传感器(通常称为惯性测量单元，简称IMU)，位置传感器等。

但目前的校准方法仅仅是在原位校准，不从任意位置开始。

发明内容

本发明的目的在于克服现有技术存在的上述不足，提出了基于多传感器混合滤波器的在线机器人运动学校准方法，它允许各种机器人之间协作，这意味着机器人应该有能力在短时间内自动校正参数。

基于多传感器混合滤波器的在线机器人运动学校准方法，包括如下步骤：

S1、用惯性传感器和位置传感器分别测量机器人末端效应器的朝向和位置；

S2、使用卡尔曼滤波(Kalman filter，KF)对机器人末端执行器的位置进行估计；

S3、使用粒子滤波(particle filter，PF)对机器人末端执行器的姿态进行估计；

S4、使用扩展卡尔曼滤波(extended Kalman filter，EKF)计算DH运动学参数的微分误差来优化运动误差；所述DH运动学参数包括连杆长度、连杆扭角、连杆偏移和关节转角。

进一步的，所述步骤S1具体包括：

为了测量机器人末端绕x,y和z轴(基座空间直角坐标系)的旋转角，一个惯性测量单元被刚性固定在机器人末端执行器上，惯性测量单元包括一个磁力计、两个陀螺仪和一个加速度计；利用基于磁场和重力场的测量数据的因式四元数算法(FQA)来提高估计过程的准确性；测量得到的欧拉角别用来表示末端执行器的姿态，四元数[q₀,q₁,q₂,q₃]由欧拉角转化获得:

其中，φ，θ，ψ分别表示绕x,y和z轴的转角。

四元数满足如下关系：

其中，q₀为四元数标量，[q₁,q₂,q₃]为四元数矢量。

从惯性传感器坐标系到世界坐标系的方向余弦矩阵表示为：

进一步的，步骤S2，使用卡尔曼滤波对机器人末端执行器的位置进行估计具体如下：

采用如下的四元数q₀、q₁、q₂和q₃在时间t上的微分函数，减小磁力仪和陀螺仪的测量误差：

其中ξ_x、ξ_y和ξ_z分别代表惯性系中x_s、y_s和z_s轴的角速度分量；

定义状态转移矩阵：

其中_Δt是采样时间，状态x_KF,k由四元数状态和角速度组成，描述为x_KF,k＝[q_0,k q_1,kq_2,k q_3,k ξ_x,k ξ_y,k ξ_z,k]，其中q_0,k、q_1,k、q_2,k和q_3,k表示时间k处的四元数状态，ξ_x,k、ξ_y,k和ξ_z,k分别在表示在惯性系中x_s、y_s和z_s轴在时间k处的角速度；

定义过程噪声矢量：

w_k＝[0 0 0 0 w_x w_y w_z]^T，

其中w_x、w_y和w_z代表角速度的过程噪声，假定校准的陀螺仪检测到角速度，则观测矩阵H_k为H_k＝[0^n×p I^n×n]，其中n是角速度矢量的数量，p是四元数的维数，观测矩阵在时间k处确定的四元数q_k的归一化形式是：

q_k＝[q_0,k/M q_1,k/M q_2,k/M q_3,k/M]

进一步的，步骤S3，使用粒子滤波对机器人末端执行器的姿态进行估计具体如下：

每个采样点的位置和加速度通过以下等式的粒子滤波估算：

其中代表第i个粒子的速度、A代表IMU的加速度、g代表局部重力矢量，代表末端位置，Cⁱ代表从惯性测量传感器坐标系到世界坐标系的旋转矩阵。

机器人末端的位置状态被定义为x_PF＝[p_x,p_y,p_z,a_x,a_y,a_z]，p_x,p_y,p_z和a_x,a_y,a_z分别代表在x,y，z轴上的位置和加速度；为了获得更准确的权重，采用时间段ΔT_s中的位置差的总和，而不是时刻k处的瞬时位置差，其中s表示迭代次数；将第i个粒子的估计值和计算值的累积位置差用于似然计算，如下：

M_s＝ΔT_s/t

M_s表示在ΔT_s内取的每个粒子计算累计位置差的时间间隔，t表示间隔数，其中分别代表在时刻k时第i个粒子在x,y和z轴上的位置状态，分别代表在时刻k时由卡尔曼滤波估计的第i个粒子在x,y和z轴上的位置状态。近似后验表示为：

其中，x_0:s表示当前粒子位置、表示到迭代次数为s时第i个粒子的位置、代表到迭代次数为s的累积位置差值；N是粒子滤波算法中的粒子的数目，是第i个粒子在时刻k处的归一化重量，δ(·)是狄拉克δ函数；后验概率满足以下等式：

(p(x_s|x_s-1)表示前验概率，表示的在迭代次数为s-1处的后验概率，表示在x_s的条件下的概率、代表求得的累计位置误差、x_s表示在迭代次数为s处的粒子位置；

第i个粒子在迭代次数为s处的归一化权重是：

其中r(·)是重要性密度；表示第i个粒子在迭代次数为s处的位置状态；通过重新采样和从 p(x_s|x_s-1)中获取重要密度，通过以下方式获得归一化重量：

其中，表示在的条件下的概率、表示第i个粒子在迭代次数为s 处的实际位置，

位置粒子的权重被定义为最可能的值；最小的累积误差被认为是最可能的位置值；因此，归一化的权重通过以下公式计算：

其中为对求标准偏差运算。

进一步的，所述步骤S4具体如下：在利用位置传感器和惯性测量单元收集机器人的位置和姿态信息的过程中，由于传感器的固有噪声，测量误差随着时间的推移而增加，扩展卡尔曼滤波被应用来优化运动误差(下面公式计算就是优化的过程)。

如果考虑N个旋转关节的4N个DH运动学参数从IMU传感器到机器人末端的位姿转换参数有4个，所述位姿转换参数为连杆长度、连杆扭角、连杆偏移和关节转角，则总参数的数量为4N+4；因此，在基于拓展卡尔曼滤波(EKF)的位姿估计过程中，估计位姿基于4(N+1)个D-H参数计算；估计状态的模型如下：

P_k+1|k＝P_k|k+Q_k

其中和P_k|k分别代表估计的位置状态和协方差矩阵，Q_k表示k时刻***噪声的协方差矩阵；雅可比矩阵J_k+1、测量误差冗余以及冗余协方差矩阵S_k+1由下式得到：

其中m_k和R_k分别代表测量的姿态值和在k时刻测量噪声的协方差矩阵，T代表关节间的转换矩阵；即前面求出的k+1时刻的估计位姿；

因此，拓展卡尔曼滤波的滤波增益K_k+1，状态测量值和状态误差协方差矩阵P_k+1更新为：

P_k+1|k+1＝(I-K_k+1J_k+1)P_k+1|k

其中I代表单位矩阵。

本发明相对于现有技术具有如下的优点及效果：

1.所提出的在线方法应用卡尔曼滤波对机器人末端姿态进行估计，用粒子滤波对机器人末端的位置进行估计，最后通过拓展卡尔曼滤波得到运动学参数误差，该方法具有较高精度和效率。

2.该方法采用惯性传感器和位置传感器，快速准确地校准误差。

3.与目前的校准方法相比，本发明不仅仅是在原位校准的，这意味着机器人从任意位置开始。此外，机器人不需要做一些特定的运动来离线测量机器人信息，这使得它更方便和有效，更重要的是，还具有更高的容错能力，使其更易于使用。

附图说明

图1是实施例的基于多传感器混合滤波器的在线机器人运动学校准方法的流程图。

具体实施方式

以下结合实施例对本发明作进一步详细的描述，但本发明的实施方式不限于此，以下若有未特别详细说明之处，均是本领域技术人员可参照现有技术实现或理解的。

如图1所示的一种基于多传感器混合滤波器的在线机器人运动学校准方法，包括如下步骤：

S1、用惯性传感器和位置传感器测量机器人末端效应器的朝向和位置

为了测量机器人末端绕x,y和z轴的旋转角，一个惯性测量单元被刚性固定在机器人末端执行器上，，惯性测量单元包括一个磁力计、两个陀螺仪和一个加速度计；利用基于磁场和重力场的测量数据的因式四元数算法(FQA)来提高估计过程的准确性。测量得到的欧拉角别用来表示末端执行器的姿态，四元数[q₀,q₁,q₂,q₃]由欧拉角转化获得:

其中，φ，θ，ψ分别表示绕x,y和z轴的转角。

四元数满足如下关系：

其中，q₀为四元数标量，[q₁,q₂,q₃]为四元数矢量。

从惯性传感器坐标系到世界坐标系的方向余弦矩阵表示为：

S2、使用卡尔曼滤波对机器人末端的姿态进行估计

采用如下的四元数q₀、q₁、q₂和q₃在时间t上的微分函数，减小磁力仪和陀螺仪的测量误差：

其中ξ_x、ξ_y和ξ_z分别代表惯性系中x_s、y_s和z_s轴的角速度分量；

定义状态转移矩阵：

其中_Δt是采样时间，状态x_KF,k由四元数状态和角速度组成，描述为 x_KF,k＝[q_0,kq_1,k q_2,k q_3,k ξ_x,k ξ_y,k ξ_z,k]，其中q_0,k、q_1,k、q_2,k和q_3,k表示时间k处的四元数状态，ξ_x,k、ξ_y,k和ξ_z,k分别在表示在惯性系中x_s、y_s和z_s轴在时间k处的角速度；

定义过程噪声矢量：

w_k＝[0 0 0 0 w_x w_y w_z]^T，

其中w_x、w_y和w_z代表角速度的过程噪声，假定校准的陀螺仪检测到角速度，则观测矩阵H_k为H_k＝[0^n×p I^n×n]，其中n是角速度矢量的数量，p是四元数的维数，观测矩阵在时间k处确定的四元数q_k的归一化形式是：

q_k＝[q_0,k/M q_1,k/M q_2,k/M q_3,k/M]

S3、使用粒子滤波对机器人末端的位置进行估计

每个采样点的位置和加速度通过以下等式的粒子滤波估算：

其中代表第i个粒子的速度、A代表IMU的加速度、g代表局部重力矢量，、代表末端位置。Cⁱ代表从惯性测量传感器坐标系到世界坐标系的旋转矩阵。

机器人末端的位置状态被定义为x_PF＝[p_x,p_y,p_z,a_x,a_y,a_z]，p_x,p_y,p_z和a_x,a_y,a_z分别代表在 x,y，z轴上的位置和加速度；为了获得更准确的权重，采用时间段ΔT_s中的位置差的总和，而不是时刻k处的瞬时位置差，其中s表示迭代次数；将第i个粒子的估计值和计算值的累积位置差用于似然计算，如下：

M_s＝ΔT_s/t

M_s表示在ΔT_s内取的每个粒子计算累计位置差的时间间隔，t表示间隔数，

其中分别代表在时刻k时第i个粒子在x,y和z轴上的位置状态，分别代表在时刻k时由卡尔曼滤波估计的第i个粒子在x,y和z轴上的位置状态。近似后验表示为：

其中，x_0:s表示当前粒子位置、表示到迭代次数为s时第i个粒子的位置、代表到迭代次数为s的累积位置差值；N是粒子滤波算法中的粒子的数目，是第i个粒子在时刻k处的归一化重量，δ(·)是狄拉克δ函数；后验概率满足以下等式：

(p(x_s|x_s-1)表示前验概率，表示的在迭代次数为s-1处的后验概率，表示在x_s的条件下的概率、代表求得的累计位置误差、x_s表示在迭代次数为s处的粒子位置；

第i个粒子在迭代次数为s处的归一化权重是：

其中r(·)是重要性密度；表示第i个粒子在迭代次数为s处的位置状态；通过重新采样和从p(x_s|x_s-1)中获取重要密度，通过以下方式获得归一化重量：

其中，表示在的条件下的概率、表示第i个粒子在迭代次数为s 处的实际位置，

位置粒子的权重被定义为最可能的值；最小的累积误差被认为是最可能的位置值；因此，归一化的权重通过以下公式计算：

其中为对求标准偏差运算。

S4、使用扩展卡尔曼滤波(extended Kalman filter，EKF)计算DH运动学参数(DH参数：连杆长度，连杆扭角，连杆偏移和关节转角)，从而优化运动误差。图1中的动力学建模是DH运动学建模，是一个通用的机器人运动学建模，指的是建立起变量间关系的数学公式模型.

在利用位置传感器和惯性测量单元收集机器人的位置和姿态信息的过程中，由于传感器的固有噪声，测量误差随着时间的推移而增加，扩展卡尔曼滤波被应用来优化运动误差(下面公式计算就是优化的过程)。

如果考虑N个旋转关节的4N个DH运动学参数从IMU传感器到机器人末端的位姿转换参数有4个，所述位姿转换参数为连杆长度、连杆扭角、连杆偏移和关节转角，则总参数的数量为4N+4；因此，在基于拓展卡尔曼滤波(EKF)的位姿估计过程中，估计位姿基于4(N+1)个D-H参数计算；估计状态的模型如下：

P_k+1|k＝P_k|k+Q_k

其中和P_k|k分别代表估计的位置状态和协方差矩阵，Q_k表示k时刻***噪声的协方差矩阵；雅可比矩阵J_k+1、测量误差冗余以及冗余协方差矩阵S_k+1由下式得到：

其中m_k和R_k分别代表测量的姿态值和在k时刻测量噪声的协方差矩阵，T代表关节间的转换矩阵；即前面求出的k+1时刻的估计位姿；

因此，拓展卡尔曼滤波的滤波增益K_k+1，状态测量值和状态误差协方差矩阵P_k+1更新为：

P_k+1|k+1＝(I-K_k+1J_k+1)P_k+1|k

其中I代表单位矩阵。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (5)

1.基于多传感器混合滤波器的在线机器人运动学校准方法，其特征在于，包括如下步骤：

S1、用惯性传感器和位置传感器分别测量机器人末端效应器的朝向和位置；

S2、使用卡尔曼滤波对机器人末端执行器的位置进行估计；

S3、使用粒子滤波对机器人末端执行器的姿态进行估计；

S4、使用扩展卡尔曼滤波计算DH运动学参数的微分误差来优化运动误差；所述DH运动学参数包括连杆长度、连杆扭角、连杆偏移和关节转角。

2.根据权利要求1所述的基于多传感器混合滤波器的在线机器人运动学校准方法，其特征在于，所述步骤S1具体包括：

为了测量机器人末端绕x,y和z轴的旋转角，一个惯性测量单元被刚性固定在机器人末端执行器上，惯性测量单元包括一个磁力计、两个陀螺仪和一个加速度计；利用基于磁场和重力场的测量数据的因式四元数算法(FQA)来提高估计过程的准确性；测量得到的欧拉角别用来表示末端执行器的姿态，四元数[q₀,q₁,q₂,q₃]由欧拉角转化获得:

其中，φ，θ，ψ分别表示绕x,y和z轴的转角；

四元数满足如下关系：

其中，q₀为四元数标量，[q₁,q₂,q₃]为四元数矢量；

从惯性传感器坐标系到世界坐标系的方向余弦矩阵表示为：

3.根据权利要求1所述的基于多传感器混合滤波器的在线机器人运动学校准方法，其特征在于，步骤S2，使用卡尔曼滤波对机器人末端执行器的位置进行估计具体如下：

采用如下的四元数q₀、q₁、q₂和q₃在时间t上的微分函数，减小磁力仪和陀螺仪的测量误差：

其中ξ_x、ξ_y和ξ_z分别代表惯性系中x_s、y_s和z_s轴的角速度分量；

定义状态转移矩阵：

其中_Δt是采样时间，状态x_KF,k由四元数状态和角速度组成，描述为x_KF,k＝[q_0,k q_1,k q_2,kq_3,k ξ_x,k ξ_y,k ξ_z,k]，其中q_0,k、q_1,k、q_2,k和q_3,k表示时间k处的四元数状态，ξ_x,k、ξ_y,k和ξ_z,k分别在表示在惯性系中x_s、y_s和z_s轴在时间k处的角速度；

定义过程噪声矢量：

w_k＝[0 0 0 0 w_x w_y w_z]^T，

其中w_x、w_y和w_z代表角速度的过程噪声，假定校准的陀螺仪检测到角速度，则观测矩阵H_k为H_k＝[0^n×p I^n×n]，0^n×p代表n行p列的零矩阵，I^n×n代表n行n列的单位矩阵，其中n是角速度矢量的数量，p是四元数的维数，观测矩阵在时间k处确定的四元数q_k的归一化形式是：

q_k＝[q_0,k/M q_1,k/M q_2,k/M q_3,k/M]

4.根据权利要求1所述的基于多传感器混合滤波器的在线机器人运动学校准方法，其特征在于，步骤S3，使用粒子滤波对机器人末端执行器的姿态进行估计具体如下：

每个采样点的位置和加速度通过以下等式的粒子滤波估算：

其中代表第i个粒子的速度、A代表IMU的加速度、g代表局部重力矢量，代表末端位置，Cⁱ代表从惯性测量传感器坐标系到世界坐标系的旋转矩阵；

机器人末端的位置状态被定义为x_PF＝[p_x,p_y,p_z,a_x,a_y,a_z]，p_x,p_y,p_z和a_x,a_y,a_z分别代表在x,y，z轴上的位置和加速度；为了获得更准确的权重，采用时间段ΔT_s中的位置差的总和，而不是时刻k处的瞬时位置差，其中s表示迭代次数；将第i个粒子的估计值和计算值的累积位置差用于似然计算，如下：

M_s＝ΔT_s/t

M_s表示在ΔT_s内取的每个粒子计算累计位置差的时间间隔，t表示间隔数，

其中分别代表在时刻k时第i个粒子在x,y和z轴上的位置状态，分别代表在时刻k时由卡尔曼滤波估计的第i个粒子在x,y和z轴上的位置状态；近似后验表示为：

其中，x_0:s表示当前粒子位置、表示到迭代次数为s时第i个粒子的位置、代表到迭代次数为s的累积位置差值；N是粒子滤波算法中的粒子的数目，是第i个粒子在时刻k处的归一化重量，δ(·)是狄拉克δ函数；后验概率满足以下等式：

(p(x_s|x_s-1)表示前验概率，表示的在迭代次数为s-1处的后验概率，表示在x_s的条件下的概率、代表求得的累计位置误差、x_s表示在迭代次数为s处的粒子位置；

第i个粒子在迭代次数为s处的归一化权重是：

其中r(·)是重要性密度；表示第i个粒子在迭代次数为s处的位置状态；通过重新采样和从p(x_s|x_s-1)中获取重要密度，通过以下方式获得归一化重量：

其中，表示在的条件下的概率、表示第i个粒子在迭代次数为s处的实际位置，

位置粒子的权重被定义为最可能的值；最小的累积误差被认为是最可能的位置值；因此，归一化的权重通过以下公式计算：

其中为对求标准偏差运算。

5.根据权利要求1所述的基于多传感器混合滤波器的在线机器人运动学校准方法，其特征在于，步骤S4，优化运动误差具体包括：

在利用位置传感器和惯性测量单元收集机器人的位置和姿态信息的过程中，由于传感器的固有噪声，测量误差随着时间的推移而增加，扩展卡尔曼滤波被应用来优化运动误差；

如果考虑N个旋转关节的4N个DH运动学参数从IMU传感器到机器人末端的位姿转换参数有4个，所述位姿转换参数为连杆长度、连杆扭角、连杆偏移和关节转角，则总参数的数量为4N+4；因此，在基于拓展卡尔曼滤波(EKF)的位姿估计过程中，估计位姿基于4(N+1)个D-H参数计算；估计状态的模型如下：

P_k+1|k＝P_k|k+Q_k

其中和P_k|k分别代表估计的位置状态和协方差矩阵，Q_k表示k时刻***噪声的协方差矩阵；雅可比矩阵J_k+1、测量误差冗余以及冗余协方差矩阵S_k+1由下式得到：

其中m_k和R_k分别代表测量的姿态值和在k时刻测量噪声的协方差矩阵，T代表关节间的转换矩阵；即前面求出的k+1时刻的估计位姿；

因此，拓展卡尔曼滤波的滤波增益K_k+1，状态测量值和状态误差协方差矩阵P_k+1更新为：

P_k+1|k+1＝(I-K_k+1J_k+1)P_k+1|k

其中I代表单位矩阵。