mpu6050 重力加速度_姿态解算进阶之加速度姿态解算

在姿态解算入门系列里 Sugar 以初中数学为基础展开了用加速度计的姿态解算方法。上一篇《姿态解算进阶之理解旋转矩阵(也称方向余弦矩阵)》进行了一次理论进阶。本篇 Sugar 用进阶的理论重写一下用加速度计做姿态解算的方法，然后与入门级算法对比一下结果有哪些改进。

姿态角定义的悄然改变

在《MPU6050 姿态解算系列一：加速度姿态解算》一文中 Sugar 有说明：
1、横滚角 roll 指：重力加速度与“X-Z 平面”的夹角；
2、俯仰角 pitch 指：重力加速度与“Y-Z 平面”的夹角。
通过我们本文的算法升级，这样的定义也会有所改变，具体怎么个变法读者掌握本文算法后自然会知道。这里 Sugar 放前面先说，目的是起到一个思维导向作用，防止思维陷在之前的定义里出不来从而影响对本文算法的理解。
本文姿态角的定义将采用飞控上常用的定义方法：绕 X 轴转是横滚角 Roll、绕 Y 轴转是俯仰角 Pitch、绕 Z 轴转是偏航角 Yaw。

加速度计姿态解算进阶

在《姿态解算进阶之理解旋转矩阵(也称方向余弦矩阵)》中 Sugar 介绍了旋转矩阵。既然用旋转矩阵能对三维空间内的向量进行任意旋转，那么我们不难知道：加速度计在各个轴上的分量就是通过在“竖直的加速度向量”上施加旋转而得到的。

在姿态解算上我们关心的是向量角度而非向量模长，为了能够顺利的使用反三角函数做姿态解算，我们必须首先对加速度计数据进行单位化，保证反三角函数有实数解。假设上图的红色箭头(直线的那个)是通过对单位向量[0;0;1]施加三维旋转得到的。为了便于表示，我们做如下约定：

% cr:cos(roll)  sr:sin(roll)% cp:cos(pitch) sp:sin(pitch)% cy:cos(yaw)   sy:sin(yaw)from_euler_xyz = [ cp*cy, cy*sp*sr - cr*sy, sr*sy + cr*cy*sp][ cp*sy, cr*cy + sp*sr*sy, cr*sp*sy - cy*sr][   -sp,            cp*sr,            cp*cr]

在这样的约定下，我们有：

[加速度计单位化后的读数] = from_euler_xyz * [0;0;1];

记加速度计单位化后的读数为[Ax;Ay;Az]，将上面方程的右侧展开，可得：

 Ax = sr*sy + cr*cy*sp Ay = cr*sp*sy - cy*sr Az =            cp*cr

这个解告诉我们一个现象：在这个旋转矩阵的作用下，单位向量在 Z 轴上的投影只与横滚角 roll和俯仰角 pitch有关。深刻理解这个现象是本篇算法的一个很重要的关键点。下面 Sugar 以roll 30 度、pitch 30 度为例，用几张动图形象地表述一下这个现象：

对于这个 3D 旋转我们看一下X-Y 方向的投影，如下：

我们看到一个形状不变的三角形在转动，说明：旋转过程过中所有姿态的 roll 和 pitch 都是相同的。我们再来看一下X-Z 方向的投影，如下：

上图表明上述 Ax、Ay、Az 的解所描述的是：一组法向量与竖直方向夹角相等的姿态。下面 Sugar 以剑影分光术的形式更清楚地表现一下这个现象，为了不让众多剑影的出现太突然，先用一成功力弄出两个剑影：

然后提高功力，输出一圈剑影：

已经眼花缭乱了，但红色的各个姿态的法向量有一个明显特征：伞骨状。为了体现量的特点，Sugar 选用了 pitch 60 的一组姿态且法向量在姿态的两侧都是单位向量，我们看一下X-Z视图表现出的量的特点：

又是动图又是剑影分光术的，Sugar 要表达的一个重点就是：在 from_euler_xyz 旋转矩阵的作用下，各个偏航角不同的姿态对应的横滚角 roll 和俯仰角 pitch 都是跟偏航角为 0 的姿态是一样的。因为用 IMU 无法估算偏航角 Yaw，不妨随意标定一个姿态是 Yaw 为 0 的姿态，这样就有 sy = 0、cy = 1，这样 Ax、Ay 和 Az 就是：

 Ax = cr*sp Ay = -sr Az = cp*cr

至此，我们就得到了用加速度计解算姿态的新公式：

r = -asin(Ay)p = atan2(Ax,Az)

进阶算法效果有什么改善

Sugar 在 MATLAB 同时绘出进阶算法和入门级算法的姿态对比图，主要代码如下：

效果对比如下：

为什么 pitch 是正负 180 度

我们知道反正切函数atan()的值域是-90 度 ~ 90 度，这个公式算出的怎么会超过这个值域范围的？原因是：这里我们使用的是四象限反正切。
四象限反正切函数的特点：

四象限反正切并不是 MATLAB 独有的，ArduPilot 代码里也是用四象限反正切函数做姿态解算的。