title: 机器人正向运动学 tags: [机器人学, 运动学, 正向运动学, DH参数] created: 2026-04-10 sources: 4

机器人正向运动学

一句话摘要：正向运动学（Forward Kinematics, FK）是机器人学的基础问题，给定所有关节角，通过坐标系变换链乘法求解机械臂末端执行器在基坐标系中的位姿（位置+姿态）。

背景与定义

机器人正向运动学回答一个核心问题：已知机器人每个关节的角度/位置，机械臂末端在空间的具体位置和姿态是什么？

对于串联机械臂，每个关节都是独立的输入变量。从基座到末端，每经过一个关节就需要一次坐标系变换，最终通过变换矩阵的连乘得到末端相对于基座的总变换。这个过程就是正向运动学的计算过程。

正向运动学是机器人学所有后续问题的基础：逆向运动学需要FK作为函数输入，运动规划需要FK计算轨迹，控制需要FK来获得当前状态，碰撞检测需要FK得到各连杆位置。

DH参数法（Denavit-Hartenberg）是目前工业界最常用的标准化建模方法，通过四个参数就能完整描述每个连杆相对于前一个连杆的几何关系。

核心原理

问题定义

给定： - 机器人结构：n自由度串联机械臂，连杆数为n - 关节变量：q = [q₁, q₂, ..., qₙ]^T，每个qᵢ是第i个关节的位置（旋转关节为角度，平移关节为位移） - 连杆几何参数：每个连杆的长度、偏移、扭角等固定参数

求解： - 末端执行器相对于基坐标系的位姿 T₀ₙ ∈ SE(3)

数学表达：

T₀ₙ(q) = T₀₁(q₁) · T₁₂(q₂) · ... · T₍ₙ₋₁₎ₙ(qₙ)

这是一个从关节空间 q ∈ ℝⁿ 到任务空间 T ∈ SE(3) 的映射。

DH参数法

Denavit-Hartenberg方法用四个参数描述相邻连杆之间的变换关系：

参数	符号	定义	说明
连杆长度	aᵢ	沿连杆i公共法线，从关节i轴到关节i+1轴的距离	总是非负
连杆扭角	αᵢ	绕公共法线，从关节i轴旋转到关节i+1轴的角度	右手定则
连杆偏移	dᵢ	沿关节i轴，从连杆i-1原点到连杆i原点的距离	旋转关节为常数
关节角	θᵢ	绕关节i轴，从连杆i-1x轴旋转到连杆ix轴的角度	平移关节为常数

标准DH vs 改进DH：两种约定仅在坐标系分配方式不同，结果等价。标准DH把坐标系{i}放在连杆i的近端，改进DH把坐标系{i}放在连杆i的远端。

单连杆变换矩阵

根据DH参数，相邻连杆坐标系{i-1}到{i}的变换矩阵为：

T_(i-1)i =
[
  [cosθᵢ,  -sinθᵢ cosαᵢ,   sinθᵢ sinαᵢ,  aᵢ cosθᵢ],
  [sinθᵢ,   cosθᵢ cosαᵢ,  -cosθᵢ sinαᵢ,  aᵢ sinθᵢ],
  [     0,          sinαᵢ,          cosαᵢ,          dᵢ],
  [     0,               0,               0,           1]
]

这个公式记住顺序：先绕z轴转θᵢ，再沿z轴移dᵢ，再沿x轴移aᵢ，最后绕x轴转αᵢ。

完整计算流程

根据机械臂结构，建立DH参数表（每行四个参数：aᵢ, αᵢ, dᵢ, θᵢ）
对每个连杆，根据DH公式计算 T_(i-1)i
依次相乘得到末端位姿：T₀ₙ = T₀₁ × T₁₂ × ... × T_(n-1)n
从T₀ₙ提取位置p = T₀ₙ[1:3, 4]，旋转R = T₀ₙ[1:3, 1:3]

例子：2-DOF平面机械臂

对于两自由度平面机械臂，DH参数表：

i	aᵢ	αᵢ	dᵢ	θᵢ
1	L₁	0	0	θ₁
2	L₂	0	0	θ₂

计算得到：

T₀₁ =
[
  [cosθ₁, -sinθ₁, 0, L₁cosθ₁],
  [sinθ₁,  cosθ₁, 0, L₁sinθ₁],
  [     0,       0, 1,       0],
  [     0,       0, 0,       1]
]

T₁₂ =
[
  [cosθ₂, -sinθ₂, 0, L₂cosθ₂],
  [sinθ₂,  cosθ₂, 0, L₂sinθ₂],
  [     0,       0, 1,       0],
  [     0,       0, 0,       1]
]

T₀₂ = T₀₁ · T₁₂

末端x,y坐标：

x = L₁ cosθ₁ + L₂ cos(θ₁+θ₂)
y = L₁ sinθ₁ + L₂ sin(θ₁+θ₂)

这个结果符合直觉，就是平面矢量和。

例子：UR5 机械臂

UR5是工业界常用的6自由度协作机器人，其DH参数为：

i	aᵢ (mm)	αᵢ (rad)	dᵢ (mm)	θᵢ
1	0	-π/2	89.16	θ₁
2	-425	0	0	θ₂
3	-392.25	0	0	θ₃
4	0	π/2	109.15	θ₄
5	0	-π/2	94.65	θ₅
6	0	0	82.3	θ₆

代入DH公式连乘即可得到末端位姿。

关键特性

优点

计算简单：只需要矩阵乘法，O(n)时间复杂度
确定性：给定关节角，只有唯一确定的末端位姿
数值稳定：浮点数计算误差可控
标准化：DH参数表是工业界通用交换格式

局限性

只给出结果，不求解逆问题：FK是多对一映射，多个关节角对应同一个末端位姿
奇异点不特殊处理：在奇异构型雅可比矩阵秩亏，FK计算依然正确，但逆问题求解困难
DH参数依赖建模约定：不同约定得到不同参数表，需要转换

其他建模方法

方法	特点	适用场景
DH参数法	参数简洁，标准化	工业串联机械臂（主流）
指数坐标	基于李群李代数，几何直观	理论分析、轨迹优化
Product of Exponentials (PoE)	不需要连杆坐标系，从基坐标系直接指数乘积	研究、理论推导
URDF	ROS通用格式，包含碰撞、惯性信息	仿真、实际系统集成

应用场景

应用	说明
逆向运动学验证：IK求解后用FK验证结果是否正确	必须的校验步骤
轨迹跟踪：关节空间轨迹 → FK计算末端笛卡尔轨迹	运动控制
碰撞检测：FK计算每个连杆位姿 → 检测碰撞	运动规划
工作空间分析：在关节空间采样 → FK得到所有可达末端点	设计阶段
视觉伺服：基于当前图像误差 → FK更新估计关节位置	视觉控制
仿真环境：实时渲染机械臂姿态	算法开发调试

forward_kinematics