写此篇目的主要是总结三轴云台姿态解算的学习过程，所参考资料在这进行罗列，并对一些小问题给出解释。会参考已有较多的博客、b站视频、学术论文，主要给大家罗列一个学习清单，节省时间，快速入门姿态解算算法。由于本人电控专业，后续一些解释会与其结合，只谈自己的理解，希望大家多包涵。
先来说明一下，我们要知道姿态解算的最终目的是要得到三个角度：偏航角、俯仰角、横滚角。进一步用于闭环控制，可以用于三轴云台的最外环--位置环控制，用于飞控的话就是对应的也是如此，都是此类控制思路。
一、基础知识
首先推荐b站up主-单三排的视频，可以对姿态解算中用到的基础知识姿态角、欧拉角与旋转矩阵包括四元数有个初步了解。
无人机DIY入门系列教程：(三）姿态解算上：姿态角、欧拉角与旋转矩阵_哔哩哔哩_bilibili
在视频给我们介绍了基础知识外，还给我们引出了加速度计计算部分姿态角（俯仰角、横滚角），这里呢提一下，加速度计的应用是对陀螺仪的改进，换句话说就是陀螺仪可以直接把三个姿态角都测出来，但是呢测得不精确！不精确！因此我们要进行修正，其中俯仰角、横滚角这两角呢用加速度计修正，还有一个偏航角是用磁力计修正，这个后面我们会提。既然提到硬件设备了，下面放几个链接，用于介绍他们的工作原理。
IMU原理及姿态融合算法详解_imu姿态解算_木独的博客-CSDN博客
另外呢，看完up主-单三排的视频可能会有点懵，坐标系变换我理解，这东北天、右前上坐标系可没听说过，还有“特定顺序”？会不会有好多特定的顺序？在用具体坐标变换公式时我怎么选呢？别慌，下面博客介绍的清清楚楚的！还要注意的是，具体项目时还要参考芯片手册来选择。
北东地/东北天两种导航坐标系与姿态转换_姿态角坐标系变换_zht2370201的博客-CSDN博客
无人机飞控算法-姿态估计-欧拉角-旋转矩阵-四元数_Choopper的博客-CSDN博客 注意里面提到的坐标系对应、左乘、右乘、内外旋的解释。以及“姿态角属于欧拉角的一种特殊形式”，便于我们下面讲解。
相信看完以上内容，大家对基础知识已经有了了解，这里总结一下：
姿态解算用的传感器：陀螺仪--输出角速度、加速度计--输出加速度、磁力计--输出磁感应强度。
欧拉角、旋转矩阵、四元数等都是用来描述旋转的，它们三者是并列的，具体用什么来描述我们要进行选取。
讲到这里，我们是不是还没有摸到姿态角呢，怎么求取它呢？下面我们继续。
二、姿态角求取
我们前面提到了检测所用到的传感器，陀螺仪输出角速度，那我们直接积分的值不就是角度了么，没我们想象的这么简单哦，关于用欧拉角法解算姿态角，可参考下面博士论文《多旋翼无人机的姿态与导航信息融合算法研究》3.2.1小节。但是由于计算过程复杂，同时还存在万向锁的问题，一般不用该方法。
万向锁的解释：
无人机飞控算法-姿态估计-欧拉角-旋转矩阵-四元数_Choopper的博客-CSDN博客这个看不明的话还有一个视频：https://v.youku.com/v_show/id_XNjk1MTkzMTM2.html?playMode=pugv&frommaciku=1
那我们看看上面提到的内容中，哪里出现姿态角呢呢？是不是在旋转矩阵中出现了呢，绕每个轴的旋转角度都在对应的矩阵中出现，这样的话我们能否通过这些矩阵把姿态角求解出来呢？我们看在up主-单三排视频中是不是通过加速度输出的机体坐标系下的三轴加速度（Ax、Ay、Az）、世界坐标系至机体坐标系的旋转矩阵、世界坐标系下的三轴加速度（0、0、g）计算出了俯仰角和横滚角。但这个是用来修正陀螺仪的，那陀螺仪如何通过其角速度来计算姿态角呢？
陀螺仪输出的是机体坐标系的角速度，想要采用加速度计的计算方法是不是缺一个已知量啊（加速度计求解姿态角可以看作是已知机体坐标系和世界坐标系的三轴加速度求解旋转矩阵，旋转矩阵中的姿态角自然而然就求解出来）但是陀螺仪呢？世界坐标系下的角速度不知道呀，相当于三个变量只有一个已知量，两个未知量，这没法解呀。
怎么办呢？这就要引入旋转的另外一种描述方式了-----四元数。四元数描述旋转也有一个对应的矩阵，这样的话要求的姿态角藏在旋转矩阵中，如果我们知道四元数这个矩阵，两个矩阵各行各列一一对应的话，姿态角就可以求解出来了。
对应关系：
问题又来了，四元数怎么来了？这里用到了有两种方法：
一阶龙格库塔法更新四元数：upadte四元数（1）-----一阶龙格库塔法_WELLSTONE_CHENG的博客-CSDN博客
博士论文《多旋翼无人机的姿态与导航信息融合算法研究》采用四阶龙格库塔法。
不论采用哪种方法，更新四元数的过程中都用到了角速度，这样是不是就完成了陀螺仪的姿态角求解啊。
下面补充磁力计的姿态角求解：四元数姿态解算中的地磁计融合解读_王尼莫的博客-CSDN博客
这里要注意下：东北天，重力是[0，0，-1]，北东地中是[0，0，1]；磁场在东北天中是[0 by bz]，在北东地中是[bx，0，bz]
三、融合算法--互补滤波
上面我们也提到了，单一的陀螺仪会有很大误差，如何弥补就用到了加速度计和磁力计，上面我们介绍各自求取姿态角的方法，那在什么时候进行弥补呢（这里应注意误差不可能全部去除，只能弥补），是各自把姿态角都求取出出来以后在进行么？
我们注意到陀螺仪解算姿态角时用到了四元数，四元数更新又与角速度有关，我们直接根据补偿这个角速度就好啦，其实就是保证旋转矩阵（四元数表示的那个）的准确性。
电赛四轴小结——姿态解算篇【mahony互补滤波算法】_mahony姿态解算_linzs.online的博客-CSDN博客
（这里gx、gy、gz表示角速度）
这里还要提及一下两个小问题：
1、什么用PI控制就可以实现互补滤波：如果Kp、Ki为0相当于不进行修正，即完全相信陀螺仪的数据。可参考第十二讲下、姿态结算代码实现_哔哩哔哩_bilibili
2、叉乘为何误差以及怎么计算：
互补滤波：为什么用向量叉积PI滤波补偿？_不会写代码的美年达的博客-CSDN博客
到这里就讲完啦，希望可以给大家带来一些帮助，在短时间内完成算法的理解，如有不当之处，恳请指正，若涉及侵权，请告知，俺及时删除。
互补滤波和梯度下降图解：
Matlab程序下载：
https://pan.baidu.com/s/1CdSllOtQ2lPznwvtjA3Fog?pwd=i5qk 提取码：i5qk
最后给大家推荐几个大佬的专栏，要赶紧看哦，发现有一篇已经变为VIP文章喽。
Pixhawk之姿态控制篇（1）_源码算法分析（超级有料）_pixhawk结合机器视觉的已开源的代码__Summer__的博客-CSDN博客
https://blog.csdn.net/qq_27114397/category_6520034.html?spm=1001.2014.3001.5482
https://blog.csdn.net/goolyoh/category_9812940.html?spm=1001.2014.3001.5482
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
