CN113932806B - 高速飞行器惯性/地磁匹配搜索区域自适应组合导航方法 - Google Patents

Abstract

本发明公开了一种超高速飞行器惯性/地磁匹配搜索区域自适应的组合导航方法，实现步骤包括：高速飞行器是否飞入地磁图的判断；纯惯导误差的粗估计；匹配搜索区域和匹配搜索步长的确定；获取惯导搜索序列和地磁传感器采集的地磁序列；最小距离度量及最佳校正位置点的确定；匹配搜索区域和搜索步长的自适应；重复上述步骤直到超高速飞行器飞出地磁图。本发明针对高速飞行器的飞行特点和导航定位要求，采用惯性导航+地磁匹配导航相组合的方式，利用地磁传感器测量序列与惯导平移搜索过程中所组成的序列组进行惯导解算轨迹校正，将匹配轨迹校正到真实航迹附近，提高了导航定位精度和定位结果的实时性。

Description

高速飞行器惯性/地磁匹配搜索区域自适应组合导航方法

技术领域

本发明涉及高速飞行器的导航定位领域，具体涉及的是一种高速飞行器惯性/地磁匹配搜索区域自适应的组合导航方法。

背景技术

现阶段，随着科学技术的发展，高速飞行器在许多领域的应用越来越广泛，对其的研究也越来越受到重视。高速飞行器在飞行过程中需要高精度的姿态和位置信息来帮助其圆满完成任务，因此飞行定位技术已经成为保证高速飞行器按照指定路线飞行的核心技术。

随着科学技术的快速发展，国内外学者已经在导航定位领域做出了丰厚的研究成果，如GPS/惯性组合、惯性/里程计/RTK组合以及激光雷达/视觉组合导航等，被广泛应用到各个领域。然而，上述技术各有缺陷，无法满足高速飞行器的定位需求，如纯惯性虽短期精度高，但其误差随时间发散；GPS受信号影响大，为半自主导航且更新频率低；里程计主要用于地面记录里程；激光雷达成本高且受环境影响大；视觉更新频率低对外界光过于敏感。

地磁是地球固有资源，且变化周期缓慢，具有全天时、全天候、全地域的优势，已成为海底核潜艇导航和部分低空低速飞行器导航的重要手段。在惯性/地磁的组合导航领域，国内外学者也做出不少研究成果，但目前绝大多数的研究成果应用于地面行人导航、车载导航以及低空低速飞行器的飞行导航。而行人、车载系统、低速飞行器的速度相对较低，且受到路面环境影响，航向姿态信息变化范围大，可利用的约束信息较少，而高速、超高速飞行器(6-10马赫)的飞行速度大，飞行目的地明确，飞行过程不会出现大角度的航向变化，现有的导航方法不适用于这一飞行特点，对于高速高机动的飞行器，除了导航定位精度是其需考虑的重要参数以外，定位结果的实时性也是间接影响其定位精度的一个重要因素，目前的导航方法尚不能较好地满足上述要求。

发明内容

为解决上述技术问题，本发明针对高速高机动的飞行器的特性，提出一种高速飞行器惯性/地磁匹配搜索区域自适应的组合导航方法，将有助于高速飞行器的高精度和实时性导航定位。在一定程度上会推动高空超高速飞行器的进一步发展。

本发明完整的技术方案包括：

一种高速飞行器惯性/地磁匹配搜索区域自适应的组合导航方法，包括如下步骤：

(1)判断高速飞行器是否飞入地磁图；

(2)粗估计纯惯性定位误差；

(3)确定匹配搜索区域和匹配搜索步长；

(4)获取惯导搜索序列和地磁传感器采集的地磁序列；

(5)最小距离度量及最佳校正位置点的确定；

(6)匹配搜索区域和搜索步长的自适应；

(7)若步骤(6)未完成，则重复上述步骤(3)-(6)，若步骤(6)条件满足，则执行步骤(8)；

(8)重复上述步骤(3)-(5)，直到超高速飞行器飞出地磁图。

所述步骤(1)具体为：

根据飞行器起始的位置点的经纬度坐标A，当前时刻惯导解算的飞行器所在位置C，获得高速飞行器前进方向上的距离S为：

S＝R·cos^-1(sin lat′₁·sin lat′₂·cos(lon′₁-lon′₂)+cos lat′₁ cos lat′₂)

