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

权利要求书

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

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

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

其中：

在式(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＝[1 0]；

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，计算公式如下：

式(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)；

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

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

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

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

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

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

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