机械臂运动学基础：正运动学、逆运动学、DH参数、关节空间与笛卡尔空间

机械臂运动学是机器人控制的理论基石，它回答了两个核心问题：“机械臂末端到底在哪里”以及“如何让末端到达我想要的位置”。在ROS2与MoveIt2的框架下，无论是拖动示教、路径规划还是力控应用，底层都依赖运动学解算提供精确的位姿数据。本章将系统梳理正运动学与逆运动学的数学本质、DH参数的建模方法，以及关节空间与笛卡尔空间的转换逻辑，帮助你在后续编写MoveIt2规划器配置和自定义运动学插件时，具备扎实的理论判断力。

3.1 正运动学：从关节角到末端位姿的确定性映射

正运动学（Forward Kinematics, FK）解决的是：给定机械臂所有关节的转角（或位移），求解末端执行器在基坐标系下的位置和姿态。这是一个确定性映射——只要关节角确定，末端位姿必然唯一确定，不存在多解问题。在ROS2中，robot_state_publisher节点正是通过正运动学计算，将URDF中定义的关节角实时转换为各连杆坐标系之间的TF变换，从而发布/tf话题供导航、感知和规划模块使用。

3.1.1 核心原理：齐次变换矩阵的链式乘法

正运动学的数学本质是齐次变换矩阵的连乘。对于n自由度的机械臂，末端位姿矩阵T_base_to_end由各相邻连杆间的变换矩阵依次相乘得到：

T_base_to_end = T_0_1 × T_1_2 × ... × T_(n-1)_n

其中每个T_(i-1)_i是一个4×4齐次矩阵，包含旋转部分（3×3）和平移部分（3×1）。在ROS2的TF2系统中，这些变换被封装为geometry_msgs/TransformStamped消息，通过tf2_ros::TransformBroadcaster发布。MoveIt2的RobotState类内部维护了每个关节的当前角度，并调用setFromIK()或update()方法触发正运动学计算，更新所有连杆的位姿缓存。

3.1.2 判断方法：如何验证正运动学实现是否正确

在实际工程中，验证正运动学正确性通常采用以下步骤：

1. 零位验证：将所有关节置为0°，检查末端位姿是否与URDF中定义的初始位姿一致。
2. 单轴旋转验证：仅旋转第i个关节，观察末端轨迹是否为一个圆弧，且圆弧半径等于该关节到末端的距离。
3. 与RViz对比：在RViz中显示机器人模型，手动拖动关节滑块，对比/tf树中末端坐标系的位姿是否与模型视觉一致。
4. 数值对比：使用moveit_commander的get_current_pose()接口获取末端位姿，与独立编写的Python正运动学脚本计算结果对比，误差应小于1e-6。

3.2 DH参数：标准化连杆建模的通用语言

DH参数（Denavit-Hartenberg Parameters）是一种系统化描述相邻连杆之间相对位置和姿态的方法。它将每个连杆的坐标系建立规则标准化，仅用四个参数即可完整表达相邻坐标系的变换关系：连杆长度a、连杆扭转角α、关节距离d、关节转角θ。在MoveIt2中，虽然URDF已经包含了完整的几何信息，但理解DH参数仍然是配置运动学插件（如KDL、IKFast、TRAC-IK）时的重要基础，因为许多IK求解器内部使用DH参数进行数值计算。

3.2.1 DH参数建模流程

建立DH参数表需要遵循以下步骤：

1. 为每个关节指定z轴方向（旋转关节的旋转轴或移动关节的移动方向）。
2. 确定相邻关节z轴之间的公垂线，其长度即为a，公垂线与z轴的夹角为α。
3. 沿z轴方向，相邻公垂线之间的距离为d，绕z轴的旋转角为θ。
4. 将每个关节的参数填入DH表，形成标准化的运动学描述。

3.2.2 案例拆解：六轴机械臂的DH建模

以典型的六轴串联机械臂（如UR5e或Aubo-i5）为例，其DH参数表通常如下（具体数值因厂商而异）：

| 关节 | a (m) | α (rad) | d (m) | θ (rad) |
|------|-------|---------|-------|---------|
| 1    | 0     | π/2     | 0.1625| θ1      |
| 2    | -0.425| 0       | 0     | θ2      |
| 3    | -0.392| 0       | 0     | θ3      |
| 4    | 0     | π/2     | 0.1333| θ4      |
| 5    | 0     | -π/2    | 0.0997| θ5      |
| 6    | 0     | 0       | 0.0996| θ6      |

在MoveIt2中，虽然URDF不直接使用DH参数，但当你配置kinematics.yaml文件时，IK求解器（如KDL）会从URDF中提取关节和连杆信息，内部转换为类似DH的表示进行求解。理解DH参数有助于你诊断IK求解失败的原因——例如，当某个关节的α角接近π/2时，数值求解器可能在该关节附近出现奇异性。

3.3 逆运动学：从目标位姿到关节角的求解挑战

逆运动学（Inverse Kinematics, IK）是运动规划中最具挑战性的环节：给定末端执行器的目标位姿（位置+姿态），求解满足该位姿的关节角组合。与正运动学的唯一解不同，IK可能存在多解、无解或无穷解的情况，且求解过程涉及非线性方程组的迭代计算。MoveIt2默认使用KDL（Kinematics and Dynamics Library）作为IK求解器，也支持TRAC-IK、IKFast等替代方案。

