第一章机械臂基础-1.2 姿态描述

最新推荐文章于 2024-07-27 02:02:04 发布

愈之~

最新推荐文章于 2024-07-27 02:02:04 发布

阅读量805

点赞数 24

分类专栏：机械臂算法仿真笔记文章标签：人工智能

本文链接：https://blog.csdn.net/hq1215758640/article/details/140207077

版权

机械臂算法仿真笔记专栏收录该内容

7 篇文章 3 订阅

订阅专栏

描述空间中的姿态在上一节中提到，为矩阵形式，由三个列矢量所组成的矩阵表示姿态，需要9个值来表示。这三个列矢量相互垂直，且都是单位矢量。可得知 ${\widehat{X}} \bullet {\widehat{Y}}=0$

$\ _B^AR=\left[\begin{matrix}r_{11}&r_{12}&r_{13}\\r_{21}&r_{22}&r_{23}\\r_{31}&r_{32}&r_{33}\\\end{matrix}\right]$ (1-2-1)

${\widehat{X}} \bullet {\widehat{Z}}=0$ , ${\widehat{Y}} \bullet {\widehat{Z}}=0$ ，但对于姿态来说，只有三个自由度，却用了九个变量，那么有没有其他的方法去描述姿态呢，答案是肯定的，在前面说过，旋转矩阵既是位姿也是变换，这里可以将其理解为一个坐标系相对于另一个坐标系的变换，那么可以这么思考，就是说空间的一个姿态变换可以由旋转x、y、z三个轴来达到，或者绕空间中的某个轴线旋转一定的角度来达到，这些都是具备描述空间姿态的条件的。

（一）固定角

空间中的任意姿态，都可以由旋转XYZ一定的角度来达到，

1、先将两个坐标系{ $A$ } 、{ $B$ } 重合，

2、坐标系{ $B$ } 绕{ $A$ } 的X轴旋转 $\gamma$ 角度（回转角）

3、坐标系{ $B$ } 绕{ $A$ } 的Y轴旋转 $\beta$ 角度（俯仰角）

4、坐标系{ $B$ } 绕{ $A$ } 的Z轴旋转 $\alpha$ 角度（偏转角）

图1.2.1 固定角变换

根据上一章节的混合变换来说，

$_B^AR_{XYZ}(\gamma,\beta,\alpha) = R_Z(\alpha)\cdot R_Y(\beta)\cdot R_X(\gamma)$

$=\left[\begin{matrix}c\alpha&-s\alpha&0\\s\alpha&c\alpha&0\\0&0&1\\\end{matrix}\right]\left[\begin{matrix}c\beta&0&s\beta\\0&1&0\\-s\beta&0&c\beta\\\end{matrix}\right]\left[\begin{matrix}1&0&0\\0&c\gamma&-s\gamma\\0&s\gamma&c\gamma\\\end{matrix}\right]$

$=\left[\begin{matrix}c\alpha c\beta&c\alpha s\beta s\gamma-s\alpha c\gamma&c\alpha s\beta c\gamma+s\alpha s\gamma\\s\alpha c\beta&s\alpha s\beta s\gamma+c\alpha c\gamma&s\alpha s\beta c\gamma-c\alpha s\gamma\\-s\beta&c\beta s\gamma&c\beta c\gamma\\\end{matrix}\right]$ (1-2-2)

其中s=sin ,c=cos 。

上面的每次变换都是基于固定坐标系的旋转，这种称为固定角转换。

（二）欧拉角

从固定角转换来看，旋转的时候是依次按照 $X$ 、 $Y$ 、 $Z$ 轴旋转，但实际上的矩阵乘法是 $R_Z(\alpha)\cdot\ R_Y(\beta)\cdot\ R_X(\gamma)$ ，这里有结论，按照固定角转换，矩阵乘法为左乘，而如果为相对坐标系旋转，也就是欧拉角，为右乘，那么若固定角的XYZ，依照右乘来看，就是相对坐标系的ZYX转换。相对坐标系的概念为，当旋转完一个坐标系之后，不是按照固定的坐标系旋转，而是按照上次变换之后的那个坐标系旋转如下图1.2.2所示

