3. 刚体运动学：刚体位置、姿态、线速度、角速度、雅可比矩阵的推导与物理意义

刚体运动学是机器人动力学与仿真的基石，它研究的是物体在空间中运动的几何描述，而不涉及产生运动的力与力矩。对于机器人而言，无论是六轴工业机械臂还是移动操作平台，其末端执行器的位置、姿态以及它们随时间的变化率（速度）都必须通过刚体运动学来精确描述。本章将系统梳理刚体的位置与姿态表示方法、线速度与角速度的耦合关系，并重点推导雅可比矩阵——这一连接关节空间与操作空间的桥梁。理解这些概念，是后续进行动力学建模、轨迹规划与控制仿真的前提。

刚体运动学核心概念与雅可比矩阵映射关系 刚体位置与姿态 • 位置：P = [x, y, z]ᵀ • 姿态：旋转矩阵 R ∈ SO(3) • 齐次变换矩阵 T ∈ SE(3) • 欧拉角 / 四元数表示 自由度：6 (3位置 + 3姿态) 线速度与角速度 • 线速度：v = ṗ ∈ R³ • 角速度：ω ∈ R³ • 反对称矩阵：S(ω) • 空间速度：V = [v, ω]ᵀ 速度旋量（Twist） 雅可比矩阵 J(q) V = J(q) · q̇ [v, ω]ᵀ = [J_v; J_ω] · q̇ 关节空间 → 操作空间映射 正向运动学 微分运动学 雅可比矩阵的物理意义与工程应用 1. 速度映射 关节速度 q̇ → 末端速度 V V = J(q) · q̇ 用于轨迹规划与速度控制 2. 奇异性分析 det(J) = 0 → 奇异位形 末端自由度损失 需规划路径避开奇异点 3. 力/力矩映射 τ = Jᵀ · F 静力传递关系 用于力控与阻抗控制 图3-1 刚体运动学核心概念关系与雅可比矩阵的桥梁作用

3.1 刚体位置与姿态的数学描述

3.1.1 核心原理：从向量到齐次变换矩阵

刚体的位姿（Pose）由位置（Position）和姿态（Orientation）两部分组成。位置描述刚体上某参考点（通常为质心或工具中心点TCP）在基坐标系中的三维坐标，用向量 p = [x, y, z]ᵀ ∈ R³ 表示。姿态描述刚体坐标系相对于参考坐标系的旋转关系，最常用的表示是旋转矩阵 R ∈ SO(3)，它是一个3×3的正交矩阵，满足 RᵀR = I 且 det(R) = +1。旋转矩阵的列向量分别表示刚体坐标系各轴在参考坐标系中的方向余弦。

为了将旋转与平移统一处理，工程中广泛采用齐次变换矩阵 T ∈ SE(3)：

T = [ R   p ]
    [ 0ᵀ  1 ]

其中 R 为3×3旋转矩阵，p 为3×1位置向量，0ᵀ 为1×3零向量。齐次变换矩阵允许我们通过矩阵乘法将多个坐标变换串联起来，例如 T_0n = T_01 · T_12 · ... · T_(n-1)n，这是机器人正向运动学的基础。

除了旋转矩阵，欧拉角（如ZYX、ZYZ）和四元数也常用于姿态表示。欧拉角直观但存在万向锁问题，四元数无奇异且计算效率高，适合插值与滤波。在仿真中，旋转矩阵与四元数之间的转换是高频操作。

3.1.2 判断方法：如何选择合适的姿态表示

表示方法优点缺点适用场景
旋转矩阵 R无奇异、组合方便9个参数冗余、需正交化理论推导、坐标变换
欧拉角 (α,β,γ)直观、3个参数万向锁、插值不平滑人机交互、姿态显示
四元数 q无奇异、插值平滑需归一化、不直观姿态控制、SLAM、仿真
轴角 (θ, k)物理意义明确存在多值性旋转插值、理论分析

在实际工程中，建议：内部计算与存储使用四元数或旋转矩阵，人机界面显示使用欧拉角，而轴角常用于旋转增量计算。例如在UR5e机械臂的仿真中，底层控制采用四元数，而上位机监控界面显示欧拉角。

3.1.3 案例拆解：六轴机械臂末端位姿计算

以典型的六自由度串联机械臂为例，各关节坐标系之间的变换矩阵由DH参数确定。假设某时刻各关节角为 q = [0, -π/4, π/3, 0, π/6, 0]ᵀ，通过连乘各相邻坐标系的齐次变换矩阵，可得到末端坐标系相对于基坐标系的 T_06。该矩阵的左上3×3子块即为末端姿态，右上3×1子块即为末端位置。例如，若计算得到 p = [0.35, 0.12, 0.58]ᵀ m，R 的第三列为 [0.707, 0, 0.707]ᵀ，则说明末端工具坐标系Z轴在基坐标系XZ平面内呈45°倾斜。这一计算过程在MATLAB Robotics Toolbox或Python的Pinocchio库中均可一行代码实现，但理解其矩阵连乘的本质至关重要。

