趁手上有个项目在做人机交互设备的惯性导航我把IMU内参标定这块重新捋了一遍。这篇A Robust and Easy to Implement Method for IMU Calibration without External Equipments以下简称多位置静止法算是我见过最适合工程落地的方案之一不需要转台、不需要精密水平仪手拿着IMU摆几个不同姿态采集一段数据用最小二乘就能把加速度计的bias、尺度因子、轴间失准一起解出来陀螺仪的bias也能顺带估计掉。它解决的是IMU内参标定的第一步——把散装传感器变成可用传感器是所有后续位姿解算、外参标定、重力对齐的前提。这篇文章我会先从原理讲清楚为什么这个方法有效再把完整的Python实现和工程中踩过的坑都摊开来说适合正在做SLAM、无人机飞控、机器人导航或者被IMU漂移搞得头疼的开发者参考。1. 项目概述与核心需求解析1.1 这篇论文解决的核心问题IMU里的加速度计和陀螺仪出厂时并不是理想传感器。加速度计存在零偏bias、尺度因子偏差scale factor error、以及三个轴之间不严格正交导致的失准misalignment陀螺仪同样存在bias和尺度问题。这些误差不校准积分出来的位姿会以肉眼可见的速度发散yaw角慢漂只是最轻的症状位置误差更是会成平方级增长。传统校准方法依赖转台或者高精度水平仪把IMU固定在已知姿态下测量然后反推误差参数。问题在于转台不是谁都买得起实验室那套环境搬到产线或外场根本不现实。这篇论文的思路非常直接静止状态下加速度计测量的是比力specific force它的模长恒等于当地重力加速度g。这个天然约束就是外部设备的替代品。只要让IMU在多个不同姿态下静止采集足够多的数据点就能在不知道每个姿态真实角度的情况下仅凭测量值模长必须等于g这一条约束把加速度计的12项误差参数或者合理的9参数模型全部解出来。1.2 为什么无需外部设备是工程刚需我自己最早做IMU校准用的是六面体大理石平台那一套精度确实高但每次标定要花将近一个小时设备还重。后来转到多位置静止法一个IMU拿在手里找张桌子摆十来个姿势五分钟采集完毕回电脑上跑个脚本就出参数了。这个流程对产线批量标定、外场快速调试、无人机起飞前自检都太友好了。从算法层面看这个方法的另一大优势是稳健性。它不要求你精确知道每个位置的姿态角甚至不需要IMU完全静止——静止段用方差检测切出来每段求平均噪声自然被抹掉。论文标题里那个Robust不是虚的实测下来即使采集环境有些微振动只要静止段够长、位置够多解出来的参数依然相当稳定。1.3 目标读者与前置知识这篇文章适合以下几类人做视觉惯导SLAM想自己标定IMU的搞无人机飞控发现姿态解算漂移严重的做组合导航需要把IMU误差参数喂给滤波器的以及对MEMS传感器建模刚入门的同学。前置知识方面你需要知道IMU坐标系、加速度计测的是比力静止时模长为g、会一点矩阵和最小二乘的概念。不需要精通文中代码部分我会把每一步都拆开讲。2. IMU误差模型与校准原理拆解2.1 IMU的核心误差来源详解先把误差模型立起来。加速度计的实际测量值a_m和真实比力a_t之间的关系可以写成a_m T_a · (a_t b_a) n_a其中T_a是3x3矩阵综合了尺度因子和轴间失准b_a是零偏矢量n_a是测量噪声。拆开看bias零偏即使真实加速度为零传感器也会输出一个非零值。单位是m/s²MEMS加速度计的零偏通常在几十mg量级1mg ≈ 0.0098 m/s²。它受温度影响显著这也是为什么标定最好在目标工作温度附近做。尺度因子误差传感器输出的数字量换算成物理量时比例系数并非严格等于标称值误差通常在0.1%1%之间。轴间失准MEMS工艺很难保证三个轴严格正交芯片贴装时也会引入少量旋转导致一个轴会感应到其他轴的加速度分量角度偏差通常在0.1°1°量级。陀螺仪的模型类似ω_m T_g · (ω_t b_g) n_g但请注意陀螺仪在静止时ω_t 0所以静止数据只能用来估计b_gT_g里的尺度因子和失准在纯静止条件下无法激励出来这我在2.3节细说。2.2 加速度计多位置校准原理关键洞察就一句话静止时加速度计测量值的模长恒等于g。不管IMU朝向如何真实比力a_t的方向始终指向天严格说是反重力方向但模长不变。于是有||T_a^(-1) · (a_m - b_a)|| g把这个约束改写成最小二乘问题目标函数就是min Σ ( ||T_a^(-1) · (a_m,i - b_a)||² - g² )²对i1...N个不同姿态的静止测量值求和。每个姿态提供一个独立的约束方程未知数是T_a里的参数和b_a。只要姿态数量足够、分布够多样这个非线性最小二乘问题可以被稳定求解。这就是多位置的价值一个位置只能约束椭球上的一个点至少需要9个或更多精心设计的姿态才能把椭球的形状、位置全部锁定。2.3 陀螺仪为什么只能估计bias这是一个很多初学者会混淆的点。陀螺仪测的是角速度静止状态下真实角速度为零所以任何稳态输出都直接归因于bias。用静止段的均值就能估计b_g非常干净。但陀螺仪的尺度因子和轴间失准在静止条件下完全无法激励因为这些参数只有在有角速度输入时才会显现。要标定陀螺仪的完整12参数需要精密转台提供已知角速度或者用速率测试的方法。多位置静止法对陀螺仪的处理仅限于bias估计这一点论文说得非常清楚很多复现者没注意。还有个容易被忽略的细节陀螺仪bias的估计受温度影响很大开机前几分钟由于自热效应bias会慢慢漂移。我的经验是传感器上电后至少预热2-3分钟再做静止采集得到的bias值在后续使用中会更可靠。2.4 参数数量与可观测性分析加速度计模型里T_a有9个元素、b_a有3个元素看起来是12个未知数。但仔细分析会发现T_a中包含了3个旋转自由度而这些旋转在模长约束下是不可观测的——旋转不改变矢量的模长。所以实际可标定的参数是3个尺度因子 3个轴间失准角 3个bias共9个参数。这也是为什么代码实现里通常把T_a参数化为上三角或下三角形式而不是让9个元素全部自由T_a [[s_x, m_xy, m_xz], [0, s_y, m_yz], [0, 0, s_z ]]这里有3个尺度参数加3个失准参数。如果你把T_a的9个元素全放开求解器会陷入退化严格讲这是模型的超参数化问题。相应地至少需要9个线性无关的静止姿态才能求解9个参数。实际操作中由于噪声和姿态分布的不理想建议至少采集15-20个姿态才能得到稳定结果。3. 实操流程从数据采集到参数求解3.1 数据采集的姿势设计与操作规范采集流程看起来简单但姿势设计直接决定求解质量。我的标准做法如下先准备一个平面桌面把IMU固定在一个小方块上或者直接手持。规划12个姿态覆盖以下朝向三个轴分别垂直向上和垂直向下共6个姿态三个轴分别指向水平方向共3个姿态额外3-4个斜45°的姿态随机搭配每个姿态保持静止5-10秒采样率100Hz的话就是500-1000个采样点。姿态切换时动作要快、要稳不要在空中划弧线切换到位后等1-2秒再开始记录。关键是不要只做四周面的六个姿态——那种分布只能在三个方向上约束椭球对轴间失准的辨识度很差解出来的参数方差会比较大。关于静止段检测我会用滑动窗口计算加速度计测量值的方差窗口长度0.5秒方差低于阈值就判定为静止然后取窗口内均值作为该姿态的测量点。这个逻辑写成代码很简单效果却很扎实。3.2 静止数据的分割与预处理拿到原始数据后第一步是画曲线看一眼整体形态确认每个姿态段确实稳定。然后用滑动窗口方差法自动分割。关键参数有两个窗口长度建议0.5-1秒方差阈值建议设为传感器噪声方差的3-5倍具体值需要根据实验数据微调。每个静止段处理完后你得到的是12个或更多三维向量每个向量是某个姿态下加速度计的均值加上同样数量的陀螺仪均值。加速度计的均值用于拟合误差参数陀螺仪的均值用于估计bias。预处理阶段我发现一个常见坑如果采集过程中传感器温度一直在上升刚上电时数据会出现明显的趋势性漂移这时分割出来的均值会有偏差。解决办法是上电后先等温度稳定或者用一个温度补偿模型这个论文没涉及但工程上很重要。3.3 加速度计参数求解的数学推导有了N个姿态的均值a_m,ii1...N下面推导求解过程。假设参数化采用上三角形式待求参数为p [b_x, b_y, b_z, s_x, s_y, s_z, m_xy, m_xz, m_yz]构建残差向量r_i(p) ||T_a(p)^(-1) · (a_m,i - b_a(p))||² - g²用非线性最小二乘求解p* argmin Σ r_i(p)²初始值怎么给bias给零向量尺度因子给1失准给0这个初始值下目标函数值不会太离谱Levenberg-Marquardt算法一般都能收敛。需要注意的是有些姿态方向刚好让某个轴几乎平行于重力方向此时该轴的信息非常充分而某些轴可能信息不足。姿态多样性不足时求解得到的参数会在某个方向上出现高方差表现为某个失准参数大于2°以上。此时应该补采姿态而不是强行接受结果。3.4 陀螺仪bias的估计陀螺仪bias估计简单得多但有一个重要细节不能只用一个静止段的均值因为每段数据里残余的噪声和微小振动会影响精度。正确做法是取所有静止段的均值再求平均b_g (1/N) Σ_i mean(ω_m,i)其中mean(ω_m,i)是第i个静止段内陀螺仪测量值的均值。这样等效于把噪声平均掉了N倍效果比单段好很多。陀螺仪的尺度因子和失准在静止条件下无法标定这点必须对项目组成员讲清楚否则会有人拿着这套代码来问为什么陀螺仪只输出了bias。4. 代码实现Python完整流程4.1 代码结构与依赖完整代码基于Python 3.8依赖numpy和scipy。核心算法只有两个滑动窗口静止检测、非线性最小二乘参数求解。整个流程不到两百行结构如下1. 生成/读取IMU原始数据 2. 滑动窗口静止检测 3. 静止段数据平均 4. 加速度计参数非线性最小二乘求解 5. 陀螺仪bias估计 6. 校准效果验证与残差输出为了演示方便我先写了一个数据生成模块用已知真值模拟多个静止姿态的IMU输出加上噪声和误差这样能直接验证求解算法是否能恢复真值。实际使用时把这段换成从传感器读到的真实数据即可。4.2 模拟IMU数据生成import numpy as np from scipy.optimize import least_squares rng np.random.default_rng(42) g 9.80665 # 加速度计真实参数 true_b_a np.array([0.05, -0.04, 0.06]) # bias, m/s^2 true_s_a np.array([1.01, 0.98, 1.02]) # 尺度因子 true_m_a np.array([[1.0, 0.012, -0.008], [0.0, 1.0, 0.015], [0.0, 0.0, 1.0]]) # 失准(上三角) # 陀螺仪真实bias true_b_g np.array([0.01, -0.012, 0.008]) # rad/s def simulate_imu_poses(num_poses15, samples_per_pose500, fs100.0): 模拟多位置静止IMU采集数据 acc_data, gyro_data [], [] for i in range(num_poses): # 随机生成姿态均匀覆盖球面 # 用随机四元数表示旋转保证姿态分布均匀 q rng.normal(size4) q q / np.linalg.norm(q) # 将四元数转为旋转矩阵 w, x, y, z q R np.array([ [1 - 2*(y*y z*z), 2*(x*y - w*z), 2*(x*z w*y)], [2*(x*y w*z), 1 - 2*(x*x z*z), 2*(y*z - w*x)], [2*(x*z - w*y), 2*(y*z w*x), 1 - 2*(x*x y*y)] ]) # 真实比力方向静止时指向天重力反方向变换到IMU坐标系为 R.T [0,0,g] a_t R.T np.array([0.0, 0.0, g]) # 加上误差模型 T_true true_s_a[:, None] * true_m_a a_m T_true (a_t true_b_a) # 添加噪声 noise_a rng.normal(0, 0.01, size(samples_per_pose, 3)) acc np.tile(a_m, (samples_per_pose, 1)) noise_a # 陀螺仪静止只有bias和噪声 noise_g rng.normal(0, 0.001, size(samples_per_pose, 3)) gyro np.tile(true_b_g, (samples_per_pose, 1)) noise_g acc_data.append(acc) gyro_data.append(gyro) return np.concatenate(acc_data, axis0), np.concatenate(gyro_data, axis0) acc_raw, gyro_raw simulate_imu_poses()这里用随机四元数生成姿态比手动指定12个角度更简单而且天然分布均匀。需要注意的是静止时真实比力方向在导航系中是[0,0,g]转到IMU坐标系时要用R的转置这个细节写错的话仿真结果会完全对不上。4.3 静止段分割与平均def detect_static_segments(acc_signal, fs, window_len0.5, threshold_mult3.0): 基于滑动窗口方差的静止段检测 window int(window_len * fs) n_samples len(acc_signal) # 计算窗口内方差 variances [] for idx in range(n_samples - window 1): seg acc_signal[idx:idxwindow] variances.append(np.var(seg, axis0).mean()) variances np.array(variances) # 阈值所有方差的分位数 thr threshold_mult * np.percentile(variances, 30) static_flags variances thr # 找连续静止段 segments [] start None for idx, flag in enumerate(static_flags): if flag and start is None: start idx elif not flag and start is not None: if idx - start window: segments.append((start, idx window - 1)) start None if start is not None: segments.append((start, len(variances) window - 1)) return segments segments detect_static_segments(acc_raw, fs100.0) acc_means, gyro_means [], [] for (s, e) in segments: acc_means.append(np.mean(acc_raw[s:e], axis0)) gyro_means.append(np.mean(gyro_raw[s:e], axis0)) acc_means np.array(acc_means) gyro_means np.array(gyro_means)这版代码用30%分位数加阈值倍数来自适应判断静止阈值比固定阈值鲁棒得多。实际使用中我还会加一个最小持续时间约束防止把切换姿态时的瞬间停顿误判为静止段。另一个细节静止段分割对加速度计的方差做判断因为静止时加速度计方差主要反映噪声陀螺仪的方差也可以用来辅助判断但在低端MEMS上陀螺仪噪声比加速度计还大反而不如单独用加速度计可靠。4.4 加速度计参数求解核心代码def build_T(params): 从9参数构建上三角矩阵 T sx, sy, sz, mxy, mxz, myz params[3:9] T np.array([ [sx, mxy, mxz], [0.0, sy, myz], [0.0, 0.0, sz ] ]) return T def acc_residual(params, acc_meas, g9.80665): 残差||T^{-1}(a_m - b)||^2 - g^2 b params[0:3] T build_T(params) T_inv np.linalg.inv(T) residuals [] for a_m in acc_meas: a_t T_inv (a_m - b) residuals.append(np.linalg.norm(a_t)**2 - g**2) return np.array(residuals) # 初始值bias0, scale1, misalignment0 p0 np.array([0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0]) result least_squares(acc_residual, p0, args(acc_means,), methodlm) p_est result.x b_est p_est[0:3] T_est build_T(p_est) s_est np.diag(T_est) m_est T_est / s_est[:, None] - np.eye(3) print(估计bias: , b_est) print(估计scale: , np.diag(T_est)) print(估计misalignment:) print(m_est)least_squares的lm方法对应Levenberg-Marquardt对这种小规模非线性最小二乘问题收敛快、效果好。如果你发现收敛到局部极小值可以试着用不同的初始值跑几次选择残差最小的结果。一个值得注意的细节我在残差里用的是模长平方差即||a_t||² - g²而不是||a_t|| - g。平方差在求解时梯度更平滑收敛范围更大如果直接用模长差在接近最优解时收敛更快但初始值不好时容易卡住。工程实现建议先用平方差跑通后续优化再考虑切换。4.5 陀螺仪bias与校准验证# 陀螺仪bias所有静止段的均值再平均 b_g_est np.mean(gyro_means, axis0) print(陀螺仪bias: , b_g_est) # 校准效果验证计算残差均方根(RMS) res acc_residual(p_est, acc_means) rms_before np.sqrt(np.mean(acc_residual(p0, acc_means)**2)) rms_after np.sqrt(np.mean(res**2)) print(f校准前RMS: {rms_before:.6f} (m/s^2)^2) print(f校准后RMS: {rms_after:.6f} (m/s^2)^2) # 用校准后的参数修正测量值再检查模长 corrected_norms [] for a_m in acc_means: a_t np.linalg.inv(T_est) (a_m - b_est) corrected_norms.append(np.linalg.norm(a_t)) corrected_norms np.array(corrected_norms) print(f修正后模长均值: {corrected_norms.mean():.4f} m/s^2) print(f修正后模长标准差: {corrected_norms.std():.4f} m/s^2)校准前RMS通常会远大于校准后。比如模拟数据里校准前模长误差可能在0.1 m/s²量级校准后能压到噪声水平约0.01 m/s²。如果你的数据校准后误差还在0.05 m/s²以上大概率是姿态覆盖不够或者静止段切得不准。整个代码实现的核心思路就是把论文里的数学模型直接翻译成可执行代码没有用到任何黑科技。这也是我觉得这个方案最值得推荐的原因它不像某些论文只有理论没有实现而是真的可以在半小时内跑出一个合格的内参标定结果。5. 常见问题与排查技巧实录5.1 数据采集中的典型问题姿态数量不够。我见过不少同事用6个姿态就是正方体六个面做标定解出来的尺度因子和bias看着还行但轴间失准完全乱。原因是六个正交姿态只能约束椭球在三个主轴方向上的信息对轴间耦合的辨识度不足。多位置法的下限是9个姿态工程上建议12-15个以上。静止时间太短。每个姿态至少保持5秒太短的话均值噪声降不下来。这个问题在低端MEMS传感器上尤其明显噪声大需要更长的平均时间才能达标。切换姿态时动作幅度过大。运动过程中的加速度可能超出传感器量程导致后续数据出现削顶畸变。实际操作中切换动作要快而稳到位后停顿2秒再进入记录段这个停顿时间不算在统计段里。5.2 求解阶段的异常与对策求解不收敛或收敛到明显错误值。检查初始值是否合理bias0、scale1检查静止段分割是否正确检查是否有某一段数据混入了运动段。排除这些后如果还不收敛可以尝试换一个初始值或者在残差函数里把g的数值换成当地精确重力值可以用在线重力公式计算或者用标准重力公式9.780327*(10.0053024*sin²φ)估算。解出的轴间失准大于2度。这通常不是真的失准大而是姿态分布畸形导致的问题。检查采集的静止段是否集中在一个方向上如果是补采一些姿态就可以解决。残差校准后依然很大。先量化噪声水平把传感器放在桌面上不动采10秒数据计算加速度计测量值的标准差。如果校准后残差和噪声标准差同一量级说明标定质量已经达标不必追求极端小残差。5.3 与LiDAR/相机联合标定的关系做lidar imu标定和相机imu联合标定的同学要注意内参标定是外参标定的前置条件。如果IMU内参不准联合标定解出来的外参会把内参误差吸收进去导致标定结果在换一种运动模式后又失效。我的习惯是每次拿到新设备先跑一遍多位置标定得到内参再做相机/雷达与IMU的外参标定这样两边互不干扰。另外一个常见疑问是为什么重力对齐之后yaw还是慢漂。多位置法解决的是IMU自身的确定性误差yaw慢漂主要来自陀螺仪噪声积分和温漂这两个靠静态标定解决不了需要后续的视觉/磁力计/多传感器融合来修正。很多人把期望寄托在标定上标完发现yaw还是漂就说方法没用——这个锅不该算法背。5.4 标定结果评估速查表检查项合格标准失败时的排查方向校准后残差RMS接近传感器噪声水平静止段分割、姿态覆盖修正后模长标准差 0.02 m/s²采样数据质量、温度漂移陀螺仪bias稳定度多次上电估计值偏差 0.005 rad/s预热时间、传感器老化失准角范围通常在-2°2°过大的失准角可能是姿态分布问题不同批次标定一致性参数偏差 2%采集流程标准化我实际跑过一条产线小批量设备用这个方法标完的IMU静止姿态下加速度计模长残差基本在0.005 m/s²以内完全满足消费级和工业级设备的初始定位需求。如果要把精度推到更高比如航空级那还是得回到转台标定的路子上毕竟物理参考的精度不可替代。后面如果想继续扩展这个方案还可以延伸到在线标定和温度补偿。你可以把不同温度下标定的参数做成温度曲线插值使用或者把残差作为状态量塞进组合导航滤波器里做在线修正。这些都是从这个基础方案上长出来的分支先把基础的地基打牢后面的路会顺畅很多。