MEMS IMUMEMS IMU的入门与应用的入门与应用一、导航系统基本概念一、导航系统基本概念二二、MEMSMEMS惯性器件；惯性器件；三、三、MEMSMEMS惯性器件的简易标惯性器件的简易标定；定；四、四元数；四、四元数；五、基于乘性四元数五、基于乘性四元数EKFEKF滤滤波器波器一、导航系统基本概念一、导航系统基本概念1.1.海里海里国际上采用1852m作为标准海里长度，1nm=1852m2.2.惯性级导航系统惯性级导航系统运行1h过程中在水平面上定位误差等于1nm（简记1nm/h）的惯性导航系统称为惯性级导航系统3.3.惯性级陀螺惯性级陀螺地球自转角速率为15.04107度/h.其千分之一为0.015度/h,称为毫地转率；将精度达到0.015度/h的陀螺称为惯性级陀螺，往往也以1meru的量级(0.01度/h)表示惯性级陀螺精度4.4.加速度计加速度计高精度：1mg;惯性级导航系统对陀螺的最低要求为0.01度/h，加速度计精度的最低要求为1x10-4g二二.MEMS.MEMS惯性器件惯性器件1.1.消费级消费级MEMSMEMS惯性器件惯性器件InvensenseMPU6000/MPU6050Invensense公司的MPU6000/MPU6050六轴惯性器件是世界上首个六轴集成的低成本MEMS惯性器件，该器件推出市场之后在各个领域（特别是四轴飞行器）得到了广泛应用二二.MEMS.MEMS惯性器件惯性器件1.1.消费级消费级MEMSMEMS惯性器件惯性器件在2010年，apple的iPhone4采用了ST公司的三轴陀螺仪L3G4200D和三轴加速度计LIS331DLH作为手机的惯性传感器。其它手机厂商随即跟进，陀螺仪与加速度计逐渐成为智能手机的标配传感器iPhone4IMU手机型号MEMS加速度计MEMS陀螺iPhone4LIS331DLHL3G4200D红米MPU6050iPhone6MPU6700三星S6MPU6500三星S7LSM6SD3小米5ICG-20660L二二.MEMS.MEMS惯性器件惯性器件2.2.工业级工业级MEMSMEMS惯性器件惯性器件ADIS16405ADIS16488AXsensMti二二.MEMS.MEMS惯性器件惯性器件2.2.常见常见MEMSMEMS惯性器件参数对比惯性器件参数对比加速度量程(最大)非线性度初始偏置轴间灵敏度噪声密度偏置温度系数输出噪声价格LSM30316g60mg0.15mg/sqrtHZ0.01量程/MPU605016g0.5%3%2%0.4mg/sqrtHZ0.02量程/MPU650016g0.5%2%0.3mg/sqrtHZ0.026%量程/BMI18016g0.5% 25mg 0.18mg/sqrtHZADIS1640518g0.5% 50mg0.20.5mg/sqrtHZ0.3mg/9mg4500ADIS1648818g0.5%0.0350.063mg/sqrtHZ0.0025mg/1.29mg14500陀螺量程(最大)非线性度(量程）对准误差初始偏置/s线性加速度影响角速度随机游走速率噪声密度偏置温度系数输出噪声/s rms价格（RMB）L3G4200D20000.2%75/s0.030.04/s/MPU605020000.2%2%量程20/s0.10.00520/s0.05MPU650020000.1%2%量程5/s0.010.24/s/0.1BMI18020000.1%3/s0.007MAX2100020000.2%1%2/s0.0090.450.0092%停产ADIS16405 350 0.1%0.053/s0.00720.0440.01/s/0.84500ADIS16488A 4800.01%0.050.260.00590.00250.13514500二二.MEMS.MEMS惯性器件惯性器件2.2.常见常见MEMSMEMS惯性器件参数对比惯性器件参数对比二二.MEMS.MEMS惯性器件惯性器件2.MEMS2.MEMS惯性器件的常见技术参数惯性器件的常见技术参数（2）非线性度（3）初始偏置误差MEMS惯性器件在开机后系统稳定输出后观测信息中包含的误差，可以通过静态情况下的初始化求取均值消除（1）量程MEMS惯性器件在开机后系统稳定输出后观测信息中包含的误差，可以通过静态情况下的初始化求取均值消除二二.MEMS.MEMS惯性器惯性器件件2.MEMS2.MEMS惯性器件的常见惯性器件的常见技术参数技术参数（4）对齐误差参考文献：MEMS陀螺仪中主要噪声源的预测和管理ADI公司应用工程师MarkLooney三轴式陀螺仪对齐误差MEMSIMU通常具有两种类型的对齐误差，它们相互关联，但在系统级建模中具有不同应用：轴到封装和轴到轴。“轴到封装对齐误差”描述陀螺仪相对于器件封装上特定机械特性的对齐情况。将IMU安装到系统后，如果系统无法支持惯性对齐，则轴到封装对齐误差将成为整体对齐误差的主要因素之一。系统与IMU的机械接口的机械缺陷也会增加整体对齐误差。“轴到轴对齐误差”描述各个陀螺仪旋转轴相对于其他两个陀螺仪的相对对齐精度。在系统可以实现简单的对齐过程时，此参数影响最大，此时通常沿系统的惯性参考系中的一个轴直线移动整个组件(IMU已安装在系统平台上)，同时需要观察传感器。二二.MEMS.MEMS惯性器件惯性器件2.MEMS2.MEMS惯性器件的常见技术参数惯性器件的常见技术参数（6）随机游走，噪声密度随机游走过程：当前观测值完全由下一时刻观测值加上现时噪声决定，即n时刻的输出由n-1时刻的输出和随机误差和加和决定陀螺角度游走过程：宽带角速率白噪声积分的结果，即陀螺从0时刻累积的总角增量误差表现为随机游走，而每一时刻的等效角速率误差表现为白噪声噪声密度：传感器固有噪声代表的是陀螺仪在静态惯性和环境条件下运行时其输出中的随机振动。MEMSIMU数据手册通常会提供速率噪声密度(RND)参数来描述陀螺仪相对于频率的固有噪声。此参数通常使用单位/s/Hz，是预测特定滤波器配置固有噪声的关键。单位：/hr功率谱N的国际单位：（rad/s）/Hz（6）随机游走，噪声密度二二.MEMS.MEMS惯性器件惯性器件2.MEMS2.MEMS惯性器件的常见技术参数惯性器件的常见技术参数信号方差强度q、功率谱密度S、随机游走系数N之间的简单换算关系：1、令陀螺输出角速率方差的单位为（rad/s），离散系统等效激励白噪声的方差等于连续时间系统白噪声方差强度与离散化周期的乘积：2、功率谱密度S（单位（rad/s）/Hz）与方差q的关系：Qk=q*TsS=q/Ts3、角度随机游走N（单位/h）与功率谱密度S的关系：N=(3600*q)/60二二.MEMS.MEMS惯性器件惯性器件2.MEMS2.MEMS惯性器件的常见技术参数惯性器件的常见技术参数（7）偏置温度系数（温飘）MEMS惯性器件的零偏会受到环境温度的影响而产生变化，针对这种影响，一种方案是对器件进行温度标定，另一种方式是采用恒温模式大疆 精灵3 IMU二二.MEMS.MEMS惯性器件惯性器件2.MEMS2.MEMS陀螺误差模型陀螺误差模型陀螺的误差主要由两部分构成：漂移和刻度系数误差。陀螺的随机漂移误差主要分为三种分量：逐次启动漂移、慢变漂移、快变漂移陀螺的逐次启动漂移与系统开机时刻的电气参数、环境条件等随机性因素相关。一旦系统启动完毕，逐次启动漂移造成的误差量将保持在某一个固定值上陀螺的慢变漂移是由于其工作过程中环境条件的随机改变所造成的缓慢变化的误差，由于其变换较为缓慢，与前后时刻的陀螺漂移存在一定的相关性，随着时间点的接近依赖关系更加明显，因此可以用一阶马尔可夫过程描述：快变漂移是在上述两个漂移分量基础上的杂乱无章的高频跳变，这种漂移分量可以抽象化为白噪声过程：二二.MEMS.MEMS惯性器件惯性器件3.MEMS3.MEMS加速度计误差模型加速度计误差模型速度计的误差模型在组合导航系统设计中，一般只考虑随机常值误差，忽略相关误差，因此加速度计误差模型如式所示：MEMS陀螺仪误差修正模型MEMS加速度计误差修正模型图3.1 3KTD-565多功能三轴转台(3.1) (3.2) 三三. .导航系统传感器标定导航系统传感器标定3.1 MEMS IMU3.1 MEMS IMU加速度计、陀螺仪实验室标定加速度计、陀螺仪实验室标定位置坐标轴方向重力加速度gX轴Y轴Z轴X轴Y轴Z轴1东天南-0.008848-1.018200-0.0059962东北天-0.0317980.000362-1.0331963地东南0.9841780.029072-0.0051414西地南-0.0600091.019633-0.0091865天西南-1.054107-0.022041-0.0098946南西地-0.0366170.0000161.008865序号角速率输入值(/s)角速率输出值(/s)X轴Y轴Z轴X轴Y轴Z轴1200019.9564080.6324330.0200652-2000-20.030685-0.463136-0.04640430200-0.48642920.0317690.08837640-2000.542587-19.9020260.05770250020-0.005255-0.05406820.171637600-200.069309-0.054871-19.959693表4.2MEMS陀螺仪标定时各个轴向输出值表4.1MEMS加速度计标定时各个轴向输出值三三. .导航系统传感器标定导航系统传感器标定3.1 MEMS IMU3.1 MEMS IMU加速度计、陀螺仪实验室标定加速度计、陀螺仪实验室标定(3-3) (3-4) (3-5) (3-6) (3-7) (3-8) 将载体分别静止放置6个位置，采集加速度计的数据:由式(3-3)、(3-4)、(3-5)可解修正模型的各个参数：加速度计标定参数求解方程三三. .导航系统传感器标定导航系统传感器标定3.1 MEMS IMU3.1 MEMS IMU加速度计、陀螺仪实验室标定加速度计、陀螺仪实验室标定三三. .导航系统传感器标定导航系统传感器标定3.1 MEMS IMU3.1 MEMS IMU加速度计、陀螺仪实验室标定加速度计、陀螺仪实验室标定由于该MEMS陀螺仪无法敏感地球自转角速度，IMU的陀螺X、Y、Z轴在三次转动中的输入角速度为：陀螺的标度因数和常值零偏：MEMS陀螺仪标定采用速率标定方式。将MEMSIMU固定于三轴转台的平台上，通过给定固定速率值测量其对应输出的方式进行实验。具体步骤如下：（1）将待标定IMU安装至三轴转台，IMU的轴向与转台轴向平行，系统开机初始化；（2）X轴陀螺标定数据采集。令转台X轴正转，Y、Z轴静止，速率稳定后记录X、Y、Z轴的MEMS陀螺输出数据；之后转台X轴反转，角速率与之前相同，Y、Z轴静止，速率稳定后记录X、Y、Z轴的MEMS陀螺输出数据，完毕后转台停止旋转；（3）Y、Z轴陀螺标定数据采集，参考步骤（2）的方式进行操作；（4）求取步骤（2）、（3）中各次角速率实验中的角速率平均值，用于标定计算。三三. .导航系统传感器标定导航系统传感器标定3.1 MEMS IMU3.1 MEMS IMU加速度计、陀螺仪实验室标定加速度计、陀螺仪实验室标定X轴MEMS陀螺标定试验中，IMU的Y轴和Z轴角速率输出：将X轴正反转时分别采集的Y轴和Z轴数据的正反输出分别求差，可得陀螺的安装误差系数同理，其它安装误差系数为：MEMS陀螺仪输出数学模型：MEMS加速度计输出数学模型：(3-10) (3-11) 三三. .导航系统传感器标定导航系统传感器标定3.1 MEMS IMU3.1 MEMS IMU加速度计、陀螺仪实验室标定加速度计、陀螺仪实验室标定三三. .导航系统传感器标定导航系统传感器标定3.2 3.2 MEMS IMUMEMS IMU加速度计六面法标定加速度计六面法标定STDT00536-pointtumblesensorcalibration将将MEMS IMUMEMS IMU放置于六面立方体上，按照图示顺序翻转放置于六面立方体上，按照图示顺序翻转6 6次，分别采次，分别采集集6 6个状态的加速度计静态数据，用于校正个状态的加速度计静态数据，用于校正三三. .导航系统传感器标定导航系统传感器标定3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪不依托额外设备实现加速度计、陀螺仪不依托额外设备实现标定标定参考论文：ArobustandeasytoimplementmethodforIMUcalibrationwithoutexternalequipmDEMO链接：https:/bitbucket.org/alberto_pretto/imu_tk视频地址：https:/www.youtube.com/watch?v=XqQCbkncVYI三三. .导航系统传感器标定导航系统传感器标定3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定该校正方法是基于一个多位置方案能够修正加速度计和陀螺仪的刻度因数误差和对准误差，同时也能够估计传感器的误差。只需要将传感器放置在一组不同的静态姿态位置即可实现。通过一个有效的和快速的标定协议,利用有效的静态检测算子来可靠地检测传感器的静态时间间隔,在这个方法中，我们假设当地重力加速度g和温度是恒定的。在校正中，我们第一次在静态测量样本间隔中进行加速度计校正。然后,我们利用这些结果，通过数值积分的方法实现陀螺仪的参数校正。校正数据采集流程3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定3.3.1系统误差模型令三轴加速度计和陀螺的坐标系为AF和GF，该坐标系为非正交坐标系；令理想三轴加速度计和陀螺的坐标系为AOF和GOF，该坐标系为正交坐标系；二者之间关系为：X轴方向AOF坐标系与AF坐标系一致；AOF的Y轴位于AF的X轴和Y轴构成的平面中；GOF和GF的关系与上面类似；校正数据采集流程令载体坐标系为BF，为正交坐标系；对于小角度的旋转，非正交坐标系下的测量值SS可以旋转变换至正交坐标系下的测量值SB：3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定3.3.1系统误差模型令载体坐标系BF与AOF坐标系方向一致（重合），在该模型下，xz、xy、yz为0，旋转矩阵可简化为：如上所述,陀螺仪和加速度计的测量值应该引用同一个参考系,仍以AOF作为基准：Wo和ws表示在AOF坐标系和GF坐标系下输出的角速度ao和as表示在AOF坐标系和GF坐标系下输出的加速度3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定3.3.1系统误差模型刻度因数：零偏：加速度计误差模型：陀螺误差模型：其中，va和vg分别为加速度计的陀螺输出的测量噪声3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定3.3.2系统基本标定模型为了校准三轴加速度计，我们需要估计以下未知参数变量：定义以下函数，其中忽略了加速度计的输出噪声：放置IMU于M个位置，维持临时的静止，可获得M个静态状态的加速度计数据（在非正交坐标系AF中测量），对每个静态状态的加速度计数据求取均值，可用以下的代价函数估计加速度计的未知参数：其中，g为当地的重力加速度，通过LM法(Levenberg-Marquardt)求取上式的最小值,获得加速度计的校正参数。3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定3.3.2系统基本标定模型为了校准三轴陀螺，我们需要估计以下未知参数变量：在陀螺的校正过程中，我们需要采用加速度计的数据作为已知量，fai表示四元数的更新运算以下的代价函数估计陀螺的未知参数：其中，uak为通过修正后的加速度计数据计算获得的单位四元数，ugk为通过陀螺更新获得的四元数，通过LM法(Levenberg-Marquardt)求取上式的最小值,获得陀螺的校正参数。3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定3.3.3系统校正步骤（1）静态状态检测在该校正方案中，区分IMU的运动状态是校正执行的关键，本系统在时间间隔Twait中，采用加速度计的方差构建静态检测算子，判断系统的运动状态：通过判断s(t)与系统设定阈值的大小，实现系统静止和运动状态的判断，其中系统设定阈值在系统初始化阶段Tinit，采用加速度计的原始数据进行计算IMU数据运动与静止状态判断结果3.3.3系统校正步骤（3）Allan方差3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外加速度计、陀螺仪无外部基准标定部基准标定XsensMti陀螺的Allan方差曲线陀螺零偏的漂移可以用Allan方差进行描述，Allan方差可定义为：X(t,k)：t时间间隔的平均值K：时间间隔的数量，计算陀螺每个轴的Allan方差，从1s至225s。由图像可知，在t=50s时Allan方差收敛，在0至50s陀螺误差的主要因素是角度随机游走，曲线收敛后证明角度随机游走对陀螺零偏的影响因素减小，取这段时间的陀螺原始数据求取均值能够较好的反映陀螺的常值零偏信息。3.3.3系统校正步骤（3）Allan方差3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外加速度计、陀螺仪无外部基准标定部基准标定柳贵福.光学陀螺输入输出特性建模及补偿技术研究D.哈尔滨工程大学,2002.P513.3.3系统校正步骤（3）Allan方差MEMS陀螺光纤陀螺激光陀螺（2）光纤陀螺的Allan方差曲线主要表现为斜率1/2（0.005s和200s）和斜率1/2（1000s以上）两段，斜率为0的时间很短，可以认为零偏不稳定影响很小或不存在（3）激光陀螺在整个Allan方差曲线上主要表现为斜率为-1的量化噪声（1）MEMS陀螺的Allan方差双对数曲线表现为明显的3段：0.011s之间斜率为-1/2,5s100s斜率为0,100s以上斜率为1/2，分别对应角速随机游走、零偏不稳定性和角速率随机游走惯性仪器测试与数据分析P145-P1463.3.3系统校正步骤（4）校正流程3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定1.将IMU静止放置，初始化50秒至1分钟；2.将IMU旋转一个位置，静止20秒钟；3.重复步骤2的操作，将IMU旋转至与之前不同的位置，可以参考六面法的旋转位置，重复步骤2的次数达到10次后结束；4.将IMU静止放置20秒，结束数据采集3.3.4demo代码3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定当地重力加速度陀螺刻度因数初始化加速度计零偏初始化3.3.4demo代码3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定原始数据路径：imu_tkbintest_data加速度计原始数据：xsens_acc.mat;陀螺原始数据：xsens_gyro.mat数据格式：时间戳（单位为秒）X轴数据Y轴数据Z轴数据文本文档格式：以UTF-8无BOM格式编码，否则程序运行会报错http:/ceres-solver.org/building.htmlhttps:/bitbucket.org/alberto_pretto/imu_tkCeres-slover数学库安装：Ceres-slover数学库安装：该方案优点：不需要额外的设备辅助标定（例如三轴转台），对IMU的摆放位置没有特殊的要求（相比于六面法标定），可以实现IMU的三轴陀螺的标定该方案缺点：加速度计标定时要预先设定传感器的零偏，若设置于实际值有很大偏差校正结果误差很大三三. .导航系统传感器标定导航系统传感器标定3.3 MEMS IMU3.3 MEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定三三. .导航系统传感器标定导航系统传感器标定3.4 3.4 加速度计计算姿态角加速度计计算姿态角Ax+Ay+Az=g令加速度计三轴输出数据为Ax，Ay，Az；当地重力加速度为g，则有如下关系：令旋转后坐标系Xb轴与水平面XOY的夹角为pitch，则重力加速度在该轴的分量为：Ax=sin(pitch)*gsin(pitch)=Ax/g;Cos(pitch)=（1-sin（pitch）=（1-（Ax/g）三三. .导航系统传感器标定导航系统传感器标定北东地（NED）坐标系东北天（ENU）坐标系EU ON俯仰横滚航向四元数法的能够对载体的刚体运动进行简洁的描述汉密尔顿四元数规则JPL四元数规则四四. .四元数四元数四元数的乘法的表现形式四四. .四元数四元数令：由四元数表示的载体姿态矩阵:四四. .四元数四元数四元数的姿态微分方程形式:式中：四元数的增量更新采用四阶龙格-库塔法:四阶龙格-库塔法中的加和运算，会使更新得到的增量四元数为非单位四元数,需要进行归一化四四. .四元数四元数四元数更新:乘性四元数：加性四元数：四四. .四元数四元数五五. . 基于乘性四元数基于乘性四元数EKFEKF滤波器滤波器对误差四元数求导可得：AHRS系统的误差状态量五五. . 基于乘性四元数基于乘性四元数EKFEKF滤波器滤波器令：对F进行离散化，可得过程状态转移矩阵：系统的误差状态传递模型:五五. . 基于乘性四元数基于乘性四元数EKFEKF滤波器滤波器对系统噪声方差阵进行离散化：综上，可得系统的误差状态模型五五. . 基于乘性四元数基于乘性四元数EKFEKF滤波器滤波器AHRS系统误差观测模型建立当地重力场、当地磁场在地理坐标系的投影：五五. . 基于乘性四元数基于乘性四元数EKFEKF滤波器滤波器综上：同理：综上所述，AHRS系统误差观测模型：五五. . 基于乘性四元数基于乘性四元数EKFEKF滤波器滤波器系统的误差观测量、观测转移矩阵：系统量测噪声方差阵：五五. . 基于乘性四元数基于乘性四元数EKFEKF滤波器滤波器参考资料：参考资料：1、Trawny N, Roumeliotis S I. Indirect Kalman filter for 3D attitude estimationJ. University of Minnesota, Dept. of Comp. Sci. & Eng., Tech. Rep, 2005, 2: 2005.2、秦永元.惯性导航M.北京：科学出版社，2006.3、秦永元，张洪钺，汪叔华. 卡尔曼滤波与组合导航原理（第2版）M. 西安：西北工业大学出版社, 2011.4、严恭敏，李四海，秦永元. 惯性仪器测试与数据分析M. 北京：科学出版社, 2011.谢谢！谢谢！