第二节：传感器模型与标定——让每个传感器都“说真话”

各位同学，欢迎来到实战课程的第二节。上一节我们聊了坐标变换的数学基础，说白了就是给传感器们搭了个“共同语言”的框架。但光有框架不够，你得先搞清楚每个传感器是怎么“看世界”的，它说的数据到底准不准。这就是我们今天要啃的硬骨头——传感器模型与标定。

我个人习惯把传感器标定比作“给设备配眼镜”。你想想看，一个相机如果内参不准，拍出来的画面就像近视眼没戴眼镜，全是虚的。LiDAR 如果没标好，点云数据可能整体歪了 5 厘米，那融合出来的结果就全完了。IMU 更别提，零偏没校准，积分 10 秒位置就漂到天边去了。所以，这一节的内容，是后续所有融合工作的基石。

    核心观点： 没有准确的传感器模型，再高级的融合算法都是空中楼阁。

2.1 相机内参模型：针孔与畸变

相机模型，最经典的就是针孔模型。它假设光线通过一个小孔，在成像平面上投影。公式很简单，就是相似三角形。但实际中，镜头不是完美的小孔，它会有畸变。

针孔模型的核心参数：

焦距 (fx, fy)： 单位是像素。注意，不是物理焦距，是物理焦距除以像元尺寸后的值。我遇到过新手直接拿镜头上的 4mm 焦距往里填，结果投影全错。
主点 (cx, cy)： 光轴与成像平面的交点，理想情况下在图像中心，但实际会有偏移。
畸变参数 (k1, k2, p1, p2, k3)： 径向畸变（桶形/枕形）和切向畸变（镜头与成像平面不平行）。

嗯，这里要注意，畸变模型有很多种，比如 OpenCV 用的就是 Brown-Conrady 模型。我个人建议，如果你做高精度视觉 SLAM，一定要标定到 k3，甚至考虑使用鱼眼模型或 Kannala-Brandt 模型，因为普通针孔模型在广角镜头下边缘畸变很大，矫正后图像分辨率损失严重。

避坑指南： 我曾经在标定一个 120 度广角相机时，用了标准的针孔+畸变模型，结果边缘区域的重投影误差死活降不下去。后来换成鱼眼模型，误差直接降了一个数量级。所以，选模型要根据镜头视场角来。

标定流程简述：

打印一张棋盘格或圆点标定板。
从不同角度拍摄 20-30 张图片。
使用 OpenCV 的 calibrateCamera() 函数求解。

// C++ 示例：使用 OpenCV 进行相机标定
#include <opencv2/opencv.hpp>
#include <vector>

int main() {
    std::vector<std::vector<cv::Point3f>> objectPoints;
    std::vector<std::vector<cv::Point2f>> imagePoints;
    cv::Size imageSize(1920, 1080);
    cv::Mat cameraMatrix, distCoeffs;
    std::vector<cv::Mat> rvecs, tvecs;

    // 假设你已经提取了所有角点并填充了 objectPoints 和 imagePoints
    double rms = cv::calibrateCamera(objectPoints, imagePoints, imageSize,
                                     cameraMatrix, distCoeffs, rvecs, tvecs);
    std::cout << "重投影误差: " << rms << std::endl;
    std::cout << "内参矩阵: \n" << cameraMatrix << std::endl;
    return 0;
}

2.2 LiDAR 模型：从激光束到点云

LiDAR 模型相对直观。每个激光发射器发出一束光，测量飞行时间，得到距离 d。再结合当前激光头的水平旋转角 α 和垂直俯仰角 β，就能算出点的三维坐标。

LiDAR 内参主要包括：

旋转轴偏移： 激光发射器中心与旋转轴不重合，导致点云出现“同心圆”误差。
俯仰角偏差： 每个激光线的实际俯仰角与设计值有微小差异。
距离偏移： 系统性的测距误差，比如温度引起的。

说白了，LiDAR 标定就是找出这些偏差，然后补偿回去。我做过一个项目，用的 16 线激光雷达，标定前点云地面是弧形的，标定后地面就平了。你看，这就是模型的力量。

注意： LiDAR 标定通常需要特定的靶标（如平面板、V 形块），或者利用自然场景中的特征（如墙角、杆子）。不要试图用手持 LiDAR 在室内随便晃晃就能标定好，那是不现实的。

