[发明专利]一种基于互补卡尔曼滤波算法计算融合姿态角度的方法

说明书

本发明公开了一种基于互补卡尔曼滤波算法计算融合姿态角度的方法，包括以下步骤：搭建姿态角度测量系统，根据陀螺仪测量数据计算得到第一姿态角度，根据加速度计测量数据计算得到第二姿态角度；根据卡尔曼滤波算法将所述第一姿态角度和所述第二姿态角度进行融合得到第三姿态角度；根据互补滤波原理，将所述第一姿态角度、所述第二姿态角度以及所述第三姿态角度进行融合计算得到融合姿态角度。本发明提出的基于互补卡尔曼滤波算法计算融合姿态角度的方法，大大提高了姿态角度的测量精度。

技术领域

本发明涉及姿态角度测量领域，尤其涉及一种基于互补卡尔曼滤波算法计算融合姿态角度的方法。

背景技术

在微波通信防抖系统中，微波天线的姿态角度是非常重要的反馈信号，防抖系统利用该反馈信号对微波天线姿态角度进行校正，使微波天线抖动幅度始终在要求的精度范围内，以保证通信信号的稳定性。因此，微波天线姿态角度的测量精度和稳定性至关重要。常用姿态角度测量元件有陀螺仪、加速度计等，陀螺仪测量角速度信号，但其测量值会随时间漂移，使得计算的角度值也随时间漂移；加速度计测量角加速度信号，但动态响应较差。因此，陀螺仪和加速度计通常不单独使用，而是根据两者的不同特性，采用某种算法对两者的测量数据进行融合得出精度更高的姿态角度测量值。

对陀螺仪和加速度计测量数据进行融合常用的算法有卡尔曼滤波算法和互补滤波算法。陈兴林等的“一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法”对卡尔曼滤波数据融合方法进行了部分改进，解决了现有卡尔曼滤波算法在四旋翼无人机姿态运算过程中运算量太大而无法实时获取姿态数据的问题；杜靖等的“基于卡尔曼滤波的加速度计陀螺仪水平角度测量方法”利用卡尔曼滤波算法对陀螺仪和加速度计测量数据进行了融合，解决了加速度计陀螺仪测量水平角度在动态测量环境下精度差的问题，但是两者对卡尔曼滤波算法中的参数——测量噪声协方差和过程噪声协方差只给出说明，并未详细说明实际应用过程中的计算方法，而测量噪声协方差和过程噪声协方差对卡尔曼滤波算法的精度有着重要影响。罗洁等的“一种用于盲杖的陀螺稳定装置及其互补滤波方法”涉及盲杖稳定装置，利用互补滤波算法融合陀螺仪和加速度计测量数据，但对算法中不同传感器测量数据权重的计算方法没有给出具体说明，在采用该种算法过程中会涉及到大量试验来确定权重值。

发明内容

为解决现有姿态角度融合算法中涉及到的参数计算方法不详问题以及进一步提高姿态角度融合算法的精度，本发明提出一种基于互补卡尔曼滤波算法计算融合姿态角度的方法，大大提高了姿态角度的测量精度。

为达到上述目的，本发明采用以下技术方案：

本发明公开了一种基于互补卡尔曼滤波算法计算融合姿态角度的方法，其特征在于，包括以下步骤：

S1：搭建姿态角度测量系统，根据陀螺仪测量数据计算得到第一姿态角度，根据加速度计测量数据计算得到第二姿态角度；

S2：根据卡尔曼滤波算法将所述第一姿态角度和所述第二姿态角度进行融合得到第三姿态角度；

S3：根据互补滤波原理，将所述第一姿态角度、所述第二姿态角度以及所述第三姿态角度进行融合计算得到融合姿态角度。

优选地，步骤S2中根据卡尔曼滤波算法将所述第一姿态角度和所述第二姿态角度进行融合得到第三姿态角度还包括：采用姿态角度方差的方法对卡尔曼滤波算法中的过程噪声协方差和测量噪声协方差进行计算。

优选地，步骤S3中，计算所述融合姿态角度时，根据所述第一姿态角度、所述第二姿态角度以及所述第三姿态角度的方差计算相应的权重，其中方差越大，相应的权重越小。

优选地，步骤S1具体包括：

以陀螺仪的测量数据和加速度计的测量数据，建立姿态角度测量系统的状态方程和测量方程分别如下式(1)和式(2)：