3.1.4 常见误区与总结

常见误区：将旋转矩阵的列向量误认为是行向量。旋转矩阵 R 的第 i 列表示刚体坐标系 i 轴在参考坐标系中的投影，而非参考坐标系轴在刚体坐标系中的投影。若混淆，会导致坐标变换方向错误。

此外，齐次变换矩阵的逆并非简单的转置，而是 T⁻¹ = [Rᵀ, -Rᵀp; 0, 1]。在仿真中频繁求逆时，应使用该解析形式而非通用矩阵求逆，以提升数值稳定性与计算效率。

3.2 线速度与角速度：刚体运动的微分描述

3.2.1 核心原理：速度旋量（Twist）

刚体的瞬时运动可分解为线速度 v 与角速度 ω。线速度是位置向量对时间的导数，即 v = ṗ；角速度则描述姿态变化的速率。对于旋转矩阵 R(t)，其时间导数满足 Ṙ = S(ω) · R，其中 S(ω) 是由角速度向量构成的反对称矩阵：

S(ω) = [  0   -ωz   ωy ]
        [  ωz   0   -ωx ]
        [ -ωy   ωx   0  ]

这一关系是刚体运动学的核心微分方程。将线速度与角速度组合为六维向量 V = [v, ω]ᵀ，称为速度旋量或空间速度。在机器人学中，通常将角速度放在前面（[ω, v]ᵀ）或后面（[v, ω]ᵀ），不同教材约定不同，使用时需注意一致性。

值得注意的是，角速度向量并非任何姿态角对时间的导数。例如欧拉角速率 [α̇, β̇, γ̇]ᵀ 与角速度 ω 之间存在一个依赖于当前欧拉角的变换矩阵 B(α,β,γ)，即 ω = B · [α̇, β̇, γ̇]ᵀ。在仿真中，若直接对欧拉角求导当作角速度使用，会产生严重误差。

3.2.2 操作流程：从关节速度到末端速度

  1. 建立运动学模型：根据DH参数或旋量理论建立各连杆坐标系。
  2. 计算各关节的贡献：对于转动关节 i，其角速度贡献为 q̇ᵢ · zᵢ₋₁（zᵢ₋₁ 为关节轴方向单位向量）；线速度贡献为 q̇ᵢ · (zᵢ₋₁ × pᵢₙ)，其中 pᵢₙ 为关节 i 到末端的向量。
  3. 叠加所有关节贡献：末端角速度 ω = Σ q̇ᵢ · zᵢ₋₁；末端线速度 v = Σ q̇ᵢ · (zᵢ₋₁ × pᵢₙ)。
  4. 组装雅可比矩阵：将上述线性关系写成矩阵形式 V = J(q) · q̇。

这一流程既适用于串联机械臂，也适用于移动机器人（将移动平台视为虚拟关节）。在仿真中，通常使用数值微分验证解析雅可比矩阵的正确性。

3.2.3 案例拆解：二连杆平面机械臂的速度分析

考虑一个平面二连杆机械臂，杆长 l₁ = l₂ = 1 m。当关节角 q = [30°, 45°]ᵀ，关节速度 q̇ = [0.5 rad/s, -0.3 rad/s]ᵀ 时，末端速度计算如下：

  • 关节1贡献：ω₁ = 0.5 · z₀，v₁ = 0.5 · (z₀ × p₁₂)，其中 p₁₂ 为关节1到末端的向量。
  • 关节2贡献：ω₂ = -0.3 · z₁，v₂ = -0.3 · (z₁ × p₂₂)，其中 p₂₂ 为关节2到末端的向量。
  • 合成：ω = (0.5 - 0.3) · z₀ = 0.2 rad/s（平面运动角速度方向恒定）。
  • 线速度：v = v₁ + v₂，经计算得 v = [0.183, 0.683]ᵀ m/s。

通过雅可比矩阵 J(q) 可直接计算：V = J(q) · q̇。该案例验证了叠加原理的正确性，也展示了雅可比矩阵如何将关节空间的运动映射到操作空间。

3.2.4 常见误区与总结

经验提醒：角速度与欧拉角速率的区别是初学者最容易犯的错误。在仿真中，若需要从角速度积分得到姿态，应使用四元数微分方程 q̇ = 0.5 · q ⊗ [0, ω]ᵀ，而非直接对欧拉角积分。

