CN105136145A

CN105136145A - 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法

Info

Publication number: CN105136145A
Application number: CN201510489795.1A
Authority: CN
Inventors: 陈兴林; 罗文嘉; 李松; 高怡然; 徐川川; 于志亮; 崔宁; 杨昱昊
Original assignee: Harbin Institute of Technology
Current assignee: Harbin Institute of Technology
Priority date: 2015-08-11
Filing date: 2015-08-11
Publication date: 2015-12-09

Abstract

一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法，涉及组合导航中多传感器数据融合领域，尤其涉及一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的改进方法。本发明是要解决现有的卡尔曼滤波在四旋翼无人机姿态运算过程中运算量太大而无法实时获取姿态数据的问题。本发明方法通过对卡尔曼滤波方程组进行改进，即对当前状态预测方程、当前状态误差协方差预测方程、当前最优姿态角方程、卡尔曼增益K_g(k)方程和当前最优姿态角方程的协方差方程进行改进，实现部分数据离线计算，大大减少飞控板处理器的计算量，从而满足四旋翼无人机姿态数据实时性要求和数据精度要求。本发明可应用于组合导航中多传感器数据融合技术领域。

Description

一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法

技术领域

本发明涉及组合导航中多传感器数据融合领域，尤其涉及一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法。

背景技术

卡尔曼滤波是一个最优化自回归数据处理算法。近年来被广泛应用于计算机图像处理方面。在四旋翼无人机飞行***中，需要对陀螺仪、加速度传感器和磁力计传感器所测得的数据进行综合和校正。如果使用简单的均值滤波，很难满足精度和实时性上的要求。故通过卡尔曼滤波将加速度传感器、磁力传感器和陀螺仪的数据进行融合，在计算四旋翼无人机飞行器的实时姿态时很好地抑制了噪声的干扰作用，以提高测量精度，为自主飞行控制创造了条件。

但是卡尔曼滤波在四旋翼无人机姿态运算过程中也存在一个明显的缺点，其实也是基于数字平台的卡尔曼滤波算法都存在的问题，算法中方程组中有大量的矩阵乘法，有的还有矩阵连乘。这会导致出现位宽很宽的中间结果，从而难以进行迭代运算。在数字实现中面临的最大困难即是矩阵求逆运算，当矩阵的阶数增加时，其求逆运算的复杂度更是以几何级数增加。卡尔曼滤波算法是通过迭代运算实现的，这就要求在每次迭代运算中，卡尔曼滤波算法方程组都必须重新运算一次，这将会使CPU承受复杂而又极其繁重运算。根据上述分析，可以看出应用卡尔曼滤波算法进行姿态数据融合，运算量非常大，对飞控板的处理器芯片要求非常高，所以对于应用在四旋翼无人机姿态数据融合的卡尔曼滤波算法进行简化就显得非常必要。

发明内容

本发明为解决现有的卡尔曼滤波在四旋翼无人机姿态运算过程中运算量太大而无法实时获取姿态数据的问题，而提出一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法。

基于卡尔曼滤波的四旋翼无人机姿态数据融合方法的理论基础：

在四旋翼无人机姿态解算中，陀螺仪能提供瞬间的动态角度变化，保证载体具有动态加速度时的稳定性，但受其本身的固有特性、温度及积分过程的影响，会随着工作时间的延长产生累积误差；加速度传感器和磁力传感器能准确地提供静态角度，但是运动时容易受到噪声干扰。所以在四旋翼无人机姿态解算过程中，应当综合陀螺仪、加速度传感器和磁力传感器的数据来解算四旋翼无人机的姿态。通过卡尔曼滤波算法可以很好地融合三个传感器的数据。

本申请所述的基于卡尔曼滤波的四旋翼无人机姿态数据融合方法的基本思路是，尽量减少矩阵乘法运算；避免矩阵的求逆运算；简化迭代过程。

一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法，按以下步骤进行：

一、根据四旋翼无人机姿态测量传感器的特性，选择以四旋翼无人机的实际姿态角θ和陀螺仪的测量误差σ为状态向量，以加速度传感器和磁力传感器测得的姿态角θ_测为观测值，得到四旋翼无人机姿态解算***相应的状态方程和观测方程式，如公式(1)所示，

其中为四旋翼无人机的实际姿态角θ的微分，为陀螺仪的测量误差σ的微分，ω为陀螺仪输出的角速度，ω_白为陀螺仪的测量白噪声，v_白为加速度传感器和磁力传感器的测量白噪声，ω_白与v_白相互独立；

二、将解算***的连续信号离散化为计算机能处理的离散化信号，令解算***的采样周期为Ts，得到离散化的状态方程X(k)和测量方程V(k)，如公式(2)所示，

