2. IMU原理及姿态融合算法详解

L☆★

已于 2023-05-05 11:35:10 修改

阅读量1.5w

点赞数 42

分类专栏： # IMU常见基础知识文章标签：算法人工智能

于 2022-12-04 15:34:46 首次发布

原文链接：https://blog.csdn.net/RoboChengzi/article/details/97616482

版权

IMU常见基础知识专栏收录该内容

2 篇文章 20 订阅

订阅专栏

文章目录

2. IMU原理及姿态融合算法详解

2. IMU原理及姿态融合算法详解

一、组合

IMU全称是惯性导航系统，主要元件有陀螺仪、加速度计和磁力计。其中，陀螺仪可以得到各个轴的加速度，而加速度计能够得到 $x$ 、 $y$ 、 $z$ 方向的加速度，而磁力计能获得周围磁场的信息。

主要的工作便是将三个传感器的数据融合得到较为准确的姿态信息。

二、原理

a) 陀螺仪

陀螺仪是通过测量科氏力来检测角速度的，科氏力在大学物理中提到过，如下图：

在这里插入图片描述

一个物体以固定的线速度 $v$ 运动，同时受到一个角速度的影响，这时候在叉乘方向上会有一个科氏力的作用，测量这个力便能直到角速度 $w$ 的大小。

在实际的MEME传感器中，大致结构如图，在一个方向保持左右运动。若有旋转的角速度则会在垂直的方向产生科氏力，通过电容的变化来反应这个力的大小便能得到旋转速度的大小。

在这里插入图片描述

b) 加速度计

加速度计的原理较为简单，就是通过牛顿第二定律来测量三轴的加速度。图中的质量块受到加速度的作用会左右运动，而两侧的电容能测量质量块的位置，从而计算出加速度的大小。

c) 磁力计

磁力计则是通过霍尔效应来测量磁场的强度，高中物理中学过霍尔效应也很简单，如图。一端通电，在磁场的作用下电子会往垂直的方向上跑，从而在侧面产生电场，通过测量这个电场的强度及正负则能间接测量出厂强的大小。

在这里插入图片描述

视频介绍如下： https://www.youtube.com/watch?v=eqZgxR6eRjo&feature=youtu.be

三、旋转的表达

a) 欧拉角

对姿态的描述，最直观的便是欧拉角。可以用维基百科上的一张动图直观的表示：

b) 旋转矩阵

线性代数中，有讲解过，使用 $\times 3$ 的矩阵可以表达物体的旋转，如绕 $Z$ 轴的旋转可以表示为：
$\begin{bmatrix} x^{'} \\ y^{'} \\ z^{'} \\ 1 \end{bmatrix} = \begin{bmatrix} \cos \theta & -\sin \theta & 0 & 0 \\ \sin theta & \cos \theta & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} \tag {1}$
其他轴旋转可以自行百度。

c) 四元数

四元数的运算表达比较难理解，但是在数学上却可以优雅而完美的表达三维空间中的旋转。它可以很好的避免欧拉角存在的万向锁问题，和轴角存在的不适合插值的缺点，同时它所需的参数量较少，因此现有的大部分效果较好的解法都是采用四元数解算的。

这里主要介绍如何通过四元数来更新姿态：

已知当前姿态为四元数 $q_1$ ，在 $\Delta t$ 时间内的角速度为 $\omega$ ，求下一刻的四元数。

一般来说，采用一阶龙格库塔法来更新四元数，主要的思路便是 泰勒展开式，然后一阶近似。

具体计算流程如下：
$\begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix}_{t + \Delta t} = \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix}_{t} + \frac{\Delta t}{2} \begin{bmatrix} -\omega _x q_1 - \omega_y q_2 - \omega_z q_3 \\ \omega_x q_0 + \omega_z q_2 - \omega_y q_3 \\ \omega_y q_0 - \omega_z q_1 + \omega_x q_3 \\ \omega_z q_0 + \omega_y q_1 - \omega_x q_2 \end{bmatrix} \tag {2}$

d) 李群 $\text{SO}(3)$ 及李代数 $\text{so}(3)$

对于这两种表达方法在主流陀螺仪的姿态解算中并不常见，但是在某些算法中需要对姿态进行求导时，便需要采用李群的方法去表达姿态。

比如对旋转的求导如下：
$\begin{aligned} \frac{\partial \textbf{Rp}}{\partial \varphi } &= \lim_{\varphi \to 0} \frac{\exp(\varphi^{\wedge}) \exp (\phi^{\wedge})\textbf{p} -\exp(\phi^{\wedge})\textbf{p}}{\varphi } \\ &= \lim_{\varphi \to 0} \frac{\textbf{I} + \varphi^{\wedge}\exp (\varphi^{\wedge})\textbf{p} -\exp (\varphi^{\wedge})\textbf{p}}{\varphi } \\ &= \lim_{\varphi \to 0} \frac{\varphi^{\wedge } \textbf{Rp}}{\varphi} \\ &= \lim_{\varphi \to 0}\frac{-(\textbf{Rp})^{\wedge} \varphi }{\varphi} \\ &= -(\textbf{Rp})^{\wedge} \end{aligned} \tag{3}$

