一种用于无人飞行器系统的多源组合导航方法与流程

本发明涉及无人飞行器导航，尤其涉及一种用于无人飞行器系统的多源组合导航方法。

背景技术：

1、实时的高精度导航信息是保障无人飞行器正常飞行的核心，也是保证无人飞行器系统顺利完成飞行任务的关键。近年来，微机电惯性传感器(mems惯组)系统得到了快速发展，该系统具备的优点有：价格低廉、尺寸小、重量轻、导航精度高、数据更新信息频率高等，使得mems惯组在无人飞行器领域中得到广泛应用。然而，仅依靠mems惯组进行导航时，导航误差会随着时间的增加而不断累积，且惯导系统的天向通道天然发散，因此mems惯组不适合在飞行器系统中独立完成导航任务，还需要与其他导航传感器，如气压高度计、磁航向仪以及卫星导航系统等组合使用来满足无人飞行器导航的要求，因此，组合导航系统成为行业发展趋势之一。

2、cn111351482a公开了一种基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法。该方法包括：首先，建立传感器的测量模型，推导四元数形式的导航方程，并对该方程进行线性化处理得到误差状态方程；然后，基于gps、磁力计和加速度计的测量信息，建立用于卡尔曼滤波估计的测量方程；最后，结合惯性-gps松组合模式，采用误差状态卡尔曼滤波器eskf对误差状态进行最优估计，并实现对导航状态的校正。

3、cn111207734a公开了一种基于ekf的无人机组合导航方法，该方法设计的组合导航系统可实时输出高精度的无人机姿态、速度以及位置信息；该方法可通过matlab直接生成可编程利用的代码，减少复杂的公式推导过程，并且可提高cpu 60％的运行效率；该方法可实时对量测信息进行一致性检测，防止误差较大的量测信息对滤波器造成的不利的影响；该方法可实时对滤波器的协方差矩阵进行正定性检测，防止滤波器发散，及时对滤波器进行调整。

4、当无人飞行器的硬件状态得到确定后，则高精度组合导航系统的关键在于如何设计合适的导航滤波器，以实现各个导航信息的高效融合。目前高精度惯导的组合导航技术的应用主要包括两种方式：

5、一、以基于导航误差模型建立的线性卡尔曼滤波(kf)方法，由于线性卡尔曼滤波(kf)的导航模型是假定导航系统的各项误差均为小量、导航误差近似为线性化作为前提而使用的，因此该方法对惯组精度要求较高，难以在低精度mems组合导航系统中应用。

6、二、以基于导航状态模型建立的扩展卡尔曼滤波(ekf)方法，从机理上来说该方法可以应用在低精度mems多源组合导航系统中，但是该方法极容易在系统量测信息失真或失效条件下出现发散。

7、因此，如何有效地对多传感器组成的多源组合导航系统进行信息融合得到高精度导航信息，仍是值得研究的课题和有待解决的技术问题。

技术实现思路

1、为了克服现有技术的不足，本发明所要解决的技术问题在于提供一种用于无人飞行器系统的多源组合导航方法。在现有ekf多源组合导航基础上，以序贯滤波形式更新量测，同时增加量测新息故障检测手段，并提出一种无卫导条件下的定高姿态导航工作模式。

2、为了解决上述技术问题，本发明提供的用于无人飞行器系统的多源组合导航方法，包括以下步骤。

3、步骤1，完成组合导航系统的状态空间模型建立，并经过线性化处理得到导航系统的状态空间方程，供组合导航ekf滤波系统时间更新，包括以下步骤：

4、1)选取以姿态四元数q、北东地速度v、北东地方向位置s的导航信息、和以三轴陀螺零偏ε、三轴加计零偏的惯性传感器误差组成的16维状态信息；

5、

6、2)根据惯性导航基本原理，省去小量，得到导航系统的状态空间方程：

7、

8、其中，ωb代表角速度增量，由陀螺采样信息得到，

9、是姿态矩阵，

10、fb表示速度增量，由加计采样信息得到，

11、g表示当地重力加速度，

12、nω表示惯性传感器陀螺的噪声，

13、nf表示惯性传感器加计的噪声；

14、3)在所述步骤2)的基础上建立经离散化后导航系统的状态空间方程：

15、xk+1＝fxk+gμk+ωk

16、其中，xk+1向量表示当前时刻的状态信息，

17、xk向量表示上一时刻的状态信息，

18、f矩阵是16×16的状态转移矩阵，

19、g矩阵是16×6的系统噪声干扰矩阵，

20、μk是6×1的干扰向量、是惯性传感器陀螺加计的测量方差，

21、ωk则是惯性传感器陀螺和加计的噪声方差；

22、4)在所述步骤3)的基础上，基于状态空间方程计算各状态信息的一阶偏微分方程，得到经线性化处理后的状态转移矩阵f；

23、

24、其中，在所述16×16的状态转移矩阵f的各分块矩阵中，

25、i3×3表示3×3的单位矩阵，

26、为关于姿态四元数的状态转移矩阵，

27、为姿态四元数关于增量角度偏差的状态转移矩阵，

28、为速度关于姿态四元数的状态转移矩阵，

