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)
- Gyroscopes (AREA)
- Navigation (AREA)
- Manipulator (AREA)
Abstract
Description
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,使用粒子滤波对机器人末端执行器的姿态进行估计具体如下:每个采样点的位置和加速度通过以下等式的粒子滤波估算:机器人末端的位置状态被定义为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表示间隔数,其中,x 0:s表示当前粒子位置、 表示到迭代次数为s时第i个粒子的位置、 代表到迭代次数为s的累积位置差值;N是粒子滤波算法中的粒子的数目, 是第i个粒子在时刻k处的归一化重量,δ(·)是狄拉克δ函数;后验概率满足以下等式:
- 根据权利要求1所述的基于多传感器混合滤波器的在线机器人运动学校准方法,其特征在于,步骤S4,优化运动误差具体包括:在利用位置传感器和惯性测量单元收集机器人的位置和姿态信息的过程中,由于传感器的固有噪声,测量误差随着时间的推移而增加,扩展卡尔曼滤波被应用来优化运动误差;如果考虑N个旋转关节的4N个DH运动学参数从IMU传感器到机器人末端的位姿转换参数有4个,所述位姿转换参数为连杆长度、连杆扭角、连杆偏移和关节转角,则总参数的数量为4N+4;因此,在基于拓展卡尔曼滤波(EKF)的位姿估计过程中,估计位姿基于4(N+1)个D-H参数计算;估计状态的模型如下:P k+1|k=P k|k+Q kP 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.3A CN110561424A (zh) | 2019-07-28 | 2019-08-19 | 基于多传感器混合滤波器的在线机器人运动学校准方法 |
CN201910765787.3 | 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 | 阿里巴巴集团控股有限公司 | 运动测量方法、装置、电子设备及计算机可读存储介质 |
TW202224872A (zh) | 2020-12-28 | 2022-07-01 | 財團法人工業技術研究院 | 機械手臂校正系統及機械手臂校正系統方法 |
CN114683259A (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) | 連接構造の動作をキャプチャするための処理方法 | |
JP5614527B2 (ja) | 姿勢情報算出装置、姿勢情報算出システム、姿勢情報算出方法及び姿勢情報算出プログラム | |
CN107014376B (zh) | 一种适用于农业机械精准作业的姿态倾角估计方法 | |
JP5618066B2 (ja) | 力制御ロボットのキャリブレーション装置と方法 | |
Ren et al. | A novel self-calibration method for MIMU | |
CN110887481B (zh) | 基于mems惯性传感器的载体动态姿态估计方法 | |
CN111949929B (zh) | 一种多传感器融合的四足机器人运动里程计设计方法 | |
CN114216456B (zh) | 一种基于imu与机器人本体参数融合的姿态测量方法 | |
Du et al. | An online method for serial robot self-calibration with CMAC and UKF | |
JP2012173190A (ja) | 測位システム、測位方法 | |
WO2023082050A1 (zh) | 一种基于双层滤波框架的高精度里程估计方法 | |
CN108871323B (zh) | 一种低成本惯性传感器在机动环境下的高精度导航方法 | |
CN115540860A (zh) | 一种多传感器融合位姿估计算法 | |
CN115533915A (zh) | 一种不确定环境下空中作业机器人主动接触检测控制方法 | |
CN115855048A (zh) | 一种多传感器融合位姿估计方法 | |
CN110967017A (zh) | 一种用于双移动机器人刚体协作搬运的协同定位方法 | |
US11904482B2 (en) | Mechanical arm calibration system and mechanical arm calibration method | |
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 |