文章翻译—基于误差状态卡尔曼滤波器的四元数运动学—第2章

2. 旋转和互相关

2.1 3D向量旋转公式

$图1：向量\mathbf{x}绕轴\mathbf{u}旋转\varphi 角度。详情见文本。$

我们在图1中说明了遵循右手法则，一般的3D向量$\mathbf{x}$绕定义的单位矢量$\mathbf{u}$旋转角度$\varphi$。通过将向量$\mathbf{x}$分解成与$\mathbf{u}$平行的部分${\mathbf{x}}_{||}$和正交的部分${\mathbf{x}}_{\perp }$，即
$$\mathbf{x}={\mathbf{x}}_{||}+{\mathbf{x}}_{\perp }.$$

这些部分可以被容易地计算出来（$\alpha$是向量$\mathbf{x}$和轴$\mathbf{u}$之间的夹角），
$$\begin{gathered} {\mathbf{x}}_{||}=\mathbf{u}(||\mathbf{x}||\cos \alpha )={\mathbf{u}\mathbf{u}}^{\top }\mathbf{x} \\ {\mathbf{x}}_{\perp }=\mathbf{x}-{\mathbf{x}}_{||}=\mathbf{x}-{\mathbf{u}\mathbf{u}}^{\top }\mathbf{x}. \end{gathered}$$

【 注，$||\mathbf{x}||\cos \alpha =||\mathbf{u}||\cdot ||\mathbf{x}||\cos \alpha =\mathbf{u}\cdot \mathbf{x}={\mathbf{u}}^{\top }\mathbf{x}$ 】

在旋转时，平行部分不旋转，
$${\mathbf{x}}_{||}^{\prime }={\mathbf{x}}_{||},$$

而正交部分在垂直于$\mathbf{u}$的平面中经过平面旋转。即，如果我们创建一个该平面的正交基 { e 1 , e 2 } \{\mathbf{e}_1, \mathbf{e}_2\} { e1​,e2​}
$$\begin{gathered} {\mathbf{e}}_1={\mathbf{x}}_{\perp } \\ {\mathbf{e}}_2={\mathbf{u}\times \mathbf{x}}_{\perp }=\mathbf{u}\times \mathbf{x}, \end{gathered}$$

【注，这里${\mathbf{e}}_1$和${\mathbf{e}}_2$应该不是单位的；$||{\mathbf{e}}_2||=||\mathbf{u}\times \mathbf{x}||=||\mathbf{u}||\cdot ||{\mathbf{x}}_{\perp }||\cdot \sin (\pi /2)=||{\mathbf{x}}_{\perp }||=||{\mathbf{e}}_1||$，向量$\mathbf{u}$和${\mathbf{x}}_{\perp }$垂直，夹角为$\pi /2$】

满足$||{\mathbf{e}}_1||=||{\mathbf{e}}_2||$，那么${\mathbf{x}}_{\perp }={\mathbf{e}}_1\cdot 1+{\mathbf{e}}_2\cdot 0$ 。对于一个在该平面的$\varphi$弧度的旋转有，
$${\mathbf{x}}_{\perp }^{\prime }={\mathbf{e}}_1\cos \varphi +{\mathbf{e}}_2\sin \varphi ,$$

进一步的，
$${\mathbf{x}}_{\perp }^{\prime }={\mathbf{x}}_{\perp }\cos \varphi +(\mathbf{u}\times \mathbf{x})\sin \varphi .$$

加上平行部分可得出旋转矢量的表达式，${\mathbf{x}}^{\prime }={\mathbf{x}}_{||}^{\prime }+{\mathbf{x}}_{\perp }^{\prime }$，这就是所谓的vector rotation formula（向量旋转公式），
$${\mathbf{x}}^{\prime }={\mathbf{x}}_{||}+{\mathbf{x}}_{\perp }\cos \varphi +(\mathbf{u}\times \mathbf{x})\sin \varphi .\text{\hspace{1em}(54)}$$

2.2 旋转群$SO(3)$