式中，R是高速飞行器的实际飞行距离，lon′₁为A点的经度值，lat′₁为A点的纬度值，lon′₂为C点的纬度值，lat′₂为C点的纬度值；

当S≥S₀时，则认为飞行器飞入了地磁图，S₀为在高速飞行器飞行方向上，初始位置到地磁图的距离。

A点和C点的经度值取值方法为：以0度经线为基准，若为东经则取为正值，若为西经则取为负值；A点和C点的纬度值取值方法为：若在北纬则取90-lat，若在南纬取则90+lat，lat为当时的实际北纬或南纬值。

所述步骤(2)具体为：

计算超高速飞行器垂向纯惯性定位误差为：

δS≈AC·tan(θ)≈AC·θ

其中，式中，δS为粗估计的惯导发散误差；θ为高速飞行器飞入地磁图匹配区域时的航向误差角；

AC＝R′·cos^-1(sin lat′₁·sin lat′₂·cos(lon′₁-lon′₂)+cos lat′₁ cos lat′₂)，R′＝6378137m。

所述步骤(3)具体为：选取粗估计的惯导发散误差δS的1.5倍作为匹配搜索区域的边长，以定位精度要求的0.5倍作为匹配搜索步长。

所述步骤(4)具体为：

超高速飞行器进入地磁图后，将连续采集的5点惯导解算点组成匹配惯导搜索序列：

[(lon_t-4，lat_t-4)(lon_t-3，lat_t-3)(lon_t-2，lat_t-2)(lon_t-1，lat_t-1)(lon_t，lat_t)]

其中，t表示当前时刻，t-1，t-2，t-3，t-4分别表示当前时刻算起，往前连续采集惯导解算点的4个时刻；

获得遍历过程中获取的地磁值数值为：

其中，表示t时刻的惯导点水平平移i个步长，垂直平移j个步长后所在地磁图位置上的地磁值；

获得地磁传感器的测量值序列为：

mag₀＝[M₀ M₁ M₂ M₃ M₄]

其中，M_i表示地磁传感器测量值。

所述步骤(5)具体为：

将步骤(4)所确定的地磁传感器测量序列与惯导平移搜索过程中所组成的序列组对应做差并求和，得：

其中，D_i，j为衡量最佳匹配位置的参数；

得到最小距离度量矩阵：

其中，n₁，n₂分别表示在在垂向方向和水平方向的遍历搜索次数；

获取最佳位置点如下：

D_min＝min(D)

则最小值D_min所对应的地磁图中的位置则为最佳校正位置。

所述步骤(6)具体为：

开始进行惯导解算轨迹校正，若连续多个校正点的距离与飞行器的飞行速度的差值在一定范围内时，则认为已经将匹配轨迹校正到真实航迹附近。判别式如下：

Δs＝[Δs_{t-t_start+i} Δs_{t-t_start+i+1} Δs_{t-t_start+i+2} Δs_{t-t_start+i+3} Δs_{t-t_start+i+4}]

其中，Lat_i表示在i时刻被校正后的惯导解算位置的纬度坐标，Lon_i表示在i时刻被校正后的惯导解算位置的经度坐标，Δs_{t-t_start+i}表示在超高速飞行器飞入地磁图之后，第i次校正点和第i-1次校正点之间的距离；

设对应时间段的飞行器飞行速度为：

v＝[v_t-i v_t-i+1 v_t-i+2 v_t-i+3 v_t-i+4] (14)

则当|v*Δt-Δs|≤ε时，则认为组合导航算法已经对纯惯导解算轨迹校正，并已经校正到真实轨迹附近；

当上式成立后，自动缩短匹配搜索区域，此时，匹配搜索区域的边长选取2倍的匹配定位精度指标大小，搜索步长缩小为匹配定位精度要求的0.3倍。

本发明相对于现有技术的优点在于：

本发明所公开的高速飞行器惯性/地磁匹配搜索区域自适应的组合导航方法，针对高速飞行器的飞行特点和导航定位要求，采用惯性导航+地磁匹配导航相组合的方式，利用地磁传感器测量序列与惯导平移搜索过程中所组成的序列组进行惯导解算轨迹校正，将匹配轨迹校正到真实航迹附近，提高了导航定位精度和定位结果的实时性，有助于高速飞行器的高精度和实时性导航定位。

