IMU标定与解算
Published: 2020-03-15
误差种类 #
\[ a^{B}=T^{a}K^{a}(a^{S}+b^{a}) \\[2mm] w^{B}=T^{g}K^{g}(w^{S}+b^{g}) \]
加速度和陀螺仪包含如下3种误差：
偏置
比例误差
\[ K^{a}= \begin{bmatrix} s^{a}_{x} & 0&0 \\ 0 &s^{a}_{x} &0 \\ 0& 0&s^{a}_{x} \end{bmatrix} K^{g}= \begin{bmatrix} s^{g}_{x} & 0&0 \\ 0 &s^{g}_{x} &0 \\ 0& 0&s^{g}_{x} \end{bmatrix} \]
- 轴向误差
建立一个正交坐标轴，x轴与加速度计的x轴重合，y轴在加速度计的xy平面上。
\[ T^{a}=\begin{bmatrix} 1 & -\alpha_{yz} & \alpha_{zy} \\ 0 & 1 & -\alpha_{zx} \\ 0 & 0 & 1 \end{bmatrix} \]
陀螺仪坐标轴与正交坐标轴之间的转换关系为：
\[ T^g=\begin{bmatrix} 1 & -\gamma_{yz} & \gamma_{zy} \\ \gamma_{xz} & 1 & -\gamma{zx} \\ \gamma_{xy} & \gamma_{yx} & 1 \end{bmatrix} \]
加速度校准 #
静态数据标记
计算方差大小，当方差小于某一阈值时，认为是静止状态。
参数估计
当加速度计静止时，输出的观测值大小为9.81，由此可以构建：
待估参数：
\[ \theta^{acc}=[\alpha_{yz},\alpha_{zy},\alpha_{zx},s^{a}_{x},s^{a}_{y},s^{a}_{z},b^{a}_{x},b^{a}_{y},b^{a}_{z}] \]
代价函数：
\[ L(\theta^{acc})=\sum_{k=1}^M(\|g\|^2-\|T^aK^a(a^S+b^a)\|^2)^2 \]
六面法
陀螺仪校准 #
偏置
Allen方差包括：
- 量化噪声
- 角度随机游走
- 零偏不稳定性
- 速度随机游走
- 速度爬升
通过Allan方差分析得到陀螺仪Bias的过程，一般要采集好几个小时的数据。如果仅需要零偏参数，则在初始放置的50s左右的时间就足够了。
参数估计
IMU从静止状态，经过一定的平移旋转，到达一个新的静止状态。由此可以构建：
待估参数：
\[ \theta ^{gyro}=[\gamma_{yz},\gamma_{zy},\gamma_{xz},\gamma_{zx},\gamma_{xy},\gamma_{yx},s^{g}_{x},s^{g}_{y},s^{g}_{z}] \]
初始的加速度向量\(u_{a,k-1}\)，经过多个陀螺仪测量值\(\omega_i\)旋转之后，采用龙格库塔积分， 计算得到旋转之后的加速度向量\(u_{g,k}\)。而真实的加速度向量为\(u_{a,k}\)。可知代价函数：
\[ L(\theta ^{gyro})=\sum^{M}_{k=2}\|u_{a,k}-\Psi[w^{S}_{i},u_{a,k-1}]\|^{2} \]
龙格库塔积分
姿态解算方法 #
mahony互补滤波
EKF
madgwick梯度下降法
参考资料 #
Next: Odometry
Previous: move_base 模块