MEMS IMU的入门与应用

MEMSIMU的入门与应用一、导航系统基本概念二、MEMS惯性器件；三、MEMS惯性器件的简易标定；四、四元数；五、基于乘性四元数EKF滤波器2021/6/272一、导航系统基本概念1.海里国际上采用1852m作为标准海里长度，1nm=1852m2.惯性级导航系统运行1h过程中在水平面上定位误差等于1nm（简记1nm/h）的惯性导航系统称为惯性级导航系统3.惯性级陀螺地球自转角速率为15.04107度/h.其千分之一为0.015度/h,称为毫地转率；将精度达到0.015度/h的陀螺称为惯性级陀螺，往往也以1meru的量级(0.01度/h)表示惯性级陀螺精度4.加速度计高精度：<10-4g;中精度：10-4g至1mg;低精度：>1mg;惯性级导航系统对陀螺的最低要求为0.01度/h，加速度计精度的最低要求为1x10-4g2021/6/273二.MEMS惯性器件1.消费级MEMS惯性器件InvensenseMPU6000/MPU6050Invensense公司的MPU6000/MPU6050六轴惯性器件是世界上首个六轴集成的低成本MEMS惯性器件，该器件推出市场之后在各个领域（特别是四轴飞行器）得到了广泛应用2021/6/274二.MEMS惯性器件1.消费级MEMS惯性器件在2010年，apple的iPhone4采用了ST公司的三轴陀螺仪L3G4200D和三轴加速度计LIS331DLH作为手机的惯性传感器。其它手机厂商随即跟进，陀螺仪与加速度计逐渐成为智能手机的标配传感器iPhone4IMU手机型号MEMS加速度计MEMS陀螺iPhone4LIS331DLHL3G4200D红米MPU6050iPhone6MPU6700三星S6MPU6500三星S7LSM6SD3小米5ICG-20660L2021/6/275二.MEMS惯性器件2.工业级MEMS惯性器件ADIS16405ADIS16488AXsensMti2021/6/276二.MEMS惯性器件2.常见MEMS惯性器件参数对比加速度量程(最大)非线性度初始偏置轴间灵敏度噪声密度偏置温度系数输出噪声价格LSM303±16g

±60mg

0.15mg/sqrtHZ

±0.01量程/℃

MPU6050±16g

0.5%

±3%

±2%

0.4mg/sqrtHZ

±0.02量程/℃

MPU6500±16g

0.5%

±2%

0.3mg/sqrtHZ±0.026%量程/℃

BMI180±16g

0.5%

±25 mg

0.18mg/sqrtHZ

ADIS16405±18g

0.5%

±50mg

0.2°

0.5mg/sqrtHZ

±0.3mg/℃9mg

4500

ADIS16488±18g

0.5%

±0.035°

0.063mg/sqrtHZ

±0.0025mg/℃1.29mg

14500