在${\mathbb{R}}^3$中，旋转群$SO(3)$是在复合作用下绕原点旋转的群。旋转是保持向量长度和相对向量方向（即旋向性）的线性变换。它在机器人学中的重要性在于表达刚体在3D空间中的旋转：一个rigid motion（刚体运动） 精确地要求刚体在运动时必须保持距离，角度和相对方向。否则，如果不能保持模长，角度或相对方向，则不能认为物体是刚性的。
    然后让我们通过满足如下属性的操作来定义旋转。可以根据欧几里得空间的度量，来定义一个作用在向量$\mathbf{v}\in {\mathbb{R}}^3$上的旋转操作$r:{\mathbb{R}}^3\to {\mathbb{R}}^3;\mathbf{v}\mapsto r(\mathbf{v})$ ，由如下的点乘和叉乘组成。

• 旋转保持向量的范数，
  $$||r(\mathbf{v})||=\sqrt{⟨r(\mathbf{v}),r(\mathbf{v})⟩}=\sqrt{⟨\mathbf{v},\mathbf{v}⟩}\triangleq ||\mathbf{v}||,\forall \mathbf{v}\in {\mathbb{R}}^3.\text{\hspace{1em}(55a)}$$

• 旋转保持向量间的角度，
  $$⟨r(\mathbf{v}),r(\mathbf{w})⟩=⟨\mathbf{v},\mathbf{w}⟩=||\mathbf{v}||||\mathbf{w}||\cos \alpha ,\forall \mathbf{v},\mathbf{w}\in {\mathbb{R}}^3.\text{\hspace{1em}(55b)}$$

• 旋转保持向量的相对旋转，
  $$\mathbf{u}\times \mathbf{v}=\mathbf{w}⟺r(\mathbf{u})\times r(\mathbf{v})=r(\mathbf{w}).\text{\hspace{1em}(56)}$$

很容易证明，前面2个条件恒成立。因此我们可以定义如下旋转群$SO(3)$，
S O ( 3 )   :   { r   : R 3 → R 3   /    ∀ v ,   w ∈ R 3 ,    ∣ ∣ r ( v ) ∣ ∣ = ∣ ∣ v ∣ ∣ ,    r ( v ) × r ( w ) = r ( v × w ) }   . (57) SO(3)~:~\{r~: \mathbb{R}^3 \to \mathbb{R}^3 ~/ ~~\forall \mathbf{v,~w} \in \mathbb{R}^3,~~||r(\mathbf{v})||=||\mathbf{v}||,~~r(\mathbf{v}) \times r(\mathbf{w}) = r(\mathbf{v \times w} ) \}~.\tag{57} SO(3) : { r :R3→R3 /  ∀v, w∈R3,  ∣∣r(v)∣∣=∣∣v∣∣,  r(v)×r(w)=r(v×w)} .(57)

    旋转群通常由一组旋转矩阵表示。但是，四元数也组成了旋转群的一个很好表达。本节的旨在表明这两种表达同样有效。正如读者在表1中看到的那样，它们在概念上和代数上展现了很多的相似性。可能，最重要的不同是，单位四元数群组成了$SO(3)$（因此，从技术上讲$SO(3)$本身并不是这样的）的双覆盖，有时这在我们的大多数应用中并不重要【5】。该表是为了快速比较和评估而预先插入的。$SO(3)$的旋转矩阵和四元数的表达将在下一节进行探究。

    【注5，当在旋转空间进行插值时，需要考虑双覆盖的效果。这如如2.7节那样很容易。】

表1：用于表达SO(3)的旋转矩阵和四元数

2.3 旋转群和旋转矩阵

操作$r()$是线性的，因为它是从在标量和向量的乘积中定义的。因此它可以通过矩阵$\mathbf{R}\in {\mathbb{R}}^{3\times 3}$来表达，它通过矩阵乘法产生对向量$\mathbf{v}\in {\mathbb{R}}^3$的旋转，
$$r(v)=\mathbf{R}\mathbf{v}.\text{\hspace{1em}(58)}$$

