4. 机器人运动学基础：正运动学与逆运动学概念、D-H参数法、使用MoveIt的IK插件

各位同学，欢迎来到第四章。前面几章我们把ROS2、MoveIt和Nav2的架子搭起来了，也聊了聊传感器。但机器人要动起来，光有框架还不够，你得知道它该怎么动。这一章，咱们就扎进机器人运动学的核心。

说白了，运动学就是研究机器人“怎么动”的学问。它不关心力，不关心扭矩，只关心位置、速度和加速度。你想想看，一个机械臂要抓杯子，你脑子里得先知道每个关节转多少度，末端才能到那个位置。这就是运动学要解决的问题。

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

正运动学，英文叫Forward Kinematics。它的任务很简单：给定每个关节的角度，算出末端执行器在空间中的位姿。

我刚开始做机器人时，觉得这玩意儿挺玄乎。后来发现，其实就是一连串的坐标变换。每个关节都有一个坐标系，你把它们一个接一个地乘起来，就得到了末端的位姿。

举个例子，一个两连杆的简单机械臂：

  • 关节1旋转θ₁，关节2旋转θ₂
  • 连杆1长度L₁，连杆2长度L₂

那么末端的位置 (x, y) 就是：

x = L₁·cos(θ₁) + L₂·cos(θ₁+θ₂)
y = L₁·sin(θ₁) + L₂·sin(θ₁+θ₂)

嗯，看起来很简单对吧？但实际工业机器人有6个关节，每个关节之间还有偏移和扭转，计算就复杂多了。这时候，我们就需要一套标准化的方法——D-H参数法。

核心要点：正运动学是唯一的。给定一组关节角度，末端位姿是确定的。这在调试时很有用——你设好角度，就知道机器人会摆成什么姿势。

4.2 D-H参数法：标准化的建模工具

D-H参数法，全称Denavit-Hartenberg参数法。这是1955年提出的方法，到现在还是机器人建模的标配。我个人习惯用改进型D-H（Modified D-H），因为它在处理树形结构时更顺手。

每个关节需要四个参数：

参数 符号 含义
连杆长度 aᵢ 沿xᵢ轴，从zᵢ₋₁到zᵢ的距离
连杆扭转 αᵢ 绕xᵢ轴，从zᵢ₋₁到zᵢ的角度
连杆偏距 dᵢ 沿zᵢ₋₁轴，从xᵢ₋₁到xᵢ的距离
关节角度 θᵢ 绕zᵢ₋₁轴，从xᵢ₋₁到xᵢ的角度

有了这四个参数，你就可以写出相邻关节的变换矩阵。把所有矩阵乘起来，就是正运动学。

我在项目中遇到过一个问题：URDF文件里定义的关节轴方向，和D-H参数约定的z轴方向不一致。结果算出来的位姿总是差一个旋转。后来我养成了一个习惯——先画坐标系，再填参数。坐标系画对了，参数就不会错。

避坑指南：我曾经因为D-H参数表里a和d搞反，导致仿真里机械臂穿模。检查了整整一天才发现。建议你在写参数表时，对照URDF文件里的joint origin，逐项核对。

D-H参数法：相邻关节坐标系变换 x₀ z₀ y₀ 关节i 连杆 aᵢ xᵢ zᵢ yᵢ 关节i+1 θᵢ αᵢ dᵢ x轴 z轴 y轴 θᵢ: 关节角 αᵢ: 扭转角

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

逆运动学（Inverse Kinematics, IK）就反过来了：给定末端位姿，求每个关节的角度。这是实际控制中最常用的——你告诉机器人“去抓那个杯子”，它得自己算出每个关节转多少。

逆运动学比正运动学难得多。为什么？

  • 多解性：同一个末端位姿，可能有多种关节角度组合。比如你伸手够一个东西，可以肘部朝下，也可以肘部朝外。
  • 无解：目标位置超出了机器人的工作空间，怎么算都到不了。
  • 奇异性：在某些位姿下，关节速度会变得无穷大，导致控制失效。

我记得有一次调试六轴机器人，末端要沿着一条直线轨迹运动。结果在某个点，机器人突然“抽搐”了一下。后来发现是碰到了奇异点——腕关节的某个轴几乎和另一个轴平行了。从那以后，我设计轨迹时都会刻意避开奇异区域。

注意：逆运动学没有通用的解析解。只有满足特定几何条件（比如三个相邻关节轴交于一点）的机器人，才有封闭形式的解析解。大多数情况下，我们依赖数值解法。

4.4 使用MoveIt的IK插件

好了，理论讲完了。咱们来看看在MoveIt里怎么用。

MoveIt默认使用KDL（Kinematics and Dynamics Library）作为IK求解器。KDL基于数值迭代，优点是通用，缺点是慢，而且可能陷入局部最优。

我个人更推荐TRAC-IK。这是2015年推出的一个IK求解器，比KDL快5-10倍，而且成功率更高。我在项目里用过，效果确实好。

配置TRAC-IK很简单。在MoveIt配置包里，找到config/kinematics.yaml文件，修改如下：

arm_group:
  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05
  kinematics_solver_attempts: 3

参数说明：

  • search_resolution：搜索步长，越小越精确，但越慢。0.005是个不错的折中。
  • timeout：每次求解的超时时间，单位秒。0.05秒就是50毫秒。
  • attempts：求解失败后的重试次数。

经验之谈：我曾经把timeout设成0.01秒，想着快点。结果求解成功率掉到了60%。后来改成0.05秒，成功率回到了95%以上。别为了省那几十毫秒，牺牲了稳定性。

在代码里调用IK也很直接。假设你已经有了一个move_group对象：

from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import Pose

# 创建move group
group = MoveGroupCommander("arm_group")

# 设定目标位姿
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

# 设置目标并求解
group.set_pose_target(target_pose)
plan = group.plan()

if plan:
    print("IK求解成功！")
    # 执行轨迹
    group.execute(plan, wait=True)
else:
    print("IK求解失败，请检查目标位姿是否可达")

嗯，这里要注意：set_pose_target只是设定了目标，真正的IK求解是在plan()里完成的。如果求解失败，plan()会返回None。

还有一个实用技巧：你可以指定初始猜测值，帮助IK求解器更快收敛：

# 设定初始关节角度
start_state = group.get_current_state()
start_state.joint_state.position = [0.0, -0.5, 0.5, 0.0, 0.0, 0.0]
group.set_start_state(start_state)

# 然后求解
plan = group.plan()

这在处理多解问题时特别有用。比如你想让机器人始终保持在“肘部朝下”的姿势，就可以把初始猜测设成那个姿势的关节角度。

核心要点：MoveIt的IK插件是可插拔的。你可以根据需求选择不同的求解器。TRAC-IK适合大多数场景，但如果你的机器人有特殊几何结构，也可以自己写插件。

好了，这一章的内容就到这儿。运动学是机器人控制的基石，理解了正逆运动学，你才能理解MoveIt是怎么规划轨迹的。下一章，咱们会深入MoveIt的规划管线，看看它内部是怎么工作的。