imu姿态模式（2. IMU原理及姿态融合算法详解）
2. IMU原理及姿态融合算法详解
一 、组合
IMU全称是惯性导航系统 ，主要元件有陀螺仪 、加速度计和磁力计 。其中 ，陀螺仪可以得到各个轴的加速度 ，而加速度计能够得到
x
x
x 、y
y
y 、z
z
z方向的加速度 ，而磁力计能获得周围磁场的信息 。主要的工作便是将三个传感器的数据融合得到较为准确的姿态信息 。
二 、 原理
a) 陀螺仪
陀螺仪是通过测量科氏力来检测角速度的 ，科氏力在大学物理中提到过 ，如下图：
一个物体以固定的线速度
v
v
v 运动 ，同时受到一个角速度的影响 ，这时候在叉乘方向上会有一个科氏力的作用 ，测量这个力便能直到角速度w
w
w 的大小 。在实际的MEME传感器中 ，大致结构如图 ，在一个方向保持左右运动 。若有旋转的角速度则会在垂直的方向产生科氏力 ，通过电容的变化来反应这个力的大小便能得到旋转速度的大小 。
b) 加速度计
加速度计的原理较为简单 ，就是通过牛顿第二定律来测量三轴的加速度 。图中的质量块受到加速度的作用会左右运动 ，而两侧的电容能测量质量块的位置 ，从而计算出加速度的大小 。
c) 磁力计
磁力计则是通过霍尔效应来测量磁场的强度 ，高中物理中学过霍尔效应也很简单 ，如图 。一端通电 ，在磁场的作用下电子会往垂直的方向上跑 ，从而在侧面产生电场 ，通过测量这个电场的强度及正负则能间接测量出厂强的大小 。
视频介绍如下： https://www.youtube.com/watch?v=eqZgxR6eRjo&feature=youtu.be
三 、 旋转的表达
a) 欧拉角
对姿态的描述 ，最直观的便是欧拉角 。可以用维基百科上的一张动图直观的表示：
b) 旋转矩阵
线性代数中 ，有讲解过 ，使用
3
×
3
3 \times 3
3×3 的矩阵可以表达物体的旋转 ，如绕Z
Z
Z轴的旋转可以表示为：
[
x
′
y
′
z
′
1
]
=
[
cos
θ
−
sin
θ
sin
t
h
e
t
a
cos
θ
1
1
]
⋅
[
x
y
z
1
]
(1)
\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}
⎣⎢⎢⎡x′y′z′1⎦⎥⎥⎤=⎣⎢⎢⎡cosθsintheta00−sinθcosθ0000100001⎦⎥⎥⎤⋅⎣⎢⎢⎡xyz1⎦⎥⎥⎤(1) 其他轴旋转可以自行百度 。c) 四元数
四元数的运算表达比较难理解 ，但是在数学上却可以优雅而完美的表达三维空间中的旋转 。它可以很好的避免欧拉角存在的万向锁问题 ，和轴角存在的不适合插值的缺点 ，同时它所需的参数量较少 ，因此现有的大部分效果较好的解法都是采用四元数解算的 。
这里主要介绍如何通过四元数来更新姿态：
已知当前姿态为四元数
q
1
q_1
q1 ，在Δ
t
\Delta t
Δt 时间内的角速度为ω
\omega
ω ， 求下一刻的四元数 。一般来说 ，采用一阶龙格库塔法来更新四元数 ，主要的思路便是 泰勒展开式 ，然后一阶近似 。
具体计算流程如下：
[
q
q
1
q
2
q
3
]
t
+
Δ
t
=
[
q
q
1
q
2
q
3
]
t
+
Δ
t
2
[
−
ω
x
q
1
−
ω
y
q
2
−
ω
z
q
3
ω
x
q
+
ω
z
q
2
−
ω
y
q
3
ω
y
q
−
ω
z
q
1
+
ω
x
q
3
ω
z
q
+
ω
y
q
1
−
ω
x
q
2
]
(2)
\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}
⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+Δt=⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+2Δt⎣⎢⎢⎡−ωxq1−ωyq2−ωzq3ωxq0+ωzq2−ωyq3ωyq0−ωzq1+ωxq3ωzq0+ωyq1−ωxq2⎦⎥⎥⎤(2)
d) 李群
SO
(
3
)
\text{SO}(3)
SO(3) 及 李代数
so
(
3
)
\text{so}(3)
so(3)
对于这两种表达方法在主流陀螺仪的姿态解算中并不常见 ，但是在某些算法中需要对姿态进行求导时 ，便需要采用李群的方法去表达姿态 。
比如对旋转的求导如下：
∂
Rp
∂
φ
=
lim
φ
→
exp
(
φ
∧
)
exp
(
ϕ
∧
)
p
−
exp
(
ϕ
∧
)
p
φ
=
lim
φ
→
I
+
φ
∧
exp
(
φ
∧
)
p
−
exp
(
φ
∧
)
p
φ
=
lim
φ
→
φ
∧
Rp
φ
=
lim
φ
→
−
(
Rp
)
∧
φ
φ
=
−
(
Rp
)
∧
(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}
∂φ∂Rp=φ→0limφexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p=φ→0limφI+φ∧exp(φ∧)p−exp(φ∧)p=φ→0limφφ∧Rp=φ→0limφ−(Rp)∧φ=−(Rp)∧(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
°
50°
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) 陀螺仪
陀螺仪获得角度的方法很简单 ，直接积分就好了 。但是直接积分会带来巨大的误差！！
第一个原因如图所示：
解决方法如下 ，采用中值积分：
另一个方面 ，陀螺仪得到的旋转数据是基于机体坐标系的 ，而我们要求的是世界坐标系下的姿态 ，这中间必然有一个坐标变换的关系：
[
ω
x
ω
y
ω
z
]
=
[
cos
γ
−
cos
θ
sin
γ
1
sin
θ
sin
γ
cos
cos
γ
]
[
θ
˙
γ
˙
ψ
˙
]
⟶
θ
,
γ
较小
[
θ
˙
γ
˙
ψ
˙
]
=
[
]
⋅
[
θ
γ
ψ
]
+
[
ω
x
ω
y
ω
z
]
\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}
⎣⎡ωxωyωz⎦⎤=⎣⎡cosγ0sinγ010−cosθsinγsinθcoscosγ⎦⎤⎣⎡θ˙创心域SEO版权声明：以上内容作者已申请原创保护，未经允许不得转载，侵权必究！授权事宜、对本内容有异议或投诉，敬请联系网站管理员，我们将尽快回复您，谢谢合作！