坐标系
Body Frame
机体坐标系（Body Frame） 就是"以机器人自身为中心"的坐标系，跟着机器人一起移动和旋转。
想象你自己站在房间里：
- 世界坐标系：房间的东南西北，你不管怎么转头，"北"永远是北
- 机体坐标系：你的"前方"永远是你面朝的方向，你的"左边"永远是你左手边
世界坐标系（固定不动） 机体坐标系（跟着机器人转）
Y(北) Y(左)
^ ^
| |
| 机器人头 --> X(前)
+-----> X(东)
假设出口在机器人的"左前方 2 米"位置：
| 表示方式 | 世界坐标系 | 机体坐标系 |
|---|---|---|
| 机器人朝北 | exit_dir = (−1, 1) | exit_dir = (1, 1)（前方偏左） |
| 机器人朝南（转了 180°） | exit_dir = (−1, 1)（不变！） | exit_dir = (−1, −1)（后方偏右） |
| 策略要学的 | 同样的 exit_dir，朝北要左转，朝南要右转 →必须结合 yaw 判断 | 直接告诉策略"出口在前方偏左" →直接左转就行 |
在世界坐标系下，策略网络必须自己学会"看 yaw 角 + 看出口世界方向 → 推算应该怎么转"这个隐式三角函数关系。这对一个 [128, 64] 的小网络来说非常困难。
齐次变换矩阵（Homogeneous Transformation）
齐次变换矩阵是一个 \(4 \times 4\) 矩阵，同时表示旋转和平移：
\[
T = \begin{bmatrix}
R & \mathbf{t} \\
\mathbf{0}^T & 1
\end{bmatrix} = \begin{bmatrix}
r_{11} & r_{12} & r_{13} & t_x \\
r_{21} & r_{22} & r_{23} & t_y \\
r_{31} & r_{32} & r_{33} & t_z \\
0 & 0 & 0 & 1
\end{bmatrix}
\]
其中：
- \(R \in SO(3)\)：\(3 \times 3\) 旋转矩阵
- \(\mathbf{t} \in \mathbb{R}^3\)：平移向量
核心优势：将旋转和平移统一为矩阵乘法，多次变换只需连乘：
\[
T_{0n} = T_{01} \cdot T_{12} \cdot T_{23} \cdots T_{(n-1)n}
\]
逆变换：
\[
T^{-1} = \begin{bmatrix}
R^T & -R^T\mathbf{t} \\
\mathbf{0}^T & 1
\end{bmatrix}
\]
DH 参数（Denavit-Hartenberg Convention）
DH 参数是机器人正运动学中最标准的连杆坐标系建立方法。
四个参数
| 参数 | 符号 | 定义 |
|---|---|---|
| 关节角 | \(\theta_i\) | 绕 \(z_{i-1}\) 轴，从 \(x_{i-1}\) 到 \(x_i\) 的旋转角 |
| 连杆偏移 | \(d_i\) | 沿 \(z_{i-1}\) 轴，从 \(x_{i-1}\) 到 \(x_i\) 的距离 |
| 连杆长度 | \(a_i\) | 沿 \(x_i\) 轴，从 \(z_{i-1}\) 到 \(z_i\) 的距离 |
| 连杆扭转 | \(\alpha_i\) | 绕 \(x_i\) 轴，从 \(z_{i-1}\) 到 \(z_i\) 的旋转角 |
对于旋转关节，\(\theta_i\) 是变量；对于移动关节，\(d_i\) 是变量。
建立步骤
- 编号连杆：从基座（连杆 0）开始，到末端执行器
- 确定 \(z\) 轴：\(z_i\) 沿关节 \(i+1\) 的运动轴方向
- 确定原点：\(O_i\) 位于 \(z_{i-1}\) 和 \(z_i\) 的公垂线与 \(z_i\) 的交点
- 确定 \(x\) 轴：\(x_i\) 沿 \(z_{i-1}\) 到 \(z_i\) 的公垂线方向
- 确定 \(y\) 轴：由右手定则确定
- 读取参数：根据坐标系之间的几何关系确定 \(\theta_i, d_i, a_i, \alpha_i\)
单关节变换矩阵
\[
A_i = Rot(z, \theta_i) \cdot Trans(z, d_i) \cdot Trans(x, a_i) \cdot Rot(x, \alpha_i)
\]
展开为：
\[
A_i = \begin{bmatrix}
\cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i \\
\sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i \\
0 & \sin\alpha_i & \cos\alpha_i & d_i \\
0 & 0 & 0 & 1
\end{bmatrix}
\]
常见坐标系
世界坐标系（World Frame）
- 定义：固定在环境中的全局参考坐标系
- 特点：不随任何物体移动或旋转
- 用途：多机器人系统中的全局定位、地图构建
- 惯例：通常 \(Z\) 轴朝上，\(X\) 轴朝前
基座坐标系（Base Frame）
- 定义：固定在机器人基座上的坐标系
- 特点：对于固定基座机器人，与世界坐标系重合或有固定偏移
- 用途：运动学计算的起点（DH 参数中的 Frame 0）
工具坐标系（Tool Frame / End-Effector Frame）
- 定义：固定在末端执行器（如夹爪、焊枪）上的坐标系
- 特点：跟随末端执行器移动
- 用途：定义工具的接触点（TCP, Tool Center Point）、力控制参考系
- TCP 标定：通过多点标定方法确定工具坐标系相对于法兰的偏移
相机坐标系（Camera Frame）
- 定义：以相机光心为原点，\(Z\) 轴沿光轴方向
- 用途：视觉伺服（Visual Servoing）、手眼标定（Eye-Hand Calibration）
旋转表示方法
旋转矩阵（Rotation Matrix）
\(R \in SO(3)\)，满足 \(R^T R = I\)，\(\det(R) = 1\)
- 优点：无奇异性、便于矩阵运算
- 缺点：9 个参数表示 3 个自由度，冗余
欧拉角（Euler Angles）
用3个角度表示旋转，常见约定：
- ZYX（Yaw-Pitch-Roll）：先绕 \(Z\) 轴旋转 \(\psi\)（偏航），再绕 \(Y\) 轴旋转 \(\theta\)（俯仰），最后绕 \(X\) 轴旋转 \(\phi\)（横滚）
- ZYZ：常用于机械臂末端姿态描述
\[
R_{ZYX} = R_z(\psi) R_y(\theta) R_x(\phi)
\]
万向锁（Gimbal Lock）
当俯仰角 \(\theta = \pm 90°\) 时，偏航和横滚退化为同一旋转轴，丢失一个自由度。这是欧拉角表示的固有缺陷。
四元数（Quaternion）
四元数 \(\mathbf{q} = w + xi + yj + zk = (w, \mathbf{v})\)，其中 \(|\mathbf{q}| = 1\)
从轴角到四元数（绕单位轴 \(\hat{\mathbf{n}}\) 旋转角度 \(\theta\)）：
\[
\mathbf{q} = \left(\cos\frac{\theta}{2}, \; \hat{\mathbf{n}}\sin\frac{\theta}{2}\right)
\]
四元数旋转：将点 \(\mathbf{p}\) 旋转后：\(\mathbf{p}' = \mathbf{q} \mathbf{p} \mathbf{q}^{-1}\)
优点：
- 仅 4 个参数（归一化后 3 个自由度）
- 无万向锁问题
- 旋转组合只需四元数乘法
- 插值方便（Slerp 球面线性插值）
缺点：
- 不如旋转矩阵直观
- 存在双覆盖：\(\mathbf{q}\) 和 \(-\mathbf{q}\) 表示相同旋转
转换关系
| 从 \ 到 | 旋转矩阵 | 欧拉角 | 四元数 |
|---|---|---|---|
| 旋转矩阵 | -- | 提取角度（注意万向锁） | Shepperd 方法 |
| 欧拉角 | 三次旋转矩阵连乘 | -- | 先转旋转矩阵再转四元数 |
| 四元数 | \(R = I + 2w[\mathbf{v}]_\times + 2[\mathbf{v}]_\times^2\) | 先转旋转矩阵再提取 | -- |
实际应用示例
示例：2-DOF 平面机械臂正运动学
考虑一个两连杆平面机械臂，连杆长度分别为 \(L_1, L_2\)：
| 关节 | \(\theta_i\) | \(d_i\) | \(a_i\) | \(\alpha_i\) |
|---|---|---|---|---|
| 1 | \(q_1\) | 0 | \(L_1\) | 0 |
| 2 | \(q_2\) | 0 | \(L_2\) | 0 |
末端位置：
\[
x = L_1 \cos q_1 + L_2 \cos(q_1 + q_2)
\]
\[
y = L_1 \sin q_1 + L_2 \sin(q_1 + q_2)
\]
示例：坐标变换在 RL 中的应用
在强化学习中，将目标位置从世界坐标系转换到机体坐标系是常见操作：
import numpy as np
def world_to_body(target_world, robot_pos, robot_yaw):
"""将世界坐标系中的目标转换到机体坐标系"""
# 计算相对位置
delta = target_world - robot_pos
# 旋转到机体坐标系
cos_yaw = np.cos(robot_yaw)
sin_yaw = np.sin(robot_yaw)
target_body = np.array([
cos_yaw * delta[0] + sin_yaw * delta[1],
-sin_yaw * delta[0] + cos_yaw * delta[1]
])
return target_body
这样策略网络直接接收机体坐标系下的目标方向，不需要自己学习三角函数关系，大大简化了学习难度。