其中ω(k-1)为k-1时刻陀螺仪输出的角速度；以加速度传感器和磁力传感器在k时刻测得的滚动角、俯仰角、偏航角为测量姿态角，v_白(k)为此测量姿态角的白噪声偏差，此白噪声偏差对应的协方差为测量噪声协方差R；

三、在公式(2)的基础上构建卡尔曼滤波方程组(3)～(7)，其中当前状态预测方程，如公式(3)所示，

X(k|k-1)＝AX(k-1|k-1)+Bω(k-1)，(3)

其中A和B为解算***参数，

A = [\begin{matrix} 1 & - T s \\ 0 & 1 \end{matrix}], B = [\begin{matrix} T s \\ 0 \end{matrix}],

X(k-1|k-1)是k-1时刻最优的姿态角，ω(k-1)为k-1时刻的控制量，即陀螺仪输出的角速度，X(k|k-1)是根据X(k-1|k-1)预测到的无人机当前姿态角；

当前状态误差协方差预测方程如公式(4)所示，

P(k|k-1)＝AP(k-1|k-1)A^T+Q，(4)

其中P(k-1|k-1)是X(k-1|k-1)对应的协方差，P(k|k-1)是X(k|k-1)对应的协方差，以陀螺仪计算得到k时刻的姿态角偏差和k-1时刻的最优姿态角之和为当前预测姿态角，此当前预测姿态角的噪声偏差为G(k)，此噪声偏差对应的协方差为过程噪声协方差Q；

将k时刻姿态角预测值与测量值以卡尔曼增益K_g(k)为比例进行融合，就可得到k时刻的最优姿态角X(k|k)，如公式(5)所示，

X(k|k)＝X(k|k-1)+K_g(k)(V(k)-HX(k|k-1))，(5)

其中H为测量***参数，H＝[10]，K_g(k)为卡尔曼增益，V(k)是加速度传感器和磁力传感器在k时刻获得姿态角的测量值，X(k|k-1)是k时刻姿态角的预测值；

公式(5)中K_g(k)，即卡尔曼增益由下式求得，如公式(6)所示，

K_{g} (k) = \frac{P (k | k - 1) H^{T}}{(H P (k | k - 1) H^{T} + R)}, - - - (6)

其中P(k|k-1)为X(k|k-1)对应的协方差，R的物理意义见步骤二中的说明；

k时刻的最优姿态角X(k|k)的协方差P(k|k)如公式(7)所示，

P(k|k)＝(I-K_g(k)H)P(k|k-1)，(7)

其中，I为二阶单位矩阵，P(k|k-1)是X(k|k-1)对应的协方差；

四、对卡尔曼滤波方程组(3)～(7)进行数学处理，得到改进后的卡尔曼滤波方程组(8)～(12)，具体过程如下：

合并公式(4)和(7)可得公式(8)：

P(k|k-1)＝A((I-K_g(k-1)H)P(k-1|k-2))A^T+Q，(8)

根据公式(6)可得公式(9)：

K_{g} (k - 1) = \frac{P (k - 1 | k - 2) H^{T}}{(H P (k - 1 | k - 2) H^{T} + R)}, - - - (9)

将公式(5)进行变形，可得公式(10)：

X(k|k)＝(I-K_g(k)H)X(k|k-1)+K_g(k)V(k)，(10)

根据公式(3)可得公式(11)：

X(k|k)＝(I-K_g(k)H)(AX(k-1|k-1)+Bω(k))+K_g(k)V(k)

＝(I-K_g(k)H)AX(k-1|k-1)+(I-K_g(k)H)Bω(k)+K_g(k)V(k)，(11)

将公式(11)中的(I-K_g(k)H)A和(I-K_g(k)H)B，分别替换为为E_A(k)和E_B(k)，代入公式(11)得公式(12)：

X(k|k)＝E_A(k)X(k-1|k-1)+E_B(k)ω(K)+K_g(k)Z(k)，(12)

五、对公式(12)中的E_A(k)、E_B(k)和K_g(k)先求值，每次迭代过程中用到E_A(k)、E_B(k)和K_g(k)时，直接在飞控板处理器芯片内调用，不断重复计算公式(8)～(10)和(12)，直至找到各个时刻的最优姿态角。

本发明包括以下有益效果：

1、减少运算时间，从而满足实时性要求：采用标准的基于卡尔曼滤波的四旋翼无人机姿态数据融合算法，由于运算量大，导致处理时间太长，在工程实际中根本无法实现实时获取四旋翼姿态数据，而采用本申请改进算法，部分数据离线计算，大大减少飞控板处理器的计算量，以常用的飞控板处理器STM32F103ZE为例，完成整个四旋翼无人机姿态数据的获取要小于1毫秒，满足实时性要求；

