CN105651242A - 一种基于互补卡尔曼滤波算法计算融合姿态角度的方法 - Google Patents

Abstract

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

Description

一种基于互补卡尔曼滤波算法计算融合姿态角度的方法

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

$\left[\begin{array}{c}\mathrm{angle}\_\mathrm{gyro}\left(k\right)\\ \mathrm{gyro}\_\mathrm{bias}\end{array}\right]=\left[\begin{array}{cc}1& -\mathrm{dt}\\ 0& 1\end{array}\right]\•\left[\begin{array}{c}\mathrm{angle}\_\mathrm{gyro}(k-1)\\ \mathrm{gyro}\_\mathrm{bias}\end{array}\right]+\begin{array}{}\left[\begin{array}{c}\mathrm{dt}\\ 0\end{array}\right]\end{array}\left[\begin{array}{c}\end{array},\left[\begin{array}{}\end{array},w\_\mathrm{gyro}(k-1)+P\left(k\right)---\left(1\right)\right]\right]$

$[\mathrm{angle}\_\mathrm{acc}(k\left)\right]=\left[\begin{array}{cc}1& 0\end{array}]\•\left[\begin{array}{c}\mathrm{angle}\_\mathrm{gyro}\left(k\right)\\ \mathrm{gyro}\_\mathrm{bias}\end{array}\right]\right]+M\left(k\right)$

其中：

在式(1)中，angle_gyro(k)、angle_gyro(k-1)分别为k时刻和k-1时刻陀螺仪测量数据计算的第一姿态角度，w_gyro(k-1)为k-1时刻陀螺仪测量的角速度，dt为采样间隔，gyro_bias为陀螺仪的偏差，其值为陀螺仪测量数据的平均值，P(k)为k时刻陀螺仪过程噪声；

在式(2)中，angle_acc(k)为加速度计测量数据计算出的第二姿态角度，M(k)为k时刻加速度计测量噪声。

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

S21：根据卡尔曼滤波的离散状态方程和测量方程以及建立的姿态角度测量系统的状态方程和测量方程进行比较，计算得到卡尔曼滤波算法中的参数；

卡尔曼滤波的离散状态方程和测量方程如下：

X(k)＝S·X(k-1)+C·U(k-1)+P(k)(3)

Y(k)＝H·X(k)+M(k)(4)

其中：X(k)和Y(k)分别是k时刻的状态量和测量量；U(k-1)为k-1时刻的系统输入量；S为系统的状态矩阵，C为系统的控制矩阵；H为测量矩阵；P和M分别表示系统的过程噪声和测量噪声，P和M的协方差分别是Q和N；

对比式(1)和式(3)可得，系统状态量状态矩阵控制矩阵

对比式(2)和式(4)可得，系统测量量Y(k)＝[angle_acc(k)]，测量矩阵H＝[10]；

S22：根据陀螺仪的测量数据，由上一时刻陀螺仪测量数据计算的姿态角度预测当前时刻的姿态角度，计算公式如下：

angle_gyro(k)＝angle_gyro(k-1)+[w_gyro(k-1)-gyro_bias]·dt(5)

S23：根据加速度计测量数据计算得到的姿态角度与步骤S22中陀螺仪测量数据计算的姿态角度，计算姿态角度误差，计算公式如下：

angle_err(k)＝angle_acc(k)-angle_gyro(k)(6)

在式(6)中，angle_acc(k)为k时刻加速度计测量数据计算的第二姿态角度，angle_gyro(k)为步骤S22中计算的第一姿态角度，angle_err(k)为k时刻的上述两者的误差值；

S25：计算卡尔曼滤波的状态量X′(k)对应的协方差R(k)，计算公式如下：

R(k)＝S·R(k-1)·S^T+Q(9)

式(9)中S为控制矩阵，S^T为控制矩阵S的转置，Q为过程噪声P的协方差；

S26：计算卡尔曼滤波增益K_g，计算公式如下：

$K_g\left(k\right)=\frac{R\left(k\right)\·H^T}{H\·R\left(k\right)\·H^T+N}---\left(10\right)$

式(10)中H为测量矩阵，H^T为测量矩阵H的转置，N为测量噪声M的协方差；

S27：计算修正后的k时刻的卡尔曼滤波的状态量X′(k)，计算方式如下：

X′(k)＝X′(k-1)+K_g(k)·angle_err(k)(11)

