【姿态解算初体验】（一）终于理解，飞行姿态控制中，为什么要用旋转矩阵的方式不停地转换来获取姿态误差

终于理解，飞行姿态控制中，为什么要用旋转矩阵的方式不停地转换来获取姿态误差

最近在学习飞行姿态控制的原理部分，看了很多博客，一直没有明白控制飞行姿态时为什么要用旋转矩阵相乘的方式来求姿态误差。

现在对该问题做以下阐述，考虑到学习总是由浅入深的过程，大部分博客都是直接怼着四元数旋转矩阵等开始讲解，因此我将以自身（入门者）的角度，通俗地讲解其思路过程，同时引申出其他的关键知识点，但并不介绍其数学模型。
以下仅帮助理解，不当做正确的系统教程。

飞行器姿态控制基本原理

相信大家在了解的过程中，都知道了飞行控制时采用的是PID控制器，这就涉及到期望值以及当前状态值的获取。

期望值获取

这个简单，无非有两种情况。

一种规划好的路径，让飞机自动飞行。那么这个期望值的坐标就是建立在NED坐标系（地面坐标系，北x 东y 地z，下面统测统称E系）上的。

另一种是遥控模式，期望值就是建立在机载坐标系上的（机头x，右翼y，机肚z，下面统称B系），因为这个时候告诉飞机的就是前后左右上下的指令，对应B系。

坐标系详细介绍可搜索关键词【飞行器姿态&坐标系】

当前值获取

通过机载传感器获取姿态信息，主要包括：

1、陀螺仪：获取当前时刻的角速度值，通过不断积分可以获取基于E系下的三个轴的角度（即roll，pitch，yaw）。
注：会有积分误差，短时间内精度较好，长时间下误差大需要通过其他传感器来不停校准。

2、加速度计：获取基于B系下的三轴加速度值，通过与重力之间的关系，可以计算出E系下的roll和pitch。
注：短时间内的噪声较大，但不会有积分误差，可用于校准陀螺仪的积分误差，具体校准方法涉及到关键词：【姿态解算&互补滤波&加速度计&陀螺仪&卡尔曼滤波】

3、磁力计：获取基于B系下的三轴磁场强度，主要目的是获取飞机的偏航角（可以通过计算得到E系下的yaw）用于校准陀螺仪。

好的先不管坐标旋转矩阵那套东西。理论上，我们现在可以获取到在E系下较为可靠的roll，pitch，yaw。也即我们现在获取到了飞机当前的绝对姿态（当前值）。

计算误差

有了当前值以及期望值就能得到误差，导入PID进行自动控制了。

最初思路

现在有了E系下的飞机当前值【r,p,y】期望值【rt,pt,yt】(t for target)。那我想这误差肯定好算啊，只需要相减不就行了，即：
err = 【rt - r, pt - p, yt - y】
这不就有误差了吗，然后把这个误差值告诉飞机，你需要绕的你x轴旋转（rt - r）度 ，绕你的y轴旋转（pt - p）度， 绕你的z轴旋转（yt - y）度，这不就可以了吗？

上述思路带来的问题

图中黑色为E系，黄色为机体系记为B系，绿色为期望姿态记为T系。

根据上图可以思考一个问题，如果给出上述思路的三个误差角的值，假设为【10,20,30】。通过这三个值我该如何在E系下，把B系旋转到T系呢？
如果我们根据欧拉角的思路，按照xyz的顺序旋转，先让B绕E系的x轴旋转10度，之后分别再绕yz轴旋转，最后便能得到我们的T系，也即期望的姿态。

那为什么不能把这个误差角度叫告诉飞机，让他这样旋转呢？关键在于，我们之所以能通过这个误差角进行旋转得到期望姿态，是因为我们绕着的是E系的坐标轴旋转得到的，然而飞机的旋转则是需要绕着B系的轴旋转。

这就好比遥控模式，给飞机的指令永远是基于B系的，也即增量式。比如飞机只能听懂，你需要向前100米，你需要向右偏转20度。而不能听懂，你需要走到经纬度（10,10）的位置，你需要旋转到（10,20,30）的姿态。

实际应用中，我们之所以仍然能输入绝对位置，让飞机汽车等自动到达，是因为飞机通过内部解算将这些绝对值信息，转换成了自身的增量值信息。

回题，而这个内部解算就是要将上述E系误差【10,20,30】转换为飞机能听懂的增量。也就是需要绕着B系的轴进行旋转，而增量的旋转角就不再是【10,20,30】了。那么如何算出飞机需要的增量呢，这个时候就引入了旋转矩阵。

解决方案

1、我们已经有了当前时刻在E系下的的姿态角，以及期望的姿态角；
2、可以计算出E旋转到B系（黄色坐标系）的旋转矩阵Re_b，以及到T系的旋转矩阵Re_t。（旋转矩阵的运算可搜索关键词【旋转矩阵】）；
3、再利用Re_b.transpose(转置)*Re_t，就得到了Rb_t。（运算规则搜索关键词【旋转矩阵】）；
4、有了Rb_t，我们就能知道了怎么绕B系旋转到T系，即通过反向解算Rb_t，便可以获取基于B系下的【r,p,y】，也即获取到了飞机的增量角；
5、再将该误差传给PID，就能完成自动控制了。

延伸

知道了大体过程后，就可以逐步攻克了，下面给出目前总结出的学习顺序：
1、了解上述传感器的基本原理和传感器的输出值；
2、如何将多个传感器的值融合到一起，涉及到互补滤波，科尔曼滤波等；
3、旋转矩阵如何根据传感器的值不断更新，也即怎么实时获取较为准确的旋转矩阵。
4、还没进一步学习确定后更新。