飞行器姿态解算相关理论知识

最新推荐文章于 2025-02-26 16:26:40 发布

明.觉

最新推荐文章于 2025-02-26 16:26:40 发布

阅读量3.8k

点赞数 31

分类专栏：飞行器文章标签： 1024程序员节

本文链接：https://blog.csdn.net/2301_81731156/article/details/142493885

版权

飞行器专栏收录该内容

1 篇文章

订阅专栏

基础知识

三个重要参数

在飞行器中，飞行姿态是非常重要的参数，以飞机自身的中心建立坐标系，当飞机绕坐标轴旋转的时候，会分别影响横滚角（φ）、俯仰角（θ）及偏航角（ψ）。

偏航角（Yaw）：绕Z轴旋转，改变物体的朝向。
俯仰角（Pitch）：绕Y轴旋转，改变物体的上下倾斜。
滚转角（Roll）：绕X轴旋转，改变物体的左右倾斜。

以飞机原始状态创立坐标系（a），俯仰角、横滚角、偏航角分别为 (b) (c) (d)图所示。

我们只要想办法测量出基于原始状态 (a) 的三个姿态角的变化量，再进行叠加，就可以获知它的实时姿态。

坐标系

这里我们用右手定则来确定坐标系，如下图所示：右手食指指向OX轴正方向，中指指向OY轴正方向，大拇指所指方向即OZ轴正方向。

地理坐标系（地球固连坐标系）：该坐标系用于研究飞行器相对于地面的运动状态，通常以飞行器起飞位置作为坐标原点，Z轴沿当地地理垂线的方向(重力加速度方向)，XY轴可沿当地经纬线的切线方向，也可先让X轴在水平面内指向某一方向，Z轴沿重力加速度方向垂直于地面向下，然后按照右手定则来确定Y轴。

载体坐标系（机体坐标系）：载体坐标系以运载体的质心为原点，一般根据运载体自身结构方向构成坐标系，如Z轴上由原点指向载体顶部，Y轴指向载体头部，X轴沿载体两侧方向。上面说基于飞机建立的坐标系就是一种载体坐标系，可类比到汽车、舰船、人体、动物或手机等各种物体。

抽象来说，姿态是“载体坐标系”与“地理坐标系”之间的转换关系。

欧拉角

欧拉角用来描述刚体/移动坐标系在一个固定坐标系中的姿态。简单的说是使用XYZ三个轴的旋转分量,来描述一个6自由度的旋转。根据欧拉定理，刚体绕固定点的旋转可以看成绕该点的若干次有限旋转的合成（e），地球固连坐标系绕固定点经过三次基本旋转，可以得到机体坐标系。在这三次旋转中，旋转轴是待转动坐标系的某一对应坐标轴，旋转角度即为欧拉角，因此，姿态矩阵与三次基本旋转的顺序密切相关，可以用三次基本旋转矩阵的乘积表示。

欧拉角有两个大类表示方法，如下：

最为常见的旋转顺序是Z-Y-X顺序（也称为航向角-俯仰角-滚转角，偏航-俯仰-滚转，Yaw-Pitch-Roll），接下来证明矩阵都会使用这个顺序。

旋转次序：欧拉角的旋转顺序不能随意改变，要注意描述时所参考的坐标系。

姿态矩阵：一个3x3的矩阵，用于描述一个坐标系相对于另一个坐标系的旋转。这个矩阵可以通过将三个基本旋转矩阵（每个旋转对应一个矩阵）相乘来得到。旋转矩阵的乘积顺序必须与欧拉角的旋转顺序相匹配。

方向余弦矩阵与旋转矩阵

基础知识

方向余弦矩阵

在坐标系R中设v为一个空间向量，在R坐标系下的投影为（vx,vy,vz），与其x y z 轴分别成α β γ角度，则cosα,cosβ,cosγ分别为v在三轴的方向余弦，大小分别等于|vx|,|vy|,|vz|。

在一个平面内对向量进行旋转相当于对坐标进行旋转，初始状态下令载体坐标系和参考坐标系完全重合，载体转动时，载体坐标系会相对于参考坐标系转动，把载体坐标系的三个轴当做三个单位向量（vx,vy,vz）,每个载体坐标轴都可以在参考坐标系上找到三个对应的方向余弦。最终得到九个方向余弦，把九个方向余弦写成矩阵形式就是方向余弦矩阵。

方向余弦矩阵机令机体坐标系与参考全局坐标系初始重合，原点始终重合，通过机体到参考坐标系各个轴的投影也就是9个余弦值来表述。

旋转矩阵

任意物体绕自己的各个轴旋转，其姿态的变化情况可以用旋转矩阵R来表示。所谓姿态的变化情况，可以这样理解：

前提：大地坐标系G系与大地固连（G系为地球固连坐标系），物体坐标系B系与飞机固连（B系为机体坐标系）。初始状态下，令两个坐标系完全重合。

问题：现在我在飞机上任取一点A，该点在大地系的坐标为A0=（x0,y0,z0），（此时该点在机体系的坐标是一模一样的），那么当飞机绕自己的X轴旋转θ弧度后，问该点在大地系的新坐标A1？

答：A1=Rx * A0，这就是旋转矩阵所代表的姿态变化情况。而且，如果飞机先绕自己的X轴转（横滚Roll），再绕自己的Y轴转（俯仰Pitch），再绕自己的Z轴转（航向Yaw），求点A在大地系的新坐标，只需把坐标A0依次左乘3个旋转矩阵即可，也即A1 = Rz*Ry*Rx*A0。如果旋转顺序不同，这3个R矩阵的连续左乘的结果，肯定是不同的（因为矩阵乘法不满足交换律，除了一类特殊矩阵以外，显然旋转矩阵几乎不存在满足交换律的情形），这就是所谓的欧拉角的旋转是有顺序的。Roll、pitch、yaw三次旋转的角度就叫欧拉角。

方向余弦矩阵推导

一.用刚体旋转矩阵推导

选取第一个从z轴俯视，1为一个向量，2为1旋转后的向量，由上图可以得到以下式子组：

$x=r\cos \alpha$

$y=r\sin \alpha$