S28：将k时刻的卡尔曼滤波的状态量X′(k)中的第一行第一列的值提取出来，即为根据卡尔曼滤波算法融合所述第一姿态角度和所述第二姿态角度而计算的所述第三姿态角度，以angle_Kalman(k)表示，计算公式如下：

angle_Kalman(k)＝X′(k)(1,1)(12)

优选地，步骤S2具体还包括：

S24：采用姿态角度方差的方法计算系统的过程噪声P的协方差Q和测量噪声M的协方差N，计算公式如下：

$Q=\left[\begin{array}{cc}Q\_acc& 0\\ 0& Q\_gyro\end{array}\right]=\left[\begin{array}{cc}Var(angle\_acc)& 0\\ 0& Var(angle\_gyro)\end{array}\right]---\left(7\right)$

$N=\sqrt{{\[Var(angle\_acc)\]}^2+{\[Var(angle\_gyro)\]}^2}---\left(8\right)$

其中：Q_acc为加速度计的过程噪声协方差，Q_gyro为陀螺仪的过程噪声协方差，Var(angle_acc)为加速度计测量数据计算的姿态角度的方差，Var(angle_gyro)为陀螺仪测量数据计算的姿态角度的方差。

优选地，步骤S2具体还包括：

在进行卡尔曼滤波算法的过程中，对下一时刻的卡尔曼滤波的状态量X′(k+1)对应的协方差R(k+1)进行更新，计算公式如下：

R(k+1)＝R(k)-K_g(k)·H·R(k)(13)

优选地，步骤S2具体还包括：在进行卡尔曼滤波算法之前，将卡尔曼滤波的状态量X′(k)的初始值X′(0)设定为任意值，将卡尔曼滤波的状态量X′(k)对应的协方差R(k)的初始值R(0)设定为非零任意值。

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

S31：根据陀螺仪测量数据计算的所述第一姿态角度和加速度计测量数据计算的所述第二姿态角度，求出所述第一姿态角度和所述第二姿态角度的权重，计算公式如下：

$B\_gyro=\frac{Var(angle\_acc)}{Var(angle\_gyro)+Var(angle\_acc)}---\left(14\right)$

其中：B_gyro为陀螺仪测量数据计算出的所述第一姿态角度在两种传感器姿态角度中的权重，Var(angle_acc)为加速度计测量数据计算的姿态角度的方差，Var(angle_gyro)为陀螺仪测量数据计算的姿态角度的方差；

S32：计算根据卡尔曼滤波算法计算得到的所述第三姿态角度在互补卡尔曼滤波算法中的权重，计算公式如下：

$A\_Kalman=\frac{Var(angle\_gyro)+Var(angle\_acc)}{Var(angle\_Kalman)+Var(angle\_gyro)+Var(angle\_acc)}---\left(15\right)$

其中：A_Kalman为根据卡尔曼滤波算法计算得到的所述第三姿态角度在互补卡尔曼滤波算法中的权重，Var(angle_Kalman)为根据卡尔曼滤波算法计算的姿态角度的方差；

S33：计算互补卡尔曼滤波算法融合三个姿态角度而计算的融合姿态角度，计算公式如下：

angle_ComKalman(k)＝A_Kalman·angle_Kalman(k)

+(1-A_Kalman)·B_gyro·angle_gyro(k)(16)

+(1-A_Kalman)·(1-B_gyro)·angle_acc(k)

其中：angle_ComKalman(k)为根据互补卡尔曼滤波算法计算的融合姿态角度。

与现有技术相比，本发明的有益效果在于：本发明将卡尔曼滤波算法与互补滤波算法进行融合，提出一种基于互补卡尔曼滤波算法计算融合姿态角度的方法，充分利用了陀螺仪测量数据计算的姿态角度、加速度计测量数据计算的姿态角度及卡尔曼滤波算法融合两种传感器测量数据得到的姿态角度，使三种不同的姿态角度数据相互补偿，使得最终得到的融合姿态角度的测量精度大大提高。

在进一步的方案中，本发明提出了采用计算姿态角度方差的方法来计算卡尔曼滤波算法中涉及到的参数——测量噪声协方差和系统过程噪声协方差，解决现有姿态角度融合算法中涉及的参数计算方法不详问题。在互补滤波算法中，本发明根据各个姿态角度对应的方差大小赋予相应的权重，使得三种姿态角度数据相互补偿，从而使得在静态条件下和动态条件下相比现有的卡尔曼滤波算法计算的姿态角度的精度大大提高，其中在动态条件下姿态角度的测量精度比现有的卡尔曼滤波算法的测量精度提高了13.3％。

