MEMS IMU的入门与应用

1、MEMS IMUMEMS IMU的入门与应用的入门与应用一、导航系统基本概念一、导航系统基本概念二二、MEMSMEMS惯性器件；惯性器件；三、三、MEMSMEMS惯性器件的简易标定；惯性器件的简易标定；四、四元数；四、四元数；五、基于乘性四元数五、基于乘性四元数EKFEKF滤波器滤波器一、导航系统基本概念一、导航系统基本概念1.1.海里海里国际上采用国际上采用1852m作为标准海里长度，作为标准海里长度，1nm=1852m2.2.惯性级导航系统惯性级导航系统运行运行1h1h过程中在水平面上定位误差等于过程中在水平面上定位误差等于1nm1nm（简记（简记1nm/h1nm/h）的惯）的惯性导航系统

2、称为惯性级导航系统性导航系统称为惯性级导航系统3.3.惯性级陀螺惯性级陀螺地球自转角速率为地球自转角速率为15.0410715.04107度度/h./h.其千分之一为其千分之一为0.0150.015度度/h,/h,称为称为毫地转率；将精度达到毫地转率；将精度达到0.0150.015度度/h/h的陀螺称为惯性级陀螺，往往的陀螺称为惯性级陀螺，往往也以也以1meru1meru的量级的量级(0.01(0.01度度/h)/h)表示惯性级陀螺精度表示惯性级陀螺精度4.4.加速度计加速度计高精度：高精度：10-4g;1mg;1mg;惯性级导航系统对陀螺的最低要求为惯性级导航系统对陀螺的最低要求为0.010

3、.01度度/h/h，加速度计精度，加速度计精度的最低要求为的最低要求为1x10-4g1x10-4g二二.MEMS.MEMS惯性器件惯性器件1.1.消费级消费级MEMSMEMS惯性器件惯性器件InvensenseInvensense MPU6000/MPU6050 MPU6000/MPU6050InvensenseInvensense公司的公司的MPU6000/MPU6050MPU6000/MPU6050六轴惯性器件是世界上首个六轴集成六轴惯性器件是世界上首个六轴集成的低成本的低成本MEMSMEMS惯性器件，该器件推出惯性器件，该器件推出市场之后在各个领域（特别是四轴飞市场之后在各个领域（特别是

4、四轴飞行器）得到了广泛应用行器）得到了广泛应用二二.MEMS.MEMS惯性器件惯性器件1.1.消费级消费级MEMSMEMS惯性器件惯性器件在在20102010年，年，appleapple的的iPhone4iPhone4采用了采用了STST公司的三轴陀螺仪公司的三轴陀螺仪L3G4200DL3G4200D和和三三轴加速度计轴加速度计LIS331DLHLIS331DLH作为手机的作为手机的惯性传感器。其它手机厂商随即跟惯性传感器。其它手机厂商随即跟进，陀螺仪与加速度计逐渐成为智进，陀螺仪与加速度计逐渐成为智能手机的标配传感器能手机的标配传感器iPhone4 IMUiPhone4 IMU手机型号 ME

5、MS加速度计MEMS陀螺iPhone4LIS331DLHL3G4200D红米MPU6050iPhone6MPU6700三星S6MPU6500三星S7LSM6SD3小米5ICG-20660L二二.MEMS.MEMS惯性器件惯性器件2.2.工业级工业级MEMSMEMS惯性器件惯性器件ADIS16405ADIS16405ADIS16488AADIS16488AXsensXsens MtiMti二二.MEMS.MEMS惯性器件惯性器件2.2.常见常见MEMSMEMS惯性器件参数对比惯性器件参数对比加速度量程(最大)非线性度初始偏置 轴间灵敏度 噪声密度偏置温度系数输出噪声价格LSM30316g60mg

