自动驾驶控制器自动驾驶多传感器联合标定系列之毫米波雷达到车体坐标系的标定工程 本商品对毫米波雷达的偏航角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, axis1) 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, replaceFalse) 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度以上。所以千万别以为标完就一劳永逸了定期复检才是王道。