此外，线速度与角速度的参考坐标系必须明确。同一物理运动在不同坐标系下的速度旋量不同，需要通过伴随变换进行转换。在机器人控制中，通常将速度旋量表示在基坐标系或工具坐标系中，需根据控制需求选择。

3.3 雅可比矩阵：从关节空间到操作空间的桥梁

3.3.1 核心原理：雅可比矩阵的定义与构造

雅可比矩阵 J(q) 是一个6×n矩阵（n为关节数），它将关节速度映射为末端速度旋量：

[ v ]   [ J_v ]
[ ω ] = [ J_ω ] · q̇

其中 J_v 是3×n的线速度雅可比子矩阵，J_ω 是3×n的角速度雅可比子矩阵。对于转动关节 i，雅可比矩阵的第 i 列为：

J_i = [ zᵢ₋₁ × (pₙ - pᵢ₋₁) ]
      [ zᵢ₋₁                  ]

对于移动关节 i，则为：

J_i = [ zᵢ₋₁ ]
      [ 0    ]

雅可比矩阵不仅是速度映射的载体，其转置还用于力/力矩映射 τ = Jᵀ · F，其行列式用于奇异性分析，其伪逆用于冗余度机器人控制。因此，雅可比矩阵是机器人运动学中最核心的数学工具。

3.3.2 判断方法：雅可比矩阵的奇异性分析

当雅可比矩阵不满秩时，机器人处于奇异位形。此时，末端在某些方向上的运动无法实现，即使关节速度无穷大也无法补偿。判断方法：

  • 对于方阵（6自由度非冗余机器人），计算 det(J)，若 det(J) = 0 则奇异。
  • 对于非方阵（冗余机器人），计算 J·Jᵀ 的行列式，若为0则奇异。
  • 使用奇异值分解（SVD），检查最小奇异值是否接近零。

典型奇异位形包括：两关节轴共线（如肘部完全伸展）、腕部三轴交于一点（球腕奇异）。在轨迹规划中，应通过添加约束或调整路径来避开奇异区域。

3.3.3 案例拆解：六轴机器人奇异位形规避

以KUKA KR6机器人为例，当腕部中心点位于关节1和关节6轴线的交点时，机器人处于腕部奇异。此时，末端无法沿某一方向运动。在仿真中，我们通过以下步骤规避：

  1. 实时监测：在每个控制周期计算 J(q) 的最小奇异值 σ_min。
  2. 设定阈值：当 σ_min < 0.01 时，判定接近奇异。
  3. 路径调整：在奇异区域附近，将期望速度分解为奇异方向与非奇异方向，仅对非奇异方向进行速度控制，奇异方向则通过关节空间规划绕行。
  4. 阻尼最小二乘：使用 Jᵀ(JJᵀ + λ²I)⁻¹ 代替 J⁻¹，其中 λ 为阻尼系数，避免在奇异点附近产生过大的关节速度。

该案例表明，雅可比矩阵不仅是理论工具，更是工程中保障机器人安全运行的关键。

3.3.4 常见误区与总结

风险提示：雅可比矩阵是位形相关的，即 J = J(q)。在高速运动或大范围运动时，不能将雅可比矩阵视为常数。若在控制中使用了过期的雅可比矩阵，可能导致末端速度方向错误甚至失控。

此外，雅可比矩阵的参考坐标系选择会影响其形式。基坐标系雅可比与工具坐标系雅可比之间存在变换关系：J_tool = [Rᵀ, 0; 0, Rᵀ] · J_base。在仿真中，应根据控制器的需求选择合适的雅可比形式。

3.4 本章总结与工程实践要点

刚体运动学为机器人动力学与仿真提供了数学基础。位置与姿态的表示方法决定了运动学模型的复杂度与数值稳定性；线速度与角速度的微分关系是速度控制与轨迹规划的核心；雅可比矩阵则连接了关节空间与操作空间，是速度映射、力映射与奇异性分析的关键工具。在工程实践中，建议：

  • 优先使用四元数或旋转矩阵进行内部姿态计算，避免欧拉角的万向锁问题。
  • 在仿真中定期验证雅可比矩阵的正确性，可通过数值微分与解析结果对比。
  • 对于奇异位形，采用阻尼最小二乘或任务优先级方法进行鲁棒控制。
  • 注意雅可比矩阵的参考坐标系一致性，避免混用基坐标系与工具坐标系雅可比。

掌握本章内容后，读者应能够独立完成任意串联机器人的运动学建模、速度分析与奇异性判断，为后续的动力学建模（如拉格朗日方程、牛顿-欧拉递推）奠定坚实基础。

← 上一章 📖 返回目录 下一章 →