坐标系与位姿与运动的描述

刚体的位置 (Position) 与姿态 (Orientation) 统称为位姿 (Location)

坐标系变换

坐标系描述及其符号

坐标系描述

一个坐标系 $\{\bm{B}\}$ 主要包含以下要素

• $x,y,z$ 三个坐标轴
  • 要求三个坐标轴相互正交且满足右手定则
  • 虽然称为轴, 但实际上仅表示方向而不是实际存在的点
• 坐标原点 $B_o$

因此只要确定了以上这四个要素, 即可唯一确定一个坐标系
注意, 如果要描述坐标系, 则至少要有两个坐标系, 除了被描述的坐标系 $\{\bm{B}\}$, 还应有另一坐标系 $\{\bm{A}\}$ 作为参考
因此, 所有坐标系的描述都是相对而言的

关于具体描述, 详见描述坐标系的一般方法

名词解释

• 观察坐标系 - 即该对象的坐标都是基于这一坐标系确定的, 通过参考坐标轴与其坐标值可唯一表示出该对象

符号约定

• 对于特定参考系, 始终以符号 $\{\bm{A}\}$ 表示, 即一个包裹在 $\{\}$ 内的粗体字母, 其中的字母即坐标系的代号
• 默认情况下, 总是以单个大写加粗字母 $\bm{A}$ 表示矩阵, 以单个小写加粗字母 $\bm{q}$ 表示矢量
• 使用 $c_\alpha$ 简略表示 $\cos\alpha$, 同理使用 $s_\alpha$ 简略表示 $\sin\alpha$
• 使用矩阵符号 $\bm{R}$ 相对的小写字母 $r$ 以及双数字下标 $ij$ 组成的符号 $r_{ij}$ 表示矩阵第 $i$ 行 $j$ 列的元素 (从 $1$ 开始索引)

矢量的符号

规定以下矢量的符号

• 符号 $^{A}\bm{p}_{N}$ 表示一个矢量
  • $\bm{p}$ 表明该矢量表示的是一个实际的点, 也称为方向矢量, 将随坐标系旋转而改变 (即齐次坐标 $w\neq 0$, 默认 $w=1$)
  • $A$ 表示该矢量的坐标值是通过坐标系 $\{\bm{A}\}$ 描述的
  • $N$ 表示该矢量表示的点为 $N$, 特别地
    • 表示点的实质即表示线段 $ON$ 在坐标轴的投影 ($O$ 为坐标轴原点)
    • 使用两个字母时, 则强调为两点构成的线段
    • 为空时表示坐标系下的任意点
  • 该矢量可以是 (笛卡尔坐标系) $3$ 或 (齐次坐标系) $4$ 元素的列矢量
• 符号 $^{A}\bm{x}_{B}$ 表示一个矢量
  • $\bm{x}$ 表明该矢量来自某一坐标系的 $x$ 轴, 类似的还有 $\bm{y},\bm{z}$
    且该矢量表示的是一个不受约束的方向 (要求该向量的长度为 $1$), 不随坐标系旋转而改变 (即齐次坐标 $w=0$), 也称为自由矢量
  • $A$ 表示该矢量的坐标值是通过坐标系 $\{\bm{A}\}$ 描述的, $B$ 表示该矢量是坐标系 $\{\bm{B}\}$ 的一个坐标轴
  • 该矢量可以是 (笛卡尔坐标系) $3$ 或 (齐次坐标系) $4$ 元素的列矢量

矩阵的符号

规定以下矩阵的符号

• 符号 $^{A}_{B}\bm{R}$ 通常表示一个 $3\times 3$ 的矩阵, 其中
  • $\bm{R}$ 表示该矩阵仅包含了坐标系的三个坐标轴方向矢量, 而不包含原点位置信息, 一般使用笛卡尔坐标系
    即有 $^{A}_{B}\bm{R}=\begin{bmatrix}{}^{A}\bm{x}_{B}&^{A}\bm{y}_{B}&^{A}\bm{z}_{B}\end{bmatrix}$
    因此该矩阵可描述刚体的姿态, 也称为姿态矩阵
  • $A$ 表示该矩阵的坐标值是通过坐标系 $\{\bm{A}\}$ 描述的, $B$ 表示该矩阵所描述的基底来自坐标系 $\{\bm{B}\}$
  • 关于该矩阵的性质见姿态矩阵
  • 组合 $\begin{bmatrix}^{A}_{B}\bm{R}&^{A}\bm{p}_{B_o}\end{bmatrix}$ 可描述在坐标系 $\{\bm{A}\}$ 观察下的坐标系 $\{\bm{B}\}$
• 符号 $^{A}_{B}\bm{T}$ 表示一个 $4\times 4$ 的矩阵, 其中
  • $\bm{T}$ 表示该矩阵仅包含了坐标系的三个坐标轴方向矢量以及原点, 且一般使用齐次坐标系
    即有 $^{A}_{B}\bm{T}=\begin{bmatrix}{}^{A}\bm{x}_{B}&^{A}\bm{y}_{B}&^{A}\bm{z}_{B}&^{A}\bm{p}_{B_o}\end{bmatrix}$
    因此该矩阵可表述刚体的位姿, 也称为齐次矩阵
  • $A$ 表示该矩阵的坐标值是通过坐标系 $\{\bm{A}\}$ 描述的
  • $B$ 表示该矩阵所描述的基底以及原点来自坐标系 $\{\bm{B}\}$
  • 易得该 $^{A}_{B}\bm{T}$ 与 $^{A}_{B}\bm{R}$ 存在关系 $^{A}_{B}\bm{T}=\begin{bmatrix}{}^{A}_{B}\bm{R}&^{A}\bm{p}_{B_o}\\\bm{0}&1\end{bmatrix}$, 该矩阵也可用于描述在坐标系 $\{\bm{A}\}$ 观察下的坐标系 $\{\bm{B}\}$

通过坐标系描述位姿

刚体的坐标系

通过如下的方式规定刚体的三个坐标轴正方向

• 选取刚体的质心或几何中心作为刚体圆心
• 选取平行于刚体主要回转轴的方向为 $z$ 轴, 并以远刚体指向正前方为其正方向
• 选取垂直并远离刚体主要平面, 竖直向上的方向为 $x$ 轴
• 通过右手定则, 确定剩余的 $y$ 轴

习惯上

• 以 $\{\bm{B}\}$ 表示刚体坐标系, 取自英文 Body frame
• 以 $\{\bm{A}\}$ 表示通常固定不动的观察坐标系
• 以 $\{\bm{W}\}$ 表示绝对坐标系, 取字英文 World frame

坐标系变换与位姿

观察以上刚体的坐标系可得
该坐标系始终固连于刚体之上, 对于刚体上的任意一点 $N$, 无论刚体如何运动始, 矢量 $^{B}\bm{p}_{N}$ 始终不变

仅需确定刚体的坐标系, 即可确定刚体的位姿, 因此可通过描述刚体的坐标系以表示刚体的位姿
下文中也默认使用刚体坐标系 $\{\bm{B}\}$ 表示其位姿

此外, 通过坐标系变换, 可得到刚体任意姿态下, 其上任意点 $N$ 在观察坐标系 $\{\bm{A}\}$ 的坐标

描述坐标系的一般方法

基于观察坐标系直接描述

根据坐标系的要素可得, 描述坐标系的本质, 即在观察坐标系 $\{\bm{A}\}$ 下

• 找出被描述坐标系 $\{\bm{B}\}$ 三个坐标轴的方向 $^{A}\vec{x}_B, ^{A}\vec{y}_B, ^{A}\vec{z}_B$
  可将 $\{\bm{B}\}$ 平移至 $A_o$ 易于求出坐标轴在 $\{\bm{A}\}$ 三个坐标轴的分量 (也即投影)
• 找出观察坐标系 $\{\bm{A}\}$ 的原点到被描述坐标系 $\{\bm{B}\}$ 原点 $B_o$ 所构成的矢量 $^{A}\bm{p}_{A_oB_o}$ 在 $\{\bm{A}\}$ 三个坐标轴的分量 (也即投影)
• 将以上四个要素组合, 即可得到坐标系描述的数学符号 $^{A}_{B}\bm{T}$

