机器人系统的精确控制依赖于对空间位置和姿态的准确描述。本章深入探讨机器人学中的坐标系建立、姿态表示方法及其工程权衡。我们将对比传统的DH参数法与现代的指数积方法，分析四元数、旋转矩阵、欧拉角在不同场景下的优劣，并介绍李群/李代数这一强大的数学工具。通过NASA瓦尔基里机器人的实例，我们将看到这些理论如何应用于复杂系统的设计中。
DH参数法是1955年提出的经典方法，通过四个参数完整描述相邻关节间的坐标变换。其核心思想是通过系统化的坐标系分配，将任意复杂的关节用最少的参数表示。
标准DH参数定义：
变换矩阵： \(T_i^{i-1} = \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}\)
DH参数的工程优势：
DH参数的局限性：
标准DH坐标系建立流程：
z_{i-1}
|
| d_i
|
x_{i-1}----> x_i (沿公垂线)
/|
/ |a_i
/ |
z_i |
PoE方法基于螺旋理论，将刚体运动表示为绕螺旋轴的指数映射。这种方法在现代机器人学中越来越受欢迎，特别是在需要优雅处理奇异性和进行几何直觉分析时。
螺旋轴表示： 对于旋转关节，螺旋轴$\xi_i$定义为： \(\xi_i = \begin{bmatrix} \omega_i \\ v_i \end{bmatrix} = \begin{bmatrix} \omega_i \\ -\omega_i \times q_i \end{bmatrix}\)
其中$\omega_i$是旋转轴方向，$q_i$是轴上任意一点。
指数映射： \(T(\theta) = e^{\hat{\xi}\theta} = I + \hat{\xi}\theta + \frac{(\hat{\xi}\theta)^2}{2!} + \frac{(\hat{\xi}\theta)^3}{3!} + ...\)
对于纯旋转，Rodrigues公式给出闭式解： \(e^{[\omega]\theta} = I + [\omega]\sin\theta + [\omega]^2(1-\cos\theta)\)
PoE的工程优势：
PoE的实施考虑：
| 场景 | 推荐方法 | 原因 |
|---|---|---|
| 工业机器人编程 | DH | 制造商支持，标准接口 |
| 研究原型开发 | PoE | 灵活性高，数值稳定 |
| 并联机构 | PoE | 自然处理闭链约束 |
| 教育入门 | DH | 系统化流程，资料丰富 |
| 轨迹优化 | PoE | 梯度计算简洁 |
| 实时控制 | DH | 计算效率略高 |
旋转矩阵$R \in SO(3)$是最直接的姿态表示，保持向量长度和夹角不变。
性质：
工程优势：
工程劣势：
正交化算法（Gram-Schmidt）：
# 伪代码
def orthogonalize(R):
c1 = R[:, 0]
c1 = c1 / ||c1||
c2 = R[:, 1] - (c1 · R[:, 1]) * c1
c2 = c2 / ||c2||
c3 = c1 × c2
return [c1, c2, c3]
欧拉角用三个连续旋转表示任意姿态，常见约定包括ZYX（航空）、ZYZ（机械臂）等。
ZYX欧拉角（Roll-Pitch-Yaw）： \(R_{ZYX}(\phi, \theta, \psi) = R_z(\psi)R_y(\theta)R_x(\phi)\)
工程优势：
工程劣势：
万向节锁检测与处理：
# 检测万向节锁
if abs(pitch) > 85 * pi/180: # 接近±90度
# 切换到四元数表示
use_quaternion_control()
else:
# 安全使用欧拉角
use_euler_control()
四元数$q = [q_w, q_x, q_y, q_z]$提供了紧凑且无奇异性的姿态表示。
基本运算：
| 单位约束：$ | q | = 1$ |
工程优势：
工程劣势：
SLERP插值实现： \(q_t = \frac{\sin((1-t)\theta)}{\sin\theta}q_0 + \frac{\sin(t\theta)}{\sin\theta}q_1\) 其中$\cos\theta = q_0 \cdot q_1$
开始
|
v
需要人机交互？ --是--> 欧拉角（配合范围限制）
|
否
v
需要优化/插值？ --是--> 四元数（SLERP）
|
否
v
需要直接变换？ --是--> 旋转矩阵
|
否
v
存储受限？ --是--> 轴角表示
|
否
v
默认：四元数（通用性最好）
李群是既具有群结构又具有流形结构的数学对象，在机器人学中主要涉及：
$SE(3)$群元素： \(T = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} \in SE(3)\) 其中$R \in SO(3)$，$p \in \mathbb{R}^3$
群运算性质：
李代数是李群在单位元处的切空间，提供了李群的线性化表示：
$\mathfrak{se}(3)$元素（螺旋）： \(\xi^{\wedge} = \begin{bmatrix} [\omega]_{\times} & v \\ 0 & 0 \end{bmatrix} \in \mathfrak{se}(3)\)
其中$[\omega]_{\times}$是反对称矩阵： \([\omega]_{\times} = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix}\)
指数映射（李代数→李群）： \(\exp: \mathfrak{se}(3) \rightarrow SE(3)\)
对于纯旋转（$v=0$）： \(e^{[\omega]_{\times}\theta} = I + \frac{\sin\theta}{\theta}[\omega]_{\times} + \frac{1-\cos\theta}{\theta^2}[\omega]_{\times}^2\)
对于一般螺旋运动： \(e^{\xi^{\wedge}\theta} = \begin{bmatrix} e^{[\omega]_{\times}\theta} & J(\theta)v \\ 0 & 1 \end{bmatrix}\)
其中$J(\theta)$是左雅可比： \(J(\theta) = I\theta + \frac{1-\cos\theta}{\theta}[\omega]_{\times} + \frac{\theta-\sin\theta}{\theta}[\omega]_{\times}^2\)
对数映射（李群→李代数）： \(\log: SE(3) \rightarrow \mathfrak{se}(3)\)
给定$T = \begin{bmatrix} R & p \ 0 & 1 \end{bmatrix}$：
1. 轨迹优化中的应用： 李代数提供了无约束优化空间，避免了旋转矩阵的正交约束：
优化问题：
min_{ξ} ||f(exp(ξ^∧)T_0) - T_target||²
梯度计算：
∂f/∂ξ = ∂f/∂T · ∂exp(ξ^∧)/∂ξ
2. 滤波器设计： 扩展卡尔曼滤波在$SE(3)$上的实现：
| 状态预测：$T_{k+1 | k} = T_{k | k} \cdot \exp(\xi^{\wedge}_k \Delta t)$ |
3. 插值与平滑：
# SE(3)测地线插值
def geodesic_interp(T1, T2, t):
T_rel = inv(T1) @ T2
xi = log(T_rel)
return T1 @ exp(t * xi)
4. 速度与加速度计算： 空间速度（Body Velocity）： \(V^b = T^{-1}\dot{T} = \begin{bmatrix} [\omega^b]_{\times} & v^b \\ 0 & 0 \end{bmatrix}\)
空间加速度需要考虑科里奥利项： \(\dot{V}^b = \text{ad}_{V^b}V^b + T^{-1}\ddot{T}\)
小角度处理： 当$\theta < \epsilon$时，使用泰勒展开避免数值不稳定： \(\frac{\sin\theta}{\theta} \approx 1 - \frac{\theta^2}{6}\) \(\frac{1-\cos\theta}{\theta^2} \approx \frac{1}{2} - \frac{\theta^2}{24}\)
归一化策略： 定期将$SO(3)$元素正交化，$SE(3)$保持最后一行为$[0,0,0,1]$
计算效率优化：
机器人系统中的奇异性主要分为三类：
1. 运动学奇异性：
2. 表示奇异性：
3. 算法奇异性：
雅可比奇异性检测：
def detect_singularity(J, threshold=1e-3):
# 方法1：条件数
cond_num = np.linalg.cond(J)
if cond_num > 1/threshold:
return True, "High condition number"
# 方法2：最小奇异值
_, s, _ = np.linalg.svd(J)
if s[-1] < threshold:
return True, "Small singular value"
# 方法3：可操作度
manipulability = np.sqrt(np.linalg.det(J @ J.T))
if manipulability < threshold:
return True, "Low manipulability"
return False, "Non-singular"
几何奇异性检测：
1. 阻尼最小二乘（DLS）： \(\dot{q} = J^T(JJ^T + \lambda^2I)^{-1}\dot{x}\) 其中$\lambda$是阻尼因子，可自适应调整： \(\lambda = \begin{cases} 0 & \text{if } \sigma_{min} \geq \epsilon \\ \lambda_{max}\left(1 - \left(\frac{\sigma_{min}}{\epsilon}\right)^2\right) & \text{if } \sigma_{min} < \epsilon \end{cases}\)
2. 梯度投影法： 利用零空间进行二次任务优化： \(\dot{q} = J^+\dot{x} + (I - J^+J)\nabla H\) 其中$H$是避奇异性的势函数： \(H = -\frac{1}{2}\log(\det(JJ^T))\)
3. 路径规划层面：
4. 冗余度利用：
# 7自由度机械臂的肘部角优化
def optimize_elbow_angle(q, J, task_vel):
# 主任务
q_dot_task = pinv(J) @ task_vel
# 零空间基
N = eye(7) - pinv(J) @ J
# 肘部角梯度
elbow_gradient = compute_elbow_gradient(q)
# 综合速度
q_dot = q_dot_task + N @ elbow_gradient
return q_dot
分层控制架构：
层级1：任务空间控制器
↓ (检测奇异性)
层级2：奇异性处理器
- 切换到关节空间控制
- 激活阻尼项
- 调整任务优先级
↓
层级3：关节控制器
状态机设计：
正常模式 ←→ 近奇异模式 ←→ 奇异模式
| | |
↓ ↓ ↓
笛卡尔控制 混合控制 关节控制
NASA瓦尔基里（Valkyrie/R5）是为火星探索任务设计的类人机器人，高1.8米，重125公斤，具有44个自由度。其坐标系设计需要满足：
基础坐标系定义：
坐标系层级：
世界坐标系
↓
基座坐标系（骨盆）
├── 躯干链
│ ├── 腰部（3 DOF）
│ └── 头部（2 DOF + 相机）
├── 左臂链（7 DOF + 手）
├── 右臂链（7 DOF + 手）
├── 左腿链（6 DOF）
└── 右腿链（6 DOF）
四元数为主，欧拉角为辅：
实现细节：
class ValkyriePose:
def __init__(self):
self.quat = [1, 0, 0, 0] # w, x, y, z
self.euler_display = None # 仅用于显示
def update_orientation(self, quat_new):
# 主更新路径
self.quat = normalize(quat_new)
# 辅助显示更新
if needs_display():
self.euler_display = quat_to_euler_safe(self.quat)
def quat_to_euler_safe(self, q):
# 检测万向节锁
test = q[0]*q[2] - q[1]*q[3]
if abs(test) > 0.4999: # 接近±90度
return None # 返回警告
return quat_to_euler(q)
扩展卡尔曼滤波（EKF）设计：
传感器同步：
IMU (1000Hz) ──┐
├─→ 时间同步器 ──→ EKF (200Hz)
相机 (30Hz) ────┤ │
│ ↓
力传感器 (500Hz)┘ 状态估计
操作员视角到机器人视角：
坐标变换链： \(T_{robot}^{world} = T_{base}^{world} \cdot T_{link}^{base} \cdot T_{tool}^{link}\)
双四元数提供了统一表示旋转和平移的优雅框架： \(\hat{q} = q_r + \epsilon q_d\) 其中$q_r$是旋转四元数，$q_d$是对偶部分，$\epsilon^2 = 0$。
优势：
瞬时螺旋轴： 任何刚体运动都可分解为绕某轴的旋转加沿该轴的平移： \(\mathcal{S} = \{\mathbf{s}; \mathbf{s}_0\}\) 其中$\mathbf{s}$是轴方向，$\mathbf{s}_0$是轴上一点。
Plücker坐标： 用6维向量表示空间直线： \(\mathcal{L} = (\mathbf{l}; \mathbf{m})\) 其中$\mathbf{l}$是方向向量，$\mathbf{m} = \mathbf{p} \times \mathbf{l}$是力矩。
并联机构分析： Stewart平台的运动学用螺旋理论更简洁：
抓取分析： 接触点的约束用旋量表示：
本章系统介绍了机器人学中的坐标系建立和姿态表示方法：
关键概念：
关键公式：
习题5.1 给定DH参数表： | i | $a_i$ | $\alpha_i$ | $d_i$ | $\theta_i$ | |—|——-|————|——-|————| | 1 | 0 | 90° | 0.1 | $\theta_1$ | | 2 | 0.5 | 0° | 0 | $\theta_2$ | | 3 | 0.3 | 0° | 0 | $\theta_3$ |
计算当$\theta_1 = 30°$，$\theta_2 = 45°$，$\theta_3 = -60°$时末端执行器的位姿。
Hint: 逐个计算变换矩阵，然后连乘。
习题5.2 证明旋转矩阵的行列式必须为1，并解释为什么-1的情况对应反射而非旋转。
Hint: 考虑旋转保持手性（chirality）。
习题5.3 实现四元数到旋转矩阵的转换函数，并验证单位四元数$q = [0.5, 0.5, 0.5, 0.5]$对应的旋转矩阵。
Hint: 使用公式$R = I + 2q_w[\mathbf{q}_v]_{\times} + 2[\mathbf{q}_v]_{\times}^2$
习题5.4 某机械臂在工作时检测到雅可比矩阵的条件数为1000，这意味着什么？应该采取什么措施？
Hint: 条件数反映矩阵的”病态”程度。
习题5.5 设计一个算法，检测6自由度机械臂的所有奇异配置，并生成奇异性地图。考虑如何在实时控制中使用这个地图。
Hint: 考虑采样关节空间，计算雅可比行列式。
习题5.6 推导并实现SE(3)上的B样条插值算法，使其能生成$C^2$连续的轨迹。与线性插值和SLERP对比。
Hint: 在李代数空间进行B样条，然后指数映射回李群。
习题5.7 某火星探测机器人使用IMU和视觉进行状态估计。由于火星重力（3.71 m/s²）与地球不同，如何修改EKF？考虑IMU标定误差的影响。
Hint: 重力向量影响加速度计测量模型。
习题5.8 分析并联机器人Stewart平台在接近奇异配置时的坐标系选择策略。如何设计坐标系使奇异性分析更简单？
Hint: 考虑动平台中心vs关节空间表示。
abs(a - b) < epsilon