附图说明

图1是本发明惯性/地磁组合导航的匹配搜索区间和步长自适应方法流程示意图。

图2是本发明高速飞行器的惯性/地磁组合导航原理示意图。

图3是惯性误差特性的分析示意图。

具体实施方式

下面结合附图和具体实施方式对本发明做进一步说明。

如图2所示，由于高速飞行器的飞行时间长，飞行速度快、可获取的高精度的地磁场数据有限，再结合高速飞行器的制导精度需求，只需在飞行器飞行过程中对导航定位结果进行校正，即可满足飞行器飞行过程中的定位需求。即当判断高速飞行器飞入地磁图区域时，开启地磁传感器，开始采集飞行器所飞行过程中的地磁数据，同时根据惯导误差特性粗估计惯导的发散误差，然后根据发散误差的粗估计值确定匹配搜索区域。然后，以5个采样点为一个序列组，将惯导解算的序列在匹配搜索区域内遍历，将遍历过程中惯导点所处位置的地磁值读取出来，并与地磁传感器采集的地磁序列做最小距离度量。相似度最接近的匹配轨迹作为组合导航的定位结果，并对纯惯导解算进行位置校正。当判断连续多个校正轨迹点的距离与该段时间内的速度的差值在一定阈值范围内时，则认为匹配准确，则自动缩短匹配搜索范围和匹配搜索步长，以提高匹配定位精度和匹配时间，以及避免匹配搜索区域过大，对测量结果的干扰。

下面对本发明惯性/地磁组合导航方法的实现步骤进行详细说明。具体为包括如下步骤：

步骤1：判断高速飞行器是否飞入地磁图

由于超高速飞行器所采用的惯性器件具有高精度的航向信息，且其机动性大，其飞行轨迹在水平面的投影近似为直线。因此，可根据其飞行过程的航向信息粗略估计其飞行方向上的距离。为验证其飞行方向上的估算精度，需对惯性误差特性进行分析。

如图3所示，A点表示飞行器起始的位置点的经纬度坐标，C点表示当前时刻惯导解算的飞行器所在位置，R是高速飞行器实际飞行距离；θ表示高速飞行器飞入地磁图匹配区域时的航向误差角，误差源为惯导初始失准角和陀螺零偏。由图可以得出，惯导在解算过程中的X轴与Y轴方向惯性系统误差Δx和Δy分别为：

Δx＝R(1-cos²θ) (1)

Δy＝0.5R sin(2θ) (2)

由式(1)和式(2)可得出惯导发散误差在垂直方向和飞行方向误差的比例Δ如下：

因为高精度惯性器件的航向失准角是一个小量，所以从式(3)可知，超高速飞行器飞行过程中的误差主要集中在垂向方向。因此，超高速飞行器飞行方向的距离的估计是可信的。

以0度经线为基准，取东经为正值，取西经为负值，北纬取90-lat，南纬取90+lat，则根据经过上述处理过后的两点A(lon′₁，lat′₁)和C(lon′₂，lat′₂)。飞行器前进方向上的距离粗估计为：

S＝R·cos^-1(sin lat′₁·sin lat′₂·cos(lon′₁-lon′₂)+cos lat′₁ cos lat′₂) (3)

其中，S表示粗估计的飞行器前进方向的飞行距离，A(lon′₁，lat′₁)和C(lon′₂，lat′₂)分别表示飞行器起始的位置点的经纬度坐标和当前时刻惯导解算的飞行器所在位置；

设在超高速飞行器飞行方向上，初始位置到地磁图的距离为S0，则当S≥S0时，则认为飞行器飞入了地磁图。

步骤2：纯惯导误差的粗估计

根据步骤1分析可知，超高速飞行器的纯惯性定位误差主要来源于飞行方向的垂向方向上。因此，估计垂向误差为：

δS≈AC·tan(θ)≈AC·θ (4)

其中，δS表示粗估计的惯导发散误差。

AC两点的距离公式如下：

AC＝R′·cos^-1(sin lat′₁·sin lat′₂·cos(lon′₁-lon′₂)+cos lat′₁ coslat′₂) (5)

其中，R′＝6378137m，为地球半径。

