CN109084752B - 一种基于全连通约束的地磁导航定位方法 - Google Patents

Abstract

本发明提出一种基于全连通约束的地磁导航定位方法，首先建立导航区域内的地磁数据库；根据设定的运载体起始位置和终止位置，得到起始位置和终止位置的磁场总强，并确定运载体的起始航向角；在每个计算周期内，采集当前位置地磁总强，判断当前位置地磁总强与地磁数据库是否唯一对应，如果唯一对应，则由地磁数据库获取当前位置，否则基于全连通的四点定位方法实现在磁图中唯一确定与目标值匹配的位置作为当前位置；确定当前位置后，判断当前位置与终止位置的距离是否小于一个运动步长，若是则结束导航，否则计算当前时刻的航向角并继续下一计算周期。本发明通过四点定位以保证地磁数据与坐标位置的唯一对应关系，从而提高地磁导航定位的准确性。

Description

一种基于全连通约束的地磁导航定位方法

技术领域

本发明涉及一种基于全连通约束的地磁导航定位方法，属于地磁导航技术领域。

背景技术

地磁导航是利用地磁图来实现的一种新型的导航定位技术，地磁匹配算法是地磁导航的核心。地磁匹配算法的本质是一种搜索的导航算法，其基本原理是：在某一个区域内建立地磁数据库，通过固定在运载体上的地磁传感器测量地磁值，并将该地磁信息与存储在计算机中的地磁数据库相匹配，从而获得运载体的具体位置。该技术能够有效地改善传统导航系统技术的缺陷，既能避免惯性导航系统随时间变化存在累积误差从而使得导航精度下降的不足，又能克服卫星导航技术中受环境干扰严重等问题。

地磁匹配算法作为一种重要的地磁导航算法，能够获得相对精度较高的位置信息，而且在一定程度上可以弥补实测数据精度不足的缺陷。可以将二次插值技术应用到地磁匹配算法中，但是通过匹配得到的位置往往由于传感器测得的地磁数据与地磁数据库中的位置不能唯一对应，容易产生较高的误匹配率，从而无法实现准确的导航定位。

发明内容

本发明针对地磁匹配导航过程中，仅使用磁场总强作为唯一的地磁参量时，由于等磁线的存在，会出现地磁值与坐标位置存在不一一对应的问题，基于图论的相关理论，提出了一种基于全连通约束的地磁导航定位方法，通过四点定位以保证地磁数据与坐标位置的唯一对应关系，从而提高地磁导航定位的准确性。

本发明的技术方案为：

所述一种基于全连通约束的地磁导航定位方法，其特征在于：包括以下步骤：

步骤1：在导航区域内，根据若干已知点的磁场总强，利用插值方法，建立该导航区域内的地磁数据库；

步骤2：在地磁坐标系二维平面内，建立运载体的运动方程为：

其中L＝V×ΔT，表示运载体的运动步长；V表示载体在ΔT时间内做匀速运动的运动速度，ΔT为采样周期；k表示载体运动的某一时刻，θ_k为k时刻的航向角，(x_k-1,y_k-1)为k-1时刻运载体的位置坐标，(x_k,y_k)为k时刻运载体的位置坐标；

步骤3：根据设定的运载体起始位置(x₀,y₀)和终止位置(x_t,y_t)，由步骤1中的地磁数据库得到起始位置和终止位置的磁场总强B₀和B_t，并确定运载体的起始航向角

运载体从起始位置(x₀,y₀)按照起始航向角θ₀前进一个运动步长；

步骤4：采集当前位置地磁总强，判断当前位置地磁总强与地磁数据库中的地磁总强是否唯一对应，如果唯一对应，则根据地磁数据库获取当前位置坐标，否则通过以下过程确定当前位置坐标：

步骤4.1：运载体从当前位置开始沿平行于地磁坐标系的坐标轴方向，行走一个边长为的L的正方形ABCD，并得到正方形ABCD四个顶点的地磁总强：B₀,B₁,B₂,B₃；

