CN106918828A - 一种飞行器自主导航方法及系统 - Google Patents

Abstract

本发明涉及飞行器导航与控制技术领域，具体来说是一种飞行器自主导航方法及系统，该方法及系统按周期获取GNSS模块输出的位置信息和磁场测量仪检测到的磁场信息。当GNSS有效时，导航输出值为GNSS接收机的位置信息；当GNSS无效时，采用融合滤波算法及牛顿迭代法依据磁场测量仪的磁场信息解算飞行器的位置信息，并将此输出值作为导航值。本发明能在GNSS有效和无效时都能进行精确导航，还通过采用滤波融合算法，使得两种导航模式的之间的数据能平滑切换，既保证飞行器的导航精度，又有效提高了整个飞行器导航系统的可靠性。

Description

一种飞行器自主导航方法及系统

技术领域

本发明涉及飞行器导航控制技术领域，具体来说是一种飞行器自主导航方法及系统。

背景技术

随着飞行器技术的发展，飞行器的飞行逐渐由人工控制飞行向自主飞行过渡，而自主可靠的导航技术是实现自主飞行亟需解决的关键技术之一。飞行器导航按照工作原理的不同可分为多种。例如现有技术中的地磁场导航，主要采用磁场匹配的方式，即将地磁场及对应的经度、纬度及高度等信息预存在飞行器的处理器中，飞行器在飞行过程中，将磁场测量仪采集到的磁场与预存的磁场强度进行匹配，进而得到相应的经度、纬度及高度，虽然因磁场测量仪体积小，在小量程时精度高、线性度高的优点在导航技术中广泛使用，但是这种导航方式对处理器的存储容量及计算能力要求高，匹配精度较低。

针对该问题，目前较为常用的提高导航精确度的方法是采用多敏感器信息融合技术，即在一个导航系统中使用多种敏感器获得的多种形式的导航信息，而后通过一定的算法“合并”来自多个信息源的信息，以产生比单个传感器所得到数据更可靠、更准确的信息，克服了单一导航敏感器可靠性及容错性低、导航精度低的缺陷。而在组合导航技术中，关于GNSS融合导航算法是采用惯测组合与GNSS的融合，其中，惯性器件的选择是关键，若惯性器件为石英加表，敏感器的体积太大；若惯性器件为微机械加表，则导航的精度及线性度太差，且惯性器件普遍存在零偏及零偏稳定性的缺陷，会导致导航精度下降。

在上述基础上，无疑会想到结合磁场测量仪和GNSS导航的优点而进行导航，但磁场测量仪导航属于无源导航，磁场测量仪由于飞行器通电时会产生磁场，难免会降低地磁场的测量精度，同时由于地磁场模型的误差，因而基于磁场测量仪的导航算法精度较低；而GNSS导航属于有源导航，虽然导航精度高，但当GPS、GLONASS或者北斗等导航星不可见或者不可用时，GNSS将失效，导致导航不可用，从而降低了导航的可靠性，因此，如何将两者进行自合形成自主导航成为了导航领域的一大难题。

发明内容

为解决在绝对导航时磁场测量仪导航精度低，而GNSS导航无效时无导航可用，导致可靠性低的问题，本发明提供一种基于地磁场测量仪和GNSS的飞行器自主导航方法，以磁场测量仪导航、GNSS导航的两种导航方式，实现高精度、高可靠的导航，提高飞行器自主绝对导航方法的可靠性。

为了解决上述技术问题，本发明采用了如下的技术方案：

一种飞行器自主导航方法，所述的导航方法的具体步骤为：

S1.按周期获取GNSS模块检测到的位置信息；

S2.按周期获取磁场测量仪检测到磁场信息；

S3.根据GNSS模块的位置信息判断GNSS模块是否有效：

若GNSS模块有效，将飞行器设置为GNSS导航模式，依据GNSS模块输出的位置信息进行导航；

