自动驾驶多传感器联合标定系列之IMU到车体坐标系的标定工程 ， 本商品在已知GNSS/GPS到车体坐标系的外参前提下，根据GNSS/GPS的定位信息与IMU信息完成IMU到GNSS/GPS 的外参标定，并进一步获得IMU到车体坐标系的外参标定 本商品提供两种标定模式：车辆直线运动及自由运动，这两种模式下的注释工程

一、工程概述

Imu2Car 是一款面向自动驾驶场景的多传感器联合标定工具，核心功能是实现 IMU（惯性测量单元）到车体坐标系的精准标定，通过计算 IMU 与车体坐标系之间的滚转角（Roll）、俯仰角（Pitch）、偏航角（Yaw）偏移量，消除传感器安装误差带来的测量偏差，为自动驾驶系统提供可靠的姿态与位置数据支撑。

工程支持两种标定模式，分别适配不同的场景需求：

1. 直线行驶标定（method_id=1）：适用于车辆能稳定直线行驶的场景，仅标定偏航角（Yaw）偏移；
2. 自由运动标定（method_id=2）：适用于复杂行驶场景，可同时标定滚转角、俯仰角、偏航角三个维度的偏移。

标定过程以 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	最小直线点数	600	800	200	500
minmovingsquare	最小移动距离平方	0.02m/10ms	0.02m/10ms	0.015m/10ms	0.015m/10ms

2.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_id：1（直线）/2（自由运动）	bool：标定成功返回true
StraightHeading	直线行驶标定实现，仅标定Yaw角	-	bool：标定成功返回true
FreeHeading	自由运动标定实现，标定Roll/Pitch/Yaw角	-	bool：标定成功返回true
StraightCalibrateSingle	直线标定单角度核心逻辑	x/y：位置数据；gnssheadingvec：GNSS姿态数据；threshold：点线距离阈值	输出caliboffset（角度偏移）、inlineridx（内点索引）
FreeCalibrateSingle	自由运动标定单角度核心逻辑	同上 + LineClusterParam：聚类参数	输出caliboffsetmean（平均角度偏移）
LeastSquare	最小二乘法拟合直线	x/y：位置数据；inliner_idx：内点索引	输出a/b/c（直线方程参数 ax+by+c=0）
LineCluster	直线聚类，从离散点中提取有效直线段	param：聚类参数	输出lines_idx（各直线段的点索引集合）
SpeedEvaluate	速度验证，评估标定结果准确性	inlineridx：内点索引；yawoffset：Yaw角偏移	输出v_componet（速度分量评估结果）
WriteToCsv	标定结果写入CSV文件	-	bool：写入成功返回true

2.2.2 RansacFitLine 类

鲁棒直线拟合类，基于 RANSAC（随机抽样一致性）算法，从含噪声的离散点中筛选内点并拟合直线，有效排除异常值干扰，提升标定精度。

核心函数：

• Estimate：RANSAC 算法核心实现，随机采样生成直线模型，筛选最优模型；
• Evaluate：模型评估，计算点到直线的距离，筛选内点并计算模型得分；
• GetLineSlope：根据两点计算直线方程参数（ax+by+c=0）；
• LeastSquare：最小二乘法拟合直线，用于内点的精准拟合。

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_id=1）

1. 数据加载：通过 LoadEnuCSVDatas 函数加载 NovAtel 离线 CSV 数据，存储到 NovAtelEnuDatas 实例中；
2. RANSAC 鲁棒拟合：调用 RansacFitLine::Estimate 对 x/y 位置数据进行直线拟合，筛选内点（符合直线趋势的有效数据）；
3. 姿态均值计算：通过 GetMeasuredMean 计算内点对应的 GNSS 偏航角（Yaw）均值，作为 IMU 测量参考值；
4. 真值计算：通过 LeastSquare 对筛选后的内点进行精准直线拟合，基于直线方程计算真实偏航角（GT 值）；
5. 误差计算：真实偏航角与 GNSS 测量均值的差值即为 Yaw 角标定偏移量，同时将角度单位转换为弧度；
6. 结果验证：调用 SpeedEvaluate 函数，基于东北天坐标系的速度分量（ve/vn）验证标定结果的合理性；
7. 结果输出：通过 WriteToCsv 将标定结果写入 CSV 文件，通过 OutputCalibResult 输出四元数表示的旋转矩阵。

3.2 自由运动标定流程（method_id=2）

自由运动标定流程在直线标定基础上增加了直线聚类步骤，支持 Roll/Pitch/Yaw 三个角度的独立标定，核心流程如下：

1. 数据加载：与直线标定一致，加载 NovAtel 离线数据；
2. 直线聚类：调用 LineCluster 函数，根据 LineClusterParam 参数对离散位置点进行聚类，提取多个有效直线段（满足最小点数、最小移动距离等条件）；
3. 分段标定：对每个有效直线段，重复直线标定的 RANSAC 拟合、内点筛选、误差计算流程，得到该线段的角度偏移量；
4. 均值融合：对所有有效直线段的角度偏移量进行加权平均（权重为线段点数），得到最终的角度标定偏移量；
5. 多角度标定：分别对 Roll（x/z 位置数据）、Pitch（y/z 位置数据）、Yaw（x/y 位置数据）执行上述流程，完成三个角度的标定；
6. 结果输出：将三个角度的标定结果（度和弧度）写入 CSV 文件。