附图说明

图1是本发明优选实施例的基于互补卡尔曼滤波算法计算融合姿态角度的流程图；

图2是本发明优选实施例的基于互补卡尔曼滤波算法计算融合姿态角度的实验系统原理图；

图3是在静态条件下，本发明优选实施例的互补卡尔曼滤波算法与卡尔曼滤波算法的对比图；

图4是在动态条件下，本发明优选实施例的互补卡尔曼滤波算法与卡尔曼滤波算法的对比图。

具体实施方式

下面对照附图并结合优选的实施方式对本发明作进一步说明。

卡尔曼滤波是以最小均方误差作为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻的估计值和现在时刻的测量值来更新对状态变量的估计，求出现在时刻的估计值，算法根据建立的系统状态方程和测量方程对需要处理的信号做出满足最小均方误差的估计。卡尔曼滤波对于状态空间模型的状态矢量估计是一种强有力的手段，在理论上具有重要价值。互补滤波可以根据设计的权重不同，而有效的利用各个数据。本发明巧妙地将卡尔曼滤波算法和互补滤波算法进行结合，提出一种基于互补卡尔曼滤波算法计算融合姿态角度的方法，对陀螺仪测量数据计算的姿态角度、加速度计测量数据计算的姿态角度和卡尔曼滤波算法融合两种传感器测量数据得到的姿态角度进行融合，本文将这种算法称为互补卡尔曼滤波算法。

在一种实施例中，本发明的基于互补卡尔曼滤波算法计算融合姿态角度的方法，包括以下步骤：首先，搭建姿态角度测量系统，根据陀螺仪测量数据计算得到第一姿态角度，根据加速度计测量数据计算得到第二姿态角度；其次，根据卡尔曼滤波算法将所述第一姿态角度和所述第二姿态角度进行融合得到第三姿态角度；最后，根据互补滤波原理，将所述第一姿态角度、所述第二姿态角度以及所述第三姿态角度进行融合计算得到融合姿态角度。在进一步的实施例中，对卡尔曼滤波算法中参数——过程噪声协方差和测量噪声协方差的计算方法，提出采用计算姿态角度方差的方法进行描述；另外，计算所述融合姿态角度时，根据所述第一姿态角度、所述第二姿态角度以及所述第三姿态角度的方差计算相应的权重，其中方差越大，相应的权重越小。

结合图1，本发明优选实施例的基于互补卡尔曼滤波算法计算融合姿态角度的方法包括以下步骤：

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

$\left[\begin{array}{c}\mathrm{angle}\_\mathrm{gyro}\left(k\right)\\ \mathrm{gyro}\_\mathrm{bias}\end{array}\right]=\left[\begin{array}{cc}1& -\mathrm{dt}\\ 0& 1\end{array}\right]\•\left[\begin{array}{c}\mathrm{angle}\_\mathrm{gyro}(k-1)\\ \mathrm{gyro}\_\mathrm{bias}\end{array}\right]+\left[\begin{array}{c}\mathrm{dt}\\ 0\end{array}\right]w\_\mathrm{gyro}(k-1)+P\left(k\right)---\left(1\right)$

$[\mathrm{angle}\_\mathrm{acc}(k\left)\right]=\left[\begin{array}{cc}1& 0\end{array}\right]\•\left[\begin{array}{c}\mathrm{angle}\_\mathrm{gyro}\left(k\right)\\ \mathrm{gyro}\_\mathrm{bias}\end{array}\right]+M\left(k\right)$

其中：

在式(1)中，angle_gyro(k)、angle_gyro(k-1)分别为k时刻和k-1时刻陀螺仪测量数据计算的第一姿态角度，w_gyro(k-1)为k-1时刻陀螺仪测量的角速度，dt为采样间隔，gyro_bias为陀螺仪的偏差，其值为陀螺仪测量数据的平均值，P(k)为k时刻陀螺仪过程噪声；

在式(2)中，angle_acc(k)为加速度计测量数据计算出的第二姿态角度，M(k)为k时刻加速度计测量噪声。

S2：用卡尔曼滤波算法对陀螺仪测量数据和加速度计测量数据进行融合，具体包括以下步骤：