若GNSS模块无效，将飞行器设置为磁场测量仪导航模式，进入步骤S4；

S4.依据上周期位置信息和本周期磁场信息进行飞行器的位置计算，依据计算得出的位置信息进行导航。

进一步地，所述的位置计算具体包括以下步骤：

S4-1.根据磁场测量仪检测到三轴磁场强度，在北东地坐标系下进行计算，计算公式如下：

将公式(1)进行转换分别得到公式(2)、(3)、(4)、(5)：

公式(1)、(2)、(3)、(4)、(5)中，R_E为地球半径，g⁰ ₁、g¹ ₁、h¹ ₁分别为高斯系数，λ为飞行器的经度，θ为飞行器的地心余纬，r为飞行器到地心的距离；

S4-2.求解公式(2)，计算经度λ；

S4-3.求解公式(3)，计算地心余纬θ；

S4-4.根据步骤S4-2及步骤S4-3计算得到的经度λ和地心余纬θ，利用公式(5)，计算此时飞行器到地心的距离r；

S4-5.根据计算得到的经度λ、地心余纬θ、飞行器到地心的距离r计算

飞行器在WGS-84系下的位置，计算方法如下：

S4-6.根据步骤S4-5分别计算飞行器在本周期k和上一周期k-1时的位置，计算飞行器在计算周期t内的速度，计算方法为：

进一步地，所述的导航方法还包括：将本周期内GNSS模块的位置信息与上一周期内依据磁场强度解算得到的位置信息进行滤波融合计算，以滤波融合计算得到位置信息作为本周期位置计算的初始值。

进一步地，所述的滤波融合计算具体为：

公式(8)式中，α、β为滤波系数，λ_{GNSS_k}、θ_{GNSS_k}、r_{GNSS_k}为k周期内GNSS模块检测到的位置信息，λ_{e_(k-1)}、θ_{e_(k-1)}、r_{e_(k-1)}为k-1周期内依据磁场强度解算得到的位置信息。

进一步地，所述的判断GNSS模块是否有效的方法如下：

Δp＝p_k-p_k-1

Δv＝v_k-v_k-1

式中，p_k为本周期位置，p_k-1为上周期位置，Δp为位置差，v_k为本周期速度，v_k-1为上周期速度，Δv为速度差，假设a_k-1为上周期加速度，若Δv>3a_k-1或者Δp>3Δv时，则GNSS模块无效。

进一步地，对所述公式(2)采用牛顿迭代法进行求解。

进一步地，所述的位置信息包括飞行器的经度λ、地心余纬θ及飞行器到地心的距离r。

进一步地，所述的磁场信息包括北东地坐标系下的三轴磁场强度B_x、B_y、B_z。

本发明另外还提供一种飞行器自主导航系统，包括GNSS模块、磁场测量仪及数据处理模块，

所述的GNSS模块检测飞行器的位置信息，并将飞行器的位置信息按周期发送至数据处理模块；

所述的磁场测量仪检测磁场信息，并将磁场信息按周期发送至数据处理模块；

所述的数据处理模块，依据GNSS模块的位置信息判断GNSS模块是否有效，并根据GNSS模块检测到的飞行器的位置信息和磁场测量仪检测到的磁场信息进行飞行器的位置计算，利用计算得到的位置信息，进一步计算得出飞行器在WGS-84系下的位置和速度，并将计算后的飞行器的位置信息和速度作为导航数据输出。

进一步地，所述的数据处理模块还包括：将本周期内GNSS模块的位置信息与上一周期内依据磁场强度解算得到的位置信息的进行滤波融合计算，将滤波融合后的位置信息作为位置计算的初始值。

本发明由于采用以上技术方案，使之与现有技术相比，具有以下的优点和积极效果：

1.本发明利用磁场测量仪和GNSS进行导航，在GNSS有效时利用GNSS输出的导航数据进行导航，而在GNSS无效时切换至磁场测量仪导航模式进行位置计算，按计算得出的位置信息作为导航数据进行导航，既保证飞行器的导航精度，又有效提高了整个飞行器导航系统的可靠性；