${x}'=rcos(\alpha +\psi )=rcos\alpha cos\psi -rsin\alpha sin\psi =xcos\psi -ysin\psi$

${y}'=rsin(\alpha +\psi )=rsin\alpha cos\psi +rcos\alpha sin\psi =xsin\psi +ycos\psi$

可以得到以下关系：

最后一行 $1 = 1$ 是因为实际上矩阵有平移，但姿态解算时可以不考虑

写成矩阵形式：

$\begin{bmatrix} {x}'\\ {y}'\\ {z}'\\ 1 \end{bmatrix}= \begin{bmatrix} cos\psi &-sin\psi &0 &0 \\ sin\psi &cos\psi &0 &0 \\ 0& 0 & 1 &0 \\ 0& 0 & 0 & 1 \end{bmatrix}$

这里我们只用取其中3*3的子阵来表示z轴的刚体旋转矩阵，即 ${R_{Z}}= \begin{bmatrix} cos\psi &-sin\psi &0 \\ sin\psi & cos\psi &0 \\ 0& 0 & 1 \end{bmatrix}$

同理可以得到：

$R_{X}= \begin{bmatrix} 1 & 0 &0 \\ 0& cos\varphi &-sin\varphi \\ 0& sin\varphi & cos\varphi \end{bmatrix}$ $R_{Y}= \begin{bmatrix} cos\theta & 0& sin\theta \\ 0& 1 &0 \\ -sin\theta & 0 & cos\theta \end{bmatrix}$

将三个矩阵相乘就可以得到方向余弦矩阵，即 $R= R_{Z}*R_{Y}*R_{X}$

得到结果为：

$\begin{bmatrix} cos\theta cos\psi &-cos\varphi sin\psi +sin\varphi sin\theta sin\psi &sin\varphi sin\psi +cos\varphi sin\theta cos \psi \\ cos\theta sin\psi & cos\varphi cos\psi +sin\varphi sin\theta sin\psi &-sin\varphi cos\psi +cos\varphi sin\theta sin\psi \\ -sin\theta &sin\varphi cos\theta & cos\varphi cos\theta \end{bmatrix}$

二.用坐标系旋转矩阵推导

将A系旋转 $\psi$ 到B可以得到以下式子组：

${x}'={oA}'=ob+ba=xcos\psi +ysin\psi$

${y}'={oB}'=oe+ed=-xsin\psi +ycos\psi$

${z}'=z$

写成矩阵形式：

$R_{Z}= \begin{bmatrix} {x}'\\ {y}'\\ {z}' \end{bmatrix}=\begin{bmatrix} cos\psi &sin\psi & 0\\ -sin\psi & cos\psi &0\\ 0& 0& 1 \end{bmatrix}\begin{bmatrix} x\\ y\\ z \end{bmatrix}$

同理可以得到： $R_{Y}= \begin{bmatrix} cos\theta & 0& -sin\theta \\ 0& 1 &0 \\ sin\theta & 0 & cos\theta \end{bmatrix}$ $R_{X}= \begin{bmatrix} 1 & 0 &0 \\ 0& cos\varphi &sin\varphi \\ 0& -sin\varphi & cos\varphi \end{bmatrix}$

$R_{e}^{b}=R_{z}*R_{y}*R_{x}$ 从机体坐标系到地球固连坐标系的旋转矩阵 $R_{b}^{e}=R_{z}^{T}*R_{y}^{T}*R_{x}^{T}$

得到方向余弦矩阵与上述推导一致

$\begin{bmatrix} cos\theta cos\psi &-cos\varphi sin\psi +sin\varphi sin\theta sin\psi &sin\varphi sin\psi +cos\varphi sin\theta cos \psi \\ cos\theta sin\psi & cos\varphi cos\psi +sin\varphi sin\theta sin\psi &-sin\varphi cos\psi +cos\varphi sin\theta sin\psi \\ -sin\theta &sin\varphi cos\theta & cos\varphi cos\theta \end{bmatrix}$

三.问题和缺点

将上述方向余弦矩阵简化为 $\begin{bmatrix} r_{11} & r_{12}& r_{13}\\ r_{21}& r_{22} & r_{23}\\ r_{31}& r_{32} & r_{33} \end{bmatrix}$ ，可以得到以下与角度相关的式子：

$tan\psi =\frac{r_{21}}{r_{11}}$ $sin\theta =-r_{3}$ $tan\varphi =\frac{r_{32}}{r_{33}}$

反解可以得到：

$\psi =arc tan\frac{r_{21}}{r_{11}}$ $\theta =arcsin(-r_{31})$ $\varphi =arctan\frac{r_{32}}{r_{33}}$

从上式可以看出几个问题：

1.函数与自变量相同

2.含有大量三角运算，效率低下

3.万向节——当 $\theta =\pm \frac{\pi }{2}$ ， $r_{11}=r_{21}=0$ ， $\psi$ 和 $\varphi$ 不能唯一确定

万向节详细解释：

一个物体先绕x轴转一定角度，再绕y转90°或-90°，再绕z轴旋转，此时与旋转x轴效果一致，两个旋转轴重合，丢失了一个自由度。

罗德里格旋转

在这之前先让我们了解一下点乘和叉乘，这对接下来的公式推导非常重要：

点乘： $\vec{A}\cdot \vec{B}=|\vec{A}||\vec{B}|cos<\vec{A},\vec{B}>$ 这个都知道就不过多讲述

叉乘：两个向量叉乘得到一个新向量，新向量与这两个向量相垂直

1.方向确认： $\vec{A}\times \vec{B}$ 用右手四指从 $\vec{A}$ 绕向 $\vec{B}$ ，大拇指指向上， $\vec{C}$ 为向上；反之 $\vec{C}$ 向下

2.计算： $\left \| \vec{C} \right \|=|\vec{A}|\times |\vec{B} |sin<\vec{A},\vec{B}>$

3.注意：叉乘不满足交换律，其左乘和右乘得到的是两个方向不同的向量

罗德里格旋转公式（Rodrigues' rotation formula）是用于计算三维空间中一个向量绕旋转轴旋转给定角度后得到的新向量的公式。其推导过程可以分为两种情况：旋转轴与旋转向量垂直，以及旋转轴与旋转向量不垂直。以下是推导过程：