2、减小误差，以满足数据精度要求：采用标准的基于卡尔曼滤波的四旋翼无人机姿态数据融合算法，在卡尔曼方程迭代运算中受处理器芯片位宽的限制必须不断地截取数据，从而造成数据精度的损失，导致最后姿态精度的误差大于15％，无法达到数据精度的要求，而采用改进算法，E_A(k)、E_B(k)和K_g(k)的离线计算不受飞控板处理器芯片位宽的限制，最后姿态精度的误差为2％～5％，满足数据精度要求。

附图说明

图1为基于卡尔曼滤波的四旋翼无人机姿态数据融合算法框图，

图中加速度计即为加速度传感器，磁力计即为磁力传感器。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合图1和具体实施方式对本发明作进一步详细的说明。

具体实施方式一、本实施方式所述一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法，按以下步骤进行：

X(k|k-1)＝AX(k-1|k-1)+Bω(k-1)，(3)

其中A和B为解算***参数，

A = [\begin{matrix} 1 & - T s \\ 0 & 1 \end{matrix}], B = [\begin{matrix} T s \\ 0 \end{matrix}],

当前状态误差协方差预测方程如公式(4)所示，

P(k|k-1)＝AP(k-1|k-1)A^T+Q，(4)

X(k|k)＝X(k|k-1)+K_g(k)(V(k)-HX(k|k-1))，(5)

公式(5)中K_g(k)，即卡尔曼增益由下式求得，如公式(6)所示，

K_{g} (k) = \frac{P (k | k - 1) H^{T}}{(H P (k | k - 1) H^{T} + R)}, - - - (6)

k时刻的最优姿态角X(k|k)的协方差P(k|k)如公式(7)所示，

P(k|k)＝(I-K_g(k)H)P(k|k-1)，(7)

其中，I为二阶单位矩阵，P(k|k-1)是X(k|k-1)对应的协方差；

公式(3)～公式(7)构成卡尔曼滤波算法方程组，其中公式(5)～(7)是卡尔曼滤波器的状态更新方程组，是当次计算中的后验估计，并作为下一次递推运算的先验估计；为实现多传感器信息融合卡尔曼方程的自回归更新，需不断更新误差估计Q和R，将公式(3)～(7)周而复始、循环反复地运算，直至找到各个时刻的最优姿态角；

四、观察卡尔曼滤波算法方程组，其中包括两个迭代过程，第一个迭代过程由公式(5)表示，目的是求出每次迭代的估计的值；另一个迭代过程由公式(4)、(6)和(7)构成，目的是在每次迭代过程中，为滤波估计方程求出对应的卡尔曼增益K_g(k)，合并公式(4)和(7)可得公式(8)：

P(k|k-1)＝A((I-K_g(k-1)H)P(k-1|k-2))A^T+Q，(8)

公式(4)和(7)的合并，完成的是P(k|k-1)的迭代；

根据公式(6)可得：

K_{g} (k - 1) = \frac{P (k - 1 | k - 2) H^{T}}{(H P (k - 1 | k - 2) H^{T} + R)}, - - - (9)

由公式(6)和(9)比较，可知公式(8)实际上完成的是卡尔曼增益的迭代；

P(k|k)和P(k|k-1)只是卡尔曼增益迭代的中间变量，而矩阵A和H为已知矩阵，利用A和H可以离线进行卡尔曼增益的迭代过程，求出每次迭代运算的卡尔曼增益K_g(k)，以供滤波估计方程使用；可以这样处理的原因是这三个方程中除H与采样频率有关，其余均为常数，而在一个实际的四旋翼姿态解算***中，其采样频率是确定的，因此可以将H看作是常数矩阵，这样，卡尔曼滤波算法就可以缩减为一个方程，即公式(5)，将公式(5)进行变形，可得：

X(k|k)＝(I-K_g(k)H)X(k|k-1)+K_g(k)V(k)，(10)

根据公式(3)可得：

X(k|k)＝(I-K_g(k)H)(AX(k-1|k-1)+Bω(k))+K_g(k)V(k)

＝(I-K_g(k)H)AX(k-1|k-1)+(I-K_g(k)H)Bω(k)+K_g(k)V(k)，(11)

其中ω(k)为k时刻陀螺仪输出的角速度；

由于迭代过程中可以先求得K_g(k)，所以在每次迭代过程中，可以将其视为常量，因此，公式(11)中的(I-K_g(k)H)A和(I-K_g(k)H)B也可以先求得，将其分别设为E_A(k)和E_B(k)，代入式公(11)得：

X(k|k)＝E_A(k)X(k-1|k-1)+E_B(k)ω(K)+K_g(k)Z(k)，(12)

