[发明专利]一种基于自适应鲁邦融合算法的动态MEMS惯性姿态测量系统

说明书

技术领域

本发明涉及机械技术领域，具体是一种基于自适应鲁邦融合算法的动态MEMS惯性 姿态测量系统。

背景技术

惯性姿态测量系统主要应用于武器平台，机器人，工程机械以及各类需要在动态 环境下保持平衡的运载体上，传统的战术级惯导能长期保持高精度，因此广泛应用于各类 武器系统，然而高精度惯导的成本往往达到数十万元，因而极大的限制了惯性技术在民用 市场中的应用。近年来，随着MEMS(MicroelectromechanicalSystems，是将微电子技术与 机械工程融合到一起的一种工业技术)技术的发展，低成本的MEMS惯性器件正在低精度领 域逐渐取代传统的光学陀螺，使得惯性技术在民用领域得到了广泛的应用。但是目前的惯 性姿态测量系统普遍存在精度较差的问题，具体来说，MEMS姿态测量系统在没有外部信号 辅助的情况下，受到加速度时容易产生较大的水平姿态误差。由于航向测量依赖磁力计，因 而在磁场干扰时航向也会产生较大偏差。抗干扰能力差、精度低的弊端限制了惯性测姿系 统在民用市场中的应用，发展一种低成本，高精度，高可靠性的MEMS惯性测姿系统有着非常 大的市场应用前景。

一种无人艇航姿测量及发送装置(专利号：201120409148.2)应用陀螺罗经来确定 无人艇姿态，在传统罗经算法的基础上进行了优化，降低了系统的成本。但初始化时间长， 对姿态数据进行的数字平滑滤波会导致姿态数据发生延迟，难以满足伺服系统实时性的要 求。

一种微型捷联航姿系统及其工作方法(专利号：201210516500.1)使用了MEMS器件 降低了成本，使用卡尔曼滤波技术对传感器原始数据进行融合，得到了相对准确的姿态角， 但是卡尔曼滤波只有在系统噪声和量测噪声已知的情况下才能进行最优估计，而其系统模 型将运动加速度视作量测噪声，因而估计结果会存在较大误差，严重时会导致滤波不稳定。 另外其系统模型选择为传感器本体坐标系，该模型容易导致外部干扰磁场同时影响到水平 姿态角和航向姿态角，因此系统的抗干扰能力较差。

发明内容

本发明的目的在于提供一种基于自适应鲁邦融合算法的动态MEMS惯性姿态测量 系统，以解决上述背景技术中提出的问题，为实现上述目的，本发明提供如下技术方案：

一种基于自适应鲁邦融合算法的动态MEMS惯性姿态测量系统，包括MEMS惯性单 元、磁力计、数据融合计算机、同步采样和数据通讯单元；所述MEMS惯性单元包括传感器及 数据采集装置；所述包括传感器陀螺仪、加速度计；所述MEMS惯性单元用于采集传感器在惯 性空间的角速度和加速度，所述磁力计用于采集地球磁场的分量信息；所述数据融合计算 机运行自适应的数据融合算法；所述同步采样和数据通讯单元用来获得同步的传感器数据 并通过数字接口传输给总线上的其他设备。

作为本发明的进一步方案：一种基于自适应鲁邦融合算法的动态MEMS惯性姿态测 量系统，其数据处理方式包括系统初始化及坐标系协议、捷联姿态更新算法、动态姿态测量 系统建模、陀螺仪漂移实时估计算法、陀螺仪振动抑制算法，具体方案如下：

1)系统初始化及坐标系协议

参考坐标系选为东-北-天导航坐标系，本体坐标系选取：前-左-上，角度正向根据 右手定则确定，

初始俯仰角计算公式：

初始横滚角计算公式：

初始航向角计算公式：

捷联姿态矩阵为

其中θ为俯仰角，ψ为横滚角，为航向角；

2)捷联姿态更新算法

标准的四元数微分方程为：

其中

由于陀螺仪无法敏感到地球的自转，这里省略同时，陀螺仪进行速度更新的误差发散很快，因而省略与速度有关项即

为陀螺仪经过标定补偿后的测量值。

常用毕卡算法来计算由角增量得到的四元数，由于毕卡算法在实际应用中省略了 泰勒级数展开的高阶项，且毕卡算法近似认为每个算法周期内角速度是一个常值，然而实 际动态环境中，来自载体的振动和载体自身运动轨迹会叠加出复杂的角振动，从而导致解 算算法中引入不可交换性误差。

理论上讲如果陀螺没有量化误差，而且系统采样周期趋于零，即姿态更新周期内 采样次数多，就可以减少或消除可交换性误差，但计算量也会随之增大，在实际应用中综合 考虑算法实时性和计算负担，选择合适的子样算法，采用二子样算法：