3、坐标系与标定：各传感器坐标系定义、内参外参标定方法及统一坐标变换
多传感器融合的前提是让所有传感器“说同一种语言”，而坐标系定义与标定正是建立这种语言的基础工程。摄像头、超声波、IMU和编码器各自拥有独立的测量基准：摄像头输出像素坐标，超声波返回极坐标形式的距离信息，IMU提供载体坐标系下的加速度与角速度，编码器则直接报告轮体旋转量。若缺乏统一的坐标变换框架，即便每个传感器都工作正常，融合结果也会因空间基准不一致而产生系统性偏差。本章围绕坐标系定义、内参外参标定流程以及统一坐标变换展开，帮助读者建立从传感器原始数据到车体统一坐标系的完整技术链路。
上图展示了本章的核心逻辑：四种传感器各自独立采集数据，经过内参标定消除自身测量误差，再通过外参标定获得传感器坐标系到车体坐标系的旋转与平移关系，最终将所有数据投影到统一的车体坐标系下进行融合。下面逐一展开坐标系定义、标定方法和变换实现。
3.1 各传感器坐标系定义
坐标系定义是标定工作的起点，不同传感器遵循不同的坐标约定，需要明确统一到同一参考框架下。
- 摄像头坐标系（O_c）：以相机光心为原点，Z轴沿光轴向前，X轴向右，Y轴向下（遵循右手定则）。像素坐标系(u,v)以图像左上角为原点，u向右，v向下。内参矩阵K将三维点投影到像素平面：
[fx 0 cx]
K = [ 0 fy cy]
[ 0 0 1]其中fx、fy为归一化焦距，cx、cy为主点坐标。畸变模型通常采用径向畸变(k1,k2,k3)和切向畸变(p1,p2)描述。
- 超声波坐标系（O_s）：以探头中心为原点，Z轴指向探测方向，X-Y平面垂直于声束轴线。超声波返回的距离值r对应目标点在传感器坐标系下的Z坐标，而水平开角θ和垂直开角φ则定义了波束覆盖范围。实际应用中，通常将超声波简化为扇形或锥形区域，目标位置表示为(r, θ, φ)的球坐标形式。
- IMU坐标系（O_i）：以IMU芯片封装中心为原点，三轴方向与芯片标注一致。加速度计测量比力，陀螺仪测量角速度，均以IMU自身坐标系为参考。IMU的安装位置通常不在车体几何中心，因此需要外参标定确定其相对于车体坐标系的位姿。
- 编码器坐标系（O_e）：编码器直接安装在轮轴上，其测量值为轮轴旋转角度或角速度。通过轮径模型可转换为轮心处的线速度。编码器坐标系通常以轮轴中心为原点，Z轴沿轮轴方向，X轴指向车辆前进方向。
3.2 内参标定方法
内参标定解决的是传感器自身的测量精度问题，不同传感器的内参标定方法差异较大。
3.2.1 摄像头内参标定
采用张正友标定法，使用棋盘格或圆点标定板，采集10-20张不同姿态的图像，通过角点检测和单应性矩阵求解内参矩阵和畸变系数。操作流程如下：
- 打印高精度棋盘格标定板（如10×7格，格边长30mm），粘贴在平面硬板上。
- 固定摄像头，移动标定板使其出现在视野不同位置和角度，采集15张以上图像。
- 使用OpenCV的
findChessboardCorners检测角点，亚像素精细化。 - 调用
calibrateCamera求解内参矩阵K和畸变系数。 - 验证重投影误差，通常要求小于0.5像素。
3.2.2 超声波内参标定
超声波内参主要指探头的安装角度和波束开角。标定时将超声波对准已知距离的平面障碍物，记录不同距离下的回波强度，拟合出波束轮廓，确定实际开角。安装角度可通过测量已知位置目标的回波方向来反推。
3.2.3 IMU内参标定
IMU内参包括加速度计零偏、比例因子、安装误差角，以及陀螺仪零偏和比例因子。标定方法：
- 六位置法：将IMU分别以六个面朝上静止放置，采集加速度计输出，求解零偏和比例因子。
- 转台法：使用高精度转台提供已知角速度，标定陀螺仪比例因子和安装误差。
- 艾伦方差法：长时间静止采集数据，分析噪声特性，分离量化噪声、角度随机游走和零偏不稳定性。
3.2.4 编码器内参标定
编码器内参主要是轮径有效值和齿数比。标定方法：让车辆沿直线行驶已知距离L，记录编码器脉冲数N，则轮径d = L / (π × N / 每转脉冲数)。为提高精度，可多次往返取平均。
| 传感器 | 内参内容 | 标定方法 | 精度要求 |
|---|---|---|---|
| 摄像头 | 焦距、主点、畸变 | 张正友标定法 | 重投影误差<0.5px |
| 超声波 | 安装角度、波束开角 | 已知距离目标回波拟合 | 角度误差<1° |
| IMU | 零偏、比例因子、安装误差 | 六位置法+转台法 | 零偏稳定性<10°/h |
| 编码器 | 轮径、齿数比 | 已知距离行驶标定 | 轮径误差<0.5% |
3.3 外参标定方法
外参标定确定各传感器坐标系相对于车体坐标系的旋转矩阵R和平移向量t。这是多传感器融合中最关键也最容易出错的环节。
3.3.1 摄像头-车体外参标定
常用方法包括：
- 手动测量法：使用卷尺和角度尺直接测量相机安装位置和姿态，精度较低但简单快速。
- 标定板法：在车辆前方放置已知尺寸的标定板，同时获取标定板在车体坐标系下的位置（通过测量）和相机图像中的位置，利用PnP算法求解外参。
- 手眼标定法：借鉴机器人领域的AX=XB方法，通过车辆移动和相机观测固定特征点，迭代求解外参。
3.3.2 IMU-车体外参标定
IMU外参标定通常利用车辆运动约束：车辆在平面上行驶时，IMU测量的加速度在车体坐标系下的投影应满足特定关系。具体步骤：
- 车辆静止，记录IMU加速度输出，确定重力方向在IMU坐标系下的投影，从而求解俯仰角和横滚角。
- 车辆直线加速行驶，利用加速度计输出和编码器速度变化，求解航向角。
- 通过多组不同运动状态的数据，使用最小二乘法优化R和t。
3.3.3 超声波-车体外参标定
超声波外参标定相对简单：将超声波对准已知距离的平面墙，测量实际距离与传感器读数之差，即可确定安装位置偏移。安装角度可通过测量不同角度下的回波强度峰值来确定。
3.3.4 编码器-车体外参标定
编码器通常直接安装在轮轴上，其坐标系与车体坐标系的关系由车辆几何结构决定。若编码器安装在非转向轮上，其坐标系与车体坐标系仅存在平移关系，旋转矩阵为单位阵。若安装在转向轮上，则需要考虑转向角的影响。
3.4 统一坐标变换实现
完成内外参标定后，需要实现从各传感器原始数据到车体坐标系的统一变换。变换流程如下：
3.4.1 摄像头数据变换
像素坐标(u,v)到车体坐标(X_v, Y_v, Z_v)的变换分三步：
1. 像素坐标 → 归一化相机坐标：
x_c = (u - cx) / fx
y_c = (v - cy) / fy
2. 去畸变：
r² = x_c² + y_c²
x_d = x_c * (1 + k1*r² + k2*r⁴ + k3*r⁶) + 2*p1*x_c*y_c + p2*(r² + 2*x_c²)
y_d = y_c * (1 + k1*r² + k2*r⁴ + k3*r⁶) + p1*(r² + 2*y_c²) + 2*p2*x_c*y_c
3. 相机坐标 → 车体坐标：
[X_v] [R_cv | t_cv] [x_d]
[Y_v] = [ | ] * [y_d]
[Z_v] [ | ] [ 1 ]其中R_cv和t_cv为摄像头到车体的外参。由于单目相机无法直接获得深度Z_c，通常需要结合地面平面假设或与其他传感器融合获取深度信息。
3.4.2 超声波数据变换
超声波返回的距离r和角度(θ, φ)在传感器坐标系下表示为：
X_s = r * sin(θ) * cos(φ)
Y_s = r * sin(φ)
Z_s = r * cos(θ) * cos(φ)然后通过外参变换到车体坐标系。需要注意的是，超声波测距存在波束宽度导致的角分辨率限制，通常将目标建模为具有一定宽度的区域而非点。
3.4.3 IMU数据变换
IMU输出的加速度和角速度在IMU坐标系下，需要变换到车体坐标系：
a_v = R_iv * a_i + ω × (ω × r_iv) + α × r_iv其中R_iv为IMU到车体的旋转矩阵，r_iv为IMU相对于车体坐标系原点的位置向量，ω为角速度，α为角加速度。当车辆运动速度较低时，后两项可忽略，但高速或急转弯时必须考虑。
3.4.4 编码器数据变换
编码器输出轮速ω_wheel，通过轮径r_wheel计算轮心线速度v_wheel = ω_wheel × r_wheel。对于非转向轮，轮心速度直接投影到车体坐标系X轴方向。对于差速驱动车辆，左右轮速可计算车体速度和横摆角速度：
v = (v_left + v_right) / 2
ω = (v_right - v_left) / L_wheelbase3.5 实战案例：园区无人车传感器标定与坐标变换
某园区物流无人车配备前视摄像头、4个超声波雷达（前保险杠2个、后保险杠2个）、IMU和双轮编码器，需要完成全套标定并验证融合效果。
背景与约束：车辆轴距1.2m，轮距0.8m，摄像头安装于前挡风玻璃顶部中心，距地面1.4m，前向安装。超声波探头安装于前后保险杠，距地面0.5m。IMU安装于车辆几何中心。要求障碍物测距精度优于±10cm。
标定过程：
- 摄像头内参标定：使用12×9棋盘格（格边长25mm），采集20张图像，重投影误差0.32像素，满足要求。
- 摄像头外参标定：在车辆前方5m处放置1.5m×1.5m标定板，标定板中心距地面1.0m。通过测量标定板在车体坐标系下的位置（X=5m, Y=0, Z=1.0m），结合图像中角点位置，使用PnP求解外参。得到旋转角：俯仰-2.3°，横滚0.1°，航向0.5°。
- IMU外参标定：车辆静止时记录加速度计输出，计算俯仰角0.8°、横滚角-0.3°。车辆直线加速至2m/s，利用加速度计和编码器速度差求解航向角0.2°。
- 超声波外参标定：将车辆停放在距墙面2m处，调整车辆朝向使超声波垂直对准墙面，记录距离读数2.03m，确定安装偏移3cm。
- 编码器标定：车辆直线行驶10m，记录编码器脉冲数，计算轮径0.198m（标称值0.2m），误差1%。
验证结果：在车辆前方3m、5m、8m处放置障碍物，分别用摄像头和超声波测量距离，融合后测距误差分别为4cm、7cm、9cm，满足±10cm要求。摄像头与超声波在5m处的测量差值为12cm，经坐标变换对齐后差值缩小至3cm。
3.6 常见误区与总结
坐标系与标定环节的常见误区包括：
- 忽略IMU杆臂效应：IMU安装位置与车体中心不重合时，角运动会产生附加线加速度，必须补偿。
- 超声波波束建模过于简化：将超声波视为理想射线会导致边缘目标测距偏差，应使用扇形模型。
- 摄像头畸变校正不彻底：仅校正径向畸变而忽略切向畸变，在图像边缘区域仍存在较大误差。
- 外参标定后未验证：标定结果必须通过独立测量验证，不能仅依赖重投影误差。
- 坐标系约定不一致：不同传感器可能采用不同的坐标轴方向约定，统一变换前必须仔细核对。
总结而言，坐标系定义是融合系统的“语法规则”，内参标定保证每个传感器“说准话”，外参标定让所有传感器“说同一句话”。只有完成这三步，后续的传感器融合算法才能建立在可靠的数据基础之上。建议在实际项目中建立标定记录表，记录每次标定的时间、环境条件、标定参数和验证结果，便于后续排查问题。