WO2021031346A1 - 基于多传感器混合滤波器的在线机器人运动学校准方法 - Google Patents
基于多传感器混合滤波器的在线机器人运动学校准方法 Download PDFInfo
- Publication number
- WO2021031346A1 WO2021031346A1 PCT/CN2019/114533 CN2019114533W WO2021031346A1 WO 2021031346 A1 WO2021031346 A1 WO 2021031346A1 CN 2019114533 W CN2019114533 W CN 2019114533W WO 2021031346 A1 WO2021031346 A1 WO 2021031346A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- robot
- particle
- time
- matrix
- quaternion
- Prior art date
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1679—Programme controls characterised by the tasks executed
- B25J9/1692—Calibration of manipulator
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
Definitions
- the invention belongs to the field of robot motion, and particularly relates to an online robot kinematics calibration method based on a multi-sensor hybrid filter.
- the current calibration method only calibrates in-situ and does not start from any position.
- the purpose of the present invention is to overcome the above-mentioned shortcomings of the prior art, and proposes an online robot kinematics calibration method based on a multi-sensor hybrid filter, which allows cooperation between various robots, which means that the robot should be capable of in a short time Automatically correct parameters.
- the online robot kinematics calibration method based on the multi-sensor hybrid filter includes the following steps:
- KF Kalman filter
- EKF extended Kalman filter
- step S1 specifically includes:
- an inertial measurement unit is rigidly fixed to the end effector of the robot.
- the inertial measurement unit includes a magnetometer, two gyroscopes and An accelerometer; the factored quaternion algorithm (FQA) based on the measurement data of the magnetic field and the gravitational field is used to improve the accuracy of the estimation process; the measured Euler angle is used to indicate the attitude of the end effector, quaternion [q 0 ,q 1 ,q 2 ,q 3 ] can be obtained from Euler angle transformation:
- ⁇ , ⁇ , ⁇ represent the rotation angles around the x, y and z axes, respectively.
- q 0 is a quaternion scalar
- [q 1 , q 2 , q 3 ] is a quaternion vector
- the direction cosine matrix from the inertial sensor coordinate system to the world coordinate system is expressed as:
- step S2 using Kalman filter to estimate the position of the robot end effector is specifically as follows:
- ⁇ x , ⁇ y and ⁇ z represent the angular velocity components of the x s , y s and z s axes in the inertial system respectively;
- w x , w y and w z represent the process noise of the angular velocity.
- step S3 using particle filtering to estimate the pose of the robot end effector is specifically as follows:
- A represents the acceleration of the IMU
- g represents the local gravity vector
- C i represents the rotation matrix from the inertial measurement sensor coordinate system to the world coordinate system.
- the cumulative position difference between the estimated value and the calculated value of i particles is used for likelihood calculation, as follows:
- M s represents the time interval for calculating the cumulative position difference of each particle taken within ⁇ T s
- t represents the number of intervals
- x 0: s represents the current particle position, Represents the position of the i-th particle when the number of iterations is s, Represents the cumulative position difference up to the number of iterations s; N is the number of particles in the particle filter algorithm, Is the normalized weight of the i-th particle at time k, and ⁇ ( ⁇ ) is the Dirac ⁇ function; the posterior probability satisfies the following equation:
- x s-1 ) represents the prior probability, Represents the posterior probability at the number of iterations s-1, Means under the condition of x s The probability, Represents the calculated cumulative position error, x s represents the particle position at the iteration number s;
- r( ⁇ ) is the importance density
- the weight of the position particle is defined as The most likely value; the smallest cumulative error It is considered the most probable position value; therefore, the normalized weight is calculated by the following formula:
- step S4 is specifically as follows: in the process of collecting the position and posture information of the robot by using the position sensor and the inertial measurement unit, due to the inherent noise of the sensor, the measurement error increases with the passage of time, and the extended Kalman filter is Application to optimize the motion error (the following formula calculation is the optimization process).
- the pose conversion parameters are link length, link torsion angle, link offset, and joint Turning angle, the number of total parameters is 4N+4; therefore, in the process of pose estimation based on extended Kalman filter (EKF), the estimated pose is calculated based on 4(N+1) DH parameters; the model of the estimated state is as follows :
- k represent the estimated position state and covariance matrix respectively
- Q k represents the covariance matrix of system noise at time k
- Jacobian matrix J k+1 measurement error redundancy
- the redundant covariance matrix S k+1 is obtained by the following formula:
- m k and R k respectively represent the measured attitude value and the covariance matrix of the noise measured at time k, and T represents the conversion matrix between joints; That is, the estimated pose at k+1 obtained above;
- I represents the identity matrix
- the present invention has the following advantages and effects:
- the proposed online method applies Kalman filter to estimate the robot end pose, uses particle filter to estimate the position of the robot end, and finally obtains the kinematic parameter error by extending the Kalman filter. This method has high accuracy and efficiency.
- This method uses inertial sensors and position sensors to quickly and accurately calibrate errors.
- the present invention is not only calibrated in situ, which means that the robot starts from any position.
- the robot does not need to do some specific movements to measure robot information offline, which makes it more convenient and effective, and more importantly, it has higher fault tolerance, making it easier to use.
- Fig. 1 is a flowchart of an embodiment of an online robot kinematics calibration method based on a multi-sensor hybrid filter.
- an online robot kinematics calibration method based on a multi-sensor hybrid filter includes the following steps:
- an inertial measurement unit is rigidly fixed to the end effector of the robot.
- the inertial measurement unit includes a magnetometer, two gyroscopes and An accelerometer; the factored quaternion algorithm (FQA) based on the measurement data of the magnetic field and the gravitational field is used to improve the accuracy of the estimation process; the measured Euler angle is used to indicate the attitude of the end effector, quaternion [q 0 ,q 1 ,q 2 ,q 3 ] can be obtained from Euler angle transformation:
- ⁇ , ⁇ , ⁇ represent the rotation angles around the x, y and z axes, respectively.
- q 0 is a quaternion scalar
- [q 1 , q 2 , q 3 ] is a quaternion vector
- the direction cosine matrix from the inertial sensor coordinate system to the world coordinate system is expressed as:
- ⁇ x , ⁇ y and ⁇ z represent the angular velocity components of the x s , y s and z s axes in the inertial system respectively;
- w x , w y and w z represent the process noise of the angular velocity.
- A represents the acceleration of the IMU
- g represents the local gravity vector
- C i represents the rotation matrix from the inertial measurement sensor coordinate system to the world coordinate system.
- the cumulative position difference between the estimated value and the calculated value of i particles is used for likelihood calculation, as follows:
- M s represents the time interval for calculating the cumulative position difference of each particle taken within ⁇ T s
- t represents the number of intervals
- x 0: s represents the current particle position, Represents the position of the i-th particle when the number of iterations is s, Represents the cumulative position difference up to the number of iterations s; N is the number of particles in the particle filter algorithm, Is the normalized weight of the i-th particle at time k, and ⁇ ( ⁇ ) is the Dirac ⁇ function; the posterior probability satisfies the following equation:
- x s-1 ) represents the prior probability, Represents the posterior probability at the number of iterations s-1, Means under the condition of x s The probability, Represents the calculated cumulative position error, x s represents the particle position at the iteration number s;
- r( ⁇ ) is the importance density
- the weight of the position particle is defined as The most likely value; the smallest cumulative error It is considered the most probable position value; therefore, the normalized weight is calculated by the following formula:
- DH kinematics modeling is a general robot kinematics modeling, which refers to the mathematical formula model that establishes the relationship between variables.
- the pose conversion parameters are link length, link torsion angle, link offset, and joint Turning angle, the number of total parameters is 4N+4; therefore, in the process of pose estimation based on extended Kalman filter (EKF), the estimated pose is calculated based on 4(N+1) DH parameters; the model of the estimated state is as follows :
- k represent the estimated position state and covariance matrix respectively
- Q k represents the covariance matrix of system noise at time k
- Jacobian matrix J k+1 measurement error redundancy
- the redundant covariance matrix S k+1 is obtained by the following formula:
- m k and R k respectively represent the measured attitude value and the covariance matrix of the noise measured at time k, and T represents the conversion matrix between joints; That is, the estimated pose at k+1 obtained above;
- I represents the identity matrix
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
- Manipulator (AREA)
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
0,q
1,q
2,q
3]由欧拉角转化获得:
其中，φ，θ，ψ分别表示绕x,y和z轴的转角。
四元数满足如下关系：
其中，q
0为四元数标量，[q
1,q
2,q
3]为四元数矢量。
从惯性传感器坐标系到世界坐标系的方向余弦矩阵表示为：
进一步的，步骤S2，使用卡尔曼滤波对机器人末端执行器的位置进行估计具体如下：
采用如下的四元数q
0、q
1、q
2和q
3在时间t上的微分函数，减小磁力仪和陀螺仪的测量误差：
其中ξ
x、ξ
y和ξ
z分别代表惯性系中x
s、y
s和z
s轴的角速度分量；
定义状态转移矩阵：
其中Δt是采样时间，状态x
K,F由四元数状态和角速度组成，描述为 x
KF,k＝[q
0,k q
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
i代表从惯性测量传感器坐标系到世界坐标系的旋转矩阵。
机器人末端的位置状态被定义为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
0,q
1,q
2,q
3]由欧拉角转化获得:
其中，φ，θ，ψ分别表示绕x,y和z轴的转角。
四元数满足如下关系：
其中，q
0为四元数标量，[q
1,q
2,q
3]为四元数矢量。
从惯性传感器坐标系到世界坐标系的方向余弦矩阵表示为：
S2、使用卡尔曼滤波对机器人末端的姿态进行估计
采用如下的四元数q
0、q
1、q
2和q
3在时间t上的微分函数，减小磁力仪和陀螺仪的测量误差：
其中ξ
x、ξ
y和ξ
z分别代表惯性系中x
s、y
s和z
s轴的角速度分量；
定义状态转移矩阵：
其中Δt是采样时间，状态x
K,F由四元数状态和角速度组成，描述为 x
KF,k＝[q
0,k q
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
i代表从惯性测量传感器坐标系到世界坐标系的旋转矩阵。
机器人末端的位置状态被定义为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、使用扩展卡尔曼滤波（extendedKalmanfilter,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)
- 基于多传感器混合滤波器的在线机器人运动学校准方法，其特征在于，包括如下步骤：S1、用惯性传感器和位置传感器分别测量机器人末端效应器的朝向和位置；S2、使用卡尔曼滤波对机器人末端执行器的位置进行估计；S3、使用粒子滤波对机器人末端执行器的姿态进行估计；S4、使用扩展卡尔曼滤波计算DH运动学参数的微分误差来优化运动误差；所述DH运动学参数包括连杆长度、连杆扭角、连杆偏移和关节转角。
- 根据权利要求1所述的基于多传感器混合滤波器的在线机器人运动学校准方法，其特征在于，所述步骤S1具体包括：为了测量机器人末端绕x,y和z轴的旋转角，一个惯性测量单元被刚性固定在机器人末端执行器上，惯性测量单元包括一个磁力计、两个陀螺仪和一个加速度计；利用基于磁场和重力场的测量数据的因式四元数算法(FQA)来提高估计过程的准确性；测量得到的欧拉角别用来表示末端执行器的姿态，四元数[q 0,q 1,q 2,q 3]由欧拉角转化获得:
其中，φ，θ，ψ分别表示绕x,y和z轴的转角；四元数满足如下关系：
其中，q 0为四元数标量，[q 1,q 2,q 3]为四元数矢量；从惯性传感器坐标系到世界坐标系的方向余弦矩阵表示为：
- 根据权利要求1所述的基于多传感器混合滤波器的在线机器人运动学校准方法，其特征在于，步骤S2，使用卡尔曼滤波对机器人末端执行器的位置进行估计具体如下：采用如下的四元数q 0、q 1、q 2和q 3在时间t上的微分函数，减小磁力仪和陀螺仪的测量误差：
其中ξ x、ξ y和ξ z分别代表惯性系中x s、y s和z s轴的角速度分量；定义状态转移矩阵：
其中Δt是采样时间，状态由四元数状态和角速度组成，描述为
其中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]
- 根据权利要求1所述的基于多传感器混合滤波器的在线机器人运动学校准方法，其特征在于，步骤S3，使用粒子滤波对机器人末端执行器的姿态进行估计具体如下：每个采样点的位置和加速度通过以下等式的粒子滤波估算：
其中代表第i个粒子的速度、A代表IMU的加速度、g代表局部重力矢量，
代表末端位置，C i代表从惯性测量传感器坐标系到世界坐标系的旋转矩阵；
机器人末端的位置状态被定义为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/tM 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处的实际位置，
位置粒子的权重被定义为最可能的值；最小的累积误差
被认为是最可能的位置值；因此，归一化的权重通过以下公式计算：
其中为对
求标准偏差运算。
- 根据权利要求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时刻系统噪声的协方差矩阵；雅可比矩阵 Jk+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代表单位矩阵。
Applications Claiming Priority (3)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910691787 | 2019-07-28 | ||
| CN201910765787.3 | 2019-08-19 | ||
| CN201910765787.3A CN110561424A (zh) | 2019-07-28 | 2019-08-19 | 基于多传感器混合滤波器的在线机器人运动学校准方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| WO2021031346A1 true WO2021031346A1 (zh) | 2021-02-25 |
Family
ID=68774081
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| PCT/CN2019/114533 WO2021031346A1 (zh) | 2019-07-28 | 2019-10-31 | 基于多传感器混合滤波器的在线机器人运动学校准方法 |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN110561424A (zh) |
| WO (1) | WO2021031346A1 (zh) |
Families Citing this family (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111222225B (zh) * | 2019-12-20 | 2023-08-29 | 浙江欣奕华智能科技有限公司 | 一种机器人中传感器位姿的确定方法及装置 |
| CN110991085B (zh) * | 2019-12-20 | 2023-08-29 | 上海有个机器人有限公司 | 一种机器人图像仿真数据的构建方法、介质、终端和装置 |
| CN111086001B (zh) * | 2019-12-25 | 2021-09-14 | 广东省智能制造研究所 | 一种足式机器人多模态感知的状态估计方法及系统 |
| CN111076721B (zh) * | 2020-01-19 | 2023-03-28 | 浙江融芯导航科技有限公司 | 一种快速收敛的惯性测量单元安装姿态估计方法 |
| CN111469130A (zh) * | 2020-04-26 | 2020-07-31 | 珠海格力智能装备有限公司 | 机器人的控制方法及装置、存储介质和处理器 |
| CN113587920B (zh) * | 2020-04-30 | 2024-02-20 | 阿里巴巴集团控股有限公司 | 运动测量方法、装置、电子设备及计算机可读存储介质 |
| CN114683259A (zh) * | 2020-12-28 | 2022-07-01 | 财团法人工业技术研究院 | 机械手臂校正系统及机械手臂校正方法 |
| TW202224872A (zh) | 2020-12-28 | 2022-07-01 | 財團法人工業技術研究院 | 機械手臂校正系統及機械手臂校正系統方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20080084435A (ko) * | 2007-03-16 | 2008-09-19 | 한국전자통신연구원 | 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법,시스템 및 그 방법을 기록한 기록매체 |
| CN105841576A (zh) * | 2015-02-04 | 2016-08-10 | 赫克斯冈技术中心 | 坐标测量机 |
| US20170261979A1 (en) * | 2016-03-09 | 2017-09-14 | International Business Machines Corporation | Automatic database filtering system utilizing robotic filters |
| CN108692701A (zh) * | 2018-05-28 | 2018-10-23 | 佛山市南海区广工大数控装备协同创新研究院 | 基于粒子滤波器的移动机器人多传感器融合定位方法 |
| US20190210227A1 (en) * | 2018-01-08 | 2019-07-11 | Anki, Inc. | Spatial Acoustic Filtering by a Mobile Robot |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9927814B2 (en) * | 2016-03-28 | 2018-03-27 | Fetch Robotics, Inc. | System and method for localization of robots |
| CN107270893B (zh) * | 2017-05-27 | 2020-11-06 | 东南大学 | 面向不动产测量的杆臂、时间不同步误差估计与补偿方法 |
| CN108318038A (zh) * | 2018-01-26 | 2018-07-24 | 南京航空航天大学 | 一种四元数高斯粒子滤波移动机器人姿态解算方法 |
| CN108827301A (zh) * | 2018-04-16 | 2018-11-16 | 南京航空航天大学 | 一种改进误差四元数卡尔曼滤波机器人姿态解算方法 |
| CN109521868B (zh) * | 2018-09-18 | 2021-11-19 | 华南理工大学 | 一种基于增强现实与移动交互的虚拟装配方法 |
| CN109395375A (zh) * | 2018-09-18 | 2019-03-01 | 华南理工大学 | 一种基于增强现实与移动交互的三维游戏接口设计方法 |
-
2019
- 2019-08-19 CN CN201910765787.3A patent/CN110561424A/zh active Pending
- 2019-10-31 WO PCT/CN2019/114533 patent/WO2021031346A1/zh active Application Filing
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20080084435A (ko) * | 2007-03-16 | 2008-09-19 | 한국전자통신연구원 | 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법,시스템 및 그 방법을 기록한 기록매체 |
| CN105841576A (zh) * | 2015-02-04 | 2016-08-10 | 赫克斯冈技术中心 | 坐标测量机 |
| US20170261979A1 (en) * | 2016-03-09 | 2017-09-14 | International Business Machines Corporation | Automatic database filtering system utilizing robotic filters |
| US20190210227A1 (en) * | 2018-01-08 | 2019-07-11 | Anki, Inc. | Spatial Acoustic Filtering by a Mobile Robot |
| CN108692701A (zh) * | 2018-05-28 | 2018-10-23 | 佛山市南海区广工大数控装备协同创新研究院 | 基于粒子滤波器的移动机器人多传感器融合定位方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110561424A (zh) | 2019-12-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2021031346A1 (zh) | 基于多传感器混合滤波器的在线机器人运动学校准方法 | |
| CN111207774B (zh) | 一种用于激光-imu外参标定的方法及系统 | |
| Du et al. | Online serial manipulator calibration based on multisensory process via extended Kalman and particle filters | |
| CN110125936B (zh) | 一种空间机器人的地面实验验证系统 | |
| JP5563200B2 (ja) | 連接構造の動作をキャプチャするための処理方法 | |
| CN107014376B (zh) | 一种适用于农业机械精准作业的姿态倾角估计方法 | |
| Ren et al. | A novel self-calibration method for MIMU | |
| JP5618066B2 (ja) | 力制御ロボットのキャリブレーション装置と方法 | |
| CN110887481B (zh) | 基于mems惯性传感器的载体动态姿态估计方法 | |
| CN111949929B (zh) | 一种多传感器融合的四足机器人运动里程计设计方法 | |
| CN114216456B (zh) | 一种基于imu与机器人本体参数融合的姿态测量方法 | |
| JP2012173190A (ja) | 測位システム、測位方法 | |
| Du et al. | An online method for serial robot self-calibration with CMAC and UKF | |
| WO2023082050A1 (zh) | 一种基于双层滤波框架的高精度里程估计方法 | |
| CN108871323B (zh) | 一种低成本惯性传感器在机动环境下的高精度导航方法 | |
| CN115533915A (zh) | 一种不确定环境下空中作业机器人主动接触检测控制方法 | |
| CN115540860A (zh) | 一种多传感器融合位姿估计算法 | |
| CN115855048A (zh) | 一种多传感器融合位姿估计方法 | |
| CN110967017A (zh) | 一种用于双移动机器人刚体协作搬运的协同定位方法 | |
| US11904482B2 (en) | Mechanical arm calibration system and mechanical arm calibration method | |
| US11685049B2 (en) | Robot localization using variance sampling | |
| CN114046800B (zh) | 一种基于双层滤波框架的高精度里程估计方法 | |
| Luo et al. | End‐Effector Pose Estimation in Complex Environments Using Complementary Enhancement and Adaptive Fusion of Multisensor | |
| CN114459478B (zh) | 一种基于姿态运动学模型的惯性测量单元数据融合方法 | |
| CN116338719A (zh) | 基于b样条函数的激光雷达-惯性-车辆融合定位方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| 121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 19942264 Country of ref document: EP Kind code of ref document: A1 |
|
| NENP | Non-entry into the national phase |
Ref country code: DE |
|
| 122 | Ep: pct application non-entry in european phase |
Ref document number: 19942264 Country of ref document: EP Kind code of ref document: A1 |
|
| 122 | Ep: pct application non-entry in european phase |
Ref document number: 19942264 Country of ref document: EP Kind code of ref document: A1 |
|
| 32PN | Ep: public notification in the ep bulletin as address of the adressee cannot be established |
Free format text: NOTING OF LOSS OF RIGHTS PURSUANT TO RULE 112(1) EPC (EPO FORM 1205 DATED 13.10.2022) |
|
| 122 | Ep: pct application non-entry in european phase |
Ref document number: 19942264 Country of ref document: EP Kind code of ref document: A1 |