3. 机械臂基础：URDF建模、运动学正逆解、MoveIt2配置与运动规划

好，我们进入第三章。这一章是机械臂导航的基石，说白了就是让机器人知道自己长什么样、胳膊能怎么动。如果你跳过这章直接去搞导航，后面大概率会摔跟头——我当年就吃过这个亏。

3.1 URDF建模：给机械臂画骨架

URDF（Unified Robot Description Format）是ROS里描述机器人结构的标准语言。它用XML把机械臂拆成一个个（刚体部件）和（关节）。

我个人习惯把URDF建模分成三步：

拆解结构：数清楚有几个连杆、几个关节，确定关节类型（旋转、平移、固定）。
定义坐标系：每个link都有一个自己的坐标系，joint描述两个link之间的相对位姿。
添加可视化与碰撞：用STL或基本几何体让模型能显示、能碰撞检测。

一个6轴机械臂的URDF骨架示例：

<robot name="my_arm">
  <link name="base_link">
    <visual>
      <geometry><box size="0.2 0.2 0.1"/></geometry>
    </visual>
  </link>

  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 0 0.1" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.14" upper="3.14" effort="10" velocity="1"/>
  </joint>

  <link name="link1">
    <visual>
      <geometry><cylinder length="0.3" radius="0.05"/></geometry>
    </visual>
  </link>
  <!-- 后续关节和连杆类似 -->
</robot>

避坑指南：我曾经在定义joint的origin时把rpy写反了，结果机械臂在Rviz里扭成了麻花。建议每次写完URDF都用check_urdf命令验证一下。

3.2 运动学正逆解：让机械臂学会算位置

运动学正解（Forward Kinematics, FK）是已知关节角度，求末端位姿。逆解（Inverse Kinematics, IK）则反过来——给定末端位姿，算关节角度。

你想想看，导航里机械臂要去抓一个目标点，本质上就是在做IK。但IK有个麻烦：解不唯一，甚至可能无解。

类型	输入	输出	特点
正解（FK）	关节角度 q₁, q₂, ..., qₙ	末端位姿 (x, y, z, roll, pitch, yaw)	唯一解，计算简单
逆解（IK）	末端位姿 (x, y, z, roll, pitch, yaw)	关节角度 q₁, q₂, ..., qₙ	多解/无解，计算复杂

我记得第一次手写IK时，用几何法解一个6轴臂，推导了整整两天。后来发现用数值法（比如牛顿-拉夫森迭代）更通用，但要注意初始值选不好容易发散。

注意：IK的奇异性问题。当机械臂处于某些特殊姿态（比如肘关节伸直）时，雅可比矩阵会降秩，导致解算失败或速度突变。我在项目里遇到过机械臂在奇异点附近剧烈抖动，后来加了阻尼最小二乘法（DLS）才稳住。

3.3 MoveIt2配置：从URDF到可规划

MoveIt2是ROS2下的运动规划框架。它把URDF、运动学、碰撞检测、规划算法打包在一起，让你能直接调用。

配置MoveIt2的标准流程：

生成SRDF：用MoveIt Setup Assistant加载URDF，定义规划组（比如"arm_group"包含joint1到joint6）、自碰撞矩阵、虚拟关节。
配置运动学插件：我常用KDL或TRAC-IK。TRAC-IK比KDL更鲁棒，尤其在奇异点附近。
设置规划管线：选择规划器（OMPL、STOMP等），配置规划时间、求解次数。
启动MoveIt2：通过launch文件启动move_group节点，连接机器人硬件或仿真。

一个简单的MoveIt2 C++规划调用示例：

#include <moveit/move_group_interface/move_group_interface.h>

moveit::planning_interface::MoveGroupInterface move_group("arm_group");
move_group.setStartStateToCurrentState();

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

move_group.setPoseTarget(target_pose);

// 规划并执行
moveit::planning_interface::MoveGroupInterface::Plan plan;
bool success = (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
if (success) {
  move_group.execute(plan);
}

避坑指南：我曾经在配置自碰撞矩阵时漏掉了一对连杆，结果规划出来的轨迹直接让机械臂自己撞自己。建议用Setup Assistant的"Auto-Generate Collision Matrix"功能，然后手动检查一遍。

3.4 运动规划：让机械臂走出安全路径

有了MoveIt2，运动规划就变成了调用API的事。但理解背后的原理能帮你更好地调参。

常用的规划算法：

RRT（快速随机搜索树）：适合高维空间，速度快但不保证最优。
RRT*：在RRT基础上优化路径，渐进最优。
STOMP：基于梯度的优化，适合平滑轨迹。

我个人习惯先用RRT快速找一个可行解，再用STOMP做平滑优化。这样既快又稳。

规划时还要考虑：

碰撞检测：用FCL库检测机械臂与自身、环境物体的碰撞。
关节限位：规划结果不能超出URDF里定义的limit范围。
速度/加速度约束：避免规划出急停急起的轨迹。

注意：MoveIt2默认的规划时间上限是5秒。如果环境复杂，5秒可能找不到解。我一般会调到10秒，同时增加重试次数。但别调太大，否则实时性会变差。

3.5 本章知识体系

下面这张图概括了URDF、运动学、MoveIt2和运动规划之间的关系：

嗯，这一章的内容就到这里。URDF是基础，运动学是核心，MoveIt2是工具，运动规划是最终目标。把这四块吃透了，后面做机械臂导航就会顺手很多。

📚 课程目录

3. 机械臂基础：URDF建模、运动学正逆解、MoveIt2配置与运动规划

3.1 URDF建模：给机械臂画骨架

3.2 运动学正逆解：让机械臂学会算位置

3.3 MoveIt2配置：从URDF到可规划

3.4 运动规划：让机械臂走出安全路径

3.5 本章知识体系