讨论一种IMU加速度数据或者磁罗盘数据的标定方式
现在讨论一种IMU加速度数据或者磁罗盘数据的标定方法。
问题描述
由于IMU传感器的制造工艺和精度的限制，IMU的加速度计和磁罗盘的输出值会存在一定的误差，这些误差会对姿态解算的精度造成影响。IMU数据的误差来源主要有以下几个方面：
- 零偏误差
- 比例误差
- 非正交误差
- 随机误差
- 温度漂移误差
其中，零偏误差、比例误差和非正交误差都可以通过标定得到，随机误差可以通过滤波算法进行抑制，温度漂移误差可以通过温度补偿进行抑制。我们这次主要讨论前三种误差的标定方法。由于原理一致，因此只讨论加速度计的标定方法。
误差模型
根据零偏误差、比例误差和非正交误差的定义，我们可以得到如下的误差模型：
\[\begin{aligned} \mathbf{a}_{s} &=\mathbf{M}_{a}(\mathbf{a}_{m} - \mathbf{b}_a - \mathbf{w}_{a}) \\ \end{aligned} \tag{1}\]其中， $\mathbf a_m$ 是加速度计的测量值， $\mathbf a_s$ 是加速度计的真实值，$\mathbf M_a$ 是加速度计的比例和非正交误差矩阵，$\mathbf b_a$ 是加速度计的零偏误差向量，$\mathbf w_a$ 是加速度计的随机误差向量，我们假设它满足高斯分布，即 $\mathbf w_a \sim \mathcal N \left(\mathbf 0, \mathbf Q_a\right)$ ，其中 $\mathbf Q_a$ 是加速度计的随机误差协方差矩阵。
误差模型中的 $\mathbf M_a$ 和 $\mathbf b_a$ 是我们需要标定的参数，其中 $\mathbf M_a$ 是一个 $3 \times 3$ 的矩阵，$\mathbf b_a$ 是一个 $3 \times 1$ 的向量。
代价函数
如果我们想得到 $\mathbf M_a$ 和 $\mathbf b_a$ 的最优解，我们需要定义一个代价函数，然后通过最小化代价函数来得到最优解。显然在静态情况下 $\mathbf a_s$ 的模长应该等于重力加速度的模长，即：
\[\begin{aligned} \left\|\mathbf{a}_{s}\right\| &= g \end{aligned} \tag{2}\]因此我们可以构建如下代价函数：
\[\begin{aligned} \mathbf{J} &= \sum_{i=1}^{N}({\mathbf{a}_{s}^{i}}^T\mathbf{a}_{s}^{i} - g^2)^2 \end{aligned} \tag{3}\]其中， $N$ 是数据的个数， $\mathbf a_s^{i}$ 是第 $i$ 个数据点的真实加速度值， $g$ 是重力加速度的模长。
通过上述代价函数不难发现，如果对 $\mathbf a_s^{i}$ 做一个给定的单位正交变换 $\mathbf A$ ，则有如下关系：
\[\begin{aligned} \mathbf{A}\mathbf{a}_{s}^{i} &= \mathbf{A}\mathbf{M}_{a}(\mathbf{a}_{m}^{i} - \mathbf{b}_a - \mathbf{w}_{a}^{i}) \\ \mathbf{J}^{'} &= \sum_{i=1}^{N}({\mathbf{A}\mathbf{a}_{s}^{i}}^T\mathbf{A}\mathbf{a}_{s}^{i} - g^2)^2 \\ &= \sum_{i=1}^{N}({\mathbf{a}_{s}^{i}}^T\mathbf{A}^T\mathbf{A}\mathbf{a}_{s}^{i} - g^2)^2 \\ &= \sum_{i=1}^{N}({\mathbf{a}_{s}^{i}}^T\mathbf{a}_{s}^{i} - g^2)^2 \\ &\equiv \mathbf{J} \end{aligned} \tag{4}\]由此可知，对于一个 $\mathbf M_a$ ，它的任意单位正交变换 $\mathbf M_a^{‘}=\mathbf A \mathbf M_a$ 其代价和 $\mathbf M_a$ 的代价是一样的，因此如果不对 $\mathbf M_a$ 做约束，那么我们可以得到无穷多个解。为了得到唯一的解，我们需要对 $\mathbf M_a$ 做约束。约束的方式如下：
- 三角约束
这种情况约束 $\mathbf M_a$ 是一个三角矩阵，上三角或者下三角是等价的，我们以下三角为例，具有如下形式：
\[\begin{aligned} \mathbf{M}_{a} &= \left[\begin{array}{ccc} m_{11} & 0 & 0 \\ m_{21} & m_{22} & 0 \\ m_{31} & m_{32} & m_{33} \end{array}\right] \end{aligned} \tag{5}\]我们可以比较形象的想象出上述约束的具体几何意义：首先通过第一行，我们可以知道我们约束测量轴x和真实轴x对准，然后第二行我们约束了测量轴y一定落在真实的xy平面上，最后第三行对于测量轴z则没有更多的约束，这就好比把一个非正交坐标架在正交坐标架中进行了“固定”，不让其随意转动产生无穷多解。
- 对称约束
这种情况约束 $\mathbf M_a$ 是一个对称矩阵，具有如下形式：
\[\begin{aligned} \mathbf{M}_{a} &= \left[\begin{array}{ccc} m_{11} & m_{12} & m_{13} \\ m_{12} & m_{22} & m_{23} \\ m_{13} & m_{23} & m_{33} \end{array}\right] \end{aligned} \tag{6}\]我认为其实这样的约束不能体现非正交误差，因为对于与对称矩阵，我们可以对其进行对角化：
\[\begin{aligned} \mathbf{M}_{a} &= \mathbf{P}\mathbf{D}\mathbf{P}^{-1} \end{aligned} \tag{7}\]其中， $\mathbf P$ 是一个正交矩阵， $\mathbf D$ 是一个对角矩阵，因此一个对称矩阵相当于正交变换+缩放变换+正交变换，因此对称矩阵的约束实际上是对比例误差的约束。那如果我们只考虑比例误差，那对称矩阵在这种情况下也是存在无穷多解。
- 对角约束
因此综上所属，如果确实想只考虑比例误差，那对 $\mathbf M_a$ 的约束应该是一个对角矩阵。即：
\[\begin{aligned} \mathbf{M}_{a} &= \left[\begin{array}{ccc} m_{11} & 0 & 0 \\ 0 & m_{22} & 0 \\ 0 & 0 & m_{33} \end{array}\right] \end{aligned} \tag{8}\]优化方法
根据代价函数，我们可以手动求导得到雅可比矩阵，然后通过优化方法求解最优解，例如可以使用ceres或者g2o
未经授权，禁止转载