IMU校正以及姿态融合

版权声明:本文为博主“声时刻”原创文章,未经博主允许不得转载。 联系方式:shenshikexmu@163.com https://blog.csdn.net/shenshikexmu/article/details/80013444

本文为博主“声时刻”原创文章,未经博主允许不得转载。
联系方式:shenshikexmu@163.com

缘起
有缘在简极科技兼职两年。接触了IMU,我去的时候那家公司还是一个要把IMU放进足球的公司,祝愿简极越来越好。IMU校正算法大概是接触传感器三个月做出来的,博客IMU加速度、磁力计校正--椭球拟合的内容,那时只是把校准问题当作椭球拟合问题。融合算法大概是接触IMU一年做出来的,中途学习了kalman滤波。在安卓上的实时算法一年半完成,视频
IMUCalibration-Gesture换了一个更好的校正算法,滤波算法不变。愿和大家一起学习并讨论。有些朋友邮件上问我问题,真的给我很大的鼓励。
校正
这边有一篇讲IMU误差的很好的博客,直接引用了,IMU误差模型和校准。也是在《A Robust and Easy to implement method for imu calibration without External Equipments》框架下解释误差的。
这里写图片描述
图源于IMU误差模型和校准
加速度
参照ICRA2014论文:《A Robust and Easy to implement method for imu calibration without External Equipments》
说明
摘数据的部分我没有按照ICRA2014的算法写,而是自己写了FindFixData这样一个函数。ICRA2014的摘数据的算法我也拿matlab写了一遍,算方差实在是太花时间了(可能我的那个算法需要优化),FindFixData算法轻便一些,就是要人为的设置参数,ICRA2014不需要人为设置参数。摘出的稳定数据校正角速度参数和磁力计参数,摘出的运动的数据用来校正角速度传感器。
角速度
参照ICRA2014论文:《A Robust and Easy to implement method for im calibration without External Equipments》
磁力计
算法1:mag2acc_matrix,假设重力与磁向量的夹角不变。

算法2:Cal_mag4acc_frame,利用不同姿态下传感器感受的磁通向量的变化与姿态变化的相关性,计算参数。

说明
算法中的校正传感器为MPU9250,这个传感器的加速度坐标系与磁力计坐标系的z轴反向。在校正时,把变换矩阵的T33T_{33}位置设为-1,如果其他传感器z轴相同,T33T_{33}设为1.

这里写图片描述
MPU9250
Tm2a=[T11T12T13T21T22T23T31T321]Tm2a=\begin{bmatrix} T_{11} & T_{12} &T_{13} \\T_{21} & T_{22}&T_{23} \\T_{31} &T_{32}&-1\end{bmatrix}

其他,是1还是-1,这个要看传感器资料了 。
Tm2a=[T11T12T13T21T22T23T31T321]Tm2a=\begin{bmatrix}T_{11} & T_{12} &T_{13} \\T_{21} & T_{22}&T_{23} \\T_{31} &T_{32}&1\end{bmatrix}
##参数
calacc=TaKa(rawacc+Ba)cal_{acc}=Ta*Ka*(raw_{acc}+Ba)
calgyro=TgKg(rawgyro+Bg)cal_{gyro}=Tg*Kg*(raw_{gyro}+Bg)
calmag=Tm2a(rawmag+Bm)cal_{mag}=Tm2a*(raw_{mag}+Bm)

姿态
Mahony filter
参考 《Nonlinear Complementery Filters on the Special Orthogonal Group》
受启发于 http://blog.csdn.net/luoshi006/article/details/51513580
EKF
参考 《A Double-Stage Kalman Filter for Orientation Tracking with an Integrated Processor in 9-D IMU》

High Low pass
Gyro进行高通滤波器,Accelerate & Magnetic进行低通滤波。

滤波结果
滤波结果
最后一帧姿态
姿态融合算法

开源代码
具体请参考github开源代码:IMUCalibration-Gesture.

赞助:如果您觉得此文对您所要做的工作有帮助,欢迎打赏。或者帮忙在github上标星星。


展开阅读全文

没有更多推荐了,返回首页