S21：根据卡尔曼滤波的离散状态方程和测量方程以及建立的姿态角度测量系统的状态方程和测量方程进行比较，计算得到卡尔曼滤波算法中的参数；

卡尔曼滤波的离散状态方程和测量方程如下：

X(k)＝S·X(k-1)+C·U(k-1)+P(k)(3)

Y(k)＝H·X(k)+M(k)(4)

其中：X(k)和Y(k)分别是k时刻的状态量和测量量；U(k-1)为k-1时刻的系统输入量；S为系统的状态矩阵，C为系统的控制矩阵；H为测量矩阵；P和M分别表示系统的过程噪声和测量噪声，P和M的协方差分别是Q和N；

对比式(1)和式(3)可得，系统状态量状态矩阵控制矩阵

对比式(2)和式(4)可得，系统测量量Y(k)＝[angle_acc(k)]，测量矩阵H＝[10]；

S22：根据陀螺仪的测量数据，由上一时刻陀螺仪测量数据计算的姿态角度预测当前时刻的姿态角度，计算公式如下：

angle_gyro(k)＝angle_gyro(k-1)+[w_gyro(k-1)-gyro_bias]·dt(5)

S23：根据加速度计测量数据计算得到的姿态角度与步骤S22中陀螺仪测量数据计算的姿态角度，计算姿态角度误差，计算公式如下：

angle_err(k)＝angle_acc(k)-angle_gyro(k)(6)

在式(6)中，angle_acc(k)为k时刻加速度计测量数据计算的第二姿态角度，angle_gyro(k)为步骤S22中计算的第一姿态角度，angle_err(k)为k时刻的上述两者的误差值；

S24：采用姿态角度方差的方法计算系统的过程噪声P的协方差Q和测量噪声M的协方差N，计算公式如下：

$Q=\left[\begin{array}{cc}Q\_acc& 0\\ 0& Q\_gyro\end{array}\right]=\left[\begin{array}{cc}Var(angle\_acc)& 0\\ 0& Var(angle\_gyro)\end{array}\right]---\left(7\right)$

$N=\sqrt{{\[Var(angle\_acc)\]}^2+{\[Var(angle\_gyro)\]}^2}---\left(8\right)$

其中：Q_acc为加速度计的过程噪声协方差，Q_gyro为陀螺仪的过程噪声协方差，Var(angle_acc)为加速度计测量数据计算的姿态角度的方差，Var(angle_gyro)为陀螺仪测量数据计算的姿态角度的方差；

S25：计算卡尔曼滤波的状态量X′(k)对应的协方差R(k)，计算公式如下：

R(k)＝S·R(k-1)·S^T+Q(9)

式(9)中S为控制矩阵，S^T为控制矩阵S的转置，Q为过程噪声P的协方差；

S26：计算卡尔曼滤波增益K_g，计算公式如下：

$K_g\left(k\right)=\frac{R\left(k\right)\·H^T}{H\·R\left(k\right)\·H^T+N}---\left(10\right)$

式(10)中H为测量矩阵，H^T为测量矩阵H的转置，N为测量噪声M的协方差；

S27：计算修正后的k时刻的卡尔曼滤波的状态量X′(k)，计算方式如下：

X′(k)＝X′(k-1)+K_g(k)·angle_err(k)(11)

S28：将k时刻的卡尔曼滤波的状态量X′(k)中的第一行第一列的值提取出来，即为根据卡尔曼滤波算法融合所述第一姿态角度和所述第二姿态角度而计算的所述第三姿态角度，以angle_Kalman(k)表示，计算公式如下：

angle_Kalman(k)＝X′(k)(1,1)(12)

S29：为了使卡尔曼滤波算法持续不断的运行，对下一时刻的卡尔曼滤波的状态量X′(k+1)对应的协方差R(k+1)进行更新，计算公式如下：

R(k+1)＝R(k)-K_g(k)·H·R(k)(13)

其中，为了使互补卡尔曼滤波算法开始工作，在进行卡尔曼滤波算法之前需要设定卡尔曼滤波算法的状态量X′(k)的初始值X′(0)及其对应的协方差R(k)的初始值R(0)，这两个值都可以设定为任意值，因为随着卡尔曼滤波算法的运行，卡尔曼滤波算法的状态量X′(k)会逐渐收敛，其中，对于R(0)的值一般不设为零值，由于设为零值时可能使卡尔曼滤波算法认为给定的X′(0)是最优的，从而使算法不能收敛，因此本发明中将R(0)设定为非零任意值。