图1.2.2 相对坐标系转换，欧拉角ZYX旋转

旋转的顺序为：

1、先将两个坐标系{ $A$ } 、{ $B$ } 重合，

2、坐标系{ $B$ } 绕{ $A$ } 的 $Z_B$ 轴旋转α 角度

3、坐标系{ $B$ } 绕{ $A$ } 的 ${Y_B}{}'$ 轴旋转β 角度

4、坐标系{ $B$ } 绕{ $A$ } 的 $X_B{}''$ 轴旋转γ 角度

那么公式：

$\ _B^AR_{ZY\prime X^{\prime\prime}}\left(\alpha,\beta,\gamma\right)=R_Z\left(\alpha\right)R_Y\left(\beta\right)R_X\left(\gamma\right)$ (1-2-3)

最后得到的表达式与固定角的 $XYZ$ 旋转变换相同。

根据公式（1-2-2）可以看出，α、β、γ 旋转之后就能得到旋转矩阵R，那么反过来，已知矩阵R ，如何得到 $\alpha,\beta,\gamma$ ，根据公式（1-2-2），可以得到

$\beta=atan2\left(-R_{3,1},\sqrt{{R_{3,2}}^2+{R_{3,3}}^2}\right);$

$\alpha=atan2(R_{2,1}/c\beta,R_{1,1}/c\beta);$

$\gamma=atan2(R_{3,2}/c\beta,R_{3,3}/c\beta);$ (1-2-4)

其实，在这里，可以发现，当 $\beta = \pm 90$ 时， $R_{3,2},R_{3,3},R_{1,1},R_{2,1}$ 全为0，那么公式（1-2-4）就不能这么使用了，那么就有以下公式：

$\beta = 90$ 时，只能求出 $\gamma,\alpha$ 两者之差

$\gamma-\ \alpha=\ atan2\left(R_{1,2},R_{2,2}\right);$ (1-2-5)

$\beta = -90$ 时，只能求出 $\gamma,\alpha$ 两者之和

$\gamma+\alpha=\ atan2\left(-R_{1,2},R_{2,2}\right);$ (1-2-6)

这其实是欧拉角在描述姿态的过程中存在的万向锁问题，而 $\gamma,\alpha$ 具体在实际中的取值，要根据实际情况来定。有些地方会采取将其中一个值强行设置为0来处理。

（三）等效轴法

在前面的姿态描述中，无论是相对坐标系还是固定坐标系，都需要分别旋转三个坐标系才能达到最后的目标，实际中可以通过空间中旋转某一个单位矢量或者轴旋转一定的角度来达到相应的位置，这里就别称为等效轴法。下面介绍两种方法来求从空间的某一个轴以及旋转的角度，来求姿态的旋转矩阵。

i)从几何的矢量的变换来求。

图1.2.3 轴角矢量图

$\overrightarrow{n}{}' = (\overrightarrow{n}\cdot \overrightarrow{v})\overrightarrow{n}$

$V_{\perp } = \overrightarrow{v}-\overrightarrow{n}{}'=\overrightarrow{v}-(\overrightarrow{n} \cdot \overrightarrow{v})\overrightarrow{n}$

$\overrightarrow{W}= \overrightarrow{n}\times V_{\perp} = \overrightarrow{n}\times(\overrightarrow{v}-(\overrightarrow{n}\cdot \overrightarrow{v})\overrightarrow{n}) = \overrightarrow{n}\times \overrightarrow{v}$

$V_{\perp}{}' = cos(\theta)V_{\perp}+sin(\theta)\overrightarrow{W}=cos(\theta)(\overrightarrow{v}-(\overrightarrow{n} \cdot \overrightarrow{v})\overrightarrow{n})+sin(\theta)\overrightarrow{n}\times \overrightarrow{v}$

