四元数介绍

本文目的

介绍四元数姿态解算，四元数基本运算（写完），四元数应用（写完），角速度如何积分得到四元数（还没写），以及如何转化为欧拉角（还没写）

前置知识点

1. 线性代数基本知识，比如矩阵乘法，向量叉乘等

四元数简介

参考B站《3B1B的四元数相关理论知识》

四元数的可视化

1. 类比复数有一个虚部，四元数有三个虚部，本质上是四维空间的向量（不写）。
2. 单位四元数在几何意义上是四维超球在三维空间中的投影（不写）。
3. 单位四元数的乘法可以表征旋转。
4. 单位四元数可以表征物体在空间的姿态。

四元数在姿态解算中的使用

1. 四元数定义（先背下来后面才好理解）

$$\vec{q}=q_0+q_{1}i+q_{2}j+q_{3}k=q_0+\vec{v}\text{\hspace{1em}(1)}$$
用矩阵表示为
$$\vec{q}=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]\text{\hspace{1em}(2)}$$
其中有乘法关系有
$$\begin{array}{c}{i}^2=j^2=k^2=-1\\ ij=-ji=k\\ jk=-kj=i\\ ki=-ki=j\end{array}\text{\hspace{1em}(3)}$$
四元数不满足交换律即
$$\overrightarrow{q_1}\overrightarrow{q_2}\ne \overrightarrow{q_2}\overrightarrow{q_1}\text{\hspace{1em}(4)}$$
在单位四元数（模长为1的情况下）有单位四元数的共轭等于求逆：
$$(\vec{q}{)}^{\ast }=(\vec{q}{)}^{-1}\text{\hspace{1em}(5)}$$

2. 四元数乘法的矩阵形式

1. 四元数$\vec{q}$左乘$\vec{p}$
   $$\vec{p}\vec{q}=\left[\begin{array}{cccc}{p}_0& -p_1& -p_2& -p_3\\ p_1& p_0& -p_3& p_2\\ p_2& p_3& p_0& -p_1\\ p_3& -p_2& p_1& p_0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]\text{\hspace{1em}(6)}$$
2. 四元数$\vec{q}$右乘$\vec{p}$
   $$\vec{q}\vec{p}=\left[\begin{array}{cccc}{p}_0& -p_1& -p_2& -p_3\\ p_1& p_0& p_3& -p_2\\ p_2& -p_3& p_0& p_1\\ p_3& p_2& -p_1& p_0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]\text{\hspace{1em}(7)}$$

3. 单位四元数乘法表示三维向量的旋转

单位四元数可以写成是三维向量$\vec{v}$和一个标量的和
$$\vec{p}=p_0+p_{1}i+p_{2}j+p_{3}k=\cos \theta +\vec{v}\sin \theta \text{\hspace{1em}(8)}$$
对于一个三维空间的表征方向的单位向量$\vec{u}$，四元数的实部为0（第四维度没有值，就好像三维空间里面$z=0$的时候变成二维向量一样）。
$$\overrightarrow{q_u}=0+u_{1}i+u_{2}j+u_{3}k=0+\vec{u}\text{\hspace{1em}(9)}$$
向量$\vec{u}$围绕$\vec{v}$所指向的方向轴，以右手定则顺时针旋转$\theta$度得到的向量$\overrightarrow{u^{\prime }}$，可以用如下计算式进行计算
$$\overrightarrow{q_u^{\prime }}=\vec{p}\overrightarrow{q_u}{\vec{p}}^{\ast }=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1-2p_2^2-2p_3^2& 2p_1{p}_2-2p_0{p}_3& 2p_0{p}_2+2p_1{p}_3\\ 0& 2p_1{p}_2+2p_0{p}_3& 1-2p_1^2-2p_3^2& 2p_2{p}_3-2p_0{p}_1\\ 0& 2q_1{q}_3-2q_0{q}_2& 2p_0{p}_1+2p_2{p}_3& 1-2p_1^2-2p_2^2\end{array}\right]\left[\begin{array}{c}0\\ u_1\\ u_2\\ u_3\end{array}\right]\text{\hspace{1em}(10)}$$
也就是说，只需要知道如下两个值，就可以求得旋转后的向量$\overrightarrow{u^{\prime }}$指向哪里。

1. 四元数$\vec{p}$（表征了旋转轴向量，旋转角度）
2. 初始向量$\vec{u}$

观察上述公式，可以化简成三维矩阵的乘法，得到四元数表征旋转的最终式子
$$\overrightarrow{u^{\prime }}=\left[\begin{array}{ccc}1-2p_2^2-2p_3^2& 2p_1{p}_2-2p_0{p}_3& 2p_0{p}_2+2p_1{p}_3\\ 2p_1{p}_2+2p_0{p}_3& 1-2p_1^2-2p_3^2& 2p_2{p}_3-2p_0{p}_1\\ 2q_1{q}_3-2q_0{q}_2& 2p_0{p}_1+2p_2{p}_3& 1-2p_1^2-2p_2^2\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right]\text{\hspace{1em}(11)}$$

4. 四元数姿态表示法

4.1 先验知识

1. 假设现在以物体本身建立一个xyz参考系，它和在世界参考系xyz相对位置就是他的姿态。
2. 不考虑伸缩变换，只考虑相对转动。
3. 两个坐标系的关系可以用过渡矩阵这一概念来进行描述
4. 任意两个这样不考虑伸缩变换的坐标系，机体参考系的基向量可以由世界参考系的对应基向量按照世界参考系的某个轴向量旋转某个角度得到

4.2 推论

1. 上文得知，四元数科研表征某个轴向量旋转某个角度，意味着可以使用四元数来描述机体参考系和世界参考系的相对旋转情况，也就是用四元数表示姿态。
2. 四元数计算得到的三维矩阵，恰好就是过渡矩阵，也可以用来表征机体参考系和世界参考系的相对旋转情况，也就是用姿态矩阵来表征姿态。
3. 这是一个可逆的过程，旋转矩阵，四元数，欧拉角可以相互换算
4. 这个姿态矩阵是坐标系之间的关系，和旋转矩阵用法不太一样

4.3 求机体姿态的x，y，z基向量在世界参考系下的表示

把[1,0,0]左乘旋转矩阵可以得到机体x轴朝向
把[0,1,0]左乘旋转矩阵可以得到机体y轴朝向
把[0,0,1]左乘旋转矩阵可以得到机体z轴朝向

4.4 根据x，y，z基向量在世界参考系下的表示求姿态矩阵

3*3建立方程组，三个方程三个变量，很好求

4.5 欧拉角，四元数，姿态矩阵相互转换

先不写，有需要用到自己再补充

5. 其他

这篇blog还需要加一些图，晚点再画

参考文献

四元数的可视化