基于模糊判断算法的多旋翼无人机位置数据融合滤波方法

摘要

本发明公开的基于模糊判断算法的多旋翼无人机位置数据融合滤波方法，包括以下步骤：1)针对多传感器融合的Kalman滤波，选取定义修正阶段方程；2)选取测量模型；3)定义包括传感器在内的精度标准；4)对多旋翼无人机的位置数据融合滤波引入模糊算法中隶属度的概念，改善在降低传感器信任程度时可能引发的数据跳变。本发明的方法是在多旋翼无人机上搭载全球卫星定位系统并利用其位置信号的基础上，对位置信号进行滤波处理，从而使得传感器的数据具有容错能力。

说明书

技术领域

本发明属于多旋翼无人机控制和智能应用技术领域，特别涉及一种基于模糊判断算法的多旋翼无人机位置数据融合滤波方法。

背景技术

凭借其良好的悬停性能，简单的机械结构，便于携带载荷的布局以及便捷的操纵能力，多旋翼无人机已经引起了国内外科研机构以及企业的广泛关注，并且已经在多个领域有了广泛的应用。例如，森林防火，地形勘察，农林植保，航空拍摄，反恐侦查，短途运输，紧急救援等。

多旋翼无人机通常搭载全球卫星定位系统,如GPS，Galieo,Glonass以及北斗系统。通过与多颗卫星进行通讯从而获得自身在地球坐标系下的绝对位置。如果采用差分技术，无人机的位置精确度可以达到厘米级。然而，所有卫星定位系统的精度均依赖于信号的优劣，在卫星数量不足的地区或者存在遮蔽效应的区域(如高楼旁、桥梁下、山岭中)，卫星系统的定位精度往往急剧下降甚至产生位置的跳变，最终给控制系统错误的反馈信息，严重情况甚至引起多旋翼无人机的坠毁。因此，发展一种高性能的具有对传感器容错能力的数据融合技术成为迫切需求。

发明内容

本发明的目的是针对多旋翼无人机在高楼旁、桥梁下、山岭中等卫星数量不足的地区或者存在遮蔽效应的区域，由于卫星系统的定位精度急剧下降甚至产生位置的跳变所导致的控制系统错误的反馈信息，设计的一种基于模糊判断算法的多旋翼无人机位置数据融合滤波方法，该方法可以广泛应用于多旋翼无人机在户外以及室内飞行中的位置定位。

本发明所要解决的技术问题可以采用如下技术方案来实现：

基于模糊判断算法的多旋翼无人机位置数据融合滤波方法，包括以下步骤：

1)针对多传感器融合的Kalman滤波，选取定义修正阶段方程；

2)选取测量模型；

3)定义包括传感器在内的精度标准；

4)对多旋翼无人机的位置数据融合滤波引入模糊算法中隶属度的概念，改善在降低传感器信任程度时可能引发的数据跳变。

在本发明的一个优选实施例中，在所述的步骤1)中，用于多传感器融合的Kalman滤波的修正阶段方程定义如下：

其中，

其中，

z_i(k)表示k时刻的第i个传感器的观测量；

用k时刻以前的数据对k时刻进行估计；

表示用k时刻以及k时刻以前的数据对k时刻的估计结果，即滤波后的输出；

P(k|k-1)表示最小预测均方差矩阵；

P(k|k)表示修正后的最小预测均方差矩阵；

K_i(k)表示误差增益；

R_i(k)表示第i个传感器的协方差矩阵；

根据测量值与预测值之间的偏差针对每一个传感器i(如GPS、IMU)提出一个相应判定函数q_i用于对传感器在第k时刻的精确程度进行判断，如下：

其中，

q_i的概率密度曲线服从χ²分布，根据χ²分布图线以及每一个时间步k计算出的第i个传感器的精度值q_i可以判断该传感器的可信程度。

在本发明的一个优选实施例中，在所述的步骤2)中，所述测量模型的选取如下：

