姿态解算 四元数、方向余弦、欧拉角、Mahony滤波、四轴

姿态解算 四元数、方向余弦、欧拉角、Mahony滤波

说明：本文只是做了一些总结，需要一些对这方面的基础概念的了解。

一般人千万不要试图去深入探讨四元数

1. 方向余弦矩阵

方向余弦矩阵是使用欧拉角（pitch,roll,yaw)对机体坐标系（b系）和地理坐标系（R系）的旋转的描述，也就是说，一个机体坐标系的向量，乘上这个方向余弦矩阵，就可以转化为一个地理坐标系的向量（对调也一样，就是这个意思）。也就是说方向余弦矩阵是对坐标系旋转的一种描述。

方向余弦矩阵如下：

其中，ψ是绕Z轴旋转的角，也是yaw角；θ是绕Y轴旋转的角，也是pitch角；φ是绕X轴旋转的角，也是roll角

显然，姿态解算的终极目的是得到这三个角度，也就是欧拉角。

2. 四元数旋转矩阵

四元数可以用来描述坐标系的旋转，因为各种各样的原因，比如运算速度、死锁等，用四元数去解算出欧拉角是更加方便的办法。

四元数旋转矩阵：

这个矩阵与方向余弦矩阵是等价的，知道了了四元数旋转矩阵，通过对应关系就可以求出欧拉角了，姿态解算绝大部分都是对四元数、角速度、加速度进行操作，只有在最后一步才将两者对应起来求欧拉角。

3. 迭代求解四元数（四元数微分方程求解）

需要深入了解四元数后才知道这个微分方程怎么来的，而且还运用了龙格库塔法解微分方程才能得到以下迭代求解形式，一般来说知道这个形式就可以了，会用即可。总之，利用四元数微分方程+龙格库塔法可以得到下式：

最为关键的是，这个迭代求解的形式居然会引入三轴角速度wx、wy、wz（也就是陀螺仪的数据）。即便没有深入知道四元数，但是知道四元数可以看做是一种用来描述坐标旋转的方法，旋转跟角速度扯上关系也是符合感觉的。四元数的初始值不会影响迭代，取0即可。注意这里存在一个近似的角速度积分的形式。

由于陀螺仪存在温漂，就是说角速度的数据总会比实际的数据大一点或者小一点，温漂导致这个大多少小多少是不确定的，有时会一直大一点，有时会一直小一点，有时大一会儿小一会儿交替地来，所以长期这样积分会导致误差越来大，必须消除这种误差才能保证四元数长期正确性。并且陀螺仪角速度的累计误差会反映到四元数中。

4. 叉积补偿滤波（或称互补滤波）

这里就是Mahony所做的贡献，巧妙的利用了加速度计的数据、地球重力G和四元数的旋转矩阵、叉积、PI控制器来将陀螺仪的误差给消除掉，计算量非常小，并且通过四元数的旋转特性将滤波完美地融入到四元数姿态解算的整个过程中。

重力加速度G是一直存在的，并且加速度计测量的数据就是重力加速度的分量（忽略芯片自身的加速度），G所处的坐标系显然是地理坐标系。如果求出了四元数，那么就可以通过四元数将地理坐标系下的重力向量（0,0,G)旋转到机体坐标系中，这个应该就是机体的加速度（注意，这时跟加速度计测量无关）。

此外，我们还有加速度计这个传感器可以比较准确的测量到机体的加速度。加速度计的特点是静态性能好，比如如果加速度计时静止的，那么直接利用加速度计测量的加速度就可以准确地测量到角度了，但是加速度计动态高频特性不理想。这里我想表达的是，从长期来看，加速度计的数据是非常准确地，并且加速度计的数据几乎不会有温漂的影响。所谓互补就是说陀螺仪的短期积分跟准确，加速度的长期数据跟准确，综合取两者之长来确定最终结果。

这样我们就有了两种加速度的数据了，一个加速度计的实测值，另一个是可以说是利用旋转的预测值（预测值这个词其实不是很合适），陀螺仪的角速度积分漂移产生的误差也体现在了这个预测值中，通过求出预测值与实测值的误差，将这个误差补偿到新的角速度数据中就可以消除角速度的温漂。求误差的方式是实测值和预测值两向量的叉积，补偿的方式是PI控制器

通过旋转求出重力加速度在机体坐标系的分量：

注意这里的旋转矩阵是地理坐标系到机体坐标系的旋转矩阵，其实是机体坐标系到地理坐标系旋转矩阵的转置。这里的是1还是用9.8不影响，因为后面还会用到PI控制器。

为什么可以用叉积求出误差？两个向量如果是完全重合的，其叉积结果的模是0，如果有夹角，则其模不会是0，并且夹角越大，其模也会越大。测量值与重力G旋转过来的值得误差为：
$$\begin{gathered} e_x=a_y\ast v_{bz}-a_z\ast v_{by} \\ {e}_y=a_z\ast v_{bx}-a_x\ast v_{bz} \\ {e}_z=a_x\ast v_{by}-a_y\ast v_{bx} \end{gathered}$$
三维向量叉积使用行列式运算非常方便。其中，ax、ay、az是加速度计测量的数据，ex、ey、ez是两者在三轴上的误差，然后再将通过PI控制补偿到陀螺仪测得的角速度当中，为什么可以这么补偿就完成了滤波前面已经说清楚了，这是最难理解的一个点。

ExSum += KI*ex;
EySum += KI*ey;
EzSum += KI*ez;
//忽略了积分封顶，其实很多情况下只用P控制就可以了，自己去实践
gx += KP*ex + ExSum;  //gx gy gz原值是陀螺仪测量的角速度数据
gy += KP*ey + EySum;
gz += KP*ez + EzSum;  //KP的范围非常大，这个参数非常好调

这样就得到了补偿后的角速度数据，然后就迭代求四元数微分方程就可以了。整个过程看源码吧

可以结合B站上一个视频看，这个视频讲的很好：https://www.bilibili.com/video/BV1px411p71z

后记：

1. 其实用加速度计得到的数据直接做个反三角就可以得到角度了，但是为什么不用？一般对实时性要求不高的应用用三轴加速度计就可以得到倾角了，但是加速度的响应速度不够快！不能直接将其用在无人机等对实时数据要求非常高的场合当中
2. mahony滤波这个滤波不是平滑操作！而是借助加速度计的数据校正陀螺仪的低频误差（温漂），还应该使用低通滤波进行平滑（可以使用MPU6050内部的低通滤波器，看手册）。如果自己做低通的话务必考虑低通给数据带来的延迟！必须满足高速实时