03、姿态解算---四元素

最新推荐文章于 2024-05-13 21:32:22 发布

keaishamao

最新推荐文章于 2024-05-13 21:32:22 发布

阅读量789

点赞数 30

分类专栏：姿态解算文章标签：算法数学建模矩阵嵌入式硬件 mcu c语言

本文链接：https://blog.csdn.net/keaishamao/article/details/137666953

版权

姿态解算专栏收录该内容

7 篇文章 0 订阅

订阅专栏

1、为什么用到四元素

在二维平面坐标系里， $\vec{u}=cos\theta +\vec{i}sin\theta$ 包含了二维平面的旋转信息，但如果用这个区表示三维空间的旋转信息，这个旋转公式就无法表示三维信息了。

按公式推理是不是可以用 $\vec{u}=cos\theta +(\vec{i}+\vec{j})sin\theta$ 来表示三维信息呢？实际上这个公式是无法表示三维空间的旋转问题的。

因为三维空间有三个坐标轴 $x,y,z$ ，所以想要表示一个三维空间的旋转信息则需要用 $\vec{Q}=cos\theta +(\vec{i}+\vec{j}+\vec{k})sin\theta$ ，其中 $\vec{i},\vec{j},\vec{k}$ 分别为 $x,y,z$ 轴的单位向量。该旋转向量也可以用 $\vec{Q}=q0+q1\vec{i}+q2\vec{j}+q3\vec{k}$ ，这也是用四元素来表示三维空间的旋转信息了。

2、四元素定义

①、在数学中四元素的定义为：

$\vec{Q}(q0,q1,q2,q3)=q0+q1\vec{i}+q2\vec{j}+q3\vec{k}$

其中 $q0,q1,q2,q3$ 为实数， $\vec{i},\vec{j},\vec{k}$ 既为互相正交的单位向量、又是虚单位 $\sqrt{-1}$ 。

②、四元素的乘法运算（定义四元素的乘法为“ $\otimes$ ”）：

$\vec{i}\otimes \vec{i}=-1,\vec{j}\otimes \vec{j}=-1,\vec{k}\otimes \vec{k}=-1$

$\vec{i}\otimes \vec{j}=\vec{k},\vec{j}\otimes \vec{k}=\vec{i},\vec{k}\otimes \vec{i}=\vec{j}$

$\vec{j}\otimes \vec{i}=-\vec{k},\vec{k}\otimes \vec{j}=-\vec{i},\vec{i}\otimes \vec{k}=-\vec{j}$

上述关系可以叙述为：相同单位向量进行四元素乘法呈虚单位特性，相异单位向量进行四元素乘法呈两向量叉乘特性，故四元素可以看做一个超复数，也可看做思维空间中的一个向量。

3、四元素表示形式

四元素的表现形式有很多种，大概有矢量式、复数式、三角式、矩阵式、指数式。

3.1、矢量式：

$\vec{Q}=q0+\vec{q}$ ，其中 $\vec{q}=q1\vec{i}+q2\vec{j}+q3\vec{k}$

$q0$ 为标量部分， $\vec{q}$ 为矢量部分。

3.2、复数式：

$\vec{Q}=q0+q1\vec{i}+q2\vec{j}+q3\vec{k}$

四元素可以看成一个超复数，其共轭复数为：

$\dot{\vec{Q}}=q0-q1\vec{i}-q2\vec{j}-q3\vec{k}$

称 $\dot{\vec{Q}}$ 为 $\vec{Q}$ 的共轭复数。

3.3、三角式：

$\vec{Q}=cos\frac{\theta }{2}+\vec{u}sin\frac{\theta }{2}$

式中 $\vec{u}=\vec{i}+\vec{j}+\vec{k}$ ， $\vec{i},\vec{j},\vec{k}$ 分别为 $x,y,z$ 轴单位向量。

3.4、矩阵式：

$\vec{Q}=\left [ \begin{matrix} q0\\ q1\\ q2\\ q3 \end{matrix} \right ]$

3.5、指数式：