将其插入到式（55a）中，使用点乘 $⟨\mathbf{a},\mathbf{b}⟩={\mathbf{a}}^{\top }\mathbf{b}$，并将其应用到所有的$\mathbf{v}$，
$$(\mathbf{R}\mathbf{v}{)}^{\top }\mathbf{R}\mathbf{v}={\mathbf{v}}^{\top }{\mathbf{R}}^{\top }\mathbf{R}\mathbf{v}={\mathbf{v}}^{\top }\mathbf{v},\text{\hspace{1em}(59)}$$

得到$\mathbf{R}$的orthogonality（正交性） 条件，
$${\mathbf{R}}^{\top }\mathbf{R}=\mathbf{I}=\mathbf{R}{\mathbf{R}}^{\top }.\text{\hspace{1em}(60)}$$
上面的条件确实是正交的条件，因为我们可以从中观察到，用$\mathbf{R}=[{\mathbf{r}}_1,{\mathbf{r}}_2,{\mathbf{r}}_3]$代替上式，$\mathbf{R}$的列向量 r i ,   i ∈ { 1 , 2 , 3 } \mathbf{r}_i,~i \in \{1,2,3\} ri​, i∈{ 1,2,3}是单位长度且相互正交，
$$\begin{gathered} ⟨{\mathbf{r}}_i,{\mathbf{r}}_i⟩={\mathbf{r}}_i^{\top }{\mathbf{r}}_i=1 \\ ⟨{\mathbf{r}}_i,{\mathbf{r}}_j⟩={\mathbf{r}}_i^{\top }{\mathbf{r}}_j=0,i\ne j. \end{gathered}$$

因此，保持向量模长和角度的一组变换称为Orthogonal group（正交群），用$O(3)$表示。该正交群包括旋转（对于刚体运动）和投影（对于非刚体）。这里 group（群） 的概念本质上意味着两个正交矩阵的乘积总是正交的【6】，并且每个正交矩阵都有一个逆矩阵。事实上，正交性条件（60）意味着逆旋转可通过转置的矩阵实现，
$${\mathbf{R}}^{-1}={\mathbf{R}}^{\top }.\text{\hspace{1em}(61)}$$

【注6：令${\mathbf{Q}}_1$和${\mathbf{Q}}_2$正交，并创建$\mathbf{Q}={\mathbf{Q}}_1{\mathbf{Q}}_2$。那么 ${\mathbf{Q}}^{\top }\mathbf{Q}={\mathbf{Q}}_2^{\top }{\mathbf{Q}}_1^{\top }{\mathbf{Q}}_1{\mathbf{Q}}_2={\mathbf{Q}}_2^{\top }\mathbf{I}{\mathbf{Q}}_2=\mathbf{I}$。】

    添加相对方向条件（56）可确保刚体运动（因此抛弃投影），从而导致$\mathbf{R}$上的一个额外的约束【7】，
$$\det (\mathbf{R})=1.\text{\hspace{1em}(62)}$$

单位行列式大于零的正交矩阵通常称为proper（适当，合适）或special（特殊）。该特殊正交矩阵的集合是$O(3)$的一个子群，即Special Orthogonal group（特殊正交群）$SO(3)$。作为一个群，两个旋转矩阵的乘积总是旋转矩阵【8】。

    【注7，注意，满足$|\mathbf{R}|=\det (\mathbf{R})=-1$的映射，不形成一个群，因为$|{\mathbf{R}}_1{\mathbf{R}}_2|=1\ne -1$）
    【注8，在脚注【6】中，对于$O(3)$，将$|{\mathbf{R}}_1|=|{\mathbf{R}}_2|=1$将加入$SO(3)$，那么有$|{\mathbf{R}}_1{\mathbf{R}}_2|=|{\mathbf{R}}_1||{\mathbf{R}}_2|=1$。】

2.3.1 指数映射

指数映射（以及下一节中提到的对数映射）是一个强大的数学工具，用于在旋转的3D空间中轻松而严谨地工作。它代表了适合旋转空间的无穷微积分的入口。指数映射允许我们合适地定义微分，扰动和速度，并操作它们。因此，在估计旋转或方向空间中的问题时是必不可少。
    旋转构成刚性运动。这种刚性意味着可以在$SO(3)$中定义一条连续的轨迹或路径$r(t)$，以使刚体从其初始方向$r(0)$连续旋转到当前方向$r(t)$。由于是连续的，因此研究此类转换的时间导数是合理的。对此，我们通过推导刚刚提到的属性（60）和（62）来实现。
    首先，我们注意到在满足（60）的同时不可能持续地逃避单位行列式条件（62），因为这将意味着行列式从+1跳到-1【9】。因此我们只需要探讨正交条件（60）的时间微分，即