相反地, 根据坐标系变换法则, 先求出 $^{B}_{A}\bm{T}$, 再由矩阵的逆可得 $^{A}_{B}\bm{T}={}^{B}_{A}\bm{T}^{-1}$

使用该方法时, 可通过检查齐次矩阵中的姿态矩阵是否满足两个约束条件以检查结果是否正确

通过第三个坐标系描述

通过第三个绝对坐标系 $\{\bm{W}\}$, 使用基于观察坐标系直接描述先求出 ${}^{W}_{A}\bm{T}$ 与 ${}^{W}_{B}\bm{T}$
根据坐标系变换法则可得 ${}^{A}_{B}\bm{T}={}^{W}_{A}\bm{T}^{-1}{}^{W}_{B}\bm{T}$

通过坐标系运动表述

理论基础详见运动算子与坐标变换的关联

当两个坐标系是通过一系列运动描述, 从一个得到另一个时, 可采用此方法获取两坐标系其中一个对另一个的描述
但要明确被描述的坐标系与运动时基于的坐标系 (是否与默认坐标系相同, 不同时应当通过此处介绍的方法进行转化)

如果运动都是基于初始坐标系 $\{\bm{A}\}$, 则应当使用从右到左的组合顺序
如果运动都是基于运动坐标系 $\{\bm{A}_{N-1}\}$ , 则应当使用从左到右的组合顺序

坐标系变换方法

坐标变换即已知空间上一个点 $N$ 在坐标系 $\{\bm{B}\}$ 中的坐标
现要求出该点在观察坐标系 $\{\bm{A}\}$ 中的坐标

笛卡尔坐标系下的表示

由于姿态矩阵 $^{A}_B\bm{R}$ 记录了坐标系 $\{\bm{B}\}$ 基底在观察坐标系 $\{\bm{A}\}$ 下的坐标值
向量 $^{B}\bm{p}_{N}$ 记录了组合得到点 $N$ 时, $\{\bm{B}\}$ 各基底的分量
因此运算 $^{A}_B\bm{R}{}^{B}\bm{p}_{N}$ 将根据分量线性组合 $^{A}_B\bm{R}$ 各列, 得到向量 $^{A}\bm{p}_{B_oN}$
注意, 此时的结果并非我们所期望的点 $^{A}\bm{p}_{N}={}^{A}\bm{p}_{A_oN}$, 而是线段 $B_oN$
因此还需要补充剩余部分 $^{A}\bm{p}_{N}={}^{A}\bm{p}_{B_o}+^{A}\bm{p}_{B_oN}$

从上所述, 在笛卡尔坐标系下, 坐标系变换的一般公式为

$${}^{A}\bm{p}_N={}^{A}_{B}\bm{R}{}^{B}\bm{p}_N + {}^{A}\bm{p}_{B_o}$$

需要特别指出, 对于自由矢量, 其表示的只是特定方向而非具体的点, 因此不会受平移的影响, 因此有

$${}^{A}\bm{n}={}^{A}_{B}\bm{R}{}^{B}\bm{n}$$

齐次坐标系下的表示

根据齐次矩阵的特点易得, 变换公式可等价表示为以下形式
即齐次坐标系下, 坐标系变换的一般公式

$$^{A}\bm{p}_N={}^{A}_{B}\bm{T} ^{B}\bm{p}_N$$

由此公式可得, 在齐次坐标系下, 仅通过矩阵的乘法即可表示出坐标系的变换
因此在公式推导中, 一般都采用齐次矩阵描述坐标系

坐标系变换法则

• 对于多个坐标系之间的变换, 仅需保证左乘的矩阵的被描述坐标系为被乘矩阵的描述坐标系即可, 如

$$^{A}_{D}\bm{T}={}^{A}_{B}\bm{T} {}^{B}_{C}\bm{T} {}^{C}_{D}\bm{T}$$

• 根据矩阵求逆的性质可得, 如果存在等式 $^{A}\bm{p}_N={}^{A}_{B}\bm{T} ^{B}\bm{p}_N$ 则有 $^{A}_{B}\bm{T}^{-1}{}^{A}\bm{p}_N={}^{B}\bm{p}_N$
  因此, 通过对齐次矩阵求逆即可交换观察与描述坐标系, 有

$${}^{A}_{B}\bm{T}^{-1}={}^{B}_{A}\bm{T}$$

齐次矩阵求逆注意

对于任意齐次矩阵 $^{A}_{B}\bm{T}$, 均可表示为以下形式 (公式右侧为运动算子)

$${}^{A}_{B}\bm{T}=\begin{bmatrix}\bm{I}&{}^{A}\bm{p}_{B_o}\\\bm{0}&1\end{bmatrix}\begin{bmatrix}{}^{A}_{B}\bm{R}&\bm{0}\\\bm{0}&1\end{bmatrix}=\operatorname{Trans}({}^{A}\bm{p}_{B_o})\operatorname{Rot}(\bm{k},\theta)$$

显然, 矩阵的两部分分别有逆

$$\begin{split} \operatorname{Trans}({}^{A}\bm{p}_{B_o})^{-1}&=\begin{bmatrix}\bm{I}&-{}^{A}\bm{p}_{B_o}\\\bm{0}&1\end{bmatrix}\\ \operatorname{Rot}(\bm{k},\theta)^{-1}&=\begin{bmatrix}{}^{A}_{B}\bm{R}^{T}&\bm{0}\\\bm{0}&1\end{bmatrix} \end{split}$$

因此齐次矩阵的逆为 (注意矩阵求逆的法则, 求逆时要交换运算顺序)

$$\begin{split} {}^{A}_{B}\bm{T}^{-1}&=[\operatorname{Trans}({}^{A}\bm{p}_{B_o})\operatorname{Rot}(\bm{k},\theta)]^{-1}\\ &=\operatorname{Rot}(\bm{k},\theta)^{-1}\operatorname{Trans}({}^{A}\bm{p}_{B_o})^{-1}\\ &=\begin{bmatrix}{}^{A}_{B}\bm{R}^{T}&\bm{0}\\\bm{0}&1\end{bmatrix}\begin{bmatrix}\bm{I}&-{}^{A}\bm{p}_{B_o}\\\bm{0}&1\end{bmatrix}\\ &=\begin{bmatrix}{}^{A}_{B}\bm{R}^{T}&-{}^{A}_{B}\bm{R}^{T}{}^{A}\bm{p}_{B_o}\\\bm{0}&1\end{bmatrix} \end{split}$$

坐标变换时的易错解析

• 仅齐次矩阵 ${}^{A}_{B}\bm{T}$ 完整描述了坐标系 $\{B\}$, 姿态矩阵 ${}^{A}_{B}\bm{R}$ 无论是笛卡尔坐标系还是齐次坐标系, 该矩阵仅包含了坐标轴的方向信息, 因此除非指明 $\{A\},\{B\}$ 原点重合, 否则以下表达式均为错误的
  • $\space^B\bm{p}={}^{B}_{A}\bm{R}\space^A\bm{p}$
  • ${}^{B}_{A}\bm{R}^{-1}\space^B\bm{p}=\space^A\bm{p}$
  • $\space^B\bm{p}={}^{A}_{B}\bm{R}{}^{B}_{A}\bm{R}\space^B\bm{p}$ 该表达式虽然不正确, 但由于姿态矩阵为标准正交矩阵, 因此等式成立

运动算子

线性变换表示运动

通过齐次坐标系下的线性变换 (由于在齐次坐标系下, 因此也称为齐次变换), 能够表示物体的平移与旋转运动
也将表示这两种运动的变换称为运动算子
通过运动算子的运算 ${}^{A}\bm{p}_{N2}=\operatorname{T}({}^{A}\bm{p}_{N1})$ 可得到点 $N1$ 经过运动 $\operatorname{T}$ 后来到下一状态 $N2$ 的坐标