其中，b_GPS(k)以及b_IMU(k)代表测量高斯白噪声；其分布服从协方差矩阵R_GPS(k)以及R_IMU(k)。；x(k)为系统的状态，包括加速度、速度以及位置。

在本发明的一个优选实施例中，在所述的步骤3)中，所述精度标准的定义如下：

其中，

为协方差矩阵；

计算出的各个传感器的精度以及设定的阀值，C_GPS代表相信GPS数据的情况，q_GPS＜GPS阀值；C_IMU表示相信IMU数据的情况，此时q_IMU＜IMU阀值，定义c_GPS表示只使用GPS的情况，定义c_IMU表示只使用IMU的情况，定义c_GPS+IMU表示同时使用GPS和IMU的情况，定义c₀表示不信任二者的情况，因此集合A＝{c₀，c_GPS，c_GPS+IMU，c_IMU}构成了数据融合的整个空间。

在本发明的一个优选实施例中，在所述的步骤4)中，所述隶属度的具体计算方法如下:

引入模糊算法中隶属度的概念用以改善在降低传感器信任程度时可能引发的数据跳变，从而降低无人机发生事故的风险，

其中，β为各种情况的系数，

且

β₀+β_GPS+β_IMU+β_GPS+IMU＝1(12)

只使用GPS数据的情况：

K_GPS(k)＝P_GPS(k|k)(H_GPS)^T(R_GPS)^-1>

(P_GPS(k|k))^-1＝P^-1(k|k-1)+(H_GPS)^T(R_GPS)^-1H_GPS>

只使用IMU数据的情况：

K_IMU(k)＝P_IMU(k|k)(H_IMU)^T(R_IMU)^-1>

(P_IMU(k|k))^-1＝P^-1(k|k-1)+(H_IMU)^T(R_IMU)^-1H_IMU>

同时使用IMU和GPS数据的情况：

其中，

K_{GPS|(GPS+IMU)}(k)＝P_GPS+IMU(k|k)(H_GPS)^T(R_GPS)^-1>

K_{IMU|(GPS+IMU)}(k)＝P_GPS+IMU(k|k)(H_IMU)^T(R_IMU)^-1>

(P_GPS+IMU(k|k))^-1＝P^-1(k|k-1)+(H_IMU)^T(R_IMU)^-1H_IMU

+(H_IMU)^T(R_IMU)^-1H_IMU+(H_GPS)^T(R_GPS)^-1H_GPS(22)

当既不采用GPS也不采用IMU数据时，滤波值直接采用Kalman滤波的估计值，即

最终数据融合结果：

与现有技术相比，本发明具有以下有益效果：本发明是在多旋翼无人机上搭载全球卫星定位系统并利用其位置信号的基础上，对位置信号进行滤波处理，从而使得传感器的数据具有容错能力。本发明可以广泛应用于多旋翼无人机在户外以及室内飞行中的位置定位。

附图说明

图1为本发明的标准χ²分布。

图2为本发明的传感器融合值示意图。

图3为本发明的对传感器的信任程度。

图4为本发明的滤波流程图。

具体实施方案

结合实施例、附图对本发明作进一步描述：

本发明的实施可以通过以下技术方案来实现:

1、设定由多旋翼飞行器的动力学方程积分计算得到的任一时刻的状态变量为由传感器测量得到的状态变量为现要通过多传感器融合滤波技术对仿真计算得到的数据进行矫正。

2、针对特殊环境下(如室内、丛林等特殊环境)位置传感器存在工作盲点或丧失精准度时，位置信号不可信的问题，提出利用数据融合滤波技术来对位置信号进行处理，从而进一步矫正仿真计算结果。本发面所提出的基于模糊判断算法的多悬疑无人机位置数据融合滤波技术的整体流程图如图4所示，下面具体详述本发明：

首先设定多旋翼飞行器系统状态变量在离散时间下的线性固定表达式为x(k+1)＝Fx(k)+w(k)，其中x(k)是系统的状态向量，w(k)是高斯白噪声。由N个位置传感器测量得到的N个位置信息表示为z_i(k)＝H_ix(k)+b_i(k)i＝1,2,…,N，其中z_i(k)表示测量值向量b_i(k)表示为第i个传感器的观测高斯白噪声，H_i表示为与传感器本身相关的测量矩阵，N表示位置传感器个数。

