姿态解算（四）四元数 - 姿态解算步骤

四元数 - 姿态解算

在前面我们介绍了旋转的表示方法
以及，四元数的定义与几何意义

我们要获得无人机的飞行姿态必须把姿态传感器获取的数据，表示为我们上面说过的几种方式，我们在这里介绍用四元数来进行姿态解算的方法和相关知识：
我们首先来了解一下姿态解算的步骤：

1 获取四元数

1. 首先我们要获取四元数，这里我们利用龙格库塔公式求四元数的q0~q3，需要这个周期的角速度gx，gy，gz和周期时间deltaT
   （注：四元数可以初始化为q0=1，q1、q2、q3=0）：
   
   这里是一阶龙格库塔公式更新四元数的理解:（为什么通过角速度gx，gy，gz和周期时间deltaT可以更新四元数)
   
   

2 惯性单元测量值融合

2. 惯性单元测量值融合:四轴上安装陀螺仪，可以测量四轴倾斜的角度，由于陀螺仪输出的是四轴的角速度，不会收到四轴振动影响。因此该信号中噪声很小。四轴的角度又是通过对角速度积分而得，这可进一步平滑信号，从而使得角度信号更加稳定。因此四轴控制所需要的角度和角速度可以使用陀螺仪所得到的信号。通过上面的公式获取四元数的值是存在误差的，这个误差的主要来源于，三轴陀螺仪获取角速度信息，是要经过积分运算的，这个过程存在微小的误差，经过积分运算之后，就会形成积累误差。随时间延长而逐步增加，最终导致无法形成正确的角度信号；

3 如何消除误差呢？

3. 如何消除误差呢？可以通过上面的加速度传感器获得的角度信息对此进行校正。利用加速度计所获得的角度信息与陀螺仪积分后的角度进行比较，叫比较的误差信号进过比例放大后与陀螺仪输出的角速度信号叠加之后在进行积分。对于加速度计给定的角度，经过比例、积分环节之后产生的角度必然等于加速度计所获得的角度。由于加速度计获得的角度信息不会存在积分误差，所以最终将输出角度中的积累误差消除了。但是加速度计所产生的角度信号中会叠加很强的四轴运动加速度噪声信号，为了避免给信号对于角度的影响，因此比例系数应该非常小。这样加速度的噪声信号经过比例、积分后，在输出角度信息中就会非常小了。由于存在积分环节，所以无论比例系数多么小，最终输出的角度必然与加速度计测量的角度相等，只是这个调节过程会随着比例系数的减小而延长。

4 得到欧拉角

4. 进行完消除误差的处理之后，我们利用四元数的方向余弦矩阵和欧拉角的关系得到欧拉角。这里我们把程序的思路整理一下：
   
   
   
   
   

5 欧拉角法和旋转矢量法介绍

5. 为什么用四元数求解姿态呢？这里有一点关于欧拉角法和旋转矢量法的介绍：
   

6 姿态控制

6. 最后利用 PID 等相关控制算法进行无人机飞行姿态控制。（会在下一篇文章中介绍）。
   四元数的几何意义