无人机姿态估计1：四元数和姿态角到底什么关系？

今天来说说消费级无人机的姿态估计方法，为什么说无人机的姿态是估计出来的呢？因为目前几乎所有的消费级无人机都没有直接测量姿态的传感器，无法直接得到姿态角。无人机是靠IMU（惯性测量单元）来解算出姿态角的。

IMU又分平台式和捷联式，这里主要说应用于消费级无人机上的捷联式的IMU，捷联式IMU包含加速度计和陀螺仪，输出的是角度增量和比力。所以无人机的姿态角是从陀螺仪给出的角速度积分得到的，无人机姿态解算目前最常用的是四元数法，四元数的微分方程如下：

四元数微分方程

由于四元数能表示一个三维空间的旋转，所以这里我们用一个四元数来表示无人机的姿态角，那角度又是怎么从四元数中得到的呢？实际上角度的计算和四元数之间没有固定的计算公式。因为姿态角和机体的旋转顺序有关，和机体的旋转方向有关，和机体的坐标系定义也有关。

旋转顺序对结果的影响

我们通过下面的例子，旋转一本书，看看旋转顺序对最终结果的影响，我们先定义坐标系，如下图：

坐标定义，Z轴向里

我们将书绕每个坐标轴旋转90度，旋转方向以右手定则为准。先按照X-Y-Z的顺序旋转：

X-Y-Z顺序旋转

最后是书背面向我们。然后我们交换一下前两项的旋转顺序，按照Y-X-Z的顺序旋转：

Y-X-Z顺序旋转

最后是书底面向我们。可以看出，其他一样，只是旋转顺序不同，得到的结果就不同，那么最终姿态角的计算方式就不同。所以我们就要定义好飞机的坐标、旋转顺序和方向。

坐标定义与顺归定义

那么怎么定义坐标、旋转顺序和方向呢？一般没有固定的方式，但是都会遵循NASA（美国国家航空航天局）制定的顺归方向，顺归方向有很多，比如X-Y-Z，X-Z-Y等。现在很多的开源飞控的顺归方向是Z-Y-X，旋转方向遵循右手定则，导航坐标系一般选择北-东-地（NED）的坐标系。

我们定义X轴沿无人机的纵轴指向机头，Y轴沿无人机的横轴指向右翼，Z轴垂直于XY轴构成的平面指向无人机下方。

导航坐标系X轴沿地球经线指北，Y轴沿地球纬线指东，Z轴垂直于XY轴构成的平面指向地心。

机体坐标系定义

无人机姿态角是在导航坐标系下定义的。三个姿态角分别是：俯仰角，滚转角，偏航角。水平面我们定义为导航坐标XY构成的平面。

俯仰角定义为机体坐标系X轴与水平面的夹角；滚转角定义为机体坐标系Y轴与水平面的夹角；偏航角定义为机体坐标系X轴在水平面的投影和导航坐标系X轴之间的夹角。

那么姿态角又是怎么和四元数联系起来的呢？这里需要一个中间者——方向余弦矩阵。

方向余弦矩阵

方向余弦矩阵也能表示旋转，从导航坐标系到机体坐标系可以通过三步来完成，下面是分别表示从导航到机体坐标系的旋转。φ是绕X轴旋转的滚转角，θ是绕Y轴旋转的俯仰角，ψ是绕Z轴旋转的偏航角：

三个旋转矩阵

按照Z-Y-X的方向顺归可以得到：

方向余弦矩阵

得到方向余弦矩阵，姿态角就能计算了。

姿态角计算

如果按照X-Y-Z的方向顺归，那么姿态角的计算方式就和Z-Y-X方向顺归的计算方式就不同了，先看方向余弦矩阵：

X-Y-Z顺归下的方向余弦矩阵

姿态角的计算就变成如下方式了：

姿态角计算

所以姿态角可以从方向余弦矩阵中得到，那么方向余弦矩阵怎么和四元数对应呢？他们的对应关系如下：

四元数和方向余弦矩阵

这个对应关系是固定的。不管选择使用哪种顺归方向，方向余弦矩阵和四元数的对应关系始终是这样。所以不同的顺归方向计算姿态角的方式一定不同。

四元数和姿态角的对应关系有了，就可以通过四元数来计算姿态角了（实际上也是通过方向余弦矩阵的对应关系），很多开源飞控只提四元数而不提方向余弦矩阵是不妥的。

然而如果只是使用陀螺仪积分得到的姿态角会产生漂移，因为陀螺仪存在温度漂移，角速度随机游走误差，积分过程会存在累计误差，所以时间越长，姿态角的误差会变得越来越大。下个章节说说无人机的姿态要如何修正。

---

我是一个工程师，分享嵌入式方向，无人机方向和自动控制方向的工程经验。