用PSINS构建IMU误差模型：零偏、随机游走与惯导解算实战

先别急着跑VINS-Fusion也别急着拿相机和IMU去做联合标定。我先问一句你的IMU数据长什么样误差大概有多少你心里有数吗很多做多传感器融合的朋友包括我自己早期最容易犯的错就是一头扎进算法里调参数结果轨迹依然飘得离谱。后来我才意识到问题多半出在IMU误差模型没搞清楚。这篇文章我就用国产开源惯导工具箱PSINS带你从零构建自己的第一个IMU误差模型。我从误差模型到底包含哪些项讲起再一步步演示怎么用PSINS生成带误差的IMU数据、怎么做纯惯导解算验证最后把我在实际使用中踩过的单位、坐标系、采样率相关的坑一并倒出来。无论是做组合导航、SLAM预积分还是lidar/相机与IMU联合标定这篇都能帮你少走很多弯路。1. IMU误差模型到底在建模什么1.1 确定性误差能测出来、能补偿的那部分IMU误差模型第一大类是确定性误差说白了就是可以通过标定实验测量出来、并且在算法里直接扣除的部分。最常见的有三个常值零偏、比例因子误差、安装误差。常值零偏Bias是IMU上电后即使输入角速度或加速度为零传感器输出仍然带有的固定偏移。陀螺零偏习惯用度每小时deg/h表示加速度计零偏则常用微重力ug1ug约等于9.8×10⁻⁶ m/s²表示。需要注意这个“常值”只针对单次上电过程实际上每次上电零偏都会变这叫重复性温度变了零偏也会漂这叫温漂。所以我们建模时通常把当前上电的常值零偏作为随机常数来估计这也是组合导航里卡尔曼滤波状态量必须包含陀螺零偏和加计零偏的原因。比例因子误差Scale Factor是输入与输出之间的比例关系偏离理想值造成的误差。比如你真实转了100度每秒陀螺却输出100.02度每秒这0.02%就是比例因子误差工程上常用ppm百万分之一表示。比例因子的特点是误差量随输入幅值增大而线性增大高动态场景下非常要命。安装误差Misalignment是因为IMU三个敏感轴不可能做到绝对正交贴着焊在电路板上时总会有点歪导致一个轴的测量值混入了另外两个轴的分量。它本质上是一个3×3的交叉耦合矩阵通常用角秒arcsec或度来度量。这三类误差组合在一起可以写成经典的测量模型测量值 (I M) × 真值 b n其中I是单位阵M由比例因子误差和安装误差组成b是常值零偏n是随机噪声。PSINS里的误差参数设置本质上就是让你填这个M、b和n。你只有先把这三类误差搞清楚后面做六位置标定、转台标定才有意义。1.2 随机误差只能用统计方法描述的那部分确定性误差补偿完之后IMU的输出里还残留着随机噪声这部分没法用一条固定的曲线拟合只能用统计量描述。最常见的两个指标是角度随机游走ARW单位deg/√h和速度随机游走VRW单位ug/√Hz。角度随机游走名字听着绕其实可以这样理解陀螺输出的角速度里叠加了一层白噪声白噪声本身均值是零但如果你把它积分成角度积分过程会把噪声一步步“累加”成随机游走。随机游走的方差随时间线性增长标准差随时间按根号t增长。所以ARW数值越小说明陀螺的角速度白噪声越低积分出来的角度越稳。另一个关键指标是零偏不稳定性Bias Instability它反映的是零偏在低频段的缓慢随机变化单位通常是deg/h。在Allan方差曲线上它表现为曲线最低谷的那一段。很多IMU数据手册上写的“零偏稳定性”其实指的就是这个它比ARW更能反映器件的中长期漂移特性。除了上面这些还有量化噪声、速率随机游走、指数相关噪声马尔可夫过程等但在工程实践中ARW、VRW加零偏不稳定性已经是90%场景下必须建模的项目。值得强调一点随机误差不是用来“补偿”的而是用来估计方差、设置滤波器Q阵的。你只有知道传感器噪声有多大卡尔曼滤波或者因子图优化才会给你一个合理的权重大小。1.3 误差对惯导和融合系统的真实影响量级很多人把误差模型当成“标定完就完事”的东西其实误差模型直接决定了你的惯导系统能跑多久、融合结果能有多准。我举个数量级的例子方便你建立直觉。先说陀螺常值零偏的影响。陀螺零偏ε会导致姿态误差随时间线性增长姿态误差又会让重力加速度在水平方向产生一个等效的投影分量g×φ这个等效水平加速度再积分两次就变成位置误差。近似公式是位置误差 ≈ (1/6) × g × ε × t³注意这里时间是三次方所以陀螺零偏对纯惯导位置误差的影响是指数级恶化的。如果陀螺零偏是0.01 deg/h大约4.85×10⁻⁸ rad/s10分钟后t600秒算下来位置误差大约0.17米但如果陀螺零偏恶化到1 deg/h同样600秒位置误差直接增大100倍到了十几米量级。再看加速度计零偏。加计零偏b直接积分成速度误差b×t再积分成位置误差0.5×b×t²。一个10微g的加计零偏10分钟的位置误差大约是0.5×9.8×10⁻⁵×600²17.6米。所以你看短时间内加计零偏才是位置漂移的主要贡献者陀螺零偏要经过姿态误差的“放大”才体现出来但时间一长三次方项就会压过二次方项。在视觉SLAM或激光SLAM里IMU误差模型参数同样重要。VINS-Fusion这类系统做IMU预积分时噪声协方差矩阵就是由ARW和VRW计算出来的。你把噪声参数给小了系统会过于相信IMU视觉一抖动整个估计就崩给大了IMU又起不到约束作用轨迹照样飘。这些都不是调优化权重能解决的根子就在误差模型上。2. 为什么选PSINS准备工作怎么做2.1 PSINS工具箱的定位和能干什么PSINS全称大概是Precise Strapdown Inertial Navigation System是国内惯导领域流传非常广的开源MATLAB工具箱由严恭敏老师团队维护。它的定位不是某个具体型号IMU的标定工具而是一整套完整的捷联惯导算法与组合导航仿真框架。我推荐PSINS的原因很直接代码可读性好、函数划分清晰、上手门槛低。你不需要自己从零写姿态更新、速度更新、位置更新也不用自己造轮子去生成仿真轨迹PSINS都给你封装好了。更重要的是它对IMU误差模型的描述非常规范内置单位换算你只要按工程单位填参数它自动转换成国际单位参与运算这一点帮我避开了无数量纲错误。当然PSINS不是万能的它主要面向算法验证和科研仿真如果你需要做Allan方差曲线拟合通常还要搭配imu_utils或者自己写脚本。但凡是涉及IMU仿真数据生成、纯惯导解算、组合导航验证PSINS基本是绕不开的首选。2.2 安装与初始化PSINS的安装没有太多花活。从官方渠道拿到工具箱压缩包后解压到任意目录然后在MATLAB里把这个目录连同子目录全部加入搜索路径。最稳的方法是直接用addpath(genpath(你的PSINS目录路径)); savepath;执行完之后运行一行glvs初始化全局变量。这一步千万别省它会把重力加速度、地球自转角速度、各种单位换算常数比如deg、ug、min、sec全部加载到全局结构体glv里。我见过不少新手朋友上来直接跑demo结果报错找不到glv就是因为没执行glvs。不同版本的PSINS函数名和调用格式有点差异这是最正常不过的。你拿到新版本后建议先跑一下工具箱自带的demo能跑通就说明环境OK然后再开始玩自己的数据。我这里讲的函数和代码风格是当前主流版本的写法你如果碰到函数名不一样直接help 函数名查一下当前版本的说明即可原理完全相通。2.3 和误差模型相关的核心函数与结构体PSINS里和IMU误差模型关系最近的是三个东西imu数据格式、imuerr结构体、imuerrset和imuadderr这两个函数。imu数据在PSINS里是N行6列的矩阵每一行是一个采样时刻的六轴数据前三列是陀螺X/Y/Z轴角速度后三列是加计X/Y/Z轴比力。你如果从真实IMU读数据记得也要整理成这个格式再喂给PSINS。imuerr是误差模型的核心结构体里面存的是陀螺零偏、加计零偏、陀螺随机游走、加计随机游走等字段。当然你不需要手工一个个填字段直接用imuerrset函数生成即可。常见调用是imuerr imuerrset(eb, db, web, wdb);其中eb是陀螺常值零偏单位deg/hdb是加计常值零偏单位ugweb是陀螺角度随机游走单位deg/√hwdb是加计速度随机游走单位ug/√Hz。你传进去的是工程单位函数内部会转成SI单位并保存在结构体里。imuadderr则是把误差结构体实际“叠加”到一组理想IMU数据上imu_err imuadderr(imu_ideal, imuerr, ts);ts是采样间隔单位秒。这个函数干的事就是你误差模型定义的那些加常值零偏、加随机游走噪声、加比例因子和安装误差。理解了这几个函数整个PSINS误差建模的骨架就搭起来了。3. 构建你的第一个IMU误差模型完整实操3.1 准备基准数据仿真轨迹与理想IMU数据要做误差模型验证第一步得有一组“理想IMU数据”作为基准。这个基准数据不能拍脑袋乱造最好是从一条已知轨迹反算出来的这样后面你才能把解算结果和真值对比误差到底多大一目了然。PSINS里生成仿真轨迹和IMU数据有很多种方式最省事的办法是用工具箱自带的轨迹生成函数。典型代码如下glvs; ts 0.01; % 采样间隔单位秒 t_end 300; % 仿真时长单位秒 trj trjgen(test_traj, ts, t_end); % 生成轨迹结构体 [imu_ideal, avp0] trjsimu(trj, 0); % 第二参数传0生成理想IMU数据这段代码会生成一条包含起步、转弯、匀速等典型机动的轨迹并反算出对应的理想IMU六轴数据imu_ideal以及初始姿态、速度、位置avp0。avp0很重要后面纯惯导解算的初值就靠它。不同版本PSINS的轨迹生成函数名可能不一样有的版本叫trjgen有的叫trajgen还有的直接用trjsimu一步到位。你只需要把握一个原则最终结果一定要得到“理想IMU数据初始姿态/速度/位置”这两样东西。如果demo里能跑通照着demo的轨迹生成方式改时长就行。3.2 配置误差参数三档IMU精度参考误差参数怎么选取决于你想仿真什么档次的IMU。我这里给你一张我常用的参数参考表覆盖高精度、中端MEMS、低成本MEMS三档单位都是工程单位方便你直接填进imuerrset。参数高精度光纤/激光陀螺中端MEMS低成本MEMS陀螺常值零偏deg/h0.001 ~ 0.011 ~ 310 ~ 100陀螺角度随机游走deg/√h0.001 ~ 0.0050.05 ~ 0.20.5 ~ 1加计常值零偏ug10 ~ 501000 ~ 30005000 ~ 20000加计速度随机游走ug/√Hz0.01 ~ 0.0550 ~ 100100 ~ 500比例因子误差ppm5 ~ 20500 ~ 10002000 ~ 5000我实际测试中经常用中端MEMS的参数来做算法验证因为这类传感器最贴近普通开发者的工程场景。你可以在imuerrset里填imuerr imuerrset(1.0, 1000, 0.1, 50);这里四个参数分别对应陀螺零偏1 deg/h、加计零偏1000 ug、陀螺随机游走0.1 deg/√h、加计随机游走50 ug/√Hz。如果想顺便加入比例因子和安装误差的影响可以查一下当前版本imuerrset是否支持额外参数通常会有对应的输入项把等级值填进去就行。3.3 注入误差并做纯惯导解算验证有了理想数据和误差结构体接下来就是把误差注入到数据里然后分别对理想数据和带误差数据做纯惯导解算对比两者差异。% 注入误差 imu_noisy imuadderr(imu_ideal, imuerr, ts); % 用理想IMU数据做纯惯导解算 avp_ideal sins(imu_ideal, avp0, ts); % 用带误差IMU数据做纯惯导解算 avp_noisy sins(imu_noisy, avp0, ts);不同版本的纯惯导解算函数接口可能有变化比如有的版本用sins(imu, avp0)有的版本要先初始化ins insinit(avp0, ts);再循环更新。最简单的办法还是翻一翻你手上版本自带的demo看看纯惯导解算到底怎么调把demo里的调用方式替换进来就行。解算完之后我一般会画两张图一张是理想数据和带误差数据解算出的位置曲线对比另一张是位置误差随时间变化曲线。你大概率会看到理想数据的解算结果基本跟真值轨迹重合而带误差数据的轨迹以肉眼可见的速度偏离。这个偏离的速度和方向就直接反映误差模型参数设置得合不合理。3.4 结果观察与误差拆分分析只看总误差还不够我建议你做一次“误差拆分”实验也就是把陀螺零偏、加计零偏、随机游走分别单独注入观察各自的独立影响。比如只加陀螺零偏的代码imuerr_only_eb imuerrset(1.0, 0, 0, 0); imu_only_eb imuadderr(imu_ideal, imuerr_only_eb, ts);然后用同样的方式解算、画误差曲线。你会发现陀螺零偏主导的姿态误差在300秒仿真里可能并不直接体现在位置误差上而是先造成水平姿态倾斜再通过重力耦合让位置慢慢跑偏。加计零偏则是“闷声发大财”直接以二次方方式污染位置。随机游走的表现则更有意思它造成的误差曲线不像常值零偏那样光滑而是带明显的随机抖动随着时间方差越来越大。做这一步的意义在于将来你拿到真实IMU数据如果纯惯导解算的位置误差曲线长得像某种特定形态你就能反推到底是哪类误差参数没标定好。这种“误差形态识别”能力在排查传感器故障时非常实用。4. 常见问题与避坑记录4.1 单位换算与量纲坑单位错误是我见过最多的问题没有之一。很多人直接把手册上的数值填进去结果仿真结果离谱得没法看。PSINS的imuerrset已经帮你把常用单位转成SI单位但你得清楚自己填进去的单位到底是什么。这里我把最常用的换算关系列一张速查表原始单位换算成SI数值1 deg/hrad/s4.848 × 10⁻⁶1 deg/√hrad/√s2.909 × 10⁻⁴1 ugm/s²9.80665 × 10⁻⁶1 arcsecrad4.848 × 10⁻⁶我最常踩的坑是把陀螺零偏单位里的deg/h直接当成rad/s来用。一个0.01 deg/h的零偏如果当rad/s用等于把误差放大了20多万倍解算结果当然秒崩。遇到结果诡异的时候第一步先检查单位。4.2 坐标系符号与IMU数据方向第二个高频坑是坐标系定义和加速度计符号。PSINS默认是东北天坐标系加速度计输出的是比力也就是“单位质量感受到的非引力外力”。静止水平放置时一个安装方向合适的IMU其Z轴加速度计应该输出约1g如果Z轴朝天或者-1g如果Z轴朝地取决于你的IMU定义。做仿真时PSINS生成的理想IMU数据天然遵循它自己的坐标系约定你只要保证初始姿态avp0的定义跟数据一致就行。但如果你导入的是真实IMU数据就必须先搞清楚传感器的坐标轴方向否则整个融合系统都会出现系统性偏差。比如IMU的X轴朝前还是朝后Y轴朝左还是朝右都会影响姿态初值的设置。我自己的习惯是任何IMU数据进入PSINS之前先画一下静止段的三轴输出数值。看到Z轴加速度在±1g附近X/Y轴基本为0再确认符号和自家坐标系定义一致才放心往里喂数据。这个习惯帮我避免了很多次“看起来代码很对但结果就是不对”的尴尬。4.3 采样时间与随机游走的离散化问题imuadderr在处理常值零偏时很简单直接加一个常数就行。但随机游走参数的处理涉及到连续时间噪声和离散采样之间的换算这里很多人容易迷糊。连续域的ARW单位是deg/√h描述的是白噪声经过积分后的角度随机游走增长速率。在离散仿真中每个采样周期要生成一个随机数叠加到角速度上这个随机数的标准差需要按1/√ts换算。举个例子如果采样率是100Hzts0.01s那么每个采样点加在陀螺角速度上的白噪声标准差就是由ARW换算成“每秒”后除以√ts得到的。好消息是PSINS的imuadderr已经替你处理好了这个离散化过程你只要把ts传对就行。但如果你自己写误差注入代码就一定要注意换个采样率叠加噪声的标准差要跟着变否则同样的误差参数在不同采样率下会得到完全不同的随机游走强度。4.4 拿到真实IMU数据怎么定参数Allan方差分析仿真的误差参数可以拍脑袋选但如果你要处理真实IMU就必须用实测数据估计误差参数。最常用的方法就是Allan方差分析。基本步骤是这样的把IMU静止放置连续采集2到4小时数据采样率最好不低于100Hz。然后对陀螺和加计各轴分别计算Allan方差或者Allan偏差画出双对数曲线。从曲线上可以读出几个关键参数斜率接近-1/2的段对应角度随机游走曲线最低谷对应的纵坐标读数近似为零偏不稳定性斜率接近1/2的段对应速率随机游走。我自己用过一个比较省事的办法MATLAB社区或者GitHub上有现成的Allan方差计算脚本搜“allan variance matlab”就能找到。把静止数据导入脚本跑完直接就能输出ARW和零偏不稳定性估计值。拿到这些数值后再去填imuerrset这样你的误差模型就有实测基础了而不是靠感觉拍脑袋。这里补充一句Allan方差做出来的参数只是随机误差部分常值零偏、比例因子和安装误差还是需要六位置法或者转台标定来获得。不过对很多做视觉SLAM的朋友来说常值零偏可以在线估计优先把ARW和VRW测准就够了。4.5 在视觉/激光IMU联合标定中的参数迁移现在很多朋友掉进IMU误差模型的坑是因为做VINS-Fusion、lidar-imu标定这类项目时需要设置IMU噪声参数。比如VINS-Fusion里配置IMU的噪声密度和零偏随机游走Kalibr标定相机/IMU时也要给IMU参数还有LIO-SAM里也有一堆噪声项要填。这些参数本质上都来自误差模型。我在做D435相机和IMU联合标定时发现直接用厂商手册给的噪声值效果往往一般。因为手册给的是典型值甚至理想值你的那一颗传感器可能偏得比较多。更稳的方式是把静止数据做一遍Allan方差分析把实测的ARW、VRW、零偏不稳定性填进去。虽然联合标定工具还会在线优化一部分参数但初始值越接近真实迭代收敛越快结果越稳。里程计和IMU融合定位同理滤波器Q阵里的陀螺/加计噪声方差我建议先按Allan方差实测值给再根据实际表现微调。如果你实在没条件做Allan方差宁可按手册典型值偏大一点给也不要给得比实际还小。给大了只是估计保守给小了会导致滤波器过于自信发散时你根本不知道该信谁。另外在Carsim这类整车仿真软件里设置IMU传感器时本质上也是在定义一组误差模型参数。很多同学直接在Carsim里给了理想IMU结果拿到和真值一模一样的数据再到自己的融合算法里一跑误差为零完全起不到验证作用。想让仿真更贴近真实就应该参照真实MEMS器件的误差等级给IMU传感器设置零偏、噪声和刻度因子误差这样算法验证才有意义。最后再分享一个我个人实验中的体会。第一次跑通PSINS的误差模型时我以为自己已经懂了直到把陀螺零偏设成0.01 deg/h、再把加计零偏设成10 ug看着位置误差曲线以漂亮的二次方和三次方形态展开我才真正理解误差模型的每一项“长什么样”。这种理解不是看公式看出来的是你亲手改参数、亲手画曲线、亲手排查单位错误之后才真正长在脑子里的。如果你也是从视觉SLAM或者激光SLAM转过来做多传感器融合的我建议你在调任何系统之前先花半天时间用PSINS把自己手里的IMU误差模型建一遍。别看这半天花得好像没出成果后面能帮你省下以周为单位的排查时间。