讨论一种IMU加速度数据或者磁罗盘数据的标定方式

现在讨论一种IMU加速度数据或者磁罗盘数据的标定方法。

问题描述

由于IMU传感器的制造工艺和精度的限制，IMU的加速度计和磁罗盘的输出值会存在一定的误差，这些误差会对姿态解算的精度造成影响。IMU数据的误差来源主要有以下几个方面：

1. 零偏误差
2. 比例误差
3. 非正交误差
4. 随机误差
5. 温度漂移误差

其中，零偏误差、比例误差和非正交误差都可以通过标定得到，随机误差可以通过滤波算法进行抑制，温度漂移误差可以通过温度补偿进行抑制。我们这次主要讨论前三种误差的标定方法。由于原理一致，因此只讨论加速度计的标定方法。

误差模型

根据零偏误差、比例误差和非正交误差的定义，我们可以得到如下的误差模型：

\[\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

---

未经授权，禁止转载