2.在磁场测量仪导航模式的位置计算中，综合GNSS模块和磁场测量仪的数据进行位置计算，提高了磁场测量仪导航模式时的导航精度；

3.在位置计算前对本周期GNSS模块和上周期磁场测量仪的数据进行滤波融合计算，提高导航精度的同时，实现了两种导航模式之间数据的平滑切换，解决了GNSS模块导航及磁场测量仪模块导航切换时数据突变导致的导航系统不稳定的问题。

附图说明

图1是本发明中飞行器自主导航方法的流程示意图；

图2是本发明中飞行器自主导航系统的连接结构示意图。

具体实施方式

以下结合附图和具体实施例对本发明提出的技术方案进一步详细说明。根据下面说明和权利要求书，本发明的优点和特征将更清楚。需说明的是，附图均采用非常简化的形式且均使用非精准的比率，仅用于方便、明晰地辅助说明本发明实施例的目的。

实施例1

参见图1，为本发明中飞行器自主导航方法的流程示意图，现根据该流程图对本发明中的飞行器自主导航方法的具体过程进行说明，飞行器自主导航方法的具体步骤如下：

S1.按周期获取内GNSS模块检测到的位置信息；

S2.按周期获取磁场测量仪检测到的磁场信息；

S3.根据GNSS模块的位置信息判断GNSS模块是否有效：

若GNSS模块有效，将飞行器设置为GNSS导航模式，依据GNSS模块输出的位置信息进行导航；

若GNSS模块无效，将飞行器设置为磁场测量仪导航模式，进入步骤S4；

S4.依据位置信息和磁场信息进行飞行器的位置计算，依据计算得出的位置信息进行导航。

其中，位置信息包括飞行器的经度、地心余纬及飞行器到地心的距离，磁场信息包括北东地坐标系下的三轴磁场强度。在GNSS模块有效的情况下，导航输出采用GNSS模块采集的位置信息作为输出值；当GPS、GLONASS或者北斗等导航星不可见或者不可用导致GNSS模块无效时，导航输出则采用磁场测量仪和GNSS模块采集的位置信息进行融合滤波计算，以计算得出的位置信息作为导航值进行后续导航，提高了导航系统的可靠性。此外，融合滤波算法保证了磁场测量仪导航模式的导航精度。

判断GNSS模块是否有效的方法如下：

Δp＝p_k-p_k-1

Δv＝v_k-v_k-1

式中，p_k为本周期位置，p_k-1为上周期位置，Δp位置差，v_k为本周期速度，v_k-1为上周期速度，Δv速度差，假设a_k-1为上周期加速度，一般情况下，若Δv>3a_k-1或者Δp>3Δv时，则认为GNSS模块无效。

为提高磁场测量仪导航模式时的导航精度，在磁场测量仪导航模式的位置计算中，需综合GNSS模块和磁场测量仪的数据进行位置计算，为便于说明，其中，位置信息分别以飞行器的经度λ、地心余纬θ及飞行器到地心的距离r为例，磁场信息以北东地坐标系下的三轴磁场强度B_x、B_y、B_z为例。

位置计算具体包括以下步骤：

首先，步骤一，根据磁场测量仪检测到三轴磁场强度，在北东地坐标系下进行计算，计算公式如下：

将公式(1)进行转换分别得到公式(2)、(3)、(4)、(5)：

公式(1)、(2)、(3)、(4)、(5)中，R_E为地球半径，一般取6378140m，g⁰ ₁、g¹ ₁、h¹ ₁分别为高斯系数，g⁰ ₁一般取值为-0.30339Gs，g¹ ₁一般取值为-0.02123Gs，h¹ ₁一般取值为0.05758Gs。