6、0.15mg/sqrtHZ0.01量程/MPU605016g0.5%3%2%0.4mg/sqrtHZ0.02量程/MPU650016g0.5%2%0.3mg/sqrtHZ0.026%量程/BMI18016g0.5%25mg0.18mg/sqrtHZADIS1640518g0.5%50mg0.20.5mg/sqrtHZ0.3mg/9mg4500ADIS1648818g0.5%0.0350.063mg/sqrtHZ0.0025mg/1.29mg14500陀螺量程(最大)非线性度(量程）对准误差初始偏置/s线性加速度影响角速度随机游走速率噪声密度偏置温度系数输出噪声/s rms价格（RMB）L3G4

7、200D2000 0.2%75/s0.030.04/s/MPU60502000 0.2% 2%量程20/s0.10.00520/s0.05MPU65002000 0.1%2%量程5/s0.010.24/s/0.1BMI1802000 0.1%3/s0.007MAX2100020000.2%1%2/s0.0090.450.0092%停产ADIS164053500.1%0.053/s 0.00720.0440.01/s/0.84500ADIS16488A4800.01%0.050.260.00590.00250.13514500二二.MEMS.MEMS惯性器件惯性器件2.2.常见常见MEMSMEM

8、S惯性器件参数对比惯性器件参数对比二二.MEMS.MEMS惯性器件惯性器件2.MEMS2.MEMS惯性器件的常见技术参数惯性器件的常见技术参数（2）非线性度）非线性度（3）初始偏置误差初始偏置误差MEMS惯性器件在开机后系统稳定输出后观测信息中包含的误差，可以通过静态情况惯性器件在开机后系统稳定输出后观测信息中包含的误差，可以通过静态情况下的初始化求取均值消除下的初始化求取均值消除（1）量程）量程MEMS惯性器件在开机后系统稳定输出后观测信息中包含的误差，可以通过静态情况惯性器件在开机后系统稳定输出后观测信息中包含的误差，可以通过静态情况下的初始化求取均值消除下的初始化求取均值消除二二.MEM

9、S.MEMS惯性器件惯性器件2.MEMS2.MEMS惯性器件的常见技术参数惯性器件的常见技术参数（4）对齐误差对齐误差参考文献：参考文献：MEMSMEMS陀螺仪中主要陀螺仪中主要噪声源噪声源的预测和的预测和管理管理 ADIADI公司公司 应用工程师应用工程师 Mark Mark LooneyLooney三轴式陀螺仪对齐误差三轴式陀螺仪对齐误差 MEMS MEMS IMUIMU通常具有两种类型的对齐误差，它们相通常具有两种类型的对齐误差，它们相互关联，但互关联，但在系统在系统级建模中具有不同应用级建模中具有不同应用：轴轴到封装和轴到轴到封装和轴到轴。 “ “ 轴到轴到封装对齐封装对齐误差误差”描

10、述陀螺仪相对于器件描述陀螺仪相对于器件封装上特定机械特性的封装上特定机械特性的对齐情况对齐情况。将。将IMUIMU安装到系统安装到系统后，如果系统无法支持惯性对齐，则后，如果系统无法支持惯性对齐，则轴到轴到封装对齐误封装对齐误差将成为整体对齐误差的主要因素之一差将成为整体对齐误差的主要因素之一。系统与。系统与IMUIMU的机械接口的机械缺陷也会增加整体对齐误差的机械接口的机械缺陷也会增加整体对齐误差。 “ “ 轴到轴轴到轴对齐对齐误差误差”描述各个陀螺仪旋转轴相描述各个陀螺仪旋转轴相对于其他两个陀螺仪的对于其他两个陀螺仪的相对对齐相对对齐精度。在系统可以实精度。在系统可以实现简单的对齐过程时，

11、此参数影响现简单的对齐过程时，此参数影响最大最大，此时通常沿，此时通常沿系统的惯性参考系中的一个轴直线移动整个系统的惯性参考系中的一个轴直线移动整个组件组件(IMU(IMU已安装在系统平台上已安装在系统平台上) )，同时需要观察传感器。，同时需要观察传感器。二二.MEMS.MEMS惯性器件惯性器件2.MEMS2.MEMS惯性器件的常见技术参数惯性器件的常见技术参数（6）随机）随机游走，噪声游走，噪声密度密度随机游走过程：当前观测值完全由下一时刻观测值加上现时噪声决定，即随机游走过程：当前观测值完全由下一时刻观测值加上现时噪声决定，即n时刻的输时刻的输出由出由n-1时刻的输出和随机误差和加和决定

12、时刻的输出和随机误差和加和决定陀螺角度游走过程：宽带角速率白噪声积分的结果，即陀螺从陀螺角度游走过程：宽带角速率白噪声积分的结果，即陀螺从0时刻累积的总角增量时刻累积的总角增量误差表现为随机游走，而每一时刻的等效角速率误差表现为白噪声误差表现为随机游走，而每一时刻的等效角速率误差表现为白噪声噪声密度：噪声密度：传感器固有噪声代表的是陀螺仪在静态惯性和环境条件下传感器固有噪声代表的是陀螺仪在静态惯性和环境条件下运行时运行时其输出中其输出中的随机振动。的随机振动。 MEMS IMUMEMS IMU数据手册通常会提供速率数据手册通常会提供速率噪声噪声密度密度(RND)(RND)参数来描述陀螺仪相参数

13、来描述陀螺仪相对于频率的固有噪声。此对于频率的固有噪声。此参数参数通常使用单位通常使用单位/s/s/HzHz，是预测特定滤波器配置固有，是预测特定滤波器配置固有噪声的噪声的关键。关键。单位：单位： / /h hr r功率谱功率谱N的国际单位：的国际单位： （radrad/s/s）/Hz/Hz（6）随机）随机游走，噪声游走，噪声密度密度二二.MEMS.MEMS惯性器件惯性器件2.MEMS2.MEMS惯性器件的常见技术参数惯性器件的常见技术参数信号方差强度信号方差强度q、功率谱密度功率谱密度S、随机游走系数、随机游走系数N之间之间的的简单换算简单换算关系关系：1、令陀螺输出角速率方差的单位为（、令

14、陀螺输出角速率方差的单位为（rad/s），离散系统等效激励白噪声的方差等于，离散系统等效激励白噪声的方差等于连续时间系统白噪声方差强度与离散化周期的乘积：连续时间系统白噪声方差强度与离散化周期的乘积：2、功率谱密度、功率谱密度S（单位（单位（radrad/s/s）/Hz/Hz）与方差）与方差q的关系：的关系：Qk=q*TsS=q/Ts3、角度随机游走、角度随机游走N（单位（单位/ /h h）与功率谱密度）与功率谱密度S的关系：的关系：N= (3600(3600* *q)/60二二.MEMS.MEMS惯性器件惯性器件2.MEMS2.MEMS惯性器件的常见技术参数惯性器件的常见技术参数（7）偏置）

15、偏置温度系数（温飘温度系数（温飘）MEMS惯性器件的零偏会受到环境温度的影响而产生变化，针对这种影响，一种方案惯性器件的零偏会受到环境温度的影响而产生变化，针对这种影响，一种方案是对器件进行温度标定，另一种方式是采用恒温模式是对器件进行温度标定，另一种方式是采用恒温模式大大疆疆 精灵精灵3 IMU二二.MEMS.MEMS惯性器件惯性器件2.MEMS2.MEMS陀螺误差模型陀螺误差模型陀螺的误差主要由两部分构成：漂移和刻度系数误差。陀螺的误差主要由两部分构成：漂移和刻度系数误差。陀螺的随机漂移误差主要分为三种分量：逐次启动漂移、慢变漂移、快变漂移陀螺的随机漂移误差主要分为三种分量：逐次启动漂移、

16、慢变漂移、快变漂移陀螺的逐次启动漂移与系统开机时刻的电气参数、环境条件等随机性因素相关。一旦系陀螺的逐次启动漂移与系统开机时刻的电气参数、环境条件等随机性因素相关。一旦系统启动完毕，逐次启动漂移造成的误差量将保持在某一个固定值上统启动完毕，逐次启动漂移造成的误差量将保持在某一个固定值上0, ,biix y z陀螺的慢变漂移是由于其工作过程中环境条件的随机改变所造成的缓慢陀螺的慢变漂移是由于其工作过程中环境条件的随机改变所造成的缓慢变化的误差，由于其变换较为缓慢，与前后时刻的陀螺漂移存在一定的相变化的误差，由于其变换较为缓慢，与前后时刻的陀螺漂移存在一定的相关性，随着时间点的接近依赖关系更加明显

17、，因此可以用一阶马尔可夫过关性，随着时间点的接近依赖关系更加明显，因此可以用一阶马尔可夫过程描述：程描述：1, ,riririGix y z 快变漂移是在上述两个漂移分量基础上的杂乱无章的高频跳变，这种漂快变漂移是在上述两个漂移分量基础上的杂乱无章的高频跳变，这种漂移分量可以抽象化为白噪声过程：移分量可以抽象化为白噪声过程： , ,gibiEtqtix y z 二二.MEMS.MEMS惯性器件惯性器件3.MEMS3.MEMS加速度计误差模型加速度计误差模型速度计的误差模型在组合导航系统设计中，一般只考虑随机常速度计的误差模型在组合导航系统设计中，一般只考虑随机常值误差，忽略相关误差，因此加速度

18、计误差模型如式所示：值误差，忽略相关误差，因此加速度计误差模型如式所示：0, ,ibiaibiix y z MEMS陀螺仪误差修正模型陀螺仪误差修正模型MEMS加速度计误差修正模型加速度计误差修正模型图图3.1 3KTD-5653KTD-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

19、.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.00525

20、5-0.05406820.171637600-200.069309-0.054871-19.959693表4.2 MEMS陀螺仪标定时各个轴向输出值表4.1 MEMS加速度计标定时各个轴向输出值三三. .导航系统传感器标定导航系统传感器标定3.1 MEMS IMU3.1 MEMS IMU加速度计、陀螺仪实验室标定加速度计、陀螺仪实验室标定1246354162012,4222xxxxxxxxxxxaxaxaxAAAAAAAAAAaSKK2356413562012,4222yyyyyyyyyyyayayayAAAAAAAAAAaSKK1345623541012,4222zzzzzzzzzzzaza

21、xazAAAAAAAAAAaSKK101202303401503602,xxaxxxaxxxaxaxxxaxxxaxaxxxaxAaKAaKAaKSAaKAaKSAaK103202301403501602,yyayayxyayyyayyyayayyyayyyayAaKSAaKAaKAaKSAaKAaK102203301402501603,zzazzzazazzzazzzazzzazzzazazAaKAaKSAaKAaKAaKAaKS(3-3) (3-4) (3-5) (3-6) (3-7) (3-8) 将载体分别静止放置将载体分别静止放置6个位置，采集加速度计的数据个位置，采集加速度计的数据:

22、由式由式(3-3)、(3-4)、(3-5)可解修正模型的各个参数：可解修正模型的各个参数：加速度计加速度计标定参数求解方程标定参数求解方程三三. .导航系统传感器标定导航系统传感器标定3.1 MEMS IMU3.1 MEMS IMU加速度计、陀螺仪实验室标定加速度计、陀螺仪实验室标定三三. .导航系统传感器标定导航系统传感器标定3.1 MEMS IMU3.1 MEMS IMU加速度计、陀螺仪实验室标定加速度计、陀螺仪实验室标定由于该由于该MEMSMEMS陀螺仪无法敏感地球自转角速度，陀螺仪无法敏感地球自转角速度，IMUIMU的陀螺的陀螺X X、Y Y、Z Z轴在三次转动中的输入轴在三次转动中的

23、输入角速度为：角速度为：100 x200y300z陀螺的标度因数和常值零陀螺的标度因数和常值零偏：偏：/ 2/ 2/ 2kxxxxkyyyykzzzzSGGSGGSGG000/ 2/ 2/ 2xxxyyyzzzGGGGGGMEMSMEMS陀螺仪标定采用速率标定方式。将陀螺仪标定采用速率标定方式。将MEMS IMUMEMS IMU固定于三轴转台的平台上，通过给定固定速率值测量其固定于三轴转台的平台上，通过给定固定速率值测量其对应输出的方式进行实验。具体步骤如下：对应输出的方式进行实验。具体步骤如下：（1 1）将待标定）将待标定IMUIMU安装至三轴转台，安装至三轴转台，IMUIMU的轴向与转台轴

24、向平行，系统开机初始化；的轴向与转台轴向平行，系统开机初始化；（2 2）X X轴陀螺标定数据采集。令转台轴陀螺标定数据采集。令转台X X轴正转，轴正转，Y Y、Z Z轴静止，速率稳定后记录轴静止，速率稳定后记录X X、Y Y、Z Z轴的轴的MEMSMEMS陀螺输出陀螺输出数据；之后转台数据；之后转台X X轴反转，角速率与之前相同，轴反转，角速率与之前相同，Y Y、Z Z轴静止，速率稳定后记录轴静止，速率稳定后记录X X、Y Y、Z Z轴的轴的MEMSMEMS陀螺输出陀螺输出数据，完毕后转台停止旋转；数据，完毕后转台停止旋转；（3 3）Y Y、Z Z轴陀螺标定数据采集，参考步骤（轴陀螺标定数据采

25、集，参考步骤（2 2）的方式进行操作；）的方式进行操作；（4 4）求取步骤（）求取步骤（2 2）、（）、（3 3）中各次角速率实验中的角速率平均值，用于标定计算。）中各次角速率实验中的角速率平均值，用于标定计算。三三. .导航系统传感器标定导航系统传感器标定3.1 MEMS IMU3.1 MEMS IMU加速度计、陀螺仪实验室标定加速度计、陀螺仪实验室标定X轴轴MEMS陀螺标定试验中，陀螺标定试验中，IMU的的Y轴和轴和Z轴角速率轴角速率输出输出：0101yyayxzzazxGKGK将将X X轴正反转时分别采集的轴正反转时分别采集的Y Y轴和轴和Z Z轴数据的正反输出分别求差，可得陀螺的安装误

26、差系数轴数据的正反输出分别求差，可得陀螺的安装误差系数11/ 2/ 2gyyyxgzzzxKGGKGG同理，同理，其它其它安装安装误差系数误差系数为：为：1222/ 2/ 2/ 2/ 2gxxxygzzzygxxxzgyyyzKGGKGGKGGKGG-0.0343181.019142-0.025580-0.0024090.0018520.0255561.018916-0.000173 .0.2471350.002376-0.0015951.021030 xxyyzzAaAaAa -0.0371380.999677-0.051451-0.0037280.0648720.0547780.99834

27、50.000040 .0.1059720.0033230.0015341.003283xxyyzzGGG MEMS陀螺仪输出数学模型陀螺仪输出数学模型：MEMS加速度计加速度计输出数学模型输出数学模型：(3-10) (3-11) 三三. .导航系统传感器标定导航系统传感器标定3.1 MEMS IMU3.1 MEMS IMU加速度计、陀螺仪实验室标定加速度计、陀螺仪实验室标定三三. .导航系统传感器标定导航系统传感器标定3.2 3.2 MEMS IMUMEMS IMU加速度计六面法标定加速度计六面法标定ST DT0053 ST DT0053 6-point tumble sensor 6-poi

28、nt tumble sensor calibrationcalibration将将MEMS IMUMEMS IMU放置于六面立方体上，按照图示顺序翻转放置于六面立方体上，按照图示顺序翻转6 6次，分别采集次，分别采集6 6个状态的加速度计个状态的加速度计静态数据，用于静态数据，用于校正校正三三. .导航系统传感器标定导航系统传感器标定3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪不依托额外设备实现标定加速度计、陀螺仪不依托额外设备实现标定参考论文：参考论文：A robust and easy to implement method for IMU calibration wi

29、thout external A robust and easy to implement method for IMU calibration without external equipmequipmDEMODEMO链接：链接：/alberto_pretto/imu_tk/alberto_pretto/imu_tk视频地址：视频地址：httpshttps:/ .导航系统传感器标定导航系统传感器标定3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定 该校正

30、方法是该校正方法是基于一个基于一个多多位置位置方案能够方案能够修正加速度计修正加速度计和和陀螺仪的刻度因数误差和对陀螺仪的刻度因数误差和对准误差，同时也能够估计传感器的准误差，同时也能够估计传感器的误差误差。只。只需要将传感器需要将传感器放置在一组放置在一组不同的静态姿态位不同的静态姿态位置即可实现。置即可实现。 通过一个有效的通过一个有效的和和快速的标定快速的标定协议协议, ,利利用有效用有效的静态检测算子来的静态检测算子来可靠地检测传感器可靠地检测传感器的静态的静态时间间隔时间间隔, ,在这个方法中，我们假设在这个方法中，我们假设当地重力加速度当地重力加速度g g和温度是恒定的。和温度是恒

31、定的。 在校正中，我们第一次在在校正中，我们第一次在静态测量样本静态测量样本间隔中进行加速度计校正。间隔中进行加速度计校正。然后然后, ,我们利用我们利用这些这些结果，通过数值积分的方法实现陀螺仪结果，通过数值积分的方法实现陀螺仪的参数校正。的参数校正。校正数据采集流程校正数据采集流程3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定3.3.1 3.3.1 系统误差模型系统误差模型 令三轴加速度计和陀螺的坐标系为令三轴加速度计和陀螺的坐标系为AFAF和和GFGF，该坐标系为非正交坐标系；，该坐标系为非正交坐标系；令理想三轴加速度计和陀螺

32、的坐标系为令理想三轴加速度计和陀螺的坐标系为AOFAOF和和GOFGOF，该坐标系为正交坐标系，该坐标系为正交坐标系； 二者二者之间关系为：之间关系为：X X轴方向轴方向AOFAOF坐标系与坐标系与AFAF坐标系一致；坐标系一致；AOFAOF的的Y Y轴位于轴位于AFAF的的X X轴和轴和Y Y轴构成的平面中；轴构成的平面中； GOF GOF和和GFGF的关系与上面类似；的关系与上面类似；校正数据采集流程校正数据采集流程令载体坐标系为令载体坐标系为BFBF，为正交坐标系为正交坐标系；对于小角度的旋；对于小角度的旋转，非正交坐标系下的测量值转，非正交坐标系下的测量值SSSS可以旋转变换至正交可以

33、旋转变换至正交坐标系下的测量值坐标系下的测量值SBSB：3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定3.3.1 3.3.1 系统误差模型系统误差模型 令载体坐标系令载体坐标系BFBF与与AOFAOF坐标系方向一致（重合），在该模型下，坐标系方向一致（重合），在该模型下，xzxz、xyxy、yzyz为为0 0，旋转矩阵可简化为：旋转矩阵可简化为：如上所述如上所述, ,陀螺仪和陀螺仪和加速度计的测量值应该加速度计的测量值应该引用同一个参考系引用同一个参考系, ,仍以仍以AOFAOF作为基准：作为基准：WoWo和和wsws表示在表示在A

34、OFAOF坐标系和坐标系和GFGF坐标系下输出的角速度坐标系下输出的角速度a ao o和和a as s表示在表示在AOFAOF坐标系和坐标系和GFGF坐标系下输出的加速度坐标系下输出的加速度3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定3.3.1 3.3.1 系统误差模型系统误差模型 刻度因数：刻度因数：零偏：零偏：加速度计误差模型：加速度计误差模型：陀螺误差模型：陀螺误差模型：其中，其中，vava和和vgvg分别为加速度计的陀螺输出的测量噪声分别为加速度计的陀螺输出的测量噪声3.3 3.3 MEMS IMUMEMS IMU加速度计

35、、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定3.3.2 3.3.2 系统基本标定模型系统基本标定模型 为了校准三轴加速度计，我们需要估计以下未知参数变量：为了校准三轴加速度计，我们需要估计以下未知参数变量：定义以下函数，其中忽略了加速度计的输出噪声：定义以下函数，其中忽略了加速度计的输出噪声：放置放置IMUIMU于于M M个位置，维持临时的静止，可获得个位置，维持临时的静止，可获得M M个静态状态的加速度计数据（在非正交个静态状态的加速度计数据（在非正交坐标系坐标系AFAF中测量），对每个静态状态的加速度计数据求取均值，可用以下的代价函数估中测量），对每个静态状态的加速度计数据求取均值

36、，可用以下的代价函数估计加速度计的未知参数：计加速度计的未知参数：其中，其中，g g为当地的重力加速度，通过为当地的重力加速度，通过LMLM法法( (LevenbergLevenberg-Marquardt)-Marquardt)求取上式的最小值求取上式的最小值, ,获获得加速度计的校正参数。得加速度计的校正参数。3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定3.3.2 3.3.2 系统基本标定模型系统基本标定模型 为了校准三轴陀螺，我们需要估计以下未知参数变量：为了校准三轴陀螺，我们需要估计以下未知参数变量：在陀螺的校正过程中，我

37、们需要采用加速度计的数据作为已知量，在陀螺的校正过程中，我们需要采用加速度计的数据作为已知量，faifai表示四元数的更表示四元数的更新运算新运算以下的代价函数估计陀螺的未知参数：以下的代价函数估计陀螺的未知参数：其中，其中，uakuak为通过修正后的加速度计数据计算获得的单位四元数，为通过修正后的加速度计数据计算获得的单位四元数，ugkugk为通过陀螺更新获为通过陀螺更新获得的四元数，得的四元数，通过通过LMLM法法( (LevenbergLevenberg-Marquardt)-Marquardt)求取上式的最小值求取上式的最小值, ,获得陀螺的获得陀螺的校正参数校正参数。3.3 3.3

38、MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定3.3.3 3.3.3 系统校正步骤系统校正步骤 （1 1）静态状态检测）静态状态检测在该校正方案中，区分在该校正方案中，区分IMUIMU的运动状态是校的运动状态是校正执行的关键，本系统在时间间隔正执行的关键，本系统在时间间隔TwaitTwait中中，采用加速度计的采用加速度计的方差构建方差构建静态检测算子，静态检测算子，判断系统的运动状态：判断系统的运动状态：通过判断通过判断s(t)s(t)与系统设定阈值的大小，实现与系统设定阈值的大小，实现系统静止和运动状态的判断，其中系统设定系统静止和运动状态的判

39、断，其中系统设定阈值在系统初始化阶段阈值在系统初始化阶段TinitTinit，采用加速度，采用加速度计的原始数据进行计算计的原始数据进行计算IMUIMU数据运动与静止状态判断结果数据运动与静止状态判断结果3.3.3 3.3.3 系统校正步骤系统校正步骤 （3 3）AllanAllan方差方差3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定XsensXsens MtiMti陀螺的陀螺的AllanAllan方差曲线方差曲线陀螺零偏的漂移可以用陀螺零偏的漂移可以用AllanAllan方差进行描述方差进行描述，AllanAllan方差可定义为

40、：方差可定义为：X(X(t,kt,k) )：t t时间间隔的平均值时间间隔的平均值K K：时间间隔的数量，时间间隔的数量，计算陀螺每个轴的计算陀螺每个轴的AllanAllan方差，从方差，从1s1s至至225s225s。由图像可知，在由图像可知，在t=50st=50s时时AllanAllan方差收敛，方差收敛，在在0 0至至50s50s陀螺误差的主要因素是角度随机游走，曲线收敛后证明角度随机游走对陀螺陀螺误差的主要因素是角度随机游走，曲线收敛后证明角度随机游走对陀螺零偏的影响因素减小，取这段时间的陀螺原始数据求取均值能够较好的反映陀螺的常零偏的影响因素减小，取这段时间的陀螺原始数据求取均值能够

41、较好的反映陀螺的常值零偏信息。值零偏信息。3.3.3 3.3.3 系统校正步骤系统校正步骤 （3 3）AllanAllan方差方差3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定柳贵福柳贵福. . 光学陀螺输入输出特性建模及补偿技术研究光学陀螺输入输出特性建模及补偿技术研究D. D. 哈尔滨工程大学哈尔滨工程大学, 2002, 2002. P51. P513.3.3 3.3.3 系统校正步骤系统校正步骤 （3 3）AllanAllan方差方差MEMSMEMS陀螺陀螺光纤光纤陀螺陀螺激光陀螺激光陀螺（2 2）光纤陀螺的）光纤陀螺的All

42、anAllan方差曲线主要表现为斜率方差曲线主要表现为斜率1/21/2（0.005s0.005s和和200s200s）和斜率）和斜率1/21/2（1000s1000s以上）两段，斜率以上）两段，斜率为为0 0的时间很短，可以认为零偏不稳定影响很小或不存的时间很短，可以认为零偏不稳定影响很小或不存在在（3 3）激光陀螺在整个）激光陀螺在整个AllanAllan方差曲线上主要表现为斜率方差曲线上主要表现为斜率为为-1-1的量化噪声的量化噪声（1 1）MEMSMEMS陀螺的陀螺的AllanAllan方差双对数曲线表现为明显的方差双对数曲线表现为明显的3 3段：段：0.011s0.011s之间斜率为之

43、间斜率为-1/2,5s100s-1/2,5s100s斜率为斜率为0,100s0,100s以以上斜率为上斜率为1/21/2，分别对应角速随机游走、零偏不稳定性，分别对应角速随机游走、零偏不稳定性和角速率随机游走和角速率随机游走惯性仪器测试与数据分析惯性仪器测试与数据分析 P145-P146P145-P1463.3.3 3.3.3 系统校正步骤系统校正步骤 （4 4）校正流程）校正流程3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定1.1.将将IMUIMU静止放置，初始化静止放置，初始化5050秒至秒至1 1分钟；分钟；2.2.将将IMU

44、IMU旋转一个位置，静止旋转一个位置，静止2020秒钟；秒钟；3 3. .重复步骤重复步骤2 2的操作，将的操作，将IMUIMU旋转至与之前不同的位置，可以参考六面法的旋转旋转至与之前不同的位置，可以参考六面法的旋转位置，重复步骤位置，重复步骤2 2的次数达到的次数达到1010次后结束；次后结束；4.4.将将IMUIMU静止放置静止放置2020秒，结束数据采集秒，结束数据采集3.3.4 demo3.3.4 demo代码代码3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定当地重力加速度当地重力加速度陀螺刻度因数初始化陀螺刻度因数初始化加

45、速度计零偏初始化加速度计零偏初始化3.3.4 demo3.3.4 demo代码代码3.3 3.3 MEMS IMUMEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定原始数据路径：原始数据路径：imu_tkimu_tkbinbintest_datatest_data加速度计原始数据：加速度计原始数据：xsens_acc.matxsens_acc.mat; ;陀螺原始数据：陀螺原始数据：xsens_gyro.matxsens_gyro.mat数据格式：时间戳（单位为秒）数据格式：时间戳（单位为秒） X X轴数据轴数据 Y Y轴数据轴数据 Z Z轴数据轴数据文本文档格式：以

46、文本文档格式：以UTF-8UTF-8无无BOMBOM格式编码，否则程序运行会报错格式编码，否则程序运行会报错/building./building.htmlhtml/alberto_pretto/imu_tk/alberto_pretto/imu_tkCeres-Ceres-sloverslover数学库安装：数学库安装：Ceres-Ceres-sloverslover数学库安装：数学库安装：该方案优点：该方案优点：不不需要额外

47、的设备辅助标定（例如三轴转台），对需要额外的设备辅助标定（例如三轴转台），对IMUIMU的摆放位置没有特的摆放位置没有特殊的要求（相比于六面法标定），可以实现殊的要求（相比于六面法标定），可以实现IMUIMU的三轴陀螺的标定的三轴陀螺的标定该方案缺点：加速度计标定时要预先设定传感器的零偏，若设置于实际值有很大偏差该方案缺点：加速度计标定时要预先设定传感器的零偏，若设置于实际值有很大偏差校正结果误差很大校正结果误差很大三三. .导航系统传感器标定导航系统传感器标定3.3 MEMS IMU3.3 MEMS IMU加速度计、陀螺仪无外部基准标定加速度计、陀螺仪无外部基准标定三三. .导航系统传感器标

48、定导航系统传感器标定3.4 3.4 加速度计计算姿态角加速度计计算姿态角Ax+Ay+Az=gAx+Ay+Az=g令令加速度计三轴输出数据为加速度计三轴输出数据为AxAx，AyAy，AzAz；当地重力加速度为；当地重力加速度为g g，则有如下关系：，则有如下关系：令令旋转后坐标系旋转后坐标系XbXb轴与水平面轴与水平面XOYXOY的夹角为的夹角为pitchpitch，则重力加速度在该轴的分量为：，则重力加速度在该轴的分量为：Ax = sin(pitch)Ax = sin(pitch)* *g gs sin(pitch)=Ax/g;in(pitch)=Ax/g;Cos(pitch)=Cos(pit

49、ch)= （1-sin1-sin（pitchpitch） ）= =（1-1-（ Ax/g Ax/g ） ）三三. .导航系统传感器标定导航系统传感器标定北东地（北东地（NEDNED）坐标系）坐标系东北天（东北天（ENUENU）坐标系）坐标系EU ON俯仰俯仰横滚横滚航向航向四元数法的能够对载体的刚体运动进行简洁的描述四元数法的能够对载体的刚体运动进行简洁的描述T00123cossinsinsin2222Tqqqqq iq jq kijk汉密尔顿四元数规则汉密尔顿四元数规则2221,ijkjkkji kiikj ijjik JPL四元数规则四元数规则2221,ijkjkkjikiikjijjik

50、 四四. .四元数四元数1T0qqqq四元数的乘法的表现形式四元数的乘法的表现形式0123012300112233011023320213203103122130TT00003 303 30()()()()()()pqpp ip jp kqq iq jq kp qp qp qp qp qp qp qp qip qp qp qp qjp qp qp qp qkpqqqpppqqIqqpIpp四四. .四元数四元数323121000q kq jqq kqiq jqi TT3 303 30qqqqIqqIqq令：令： T2222012312031302222212030123230122221302230101232()2()2()2()2()2()nbCA qqqqqqqq qq qq qq qq qq qqqqqq qq qq qq qq qq qqqqq 由四元数表示的载