有人问我关于四元数姿态解算算法的分析，每次都解释好久，今日空闲，特发一帖，供大家参考。本分析将结合程序，分析姿态解算思路，由于能力有限，难免有错误之处请谅解，同时希望能够抛砖引玉，得到大神指点。再此感谢圆点博士提供资料参考。

为尊重原创，转载请注明原出处，谢谢。

四轴之互补滤波与四元数算法最详细分析

原创：冰三点水

关键字：姿态解算 互补滤波 四元数

下面进入正题。

第一部分 传感器说明

         通常我们开源小四轴使用mpu6050姿态传感器，可以输出三轴加速度和三轴角速度数据。角速度数据就是x、y、z三个轴的旋转角速度。而加速度是沿三个轴的加速度，这个加速度计测量的是比力。这里对于加速度计有个重点，我们想象这样一个模型，在太空中有一个盒子，盒子里连着三轴弹簧，力感应球位于弹簧连接点，在外太空失重时，三条弹簧伸缩量为0，x = y = z= 0；

当在重力场内，盒子静置与地面时，力感应球收到重力的吸引孤儿有一个轴的弹簧有伸缩变化，或者说当盒子（加速度计）静置时会有一个输出，大小等于重力，这就是我们说的比力。（感谢第七实验室的图片）

在四轴的惯性导航中，我们认为四轴是平滑运动，也就是说我们忽略了加速度计测量的和运动带来的影响，而认为加速度计测量的就是重力（比力）不包括和运动，这是互补滤波是局限性所在，补偿方法请自行分析。（感谢投笔从容提供思路）

         实际上，我们单单通过三轴角速度就可以得到四元数，进而将四元数转换成欧拉角，也就是姿态角，问题是陀螺仪会有积分漂移，得到的角度会有误差，这时候我们就引入加速度计对陀螺仪数据进行校正，以得到更准确的角度。不过这时yaw，也会存在误差，在误差不能忽略的使用情况下，我们就需要用磁力计了。

第二部分 四元数

    二维平面内的旋转可以用复数表示（即a+bi），三维空间内的旋转需要用四元数（即a+bi+cj+dk）来表示，而不是我们猜测的类似于三元数（a+bi+cj）表示。注意四元数表示的是一个旋转。如果我们得到了四元数，只要通过一个公式就可以将四元数转换到欧拉角，得到我们需要的姿态。那么怎么得到四元数呢？

    我们常听说四元数微分方程，这个方程的输入是角速度数据，我们只要输入角速度数据，就可以更新四元数数据。

上图为程序实现的对四元数微分方程的解。这个方程是怎么来的呢？由于机体的运动，四元数Q也是变化的，换句话说，q0 q1 q2q3是时间的函数。我们把四元数写成三角形式，

即

这里有角度量 ，然后对这个式子进行求导，然后进行一下相应的转换，这里提供一个四路，细节就不讲了。上面的程序是解这个方程的常用方法。

第三部分 四元数到欧拉角

         四元数到欧拉角有公式，对于一个固定的欧拉角旋转顺序有相应的公式。程序上实现起来比较容易

程序中的57.3是个转换系数，是弧度和角度的转换系数。

这个公式具体请参考秦永元老师的惯性导航。

第四部分 互补滤波

         到目前为止，我们已经可以通过四元数微分方程得到四元数，在通过四元数到欧拉角的转换得到四轴的姿态，但是为了得到更加准确的姿态，我们引入了加速度计。

在IMU程序中我们刚开始看到的往往是将加速度原始数据归一化。

归一化后的ax、ay、az的平方和为1，各自方向不变，也就是合向量的方向不变，模等于1，这是个重点。

建立小四轴坐标系

这个很简单，所谓的建立小四轴坐标系不过是把世界坐标系下的重力向量（0 0 1）转换到小四轴坐标系下而已。图就不画了，有想要的@我。

R为小四轴坐标系，b为世界坐标系，是方向余弦矩阵

这个方向余弦矩阵是怎么来的呢？

请参考惯性导航或下面链接

http://www.doc88.com/p-893241688972.html

互补滤波这个部分到目前为止还没进入正题。（楼主已经写的好烦啊。。。。。。）

目前的进度是建立了小四轴坐标系。

接下来是重点！

将刚才转移到小四轴坐标系下的重力向量和机体加速度计测量的重量向量叉乘！

得到一个误差向量。这个误差向量是有物理意义的。叉乘等于|A|*|B|*sin(角度),转移到小四轴坐标系的重力向量模为1，加速度计测量的重力向量经过归一化也为1，那么叉乘的结果之与角度有关，并且角度越大，误差向量的莫越大！归一化的目的在这里体现。向量的叉乘还包括方向信息，还是一个向量，有人认为，在固定的采样计算时间内的按一定方向转过这些误差向量，也可以看作成角速度（来自黑暗骑士）这读者自己揣摩吧。总之，我们就是通过这个误差向量来修正陀螺仪数据的。

通过PI运算来融合陀螺仪和加速度数据。

如果Kp = Ki = 0就是完全相信陀螺仪数据。

将融合后的角速度输入到四元数微分方程 就得到四元数，进而得到欧拉角。

至于为什么四元数又归一化处理，为因为引入误差向量后四元数失去了规范化，请参考惯性导航。

最后在说两句

得到的原始角速度数据需要乘一个系数转换成实际的角速度（弧度每秒），而加速度不需要，因为加速度归一化后与大小无关了。

分析完毕。

笔者能力有限，希望能够给各位带来启发，而不是误人子弟，同时欢迎大神指正其中的错误。

为尊重原创，转载请注明原出处，谢谢。

四轴之互补滤波与四元数算法最详细分析