CN108917754B - 一种旋翼飞行器速度信号融合滤波方法 - Google Patents

Abstract

本发明公开一种旋翼飞行器速度信号融合滤波方法，属于旋翼飞行器导航技术领域。它包括：采集GPS输出的东向和北向速度信息，采集IMU输出的旋翼飞行器机体轴系下的加速度信息和磁力计输出的磁航向角信息并对GPS输出的东向和北向速度信息进行截止滤波；采集GPS输出的位置信息，采集GPS输出的地面速度信息并结合上述得到的经截止滤波后的东向和北向速度信息对GPS输出的位置信息进行截止滤波；利用上述得到的经截止滤波后的东向和北向速度信息以及上述得到的经截止滤波后的位置信息对IMU输出的旋翼飞行器机体轴系下的加速度信息进行融合校正。利用本发明的方法可以获得更加可靠的飞行速度信息。

Description

一种旋翼飞行器速度信号融合滤波方法

技术领域

本发明属于旋翼飞行器导航技术领域，尤其是一种旋翼飞行器速度信号融合滤波方法。

背景技术

目前旋翼飞行器进行自主航线飞行或定点悬停飞行都需要实时的飞行器速度信息。现有的速度信息获取主要有两个途径：(1)GPS获得速度信号；(2)通过加速度积分获得速度信号。其中，GPS信号噪声较大，特别是在信号弱的区域或者低速运动状态下的原始GPS速度信息无法使用。加速度积分得到的速度信号会随时间产生漂移。

发明内容

为克服现有技术存在的不足，本发明提供一种旋翼飞行器速度信号融合滤波方法，将GPS信号和加速度积分这两种信息源进行融合滤波，从而获得更加可靠的飞行速度信息。

为实现上述目的，本发明采用下述技术方案：

一种旋翼飞行器速度信号融合滤波方法，它包括以下步骤：

步骤1，采集GPS输出的东向和北向速度信息，采集IMU(惯性测量单元)输出的旋翼飞行器机体轴系下的加速度信息和磁力计输出的磁航向角信息并对GPS输出的东向和北向速度信息进行截止滤波；

步骤2，采集GPS输出的位置信息，采集GPS输出的地面速度信息并结合步骤1得到的经截止滤波后的东向和北向速度信息对GPS输出的位置信息进行截止滤波；

步骤3，利用步骤1得到的经截止滤波后的东向和北向速度信息以及步骤2得到的经截止滤波后的位置信息对IMU输出的旋翼飞行器机体轴系下的加速度信息进行融合校正。

进一步地，步骤1中的截止滤波包括以下步骤：

步骤11，采集当前时刻的GPS输出的东向和北向速度信息，并与上一时刻的GPS输出的东向和北向速度信息相减，得到速度的增量；

步骤12，采集IMU输出的旋翼飞行器机体轴系下的加速度信息，采集磁力计输出的磁航向角信息，通过磁航向角信息将旋翼飞行器机体轴系下的加速度信息转换为东向和北向加速度信息，并将转换得到的东向和北向加速度信息乘以时间间隔t得到速度增量的截止量；

步骤13，上一时刻的GPS输出的东向和北向速度加上速度的增量和截止量的较小值，得到东向和北向的截止滤波速度。

进一步地，步骤2中的截止滤波包括以下步骤：

步骤21，采集当前时刻的GPS输出的经纬度信息，并与上一时刻的GPS输出的经纬度信息相减，得到位置信息的增量；

步骤22，采集GPS输出的地面速度信息，与步骤13得到的截止滤波速度信息进行加权平均并乘以时间间隔t得到位置信息增量的截止量；

步骤23，上一时刻的GPS输出的经纬度信息加上位置的增量和截止量的较小值，得到东向和北向的截止滤波位置量。

进一步地，步骤3中的融合校正包括以下步骤：

步骤31，将步骤13得到的截止滤波速度信息和步骤23得到的截止滤波位置信息分别乘以相应的权重系数并相加，得到加速度的校正值；

步骤32，将该校正值与经磁航向角信息转换的东向和北向加速度信息相加，得到校正后的加速度值；

步骤33，将该加速度值对时间进行积分，得到最终融合后的东向和北向速度信息。

有益效果：

1.本发明通过加速度信息和速度信息的截止滤波削弱了GPS输出的速度信息和位置信息中有害噪声的影响；将截止滤波后的速度信息和位置信息作为校正量，来实时修正加速度信息，并通过权重来调节校正量的比重。

2.本发明利用截止滤波得到的GPS校正量来不断修正加速度值，从而避免了加速度积分造成的速度量的漂移。

附图说明

图1为本发明一实施例的旋翼飞行器速度信号融合滤波方法的流程图。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