3.3.1 核心原理：数值迭代与解析法

IK求解方法主要分为两类：

• 解析法（Closed-form）：针对特定构型的机械臂（如满足Pieper准则——三个相邻关节轴交于一点），通过代数推导直接得到封闭解。速度快、精度高，但通用性差。
• 数值法（Numerical）：基于雅可比矩阵的迭代优化，如牛顿-拉夫森法、阻尼最小二乘法（DLS）。通用性强，但可能陷入局部最优，且计算开销大。

MoveIt2的KinematicsBase接口定义了统一的IK求解接口，getPositionIK()方法接收目标位姿和初始猜测关节角，返回求解结果和求解状态。TRAC-IK通过同时运行多个随机初始猜测的求解器，显著提高了求解成功率。

3.3.2 判断方法：IK求解失败时的排查路径

当MoveIt2规划失败且日志提示IK无解时，按以下顺序排查：

1. 检查目标位姿是否在可达工作空间内：使用moveit_commander的get_current_state()和set_from_ik()测试，或通过compute_cartesian_path()验证路径点是否可达。
2. 检查初始猜测值：数值IK对初始值敏感，尝试将当前关节角作为种子，或使用set_pose_target()时传入avoid_collisions=True。
3. 检查奇异位形：当机械臂处于奇异位形（如肘部完全伸直）时，雅可比矩阵秩亏，IK求解不稳定。此时应调整目标位姿或增加冗余自由度。
4. 更换求解器：在kinematics.yaml中切换为TRAC-IK，其求解成功率通常比KDL高10%-20%。

3.4 关节空间与笛卡尔空间：规划器的双重视角

在MoveIt2中，运动规划可以在两个空间中进行：关节空间规划和笛卡尔空间规划。理解两者的区别和适用场景，是配置规划器参数和调试路径质量的关键。

3.4.1 核心原理与对比

3.4.2 操作流程：在MoveIt2中切换规划空间

在MoveIt2的C++或Python接口中，切换规划空间主要通过以下方式实现：

# Python示例：笛卡尔空间路径规划
from moveit_msgs.msg import RobotTrajectory
from moveit_commander import MoveGroupCommander

group = MoveGroupCommander("arm_group")
waypoints = []
waypoints.append([0.4, 0.1, 0.3, 0, 0, 0])  # 起点
waypoints.append([0.5, 0.2, 0.4, 0, 0, 0])  # 中间点
waypoints.append([0.6, 0.1, 0.3, 0, 0, 0])  # 终点

(plan, fraction) = group.compute_cartesian_path(
    waypoints,   # 路径点列表
    0.01,        # 步长（米）
    0.0,         # 跳跃阈值
    avoid_collisions=True
)

关节空间规划则直接使用set_joint_value_target()指定目标关节角，或使用set_pose_target()让MoveIt2内部先调用IK再规划关节路径。

3.4.3 案例拆解：笛卡尔路径规划失败后的降级策略

某3C电子装配场景中，机械臂需要沿直线轨迹将芯片从传送带搬运到测试工位。使用compute_cartesian_path()规划时，fraction返回值仅为0.72，意味着只有72%的路径点成功求解IK。分析发现，路径中段经过机械臂的奇异位形区域（肘部接近完全伸展）。

解决方案：

1. 将路径拆分为两段，中间插入一个过渡点，避开奇异区域。
2. 将jump_threshold从0.0调整为0.5，允许关节速度有适度突变。
3. 改用关节空间规划，先规划到过渡点，再规划到目标点，虽然路径不是严格直线，但通过增加中间点数量（每10mm一个点）将误差控制在±2mm以内，满足装配精度要求。

复盘经验：笛卡尔空间规划并非万能，当路径穿越奇异位形或超出工作空间时，规划器会返回部分路径。此时应结合任务精度要求，灵活选择规划空间，或通过增加冗余自由度（如7轴机械臂）来规避奇异问题。

3.5 常见误区与总结复盘

在机械臂运动学的学习和工程应用中，以下误区值得特别注意：

• 误区一：混淆关节空间与笛卡尔空间的单位。关节空间使用弧度，笛卡尔空间使用米和弧度，MoveIt2的API中set_pose_target()接受的位置单位是米，姿态单位是弧度，但部分用户误将角度制（度）直接传入，导致末端位姿偏差巨大。
• 误区二：忽略IK求解的初始值敏感性。数值IK求解器需要合理的初始猜测，否则可能收敛到错误的解或直接失败。MoveIt2默认使用当前关节角作为种子，但如果当前位形与目标位形差异过大，应手动设置set_start_state()。
• 误区三：认为DH参数与URDF完全等价。URDF使用XYZ+RPY描述关节变换，而DH参数使用a/α/d/θ，两者可以互相转换，但URDF不直接存储DH参数。在配置kinematics.yaml时，不需要手动填写DH参数，但理解DH有助于调试IK求解器的数值行为。
• 误区四：忽视奇异位形对规划的影响。即使目标位姿可达，如果路径经过奇异位形，笛卡尔规划可能失败，关节空间规划虽然能通过，但关节速度可能急剧增大，导致实际运动不平稳。