S3：利用陀螺仪测量数据计算的第一姿态角度、加速度计测量数据计算的第二姿态角度及卡尔曼滤波算法融合两种传感器测量数据而计算的第三姿态角度，根据三者方差赋予相应的姿态角度权重，方差大者，在互补卡尔曼滤波算法中所占比重小。基于上述思想，设计互补卡尔曼滤波算法，计算过程包括如下步骤：

S31：根据陀螺仪测量数据计算的所述第一姿态角度和加速度计测量数据计算的所述第二姿态角度，求出所述第一姿态角度和所述第二姿态角度的权重，计算公式如下：

$B\_gyro=\frac{Var(angle\_acc)}{Var(angle\_gyro)+Var(angle\_acc)}---\left(14\right)$

其中：B_gyro为陀螺仪测量数据计算出的所述第一姿态角度在两种传感器姿态角度中的权重，Var(angle_acc)为加速度计测量数据计算的姿态角度的方差，Var(angle_gyro)为陀螺仪测量数据计算的姿态角度的方差；

S32：计算根据卡尔曼滤波算法计算得到的所述第三姿态角度在互补卡尔曼滤波算法中的权重，计算公式如下：

$A\_Kalman=\frac{Var(angle\_gyro)+Var(angle\_acc)}{Var(angle\_Kalman)+Var(angle\_gyro)+Var(angle\_acc)}---\left(15\right)$

其中：A_Kalman为根据卡尔曼滤波算法计算得到的所述第三姿态角度在互补卡尔曼滤波算法中的权重，Var(angle_Kalman)为根据卡尔曼滤波算法计算的姿态角度的方差；

S33：计算互补卡尔曼滤波算法融合三个姿态角度而计算的融合姿态角度，计算公式如下：

angle_ComKalman(k)＝A_Kalman·angle_Kalman(k)

+(1-A_Kalman)·B_gyro·angle_gyro(k)(16)

+(1-A_Kalman)·(1-B_gyro)·angle_acc(k)

其中：angle_ComKalman(k)为根据互补卡尔曼滤波算法计算的融合姿态角度。

为了验证本发明，采用如图2所示的基于互补卡尔曼滤波算法计算融合姿态角度的方法的实验系统来进行计算，以STM32F103RB单片机为控制器，传感器采用MAX21100单芯片“3轴陀螺仪+3轴加速度计”惯性测量单元(IMU)。该系统还包括电源稳压电路(采用LT8620降压型开关稳压器)、晶振电路(晶振频率为8MHz)、复位电路、输出短路保护电路(采用LT3579EFE具故障保护功能的升压/负输出转换器)、D/A转换滤波电路、RS-232线路驱动/接收电路、CMSIS-DAP仿真下载器、输出信号放大电路等。传感器固定连接在微波天线上，利用振动台来产生振动信号，将传感器信号通过RS232接口传给STM32单片机，STM32单片机将采集到的陀螺仪测量数据和加速度计测量数据输出到PC机上并保存，后续利用MATLAB编程处理数据，对本发明的一种基于互补卡尔曼滤波算法计算的融合姿态角度的方法进行验证。验证结果如图3和图4所示，其中图3是在静态条件下，本发明的互补卡尔曼滤波算法与卡尔曼滤波算法计算的姿态角度的对比图，其中采用卡尔曼滤波算法的方差为0.0010194，而采用本发明的互补卡尔曼滤波算法的方差为0.0007526，方差大大下降，精度得到提高；图4是在动态条件下，本发明的互补卡尔曼滤波算法与卡尔曼滤波算法计算的姿态角度的对比图，采用本发明的互补卡尔曼滤波算法计算的融合姿态角度比卡尔曼滤波算法计算的姿态角度的测量精度提高了13.3％。

本发明的基本思路是：首先搭建微波天线姿态角度测量系统，对卡尔曼滤波算法中参数——过程噪声协方差和测量噪声协方差的计算方法，提出采用计算姿态角度方差的方法进行描述；其次，根据互补滤波原理，提出融合陀螺仪测量数据计算的第一姿态角度、加速度计测量数据计算的第二姿态角度和卡尔曼滤波算法融合两种传感器测量数据得到的第三姿态角度的互补卡尔曼滤波算法；最后，利用测量系统采集数据，基于MATLAB编写互补卡尔曼滤波算法程序，对互补卡尔曼滤波算法进行验证，最终验证得到本发明的基于互补卡尔曼滤波算法计算的融合姿态角度的测量精度大大提高。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。

