3、机械臂运动学基础：正运动学与逆运动学原理、DH参数、常用机械臂模型（UR5、Kinova等）的URDF/Xacro建模方法

3.1 正运动学与逆运动学原理

3.1.1 核心原理：从关节空间到笛卡尔空间

正运动学解决的是"给定一组关节角度，求末端执行器在笛卡尔空间中的位置与姿态"的问题。对于n自由度的串联机械臂，其末端位姿可以通过依次连乘各关节的齐次变换矩阵得到。每个关节的变换矩阵描述了相邻连杆坐标系之间的相对关系，最终末端位姿矩阵为：

T_end = T_0_1 * T_1_2 * ... * T_(n-1)_n

其中T_(i-1)_i表示从第i-1个坐标系到第i个坐标系的齐次变换。正运动学是唯一确定的，计算复杂度为O(n)，适合实时计算。

逆运动学则相反，需要根据末端期望位姿反解所有关节角。这是一个非线性问题，存在多重解、无解或无穷解的情况。对于6自由度机械臂，通常存在8组解析解（若满足Pieper准则），但实际中需根据关节限位、避障和运动平滑性选择最优解。数值解法（如雅可比迭代法）通用性强，但计算量大且可能陷入局部极小值。

3.1.2 判断方法与操作流程

在实际ROS开发中，选择FK还是IK取决于任务需求：

• 轨迹规划：MoveIt内部同时使用FK和IK。规划前用FK验证当前状态，规划后用IK将目标位姿转换为关节角。
• 视觉抓取：相机识别物体得到末端目标位姿，必须通过IK求解关节角。
• 奇异位形判断：通过计算雅可比矩阵的行列式判断是否接近奇异，此时IK解不稳定。

操作流程上，ROS中通常使用moveit_commander或C++的KinematicsBase接口调用IK求解器。推荐使用KDL（Kinematics and Dynamics Library）或IKFast（基于解析法的快速求解器）。

3.1.3 案例拆解：UR5的IK求解与多解选择

以UR5机械臂为例，其6个旋转关节满足Pieper准则（后三个关节轴交于一点），因此存在解析解。在ROS中配置IKFast插件后，求解速度可达微秒级。实际抓取场景中，我们设定末端目标位姿为：

位置: [0.3, 0.0, 0.5]m
姿态: RPY = [0, 3.14, 0]rad（末端朝下）

IKFast返回8组解，但其中4组超出关节限位，2组会导致连杆碰撞，最终仅2组可行。通过选择"关节角变化最小"的准则，确定最优解。这一过程在MoveIt的ChooseSolutionCallback中实现。

3.1.4 常见误区与总结复盘

• 误区1：认为IK一定有解。实际上，目标位姿超出工作空间时IK无解，需在规划前检查可达性。
• 误区2：忽略多解选择策略。不同解对应不同构型，可能导致后续运动规划失败。
• 误区3：混淆FK与IK的输入输出。在调试时，应先用rosrun tf tf_echo验证FK结果，再调试IK。

3.2 DH参数建模方法

3.2.1 核心原理：标准DH与改进DH

DH参数通过4个参数（a, α, d, θ）描述相邻连杆间的相对位姿。标准DH（SDH）将坐标系固定在连杆末端，而改进DH（MDH）将坐标系固定在连杆首端。UR5官方使用SDH，而Kinova Gen3使用MDH。两者在URDF中的体现不同：SDH的变换顺序为RotZ→TransZ→TransX→RotX，MDH为RotX→TransX→RotZ→TransZ。

3.2.2 判断方法与操作流程

建立DH参数表的步骤：

1. 确定各关节的旋转轴方向，建立Z轴。
2. 确定相邻Z轴的公垂线方向，建立X轴。
3. 根据右手定则确定Y轴。
4. 测量或查阅官方文档获取a、α、d值。
5. 将DH参数转换为URDF中的joint origin（平移+旋转）。

转换公式为：平移量 = [a, -d·sin(α), d·cos(α)]，旋转量 = Rz(θ)·Rx(α)。

3.2.3 案例拆解：从UR5官方DH表到URDF

UR5官方DH表（SDH）如下：