$$\frac{d}{dt}({\mathbf{R}}^{\top }\mathbf{R})={\dot{\mathbf{R}}}^{\top }\mathbf{R}+{\mathbf{R}}^{\top }\dot{\mathbf{R}}=0,\text{\hspace{1em}(63)}$$

    【注9，换句话说，一个旋转不能通过一个持续的变换来变成一个映射。】

可以推出
$${\mathbf{R}}^{\top }\dot{\mathbf{R}}=-({\mathbf{R}}^{\top }\dot{\mathbf{R}}{)}^{\top },\text{\hspace{1em}(64)}$$

这意味着矩阵${\mathbf{R}}^{\top }\dot{\mathbf{R}}$是反对称的（即等于其转置的负数）。反对称的$3\times 3$矩阵可以用符号$\mathfrak{s}\mathfrak{o}(3)$表示，名为$SO(3)$的李代数。反对称$3\times 3$矩阵有如下形式，
$$[\mathbf{\omega }{]}_{\times }\triangleq \left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right];\text{\hspace{1em}(65)}$$

它们有3个自由度，与我们在（20）中已经引入的叉乘矩阵相对应。这建立了一个一对一的映射$\mathbf{\omega }\in {\mathbb{R}}^3\leftrightarrow [\mathbf{\omega }{]}_{\times }\in so(3)$。然后让我们取一个向量$\mathbf{\omega }=({\omega }_x,{\omega }_y,{\omega }_z)\in R^3$，并写成
$${\mathbf{R}}^{\top }\dot{\mathbf{R}}=[\mathbf{\omega }{]}_{\times }.\text{\hspace{1em}(66)}$$

这就得到了常微分方程(ODE)，
$$\dot{\mathbf{R}}=\mathbf{R}[\mathbf{\omega }{]}_{\times }.\text{\hspace{1em}(67)}$$

在单位元附近，我们有$\mathbf{R}=\mathbf{I}$，然后上述等式可以简化成$\dot{\mathbf{R}}=[\mathbf{\omega }{]}_{\times }$。因此，我们可以将李代数$\mathfrak{s}\mathfrak{o}(3)$解释为$r(t)$在原点的微分空间；它构造了$SO(3)$的正切空间，或者速度空间。基于这些事实，我们可以很好地称$\mathbf{\omega }$为即时的角速度向量。

    如果$\mathbf{\omega }$是常量，上面的差分方程可以变为时间积分的形式
$$\mathbf{R}(t)=\mathbf{R}(0)e^{[\mathbf{\omega }{]}_{\times }t}=\mathbf{R}(0)e^{[\mathbf{\omega }t{]}_{\times }}\text{\hspace{1em}(68)}$$

其中指数$e^{[x{]}_{\times }}$由泰勒级数定义，我们将在下一节提到。因为$\mathbf{R}(0)$和$\mathbf{R}(t)$是旋转矩阵，那么很明显$e^{[\omega t{]}_{\times }}=\mathbf{R}(0{)}^{\top }\mathbf{R}(t)$是一个旋转矩阵。定义向量$\mathbf{v}\triangleq \mathbf{\omega }\Delta t$作为编码了时间间隔$\Delta t$内全部旋转的旋转向量，我们有，
$$\mathbf{R}=e^{[\mathbf{v}{]}_{\times }}.\text{\hspace{1em}(69)}$$

这就是所谓的指数映射，一个从$\mathfrak{s}\mathfrak{o}(3)$到$SO(3)$的应用，
$$\exp :\mathfrak{s}\mathfrak{o}(3)\to SO(3);[\mathbf{v}{]}_{\times }\mapsto \exp ([\mathbf{v}{]}_{\times })=e^{[\mathbf{v}]}$$