3、多传感器标定基础：坐标系定义与标定原理
各位同学，欢迎来到多传感器标定的第一课。
说实话，做仿生眼这么多年，我踩过最大的坑就是标定。传感器买回来，数据能读，但一融合就炸。为什么？坐标系没对齐。你想想看，IMU说“我往东转了30度”，相机说“我看到目标在正前方”，结果一算，完全对不上。嗯，这就是坐标系没统一的问题。
3.1 坐标系定义——先把“世界观”统一了
多传感器融合的第一步，不是调算法，而是定义清楚每个传感器在“说哪门子语言”。我个人习惯把坐标系分成四层：
| 坐标系 | 符号 | 原点 | 用途 |
|---|---|---|---|
| 世界坐标系 | W | 固定点（如起始位置） | 全局定位、地图构建 |
| 机体坐标系 | B | 机器人/仿生眼中心 | 运动控制、姿态描述 |
| 相机坐标系 | C | 相机光心 | 视觉测量、特征点投影 |
| IMU坐标系 | I | IMU芯片中心 | 加速度、角速度测量 |
这里有个关键点：世界坐标系是唯一的，其他坐标系都通过外参变换到世界系下。我在项目中遇到过有人把IMU坐标系直接当世界系用，结果机器人转了个身，所有坐标全乱了。说白了，世界系必须是一个不动的参考系。
核心原则：所有传感器数据最终都要统一到世界坐标系下。IMU测的是相对于自身的运动，相机看到的是相对于光心的图像，不转换就是鸡同鸭讲。
3.2 标定板设计——别小看这块板子
标定板这东西，看着简单，但设计不好能让你标定结果偏到姥姥家。我刚开始做的时候，随便打印了一张棋盘格贴在墙上，结果角点检测总是飘。后来才发现，板子的平整度、反光程度、格子尺寸都有讲究。
常用的标定板有两种：
- 棋盘格（Checkerboard）：角点检测稳定，适合单目/双目标定。建议用9×7或12×9的内角点，格子边长30-50mm。
- 圆点阵列（Circle Grid）：圆心定位精度更高，适合多相机联合标定。但要注意，圆点不能太小，否则边缘检测会丢。
我的经验：打印标定板时，一定要用哑光纸。反光纸在强光下角点会“消失”，我曾经因为这个浪费了一整天采集数据。另外，板子贴在刚性背板上，别用软纸，否则弯曲变形会引入系统误差。
3.3 外参标定原理——手眼标定与联合标定
外参标定，说白了就是求两个坐标系之间的旋转和平移。对于仿生眼来说，主要做两件事：
3.3.1 手眼标定（Eye-in-Hand / Eye-to-Hand）
手眼标定解决的是“相机相对于机械臂末端”的位姿关系。公式很简单：
AX = XB其中A是机械臂末端的运动，B是相机的运动，X就是我们要找的外参。嗯，看着简单，但解起来有点绕。我记得第一次手算这个方程，算了三遍结果都不一样，后来才知道要用李代数优化。
实际工程中，我推荐用Tsai-Lenz方法或者基于非线性优化的方法。采集数据时要注意：
- 至少采集15-20组不同姿态的数据
- 运动幅度要大，旋转和平移都要覆盖
- 避免纯平移或纯旋转，否则方程会退化
避坑指南：我曾经采集了30组数据，结果标定结果还是不对。后来发现，机械臂的位姿读数有延迟，相机采集时没做时间戳同步。记住：手眼标定的前提是数据同步，否则X永远算不准。
3.3.2 联合标定（Camera-IMU Calibration）
相机和IMU的联合标定，是仿生眼最难的一步。为什么？因为IMU是连续测量，相机是离散帧，时间轴对不上。
联合标定的核心思路：
- 时间同步：用硬件触发或软件插值，把IMU数据和图像帧对齐
- 运动激励：让传感器做充分的旋转和平移运动，激发所有自由度
- 联合优化：同时优化相机内参、IMU内参、以及两者之间的外参
常用的工具是Kalibr和MSCKF的标定模块。我个人习惯用Kalibr，因为它支持多种标定板，而且能自动检测时间偏移。
关键公式：联合标定的目标函数通常写成：
min ∑(重投影误差) + ∑(IMU预积分误差)说白了，就是让相机看到的特征点和IMU算出的运动轨迹“对上”。
3.4 知识体系总览
下面这张图是我自己整理的标定知识框架，你可以把它当成一个“导航图”：
这张图把本章内容串起来了。从上到下，先定义坐标系，再设计标定板，最后做外参标定。每一步都依赖上一步的结果，所以千万别跳步。
我的建议：刚开始做标定的时候，别急着上联合标定。先把相机内参标准了，再标手眼，最后才做相机-IMU联合。一步一步来，出了问题也好排查。我曾经图省事一步到位，结果花了三周才找到是IMU内参没标对。
好了，这一章的内容就到这里。标定是个细致活，多练几次就熟了。记住：坐标系是基础，标定板是工具，外参是目标。三者缺一不可。