步骤3：匹配搜索区域和匹配搜索步长的确定

为了保证匹配搜索区域内包含超高速飞行器飞行过的轨迹，选取粗估计的惯导发散误差δS的1.5倍作为匹配搜索区域的边长，以定位精度要求的0.5倍作为匹配搜索步长。

步骤4：获取惯导搜索序列和地磁传感器采集的地磁序列

当判断超高速飞行器进入地磁图后，将连续采集的5点惯导解算点组成匹配搜索序列组：

[(lon_t-4，lat_t-4) (lon_t-3，lat_t-3) (lon_t-2，lat_t-2) (lon_t-1，lat_t-1) (lon_t，lat_t)] (6)

其中，t表示当前时刻，t-1，t-2，t-3，t-4分别表示当前时刻算起，往前连续采集惯导解算点的4个时刻。

设遍历过程中获取的地磁值数值为：

其中，表示t时刻的惯导点水平平移i个步长，垂直平移j个步长后所在地磁图位置上的地磁值。

则地磁传感器的测量值序列为：

mag₀＝[M₀ M₁ M₂ M₃ M₄] (8)

其中，M_i表示地磁传感器测量值。

步骤5：最小距离度量及最佳校正位置点的确定

将步骤4所确定的地磁传感器测量序列与惯导平移搜索过程中所组成的序列组对应做差并求和，可得：

其中，D_i,j作为衡量最佳匹配位置的参数。

则最小距离度量矩阵可写为：

其中，n₁，n₂分别表示在在垂向方向和水平方向的遍历搜索次数。

最佳位置点的获取如下：

D_min＝min(D) (11)

则最小值所对应的地磁图中的位置则为最佳校正位置。

步骤6：匹配搜索区域和搜索步长的自适应

当开始进行惯导解算轨迹校正时，若连续多个校正点的距离与飞行器的飞行速度的差值在一定范围内时，则认为已经将匹配轨迹校正到真实航迹附近。判别式如下：

Δs＝[Δs_{t-t_start+i} Δs_{t-t_start+i+1} Δs_{t-t_start+i+2} Δs_{t-t_start+i+3} Δs_{t-t_start+i+4}] (13)

其中，Lat_i表示在i时刻被校正后的惯导解算位置的纬度坐标，Lon_i表示在i时刻被校正后的惯导解算位置的经度坐标，t_start是指采集惯导解算点的开始时刻，Δs_{t-t_start+i}表示在超高速飞行器飞入地磁图之后，第i次校正点和第i-1次校正点之间的距离。

设对应时间段的飞行器飞行速度为：

v＝[v_t-i v_t-i+1 v_t-i+2 v_t-i+3 v_t-i+4] (14)

则当|v*Δt-Δs|≤ε时，则认为组合导航算法已经对纯惯导解算轨迹校正，并已经校正到真实轨迹附近，式中Δt为惯导解算点采样的时间间隔，ε为匹配定位准确的判定阈值，具体值可根据实际情况自行设定。

当上式成立后，自动缩短匹配搜索区域，此时，匹配搜索区域的边长选取2倍的匹配定位精度指标大小，搜索步长缩小为匹配定位精度要求的0.3倍。

步骤7：若步骤6判别式不成立，则重复上述步骤3-6，若步骤6条件满足，则执行步骤8。

步骤8：重复上述步骤3-5，直到超高速飞行器飞出地磁图。

以上所述，仅是本发明的较佳实施例，并非对本发明作任何限制，凡是根据本发明技术实质对以上实施例所作的任何简单修改、变更以及等效结构变化，均仍属于本发明技术方案的保护范围内。

Claims (2)

1.一种高速飞行器惯性/地磁匹配搜索区域自适应的组合导航方法，其特征在于，包括如下步骤：

(1)判断高速飞行器是否飞入地磁图；具体为：

根据飞行器起始的位置点的经纬度坐标A，当前时刻惯导解算的飞行器所在位置C，获得高速飞行器前进方向上的距离S为：

S＝R·cos^-1(sinlat′₁·sinlat′₂·cos(lon′₁-lon′₂)+coslat′₁coslat′₂)

式中，R是高速飞行器的实际飞行距离，lon′₁为A点的经度值，lat′₁为A点的纬度值，lon′₂为C点的纬度值，lat′₂为C点的纬度值；

