stm32f4 mpu6050与姿态解算（仅供自己学习使用，学习笔记)

一.陀螺仪的组成原理

1）加速度计

MEMS加速度计利用红色的这部分质量，当这一方向上存在加速度时，利用形变，可以检测到电容值的变化，就可推算出这个方向上的加速度了

2）陀螺仪

由于科利奥氏现象，一个物体朝一个方向运动时，如果有外部的角速度，在这个物体系下观察垂直于速度方向的位移，这时候在位移方向会有一个科氏力的作用，测量这个力便能直到角速度W的大小。

在实际的MEME传感器中，大致结构如图，在一个方向保持左右运动，若有旋转的角速度则会在垂直的方向产生科氏力，通过电容的变化来反应这个力的大小便能得到旋转速度的大小。黄色的部分带动红色的负载在Driving Direction上不停的震动这时如果有一个外部的角速度，会产生垂直于Driving Direction的位移，使得最里面的红色和蓝色间的电容值发生变化。

3）磁力计

在六轴数据融合姿态解算算法中，我们通过理论和实际重力加速度向量来补偿陀螺仪误差。但重力加速度向量垂直于大地坐标系的xoy平面，即平行于Yaw轴转轴，故无法用于修正Yaw轴的角度数据。因此我们引入磁力计数据，由于地球磁场方向在中低纬度地区与地面大致平行，因此通过磁力计数据我们可以有效的修正Yaw轴的角度数据。

但在一部分情况中，复杂的磁场环境会导致九轴数据融合的结果并不能远好于六轴数据，甚至劣于六轴数据融合的结果，比如RoboMaster机器人云台在某些情况下不使用磁力计数据。原因是云台复杂多变的磁场环境会给磁力计引入很大的噪声。综上所述，具体选择九轴还是六轴数据的融合还需要根据具体环境来决定。
磁力计是通过霍尔效应来测量磁场的强度。这个视频中会有详细讲解，可以参考这个视频: https://www.youtube.com/watch?v=eqZgxR6eRjo&feature=youtu.be.
可也以参考这篇文章，这篇文章对内部传感器的原理解释的更加详细充分: http://www.51hei.com/bbs/dpj-92911-1.html.
值得注意的是磁力计解算姿态时不能直接采用公式
anglez=−arctan2(mny,mnx)
而是需要使用加速度计计算出的roll和pitch做一个坐标变换，再带入计算。

这个地方为什么这个不可以直接采用公式？，改日进网址再看一下

二.椭球拟合

由于 x,y,z 三轴的单位存在差异，测量数据的向量顶点会落在椭球面上而非理想状态下的球面上。另外，电路板产生的电磁场会使磁力计测量数据出现偏移，这则会导致椭球的中心不在原点。因此，在磁力计进行椭球拟合是很有必要的。具体椭球拟合算法可以参考https://blog.csdn.net/shenshikexmu/article/details/70143455.

三.姿态的描述

1)坐标系

载体坐标系

当地导航坐标系

当地导航坐标系中常用的有东-北-天坐标系和北-东-地坐标系
东北天坐标系：
X轴：指东;
Y轴：指北;
Z轴：指天；
画了一个大概示意图

北东地坐标系：
X轴：指北;
Y轴：指东;
Z轴：指地；

载体坐标系

与导航坐标系类似，常用的载体坐标系也有如下两种：
前右下坐标系—对应北东地导航坐标系
X轴：指向载体前进方向;
 Y轴：指向载体右侧;
 Z轴：指向下
右前上坐标系—对应东北天导航坐标系
X轴：指向载体右侧;
 Y轴：指向载体前进方向;
 Z轴：指向上

2)四元数

我理解的四元数是用1，i，j，k来表示四维球的坐标和旋转
比如说复数是在一维平面内的，复数的运算实际上是线的伸缩和旋转，扩展到三维坐标系中，设三维坐标系中有一个单位为1的圆球，当三维的球映射在二维的坐标系中，球的上半部分会映射在xyij的单位圆内，而圆的下半部分会映射在那个圆以外的地方直至无穷远，坐标轴zk穿过的圆球上方的那个点映射在原点，对应的圆球下方的那个点，映射在无穷远处，于是在三维坐标系中1，i，j，-1，-i，-j代表的其实是一个圆，拓展到四维球，给原点乘一个i，就相当于把圆点拉向i的那个点，乘两个i以后，整个球就会变成里子被翻到外面的状态，，乘四个i以后，球就回到了原来的那个状态，我画了一个大致图，有点简陋，这就是我对四元数乘法的理解

四元数有以下运算规则
j∗k=i,k∗j=−i;
j∗k=i,k∗j=−i;
k∗i=j,i∗k=−j;
i∗i=j∗j=k∗k=i∗j∗k=−1
其中
i旋转代表X轴与Y轴相交平面中X轴正向向Y轴正向的旋转
j旋转代表Z轴与X轴相交平面中Z轴正向向X轴正向的旋转
k旋转代表Y轴与Z轴相交平面中Y轴正向向Z轴正向的旋转
-i、-j、-k分别代表i、j、k旋转的反向旋转
一个四元数可以写作
(刚刚开始写，不太会写數學公式)
$$q1(w1+xi+yj+zk)$$