本实施例提出的一种旋翼飞行器速度信号融合滤波方法，如图1所示，该方法包括以下步骤：

步骤1，获取t1时刻的GPS输出的东向速度Ve_t1和北向速度Vn_t1并保存，再获取t2时刻的GPS输出的东向速度Ve_t2和北向速度Vn_t2，则截止滤波速度Ve，Vn可由下式给出：

Ve＝Ve_t1+Min{Ve_t2-Ve_t1,Acc_e*dt}；

Vn＝Vn_t1+Min{Vn_t2-Vn_t1,Acc_n*dt}；

其中：Acc_e＝Acc_x*cos(Φ)+Acc_y*sin(Φ)；

Acc_n＝Acc_x*sin(Φ)-Acc_y*cos(Φ)；

Acc_x和Acc_y为IMU获取的旋翼飞行器机体轴系下的加速度值，Φ为磁力计获取的磁航向角，dt为t1与t2之间的时间间隔；

步骤2，获取t1时刻的GPS输出的纬度Lat_t1和经度Lon_t1，获取t2时刻的GPS输出的纬度Lat_t2和经度Lon_t2，则截止滤波位置Lat，Lon可由下式给出：

Lat＝Lat_t1+Min{Lat_t2-Lat_t1,(Ve*k1+Vd*k2)*dt}；

Lon＝Lon_t1+Min{Lon_t2-Lon_t1,(Vn*k1+Vd*k2)*dt}；

其中，Ve，Vn为上述截止滤波速度，Vd是GPS获取的地面速度，dt为t1与t2之间的时间间隔；k1和k2为权重系数；

步骤3，对校正后的加速度进行积分，得到最终的融合滤波速度Ve_f，Vn_f可由下式表示：

Ve_f＝∫(Acc_e+Ve*k3+Lat*k4)*△t；

Ve_f＝∫(Acc_n+Vn*k3+Lon*k4)*△t；

其中：Acc_e＝Acc_x*cos(Φ)+Acc_y*sin(Φ)；

Acc_n＝Acc_x*sin(Φ)-Acc_y*cos(Φ)；

Acc_x和Acc_y为IMU获取的旋翼飞行器机体轴系下的加速度值，Φ为磁力计获取的磁航向角，k3和k4为权重系数；△t为数字积分运算的时间间隔。

对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (1)

1.一种旋翼飞行器速度信号融合滤波方法，其特征在于，它包括以下步骤：

步骤1，采集GPS输出的东向和北向速度信息，采集IMU输出的旋翼飞行器机体轴系下的加速度信息和磁力计输出的磁航向角信息并对GPS输出的东向和北向速度信息进行截止滤波；

步骤2，采集GPS输出的位置信息，采集GPS输出的地面速度信息并结合所述步骤1得到的经截止滤波后的东向和北向速度信息对GPS输出的位置信息进行截止滤波；

步骤3，利用所述步骤1得到的经截止滤波后的东向和北向速度信息以及所述步骤2得到的经截止滤波后的位置信息对IMU输出的旋翼飞行器机体轴系下的加速度信息进行融合校正；

所述步骤1中的截止滤波包括以下步骤：

步骤11，采集当前时刻的GPS输出的东向和北向速度信息，并与上一时刻的GPS输出的东向和北向速度信息相减，得到速度的增量；

步骤12，采集IMU输出的旋翼飞行器机体轴系下的加速度信息，采集磁力计输出的磁航向角信息，通过磁航向角信息将旋翼飞行器机体轴系下的加速度信息转换为东向和北向加速度信息，并将转换得到的东向和北向加速度信息乘以时间间隔t得到速度增量的截止量；

步骤13，上一时刻的GPS输出的东向和北向速度加上速度的增量和截止量的较小值，得到东向和北向的截止滤波速度；

所述步骤2中的截止滤波包括以下步骤：

步骤21，采集当前时刻的GPS输出的经纬度信息，并与上一时刻的GPS输出的经纬度信息相减，得到位置信息的增量；

步骤22，采集GPS输出的地面速度信息，与所述步骤13得到的截止滤波速度信息进行加权平均并乘以时间间隔t得到位置信息增量的截止量；

步骤23，上一时刻的GPS输出的经纬度信息加上位置的增量和截止量的较小值，得到东向和北向的截止滤波位置量；

所述步骤3中的融合校正包括以下步骤：

步骤31，将所述步骤13得到的截止滤波速度信息和所述步骤23得到的截止滤波位置信息分别乘以相应的权重系数并相加，得到加速度的校正值；

步骤32，将该校正值与经磁航向角信息转换的东向和北向加速度信息相加，得到校正后的加速度值；

步骤33，将该加速度值对时间进行积分，得到最终融合后的东向和北向速度信息。

Images