2.3 IMU 模型：加速度计与陀螺仪

IMU 模型是所有传感器里最“脏”的。它直接测量角速度和加速度，但噪声大、零偏不稳定、还有尺度因子和交轴耦合。

IMU 误差模型（简化版）：

测量值 = 真实值 + 零偏 + 尺度因子误差 + 交轴耦合 + 高斯白噪声

其中，零偏是最大的敌人。它随时间缓慢变化，称为“零偏不稳定性”。我习惯用 Allan 方差来分析 IMU 的噪声特性，它能帮你把白噪声、零偏不稳定性、随机游走等分量都分离出来。

标定方法：

六位置法： 将 IMU 分别朝上、朝下、朝北等六个方向静止放置，利用重力向量标定加速度计的零偏和尺度。
转台标定： 用高精度转台提供已知角速度，标定陀螺仪。
温补标定： 在不同温度下记录零偏，建立温度模型。这个在工业级应用中必不可少。

个人经验： 我曾经在无人机上用过一款消费级 IMU，常温下标定得很好，但飞到 500 米高空温度降到 0 度，零偏直接漂了 3 倍。从那以后，我所有项目都要求做温补标定，哪怕只是简单的线性插值。

2.4 手眼标定原理：让相机和机械臂“握手”

手眼标定，解决的是“相机坐标系”与“机械臂末端坐标系”之间的变换关系。说白了，就是让机械臂知道它抓的东西在相机里看到的坐标，对应到自己的坐标系里是什么位置。

有两种经典配置：

Eye-in-Hand： 相机固定在机械臂末端，随机械臂移动。求解的是相机与末端的变换 (X)。
Eye-to-Hand： 相机固定在世界坐标系中，看机械臂运动。求解的是相机与机械臂基座的变换。

核心方程：

对于 Eye-in-Hand，我们有：

A * X = X * B

其中，A 是机械臂末端两次运动之间的变换（从运动学得到），B 是相机两次观测到的标定板之间的变换（从视觉得到）。X 就是我们要求的未知变换。

这个方程看起来简单，但求解起来有讲究。常用的方法有 Tsai 两步法、Park-Martin 法等。我建议你用 cv::calibrateHandEye() 函数，它封装了多种解法。

    关键点： 手眼标定的精度，很大程度上取决于机械臂的运动精度和视觉观测的精度。如果机械臂本身有回差，或者相机标定板角点提取不准，那标定结果就会很差。

标定步骤：

固定标定板，让机械臂带着相机移动 15-20 个位姿。
记录每个位姿下机械臂的末端位姿（从控制器读取）。
同时记录相机拍摄的标定板图像，解算出相机相对于标定板的位姿。
构建 A 和 B 矩阵，求解 X。

// C++ 示例：OpenCV 手眼标定
cv::Mat R_cam2gripper, t_cam2gripper;
cv::calibrateHandEye(R_gripper2base, t_gripper2base,
                     R_target2cam, t_target2cam,
                     R_cam2gripper, t_cam2gripper,
                     cv::CALIB_HAND_EYE_TSAI);

知识体系总览

为了让你更直观地理解这一节的知识结构，我画了一张图。它把相机、LiDAR、IMU 的模型和标定，以及手眼标定串联了起来。你可以看到，所有标定的最终目的，都是为了得到准确的传感器内参和外参，为后续的跨传感器对齐做准备。

好了，这一节的内容就到这里。传感器模型和标定是枯燥但极其重要的一步。你可能会觉得公式多、流程繁琐，但相信我，在真实项目中，花 70% 的时间在标定和数据预处理上，是常有的事。把这一步做扎实了，后面的融合工作会顺畅很多。

    一句话总结： 标定不是一次性的工作，而是需要定期维护和校验的。环境温度变化、机械振动、时间老化，都会让标定参数失效。

课程目录

第二节：传感器模型与标定——让每个传感器都“说真话”

2.1 相机内参模型：针孔与畸变

2.2 LiDAR 模型：从激光束到点云

2.3 IMU 模型：加速度计与陀螺仪

2.4 手眼标定原理：让相机和机械臂“握手”

知识体系总览