简介面向惯性导航与组合导航初学者的Matlab实验源码包对应车载惯性/GPS/里程计组合导航场景。数据采集自车辆静止晃动、10分钟后行驶的实测过程代码含粗对准与精对准两阶段可分别输出惯性导航、惯性/GPS组合、惯性/里程计组合三种结果。资源共16个文件全部为m脚本涉及初始对准、姿态四元数/欧拉角/方向余弦矩阵变换、卡尔曼滤波GPS修正与里程计修正等核心模块包体仅12KB结构紧凑、易于逐文件阅读。已有728人学习适合需要理解组合导航解算流程、开展课程实验或科研预研的读者。利用该资源可快速搭建捷联惯导仿真框架掌握IMU、GPS与里程计数据融合的编程实现也可参考其粗对准与精对准分段处理思路用于实测数据预处理。1. 拿到 exp3.zip 之前先想清楚组合导航到底要解决什么轮式里程计在平整地面足够诚实可一旦打滑、下坎或者悬挂大幅压缩轮速立刻失真纯 IMU 积分短时看着平滑但加速度计和陀螺仪的零偏随时间累积yaw 角度肉眼可见地慢漂。惯导里程计组合导航解决的就是这一对“互相拆台”的传感器如何在同一个状态估计框架里自洽IMU 负责高频递推里程计负责把位置和航向拉回真实值。exp3.zip 这类实验数据包通常就是按这个思路采集的 IMU 原始数据加里程计计数值需要你自行完成时间同步、标定和滤波融合。适合正在做机器人定位、车辆状态估计或组合导航课程实验的工程师也适合准备做 lidar/相机与 IMU 外参标定却还缺一块“里程计观测”的人。2. 解析 exp3.zip 里 IMU 与里程计数据的最小流程2.1 先看压缩包结构再决定用哪一层数据实验包通常不是一份整理好的 CSV而是“原始 bin 文件 采集说明 若干参考脚本”的组合。我一般拿到 exp3.zip 后先做三件事unzip -l看目录结构du -sh看体积分布再用file判断文本或二进制格式。体积异常大的往往不是 IMU 数据而是图像或激光点云体积小的 bin 多半是高频 IMU 裸数据。unzip -l exp3.zip du -sh exp3/* file exp3/data/imu.bin exp3/data/odom.bin参数说明unzip -l只列目录不释放文件适合先确认是否存在 readmefile能区分 ASCII 文本、二进制和 ROS bag 的容器格式避免后续用错解析库。如果 exp3.zip 里是 ROS bag就不需要自己拆 bin直接用rosbag play配合rostopic echo看话题类型即可。命令行先跑一遍的意义在于快速判断这个包是“干净对齐好的数据集”还是“原始采集数据 你来做标定和融合”。两者的工作量相差一个数量级。2.2 用 Python 把 IMU 加速度与角速度读成可用数组以最常见的二进制格式为例IMU 数据通常按固定字节数存储时间戳 uint64、三轴加速度 float32、三轴角速度 float32外加温度和状态字。读取时要按实际字段宽度切分不要用np.fromfile一把梭因为采集板可能混入丢帧标志或 CRC。import numpy as np import struct def load_imu(path): raw np.fromfile(path, dtypenp.uint8) record_len 8 12 12 4 4 # ts acc gyro temp state n len(raw) // record_len recs raw[:n * record_len].reshape(-1, record_len) ts np.array([struct.unpack_from(Q, r, 0)[0] for r in recs]) # 前 12 字节加速度(float32 x3)随后 12 字节角速度 acc np.array([struct.unpack_from(3f, r, 8) for r in recs]) gyr np.array([struct.unpack_from(3f, r, 20) for r in recs]) ts (ts - ts[0]) * 1e-6 # 转成相对秒 return ts, acc, gyr逻辑说明record_len必须和采集端的结构体严格对应reshape(-1, record_len)先做整帧切分避免unpack_from越界。时间戳先归一化到相对秒后续做时间同步时不容易被 uint64 的大数值干扰。参数说明Q表示小端无符号 64 位3f是小端三个 float32。采集板如果固件不同字段顺序可能是“角速度在前、加速度在后”解析前务必用静态段数据做自检。静止时刻加速度幅值应接近当地重力若出现加速度约等于 0 而角速度很大的怪值大概率是字段顺序写反了。2.3 里程计数据的三种常见格式与时间对齐策略里程计不会像 IMU 那样高频输出。常见格式有三种轮速脉冲计数左右轮各一个 int32、底盘解算后的线速度与角速度、或已经输出位置增量的 odom topic。脉冲格式需要你自己乘上轮周长和减速比换算成速度这一步最容易把单位搞错。import numpy as np def load_odom(path, wheel_r0.125, gear20.0, ticks_per_rev2048): data np.loadtxt(path, delimiter,) # t, left_ticks, right_ticks ts, lticks, rticks data[:, 0], data[:, 1], data[:, 2] # 相邻脉冲差对应轮子转过的圈数 dl np.diff(lticks) / (ticks_per_rev * gear) * (2 * np.pi * wheel_r) dr np.diff(rticks) / (ticks_per_rev * gear) * (2 * np.pi * wheel_r) dt np.diff(ts) vx (dl dr) / (2 * dt) wz (dr - dl) / (np.linalg.norm([0.5, 0.5]) * 1.0) # 轴距需按实车修改 return ts[1:], vx, wz逻辑说明里程计给出的原始量是“计数增量”不是速度先转成弧长增量再除以时间才是速度。wz的分母是轴距代码里用1.0是占位实际必须填左右轮间距否则 yaw 增量整体放大或缩小组合导航里这条观测会被错误地加权。时间对齐的常见做法是线性插值把里程计速度插值到 IMU 时间戳上或者反过来把 IMU 降采样到里程计时刻。我一般保持 IMU 原始高频率里程计观测生成时打上最近的 IMU 时间戳这样在滤波更新时可以直接索引。需要注意里程计本身有接收延迟若数据包里同时提供 CAN 时间戳和系统时间戳优先用采集板时间戳。2.4 用静态段快速估计 IMU 零偏避免把错误偏置喂给滤波器零偏估计是 exp3.zip 处理里成本最低但收益最大的步骤。把数据开头或结尾的一段静止段截出来对陀螺仪三个轴取均值就是角速度零偏加速度均值减去当地重力向量就能得到加速度计偏置。mask (ts 2.0) (ts 8.0) # 取 2s~8s 静止段 acc_bias np.mean(acc[mask], axis0) gyr_bias np.mean(gyr[mask], axis0) g_norm np.linalg.norm(acc_bias) print(facc_bias{acc_bias}, gyr_bias{gyr_bias} deg/s? - {gyr_bias * 180 / np.pi})参数说明静止段长度至少 5 秒太短会混入振动和温漂太长可能混入平台被碰一下的片段。gyr_bias单位如果是弧度每秒后续滤波里所有角速度都要减掉这个偏置后再用否则 yaw 积分会以固定速率线性增长。加速度计偏置在滤波里通常会被状态向量吸收但静止段均值仍值得先看一眼数值过大的话说明传感器出厂标定本身有问题需要先做 Allan 方差分析而不是急着改滤波器参数。3. 惯导里程计组合导航的误差状态模型与量测构建3.1 为什么用误差状态卡尔曼滤波而不是直接融合位置把 IMU 积分的位移和里程计位移直接做加权平均是初学者最容易踩的坑。问题是 IMU 积分位姿的不确定性随时间增长而里程计观测噪声相对稳定加权平均永远给不出“当前到底信谁”只会把两边的误差都留着。误差状态卡尔曼滤波ESKF的思路是把真实状态拆成「标称状态 误差状态」标称状态由 IMU 高速递推误差状态用里程计观测做低频修正。这样修改的是误差而不是直接覆盖积分结果数值上更稳定也方便处理小角度误差。exp3.zip 里的组合导航任务状态向量一般取 15 维位置 p3、速度 v3、姿态四元数 q4、陀螺零偏 bg3、加速度计零偏 ba3。ESKF 里真正被滤波估计的是误差状态 δx维度通常压缩到 15 维的误差形式。误差状态分量含义观测约束来源δp位置误差里程计位置增量δv速度误差里程计速度弱约束δθ姿态误差小角度里程计 yawδbg陀螺零偏误差间接通过 yaw 漂移反馈δba加速度计零偏误差重力与里程计交互观测表格要表达的核心是量测更新不直接约束所有状态。位置误差和姿态误差能直接被里程计修正零偏误差只能靠误差状态的耦合间接收敛这也是为什么滤波跑起来后零偏收敛慢的现象很常见。3.2 ESKF 预测步把 IMU 量测转成标称状态递推预测步没有观测参与直接用 IMU 加速度和角速度做机械编排。代码如下输入为上一帧标称状态、IMU 读数、零偏估计和时间间隔。import numpy as np from scipy.spatial.transform import Rotation as R def eskf_predict(p, v, q, bg, ba, acc, gyr, dt, gnp.array([0, 0, -9.81])): acc_c acc - ba gyr_c gyr - bg # 姿态更新四元数乘以角速度增量对应的小旋转 delta_q R.from_rotvec(gyr_c * dt) q_new (R.from_quat(q) * delta_q).as_quat() # 在导航系中解算加速度 a_nav R.from_quat(q_new).apply(acc_c) g v_new v a_nav * dt p_new p v * dt 0.5 * a_nav * dt * dt # 零偏假设随机游走这里保持不变 return p_new, v_new, q_new, bg, ba逻辑说明gyr_c * dt形成一个小角度旋转用R.from_rotvec转成四元数更新姿态acc_c是比力加到导航系需要旋转到世界系并叠加重力。这版代码把零偏当作随机游走处理没有在预测里加入噪声驱动原型验证够用正式系统要补过程噪声协方差 Q。参数说明重力向量g的符号取决于坐标轴定义一般取东北天坐标时是[0, 0, -9.81]如果数据包的加速度计在静止时读数为正 Z需要确认是比力还是重力表达。dt必须用相邻 IMU 时间戳差不能假设固定采样率否则滤波会在采集丢帧时刻注入错误误差。3.3 里程计观测方程只约束位置和航向里程计能直接约束的是平面位置和航向角 yaw对 pitch、roll 几乎没有约束力。观测方程写作z_pos p_xy - odom_xy z_yaw normalize(yaw_est - yaw_odom)这里的odom_xy是里程计在导航系下的位置。如果 exp3.zip 里里程计给的是速度而不是位置可以把速度积分成位置增量后再做观测也可以直接在速度层面建观测方程。位置层面的观测收敛更快但对积分误差更敏感速度层面更平滑适合底盘速度噪声较大的场景。def eskf_update_odom(p, q, odom_xy, odom_yaw, R_meas, P): yaw_est R.from_quat(q).as_euler(ZYX)[0] z np.array([p[0] - odom_xy[0], p[1] - odom_xy[1], yaw_est - odom_yaw]) z[2] np.arctan2(np.sin(z[2]), np.cos(z[2])) # 角度归一化 H np.zeros((3, 15)) H[0, 0] H[1, 1] 1.0 H[2, 6] 1.0 # 假设误差状态索引 6 是 yaw # 标准卡尔曼更新K P H^T (H P H^T R)^-1 return z, H参数说明R_meas是对角阵位置噪声和 yaw 噪声单位差别很大通常位置给0.05m量级yaw 给0.01rad量级不能混成一个值。arctan2(sin, cos)是角度归一化跳过这一步会在 ±π 附近产生跳变导致滤波发散。yaw 慢漂在 ESKF 里的本质原因在于里程计观测如果只在水平面约束位置车直行时 yaw 和位置耦合较弱陀螺零偏误差难快速收敛yaw 就会缓慢漂移。解决方向有两个一个是提高里程计 yaw 观测频率比如用底盘反馈的转向角另一个是引入 lidar 或视觉的绝对航向观测。3.4 里程计组合中容易被忽略的触发条件IMU 和里程计时间戳如果没有硬件同步通常会有几十到几百毫秒延迟。几十毫秒延迟在低速场景不明显但转弯时位置观测和状态预测错位会造成固定方向的误差。常见做法是标一个固定的时间偏移量在滤波预测时延迟对齐更稳妥的方法是直接把时间偏移当成扩展状态估计出来。另一个常见问题是底盘打滑。轮速里程计在光滑地面或急加速时输出会比实际运动偏大但滤波没法判断打滑会把错误观测当成真实值。工程上常见做法是检测轮速变化率和 IMU 加速度的一致性如果两者矛盾超过阈值暂停里程计更新。如果 |IMU 推算速度 - 里程计速度| 阈值则跳过观测更新这个逻辑写起来很朴素但效果比任何鲁棒核函数都直接。exp3.zip 的数据如果包含急转弯或地面摩擦变化的片段这种触发条件几乎必然被触发。4. lidar/相机与 IMU 外参标定组合导航的前置对齐4.1 为什么组合导航要关心外参而不是直接看数据IMU 和里程计都装在车体上里程计给出的是轮轴处的运动IMU 给出的是传感器自身运动。两者之间的杆臂lever arm如果忽略转弯时会产生虚假的向心加速度位置误差周期性波动。lidar 或相机参与组合导航时外参标定决定点云投影和航向观测的准确性。所以 exp3.zip 如果包含点云或图像数据外参标定是绕不开的一步。常见做法是使用 kalibr 标定相机与 IMU 之间的外参和时延lidar 与 IMU 则常用lidar_imu_calib这类基于手眼标定的工具。4.2 kalibr 相机与 IMU 联合标定的命令与参数kalibr 标定需要一段带棋盘格的运动数据操作要点是“慢速、旋转、充分激励六自由度”。标定命令如下kalibr_calibrate_imu_camera \ --target src/target/aprilgrid_6x6.yaml \ --cam camchain.yaml \ --imu imu.yaml \ --bag dataset.bag \ --time-calibration参数说明--target指向标定板配置棋盘格尺寸必须和实物一致--imu文件里要写明采样率、噪声密度和随机游走kalibr 会据此估计相机与 IMU 之间的T_cam_imu和时延td。--time-calibration必须开启否则固定时延假设会让外参估计带上误差。运行前应rosbag info确认IMU 话题频率稳定如果数据包里有丢帧先剔除再标定。标定结果里重点关注T_cam_imu的平移向量是否和实际安装量级一致旋转角是否有明显不合理值。如果旋转角接近 90 度的整数倍大概率是坐标轴定义问题而不是真实标定结果。4.3 lidar 与 IMU 外参标定的两个关键初始化与迭代优化lidar 与 IMU 标定常用方式是采集一段静止数据估初始外参再用手眼标定公式AX XB求解。原理上lidar 的帧间运动与 IMU 积分运动应一致外参正是这个一致性约束的解。import numpy as np from scipy.spatial.transform import Rotation as R def hand_eye(A_list, B_list): # A_list: lidar 帧间旋转矩阵, B_list: IMU 帧间旋转矩阵 M [] for Ai, Bi in zip(A_list, B_list): Ra, Rb R.from_dcm(Ai[:3,:3]), R.from_dcm(Bi[:3,:3]) r np.kron(Rb.as_dcm().flatten(), Ra.as_dcm().flatten()) M.append(r) M np.array(M) _, _, Vt np.linalg.svd(M) x Vt[-1] Rx R.from_dcm(x.reshape(3,3)).as_dcm() return Rx逻辑说明手眼标定的核心是构造一个线性方程组把外参旋转矩阵作为未知特征向量用 SVD 求最小二乘解。标定的充分条件是采集数据里要有绕多个轴旋转的运动单轴转动会让 M 矩阵秩亏解出来会乱。参数说明np.kron用于构造 Kronecker 积矩阵采集时尽量转大角度、多个方向小于 5 度的微转会直接被噪声淹没。实际工程中手眼标定只能得到相对外参绝对尺度需要靠 lidar 点云匹配或已知尺寸的标定物消解。4.4 IMU 内参标定Allan 方差与噪声参数提取滤波器里的过程噪声 Q 需要陀螺和加速度计的噪声密度与随机游走参数这些参数没有现成值必须从静止数据里用 Allan 方差提取。Allan 方差的基本做法是把静止数据按不同时间长度分段计算每个段均值的方差再画对数-对数曲线。曲线斜率为 -0.5 段的截距是角度随机游走斜率为 0.5 段的截距是速率随机游走。代码如下计算不同积分时间段的 Allan 方差import numpy as np def allan_dev(data, fs, max_cluster1000): N len(data) result [] for m in range(1, max_cluster): n int(fs * m) if n N // 2: break k N // n # 每个集群均值 clusters data[: k * n].reshape(k, n).mean(axis1) ad 0.5 * np.mean(np.diff(clusters) ** 2) result.append([m, np.sqrt(ad)]) return np.array(result) # 陀螺 z 轴静止数据 gyr_z gyr_static[:, 2] out allan_dev(gyr_z, fs200.0)逻辑说明np.diff(clusters)是相邻集群均值的差乘 0.5 后开根号就是 Allan 标准差。随着集群长度增大方差曲线先降后升最低点对应相关时间。注意max_cluster不要太大集群数少于三个时统计意义消失。参数说明静止数据最好是一小时以上少于 30 分钟得到的随机游走段不可靠。工程妥协做法是取 20 分钟静止数据曲线只在低频部分有一点上升趋势也可以接受。得到的角度随机游走和速率随机游走将直接填入滤波器 Q 矩阵的对角块。5. 双足与轮式里程计场景中的 yaw 慢漂验证方法5.1 用 Allan 方差确认漂移来源是仪表噪声还是标定残留yaw 慢漂在 exp3.zip 里很常见。先不要急于把问题归结为滤波器调参先做一次开环积分对比把滤波关闭直接用 IMU 积分 yaw再与里程计 yaw 差做曲线。如果两者之差近似线性增长说明陀螺零偏没消干净如果差值呈缓慢波动更像外参或杆臂误差。如果静止段 Allan 方差显示角度随机游走异常大说明传感器本身噪声偏高只能靠后续融合矫正如果是零偏不稳定度大则应该考虑温补或陀螺零偏在线估计不能只靠固定阈值触发。5.2 双足机器人的导航里程计与轮式里程计有什么不同双足机器人没有轮速只能靠 IMU 加腿部运动学模型合成“虚拟里程计”数据里通常有足端接触开关或关节角估算的速度。虚拟里程计不像轮式那样连续每个步态周期只有一个有效的位移增量。此时组合导航的更新时机需要和步态相位绑定。常见做法是采用触地检测触发观测更新支撑相结束瞬间把IMU推算的位置差分与足端位移做一次约束。yaw 观测来自髋关节朝向和 IMU 积分结果的对齐比轮式更容易抖动需要对角度差做较小的置信度设置否则滤波会跟着步态摆动一起震荡。双足场景里里程计 yaw 观测通常更依赖绝对参考如果只有惯性加运动学yaw 不可观问题会比轮式更严重。推荐的做法是定期用 lidar 匹配或点云配准给出绝对航向把 yaw 束紧。5.3 yaw 慢漂验证的具体检查技巧与参数阈值验证系统性改进时我习惯固化三个数值直线段 yaw 漂移率、往返圈闭环 yaw 误差、急转弯后 10 秒内 yaw 恢复误差。直线段漂移率是最容易测的指标低速均匀直行 60 秒yaw 变化不超过 2 度算合格。急转弯后漂移的检查更严格转一个 90 度弯等滤波稳定 5 秒看 yaw 与真实航向的偏差。如果每次转弯后残留偏差方向一致说明里程计与 IMU 之间存在固定外参误差如果方向随机则多半是观测噪声协方差给得不对。检查代码如下直接比较滤波输出 yaw 与里程计 yawyaw_ekf np.unwrap(R.from_quat(q_out).as_euler(ZYX)[:, 0]) yaw_odom np.unwrap(np.cumsum(odom_wz * dt_odom)) err yaw_ekf - yaw_odom # 找直线段再算漂移率 straight_mask np.abs(odom_wz) 0.02 segments_time ts[straight_mask]np.unwrap能避免由于 ±π 跳变导致的虚假漂移straight_mask滤掉转弯区间使漂移率反映的是陀螺零偏残留而不是动态跟踪误差。若err斜率达到每秒 0.1 度以上需要回到上一步重新估计零偏而不是继续缩放噪声协方差。5.4 标定完成后复跑 exp3.zip 的完整验证流程拿到外参和内参后完整的验证流程是先做一次不加里程计更新的纯惯性积分记录 yaw 偏移再加里程计更新跑一次 ESKF对比同样的时间段。两者差异越大说明观测约束越有效如果加了观测后 yaw 偏移反而变大说明观测方程的方向或量测噪声设置有误优先检查坐标变换方向。复跑时还要留一个互斥检查故意把里程计观测噪声调小 100 倍滤波应该会被里程计带偏产生明显的锯齿形速度跳变如果不产生跳变多半是卡尔曼增益计算或协方差初始化有误。之后再恢复正常噪声确认 yaw 漂移回到正常范围。这一轮消融把几个最容易出错的环节都覆盖到了后续替换新数据包时只用跑同一流程即可。本文还有配套的精品资源点击获取