$\overrightarrow{v}{}'=V_{\perp}{}'+\overrightarrow{n}{}'=(\overrightarrow{n}\cdot \overrightarrow{v})\overrightarrow{n}+cos(\theta)(\overrightarrow{v}-(\overrightarrow{n}\cdot \overrightarrow{v})\overrightarrow{n})+sin(\theta)\overrightarrow{n}\times \overrightarrow{v}$ (1-2-7)

如果从轴的方向上去看图1.2.3的话， ${\overrightarrow{W}}$ 矢量与 $V_{\perp}$ 矢量垂直，如下1.2.4所示：

图1.2.4 轴角矢量俯视图

若空间旋转轴为 $\overrightarrow{n}=\begin{bmatrix} K_X\\ K_Y\\ K_Z \end{bmatrix} = \widehat{K}$ ，而需要旋转矢量假如为姿态坐标系的 $X$

轴，令 $\widehat{v} = \widehat{v_x} = \begin{bmatrix} 1\\ 0\\ 0 \end{bmatrix}$ ，那么就有

$\overrightarrow{v}_x{}'=\left[\begin{matrix}K_XK_X\\K_XK_Y\\K_XK_Z\\\end{matrix}\right]+cos{\left(\theta\right)}\left(\left[\begin{matrix}1\\0\\0\\\end{matrix}\right]-\left[\begin{matrix}K_XK_X\\K_XK_Y\\K_XK_Z\\\end{matrix}\right]\right)+sin{\left(\theta\right)}\left[\begin{matrix}0\\K_Z\\-K_Y\\\end{matrix}\right]$

$=\left[\begin{matrix}K_XK_Xv\theta+cos{\left(\theta\right)}\\K_XK_Yv\theta+sin{\left(\theta\right)}K_Z\\K_XK_Zv\theta-sin{\left(\theta\right)}K_Y\\\end{matrix}\right]$ (1-2-8)

同理可以求得旋转姿态坐标系的 $YZ$ 两轴

最后组成旋转矩阵得

$R_K\left(\theta\right)=\left[\begin{matrix}K_XK_Xv\theta+c\theta&K_XK_Yv\theta-K_Zs\theta&K_XK_Zv\theta+K_Ys\theta\\K_XK_Yv\theta+K_Zs\theta&K_YK_Yv\theta+c\theta&K_YK_Zv\theta-K_Xs\theta\\K_XK_Zv\theta-K_Ys\theta&K_YK_Zv\theta+K_Xs\theta&K_ZK_Zv\theta+c\theta\\\end{matrix}\right]$ (1-2-9)

其中 $v\theta=1-cos{\left(\theta\right)}$

那么最终若空间中某个变换，以 ${\overrightarrow{n}}$ 矢量为轴，旋转角度为 $\theta$ 时，轴角转换公式(1-2-9)即为所求。

ii)从微分方程的角度来求。首先介绍一下旋转矩阵得求导，微分方程的求导方法解

角需要利用旋转矩阵的导数，得

$\dot{R} = \underset{\Delta t->0}{lim}\frac{R(t+\Delta t))-R(t))}{\Delta t}$ (1-2-10)

把 $R(t+\Delta t)$ 写成另个矩阵的组合

$R\left(t+\Delta t\right)=R_K\left(\Delta\theta\right)R\left(t\right)$ (1-2-11)

则 $\dot{R} = \underset{ \Delta t->0}{lim}\frac{(R_k(\Delta \theta)-I_3)R(t)}{\Delta t}$ (1-2-12)

对公式（1-2-9）进行角度微分变换得

$R_K\left(\Delta\theta\right)=\left[\begin{matrix}1&-k_z\Delta\theta&k_y\Delta\theta\\k_z\Delta\theta&1&-k_x\Delta\theta\\-k_y\Delta\theta&k_x\Delta\theta&1\\\end{matrix}\right]$ (1-2-13)

