IMU校正以及姿态融合

版权声明：本文为博主原创文章，未经博主允许不得转载。

博主：shenshikexmu

联系方式：[email protected]

缘起

有缘在简极科技兼职两年。接触了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轴反向。在校正时，把变换矩阵的T33$T_{33}$位置设为-1，如果其他传感器z轴相同，T33$T_{33}$设为1.

MPU9250
Tm2a=⎡⎣⎢T11T21T31T12T22T32T13T23−1⎤⎦⎥$Tm2a=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& -1\end{array}\right]$

其他，是1还是-1，这个要看传感器资料了 。
Tm2a=⎡⎣⎢T11T21T31T12T22T32T13T231⎤⎦⎥$Tm2a=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& 1\end{array}\right]$

参数

calacc=Ta∗Ka∗(rawacc+Ba)$ca{l}_{acc}=Ta\ast Ka\ast (ra{w}_{acc}+Ba)$
calgyro=Tg∗Kg∗(rawgyro+Bg)$ca{l}_{gyro}=Tg\ast Kg\ast (ra{w}_{gyro}+Bg)$
calmag=Tm2a∗(rawmag+Bm)$ca{l}_{mag}=Tm2a\ast (ra{w}_{mag}+Bm)$

姿态

Mahony filter

参考 《Nonlinear Complementery Filters on the Special Orthogonal Group》
受启发于 http://blog.****.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上标星星。

---