2、单目相机建模：相机link与joint定义、光学中心坐标系、相机参数在URDF中的表示

单目相机，说白了就是咱们手机上最常见的那个摄像头。在机器人世界里，它是最基础也最常用的视觉传感器。今天我就带你一步步把它装进URDF里。

我记得刚入行那会儿，总觉得相机建模不就是加个盒子嘛，结果在标定环节吃了大亏——相机坐标系没对齐，整个视觉定位全偏了。嗯，咱们今天就把这事彻底理清楚。

2.1 相机link与joint的定义

相机在URDF里，本质上就是一个link加上一个joint。link描述相机的外形和物理属性，joint把它固定在机器人本体上。

2.1.1 相机link

一个典型的单目相机link长这样：

<link name="camera_link">
  <visual>
    <geometry>
      <box size="0.03 0.05 0.02"/>
    </geometry>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <material name="camera_black">
      <color rgba="0.1 0.1 0.1 1.0"/>
    </material>
  </visual>
  <collision>
    <geometry>
      <box size="0.03 0.05 0.02"/>
    </geometry>
  </collision>
  <inertial>
    <mass value="0.02"/>
    <inertia ixx="1e-6" iyy="1e-6" izz="1e-6" ixy="0" ixz="0" iyz="0"/>
  </inertial>
</link>

这里我习惯把相机外形做成一个扁长方体，尺寸根据实际硬件来。比如常见的USB摄像头，大概是30mm×50mm×20mm。质量设个20克，惯性矩算个近似值就行——又不是做有限元分析，差不多得了。

2.1.2 相机joint

相机怎么装到机器人上？用joint。我一般用fixed joint，因为相机通常是刚性固定的：

<joint name="camera_joint" type="fixed">
  <parent link="base_link"/>
  <child link="camera_link"/>
  <origin xyz="0.1 0 0.05" rpy="0 0 0"/>
</joint>

xyz表示相机相对于base_link的位置偏移，rpy是旋转角度。这里我把相机放在机器人前方10cm、上方5cm处，水平安装。

2.2 光学中心坐标系

这才是相机建模的核心。相机link的坐标系原点在物理中心，但真正成像的光学中心并不在那里。

光学中心坐标系，也叫camera_optical_frame，它的规矩是：

• Z轴：指向相机前方（镜头朝向）
• Y轴：指向下方（图像的行方向）
• X轴：指向右方（图像的列方向）

这和ROS里图像消息的坐标系约定完全一致。为什么这么定？因为OpenCV的图像坐标系是左上角为原点，行向下、列向右。ROS为了对齐，就把相机光轴指向Z正方向，Y向下，X向右。

咱们得在URDF里再加一个link来表示光学中心：

<link name="camera_optical_frame"/>

<joint name="camera_optical_joint" type="fixed">
  <parent link="camera_link"/>
  <child link="camera_optical_frame"/>
  <origin xyz="0 0 0.015" rpy="-1.5708 0 -1.5708"/>
</joint>

这里rpy的旋转很关键。从camera_link到camera_optical_frame，需要做两次旋转：

1. 绕X轴转-90度（让Y轴指向下）
2. 绕Z轴转-90度（让Z轴指向前）

合起来就是rpy="-1.5708 0 -1.5708"。xyz的偏移量0.015m，是因为我用的相机镜头到外壳表面有15mm的距离，光学中心在镜头处。

2.3 相机参数在URDF中的表示

URDF本身不直接存储相机内参（焦距、畸变系数等），它只描述几何和运动学。那相机参数放哪儿？

ROS的惯例是：

• URDF里只放相机link和joint，以及一个指向参数文件的标签
• 内参、分辨率、帧率等放在YAML配置文件里
• 通过robot_state_publisher发布tf树

但URDF里可以加一个<gazebo>标签来传递相机参数（如果你用Gazebo仿真的话）：

<gazebo reference="camera_link">
  <sensor type="camera" name="camera_sensor">
    <update_rate>30.0</update_rate>
    <camera>
      <horizontal_fov>1.0472</horizontal_fov>
      <image>
        <width>640</width>
        <height>480</height>
        <format>R8G8B8</format>
      </image>
      <clip>
        <near>0.1</near>
        <far>100.0</far>
      </clip>
    </camera>
  </sensor>
</gazebo>

这里horizontal_fov是水平视场角，单位弧度。1.0472弧度约等于60度，是常见的相机视场角。near和far是近远裁剪面，决定了相机能看到的最远最近距离。

对于真实机器人，相机参数通常放在一个单独的YAML文件里，比如camera_params.yaml：

camera_name: "my_camera"
image_width: 640
image_height: 480
camera_matrix:
  rows: 3
  cols: 3
  data: [530.0, 0.0, 320.0, 0.0, 530.0, 240.0, 0.0, 0.0, 1.0]
distortion_coefficients:
  rows: 1
  cols: 5
  data: [0.1, -0.05, 0.0, 0.0, 0.0]
rectification_matrix:
  rows: 3
  cols: 3
  data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
projection_matrix:
  rows: 3
  cols: 4
  data: [530.0, 0.0, 320.0, 0.0, 0.0, 530.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0]

这些参数怎么来的？用相机标定工具（比如ROS的camera_calibration包）对着一张棋盘格拍几张照片就出来了。我一般标定完会把参数存到这个YAML里，然后在launch文件里加载。

2.4 完整示例：一个可用的单目相机URDF

把上面所有东西拼起来，就是一个完整的单目相机URDF片段：

<robot name="camera_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- 相机本体 -->
  <link name="camera_link">
    <visual>
      <geometry>
        <box size="0.03 0.05 0.02"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="black">
        <color rgba="0.1 0.1 0.1 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.03 0.05 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.02"/>
      <inertia ixx="1e-6" iyy="1e-6" izz="1e-6" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <!-- 光学中心坐标系 -->
  <link name="camera_optical_frame"/>

  <!-- 相机到机器人本体的joint -->
  <joint name="camera_joint" type="fixed">
    <parent link="base_link"/>
    <child link="camera_link"/>
    <origin xyz="0.1 0 0.05" rpy="0 0 0"/>
  </joint>

  <!-- 光学中心到相机本体的joint -->
  <joint name="camera_optical_joint" type="fixed">
    <parent link="camera_link"/>
    <child link="camera_optical_frame"/>
    <origin xyz="0 0 0.015" rpy="-1.5708 0 -1.5708"/>
  </joint>

  <!-- Gazebo仿真参数 -->
  <gazebo reference="camera_link">
    <sensor type="camera" name="camera_sensor">
      <update_rate>30.0</update_rate>
      <camera>
        <horizontal_fov>1.0472</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.1</near>
          <far>100.0</far>
        </clip>
      </camera>
    </sensor>
  </gazebo>

</robot>

这个URDF文件可以直接被robot_state_publisher解析，发布出完整的tf树：base_link → camera_link → camera_optical_frame。

2.5 知识结构总览

下面这张图把单目相机建模的核心逻辑串起来了：

从这张图可以清楚看到：base_link → camera_joint → camera_link → camera_optical_joint → camera_optical_frame，一条完整的tf链。每个环节都有对应的参数要填，缺一不可。

2.6 常见问题与避坑