CN105651242B - 一种基于互补卡尔曼滤波算法计算融合姿态角度的方法 - Google Patents
一种基于互补卡尔曼滤波算法计算融合姿态角度的方法 Download PDFInfo
- Publication number
- CN105651242B CN105651242B CN201610207713.4A CN201610207713A CN105651242B CN 105651242 B CN105651242 B CN 105651242B CN 201610207713 A CN201610207713 A CN 201610207713A CN 105651242 B CN105651242 B CN 105651242B
- Authority
- CN
- China
- Prior art keywords
- attitude angle
- angle
- kalman filtering
- calculated
- filtering algorithm
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
- 238000001914 filtration Methods 0.000 title claims abstract description 125
- 238000000034 method Methods 0.000 title claims abstract description 60
- 230000000295 complement effect Effects 0.000 title claims abstract description 51
- 230000004927 fusion Effects 0.000 title claims abstract description 43
- 238000005259 measurement Methods 0.000 claims abstract description 96
- 238000004364 calculation method Methods 0.000 claims abstract description 52
- 239000011159 matrix material Substances 0.000 claims description 27
- 230000017105 transposition Effects 0.000 claims description 6
- 230000009897 systematic effect Effects 0.000 claims description 4
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 3
- 230000001133 acceleration Effects 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 5
- 238000000205 computational method Methods 0.000 description 4
- 241001269238 Data Species 0.000 description 3
- 230000006641 stabilisation Effects 0.000 description 3
- 238000011105 stabilization Methods 0.000 description 3
- 230000003068 static effect Effects 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 239000013078 crystal Substances 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- 230000003321 amplification Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 239000000155 melt Substances 0.000 description 1
- 238000002156 mixing Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000003199 nucleic acid amplification method Methods 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 230000000452 restraining effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C1/00—Measuring angles
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
Abstract
本发明公开了一种基于互补卡尔曼滤波算法计算融合姿态角度的方法，包括以下步骤：搭建姿态角度测量系统，根据陀螺仪测量数据计算得到第一姿态角度，根据加速度计测量数据计算得到第二姿态角度；根据卡尔曼滤波算法将所述第一姿态角度和所述第二姿态角度进行融合得到第三姿态角度；根据互补滤波原理，将所述第一姿态角度、所述第二姿态角度以及所述第三姿态角度进行融合计算得到融合姿态角度。本发明提出的基于互补卡尔曼滤波算法计算融合姿态角度的方法，大大提高了姿态角度的测量精度。
Description
技术领域
本发明涉及姿态角度测量领域，尤其涉及一种基于互补卡尔曼滤波算法计算融合姿态角度的方法。
背景技术
在微波通信防抖系统中，微波天线的姿态角度是非常重要的反馈信号，防抖系统利用该反馈信号对微波天线姿态角度进行校正，使微波天线抖动幅度始终在要求的精度范围内，以保证通信信号的稳定性。因此，微波天线姿态角度的测量精度和稳定性至关重要。常用姿态角度测量元件有陀螺仪、加速度计等，陀螺仪测量角速度信号，但其测量值会随时间漂移，使得计算的角度值也随时间漂移；加速度计测量角加速度信号，但动态响应较差。因此，陀螺仪和加速度计通常不单独使用，而是根据两者的不同特性，采用某种算法对两者的测量数据进行融合得出精度更高的姿态角度测量值。
对陀螺仪和加速度计测量数据进行融合常用的算法有卡尔曼滤波算法和互补滤波算法。陈兴林等的“一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法”对卡尔曼滤波数据融合方法进行了部分改进，解决了现有卡尔曼滤波算法在四旋翼无人机姿态运算过程中运算量太大而无法实时获取姿态数据的问题；杜靖等的“基于卡尔曼滤波的加速度计陀螺仪水平角度测量方法”利用卡尔曼滤波算法对陀螺仪和加速度计测量数据进行了融合，解决了加速度计陀螺仪测量水平角度在动态测量环境下精度差的问题，但是两者对卡尔曼滤波算法中的参数——测量噪声协方差和过程噪声协方差只给出说明，并未详细说明实际应用过程中的计算方法，而测量噪声协方差和过程噪声协方差对卡尔曼滤波算法的精度有着重要影响。罗洁等的“一种用于盲杖的陀螺稳定装置及其互补滤波方法”涉及盲杖稳定装置，利用互补滤波算法融合陀螺仪和加速度计测量数据，但对算法中不同传感器测量数据权重的计算方法没有给出具体说明，在采用该种算法过程中会涉及到大量试验来确定权重值。
发明内容
为解决现有姿态角度融合算法中涉及到的参数计算方法不详问题以及进一步提高姿态角度融合算法的精度，本发明提出一种基于互补卡尔曼滤波算法计算融合姿态角度的方法，大大提高了姿态角度的测量精度。
为达到上述目的，本发明采用以下技术方案：
本发明公开了一种基于互补卡尔曼滤波算法计算融合姿态角度的方法，其特征在于，包括以下步骤：
S1：搭建姿态角度测量系统，根据陀螺仪测量数据计算得到第一姿态角度，根据加速度计测量数据计算得到第二姿态角度；
S2：根据卡尔曼滤波算法将所述第一姿态角度和所述第二姿态角度进行融合得到第三姿态角度；
S3：根据互补滤波原理，将所述第一姿态角度、所述第二姿态角度以及所述第三姿态角度进行融合计算得到融合姿态角度。
优选地，步骤S2中根据卡尔曼滤波算法将所述第一姿态角度和所述第二姿态角度进行融合得到第三姿态角度还包括：采用姿态角度方差的方法对卡尔曼滤波算法中的过程噪声协方差和测量噪声协方差进行计算。
优选地，步骤S3中，计算所述融合姿态角度时，根据所述第一姿态角度、所述第二姿态角度以及所述第三姿态角度的方差计算相应的权重，其中方差越大，相应的权重越小。
优选地，步骤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(k1)-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)·ST+Q (9)
式(9)中S为控制矩阵，ST为控制矩阵S的转置，Q为过程噪声P的协方差；
S26：计算卡尔曼滤波增益Kg，计算公式如下：
式(10)中H为测量矩阵，HT为测量矩阵H的转置，N为测量噪声M的协方差；
S27：计算修正后的k时刻的卡尔曼滤波的状态量X′(k)，计算方式如下：
X′(k)＝X′(k-1)+Kg(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_acc为加速度计的过程噪声协方差，Q_gyro为陀螺仪的过程噪声协方差，Var(angle_acc)为加速度计测量数据计算的姿态角度的方差，Var(angle_gyro)为陀螺仪测量数据计算的姿态角度的方差。
优选地，步骤S2具体还包括：
在进行卡尔曼滤波算法的过程中，对下一时刻的卡尔曼滤波的状态量X′(k+1)对应的协方差R(k+1)进行更新，计算公式如下：
R(k+1)＝R(k)-Kg(k)·H·R(k) (13)
优选地，步骤S2具体还包括：在进行卡尔曼滤波算法之前，将卡尔曼滤波的状态量X′(k)的初始值X′(0)设定为任意值，将卡尔曼滤波的状态量X′(k)对应的协方差R(k)的初始值R(0)设定为非零任意值。
优选地，步骤S3具体包括：
S31：根据陀螺仪测量数据计算的所述第一姿态角度和加速度计测量数据计算的所述第二姿态角度，求出所述第一姿态角度和所述第二姿态角度的权重，计算公式如下：
其中：B_gyro为陀螺仪测量数据计算出的所述第一姿态角度在两种传感器姿态角度中的权重，Var(angle_acc)为加速度计测量数据计算的姿态角度的方差，Var(angle_gyro)为陀螺仪测量数据计算的姿态角度的方差；
S32：计算根据卡尔曼滤波算法计算得到的所述第三姿态角度在互补卡尔曼滤波算法中的权重，计算公式如下：
其中：A_Kalman为根据卡尔曼滤波算法计算得到的所述第三姿态角度在互补卡尔曼滤波算法中的权重，Var(angle_Kalman)为根据卡尔曼滤波算法计算的姿态角度的方差；
S33：计算互补卡尔曼滤波算法融合三个姿态角度而计算的融合姿态角度，计算公式如下：
其中：angle_ComKalman(k)为根据互补卡尔曼滤波算法计算的融合姿态角度。
与现有技术相比，本发明的有益效果在于：本发明将卡尔曼滤波算法与互补滤波算法进行融合，提出一种基于互补卡尔曼滤波算法计算融合姿态角度的方法，充分利用了陀螺仪测量数据计算的姿态角度、加速度计测量数据计算的姿态角度及卡尔曼滤波算法融合两种传感器测量数据得到的姿态角度，使三种不同的姿态角度数据相互补偿，使得最终得到的融合姿态角度的测量精度大大提高。
在进一步的方案中，本发明提出了采用计算姿态角度方差的方法来计算卡尔曼滤波算法中涉及到的参数——测量噪声协方差和系统过程噪声协方差，解决现有姿态角度融合算法中涉及的参数计算方法不详问题。在互补滤波算法中，本发明根据各个姿态角度对应的方差大小赋予相应的权重，使得三种姿态角度数据相互补偿，从而使得在静态条件下和动态条件下相比现有的卡尔曼滤波算法计算的姿态角度的精度大大提高，其中在动态条件下姿态角度的测量精度比现有的卡尔曼滤波算法的测量精度提高了13.3％。
附图说明
图1是本发明优选实施例的基于互补卡尔曼滤波算法计算融合姿态角度的流程图；
图2是本发明优选实施例的基于互补卡尔曼滤波算法计算融合姿态角度的实验系统原理图；
图3是在静态条件下，本发明优选实施例的互补卡尔曼滤波算法与卡尔曼滤波算法的对比图；
图4是在动态条件下，本发明优选实施例的互补卡尔曼滤波算法与卡尔曼滤波算法的对比图。
具体实施方式
下面对照附图并结合优选的实施方式对本发明作进一步说明。
卡尔曼滤波是以最小均方误差作为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻的估计值和现在时刻的测量值来更新对状态变量的估计，求出现在时刻的估计值，算法根据建立的系统状态方程和测量方程对需要处理的信号做出满足最小均方误差的估计。卡尔曼滤波对于状态空间模型的状态矢量估计是一种强有力的手段，在理论上具有重要价值。互补滤波可以根据设计的权重不同，而有效的利用各个数据。本发明巧妙地将卡尔曼滤波算法和互补滤波算法进行结合，提出一种基于互补卡尔曼滤波算法计算融合姿态角度的方法，对陀螺仪测量数据计算的姿态角度、加速度计测量数据计算的姿态角度和卡尔曼滤波算法融合两种传感器测量数据得到的姿态角度进行融合，本文将这种算法称为互补卡尔曼滤波算法。
在一种实施例中，本发明的基于互补卡尔曼滤波算法计算融合姿态角度的方法，包括以下步骤：首先，搭建姿态角度测量系统，根据陀螺仪测量数据计算得到第一姿态角度，根据加速度计测量数据计算得到第二姿态角度；其次，根据卡尔曼滤波算法将所述第一姿态角度和所述第二姿态角度进行融合得到第三姿态角度；最后，根据互补滤波原理，将所述第一姿态角度、所述第二姿态角度以及所述第三姿态角度进行融合计算得到融合姿态角度。在进一步的实施例中，对卡尔曼滤波算法中参数——过程噪声协方差和测量噪声协方差的计算方法，提出采用计算姿态角度方差的方法进行描述；另外，计算所述融合姿态角度时，根据所述第一姿态角度、所述第二姿态角度以及所述第三姿态角度的方差计算相应的权重，其中方差越大，相应的权重越小。
结合图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时刻的上述两者的误差值；
S24：采用姿态角度方差的方法计算系统的过程噪声P的协方差Q和测量噪声M的协方差N，计算公式如下：
其中：Q_acc为加速度计的过程噪声协方差，Q_gyro为陀螺仪的过程噪声协方差，Var(angle_acc)为加速度计测量数据计算的姿态角度的方差，Var(angle_gyro)为陀螺仪测量数据计算的姿态角度的方差；
S25：计算卡尔曼滤波的状态量X′(k)对应的协方差R(k)，计算公式如下：
R(k)＝S·R(k-1)·ST+Q (9)
式(9)中S为控制矩阵，ST为控制矩阵S的转置，Q为过程噪声P的协方差；
S26：计算卡尔曼滤波增益Kg，计算公式如下：
式(10)中H为测量矩阵，HT为测量矩阵H的转置，N为测量噪声M的协方差；
S27：计算修正后的k时刻的卡尔曼滤波的状态量X′(k)，计算方式如下：
X′(k)＝X′(k-1)+Kg(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)Kg(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为陀螺仪测量数据计算出的所述第一姿态角度在两种传感器姿态角度中的权重，Var(angle_acc)为加速度计测量数据计算的姿态角度的方差，Var(angle_gyro)为陀螺仪测量数据计算的姿态角度的方差；
S32：计算根据卡尔曼滤波算法计算得到的所述第三姿态角度在互补卡尔曼滤波算法中的权重，计算公式如下：
其中：A_Kalman为根据卡尔曼滤波算法计算得到的所述第三姿态角度在互补卡尔曼滤波算法中的权重，Var(angle_Kalman)为根据卡尔曼滤波算法计算的姿态角度的方差；
S33：计算互补卡尔曼滤波算法融合三个姿态角度而计算的融合姿态角度，计算公式如下：
其中：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 (6)
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)·ST+Q (9)
式(9)中S为控制矩阵，ST为控制矩阵S的转置，Q为过程噪声P的协方差；
S26：计算卡尔曼滤波增益Kg，计算公式如下：
式(10)中H为测量矩阵，HT为测量矩阵H的转置，N为测量噪声M的协方差；
S27：计算修正后的k时刻的卡尔曼滤波的状态量X′(k)，计算方式如下：
X(k)＝X(k-1)+Kg(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)为根据互补卡尔曼滤波算法计算的融合姿态角度。
2.根据权利要求1所述的方法，其特征在于，步骤S2中根据卡尔曼滤波算法将所述第一姿态角度和所述第二姿态角度进行融合得到第三姿态角度还包括：采用姿态角度方差的方法对卡尔曼滤波算法中的过程噪声协方差和测量噪声协方差进行计算。
3.根据权利要求1或2所述的方法，其特征在于，步骤S3中，计算所述融合姿态角度时，根据所述第一姿态角度、所述第二姿态角度以及所述第三姿态角度的方差计算相应的权重，其中方差越大，相应的权重越小。
4.根据权利要求1所述的方法，其特征在于，步骤S2具体还包括：
S24：采用姿态角度方差的方法计算系统的过程噪声P的协方差Q和测量噪声M的协方差N，计算公式如下：
其中：Q_acc为加速度计的过程噪声协方差，Q_gyro为陀螺仪的过程噪声协方差，Var(angle_acc)为加速度计测量数据计算的姿态角度的方差，Var(angle_gyro)为陀螺仪测量数据计算的姿态角度的方差。
5.根据权利要求1所述的方法，其特征在于，步骤S2具体还包括：
在进行卡尔曼滤波算法的过程中，对下一时刻的卡尔曼滤波的状态量X′(k+1)对应的协方差R(k+1)进行更新，计算公式如下：
R(k+1)＝R(k)-Kg(k)·H·R(k) (13)
6.根据权利要求1、4、5中任一项所述的方法，其特征在于，步骤S2具体还包括：在进行卡尔曼滤波算法之前，将卡尔曼滤波的状态量X′(k)的初始值X′(0)设定为任意值，将卡尔曼滤波的状态量X′(k)对应的协方差R(k)的初始值R(0)设定为非零任意值。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610207713.4A CN105651242B (zh) | 2016-04-05 | 2016-04-05 | 一种基于互补卡尔曼滤波算法计算融合姿态角度的方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610207713.4A CN105651242B (zh) | 2016-04-05 | 2016-04-05 | 一种基于互补卡尔曼滤波算法计算融合姿态角度的方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105651242A CN105651242A (zh) | 2016-06-08 |
| CN105651242B true CN105651242B (zh) | 2018-08-24 |
Family
ID=56497098
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610207713.4A Expired - Fee Related CN105651242B (zh) | 2016-04-05 | 2016-04-05 | 一种基于互补卡尔曼滤波算法计算融合姿态角度的方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105651242B (zh) |
Families Citing this family (23)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106092141B (zh) * | 2016-07-19 | 2019-03-01 | 纳恩博（常州）科技有限公司 | 一种改善相对位置传感器性能的方法及装置 |
| CN106546261B (zh) * | 2016-09-20 | 2019-08-23 | 捷开通讯(深圳)有限公司 | 一种基于虚拟现实设备的角度数据补偿方法及装置 |
| CN106197376B (zh) * | 2016-09-23 | 2018-08-07 | 华南农业大学 | 基于单轴mems惯性传感器的车身倾角测量方法 |
| CN106482734A (zh) * | 2016-09-28 | 2017-03-08 | 湖南优象科技有限公司 | 一种用于imu多传感器数据融合的滤波方法 |
| CN108255201A (zh) * | 2016-12-29 | 2018-07-06 | 昊翔电能运动科技(昆山)有限公司 | 无人机云台姿态调整方法及其系统 |
| CN107607113B (zh) * | 2017-08-02 | 2020-03-17 | 华南农业大学 | 一种两轴姿态倾角测量方法 |
| CN108170154A (zh) * | 2017-12-19 | 2018-06-15 | 广东省航空航天装备技术研究所 | 一种无人机多传感器正向摄影倾斜飞控调试方法 |
| CN108398128B (zh) * | 2018-01-22 | 2021-08-24 | 北京大学深圳研究生院 | 一种姿态角的融合解算方法和装置 |
| CN108362493B (zh) * | 2018-01-23 | 2019-10-11 | 大连理工大学 | 一种数控机床直线轴转角误差快速检测方法 |
| CN108366201B (zh) * | 2018-02-12 | 2020-11-06 | 天津天地伟业信息系统集成有限公司 | 一种基于陀螺仪的电子防抖方法 |
| CN108830666A (zh) * | 2018-03-26 | 2018-11-16 | 杭州电子科技大学 | 一种服装试穿管理装置 |
| CN108615061B (zh) * | 2018-03-26 | 2024-04-09 | 杭州电子科技大学 | 一种服装试穿配对管理装置 |
| CN110345943A (zh) * | 2018-04-02 | 2019-10-18 | 哈尔滨工业大学（威海） | 船舶姿态监测预报系统及其预报方法 |
| CN108663045B (zh) * | 2018-04-28 | 2024-05-07 | 山东交通学院 | 一种骑行载具姿态识别报警方法和姿态监测报警装置 |
| CN109000612B (zh) * | 2018-06-19 | 2020-11-27 | 深圳市道通智能航空技术有限公司 | 设备的角度估算方法、装置、摄像组件及飞行器 |
| CN109990776B (zh) * | 2019-04-12 | 2021-09-24 | 武汉深海蓝科技有限公司 | 一种姿态测量方法及装置 |
| CN110207647B (zh) * | 2019-05-08 | 2021-11-09 | 诺百爱（杭州）科技有限责任公司 | 一种基于互补卡尔曼滤波器的臂环姿态角计算方法 |
| CN110440805B (zh) * | 2019-08-09 | 2021-09-21 | 深圳市道通智能航空技术股份有限公司 | 一种偏航角的融合方法、装置及飞行器 |
| CN110542417B (zh) * | 2019-09-05 | 2022-12-13 | 武汉理工大学 | 基于静态和和动态倾角仪校正的陀螺仪线形测量方法与系统 |
| CN112033345B (zh) * | 2020-11-04 | 2021-02-02 | 湖南联智科技股份有限公司 | 一种基于北斗的变形监测系统及方法 |
| CN113188505B (zh) * | 2021-04-14 | 2022-09-27 | 湖南三一智能控制设备有限公司 | 姿态角度的测量方法、装置、车辆及智能臂架 |
| CN113674412B (zh) * | 2021-08-12 | 2023-08-29 | 浙江工商大学 | 基于位姿融合优化的室内地图构建方法、系统及存储介质 |
| CN114279446B (zh) * | 2021-12-22 | 2023-11-03 | 广东汇天航空航天科技有限公司 | 飞行汽车航姿测量方法、装置及飞行汽车 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102750020A (zh) * | 2012-07-18 | 2012-10-24 | 深圳数字电视国家工程实验室股份有限公司 | 获取空中鼠标位移的方法、空中鼠标及空中鼠标控制系统 |
| CN103697855A (zh) * | 2014-01-07 | 2014-04-02 | 中国人民解放军国防科学技术大学 | 一种基于海天线检测的船体水平姿态测量方法 |
| CN104006787A (zh) * | 2014-05-01 | 2014-08-27 | 哈尔滨工业大学 | 空间飞行器姿态运动模拟平台高精度姿态确定方法 |
| CN104197927A (zh) * | 2014-08-20 | 2014-12-10 | 江苏科技大学 | 水下结构检测机器人实时导航系统及方法 |
| CN104546391A (zh) * | 2015-01-31 | 2015-04-29 | 中山大学 | 一种用于盲杖的陀螺稳定装置及其互补滤波方法 |
| CN105043348A (zh) * | 2015-07-11 | 2015-11-11 | 哈尔滨工业大学 | 基于卡尔曼滤波的加速度计陀螺仪水平角度测量方法 |
| CN105136145A (zh) * | 2015-08-11 | 2015-12-09 | 哈尔滨工业大学 | 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法 |
-
2016
- 2016-04-05 CN CN201610207713.4A patent/CN105651242B/zh not_active Expired - Fee Related
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102750020A (zh) * | 2012-07-18 | 2012-10-24 | 深圳数字电视国家工程实验室股份有限公司 | 获取空中鼠标位移的方法、空中鼠标及空中鼠标控制系统 |
| CN103697855A (zh) * | 2014-01-07 | 2014-04-02 | 中国人民解放军国防科学技术大学 | 一种基于海天线检测的船体水平姿态测量方法 |
| CN104006787A (zh) * | 2014-05-01 | 2014-08-27 | 哈尔滨工业大学 | 空间飞行器姿态运动模拟平台高精度姿态确定方法 |
| CN104197927A (zh) * | 2014-08-20 | 2014-12-10 | 江苏科技大学 | 水下结构检测机器人实时导航系统及方法 |
| CN104546391A (zh) * | 2015-01-31 | 2015-04-29 | 中山大学 | 一种用于盲杖的陀螺稳定装置及其互补滤波方法 |
| CN105043348A (zh) * | 2015-07-11 | 2015-11-11 | 哈尔滨工业大学 | 基于卡尔曼滤波的加速度计陀螺仪水平角度测量方法 |
| CN105136145A (zh) * | 2015-08-11 | 2015-12-09 | 哈尔滨工业大学 | 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法 |
Non-Patent Citations (2)
| Title |
|---|
| Research on Data Fusion Technology of Body Posture Detection Based on Kalman Filter;Xianwei Wang et,al.;《Applied Mechanics and Materials》;20141008;第668-669卷;第1003-1006页 * |
| 多传感器目标跟踪数据融合关键技术研究;崔波;《中国优秀博士学位论文全文数据库信息科技辑》;20131015(第10期);第2.4.1节，第7.1-7.4节 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN105651242A (zh) | 2016-06-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105651242B (zh) | 一种基于互补卡尔曼滤波算法计算融合姿态角度的方法 | |
| Gui et al. | MEMS based IMU for tilting measurement: Comparison of complementary and kalman filter based data fusion | |
| RU2285902C1 (ru) | Способ определения и компенсации ухода гиростабилизированной платформы и устройство для его осуществления | |
| Gao et al. | Adaptive Kalman filtering with recursive noise estimator for integrated SINS/DVL systems | |
| US10627237B2 (en) | Offset correction apparatus for gyro sensor, recording medium storing offset correction program, and pedestrian dead-reckoning apparatus | |
| CN109508025B (zh) | 一种弹性飞行器的自抗扰姿态控制方法 | |
| US20160363460A1 (en) | Orientation model for inertial devices | |
| CN103324087B (zh) | 基于神经网络的微陀螺仪的自适应反演控制系统及方法 | |
| US9863784B2 (en) | Orientation estimation utilizing a plurality of adaptive filters | |
| CN103033186A (zh) | 一种用于水下滑翔器的高精度组合导航定位方法 | |
| US9222237B1 (en) | Earthmoving machine comprising weighted state estimator | |
| EP2889577A1 (en) | Signal processing device, detection device, sensor, electronic apparatus and moving object | |
| CN104503246B (zh) | 微陀螺仪系统的间接自适应神经网络滑模控制方法 | |
| CN106546261B (zh) | 一种基于虚拟现实设备的角度数据补偿方法及装置 | |
| CN110377034A (zh) | 一种基于蜻蜓算法优化的水面船轨迹跟踪全局鲁棒滑模控制方法 | |
| CN111240347A (zh) | 一种自抗扰控制的无人机航向角误差补偿方法、系统 | |
| US8884711B2 (en) | MEMS device oscillator loop with amplitude control | |
| US20180348277A1 (en) | Circuit Apparatus, Physical Quantity Measuring Apparatus, Electronic Device, And Vehicle | |
| US20180283912A1 (en) | Signal processing device, detection device, physical quantity measurement device, electronic apparatus, and vehicle | |
| CN102880049A (zh) | 一种基于帆板挠性形变测量的自适应振动控制方法 | |
| CN109917645B (zh) | 微陀螺双反馈模糊神经网络超扭曲滑模控制系统设计方法 | |
| Zhang et al. | Adaptive mismatch compensation for vibratory gyroscopes | |
| CN103472725B (zh) | 一种基于名义控制器的神经网络全调节的控制方法 | |
| CN113609581B (zh) | 运载火箭弹性频率在线辨识的方法及存储介质 | |
| CN111025915B (zh) | 一种基于干扰观测器的z轴陀螺仪神经网络滑模控制方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CP03 | Change of name, title or address |
Address after: 518000 Guangdong city in Shenzhen Province, Nanshan District City Xili street Shenzhen University Tsinghua Campus A building two floor Patentee after: Tsinghua Shenzhen International Graduate School Address before: 518055 Guangdong city of Shenzhen province Nanshan District Xili of Tsinghua Patentee before: GRADUATE SCHOOL AT SHENZHEN, TSINGHUA University |
|
| CP03 | Change of name, title or address | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20200615 Address after: Room 2101-2108, 21 / F, Kerong Chuangye building, No. 666, Zhongkai Avenue (Huihuan section), Zhongkai high tech Zone, Huizhou City, Guangdong Province Patentee after: Huizhou Frant Photoelectric Technology Co.,Ltd. Address before: 518000 Guangdong city in Shenzhen Province, Nanshan District City Xili street Shenzhen University Tsinghua Campus A building two floor Patentee before: Tsinghua Shenzhen International Graduate School |
|
| TR01 | Transfer of patent right | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20180824 |
|
| CF01 | Termination of patent right due to non-payment of annual fee |