由矩阵与线性变换的内容可知, 线性变换均可表示为矩阵的形式, 且满足 $\bm{p}_2=\operatorname{T}(\bm{p}_1)=\bm{T}\bm{p}_1$
因此下文中也将默认将运动算子视为变换矩阵处理

运动算子的符号表示

使用符号表示时, 可写为 ${}^{A}\operatorname{T}(\dots)\bm{v}$ 或 ${}^{A}\bm{T}\bm{v}$

其中

• 上标 $A$ 强调描述该运动算子的观察坐标系 $\{\bm{A}\}$, 若没有上标则默认使用与被变换向量 $\bm{p}$ 一致的坐标系
  根据矩阵与线性变换内容可知, 线性变换或者说运动算子的矩阵表示本质上也是一组特定坐标系观察下的坐标, 因此运动算子也存在观察坐标系, 且同样重要
• 第一种形式, 即原始的运动算子, 可能带有一系列参数
• 第二种形式, 即强调以矩阵形式表示运动算子, 常用于理论推导, 参数可省略
  注意与坐标系变换中的符号区分, 运动算子均不存在左下标, 这是区分两者最简单的标志

对于特定形式的运动算子表示如下

旋转算子

• 算子形式 $\operatorname{Rot}(\bm{k},\theta)$
• 矩阵形式 $\bm{R}(\bm{k},\theta)$, 也可写为 $\bm{R}_{\bm{k}}(\theta)$
• 参数 $\bm{k}$ 为单位矢量表示旋转轴, $\theta$ 表示旋转角度
• 关于旋转算子的矩阵表示可参考简单旋转变换以及齐次坐标系下的线性变换

在笛卡尔坐标系中旋转算子可表示为 (其中 $\bm{P}=\bm{k}\bm{k}^T$, $[\bm{k}]$ 为交叉积矩阵)

$$\bm{R}(\bm{k},\theta)=\bm{P}+(\bm{I}-\bm{P})\cos\theta+[\bm{k}]\sin\theta$$

特别地, 当以三个坐标轴为转轴时

$$\bm{R}_{\bm{x}}(\theta)=\begin{bmatrix}1& 0& 0\\ 0& \cos\theta& -\sin\theta\\ 0& \sin\theta& \cos\theta\end{bmatrix}\space \bm{R}_{\bm{y}}(\theta)=\begin{bmatrix}\cos\theta& 0& \sin\theta\\ 0& 1& 0\\ -\sin\theta& 0& \cos\theta\end{bmatrix}\space \bm{R}_{\bm{z}}(\theta)=\begin{bmatrix}\cos\theta& -\sin\theta& 0\\ \sin\theta& \cos\theta& 0\\ 0& 0& 1\end{bmatrix}$$

在齐次坐标系中

$$\operatorname{Rot}(\bm{k},\theta)=\begin{bmatrix}\bm{R}'(\bm{k},\theta)&\bm{0}\\\bm{0}&1\end{bmatrix}$$

平移算子

• 算子形式 $\operatorname{Trans}(\bm{p})$ 或 $\operatorname{Trans}(\bm{n},q)$
• 矩阵形式 $\bm{D}(\bm{p})$ 或 $\bm{D}(\bm{n},q)$, 也可写为 $\bm{D}_{\bm{n}}(q)$
• 参数 $\bm{p}$ 表示位移矢量, $\bm{n}$ 表示位移方向, $q$ 表示位移距离
• 关于平移算子的矩阵表示可参考平移变换矩阵

平移变换只能在齐次空间中表示, 有

$$\operatorname{Trans}(\bm{p})=\begin{bmatrix}I&\bm{p}\\\bm{0}&1\end{bmatrix}$$

运动算子的一般形式

注意, 运动算子可通过矩阵相乘的方式组合
根据欧拉定律, 无论如何这些旋转与平移, 最终总能用一个旋转与一个平移运动表示

这也表明无论多么复杂的运动, 最终总能组合得到如下形式

$$\bm{T}=\operatorname{Trans}(\bm{p})\operatorname{Rot}(\bm{k},\theta)$$

注意, 公式中的矢量 $\bm{p},\bm{k}$ 所在的观察坐标系及该运动算子的观察坐标系

因此使用矩阵 $\bm{T}$ 或算子 $\operatorname{T}$ 表示一般的运动算子
特别地, 关于时间的矩阵函数 $\bm{T}(t)$ 可用于描述特定点
关于刚体的运动变换则可以使用 ${}^{A}\bm{T}_{B_0\to B_1}$ 表示在观察坐标系 $\{\bm{A}\}$ (此时这一坐标系应当被强调, 其强调了与之右乘的点应处于的观察坐标系) 下刚体从姿态 $\{\bm{B}_0\}$ 到 $\{\bm{B}_1\}$ 的运动

注意, 运动算子一般形式整理为相对观察坐标系先旋转再平移的方式, 正好与齐次矩阵相对应, 有

$$\bm{T}=\begin{bmatrix} \operatorname{Rot}(\bm{k},\theta)&\bm{p}\\ \bm{0}&1 \end{bmatrix}$$

根据齐次坐标系下的坐标系表示与运动算子与坐标变换的关联
假设观察坐标系, 即运动起始位姿为 $\{\bm{B}_0\}$, 运动后的位姿为 $\{\bm{B}_1\}$

• $\operatorname{Rot}(\bm{k},\theta)={^{B_0}_{B_1}\bm{R}}$, 组合运动中的旋转运动效果即将刚体从观察坐标系姿态旋转至与描述坐标系姿态相同
• $\bm{p}={^{B_0}\bm{p}_{OB_1}}$, 组合运动中的平移运动矢量与观察坐标系下, 描述坐标系的原点坐标

运动算子的组合

基于观察坐标系的运动组合

已知, 对于点 ${}^{A}\bm{p}_{N1}$, 通过左乘变换矩阵 $\bm{T}$ 即可得到对应的运动后的点坐标 ${}^{A}\bm{p}_{N2}=\bm{T}{}^{A}\bm{p}_{N1}$
因此只需要不断左乘新的变换矩阵, 即可实现点 $N$ 的连续运动

• 由于 $\bm{T}$ 默认使用被变换对象的观察坐标系 $\{\bm{A}\}$ 为自己的观察坐标系, 因此变换 $\bm{T}$ 将也基于观察坐标系 $\{\bm{A}\}$
  与坐标变换不同, 被变换对象经过运动后, 其结果依然以同一坐标系描述, 因此观察坐标系不因运动而改变, 这一系列的运动始终基于被变换对象的观察坐标系 $\{\bm{A}\}$
• 根据矩阵乘法法则, 矩阵乘法不满足交换律, 因此多个运动算子组合时, 必须按从右往左的顺序依次变换
  但是注意, 根据平移的特性易得, 对于连续的平移算子相乘允许交换次序
• 虽然默认情况下认为变换矩阵的观察坐标系与被变换对象的坐标系一致, 但还是要强调, 仅当被变换的向量与刚体姿态或组合的变换矩阵具有同一观察坐标系时, 他们之间才能相乘, 并且变换结果依然在该坐标系中, 这点务必与坐标系变换进行区分

因此
对于点 ${}^{A}\bm{p}_{N1}$ 经一系列运动后在同一观察坐标系下有

$$\bm{T}_N\dots\bm{T}_2\bm{T}_1{}^{A}\bm{p}_{N1}={}^{A}\bm{p}_{N2}$$

对于观察坐标系 $\{\bm{A}\}$ 下由初始姿态 $\{\bm{B_0}\}$ 经过一系列运动来到姿态 $\{\bm{B_1}\}$ 可表示为

$$\bm{T}_N\dots\bm{T}_2\bm{T}_1{}^{A}_{B_0}\bm{T}={}^{A}_{B_1}\bm{T}$$

其中, 将得到 $B_1$ 的一系列运动统称为

$${}^{A}\bm{T}_{B_0\to B_1}=\bm{T}_N\dots\bm{T}_2\bm{T}_1$$

