四旋翼姿态解算之理论推导

今天转载一篇咱们队长写的关于四旋翼的博客！

转载声明：转自http://www.cnblogs.com/xuhongbin/p/6538345.html

 

四旋翼姿态解算——基础理论及推导

对于每一个像我同样入坑四轴飞行器不久的新手来讲，最初接触也颇为头疼的东西之一就是四轴的姿态解算。因为涉及较多的数学知识，不少人也是以为十分头疼。因此，我在这里分享一些我学习过程当中的笔记和经验，以便你们学习。

两个坐标系：
首先，在一个姿态航向参考系统（简称AHRS）中，咱们要定义两个坐标系：导航坐标系 n 和载体坐标系 b 。导航坐标系 n 指的是以地球为参考的坐标系，定义为东北天右手直角坐标系；载体坐标系 b 则是以四轴飞行器自身为参考的坐标系， 也定义为右手直角坐标系，取飞机向前的方向为 Y 轴正方向，取飞机向右的方向为X轴正方向，取飞机向上的方向为Z轴正方向。

四元数、欧拉角、方向余弦：
在百度百科中，欧拉角是这样被描述的：用来肯定定点转动刚体位置的3个一组独立角参量，由章动角θ、旋进角（即进动角）ψ和自转角j组成，为欧拉首先提出而得名。简单点来讲，就是：绕Z轴旋转为偏航角（YAW）ψ，绕Y轴旋转为横滚角（ROLL）θ，绕X轴旋转为俯仰角（PITCH）φ。

绕Z轴旋转ψ角（YAW）：

定义导航坐标系 n 中某一点的坐标为（x，y，z），使用矩阵表示为：。设该点在载体坐标系中坐标为（x’，y’，z’），使用矩阵表示为：。对于该任意点，易获得两个坐标系下坐标之间的关系：。
表示成矩阵的形式以下：

同理可得：
绕Y轴旋转θ角（ROLL）：

两个坐标系下的转换关系：

绕X轴旋转φ角（PITCH）：

两个坐标系下的转换关系：

由前面的结论能够获得进过三个欧拉角的旋转，获得导航坐标系下的向量与旋转后的载体坐标系下的向量之间的关系：

给出由到的坐标变换矩阵：。
因此能够获得用欧拉角表示的坐标变换矩阵：

这样咱们就获得了使用欧拉角表示的坐标变换矩阵，这个公式先放在这里，等会再用。

接下来咱们来看看四元数：
先看看百度百科中对四元数概念的介绍：（四元数-百度百科 连接：http://baike.baidu.com/link?url=oQzRKzHEoKP6SgD9_qhBZKmsTU5NgSLqtxg4pXtw2hN0dXJQ9v9m11aNVW_M64b7vCeQ_9VNsKXQSnl2rR_FK0NVvGKcIF05d-N2_R9vQ0SLtrzKx9WQ19hHUvbYmd1z）
四元数是简单的超复数。 复数是由实数加上虚数单位 i 组成，其中i^2 = -1。 类似地，四元数都是由实数加上三个虚数单位 i、j、k 组成，并且它们有以下的关系： i^2 = j^2 = k^2 = -1， i^0 = j^0 = k^0 = 1 , 每一个四元数都是 一、i、j 和 k 的线性组合，便是四元数通常可表示为a + bk+ cj + di，其中a、b、c 、d是实数。
对于i、j、k自己的几何意义能够理解为一种旋转，其中i旋转表明X轴与Y轴相交平面中X轴正向向Y轴正向的旋转，j旋转表明Z轴与X轴相交平面中Z轴正向向X轴正向的旋转，k旋转表明Y轴与Z轴相交平面中Y轴正向向Z轴正向的旋转，-i、-j、-k分别表明i、j、k旋转的反向旋转。

这里已经讲得比较清楚了，咱们能够把四元数当作一个常数加上一个三维矢量，即

四元数的乘法运算：
对于任意一个四元数来讲，q0、q一、q二、q3都是实数，i、j、k为互相正交的单位向量，也是虚单位。
知足乘法关系以下：

举例：
假设有两个四元数，和。
则这两个四元数相乘结果为：

将上面的运算表示成矩阵形式：
设两个四元数Q和P的乘积为四元数。
则有：

或者
从M(Q)中，第一列为四元数Q自己，第一行为四元数Q的共轭的转置，无论第一行和第一列，咱们能够提取出一个3*3的矩阵VQ，称其为M(Q)的核。

同理可得，M(P)的核VP：

四元数的相关知识的准备差很少完成了，下面开始推导四元数的公式：
咱们定义一个四元数，用来表示从导航坐标系n和载体坐标系b之间的旋转变换：


代入求得：

能够获得旋转矩阵的数学关系：

到这里咱们就推出了使用四元数表示的旋转矩阵：

前面使用欧拉角也导出了一个旋转矩阵：

联立二者对应项相等，求解方程组便可。解方程的步骤就省略了，直接写出结果。
令
推出结果：
前面咱们用欧拉角推导出来的旋转矩阵也能够叫作方向余弦矩阵（DCM），使用的是Z-Y-X顺规，不作赘述，有兴趣能够再去查找相关资料。
这里咱们代入方向余弦矩阵对应项的值求出欧拉角与四元数的关系，并作一些三角函数的变换整理获得下面的形式：

上式是欧拉角用表示四元数的公式。
仍是由方向余弦矩阵（DCM）能够获得：



这四个公式的意义是，给出了四元数与欧拉角之间的关系，咱们能够很方便地使用这几个公式将欧拉角与四元数相互转换。还须要注意一点，由于方向余弦矩阵的定义不一样，对应的欧拉角旋转方式不一样，公式也会不一样。

到此结束。 这些是我前段时间的学习笔记，最近才开始整理。但愿能对更多人的学习提供帮助。欢迎你们互相交流指正。