标定与系统集成

概述

标定（Calibration）是机器人感知和操作精度的基础。系统集成则将传感器、控制器、执行器通过统一的坐标框架和通信架构连接成完整系统。本文涵盖相机标定、手眼标定、多传感器外参标定及 ROS2 下的系统集成。

精度链

最终操作精度 = 相机内参精度 × 手眼标定精度 × 运动学标定精度 × 控制精度。任何一环的误差都会累积到末端。

---

一、相机内参标定

1.1 针孔相机模型

相机将三维世界点 \(P_w = [X, Y, Z]^T\) 投影到二维像素 \(p = [u, v]^T\)：

\[ s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = K [R | t] \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix} \]

其中内参矩阵：

\[ K = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \]

• \(f_x, f_y\)：焦距（像素单位）
• \(c_x, c_y\)：主点坐标
• 畸变参数：\(k_1, k_2, p_1, p_2, k_3\)（径向 + 切向）

1.2 Zhang 标定法

Zhang's method（张正友标定法）使用平面棋盘格，是最广泛使用的相机标定方法：

算法流程：

1. 拍摄多张棋盘格图片（不同角度，建议 15-25 张）
2. 检测角点：亚像素级精度的角点提取
3. 计算单应矩阵：每张图片得到一个单应矩阵 \(H_i\)
4. 求解内参：利用单应矩阵的约束关系
5. 求解外参：利用内参分解每个 \(H_i\)
6. 非线性优化：最小化重投影误差

\[ \min_{K, k_1, k_2, R_i, t_i} \sum_{i=1}^{n} \sum_{j=1}^{m} \| p_{ij} - \hat{p}(K, k_1, k_2, R_i, t_i, P_j) \|^2 \]

标定技巧

• 棋盘格应覆盖图像的各个区域
• 角度变化范围尽量大（倾斜 ±45°）
• 重投影误差应 < 0.5 pixel（好）或 < 0.3 pixel（优秀）
• 标定板平面度要求高（建议铝基板或玻璃板印刷）

1.3 深度相机标定

相机类型	额外标定内容	方法
结构光（RealSense）	深度-RGB 对齐	工厂标定 + 用户微调
ToF（Azure Kinect）	深度畸变校正	多距离平面拟合
双目（ZED）	立体校正	Bouguet 校正算法

---

二、手眼标定（Hand-Eye Calibration）

2.1 问题定义

手眼标定求解相机与机器人末端执行器之间的固定变换关系。

Eye-in-Hand 配置（相机固定在机械臂末端）：

\[ A_i X = X B_i \]

其中：

• \(A_i = T_{end}^{(i+1)^{-1}} T_{end}^{(i)}\)：两次运动之间的末端变换
• \(B_i = T_{cam}^{(i+1)} T_{cam}^{(i)^{-1}}\)：两次运动之间的相机变换
• \(X = T_{end}^{cam}\)：待求的手眼变换

Eye-to-Hand 配置（相机固定在外部）：

\[ A_i X = X B_i \quad \text{（形式相同，物理含义不同）} \]

2.2 Tsai-Lenz 解法

Tsai-Lenz（1989）是经典的两步解法：

第一步：求解旋转

将旋转参数化为轴角表示 \(\text{Rodrigues}(R) = \theta \hat{n}\)，利用：

\[ R_A n_X = n_X \quad \Rightarrow \quad (R_A - I) n_X = 0 \]

修正为使用半角公式，得到线性方程组：