$\dot{R} = (\underset{ \Delta t->0}{lim}\frac{\left[\begin{matrix}1&-k_z\Delta\theta&k_y\Delta\theta\\k_z\Delta\theta&1&-k_x\Delta\theta\\-k_y\Delta\theta&k_x\Delta\theta&1\\\end{matrix}\right]}{\Delta t})R(t)$ (1-2-14)

最后，用 $\Delta t$ 除以这个矩阵，并取极限得

$\dot{R} = \left[\begin{matrix}0&-k_z\dot{\theta}&k_y\dot{\theta}\\k_z\dot{\theta}&0&-k_x\dot{\theta}\\-k_y\dot{\theta}&k_x\dot{\theta}&0\\\end{matrix}\right]R\left(t\right)$ (1-2-15)

令

$S(w)= \left[\begin{matrix}0&-k_z\dot{\theta}&k_y\dot{\theta}\\k_z\dot{\theta}&0&-k_x\dot{\theta}\\-k_y\dot{\theta}&k_x\dot{\theta}&0\\\end{matrix}\right] = \begin{bmatrix} 0 & -\Omega_z &\Omega_y \\ \Omega_z& 0 & -\Omega_x\\ -\Omega_y & \Omega_x & 0 \end{bmatrix}$ (1-2-16)

角速度算子矩阵 $S(w)$ 和角速度矢量 $w$ 是角速度的两种描述，在任意矢径p处引起的线速度为

$V_p=S\left(\omega\right)p=\omega\times\ p$ (1-2-17)

$\omega=\left[\begin{matrix}k_x\dot{\theta}\\k_y\dot{\theta}\\k_z{\dot{\theta}}_z\\\end{matrix}\right]=\left[\begin{matrix}\Omega_x\\\Omega_y\\\Omega_z\\\end{matrix}\right]$

可以得到：

$\dot{R}=S\left(\omega\right)R=\omega\times\ R$ (1-2-18)

$S\left(\omega\right)$ 为反对称矩阵，所有的反对称矩阵的矢量空间记为 $so(3)$ ，这个矢量空间也可以称为群，3示三维，绕n维空间旋转的反对称矩阵为 $so(n)$ ， ${so}({3})\in{R}^{{3}\times{3}}$ 是一个包含整体实域的矢量空间，在这个矢量空间中， ${so}({3})$ 中的元素之和仍在这个群 ${so}({3})$ 中， ${so}({3})$ 中的元素与标量之积仍在这个群 ${so}({3})$ 中，这是群的一些部分性质。而两坐标系之间的所有转换矩阵 ${_B^A}R$ 所形成的矩阵空间称为 ${SO}({3})$ ，称为特殊正交群，空间中的所有旋转描述都在这个群中， ${so}({3})$ 与 ${SO}({3})$ 的关系为从 ${so}({3})$ 到 ${SO}({3})$ 是指数变换，从 ${SO}({3})$ 到 ${so}({3})$ 被称为对数变换。而齐次矩阵 $\ _A^BT$ 所形成的群被称为 ${SE}({3})$ ，空间中的所有旋转平移描述所形成的群为 ${SE}({3})$ 。

这里引申以下群，关于群有如下性质：

若 ${{SO}}({{3}}){\in }{R}^{{3}\times{3}}$ 是满足矩阵相乘运算的群，对于元素可用算子°，构成二元运算的集合G

，这里的算子° 可能是加法、乘法、除法…。如果满足以下条件，则称为群：

封闭性：若g1,g2∈R3×3 ，则g1°g2∈G
单位性：对于任意的g∈G ，一定存在一个单位元素e ，使得g °e= e°g=g ;
可逆性：对于任意的g∈G ，一定存在唯一的逆g-1∈G ，使得g °g-1=g-1°g=e ;
结合性：若g1,g2,g3∈G ，那么g1°g2°g3= g1°(g2°g3)