四、关键算法原理

4.1 RANSAC 鲁棒拟合算法

RANSAC 算法是一种鲁棒的参数估计算法，用于从含噪声和异常值的数据中拟合模型，核心步骤如下：

1. 随机采样：从数据集中随机选择 2 个点（直线拟合最小样本数），生成直线模型（ax+by+c=0）；
2. 内点筛选：计算所有数据点到该直线的距离，距离小于阈值的点视为内点；
3. 模型评估：计算内点占比作为模型得分；
4. 迭代优化：重复上述步骤至最大迭代次数，选择得分最高的模型作为最优直线拟合结果。

该算法能有效排除道路颠簸、传感器噪声等导致的异常数据，提升标定的鲁棒性。

4.2 最小二乘法直线拟合

对 RANSAC 筛选出的内点进行精准拟合，最小化点到直线的距离平方和，求解直线方程参数（a/b/c）。核心公式如下：

自动驾驶多传感器联合标定系列之IMU到车体坐标系的标定工程 ， 本商品在已知GNSS/GPS到车体坐标系的外参前提下，根据GNSS/GPS的定位信息与IMU信息完成IMU到GNSS/GPS 的外参标定，并进一步获得IMU到车体坐标系的外参标定 本商品提供两种标定模式：车辆直线运动及自由运动，这两种模式下的注释工程

给定内点集合 \((xi, yi)\)（i=1,2,...,n），目标是找到直线 \(ax + by + c = 0\)，使得 \(\sum{i=1}^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 b=1, \quad c = -\frac{\sum xi^2 \sum yi - \sum xi \sum xiyi}{n\sum xi^2 - (\sum x_i)^2}

\]

4.3 角度偏移计算

标定的核心是计算 IMU 测量角度与真实角度的偏差，步骤如下：

1. 真实角度（GT）：基于拟合直线的斜率计算，公式为 \(gt\_degree = \arctan(-a/b) \times \frac{180}{\pi}\)（a、b 为直线方程参数）；
2. 测量角度（GNSS）：对 RANSAC 内点对应的 GNSS 姿态数据取均值，转换为角度单位；
3. 角度修正：根据测量角度的范围对真实角度进行修正（如 GNSS 角度在 90°~270° 时，GT 角度加 180°）；
4. 偏移计算：\(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 编译步骤

1. 安装依赖工具与库：

# 基础工具
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 库（参考官方文档或博客安装）

1. 编译工程：

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 ./data

5.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_degree	pitch_offset_degree	yaw_offset_degree	roll_offset_rad	pitch_offset_rad	yaw_offset_rad
滚转偏移（度）	俯仰偏移（度）	偏航偏移（度）	滚转偏移（弧度）	俯仰偏移（弧度）	偏航偏移（弧度）

六、结果验证与精度保障

6.1 速度验证机制

直线标定模式中，SpeedEvaluate 函数通过对比标定前后的速度分量一致性来验证结果准确性：

1. 基于 IMU 测量角度和标定后角度，分别计算东北天坐标系下的速度分量；
2. 统计标定前后速度分量的均值、最大值、最小值；
3. 理想情况下，标定后的速度分量应更稳定（波动范围更小），均值更接近真实速度方向。

6.2 参数调优建议

• 直线标定阈值（threshold）：默认 0.15m，可根据道路平整度调整，颠簸路面可适当增大；
• 自由运动聚类参数：通过 LineClusterParam 的 YawParam/PitchParam/RollParam 函数单独配置，确保各角度的有效直线段提取；
• RANSAC 迭代次数：默认 15 次（直线标定）/20 次（自由运动标定），数据噪声较大时可增加迭代次数。

七、工程扩展与应用场景

7.1 工程扩展方向

• 实时标定支持：当前工程基于离线数据，可扩展为实时读取传感器数据（GNSS+IMU），实现在线标定；
• 多传感器融合：增加激光雷达、相机等传感器数据，实现多源数据联合标定，提升标定精度；
• 动态阈值调整：基于数据质量自动调整 RANSAC 阈值、聚类参数等，适配不同场景；
• 可视化功能：增加标定过程可视化（如直线拟合结果、内点筛选效果），便于问题排查。

7.2 应用场景

• 自动驾驶车辆传感器安装后标定：消除 IMU 安装偏差，确保姿态测量准确性；
• 传感器维护后的重新标定：车辆维修、传感器拆卸后，快速恢复标定参数；
• 多传感器融合系统初始化：为 IMU 与 GNSS、激光雷达等传感器的融合提供准确的外参。

八、总结

Imu2Car 工程通过模块化设计，实现了 IMU 到车体坐标系的高精度标定，支持两种标定模式，适配不同的自动驾驶场景。核心算法采用 RANSAC 鲁棒拟合与最小二乘法结合，有效抵抗噪声和异常值干扰，保障标定精度；工程提供完整的编译、运行流程，输出结果直观易懂，便于集成到自动驾驶系统中。

该工程的核心价值在于解决了 IMU 安装偏差导致的姿态测量误差问题，为自动驾驶系统的定位、导航、决策提供可靠的姿态数据支撑，是自动驾驶多传感器联合标定环节的关键工具。