$\vec{Q}=e^{\vec{u}\frac{\theta }{2}}$ ，式中 $\vec{u}=\vec{i}+\vec{j}+\vec{k}$ $\vec{u}=\vec{i}+\vec{j}+\vec{k}$ ， $\vec{i},\vec{j},\vec{k}$ 分别为 $x,y,z$ 轴单位向量。

4、四元素的大小

在数学中，用四元素的范数来表示四元素的大小，具体如下：

$\left \| \vec{Q} \right \|=q0^{2}+q1^{2}+q2^{2}+q3^{2}$ ，若 $\left \| \vec{Q} \right \|=1$ ，则称该四元素为规范四元素。

5、四元素的运算法则

5.1、加法与减法

若有如下两个四元素：

$\vec{Q}=q0+q1\vec{i}+q2\vec{j}+q3\vec{k}$

$\vec{P}=p0+p1\vec{i}+p2\vec{j}+p3\vec{k}$

则 $\vec{Q}\pm \vec{P}=(q0\pm p0)+(q1\pm p1)\vec{i}+(q2\pm p2)\vec{j}+(q3\pm p3)\vec{k}$

5.2、乘法

乘法又分为四元素与四元素相乘，四元素和标量相乘。

①、四元素与标量相乘

$e\vec{Q}=eq0+eq1\vec{i}+eq2\vec{j}+eq3\vec{k}$

②、四元素与四元素相乘

若有如下两个四元素：

$\vec{Q}=q0+q1\vec{i}+q2\vec{j}+q3\vec{k}$

$\vec{P}=p0+p1\vec{i}+p2\vec{j}+p3\vec{k}$

则有：

$\vec{Q}\otimes \vec{P}=(q0+q1\vec{i}+q2\vec{j}+q3\vec{k})\otimes (p0+p1\vec{i}+p2\vec{j}+p3\vec{k})=(p0q0-p1q1-p2q2-p3q3)+(p0q1+p1q0+p2q3-p3q2)\vec{i}+(p0q2+p2q0+p3q1-p1q3)\vec{j}+(p0q3+p3q0+p1q2-p2q1)\vec{k}$

将上式写为矩阵形式如下：

$\vec{P}\otimes \vec{Q}=\begin{bmatrix} p0 &-p1 &-p2 &-p3 \\ p1 &p0 &-p3 &p2 \\ p2 &p3 &p0 &-p1 \\ p3 &-p2 &p1 &p0 \end{bmatrix}\begin{bmatrix} q0\\ q1\\ q2\\ q3 \end{bmatrix}$

或

$\vec{Q}\otimes \vec{P}=\begin{bmatrix} q0 &-q1 &-q2 &-a3 \\ q1 &q0 &q3 &-q2 \\ q2 &-q3 &q0 &q1 \\ q3 &q2 &-q1 &q0 \end{bmatrix}\begin{bmatrix} p0\\ p1\\ p2\\ p3 \end{bmatrix}$

5.3、除法

除法称为求逆。

逆的定义：

如果 $\vec{P}\otimes \vec{R}=1$ ，称 $\vec{R}$ 为 $\vec{P}$ 的逆，记作 $\vec{R}=\vec{P}^{-1}$ ，也可称为 $\vec{P}$ 为 $\vec{R}$ 的逆，记作 $\vec{P}=\vec{R}^{-1}$ 。

若 $\vec{Q}=q0+q1\vec{i}+q2\vec{j}+q3\vec{k}$ ， $\dot{\vec{Q}}=q0-q1\vec{i}-q2\vec{j}-q3\vec{k}$ ，由四元素乘法，可有：

$\vec{Q}\otimes \dot{\vec{Q}}=(q0+q1\vec{i}+q2\vec{j}+q3\vec{k})\otimes (q0-q1\vec{i}-q2\vec{j}-q3\vec{k})=q0^{2}+q1^{2}+q2^{2}+q3^{2}$

也即有： $\vec{Q}\otimes\frac{\dot{\vec{Q}}}{\left \| \vec{Q} \right \|} =1$ ，所以 $\frac{\dot{\vec{Q}}}{\left \| \vec{Q} \right \|}$ 为 $\vec{Q}$ 的逆，记作 $\vec{Q}^{-1}=\frac{\dot{\vec{Q}}}{\left \| \vec{Q} \right \|}$