尧图网页设计 
如果你玩过自组四轴、轮足机器人或者激光雷达SLAM大概率遇到过这种情况IMU买回来直接怼进系统姿态解算出来的Roll和Pitch还行Yaw却像喝醉了一样慢慢飘或者静止不动加速度计读数的模长一会儿9.6一会儿10.2。这时候懂行的人会提醒你——先标定再谈融合。但标定这件事说容易也容易说难也难。传统方案绕不开精密转台一套入门级的三轴转台价格够买好几台飞控就算借到转台装夹、对中、规划角速度轨迹每一步都在消耗耐心。直到我重新翻出Tedaldi、Pretto和Menegatti在ICRA 2014上发表的那篇经典论文《A Robust and Easy to Implement Method for IMU Calibration without External Equipments》以及作者后来开源的imu_tk工具才意识到原来在绝大多数工程场景下标定IMU根本不需要什么昂贵设备只需要一个水平桌面和几分钟的耐心。这篇就是用Python把论文的方法从零复现了一遍的完整记录。内容包括误差模型怎么建、六个静止方位怎么采、手动旋转数据怎么用、优化目标怎么设以及我自己实测踩过的几个大坑。如果你手里有一块MPU6050、ICM20602或者任何六轴模块又不想写邮件求人帮忙标定这篇应该能帮你省下不少时间。这里需要先解释一下标题里without External Equipments的含义。它并不是说不需要任何工具而是说不需要转台、激光跟踪仪这类高精度外部参考。整个标定过程只依赖两个物理事实一是静止时加速度计的输出模长必然等于当地重力加速度二是重力方向在空间中固定指地。利用这两条最基本的物理规律就能把加速度计和陀螺仪的误差参数全部反解出来。1. 不标定就上机IMU的出厂素颜到底有多不靠谱1.1 三类误差源零偏、比例因子和安装误差MEMS IMU的误差主要分三类这三类也是整个标定过程要解决的三个问题。第一类是零偏也叫Bias。静止时加速度计理论上应该输出0陀螺仪理论上应该输出0但实际芯片制造工艺和封装应力决定了它不可能输出精确的0。有的模块上电后加速度计三个轴分别读出0.2、-0.15、9.62这中间的偏差就是零偏。陀螺仪的零偏更明显典型MPU6050在室温下的零偏可能在0.05~0.2 rad/s之间波动如果不做任何处理直接用一秒就偏好几度几分钟后Yaw飘到没法看。第二类是比例因子误差Scale Factor。ADC读到的原始量纲和物理量纲之间的转换系数并不是完美的厂家datasheet上给的是典型值比如加速度计±2g量程对应的灵敏度16384 LSB/g但实际芯片可能偏了0.5%~1%。这个偏差在静止小加速度下影响不大但在车辆急加速、机器人急转这种动态场景下会被放大。第三类是安装误差Misalignment。三轴MEMS芯片在PCB上焊接时不可能做到绝对正交三个敏感轴之间可能有0.1°~1°的夹角偏差。这个误差在高动态场景下同样会被放大而且它和比例因子误差在数学模型上高度耦合必须放在同一个矩阵里一起求。这三类误差综合作用的结果就是静止时加速度计模长不在9.8附近、动态时陀螺仪积分出来的轨迹严重偏离真实路径。更麻烦的是它们之间会互相掩盖光看某一个轴的原始数据很难判断问题出在哪个环节。1.2 不标定直接用的后果一组实测日志我拿一块ICM20602六轴模块做了个对比实验不标定直接上电采样率200Hz静止放在桌面上记录一分钟数据。结果如下指标原始数据表现标定后表现加速度计模长均值9.72 m/s²9.806 m/s²模长波动范围±0.35 m/s²±0.03 m/s²三轴加速度零偏估计模长未补偿0.021 m/s²静止陀螺仪零偏z轴-0.08 rad/s0.001 rad/s静止120秒Yaw漂移积分约8.2°约1.1°这里的加速度计模长波动±0.35算是比较差的个体但市面上便宜的模块大概率就是这个水平。最致命的是Yaw漂移8.2°的漂移意味着做任何航向相关的东西都只能是空想。有人可能会说我用卡尔曼滤波融合磁力计不就行了但磁力计本身也有一堆校正问题而且室内钢筋结构、电机磁场都会干扰它把IMU内参先标好至少能保证姿态解算的底子是干净的。很多做Lidar-IMU外参标定、相机-IMU联合标定的人会问外参标定时需不需要先把内参标好我的回答是内参不标外参标出来也不稳。因为外参标定依赖IMU积分给出的运动轨迹内参误差会被直接投影到外参残差里最后标出来的旋转矩阵和位移向量看起来收敛了换个场景又偏了。2. 论文核心思路拆解重力当标准砝码加速度计当旋转真值2.1 加速度计标定的数学本质9参数模型的模长约束论文的核心思路之一是把重力加速度当作一个天然、免费、处处存在的标准砝码。加速度计静止时测到的物理量就是重力加速度不管IMU朝向如何、放在哪儿模长必须等于当地重力加速度g这个约束和设备本身的质量没有任何关系。用数学语言描述误差模型a_m T_a * a_true b_a noise其中a_m是读数a_true是真实加速度b_a是零偏向量T_a是3x3矩阵包含比例因子和安装误差。在论文的标准模型里T_a取上三角或者下三角形式一共6个自由度加上3个零偏总共9个待求参数。标定的目标函数非常直白min Σ || T_a^(-1) * (a_m - b_a) ||^2 - g^2这里对每一组静止样本求补偿后的加速度模长让它尽可能接近g。正因为约束是模长等于g我们完全不需要知道IMU在这个静止姿态下具体朝向哪里所以不需要外部设备提供姿态真值。6个不同的静止方位本质上就是让重力矢量在IMU的三轴坐标系下产生6种不同的投影组合从而把9个参数全部激励出来。我在实现时做了一个等价变换把T_a^(-1)整体当作优化变量之一直接令a_true A_a * (a_m - b_a)然后用||a_true|| g做约束。这样做的好处是不用在代码里反复做矩阵求逆优化变量的数量不变初值也更直观A_a取单位阵。2.2 陀螺仪标定的巧妙之处用加速度计充当旋转真值陀螺仪标定比加速度计标定麻烦得多原因在于角速度的真值很难获得。转台能提供精确角速度但转台价格摆在那里。论文的解决办法是把加速度计当作旋转的真值来源。思路是这样的如果IMU从静止状态A经过一个旋转到达静止状态B那么旋转前后的重力方向在IMU坐标系下的表示必然发生变化。这个变化可以用加速度计在两个静止段的测量值分别估计出来。同时陀螺仪在旋转段积分的姿态变化也应该等于这个变化。两边一对照就能构建出陀螺仪的误差模型约束。具体来说旋转前静止段取加速度计均值并归一化得到单位向量g0。旋转后静止段取加速度计均值并归一化得到单位向量g1。用公式axis g0 × g1、angle atan2(||axis||, g0·g1)构造出参考旋转这里用到了重力对齐的基本思想。用陀螺仪读数做四元数积分得到实测旋转。两者差一个旋转矩阵误差优化目标就是让所有旋转段上的误差最小。这样就能把陀螺仪的零偏、比例因子和安装误差全部估计出来。需要强调一点整个过程中没有使用任何外部运动捕捉系统或转台所有的真值都来自重力和已经标定完成的加速度计。这就是论文标题里without External Equipments的真正含义。2.3 为什么这个方法Robust异常值剔除机制论文标题里还有个关键词是Robust。实际采集数据时手抖、桌面震动、模块松动都会引入异常值。如果不做任何处理一个离群样本就可能把优化结果拉偏。论文的处理策略是在优化迭代过程中动态剔除残差过大的样本。具体实现上我在每次迭代后计算所有样本残差的标准差σ把残差绝对值超过3σ的样本剔除掉然后用剩余样本继续迭代。这个策略的执行顺序很关键必须先用当前参数算残差再决定剔除哪些样本否则初值很烂的时候可能把所有样本都误杀了。3. 数据采集实操六面静止与手动旋转的完整流程3.1 加速度计标定采集六面静止法的顺序与时长加速度计标定需要六组静止数据分别对应六个方位让重力分别作用在三个轴的正负方向上。我习惯的顺序如下序号放置方式对应重力方向1平放正面朝上z轴朝上2倒扣正面朝下z轴朝下3侧立x轴朝上x轴朝上4侧立x轴朝下x轴朝下5侧立y轴朝上y轴朝上6侧立y轴朝下y轴朝下每个面建议采集60秒以上。放置时要用双面胶或者橡皮泥固定模块不要用手扶着因为手部的微小抖动会污染加速度计数据。模块要放在水平的桌面上虽然这个方法的约束不要求桌面绝对水平——桌面稍微倾斜一点重力矢量在IMU坐标系下的投影依然等于g优化目标里模长等于g的约束照样成立——但水平放可以减少后续陀螺仪标定时的麻烦。60秒的采集可以分成两段前10秒让模块完全稳定中间40秒作为有效数据最后10秒用于操作切换。我实际写代码时会把整个采集过程的时间戳记录下来后续按时间段切分静止段而不是靠算法自动识别静止段。原因很简单六面放置时切面的过程也是数据的一部分与其让算法区分运动段和静止段不如采集时控制好节奏减少无关的运动数据。3.2 陀螺仪标定采集旋转段设计原则陀螺仪标定需要旋转激励。这里的关键不是随便转几圈而是要让旋转覆盖三个轴的角速度激励而且旋转前后都要有静置段。我推荐的做法是每段旋转都设计成从一个面翻到另一个面平放静止3-5秒。沿着水平轴快速但平缓地翻转90°让IMU侧立x轴朝上。静止3-5秒。再沿着另一个轴翻转回平放或者继续翻到下一个姿态。旋转角度控制在60°~120°之间不要转180°。原因我在代码实现章节会详细解释——180°旋转后重力方向反向从两个单位向量无法唯一确定旋转轴优化时这个数据段会退化。还有一个很多人会踩的设计误区绕竖直轴原地旋转。比如把IMU平放在桌面上然后像拧瓶盖一样绕z轴旋转这种情况下重力方向在IMU坐标系下几乎不变加速度计完全起不到参考作用。正确的做法是让旋转轴尽量与重力方向垂直这样重力方向变化最大参考信息最充足。完整的陀螺仪标定建议采集8~10段旋转覆盖绕三个不同水平轴的翻转每段旋转前静止3~5秒旋转后静止3~5秒。3.3 采集环境的几个细节采集环境对数据质量的影响被很多人低估。首先是桌面要稳固不要放在机箱上面或者有人在旁边走来走去的地方。其次是模块供电要稳定USB供电和电池供电的噪声特征完全不同。再者是温度MEMS IMU的零偏对温度很敏感刚从室内拿到室外前几分钟的数据往往会明显漂移所以最好让模块上电预热10分钟再开始采集。4. 代码实现从CSV原始数据到9参数标定的完整链路4.1 数据格式与预处理去异常值、提取静止段原始数据我按CSV格式记录字段为t, ax, ay, az, gx, gy, gz加速度单位m/s²角速度单位rad/s时间戳单位秒。采集时统一先启动串口读取程序开始记录后再摆模块整个过程连续记录不人为切分文件。import numpy as np import pandas as pd from scipy.optimize import least_squares def quat_mult(p, q): 四元数乘法p和q都是[w, x, y, z]形式 w1, x1, y1, z1 p w2, x2, y2, z2 q return np.array([ w1*w2 - x1*x2 - y1*y2 - z1*z2, w1*x2 x1*w2 y1*z2 - z1*y2, w1*y2 - x1*z2 y1*w2 z1*x2, w1*z2 x1*y2 - y1*x2 z1*w2 ]) def quat_to_rot(q): 四元数转旋转矩阵 w, x, y, z q return np.array([ [1-2*(y*yz*z), 2*(x*y-z*w), 2*(x*zy*w)], [2*(x*yz*w), 1-2*(x*xz*z), 2*(y*z-x*w)], [2*(x*z-y*w), 2*(y*zx*w), 1-2*(x*xy*y)] ]) def rot_to_rotvec(R): 旋转矩阵转旋转矢量轴角表示 angle np.arccos(np.clip((np.trace(R) - 1) / 2, -1, 1)) if angle 1e-9: return np.zeros(3) axis np.array([ R[2,1] - R[1,2], R[0,2] - R[2,0], R[1,0] - R[0,1] ]) / (2 * np.sin(angle)) return axis * angle def integrate_gyro(gyro_seg, dt): 四元数积分输入角速度段和采样间隔 q np.array([1.0, 0, 0, 0]) for w in gyro_seg: w_norm np.linalg.norm(w) if w_norm 1e-12: angle w_norm * dt axis w / w_norm dq np.array([np.cos(angle/2), *(axis * np.sin(angle/2))]) else: dq np.array([1.0, 0, 0, 0]) q quat_mult(q, dq) q q / np.linalg.norm(q) return q预处理阶段最重要的任务是去异常值和分静止段。加速度计偶发的尖峰脉冲比如桌面微震、电机启动瞬间会严重污染均值和方差估计。我采用的做法是先用中值滤波去掉明显孤立的野点再用滑动窗口方差判断静止/运动状态。def preprocess_accel(acc, fs, window_s0.5, sigma_th3.0): 对加速度数据做去野点和平滑返回清洗后的数据 from scipy.ndimage import median_filter acc_clean median_filter(acc, size(5, 1), modenearest) win int(fs * window_s) std_list [] for i in range(len(acc_clean)): lo max(0, i-win) hi min(len(acc_clean), iwin) std_list.append(acc_clean[lo:hi].std(axis0).mean()) std_arr np.array(std_list) mask std_arr np.percentile(std_arr, 85) * 3 return acc_clean[mask]这里需要注意去野点必须是局部操作中值滤波窗口不能太大否则会把真实的重力方向变化也抹平了。5个样本的窗口在200Hz采样率下只有25ms足够去掉明显的脉冲。4.2 加速度计参数求解目标函数、初始值与优化器选择加速度计标定的优化变量初始值设为A_a为单位阵b_a为零向量。这个初值非常稳定几乎不会陷入局部极小。def acc_residual(params, acc_segments, g9.80665): 加速度计标定残差 params: [b_x, b_y, b_z, A11, A12, A13, A21, A22, A23, A31, A32, A33] b params[:3] A params[3:].reshape(3, 3) res [] for seg in acc_segments: for sample in seg: a_true A (sample - b) res.append(np.linalg.norm(a_true) - g) return np.array(res) def calibrate_accel(acc_segments, g9.80665): 输入为静止段列表每段是(N, 3)数组 x0 np.zeros(12) x0[3:] np.eye(3).reshape(-1) # A I result least_squares( acc_residual, x0, args(acc_segments, g), methodlm, max_nfev200 ) b result.x[:3] A result.x[3:].reshape(3, 3) return A, b这段代码里我用的是LM算法而不是高斯牛顿因为LM在残差较大的初期迭代阶段更稳。加速度计标定的目标函数只有3个残差分量但样本量可能上万所以我在残差计算里没有把每个样本都压扁成单个残差而是直接返回所有样本的模长- g值。这样least_squares内部会处理残差向量的shape。一个埋点A矩阵在优化过程中可能偏离旋转缩放的合理结构但如果只做校正用不要求严格的物理语义A矩阵的自由形式反而能吸收更多误差。我在实验中发现自由形式的A矩阵标定效果比约束为下三角的版本还要好一点这可能是由于PCB工艺带来的轴间交叉耦合比理论模型更复杂。4.3 陀螺仪参数求解旋转段识别与角度残差构造陀螺仪标定的输入数据是完整时间序列首先要识别出每次旋转的起始和结束位置。我采用阀值法计算每帧陀螺仪角速度的模长连续超过阈值比如0.3 rad/s的帧标记为运动段前后各扩展0.5秒作为旋转区间。def detect_rotation_segments(data, fs200.0, gyro_th0.3, min_duration0.3): 识别旋转段返回[(start_idx, end_idx), ...] t data[t].values gyro data[[gx,gy,gz]].values gyro_norm np.linalg.norm(gyro, axis1) moving gyro_norm gyro_th segments [] start None for i, flag in enumerate(moving): if flag and start is None: start i elif not flag and start is not None: if (i - start) / fs min_duration: segments.append((start, i)) start None return segments识别出旋转段后对每段旋转确定旋转前后的静止参考向量取旋转段前1秒的加速度均值归一化为g0旋转段后1秒的加速度均值归一化为g1。然后构造参考旋转和陀螺仪积分的残差。def gyro_residual(params, segments_data, fs, g_vec_normTrue): 陀螺仪标定残差 params: [b_x, b_y, b_z, A11, A12, A13, A21, A22, A23, A31, A32, A33] segments_data: 每段包含(gyro_seg, g0, g1, dt) b params[:3] A params[3:].reshape(3, 3) res [] for gyro_seg, g0, g1, dt in segments_data: # 校正角速度 w_corrected np.array([A (w - b) for w in gyro_seg]) # 陀螺仪积分 q integrate_gyro(w_corrected, dt) R_gyro quat_to_rot(q) # 加速度计参考旋转从g0到g1的最短旋转 u0 g0 / np.linalg.norm(g0) u1 g1 / np.linalg.norm(g1) axis np.cross(u0, u1) axis_norm np.linalg.norm(axis) if axis_norm 1e-8: axis_unit axis / axis_norm angle np.arctan2(axis_norm, np.dot(u0, u1)) q_ref np.array([np.cos(angle/2), *(axis_unit * np.sin(angle/2))]) else: q_ref np.array([1.0, 0, 0, 0]) R_ref quat_to_rot(q_ref) # 误差旋转矩阵陀螺仪积分结果应该等于参考旋转 err_R R_gyro R_ref.T res.extend(rot_to_rotvec(err_R)) return np.array(res) def calibrate_gyro(gyro_segments, g0_list, g1_list, fs): 陀螺仪标定主函数 segments_data [] for i, (gyro_seg, g0, g1) in enumerate(zip(gyro_segments, g0_list, g1_list)): dt 1.0 / fs segments_data.append((gyro_seg, g0, g1, dt)) x0 np.zeros(12) x0[3:] np.eye(3).reshape(-1) # A I result least_squares( gyro_residual, x0, args(segments_data, fs), methodlm, max_nfev200 ) b result.x[:3] A result.x[3:].reshape(3, 3) return A, b这里有一个关键点陀螺仪残差用的是旋转矢量而不是旋转角度标量。每个旋转段提供3个残差分量rx, ry, rz比单一角度残差的信息量更丰富对安装误差和比例因子各轴之间的耦合辨识也更充分。为什么不用旋转矩阵9个元素全部作为残差那样会引入旋转矩阵的冗余约束导致雅可比条件数变差LM迭代效率反而下降。4.4 标定结果验证模长误差与零漂测试标定完不能直接收工必须验证。我的验证分三层第一层是加速度计静止模长测试。重新采集一组静态数据用标定参数补偿后计算模长与g的偏差。合格标准是均值偏差小于0.02 m/s²标准差小于0.05 m/s²。第二层是陀螺仪静止零偏测试。把标定后的陀螺仪放在桌面上静置2分钟计算平均输出。理论上三轴都应该接近0。常见的现象是加速度计标定得很好但陀螺仪零偏还有残留这通常是因为旋转段数量不够、覆盖方向不全面。第三层是姿态漂移测试。静止状态下对陀螺仪角速度积分120秒观察Yaw漂移。原始数据可能漂8°标定后如果还有2°以上的漂移需要检查是不是旋转段覆盖不完整或者陀螺仪温度漂移太严重。def verify_calibration(acc_test, gyro_test, A, b, fs200.0): 简单验证加速度计模长误差 陀螺仪静止零偏 120秒Yaw漂移 # 加速度计验证 acc_corrected np.array([A (a - b_[:3]) for a in acc_test]) acc_norm np.linalg.norm(acc_corrected, axis1) acc_err np.mean(acc_norm) - 9.80665 # 陀螺仪零偏 gyro_bias np.mean(gyro_test, axis0) - b[:3] # 120秒漂移 q np.array([1.0, 0, 0, 0]) n int(120 * fs) gyro_seg gyro_test[:n] for w in gyro_seg: w_corr A (w - b[:3]) w_norm np.linalg.norm(w_corr) if w_norm 1e-12: angle w_norm / fs axis w_corr / w_norm dq np.array([np.cos(angle/2), *(axis * np.sin(angle/2))]) else: dq np.array([1.0, 0, 0, 0]) q quat_mult(q, dq) q q / np.linalg.norm(q) yaw_drift np.arctan2(2*(q[0]*q[3] q[1]*q[2]), 1 - 2*(q[2]*q[2] q[3]*q[3])) return { acc_mean_err: acc_err, gyro_bias: gyro_bias, yaw_drift_120s: np.degrees(yaw_drift) }这段验证代码里Yaw漂移我直接用了四元数转欧拉角的公式。实际工程中Yaw的定义依赖坐标系的约定不同约定下符号可能不一样但幅度评估完全够用。5. 实测效果与常见坑标定结果怎么评判才算合格5.1 标定结果的评判标准看到什么数字才算合格很多朋友标定完第一件事是看参数值本身比如A矩阵对角线是不是接近1、零偏是不是接近0。我的建议是别看参数看数据。参数只是中间产物衡量标定效果的唯一标准是补偿后的数据质量。我在多块IMU上测试后总结了以下参考区间验证项优秀合格不合格静止加速度模长均值误差 0.01 m/s² 0.03 m/s² 0.05 m/s²静止加速度模长标准差 0.02 m/s² 0.05 m/s² 0.10 m/s²补偿后陀螺仪静止零偏 0.005 rad/s 0.01 rad/s 0.02 rad/s静止120秒Yaw漂移 0.5° 2° 5°这里要特别说明Yaw漂移并不完全是标定能解决的。即使标定做到优秀陀螺仪的随机游走Angle Random Walk依然存在这是MEMS器件的物理极限。标定解决的是常值零偏、比例因子和安装误差随机游走只能通过更高端的器件或者外部辅助传感器来抑制。如果你的标定后数据在合格和不合格之间最常见的两个原因一是旋转段覆盖不全某个轴的角速度激励不够导致比例因子没辨识出来二是加速度计静止段数据里有手抖或桌面微震把零偏估计拉偏了。5.2 六个最容易翻车的细节与解决办法第一个坑是180度旋转导致的退化。前面提过当旋转角度接近180度时g0和g1几乎反向axis g0 × g1的模长趋近于0旋转轴的方向变得极不稳定。我一开始设计数据采集方案时有几次是从平放直接翻到倒扣翻转正好180度结果陀螺仪标定的雅可比矩阵近似奇异。后来把所有翻转都控制在60到120度之间问题就消失了。第二个坑是采集时把模块放在手上。手上的肌肉颤动虽然细微但对MEMS加速度计来说已经足够产生噪声。更糟糕的是手的温度会持续给模块加热导致零偏缓慢漂移。正确做法是用双面胶固定到木质或塑料台面上既稳定又散热均匀。第三个坑是忽略静止段与旋转段的边界。分割旋转段时如果直接在检测到运动标记的瞬间切割会把旋转刚开始时的动态加速度混入静止参考向量导致g0或g1不准。解决方法是每次旋转开始前故意静置3秒以上检测到运动后向前多取1秒数据作为静止参考窗口旋转结束后也等1秒再取后参考。第四个坑是采样率太低。如果IMU采样率低于100Hz快速翻转时陀螺仪积分的离散化误差会急剧增大。我用200Hz采样旋转90度耗时1秒左右相当于200个样本积分离散化误差可以忽略。如果你的传感器只能输出50Hz翻转速度一定要放慢最好2秒以上完成90度。第五个坑是加速度计标定和陀螺仪标定分开采集但环境条件不同。比如加速度计标定在室内25度完成陀螺仪标定在室外10度完成两组数据的温度基线不一致导致联合使用时参数不匹配。建议一次标定流程内连续完成中间不要断电、不要换环境。第六个坑是过度相信残差数值。LM算法给出的残差收敛到很小并不代表标定一定成功。如果某个轴在采集过程中没有充分旋转激励优化器会把该轴的参数推到不合理的值来硬凑残差。所以每次标定完都要人工检查A矩阵对角线是否在0.9~1.1之间非对角线元素是否明显小于对角线。如果发现某个非对角线元素异常大大概率是激励不足而不是安装误差真的那么大。5.3 一个真实的标定前后对比最后放一组我今天刚跑的实测数据。模块是ICM20602采样率200Hz按上述流程采集六个静止面每个60秒8段翻转。标定后验证结果如下这块模块的原始静止模长均值是9.68标定后变成9.803残差标准差从0.13降到0.024。陀螺仪z轴零偏从-0.023 rad/s降到-0.0012 rad/s。120秒静止Yaw漂移从3.7°降到了0.8°。有意思的是标定参数里A矩阵的非对角元素并不算小最大的达到了0.014说明这枚芯片的轴间耦合确实存在接近0.8°的安装误差。如果不做这一步后续就算做Lidar-IMU外参标定这些误差也会被外参吸收一部分但你换一个运动场景外参就又不对了。最后再提醒一句这个方法标定出来的零偏属于当前温度下的零偏。MEMS的零偏随温度变化是硬伤如果你在室外低温或者设备发热严重的场景使用最好在对应温度下重新标定或者自己做一个小型的温度补偿表。我个人的习惯是每次换环境、换设备都先跑一遍这十几分钟的流程成本很低收益却非常直接。说到底IMU标定并不神秘关键是理解误差从哪来、物理参考是什么、数据怎么合理地喂给优化器。把这三点想透了你也能根据自己的传感器灵活调出一套好用的标定流程。 IMU标定实战：无外部设备、无需转台的Python完整实现
如果你玩过自组四轴、轮足机器人或者激光雷达SLAM大概率遇到过这种情况IMU买回来直接怼进系统姿态解算出来的Roll和Pitch还行Yaw却像喝醉了一样慢慢飘或者静止不动加速度计读数的模长一会儿9.6一会儿10.2。这时候懂行的人会提醒你——先标定再谈融合。但标定这件事说容易也容易说难也难。传统方案绕不开精密转台一套入门级的三轴转台价格够买好几台飞控就算借到转台装夹、对中、规划角速度轨迹每一步都在消耗耐心。直到我重新翻出Tedaldi、Pretto和Menegatti在ICRA 2014上发表的那篇经典论文《A Robust and Easy to Implement Method for IMU Calibration without External Equipments》以及作者后来开源的imu_tk工具才意识到原来在绝大多数工程场景下标定IMU根本不需要什么昂贵设备只需要一个水平桌面和几分钟的耐心。这篇就是用Python把论文的方法从零复现了一遍的完整记录。内容包括误差模型怎么建、六个静止方位怎么采、手动旋转数据怎么用、优化目标怎么设以及我自己实测踩过的几个大坑。如果你手里有一块MPU6050、ICM20602或者任何六轴模块又不想写邮件求人帮忙标定这篇应该能帮你省下不少时间。这里需要先解释一下标题里without External Equipments的含义。它并不是说不需要任何工具而是说不需要转台、激光跟踪仪这类高精度外部参考。整个标定过程只依赖两个物理事实一是静止时加速度计的输出模长必然等于当地重力加速度二是重力方向在空间中固定指地。利用这两条最基本的物理规律就能把加速度计和陀螺仪的误差参数全部反解出来。1. 不标定就上机IMU的出厂素颜到底有多不靠谱1.1 三类误差源零偏、比例因子和安装误差MEMS IMU的误差主要分三类这三类也是整个标定过程要解决的三个问题。第一类是零偏也叫Bias。静止时加速度计理论上应该输出0陀螺仪理论上应该输出0但实际芯片制造工艺和封装应力决定了它不可能输出精确的0。有的模块上电后加速度计三个轴分别读出0.2、-0.15、9.62这中间的偏差就是零偏。陀螺仪的零偏更明显典型MPU6050在室温下的零偏可能在0.05~0.2 rad/s之间波动如果不做任何处理直接用一秒就偏好几度几分钟后Yaw飘到没法看。第二类是比例因子误差Scale Factor。ADC读到的原始量纲和物理量纲之间的转换系数并不是完美的厂家datasheet上给的是典型值比如加速度计±2g量程对应的灵敏度16384 LSB/g但实际芯片可能偏了0.5%~1%。这个偏差在静止小加速度下影响不大但在车辆急加速、机器人急转这种动态场景下会被放大。第三类是安装误差Misalignment。三轴MEMS芯片在PCB上焊接时不可能做到绝对正交三个敏感轴之间可能有0.1°~1°的夹角偏差。这个误差在高动态场景下同样会被放大而且它和比例因子误差在数学模型上高度耦合必须放在同一个矩阵里一起求。这三类误差综合作用的结果就是静止时加速度计模长不在9.8附近、动态时陀螺仪积分出来的轨迹严重偏离真实路径。更麻烦的是它们之间会互相掩盖光看某一个轴的原始数据很难判断问题出在哪个环节。1.2 不标定直接用的后果一组实测日志我拿一块ICM20602六轴模块做了个对比实验不标定直接上电采样率200Hz静止放在桌面上记录一分钟数据。结果如下指标原始数据表现标定后表现加速度计模长均值9.72 m/s²9.806 m/s²模长波动范围±0.35 m/s²±0.03 m/s²三轴加速度零偏估计模长未补偿0.021 m/s²静止陀螺仪零偏z轴-0.08 rad/s0.001 rad/s静止120秒Yaw漂移积分约8.2°约1.1°这里的加速度计模长波动±0.35算是比较差的个体但市面上便宜的模块大概率就是这个水平。最致命的是Yaw漂移8.2°的漂移意味着做任何航向相关的东西都只能是空想。有人可能会说我用卡尔曼滤波融合磁力计不就行了但磁力计本身也有一堆校正问题而且室内钢筋结构、电机磁场都会干扰它把IMU内参先标好至少能保证姿态解算的底子是干净的。很多做Lidar-IMU外参标定、相机-IMU联合标定的人会问外参标定时需不需要先把内参标好我的回答是内参不标外参标出来也不稳。因为外参标定依赖IMU积分给出的运动轨迹内参误差会被直接投影到外参残差里最后标出来的旋转矩阵和位移向量看起来收敛了换个场景又偏了。2. 论文核心思路拆解重力当标准砝码加速度计当旋转真值2.1 加速度计标定的数学本质9参数模型的模长约束论文的核心思路之一是把重力加速度当作一个天然、免费、处处存在的标准砝码。加速度计静止时测到的物理量就是重力加速度不管IMU朝向如何、放在哪儿模长必须等于当地重力加速度g这个约束和设备本身的质量没有任何关系。用数学语言描述误差模型a_m T_a * a_true b_a noise其中a_m是读数a_true是真实加速度b_a是零偏向量T_a是3x3矩阵包含比例因子和安装误差。在论文的标准模型里T_a取上三角或者下三角形式一共6个自由度加上3个零偏总共9个待求参数。标定的目标函数非常直白min Σ || T_a^(-1) * (a_m - b_a) ||^2 - g^2这里对每一组静止样本求补偿后的加速度模长让它尽可能接近g。正因为约束是模长等于g我们完全不需要知道IMU在这个静止姿态下具体朝向哪里所以不需要外部设备提供姿态真值。6个不同的静止方位本质上就是让重力矢量在IMU的三轴坐标系下产生6种不同的投影组合从而把9个参数全部激励出来。我在实现时做了一个等价变换把T_a^(-1)整体当作优化变量之一直接令a_true A_a * (a_m - b_a)然后用||a_true|| g做约束。这样做的好处是不用在代码里反复做矩阵求逆优化变量的数量不变初值也更直观A_a取单位阵。2.2 陀螺仪标定的巧妙之处用加速度计充当旋转真值陀螺仪标定比加速度计标定麻烦得多原因在于角速度的真值很难获得。转台能提供精确角速度但转台价格摆在那里。论文的解决办法是把加速度计当作旋转的真值来源。思路是这样的如果IMU从静止状态A经过一个旋转到达静止状态B那么旋转前后的重力方向在IMU坐标系下的表示必然发生变化。这个变化可以用加速度计在两个静止段的测量值分别估计出来。同时陀螺仪在旋转段积分的姿态变化也应该等于这个变化。两边一对照就能构建出陀螺仪的误差模型约束。具体来说旋转前静止段取加速度计均值并归一化得到单位向量g0。旋转后静止段取加速度计均值并归一化得到单位向量g1。用公式axis g0 × g1、angle atan2(||axis||, g0·g1)构造出参考旋转这里用到了重力对齐的基本思想。用陀螺仪读数做四元数积分得到实测旋转。两者差一个旋转矩阵误差优化目标就是让所有旋转段上的误差最小。这样就能把陀螺仪的零偏、比例因子和安装误差全部估计出来。需要强调一点整个过程中没有使用任何外部运动捕捉系统或转台所有的真值都来自重力和已经标定完成的加速度计。这就是论文标题里without External Equipments的真正含义。2.3 为什么这个方法Robust异常值剔除机制论文标题里还有个关键词是Robust。实际采集数据时手抖、桌面震动、模块松动都会引入异常值。如果不做任何处理一个离群样本就可能把优化结果拉偏。论文的处理策略是在优化迭代过程中动态剔除残差过大的样本。具体实现上我在每次迭代后计算所有样本残差的标准差σ把残差绝对值超过3σ的样本剔除掉然后用剩余样本继续迭代。这个策略的执行顺序很关键必须先用当前参数算残差再决定剔除哪些样本否则初值很烂的时候可能把所有样本都误杀了。3. 数据采集实操六面静止与手动旋转的完整流程3.1 加速度计标定采集六面静止法的顺序与时长加速度计标定需要六组静止数据分别对应六个方位让重力分别作用在三个轴的正负方向上。我习惯的顺序如下序号放置方式对应重力方向1平放正面朝上z轴朝上2倒扣正面朝下z轴朝下3侧立x轴朝上x轴朝上4侧立x轴朝下x轴朝下5侧立y轴朝上y轴朝上6侧立y轴朝下y轴朝下每个面建议采集60秒以上。放置时要用双面胶或者橡皮泥固定模块不要用手扶着因为手部的微小抖动会污染加速度计数据。模块要放在水平的桌面上虽然这个方法的约束不要求桌面绝对水平——桌面稍微倾斜一点重力矢量在IMU坐标系下的投影依然等于g优化目标里模长等于g的约束照样成立——但水平放可以减少后续陀螺仪标定时的麻烦。60秒的采集可以分成两段前10秒让模块完全稳定中间40秒作为有效数据最后10秒用于操作切换。我实际写代码时会把整个采集过程的时间戳记录下来后续按时间段切分静止段而不是靠算法自动识别静止段。原因很简单六面放置时切面的过程也是数据的一部分与其让算法区分运动段和静止段不如采集时控制好节奏减少无关的运动数据。3.2 陀螺仪标定采集旋转段设计原则陀螺仪标定需要旋转激励。这里的关键不是随便转几圈而是要让旋转覆盖三个轴的角速度激励而且旋转前后都要有静置段。我推荐的做法是每段旋转都设计成从一个面翻到另一个面平放静止3-5秒。沿着水平轴快速但平缓地翻转90°让IMU侧立x轴朝上。静止3-5秒。再沿着另一个轴翻转回平放或者继续翻到下一个姿态。旋转角度控制在60°~120°之间不要转180°。原因我在代码实现章节会详细解释——180°旋转后重力方向反向从两个单位向量无法唯一确定旋转轴优化时这个数据段会退化。还有一个很多人会踩的设计误区绕竖直轴原地旋转。比如把IMU平放在桌面上然后像拧瓶盖一样绕z轴旋转这种情况下重力方向在IMU坐标系下几乎不变加速度计完全起不到参考作用。正确的做法是让旋转轴尽量与重力方向垂直这样重力方向变化最大参考信息最充足。完整的陀螺仪标定建议采集8~10段旋转覆盖绕三个不同水平轴的翻转每段旋转前静止3~5秒旋转后静止3~5秒。3.3 采集环境的几个细节采集环境对数据质量的影响被很多人低估。首先是桌面要稳固不要放在机箱上面或者有人在旁边走来走去的地方。其次是模块供电要稳定USB供电和电池供电的噪声特征完全不同。再者是温度MEMS IMU的零偏对温度很敏感刚从室内拿到室外前几分钟的数据往往会明显漂移所以最好让模块上电预热10分钟再开始采集。4. 代码实现从CSV原始数据到9参数标定的完整链路4.1 数据格式与预处理去异常值、提取静止段原始数据我按CSV格式记录字段为t, ax, ay, az, gx, gy, gz加速度单位m/s²角速度单位rad/s时间戳单位秒。采集时统一先启动串口读取程序开始记录后再摆模块整个过程连续记录不人为切分文件。import numpy as np import pandas as pd from scipy.optimize import least_squares def quat_mult(p, q): 四元数乘法p和q都是[w, x, y, z]形式 w1, x1, y1, z1 p w2, x2, y2, z2 q return np.array([ w1*w2 - x1*x2 - y1*y2 - z1*z2, w1*x2 x1*w2 y1*z2 - z1*y2, w1*y2 - x1*z2 y1*w2 z1*x2, w1*z2 x1*y2 - y1*x2 z1*w2 ]) def quat_to_rot(q): 四元数转旋转矩阵 w, x, y, z q return np.array([ [1-2*(y*yz*z), 2*(x*y-z*w), 2*(x*zy*w)], [2*(x*yz*w), 1-2*(x*xz*z), 2*(y*z-x*w)], [2*(x*z-y*w), 2*(y*zx*w), 1-2*(x*xy*y)] ]) def rot_to_rotvec(R): 旋转矩阵转旋转矢量轴角表示 angle np.arccos(np.clip((np.trace(R) - 1) / 2, -1, 1)) if angle 1e-9: return np.zeros(3) axis np.array([ R[2,1] - R[1,2], R[0,2] - R[2,0], R[1,0] - R[0,1] ]) / (2 * np.sin(angle)) return axis * angle def integrate_gyro(gyro_seg, dt): 四元数积分输入角速度段和采样间隔 q np.array([1.0, 0, 0, 0]) for w in gyro_seg: w_norm np.linalg.norm(w) if w_norm 1e-12: angle w_norm * dt axis w / w_norm dq np.array([np.cos(angle/2), *(axis * np.sin(angle/2))]) else: dq np.array([1.0, 0, 0, 0]) q quat_mult(q, dq) q q / np.linalg.norm(q) return q预处理阶段最重要的任务是去异常值和分静止段。加速度计偶发的尖峰脉冲比如桌面微震、电机启动瞬间会严重污染均值和方差估计。我采用的做法是先用中值滤波去掉明显孤立的野点再用滑动窗口方差判断静止/运动状态。def preprocess_accel(acc, fs, window_s0.5, sigma_th3.0): 对加速度数据做去野点和平滑返回清洗后的数据 from scipy.ndimage import median_filter acc_clean median_filter(acc, size(5, 1), modenearest) win int(fs * window_s) std_list [] for i in range(len(acc_clean)): lo max(0, i-win) hi min(len(acc_clean), iwin) std_list.append(acc_clean[lo:hi].std(axis0).mean()) std_arr np.array(std_list) mask std_arr np.percentile(std_arr, 85) * 3 return acc_clean[mask]这里需要注意去野点必须是局部操作中值滤波窗口不能太大否则会把真实的重力方向变化也抹平了。5个样本的窗口在200Hz采样率下只有25ms足够去掉明显的脉冲。4.2 加速度计参数求解目标函数、初始值与优化器选择加速度计标定的优化变量初始值设为A_a为单位阵b_a为零向量。这个初值非常稳定几乎不会陷入局部极小。def acc_residual(params, acc_segments, g9.80665): 加速度计标定残差 params: [b_x, b_y, b_z, A11, A12, A13, A21, A22, A23, A31, A32, A33] b params[:3] A params[3:].reshape(3, 3) res [] for seg in acc_segments: for sample in seg: a_true A (sample - b) res.append(np.linalg.norm(a_true) - g) return np.array(res) def calibrate_accel(acc_segments, g9.80665): 输入为静止段列表每段是(N, 3)数组 x0 np.zeros(12) x0[3:] np.eye(3).reshape(-1) # A I result least_squares( acc_residual, x0, args(acc_segments, g), methodlm, max_nfev200 ) b result.x[:3] A result.x[3:].reshape(3, 3) return A, b这段代码里我用的是LM算法而不是高斯牛顿因为LM在残差较大的初期迭代阶段更稳。加速度计标定的目标函数只有3个残差分量但样本量可能上万所以我在残差计算里没有把每个样本都压扁成单个残差而是直接返回所有样本的模长- g值。这样least_squares内部会处理残差向量的shape。一个埋点A矩阵在优化过程中可能偏离旋转缩放的合理结构但如果只做校正用不要求严格的物理语义A矩阵的自由形式反而能吸收更多误差。我在实验中发现自由形式的A矩阵标定效果比约束为下三角的版本还要好一点这可能是由于PCB工艺带来的轴间交叉耦合比理论模型更复杂。4.3 陀螺仪参数求解旋转段识别与角度残差构造陀螺仪标定的输入数据是完整时间序列首先要识别出每次旋转的起始和结束位置。我采用阀值法计算每帧陀螺仪角速度的模长连续超过阈值比如0.3 rad/s的帧标记为运动段前后各扩展0.5秒作为旋转区间。def detect_rotation_segments(data, fs200.0, gyro_th0.3, min_duration0.3): 识别旋转段返回[(start_idx, end_idx), ...] t data[t].values gyro data[[gx,gy,gz]].values gyro_norm np.linalg.norm(gyro, axis1) moving gyro_norm gyro_th segments [] start None for i, flag in enumerate(moving): if flag and start is None: start i elif not flag and start is not None: if (i - start) / fs min_duration: segments.append((start, i)) start None return segments识别出旋转段后对每段旋转确定旋转前后的静止参考向量取旋转段前1秒的加速度均值归一化为g0旋转段后1秒的加速度均值归一化为g1。然后构造参考旋转和陀螺仪积分的残差。def gyro_residual(params, segments_data, fs, g_vec_normTrue): 陀螺仪标定残差 params: [b_x, b_y, b_z, A11, A12, A13, A21, A22, A23, A31, A32, A33] segments_data: 每段包含(gyro_seg, g0, g1, dt) b params[:3] A params[3:].reshape(3, 3) res [] for gyro_seg, g0, g1, dt in segments_data: # 校正角速度 w_corrected np.array([A (w - b) for w in gyro_seg]) # 陀螺仪积分 q integrate_gyro(w_corrected, dt) R_gyro quat_to_rot(q) # 加速度计参考旋转从g0到g1的最短旋转 u0 g0 / np.linalg.norm(g0) u1 g1 / np.linalg.norm(g1) axis np.cross(u0, u1) axis_norm np.linalg.norm(axis) if axis_norm 1e-8: axis_unit axis / axis_norm angle np.arctan2(axis_norm, np.dot(u0, u1)) q_ref np.array([np.cos(angle/2), *(axis_unit * np.sin(angle/2))]) else: q_ref np.array([1.0, 0, 0, 0]) R_ref quat_to_rot(q_ref) # 误差旋转矩阵陀螺仪积分结果应该等于参考旋转 err_R R_gyro R_ref.T res.extend(rot_to_rotvec(err_R)) return np.array(res) def calibrate_gyro(gyro_segments, g0_list, g1_list, fs): 陀螺仪标定主函数 segments_data [] for i, (gyro_seg, g0, g1) in enumerate(zip(gyro_segments, g0_list, g1_list)): dt 1.0 / fs segments_data.append((gyro_seg, g0, g1, dt)) x0 np.zeros(12) x0[3:] np.eye(3).reshape(-1) # A I result least_squares( gyro_residual, x0, args(segments_data, fs), methodlm, max_nfev200 ) b result.x[:3] A result.x[3:].reshape(3, 3) return A, b这里有一个关键点陀螺仪残差用的是旋转矢量而不是旋转角度标量。每个旋转段提供3个残差分量rx, ry, rz比单一角度残差的信息量更丰富对安装误差和比例因子各轴之间的耦合辨识也更充分。为什么不用旋转矩阵9个元素全部作为残差那样会引入旋转矩阵的冗余约束导致雅可比条件数变差LM迭代效率反而下降。4.4 标定结果验证模长误差与零漂测试标定完不能直接收工必须验证。我的验证分三层第一层是加速度计静止模长测试。重新采集一组静态数据用标定参数补偿后计算模长与g的偏差。合格标准是均值偏差小于0.02 m/s²标准差小于0.05 m/s²。第二层是陀螺仪静止零偏测试。把标定后的陀螺仪放在桌面上静置2分钟计算平均输出。理论上三轴都应该接近0。常见的现象是加速度计标定得很好但陀螺仪零偏还有残留这通常是因为旋转段数量不够、覆盖方向不全面。第三层是姿态漂移测试。静止状态下对陀螺仪角速度积分120秒观察Yaw漂移。原始数据可能漂8°标定后如果还有2°以上的漂移需要检查是不是旋转段覆盖不完整或者陀螺仪温度漂移太严重。def verify_calibration(acc_test, gyro_test, A, b, fs200.0): 简单验证加速度计模长误差 陀螺仪静止零偏 120秒Yaw漂移 # 加速度计验证 acc_corrected np.array([A (a - b_[:3]) for a in acc_test]) acc_norm np.linalg.norm(acc_corrected, axis1) acc_err np.mean(acc_norm) - 9.80665 # 陀螺仪零偏 gyro_bias np.mean(gyro_test, axis0) - b[:3] # 120秒漂移 q np.array([1.0, 0, 0, 0]) n int(120 * fs) gyro_seg gyro_test[:n] for w in gyro_seg: w_corr A (w - b[:3]) w_norm np.linalg.norm(w_corr) if w_norm 1e-12: angle w_norm / fs axis w_corr / w_norm dq np.array([np.cos(angle/2), *(axis * np.sin(angle/2))]) else: dq np.array([1.0, 0, 0, 0]) q quat_mult(q, dq) q q / np.linalg.norm(q) yaw_drift np.arctan2(2*(q[0]*q[3] q[1]*q[2]), 1 - 2*(q[2]*q[2] q[3]*q[3])) return { acc_mean_err: acc_err, gyro_bias: gyro_bias, yaw_drift_120s: np.degrees(yaw_drift) }这段验证代码里Yaw漂移我直接用了四元数转欧拉角的公式。实际工程中Yaw的定义依赖坐标系的约定不同约定下符号可能不一样但幅度评估完全够用。5. 实测效果与常见坑标定结果怎么评判才算合格5.1 标定结果的评判标准看到什么数字才算合格很多朋友标定完第一件事是看参数值本身比如A矩阵对角线是不是接近1、零偏是不是接近0。我的建议是别看参数看数据。参数只是中间产物衡量标定效果的唯一标准是补偿后的数据质量。我在多块IMU上测试后总结了以下参考区间验证项优秀合格不合格静止加速度模长均值误差 0.01 m/s² 0.03 m/s² 0.05 m/s²静止加速度模长标准差 0.02 m/s² 0.05 m/s² 0.10 m/s²补偿后陀螺仪静止零偏 0.005 rad/s 0.01 rad/s 0.02 rad/s静止120秒Yaw漂移 0.5° 2° 5°这里要特别说明Yaw漂移并不完全是标定能解决的。即使标定做到优秀陀螺仪的随机游走Angle Random Walk依然存在这是MEMS器件的物理极限。标定解决的是常值零偏、比例因子和安装误差随机游走只能通过更高端的器件或者外部辅助传感器来抑制。如果你的标定后数据在合格和不合格之间最常见的两个原因一是旋转段覆盖不全某个轴的角速度激励不够导致比例因子没辨识出来二是加速度计静止段数据里有手抖或桌面微震把零偏估计拉偏了。5.2 六个最容易翻车的细节与解决办法第一个坑是180度旋转导致的退化。前面提过当旋转角度接近180度时g0和g1几乎反向axis g0 × g1的模长趋近于0旋转轴的方向变得极不稳定。我一开始设计数据采集方案时有几次是从平放直接翻到倒扣翻转正好180度结果陀螺仪标定的雅可比矩阵近似奇异。后来把所有翻转都控制在60到120度之间问题就消失了。第二个坑是采集时把模块放在手上。手上的肌肉颤动虽然细微但对MEMS加速度计来说已经足够产生噪声。更糟糕的是手的温度会持续给模块加热导致零偏缓慢漂移。正确做法是用双面胶固定到木质或塑料台面上既稳定又散热均匀。第三个坑是忽略静止段与旋转段的边界。分割旋转段时如果直接在检测到运动标记的瞬间切割会把旋转刚开始时的动态加速度混入静止参考向量导致g0或g1不准。解决方法是每次旋转开始前故意静置3秒以上检测到运动后向前多取1秒数据作为静止参考窗口旋转结束后也等1秒再取后参考。第四个坑是采样率太低。如果IMU采样率低于100Hz快速翻转时陀螺仪积分的离散化误差会急剧增大。我用200Hz采样旋转90度耗时1秒左右相当于200个样本积分离散化误差可以忽略。如果你的传感器只能输出50Hz翻转速度一定要放慢最好2秒以上完成90度。第五个坑是加速度计标定和陀螺仪标定分开采集但环境条件不同。比如加速度计标定在室内25度完成陀螺仪标定在室外10度完成两组数据的温度基线不一致导致联合使用时参数不匹配。建议一次标定流程内连续完成中间不要断电、不要换环境。第六个坑是过度相信残差数值。LM算法给出的残差收敛到很小并不代表标定一定成功。如果某个轴在采集过程中没有充分旋转激励优化器会把该轴的参数推到不合理的值来硬凑残差。所以每次标定完都要人工检查A矩阵对角线是否在0.9~1.1之间非对角线元素是否明显小于对角线。如果发现某个非对角线元素异常大大概率是激励不足而不是安装误差真的那么大。5.3 一个真实的标定前后对比最后放一组我今天刚跑的实测数据。模块是ICM20602采样率200Hz按上述流程采集六个静止面每个60秒8段翻转。标定后验证结果如下这块模块的原始静止模长均值是9.68标定后变成9.803残差标准差从0.13降到0.024。陀螺仪z轴零偏从-0.023 rad/s降到-0.0012 rad/s。120秒静止Yaw漂移从3.7°降到了0.8°。有意思的是标定参数里A矩阵的非对角元素并不算小最大的达到了0.014说明这枚芯片的轴间耦合确实存在接近0.8°的安装误差。如果不做这一步后续就算做Lidar-IMU外参标定这些误差也会被外参吸收一部分但你换一个运动场景外参就又不对了。最后再提醒一句这个方法标定出来的零偏属于当前温度下的零偏。MEMS的零偏随温度变化是硬伤如果你在室外低温或者设备发热严重的场景使用最好在对应温度下重新标定或者自己做一个小型的温度补偿表。我个人的习惯是每次换环境、换设备都先跑一遍这十几分钟的流程成本很低收益却非常直接。说到底IMU标定并不神秘关键是理解误差从哪来、物理参考是什么、数据怎么合理地喂给优化器。把这三点想透了你也能根据自己的传感器灵活调出一套好用的标定流程。