步骤二，对公式进行求解：求解公式(2)，计算经度λ，可采用牛顿迭代法对公式(2)进行求解，可以用公式(8)融合滤波计算得到数值作为迭代的初始值，以提高磁场测量仪导航的精度，下文中将会详细介绍融合滤波计算过程；求解公式(3)，计算地心余纬θ；根据计算得到的经度λ和地心余纬θ及公式(5)，计算此时飞行器到地心的距离r。

步骤三，根据计算得到的经度λ、地心余纬θ、飞行器到地心的距离r计算飞行器在WGS-84系下的位置，计算方法如下：

步骤四，分别计算飞行器在本周期k和上一周期k-1时的位置，计算飞行器在计算周期t内的速度，计算方法为：

为解决GNSS模块导航及磁场测量仪模块导航切换时数据突变导致的导航系统不稳定的问题，还可将本周期内GNSS模块的位置信息与上一周期内依据磁场强度解算得到的位置信息进行滤波融合计算，以滤波融合计算得到位置信息作为本周期位置计算的初始值，滤波融合计算的公式具体为：

公式(8)式中，α、β为滤波系数，当GNSS有效时，α、β可根据导航系统特性取值；当GNSS无效时，β＝0，λ_{GNSS_k}、θ_{GNSS_k}、r_{GNSS_k}对应依次为k周期内GNSS模块检测到的飞行器的经度、地心余纬及飞行器到地心的距离，λ_{e_(k-1)}、θ_{e_(k-1)}、r_{e_(k-1)}为k-1周期内依据磁场强度解算得到的飞行器的经度、地心余纬及飞行器到地心的距离，将滤波融合计算计算得出的经度λ、地心余纬θ、飞行器到地心的距离r作为公式(2)中的牛顿迭代的初始值，以此提高磁场测量仪模式的导航精度，并实现了GNSS模块导航模式和磁场测量仪导航模式之间的数据的平滑切换。

本发明是综合地磁场测量仪和GNSS的优点，在GNSS有效时利用GNSS输出的导航数据进行导航，而在GNSS无效时切换至磁场测量仪导航模式进行位置计算，按计算得出的位置信息作为导航数据进行导航，在保证飞行器的导航精度的情况下，有效提高了整个飞行器导航过程及导航系统的可靠性。

实施例2

本发明另外还提供一种飞行器自主导航系统，参见图2，飞行器自主导航系统包括GNSS模块、磁场测量仪及数据处理模块，GNSS模块检测飞行器的位置信息，并将飞行器的位置信息按周期发送至数据处理模块；磁场测量仪检测磁场信息和位置信息，并将磁场信息和位置信息按周期发送至数据处理模块；数据处理模块，依据GNSS模块的位置信息判断GNSS模块是否有效，并根据GNSS模块检测到的飞行器的位置信息和磁场测量仪检测到的磁场信息，进行飞行器的位置计算，解算飞行器的位置信息，利用计算得到的位置信息进一步计算得出飞行器在WGS-84系下的位置和速度，并将计算后的飞行器的位置信息和速度作为导航数据输出。本系统利用磁场测量仪和GNSS进行导航，在GNSS有效时利用GNSS输出的导航数据进行导航，而在GNSS无效时切换至磁场测量仪导航模式进行位置计算，并按计算得出的位置信息作为导航数据进行导航，可广泛应用于多种飞行器，例如飞机、卫星等，以实现飞行器精确、可靠的导航过程。

另外，为实现GNSS模块导航及磁场测量仪模块导航两种导航模式之间的平滑切换，解决GNSS模块导航及磁场测量仪模块导航切换时数据突变导致的导航系统不稳定的问题，数据处理模块还将本周期内GNSS模块的位置信息与上一周期内依据磁场强度解算得到的位置信息进行滤波融合计算，将计算后的位置信息作为导航数据输出。

飞行器自主导航系统中，数据处理模块对飞行器进行的位置计算和滤波融合计算与实施例1中的计算过程相似，在此不做赘述。

