坐标变换

3 most important points

1. ${}_B^{A}P$ 是 B 以 A 为参考坐标系时的位置矢量；${}_B^{A}R$ / ${}_B^{A}T$ 表示 B 坐标系到 A 坐标系的旋转/变换
2. 平移旋转复合变换：${}_C^{A}P{=}_B^A{R}_C^{B}P{+}_B^{A}P$
3. 齐次变换矩阵分块：$$ T = \begin{bmatrix} R & P \ 0 & 1 \end{bmatrix}

## 5 thoughts 1. 2. 3. 4. 5. ## Notes ### 基本概念 > [!question] 为什么需要坐标变换 > > 比如在手眼系统中，我们可以通过视觉算法获取到工件坐标系 $P$ 与相机坐标系 $C$ 之间的关系，我们想要控制机械臂的末端运动到工件坐标系 $P$ 进行夹取，那么我们就要知道工件坐标系 $P$ 在机器人基坐标系 $B$ 下的位姿，如何获取呢？这就需要进行坐标变换了。 > > ![[Pasted image 20250901223936.png]] - 平移坐标变换 - 条件：坐标系 $A$、$B$、$C$ 的姿态是相同的，即三者之间的姿态对应的旋转矩阵都是单位矩阵 - 已知： - $A$ 为参考坐标系，$B$ 的位置矢量 $^A_BP=[1,1,1]^T$ - $B$ 为参考坐标系，$C$ 的位置矢量 $^B_CP=[3,0,1]^T$ - 求：$A$ 为参考坐标系，$C$ 的位置矢量 $^A_CP$ - 算法：$$ ^A_CP=^A_BP+^B_CP=[4,1,2]^T $$ - 旋转坐标变换 - 条件：坐标系 $A$、$B$ 原点重合 - 已知： - $B$ 相对 $A$ 的 $z$ 轴旋转了 $45\degree$ - $B$ 与 $C$ 姿态相同，$B$ 为参考坐标系，$C$ 的位置矢量为 $^B_CP=[3,0,1]^T$ - 求： - $A$ 为参考坐标系，$C$ 的位置矢量 $^A_CP$ - $A$ 为参考坐标系，$C$ 的旋转矩阵 $^A_CR$ - 算法：$$ ^A_CR = ^A_BR = \begin{bmatrix} B_x \cdot A_x & B_y \cdot A_x & B_z \cdot A_x \\ B_x \cdot A_y & B_y \cdot A_y & B_z \cdot A_y \\ B_x \cdot A_z & B_y \cdot A_z & B_z \cdot A_z \end{bmatrix} = \begin{bmatrix} \cos(45) & \cos(90+45) & \cos(90) \\ \cos(90-45) & \cos(45) & \cos(90) \\ \cos(90) & \cos(90) & \cos(0) \end{bmatrix} $$ > [!tip] 旋转变换公式 > > 绕 z 轴的旋转矩阵可以用以下公式表示： > > $$ > R(z, \theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} > $$ > > 绕 x 轴的旋转矩阵可以用以下公式表示： > > $$ > R(x, \theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta \\ 0 & \sin\theta & \cos\theta \end{bmatrix} > $$ > > 绕 y 轴的旋转矩阵可以用以下公式表示： > > $$ > R(y, \theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix} > $$ - & 平移旋转复合变换：拆分成旋转+平移 $$ ^A_CP=^A_BR^B_CP+^A_BP $$ ### 齐次变换矩阵 - 定义 - 已知旋转矩阵和平移矩阵：$$R = \begin{bmatrix}r_{11} & r_{12} & r_{13} \\r_{21} & r_{22} & r_{23} \\r_{31} & r_{32} & r_{33}\end{bmatrix} \quad \text{(旋转矩阵)},\space P = \begin{bmatrix}x \\ y \\ z \end{bmatrix} \quad \text{(平移矩阵)}$$ - 平移旋转复合变换=旋转+平移，可以被组合成一个 $4 \times 4$ 方阵，即**齐次变换矩阵**：$$T = \begin{bmatrix} r_{11} & r_{12} & r_{13} & x \\ r_{21} & r_{22} & r_{23} & y \\ r_{31} & r_{32} & r_{33} & z \\ 0 & 0 & 0 & 1 \end{bmatrix} \quad \text{(齐次矩阵)} $$ - $^A_B T$ 表示 B 坐标系到 A 坐标系的齐次变换，有 $^A_B T^{-1} = ^B_A T$ > [!example] 推导 > > 将齐次变换矩阵分块： > $$ > T = \begin{bmatrix} > R & P \\ > 0 & 1 > \end{bmatrix} > $$ > 假设 $^A_B T$ 表示 B 坐标系到 A 坐标系的齐次变换，B 坐标系下的点 C 坐标为 $^B_C P$，求 C 在 A 坐标系下的坐标 $^A_C P$，则有： > $$ > ^A_C P = ^A_B T \cdot ^B_C P > $$ > > 根据扩展后的矩阵计算 $^A_B T \cdot ^B_C P$： > $$ > \begin{bmatrix} > ^A_C P \\ > 1 > \end{bmatrix} = > \begin{bmatrix} > ^A_B R & ^A_B P \\ > 0 & 1 > \end{bmatrix} > \begin{bmatrix} > ^B_C P \\ > 1 > \end{bmatrix} = > \begin{bmatrix} > ^A_B R ^B_C P + ^A_B P \\ > 1 > \end{bmatrix} > $$ > > 与先前推导得到的公式相符。 - 运算意义 - 与位置矢量相乘：求出该位置矢量在另一坐标系下的表示 - 与齐次矩阵相乘：转换不同坐标系之间的关系，如：$$ ^A_B T^B_C T = ^A_C T $$ 例如，有个六自由度的机械臂，知道两两相邻关节之间的关系，就可以通过其次矩阵相乘的方法求出关节 6 在关节 0 下的位置和姿态 - 合成方法 - 使用 transfoms3d：`tfs.affines.compose(T,R,[1,1,1])` - 第三个参数是缩放因子，确保在进行变换时，对象在各个轴上的大小不会发生改变 ## Related ## Reference - [坐标变换](https://fishros.com/d2lros2/#/humble/chapt6/get_started/1.%E7%A9%BA%E9%97%B4%E5%9D%90%E6%A0%87%E6%8F%8F%E8%BF%B0?id=_5%e5%9d%90%e6%a0%87%e5%8f%98%e6%8d%a2) - [齐次坐标变换](https://fishros.com/d2lros2/#/humble/chapt6/get_started/5.%E9%BD%90%E6%AC%A1%E5%9D%90%E6%A0%87%E5%8F%98%E6%8D%A2)