『robotics-1』spatial transform
空间变换与运动
一、自由度
位形：机器人上每个点位的全体的集合，可根据位形确定机器人上所有点的位置；如二维坐标 (x, y) 可表示平面上任意点位置
位形空间：包含所有可能的机器人位形的 n 维空间，一般记作 C-space；机器人位形通常用 C-space 中的一个特征点表示
自由度定义：表示机器人位形的最少连续实值坐标数
刚体的自由度：系统自由度 = (系统内各点的自由度之和) - (系统独立约束数)
三维空间中的刚体：6 个自由度，推导过程如下：
- 任选三维空间中刚体上的三个点A、B、C，分别记作 \((x_A, y_A, z_A)\)、\((x_B, y_B, z_B)\)、\((x_C, y_C, z_C)\)
- 由刚体性质，A和B的间距恒定值\(d_{AB}\)，故B只可能分布在以A为球心、\(d_{AB}\)为半径的球面上，即B可用经纬度 \((\lambda, \phi)\) 表示
- 由刚体性质，A和C、B和C的间距均为定值\(d_{AC}\)、\(d_{BC}\)，故C只可能分布在球A和球B的交圆上，即C可用圆上转角 \(\theta\) 表示
故三维空间内的刚体自由度为 6，可以用 \((x_A, y_A, z_A, \lambda, \phi, \theta)\) 表示
二维平面中的刚体：3 个自由度，相当于在三维空间的情形下新增了三个独立约束 \(z_A = 0, z_B = 0, z_C = 0\)
机器人的自由度：通过刚体和关节得到机器人的自由度
机器人关节：连接两个构件，可分为以下几个类别：
转动副：关节自由度 = 1，允许两个构件绕同一个关节轴旋转，又称铰链
注意：转动副 对另一平面刚体的约束 = 2，对另一空间刚体的约束 = 5（保留一个旋转自由度）
移动副：关节自由度 = 1，允许关节沿轴线方向平移运动，又称滑动
注意：移动副 对另一平面刚体的约束 = 2，对另一空间刚体的约束 = 5（保留一个平移自由度）
螺旋副：关节自由度 = 1，允许两个构件绕同一个关节轴旋转、且在旋转的同时平移运动
注意：螺旋副 对空间刚体的约束 = 5（保留一个旋转自由度，无法讨论对平面刚体的约束，下同）
圆柱副：关节自由度 = 2，允许两个构件沿关节轴做独立的平移和旋转
注意：圆柱副 对另一空间刚体的约束 = 4（保留一个旋转 + 一个平移自由度）
虎克铰：关节自由度 = 2，由两个正交排列的转动副组成，又称万向节
注意：虎克铰 对另一空间刚体的约束 = 4（保留两个旋转自由度）
球铰：关节自由度 = 3，类似人类的肩关节
注意：球铰 对另一空间刚体的约束 = 3（保留三个旋转自由度）
\(\text{Gr}\ddot{\text{u}}\text{bler}\) 公式：设机构有 \(N\) 个构件（含基座），有 \(J\) 个关节；第 \(i\) 个关节的自由度为 \(f_i\)、独立约束数为 \(c_i\)
则该机器人的自由度如下： \[ \begin{align} \text{dof} &= m(N - 1) - \sum_{i=1}^{J}c_i \\ &=m(N-1) - \sum_{i=1}^{J}(m-f_i) \\ &=m(N-1-J) + \sum_{i=1}^{J} f_i \end{align} \] 其中 \(m\) 表示刚体自由度，对于空间刚体 \(m = 6\)、对于平面刚体 \(m = 3\)；对于所有关节都满足 \(f_i + c_i = m\)平面 \(k\) 杆结构（例）：属于平面机构，\(m = 3\)
- 开链结构：又称串联机构，机构中不存在任何闭环
设机构有 \(k\) 个关节，则（含地面）有 \(k+1\) 个构件，\(\text{dof}=3((k+1) - 1 - k) + k*1 = k\)
- 闭链结构：有闭环的结构，由于地面本身也是一个构件，故可以与地面构成闭环
设机构有 \(k\) 个关节，则（含地面）有 \(k\) 个构件，\(\text{dof} = 3(k - 1 - k) + k*1 = k - 3\)
- 曲柄滑块结构：属于闭链结构；如下图共有 3 个转动副和
1 个移动副，与地面形成闭环
\(\text{dof} = 3(4 - 1 - 4) + 3 * 1 + 1 * 1 = 1\)
- 开链结构：又称串联机构，机构中不存在任何闭环
空间结构（例）：如下图 Delta 机器人，\(m = 6\)；如下图每条腿有 4 个球铰、3 个转动副、5 根杆
\(\text{则 dof} = 6(3*5+2 - 1 - (4*3+3*3)) + 3*3 * 1 + 3*4 * 3 = 15\)
注意：上平台为静平台（自由度 = 0），下平台为动平台（自由度 = 3），剩余 12 个自由度表示 3 个平行四边形分别绕其长轴旋转的自由度
二、刚体位形与转动
空间拓扑表示：
- n 维欧氏空间用 \(\mathbb{R}^n\) 表示，\(\text{S}^n\) 表示 n + 1 维空间内的 n 维球表表面（如 \(\text{S}^1\) 表示平面圆、\(\text{S}^2\) 表示空间内的球面）
- 拓扑表示的笛卡尔积：如 2R 机械臂的 C-space 可表示为 \(\text{S}^1 \times \text{S}^1\)，三维欧氏空间可表示为 \(\text{R}^3 \times \text{S}^2 \times \text{S}^1\)
参考坐标系：单位长度预先固定，可以放在空间中的任何位置
- 空间坐标系：定义的相对静止的坐标系，记作 {s}
- 物体坐标系：附着在某个运动刚体上的随动坐标系，记作 {b}
注意：任何参考三维坐标系都满足右手系
旋转操作与角速度：
旋转矩阵：记作 \(\textbf{R} = (r_{ij})_{3 \times 3} = (\hat{x}_b, \hat{y}_b, \hat{z}_b)\)，其三个列向量分别对应 {b} 的三个单位坐标轴
- 正则条件：\(\hat{x}_b\)、\(\hat{y}_b\)、\(\hat{z}_b\) 均为 单位向量
- 正交条件：\(\hat{x}_b \cdot \hat{y}_b = \hat{x}_b \cdot \hat{z}_b = \hat{y}_b \cdot \hat{z}_b = 0\)，即三个单位向量两两正交
注意：以上两个条件可以精简为矩阵形式 \(\textbf{R}^T \textbf{R} = \textbf{I}\)，且右手系条件下有 $ = 1$
旋转矩阵的应用：主要包含以下三种
表示姿态：\(\textbf{R}\) 本身就是原点与O重合的坐标系，\(\textbf{R}_{bc}\) 表示 {c} 相对 {b} 的位姿态；若基础坐标系默认为 {s} 系：
- 第一列表示 \(\textbf{R}\) 的 x轴 在 {s} 系下的坐标
- 第二列表示 \(\textbf{R}\) 的 y轴 在 {s} 系下的坐标
- 第三列表示 \(\textbf{R}\) 的 z轴 在 {s} 系下的坐标
注意：由旋转矩阵性质可知，\(\textbf{R}_{ab} = \textbf{R}_{ba}^{-1} = \textbf{R}_{ba}^{T}\)
变换参考坐标系：\(\textbf{R}_{ac} = \textbf{R}_{ab} \textbf{ R}_{bc}\)，可理解为将 {c} 在 {b} 下的姿态转移到 {a} 中
向量的坐标系变换：\(p_{a} = \textbf{R}_{ab} \text{ }p_b\)，也就是将向量 \(p\) 的坐标从 {b} 转移到 {a} 中注意：以上是"下角标相消"的两种等价表述，因为 \(\textbf{R}\) 本质上就是三个列向量坐标
旋转某一向量或坐标系：将 \(\textbf{R}\) 表示为旋转操作算子，记作 \(\textbf{R} = \textbf{Rot}(\hat{\omega}, \theta)\)，表示绕单位轴 \(\omega\) 旋转 \(\theta\) 角
相对基本轴的旋转：
- 围绕 x轴 旋转：\[\textbf{R} = \textbf{Rot}(\hat{x}, \theta) = \begin{bmatrix} 1 & 0 & 1 \\ 0 & \cos\theta & -\sin \theta \\ 0 & \sin \theta & \cos \theta \end{bmatrix} \]
- 围绕 y轴 旋转：\[\textbf{R} = \textbf{Rot}(\hat{y}, \theta) = \begin{bmatrix} \cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta \end{bmatrix} \]
- 围绕 z轴 旋转：\[\textbf{R} = \textbf{Rot}(\hat{z}, \theta) = \begin{bmatrix} \cos \theta & -\sin \theta & 0 \\ \sin\theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{bmatrix} \]
旋转坐标系：考虑将 {b} 绕单位轴 \(\hat{\omega}\) 旋转 \(\theta\) 角，即 \(\textbf{R} = \textbf{Rot}(\hat{\omega}, \theta)\)
- \(\hat{\omega}\) 在 {s} 中表达，绕固定坐标系的轴 \(\hat{\omega}\) 转动：\(\textbf{R}_{sb'} = \textbf{R R}_{sb}\)，即算子左乘
- \(\hat{\omega}\) 在 {b} 中表达，绕物体坐标系的轴 \(\hat{\omega}\) 转动：\(\textbf{R}_{sb'} = \textbf{R}_{sb}\textbf{ R}\)，即算子右乘
注意：对于两个坐标系，\(\omega\) 的值是相等的（比如都是各自坐标系下的 z 轴），但实际很可能不是空间内同一个轴
旋转向量：\(v' = \textbf{R} v\)，其中 \(\textbf{R}\) 是旋转算子（表示绕哪个轴旋转多少角度的 \(\textbf{R}\)）
角速度：设一个单位坐标系 \((\hat{x}, \hat{y}, \hat{z})\) 附着在一个旋转物体上，旋转轴为 \(\hat{\omega}\)
- 旋转角速度：\(\omega = \hat{\omega} \dot{\theta}\)，方向沿 \(\hat{\omega}\) 方向
- 单位轴速度：\(\dot{\hat{x}} = \omega \times \hat{x}\)，\(\dot{\hat{y}} = \omega \times \hat{y}\)，\(\dot{\hat{z}} = \omega \times \hat{z}\)，方向沿旋转切线方向
- 角速度矩阵表示：设 \(\mathbf{R}(t)\) 是 {b} 相对 {s} 在时刻 t
的姿态矩阵，\(\omega_s\) 表示 {s}
中的角速度，\(r_1(t)\) 表示{s} 中的
\(\hat{x}\)，其余轴同理
则有 \(\dot{r}_i = \omega_s \times r_i\)，\(i = 1,2,3\)；\(\dot{\mathbf{R}} = [\omega_s \times r_1, \omega_s \times r_2, \omega_s \times r_3] =\omega_s \times \mathbf{R}\)
三、转动的其它表示方法
欧拉角：又称 ZYX 欧拉角，总是围绕刚体自身附着的坐标系转动；令刚体 {b} 初始时与 {s} 重合
- 令刚体绕 \(\hat{z}_b\) 轴旋转角度 \(\alpha\)
- 令刚体绕新的 \(\hat{y}_b\) 轴旋转角度 \(\beta\)
- 令刚体绕新的 \(\hat{x}_b\) 轴旋转角度 \(\gamma\)
连续的欧拉角旋转运动可由以下算子描述： \[ \begin{align} &\text{R}(\alpha, \beta, \gamma) = \text{I} \text{ Rot}(\hat{z}, \alpha)\text{ Rot}(\hat{y}, \beta)\text{ Rot}(\hat{x}, \gamma) \\ &\text{由于始终是绕刚体附着坐标系旋转，故旋转算子都是}\textbf{右乘} \end{align} \]
注意：给定任意旋转矩阵 \(\text{R}\)，总存在一组参数 \((\alpha, \beta, \gamma)\) 满足上式；"ZYX" 表示旋转轴顺序
RPY角：相较于欧拉角，RPY角总是围绕固定坐标系转动；令刚体 {b} 初始时与 {s} 重合
- 令刚体绕 \(\hat{x}_s\) 轴转动角度 \(\gamma\)
- 令刚体绕 \(\hat{y}_s\) 轴转动角度 \(\beta\)
- 令刚体绕 \(\hat{z}_s\) 轴转动角度 \(\alpha\)
连续的RPY角旋转运动可由以下算子描述： \[ \begin{align} &\text{R}(\alpha, \beta, \gamma) = \text{Rot}(\hat{z}, \alpha)\text{ Rot}(\hat{y}, \beta)\text{ Rot}(\hat{x}, \gamma) \text{ }\text{I} \\ &\text{由于始终是围绕固定坐标系旋转，故旋转算子都是}\textbf{左乘} \end{align} \]
注意：RPY角通常用来描述飞机（或船）的转动，\(\hat{x}\) 沿飞行方向、\(\hat{y}\) 沿翼展方向、\(\hat{z}\) 沿垂直指向地面方向
单位四元数：设 \(\text{R}(\hat{\omega}, \theta) \in SO(3)\) 是旋转矩阵，则与 \(\text{R}\) 对应的单位四元数 \(q \in \mathbb{R}^4\) 定义为： \[ \begin{align} q = \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix} = \begin{bmatrix} \cos{(\dfrac{\theta}{2})} \\ \hat{\omega}\sin{(\dfrac{\theta}{2})} \end{bmatrix} \in \mathbb{R}^4 \end{align} \] 其中 \(||\hat{\omega}|| = ||q|| = 1\)，\(\theta \in [0, \pi]\)；单位四元数表示在三维单位球面上的一个动点
单位四元数 和 旋转矩阵 之间的转换：
- 旋转矩阵 \(\Rightarrow\) 单位四元数： \[ \begin{align} &q_0 = \dfrac{1}{2} \sqrt{1 + r_{11} + r_{22} + r_{33}} \\ &\begin{bmatrix} q_1 \\ q_2 \\ q_3 \end{bmatrix} = \dfrac{1}{4q_0} \begin{bmatrix} r_{32} - r_{23} \\ r_{13} - r_{31} \\ r_{21} - r_{12} \end{bmatrix} \end{align} \]
- 单位四元数 \(\Rightarrow\) 旋转矩阵：设 \(q = (q_0, q_1, q_2, q_3)\)，\(R\) 表示绕朝 \((q_1, q_2, q_3)\) 的单位轴旋转 \(2\cos^{-1} q_0\) 角度
单位四元数 和 旋转矩阵乘积：设 \(\text{R}_q, \text{R}_p\) 分别表示两个旋转矩阵，对应四元数分别为 \(q, p\)
将 \(q, p\) 分别写成复数矩阵形式： \[ Q = \begin{bmatrix} q_0 + iq_1 & q_2 + iq_3 \\ -q_2 + iq_3 & q_0 - iq_1 \end{bmatrix} \quad P = \begin{bmatrix} p_0 + ip_1 & p_2 + ip_3 \\ -p_2 + ip_3 & p_0 - ip_1 \end{bmatrix} \]
令 \(N = QP\)，记 \(N\) 为： \[ N = \begin{bmatrix} n_0 + in_1 & n_2 + in_3 \\ -n_2 + in_3 & n_0 - in_1 \end{bmatrix} \]
矩阵乘积 \(\text{R}_q\text{R}_p\) 对应的单位四元数即为 \((n_0, n_1, n_2, n_3)\)
四、刚体的一般运动
齐次变换矩阵：又称刚体运动群，是所有 \(4 \times 4\) 的实矩阵 \(\text{T}\) 的集合： \[\text{T} = \begin{bmatrix} \text{R} & p \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & p_1 \\ r_{21} & r_{22} & r_{23} & p_2 \\ r_{13} & r_{23} & r_{33} & p_3 \\ 0 & 0 &0 & 1 \end{bmatrix}\]
注意：\(\text{R}\) 是旋转矩阵，描述 {b} 相对 {s} 的姿态；\(p \in \mathbb{R}^3\) 是 {b}原点 在 {s} 中的坐标
齐次变换矩阵的性质：
- \(\text{T}^{-1}\) 也是齐次变换矩阵，\[\text{T}^{-1} = \begin{bmatrix} \text{R}^T & -\text{R}^Tp \\ 0 & 1 \\ \end{bmatrix}\]
- 两个齐次变换矩阵的乘积也是齐次变换矩阵
- 满足乘法结合律 \((\text{T}_1\text{T}_2)\text{T}_3 = \text{T}_1(\text{T}_2\text{T}_3)\)，但一般不满足交换律 \(\text{T}_1\text{T}_2 \ne \text{T}_2\text{T}_1\)
齐次变换的性质：设 \(x, y \in \mathbb{R}^3\)，用 \((\text{R}, p)\) 表示 \(\text{T}\)，用 \(\text{R}x + p\) 表示 \(\text{T}x\)
- 保等距性：\(||\text{T}x - \text{T}y|| = ||x - y||\)，其中 \(||\cdot||\) 表示 \(\mathbb{R}^3\) 中的标准范数
- 保角性：对 \(\forall z \in \mathbb{R}^3\)，都有 \(<\text{T}x-\text{T}z, Ty-Tz> = <x-z, y-z>\)，\(<\cdot, \cdot>\) 表示 \(\mathbb{R}^3\) 中的标准内积
注意：三角形(x, y, z)和变换后的新三角形(Tx, Ty, Tz)是全等的，即刚体运动
齐次变换矩阵的用途：
表示位形：\(\text{T}_{ab} \leftarrow (\text{R}_{ab}, p_{ab})\)，其中 \(\text{R}_{ab}\) 表示 {b} 相对 {a} 的位姿，\(p_{ab}\) 表示 {b}原点 在 {a} 中的坐标
注意：对于任意两个坐标系 {a} 和 {b}，总有 \(\text{T}_{ab} = \text{T}_{ba}^{-1}\)
改变参考坐标系：类似于旋转运动中的下角标相消原则
- \(\text{T}_{a\mathbf{b}}\text{T}_{\mathbf{b}c} = \text{T}_{ac}\)，相当于把 {c} 在 {b} 中的位形转移到 {a}
- \(\text{T}_{a\mathbf{b}} v_{\mathbf{b}} = v_{a}\)，其中 \(v_b\) 指向量\(v\)在{b}中的表达，\(v_a\) 指向量\(v\)在{a}中的表达
移动坐标系：
旋转算子：\[\textbf{Rot}(\hat{\omega}, \theta) = \begin{bmatrix} \textbf{R} & 0 \\ 0 & 1 \end{bmatrix}\]
移动算子：\[\textbf{Trans}(p) = \begin{bmatrix} 1 & 0 & 0 & p_x \\ 0 & 1 & 0 & p_y \\ 0 & 0 & 1 & p_z \\ 0 & 0 & 0 & 1 \end{bmatrix}\]
{b} 先相对 {s} 的 轴 \(\hat{\omega}\) 旋转 \(\theta\)，再相对 {s} 移动 \(p\)：（"先转再移"左乘）
\[\text{T}_{sb'} = \text{T}\text{T}_{sb} = \textbf{Trans}(p) \textbf{Rot}(\hat{\omega}, \theta)\text{T}_{sb} \\ = \begin{bmatrix} \text{R}\text{R}_{sb} & \text{R} p_{sb} + p \\ 0 & 1 \end{bmatrix}\]
{b} 先相对自身移动 \(p\)，再相对移动后坐标系的轴 \(\hat{\omega}\) 转动：（"先移再转"右乘）
\[\text{T}_{sb'} = \text{T}_{sb}\text{T} = \text{T}_{sb}\textbf{Trans}(p) \textbf{Rot}(\hat{\omega}, \theta) \\ = \begin{bmatrix} \text{R}_{sb}\text{R} & \text{R}_{sb}p + p_{sb} \\ 0 & 1 \end{bmatrix}\]
移动向量：\(v' = \textbf{R}v + p\)，其中 \(\textbf{R}\) 为旋转算子（表示绕哪个轴旋转多少角度的 \(\textbf{R}\)），\(p\) 为平移向量