加速度计和陀螺仪的实时降噪系统和方法
【技术领域】
[0001] 本发明设及加速度计和巧螺仪降噪，尤其设及一种加速度计和巧螺仪的实时降噪 系统和方法。
【背景技术】
[0002] 惯性测量单元被广泛的使用在惯性导航系统中，其中，惯性测量单元的零偏稳定 性、常值误差、标度系数误差、非线性误差W及随机噪声对惯性导航系统的对准精度、导航 解算精度都起着决定性作用。因此，提高惯性测量单元的量测精度和可靠性显得至关重要。 提高惯性测量单元的量测精度的一种有效方法就是降低惯性测量单元的量测噪声，提高信 噪比。
[0003] 早期，针对惯性测量单元降噪的最简单的方法主要集中在低通滤波器的设计，但 低通滤波器只对固定带宽的噪声有很好的滤波效果，所W只能针对特定环境下的特定信息 进行有效处理，而不适用于任意机动环境且低通滤波器存在时间延迟的问题较为严重。
[0004] 随着计算机性能的提高，离散小波变换技术也被广泛的应用于惯性测量单元的降 噪处理，该方法在静态环境中的运用得到了很好的效果，但是在动态环境中该方法却失效 且该方法也会引入时间延迟，降低信息的跟踪能力。
[0005] 卡尔曼滤波可W有效处理系统过程随机噪声和量测随机噪声的能力，因此可W将 卡尔曼滤波运用于惯性测量单元的随机噪声处理。现有的自适应卡尔曼滤波降噪方法虽然 可W适应机动变化但是计算量较大，在高采样频率的实时环境中使用存在一定的困难。
【发明内容】
[0006] 为了解决在动、静态环境中皆能有效滤除加速度计和巧螺仪随机量测噪声的问 题，本发明提供了一种加速度计和巧螺仪的实时降噪系统及实时降噪方法，W达到减少滤 波时延，降低计算量，提高加速度计和巧螺仪量测精度的效果。
[0007] 为了解决上述问题，本发明一方面提供了一种加速度计和巧螺仪的实时降噪系 统，包括：
[000引惯性测量单元，用于提取加速度计和巧螺仪的量测信息，所述量测信息为被测载 体的线加速度和角速度信息；
[0009] 信息处理单元，用于对所述量测信息进行卡尔曼滤波并在线调整卡尔曼滤波器参 数得到降噪后的量测信息；
[0010] 导航计算机，用于利用所述降噪后的量测信息进行导航解算，输出所述被测载体 的导航信息。
[0011] 进一步地，所述信息处理单元包括：
[0012] 建立方程模块，用于建立关于所述加速度计和巧螺仪的单输入单输出卡尔曼滤波 状态方程和观测方程；
[0013] 迭代计算模块，用于进行卡尔曼滤波迭代计算，并根据残差X 2检测法得到检测函 数值丫 k，并将所述检测函数值丫 k与预先设定的阔值进行比较得出所述被测载体的机动变 化情况；
[0014] 在线调整模块，用于根据所述被测载体的机动变化情况调整卡尔曼滤波器的滤波 增益矩阵K的值和观测噪声方差矩阵R的值，并将调整后的值反馈给迭代计算模块。
[0015] 另一方面，本发明还提供了一种加速度计和巧螺仪的实时降噪方法，包括如下步 骤：
[0016] 提取加速度计和巧螺仪的量测信息，所述量测信息为被测载体的线加速度和角速 度信息；
[0017] 对所述量测信息进行卡尔曼滤波并在线调整卡尔曼滤波器参数得到降噪后的量 测信息；
[0018] 利用所述降噪后的量测信息进行导航解算，输出所述被测载体的导航信息。
[0019] 进一步地，所述在线调整卡尔曼滤波器参数的具体步骤为：
[0020] S1、建立关于加速度计和巧螺仪的单输入单输出卡尔曼滤波状态方程和观测方 程；
[0021] S2、进行卡尔曼滤波迭代计算，并根据残差X 2检测法得到检测函数值丫 k，并将所 述检测函数值丫 k与预先设定的阔值进行比较得出所述被测载体的机动变化情况；
[0022] S3、根据所述机动变化情况在线调整卡尔曼滤波器的滤波增益矩阵K的值和观测 噪声方差矩阵R的值。
[0023] 为了较准确地反映被测载体的实际运动情况，所述步骤S1中建立关于加速度计 和巧螺仪的单输入单输出卡尔曼滤波状态方程和观测方程的具体步骤为：
[0024] 将扰动信息作为系统随机噪声，且在采样周期内设线加速度的变化率为0, W所述 被测载体的线速度、线加速度为状态量建立所述加速度计的连续状态方程，W所述被测载 体的线加速度和量测噪声建立所述加速度计的连续观测方程；
[0025] 将扰动信息作为系统随机噪声，且在采样周期内设角加速度的变化率为0, W所述 被测载体的角速度、角加速度为状态量建立所述巧螺仪的连续状态方程，W所述被测载体 的角速度和量测噪声建立所述巧螺仪的连续观测方程；
[0026] 分别对所述加速度计的连续状态方程和连续观测方程，所述巧螺仪的连续状态方 程和连续观测方程进行离散化处理，得到所述加速度计的离散卡尔曼滤波状态方程和观测 方程W及所述巧螺仪的离散卡尔曼滤波状态方程和观测方程。
[0027] 为了更好地适应被测载体的实际运动情况，尤其是机动状态突变的情况，所述步 骤S2中获取所述被测载体的机动变化情况的具体步骤为：
[002引将所述检测函数值丫k与所述阔值比较，若所述检测函数值丫 k大于所述阔值，则 判决被所述被测载体处于快速机动变化状态；
[0029] 若所述检测函数值丫 k小于阔值，则判决所述被测载体处于机动变化缓慢或匀速 运动或静止的状态。
[0030] 为了提高降噪性能，所述步骤S3中在线调整卡尔曼滤波的滤波增益矩阵K的值和 观测噪声方差矩阵R的值的具体步骤为：
[0031] 设置滤波初始值，包括；加速度计的观测噪声方差阵R。、过程噪声方差阵Q。、卡尔 曼滤波的估计误差方差阵P。和系统状态X巧螺仪的观测噪声方差阵Rg、过程噪声方差阵 Qg、卡尔曼滤波的估计误差方差阵Pg和系统状态X g;
[0032] 当判决所述被测载体处于快速机动变化状态时，对当前迭代计算中的滤波增益矩 阵K乘W-个大于1的加权系数并保持所述加速度计的观测噪声方差阵氏和所述巧螺仪 的观测噪声方差阵Rg的值不变，将得到的矩阵值用于后续迭代计算；
[0033] 当判决所述被测载体处于机动变化缓慢或静止或匀速机动状态时，对当前迭代计 算中的所述加速度计的观测噪声方差阵氏和所述巧螺仪的观测噪声方差阵Rg的值分别乘 W-个远大于1的加权系数且保持滤波增益阵K的值不变，将得到的矩阵值用于后续迭代 计算。
[0034] 为了保证加速计和巧螺测量值的滤波结果能够实时跟随载体的机动，需要适当将 Rg和R。初始值设置的小一些，所述加速度计的滤波初始值和所述巧螺仪的滤波初始值分别 设置为：
[003引 Xg= [0 0 0] T，Pg= diag[10 10 10]，
[0036] Rg= 0. 00001，Q g= diag 巧 X 10 -8 3X 1〇-8]，
[0037] Xa= [0 0 0] T，Pa= diag[10 10 10]，
[003引 Ra= 0. 00001，Q a= diag[5X 10 -4 5X l0-4]。
[0039] 本发明与现有技术相比，包括如下优点：
[0040] 1.本发明针对加速度计和巧螺仪的特性分别建立了单输入单输出的卡尔曼滤波 状态方程和观测方程，滤波系统将加速度计和巧螺仪的量测噪声作为卡尔曼滤波系统的观 测噪声处理，利用卡尔曼滤波的工作特性，有效降低了加速度计和巧螺仪量测数据中的随 机噪声。
[0041] 2.本发明利用残差X 2检测法对载体机动变化情况进行实时监测，残差X 2检测 法是一种无法确定故障原因、但能实时确定一组量测信息有效性的检测方法，常常被用于 突变型故障检测，实现了对载体机动变化情况进行实时监测的功能。
[0042] 3.本发明根据机动情况在线调整卡尔曼滤波的滤波增益阵K的值和观测噪声方 差阵R阵的值。加速度计和巧螺仪一般运用在动态环境中，为了保证加速计和巧螺量测值 经过滤波后的结果能够实时跟随载体的机动，需要适当将Rg和R。的初始值设置的小一些。 当系统被判为处于快速机动变化的阶段时，为了使滤波结果能够更快的跟踪载体的机动， 可W保持Rg和R。不变，同时，在当前卡尔曼滤波迭代计算的出的滤波增益阵K前乘W-个 大于1的加权系数，然后在后续迭代计算中使用加权W后的K阵；当系统被判为处于缓慢机 动变化或者静止或匀速机动的阶段时，为了增加系统的降噪性能，可W直接利用当前卡尔 曼滤波迭代计算出的滤波增益阵K，但需要在Rg和R。前分