举个例子，整数所形成的集合，以0为单位元素，以加法为群算子所构成的集合。称为整数加法群，这里注意，群必须指定群算子，可以看到，没有整数减法群，因为不满足第二条合第四条。还有些别的群，这里就不一一列举了。

由上所述， $SO(3)$ 可以简单证明一下就可以得出它是一个以单位矩阵 $I$ 作为单位元素，

以矩阵乘法为群算子的群。

利用微分方程求解轴角，需要旋转矩阵求导的知识。

期望描述空间中连杆绕某一固定轴旋转，如图1.2.5所示，设连杆绕轴Z轴或者空间

某一个轴 $w$ ， $w$ 为单位矢量，从 $q$ 位置运动到 $q(t)$ 位置，那么考虑到连杆在某一时刻的速度 $\dot q(t)$ ，那么根据矢量的叉乘原理可以得到

图1.2.5 连杆绕某一轴旋转

$\dot{q}\left(t\right)=\ \omega\times\ q(t)$ (1-2-19)

利用公式(1-2-17)可得

$\dot{q}\left(t\right)=\ s(\omega)\bullet\ q(t)$ (1-2-20)

根据微分方程可得求解上述公式(1-2-20)可得：

$q\left(t\right)=\ e^{s(\omega)t}\bullet\ q(0)$ (1-2-21)

这里的 $q(0)$ 为初始时连杆的位置。 $e^{s(\omega)t}$ 为矩阵指数：

$e^{s(\omega)t}=\ I+s\left(\omega\right)t+\frac{{(s\left(\omega\right)t)}^2}{2!}+\frac{{(s\left(\omega\right)t)}^3}{3!}+...$ (1-2-22)

将实数 $\theta$ 替换成时间t

$e^{s(\omega)\theta}=\ I+s\left(\omega\right)\theta+\frac{{(s\left(\omega\right)\theta)}^2}{2!}+\frac{{(s\left(\omega\right)\theta)}^3}{3!}+\cdots$ (1-2-23)

对于反对称矩阵有如下性质：

${s\left(\omega\right)}^2=\ s\left(\omega\right)\bullet{s\left(\omega\right)}^T-{|\left|s\left(\omega\right)\right||}^2\bullet\ I$ (1-2-24)

${s\left(\omega\right)}^3=\ -{|\left|s\left(\omega\right)\right||}^2\bullet\ s\left(\omega\right)$ (1-2-25)

利用公式（1-2-24）、（1-2-25）、 $\left|\left|s\left(\omega\right)\right|\right|=1$ 可以将公式（1-2-23）化简得到

$e^{s(\omega)\theta}=\ I+\left(\theta-\frac{\theta^3}{3!}+\frac{\theta^5}{5!}-\cdots\right)\bullet\ s\left(\omega\right)+(\frac{\theta^2}{2!}-\frac{\theta^4}{4!}+\frac{\theta^6}{6!}-\cdots)\bullet{s\left(\omega\right)}^2$ (1-2-26)

因此可得：

$e^{s(\omega)\theta}=\ \ I+s\left(\omega\right)\bullet\ s\ i\ n{\left(\theta\right)}+s\left(\omega\right)\bullet (1-cos(\theta))$ (1-2-27)

上述公式被称为Rodrigues公式， $e^{s(\omega)\theta}$ 也被称为指数积形式，也被称为从so(3) 到SO(3) 的指数变换，当 $\left|\left|s\left(\omega\right)\right|\right|\neq1$ 时，

$e^{s(\omega)\theta}=\ \ I+\frac{s\left(\omega\right)}{\left|\left|s\left(\omega\right)\right|\right|}\bullet\ s\ i\ n{\left(\left|\left|s\left(\omega\right)\right|\right|\bullet\theta\right)}+\frac{{s\left(\omega\right)}^2}{\left|\left|s\left(\omega\right)\right|\right|^2}\bullet(1-cos(\left|\left|s\left(\omega\right)\right|\right|\bullet\theta))$ (1-2-28)