当S≥S₀时，则认为飞行器飞入了地磁图，S₀为在高速飞行器飞行方向上，初始位置到地磁图的距离；

(2)粗估计纯惯性定位误差；具体为：

计算超高速飞行器垂向纯惯性定位误差为：

δS≈AC·tan(θ)≈AC·θ

其中，式中，δS为粗估计的惯导发散误差；θ为高速飞行器飞入地磁图匹配区域时的航向误差角；

AC＝R′·cos^-1(sin lat′₁·sin lat′₂·cos(lon′₁-lon′₂)+cos lat′₁cos lat′₂)，R′＝6378137m；

(3)确定匹配搜索区域和匹配搜索步长；具体为：选取粗估计的惯导发散误差δS的1.5倍作为匹配搜索区域的边长，以定位精度要求的0.5倍作为匹配搜索步长；

(4)获取惯导搜索序列和地磁传感器采集的地磁序列；具体为：

超高速飞行器进入地磁图后，将连续采集的5点惯导解算点组成匹配惯导搜索序列：

[(lon_t-4,lat_t-4)(lon_t-3,lat_t-3)(lon_t-2,lat_t-2)(lon_t-1,lat_t-1)(lon_t,lat_t)]

其中，t表示当前时刻，t-1，t-2，t-3，t-4分别表示当前时刻算起，往前连续采集惯导解算点的4个时刻；

获得遍历过程中获取的地磁值数值为：

其中，表示t时刻的惯导点水平平移i个步长，垂直平移j个步长后所在地磁图位置上的地磁值；

获得地磁传感器的测量值序列为：

mag₀＝[M₀ M₁ M₂ M₃ M₄]

其中，M_i表示地磁传感器测量值；

(5)最小距离度量及最佳校正位置点的确定；具体为：

将步骤(4)所确定的地磁传感器测量序列与惯导平移搜索过程中所组成的序列组对应做差并求和，得：

其中，D_i,j为衡量最佳匹配位置的参数；

得到最小距离度量矩阵：

其中，n₁,n₂分别表示在在垂向方向和水平方向的遍历搜索次数；

获取最佳位置点如下：

D_min＝min(D)

则最小值D_min所对应的地磁图中的位置则为最佳校正位置；

(6)匹配搜索区域和搜索步长的自适应；具体为：

开始进行惯导解算轨迹校正，若连续多个校正点的距离与飞行器的飞行速度的差值在一定范围内时，则认为已经将匹配轨迹校正到真实航迹附近，判别式如下：

Δs＝[Δs_{t-t_start+i} Δs_{t-t_start+i+1} Δs_{t-t_start+i+2} Δs_{t-t_start+i+3} Δs_{t-t_start+i+4}]

其中，Lat_i表示在i时刻被校正后的惯导解算位置的纬度坐标，Lon_i表示在i时刻被校正后的惯导解算位置的经度坐标，Δs_{t-t_start+i}表示在超高速飞行器飞入地磁图之后，第i次校正点和第i-1次校正点之间的距离；

设对应时间段的飞行器飞行速度为：

v＝[v_t-i v_t-i+1 v_t-i+2 v_t-i+3 v_t-i+4] (14)

则当|v*Δt-Δs|≤ε时，则认为组合导航算法已经对纯惯导解算轨迹校正，并已经校正到真实轨迹附近，式中Δt为惯导解算点采样的时间间隔，ε为匹配定位准确的判定阈值；

当上式成立后，自动缩短匹配搜索区域，此时，匹配搜索区域的边长选取2倍的匹配定位精度指标大小，搜索步长缩小为匹配定位精度要求的0.3倍；

(7)若步骤(6)未完成，则重复上述步骤(3)-(6)，若步骤(6)条件满足，则执行步骤(8)；

(8)重复上述步骤(3)-(5)，直到超高速飞行器飞出地磁图。

2.根据权利要求1所述的一种高速飞行器惯性/地磁匹配搜索区域自适应的组合导航方法，其特征在于，A点和C点的经度值取值方法为：以0度经线为基准，若为东经则取为正值，若为西经则取为负值；A点和C点的纬度值取值方法为：若在北纬则取90-lat，若在南纬取则90+lat，lat为当时的实际北纬或南纬值。