一、旋转轴与旋转向量垂直的情况

设定：
- 旋转轴为单位向量k。
- 旋转向量为v。
- 旋转角度为θ。
推导：

如上图： $\vec{v_{rot}}$ 在x轴上分量就为 $\vec{v}cos\Theta$ ；我们让k叉乘v得到沿y轴正方向的一个新向量大小为 $\vec{k}\times \vec{v}$ ，则 $\vec{v_{rot}}$ 在y轴上分量就为 $sin\Theta (\vec{k}\times \vec{v})$ ，所以 $\vec{v_{rot}}=\vec{v}cos\Theta +sin\Theta (\vec{k}\times \vec{v})$

二、旋转轴与旋转向量不垂直的情况

设定：
- 同样设定旋转轴为单位向量k，旋转向量为v，旋转角度为θ。
推导：

单位向量的方向表示为 $\frac{\vec{k}}{|\vec{k}|}$
由上图可以知道旋转后的 $\vec{v_{rot}}$ 可以看做是 $\vec{v_{z}}$ 与 $\vec{v_{roty}}$ 的合成

$\vec{v_{z}}=|\vec{v}|\cdot cos<\vec{v},\vec{k}>\cdot \frac{\vec{k}}{|\vec{k}|}=\vec{v}\cdot \vec{k}\vec{k}$

$\vec{v_{y}}=\vec{v}-\vec{v_{z}}=\vec{v}-\vec{v}\cdot \vec{k}\vec{k}$

$\vec{v_{roty}}=\vec{v_{y}}\cdot cos\Theta +sin\Theta (\vec{k}\times \vec{v_{y}})$

结合上式可以得出：

$\vec{v_{rot}}=\vec{v_{roty}} + \vec{v_{z}} =\vec{v_{y}}\cdot cos\Theta +sin\Theta (\vec{k}\times \vec{y}) + \vec{v}\cdot \vec{k}\vec{k}=(\vec{v}-\vec{v}\cdot \vec{k}\vec{k})\cdot cos\Theta + \vec{v}\cdot \vec{k}\vec{k}+sin\Theta (\vec{k}\times \vec{y})=\vec{v}cos\Theta +(\vec{v}\cdot \vec{k}\vec{k})\cdot (1-cos\Theta) +sin\Theta (\vec{k}\times \vec{v_{y}})$

又因为

$\vec{k}\times \vec{v} =|\vec{k}|\times |\vec{v} |sin<\vec{k},\vec{v}>=|\vec{v_{y}}|$ $\vec{k}\times \vec{v_{y}} =|\vec{k}|\times |\vec{v_{y}} |sin<\vec{k},\vec{v}>=|\vec{v_{y}}|$

所以最终推导出的公式是 $\vec{v_{rot}}=\vec{v}cos\Theta +(\vec{v}\cdot \vec{k}\vec{k})\cdot (1-cos\Theta) +sin\Theta (\vec{k}\times \vec{v_{}})$

罗德里格旋转矩阵

罗德里格旋转公式 $\vec{v_{rot}}=\vec{v}cos\Theta +(\vec{v}\cdot \vec{k}\vec{k})\cdot (1-cos\Theta) +sin\Theta (\vec{k}\times \vec{v_{}})$

可以看做 $\vec{v_{rot}}=R\cdot \vec{v}$ ； $\vec{v}$ 是共有部分，R是对应部分的矩阵形式。

在开始推导R对应部分的矩阵形式之前我们先了解一下叉乘矩阵

叉乘矩阵

$\vec{a}=\begin{bmatrix} a1\\ a2\\ a3 \end{bmatrix}$ $\vec{b}=\begin{bmatrix} b1\\ b2\\ b3 \end{bmatrix}$

$\vec{a}=a1\cdot \vec{i}+a2\cdot \vec{j}+a3\cdot \vec{k}$ $\vec{b}=b1\cdot \vec{i}+b2\cdot \vec{j}+b3\cdot \vec{k}$

$\vec{a}\times \vec{b}=(a2b3-a3b2)\cdot \vec{i}+(a3b1-a1b3)\cdot \vec{j}+(a1b2-a2b1)\cdot \vec{k}$

写成矩阵形式为：

$\begin{bmatrix} a2b3-a3b2\\ a3b1-a1b3\\ a1b2-a2b1 \end{bmatrix}=\begin{bmatrix} 0 & -a3&a2 \\ a3& 0 & -a1\\ -a2& a1 & 0 \end{bmatrix}\cdot \begin{bmatrix} b1\\ b2\\ b3 \end{bmatrix}$

最终化简形式 $\begin{bmatrix} 0 & -a3&a2 \\ a3& 0 & -a1\\ -a2& a1 & 0 \end{bmatrix}\cdot \begin{bmatrix} b1\\ b2\\ b3 \end{bmatrix}$ 就被称为 $\vec{a}$ 的叉乘矩阵，记作 $[\vec{a}]_{\times }$

1. $sin\Theta (\vec{k}\times \vec{v_{}})=\begin{bmatrix} 0 & -k3&k2 \\ k3& 0 & -k1\\ -k2& k1 & 0 \end{bmatrix}\cdot \begin{bmatrix} v1\\ v2\\ v3 \end{bmatrix}$

2. $\vec{v}^{T}\cdot \vec{k}\vec{k}=\vec{k}(\vec{v}^{T}\cdot \vec{k})=\vec{k}(\vec{k}^{T}\cdot \vec{v}^{})=(\vec{k}\cdot \vec{k}^{T})\vec{v}^{}$ 矩阵乘法点乘的三个坐标对应相乘，转置谁都一样

所以R矩阵部分可表示为 $I_{3*3}cos\Theta +(1-cos\Theta )\begin{bmatrix} k1\\ k2\\ k3 \end{bmatrix}\begin{bmatrix} k1 & k2& k3 \end{bmatrix}+sin\Theta\begin{bmatrix} 0 & -k3&k2 \\ k3& 0 & -k1\\ -k2& k1 & 0 \end{bmatrix}$

四元数

