机器人运动学
概述
运动学（Kinematics）研究机器人各连杆之间的几何关系与运动关系，不涉及力和力矩。它是机器人学中最基础的分析工具，分为正运动学（FK）和逆运动学（IK）两大方向。
graph LR
A["关节空间<br/>q = (q₁, q₂, ..., qₙ)"] -->|正运动学 FK| B["任务空间<br/>x = (x, y, z, φ, θ, ψ)"]
B -->|逆运动学 IK| A
style A fill:#e8f4fd,stroke:#2196F3
style B fill:#fff3e0,stroke:#FF9800
前置知识
本文假设读者已掌握齐次变换矩阵与坐标系描述，详见 坐标系与变换。
1 DH 参数与正运动学
1.1 Denavit-Hartenberg 约定
DH 参数（Denavit & Hartenberg, 1955）用 4 个参数 描述相邻连杆之间的空间关系：
| 参数 | 符号 | 含义 |
|---|---|---|
| 连杆长度 | \(a_i\) | 沿 \(x_i\) 轴从 \(z_{i-1}\) 到 \(z_i\) 的距离 |
| 连杆扭转 | \(\alpha_i\) | 绕 \(x_i\) 轴从 \(z_{i-1}\) 到 \(z_i\) 的旋转角 |
| 连杆偏距 | \(d_i\) | 沿 \(z_{i-1}\) 轴从 \(x_{i-1}\) 到 \(x_i\) 的距离 |
| 关节角 | \(\theta_i\) | 绕 \(z_{i-1}\) 轴从 \(x_{i-1}\) 到 \(x_i\) 的旋转角 |
旋转关节 vs 移动关节
- 旋转关节：\(\theta_i\) 为关节变量，其余三个为常数
- 移动关节：\(d_i\) 为关节变量，其余三个为常数
1.2 相邻连杆齐次变换矩阵
从坐标系 \(\{i-1\}\) 到坐标系 \(\{i\}\) 的变换可分解为四步基本变换：
展开得到完整的 \(4 \times 4\) 齐次变换矩阵：
1.3 正运动学求解
对 \(n\) 自由度机器人，末端执行器相对于基座的位姿为所有相邻变换的连乘：
示例：2-DOF 平面机械臂
假设两个旋转关节，连杆长度分别为 \(l_1, l_2\)，DH 参数表：
| 连杆 | \(\theta_i\) | \(d_i\) | \(a_i\) | \(\alpha_i\) |
|---|---|---|---|---|
| 1 | \(\theta_1\) | 0 | \(l_1\) | 0 |
| 2 | \(\theta_2\) | 0 | \(l_2\) | 0 |
末端位置：
2 逆运动学
2.1 问题定义
给定末端执行器的期望位姿 \(T_{\text{desired}}\)，求解关节变量 \(q = (\theta_1, \theta_2, \ldots, \theta_n)\)：
逆运动学的核心挑战：
- 多解性：同一位姿可能对应多组关节角
- 无解性：目标可能超出工作空间
- 奇异性：某些构型下自由度退化
2.2 几何法（解析法）
适用于结构简单的机械臂（如 2-link、6-DOF with spherical wrist）。
2-link 平面臂逆解推导：
已知末端位置 \((x, y)\)，求 \((\theta_1, \theta_2)\)。
Step 1：由余弦定理求 \(\theta_2\)
\(\pm\) 号对应肘上/肘下两种构型
Step 2：利用 \(\text{atan2}\) 求 \(\theta_1\)
2.3 数值法（Newton-Raphson 迭代）
适用于一般构型的机械臂。
基本思想：将正运动学 \(f(q)\) 在当前解 \(q_k\) 处线性化，迭代求解。
完整迭代算法：
输入: 目标位姿 x_d, 初始猜测 q_0, 容差 ε, 最大迭代 N
输出: 关节角 q*
q ← q_0
for k = 1 to N:
e ← x_d - f(q) # 位姿误差
if ||e|| < ε:
return q # 收敛
J ← Jacobian(q) # 计算雅可比
Δq ← J⁺ · e # 伪逆求增量
q ← q + α · Δq # 步长 α ∈ (0,1]
return FAILURE
实践建议
- 当 \(J\) 不是方阵或接近奇异时，使用 阻尼最小二乘（DLS）：\(\Delta q = J^T(JJ^T + \lambda^2 I)^{-1} e\)
- \(\lambda\) 为阻尼因子，可自适应调节（Levenberg-Marquardt）
- 多次随机初始化有助于找到全局解
3 雅可比矩阵
3.1 定义与意义
雅可比矩阵描述了关节速度与末端速度之间的线性映射：
其中 \(\dot{x} \in \mathbb{R}^m\) 为末端速度（含线速度和角速度），\(\dot{q} \in \mathbb{R}^n\) 为关节速度。
对于空间机械臂，\(m = 6\)（3 线速度 + 3 角速度），雅可比形式为：
3.2 雅可比的计算方法
方法一：微分法
对正运动学方程直接求偏导。
方法二：几何法（更常用）
对旋转关节 \(i\)：
对移动关节 \(i\)：
其中 \(z_{i-1}\) 是关节 \(i\) 的旋转轴方向，\(p_{i-1}\) 是关节 \(i\) 的位置，\(p_n\) 是末端位置。
3.3 雅可比的应用
| 应用 | 公式 |
|---|---|
| 速度映射 | \(\dot{x} = J \dot{q}\) |
| 力映射 | \(\tau = J^T F\) |
| 逆运动学 | \(\dot{q} = J^{-1} \dot{x}\)（或伪逆） |
| 冗余分解 | \(\dot{q} = J^{\dagger}\dot{x} + (I - J^{\dagger}J)\dot{q}_0\) |
4 奇异性
4.1 定义
当雅可比矩阵 \(J(q)\) 秩亏 时，机器人处于奇异构型：
4.2 奇异性的后果
- 末端执行器在某些方向上 丧失自由度
- 逆运动学的关节速度趋于 无穷大
- 力的传递特性发生退化
4.3 奇异性的类型
| 类型 | 描述 | 例子 |
|---|---|---|
| 边界奇异 | 臂完全伸展或折叠 | 2-link 臂完全伸直 |
| 内部奇异 | 两个关节轴对齐 | 腕部万向节锁 |
4.4 处理方法
- 阻尼最小二乘（DLS）：添加正则项避免速度爆炸
- 奇异性感知规划：在轨迹生成时避开奇异区域
- 冗余自由度：利用零空间运动绕过奇异
5 工作空间
5.1 定义
机器人末端执行器所能到达的所有位置（和姿态）的集合。
- 可达工作空间（Reachable workspace）：末端能到达的所有点，不考虑姿态
- 灵巧工作空间（Dexterous workspace）：末端能以任意姿态到达的点
5.2 影响因素
- 连杆长度和 DH 参数
- 关节运动范围限制
- 关节类型（旋转/移动）
- 机械干涉和自碰撞
5.3 工作空间分析方法
- 解析法：对简单机构推导边界方程
- 数值法：蒙特卡洛采样 + 正运动学计算
- 操作度指标：\(w = \sqrt{\det(J J^T)}\)，量化离奇异性的远近
6 高级话题
6.1 冗余机器人
当自由度 \(n > m\)（关节数大于任务空间维度）时，机器人具有运动冗余。利用零空间投影可优化辅助目标：
其中 \(\dot{q}_0\) 是零空间中的任意速度，可用于：
- 避障
- 远离关节极限
- 最小化能量消耗
6.2 人形机器人运动学
人形机器人（见 人形机器人）具有高度冗余的树形运动链结构，其运动学分析需要：
- 浮动基座建模（6-DOF 虚拟关节）
- 多末端约束（双足接触、手部操作）
- 质心运动学（\(x_{\text{CoM}} = \frac{1}{M}\sum m_i p_i\)）
参考文献
- Craig, J. J. (2005). Introduction to Robotics: Mechanics and Control. Pearson.
- Siciliano, B. et al. (2009). Robotics: Modelling, Planning and Control. Springer.
- Lynch, K. M. & Park, F. C. (2017). Modern Robotics. Cambridge University Press.
- Corke, P. (2017). Robotics, Vision and Control. Springer.