学习目标吃透数学模型、物理含义、噪声离散转换、IMU 积分漂移机理、GNSS‑RTK 数据全处理链路扫清工程上单位、坐标系、符号的坑为 ESKF 打下坚实基础。前置总览六轴 IMU 3 轴陀螺仪输出机体角速度 3 轴加速度计输出比力⚠️核心事实IMU 输出不是位置不是速度只是角速度、比力必须积分才能得到姿态、速度、位置。姿态角速度做一阶积分速度比力补偿重力后做一阶积分位置速度再做二次积分二次积分会放大噪声与零偏纯 IMU 必然漂移发散必须外部传感器GNSS、激光、视觉做观测约束。3.1 IMU 系统的运动学3.1.1 连续时间运动学方程载体运动微分方程变量释义变量含义R∈SO(3)R\in SO(3)R∈SO(3)世界坐标系→机体坐标系旋转矩阵描述载体姿态ω\boldsymbol{\omega}ω机体坐标系下角速度向量ωΛ\boldsymbol{\omega}^\LambdaωΛ是角速度构造的 3×3 反对称矩阵qqq单位四元数姿态另一种表达⊗\otimes⊗代表四元数乘法p\boldsymbol pp载体世界坐标系位置v\boldsymbol vv载体世界坐标系速度a\boldsymbol aa载体世界坐标系真实运动加速度约定世界坐标系 Z 轴向上重力IMU 原始测量方程重点加速度计输出是【比力】不是物体真实加速度无重力理想太空环境a~\tilde{\boldsymbol a}a~、ω~\tilde{\boldsymbol\omega}ω~IMU 硬件读出测量值机体坐标系。R⊤R^\topR⊤把世界坐标系向量变换到机体坐标系。地球地表真实环境a~\tilde{\boldsymbol a}a~叫比力 specific force机体感受到除去重力之外的受力对应的加速度。物理直观理解载体静止真实运动加速度a0\boldsymbol a\boldsymbol 0a0。加速度计读出反向重力。物理来源地面支持力抵消重力。载体自由落体载体真实加速度ag\boldsymbol a\boldsymbol gag。加速度计读数为 0。通俗讲自由下落 IMU 感受不到地面支持加速度计输出 0。补充说明如果 IMU 安装点不在载体旋转中心载体旋转会引入离心力、科氏力、角加速度项会污染加速度计读数。教学模型忽略工程实践尽量将 IMU 布置在载体旋转中心消除该项。3.1.2 IMU 测量噪声模型连续时间真实 IMU 读数包含两项误差高斯白噪声 零偏 bias随机游走 / 维纳过程符号含义随机过程模型ηa\boldsymbol \eta_aηa​加速度计测量白噪声零均值高斯白噪声 GPηg\boldsymbol \eta_gηg​陀螺仪测量白噪声零均值高斯白噪声 GPba\boldsymbol b_aba​加速度计零偏b˙aηba\dot{\boldsymbol b}_a\boldsymbol \eta_{ba}b˙a​ηba​随机游走bg\boldsymbol b_gbg​陀螺仪零偏b˙gηbg\dot{\boldsymbol b}_g\boldsymbol \eta_{bg}b˙g​ηbg​随机游走ηba,ηbg\boldsymbol \eta_{ba},\boldsymbol \eta_{bg}ηba​,ηbg​驱动零偏变化的高斯噪声高斯白噪声关键概念辨析零偏 bias 不是常数不是固定偏移。它的导数是高斯噪声零偏会随时间做布朗运动缓慢漂移受温度、震动影响。随机游走只是数学建模不是 IMU 器件的物理本质。只是为方便滤波做的简化工程够用想要更高精度需要额外温度标定。3.1.3 IMU 离散时间噪声模型工程代码实现核心连续→离散转换IMU 硬件输出是离散采样采样间隔Δt\Delta tΔt。连续是谱密度离散是采样点上的噪声方差二者必须转换直接抄手册参数不做转换滤波器直接发散。1测量白噪声陀螺仪、加速度计写成标准差形式物理结论IMU 采样频率越高Δt\Delta tΔt越小离散测量噪声越小。高频 IMU 的优势来源于此。2零偏随机游走bias 的增量相邻采样时刻 bias 变化量标准差形式物理结论采样间隔Δt\Delta tΔt越大一个采样周期内零偏漂移越大。⚠️单位区分阅读 IMU 硬件手册必看连续时间谱密度单位分母带有s\sqrt ss​。陀螺角度随机游走deg/h\mathrm{deg}/\sqrt{\mathrm h}deg/h​加速度速度随机游走m/s/h\mathrm{m/s}/\sqrt{\mathrm h}m/s/h​。离散时间代码中直接加到观测单位就是物理量本身rad/s、m/s2\mathrm{rad/s}、\mathrm{m/s^2}rad/s、m/s2。调参坑手册给出是连续谱密度代码 ESKF 需要离散噪声方差必须带入Δt\Delta tΔt换算。3.1.4 现实中的 IMU 硬件与手册指标器件分类MEMS IMU手机、车载低成本设备噪声大零偏不稳定价格低。光纤陀螺 FOG高精度导航噪声小、零偏稳定成本高、体积大。手册关键指标与模型对应关系| 手册指标 | 模型参数 ||---|---|| 角度随机游走 ARW | 陀螺白噪声σg\sigma_gσg​|| 速度随机游走 VRW | 加速度计白噪声σa\sigma_aσa​|| 运行时零偏稳定性 | 零偏随机游走σbg,σba\sigma_{bg},\sigma_{ba}σbg​,σba​|厂商手册一般不直接给出随机游走参数工程上拿运行时零偏稳定性作为σbg,σba\sigma_{bg},\sigma_{ba}σbg​,σba​调参参考。3.2 使用 IMU 进行航迹推算纯 IMU 积分无外部观测输入 IMU 传感器读数ω~,a~\tilde{\boldsymbol\omega},\tilde{\boldsymbol a}ω~,a~输出载体姿态RRR、世界坐标系速度v\boldsymbol vv、位置p\boldsymbol pp简称 PVQ位置‑速度‑四元数。3.2.1 连续时间积分方程把 IMU 测量减去当前估计零偏代入运动学方程注意a~\tilde{\boldsymbol a}a~是机体坐标系比力左乘RRR变换到世界坐标系再加重力。欧拉离散积分以区间起始时刻的值代表整个Δt\Delta tΔt时间段输入从时刻ttt积分到tΔtt\Delta ttΔt姿态角速度积分一阶exp⁡\expexp是 SO (3) 的指数映射。速度世界坐标系加速度一阶积分。位置速度二次积分。数值积分方法对比欧拉积分只用区间起点 IMU 值最简单数值误差较大。书中示例代码imu_integration.h使用欧拉积分。中值积分 / 梯形积分取区间两端 IMU 测量的平均值精度更高。VIO、预积分工程实现普遍使用。高阶龙格‑库塔精度进一步提升但计算开销大。3.2.2 纯 IMU 航迹推算发散机理核心理解书中实验给 IMU 数据做积分姿态变化尚可速度、位置短时间剧烈漂移。误差传播链条陀螺仪噪声、零偏 →姿态RRR估计存在误差姿态不准导致把机体坐标系加速度向世界坐标系投影出错加速度计噪声、零偏经过一次积分得到速度误差二次积分得到位置误差位置误差随时间平方量级增长快速发散。物理结论IMU 适合短时间预测不能长时间独立定位必须外部传感器观测约束漂移。3.2.3 书中代码模块解析imu_integration.hvoidAddIMU(constIMUimu){doubledtimu.timestamp_-timestamp_;//位置积分p_p_v_*dt0.5*gravity_*dt*dt0.5*(R_*(imu.acce_-ba_))*dt*dt;//速度积分v_v_R_*(imu.acce_-ba_)*dtgravity_*dt;//姿态积分SO3指数映射R_R_*Sophus::SO3d::exp((imu.gyro_-bg_)*dt);}输入 IMU 的加速度、陀螺仪读数减去已知 / 估计零偏ba,bg\boldsymbol b_a,\boldsymbol b_gba​,bg​执行欧拉积分更新p,v,R\boldsymbol p,\boldsymbol v,Rp,v,R不做任何外部观测修正实验现象轨迹很快发散车辆跑出可视化窗口。代码启示零偏bg,ba\boldsymbol b_g,\boldsymbol b_abg​,ba​如果设置错误哪怕硬件很好积分结果依然完全不可用。3.2 小节总结关键点IMU 积分前提需要零偏bg,ba\boldsymbol b_g,\boldsymbol b_abg​,ba​零偏不准直接导致结果崩坏。姿态来自角速度一阶积分速度加速度一阶积分位置二次积分。欧拉积分简单但精度有限工程优先中值积分。漂移不是硬件故障是二次积分放大噪声带来的数学固有性质。3.3 卫星导航 GNSS / RTKGNSS 提供绝对世界坐标系观测用来约束 IMU 积分漂移GINS 组合导航 GNSSIMU。3.3.1 GNSS 系统基础四大全球卫星导航系统GPS (美)、北斗 BDS (中)、GLONASS (俄)、Galileo (欧盟)。接收机同时接收多套卫星信号提升可靠性。定位模式对比单点 GNSS米级定位精度手机普通定位。缺陷不足以区分高速主路 / 辅路、车道无法车道级自动驾驶。RTK 实时动态差分定位 Real‑Time Kinematic利用地面基站做载波相位差分厘米级定位精度。优点输出绝对全局坐标缺点必须直视卫星隧道、高楼峡谷、树荫遮挡信号丢失输出频率低一般 1‑10Hz远低于 IMU 几百 Hz。GINS 工作模式RTK 有效时做观测修正 IMURTK 丢失依靠 IMU 做短时间航迹推算。RTK 硬件单天线 vs 双天线单天线 RTK仅输出位置不能输出航向 yaw 角。双天线 RTK两个蘑菇头两根天线拉开基线利用两天线的位置差解算载体航向 yaw。基线距离越大航向角度噪声越小。双天线安装形式左右、前后、对角安装。工程必须标定两套外参1安装偏移天线相对于车身本体中心的平移向量2安装偏角天线坐标系相对车身坐标系的旋转角度。⚠️重要RTK 输出的是天线的位姿不是车身本体中心位姿必须做外参变换。位姿变换公式TWGT_{WG}TWG​世界坐标系到 GNSS 天线坐标系RTK 直接输出TGBT_{GB}TGB​天线坐标系到车身本体坐标系标定得到的外参TWBT_{WB}TWB​世界坐标系到车身本体中心是 SLAM / 定位算法真正需要的位姿。3.3.2 坐标系系统工程高频踩坑LLA 经纬‑高坐标系 Latitude‑Longitude‑Altitude地球全局坐标系角度 海拔高度。缺点不是米制局部计算不方便经度纬度随地球曲率距离不是线性。UTM 投影坐标系 Universal Transverse Mercator把地球椭球体分 60 个经度带投影到平面输出米制坐标。自动驾驶局部定位首选。北半球常用东北天 ENU 坐标系X 向东Y 向北Z 向上。RTK 厂商经常输出北东地 NED 坐标系X 向北、Y 向东、Z 向下。⚠️坐标系角度转换NED 航向角hhh转 ENU 航向角h′hh′坑直接使用厂商原始航向角不做坐标系转换姿态直接 90 度偏差。UTM 局限跨带的时候需要额外处理两极不适用。3.3.3 RTK 原始数据完整处理链路书中utm_convert.cc原始文本数据样例GNSS timestamp 纬度 经度 高度 航向角 航向有效标志完整处理流程读取原始 GNSS 报文时间、LLA、航向、航向有效位。LLA 经纬高 → UTM 米制坐标。使用标定外参安装偏移、安装偏角天线位姿TWGT_{WG}TWG​转换车身本体位姿TWBT_{WB}TWB​。减去初始原点坐标平移到局部原点附近规避很大 UTM 原始数值造成浮点数有效数字丢失。判断航向有效标志双天线 RTK 航向失效角度不可信不能当做 6 自由度观测只能用位置观测。RTK 传感器固有缺陷高度 Z 轴噪声显著大于平面 XY轨迹上高度明显抖动。遮挡场景信号丢失甚至输出虚假 “固定解”产生野值跳变。更新频率低需要与高频 IMU 做时间戳对齐。双天线航向容易跳变受多路径效应干扰。书中实验现象RTK 轨迹 XY 平滑Z 抖动航向标志会频繁失效。3.3 小节核心工程要点RTK 输出是天线位置不是车体中心外参必不可少。NED、ENU 两套坐标系角度要转换符号极易踩坑。航向有效标志位一定要判断失效不能使用角度观测。UTM 原始坐标数值巨大平移到局部原点保护浮点数精度。