变换矩阵的观察坐标变换

由使用矩阵表示线性变换部分的内容介绍可知, 即使是同一效果的线性变换, 如果所在的坐标系不同, 线性变换矩阵将完全不同

假设运动点所在的观察坐标系为 $\{\bm{A}\}$, 然而运动算子却是基于坐标系 $\{\bm{B}\}$ 描述的, 因此

• 在线性变换前, 需要将被变换点 ${}^{A}\bm{p}$ 的坐标系转为 $\{\bm{B}\}$, 保证变换进行
• 在线性变换后, 由于结果仍在坐标系 $\{\bm{B}\}$ 下, 因此还需要将坐标系变换回 $\{\bm{A}\}$

由上可得, 对于与坐标系 $\{\bm{B}\}$ 为观察坐标系的变换 ${}^{B}\bm{T}$, 转为以坐标系 $\{\bm{A}\}$ 为观察坐标系时有

$${}^{A}\bm{T}={}^{A}_{B}\bm{T}{}^{B}\bm{T}{}^{A}_{B}\bm{T}^{-1}={}^{A}_{B}\bm{T}{}^{B}\bm{T}{}^{B}_{A}\bm{T}$$

基于运动坐标系的运动组合

在部分情况下, 基于刚体当前的坐标系描述运动能带来许多便利
假设刚体从初始位姿 $\{\bm{B}_0\}$ 开始, 进行了 $N$ 次运动, 且这 $N$ 次运动均基于运动前的坐标系描述 (也称为运动坐标系), 分别有变换矩阵 ${}^{B_0}\bm{T}_1,{}^{B_1}\bm{T}_2,\dots,\space^{B_{N-2}}\bm{T}_{N-1},\space^{B_{N-1}}\bm{T}_{N}$

根据变换矩阵的观察坐标变换, 将第 $k$ 个运动的变换矩阵的观察坐标系转换为初始坐标系 $\{\bm{B}_0\}$

$${}^{B_0}\bm{T}_k={}^{B_{0}}_{B_{k-1}}\bm{T}\space^{B_{k-1}}\bm{T}_k\space^{B_{k-1}}_{B_{0}}\bm{T}$$

由运动算子与坐标变换的关联可以知道, 齐次矩阵 ${}^{B_{0}}_{B_{k-1}}\bm{T}$ 可通过坐标系 $\{\bm{B}_0\}$ 到 $\{\bm{B}_{k-1}\}$ 的一系列运动算子的组合表示, 有

$${}^{B_{0}}\bm{T}_{B_{0}\to B_{k-1}}={}^{B_{0}}_{B_{k-1}}\bm{T}={}^{B_0}T_{k-1}\dots{}^{B_0}T_{2}{}^{B_0}T_{1}$$

注意到, 前 $k-1$ 次运动之积即 ${}^{B_{0}}\bm{T}_{B_{0}\to B_{k-1}}={}^{B_{0}}_{B_{k-1}}\bm{T}$
根据坐标系变换法则中关于齐次矩阵的逆的性质可得 $\space^{B_{k-1}}_{B_{0}}\bm{T}{}^{B_{0}}_{B_{k-1}}\bm{T}=I$
因此带入第 $k$ 次变换时有

$$\begin{split} {}^{B_{0}}\bm{T}_{B_{0}\to B_{k}}&={}^{B_0}\bm{T}_k{}^{B_{0}}\bm{T}_{B_{0}\to B_{k-1}}\\ &=({}^{B_{0}}_{B_{k-1}}\bm{T}\space^{B_{k-1}}\bm{T}_k\space^{B_{k-1}}_{B_{0}}\bm{T}){}^{B_{0}}\bm{T}_{B_{0}\to B_{k-1}}\\ &={}^{B_{0}}_{B_{k-1}}\bm{T}\space^{B_{k-1}}\bm{T}_k\\ &={}^{B_{0}}\bm{T}_{B_0\to B_{k-1}}\space^{B_{k-1}}\bm{T}_k \end{split}$$

由上可得出以下结论, 当观察坐标系 $\{\bm{A}\}$ 同时与初始姿态 $\{\bm{B_0}\}$ 重合时 (在描述刚体运动时, 这一前提通常成立)

• 对于前 $k-1$ 次运动有运动算子 ${}^{B_0}\bm{T}_{B_0\to B_{k-1}}$
• 当第 $k$ 次运动的描述基于观察坐标系, 为 ${}^{A}\bm{T}_{k}$, 则使用一般规则可得 (这一规则在任何情况下通用)
  通过左乘下一运动算子即可进行组合 ${}^{B_0}\bm{T}_{B_0\to B_{k}}={}^{B_0}\bm{T}_{k}{}^{B_0}\bm{T}_{B_0\to B_{k-1}}$
• 当第 $k$ 次运动的描述基于运动坐标系, 为 $\space^{B_{k-1}}\bm{T}_{k}$, 则使用上述结论可得 (注意这一规则有重合要求)
  通过右乘下一运动算子即可进行组合 ${}^{B_0}\bm{T}_{B_0\to B_{k}}={}^{B_0}\bm{T}_{B_0\to B_{k-1}}\space^{B_{k-1}}\bm{T}_{k}$
• 特别地, 当所有运动都基于运动坐标系时, 运动算子的组合通过从右到左的顺序进行运动

以此规定

• 当运动算子是基于观察坐标系进行的, 则直接使用左上标 $s$, 表示 space (可省略, 或使用 $B_0$)
  如 ${}^{B_0}\bm{T}_{B_0\to B_{k}}={}^{s}\bm{T}_{k}{}^{B_0}\bm{T}_{B_0\to B_{k-1}}$
• 当运动算子是基于运动坐标系进行的, 则直接使用左上标 $b$, 表示 body
  如 ${}^{B_0}\bm{T}_{B_0\to B_{k}}={}^{B_0}\bm{T}_{B_0\to B_{k-1}}{}^{b}\bm{T}_{k}$

绕任意点旋转运动

在观察坐标系 $\{\bm{A}\}$ 下刚体绕点 $M$ 以方向为 $\bm{k}$ 的转轴旋转了 $\theta$, 现希望求出该运动的矩阵形式
对于绕点 $M$ 的旋转运动, 如果以一个原点与 $M$ 重合的坐标系描述, 就是绕过原点轴旋转, 而该运动的矩阵形式已知
如果在一个坐标轴与 $\{\bm{A}\}$ 平行, 原点位于 $M$ 的坐标系 $\{\bm{A}_M\}$ 下描述, 该运动有矩阵形式 ${}^{A_M}\bm{T}=\bm{R}(\bm{k},\theta)$

可通过变换矩阵的观察坐标变换, 将上述的齐次矩阵的描述坐标系变换到观察坐标系 $\{\bm{A}\}$ 下有 ${}^{A}\bm{T}={}^{A}_{A_M}\bm{T}{}^{A_M}\bm{T}{}^{A}_{A_M}\bm{T}^{-1}$
由此可得到绕过点 $\bm{p}$ 的轴 $\bm{k}$ 旋转 $\theta$ 的运动的矩阵形式为

$$\bm{T}(\bm{p},\bm{k},\theta)=\bm{D}(\bm{p})\bm{R}(\bm{k},\theta)\bm{D}^{-1}(\bm{p})$$

公式中 $\bm{p},\bm{k}$ 的观察坐标系即该运动的观察坐标系

通过运动描述位姿

刚体的运动变换

已知刚体的位姿可通过一个坐标系 $\{\bm{B}\}$ 表示, 在观察坐标系 $\{\bm{A}\}$ 下, 该坐标系 $\{\bm{B}\}$ 可使用一个齐次矩阵 ${}^{A}_{B}\bm{T}$ 描述
该齐次矩阵的本质即三个自由矢量与一个点组合而成的序列 ${}^{A}_{B}\bm{T}=\begin{bmatrix}{}^{A}\bm{x}&{}^{A}\bm{y}&{}^{A}\bm{z}&{}^{A}\bm{p}_{Bo}\end{bmatrix}$
根据矩阵乘法可知, ${}^{A}_{B}\bm{T}(t)=\bm{T}(t){}^{A}_{B}\bm{T}(0)=\begin{bmatrix}\bm{T}(t){}^{A}\bm{x}&\bm{T}(t){}^{A}\bm{y}&\bm{T}(t){}^{A}\bm{z}&\bm{T}(t){}^{A}\bm{p}_{Bo}\end{bmatrix}$