其中

$$w^2+x^2+y^2+z^2=1$$
四元数的模：也表示原点到那个点的距离
通过四元数反解欧拉角
得到四元数后，可以通过四元数的值反解出机体坐标系的欧拉角，这里省略推导过程直接给出公式：

这里就先简单写到这里，具体更详细的可以参考这篇文章https://blog.csdn.net/AndrewFan/article/details/62057519.

3)欧拉角

使用欧拉角会出现万向节锁死的问题具体可以参考这篇文章https://blog.csdn.net/AndrewFan/article/details/60981437#.
因为会出现万向节锁死的问题，所以欧拉角方法只适用于水平姿态变化不大的情况，而不适用于全姿态飞行器的姿态确定

欧拉角的直观表现可以参考这篇https://www.zhihu.com/question/47736315.
以下为我自己对欧拉角的理解，用欧拉角表示旋转时，有两组坐标，一个是全局坐标，一个是局部坐标，旋转步骤为：
1.物体绕全局的z轴旋转
2.物体再绕自己的x轴旋转
3.最后再绕自己的z轴旋转

总结以下三点：
旋转的正方向为：从旋转轴看的逆时针方向
旋转的顺序为：Z-X-Y
对应的欧拉角：Yaw-Pitch-Roll
每当用到欧拉角时，我们必须明确的表示出夹角的顺序，指定其参考轴
欧拉角的基本思想是将角位移分解为绕三个互相垂直轴的三个旋转组成的序列。所以，欧拉旋转的三个角，可以对应于三个旋转矩阵。
Yaw（偏航）：欧拉角向量的y轴
Pitch（俯仰）：欧拉角向量的x轴
Roll（翻滚）： 欧拉角向量的z轴

4）旋转矩阵

更详细的导航坐标系与载体坐标系之间的姿态旋转矩阵可参考这篇文章https://blog.csdn.net/zht2370201/article/details/89313370?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522161530989616780266271997%2522%252C%2522scm%2522%253A%252220140713.130102334…%2522%257D&request_id=161530989616780266271997&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2_allsobaiduend~default-1-89313370.first_rank_v2_pc_rank_v29&utm_term=%E4%B8%9C%E5%8C%97%E5%A4%A9%E5%9D%90%E6%A0%87%E7%B3%BB%E5%AE%9A%E4%B9%89.
对于两个三维点 p1(x1,y1,z1)，p2(x2,y2,z2)，由点 p1 经过旋转矩阵 R 旋转到 p2，（旋转矩阵为正交矩阵）
（由于我不会打數學公式，所以只能搬别人的了）

绕x轴旋转：

绕y轴旋转：

绕z轴旋转：

任意旋转矩阵（欧拉角）：

解方程得：

还有的文章说使用欧拉角的话套用欧拉微分方程就可以了

https://blog.csdn.net/yeppchan/article/details/83992434.
可以参考这篇文章

用四元数表示旋转矩阵可以表示为

其中：

对应四元数为：

也可写成有序数对形式，即2.1旋转公式中的：

四.传感器的噪声及去除与误差补偿

基本原理

对角速度进行积分可以得到角度，但是积分会使结果有误差。对加速度积分，然后正交分解可以得到角度，但是电机旋转震动会给加速度计的数据带来高频噪声，将两个数据进行融合，用加速度计来补偿角度的误差
设有大地坐标下的重力加速度 g，利用矩阵的逆坐标变换后得到机体坐标系下的重力加速度
不难看出，将重力加速度向量变换至机体坐标系后，恰好是矩阵的最后一列。这样一来，我们就得到了由描述刚体姿态的四元数推导出的理论重力加速度向量 v^ 。另外，我们还可以通过加速度计测量出实际重力加速度向量 v。

理论重力加速度和实际重力加速度的误差主要来自于陀螺仪数据产生的角速度误差引起的，所以可以根据理论重力加速度和实际重力加速度的误差来补偿陀螺仪数据中的误差，将隐藏在四元数中的误差显化，解算出较为准确的姿态

误差补偿

计算向量间的夹角一般有两种方法内积（点乘）和外积（叉乘），考虑到向量外积模的大小与向量夹角呈正相关，故通过计算外积来得到向量方向差值 θ：

首先先将两向量单位化

考虑到实际情况中理论向量 v^ 和实际向量 v 偏差角不会超过45°，而当θ在±45°内时，sinθ 与θ的值非常接近，因此上式可进一步简化为：

得到向量偏差后，即可通过构建PI补偿器来计算角速度补偿值

其中，比例项用于控制传感器的“可信度”，积分项用于消除静态误差。KP越大，意味着通过加速度计得到误差后补偿越显著，即是越信任加速度计。反之KP越小时，加速度计对陀螺仪的补偿作用越弱，也就越信任陀螺仪。而积分项则用于消除角速度测量值中的有偏噪声，故对于经过零篇矫正的角速度测量值，一般选取很小的KI。最后将补偿值补偿给角速度测量值，带入四元数差分方程中即可更新当前四元数。
最后还需要通过四元数反解出欧拉角

五.传感器数据融合

磁力计数据融合

六.滤波

七.代码分析与融合