显然，本领域的技术人员可以对发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (10)

1.一种飞行器自主导航方法，其特征在于，所述的导航方法的具体步骤为：

S1.按周期获取GNSS模块检测到的位置信息；

S2.按周期获取磁场测量仪检测到磁场信息；

S3.根据GNSS模块的位置信息判断GNSS模块是否有效：

若GNSS模块有效，将飞行器设置为GNSS导航模式，依据GNSS模块输出的位置信息进行导航；

若GNSS模块无效，将飞行器设置为磁场测量仪导航模式，进入步骤S4；

S4.依据上周期位置信息和本周期磁场信息进行飞行器的位置计算，依据计算得出的位置信息进行导航。

2.根据权利要求1所述的一种飞行器自主导航方法，其特征在于，所述的位置计算具体包括以下步骤：

S4-1.根据磁场测量仪检测到三轴磁场强度，在北东地坐标系下进行计算，计算公式如下：

$\left\{\begin{array}{c}{B}_x=-R_E^3/r^3(g_1^{0}sin\mathrm{\θ}-(g_1^{1}cos\mathrm{\λ}+h_1^{1}sin\mathrm{\λ}\left)cos\mathrm{\θ}\right)\\ B_y=R_E^3/r^3(g_1^{1}sin\mathrm{\λ}+h_1^1\cos \mathrm{\λ})\\ B_z=-2R_E^3/r^3(g_1^0\cos \mathrm{\θ}+(g_1^1\cos \mathrm{\λ}+h_1^1\sin \mathrm{\λ}\left)sin\mathrm{\θ}\right)\end{array}\right.---\left(1\right)$

将公式(1)进行转换分别得到公式(2)、(3)、(4)、(5)：

$\begin{array}{c}{B}_y\left(g_1^{0}sin\right(atan\left(2B_x/B_z\right)+atan\left(f/g_1^0\right))+fcos(atan\left(2B_x/B_z\right)+atan\left(f/g_1^0\right)\left)\right)\\ =B_x(g_1^1\sin \mathrm{\λ}+h_1^1\cos \mathrm{\λ})\end{array}---\left(2\right)$

$\mathrm{\θ}=atan(2B_x/B_z)+atan(f/g_1^0)---\left(3\right)$

$f=g_1^{1}cos\mathrm{\λ}+h_1^{1}sin\mathrm{\λ}---\left(4\right)$

$\left\{\begin{array}{cc}r=\sqrt[3]{-R_E^3(g_1^0\sin \mathrm{\θ}-(g_1^1\cos \mathrm{\λ}+h_1^1\sin \mathrm{\λ}\left)\cos \mathrm{\θ}\right)/B_x}& (B_x\≠0)\\ r=\sqrt{-2R_E^3(g_1^0\cos \mathrm{\θ}+(g_1^1\cos \mathrm{\λ}+h_1^1\sin \mathrm{\λ}\left)\sin \mathrm{\θ}\right)/B_z}& (B_x=0)\end{array}\right.---\left(5\right)$

公式(1)、(2)、(3)、(4)、(5)中，R_E为地球半径，g⁰ ₁、g¹ ₁、h¹ ₁分别为高斯系数，λ为飞行器的经度，θ为飞行器的地心余纬，r为飞行器到地心的距离；

S4-2.将公式(4)带入公式(2)，计算经度λ；

S4-3.求解公式(3)，计算地心余纬θ；

S4-4.根据步骤S4-2及步骤S4-3计算得到的经度λ和地心余纬θ，利用公式(5)，计算此时飞行器到地心的距离r；

S4-5.根据计算得到的经度λ、地心余纬θ、飞行器到地心的距离r计算飞行器在WGS-84系下的位置，计算方法如下：

$\left\{\begin{array}{c}x=r\cos (pi/2-\mathrm{\θ})\cos \mathrm{\λ}\\ y=r\cos (pi/2-\mathrm{\θ})\sin \mathrm{\λ}\\ z=r\sin (pi/2-\mathrm{\θ})\end{array}\right.---\left(6\right)$

S4-6.根据步骤S4-5分别计算飞行器在本周期k和上一周期k-1时的位置，计算飞行器在计算周期t内的速度，计算方法为：

$\left\{\begin{array}{c}{v}_x=(x_k-x_{k-1})/t\\ v_y=(y_k-y_{k-1})/t\\ v_z=(z_k-z_{k-1})/t\end{array}\right.---\left(7\right).$

3.根据权利要求1或2所述的一种飞行器自主导航方法，其特征在于，所述的导航方法还包括：将本周期内GNSS模块的位置信息与上一周期内依据磁场强度解算得到的位置信息进行滤波融合计算，以滤波融合计算得到位置信息作为本周期位置计算的初始值。

4.根据权利要求3所述的一种飞行器自主导航方法，其特征在于，所述的滤波融合计算具体为：

$\left\{\begin{array}{c}\mathrm{\λ}={\mathrm{\α\λ}}_{e\_(k-1)}+{\mathrm{\β\λ}}_{GNSS\_k}\\ \mathrm{\θ}={\mathrm{\α\θ}}_{e\_(k-1)}+{\mathrm{\β\θ}}_{GNSS\_k}\\ r={\mathrm{\αr}}_{e\_(k-1)}+{\mathrm{\βr}}_{GNSS\_k}\\ \mathrm{\α}+\mathrm{\β}=1\end{array}\right.---\left(8\right)$

公式(8)式中，α、β为滤波系数，λ_{GNSS_k}、θ_{GNSS_k}、r_{GNSS_k}为k周期内GNSS模块检测到的位置信息，λ_{e_(k-1)}、θ_{e_(k-1)}、r_{e_(k-1)}为k-1周期内依据磁场强度解算得到的位置信息。

5.根据权利要求1所述的一种飞行器自主导航方法，其特征在于，所述的判断GNSS模块是否有效的方法如下：

Δp＝p_k-p_k-1

Δv＝v_k-v_k-1

式中，p_k为本周期位置，p_k-1为上周期位置，Δp为位置差，v_k为本周期速速，v_k-1为上周期速度，Δv为速度差，假设a_k-1为上周期加速度，若Δv>3a_k-1或者Δp>3Δv时，则GNSS模块无效。

6.根据权利要求2所述的一种飞行器自主导航方法，其特征在于，对所述公式(2)采用牛顿迭代法进行求解。

7.根据权利要求1所述的一种飞行器自主导航方法，其特征在于，所述的位置信息包括飞行器的经度λ、地心余纬θ及飞行器到地心的距离r。

8.根据权利要求1所述的一种飞行器自主导航方法，其特征在于，所述的磁场信息包括北东地坐标系下的三轴磁场强度B_x、B_y、B_z。

9.一种飞行器自主导航系统，包括GNSS模块、磁场测量仪及数据处理模块，其特征在于，

所述的GNSS模块检测飞行器的位置信息，并将飞行器的位置信息按周期发送至数据处理模块；

所述的磁场测量仪检测磁场信息，并将磁场信息按周期发送至数据处理模块；

所述的数据处理模块，依据GNSS模块的位置信息判断GNSS模块是否有效，并根据GNSS模块检测到的飞行器的位置信息和磁场测量仪检测到的磁场信息进行飞行器的位置计算，利用计算得到的位置信息进一步计算得出飞行器在WGS-84系下的位置和速度，并将计算后的飞行器的位置信息和速度作为导航数据输出。

10.根据权利要求9所述的一种飞行器自主导航系统，其特征在于，所述的数据处理模块还包括：将本周期内GNSS模块的位置信息与上一周期内依据磁场强度解算得到的位置信息的进行滤波融合计算，将滤波融合后的位置信息作为位置计算的初始值。