joint1: a=0,     alpha=90°, d=0.0892, theta=q1
joint2: a=-0.425, alpha=0°,  d=0,      theta=q2
joint3: a=-0.39225, alpha=0°, d=0,     theta=q3
joint4: a=0,     alpha=90°, d=0.10915, theta=q4
joint5: a=0,     alpha=-90°, d=0.09465, theta=q5
joint6: a=0,     alpha=0°,  d=0.0823,  theta=q6

在URDF中，joint2的origin应为：

<origin xyz="-0.425 0 0" rpy="0 0 0"/>

而joint4的origin需同时处理平移和旋转：

<origin xyz="0 0.10915 0" rpy="0 0 1.5708"/>

注意：URDF中的rpy旋转顺序为固定轴RPY（绕固定X→Y→Z），与DH的旋转顺序不同，需仔细换算。

3.2.4 常见误区与总结复盘

• 误区1：混淆SDH与MDH。UR5官方使用SDH，但ROS的robot_state_publisher默认按URDF中的joint origin计算，不关心DH类型。
• 误区2：忽略关节零位偏移。UR5的关节零位与DH表中的θ=0对应，但实际机械臂可能有机械零位偏移，需在URDF中通过joint_origin的额外旋转补偿。
• 误区3：未验证DH参数。建模后应使用check_urdf和tf_echo验证各坐标系关系，确保与真实机械臂一致。

3.3 常用机械臂的URDF/Xacro建模方法

3.3.1 核心原理：URDF与Xacro的关系

URDF（Unified Robot Description Format）是ROS中描述机器人几何、运动学与动力学属性的XML格式。但URDF不支持变量、循环和宏定义，导致代码冗余。Xacro（XML Macros）通过宏定义、属性继承和数学表达式，实现参数化建模。例如，UR5的6个关节结构相似，可用一个宏定义重复调用。

3.3.2 操作流程：从零构建UR5的Xacro模型

1. 定义宏：创建ur5_macro.xacro，定义ur5_arm宏，包含6个link和6个joint。
2. 参数化：将DH参数作为宏的输入参数，通过${}表达式计算joint origin。
3. 添加mesh：使用STL或DAE文件作为可视化模型，并设置碰撞简化几何体（如圆柱、盒子）。
4. 配置传动：添加transmission标签，定义关节与执行器的映射关系，供ros_control使用。
5. 集成gazebo：添加gazebo标签，设置摩擦系数、阻尼等仿真参数。

关键代码片段：

<xacro:macro name="ur5_arm" params="prefix">
  <link name="${prefix}base_link">
    <visual>
      <geometry><mesh filename="package://ur5_description/meshes/base.dae"/></geometry>
    </visual>
    <collision>
      <geometry><box size="0.15 0.15 0.12"/></geometry>
    </collision>
  </link>
  
  <joint name="${prefix}shoulder_pan_joint" type="revolute">
    <origin xyz="0 0 0.0892" rpy="0 0 0"/>
    <parent link="${prefix}base_link"/>
    <child link="${prefix}shoulder_link"/>
    <axis xyz="0 0 1"/>
    <limit lower="-6.28" upper="6.28" effort="150" velocity="3.15"/>
  </joint>
  ...
</xacro:macro>

3.3.3 案例拆解：Kinova Gen3的模块化建模

Kinova Gen3是7自由度机械臂，其URDF建模更复杂。我们采用模块化策略：

• 将每个关节模块（含电机、减速器、编码器）封装为独立宏。
• 使用xacro:include引入公共的传感器和线缆模型。
• 通过property定义各模块的安装角度，实现不同构型。

实际项目中，我们为Gen3添加了末端力传感器和相机模型，通过xacro:include集成到主URDF中。在Gazebo仿真中，该模型成功实现了抓取任务的动力学仿真，关节力矩误差小于5%。

3.3.4 常见误区与总结复盘

• 误区1：忽略碰撞模型。使用精细mesh作为碰撞模型会导致碰撞检测极慢，应使用简化几何体。
• 误区2：未设置惯性参数。Gazebo仿真需要准确的惯性矩阵，否则动力学控制失效。可通过urdf_to_collada估算。
• 误区3：坐标系命名不一致。MoveIt和TF要求所有link名称唯一且与控制器配置匹配。