1、MEMS IMU的入门与应用,一、导航系统基本概念 二、MEMS惯性器件； 三、MEMS惯性器件的简易标定； 四、四元数； 五、基于乘性四元数EKF滤波器,一、导航系统基本概念,1.海里,国际上采用1852m作为标准海里长度，1nm=1852m,2.惯性级导航系统,运行1h过程中在水平面上定位误差等于1nm（简记1nm/h）的惯性导航系统称为惯性级导航系统,3.惯性级陀螺,地球自转角速率为15.04107度/h.其千分之一为0.015度/h,称为毫地转率；将精度达到0.015度/h的陀螺称为惯性级陀螺，往往也以1meru的量级(0.01度/h)表示惯性级陀螺精度,4.加速度计,高精度：1mg;
2、,惯性级导航系统对陀螺的最低要求为0.01度/h，加速度计精度的最低要求为1x10-4g,二.MEMS惯性器件,1.消费级MEMS惯性器件,Invensense MPU6000/MPU6050,Invensense公司的MPU6000/MPU6050六轴惯性器件是世界上首个六轴集成的低成本MEMS惯性器件，该器件推出市场之后在各个领域（特别是四轴飞行器）得到了广泛应用,二.MEMS惯性器件,1.消费级MEMS惯性器件,在2010年，apple的iPhone4采用了ST公司的三轴陀螺仪L3G4200D和三轴加速度计LIS331DLH作为手机的惯性传感器。其它手机厂商随即跟进，陀螺仪与加速度计逐渐
3、成为智能手机的标配传感器,iPhone4 IMU,二.MEMS惯性器件,2.工业级MEMS惯性器件,ADIS16405,ADIS16488A,Xsens Mti,二.MEMS惯性器件,2.常见MEMS惯性器件参数对比,二.MEMS惯性器件,2.常见MEMS惯性器件参数对比,二.MEMS惯性器件,2.MEMS惯性器件的常见技术参数,（2）非线性度,（3）初始偏置误差,MEMS惯性器件在开机后系统稳定输出后观测信息中包含的误差，可以通过静态情况下的初始化求取均值消除,（1）量程,MEMS惯性器件在开机后系统稳定输出后观测信息中包含的误差，可以通过静态情况下的初始化求取均值消除,二.MEMS惯性器件
4、,2.MEMS惯性器件的常见技术参数,（4）对齐误差,参考文献：MEMS陀螺仪中主要噪声源的预测和管理 ADI公司 应用工程师 Mark Looney,三轴式陀螺仪对齐误差,MEMS IMU通常具有两种类型的对齐误差，它们相互关联，但在系统级建模中具有不同应用： 轴到封装和轴到轴。“ 轴到封装对齐误差”描述陀螺仪相对于器件封装上特定机械特性的对齐情况。将IMU安装到系统后，如果系统无法支持惯性对齐，则轴到封装对齐误差将成为整体对齐误差的主要因素之一。系统与IMU的机械接口的机械缺陷也会增加整体对齐误差。“ 轴到轴对齐误差”描述各个陀螺仪旋转轴相对于其他两个陀螺仪的相对对齐精度。在系统可以实现简
5、单的对齐过程时，此参数影响最大，此时通常沿系统的惯性参考系中的一个轴直线移动整个组件(IMU已安装在系统平台上)，同时需要观察传感器。,二.MEMS惯性器件,2.MEMS惯性器件的常见技术参数,（6）随机游走，噪声密度,随机游走过程：当前观测值完全由下一时刻观测值加上现时噪声决定，即n时刻的输出由n-1时刻的输出和随机误差和加和决定,陀螺角度游走过程：宽带角速率白噪声积分的结果，即陀螺从0时刻累积的总角增量误差表现为随机游走，而每一时刻的等效角速率误差表现为白噪声,噪声密度：传感器固有噪声代表的是陀螺仪在静态惯性和环境条件下运行时其输出中的随机振动。 MEMS IMU数据手册通常会提供速率噪声
6、密度(RND)参数来描述陀螺仪相对于频率的固有噪声。此参数通常使用单位/s/Hz，是预测特定滤波器配置固有噪声的关键。,单位： /hr,功率谱N的国际单位： （rad/s）/Hz,（6）随机游走，噪声密度,二.MEMS惯性器件,2.MEMS惯性器件的常见技术参数,信号方差强度q、功率谱密度S、随机游走系数N之间的简单换算关系：,1、令陀螺输出角速率方差的单位为（rad/s），离散系统等效激励白噪声的方差等于连续时间系统白噪声方差强度与离散化周期的乘积：,2、功率谱密度S（单位（rad/s）/Hz）与方差q的关系：,Qk=q*Ts,S=q/Ts,3、角度随机游走N（单位/h）与功率谱密度S的关系
7、：,N= (3600*q)/60,二.MEMS惯性器件,2.MEMS惯性器件的常见技术参数,（7）偏置温度系数（温飘）,MEMS惯性器件的零偏会受到环境温度的影响而产生变化，针对这种影响，一种方案是对器件进行温度标定，另一种方式是采用恒温模式,大疆 精灵3 IMU,二.MEMS惯性器件,2.MEMS陀螺误差模型,陀螺的误差主要由两部分构成：漂移和刻度系数误差。,陀螺的随机漂移误差主要分为三种分量：逐次启动漂移、慢变漂移、快变漂移,陀螺的逐次启动漂移与系统开机时刻的电气参数、环境条件等随机性因素相关。一旦系统启动完毕，逐次启动漂移造成的误差量将保持在某一个固定值上,陀螺的慢变漂移是由于其工作过程
8、中环境条件的随机改变所造成的缓慢变化的误差，由于其变换较为缓慢，与前后时刻的陀螺漂移存在一定的相关性，随着时间点的接近依赖关系更加明显，因此可以用一阶马尔可夫过程描述：,快变漂移是在上述两个漂移分量基础上的杂乱无章的高频跳变，这种漂移分量可以抽象化为白噪声过程：,二.MEMS惯性器件,3.MEMS加速度计误差模型,速度计的误差模型在组合导航系统设计中，一般只考虑随机常值误差，忽略相关误差，因此加速度计误差模型如式所示：, = 0 0 0 + 1 2 1 2 1 2 . ,MEMS陀螺仪误差修正模型, = 0 0 0 + 1 2 1 2 1 2 . ,MEMS加速度计误差修正模型,图3.1 3K
9、TD-565多功能三轴转台,陀螺零偏： 0 、 0 、 0 标度因数： 、 、 安装误差： 1 、 2 、 1 、 1 、 2,加速度计零偏： 0 、 0 、 0 标度因数： 、 、 安装误差： 1 、 2 、 1 、 2 、 1 、 2,(3.1),(3.2),三.导航系统传感器标定,3.1 MEMS IMU加速度计、陀螺仪实验室标定,表4.2 MEMS陀螺仪标定时各个轴向输出值,表4.1 MEMS加速度计标定时各个轴向输出值,三.导航系统传感器标定,3.1 MEMS IMU加速度计、陀螺仪实验室标定,(3-3),(3-4),(3-5),(3-6),(3-7),(3-8),将载体分别静止放置
10、6个位置，采集加速度计的数据:,由式(3-3)、(3-4)、(3-5)可解修正模型的各个参数：,加速度计标定参数求解方程,三.导航系统传感器标定,3.1 MEMS IMU加速度计、陀螺仪实验室标定,三.导航系统传感器标定,3.1 MEMS IMU加速度计、陀螺仪实验室标定,由于该MEMS陀螺仪无法敏感地球自转角速度，IMU的陀螺X、Y、Z轴在三次转动中的输入角速度为：,陀螺的标度因数和常值零偏：,MEMS陀螺仪标定采用速率标定方式。将MEMS IMU固定于三轴转台的平台上，通过给定固定速率值测量其对应输出的方式进行实验。具体步骤如下： （1）将待标定IMU安装至三轴转台，IMU的轴向与转台轴向
11、平行，系统开机初始化； （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 IMU加速度计、陀螺仪实验室标定,X轴MEMS陀螺标定试验中，IMU的Y轴和Z轴角速率输出：,将X轴正反转时分别采集的Y轴和Z轴数据的正反输出分别求差，可得陀螺的
12、安装误差系数,同理，其它安装误差系数为：,MEMS陀螺仪输出数学模型：,MEMS加速度计输出数学模型：,(3-10),(3-11),三.导航系统传感器标定,3.1 MEMS IMU加速度计、陀螺仪实验室标定,三.导航系统传感器标定,3.2 MEMS IMU加速度计六面法标定,ST DT0053 6-point tumble sensor calibration,将MEMS IMU放置于六面立方体上，按照图示顺序翻转6次，分别采集6个状态的加速度计静态数据，用于校正,三.导航系统传感器标定,3.3 MEMS IMU加速度计、陀螺仪不依托额外设备实现标定,参考论文：A robust and eas
13、y to implement method for IMU calibration without external equipm DEMO链接：https:/bitbucket.org/alberto_pretto/imu_tk,视频地址：https:/ MEMS IMU加速度计、陀螺仪无外部基准标定,该校正方法是基于一个多位置方案能够修正加速度计和陀螺仪的刻度因数误差和对准误差，同时也能够估计传感器的误差。只需要将传感器放置在一组不同的静态姿态位置即可实现。通过一个有效的和快速的标定协议,利用有效的静态检测算子来可靠地检测传感器的静态时间间隔,在这个方法中，我们假设当地重力加速度g和温度是
14、恒定的。在校正中，我们第一次在静态测量样本间隔中进行加速度计校正。然后,我们利用这些结果，通过数值积分的方法实现陀螺仪的参数校正。,校正数据采集流程,3.3 MEMS IMU加速度计、陀螺仪无外部基准标定,3.3.1 系统误差模型,令三轴加速度计和陀螺的坐标系为AF和GF，该坐标系为非正交坐标系； 令理想三轴加速度计和陀螺的坐标系为AOF和GOF，该坐标系为正交坐标系；二者之间关系为： X轴方向AOF坐标系与AF坐标系一致； AOF的Y轴位于AF的X轴和Y轴构成的平面中；GOF和GF的关系与上面类似；,校正数据采集流程,令载体坐标系为BF，为正交坐标系；对于小角度的旋转，非正交坐标系下的测量值
15、SS可以旋转变换至正交坐标系下的测量值SB：,3.3 MEMS IMU加速度计、陀螺仪无外部基准标定,3.3.1 系统误差模型,令载体坐标系BF与AOF坐标系方向一致（重合），在该模型下，xz、xy、yz为0，旋转矩阵可简化为：,如上所述,陀螺仪和加速度计的测量值应该引用同一个参考系,仍以AOF作为基准：,Wo和ws表示在AOF坐标系和GF坐标系下输出的角速度,ao和as表示在AOF坐标系和GF坐标系下输出的加速度,3.3 MEMS IMU加速度计、陀螺仪无外部基准标定,3.3.1 系统误差模型,刻度因数：,零偏：,加速度计误差模型：,陀螺误差模型：,其中，va和vg分别为加速度计的陀螺输出的
16、测量噪声,3.3 MEMS IMU加速度计、陀螺仪无外部基准标定,3.3.2 系统基本标定模型,为了校准三轴加速度计，我们需要估计以下未知参数变量：,定义以下函数，其中忽略了加速度计的输出噪声：,放置IMU于M个位置，维持临时的静止，可获得M个静态状态的加速度计数据（在非正交坐标系AF中测量），对每个静态状态的加速度计数据求取均值，可用以下的代价函数估计加速度计的未知参数：,其中，g为当地的重力加速度，通过LM法(Levenberg-Marquardt)求取上式的最小值,获得加速度计的校正参数。,3.3 MEMS IMU加速度计、陀螺仪无外部基准标定,3.3.2 系统基本标定模型,为了校准三轴
17、陀螺，我们需要估计以下未知参数变量：,在陀螺的校正过程中，我们需要采用加速度计的数据作为已知量，fai表示四元数的更新运算,以下的代价函数估计陀螺的未知参数：,其中，uak为通过修正后的加速度计数据计算获得的单位四元数，ugk为通过陀螺更新获得的四元数，通过LM法(Levenberg-Marquardt)求取上式的最小值,获得陀螺的校正参数。,3.3 MEMS IMU加速度计、陀螺仪无外部基准标定,3.3.3 系统校正步骤,（1）静态状态检测,在该校正方案中，区分IMU的运动状态是校正执行的关键，本系统在时间间隔Twait中，采用加速度计的方差构建静态检测算子，判断系统的运动状态：,通过判断s
18、(t)与系统设定阈值的大小，实现系统静止和运动状态的判断，其中系统设定阈值在系统初始化阶段Tinit，采用加速度计的原始数据进行计算,IMU数据运动与静止状态判断结果,3.3.3 系统校正步骤,（3）Allan方差,3.3 MEMS IMU加速度计、陀螺仪无外部基准标定,Xsens Mti陀螺的Allan方差曲线,陀螺零偏的漂移可以用Allan方差进行描述，Allan方差可定义为：,X(t,k)：t时间间隔的平均值 K：时间间隔的数量，,计算陀螺每个轴的Allan方差，从1s至225s。由图像可知，在t=50s时Allan方差收敛，在0至50s陀螺误差的主要因素是角度随机游走，曲线收敛后证明角
19、度随机游走对陀螺零偏的影响因素减小，取这段时间的陀螺原始数据求取均值能够较好的反映陀螺的常值零偏信息。,3.3.3 系统校正步骤,（3）Allan方差,3.3 MEMS IMU加速度计、陀螺仪无外部基准标定,柳贵福. 光学陀螺输入输出特性建模及补偿技术研究D. 哈尔滨工程大学, 2002. P51,3.3.3 系统校正步骤,（3）Allan方差,MEMS陀螺,光纤 陀螺,激光陀螺,（2）光纤陀螺的Allan方差曲线主要表现为斜率1/2（0.005s和200s）和斜率1/2（1000s以上）两段，斜率为0的时间很短，可以认为零偏不稳定影响很小或不存在,（3）激光陀螺在整个Allan方差曲线上主要
20、表现为斜率为-1的量化噪声,（1）MEMS陀螺的Allan方差双对数曲线表现为明显的3段：0.011s之间斜率为-1/2,5s100s斜率为0,100s以上斜率为1/2，分别对应角速随机游走、零偏不稳定性和角速率随机游走,惯性仪器测试与数据分析 P145-P146,3.3.3 系统校正步骤,（4）校正流程,3.3 MEMS IMU加速度计、陀螺仪无外部基准标定,1.将IMU静止放置，初始化50秒至1分钟；,2.将IMU旋转一个位置，静止20秒钟；,3.重复步骤2的操作，将IMU旋转至与之前不同的位置，可以参考六面法的旋转位置，重复步骤2的次数达到10次后结束；,4.将IMU静止放置20秒，结束
21、数据采集,3.3.4 demo代码,3.3 MEMS IMU加速度计、陀螺仪无外部基准标定,当地重力加速度,陀螺刻度因数初始化,加速度计零偏初始化,3.3.4 demo代码,3.3 MEMS IMU加速度计、陀螺仪无外部基准标定,原始数据路径：imu_tkbintest_data,加速度计原始数据：xsens_acc.mat;陀螺原始数据：xsens_gyro.mat,数据格式：时间戳（单位为秒） X轴数据 Y轴数据 Z轴数据 文本文档格式：以UTF-8无BOM格式编码，否则程序运行会报错,http:/ceres-solver.org/building.html,https:/bitbucke
22、t.org/alberto_pretto/imu_tk,Ceres-slover数学库安装：,Ceres-slover数学库安装：,该方案优点：不需要额外的设备辅助标定（例如三轴转台），对IMU的摆放位置没有特殊的要求（相比于六面法标定），可以实现IMU的三轴陀螺的标定,该方案缺点：加速度计标定时要预先设定传感器的零偏，若设置于实际值有很大偏差校正结果误差很大,三.导航系统传感器标定,3.3 MEMS IMU加速度计、陀螺仪无外部基准标定,三.导航系统传感器标定,3.4 加速度计计算姿态角,Ax+Ay+Az=g,令加速度计三轴输出数据为Ax，Ay，Az；当地重力加速度为g，则有如下关系：,令旋
23、转后坐标系Xb轴与水平面XOY的夹角为pitch，则重力加速度在该轴的分量为：,Ax = sin(pitch)*g sin(pitch)=Ax/g; Cos(pitch)= （1-sin（pitch） =（1-（ Ax/g ）,三.导航系统传感器标定,北东地（NED）坐标系,东北天（ENU）坐标系,俯仰,横滚,航向,四元数法的能够对载体的刚体运动进行简洁的描述,汉密尔顿四元数规则,JPL四元数规则,四.四元数,四元数的逆 1 等于其共轭 ,四元数的乘法的表现形式,四.四元数, 为 反对称阵：,令：,由四元数表示的载体姿态矩阵:,四.四元数,四元数的姿态微分方程形式:,式中：,四元数的增量更新采
24、用四阶龙格-库塔法:,四阶龙格-库塔法中的加和运算，会使更新得到的增量四元数为非单位四元数,需要进行归一化,四.四元数,四元数更新:,乘性四元数：,加性四元数：,四.四元数,令系统的误差四元数为 ，由于误差四元数 在系统姿态更新中非常小，因此可以用小角度旋转近似描述四元数：,对陀螺的模型进行建立： 陀螺的逐次启动漂移在系统初始化求取零偏后消除，令陀螺的角速度输入值为 ，陀螺零偏为，陀螺信号噪声为 ：,五. 基于乘性四元数EKF滤波器,对误差四元数求导,可得：,k时刻陀螺的零偏为，k-1时刻陀螺的零偏为 ，陀螺的零偏增量为,AHRS系统的误差状态量,五. 基于乘性四元数EKF滤波器,令：,对F进
25、行离散化，可得过程状态转移矩阵：,系统的误差状态传递模型:,五. 基于乘性四元数EKF滤波器,对系统噪声方差阵进行离散化：,综上，可得系统的误差状态模型,五. 基于乘性四元数EKF滤波器,AHRS系统误差观测模型建立,当地重力场、当地磁场在地理坐标系的投影：,以 _ 部分为例，对AHRS系统的误差观测模型进行推导, ：k时刻状态更新后的四元数 构成的捷联矩阵 ：k-1时刻四元数 构成的捷联矩阵,对 进行小角度近似：,五. 基于乘性四元数EKF滤波器,综上：,同理：,综上所述，AHRS系统误差观测模型：,五. 基于乘性四元数EKF滤波器,系统的误差观测量、观测转移矩阵：,系统量测噪声方差阵：,五
26、. 基于乘性四元数EKF滤波器,参考资料：,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.,谢谢！,
