一种mems惯性导航系统及基于该系统的轨迹重构方法
【专利摘要】本发明公开了一种MEMS惯性导航系统及基于该系统的轨迹重构方法，应用六位置法对陀螺仪和加速度计进行误差因数的辨识得到静态误差模型从而补偿静态误差；经过低通滤波器滤波显著地降低陀螺仪和加速度计的随机误差，降低噪声，提高输出精度；采用互补滤波算法将陀螺仪和加速度计更新的姿态角度有效地结合起来得到姿态矩阵；利用姿态矩阵变换加速度和重力补偿后得到导航模块在惯性坐标系下的三轴加速度；最后通过迭代式双重积分得到轨迹和速度信息。本发明的互补滤波算法将加速度计和陀螺仪独立解算的姿态角融合，可以抑制姿态角漂移的发散，使得姿态推算在动态和静态下都大为改善。
【专利说明】
一种MEMS惯性导航系统及基于该系统的轨迹重构方法
技术领域
[0001]本发明属于惯性导航技术领域，尤其涉及一种基于MEMS惯性导航系统的轨迹重构 方法。 技术背景
[0002] 微机械电子系统(MEMS,Micro Electro Mechanical System)是微电子技术的延 伸和发展，在二十世纪末以及本世纪的十几年中得到较快的发展。由于电子工业和计算机 技术的快速发展以及电子器件制造成本的不断降低，一些军事科技方面的高端技术开始应 用于消费电子领域，其中MEMS在人机交互中的应用越来越引起关注。MEMS惯性传感器是 MEMS在惯导领域应用的产物，它是由MEMS加速度计和MEMS陀螺仪构成。这一技术不但在航 空航天、国防科技等军事领域有着较大的应用，在一些民用领域也慢慢有了较大的发展，比 如体感游戏、人体运动感知、空中鼠标系统，以及目前比较热门的VR设备上。在基于MEMS的 惯性导航系统轨迹重构技术上，对惯性传感器的精度要求非常高，因为这将直接影响惯性 导航重构轨迹的精度。从现在的技术发展趋势上看，MEMS惯性传感器也是沿着高精度、高集 成度趋势发展，现有的MEMS加速度传感器精度可以达1 · l(T4g，并且还有更进一步的提升空 间。现有的MEMS陀螺仪极限精度能够达到0.01/h以下，随机游走系数可以控制在Ι/h以下。 在消费电子领域中由于成本限制，选用的MEMS陀螺仪精度一般在10-1 ΟΟ/h范围内。因此精 度的降低需要用特殊的算法来确保重构轨迹的精度。
[0003] MEMS惯性传感器在消费电子领域轨迹重构应用已经成为当前科研的热点。05年以 前MEMS传感器的研究主要集中于MEMS传感器的制造研究、MEMS传感器的标定误差处理方法 研究等方面。近几年对着人们生活质量的不断提高、智能手机的兴起，MEMS传感器的应用逐 渐进入普通人生活。如当前IBM推出的VR游戏体验的装备中就有惯性传感器，荷兰XSENS公 司、英国的x _i〇 Technologies Limited公司和背景WSENS公司推出的人体运动识别技术 等，这些技术无疑的完善了 MEMS技术应用，同时也丰富了人们的生活。
【发明内容】
[0004] 本发明旨在提供一种基于MEMS惯性导航系统的轨迹重构方法，通过对传感器的模 型校正，采用滤波算法，减小导航误差，在一个范围内提供该系统的运动姿态和运动轨迹， 以满足对小范围内的高精度运动轨迹的导航或跟踪要求。
[0005] 为了实现上述目的，本发明所采用的技术方案如下:一种MEMS惯性导航系统，包括 惯性传感器、第一电平转换模块、电源模块、第二电平转换模块、上位机;所述惯性传感器由 加速度计和陀螺仪组成;所述第一电平转换模块上搭载无线传输模块;所述第二电平转换 模块上搭载无线接收模块;所述加速度计用于测量传感器的三轴加速度，陀螺仪用于测量 传感器的三轴角速度;所述加速度计和陀螺仪均与第一电平转换模块相连;所述无线传输 模块与无线接收模块通过无线传输数据;所述第二电平转换模块通过USB与上位机相连;上 位机对接收到的信号进行静态误差补偿、滤波，并通过分析处理算法解算和更新姿态，最后 对瞬时姿态进行重力补偿和双积分，得到该时间段内的惯导轨迹，实现轨迹重构。
[0006] -种上述的系统的轨迹重构方法，具体包括以下步骤：
[0007] (1)所述加速度计测量传感器的三轴加速度，陀螺仪测量传感器的三轴角速度；
[0008] (2)所述第一电平转换模块接收加速度计测量到的三轴加速度和陀螺仪测量到的 三轴角速度，并通过其上搭载无线传输模块传输给搭载在第二电平转换模块上的无线接收 模块，第二电平转换模块将该信号传输给上位机的误差补偿模块，对由六位置法标定的陀 螺仪和加速度计得到的数据进行校正，得到优化补偿后的加速度和角速度信号；
[0009] (3)将步骤2得到优化补偿后的的加速度和角速度信号送往上位机的滤波模块，滤 波模块先对信号进行数字低通滤波，以消除高频的噪声信号，减小信号的随机误差；
[0010] (4)将步骤3得到的数字低通滤波后的加速度信号通过三轴分量的关系求解加速 度计的姿态角；
[0011] (5)将步骤3得到的数字低通滤波后的角速度信号通过欧拉法求解陀螺仪的姿态 角；
[0012] (6)将步骤4和步骤5得到的加速度计的姿态角和陀螺仪的姿态角送往互补滤波模 块，将加速度计和陀螺仪的姿态角进行互补，得到惯性传感器的最佳姿态角；
[0013] (7)由步骤6得到的惯性传感器的最佳姿态角，可获得惯性传感器的最佳姿态矩 阵，将步骤3得到的加速度计的三轴加速度通过姿态矩阵进行坐标系的变换，得到惯性系下 的三轴加速度;再对三轴加速度进行重力补偿，对补偿后的三轴加速度进行双重积分，得到 物体的位置和速度信息，从而实现轨迹重构。
[0014] 进一步的，所述误差补偿模块的处理步骤具体如下：
[0015] 建立加速度计误差模型方程，模型方程如下：
[0016]
[0017] 其中，Ax、Ay、Az分别代表加速度计x、y、z三轴上的输出，a XQ、ayQ、az()分别代表加速度 计x、y、z三轴的零偏，Sx、Sy、Sz分别代表加速度计x、y、z三轴上的刻度因数，K xy表示X与y轴间 的耦合误差因数，Kyx表示y与X轴间的耦合误差因数;Kyz表示y与z轴间的耦合误差因数，K zy 表示z与y轴间的耦合误差因数;Kzx表示z与X轴间的耦合误差因数，Kxz表示X与z轴间的耦合 误差因数，K x2、Ky2、Kz2均代表加速度计三轴关于二次方的误差系数，Vx、 Vy、Vz分别代表加速 度计X、y、z三轴的随机误差；
[0018] 建立陀螺仪的误差模型方程，模型方程如下：
[0019]
[0020] 具中，Gx、Gy、Gz分别代表阳螺仪x、y、z三雅上的湔出，g XQ、gyQ、gz()分别代表陀螺仪X、 y、z三轴的零偏，Sx、Sy、Sz分别代表陀螺仪X、y、z三轴上的刻度因数，K xy表示X与y轴间的耦合 误差因数，Kyx表示y与X轴间的耦合误差因数;Kyz表示y与z轴间的耦合误差因数，K zy表示z与 y轴间的耦合误差因数;Kzx表示z与X轴间的耦合误差因数，Kxz表示X与z轴间的耦合误差因 数，Kx2、Ky2、Kz2分别代表陀螺仪三轴关于二次方的误差系数， Vx、Vy、Vz分别代表陀螺仪x、y、z 二轴的随机误差；
[0021]在建立MEMS加速度计和陀螺仪的误差模型方程后，通过六位置标定法标定加速度 计陀螺仪的模型误差系数，它们分别为：
[0024] 在误差系数的求解上加速度计与陀螺仪相同，以加速度计为例，按照加速度计标 定过程，将加速度计检测到的数据代入加速度计误差模型方程中，得到加速度计误差模型 里面的相关误差系数，即
[0025] 加速度计X轴：
[0026]
[0027]
[0028]
[0029]
[0030]
[0031]
[0032]
[0033]
[0034]解这三组方程得到所有的模型误差系数；同理，得到陀螺仪误差模型里面的相关 误差系数。
[0035]进一步的，所述滤波模块的处理步骤具体如下：
[0036]采用基于Kaiser窗的有限冲激响应数字滤波器对陀螺仪、加速度计的随机误差进 行抑制并且对滞后的相位进行补偿。
[0037]进一步的，所述步骤4具体如下：
[0038]由公式(3)和公式(4)展开可得公式(5)和公式(6)，并将步骤3得到的数字低通滤 波后的加速度信号代入公式(5)和公式(6)，得到加速度计的姿态角；
ax、ay、aj别是X、Y、Z轴的方向余弦，g x、gy、gz分别为重力在三轴上的分 量;Θ为加速度计的横滚角和γ为俯仰角。
[0044] 进一步的，所述步骤5具体如下：
[0045] 将步骤3得到的数字低通滤波后的角速度信号代入公式（7)，得到陀螺仪的姿态 角；
[0046] (7).
[0047] 共中/、g、#分別表不萩体的航冋用、横滚角、俯仰角的微分，ωχ、ωΥ、ω#别 表示陀螺仪的三轴角速度。
[0048] 进一步的，所述互补滤波模块的处理步骤如下：
[0049] 将步骤4和步骤5得到的加速度计的姿态角和陀螺仪的姿态角代入公式(8)，得到 惯性传感器的最佳姿态角；
[0050] Jk = Kj (jA-jc)+jG (8)
[0051] qK = Kq(qA-qG)+qG (9)
[0052] gK = Kg(gA_gG) +gG (10)其中，jK、qK、gK为惯性传感器的三个最佳姿态角；Kj、Kq、K g 均为三个姿态角融合的增益值；<^、0/1、丫/1均为表示加速度计解算得到的姿态角，供( ?、0(；、 γ c表示陀螺仪解算得到的姿态角。
[0053] 进一步的，所述步骤7具体为：
[0054] 由步骤6得到的惯性传感器的最佳姿态角，可获得惯性传感器的最佳姿态矩阵，将 步骤3得到的加速度计的三轴加速度通过姿态矩阵进行坐标系的变换，得到惯性系下的三 轴加速度;再对三轴加速度进行重力补偿，对补偿后的三轴加速度进行双重积分，得到物体 的位置和速度信息，从而实现轨迹重构。
[0055] 将步骤6得到的惯性传感器的最佳姿态角代入公式（11)，得到惯性传感器的最佳 姿态矩F-
[0056] (U)
[0057] 通过上述最佳姿态矩阵将三轴加速度变换到惯性坐标系下，并进行重力加速度补 偿，公式加下，
[0058] (12)
[0059] (13)
[0060]其中，xb、yb、zb表示载体坐标系下三轴的加速度信号; Xn、yn、zn表示惯性坐标系下 三轴的加速度信号，g为重力矢量，Xa、ya、Za为经过重力补偿后三轴的加速度信号；
[0061 ]假定从to时刻开始采样，t时刻载体的位移s (t)和速度V (t)的表达式为：
[0062] (.14)
[0063] (15)
[0064] 其中，SQ、VQ表示初始时刻载体的位移和速度;(1τ是一个小量，令
[0065] dt = ti_ti-i = Dt，to ￡ ti ￡ t (16)
[0066] 其中，i为正整数；
[0067] 将公式(14)和公式(15)改写为累加式，得到公式(17)和公式(18):
[0068] (17)
[0069] (18)
[0070] 其中，a(i)、v(i)表示i时刻载体的加速度和速度;η为正数;s(n)为载体最终的位 移;V (η)为载体最终的速度;Dt为时间间隔；
[0074] 由公式(19)和公式(20)得到载体最终的位移和速度计算公式：
[0071 ] 通过公式（17)，对s(n)与s(n-l)进行相减，通过公式（18)对ν(η)与v(n-l)相减得 到递推广
[0072] ( 19)
[0073] (20)
[0077] 与现有技术相比，本发明的技术方案具有以下优势：[0078] 1、本发明对硬件的要求较低，仅需要一个多传感器的惯导模块和信号远传模块。
[0075] (21)
[0076] (22) 而信号处理的过程主要在上位机内完成，因此惯导系统的体积较小，可以在微小的工作区 域内实现惯导轨迹重构，而不会因自身体积太大影响精度。
[0079] 2、本发明针对传感器自身的特性进行误差模型的测试。通过六位置法收集系统中 搭载的传感器的大量数据，通过对这些数据的误差分析统计出传感器的零偏等只针对该传 感器的补偿数据，建立误差补偿矩阵，具有较好的专一性，而不是采取普遍的校正模型。所 以能根据自身的结构特点和精度等级对传感器进行合适的校正。
[0080] 3、本发明利用互补滤波的方法，确定当前时刻姿态角的最佳估计值，概念清晰，比 较符合工程实际的应用，并且具有较好的补偿效果，具有较高的实用价值。陀螺仪和加速度 计都可以独立的得到载体的姿态角，但是两种信号都存在较大的误差，如果将这两种传感 器的信号融合在一起，将可以互补彼此的缺点，使得算法的误差收敛，姿态推算的精度大为 改善。
【附图说明】
[0081 ]图1为本发明基于MEMS惯性导航系统的轨迹重构算法结构流程图；
[0082]图2为加速度计、陀螺仪数据融合的互补滤波算法框图；
[0083]图3为捷联式惯性导航系统原理框图。
【具体实施方式】
[0084]下面结合附图对本发明作进一步的说明。
[0085] 一种MEMS惯性导航系统，包括惯性传感器、第一电平转换模块、电源模块、第二电 平转换模块、上位机;所述惯性传感器由加速度计和陀螺仪组成;所述第一电平转换模块上 搭载无线传输模块;所述第二电平转换模块上搭载无线接收模块;所述加速度计用于测量 传感器的三轴加速度，陀螺仪用于测量传感器的三轴角速度;所述加速度计和陀螺仪均与 第一电平转换模块相连;所述无线传输模块与无线接收模块通过无线传输数据;所述第二 电平转换模块通过USB与上位机相连;上位机对接收到的信号进行静态误差补偿、滤波，并 通过分析处理算法解算和更新姿态，最后对瞬时姿态进行重力补偿和双积分，得到该时间 段内的惯导轨迹，实现轨迹重构。
[0086] -种上述的系统的轨迹重构方法，具体包括以下步骤：
[0087] (1)所述加速度计测量传感器的三轴加速度，陀螺仪测量传感器的三轴角速度；
[0088] (2)所述第一电平转换模块接收加速度计测量到的三轴加速度和陀螺仪测量到的 三轴角速度，并通过其上搭载无线传输模块传输给搭载在第二电平转换模块上的无线接收 模块，第二电平转换模块将该信号传输给上位机的误差补偿模块，对由六位置法标定的陀 螺仪和加速度计得到的数据进行校正，得到优化补偿后的加速度和角速度信号；
[0089] (3)将步骤2得到优化补偿后的的加速度和角速度信号送往上位机的滤波模块，滤 波模块先对信号进行数字低通滤波，以消除高频的噪声信号，减小信号的随机误差；
[0090] (4)将步骤3得到的数字低通滤波后的加速度信号通过三轴分量的关系求解加速 度计的姿态角；
[0091] (5)将步骤3得到的数字低通滤波后的角速度信号通过欧拉法求解陀螺仪的姿态 角；
[0092] (6)将步骤4和步骤5得到的加速度计的姿态角和陀螺仪的姿态角送往互补滤波模 块，将加速度计和陀螺仪的姿态角进行互补，得到惯性传感器的最佳姿态角；
[0093] (7)由步骤6得到的惯性传感器的最佳姿态角，可获得惯性传感器的最佳姿态矩 阵，将步骤3得到的加速度计的三轴加速度通过姿态矩阵进行坐标系的变换，得到惯性系下 的三轴加速度;再对三轴加速度进行重力补偿，对补偿后的三轴加速度进行双重积分，得到 物体的位置和速度信息，从而实现轨迹重构。
[0094] A:误差模型分析：分析MEMS加速度计误差形成的原因可以得到误差模型方程如 下：
[0095]
[0096] 其中，Ax、Ay、Az分别代表加速度计x、y、z三轴上的输出，a xQ、ayQ、az()分别代表加速度 计x、y、z三轴的零偏，Sx、Sy、Sz分别代表加速度计x、y、z三轴上的刻度因数，K xy表示X与y轴间 的耦合误差因数，Kyx表示y与X轴间的耦合误差因数;Kyz表示y与z轴间的耦合误差因数，K zy 表示z与y轴间的耦合误差因数;Kzx表示z与X轴间的耦合误差因数，Kxz表示X与z轴间的耦合 误差因数，K x2、Ky2、Kz2均代表加速度计三轴关于二次方的误差系数，Vx、 Vy、Vz分别代表加速 度计X、y、z三轴的随机误差；
[0097] 与MEMS加速度计相似，也建立了陀螺仪的误差模型方程如下：
[0098]
[0099]其中，Gx、Gy、Gz分别代表陀螺仪三轴上的输出，g xQ、gyQ、gz()分别代表陀螺仪三轴的 零偏，Sx、Sy、Sz分别代表陀螺仪三轴上的刻度因数，Kyx、K xy表示X与Y轴间的耦合误差因数； Kzy、Kyz表示Y与Z轴间的耦合误差因数;KXZ、KZX表示Z与X轴间的耦合误差因数，K x2、Ky2、Kz2分 别代表陀螺仪关于二次方的误差系数，Vx、v y、vz分别代表陀螺仪三轴的随机误差。
[0100]在建立MEMS加速度计和陀螺仪的误差模型方程后，可以通过位置标定法标定两种 传感器的模型误差系数。本发明根据误差模型的未知系数的个数采用了六位置的标定方 法，它们分别为：
[0102] 实验中对于加速度计和陀螺仪唯一的区别在于加速度计需要恒定的g输入，模块 需要保持静止状态，可以在平台上进行静态的测量;但是陀螺仪在静态是没有输出的，需要 将模块固定在转台上保持转台转轴与模块坐标轴的重合并且依靠转台的匀速转动完成测 试。
[0103] 误差系数的求解上加速度计与陀螺仪相同，以加速度计为例，按照规定的MEMS加 速度计标定过程，将加速度计测得的数据代入加速度计的误差模型，得到如下三组方程：
[0104] 加速度计的X轴：
[0105]
[0106]
[0107]
[0108]
[0109]
[0110]
[0111]
[0112]
[0113] 由上述三组方程可以分别得到加速度传感器误差模型里面的相关误差系数：
[0114] 加谏庶i+X轴：
[0115]
[0116]
[0117]
[0118]
[0119]
[0120]
[0121]
[0122]
[0123] B:低通滤波模块:建立加速度计和陀螺仪的误差模型后就可以对其输出进行静态 的补偿，但是传感器的随机误差是无法用实验的方法确定下来的。因为在A中标定加速度计 和陀螺仪误差模型时每个测试点值用的都是平均值，所以在求解参数的过程中忽略随机误 差~、^、^。一般处理随机误差的方法有低通滤波、卡尔曼滤波以及启发式消减法滤波 (HDR)，本发明将用基于Kaiser窗的有限冲激响应数字滤波器对陀螺仪、加速度计三轴的随 机误差进行抑制并且对滞后的相位进行补偿。
[0124] C:姿态更新模块:矫正三轴角速度和三轴加速度的零偏误差和随机噪声后，三轴 角速度通过欧拉法计算此时模块姿态角的微分;三轴加速度通过坐标变换表达当前的姿态 角。
[0125] 以重力为例，在三维空间中，重力在三个坐标轴上的分量与重力方向和三个坐标 轴的方向有如下关系：
[0126] q = 9〇-ay (3)
[0127] cosqcosg = cosaz (4)
[0128] 由公式(3)和公式(4)得到公式(5)和公式(6)即横滚角和俯仰角的表达式：
[0129]
[0130]
[0131]
别是X、Y、Z轴的方向余弦，gx、gy、gz分别为重力在三轴上的分 M; ? 73 皮it的楨滚角和γ为俯仰角。
[0132] 欧拉角姿态更新算法利用微分方程关系，通过角速度得到三个姿态角的微分，方 程最终姑里加下.
[0133] (7)
[0134] 其中，/、. .g、女分别表不载体的航向角、横滚角、俯仰角的微分，ωχ、c〇y、(〇2表 示陀螺仪的三轴角速度，并且经过地转角速度和相关误差的补偿。
[0135] D:多传感器互补融合滤波模块：加速度计和陀螺仪可以独立解算出载体的姿态 角，利用互补滤波模块将两者通过一个随运动状态变化的增益值融合起来，得到当前载体 姿态角的最佳估计值，互补滤波的方程如下：
[0136] jK = Kj( jA-jG)+jG (8)
[0137] qK = Kq(qA-qc)+qG (9)
[0138] gK = Kg(gA-gG)+gG (10)其中，攸、θκ、γκ为惯性传感器的三个最佳姿态角；御、 Κθ、Κγ为三个姿态角融合的增益值，它与载体当前的运动状态有关。?y、0A、γ Α表示加速度计 解算得到的姿态角，抖,'、9C、y c表示陀螺仪解算得到的姿态角。
[0139] E:加速度变换和重力补偿模块:通过互补滤波解算得到的当前时刻最优姿态角， 得到当前时刻模块的姿态矩阵：
[0140]
(11)
[0141] 通过上述最佳姿态矩阵将三轴加速度变换到惯性坐标系下，并进行重力加速度补 偿，公式如下：
[0142] (12)
[0143] (13)
[0144] 其中，Xb、yb、Zb表示载体坐标系下的加速度信号;&、5^、 211表示惯性坐标系下加速 度信号，g为重力矢量，Xa、ya、z a为经过重力补偿后的加速度信号；
[0145] F:积分环节和轨迹重构模块:通过坐标系转换和重力滤波的加速度信号代表载体 当前时刻在惯性系下的三轴加速度，对其进行双重积分后就可以得到载体的位置和速度信 息，从而实现载体的轨迹重构。
[0146] 假定从to时刻开始采样，t时刻载体的位移s (t)和速度V (t)的表达式为：
[0147] ^ 14)
[0148] (15.)
[0149] 其中，SQ、VQ表示初始时刻载体的位移和速度，(1τ是一个小量，令
[0150] dt = ti_ti-i = Dt，to ￡ ti ￡ t (16)
[0151] 其中i为正整数；
[0152] 将公式（14)和公式（15)改写为累加式，得到公式(17)和公式(18):
[0153]
(17)
[0154]
(18)
[0155] 其中，a(i)、v(i)表示i时刻载体的加速度和速度;η为正数;s(n)为载体最终的位 移;V (η)为载体最终的速度;Dt为时间间隔；
[0?5?] 通过公式（17)，对s(n)与s(n-l)进行相减，通过公式（18)对ν(η)与v(n-l)相减得 到递推式：
[0159] 由公式(19)和公式(20)得到载体最终的位移和速度计算公式：
[0157] (19)
[0158] P〇)
[0160] (21)
[0161] (22)
【主权项】
1. 一种MEMS惯性导航系统，其特征在于，包括惯性传感器、第一电平转换模块、电源模 块、第二电平转换模块、上位机;所述惯性传感器由加速度计和陀螺仪组成;所述第一电平 转换模块上搭载无线传输模块;所述第二电平转换模块上搭载无线接收模块;所述加速度 计用于测量传感器的三轴加速度，陀螺仪用于测量传感器的三轴角速度;所述加速度计和 陀螺仪均与第一电平转换模块相连;所述无线传输模块与无线接收模块通过无线传输数 据;所述第二电平转换模块通过USB与上位机相连;上位机对接收到的信号进行静态误差补 偿、滤波，并通过分析处理算法解算和更新姿态，最后对瞬时姿态进行重力补偿和双积分， 得到该时间段内的惯导轨迹，实现轨迹重构。2. -种利用权利要求1所述的系统的轨迹重构方法，其特征在于，具体包括以下步骤： (1) 所述加速度计测量传感器的三轴加速度，陀螺仪测量传感器的三轴角速度； (2) 所述第一电平转换模块接收加速度计测量到的三轴加速度和陀螺仪测量到的三轴 角速度，并通过其上搭载无线传输模块传输给搭载在第二电平转换模块上的无线接收模 块，第二电平转换模块将该信号传输给上位机的误差补偿模块，对由六位置法标定的陀螺 仪和加速度计得到的数据进行校正，得到优化补偿后的加速度和角速度信号； (3) 将步骤2得到优化补偿后的的加速度和角速度信号送往上位机的滤波模块，滤波模 块先对信号进行数字低通滤波，以消除高频的噪声信号，减小信号的随机误差； (4) 将步骤3得到的数字低通滤波后的加速度信号通过三轴分量的关系求解加速度计 的姿态角； (5) 将步骤3得到的数字低通滤波后的角速度信号通过欧拉法求解陀螺仪的姿态角； (6) 将步骤4和步骤5得到的加速度计的姿态角和陀螺仪的姿态角送往互补滤波模块， 将加速度计和陀螺仪的姿态角进行互补，得到惯性传感器的最佳姿态角； (7) 由步骤6得到的惯性传感器的最佳姿态角，可获得惯性传感器的最佳姿态矩阵，将 步骤3得到的加速度计的三轴加速度通过姿态矩阵进行坐标系的变换，得到惯性系下的三 轴加速度;再对三轴加速度进行重力补偿，对补偿后的三轴加速度进行双重积分，得到物体 的位置和速度信息，从而实现轨迹重构。3. 根据权利要求2所述的轨迹重构方法，其特征在于，所述误差补偿模块的处理步骤具 体如下： 建立加速度计误差模型方程，模型方程如下：其中，Ax、Ay、Az分别代表加速度计x、y、z三轴上的输出，a xQ、ayQ、az()分别代表加速度计X、 y、z三轴的零偏，Sx、Sy、Sz分别代表加速度计x、y、z三轴上的刻度因数，K xy表示X与y轴间的耦 合误差因数，Kyx表示y与X轴间的耦合误差因数;Kyz表示y与z轴间的耦合误差因数，K zy表示z 与y轴间的耦合误差因数;Kzx表示z与X轴间的耦合误差因数，Kxz表示X与z轴间的耦合误差 因数，K x2、Ky2、Kz2均代表加速度计三轴关于二次方的误差系数，Vx、 Vy、Vz分别代表加速度计 X、y、z三轴的随机误差； 建立陀螺仪的误差模型方程，模型方程如下：其中，Gx、Gy、Gz分别代表陀螺仪x、y、z三轴上的输出，g xQ、gyQ、gzQ分别代表陀螺仪x、y、z 三轴的零偏，Sx、Sy、Sz分别代表陀螺仪X、y、z三轴上的刻度因数，K xy表示X与y轴间的耦合误 差因数，Kyx表示y与X轴间的耦合误差因数;Kyz表示y与z轴间的耦合误差因数，K zy表示z与y 轴间的耦合误差因数;Kzx表示z与X轴间的耦合误差因数，Kxz表示X与z轴间的耦合误差因 数，K x2、Ky2、Kz2分别代表陀螺仪三轴关于二次方的误差系数，Vx、 Vy、Vz分别代表陀螺仪x、y、z 二轴的随机误差； 在建立MEMS加速度计和陀螺仪的误差模型方程后，通过六位置标定法标定加速度计陀 螺仪的模型误差系数，它们分别为：在误差系数的求解上加速度计与陀螺仪相同，以加速度计为例，按照加速度计标定过 程，将加速度计检测到的数据代入加速度计误差模型方程中，得到加速度计误差模型里面 的相关误差系数，即 加速度计X轴： Axl - ax〇+Sx+Kx2，Αχ2 - 3<x〇+Kyx，Αχ3 - 3·χ〇+Κζχ Αχ4 - 3<x〇_Sx+Kx2，Αχ5 - BxO-Kyx，Αχ6 - 3·χ〇_Κζχ 加速度计Y轴： Ayl - 3y〇+Kxy，Ay2 - 3<y〇+Sy+Ky2，Ay3 - 3<y〇+Kzy Ay4-3<y〇_Kxy，Ay5 - 3<y〇_Sy+Ky2，Ay6 - 3<y〇_Kzy 加速度计Z轴： Azl - ΒζΟ+Κχζ，Ay2 - BzO+Kyz，Az3 - 3<zO+Sz+Kz2 Az4 - BzO-Kxz，Az5 - BzO-Kzy，Az6 - 3<zO_Sz+Kz2 解这三组方程得到所有的模型误差系数；同理，得到陀螺仪误差模型里面的相关误差 系数。4. 根据权利要求2所述的轨迹重构方法，其特征在于，所述滤波模块的处理步骤具体如 下： 采用基于Kaiser窗的有限冲激响应数字滤波器对陀螺仪、加速度计的随机误差进行抑 制并且对滞后的相位进行补偿。5. 根据权利要求2所述的轨迹重构方法，其特征在于，所述步骤4具体如下： 由公式(3)和公式(4)展开可得公式(5)和公式(6)，并将步骤3得到的数字低通滤波后 的加速度信号代入公式(5)和公式(6)，得到加速度计的姿态角；量;θ为加速度计的横滚角和γ为俯仰角。6. 根据权利要求2所述的轨迹重构方法，其特征在于，所述步骤5具体如下： 将步骤3得到的数字低通滤波后的角速度信号代入公式(7)，得到陀螺仪的姿态角；(7) 其中@、会分别表不载体的航向角、横滚角、俯仰角的微分，《。0^、(02分别表不 陀螺仪的三轴角速度。7. 根据权利要求2所述的轨迹重构方法，其特征在于，所述互补滤波模块的处理步骤如 下： 将步骤4和步骤5得到的加速度计的姿态角和陀螺仪的姿态角代入公式(8)，得到惯性 传感器的最佳姿态角； jK = Kj( jA-jc)+jG (8) qK = Kq (qA_qc) +qc (9) gK = Kg(gA-gG)+gG (10) 其中，jK、qK、gK为惯性传感器的三个最佳姿态角;Kj、K q、Kg均为三个姿态角融合的增益 值;<^、ΘΑ、γΑ均为表示加速度计解算得到的姿态角表示陀螺仪解算得到的姿态 角。8. 根据权利要求2所述的轨迹重构方法，其特征在于，所述步骤7具体为： 由步骤6得到的惯性传感器的最佳姿态角，可获得惯性传感器的最佳姿态矩阵，将步骤 3得到的加速度计的三轴加速度通过姿态矩阵进行坐标系的变换，得到惯性系下的三轴加 速度;再对三轴加速度进行重力补偿，对补偿后的三轴加速度进行双重积分，得到物体的位 置和速度信息，从而实现轨迹重构。 将步骤6得到的惯性传感器的最佳姿态角代入公式（11)，得到惯性传感器的最佳姿态 矩阵：(11)通过上述最佳姿态矩阵将三轴加速度变换到惯性坐标系下，并进行重力加速度补偿， 公式如下 (12) (13) 其中，Xb、yb、Zb表示载体坐标系下三轴的加速度信号;知+、^表示惯性坐标系下三轴 的加速度信号，g为重力矢量，Xa、ya、Za为经过重力补偿后三轴的加速度信号； 假定从to时刻开始采样，t时刻载体的位移S (t)和速度V (t)的表达式为：其中，so、VO表示初始时亥Ij载体的位移和速度；办是一个小量，令 (14) (15)(16) 其中，i为正整数；将公式(14)和公式(15)改写为累加式，得到公式(17)和公式(18):(Π ) (18) 其中，a(i)、v(i)表示i时刻载体的加速度和速度;η为正数;s(n)为载体最终的位移;V (η)为载体最终的速度;Dt为时间间隔； 通过公式（17)，对s(n)与s(n-l)进行相减，通过公式（18)对ν(η)与v(n-l)相减得到递 推式： (19) 20)(21) (22)
【文档编号】G01C21/16GK105890593SQ201610210992
【公开日】2016年8月24日
【申请日】2016年4月6日
【发明人】邬博骋, 徐方凯, 郑开瑜, 谢磊
【申请人】浙江大学