📚 课程目录第3章机器人建模

3. 机器人建模：URDF模型、传感器配置、TF坐标变换

3.1 核心原理：从物理机器人到数字孪生

在ROS2中开展路径规划之前，必须先回答三个问题：机器人长什么样？（几何与惯量）、它用什么感知世界？（传感器外参）、各个部件之间如何关联？（坐标树）。URDF（Unified Robot Description Format）正是解决这三个问题的统一描述框架。URDF基于XML语法，用描述刚体部件（含视觉、碰撞、惯性属性），用描述部件间的连接关系（含类型、原点、旋转轴）。

需要特别强调的是，URDF并非单纯的“画图工具”，它同时服务于仿真物理引擎（如Gazebo）、可视化调试（RViz）和导航规划（costmap、AMCL）。一个典型的差速驱动机器人URDF中，base_link是整棵坐标树的根，base_footprint是地面投影参考点，而激光雷达坐标系laser则通过固定关节（fixed joint）挂载在底盘上方。这种树状结构最终形成TF树，路径规划节点通过TF查询即可实时获取传感器相对于机器人中心的位置关系。

关键认知：URDF描述的是“静态结构”，TF描述的是“动态状态”。URDF中的joint原点定义了默认相对位姿，而TF广播则发布实时变化的关节角度或里程计位姿。两者缺一不可。

3.2 方法流程：从零编写URDF并完成传感器配置

编写URDF文件建议遵循“自底向上、先静后动、先核心后感知”的流程。下面以一台搭载2D激光雷达、IMU和RGB-D相机的差速机器人为例，给出核心步骤。

步骤1：定义底盘与轮子

底盘是承载所有传感器和轮子的基础。在URDF中，每个link需要提供visual（可视化网格）、collision（碰撞简化体）和inertial（惯性参数）。轮子通过continuous关节连接到底盘，以便仿真时驱动旋转。

<link name="base_link">
  <visual>
    <geometry><box size="0.4 0.3 0.12"/></geometry>
    <material name="blue"/>
  </visual>
  <collision>
    <geometry><box size="0.4 0.3 0.12"/></geometry>
  </collision>
  <inertial>
    <mass value="3.0"/>
    <inertia ixx="0.02" ixy="0" ixz="0" iyy="0.03" iyz="0" izz="0.02"/>
  </inertial>
</link>

<joint name="left_wheel_joint" type="continuous">
  <parent link="base_link"/>
  <child link="left_wheel_link"/>
  <origin xyz="0.15 0.18 -0.06" rpy="0 0 0"/>
  <axis xyz="0 1 0"/>  <!-- 绕y轴旋转 -->
</joint>

步骤2：挂载传感器并配置外参

传感器与底盘的相对位姿（即外参）通过fixed关节的origin定义。这里最容易出错的是旋转顺序：URDF中rpy按固定轴Z→Y→X旋转（即欧拉角），与TF的RPY约定一致。例如将激光雷达安装在底盘正上方0.2m处，水平朝前，则无需旋转；若安装时偏转90°，则需设置rpy="0 0 1.5708"。

<joint name="laser_joint" type="fixed">
  <parent link="base_link"/>
  <child link="laser_link"/>
  <origin xyz="0 0 0.2" rpy="0 0 0"/>
</joint>

<link name="laser_link">
  <visual>
    <geometry><cylinder radius="0.05" length="0.03"/></geometry>
  </visual>
</link>

步骤3：使用robot_state_publisher发布TF

URDF本身只是“图纸”，需要由robot_state_publisher节点解析并广播TF。启动后，该节点会读取参数服务器上的robot_description，周期性发布所有关节的TF变换。对于固定关节，只发布一次静态变换；对于连续关节，则需订阅/joint_states话题获取实时角度。

# 加载URDF到参数服务器
ros2 param set /robot_state_publisher robot_description "$(cat robot.urdf)"

# 启动TF发布节点
ros2 run robot_state_publisher robot_state_publisher

# 查看TF树
ros2 run tf2_tools view_frames

3.3 案例实战：在RViz中验证模型与TF

完成URDF编写后，必须进行可视化验证。启动RViz并添加RobotModel和TF显示组件，即可看到机器人三维模型和坐标轴。此时应重点检查：

坐标轴方向：base_link的X轴应指向机器人前进方向，Z轴竖直向上。若激光雷达的Z轴与底盘不一致，会导致点云旋转错位。
传感器位置：在RViz中点击laser_link坐标轴，确认其位于底盘正上方且水平。
TF树完整性：执行ros2 run tf2_tools view_frames生成PDF，检查是否存在断链或多余分支。

下面给出一个典型的TF树片段，展示各坐标系间的父子关系：

map
└── odom
    └── base_footprint
        └── base_link
            ├── left_wheel_link
            ├── right_wheel_link
            ├── laser_link
            ├── imu_link
            └── camera_link

常见错误：若在RViz中看到机器人模型“散架”或传感器位置漂移，首先检查URDF中joint的origin是否写错；其次确认robot_state_publisher是否正常运行。若使用Gazebo仿真，还需在URDF中添加<gazebo>标签配置传感器插件，否则传感器不会发布数据。

3.4 误区与复盘：建模阶段的五个“坑”

误区	后果	正确做法
1. 所有link都从base_link直接挂载	TF树过深或过浅，导航中坐标查询延迟高	按物理装配关系分层，如base_footprint→base_link→传感器
2. 忽略collision几何体	Gazebo中机器人陷入地面或碰撞异常	用简单box/cylinder近似，避免复杂mesh碰撞计算
3. 传感器rpy设置错误	点云/图像方向颠倒，路径规划避障失效	用RViz坐标轴逐轴验证，必要时用`tf2_echo`检查
4. 忘记设置inertial	仿真时机器人翻倒或运动异常	为每个link提供合理的质量和惯性张量
5. 将odom直接连到base_link	轮子打滑时定位漂移无法修正	严格遵循map→odom→base_footprint→base_link层级

复盘一个真实案例：某团队在调试导航时，发现代价地图中的障碍物始终偏离实际位置约10cm。排查后确认，激光雷达的URDF外参中z偏移写成了0.15m，而实际安装高度为0.25m。修正后障碍物对齐。这个案例说明，URDF中的每一个数字都会直接影响感知与规划的准确性，务必通过实测工具（如卷尺、水平仪）确认安装尺寸后再填入。

最后，建议将URDF文件纳入版本管理，并在每次修改后运行check_urdf工具进行语法校验。一个结构清晰、参数准确的URDF模型，是后续所有路径规划算法能够稳定运行的基石。