经过上述一系列的推导我们了解了万向节问题，并得到了重要的罗德里格旋转公式，接下来引入四元数，开始讨论其与旋转之间的关系。

所有的四元数𝑞∈H（H代表四元数的发现者WilliamRowanHamilton）都可以写成下面这种形式：

q = 𝑎+𝑏𝑖+𝑐𝑗+𝑑𝑘, (𝑎,𝑏,𝑐,𝑑 ∈ R)

其中

1^2 = 𝑗^2 = 𝑘^2 = 𝑖𝑗𝑘 = −1

四元数其实就是对于基{1,𝑖,𝑗,𝑘}的线性组合，四元数也可以写成向量的形式

$q=\begin{bmatrix} a\\ b\\ c\\ d \end{bmatrix}$

除此之外，我们经常将四元数的实部与虚部分开，并用一个三维的向量来表示虚部，将它表示为标量和向量的有序对形式

定义与性质

1.模长

我们可以暂时先将一个四元数𝑞=𝑎+𝑏𝑖+𝑐𝑗+𝑑𝑘的模长定义为 $\left \| q \right \|=\sqrt{a^{2}+b^{2}+c_{2}+d^{2}}$

如果用标量向量有序对的形式进行表示的话，𝑞=[𝑠,v]的模长为

2.加减法

与复数类似，四元数的加法只需要将分量相加就行。如果我们有两个四元数q1=𝑎+𝑏𝑖+𝑐𝑗+𝑑𝑘，q2 = 𝑒+ 𝑓𝑖+𝑔𝑗+ℎ𝑘，那么它们的和为q1+q2 = 𝑎 +𝑏𝑖 +𝑐𝑗 +𝑑𝑘 +𝑒 + 𝑓𝑖+ 𝑔𝑗+ ℎ𝑘 = (𝑎 +𝑒)+(𝑏 + 𝑓)𝑖 +(𝑐 + 𝑔)𝑗 +(𝑑+ ℎ)𝑘

减法也是同理，即将加号改为减号q1−q2 = (𝑎 −𝑒)+(𝑏 − 𝑓)𝑖 +(𝑐 − 𝑔)𝑗 +(𝑑− ℎ)𝑘

如果四元数是以标量向量有序对形式定义的，比如说𝑞1=[𝑠,v]，𝑞2=[𝑡,u]，那么

$q_{1}\pm q_{2}=[s\pm t,v\pm u]$

3.标量乘法

如果我们有一个四元数𝑞=𝑎+𝑏𝑖+𝑐𝑗+𝑑𝑘和一个标量𝑠，那么它们的乘积为：

𝑠𝑞 = 𝑠(𝑎 +𝑏𝑖 +𝑐𝑗 +𝑑𝑘) = 𝑠𝑎 +𝑠𝑏𝑖 +𝑠𝑐𝑗 +𝑠𝑑𝑘

四元数与标量的乘法是遵守交换律的，也就是说𝑠𝑞=𝑞𝑠。

4.四元数乘法

四元数之间的乘法比较特殊，它们不遵守交换律，也就是说一般情况下𝑞1𝑞2 ≠ 𝑞2𝑞1．这也就有了左乘和右乘的区别．如果是𝑞1𝑞2，那么我们就说「𝑞2 左乘以𝑞1」，如果是𝑞2𝑞1，那我们就说「𝑞2右乘以𝑞1」．除了交换律之外，我们经常使用的结合律和分配律在四元数内都是成立的。

现有两个四元数𝑞1=𝑎+𝑏𝑖+𝑐𝑗+𝑑𝑘和𝑞2=𝑒+ 𝑓𝑖+𝑔𝑗+ℎ𝑘，那么它们的乘积为

𝑞1𝑞2 = (𝑎 +𝑏𝑖 + 𝑐𝑗 +𝑑𝑘)(𝑒 + 𝑓𝑖 + 𝑔𝑗 + ℎ𝑘) =

𝑎𝑒 +𝑎𝑓𝑖 +𝑎𝑔𝑗 +𝑎ℎ𝑘+

𝑏𝑒𝑖 + 𝑏𝑓𝑖^2 +𝑏𝑔𝑖𝑗 +𝑏ℎ𝑖𝑘+

𝑐𝑒𝑗 + 𝑐𝑓𝑗𝑖 + 𝑐𝑔𝑗^2 + 𝑐ℎ𝑗𝑘+

𝑑𝑒𝑘 + 𝑑𝑓𝑘𝑖 + 𝑑𝑔𝑘𝑗 + 𝑑ℎ𝑘^2

我们可以通过之前的在叉乘矩阵中定义的i，j，k的关系来化简上述公式：

𝑞1𝑞2=(𝑎𝑒+𝑎𝑓𝑖+𝑎𝑔𝑗+𝑎ℎ𝑘)+(𝑏𝑒𝑖−𝑏𝑓+𝑏𝑔𝑘−𝑏ℎ𝑗)+ (𝑐𝑒𝑗−𝑐𝑓𝑘−𝑐𝑔+𝑐ℎ𝑖)+( 𝑑𝑒𝑘+𝑑𝑓𝑗−𝑑𝑔𝑖−𝑑ℎ)

=(𝑎𝑒−𝑏𝑓−𝑐𝑔−𝑑ℎ)+ (𝑏𝑒+𝑎𝑓−𝑑𝑔+𝑐ℎ)𝑖+ (𝑐𝑒+𝑑𝑓+𝑎𝑔−𝑏ℎ)𝑗+ (𝑑𝑒−𝑐𝑓+𝑏𝑔+𝑎ℎ)𝑘

5.矩阵形式

四元数的相乘也是一个线性组合，我们可以将它写成矩阵的形式

$q_{1}q_{2}=\begin{bmatrix} a & -b &-c & -d\\ b& a & -d & c\\ c& d& a& -b\\ d &-c & b & a \end{bmatrix}\begin{bmatrix} e\\ f\\ g\\ g \end{bmatrix}$

上述矩阵所做出的变换等价于左乘𝑞1．因为四元数不符合交换律，所以右乘𝑞1的变换是另一个不同的矩阵，它的推导方法与上面的方法一致，这里直接给出结果，下面是这个矩阵所做出的变换等价于右乘𝑞1：