因此将描述刚体坐标系的齐次矩阵左乘运动算子, 即可得到刚体经过一系列运动后的位姿, 也使用如下的矩阵函数表示 ${}^{A}_{B}\bm{T}(t)$

在刚体的运动变换中注意, 当刚体原点与观察坐标系原点不重合时, 绕观察坐标系轴旋转时, 除了原点绕刚体旋转, 刚体坐标系的三个坐标轴也将随之旋转

运动算子与坐标变换的关联

• 由坐标系描述可得, 当使用任意坐标系 $\{\bm{A}\}$ 描述自身时, 必然有齐次矩阵 ${}^{A}_{A}\bm{T}=\bm{I}$
• 运动算子的一般形式与矩阵的符号具有相同的形式, 均为由一个旋转矩阵 $\bm{R}$, 一个平移列向量 $\bm{p}$ 组成的 $4\times 4$ 的齐次坐标系下的矩阵
• 乘以运动算子进行变换, 并不会改变观察坐标系, 只会改变齐次矩阵所描述的姿态 (因此不能以坐标系变换的方式进行理解)

由以上两点可得, 对于观察坐标系 $\{\bm{A}\}$ 描述下的坐标系 $\{\bm{B}\}$ 有 (注意左侧等式两侧应当基于同一观察坐标系, 因此使用的是 ${}^{A}_{A}\bm{T}=\bm{I}$)

$${}^{A}_{B}\bm{T}=({}^{s}\bm{T}_N\dots{}^{s}\bm{T}_2{}^{s}\bm{T}_1){}^{A}_{A}\bm{T}={}^{s}\bm{T}_N\dots{}^{s}\bm{T}_2{}^{s}\bm{T}_1$$

类似地, 当基于运动坐标系的运动组合时有

$${}^{A}_{B}\bm{T}={}^{A}_{A}\bm{T}({}^{b}\bm{T}_1{}^{b}\bm{T}_2\dots{}^{b}\bm{T}_N)={}^{b}\bm{T}_1{}^{b}\bm{T}_2\dots{}^{b}\bm{T}_N$$

由此可得, 齐次矩阵 ${}^{A}_{B}\bm{T}$ 除了描述坐标系, 还可以表示通过观察坐标系 $\{\bm{A}\}$ 到被描述坐标系 $\{\bm{B}\}$ 所经历的一系列运动的组合
而这也是描述坐标系的另一重要方法

如果视 $\{\bm{A}\}$ 与 $\{\bm{B}\}$ 为刚体运动的两个状态, 以上等式与描述在本质上表示的是 ${}^{A}\bm{T}_{A\to B}$ (注意观察坐标系)
因此, 部分情况下也会用符号 ${}^{A}_{B}\bm{T}$ 表示以初始位姿 $\{\bm{A}\}$ 为观察参考系时, 刚体位姿 $A\to B$ 的运动 ${}^{A}\bm{T}_{A\to B}$

当刚体的位姿相对时间 $t$ 变化时, 也简记为 $\bm{T}(t)$, 表示 ${}^{B_0}\bm{T}_{B_0\to B_t}={}^{B_0}_{B_t}\bm{T}=\bm{T}(t)$
但是要始终明确, 位姿 $\bm{T}(t)$ 是以起始位姿 $\{\bm{B}_0\}$ 作为描述坐标系, 如果希望变换坐标系, 则应当右乘相应的齐次矩阵 ${}^{A}_{B_0}\bm{T}$, 并以左上标强调 ${}^{A}\bm{T}(t)$

运动算子与坐标变换的异同

• 在应用上, 运动算子主要用于描述两个刚体之间的运动, 而坐标系变换则用于描述各个坐标系之间的关系
• 两者在描述时都需要借助观察坐标系, 但运动算子的描述对象是两个姿态间的运动 $B_0\to B_1$, 而坐标系变换则是被描述坐标系 $B$
• 运动算子与坐标系描述都可使用 $4\times 4$ 的, 齐次空间中的矩阵描述

姿态或旋转的描述

确定姿态对于描述刚体的位姿与运动具有重要意义
除了姿态矩阵外还有众多关于姿态的表示方式, 这些表示方式可在不同场合下用于表示姿态

此外, 由运动算子与坐标变换的关联可知, 除了描述姿态, 也可用于描述旋转运动

姿态矩阵

用于表示旋转运动时, 也称之为旋转矩阵
该矩阵为一个 $3\times 3$ 的矩阵, 表示了刚体坐标系中三个坐标轴的方向 $^{A}_{B}\bm{R}=\begin{bmatrix}{}^{A}\bm{x}_{B}&^{A}\bm{y}_{B}&^{A}\bm{z}_{B}\end{bmatrix}$

姿态矩阵的约束条件

一个合法的姿态矩阵应当满足以下约束条件

• 正交性 该性质表明姿态矩阵是一个标准正交矩阵, 满足 $\bm{R}^T=\bm{R}^{-1}$
• 特殊条件 姿态矩阵的三个列向量以此表示 $x,y,z$ 轴, 且始终表示右手系, 满足 $\det(R)=1$

以上约束条件共限制了矩阵 $9$ 个元素中的 $6$ 个, 因此实际上最少只需要三个参数即可表示一个姿态矩阵

姿态矩阵的标准化

参考 https://zhuanlan.zhihu.com/p/665225211

奇异值 $\sigma_i$ 反映了坐标轴的拉伸与镜像, 显然旋转变换不会对坐标轴产生影响
因此旋转矩阵的奇异值必定为 $\sigma_i = 1$, 有 $\Sigma=I$

当旋转矩阵因舍入问题蠕变时, 实质即其中被引入了缩放等额外的变换, 此时 $\sigma_i\neq 1$
通过求出蠕变旋转矩阵 $R'$ 的奇异值分解 $U\Sigma' V^T$, 令 $\Sigma'=I$, 即 $R=UV^T$ 即可标准化旋转矩阵
这其中的运算量显然大于标准化四元数

姿态矩阵的特点

姿态矩阵有优点

• 如果要具体刚体上的点在运动后的坐标, 必须借助包含姿态矩阵的齐次矩阵进行坐标变换
• 通过矩阵的相乘能够很好地与其他运动组合

姿态矩阵有缺点

• 直接使用姿态矩阵表示方位时, 一个姿态需要 $9$ 个参数, 然而实际有 $3$ 个独立的参数
• 姿态矩阵可能因浮点误差导致旋转矩阵蠕变, 使矩阵包含了额外的缩放或表示的姿态发生偏差, 需要通过姿态矩阵的标准化补救

欧拉角与固定角

欧拉角与固定角的本质为通过三个绕坐标轴旋转的旋转算子来表示姿态
只需明确组合的约定, 三次旋转轴以及三个旋转角度即可表示姿态

易得欧拉角或固定角共有 $12$ 种不同的约定, 分别为六种 $xyz$ 的排列以及六种 $xyx$ 的排列

欧拉角与固定角的表示符号

• 对于欧拉角, 其描述的三次绕坐标轴旋转均是基于运动坐标系组合
• 对于固定角 (默认情况), 其描述的三次绕坐标轴选准均是基于观察坐标系组合

使用符号 ${}^{A}_{B}\bm{R}_{xyz}(\gamma,\beta,\alpha)$ 表示固定角