根据上述公式将 $\omega=\overrightarrow{n}=\left[\begin{matrix}K_X\\K_Y\\K_Z\\\end{matrix}\right]=\hat{K}$ 带入公式(1-2-28)或者(1-2-27)，最后得到

$e^{s(\omega)\theta}=\ R_K\left(\theta\right)=\left[\begin{matrix}K_XK_Xv\theta+c\theta&K_XK_Yv\theta-K_Zs\theta&K_XK_Zv\theta+K_Ys\theta\\K_XK_Yv\theta+K_Zs\theta&K_YK_Yv\theta+c\theta&K_YK_Zv\theta-K_Xs\theta\\K_XK_Zv\theta-K_Ys\theta&K_YK_Zv\theta+K_Xs\theta&K_ZK_Zv\theta+c\theta\\\end{matrix}\right]$ (1-2-29)

跟(1-2-9)一致。同样，给定 $R_K(\theta)$ ，反求 $w$ 、 $\theta$ 。

$\theta={\ cos}^{-1}(\frac{Trace\left(R_K\right)-1}{2})$ (1-2-30)

$\omega=\ \frac{1}{2s\theta}\bullet\left[\begin{matrix}R_{K,3,2}-\ R_{K,2,3}\\R_{K,1,3}-\ R_{K,3,1}\\R_{K,2,1}-\ R_{K,1,2}\\\end{matrix}\right]$ (1-2-31)

至此等效轴角的变换及转化完成。

（四）单位四元数

在前面的描述中，可以看到矩阵、欧拉角用三个围绕轴的旋转表示姿态，而轴角用空间中的一个轴与角度表示姿态，但是欧拉角会有万向节问题出现，矩阵形式会相对来说数值的存储量比较多，而单位四元数只用四个数值来表示姿态，四元数并不是生来为了解决旋转姿态问题的，但是它的性质是非常有利于表达旋转信息，它能较好的解决欧拉角万向节问题，相对矩阵也更加高效。非单位四元数是不能表示姿态的，只有单位四元数才可以表示姿态。

$q=\varepsilon_x\bullet\ i+\varepsilon_y\bullet\ j+\varepsilon_z\bullet\ k+v$ (1-2-32)

这里也可以写成一个矢量和标量形式 $q=\overrightarrow{\varepsilon}+v$ 的形式，当v=0 时， $q$ 为纯四元数

${\varepsilon_x}^2+{\varepsilon_y}^2+{\varepsilon_z}^2+v^2=1$ (1-2-33)

$i,j,k$ 为虚数单位， $i^2=\ j^2=\ k^2=\ -1$ , $v$ 为标量, $\varepsilon_x,\varepsilon_y,\varepsilon_z \in R$ , $q$ 整体为复数形式，那么如何理解四元数呢，四元数存在于四维空间，为一个超球体，这里稍微比较难以理解。举一个例子，四维空间理解上是比较费劲，如果我们在三维上理解，那就方便很多，例如：

$x^2+y^2+z^2=1$ (1-2-34)

在空间中表示一个球体的外壳，球面上的点全部满足上述（1-2-34），但是如果从二维的角度来思考， $x^2+y^2=1-z^2$ ，那其实就是三维在二维xy平面的投影，球面上一点在xy平面投影出圆内一点，那么z呢，当z = 0时，其实就是投影到xy平面的上的圆的外轮廓，z≠ 0时，球上的点将投影在xy平面圆内，将所有的球上的点投影到xy平面的圆内，其实构成了实心的圆。同理可以理解公式（1-2-33），单位四元数，在实际中为四维空间的超球体，投影到三维空间为一个球体，当 $v=0$

时，四维空间所有的点投影到三维空间组成球的外轮廓，而当 $v\neq 0$ 时，其实投影到三维空间就是一个实心的球体。

${\varepsilon_x}^2+{\varepsilon_y}^2+{\varepsilon_z}^2=1-v^2$ (1-2-35)

