3. 机械臂运动学基础：正运动学与逆运动学、DH参数、常用坐标系

机械臂运动学是机器人控制的理论基石。它回答两个核心问题：“关节转了多少，末端在哪？”（正运动学）和“末端要去哪，关节该转多少？”（逆运动学）。在ROS2中，无论是MoveIt2的路径规划、Rviz2中的模型显示，还是真实机械臂的伺服控制，底层都依赖运动学求解结果。本章将围绕DH参数建模、坐标系变换与正逆解逻辑展开，为后续编写ROS2控制节点打下数学基础。

3.1 常用坐标系：从世界到末端

在ROS2中，机械臂涉及多个坐标系，它们通过TF树（坐标变换树）关联。理解这些坐标系是理解运动学的第一步。

在Rviz2中，你可以通过“Fixed Frame”设置固定坐标系（通常为base_link），并显示TF树来观察各坐标系间的相对位姿。ROS2的tf2库负责维护这些坐标系的实时变换关系，而运动学计算正是提供这些变换的数学依据。

3.2 DH参数：描述相邻关节的“四要素”

Denavit-Hartenberg（DH）参数是一种系统化描述相邻连杆间几何关系的方法。它用四个参数唯一确定两个相邻关节坐标系间的变换矩阵。对于标准DH（SDH）和修正DH（MDH），参数定义略有差异，ROS2中URDF默认使用类似MDH的视觉习惯，但运动学库（如KDL）内部仍按标准DH逻辑处理。这里以标准DH为例说明。

四个参数分别为：

• a_i-1（连杆长度）：沿X_i-1轴，从Z_i-1移动到Z_i的距离。
• α_i-1（连杆扭转角）：绕X_i-1轴，从Z_i-1旋转到Z_i的角度。
• d_i（连杆偏距）：沿Z_i轴，从X_i-1移动到X_i的距离。
• θ_i（关节角）：绕Z_i轴，从X_i-1旋转到X_i的角度（对于旋转关节，这是变量）。

以常见的六轴串联机械臂（如UR5e或自研教学臂）为例，其DH参数表通常如下（数值仅为示意）：

有了DH表，相邻关节的齐次变换矩阵^T_i^i-1可由下式得到：

T_i^(i-1) = Rot(z, θ_i) * Trans(z, d_i) * Trans(x, a_i) * Rot(x, α_i)

在C++中，你可以使用Eigen库手动实现该矩阵，或直接调用ROS2的kdl_parser和KDL::ChainFkSolverPos_recursive完成正解计算。

3.3 正运动学：从关节空间到笛卡尔空间

正运动学（Forward Kinematics, FK）是已知各关节角度，求解末端执行器在基座坐标系下的位姿（位置+姿态）。其本质是依次右乘各关节的齐次变换矩阵：

T_06 = T_01 * T_12 * T_23 * T_34 * T_45 * T_56

其中T_06左上3×3子矩阵为姿态（旋转矩阵R），右上3×1为位置向量P。在ROS2中，你可以通过tf2_ros的TransformBroadcaster发布该变换，或使用moveit_msgs::msg::RobotState直接设置关节位置并查询末端位姿。

下面是一个使用moveit_py或C++接口计算正解的伪代码示例：

// 假设使用MoveIt2的RobotState
robot_state::RobotStatePtr current_state = move_group->getCurrentState();
const robot_state::JointModelGroup* joint_model_group =
    current_state->getJointModelGroup("arm_group");

std::vector<double> joint_values = {0.0, -1.57, 0.0, -1.57, 0.0, 0.0};
current_state->setJointGroupPositions(joint_model_group, joint_values);

Eigen::Isometry3d end_effector_pose =
    current_state->getGlobalLinkTransform("tool0");
std::cout << "末端位置: " << end_effector_pose.translation().transpose() << std::endl;
std::cout << "末端姿态(四元数): "
          << Eigen::Quaterniond(end_effector_pose.linear()).coeffs().transpose() << std::endl;

正运动学计算简单、唯一且无奇异问题，常用于仿真验证、可视化以及逆解结果的校验。

3.4 逆运动学：从笛卡尔空间回到关节空间

逆运动学（Inverse Kinematics, IK）是给定末端目标位姿，求解满足条件的关节角度。这是运动规划中最具挑战性的部分，因为：

• 多解性：同一末端位姿可能对应多组关节角（如肘部向上/向下）。
• 可能无解：目标点超出机械臂工作空间。
• 奇异位形：在特定姿态下，关节速度与末端速度的映射关系丢失自由度，导致求解不稳定。

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

1. 解析法（封闭解）：适用于满足Pieper准则的机械臂（后三个关节轴交于一点）。求解快、精度高，但需要针对具体构型推导公式。
2. 数值法（迭代法）：如雅可比伪逆法、牛顿-拉夫逊法。通用性强，但需要提供初始猜测，可能陷入局部最优，且计算量较大。

在ROS2中，MoveIt2默认使用KDL（Kinematics and Dynamics Library）的数值IK求解器，也支持配置TRAC-IK（一种更鲁棒的数值求解器）。实际使用中，TRAC-IK在奇异点附近表现更好，建议在kinematics.yaml中切换。

# 在move_group的config/kinematics.yaml中
arm_group:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  # 或使用 trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05
  kinematics_solver_attempts: 3

下面的SVG图展示了正逆运动学在ROS2控制流水线中的位置与关系，以及IK多解与奇异点的概念。

3.5 案例：在MoveIt2中完成一次正逆解验证

假设你已在RViz中加载了一个六轴机械臂（如panda或ur5e），并配置好MoveIt2。下面通过Python脚本演示如何调用正逆解接口：

import rclpy
from moveit_py.core import RobotModel, RobotState
from moveit_py.planning import MoveGroupPy

rclpy.init()
node = rclpy.create_node('fk_ik_demo')

# 加载机器人模型
robot_model = RobotModel(urdf='path/to/robot.urdf', srdf='path/to/robot.srdf')
robot_state = RobotState(robot_model)
joint_group = robot_model.get_joint_model_group('arm_group')

# --- 正运动学 ---
joint_values = [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785]  # 7自由度示例
robot_state.set_joint_group_positions(joint_group, joint_values)
fk_pose = robot_state.get_global_link_transform('tool0')
print(f"FK 位置: {fk_pose.translation()}")

# --- 逆运动学 ---
target_pose = fk_pose  # 用正解结果作为目标
success = robot_state.set_from_ik(joint_group, target_pose, 'tool0', timeout=0.5)
if success:
    ik_joints = robot_state.get_joint_group_positions(joint_group)
    print(f"IK 关节角: {ik_joints}")
else:
    print("IK 求解失败")

node.destroy_node()
rclpy.shutdown()

运行该脚本，你会发现IK求得的关节角与初始值不同（多解），但末端位姿一致。这就是正逆解闭环验证的基本方法。

3.6 常见误区与复盘

运动学是连接“关节角”与“笛卡尔空间”的桥梁。正运动学提供确定性映射，逆运动学则引入多解与奇异问题。在ROS2中，MoveIt2封装了大部分求解细节，但理解DH参数与坐标系变换的本质，能帮助你在遇到规划失败、奇异规避或自定义机械臂时快速定位问题。下一阶段，我们将在此基础上引入速度级运动学（雅可比矩阵），为力控与轨迹跟踪做准备。