Claims (9)

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

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

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

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

2.根据权利要求1所述的方法，其特征在于，步骤S2中根据卡尔曼滤波算法将所述第一姿态角度和所述第二姿态角度进行融合得到第三姿态角度还包括：采用姿态角度方差的方法对卡尔曼滤波算法中的过程噪声协方差和测量噪声协方差进行计算。

3.根据权利要求1或2所述的方法，其特征在于，步骤S3中，计算所述融合姿态角度时，根据所述第一姿态角度、所述第二姿态角度以及所述第三姿态角度的方差计算相应的权重，其中方差越大，相应的权重越小。

4.根据权利要求1所述的方法，其特征在于，步骤S1具体包括：

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

$\left[\begin{array}{c}angle\_gyro\left(k\right)\\ gyro\_bias\end{array}\right]=\left[\begin{array}{cc}1& -dt\\ 0& 1\end{array}\right]\·\left[\begin{array}{c}angle\_gyro\left(k-1\right)\\ gyro\_bias\end{array}\right]+\left[\begin{array}{c}dt\\ 0\end{array}\right]w\_gyro\left(k-1\right)+P\left(k\right)---\left(1\right)$

$\[angle\_acc\left(k\right)\]=\left[\begin{array}{cc}1& 0\end{array}\right]\·\left[\begin{array}{c}angle\_gyro\left(k\right)\\ gyro\_bias\end{array}\right]+M\left(k\right)---\left(2\right)$

其中：

在式(1)中，angle_gyro(k)、angle_gyro(k-1)分别为k时刻和k-1时刻陀螺仪测量数据计算的第一姿态角度，w_gyro(k-1)为k-1时刻陀螺仪测量的角速度，dt为采样间隔，gyro_bias为陀螺仪的偏差，其值为陀螺仪测量数据的平均值，P(k)为k时刻陀螺仪过程噪声；

在式(2)中，angle_acc(k)为加速度计测量数据计算出的第二姿态角度，M(k)为k时刻加速度计测量噪声。

5.根据权利要求4所述的方法，其特征在于，步骤S2具体包括：

S21：根据卡尔曼滤波的离散状态方程和测量方程以及建立的姿态角度测量系统的状态方程和测量方程进行比较，计算得到卡尔曼滤波算法中的参数；

卡尔曼滤波的离散状态方程和测量方程如下：

X(k)＝S·X(k-1)+C·U(k-1)+P(k)(3)

Y(k)＝H·X(k)+M(k)(4)

其中：X(k)和Y(k)分别是k时刻的状态量和测量量；U(k-1)为k-1时刻的系统输入量；S为系统的状态矩阵，C为系统的控制矩阵；H为测量矩阵；P和M分别表示系统的过程噪声和测量噪声，P和M的协方差分别是Q和N；

对比式(1)和式(3)可得，系统状态量状态矩阵控制矩阵

对比式(2)和式(4)可得，系统测量量Y(k)＝[angle_acc(k)]，测量矩阵H＝[10]；

S22：根据陀螺仪的测量数据，由上一时刻陀螺仪测量数据计算的姿态角度预测当前时刻的姿态角度，计算公式如下：

angle_gyro(k)＝angle_gyro(k-1)+[w_gyro(k-1)-gyro_bias]·dt(5)

S23：根据加速度计测量数据计算得到的姿态角度与步骤S22中陀螺仪测量数据计算的姿态角度，计算姿态角度误差，计算公式如下：

angle_err(k)＝angle_acc(k)-angle_gyro(k)(6)

在式(6)中，angle_acc(k)为k时刻加速度计测量数据计算的第二姿态角度，angle_gyro(k)为步骤S22中计算的第一姿态角度，angle_err(k)为k时刻的上述两者的误差值；

S25：计算卡尔曼滤波的状态量X′(k)对应的协方差R(k)，计算公式如下：

R(k)＝S·R(k-1)·S^T+Q(9)式(9)中S为控制矩阵，S^T为控制矩阵S的转置，Q为过程噪声P的协方差；

S26：计算卡尔曼滤波增益K_g，计算公式如下：