如图1.2.6所示，左图中，球面上一点 $P$ ，投影到xy平面，形成点 $P'$ ，所有的点都投影到二维xy平面形成一个实心圆，右图中，四维空间中一点投影到三维空间形成实心的球体。

图1.2.6 三维空间与四维空间在二维三维空间上的投影

单位四元数的性质有如下：

给定两个四元数 $q_1=[\varepsilon_{x1}\ \varepsilon_{y1}\ \ \varepsilon_{z1}\ v_1]$ 、 $q_2=[\varepsilon_{x2}\ \varepsilon_{y2}\ \ \varepsilon_{z2}\ v_2]$ ，两四元数的乘积表示空间的旋转：

$q_1\bullet\ q_2=\left(v_1\bullet v_2-\varepsilon_{x1}\bullet\varepsilon_{x2}-\varepsilon_{y1}\bullet\varepsilon_{y2}-\varepsilon_{z1}\bullet\varepsilon_{z2}\right)+$

$\left(\ v_1\bullet\varepsilon_{x2}+\varepsilon_{x1}\bullet v_2+\varepsilon_{y1}\bullet\varepsilon_{z2}-\varepsilon_{z1}\bullet\varepsilon_{y2}\right)i+$

$\ \ \left(\ v_1\bullet\varepsilon_{y2}-\varepsilon_{x1}\bullet\varepsilon_{z2}+\varepsilon_{y1}\bullet v_2+\varepsilon_{z1}\bullet\varepsilon_{x2}\right)j+$

$\left(\ v_1\bullet\varepsilon_{z2}+\varepsilon_{x1}\bullet\varepsilon_{y2}-\varepsilon_{y1}\bullet\varepsilon_{x2}+\varepsilon_{z1}\bullet v_2\right)$ (1-2-36)

设 $q$ 可以设置为纯量和向量 $v$ 、矢量 $\varepsilon$ ，那么 $q$ 可以表示为 $v,\varepsilon$ ，故乘法也可以表示为：

$q_1\bullet\ q_2=\left(\ v_1+\varepsilon_1\right)\bullet\left(\ v_2+\varepsilon_2\right)=\left(v_1\bullet\ v_2-\varepsilon_1\bullet\varepsilon_2\right)+v_1\bullet\varepsilon_2+v_2\bullet\varepsilon_1+\varepsilon_2\times\varepsilon_1$ (1-2-37)

四元数的共轭：

$q^\ast=[v,-\varepsilon]$ (1-2-38)

四元数的模：

$\left|q\right|=\ \sqrt{{\varepsilon_x}^2+{\varepsilon_y}^2+{\varepsilon_z}^2+v^2}$ (1-2-39)

四元数的逆：

$q^{-1}=\ \frac{q^\ast}{\left|q\right|^2}$ (1-2-40)

对于单位四元数来说：

$q^{-1}=\ q^\ast$ (1-2-41)

这里就不花大量的篇幅去介绍四元数是如何与旋转产生联系的，如果大家感兴趣，可以网上自行搜索。下面是旋转矩阵与四元数之间的关系：

$R_q=\left[\begin{matrix}1-2{\varepsilon_y}^2-2{\varepsilon_z}^2&2\left(\varepsilon_x\varepsilon_y-\varepsilon_zv\right)&2\left(\varepsilon_x\varepsilon_z+\varepsilon_yv\right)\\2\left(\varepsilon_x\varepsilon_y+\varepsilon_zv\right)&1-2{\varepsilon_x}^2-2{\varepsilon_z}^2&2\left(\varepsilon_y\varepsilon_z-\varepsilon_xv\right)\\2\left(\varepsilon_x\varepsilon_z-\varepsilon_yv\right)&2\left(\varepsilon_y\varepsilon_z+\varepsilon_xv\right)&1-2{\varepsilon_x}^2-2{\varepsilon_y}^2\\\end{matrix}\right]$ (1-2-42)