2021/6/277陀螺量程(最大)非线性度(量程）对准误差初始偏置°/s线性加速度影响角速度随机游走速率噪声密度偏置温度系数输出噪声°/srms价格（RMB）L3G4200D±2000

0.2%

±75°/s

0.03±0.04°/s/℃

MPU6050±2000

0.2%

±2%量程±20°/s

0.1

0.005±20°/s

0.05

MPU6500±2000

0.1%±2%量程±5°/s

0.01±0.24°/s/℃0.1

BMI180±2000

0.1%±3°/s

0.007

MAX21000±20000.2%1%±2°/s0.0090.450.009±2%停产ADIS16405±350±0.1%

±0.05°±3°/s

0.007

2

0.044±0.01°/s/℃0.8

4500

ADIS16488A±480

±0.01%

±0.05°

0.26

0.0059±0.0025

0.135

14500

二.MEMS惯性器件2.常见MEMS惯性器件参数对比2021/6/278二.MEMS惯性器件2.MEMS惯性器件的常见技术参数（2）非线性度（3）初始偏置误差MEMS惯性器件在开机后系统稳定输出后观测信息中包含的误差，可以通过静态情况下的初始化求取均值消除（1）量程MEMS惯性器件在开机后系统稳定输出后观测信息中包含的误差，可以通过静态情况下的初始化求取均值消除2021/6/279二.MEMS惯性器件2.MEMS惯性器件的常见技术参数（4）对齐误差参考文献：MEMS陀螺仪中主要噪声源的预测和管理ADI公司应用工程师MarkLooney三轴式陀螺仪对齐误差MEMSIMU通常具有两种类型的对齐误差，它们相互关联，但在系统级建模中具有不同应用：轴到封装和轴到轴。“轴到封装对齐误差”描述陀螺仪相对于器件封装上特定机械特性的对齐情况。将IMU安装到系统后，如果系统无法支持惯性对齐，则轴到封装对齐误差将成为整体对齐误差的主要因素之一。系统与IMU的机械接口的机械缺陷也会增加整体对齐误差。“轴到轴对齐误差”描述各个陀螺仪旋转轴相对于其他两个陀螺仪的相对对齐精度。在系统可以实现简单的对齐过程时，此参数影响最大，此时通常沿系统的惯性参考系中的一个轴直线移动整个组件(IMU已安装在系统平台上)，同时需要观察传感器。2021/6/2710二.MEMS惯性器件2.MEMS惯性器件的常见技术参数（6）随机游走，噪声密度随机游走过程：当前观测值完全由下一时刻观测值加上现时噪声决定，即n时刻的输出由n-1时刻的输出和随机误差和加和决定陀螺角度游走过程：宽带角速率白噪声积分的结果，即陀螺从0时刻累积的总角增量误差表现为随机游走，而每一时刻的等效角速率误差表现为白噪声噪声密度：传感器固有噪声代表的是陀螺仪在静态惯性和环境条件下运行时其输出中的随机振动。MEMSIMU数据手册通常会提供速率噪声密度(RND)参数来描述陀螺仪相对于频率的固有噪声。此参数通常使用单位°/s/√Hz，是预测特定滤波器配置固有噪声的关键。单位：°/√hr功率谱N²的国际单位：

（rad/s）²/Hz2021/6/2711（6）随机游走，噪声密度二.MEMS惯性器件2.MEMS惯性器件的常见技术参数信号方差强度q、功率谱密度S、随机游走系数N之间的简单换算关系：1、令陀螺输出角速率方差的单位为（rad/s）²，离散系统等效激励白噪声的方差等于连续时间系统白噪声方差强度与离散化周期的乘积：2、功率谱密度S（单位（rad/s）²/Hz）与方差q的关系：Qk=q*TsS=q/Ts3、角度随机游走N（单位°/√h）与功率谱密度S的关系：N=√(3600²*q)/602021/6/2712二.MEMS惯性器件2.MEMS惯性器件的常见技术参数（7）偏置温度系数（温飘）MEMS惯性器件的零偏会受到环境温度的影响而产生变化，针对这种影响，一种方案是对器件进行温度标定，另一种方式是采用恒温模式大疆

精灵3IMU2021/6/2713二.MEMS惯性器件2.MEMS陀螺误差模型陀螺的误差主要由两部分构成：漂移和刻度系数误差。陀螺的随机漂移误差主要分为三种分量：逐次启动漂移、慢变漂移、快变漂移陀螺的逐次启动漂移与系统开机时刻的电气参数、环境条件等随机性因素相关。一旦系统启动完毕，逐次启动漂移造成的误差量将保持在某一个固定值上2021/6/2714陀螺的慢变漂移是由于其工作过程中环境条件的随机改变所造成的缓慢变化的误差，由于其变换较为缓慢，与前后时刻的陀螺漂移存在一定的相关性，随着时间点的接近依赖关系更加明显，因此可以用一阶马尔可夫过程描述：快变漂移是在上述两个漂移分量基础上的杂乱无章的高频跳变，这种漂移分量可以抽象化为白噪声过程：2021/6/2715二.MEMS惯性器件3.MEMS加速度计误差模型速度计的误差模型在组合导航系统设计中，一般只考虑随机常值误差，忽略相关误差，因此加速度计误差模型如式所示：2021/6/2716MEMS陀螺仪误差修正模型MEMS加速度计误差修正模型图3.13KTD-565多功能三轴转台(3.1)

(3.2)

三.导航系统传感器标定3.1MEMSIMU加速度计、陀螺仪实验室标定2021/6/2717位置坐标轴方向重力加速度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.1MEMSIMU加速度计、陀螺仪实验室标定2021/6/2718(3-3)

(3-4)

(3-5)

(3-6)

(3-7)

(3-8)

将载体分别静止放置6个位置，采集加速度计的数据:由式(3-3)、(3-4)、(3-5)可解修正模型的各个参数：加速度计标定参数求解方程三.导航系统传感器标定3.1MEMSIMU加速度计、陀螺仪实验室标定2021/6/2719三.导航系统传感器标定3.1MEMSIMU加速度计、陀螺仪实验室标定由于该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）中各次角速率实验中的角速率平均值，用于标定计算。2021/6/2720三.导航系统传感器标定3.1MEMSIMU加速度计、陀螺仪实验室标定X轴MEMS陀螺标定试验中，IMU的Y轴和Z轴角速率输出：将X轴正反转时分别采集的Y轴和Z轴数据的正反输出分别求差，可得陀螺的安装误差系数同理，其它安装误差系数为：2021/6/2721MEMS陀螺仪输出数学模型：MEMS加速度计输出数学模型：(3-10)

(3-11)

三.导航系统传感器标定3.1MEMSIMU加速度计、陀螺仪实验室标定2021/6/2722三.导航系统传感器标定3.2MEMSIMU加速度计六面法标定STDT00536-pointtumblesensorcalibration将MEMSIMU放置于六面立方体上，按照图示顺序翻转6次，分别采集6个状态的加速度计静态数据，用于校正2021/6/2723三.导航系统传感器标定3.3MEMSIMU加速度计、陀螺仪不依托额外设备实现标定参考论文：ArobustandeasytoimplementmethodforIMUcalibrationwithoutexternalequipmDEMO链接：/alberto_pretto/imu_tk视频地址：/watch?v=XqQCbkncVYI2021/6/2724三.导航系统传感器标定3.3MEMSIMU加速度计、陀螺仪无外部基准标定

该校正方法是基于一个多位置方案能够修正加速度计和陀螺仪的刻度因数误差和对准误差，同时也能够估计传感器的误差。只需要将传感器放置在一组不同的静态姿态位置即可实现。通过一个有效的和快速的标定协议,利用有效的静态检测算子来可靠地检测传感器的静态时间间隔,在这个方法中，我们假设当地重力加速度g和温度是恒定的。在校正中，我们第一次在静态测量样本间隔中进行加速度计校正。然后,我们利用这些结果，通过数值积分的方法实现陀螺仪的参数校正。校正数据采集流程2021/6/27253.3MEMSIMU加速度计、陀螺仪无外部基准标定3.3.1系统误差模型

令三轴加速度计和陀螺的坐标系为AF和GF，该坐标系为非正交坐标系；令理想三轴加速度计和陀螺的坐标系为AOF和GOF，该坐标系为正交坐标系；二者之间关系为：X轴方向AOF坐标系与AF坐标系一致；AOF的Y轴位于AF的X轴和Y轴构成的平面中；GOF和GF的关系与上面类似；校正数据采集流程令载体坐标系为BF，为正交坐标系；对于小角度的旋转，非正交坐标系下的测量值SS可以旋转变换至正交坐标系下的测量值SB：2021/6/27263.3MEMSIMU加速度计、陀螺仪无外部基准标定3.3.1系统误差模型

令载体坐标系BF与AOF坐标系方向一致（重合），在该模型下，xz、xy、yz为0，旋转矩阵可简化为：如上所述,陀螺仪和加速度计的测量值应该引用同一个参考系,仍以AOF作为基准：Wo和ws表示在AOF坐标系和GF坐标系下输出的角速度ao和as表示在AOF坐标系和GF坐标系下输出的加速度2021/6/27273.3MEMSIMU加速度计、陀螺仪无外部基准标定3.3.1系统误差模型

刻度因数：零偏：加速度计误差模型：陀螺误差模型：其中，va和vg分别为加速度计的陀螺输出的测量噪声2021/6/27283.3MEMSIMU加速度计、陀螺仪无外部基准标定3.3.2系统基本标定模型

为了校准三轴加速度计，我们需要估计以下未知参数变量：定义以下函数，其中忽略了加速度计的输出噪声：放置IMU于M个位置，维持临时的静止，可获得M个静态状态的加速度计数据（在非正交坐标系AF中测量），对每个静态状态的加速度计数据求取均值，可用以下的代价函数估计加速度计的未知参数：其中，g为当地的重力加速度，通过LM法(Levenberg-Marquardt)求取上式的最小值,获得加速度计的校正参数。2021/6/27293.3MEMSIMU加速度计、陀螺仪无外部基准标定3.3.2系统基本标定模型

为了校准三轴陀螺，我们需要估计以下未知参数变量：在陀螺的校正过程中，我们需要采用加速度计的数据作为已知量，fai表示四元数的更新运算以下的代价函数估计陀螺的未知参数：其中，uak为通过修正后的加速度计数据计算获得的单位四元数，ugk为通过陀螺更新获得的四元数，通过LM法(Levenberg-Marquardt)求取上式的最小值,获得陀螺的校正参数。2021/6/27303.3MEMSIMU加速度计、陀螺仪无外部基准标定3.3.3系统校正步骤

（1）静态状态检测在该校正方案中，区分IMU的运动状态是校正执行的关键，本系统在时间间隔Twait中，采用加速度计的方差构建静态检测算子，判断系统的运动状态：通过判断s(t)与系统设定阈值的大小，实现系统静止和运动状态的判断，其中系统设定阈值在系统初始化阶段Tinit，采用加速度计的原始数据进行计算IMU数据运动与静止状态判断结果2021/6/27313.3.3系统校正步骤

（3）Allan方差3.3MEMSIMU加速度计、陀螺仪无外部基准标定XsensMti陀螺的Allan方差曲线陀螺零偏的漂移可以用Allan方差进行描述，Allan方差可定义为：X(t,k)：t时间间隔的平均值K：时间间隔的数量，计算陀螺每个轴的Allan方差，从1s至225s。由图像可知，在t=50s时Allan方差收敛，在0至50s陀螺误差的主要因素是角度随机游走，曲线收敛后证明角度随机游走对陀螺零偏的影响因素减小，取这段时间的陀螺原始数据求取均值能够较好的反映陀螺的常值零偏信息。2021/6/27323.3.3系统校正步骤

（3）Allan方差3.3MEMSIMU加速度计、陀螺仪无外部基准标定柳贵福.光学陀螺输入输出特性建模及补偿技术研究[D].哈尔滨工程大学,2002.P512021/6/27333.3.3系统校正步骤

（3）Allan方差MEMS陀螺光纤陀螺激光陀螺（2）光纤陀螺的Allan方差曲线主要表现为斜率1/2（0.005s和200s）和斜率1/2（1000s以上）两段，斜率为0的时间很短，可以认为零偏不稳定影响很小或不存在（3）激光陀螺在整个Allan方差曲线上主要表现为斜率为-1的量化噪声（1）MEMS陀螺的Allan方差双对数曲线表现为明显的3段：0.01~1s之间斜率为-1/2,5s~100s斜率为0,100s以上斜率为1/2，分别对应角速随机游走、零偏不稳定性和角速率随机游走惯性仪器测试与数据分析P145-P1462021/6/27343.3.3系统校正步骤

（4）校正流程3.3MEMSIMU加速度计、陀螺仪无外部基准标定1.将IMU静止放置，初始化50秒至1分钟；2.将IMU旋转一个位置，静止20秒钟；3.重复步骤2的操作，将IMU旋转至与之前不同的位置，可以参考六面法的旋转位置，重复步骤2的次数达到10次后结束；4.将IMU静止放置20秒，结束数据采集2021/6/27353.3.4demo代码3.3MEMSIMU加速度计、陀螺仪无外部基准标定当地重力加速度陀螺刻度因数初始化加速度计零偏初始化2021/6/27363.3.4demo代码3.3MEMSIMU加速度计、陀螺仪无外部基准标定原始数据路径：imu_tk\bin\test_data加速度计原始数据：xsens_acc.mat;陀螺原始数据：xsens_gyro.mat数据格式：时间戳（单位为秒）X轴数据Y轴数据Z轴数据文本文档格式：以UTF-8无BOM格式编码，否则程序运行会报错/building.html/alberto_pretto/imu_tkCeres-slover数学库安装：Ceres-slover数学库安装：2021/6/2737该方案优点：不需要额外的设备辅助标定（例如三轴转台），对IMU的摆放位置没有特殊的要求（相比于六面法标定），可以实现IMU的三轴陀螺的标定该方案缺点：加速度计标定时要预先设定传感器的零偏，若设置于实际值有很大偏差校正结果误差很大三.导航系统传感器标定3.3MEMSIMU加速度计、陀螺仪无外部基准标定2021/6/2738三.导航系统传感器标定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）²）2021/6/2739三.导航系统传感器标定北东地（NED）坐标系东北天（ENU）坐标系EU

ON俯仰横滚航向2021/6/2740四元数法的能够对载体的刚体运动进行简洁的描述