一种基于互补卡尔曼滤波算法计算融合姿态角度的方法
【技术领域】
[0001] 本发明涉及姿态角度测量领域，尤其涉及一种基于互补卡尔曼滤波算法计算融合 姿态角度的方法。
【背景技术】
[0002] 在微波通信防抖系统中，微波天线的姿态角度是非常重要的反馈信号，防抖系统 利用该反馈信号对微波天线姿态角度进行校正，使微波天线抖动幅度始终在要求的精度范 围内，以保证通信信号的稳定性。因此，微波天线姿态角度的测量精度和稳定性至关重要。 常用姿态角度测量元件有陀螺仪、加速度计等，陀螺仪测量角速度信号，但其测量值会随时 间漂移，使得计算的角度值也随时间漂移;加速度计测量角加速度信号，但动态响应较差。 因此，陀螺仪和加速度计通常不单独使用，而是根据两者的不同特性，采用某种算法对两者 的测量数据进行融合得出精度更高的姿态角度测量值。
[0003] 对陀螺仪和加速度计测量数据进行融合常用的算法有卡尔曼滤波算法和互补滤 波算法。陈兴林等的"一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法"对卡尔曼 滤波数据融合方法进行了部分改进，解决了现有卡尔曼滤波算法在四旋翼无人机姿态运算 过程中运算量太大而无法实时获取姿态数据的问题;杜靖等的"基于卡尔曼滤波的加速度 计陀螺仪水平角度测量方法"利用卡尔曼滤波算法对陀螺仪和加速度计测量数据进行了融 合，解决了加速度计陀螺仪测量水平角度在动态测量环境下精度差的问题，但是两者对卡 尔曼滤波算法中的参数一一测量噪声协方差和过程噪声协方差只给出说明，并未详细说明 实际应用过程中的计算方法，而测量噪声协方差和过程噪声协方差对卡尔曼滤波算法的精 度有着重要影响。罗洁等的"一种用于盲杖的陀螺稳定装置及其互补滤波方法"涉及盲杖稳 定装置，利用互补滤波算法融合陀螺仪和加速度计测量数据，但对算法中不同传感器测量 数据权重的计算方法没有给出具体说明，在采用该种算法过程中会涉及到大量试验来确定 权重值。
【发明内容】
[0004] 为解决现有姿态角度融合算法中涉及到的参数计算方法不详问题以及进一步提 高姿态角度融合算法的精度，本发明提出一种基于互补卡尔曼滤波算法计算融合姿态角度 的方法，大大提高了姿态角度的测量精度。
[0005] 为达到上述目的，本发明采用以下技术方案：
[0006] 本发明公开了一种基于互补卡尔曼滤波算法计算融合姿态角度的方法，其特征在 于，包括以下步骤：
[0007] S1:搭建姿态角度测量系统，根据陀螺仪测量数据计算得到第一姿态角度，根据加 速度计测量数据计算得到第二姿态角度；
[0008] S2:根据卡尔曼滤波算法将所述第一姿态角度和所述第二姿态角度进行融合得到 第三姿态角度；
[0009] S3:根据互补滤波原理，将所述第一姿态角度、所述第二姿态角度以及所述第三姿 态角度进行融合计算得到融合姿态角度。
[0010] 优选地，步骤S2中根据卡尔曼滤波算法将所述第一姿态角度和所述第二姿态角度 进行融合得到第三姿态角度还包括:采用姿态角度方差的方法对卡尔曼滤波算法中的过程 噪声协方差和测量噪声协方差进行计算。
[0011] 优选地，步骤S3中，计算所述融合姿态角度时，根据所述第一姿态角度、所述第二 姿态角度以及所述第三姿态角度的方差计算相应的权重，其中方差越大，相应的权重越小。 [00 12]优选地，步骤S1具体包括：
[0013] 以陀螺仪的测量数据和加速度计的测量数据，建立姿态角度测量系统的状态方程 和测量方程分别如下式(1)和式(2):
[0014] (1)
[0015] (2)
[0016] 其中：
[00?7] 在式（1)中，angle_gyro(k)、angle_gyro(k-Ι)分别为k时刻和k-Ι时刻陀螺仪测量 数据计算的第一姿态角度，w_gyro(k-l)为k-1时刻陀螺仪测量的角速度，dt为采样间隔， gyro_bias为陀螺仪的偏差，其值为陀螺仪测量数据的平均值，P(k)为k时刻陀螺仪过程噪 声；
[0018] 在式（2)中，angle_acc(k)为加速度计测量数据计算出的第二姿态角度，M(k)为k 时刻加速度计测量噪声。
[0019] 优选地，步骤S2具体包括：
[0020] S21:根据卡尔曼滤波的离散状态方程和测量方程以及建立的姿态角度测量系统 的状态方程和测量方程进行比较，计算得到卡尔曼滤波算法中的参数；
[0021 ]卡尔曼滤波的离散状态方程和测量方程如下：
[0022] X(k)=S · X(k-1)+C · U(k-1)+P(k) (3)
[0023] Y(k)=H · X(k)+M(k) (4)
[0024] 其中：X(k)和Y(k)分别是k时刻的状态量和测量量;U(k-l)为k-1时刻的系统输入 量;S为系统的状态矩阵，C为系统的控制矩阵;Η为测量矩阵;P和Μ分别表不系统的过程噪声 和测量噪声，Ρ和Μ的协方差分别是Q和Ν;
[0025] 对比式（1)和式（3)可得，系统状态量
，状态矩阵
[0026] 对比式(2)和式(4)可得，系统测量量Y(k) = [angle_acc(k)]，测量矩阵H=[l 0];
[0027] S22:根据陀螺仪的测量数据，由上一时刻陀螺仪测量数据计算的姿态角度预测当 前时刻的姿态角度，计算公式如下：
[0028] angle_gyro(k)=angle_gyro(k_l) + [w_gyro(k_l)_gyro_bias] · dt (5)
[0029] S23:根据加速度计测量数据计算得到的姿态角度与步骤S22中陀螺仪测量数据计 算的姿态角度，计算姿态角度误差，计算公式如下：
[0030] ang1e_err(k)= ang1e_acc(k)-ang1e_gyro(k) (6)
[0031]在式（6)中，angle_acc(k)为k时刻加速度计测量数据计算的第二姿态角度， angle_gyro(k)为步骤S22中计算的第一姿态角度，angle_err(k)为k时刻的上述两者的误 差值；
[0032] S25:计算卡尔曼滤波的状态量X' (k)对应的协方差R(k)，计算公式如下：
[0033] R(k)=S · R(k-l) · ST+Q (9)
[0034] 式(9)中S为控制矩阵，ST为控制矩阵S的转置，Q为过程噪声P的协方差；
[0035] S26:计算卡尔曼滤波增益Kg，计算公式如下：
[0036]
(1〇)
[0037] 式(10)中Η为测量矩阵，Ητ为测量矩阵Η的转置，N为测量噪声Μ的协方差；
[0038] S27:计算修正后的k时刻的卡尔曼滤波的状态量Xlk)，计算方式如下：
[0039] X7 (k)=X7 (k-l)+Kg(k) · angle_err(k) (11)
[0040] S28:将k时刻的卡尔曼滤波的状态量X' (k)中的第一行第一列的值提取出来，即为 根据卡尔曼滤波算法融合所述第一姿态角度和所述第二姿态角度而计算的所述第三姿态 角度，以angle_Kalman(k)表示，计算公式如下：
[0041] angle_Kalman(k) =X7 (k)(1,1) (12)
[0042] 优选地，步骤S2具体还包括：
[0043] S24:采用姿态角度方差的方法计算系统的过程噪声P的协方差Q和测量噪声Μ的协 方差Ν，计算公式如下：
[0044] (7)
[0045] (8)
[0046] 其中：Q_acc为加速度计的过程噪声协方