• 上标 $A$ 表示该运动是基于观察坐标系 $\{\bm{A}\}$ 描述的
• 下标 $B$ 强调该运动是刚体由姿态 $\{\bm{A}\}$ 到姿态 $\{\bm{B}\}$ 的运动, 或以观察坐标系 $\{\bm{A}\}$ 描述坐标系 $\{\bm{B}\}$ (根据运动算子与坐标变换的关联, 二者等价)
• 右下标 $xyz$ 从左到右表明旋转运动的顺序, 其中的字母即表示旋转时的转轴为哪个坐标轴
• 参数 $\gamma,\beta,\alpha$ 从左到右分别对应了三次旋转的参数, 与参数符号无关

欧拉角与固定角基本类似, 仅有组合方式的区别, 因此符号表示也基本一致, 一般通过以下方式区别

• 要求参数符号 $\alpha,\beta,\gamma$ 在满足基本要求的情况下, 以矩阵形式组合运动时, 总是以从左向右的顺序出现
  • ${}^{A}_{B}\bm{R}_{xyz}(\alpha,\beta,\gamma)=Rot(x,\alpha)Rot(y,\beta)Rot(z,\gamma)$
    以 $\alpha,\beta,\gamma$ 从左向右的顺序排列, 表示基于运动坐标系的欧拉角
  • ${}^{A}_{B}\bm{R}_{xyz}(\gamma,\beta,\alpha)=Rot(z,\alpha)Rot(y,\beta)Rot(x,\gamma)$
    以 $\gamma,\beta,\alpha$ 从右向左的顺序排列, 表示基于观察坐标系的固定角
• 在符号的右上标添加 $'$ 进行区分, 例如符号 ${}^{A}_{B}\bm{R}_{xyz}'(\alpha,\beta,\gamma)$

常用的欧拉角约定

以下为常用的欧拉角约定

• $RPY$ 角 ($xyz$ 固定角)
  即绕观察坐标系, 以 $xyz$ 的顺序旋转, 表示为 ${}^{A}_{B}\bm{R}_{xyz}(\gamma,\beta,\alpha)$
  其中规定了
  • 绕 $x$ 轴旋转的角度 $\gamma$ 为偏转角 yaw
  • 绕 $y$ 轴旋转的角度 $\beta$ 为俯仰角 pitch
  • 绕 $z$ 轴旋转的角度 $\alpha$ 为翻滚角 roll
• $zyx$ 欧拉角
  即绕运动坐标系, 以 $xyz$ 的顺序旋转, 表示为 ${}^{A}_{B}\bm{R}_{zyx}'(\alpha,\beta,\gamma)$
• $zyz$ 欧拉角
  即绕运动坐标系, 以 $zyz$ 的顺序旋转, 表示为 ${}^{A}_{B}\bm{R}_{zyz}'(\alpha,\beta,\gamma)$

根据运动算子的组合可知, 对于任意欧拉角约定总存在一种固定角约定与之具有完全相同的运动算子组合
例如以上的 $RPY$ 角与 $zyx$ 欧拉角约定满足 ${}^{A}_{B}\bm{R}_{xyz}(\gamma,\beta,\alpha)={}^{A}_{B}\bm{R}_{zyx}(\alpha,\beta,\gamma)$

由欧拉角得到姿态矩阵

将欧拉角或固定角的三次旋转算子使用矩阵表示后, 将矩阵相乘即可得到对应的姿态矩阵
例如 $RPY$ 角有

$$\begin{split} {}^{A}_{B}\bm{R}_{xyz}(\gamma,\beta,\alpha)&=Rot(z,\gamma)Rot(y,\beta)Rot(x,\alpha)\\ &=\begin{bmatrix} c_\alpha c_\beta&c_\alpha s_\beta s_\gamma-s_\alpha c_\gamma& c_\alpha s_\beta c_\gamma+s_\alpha s_\alpha\\ s_\alpha c_\beta&s_\alpha s_\beta s_\gamma+c_\alpha c_\gamma& s_\alpha s_\beta c_\gamma-c_\alpha s_\alpha\\ -s_\beta &c_\beta s_\gamma& c_\beta c_\gamma \end{bmatrix} \end{split}$$

对于其他约定的欧拉角或固定角表示为姿态矩阵时也有类似的结果

由姿态矩阵得到欧拉角

以 $RPY$ 角为例

• 观察到矩阵元素 $r_{11},r_{21}$ 具有相同的因子 $\cos\beta$, 且另一因子分别为 $\cos\alpha$ 与 $\sin\alpha$
• 对于矩阵元素 $r_{32},r_{33}$ 类似, 另一因子为 $\sin\gamma$ 与 $\cos\gamma$

规定 $\cos\beta=\sqrt{r_{11}^2+r_{21}^2}>0$, 因此可解出 $RPY$ 角的三个参数
(本笔记中均采用此标准的四象限反正切函数, 即 $\operatorname{Arctan}(x,y)$, 可能与主流约定存在区别)

$$\begin{cases} \beta=\operatorname{Arctan}(\sqrt{r_{11}^2+r_{21}^2},-r_{31})\\ \alpha=\operatorname{Arctan}(r_{11},r_{21})\\ \gamma=\operatorname{Arctan}(r_{33},r_{32}) \end{cases}$$

当 $\sqrt{r_{11}^2+r_{21}^2}=0$, 表明 $\beta=\pm 90^\circ$, 此时 $r_{11},r_{21},r_{32},r_{33}=0$, 即使通过其他参数也无法准确求出 $\alpha,\gamma$, 还需要额外的条件, 一般要求 $\alpha=0$

欧拉角的特点

欧拉角有优点

• 欧拉角只需要三个数就能表示方位
• 欧拉数中任意三个数都是合法的 (但需要明确角度制还是弧度制)
• 欧拉角能直观地反应方位

欧拉角有缺点

• 存在无数种欧拉角的组合能够表达同一个方位, 因此对于 $RPY$ 角, 一般限制 $\alpha,\gamma\in(-180^\circ,180^\circ],\beta\in(-90^\circ,90^\circ]$, 例如 $\beta=135^\circ$ 与 $\gamma=180^\circ,\beta=45^\circ,\alpha=180^\circ$ 表达的方位相同
• 此外还将导致欧拉角不易进行插值, 例如 $720^\circ\to 60^\circ$ 的插值将导致额外的旋转
• 当 $RPY$ 角的参数 $\beta=\pm 90^\circ$ 时, 将出现万向锁问题, 这将导致 $\alpha$ 失效

轴角对与四元数

根据欧拉定理, 任意绕过原点轴的旋转的组合均能表示为单个绕过原点轴的旋转
因此仅需给出轴方向 ${}^{A}\bm{k}$ (下文省略观察方向) 与旋转角度 $\theta$ 即可表示任意姿态

关于四元数的基础见四元数的基础介绍, 根据四元数转为轴角对, 即可将二者相互转换

• $w=\cos(\theta/2)$
• $\sin(\theta/2)=\sqrt{x^2+y^2+z^2}=\sqrt{1-w^2}$
• $k_x=\frac{x}{\sin(\theta/2)}=\frac{x}{\sqrt{x^2+y^2+z^2}}$, $k_y,k_z$ 类似

注意轴角对与四元数各有约束条件

• 要求轴角对的轴方向 $\bm{k}$ 为单位矢量
• 要求四元数表示姿态时, 为单位四元数, 满足 $w^2+x^2+y^2+z^2=1$

由轴角对或四元数得到姿态矩阵

通过绕任意轴旋转矩阵即可将轴角对转为旋转矩阵

$$\begin{split} \bm{R}(\bm{k},\theta)=\bm{P}+(\bm{I}-\bm{P})\cos\theta+[\tilde{k}]\sin\theta \end{split}$$

带入轴角对与四元数的关系可得

$$\bm{R}(q)=\begin{bmatrix} 1-2y^2-2z^2&2xy-2wz&2xz+2wy\\ 2xy+2wz&1-2x^2-2z^2&2yz-2wx\\ 2xz-2wy&2yz+2wx&1-2x^2-2y^2 \end{bmatrix}$$

由姿态矩阵得到轴角对

