第4章 传感器与工具

添加激光雷达、摄像头、IMU模型，并用xacro优化你的URDF。

好，这一章我们开始给机器人装上「感官」。没有传感器的机器人就像盲人摸象，你想想看，光有骨架和关节，它根本不知道周围发生了什么。我个人习惯在建模初期就把传感器模型加进去，哪怕只是占位符——后面调试起来会省很多事。

4.1 添加激光雷达（Lidar）模型

激光雷达在ROS里通常用 gazebo_ros_laser 或者 gazebo_ros_ray_sensor 来模拟。但URDF层面，我们只需要定义它的几何形状和安装位置。说白了，就是把它当成一个特殊的连杆。

我在项目中遇到过好几次，有人把雷达直接焊在底盘上，结果雷达的坐标系和底盘重合，点云数据全乱套。所以一定要单独定义一个 lidar_link，然后用固定关节连到车体上。

<!-- 激光雷达连杆 -->
<link name="lidar_link">
  <visual>
    <geometry>
      <cylinder length="0.05" radius="0.04"/>
    </geometry>
    <material name="black"/>
  </visual>
  <collision>
    <geometry>
      <cylinder length="0.05" radius="0.04"/>
    </geometry>
  </collision>
</link>

<joint name="lidar_joint" type="fixed">
  <parent link="base_link"/>
  <child link="lidar_link"/>
  <origin xyz="0 0 0.15" rpy="0 0 0"/>
</joint>

嗯，这里要注意：origin 的 z 值决定了雷达离地高度。如果你做的是室内机器人，0.15米左右比较合理；户外越野的话，我建议抬高到0.3米以上，避免被杂草干扰。

4.2 添加摄像头（Camera）模型

摄像头模型更简单——一个长方体或者圆柱体都行。但真正重要的是它的光学中心位置。我记得有一次帮客户调试视觉抓取，摄像头模型放歪了0.5厘米，结果手眼标定折腾了两天。所以，一定要把摄像头的坐标系原点放在感光芯片中心。

<link name="camera_link">
  <visual>
    <geometry>
      <box size="0.03 0.04 0.02"/>
    </geometry>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <material name="dark_gray"/>
  </visual>
</link>

<joint name="camera_joint" type="fixed">
  <parent link="base_link"/>
  <child link="camera_link"/>
  <origin xyz="0.12 0 0.08" rpy="0 0.2 0"/>
</joint>

你看，rpy="0 0.2 0" 让摄像头稍微向下倾斜，这样能更好地看到地面附近的物体。如果你做的是自动驾驶，可能得平视甚至仰视。这个角度没有标准答案，全靠实际场景调。

4.3 添加IMU模型

IMU 在URDF里几乎不需要几何形状——它太小了，通常直接集成在主板或底盘上。但我强烈建议单独定义一个 imu_link，哪怕它的视觉尺寸是0。为什么？因为IMU的数据坐标系必须明确，你把它挂在 base_link 下，后面做滤波和姿态估计才不容易出错。

<link name="imu_link">
  <visual>
    <geometry>
      <box size="0.01 0.01 0.01"/>
    </geometry>
    <material name="red"/>
  </visual>
</link>

<joint name="imu_joint" type="fixed">
  <parent link="base_link"/>
  <child link="imu_link"/>
  <origin xyz="0 0 0.02" rpy="0 0 0"/>
</joint>

我曾经犯过一个错：把IMU直接放在 base_link 的origin上，结果后来换了一块更大的底盘，IMU的物理位置变了，但URDF里忘了改，导致IMU数据一直有偏差。所以，哪怕IMU看不见，也要给它一个独立的link，方便以后调整。

4.4 使用xacro优化模型

好了，现在你的URDF里多了三个传感器link和三个joint。如果每个传感器都手写一遍，文件会变得又臭又长。这时候xacro就派上用场了。说白了，xacro就是URDF的「宏模板」，让你用参数代替重复代码。

举个例子，把上面三个传感器统一成一个宏：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="my_robot">

  <xacro:property name="pi" value="3.1415926"/>

  <xacro:macro name="sensor_link" params="sensor_name parent_link xyz rpy">
    <link name="${sensor_name}_link">
      <visual>
        <geometry>
          <box size="0.02 0.02 0.02"/>
        </geometry>
        <material name="blue"/>
      </visual>
    </link>
    <joint name="${sensor_name}_joint" type="fixed">
      <parent link="${parent_link}"/>
      <child link="${sensor_name}_link"/>
      <origin xyz="${xyz}" rpy="${rpy}"/>
    </joint>
  </xacro:macro>

  <!-- 调用宏，一行一个传感器 -->
  <xacro:sensor_link sensor_name="lidar"  parent_link="base_link" xyz="0 0 0.15" rpy="0 0 0"/>
  <xacro:sensor_link sensor_name="camera" parent_link="base_link" xyz="0.12 0 0.08" rpy="0 0.2 0"/>
  <xacro:sensor_link sensor_name="imu"    parent_link="base_link" xyz="0 0 0.02" rpy="0 0 0"/>

</robot>

你看，原来要写十几行的内容，现在三行搞定。而且如果你想给所有传感器统一加一个颜色或者碰撞属性，只需要改宏定义就行。我在实际项目中，经常把传感器的型号、安装高度、甚至颜色都做成参数，方便不同车型复用。

最后总结一下这章的核心：传感器模型本质就是带特殊标签的连杆，而xacro能让你用更少的代码维护更多的传感器。我个人习惯把传感器宏单独放在一个 sensors.xacro 文件里，然后用 <xacro:include> 包含到主模型里。这样结构清晰，后期换传感器型号也方便。