$q_{2}q_{1}=\begin{bmatrix} a & -b &-c & -d\\ b& a & d &- c\\ c& -d& a& b\\ d &c & -b & a \end{bmatrix}\begin{bmatrix} e\\ f\\ g\\ g \end{bmatrix}$

6.Graßmann积

我们再看一下上面推导出的式子

𝑞1𝑞2=(𝑎𝑒−(𝑏𝑓+𝑐𝑔+𝑑ℎ))+ (𝑏𝑒+𝑎𝑓+𝑐ℎ−𝑑𝑔)𝑖+ (𝑐𝑒+𝑎𝑔+𝑑𝑓−𝑏ℎ)𝑗+ (𝑑𝑒+𝑎ℎ+𝑏𝑔−𝑐𝑓)𝑘

如果 $v=\begin{bmatrix} b\\ c\\ d \end{bmatrix}$ $u=\begin{bmatrix} f\\ g\\ h \end{bmatrix}$ ，则：

点乘： $v\cdot u=bf+cg+dh$

叉乘： $v\times u=\begin{vmatrix} i & j &k \\ b& c &d \\ f& g &h \end{vmatrix}=(ch-dg)i-(bh-df)j+(bg-cf)k$

注意v×u的结果是一个向量，这里的i、j、k是向量的基，写成这种形式结果应该就非常清楚了，如果使用标量向量有序对形式来表示，𝑞1𝑞2的结果可以用向量点乘和叉乘的形式表示出来

𝑞1𝑞2=[𝑎𝑒−v·u, 𝑎u+𝑒v+v×u]

这个结果被叫做Graßmann积

7.纯四元数

我们则称𝑣=[0,v]为一个纯四元数，即仅有虚部的四元数．因为纯四元数仅由虚部的3D向量决定，我们可以将任意的3D向量转换为纯四元数。

纯四元数有一个很重要的特性：如果有两个纯四元数𝑣=[0,v],𝑢=[0,u]，那么

𝑣𝑢 =[0−v·u,0+v×u] = [−v·u,v×u]

8.逆和共轭

因为四元数不遵守交换律，我们通常不会将两个四元数相除写为 $\frac{p}{q}$ 的形式；而是将乘法的逆运算定义为 $pq^{-1}$ 或 $q^{-1}p$ ，它们在一般情况下表示不同值。

其中， $q^{-1}$ 是 $q$ 的逆，规定 $qq^{-1}=q^{-1}q=1(q\neq 0)$

那么：

$(pq^{-1})q=p(q^{-1}q)=p\cdot 1=p$ $q^{-1}(qp)=(q^{-1}q)p=1\cdot p=p$

所以，右乘𝑞的逆运算为右乘 $q^{-1}$ ，左乘𝑞的逆运算为左乘 $q^{-1}$ ，这与矩阵的性质十分相似。

这里可以使用四元数共轭的一些性质来获得 $q^{-1}$ ：

我们定义，一个四元数𝑞=𝑎+𝑏𝑖+𝑐𝑗+𝑑𝑘的共轭为𝑞∗=𝑎−𝑏𝑖−𝑐𝑗−𝑑𝑘。如果用标量向量有序对的形式来定义的话，𝑞 =[𝑠,v]的共轭为𝑞∗=[𝑠,−v]。共轭四元数的一个非常有用的性质就是

𝑞𝑞∗ = [𝑠,v] · [𝑠,−v]

= [𝑠^2 −v·(−v),𝑠(−v) + 𝑠v+v×(−v)] （v平行于-v，所以它们的叉乘为0）

= [𝑠^2 +v·v,0]

可以看到，最终的结果是一个实数，而它正是四元数模长的平方

𝑞𝑞∗ = [𝑠^2 +v·v,0]

        = 𝑠^2 +𝑥^2 +𝑦^2+𝑧^2

        = $\left \| q \right \|^{2}$

又

𝑞∗𝑞 = (𝑞∗)(𝑞∗)∗                                [(𝑞∗)∗ = [𝑠,−(−v)] = [𝑠,v] =𝑞]

        = $\left \| q^{*} \right \|^{2}$

        = 𝑠^2 +𝑥^2 +𝑦^2+𝑧^2

        = $\left \| q \right \|^{2}$

=𝑞𝑞∗

所以我们得到了，𝑞∗𝑞=𝑞𝑞∗，这个特殊的乘法是遵守交换律的。

与之前四元数逆的定义相结合可以得到

𝑞 $q^{-1}$ = 1

𝑞∗𝑞 $q^{-1}$ = 𝑞∗ (等式两边同时乘上𝑞∗)

(𝑞∗𝑞) $q^{-1}$ = 𝑞∗

$\left \| q \right \|^{2}$ · $q^{-1}$ = 𝑞∗ （𝑞∗𝑞 = $\left \| q \right \|^{2}$ ）

$q^{-1}$ = $\frac{q ^{*} }{\left \| q^{2} \right \|}$

这样就可以快速的找到四元数的逆，将一个四元数的虚部改变符号，除以它模长的平方就能获得这个四元数的逆。如果q是一个单元四元数即 $\left \| q \right \|=1$ ，那么 $q^{-1}$ = $\frac{q ^{*} }{1^{2} }$ = 𝑞∗。

与3D旋转

有了上面的相关理论，接下来我们就可以讨论四元数与3D旋转的关系了。

回忆一下我们之前讨论过的罗德里格旋转：如果我们需要将一个向量v沿着一个用单位向量所定义的旋转轴u旋转θ度，那么我们可以将这个向量v拆分为正交于旋转轴的 $v_{\perp }$ 以及平行于旋转轴的 $v_{\parallel }$ 。我们可以对这两个分向量分别进行旋转，获得 $v_{\perp }^{' }$ 和 $v_{\parallel }^{' }$ 。将它们相加就是v旋转之后的结果v′= $v_{\parallel }^{' }$ + $v_{\perp }^{' }$ 。

将这些向量定义为纯四元数：

𝑣 =[0,v] 𝑣′ = [0,v′]

