一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法，涉及组合导航中多传感器数据融合领域，尤其涉及一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的改进方法。本发明专利技术是要解决现有的卡尔曼滤波在四旋翼无人机姿态运算过程中运算量太大而无法实时获取姿态数据的问题。本发明专利技术方法通过对卡尔曼滤波方程组进行改进，即对当前状态预测方程、当前状态误差协方差预测方程、当前最优姿态角方程、卡尔曼增益Kg(k)方程和当前最优姿态角方程的协方差方程进行改进，实现部分数据离线计算，大大减少飞控板处理器的计算量，从而满足四旋翼无人机姿态数据实时性要求和数据精度要求。本发明专利技术可应用于组合导航中多传感器数据融合技术领域。
【技术实现步骤摘要】
本专利技术涉及组合导航中多传感器数据融合领域，尤其涉及一种基于卡尔曼滤波的 四旋翼无人机姿态数据融合的方法。
技术介绍
卡尔曼滤波是一个最优化自回归数据处理算法。近年来被广泛应用于计算机图像 处理方面。在四旋翼无人机飞行系统中，需要对陀螺仪、加速度传感器和磁力计传感器所测 得的数据进行综合和校正。如果使用简单的均值滤波，很难满足精度和实时性上的要求。故 通过卡尔曼滤波将加速度传感器、磁力传感器和陀螺仪的数据进行融合，在计算四旋翼无 人机飞行器的实时姿态时很好地抑制了噪声的干扰作用，以提高测量精度，为自主飞行控 制创造了条件。 但是卡尔曼滤波在四旋翼无人机姿态运算过程中也存在一个明显的缺点，其实也 是基于数字平台的卡尔曼滤波算法都存在的问题，算法中方程组中有大量的矩阵乘法，有 的还有矩阵连乘。这会导致出现位宽很宽的中间结果，从而难以进行迭代运算。在数字实 现中面临的最大困难即是矩阵求逆运算，当矩阵的阶数增加时，其求逆运算的复杂度更是 以几何级数增加。卡尔曼滤波算法是通过迭代运算实现的，这就要求在每次迭代运算中，卡 尔曼滤波算法方程组都必须重新运算一次，这将会使CPU承受复杂而又极其繁重运算。根 据上述分析，可以看出应用卡尔曼滤波算法进行姿态数据融合，运算量非常大，对飞控板的 处理器芯片要求非常高，所以对于应用在四旋翼无人机姿态数据融合的卡尔曼滤波算法进 行简化就显得非常必要。
技术实现思路
本专利技术为解决现有的卡尔曼滤波在四旋翼无人机姿态运算过程中运算量太大而 无法实时获取姿态数据的问题，而提出一种基于卡尔曼滤波的四旋翼无人机姿态数据融合 的方法。 基于卡尔曼滤波的四旋翼无人机姿态数据融合方法的理论基础： 在四旋翼无人机姿态解算中，陀螺仪能提供瞬间的动态角度变化，保证载体具有 动态加速度时的稳定性，但受其本身的固有特性、温度及积分过程的影响，会随着工作时间 的延长产生累积误差；加速度传感器和磁力传感器能准确地提供静态角度，但是运动时容 易受到噪声干扰。所以在四旋翼无人机姿态解算过程中，应当综合陀螺仪、加速度传感器和 磁力传感器的数据来解算四旋翼无人机的姿态。通过卡尔曼滤波算法可以很好地融合三个 传感器的数据。 本申请所述的基于卡尔曼滤波的四旋翼无人机姿态数据融合方法的基本思路是， 尽量减少矩阵乘法运算；避免矩阵的求逆运算；简化迭代过程。 -种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法，按以下步骤进行： -、根据四旋翼无人机姿态测量传感器的特性，选择以四旋翼无人机的实际姿态 角Θ和陀螺仪的测量误差σ为状态向量，以加速度传感器和磁力传感器测得的姿态角θ #为观测值，得到四旋翼无人机姿态解算系统相应的状态方程和观测方程式，如公式（1)所 示，Cl) 其中θ:为四旋翼无人机的实际姿态角Θ的微分，σ为陀螺仪的测量误差〇的微 分，ω为陀螺仪输出的角速度，COe为陀螺仪的测量白噪声，ve为加速度传感器和磁力传感 器的测量白噪声，《^与％相互独立； 二、将解算系统的连续信号离散化为计算机能处理的离散化信号，令解算系统的 采样周期为Ts，得到离散化的状态方程X (k)和测量方程V (k)，如公式（2)所示，(:2> 其中ω (k-Ι)为k-Ι时刻陀螺仪输出的角速度；以加速度传感器和磁力传感器在 k时刻测得的滚动角、俯仰角、偏航角为测量姿态角，(k)为此测量姿态角的白噪声偏差， 此白噪声偏差对应的协方差为测量噪声协方差R ; 三、在公式（2)的基础上构建卡尔曼滤波方程组（3)~（7)，其中当前状态预测方 程，如公式（3)所示， X (k I k_l) = AX (k_l I k_l)+B ω (k_l)， (3) 其中A和B为解算系统参数，是k_l时刻 最优的姿态角，ω (k-Ι)为k-Ι时刻的控制量，即陀螺仪输出的角速度，X(k|k-1)是根据 X (k-1 |k-l)预测到的无人机当前姿态角； 当前状态误差协方差预测方程如公式（4)所示， P (k I k-1) = AP(k-l|k-l)AT+Q, (4) 其中P (k_l I k_l)是X (k_l I k_l)对应的协方差，P (k I k_l)是X (k I k_l)对应的协方 差，以陀螺仪计算得到k时刻的姿态角偏差和k-Ι时刻的最优姿态角之和为当前预测姿态 角，此当前预测姿态角的噪声偏差为G (k)，此噪声偏差对应的协方差为过程噪声协方差Q ; 将k时刻姿态角预测值与测量值以卡尔曼增益Kg(k)为比例进行融合，就可得到k 时刻的最优姿态角X(k|k)，如公式（5)所示， X(k|k) = X(k|k-1)+Kg(k) (V(k)-HX(k|k-l))， (5) 其中H为测量系统参数，H = ，Kg(k)为卡尔曼增益，V(k)是加速度传感器和 磁力传感器在k时刻获得姿态角的测量值，X (k I k-i)是k时刻姿态角的预测值； 公式（5)中Kg(k)，即卡尔曼增益由下式求得，如公式（6)所示，(6) 其中P (k |k-l)为X (k |k-l)对应的协方差，R的物理意义见步骤二中的说明； k时刻的最优姿态角X (k I k)的协方差P (k I k)如公式（7)所示， P(k|k) = (I-KgGOH)P(kIk-1)， (7) 其中，I为二阶单位矩阵，P (k I k_l)是X (k I k_l)对应的协方差； 四、对卡尔曼滤波方程组（3)~（7)进行数学处理，得到改进后的卡尔曼滤波方程 组（8)~（12)，具体过程如下： 合并公式⑷和（7)可得公式（8): P(k|k-1) = A((I-Kg(k-l)H)P(k-l|k-2))AT+Q， (8) 根据公式（6)可得公式（9):(9) 将公式（5)进行变形，可得公式（10): X(k|k) = (I-Kg (k) H) X (k Ik-1)+Kg (k) V (k), (10) 根据公式⑶可得公式（11): X (k I k) = (I-Kg (k) H) (AX (k-11 k-1) +B ω (k)) +Kg (k) V (k) = (I-Kg (k) H) AX (k-11 k-1) + (I-Kg (k) H) B ω (k) +Kg (k) V (k)， (11) 将公式（11)中的（I-Kg(k)H) A和（I-Kg(k)H) Β，分别替换为为Ea (k)和Eb (k)，代入 公式（11)得公式（12): X (k I k) = EA(k) X (k-11 k-1)+EB(k) ω (K)+Kg(k) Z (k)， (12) 五、对公式（12)中的Ea(k)、Eb(k)和Kg(k)先求值，每次迭代过程中用到E a(k)、 EB(k)和1(1〇时，直接在飞控板处理器芯片内调用，不断重复计算公式（8)~（10)和（12)， 直至找到各个时刻的最优姿态角。 本专利技术包括以下有益效果： 1、减少运算时间，从而满足实时性要求：采用标准的基于卡尔曼滤波的四旋翼无 人机姿态数据融合算法，由于运算量大，导致处理时间太长，在工程实际中根本无法实现实 时获取四旋翼姿态数据，而采用本申请改进算法，部分数据离线计算，大大减少飞控板处理 器的计算量，以常用的飞控板处理器STM32F103本文档来自技高网...
【技术保护点】
一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法，其特征在于它是通过以下步骤实现的：一、根据四旋翼无人机姿态测量传感器的特性，选择以四旋翼无人机的实际姿态角θ和陀螺仪的测量误差σ为状态向量，以加速度传感器和磁力传感器测得的姿态角θ测为观测值，得到四旋翼无人机姿态解算系统相应的状态方程和观测方程式，如公式(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=1-Ts01,B=Ts0,]]>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)AT+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时刻姿态角预测值与测量值以卡尔曼增益Kg(k)为比例进行融合，就可得到k时刻的最优姿态角X(k|k)，如公式(5)所示，X(k|k)＝X(k|k‑1)+Kg(k)(V(k)‑HX(k|k‑1))， (5)其中H为测量系统参数，H＝[10]，Kg(k)为卡尔曼增益，V(k)是加速度传感器和磁力传感器在k时刻获得姿态角的测量值，X(k|k‑1)是k时刻姿态角的预测值；公式(5)中Kg(k)，即卡尔曼增益由下式求得，如公式(6)所示，Kg(k)=P(k|k-1)HT(HP(k|k-1)HT+R),---(6)]]>其中P(k|k‑1)为X(k|k‑1)对应的协方差，R的物理意义见步骤二中的说明；k时刻的最优姿态角X(k|k)的协方差P(k|k)如公式(7)所示，P(k|k)＝(I‑Kg(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‑Kg(k‑1)H)P(k‑1|k‑2))AT+Q， (8)根据公式(6)可得公式(9)：Kg(k-1)=P(k-1|k-2)HT(HP(k-1|k-2)HT+R),---(9)]]>将公式(5)进行变形，可得公式(10)：X(k|k)＝(I‑Kg(k)H)X(k|k‑1)+Kg(k)V(k)， (10)根据公式(3)可得公式(11)：X(k|k)＝(I‑Kg(k)H)(AX(k‑1|k‑1)+Bω(k))+Kg(k)V(k) ， (11)＝(I‑Kg(k)H)AX(k‑1|k‑1)+(I‑Kg(k)H)Bω(k)+Kg(k)V(k)将公式(11)中的(I‑Kg(k)H)A和(I‑Kg(k)H)B，分别替换为为EA(k)和EB(k)，代入公式(11)得公式(12)：X(k|k)＝EA(k)X(k‑1|k‑1)+EB(k)ω(K)+Kg(k)Z(k)， (12)五、对公式(12)中的EA(k)、EB(k)和Kg(k)先求值，每次迭代过程中用到EA(k)、EB(k)和Kg(k)时，直接在飞控板处理器芯片中调用，不断重复计算公式(8)～(10)和(12)，直至找到各个时刻的最优姿态角。...
【技术特征摘要】
【专利技术属性】
技术研发人员：陈兴林，罗文嘉，李松，高怡然，徐川川，于志亮，崔宁，杨昱昊，
申请(专利权)人：哈尔滨工业大学，
类型：发明
国别省市：黑龙江;23
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。