向量
向量点乘
\[
a \cdot b = \|a\| \|b\|cos(\theta)
\]
两个向量点乘得意义在于计算向量之间的夹角。以及一个向量在另一个向量上的投影的积。当向量为单位向量很容易计算向量之间的夹角。
向量叉乘
\[
a \times b = \left( \|a\|\|b\|sin(\theta) \right) n_0
\]
其中\(n_0\)为垂直两个向量平面的单位向量，方向满足右手定则。
反对称矩阵(Skew-symmetric Matrix)
两个三维列向量\(\boldsymbol{V}_{1} = [V_{1x} V_{1y} V_{1z}]^T\)和\(\boldsymbol{V}_{2} = [V_{2x} V_{2y} V_{2z}]^T\)之间的叉乘(外积)可用行列式计算规则表示：
\[
\boldsymbol{V}_{1} \times \boldsymbol{V}_{2}=\left|\begin{array}{ccc}
{\boldsymbol{i}} & {\boldsymbol{j}} & {\boldsymbol{k}} \\
{V_{1 x}} & {V_{1 y}} & {V_{1 z}} \\
{V_{2 x}} & {V_{2 y}} & {V_{2 z}}
\end{array}\right|=\left[\begin{array}{c}
{V_{1 y} V_{2 z}-V_{1 z} V_{2 y}} \\
{-\left(V_{1 x} V_{2 z}-V_{1 z} V_{2 x}\right)} \\
{V_{1 x} V_{2 y}-V_{1 y} V_{2 x}}
\end{array}\right] \tag{1.1.1}\label{1.1.1}
\]
其中\(i,j,k\)分别为直角坐标系三个坐标轴向的单位矢量。写成矩阵的形式：
\[
\left[\begin{array}{ccc}
{0} & {-V_{1 z}} & {V_{1 y}} \\
{V_{1 z}} & {0} & {-V_{1 x}} \\
{-V_{1 y}} & {V_{1 x}} & {0}
\end{array}\right]\left[\begin{array}{l}
{V_{2 x}} \\
{V_{2 y}} \\
{V_{2 z}}
\end{array}\right]=\left[\begin{array}{c}
{V_{1 y} V_{2 z}-V_{1 z} V_{2 y}} \\
{-\left(V_{1 x} V_{2 z}-V_{1 z} V_{2 x}\right)} \\
{V_{1 x} V_{2 y}-V_{1 y} V_{2 x}}
\end{array}\right] \tag{1.1.2}\label{1.1.2}
\]
称左边的特殊矩阵：
\[
(\boldsymbol{V} \times)=\left[\begin{array}{ccc}
{0} & {-V_{z}} & {V_{y}} \\
{V_{z}} & {0} & {-V_{x}} \\
{-V_{y}} & {V_{x}} & {0}
\end{array}\right] \tag{1.1.3}\label{1.1.3}
\]
为反对称矩阵，即满足\((\boldsymbol{V}_{\times}) = - {(\boldsymbol{V}_{\times})}^T\)。因此两向量之间叉乘运算可以等价为前一向量的反对称矩阵和后一向量之间的矩阵乘法运算：
\[
\boldsymbol{V}_1 \times \boldsymbol{V}_2 = ({\boldsymbol{V}_1}_{\times})\boldsymbol{V}_2 \tag{1.1.4}\label{1.1.4}
\]
反对称矩阵有如下常用的性质：
幂方公式
\[
\begin{aligned}
&(\boldsymbol{V} \times)^{1}=v^{0}(\boldsymbol{V} \times)\\
&(\boldsymbol{V} \times)^{2}=\boldsymbol{V} \boldsymbol{V}^{\mathrm{T}}-\boldsymbol{v}^{2} \boldsymbol{I}=\boldsymbol{v}^{0}(\boldsymbol{V} \times)^{2}\\
&(\boldsymbol{V} \times)^{3}=(\boldsymbol{V} \times)^{2}(\boldsymbol{V} \times)=\left(\boldsymbol{V} \boldsymbol{V}^{\mathrm{T}}-\boldsymbol{v}^{2} \boldsymbol{I}\right)(\boldsymbol{V} \times)=\boldsymbol{V} \boldsymbol{V}^{\mathrm{T}}(\boldsymbol{V} \times)-\boldsymbol{v}^{2}(\boldsymbol{V} \times)=\boldsymbol{V} \cdot \boldsymbol{0}_{\mathrm{IB}}-\boldsymbol{v}^{2}(\boldsymbol{V} \times)=-\boldsymbol{v}^{2}(\boldsymbol{V} \times)\\
&(\boldsymbol{V} \times)^{4}=(\boldsymbol{V} \times)^{3}(\boldsymbol{V} \times)=-v^{2}(\boldsymbol{V} \times)^{2}\\
&(\boldsymbol{V} \times)^{5}=(\boldsymbol{V} \times)^{2}(\boldsymbol{V} \times)^{3}=\left(\boldsymbol{V} \boldsymbol{V}^{\mathrm{T}}-\boldsymbol{v}^{2} \boldsymbol{I}\right)\left[-v^{2}(\boldsymbol{V} \times)\right]=v^{4}(\boldsymbol{V} \times)\\
&(\boldsymbol{V} \times)^{6}=(\boldsymbol{V} \times)^{3}(\boldsymbol{V} \times)^{3}=\left[-v^{2}(\boldsymbol{V} \times)\right]\left[-v^{2}(\boldsymbol{V} \times)\right]=v^{4}(\boldsymbol{V} \times)^{2}
\end{aligned}
\]
即：
\[
\begin{aligned}
&(\boldsymbol{V} \times)^{i}=\left\{\begin{array}{ll}
{(-1)^{(i-1) / 2} v^{i-1}(\boldsymbol{V} \times)} & {i=1,3,5, \cdots} \\
{(-1)^{(i-2) / 2} v^{i-2}(\boldsymbol{V} \times)^{2}} & {i=2,4,6, \ldots}
\end{array}\right. \\
&v=|\boldsymbol{V}|=\sqrt{V_{x}^{2}+V_{y}^{2}+V_{z}^{2}}
\end{aligned}
\tag{1.1.5}\label{1.1.5}
\]
反交换律
\[
({\boldsymbol{V}_1}_{\times})\boldsymbol{V}_2 = \boldsymbol{V}_1 \times \boldsymbol{V}_2 = -(\boldsymbol{V}_2 \times \boldsymbol{V}_1) = - ({\boldsymbol{V}_2}_{\times})\boldsymbol{V}_1 \tag{1.1.6}\label{1.1.6}
\]
三重矢积公式
\[
\boldsymbol{V}_{1} \times\left(\boldsymbol{V}_{2} \times \boldsymbol{V}_{3}\right)=\left(\boldsymbol{V}_{1} \cdot \boldsymbol{V}_{3}\right) \boldsymbol{V}_{2}-\left(\boldsymbol{V}_{1} \cdot \boldsymbol{V}_{2}\right) \boldsymbol{V}_{3}
\]
若令\(\boldsymbol{V}_{1}=\boldsymbol{V}_{2}=\boldsymbol{V}\)，可得：
\[
\begin{aligned}
&\left(\boldsymbol{V} \cdot \boldsymbol{V}_{3}\right) \boldsymbol{V}=\boldsymbol{V} \times\left(\boldsymbol{V} \times \boldsymbol{V}_{3}\right)+\boldsymbol{v}^{2} \boldsymbol{V}_{3} \\
&v=|\boldsymbol{V}|=\sqrt{\boldsymbol{V}^{\mathrm{T}} \boldsymbol{V}}
\end{aligned}
\]
旋转运动
如图所示，质量快A在旋转坐标系World中，沿着\(\vec{v}\)方向匀速运动，相对于Body坐标系下，将是一个曲线运动。
设A在body坐标系下的坐标为：\(p^b = (x, y, z)^T\).忽略平移，则：
\[
p^w(t) = \mathbf{R}^w_b p^b
\]
对时间一阶求导，得：
\[
\begin{aligned}
\dot{p^w} &= \mathbf{R^w_b}\dot{p^b} + \dot{\mathbf{R^w_b}}p^b \\
&=\mathbf{R^w_b}\dot{p^b} + \mathbf{R^w_b} \left[ \omega^b \right]_{\times} p^b \\
&=\mathbf{R^w_b}\dot{p^b} + \left[ \mathbf{R^w_b} \omega^b \right]_{\times} \mathbf{R^w_b} p^b \\
&=\mathbf{R^w_b}{v^b} + \left[ \mathbf{R^w_b} \omega^b \right]_{\times} p^w \\
v &=\mathbf{R^w_b}{v^b} + \left[ \omega \right]_{\times} p^w =\mathbf{R^w_b}{v^b} + \omega {\times} p^w \\
\end{aligned}
\]
其中\(\omega = \mathbf{R}^w_b \omega^b\)表示body坐标系的角速度在World系下的表示。
对时间二阶求导，得：
\[
\begin{aligned}
\ddot{p^w} &= \mathbf{R^w_b}\dot{v^b} + \dot{\mathbf{R^w_b}}v^b + \omega \dot{p^w} + \left[ \dot{\mathbf{R^w_b}} \omega^b + \mathbf{R^w_b} \dot{\omega^b} \right]_{\times}p^w \\
&= \mathbf{R^w_b}\dot{v^b} + \dot{\mathbf{R^w_b}}v^b + \omega \times \left( v + \omega \times p^w \right) + \left[ \mathbf{R}^w_b \dot{\omega^b} \right]_{\times} p^w \\
&= \mathbf{R^w_b} a^b + 2\omega \times v + \omega \times (\omega \times p^w) + \dot{\omega} \times p^w \\
a &= a^w - \underbrace{2\omega \times v}_{科氏加速度} - \underbrace{\dot{\omega} \times p^w}_{欧拉加速度} - \underbrace{\omega \times(\omega \times p^w)}_{向心加速度} \\
\end{aligned}
\]
其中\(v = \mathbf{R}^w_b v^b\)，\(a = \mathbf{R}^w_b a^b\)分别表示body坐标系。
因此质量快A收到合力影响做曲线运动。
罗德里格斯(Rodrigues)旋转公式
假设三维空间矢量\(\boldsymbol{r}\)绕另一单位矢量\(\boldsymbol{u}\)转动\(\phi \geqslant 0\)角度，得矢量\(\boldsymbol{r}^{\prime}\).
设矢量\(\boldsymbol{r}\)和单位矢量\(\boldsymbol{u}\)具有共同起始点\(O\),记\(\boldsymbol{r}\)的矢端\(A\)在\(\boldsymbol{u}\)上的投影为\(O^{\prime}\).以\(O^{\prime}\)为圆心\(O^{\prime}A\)为半径作圆，使\(\boldsymbol{r}^{\prime}\)的矢端\(A^{\prime}\)也在圆周上。在圆上取一点\(B\)使得\(O^{\prime}B \perp O^{\prime}A\)则有：
\[
\begin{aligned}
&\overrightarrow{O^{\prime} B}=\boldsymbol{u} \times \boldsymbol{r}
\end{aligned} \tag{1.2.1} \label{1.2.1}
\]
其中：
\[
\begin{aligned}
&|\overrightarrow{O^{\prime} B}|= |\boldsymbol{u}| \cdot |\boldsymbol{r}| \cdot sin(\theta) = |\overrightarrow{O^{\prime} A}|
\end{aligned}
\]
转动前的矢量\(\boldsymbol{r}\)相对于单位矢量\(\boldsymbol{u}\)可分解为平行于\(\boldsymbol{u}\)的分量\(\boldsymbol{r}_{\|}\)和垂直分量\(\boldsymbol{r}_{\perp}\).
\[
\boldsymbol{r}=\boldsymbol{r}_{\|}+\boldsymbol{r}_{\perp} \tag{1.2.2}\label{1.2.2}
\]
其中：
\[
\begin{aligned}
&{\boldsymbol{r}_{\|}=(\boldsymbol{r} \cdot \boldsymbol{u}) \boldsymbol{u}} \\
&{\boldsymbol{r}_{\perp}=\overline{O^{\prime} B} \times \boldsymbol{u}=(\boldsymbol{u} \times \boldsymbol{r}) \times \boldsymbol{u}}
\end{aligned} \tag{1.2.3}\label{1.2.3}
\]
其中：
\[
|{\boldsymbol{r}_{\|}}| = |\boldsymbol{r}| cos(\theta) =\frac{\boldsymbol{r\cdot u}}{|\boldsymbol{u}|}
\]
同理，转动后的矢量\(\boldsymbol{r}^{\prime}\)相对于\(\boldsymbol{u}\)也可以分解为平行分量和垂直分量：
\[
\boldsymbol{r}^{\prime} = \boldsymbol{r}_{\|}^{\prime}+\boldsymbol{r}_{\perp}^{\prime} \tag{1.2.4}\label{1.2.4}
\]
其中：
\[
\begin{aligned}
&r_{\|}^{\prime}=r_{\|}\\
&r_{\perp}^{\prime}=\overrightarrow{O^{\prime} A} \cos \phi+\overrightarrow{O^{\prime} B} \sin \phi=(\boldsymbol{u} \times \boldsymbol{r}) \times \boldsymbol{u} \cos \phi+\boldsymbol{u} \times \boldsymbol{r} \sin \phi
\end{aligned} \tag{1.2.5}\label{1.2.5}
\]
将1.2.3 和1.2.5 带入1.2.4得：
\[
\boldsymbol{r}^{\prime}=(\boldsymbol{r} \cdot \boldsymbol{u}) \boldsymbol{u}+(\boldsymbol{u} \times \boldsymbol{r}) \times \boldsymbol{u} \cos \phi+\boldsymbol{u} \times \boldsymbol{r} \sin \phi \tag{1.2.6}\label{1.2.6}
\]
由三重矢积公式\((\boldsymbol{V} \cdot \boldsymbol{V_3})\boldsymbol{V} = \boldsymbol{V} \times(\boldsymbol{V} \times \boldsymbol{V}_3) + v^2 \boldsymbol{V}_3\)得：
\[
(\boldsymbol{r} \cdot \boldsymbol{u}) \boldsymbol{u}=(\boldsymbol{u} \cdot \boldsymbol{r}) \boldsymbol{u}=\boldsymbol{u} \times(\boldsymbol{u} \times \boldsymbol{r})+|\boldsymbol{u}|^{2} \boldsymbol{r}=\left[\boldsymbol{I}+(\boldsymbol{u} \times)^{2}\right] \boldsymbol{r} \tag{1.2.7}\label{1.2.7}
\]
将1.2.7带入1.2.6得：
\[
\color{red}{\begin{aligned}
\boldsymbol{r}^{\prime} &=\left[\boldsymbol{I}+(\boldsymbol{u} \times)^{2}\right] \boldsymbol{r}-(\boldsymbol{u} \times)^{2} \boldsymbol{r} \cos \phi+\boldsymbol{u} \times \boldsymbol{r} \sin \phi \\
&=\left[\boldsymbol{I}+\sin \phi(\boldsymbol{u} \times)+(1-\cos \phi)(\boldsymbol{u} \times)^{2}\right] \boldsymbol{r} \\
&=\boldsymbol{D} \boldsymbol{r}
\end{aligned}} \tag{1.2.8}\label{1.2.8}
\]
记：
\[
\boldsymbol{D} = \boldsymbol{I}+\sin \phi(\boldsymbol{u} \times)+(1-\cos \phi)(\boldsymbol{u} \times)^{2} \tag{1.2.9}\label{1.2.9}
\]
称1.2.8为罗德里格斯(Rodrigues)旋转公式。它建立了旋转前后矢量\(\boldsymbol{r}\)和\(\boldsymbol{r}^{\prime}\)之间的线性变换关系，该变换是转轴\(\boldsymbol{u}\)及转角\(\phi\)的函数。
进一步，若记\(\boldsymbol{\phi} = \phi \boldsymbol{u}\)和\(\phi = |\boldsymbol{\phi}|\)则\(\boldsymbol{u} = \boldsymbol{\phi}/\phi\)带入1.2.9得
\[
\boldsymbol{D}=\boldsymbol{I}+\frac{\sin \phi}{\phi}(\boldsymbol{\phi} \times)+\frac{1-\cos \phi}{\phi^{2}}(\boldsymbol{\phi} \times)^{2} \tag{1.2.10}\label{1.2.10}
\]
其中\(\boldsymbol{\phi}\)称为等效旋转矢量(equipment rotation vector),其矢量方向表示转轴方向，而模值大小表示旋转角度大小。从转动的物理含义上看，\((\phi\pm 2k\pi)\boldsymbol{u} (k = 0,1, \cdots)\)表示的是相同的转动。
旋转矩阵
在3D空间中，旋转变换是一种线性变换，因此旋转前后保证了向量的长度以及向量之间的角度，因此旋转变化是刚体运动。
\[
r(\mathbf{v}) = \mathbf{R} \, \mathbf{v} \quad R \in \Bbb{R}^3
\]
对于任意向量\(\mathbf{v} \in \Bbb{R}^3\), 通过旋转操作\(r()\)满足以下性质：
Rotation preserves the vector norm
\[
|| r(\mathbf{v})|| = \sqrt{\langle r(\mathbf{v}), r(\mathbf{v})\rangle} = \sqrt{\langle \mathbf{v}, \mathbf{v} \rangle} = ||\mathbf{v} || \quad \mathbf{v} \in \Bbb{R}^3 \tag{1.3.1}\label{1.3.1}
\]
Rotation preserves angles between vectors
\[
\langle r(\mathbf{v}),r(\mathbf{w}) \rangle = \langle \mathbf{v}, \mathbf{w} \rangle = || \mathbf{v} || || \mathbf{w} || cos \alpha \quad \mathbf{v}, \mathbf{w} \in \Bbb{R}^3 \tag{1.3.2}\label{1.3.2}
\]
Rotation preserves the relative orientations of vectors
\[
\mathbf{u} \times \mathbf{v} = \mathbf{w} \iff r(\mathbf{u}) \times r(\mathbf{v}) = r(\mathbf{w}) \tag{1.3.3}\label{1.3.3}
\]
因此：
\[
(\mathbf{R} \mathbf{v})^T(\mathbf{R} \mathbf{v}) = \mathbf{v}^T \mathbf{R}^T\mathbf{R} \mathbf{v} = \mathbf{v}^T \mathbf{v}
\]
可得到\(\mathbf{R}\)为正交矩阵：
\[
\mathbf{R}^T\mathbf{R} = \mathbf{I} = \mathbf{R}\mathbf{R}^T
\]
对于正交矩阵每列向量\(\mathbf{R} = [\mathbf{r}_1, \mathbf{r}_2, \mathbf{r}_3]\)均为单位长度并且相互正交：
\[
\begin{aligned}
&\langle\mathbf{r}_i, \mathbf{r}_i\rangle = \mathbf{r}_i^T\mathbf{r}_i = 1 \\
&\langle \mathbf{r}_i, \mathbf{r}_j \rangle = \mathbf{r}_i^T \mathbf{r}_j = 0 \quad i \neq j
\end{aligned}
\]
因此旋转矩阵具有一下性质 ：
\[
\begin{aligned}
&\mathbf{R}^{-1} = \mathbf{R}^T \\
&det(\mathbf{R}) = 1
\end{aligned}
\]
四元数(Hamilton)
四元数定义成实部+虚部的形式(scalar + vector)：
\[
Q = q_w + q_x i + q_y j + q_z k = q_w + \boldsymbol{q}_v
\]
因此表示一个四元数为：
\[
\boldsymbol{q} = \left[ \begin{matrix} q_w \\ \boldsymbol{q}_v \end{matrix} \right] = \left[q_w \ q_x \ q_y \ q_w \right]^T \tag{1.4.1}\label{1.4.1}
\]
其中：
\[
\begin{aligned}
&i^2 = j^2 = k^2 = ijk = -1\\
& ij = -ji = k \\
& jk = -kj = i \\
& ki = -ik = j
\end{aligned}
\]
乘法
\[
\mathbf{p} \otimes \mathbf{q}=\left[\begin{array}{l}
{p_{w} q_{w}-p_{x} q_{x}-p_{y} q_{y}-p_{z} q_{z}} \\
{p_{w} q_{x}+p_{x} q_{w}+p_{y} q_{z}-p_{z} q_{y}} \\
{p_{w} q_{y}-p_{x} q_{z}+p_{y} q_{w}+p_{z} q_{x}} \\
{p_{w} q_{z}+p_{x} q_{y}-p_{y} q_{x}+p_{z} q_{w}}
\end{array}\right] = \left[\begin{array}{c}
{p_{w} q_{w}-\mathbf{p}_{v}^{\top} \mathbf{q}_{v}} \\
{p_{w} \mathbf{q}_{v}+q_{w} \mathbf{p}_{v}+\mathbf{p}_{v} \times \mathbf{q}_{v}}
\end{array}\right]
\tag{1.4.2}\label{1.4.2}
\]
四元数乘法不满足交换律(not commutative):
\[
\boldsymbol{p} \otimes \boldsymbol{q} \neq \boldsymbol{q} \otimes \boldsymbol{p}
\]
满足结合律(associative):
\[
(\boldsymbol{p} \otimes \boldsymbol{q}) \otimes \boldsymbol{r} = \boldsymbol{p} \otimes (\boldsymbol{q} \otimes \boldsymbol{r})
\]
满足加法分配率(distributive):
\[
\begin{aligned}
&\boldsymbol{p} \otimes(\boldsymbol{q} + \boldsymbol{r}) = \boldsymbol{p} \otimes \boldsymbol{q} + \boldsymbol{p} \otimes \boldsymbol{r} \\
& (\boldsymbol{p} + \boldsymbol{q}) \otimes \boldsymbol{r} = \boldsymbol{p} \otimes \boldsymbol{r} + \boldsymbol{q} \otimes \boldsymbol{r}
\end{aligned}
\]
对于四元数乘法可以写成矩阵的形式：
\[
\boldsymbol{q}_1 \otimes \boldsymbol{q}_2 = \left[\boldsymbol{q}_1\right]_L \boldsymbol{q}_2 \quad and \quad \boldsymbol{q}_1 \otimes \boldsymbol{q}_2 = \left[ \boldsymbol{q}_2 \right]_R \boldsymbol{q}_1 \tag{1.4.3}\label{1.4.3}
\]
其中：
\[
\begin{aligned}
& [\mathbf{q}]_{L}
=\left[\begin{array}{cccc}
{q_{w}} & {-q_{x}} & {-q_{y}} & {-q_{z}} \\
{q_{x}} & {q_{w}} & {-q_{z}} & {q_{y}} \\
{q_{y}} & {q_{z}} & {q_{w}} & {-q_{x}} \\
{q_{z}} & {-q_{y}} & {q_{x}} & {q_{w}}
\end{array}\right]
= q_{w} \mathbf{I}+\left[\begin{array}{cc}
{0} & {-\mathbf{q}_{v}^{\top}} \\
{\mathbf{q}_{v}} & {\left[\mathbf{q}_{v}\right]_{\times}}
\end{array}\right] \\
& [\mathbf{q}]_{R}
=\left[\begin{array}{cccc}
{q_{w}} & {-q_{x}} & {-q_{y}} & {-q_{z}} \\
{q_{x}} & {q_{w}} & {q_{z}} & {-q_{y}} \\
{q_{y}} & {-q_{z}} & {q_{w}} & {q_{x}} \\
{q_{z}} & {q_{y}} & {-q_{x}} & {q_{w}}
\end{array}\right]
= q_{w} \mathbf{I}+\left[\begin{array}{cc}
{0} & {-\mathbf{q}_{v}^{\top}} \\
{\mathbf{q}_{v}} & {-\left[\mathbf{q}_{v}\right]_{\times}}
\end{array}\right]
\end{aligned} \tag{1.4.4}\label{1.4.4}
\]
因此：
\[
(\mathbf{q} \otimes \mathbf{x}) \otimes \mathbf{p} = \left[ \mathbf{p} \right]_R \left[ \mathbf{q} \right]_L \mathbf{x} = \mathbf{q} \otimes (\mathbf{x} \otimes \mathbf{p}) = \left[ \mathbf{q} \right]_L \left[ \mathbf{p} \right]_R \mathbf{x} \tag{1.4.5}\label{1.4.5}
\]
四元数表示旋转
类似复数的三角表示法，四元数也可以表示为三角函数的形式：
\[
\mathbf{q} = |\mathbf{q}|(cos\theta/2 + \mathbf{u}sin\theta/2)
\]
特别的当\(|\mathbf{q}| = 1\)时，即对单位四元数：
\[
\mathbf{q} = \left[ \begin{matrix} q_w \\ \boldsymbol{q}_v \end{matrix} \right] = \left[ \begin{matrix} cos \theta/2 \\ \boldsymbol{u} sin \theta/2 \end{matrix} \right] \tag{1.4.6}\label{1.4.6}
\]
其中\(\mathbf{u}\)为单位长度三维矢量，表示旋转轴，即\(\mathbf{u}^T\mathbf{u} = 1\)，\(\theta\)表示轴角。因此\(q_0^2 + \mathbf{q}^T\mathbf{q} = 1\)为单位四元数。
四元数与旋转矩阵的关系
由1.2.9，并将\(cos \theta/2 = q_w \quad \mathbf{u} sin \theta/2 = \mathbf{q}_v\)带入。得：
\[
\begin{aligned}
\mathbf{D} &= \mathbf{I}+\sin \phi(\mathbf{u} \times)+(1-\cos \phi)(\mathbf{u} \times)^{2} \\
&= \mathbf{I} + 2 sin \theta/2 \ cos \theta/2 (\mathbf{u}_\times) + 2 sin^2 \theta/2 \ (\mathbf{u}_\times)^2 \\
&= \mathbf{I} + 2 cos \theta/2 (sin \theta/2 \ \mathbf{u}_\times) + 2 (sin \theta/2 \ \mathbf{u}_\times)^2 \\
&= \mathbf{I} + 2 q_w([\mathbf{q}_v]_\times) + 2([\mathbf{q}_v]_\times)^2 \\
&= \left[\begin{matrix}
1 - 2(q^2_y + q^2_z) & 2(q_xq_y - q_wq_z) & 2(q_xq_z + q_wq_y) \\
2(q_xq_y + q_wq_z) & 1 - 2(q^2_x + q^2_z) & 2(q_yq_z - q_wq_x) \\
2(q_xq_z - q_wq_y) & 2(q_yq_z + q_wq_x) & 1 - 2(q^2_x + q^2_y)
\end{matrix}\right] \\
&= \left[\begin{matrix}
q^2_w + q^2_x - q^2_y - q^2_z & 2(q_xq_y - q_wq_z) & 2(q_xq_z + q_wq_y) \\
2(q_xq_y + q_wq_z) & q^2_w - q^2_x + q^2_y - q^2_z & 2(q_yq_z - q_wq_x) \\
2(q_xq_z - q_wq_y) & 2(q_yq_z + q_wq_x) & q^2_w - q^2_x - q^2_y + q^2_z
\end{matrix}\right]
\end{aligned}
\tag{1.4.7}\label{1.4.7}
\]
建立了单位四元数与方向余弦矩阵之间的关系。
设一个三维矢量\(\mathbf{r}\),在动坐标系(b 系)和参考坐标系(i 系)的投影坐标分别为\(\mathbf{r}^b\)和\(\mathbf{r}^i\)， 先对矢量\(\mathbf{r}^b\)(零标量四元数)实施四元数乘法操作：
\[
\begin{aligned}
\mathbf{q}^i_b \otimes \mathbf{r}^b \otimes {\mathbf{q}^i_b}^{-1}
&= \mathbf{q}^i_b \otimes \mathbf{r}^b \otimes {\mathbf{q}^i_b}^{*}
= \mathbf{q}^i_b \otimes \mathbf{r}^b \otimes \mathbf{q}^b_i
= \left[ \mathbf{q}^i_b \right]_L \left[ \mathbf{q}^b_i \right]_R \left[ \begin{matrix} 0 \\ \mathbf{r}^b \end{matrix} \right]\\
&= \left[\begin{matrix}
1 & 0 & 0 & 0 \\
0 & q^2_w + q^2_x - q^2_y - q^2_z & 2(q_xq_y - q_wq_z) & 2(q_xq_z + q_wq_y) \\
0 & 2(q_xq_y + q_wq_z) & q^2_w - q^2_x + q^2_y - q^2_z & 2(q_yq_z - q_wq_x) \\
0 & 2(q_xq_z - q_wq_y) & 2(q_yq_z + q_wq_x) & q^2_w - q^2_x - q^2_y + q^2_z
\end{matrix}\right] \left[ \begin{matrix} 0 \\ {r}_x^b \\ {r}_y^b \\ {r}_z^b \end{matrix} \right] \\
&= \left[ \begin{matrix} 1 & \mathbf{0} \\ \mathbf{0} & C^i_b \end{matrix} \right] \left[ \begin{matrix} 0 \\ \mathbf{r}^b \end{matrix} \right] \\
&= \left[ \begin{matrix} 0 \\ \mathbf{r}^i \end{matrix} \right]
\end{aligned} \tag{1.4.8}\label{1.4.8}
\]
可定义四元数与三维矢量的乘法运算为坐标旋转公式。
Double Cover
四元数与3D旋转并不是一一对应的，同一个3D旋转可以使用两个四元数表示，对任意单位四元数\(q = [cons(\theta/2),sin(\theta/2)\boldsymbol{u}]\)，\(q\)与\(-q\)表示的是用一个旋转，分别表示绕着旋转轴\(\boldsymbol{u}\)旋转\(\theta\)角度和绕着旋转轴\(-\boldsymbol{u}\)旋转\(2\pi - \theta\)角度：
\[
-q = [-cos(\theta/2), -sin(\theta/2)\boldsymbol{u}] = [cos(\pi - \theta/2), sin(\pi - \theta/2)(-\boldsymbol{u})]
\]
由四元数旋转公式：
\[
(-q)v(-q)^{*} = (-1)^2qvq^* = qvq^*
\]
因此两个四元数表示的旋转等价，称为双倍覆盖(double cover)。
四元数微分方程
对于连续形式下的旋转状态递推：
\[
q_{b_{k+1}}^{w}=q_{b_{k}}^{w} \otimes \int_{t \in[k, k+1]} \dot{q}_{t} d t
\]
由四元数旋转公式，当旋转一段微小时间(t)，即角度趋向于0时（\(sin\theta \approx \theta \quad cos\theta \approx 1\)），得：
\[
\Delta\mathbf{q} = q^{t}_{t+\delta t} = \left[ \begin{matrix} q_w \\ \boldsymbol{q}_v \end{matrix} \right] = \left[ \begin{matrix} cos \delta\theta/2 \\ \boldsymbol{u} sin \delta\theta/2 \end{matrix} \right]
\approx \left[\begin{matrix} 1 \\ u \delta\theta/2 \end{matrix}\right]
\]
角速度：
\[
\omega = \lim\limits_{x\rightarrow0}\frac{u\delta \theta/2}{\delta t}
\]
因此四元数微分方程：
\[
\begin{aligned}
\dot{q_t} &= \lim\limits_{\delta t\rightarrow0}\frac{q_{t + \delta t} - q_t}{\delta t} \\
&= \lim\limits_{x\rightarrow0}\frac{q_t \bigotimes q_{t+\delta t}^{t} - q_t \bigotimes \left[\begin{matrix} 1 \\ 0 \end{matrix}\right] }{\delta t} \\
&= \lim\limits_{x\rightarrow0}\frac{q_t \bigotimes \left( \left[\begin{matrix} 1 \\ u \delta\theta/2 \end{matrix}\right] - \left[\begin{matrix} 1 \\ 0 \end{matrix}\right] \right)}{\delta t} \\
&= q_t \bigotimes \left[\begin{matrix} 0 \\ \omega/2 \end{matrix}\right]
\end{aligned}
\]
李群李代数伴随性质（Adjoint）
对于\(SO_3\),有：
\[
R\exp (p^{\wedge})R^T = \exp((Rp)^{\wedge})
\]
证明：
\[
a^{\wedge}v = a\times v
\]
我们可以通过使等式的RHS作用于任意向量v来证明该等式：
\[
\begin{aligned}
(Ra)^{\wedge}v &= (Ra)\times v
\\&=(Ra)\times (RR^{-1}v) &(RR^{-1}=I)
\\&=R[a \times(R^{-1}v)] &(分配律)
\\&=Ra^{\wedge}R^{-1}v &(结合律)
\end{aligned}
\]
因此得到：
\[
(Ra)^{\wedge}=Ra^{\wedge}R^{-1}
\]
而R是正交矩阵，因此:
\[
(Ra)^{\wedge}=Ra^{\wedge}R^{T}
\]
令\(\rho=\theta a\)：
\[
\begin{aligned}
R\exp(\theta a^{\wedge})R^T & = R(\cos\theta I+(1-\cos\theta)aa^T+\sin\theta a^{\wedge})R^T\\
& = \cos\theta I+(1-cos\theta)Ra(Ra)^T+sin\theta Ra^{\wedge}R^T\\
&=\cos\theta I+(1-cos\theta)Ra(Ra)^T+sin\theta (Ra)^{\wedge}\\
&= \exp(\theta (Ra)^{\wedge})\\
&= \exp((R\rho)^{\wedge})
\end{aligned}
\]
常用四元数求导案例(右乘扰动方式)
-
\(f(q^w_b) = q^w_b * v^b\)
\[ \begin{aligned} \frac{\partial f\left(q_{b}^{w}\right)}{\partial q_{b}^{w}} &= \lim _{\delta \theta_{b}^{w} \rightarrow 0} \frac{ {R}_{b}^{w} exp(\left[\delta \theta_{b}^{w}\right]_{\times}) \left( v^b \right) - {R}_{b}^{w} \left( v^b \right) }{\delta \theta_{b}^{w}} \\ &\approx \lim_{\delta \theta_{b}^{w} \rightarrow 0} \frac{ {R}_{b}^{w} (I^{3 \times 3} + \left[ \delta \theta_{b}^{w} \right]_\times) \left( v^b \right) - {R}_{b}^{w} \left( v^b \right) }{\delta \theta_{b}^{w}} (泰勒展开)\\ &= \lim _{\delta \theta_{b}^{w} \rightarrow 0} \frac{ {R}_{b}^{w} \left[ \delta \theta_{b}^{w} \right]_\times \left( v^b \right) }{\delta \theta_{b}^{w}} \\ &= \lim _{\delta \theta_{b}^{w} \rightarrow 0} \frac{ -{R}_{b}^{w} \delta \theta_{b}^{w} \left[ \left( v^b \right)\right]_\times }{\delta \theta_{b}^{w}} (叉乘性质)\\ &= -{R}_{b}^{w} \left[ \left( v^b \right)\right]_\times \\ \end{aligned} \] -
\(f(v^b) = q^w_b * v^b\)
\[ \begin{aligned} \frac{\partial f\left(v^b\right)}{\partial v^b} &= \lim _{\delta v^b \rightarrow 0} \frac{\mathbf{R} (v + \delta v) - \mathbf{R}v}{\delta v} = \mathbf{R} \end{aligned} \] -
\(f(q^w_b) = (q^w_b)^{-1} * v^w\)
\[ \begin{aligned} \frac{\partial f\left(q_{b}^{w}\right)}{\partial q_{b}^{w}} &= \lim _{\delta \theta_{b}^{w} \rightarrow 0} \frac{ \left({R}_{b}^{w} exp(\left[\delta \theta_{b}^{w}\right]_{\times}) \right)^{-1} \left( v^w \right) - {R}_{b}^{w} \left( v^w \right) }{\delta \theta_{b}^{w}} \\ &= \lim _{\delta \theta_{b}^{w} \rightarrow 0} \frac{ \left( exp(\left[-\delta \theta_{b}^{w}\right]_{\times}) \left({R}_{b}^{w}\right)^{-1} \right) \left( v^w \right) - {R}_{b}^{w} \left( v^w \right) }{\delta \theta_{b}^{w}} \\ &\approx \lim_{\delta \theta_{b}^{w} \rightarrow 0} \frac{ \left( (I^{3 \times 3} - \left[ \delta \theta_{b}^{w} \right]_\times)({R}_{b}^{w})^{-1}\right) \left( v^w \right) - {R}_{b}^{w} \left( v^w \right) }{\delta \theta_{b}^{w}} \\ &= \lim _{\delta \theta_{b}^{w} \rightarrow 0} \frac{ -\left[ \delta \theta_{b}^{w} \right]_\times \left(({R}_{b}^{w})^{-1} \left( v^w \right)\right) }{\delta \theta_{b}^{w}} \\ &= \lim _{\delta \theta_{b}^{w} \rightarrow 0} \frac{ \left[({R}_{b}^{w})^{-1} \left( v^w \right)\right]_{\times} \delta \theta_{b}^{w} }{\delta \theta_{b}^{w}} \\ &= \left[({R}_{b}^{w})^{-1} \left( v^w \right)\right]_{\times} \\ \end{aligned} \]
李群李代数
SO(3)的伴随性质
Reference
[1] <捷联惯导算法与组合导航原理讲义> [2] <视觉SLAM十四讲> [3] Quaternion kinematics for the error-state Kalman filter [4] A micro Lie theory for state estimation in robotics [5] https://opensource.docs.anymal.com/doxygen/kindr/master/cheatsheet_latest.pdf [6] https://quaternions.online/| [7] https://eater.net/quaternions/ [8] https://www.bilibili.com/video/av33385105 [] https://www.zhihu.com/question/23005815?sort=created [] http://danceswithcode.net/engineeringnotes/rotations_in_3d/demo3D/rotations_in_3d_tool.html [] Robot Dynamics Lecture Notes--Robotic Systems Lab, ETH Zurich [] http://underactuated.csail.mit.edu/index.html [] https://krasjet.github.io/quaternion/quaternion.pdf