步骤4.2：在地磁数据库中查找所有与B₀,B₁,B₂,B₃匹配的地磁总强，分别记为

其中i＝1,2,...,m；j＝1,2,...,n；k＝1,2,...,p；l＝1,2,...,q，m,n,p,q分别为与B₀,B₁,B₂,B₃相同的地磁总强的个数，将每个

地磁总强对应的具体位置记为：

步骤4.3：通过遍历计算步骤4.2得到的具体位置中，每相邻两个点之间的距离：

共m*n个；

共n*p个

共p*q个

共q*m个

并选择得到满足相邻两点之间的距离与正方形边长L相等的所有位置；

步骤4.4：在步骤4.3得到的所有位置中，通过多点连通性，由四点约束的条件唯一确定出一个正方形，并以该正方形的起点位置作为当前位置；

步骤5：确定当前位置后，判断当前位置与终止位置的距离是否小于一个运动步长，若是，则结束导航，若不是，则进入步骤6；

步骤6：根据当前位置坐标和设定的终止位置(x_t,y_t)，计算当前时刻的航向角，并以该航向角前进一个运动步长后，返回步骤4。

有益效果

本发明提出一种基于全连通约束的地磁导航定位方法，通过四点定位以保证地磁数据与坐标位置的唯一对应关系，从而提高地磁导航定位的准确性。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1全连通约束的地磁导航定位算法流程图；

图2运载体的移动路线及地磁信息与位置信息匹配；

图3利用三轮移动机器人搭建的运载体实物图；

图4某一导航区域内地磁背景环境三维图及二维等值线图；地磁总强单位为：μT；

图5地磁多值点分布；

图6基于全连通约束唯一确定的四点定位结果；

图7基于全连通约束的地磁导航结果。

具体实施方式

下面详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

本发明针对地磁匹配导航过程中，仅使用磁场总强作为唯一的地磁参量时，由于等磁线的存在，会出现地磁值与坐标位置存在不一一对应的问题，基于图论的相关理论，提出了一种基于全连通约束的地磁导航定位方法，通过四点定位以保证地磁数据与坐标位置的唯一对应关系，从而提高地磁导航定位的准确性。

该方法包括以下步骤：

步骤1：在导航区域内，根据若干已知点的磁场总强，利用插值方法，建立该导航区域内的地磁数据库；地磁背景环境三维图及二维等值线图如附图4所示。

步骤2：在地磁坐标系二维平面内，根据运载体与目标位置之间的几何关系确定载体的运动学方程，

式中，k表示载体运动的某一时刻，v_k为载体在k时刻的运动速度，θ_k为k时刻的航向角，ΔT为采样周期，(x_k-1,y_k-1)为k-1时刻运载体的位置坐标，(x_k,y_k)为k时刻运载体的位置坐标。当载体在ΔT时间内做匀速运动，即v_k可以用一个常量V表示，则式(1)可以简化如下：

式中，L＝V×ΔT，表示载体的运动步长。

步骤3：初始化：根据设定的运载体起始位置(x₀,y₀)和终止位置(x_t,y_t)，由步骤1中的地磁数据库得到起始位置和终止位置的磁场总强B₀＝56.5μT和B_t＝62.9μT，并确定运载体的起始航向角

运载体从起始位置(x₀,y₀)按照起始航向角θ₀前进一个运动步长。

步骤4：采集当前位置地磁总强，判断当前位置地磁总强与地磁数据库中的地磁总强是否唯一对应，如果唯一对应，则根据地磁数据库获取当前位置坐标，否则，如附图5所示，通过以下基于全连通的四点定位方法实现在磁图中唯一确定一个与目标值匹配的位置作为当前位置：

步骤4.1：运载体从当前位置开始沿平行于地磁坐标系的坐标轴方向，如图2所示行走一个边长为的L的正方形ABCD，并得到正方形ABCD四个顶点的地磁总强：B₀,B₁,B₂,B₃；

步骤4.2：在地磁数据库中查找所有与B₀,B₁,B₂,B₃匹配的地磁总强，分别记为

