运动学基础：正运动学与逆运动学原理、DH参数建模、使用MoveIt2进行运动学求解

各位同学，欢迎来到第二章。上一章我们把仿真环境搭起来了，但机械臂还只是个“空壳子”。想让它在仿真里动起来，甚至完成抓取任务，必须先搞懂一件事——运动学。

说白了，运动学就是研究“关节怎么转，末端去哪”以及“末端要去哪，关节怎么转”这两件事。前者叫正运动学，后者叫逆运动学。嗯，听起来像绕口令，但别急，我带你一步步拆开。

2.1 正运动学：从关节到末端

正运动学很简单：你告诉机器人每个关节的角度，它告诉你末端执行器在空间里的位置和姿态。这就像你抬起手臂，知道肩膀和肘弯了多少度，自然就知道手在哪。

数学上，我们用齐次变换矩阵来描述这种关系。每个关节的转动或平移，都可以用一个4x4的矩阵表示。把从基座到末端的所有矩阵乘起来，就得到了末端的位姿。

核心公式：T_0^n = T_0^1 × T_1^2 × ... × T_{n-1}^n

其中 T_0^n 表示从基座坐标系到末端坐标系的变换矩阵。

我在项目中遇到过一个问题：有个同事写正运动学代码时，把矩阵乘法的顺序搞反了。结果机械臂末端的位置完全不对，差点撞到旁边的设备。记住，矩阵乘法不满足交换律，顺序错了，结果就全错了。

2.2 逆运动学：从末端到关节

逆运动学就麻烦多了。你告诉机器人末端要去哪，它得算出每个关节该转多少度。这就像你伸手去拿桌上的水杯——大脑自动算出了肩、肘、腕的角度，但这个过程其实非常复杂。

为什么复杂？因为逆运动学通常有多解，甚至无解。比如一个6轴机械臂，同一个末端位姿可能有8组关节角度解。你想想看，该选哪一组？

我的经验：实际项目中，我一般按“最短路径”原则选解——让每个关节的运动量最小，这样机械臂动作最平滑，也最省时间。

逆运动学的求解方法主要有两种：

解析法：通过几何或代数推导出封闭解。速度快，但只适用于特定构型的机械臂（比如后三个关节轴线交于一点）。
数值法：用迭代逼近的方式求解。通用性强，但速度慢，且可能陷入局部最优。

我曾经在一个项目里用数值法解逆运动学，结果机械臂在奇异点附近死活不收敛。后来换成解析法，问题就解决了。所以，能解析就别数值，这是老工程师的忠告。

2.3 DH参数建模

DH参数是运动学建模的“通用语言”。它用四个参数来描述相邻两个关节坐标系之间的关系：

参数	符号	含义
连杆长度	a_i	沿X_i轴，从Z_{i-1}到Z_i的距离
连杆扭角	α_i	绕X_i轴，从Z_{i-1}到Z_i的转角
连杆偏距	d_i	沿Z_{i-1}轴，从X_{i-1}到X_i的距离
关节角	θ_i	绕Z_{i-1}轴，从X_{i-1}到X_i的转角

我个人习惯用改进DH（Modified DH），因为它在处理树形结构时更直观。标准DH和改进DH的区别在于坐标系附着的位置不同，但最终结果是一样的。

避坑指南：我曾经在定义DH参数时，把a_i和d_i搞混了。结果仿真里机械臂的连杆长度全不对，看起来像变形金刚。后来我养成了一个习惯：每次建完DH模型，先用正运动学算一个已知位姿验证一下，确认无误再往下走。

下面这张图展示了DH参数建模的核心流程：

2.4 使用MoveIt2进行运动学求解

好了，理论讲完了。但实际开发中，我们不会手写运动学求解器——太容易出错了。MoveIt2帮我们封装好了这一切。

在MoveIt2中，运动学求解是通过插件机制实现的。默认的求解器是KDL（Kinematics and Dynamics Library），但你可以换成TRAC-IK、BioIK等。我个人推荐TRAC-IK，它在奇异点附近的鲁棒性比KDL好很多。

配置起来也很简单。在MoveIt2的配置文件中，找到kinematics.yaml，像这样设置：

# kinematics.yaml
arm:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
  kinematics_solver_attempts: 3

如果你想换成TRAC-IK，只需要把kinematics_solver改成：

arm:
  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  kinematics_solver_timeout: 0.005
  kinematics_solver_attempts: 3

小技巧：kinematics_solver_timeout这个参数很关键。设得太小，求解容易失败；设得太大，机械臂响应会变慢。我一般设0.005秒，够用。

在代码里调用运动学求解也很直观。下面是一个C++示例：

#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>

// 创建规划场景
planning_scene::PlanningScenePtr planning_scene = ...;

// 获取机器人模型
const robot_model::JointModelGroup* joint_model_group = 
    planning_scene->getRobotModel()->getJointModelGroup("arm");

// 设置目标位姿
geometry_msgs::Pose target_pose;
target_pose.position.x = 0.5;
target_pose.position.y = 0.2;
target_pose.position.z = 0.3;
target_pose.orientation.w = 1.0;

// 创建运动学约束
moveit_msgs::Constraints constraints = 
    kinematic_constraints::constructGoalConstraints("tool0", target_pose);

// 求解逆运动学
std::vector<double> joint_values;
bool found_ik = planning_scene->getKinematicSolver()->getPositionIK(
    constraints, joint_values, joint_model_group);

if (found_ik) {
    ROS_INFO("找到逆运动学解！");
    // 输出关节角度
    for (size_t i = 0; i < joint_values.size(); ++i) {
        ROS_INFO("Joint %zu: %f", i, joint_values[i]);
    }
} else {
    ROS_WARN("未找到逆运动学解！");
}

这段代码的核心逻辑很简单：构造一个目标位姿，丢给求解器，它返回一组关节角度。如果返回false，说明无解——这时候你需要检查目标位姿是否在机械臂的工作空间内。

我曾经踩过的坑：有一次我忘了设置orientation.w = 1.0，结果目标姿态是一个无效的四元数。求解器一直返回false，我排查了半天才发现是这个问题。所以，检查四元数是否归一化，这是基本功。

2.5 本章小结

运动学是机械臂控制的基石。正运动学告诉你“关节怎么动，末端去哪”，逆运动学告诉你“末端去哪，关节怎么动”。DH参数是描述这两者关系的标准语言。而MoveIt2把这些复杂的数学运算封装成了简单的API调用。

📚 课程目录

运动学基础：正运动学与逆运动学原理、DH参数建模、使用MoveIt2进行运动学求解

2.1 正运动学：从关节到末端

2.2 逆运动学：从末端到关节

2.3 DH参数建模

2.4 使用MoveIt2进行运动学求解

2.5 本章小结