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

2023-02-28

cos 加速度计陀螺仪

文章目录2.IMU原理及姿态融合算法详解一、组合二、原理a)陀螺仪b)加速度计c)磁力计三、旋转的表达a)欧拉角b)旋转矩阵c)四元数d)李群SO(3)\text{SO}(3)SO(3)及李代数so(3)\text{so}(3)so(3)四、传感器的噪声及去除a)陀螺仪b)加速度计c)磁力计五、姿态

文章目录

2. IMU原理及姿态融合算法详解
- 一、组合
- 二、原理
- - a) 陀螺仪
  - b) 加速度计
  - c) 磁力计
- 三、旋转的表达
- - a) 欧拉角
  - b) 旋转矩阵
  - c) 四元数
  - d) 李群 $\text{SO}(3)$ 及李代数 $\text{so}(3)$
- 四、传感器的噪声及去除
- - a) 陀螺仪
  - b) 加速度计
  - c) 磁力计
- 五、姿态解算原理
- - a) 陀螺仪
  - b) 加速度计
  - c) 磁力计
- 六、姿态解算算法
- - a) 互补滤波
  - b) AHRS
  - c) 卡尔曼
- 七、云台的特性及要求

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$ 轴的旋转可以表示为：

⎡ ⎣ ⎢ ⎢ ⎢ ⎢ x' y' z' 1 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥

$\begin{bmatrix} x^{'} \\ y^{'} \\ z^{'} \\ 1 \end{bmatrix}$ =

⎡ ⎣ ⎢ ⎢ ⎢ cos θ sin t h e t a 00 - sin θ cos θ 00 00100001 ⎤ ⎦ ⎥ ⎥ ⎥

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

⎡ ⎣ ⎢ ⎢ ⎢ x y z 1 ⎤ ⎦ ⎥ ⎥ ⎥

$\begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix}$ \tag {1}

⎣ ⎢ ⎢ ⎡ x^{^{'}} y^{^{'}} z^{^{'}} 1 ⎦ ⎥ ⎥ ⎤ = ⎣ ⎢ ⎢ ⎡ cos θ sin t h e t a 00 - sin θ cos θ 00 00100001 ⎦ ⎥ ⎥ ⎤ \cdot ⎣ ⎢ ⎢ ⎡ x y z 1 ⎦ ⎥ ⎥ ⎤ (1)

其他轴旋转可以自行百度。

c) 四元数

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

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

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

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

具体计算流程如下：

⎡ ⎣ ⎢ ⎢ ⎢ q 0 q 1 q 2 q 3 ⎤ ⎦ ⎥ ⎥ ⎥

$\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}

⎣ ⎢ ⎢ ⎡ q_{0} q_{1} q_{2} q_{3} ⎦ ⎥ ⎥ ⎤_{t + Δ t} = ⎣ ⎢ ⎢ ⎡ q_{0} q_{1} q_{2} q_{3} ⎦ ⎥ ⎥ ⎤_{t} + \frac{Δ t}{2} ⎣ ⎢ ⎢ ⎡ - ω_{x} q_{1} - ω_{y} q_{2} - ω_{z} q_{3} ω_{x} q_{0} + ω_{z} q_{2} - ω_{y} q_{3} ω_{y} q_{0} - ω_{z} q_{1} + ω_{x} q_{3} ω_{z} q_{0} + ω_{y} q_{1} - ω_{x} q_{2} ⎦ ⎥ ⎥ ⎤ (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}

\frac{\partial Rp}{\partial φ} = φ \to 0 lim \frac{exp ( φ ^{\land} ) exp ( ϕ ^{\land} ) p - exp ( ϕ ^{\land} ) p}{φ} = φ \to 0 lim \frac{I + φ ^{\land} exp ( φ ^{\land} ) p - exp ( φ ^{\land} ) p}{φ} = φ \to 0 lim \frac{φ ^{\land} Rp}{φ} = φ \to 0 lim \frac{- ( Rp ) ^{\land} φ}{φ} = - (Rp)^{\land} (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