ROS2机械臂视觉抓取系统开发 - 第4节
4. 机械臂运动学基础
4、机械臂运动学基础：正运动学、逆运动学、DH参数建模、使用MoveIt2进行运动规划
各位同学，欢迎来到第四章。前面我们聊了ROS2的基础，也搭好了机械臂的仿真环境。从这一章开始，我们要真正触及机械臂的「灵魂」——运动学。
说白了，运动学就是研究「怎么动」的学问。它不关心你用了多大的力，也不关心电机烫不烫，只关心你的关节转了多少度，末端执行器到了什么位置。嗯，这听起来有点抽象，但别急，我们一步步来。
本章核心脉络：从正运动学（已知关节角度求末端位姿）到逆运动学（已知末端位姿求关节角度），再到DH参数这个建模工具，最后用MoveIt2把理论跑通。一条线下来，你就能理解机械臂「想」和「做」之间的桥梁。
4.1 正运动学：从关节到指尖
正运动学，英文叫 Forward Kinematics。它的任务很简单：给你一组关节角度，算出末端执行器在空间中的位置和姿态。
举个例子。你抬起手臂，肩关节转了30度，肘关节转了45度，手腕转了10度。你的手指尖在哪里？正运动学就是回答这个问题的数学工具。
我记得刚入行时，带我的老工程师说：「正运动学是机械臂的呼吸，你必须闭着眼睛都能写出来。」当时觉得夸张，后来自己调了三年机器人，才明白这句话的分量。
在数学上，正运动学通常用齐次变换矩阵的连乘来表示：
T_0_n = T_0_1 * T_1_2 * ... * T_{n-1}_n其中每个 T 都是一个 4x4 的矩阵，包含了旋转和平移信息。你从基座开始，一个关节一个关节地乘过去，最后得到的就是末端位姿。
我的小技巧：刚开始学的时候，别急着看公式。先拿一支笔当机械臂，手动比划一下。肩关节转、肘关节转、腕关节转，看看笔尖怎么动。身体记忆比大脑记忆更可靠。
4.2 逆运动学：从指尖到关节
逆运动学（Inverse Kinematics, IK）就更有意思了。它反过来：给你一个目标位姿，让你算出每个关节该转多少度。
为什么说它「更有意思」？因为正运动学是唯一解——给定关节角度，末端位置是确定的。但逆运动学不同，它可能有多个解，也可能无解。
你想想看，你的手臂够一个杯子，可以肘关节朝下，也可以肘关节朝外。两种方式都能碰到杯子，但关节角度完全不同。这就是多解问题。
我在做焊接机器人项目时，就遇到过逆运动学无解的情况。目标点明明在机械臂的工作半径内，但就是算不出来。后来排查了半天，发现是腕关节的奇异性问题——说白了，就是某个关节到了极限位置，数学上出现了「除以零」的情况。
避坑指南：我曾经在调试六轴机器人时，逆运动学求解一直报错。查了两天，最后发现是DH参数中一个连杆长度写错了0.5毫米。嗯，0.5毫米，让整个求解器崩溃。所以，参数校验一定要做，别偷懒。
逆运动学的常用解法有两种：
- 解析法：通过几何关系直接推导出公式。速度快，但只适用于特定构型的机械臂（比如6轴臂有封闭解的条件）。
- 数值法：用迭代逼近的方式求解。通用性强，但速度慢，且可能陷入局部最优。
在实际工程中，我一般先用解析法快速求一组解，再用数值法做微调和避障优化。两者结合，效果最好。
4.3 DH参数建模：给机械臂「上户口」
DH参数，全称 Denavit-Hartenberg 参数。它是一套标准化的方法，用来描述机械臂每个连杆的几何关系。
说白了，就是给机械臂的每个关节和连杆「上户口」。四个参数：
| 参数 | 符号 | 含义 |
|---|---|---|
| 连杆长度 | a | 相邻关节轴之间的公垂线长度 |
| 连杆扭转角 | α | 相邻关节轴之间的夹角 |
| 关节距离 | d | 沿关节轴方向的偏移量 |
| 关节角度 | θ | 绕关节轴的旋转角度（变量） |
有了这四个参数，你就可以唯一确定相邻两个坐标系之间的变换关系。然后连乘起来，就是整个机械臂的运动学模型。
我个人习惯用改进型DH参数（Modified DH），因为它在处理树形结构时更直观。标准DH和改进DH的区别，说白了就是坐标系附着的位置不同——一个附着在连杆前端，一个附着在后端。选哪个不重要，重要的是保持一致。
实战要点：在ROS2中，URDF文件里定义的关节和连杆，最终都会被转换成DH参数。你写URDF时，其实就是在隐式地定义DH参数。所以，理解DH参数，能帮你写出更准确的URDF模型。
4.4 使用MoveIt2进行运动规划
好了，理论讲完了。现在我们来点实际的——用MoveIt2做运动规划。
MoveIt2是ROS2中用于运动规划的核心框架。它把正运动学、逆运动学、碰撞检测、轨迹优化这些东西都封装好了。你只需要告诉它「我要去哪里」，它就能帮你算出一条安全、平滑的路径。
使用MoveIt2的基本流程：
- 加载机器人模型：通过URDF和SRDF文件描述机械臂的几何和运动学信息。
- 配置规划器：选择运动规划算法，比如RRT、PRM、OMPL等。
- 设置目标：指定末端执行器的目标位姿，或者指定关节空间的目标角度。
- 执行规划：调用规划器求解，得到一条无碰撞的轨迹。
- 执行轨迹：将轨迹发送给控制器，驱动真实或仿真机械臂运动。
下面是一个简单的MoveIt2规划代码示例：
# Python示例：使用MoveIt2进行运动规划
from moveit2 import MoveIt2
from geometry_msgs.msg import Pose
# 初始化MoveIt2
moveit2 = MoveIt2(
node=node,
joint_model_group_name="arm_group",
planner_plugin_name="ompl_interface/OMPLPlanner"
)
# 设置目标位姿
target_pose = 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
# 规划并执行
result = moveit2.plan_pose(target_pose)
if result.success:
moveit2.execute(result.trajectory)
print("规划成功，机械臂已运动到目标位置")
else:
print("规划失败，请检查目标位姿是否可达")
我的经验：MoveIt2的规划器有很多参数可以调。比如RRT的步长、最大迭代次数。我一般先把步长设大一点（快速找到路径），再逐步缩小（优化路径质量）。别一上来就用默认参数，默认参数往往不是最优的。
另外，MoveIt2的碰撞检测非常实用。你可以在规划场景中添加障碍物，规划器会自动避开。我在做仓储机器人项目时，就是靠这个功能让机械臂在货架间穿梭，从来没撞过。
注意：MoveIt2的规划结果只是「运动学可行」，不代表「动力学可行」。也就是说，它算出来的轨迹可能加速度太大，导致电机过载。所以，实际部署时，一定要做轨迹的速度和加速度限制。
好了，这一章的内容就到这里。正运动学、逆运动学、DH参数、MoveIt2规划，这四个东西是机械臂开发的基石。你把这章吃透了，后面学轨迹规划、力控制、视觉抓取都会轻松很多。