观察矩阵 $\bm{R}(\bm{k},\theta)$, 矩阵 $\bm{P}$ 对角线上的元素为 $p_{11}=k_x^2,p_{22}=k_y^2,p_{33}=k_z^2$, 矩阵 $[\tilde{k}]$ 对角线上元素均为 $0$

因此矩阵的迹满足 $\operatorname{tr}[\bm{R}(\bm{k},\theta)]=(k_x^2+k_y^2+k_z^2)+3\cos\theta-(k_x^2+k_y^2+k_z^2)\cos\theta$
根据轴角对对 $\bm{k}$ 的单位长度约束可得 $\operatorname{tr}[\bm{R}(\bm{k},\theta)]=1+2\cos\theta$

此外, 将姿态矩阵相对对角线对称的两个元素相加或相减有 (可观察四元数的姿态矩阵表示得到该结论)

$$\begin{cases} r_{32}-r_{23}=4wx=2k_x\sin\theta \\ r_{13}-r_{31}=4wy=2k_y\sin\theta \\ r_{21}-r_{12}=4wz=2k_z\sin\theta \end{cases}$$

根据 $\bm{k}$ 的单位长度约束通过以上三式可解出 $\sin\theta$ (认为 $\sin\theta>0$), 从而使用四象限反正切解出 $\theta$
再使用 $\sin\theta$ 解出剩余的三个分量

由姿态矩阵得到四元数

根据单位四元数的特点, 通过不同方式组合姿态矩阵对角线上的元素有

$$\begin{split} r_{11}+r_{22}+r_{33}&=4w^2-1\\ r_{11}-r_{22}-r_{33}&=4x^2-1\\ -r_{11}+r_{22}-r_{33}&=4y^2-1\\ -r_{11}-r_{22}+r_{33}&=4z^2-1 \end{split}$$

注意, 以上公式无法判断系数的正负负号, 因此一般仅用以上公式计算求出其中一个系数
实际使用中, 将计算四个系数中, 绝对值最大的一个 (即 $4m^2-1$ 最大, 比较时不需要具体确定系数值)

之后, 通过将姿态矩阵相对对角线对称的两个元素相加或相减, 即可得到任意两个四元数元素之积, 通过此方法求出剩余系数

四元数的特点

四元数有优点

• 仅四元数有简单平滑插值的方法, 另外两种表示方法则没有
• 四元数的标准乘法能快速组合多个旋转, 运算速度快于矩阵
• 四元数仅占用四个数, 仅稍大于欧拉角

四元数有缺点

• 由于运算精度, 四元数可能不合法, 需要对四元数进行单位化
• 四元数仅能相对观察坐标系进行旋转, 无法像矩阵能够对惯性坐标系进行旋转
• 四元数并不直观, 需要转为轴角对或欧拉角

附录: 四元数介绍

四元数表示与基本性质

四元数基本运算法则

四元数与复数类似, 但存在三个虚数单位 $\bm{i},\bm{j},\bm{k}$, 并且有运算法则

$$\begin{split} &\bm{i}^2=\bm{j}^2=\bm{k}^2=-1\\ &\bm{i}\bm{j}=\bm{k},\bm{j}\bm{i}=-\bm{k}\\ &\bm{j}\bm{k}=\bm{i},\bm{k}\bm{j}=-\bm{i}\\ &\bm{k}\bm{i}=\bm{j},\bm{i}\bm{k}=-\bm{j} \end{split}$$

四元数表示方式

由三个虚数单位分别乘以一个实数, 并与一个实数共同组成四元数

$$q=w+x\bm{i}+y\bm{j}+z\bm{k}$$

此外也常用如下方式表示一个四元数

$$q=\begin{bmatrix}w&(x&y&z)\end{bmatrix}=\begin{bmatrix}w&\vec{v}\end{bmatrix}$$

在部分标准下, 使用如下方式表示四元数

$$q=\begin{bmatrix}(x&y&z)&w\end{bmatrix}=\begin{bmatrix}\vec{v}&w\end{bmatrix}$$

四元数与轴角对的关系

轴角对

欧拉已经证明, 任意三维中的旋转组合, 均等价于一个绕特定轴的旋转
因此可使用旋转轴方向的单位向量 $\vec{n}$ 与旋转角度 $\theta$ 表示旋转变换
四元数也可视为轴角对的一种特殊表示方式
其与轴角对的参数 $\theta,\vec{n}$ 之间满足

$$q=\begin{bmatrix}\cos(\theta/2)&(\sin(\theta/2)n_x&\sin(\theta/2)n_y&\sin(\theta/2)n_z)\end{bmatrix}=\begin{bmatrix}\cos(\theta/2)&\sin(\theta/2)\vec{n}\end{bmatrix}$$

与欧拉角不同, 四元数的插值旋转永远走最短圆弧, 例如当旋转角度 $\theta=240^\circ$, 则通过插值旋转路径为 $0\to -120^\circ$

共轭四元数

与复数类似, 四元数的共轭 $q^*$ 与模长 $\begin{Vmatrix}q\end{Vmatrix}$ 满足

$$q^*=\begin{bmatrix}w&-\vec{v}\end{bmatrix}=\begin{bmatrix}w&(-x&-y&-z)\end{bmatrix}$$

$$\begin{Vmatrix}q\end{Vmatrix}=\sqrt{w^2+\begin{Vmatrix}v\end{Vmatrix}^2}=\sqrt{w^2+x^2+y^2+z^2}$$

注意到, 当轴角对以四元数的方式表示时

$$\begin{Vmatrix}q\end{Vmatrix}=\sqrt{\cos(\theta/2)^2+\sin(\theta/2)\begin{Vmatrix}v\end{Vmatrix}^2}=1$$

四元数与轴角对的性质

因此

• 如果四元数要表示为一个合法的方位 / 角位移时, 其模长必须为 $1$, 也称为单位四元数
• 对于一个四元数 $q^*$ , 其共轭相当于将角度 $\theta$ 取反, 因此其共轭反映了一个相反效果的变换 $q^*$
• 而将四元数取负 $-q$, 相当于将旋转轴与旋转角度同时取反, 旋转效果与未取反的四元数相同, 因此任意方位都存在两种不同的表达方式 $q$ 与 $-q$
• 当轴角对的 $\theta=0$, 则四元数为 $q=\begin{bmatrix}1&\vec{0}\end{bmatrix}$, 该四元数不反映任何旋转, 因此称为标准四元数

四元数乘法运算

标准乘法定义

对于任意四元数 $q_1,q_2$, 与复数乘法类似, 定义四元数的叉乘 (也是四元数的标准乘法)

$$\begin{split}q_1q_2&=(w_1+x_1\bm{i}+y_1\bm{j}+z_1\bm{k})(w_2+x_2\bm{i}+y_2\bm{j}+z_2\bm{k})\\ &=\begin{bmatrix}w_1&\vec{v}_1\end{bmatrix}\begin{bmatrix}w_2&\vec{v}_2\end{bmatrix}\\ &=\begin{bmatrix}w_1w_2-\vec{v}_1\vec{v}_2&w_1\vec{v}_2+w_2\vec{v}_1-\vec{v}_1\times\vec{v}_2\end{bmatrix}\end{split}$$

四元数乘法的本质即连接了两个角位移

四元数标准乘法的运算法则

由标准乘法中涉及到的叉乘可得, 四元数的标准乘法满足结合律, 但不满足交换律

$$\begin{cases} &(q_1q_2)q_3=q_1(q_2q_3)\\ &q_1q_2\neq q_2q_1 \end{cases}$$

易得, 四元数乘积的模长等于两个四元数模长的乘积

$$\begin{Vmatrix}q_1q_2\end{Vmatrix}=\begin{Vmatrix}q_1\end{Vmatrix}\begin{Vmatrix}q_2\end{Vmatrix}$$

四元数的逆与方位间角位移

四元数 $q$ 乘以其共轭 $q^*$ 后可得到 $qq^*=\begin{Vmatrix}q\end{Vmatrix}$

以此定义四元数的逆满足 $qq^{-1}=1$
可得四元数的逆表达式为如下

