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

xinyu_khan

已于 2024-02-21 17:37:49 修改

阅读量573

点赞数 21

分类专栏：知识分享文章标签：算法机器人矩阵线性代数概率论自动驾驶自动化

于 2024-02-21 17:13:45 首次发布

本文链接：https://blog.csdn.net/hxy_1993_08_18/article/details/136216448

版权

知识分享专栏收录该内容

14 篇文章 0 订阅

订阅专栏

讨论一种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$ 是一个 $\times 3$ 的矩阵， $\mathbf b_a$ 是一个 $\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}$