\[ (\text{skew}(n_{A_i} + n_{B_i})) n_X' = n_{B_i} - n_{A_i} \]

第二步：求解平移

\[ (R_{A_i} - I) t_X = R_X t_{B_i} - t_{A_i} \]

2.3 其他手眼标定方法

方法	年份	特点	精度
Tsai-Lenz	1989	分离旋转和平移	中等
Park-Martin	1994	利用李群结构	较高
Daniilidis (双四元数)	1999	同时求解旋转和平移	高
Shah (迭代优化)	2013	非线性优化	最高

实践建议

• 采集 15-25 组不同姿态的数据
• 运动应包含所有6个自由度的变化
• 避免小角度运动（<10°），应有较大角度变化
• 使用多种方法交叉验证

---

三、多传感器外参标定

3.1 LiDAR-Camera 外参标定

求解 LiDAR 坐标系到相机坐标系的变换 \(T_{lidar}^{cam}\)：

基于标靶的方法：

1. 使用带有已知几何特征的标靶（棋盘格、圆形标靶）
2. 在相机和 LiDAR 中分别检测标靶
3. 建立对应关系，最小化配准误差

无标靶方法：

• 基于边缘对齐：将 LiDAR 点云投影到图像，对齐边缘
• 基于互信息（Mutual Information）：最大化两个模态的统计相关性
• 基于学习的方法：CalibNet、LCCNet 等

3.2 IMU-Camera 外参标定

Kalibr 工具是事实标准：

• 使用 AprilGrid 标靶
• 连续运动采集数据（充分激励所有轴）
• 联合优化 IMU 外参、时间偏移、IMU 内参

\[ \min_{T_{imu}^{cam}, t_d, b_a, b_g} \sum_k \left[ \|e_{cam}^k\|^2 + \|e_{imu}^k\|^2 \right] \]

3.3 标定流水线

graph TD
    A[相机内参标定] --> B[手眼标定]
    A --> C[LiDAR-Camera 标定]
    D[IMU 内参标定] --> E[IMU-Camera 标定]
    B --> F[统一坐标系]
    C --> F
    E --> F
    F --> G[tf2 变换树发布]
    G --> H[系统集成验证]
    H -->|精度不足| A
    H -->|通过| I[部署就绪]

---

四、ROS2 下的系统集成

4.1 tf2 变换树

ROS2 使用 tf2 管理所有坐标系之间的变换关系：

world
  └── base_link
       ├── base_footprint
       ├── joint_1
       │    └── link_1
       │         └── joint_2
       │              └── link_2
       │                   └── ... 
       │                        └── end_effector
       │                             └── camera_link
       │                                  ├── camera_color_optical_frame
       │                                  └── camera_depth_optical_frame
       └── lidar_link

4.2 静态变换与动态变换

静态变换（标定结果，不随时间变化）：

# 发布手眼标定结果为静态变换
from tf2_ros import StaticTransformBroadcaster

static_broadcaster = StaticTransformBroadcaster(node)
t = TransformStamped()
t.header.frame_id = 'end_effector'
t.child_frame_id = 'camera_link'
t.transform.translation.x = 0.05  # 标定得到
t.transform.translation.y = 0.0
t.transform.translation.z = 0.03
t.transform.rotation = quaternion_from_euler(0, -pi/2, 0)
static_broadcaster.sendTransform(t)

动态变换（关节状态，实时更新）：

# robot_state_publisher 自动从 URDF + joint_states 计算
# 发布频率通常 100-500 Hz

4.3 URDF/Xacro 模型

<!-- 相机安装描述 -->
<joint name="camera_joint" type="fixed">
  <parent link="end_effector"/>
  <child link="camera_link"/>
  <!-- 手眼标定结果 -->
  <origin xyz="0.05 0.0 0.03" rpy="0 -1.5708 0"/>
</joint>

<link name="camera_link">
  <visual>
    <geometry>
      <mesh filename="package://robot_description/meshes/camera.stl"/>
    </geometry>
  </visual>
</link>

4.4 系统集成架构

graph LR
    subgraph 传感器层
        CAM[RGB-D 相机]
        LID[LiDAR]
        FT[力矩传感器]
        ENC[编码器]
    end

    subgraph 驱动层
        CD[camera_driver]
        LD[lidar_driver]
        RD[robot_driver]
    end

    subgraph 中间件层
        TF[tf2 变换树]
        PC[点云融合]
        ST[状态估计]
    end

    subgraph 应用层
        PER[感知模块]
        PLA[规划模块]
        CTR[控制模块]
    end

    CAM --> CD --> TF
    LID --> LD --> TF
    FT --> RD --> TF
    ENC --> RD
    TF --> PC --> PER
    TF --> ST --> PLA
    PER --> PLA --> CTR --> RD

---

五、标定验证与维护

5.1 验证方法

验证方式	方法	合格标准
重投影误差	标定板角点投影对比	< 0.5 pixel
点云-图像对齐	LiDAR 点投影到图像	边缘偏差 < 3 pixel
抓取精度	已知位置物体抓取	位置误差 < 2mm
手眼一致性	多姿态下标靶定位	位置标准差 < 1mm

5.2 在线标定监控

• 标定漂移检测：定期检查固定标靶的检测偏差
• 自动重标定触发：误差超过阈值时触发重标定
• 温度补偿：相机焦距随温度变化的补偿模型

5.3 常见问题排查

现象	可能原因	解决方案
抓取偏移固定方向	手眼标定平移误差	重新采集数据标定
抓取偏移随位置变化	相机畸变标定不准	增加标定图片数量
点云与图像不对齐	外参标定漂移	检查固定机构，重标定
远处误差大近处准	深度标定问题	多距离深度校正

---

延伸阅读

• 传感器 - 各类传感器原理与选型
• ROS2 系统 - ROS2 框架与通信
• Zhang, Z. "A Flexible New Technique for Camera Calibration." IEEE TPAMI, 2000.
• Tsai, R.Y., Lenz, R.K. "A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration." IEEE TRA, 1989.
• Kalibr: https://github.com/ethz-asl/kalibr

---

评论