定义多传感器融合的Kalman滤波的修正阶段方程为：

其中，z_i(k)表示k时刻的第i个传感器的观测量；(k|k-1)用k时刻以前的数据对k时刻进行估计；表示用k时刻以及k时刻以前的数据对k时刻的估计结果，即滤波后的输出；P(k|k-1)表示最小预测均方差矩阵；P(k|k)表示修正后的最小预测均方差矩阵；K_i(k)表示误差增益；R_i(k)表示第i个传感器的协方差矩阵。

根据测量值与预测值之间的偏差针对每一个位置传感器i(如GPS、IMU)提出一个相应判定函数q_i用于对传感器在第k时刻的精确程度进行判断，如下

其中

q_i的概率密度曲线服从图1的χ²分布。根据χ²分布图以及每一个时间步k计算出的第i个传感器的精度值q_i可以判断该传感器的可信程度。例如，当实时计算出的q_i值大于7.8时，传感器精度超出95％的置信区间，此时应当认为此传感器的反馈信息出现问题，此时数据融合系统应当降低对该传感器反馈的相信程度。

根据位置传感器的特有测量方式，其测量模型的选取如下：

其中b_GPS(k)以及b_IMU(k)代表测量高斯白噪声。其分布服从协方差矩阵R_GPS(k)以及R_IMU(k)。x(k)为系统的状态，包括加速度，速度，位置。

对传感器的精度标准的定义如下：

其中为协方差矩阵。

计算出的各个传感器的精度以及设定的阀值。C_GPS代表相信GPS数据的情况，q_GPS＜GPS阀值；C_IMU表示相信IMU数据的情况，此时q_IMU＜IMU阀值。定义c_GPS表示只使用GPS的情况，定义c_IMU表示只使用IMU的情况，定义c_GPS+IMU表示同时使用GPS和IMU的情况。定义c₀表示不信任二者的情况，因此集合A＝{c₀，c_GPS，c_GPS+IMU，c_IMU}构成了数据融合的整个空间。

对多旋翼无人机的位置数据融合滤波引入模糊算法中隶属度的概念，改善在降低传感器信任程度时可能引发的数据跳变。隶属度的具体计算方法如下：

其中β为各种情况的系数，根据不同位置传感器的测量方式，其定义如下：

且

β₀+β_GPS+β_IMU+β_GPS+IMU＝1>

综上，改进的多传感器数据融合Kalman滤波算法框图如图2所示，而对各个传感器的信任程度如图3所示。

只使用GPS数据的情况：

K_GPS(k)＝P_GPS(k|k)(H_GPS)^T(R_GPS)^-1(10)

(P_GPS(k|k))^-1＝P^-1(k|k-1)+(H_GPS)^T(R_GPS)^-1H_GPS>

只使用IMU数据的情况：

K_IMU(k)＝P_IMU(k|k)(H_IMU)^T(R_IMU)^-1>

(P_IMU(k|k))^-1＝P^-1(k|k-1)+(H_IMU)^T(R_IMU)^-1H_IMU>

同时使用IMU和GPS数据的情况：

其中，

K_{GPS|(GPS+IMU)}(k)＝P_GPS+IMU(k|k)(H_GPS)^T(R_GPS)^-1>

K_{IMU|(GPS+IMU)}(k)＝P_GPS+IMU(k|k)(H_IMU)^T(R_IMU)^-1>

(P_GPS+IMU(k|k))^-1＝P^-1(k|k-1)+(H_IMU)^T(R_IMU)^-1H_IMU

+(H_IMU)^T(R_IMU)^-1H_IMU+(H_GPS)^T(R_GPS)^-1H_GPS>

当既不采用GPS也不采用IMU数据时，滤波值直接采用Kalman滤波的估计值，即

最终数据融合结果：

。