自动驾驶多传感器联合标定系列之IMU到车体坐标系的标定工程 ， 本商品在已知GNSS/GPS到...

自动驾驶多传感器联合标定系列之IMU到车体坐标系的标定工程 本商品在已知GNSS/GPS到车体坐标系的外参前提下根据GNSS/GPS的定位信息与IMU信息完成IMU到GNSS/GPS 的外参标定并进一步获得IMU到车体坐标系的外参标定 本商品提供两种标定模式车辆直线运动及自由运动这两种模式下的注释工程一、工程概述Imu2Car 是一款面向自动驾驶场景的多传感器联合标定工具核心功能是实现 IMU惯性测量单元到车体坐标系的精准标定通过计算 IMU 与车体坐标系之间的滚转角Roll、俯仰角Pitch、偏航角Yaw偏移量消除传感器安装误差带来的测量偏差为自动驾驶系统提供可靠的姿态与位置数据支撑。工程支持两种标定模式分别适配不同的场景需求直线行驶标定method_id1适用于车辆能稳定直线行驶的场景仅标定偏航角Yaw偏移自由运动标定method_id2适用于复杂行驶场景可同时标定滚转角、俯仰角、偏航角三个维度的偏移。标定过程以 NovAtel 导航系统输出的离线 CSV 数据作为真值参考通过 RANSAC 鲁棒拟合、最小二乘法等算法完成误差计算最终输出标定结果文件支持后续自动驾驶系统的传感器数据融合与姿态解算。二、核心模块与类结构2.1 核心数据结构2.1.1 HeadingResult用于存储标定结果包含角度偏移量的度和弧度两种单位表示方便不同场景下的使用。struct HeadingResult { Eigen::Vector3d RPY_offset_degree; // 滚转、俯仰、偏航偏移度 Eigen::Vector3d RPY_offset_rad; // 滚转、俯仰、偏航偏移弧度 };2.1.2 LineClusterParam直线聚类参数结构体为自由运动标定模式提供聚类阈值配置支持针对不同角度Yaw/Pitch/Roll单独设置参数适配各角度的标定特性。参数名含义默认值Yaw参数Pitch参数Roll参数maxdegreegap角度间隙阈值0.1°0.5°0.08°0.5°maxdegreerange角度范围阈值0.2°1°0.1°1°minlinepoint_num最小直线点数600800200500minmovingsquare最小移动距离平方0.02m/10ms0.02m/10ms0.015m/10ms0.015m/10ms2.1.3 NovAtelEnuDatas导航数据存储类用于加载和管理 NovAtel 输出的 CSV 数据包含时间戳、位置x/y/z、速度ve/vn/vu东北天坐标系、姿态roll/pitch/yaw等关键数据。2.2 核心业务类2.2.1 ImuHeading 类标定核心类封装了两种标定模式的完整流程包括数据加载、鲁棒拟合、误差计算、结果输出等功能是工程的核心业务载体。构造函数ImuHeading(const std::string input_csv_path, const std::string output_dir);输入参数inputcsvpath为 NovAtel 离线数据 CSV 文件路径output_dir为标定结果输出目录。功能初始化输入输出路径创建标定实例。核心成员函数函数名功能描述关键参数返回值Calibrate标定入口函数根据 method_id 选择标定模式method_id1直线/2自由运动bool标定成功返回trueStraightHeading直线行驶标定实现仅标定Yaw角-bool标定成功返回trueFreeHeading自由运动标定实现标定Roll/Pitch/Yaw角-bool标定成功返回trueStraightCalibrateSingle直线标定单角度核心逻辑x/y位置数据gnssheadingvecGNSS姿态数据threshold点线距离阈值输出caliboffset角度偏移、inlineridx内点索引FreeCalibrateSingle自由运动标定单角度核心逻辑同上 LineClusterParam聚类参数输出caliboffsetmean平均角度偏移LeastSquare最小二乘法拟合直线x/y位置数据inliner_idx内点索引输出a/b/c直线方程参数 axbyc0LineCluster直线聚类从离散点中提取有效直线段param聚类参数输出lines_idx各直线段的点索引集合SpeedEvaluate速度验证评估标定结果准确性inlineridx内点索引yawoffsetYaw角偏移输出v_componet速度分量评估结果WriteToCsv标定结果写入CSV文件-bool写入成功返回true2.2.2 RansacFitLine 类鲁棒直线拟合类基于 RANSAC随机抽样一致性算法从含噪声的离散点中筛选内点并拟合直线有效排除异常值干扰提升标定精度。核心函数EstimateRANSAC 算法核心实现随机采样生成直线模型筛选最优模型Evaluate模型评估计算点到直线的距离筛选内点并计算模型得分GetLineSlope根据两点计算直线方程参数axbyc0LeastSquare最小二乘法拟合直线用于内点的精准拟合。2.3 工具类与辅助模块2.3.1 GPS_time 模块提供 GPS 时间与 UTC 时间的转换功能支持时间字符串与时间戳微秒级的互转确保数据时间同步的准确性。核心类GPSTime可通过 GPS 周数和周内秒数计算 UTC 时间精确到毫秒/微秒。2.3.2 common.h 工具头文件包含通用工具函数与宏定义如日志宏定义LOGINFO/LOGWARN/LOGERROR输出带颜色的日志信息包含文件、行号、时间时间工具类TicToc用于代码执行耗时统计日期工具函数isleapyear判断闰年路径工具函数GotProjectDir获取工程根目录。三、标定核心流程3.1 直线行驶标定流程method_id1数据加载通过LoadEnuCSVDatas函数加载 NovAtel 离线 CSV 数据存储到NovAtelEnuDatas实例中RANSAC 鲁棒拟合调用RansacFitLine::Estimate对 x/y 位置数据进行直线拟合筛选内点符合直线趋势的有效数据姿态均值计算通过GetMeasuredMean计算内点对应的 GNSS 偏航角Yaw均值作为 IMU 测量参考值真值计算通过LeastSquare对筛选后的内点进行精准直线拟合基于直线方程计算真实偏航角GT 值误差计算真实偏航角与 GNSS 测量均值的差值即为 Yaw 角标定偏移量同时将角度单位转换为弧度结果验证调用SpeedEvaluate函数基于东北天坐标系的速度分量ve/vn验证标定结果的合理性结果输出通过WriteToCsv将标定结果写入 CSV 文件通过OutputCalibResult输出四元数表示的旋转矩阵。3.2 自由运动标定流程method_id2自由运动标定流程在直线标定基础上增加了直线聚类步骤支持 Roll/Pitch/Yaw 三个角度的独立标定核心流程如下数据加载与直线标定一致加载 NovAtel 离线数据直线聚类调用LineCluster函数根据LineClusterParam参数对离散位置点进行聚类提取多个有效直线段满足最小点数、最小移动距离等条件分段标定对每个有效直线段重复直线标定的 RANSAC 拟合、内点筛选、误差计算流程得到该线段的角度偏移量均值融合对所有有效直线段的角度偏移量进行加权平均权重为线段点数得到最终的角度标定偏移量多角度标定分别对 Rollx/z 位置数据、Pitchy/z 位置数据、Yawx/y 位置数据执行上述流程完成三个角度的标定结果输出将三个角度的标定结果度和弧度写入 CSV 文件。四、关键算法原理4.1 RANSAC 鲁棒拟合算法RANSAC 算法是一种鲁棒的参数估计算法用于从含噪声和异常值的数据中拟合模型核心步骤如下随机采样从数据集中随机选择 2 个点直线拟合最小样本数生成直线模型axbyc0内点筛选计算所有数据点到该直线的距离距离小于阈值的点视为内点模型评估计算内点占比作为模型得分迭代优化重复上述步骤至最大迭代次数选择得分最高的模型作为最优直线拟合结果。该算法能有效排除道路颠簸、传感器噪声等导致的异常数据提升标定的鲁棒性。4.2 最小二乘法直线拟合对 RANSAC 筛选出的内点进行精准拟合最小化点到直线的距离平方和求解直线方程参数a/b/c。核心公式如下自动驾驶多传感器联合标定系列之IMU到车体坐标系的标定工程 本商品在已知GNSS/GPS到车体坐标系的外参前提下根据GNSS/GPS的定位信息与IMU信息完成IMU到GNSS/GPS 的外参标定并进一步获得IMU到车体坐标系的外参标定 本商品提供两种标定模式车辆直线运动及自由运动这两种模式下的注释工程给定内点集合 \((xi, yi)\)i1,2,...,n目标是找到直线 \(ax by c 0\)使得 \(\sum{i1}^n (axi by_i c)^2\) 最小。通过求解线性方程组得到参数 a、b、c\[a -\frac{n\sum xiyi - \sum xi \sum yi}{n\sum xi^2 - (\sum xi)^2}, \quad b1, \quad c -\frac{\sum xi^2 \sum yi - \sum xi \sum xiyi}{n\sum xi^2 - (\sum x_i)^2}\]4.3 角度偏移计算标定的核心是计算 IMU 测量角度与真实角度的偏差步骤如下真实角度GT基于拟合直线的斜率计算公式为 \(gt\_degree \arctan(-a/b) \times \frac{180}{\pi}\)a、b 为直线方程参数测量角度GNSS对 RANSAC 内点对应的 GNSS 姿态数据取均值转换为角度单位角度修正根据测量角度的范围对真实角度进行修正如 GNSS 角度在 90°~270° 时GT 角度加 180°偏移计算\(calib\offset gt\degree - gnss\_degree\)即得到 IMU 相对于车体坐标系的角度偏移量。五、工程编译与运行5.1 环境依赖操作系统Ubuntu推荐 18.04/20.04编译工具CMake 3.10、GCC/G 7.0第三方库Boost、Eigen 3.3.7、PCL点云库。5.2 编译步骤安装依赖工具与库# 基础工具 sudo apt-get install vim cmake git gcc g # Boost 库 sudo apt install libboost-all-dev # Eigen 3.3.7源码安装 tar -zxvf eigen-git-mirror.tar.gz cd eigen-git-mirror mkdir build cd build cmake .. sudo make install sudo cp -r /usr/local/include/eigen3/Eigen /usr/local/include # PCL 库参考官方文档或博客安装编译工程cd Imu2Car mkdir build cd build cmake .. make -j 4 # 多线程编译根据CPU核心数调整编译完成后在bin目录下生成可执行文件runimuheading。5.3 运行说明5.3.1 运行命令格式./bin/run_imu_heading [method_id] [data_dir]method_id标定模式1 为直线行驶标定2 为自由运动标定data_dir离线数据目录需包含 NovAtel 输出的 CSV 文件命名为novatel-utm.csv。5.3.2 示例命令# 直线行驶标定 ./bin/run_imu_heading 1 ./data # 自由运动标定 ./bin/run_imu_heading 2 ./data5.3.3 数据要求CSV 文件格式包含gps_time、x、y、z、ve、vn、vu、roll、pitch、yaw 共 10 列数据直线标定数据车辆需稳定直线行驶至少 300 米确保有足够的直线数据用于拟合自由运动标定数据车辆可自由行驶包含直线、转向等动作需保证数据中包含多个有效直线段。5.3.4 输出结果运行成功后在data/calibration目录下生成imu_heading.csv文件包含以下字段roll_offset_degreepitch_offset_degreeyaw_offset_degreeroll_offset_radpitch_offset_radyaw_offset_rad滚转偏移度俯仰偏移度偏航偏移度滚转偏移弧度俯仰偏移弧度偏航偏移弧度六、结果验证与精度保障6.1 速度验证机制直线标定模式中SpeedEvaluate函数通过对比标定前后的速度分量一致性来验证结果准确性基于 IMU 测量角度和标定后角度分别计算东北天坐标系下的速度分量统计标定前后速度分量的均值、最大值、最小值理想情况下标定后的速度分量应更稳定波动范围更小均值更接近真实速度方向。6.2 参数调优建议直线标定阈值threshold默认 0.15m可根据道路平整度调整颠簸路面可适当增大自由运动聚类参数通过LineClusterParam的YawParam/PitchParam/RollParam函数单独配置确保各角度的有效直线段提取RANSAC 迭代次数默认 15 次直线标定/20 次自由运动标定数据噪声较大时可增加迭代次数。七、工程扩展与应用场景7.1 工程扩展方向实时标定支持当前工程基于离线数据可扩展为实时读取传感器数据GNSSIMU实现在线标定多传感器融合增加激光雷达、相机等传感器数据实现多源数据联合标定提升标定精度动态阈值调整基于数据质量自动调整 RANSAC 阈值、聚类参数等适配不同场景可视化功能增加标定过程可视化如直线拟合结果、内点筛选效果便于问题排查。7.2 应用场景自动驾驶车辆传感器安装后标定消除 IMU 安装偏差确保姿态测量准确性传感器维护后的重新标定车辆维修、传感器拆卸后快速恢复标定参数多传感器融合系统初始化为 IMU 与 GNSS、激光雷达等传感器的融合提供准确的外参。八、总结Imu2Car 工程通过模块化设计实现了 IMU 到车体坐标系的高精度标定支持两种标定模式适配不同的自动驾驶场景。核心算法采用 RANSAC 鲁棒拟合与最小二乘法结合有效抵抗噪声和异常值干扰保障标定精度工程提供完整的编译、运行流程输出结果直观易懂便于集成到自动驾驶系统中。该工程的核心价值在于解决了 IMU 安装偏差导致的姿态测量误差问题为自动驾驶系统的定位、导航、决策提供可靠的姿态数据支撑是自动驾驶多传感器联合标定环节的关键工具。