自动驾驶控制器，自动驾驶多传感器联合标定系列之毫米波雷达到车体坐标系的标定工程 ， 本商品对毫米波雷达的偏航角yaw进行标定,分为粗略标定、静态目标识别和曲线拟合三个步骤。 这个工程带有代码注释，帮助您对标定算法的的理解和学习。 实实在在的工作经验总结
毫米波雷达的yaw角标定是个挺有意思的活，咱们搞自动驾驶的同行应该都懂，这玩意儿要是没标准，后边融合感知的数据就跟喝醉似的到处飘。今天就跟大伙聊聊我们项目中实际应用的三步标定法，手把手带你走完整个流程。
先看第一步的粗略标定。这个阶段咱们要解决的是毫米波雷达坐标系与车体坐标系之间的初始角度偏差。举个栗子，假设雷达安装时歪了5度，这时候不校正的话，目标物的位置估计直接偏差两米开外（按50米探测距离算）。核心代码片段长这样：
radar_points = [[0.5, -0.3], [0.5, 0.3], [-0.5, -0.3], [-0.5, 0.3]] # 雷达坐标系坐标 car_points = [[1.2, -0.5], [1.2, 0.5], [0.2, -0.5], [0.2, 0.5]] # 车体坐标系坐标 # 计算各点对的相对角度 angles = [] for rp, cp in zip(radar_points, car_points): dx = cp[0] - rp[0] dy = cp[1] - rp[1] angles.append(np.arctan2(dy, dx)) # 反正切计算初始偏角 initial_yaw = np.median(angles) # 取中位数抗异常值 print(f"初始标定角度：{np.degrees(initial_yaw):.2f}度")这里有几个实战经验：1）选点要避开曲面部位防止形变误差 2）取中位数比平均值更抗造 3）实际操作时得用激光测距仪复核物理尺寸。记得某次施工队把安装支架装反了，这套代码直接揪出15度的离谱偏差。
自动驾驶控制器，自动驾驶多传感器联合标定系列之毫米波雷达到车体坐标系的标定工程 ， 本商品对毫米波雷达的偏航角yaw进行标定,分为粗略标定、静态目标识别和曲线拟合三个步骤。 这个工程带有代码注释，帮助您对标定算法的的理解和学习。 实实在在的工作经验总结
接下来是静态目标识别环节，重点是从雷达点云中筛出真正的静止物体。这里有个坑——很多动态目标在特定时刻也会呈现静止特征。我们的解决方案是引入速度一致性检验：
// 雷达点云处理片段 for (auto& point : pointCloud) { // 基于多普勒速度的初步筛选 if (abs(point.doppler) > 0.3) { // 速度阈值根据雷达精度调整 point.isStatic = false; continue; } // 位置变化率二次验证 Eigen::Vector3d pos = point.getPosition(); if (historyPositions.count(point.id)) { double dist = (pos - historyPositions[point.id]).norm(); if (dist > positionChangeThreshold) { point.isStatic = false; } } historyPositions[point.id] = pos; // 更新历史位置 }实测中发现，单纯依赖多普勒速度会漏掉垂直于雷达径向运动的物体。所以我们加了位置变化率判断，相当于用两帧以上的位置变化做双重验证。曾经在测试场里有个横移的机器人小车，就靠这个方法准确识别出来了。
最后的重头戏是曲线拟合优化，这里采用最小二乘法+RANSAC的方案。重点看损失函数的设计：
def yaw_calibration_loss(params, radar_points, car_points): yaw_offset, = params rotation_matrix = np.array([[np.cos(yaw_offset), -np.sin(yaw_offset)], [np.sin(yaw_offset), np.cos(yaw_offset)]]) transformed = radar_points @ rotation_matrix.T errors = np.linalg.norm(transformed - car_points, axis=1) return np.sum(errors**2) # RANSAC迭代优化 best_yaw = initial_yaw best_error = float('inf') for _ in range(100): sample_indices = np.random.choice(len(points), 3, replace=False) result = least_squares(yaw_calibration_loss, [best_yaw], args=(radar_points[sample_indices], car_points[sample_indices])) if result.cost < best_error: best_yaw = result.x[0] best_error = result.cost print(f"优化后偏航角：{np.degrees(best_yaw):.2f}±{np.degrees(result.jac[0][0]):.2f}度")这里有个骚操作：用随机采样代替全量计算，实测效率提升40%以上。特别注意雅可比矩阵的最后一列给出了角度估计的不确定度，这个值如果超过0.5度就得回炉重造了。
整套流程跑下来，我们在测试场上实现了±0.3度的标定精度。不过要提醒各位，不同车型的悬挂形变对结果影响很大，特别是SUV在负重状态下的偏差能到1度以上。所以千万别以为标完就一劳永逸了，定期复检才是王道。