$K_g\left(k\right)=\frac{R\left(k\right)\·H^T}{H\·R\left(k\right)\·H^T+N}---\left(10\right)$

式(10)中H为测量矩阵，H^T为测量矩阵H的转置，N为测量噪声M的协方差；

S27：计算修正后的k时刻的卡尔曼滤波的状态量X′(k)，计算方式如下：

X′(k)＝X′(k-1)+K_g(k)·angle_err(k)(11)

S28：将k时刻的卡尔曼滤波的状态量X′(k)中的第一行第一列的值提取出来，即为根据卡尔曼滤波算法融合所述第一姿态角度和所述第二姿态角度而计算的所述第三姿态角度，以angle_Kalman(k)表示，计算公式如下：

angle_Kalman(k)＝X′(k)(1,1)(12)

6.根据权利要求5所述的方法，其特征在于，步骤S2具体还包括：

S24：采用姿态角度方差的方法计算系统的过程噪声P的协方差Q和测量噪声M的协方差N，计算公式如下：

$Q=\left[\begin{array}{cc}Q\_acc& 0\\ 0& Q\_gyro\end{array}\right]=\left[\begin{array}{cc}Var(angle\_acc)& 0\\ 0& Var(angle\_gyro)\end{array}\right]---\left(7\right)$

$N=\sqrt{{\[Var(angle\_acc)\]}^2+{\[Var(angle\_gyro)\]}^2}---\left(8\right)$

其中：Q_acc为加速度计的过程噪声协方差，Q_gyro为陀螺仪的过程噪声协方差，Var(angle_acc)为加速度计测量数据计算的姿态角度的方差，Var(angle_gyro)为陀螺仪测量数据计算的姿态角度的方差。

7.根据权利要求5所述的方法，其特征在于，步骤S2具体还包括：

在进行卡尔曼滤波算法的过程中，对下一时刻的卡尔曼滤波的状态量X′(k+1)对应的协方差R(k+1)进行更新，计算公式如下：

R(k+1)＝R(k)-K_g(k)·H·R(k)(13)

8.根据权利要求5至7任一项所述的方法，其特征在于，步骤S2具体还包括：在进行卡尔曼滤波算法之前，将卡尔曼滤波的状态量X′(k)的初始值X′(0)设定为任意值，将卡尔曼滤波的状态量X′(k)对应的协方差R(k)的初始值R(0)设定为非零任意值。

9.根据权利要求5至7任一项所述的方法，其特征在于，步骤S3具体包括：

S31：根据陀螺仪测量数据计算的所述第一姿态角度和加速度计测量数据计算的所述第二姿态角度，求出所述第一姿态角度和所述第二姿态角度的权重，计算公式如下：

$B\_gyro=\frac{Var(angle\_acc)}{Var(angle\_gyro)+Var(angle\_acc)}---\left(14\right)$

其中：B_gyro为陀螺仪测量数据计算出的所述第一姿态角度在两种传感器姿态角度中的权重，Var(angle_acc)为加速度计测量数据计算的姿态角度的方差，Var(angle_gyro)为陀螺仪测量数据计算的姿态角度的方差；

S32：计算根据卡尔曼滤波算法计算得到的所述第三姿态角度在互补卡尔曼滤波算法中的权重，计算公式如下：

$A\_Kalman=\frac{Var(angle\_gyro)+Var(angle\_acc)}{Var(angle\_Kalman)+Var(angle\_gyro)+Var(angle\_acc)}---\left(15\right)$

其中：A_Kalman为根据卡尔曼滤波算法计算得到的所述第三姿态角度在互补卡尔曼滤波算法中的权重，Var(angle_Kalman)为根据卡尔曼滤波算法计算的姿态角度的方差；

S33：计算互补卡尔曼滤波算法融合三个姿态角度而计算的融合姿态角度，计算公式如下：

$\begin{array}{c}angle\_ComKalman\left(k\right)=A\_Kalman\·angle\_Kalman\left(k\right)\\ +\left(1-A\_Kalman\right)\·B\_gyro\·angle\_gyro\left(k\right)\\ +\left(1-A\_Kalman\right)\·\left(1-B\_gyro\right)\·angle\_acc\left(k\right)\end{array}---\left(16\right)$

其中：angle_ComKalman(k)为根据互补卡尔曼滤波算法计算的融合姿态角度。