[发明专利]一种基于全连通约束的地磁导航定位方法

权利要求书

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。