$$q^{-1}=\frac{q^{*}}{\begin{Vmatrix}q\end{Vmatrix}}$$

易得, 对于多个四元数相乘的逆有

$$(q_1q_2)^{-1}=q_2^{-1}q_1^{-1}$$

已知方位 $q_1$ 与 $q_2$, 其中方位 $q_1$ 经过角位移 $d$ 得到 $q_2$, 则有 (注意四元数乘法不满足交换律, 且角位移结合顺序为从右向左)

$$\begin{split} dq_1&=q_2\\ d&=q_2q_1^{-1} \end{split}$$

其中的 $d$ 为方位 $q_1$ 到方位 $q_2$ 的角位移

四元数与旋转

对于向量 $\vec{v}$, 定义四元数 $p_v=\begin{bmatrix}0&\vec{v}\end{bmatrix}$ 来表示这一向量
设对于来自轴角对 $\theta,\vec{n}$ 的四元数 $a$, 将 $\vec{v}$ 绕 $\vec{n}$ 旋转 $\theta$ 度得到向量 $\vec{u}$
则对于表示 $\vec{u}$ 的四元数 $q_u$ 满足

$$p_u=ap_va^{-1}$$

显然, 当对 $p_u$ 再进行一次旋转 $b$ 得到 $p'$, 等价于先旋转 $a$, 再旋转 $b$ 的组合

$$p'=(ba)p_v(a^{-1}b^{-1})=(ba)p_v(ba)^{-1}$$

因此与矩阵类似, 通过左乘四元数 $q$, 可以以从右到左的顺序连接多个四元数
四元数表示角位移时, 连接顺序为从右向左 (观察坐标系不变)

有的时候也将重新定义四元数的乘法为

$$q_1q_2=\begin{bmatrix}w_1w_2-\vec{v}_1\vec{v}_2&w_1\vec{v}_2+w_2\vec{v}_1+\vec{v}_1\times\vec{v}_2\end{bmatrix}$$

此时通过四元数分别完成旋转 $a,b$ 的表达式变为 $q=(ab)p_v(ab)^{-1}$

四元数的其他运算

四元数的点乘

将四元数的四个参数视为一个 $R^4$ 向量
四元数 $q_1q_2$ 的点乘即这两个向量之间点乘, 得到一个实数

$$q_1\cdot q_2=w_1w_2+\vec{v}_1\cdot\vec{v}_2$$

当 $q_1,q_2$ 为单位四元数时, 显然 $-1\le q_1\cdot q_2\le 1$
当两个四元数点乘乘积的绝对值越接近 $1$, 表明两个四元数越相近 (相反的四元数表示的旋转相同)

四元数的对数函数

与复数的对数运算类似, 对于单位四元数 $q=\begin{bmatrix}\cos(\theta/2)&\sin(\theta/2)\vec{n}\end{bmatrix}$, 其对数函数满足

$$\log q=\begin{bmatrix}0&\frac{\theta}{2}\vec{n}\end{bmatrix}$$

单位四元数经过对数函数, 能够提取四元数中的轴角对信息

四元数的指数函数

四元数的指数函数即对数函数的逆运算, 对于 $w=0$ 的四元数 $q=\begin{bmatrix}0&\frac{\theta}{2}\vec{n}\end{bmatrix}$, 其指数函数满足

$$\exp q=\begin{bmatrix}\cos(\frac{\theta}{2})&\sin(\frac{\theta}{2})\vec{n}\end{bmatrix}$$

$w=0$ 的四元数 (轴角对) 经过指数函数, 能够转换为单位四元数

四元数的幂

已知四元数的对数与指数函数后, 根据对数与指数函数的性质, 即可定义四元数的幂满足

$$q^{k}=\exp(k\log q)$$

由此可得, 与复数类似, 对于四元数 $q$ 的 $k$ 次幂相当于将原旋转角度乘以 $k$ 倍
注意 $k$ 可以是任意实数, 如 $q^{-1/3}$ 将表示一个绕相同轴的旋转, 但旋转角度为原来的 $\frac{1}{3}$, 且旋转方向相反

四元数两点插值与样条插值

四元数插值 Slerp

$slerp$ 即球面线性插值, 是一种基于四元数的, 在两个旋转之间平滑线性插值的最优运算
$slerp$ 的本质为一个三元函数 $q_t=slerp(q_1,q_2,t)$
其中 $q_1,q_2$ 为起始与结束方位的四元数, $t$ 为一个 $(0,1)$ 的比例值, 表示中间位置, 结果 $q_t$ 为插值结果
物体沿两个方位间的 $slerp$ 插值旋转运动时能保证其角速度始终不变

根据线性插值思想

• 计算首尾两个值的差
• 将初始值加上首尾差乘上比例值, 得到插值结果

因此根据四元数的方位间角位移与幂运算可得
首先求出方位间角位移计算首尾方位的角位移, 然后通过求 $t$ 的幂, 得到部分角位移, 最后与初始位置的方位连接得到插值结果

$$slerp(q_1,q_2,t)=(q_2q_1^{-1})^{t}q_1$$

可得, $slerp$ 的本质即沿 $q_1,q_2$ 构成的最短圆弧上进行线性插值, 插值结果即圆弧上的各个点
实际应用中, 由于合法方位均为单位四元数, 因此可将单位四元数 $q_1,q_2$ 视为两个长度为 $1$ 的 $R^4$ 向量, 插值结果在 $r=1$ 的四维圆弧上

为了方便理解与推导, 将 $q_1,q_2$ 视为二维单位圆上的两条半径上的向量, 插值结果为这两个向量的线性组合 $q_t=k_1q_1+k_2q_2$
如图所示

$$QH=k_1\sin(\omega)=1\sin(t\omega)\\ \to k_1=\frac{\sin(t\omega)}{\sin(\omega)}$$

同理可得 $k_2=\frac{\sin[(1-t)\omega]}{\sin(\omega)}$ (调换 $q_1,q_2$ 即可得), 其中 $q_1\cdot q_2=1\cdot 1\cos\omega$
因此对于单位四元数 $q_1,q_2$ 有球面插值

$$q_t=\frac{\sin(t\omega)}{\sin(\omega)}q_1+\frac{\sin[(1-t)\omega]}{\sin(\omega)}q_2$$

在以上针对单位四元数的球面插值在具体实现时需要注意

• 由于 $q_1,q_2$ 的方向不同可能对应两个不同的插值圆弧, 为了保持最小圆弧, 应保证 $q_1\cdot q_2\ge 0$, 若不满足则取 $q_1=-q_1$ (注意四元数取反并不会改变旋转效果)
• 当 $q_1,q_2$ 非常接近时, $\omega,\sin\omega\approx 0$, 这不利于除法运算, 因此可改为向量 $q_1,q_2$ 四个元素的线性插值代替球面插值

四元数样条 Squad

对于两个方位之间的移动, $slerp$ 能够保证角速度不变, 但对于多个方位之间的移动, 各段方位之间的角速度将产生突变
以下介绍四元数样条算法 $squad$ (Spherical and Quadrangle), 该样条算法能保证轨迹的角速度连续可导, 但角速度不再是不变的

已知各个控制点的方位构成一个四元数序列 $q_1,q_2,\dots,q_n$
现引入辅助控制点的方位序列 $s_1,s_2,\dots,s_n$
其中的辅助控制点的方位满足

$$s_i=q_i\exp(-\frac{\log(q_i^{-1}q_{i+1})+\log(q_i^{-1}q_{i-1})}{4})$$

注意到, 由于 $q_0,q_{n+1}$ 未定义, 因此辅助控制点方位 $s_1,s_n$ 无法确定, 一般规定 $q_0=q_1,q_{n+1}=q_n$

现对于序列中相邻的两个控制点 $q_i,q_{i+1}$, 设控制点间的比例值为 $h$, 则这两个控制点之间的样条插值结果为

$$squad(q_i,q_{i+1},h)=slerp[slerp(q_i,q_{i+1},h),slerp(s_i,s_{i+1},h),2h(1-h)]$$