五、对公式(12)中的E_A(k)、E_B(k)和K_g(k)先求值，每次迭代过程中用到E_A(k)、E_B(k)和K_g(k)时，直接在飞控板处理器芯片中调用，不断重复计算公式(8)～(10)和(12)过程，直至找到各个时刻的最优姿态角，从而使得飞控板处理器芯片计算量得到大大简化。

本发明包括以下有益效果：

2、减小误差，以满足数据精度要求：采用标准的基于卡尔曼滤波的四旋翼无人机姿态数据融合算法，在卡尔曼方程迭代运算中受处理器芯片位宽的限制必须不断地截取数据，从而造成数据精度的损失，导致最后姿态精度的误差大于15％，无法达到数据精度的要求，而采用改进算法，E_A(k)、E_B(k)和K_g(k)的离线计算不受飞控板处理器芯片位宽的限制，数据精度的误差为2％～5％，满足数据精度要求。

具体实施方式二、本实施方式是对具体实施方式一所述的一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法的进一步说明，步骤一中所述实际姿态角θ为四旋翼无人机实际滚动角、俯仰角和偏航角构成的向量角。

实际姿态角θ无法通过测量仪器直接测得，只能通过实际测量无人机的滚动角、俯仰角和偏航角，并借助向量的形式来表示实际姿态角θ。

具体实施方式三、本实施方式是对具体实施方式一所述的一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法的进一步说明，步骤十三中所述E_A(k)、E_B(k)和K_g(k)的求值采用离线方式，并将求值结果储存在飞控板处理器芯片中。

E_A(k)、E_B(k)和K_g(k)的求值采用离线方式，大大地减少飞控板处理器的计算量。

Claims

1.一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法，其特征在于它是通过以下步骤实现的：

X(k|k-1)＝AX(k-1|k-1)+Bω(k-1)，(3)

其中A和B为解算***参数，

A = [\begin{matrix} 1 & - T s \\ 0 & 1 \end{matrix}], B = [\begin{matrix} T s \\ 0 \end{matrix}],

X(k^-1|k^-1)是k-1时刻最优的姿态角，ω(k-1)为k-1时刻的控制量，即陀螺仪输出的角速度，X(k|k-1)是根据X(k-1|k-1)预测到的无人机当前姿态角；

当前状态误差协方差预测方程如公式(4)所示，

P(k|k-1)＝AP(k-1|k-1)A^T+Q，(4)

X(k|k)＝X(k|k-1)+K_g(k)(V(k)-HX(k|k-1))，(5)

公式(5)中K_g(k)，即卡尔曼增益由下式求得，如公式(6)所示，

K_{g} (k) = \frac{P (k | k - 1) H^{T}}{(H P (k | k - 1) H^{T} + R)}, - - - (6)

k时刻的最优姿态角X(k|k)的协方差P(k|k)如公式(7)所示，

P(k|k)＝(I-K_g(k)H)P(k|k-1)，(7)

其中，I为二阶单位矩阵，P(k|k-1)是X(k|k-1)对应的协方差；

合并公式(4)和(7)可得公式(8)：

P(k|k-1)＝A((I-K_g(k-1)H)P(k-1|k-2))A^T+Q，(8)

根据公式(6)可得公式(9)：

K_{g} (k - 1) = \frac{P (k - 1 | k - 2) H^{T}}{(H P (k - 1 | k - 2) H^{T} + R)}, - - - (9)

将公式(5)进行变形，可得公式(10)：

X(k|k)＝(I-K_g(k)H)X(k|k-1)+K_g(k)V(k)，(10)

根据公式(3)可得公式(11)：

X(k|k)＝(I-K_g(k)H)(AX(k-1|k-1)+Bω(k))+K_g(k)V(k)

，(11)

＝(I-K_g(k)H)AX(k-1|k-1)+(I-K_g(k)H)Bω(k)+K_g(k)V(k)

X(k|k)＝E_A(k)X(k-1|k-1)+E_B(k)ω(K)+K_g(k)Z(k)，(12)

五、对公式(12)中的E_A(k)、E_B(k)和K_g(k)先求值，每次迭代过程中用到E_A(k)、E_B(k)和K_g(k)时，直接在飞控板处理器芯片中调用，不断重复计算公式(8)～(10)和(12)，直至找到各个时刻的最优姿态角。

2.如权利要求1所述的一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法，其特征在于步骤一中所述实际姿态角θ为四旋翼无人机实际滚动角、俯仰角和偏航角构成的向量角。

3.如权利要求1或2所述的一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法，其特征在于步骤十三中所述E_A(k)、E_B(k)和K_g(k)的求值采用离线方式，并将求值结果储存在飞控板处理器芯片中。