差，Q_gyro为陀螺仪的过程噪声协方差， Var(angle_acc)为加速度计测量数据计算的姿态角度的方差，Var(angle_gyro)为陀螺仪 测量数据计算的姿态角度的方差。
[0047] 优选地，步骤S2具体还包括：
[0048] 在进行卡尔曼滤波算法的过程中，对下一时刻的卡尔曼滤波的状态量X' (k+Ι)对 应的协方差R(k+1)进行更新，计算公式如下：
[0049] R(k+l)=R(k)-Kg(k) · Η · R(k) (13)
[0050] 优选地，步骤S2具体还包括:在进行卡尔曼滤波算法之前，将卡尔曼滤波的状态量 X' (k)的初始值X' (0)设定为任意值，将卡尔曼滤波的状态量X' (k)对应的协方差R(k)的初 始值R(〇)设定为非零任意值。
[0051 ] 优选地，步骤S3具体包括：
[0052] S31:根据陀螺仪测量数据计算的所述第一姿态角度和加速度计测量数据计算的 所述第二姿态角度，求出所述第一姿态角度和所述第二姿态角度的权重，计算公式如下：
[0053]
(14)
[0054]其中：B_gyr〇为陀螺仪测量数据计算出的所述第一姿态角度在两种传感器姿态角 度中的权重，Var(angle_acc)为加速度计测量数据计算的姿态角度的方差，Var(angle_ gyro)为陀螺仪测量数据计算的姿态角度的方差；
[0055] S32:计算根据卡尔曼滤波算法计算得到的所述第三姿态角度在互补卡尔曼滤波 算法中的权重，计算公式如下：
[0056]
15)
[0057]其中：A_Kalman为根据卡尔曼滤波算法计算得到的所述第三姿态角度在互补卡尔 曼滤波算法中的权重，Var(angle_Kalman)为根据卡尔曼滤波算法计算的姿态角度的方差； [0058] S33:计算互补卡尔曼滤波算法融合三个姿态角度而计算的融合姿态角度，计算公 式如下：
[0059] angle_ComKalman(k)=A_Kalman · angle_Kalman(k)
[0060] +(l-A_Kalman) · B_gyro · angle_gyro(k) (16)
[0061] +(l_A_Kalman) · (l_B_gyro) · angle_acc(k)
[0062]其中：angle_ComKalman(k)为根据互补卡尔曼滤波算法计算的融合姿态角度。
[0063] 与现有技术相比，本发明的有益效果在于:本发明将卡尔曼滤波算法与互补滤波 算法进行融合，提出一种基于互补卡尔曼滤波算法计算融合姿态角度的方法，充分利用了 陀螺仪测量数据计算的姿态角度、加速度计测量数据计算的姿态角度及卡尔曼滤波算法融 合两种传感器测量数据得到的姿态角度，使三种不同的姿态角度数据相互补偿，使得最终 得到的融合姿态角度的测量精度大大提高。
[0064] 在进一步的方案中，本发明提出了采用计算姿态角度方差的方法来计算卡尔曼滤 波算法中涉及到的参数一一测量噪声协方差和系统过程噪声协方差，解决现有姿态角度融 合算法中涉及的参数计算方法不详问题。在互补滤波算法中，本发明根据各个姿态角度对 应的方差大小赋予相应的权重，使得三种姿态角度数据相互补偿，从而使得在静态条件下 和动态条件下相比现有的卡尔曼滤波算法计算的姿态角度的精度大大提高，其中在动态条 件下姿态角度的测量精度比现有的卡尔曼滤波算法的测量精度提高了 13.3%。
【附图说明】
[0065] 图1是本发明优选实施例的基于互补卡尔曼滤波算法计算融合姿态角度的流程 图；
[0066] 图2是本发明优选实施例的基于互补卡尔曼滤波算法计算融合姿态角度的实验系 统原理图；
[0067] 图3是在静态条件下，本发明优选实施例的互补卡尔曼滤波算法与卡尔曼滤波算 法的对比图；
[0068] 图4是在动态条件下，本发明优选实施例的互补卡尔曼滤波算法与卡尔曼滤波算 法的对比图。
【具体实施方式】
[0069] 下面对照附图并结合优选的实施方式对本发明作进一步说明。
[0070] 卡尔曼滤波是以最小均方误差作为估计的最佳准则，来寻求一套递推估计的算 法，其基本思想是:采用信号与噪声的状态空间模型，利用前一时刻的估计值和现在时刻的 测量值来更新对状态变量的估计，求出现在时刻的估计值，算法根据建立的系统状态方程 和测量方程对需要处理的信号做出满足最小均方误差的估计。卡尔曼滤波对于状态空间模 型的状态矢量估计是一种强有力的手段，在理论上具有重要价值。互补滤波可以根据设计 的权重不同，而有效的利用各个数据。本发明巧妙地将卡尔曼滤波算法和互补滤波算法进 行结合，提出一种基于互补卡尔曼滤波算法计算融合姿态角度的方法，对陀螺仪测量数据 计算的姿态角度、加速度计测量数据计算的姿态角度和卡尔曼滤波算法融合两种传感器测 量数据得到的姿态角度进行融合，本文将这种算法称为互补卡尔曼滤波算法。
[0071] 在一种实施例中，本发明的基于互补卡尔曼滤波算法计算融合姿态角度的方法， 包括以下步骤:首先，搭建姿态角度测量系统，根据陀螺仪测量数据计算得到第一姿态角 度，根据加速度计测量数据计算得到第二姿态角度;其次，根据卡尔曼滤波算法将所述第一 姿态角度和所述第二姿态角度进行融合得到第三姿态角度;最后，根据互补滤波原理，将所 述第一姿态角度、所述第二姿态角度以及所述第三姿态角度进行融合计算得到融合姿态角 度。在进一步的实施例中，对卡尔曼滤波算法中参数一一过程噪声协方差和测量噪声协方 差的计算方法，提出采用计算姿态角度方差的方法进行描述;另外，计算所述融合姿态角度 时，根据所述第一姿态角度、所述第二姿态角度以及所述第三姿态角度的方差计算相应的 权重，其中方差越大，相应的权重越小。
[0072] 结合图1，本发明优选实施例的基于互补卡尔曼滤波算法计算融合姿态角度的方 法包括以下步骤：
[0073] S1:以陀螺仪的测量数据和加速度计的测量数据，建立姿态角度测量系统的状态 方程和测量方程分别如下式(1)和式(2):
[0074]
[0075]
[0076] 其中：
[0077] 在式（1)中，angle_gyro(k)、angle_gyro(k-Ι)分别为k时刻和k-Ι时刻陀螺仪测量 数据计算的第一姿态角度，w_gyro(k-l)为k-1时刻陀螺仪测量的角速度，dt为采样间隔， gyro_bias为陀螺仪的偏差，其值为陀螺仪测量数据的平均值，P(k)为k时刻陀螺仪过程噪 声；
[0078]在式（2)中，angle_aCC(k)为加速度计测量数据计算出的第二姿态角度，M(k)为k 时刻加速度计测量噪声。
[0079] S2:用卡尔曼滤波算法对陀螺仪测量数据和加速度计测量数据进行融合，具体包 括以下步骤：
[0080] S21:根据卡尔曼滤波的离散状态方程和测量方程以及建立的姿态角度测量系统 的状态方程和测量方程进行比较，计算得到卡尔曼滤波算法中的参数；
[0081 ]卡尔曼滤波的离散状态方程和测量方程如下：
[0082] X(k)=S · X(k-1)+C · U(k-1)+P(k) (3)
[0083] Y(k)=H · X(k)+M(k) (4)
[0084] 其中：X(k)和Y(k)分别是k时刻的状态量和测量量;U(k-l)为k-1时刻的系统输入 量;S为系统的状态矩阵，C为系统的控制矩阵;Η为测量矩阵;P和Μ分别表不系统的过程噪声 和测量噪声，Ρ和Μ的协方差分别是Q和Ν; angle gyro(k)
[0085] 对比式（1)和式（3)可得，系统