具体可参考高翔博士的《视觉SLAM 14讲》

博客：https://www.cnblogs.com/gaoxiang12/p/5137454.html

视频：https://www.bilibili.com/video/BV16t411g7FR?p=3&vd_source=484659340e491a658a0140936c410c09

个人笔记：https://blog.csdn.net/weixin_43662553/article/details/128161000?spm=1001.2014.3001.5502

四、传感器的噪声及去除

传感器噪声，一般分为两种：

随机噪声：一般认为随机噪声是符合高斯分布的
固定误差：固定误差是由于传感器的测量原理导致的，这部分通常是去噪的重点。实际上，由于制造和安装误差，会有许许多多的固定误差。但是由于使用要求不高，且大部分校准需要高精度转台，只能做较为简单的校准工作。

a) 陀螺仪

陀螺仪直接测量的是角速度而非角度，所以需要通过一次积分才能得到角度值。

在积分过程中，若有固定的、某一个方向的数据，则会在积分的过程中，不断加大影响导致角度偏差。

通常来说，陀螺仪的温漂是比较严重的，基本上温漂正比于芯片的价格，越贵的芯片漂的越少。温漂的数据既与温度相关，又与时间相关。也就是说，不同温度下不一样，不同上电时间下也不一样。

通常简单的做法是：在上电的时候静止一段时间计算出此时的零偏，然后每次减去零偏。

更高级的方法需要标定温度与零偏的关系，然后线性插补；另一方面使用艾伦方差分析法得到零偏和时间的关系（艾伦方差法见博客https://blog.csdn.net/yandld/article/details/81101984）。

对于其他的误差，比如三轴不相互垂直，以及尺度因子不一致等误差，都可以忽略。

当然，更好的情况是在电路上做一个温度控制，维持在温度 $50°$ 左右（必须要在常温以上）。

b) 加速度计

对于加速度计，同样会有零漂和尺度因子的误差，但是加速度计在静止时可直接得到角度而不需要积分，所以零漂的影响很小，但是尺度因子的影响较大。

同样是重力加速度，各个面朝下时检测到的数值是不一样的。一般来说，校准的方法有六面校准。就是各个面朝下，然后记录重力的数值，计算得到尺度因子。目前MEMS传感器的精度已经很高了，很多情况下只用正面朝上校准一次即可（仅适用于无人机）。

若要求不高，可以不去校准加速度计，而对于云台有其他的校准思路。

c) 磁力计

磁力计的数据误差较大，校准便显得很重要。

一般可以导出数据到MATLAB中，然后采用 椭球校准的方法 。但是这样比较麻烦，主要用于写论文…

而大多数飞控的做法，都是直接在单片机上处理的，步骤如下：

先头朝上，水平旋转一周
然后头朝下，再水平旋转一周。
若计算能力有限，可直接求最大最小数据的中值，得到偏差，然后计算幅值，

save.mag_offset[i] = 0.5f *(max_t[i] + min_t[i]);//中值校准
save.mag_gain[i] = safe_div(200.0f ,(0.5f *(max_t[i] - min_t[i])),0);//幅值校准

若计算能力较充裕，采用LM算法可计算出三维的偏差和三维的尺度因子，具体参考天穹飞控代码。

五、姿态解算原理

IMU的算法紧紧地围绕着如何利用这三个元器件，获得准确的姿态，基本要求有几点：

滞后效应不明显
角度准确
静止时角度不漂

但很多时候，都无法满足所有的要求，需要根据实际情况的需求来有所取舍。

a) 陀螺仪

陀螺仪获得角度的方法很简单，直接积分就好了。但是直接积分会带来巨大的误差！！

第一个原因如图所示：

解决方法如下，采用中值积分：

在这里插入图片描述

另一个方面，陀螺仪得到的旋转数据是基于机体坐标系的，而我们要求的是世界坐标系下的姿态，这中间必然有一个坐标变换的关系：
$\begin{bmatrix} \omega _x \\ \omega_y \\ \omega_z \end{bmatrix} = \begin{bmatrix} \cos \gamma & 0 & -\cos \theta \sin \gamma \\ 0 & 1 & \sin \theta \\ \sin \gamma & 0 & \cos \cos \gamma \end{bmatrix}\begin{bmatrix} \dot{\theta} \\ \dot{\gamma } \\ \dot{\psi} \end{bmatrix}\stackrel{\theta,\space \gamma \text{较小}}{\longrightarrow} \begin{bmatrix} \dot{\theta} \\ \dot{\gamma } \\ \dot{\psi} \end{bmatrix}=\begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix} \cdot \begin{bmatrix} \theta \\ \gamma \\ \psi \end{bmatrix} + \begin{bmatrix} \omega _x \\ \omega_y \\ \omega_z \end{bmatrix}$
对于无人机来说，pitch和roll都比较小，可以认为是 $0$ ，则旋转矩阵变为了单位矩阵。