$v_{\perp }$ = [0, $v_{\perp }$ ] $v_{\perp }^{' }$ = [0, $v_{\perp }^{' }$ ]

$v_{\parallel }$ = [0, $v_{\parallel }$ ] $v_{\parallel }^{' }$ = [0, $v_{\parallel }^{' }$ ]

𝑢 =[0,u]

1. $v_{\perp }$ 的旋转

首先讨论正交于旋转轴的 $v_{\perp }$ 。我们之前推导过，如果一个向量v⊥正交于旋转轴u，那么

$v_{\perp }^{'}=cos\Theta v_{\perp }+sin\Theta (u\times v_{\perp })$

可以将前面的 $v_{\perp }^{' }$ 和 $v_{\perp }$ 替换为𝑣′ ⊥和𝑣⊥，但是仍遗留下来了一个 $(u\times v_{\perp })$ 。但是，利用四元数的性质，我们可以将它写成四元数积的形式。我们之前推导过，如果有两个纯四元数𝑣=[0,v], 𝑢=[0,u]，那么𝑣𝑢 =[−v·u,v×u]。

类似地：𝑢 $v_{\perp }$ = [−u· $v_{\perp }$ , u× $v_{\perp }$ ]

因为 $v_{\perp }$ 正交于u，所以u· $v_{\perp }$ =0，也就是说𝑢 $v_{\perp }$ = [0, u× $v_{\perp }$ ] = u× $v_{\perp }$

将这个等式以及之前定义的纯四元数代入，我们就能获得

$v_{\perp }^{' }$ = cos(θ) $v_{\perp }$ + sin(θ)(𝑢 $v_{\perp }$ )

因为四元数的乘法遵守分配律，我们可以继续变换这个等式

$v_{\perp }^{' }$ =(cos(θ) + sin(θ)𝑢) $v_{\perp }$

不难发现，如果我们将(cos(θ)+sin(θ)𝑢)看做是一个四元数，我们就能将旋转写成四元数的乘积。这样我们就将旋转与四元数的积联系起来了。

如果令𝑞=cos(θ)+sin(θ)𝑢，我们可以得到 $v_{\perp }^{' }$ = $qv_{\perp }$

如果能构造一个𝑞，那么我们就能完成这个旋转，于是我们对𝑞继续进行变形

𝑞=cos(θ)+sin(θ)𝑢

= [cos(θ), 0] + [0, sin(θ)u]

= [cos(θ), sin(θ)u]

也就是说，如果旋转轴u的坐标为 $\begin{bmatrix} u_{x}\\ u_{y}\\ u_{z} \end{bmatrix}$ ，旋转角为θ，那么完成这一旋转所需要的四元数𝑞可以构造为

$q=cos\Theta +sin\Theta u_{x}\cdot i+sin\Theta u_{y}\cdot j+sin\Theta u_{z}\cdot k$

3D 旋转公式（四元数，正交情况）

当 $v_{\perp }$ 正交于旋转轴u时，旋转θ角度之后的 $v_{\perp }^{' }$ 可以使用四元数乘法来获得．令 $v_{\perp }$ =[0, $v_{\perp }$ ]，𝑞 =[cos(θ), sin(θ)u]，那么：

$v_{\perp }^{' }=qv_{\perp }$

2. $v_{\parallel }$ 的旋转

$v_{\parallel }$ 平行于旋转轴，因此其旋转前后值不变， $v_{\parallel }^{' }$ = $v_{\parallel }$

3.v的旋转

$v_{\perp }$ 与 $v_{\parallel }$ 都已经知道了，就能获得一般情况下𝑣′的结果

𝑣′ = $v_{\parallel }^{' }$ + $v_{\perp }^{' }$ = $v_{\parallel }$ +𝑞 $v_{\perp }$ （其中𝑞=[cos(θ), sin(θ)u]）

在进一步化简上述公式之前，我们需要几个引理：

1：

如果𝑞 = [cos(θ), sin(θ)u]，而且 u 为单位向量，那么 𝑞^2 = 𝑞𝑞 = [cos(2θ), sin(2θ)u]

这个引理的证明需要使用前面推导出的Graßmann积和一些三角恒等式

𝑞^2 = [cos(θ), sin(θ)u] · [cos(θ), sin(θ)u]

        = [ $\cos ^{2}\theta$ − (sin(θ)u · sin(θ)u), (cos(θ)sin(θ) + sin(θ)cos(θ))u + (sin(θ)u × sin(θ)u)]

        = [ $\cos ^{2}\theta$ − $\sin ^{2}\theta$ $\left \| u\right \|^{2}$ , 2sin(θ)cos(θ)u + 0]

        = [ $\cos ^{2}\theta$ − $\sin ^{2}\theta$ , 2sin(θ)cos(θ)u]

        = [cos(2θ), sin(2θ)u]

这个引理的几何意义就是：如果绕着同一个轴u连续旋转θ度两次，那么所做出的变换等同于直接绕着u旋转2θ度

2：

假设 $v_{\parallel }$ =[0, $v_{\parallel }$ ]是一个纯四元数，而𝑞 =[𝛼, 𝛽u]，其中u是一个单位向量，𝛼,𝛽 ∈R．在这种条件下，如果 $v_{\parallel }$ 平行于u，那么𝑞 $v_{\parallel }$ = $v_{\parallel }$ 𝑞

这个引理的证明也需要用到Graßmann积

左边=𝑞 $v_{\parallel }$

        = [𝛼, 𝛽u]·[0, $v_{\parallel }$ ]

        = [0−𝛽u· $v_{\parallel }$ , 𝛼 $v_{\parallel }$ +0+𝛽u× $v_{\parallel }$ ]

        = [−𝛽u· $v_{\parallel }$ , 𝛼 $v_{\parallel }$ ]                                （ $v_{\parallel }$ 平行于u，所以𝛽u× $v_{\parallel }$ =0）

右边= $v_{\parallel }$ 𝑞

        = [0, $v_{\parallel }$ ] · [𝛼, 𝛽u]

        = [0− $v_{\parallel }$ ·𝛽u, 0+𝛼 $v_{\parallel }$ + $v_{\parallel }$ ×𝛽u]

        = [− $v_{\parallel }$ · 𝛽u, 𝛼 $v_{\parallel }$ ]                                （ $v_{\parallel }$ 平行于u，所以 $v_{\parallel }$ ×𝛽u=0）

        = [−𝛽u· $v_{\parallel }$ , 𝛼 $v_{\parallel }$ ]                                （点乘遵守交换律）

        =左边

3：

假设 $v_{\perp }$ =[0, $v_{\perp }$ ]是一个纯四元数，而𝑞=[𝛼, 𝛽u]，其中u是一个单位向量，𝛼,𝛽 ∈R．在这种条件下，如果 $v_{\perp }$ 正交于u，那么𝑞 $v_{\perp }$ = $v_{\perp }$ 𝑞∗

这个引理的证明于上述 $v_{\parallel }$ 相似

左边=𝑞 $v_{\perp }$

= [𝛼,𝛽u] · [0, $v_{\perp }$ ]

= [0−𝛽u· $v_{\perp }$ , 𝛼 $v_{\perp }$ +0+𝛽u× $v_{\perp }$ ]

= [0, 𝛼 $v_{\perp }$ +𝛽u× $v_{\perp }$ ]                                （ $v_{\perp }$ 正交于u，所以𝛽u· $v_{\perp }$ =0）

右边= $v_{\perp }$ 𝑞∗

= [0, $v_{\perp }$ ] · [𝛼, −𝛽u]

= [0+ $v_{\perp }$ ·𝛽u, 0+𝛼 $v_{\perp }$ + $v_{\perp }$ ×(−𝛽u)]

= [0, 𝛼 $v_{\perp }$ + $v_{\perp }$ ×(−𝛽u)]                                （ $v_{\perp }$ 正交于u，所以 $v_{\perp }$ ·𝛽u=0）

= [0, 𝛼 $v_{\perp }$ −(−𝛽u)× $v_{\perp }$ ]                                （a×b=−b×a）

= [0, 𝛼 $v_{\perp }$ +𝛽u× $v_{\perp }$ ]

=左边

现在就可以对原本的旋转公式进行变形了

𝑣′ = $v_{\parallel }$ +𝑞 $v_{\perp }$ （𝑞 =[cos(θ), sin(θ)u]）

=1 · $v_{\parallel }$ +𝑞 $v_{\perp }$

= $pp^{-1}$ $v_{\parallel }$ + 𝑝𝑝 $v_{\perp }$ （令 𝑞 = 𝑝^2，则 $p=[\cos (\frac{1}{2}\theta) ,sin(\frac{1}{2}\theta)u]$ ）

在这里，我们引入了一个新的四元数 $p=[\cos (\frac{1}{2}\theta) ,sin(\frac{1}{2}\theta)u]$ ，根据引理1我们可以验证其正确性：

𝑝𝑝 = 𝑝^2

= $p=[\cos (2\cdot \frac{1}{2}\theta) ,sin(2\cdot \frac{1}{2}\theta)u]$

= [cos(θ), sin(θ)u]

= 𝑞

和𝑞一样， $\left \| p \right \|$ =1，𝑝也是一个单位四元数，也就是说 $p^{-1}=p^{*}$ ，将它代入之前的等式中

𝑣′= $pp^{-1}$ $v_{\parallel }$ + $pp v_{\perp }$

= $pp^{*}$ $v_{\parallel }$ + $ppv_{\perp }$

= $pv_{\parallel }p^{*}$ + $pv_{\perp }p^{*}$ （引理2和引理3变形得到）

= $p(v_{\parallel }+v_{\perp })p^{*}$

= $pvp^{*}$ （ $v_{\parallel }+v_{\perp }$ =v）

经过上述一系列推导，我们可以总结出一个定理：

3D 旋转公式（四元数型，一般情况）

任意向量v沿着以单位向量定义的旋转轴u旋转θ度之后的v′可以使用四元数乘法来获得。令𝑣=[0, v]， $q=[\cos (\frac{1}{2}\theta) ,sin(\frac{1}{2}\theta)u]$ ，那么：

𝑣′= $pvp^{*}$ = $pvp^{-1}$

3D旋转的矩阵形式

在实际的应用中，我们可能会需要将旋转与平移和缩放进行复合，所以需要用到四元数旋转的矩阵形式。经过上面的讨论，我们知道，左乘一个四元数𝑞=𝑎+𝑏𝑖+𝑐𝑗+𝑑𝑘等同于下面这个矩阵

$L=\begin{bmatrix} a & -b &-c & -d\\ b& a & -d & c\\ c& d& a&- b\\ d &-c &b & a \end{bmatrix}$

右乘𝑞等同于这个矩阵

$R=\begin{bmatrix} a & -b &-c & -d\\ b& a & d &- c\\ c& -d& a& b\\ d &c &- b & a \end{bmatrix}$

于是，我们可以用这两个公式将𝑣′= $pvp^{*}$ 写成矩阵形式。假设 $a=cos(\frac{1}{2}\theta )$ ， $b=sin(\frac{1}{2}\theta )u_{x}$ ， $c=sin(\frac{1}{2}\theta )u_{y}$ ， $d=sin(\frac{1}{2}\theta )u_{z}$ ，𝑞=𝑎+𝑏𝑖+𝑐𝑗+𝑑𝑘，我们就可以得到

$pvp^{*}$

=𝐿(𝑞)𝑅(𝑞∗)𝑣

$=\begin{bmatrix} a & -b &-c & -d\\ b& a & -d & c\\ c& d& a&- b\\ d &-c &b & a \end{bmatrix} \begin{bmatrix} a & -b &-c & -d\\ -b& a & -d &c\\ -c& d& a& -b\\ -d &-c &b & a \end{bmatrix} v$ （注意𝑅(𝑞∗)=𝑅(𝑞)𝑇）

$=\begin{bmatrix} a^{2}+b^{2}+c^{2}+d^{2}&ab-ab-c+cd &ac+bd-ac-bd &ad-bc+bc-ad \\ ab-ab+cd-cd& b^{2}+a^{2}-d^{2}-c^{2}&bc-ad-ad+bc &bd+ac+bd+ac \\ ac-bd+bd-ac& bc+ad+ad+bc& c^{2}-d^{2}+a^{2}-b^{2}&cd+cd-ab-ab \\ad+bc-bc-ad &bd-ac+bd-ac &cd+cd+ab+ab &d^{2}-c^{2}-b^{2}+a^{2} \end{bmatrix} v$

$=\begin{bmatrix} 1& 0&0 & 0\\ 0& 1-2c^{2}-2d^{2}&2bc-2ad &2ac+2bd \\ 0& 2bc+2ad&1-2b^{2}-2d^{2} &2cd-2ab \\ 0&2bd-2ac &2ab+2cd &1-2b^{2}-2c^{2} \end{bmatrix}v$ (由𝑎^2+𝑏^2+𝑐^2+𝑑^2 =1变形)

这样我们就得到了3D旋转的矩阵形式。因为矩阵的最外圈不会对𝑣进行任何变换，我们可以将它压缩成3×3矩阵（用作3D向量的变换）：

任意向量v沿着以单位向量定义的旋转轴u旋转θ角度之后的v′可以使用矩阵乘法来获得．令 $a=cos(\frac{1}{2}\theta )$ ， $b=sin(\frac{1}{2}\theta )u_{x}$ ， $c=sin(\frac{1}{2}\theta )u_{y}$ ， $d=sin(\frac{1}{2}\theta )u_{z}$ ，那么：

$v^{'}=\begin{bmatrix} 1-2c^{2}-2d^{2}&2bc-2ad &2ac+2bd \\ 2bc+2ad&1-2b^{2}-2d^{2} &2cd-2ab \\ 2bd-2ac &2ab+2cd &1-2b^{2}-2c^{2} \end{bmatrix}v$

反解欧拉角

让我们把注意力回到之前的解欧拉角上，前面我们已经推出了

$\psi =arc tan\frac{r_{21}}{r_{11}}$ $\theta =arcsin(-r_{31})$ $\varphi =arctan\frac{r_{32}}{r_{33}}$

结合上面的3D旋转矩阵我们就可以用四元数表示出欧拉角，即：

$\psi =arc tan\frac{2(q_{0}q_{3}+q_{1}q_{2})}{1-2(q_{2}^{2}+q_{3}^{2})}$

$\theta =arcsin(2(q_{0}q_{2}-q_{1}q_{3}))$

$\varphi =arctan\frac{2(q_{0}q_{1}-q_{2}q_{3})}{1-2(q_{1}^{2}+q_{2}^{2})}$

(公式中𝑞0 𝑞1 𝑞2 𝑞3就是3D旋转矩阵中a b c d)

这样我们就得到了欧拉角的四元数表达式。

四元数求解及更新

在实际情况中，飞行器姿态是实时改变的，即四元数也在改变，如果需要实时获取飞行器的姿态信息，我们就需要实时更新四元数。因此，我们在这里构建四元数关于时间的微分方程，并利用一阶龙格库塔法求解它，进而研究四元数的变化规律。

首先令四元数 $Q=cos\frac{\theta }{2}+\lambda sin\frac{\theta }{2}$ ，对时间t求微分，可得微分方程

$\frac{dQ}{dt}=\frac{1}{2}\begin{bmatrix} -\omega _{x}q_{1}- \omega _{y}q_{2}-\omega _{z}q_{3}\\ \omega _{x}q_{0}+\omega _{z}q_{2}-\omega _{y}q_{3}\\ \omega _{y}q_{0}- \omega _{z}q_{1}+\omega _{x}q_{3}\\ \omega _{z}q_{0}+ \omega _{y}q_{1}-\omega _{x}q_{2} \end{bmatrix}$

将它写成矩阵形式为

$\frac{dQ}{dt}=\frac{1}{2}\begin{bmatrix} 0 & - \omega _{x} & - \omega _{y} &- \omega _{z} \\ \omega _{x} & 0& \omega _{z} &-\omega _{y} \\ \omega _{y}&-\omega _{z} & 0&\omega _{x} \\ \omega _{z}& \omega _{y} & -\omega _{x} & 0 \end{bmatrix} \begin{bmatrix} q_{0}\\ q_{1}\\ q_{2}\\ q_{3} \end{bmatrix}$

记为

$\Omega _{b}=\frac{1}{2}\begin{bmatrix} 0 & - \omega _{x} & - \omega _{y} &- \omega _{z} \\ \omega _{x} & 0& \omega _{z} &-\omega _{y} \\ \omega _{y}&-\omega _{z} & 0&\omega _{x} \\ \omega _{z}& \omega _{y} & -\omega _{x} & 0 \end{bmatrix}$

则有 $\frac{dQ}{dt}=\Omega _{b}Q$ ，因此我们可以通过陀螺仪获得角速度就能求出四元数参数，但我们需要求解一个四元数微分方程，通过求解微分方程，就可以得到我们需要的四元数参数。

一阶龙格库塔法

龙格库塔法一时也讲不清，大家可以自己下来去研究，这里就直接给出一阶龙格库塔法的结论：

设有微分方程 $\frac{d_{y}}{d_{t}}=f(x,y)$

由一阶龙格库塔法可以得到求解y的迭代公式：

$y(\lambda +\Delta \lambda )=y(\lambda )+\Delta \lambda \cdot f(x(\lambda ),y(\lambda ))$

用一阶龙格库塔法对上面的四元数微分方程进行迭代可以得到

$\begin{bmatrix} q_{0}\\ q_{1}\\ q_{2}\\ q_{3} \end{bmatrix}_{t+\Delta t}= \begin{bmatrix} q_{0}\\ q_{1}\\ q_{2}\\ q_{3} \end{bmatrix}_{t}+\frac{1}{2}\Delta t\begin{bmatrix} -\omega _{x}q_{1}- \omega _{y}q_{2}-\omega _{z}q_{3}\\ \omega _{x}q_{0}+\omega _{z}q_{2}-\omega _{y}q_{3}\\ \omega _{y}q_{0}- \omega _{z}q_{1}+\omega _{x}q_{3}\\ \omega _{z}q_{0}+ \omega _{y}q_{1}-\omega _{x}q_{2} \end{bmatrix}$