29、为速度关于增量速度偏差的状态转移矩阵，

30、为位置关于速度的状态转移矩阵；

31、在所述步骤4)的基础上，将状态信息对角度增量和速度增量的测量值求偏导，计算系统噪声干扰矩阵g：

32、

33、其中，为姿态四元数关于增量角度噪声的控制输入矩阵，

34、为速度关于增量速度噪声的控制输入矩阵，

35、o3x3表示3×3的全零矩阵，

36、o4x3表示4×3的全零矩阵；

37、步骤2，根据各导航传感器信息与组合导航系统各状态空间模型之间的关系，分别得到各导航传感器信息与状态模型之间的测量融合方程，供组合导航ekf滤波系统测量更新，包括以下步骤：

38、1)ekf滤波系统的测量融合方程为：

39、zk+1＝hk+1xk+1+vk+1

40、其中，zk+1向量表示的是当前时刻的测量信息，

41、hk+1矩阵是对应测量通道的测量矩阵，

42、xk+1向量表示当前时刻的状态信息，

43、vk+1矩阵是对应的量测噪声矩阵；

44、2)通过卫星导航接收机获得无人飞行器的三轴速度和水平位置信息，计算卫星导航系统(gnss)的三轴速度和水平位置量测矩阵hv,s：

45、

46、通过气压高度计获得无人飞行器的高度信息，计算气压高度计的高度信息量测矩阵

47、

48、通过磁力计或卫星航迹角获得无人飞行器的实时航向信息ψ，根据航向角与姿态四元数q的关系，通过计算各状态信息的一阶偏微分方程得到航向量测的量测矩阵；

49、航向测量值ψ与姿态四元数q存在如下关系：

50、

51、计算航向测量值的观测相对于状态向量x的量测矩阵hψ：

52、

53、步骤3，以多源组合导航系统状态空间方程和测量融合方程为基础，完成扩展卡尔曼滤波(ekf)离散化后的程序流程设计，得到多源组合导航方法；

54、经离散化处理得到扩展卡尔曼滤波(ekf)的基本方程包括时间更新环节和测量更新环节，

55、扩展卡尔曼滤波经离散化处理后的时间更新环节的基本方程为：

56、

57、其中，xk表示上一时刻的状态信息，

58、xk|k+1表示一步预测状态信息，

59、f是状态转移矩阵，

60、pk是上一时刻的协方差矩阵，

61、pk|k+1是一步预测协方差矩阵，

62、qk是系统噪声矩阵，

63、g是16×6的系统噪声干扰矩阵，

64、t为矩阵转置；

65、扩展卡尔曼滤波经离散化处理后的测量更新环节的基本方程为：

66、

67、其中，kk+1表示增益矩阵，i是16×16的单位阵

68、rk+1表示量测噪声矩阵；当判断当前更新周期组合导航系统的量测信息zk+1有效，则根据已知的量测噪声矩阵rk+1和时间更新环节得到的一步预测协方差矩阵pk|k+1、一步预测状态信息xk|k+1以及所述测量融合方程中得到的量测矩阵hk+1，可以完成ekf滤波器的测量更新环节，进而实时迭代计算得到经滤波融合后的最新时刻的协方差矩阵pk+1和最新时刻的状态信息xk+1；

69、针对所述测量更新环节，采用序贯滤波的方式，在每个量测信息zk+1独立有效时，依次对各zk+1(i)通道的信息完成相应的测量更新；测量环节的序贯滤波更新方式如下：

70、

71、上式中，sk+1(i,i)是表示当前量测通道的量测信息的协方差矩阵，

72、hk+1(i,:)表示的是hk+1矩阵第i行的信息；

73、针对不同测量信息采样频率不相同的情况，分别对高度量测、北东地三轴速度与水平轴向位置量测、航向量测三个通道的量测信息，采取“量测周期有效则进行更新、量测周期无效则不更新”的方式进行滤波融合；

74、在ekf滤波系统中，针对量测信息失真条件容易出现发散的问题，增加每个量测通道的新息检验环节，新息故障检测因子λ的计算公式如下：

75、

76、当判断新息故障检测因子λ大于某一阈值n，则对该通道进行隔离，该量测周期内不进行对应通道的量测更新，待下一量测周期新息故障检测因子λ的判断；

77、当卫星导航无效一定时间后，组合导航系统将自动降级为定高姿态模式，即组合导航系统中仅进行高度量测、水平位置量测，以保证组合导航系统水平姿态和高度通道的不发散，待收到卫星信息后再次进行多源信息的融合；即：zk+1中水平位置量测的信息设为固定值；zk+1中高度量测的信息实时采用气压高度计信息。

78、本发明提供的技术方案，能对无人飞行器系统的多传感器信息进行融合，实时保证组合导航系统导航输出精度；一方面，采用对各量测通道序贯滤波的方式对多源组合导航系统进行数据融合，具有计算量较小，结构简单，实时性好等优点；另一方面，增加了量测新息故障检测环节，很好地避免滤波器发散的现象，使得组合导航系统具有更好的鲁棒性；同时，设计增加了卫星导航较长时间无效后的定高姿态模式，使无人飞行器具有更好的容错性。