其中i＝1,2,...,m；j＝1,2,...,n；k＝1,2,...,p；l＝1,2,...,q，m,n,p,q分别为与B₀,B₁,B₂,B₃相同的地磁总强的个数，将每个

地磁总强对应的具体位置记为：

步骤4.3：通过遍历计算步骤4.2得到的具体位置中，每相邻两个点之间的距离：

共m*n个；

共n*p个

共p*q个

共q*m个

并选择得到满足相邻两点之间的距离与正方形边长L相等的所有位置；

步骤4.4：为了保证定位点的唯一性，在步骤4.3得到的所有位置中，通过多点连通性，由四点约束的条件，即在地磁坐标系中，x_A＝x_B,x_C＝x_D,y_A＝y_D,y_B＝y_c，唯一确定出一个正方形，并以该正方形的起点位置作为当前位置。如附图6所示。

步骤5：确定当前位置后，判断当前位置与终止位置的距离是否小于一个运动步长，若是，则结束导航，若不是，则进入步骤6；

步骤6：根据当前位置坐标和设定的终止位置(x_t,y_t)，计算当前时刻的航向角，并以该航向角前进一个运动步长后，返回步骤4。

由导航定位的路径轨迹附图7可以看出，该算法可以解决由于地磁值与位置坐标的多值问题导致在地磁导航过程中位置无法唯一确定的缺陷，通过全连通约束的四点定位地磁导航定位可以实现导航目的，从而验证了该发明的有效性。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (1)

1.一种基于全连通约束的地磁导航定位方法，其特征在于：包括以下步骤：

步骤1：在导航区域内，根据若干已知点的磁场总强，利用插值方法，建立该导航区域内的地磁数据库；

步骤2：在地磁坐标系二维平面内，建立运载体的运动方程为：

其中L＝V×ΔT，表示运载体的运动步长；V表示载体在ΔT时间内做匀速运动的运动速度，ΔT为采样周期；k表示载体运动的某一时刻，θ_k为k时刻的航向角，(x_k-1,y_k-1)为k-1时刻运载体的位置坐标，(x_k,y_k)为k时刻运载体的位置坐标；

步骤3：根据设定的运载体起始位置(x₀,y₀)和终止位置(x_t,y_t)，由步骤1中的地磁数据库得到起始位置和终止位置的磁场总强B₀和B_t，并确定运载体的起始航向角

运载体从起始位置(x₀,y₀)按照起始航向角θ₀前进一个运动步长；

步骤4：采集当前位置地磁总强，判断当前位置地磁总强与地磁数据库中的地磁总强是否唯一对应，如果唯一对应，则根据地磁数据库获取当前位置坐标，否则通过以下过程确定当前位置坐标：

步骤4.1：运载体从当前位置开始沿平行于地磁坐标系的坐标轴方向，行走一个边长为的L的正方形ABCD，并得到正方形ABCD四个顶点的地磁总强：B₀,B₁,B₂,B₃；

步骤4.2：在地磁数据库中查找所有与B₀,B₁,B₂,B₃匹配的地磁总强，分别记为

其中i＝1,2,...,m；j＝1,2,...,n；k＝1,2,...,p；l＝1,2,...,q，m,n,p,q分别为与B₀,B₁,B₂,B₃相同的地磁总强的个数，将每个

地磁总强对应的具体位置记为：

步骤4.3：通过遍历计算步骤4.2得到的具体位置中，每相邻两个点之间的距离：

共m*n个；

共n*p个

共p*q个

共q*m个

并选择得到满足相邻两点之间的距离与正方形边长L相等的所有位置；

步骤4.4：在步骤4.3得到的所有位置中，通过多点连通性，由四点约束的条件唯一确定出一个正方形，并以该正方形的起点位置作为当前位置；

步骤5：确定当前位置后，判断当前位置与终止位置的距离是否小于一个运动步长，若是，则结束导航，若不是，则进入步骤6；

步骤6：根据当前位置坐标和设定的终止位置(x_t,y_t)，计算当前时刻的航向角，并以该航向角前进一个运动步长后，返回步骤4。

Images