[发明专利]一种基于粒子滤波算法的地磁_INS组合导航方法

权利要求书

1.一种基于粒子滤波算法的地磁_INS组合导航方法，其特征在于，包括以下步骤：

步骤S1：通过一个三轴地磁传感器模块和一个惯性器件模块采集地磁信息；

步骤S2：根据采集的地磁信息建立离散地磁数据库并在地磁图上获取一运动轨迹；

步骤S3：利用导数动态时间规整算法进行趋势匹配，得到一定位结果；

步骤S4：通过惯性导航(inertial navigation system，INS)解算的运动轨迹进行辅助地磁匹配定位解算，得到另一定位结果；

步骤S5：采用粒子滤波算法对步骤S3的定位结果和步骤S4的定位结果进行融合，得到地磁导航最终的定位估计；

所述步骤S4中，通过INS解算的运动轨迹进行辅助地磁匹配定位解算的具体方法为：采用滑动窗口的形式对地磁序列进行整合，滑动窗口的长度主要由经验值得到，通过轮数计解算的里程信息缩小离散地磁数据库中待匹配的地磁场强序列范围，采用滑动匹配的方式进行定位解算，当匹配条件成立的时候，即输出匹配的定位位置，否则地磁数据库场强序列的窗口向左或向右滑动。

2.根据权利要求1所述的一种基于粒子滤波算法的地磁_INS组合导航方法，其特征在于，所述步骤S3中，利用导数动态时间规整算法进行趋势匹配的具体方法为：在地磁匹配中，通过考虑序列的一阶导数来获取形状的相关信息，将序列的形态特征考虑进来，导数动态时间规整算法(derivative dynamic time warping，DDTW)的实现是通过构造一个n×m的矩阵，其中矩阵中的(i^th,j^th)参数包含两条时间序列上的点p_i和q_j的对于的D_x[p]和D_x[q]导数之差的平方，这一点与DTW不同，用以下计算式进行计算：

计算基于导数估计的局部距离为：

d(i,j)＝(D(p_i)-D(q_j))²

由上式得出，该导数估计值D_x[p]通过该点p_i与该点左邻点的直线斜率以及通过该点左邻点和右邻点的直线斜率的平均值，DDTW对时间序列的第二个节点和倒数第二个节点进行估计，地磁匹配之前将待匹配的地磁序列进行平滑处理，采用广义延拓拟合的办法，DDTW的时间复杂度为O(mn)。

3.根据权利要求1或2所述的一种基于粒子滤波算法的地磁_INS组合导航方法，其特征在于，所述步骤S5中，采用粒子滤波算法进行地磁导航最终定位估计时，包括以下步骤：

步骤S51：初始化，根据先验条件概率抽取随机样本其中N为样本个数；

步骤S52：预测过程，由系统的状态方程和随机样本得到进一步预测样本

步骤S53：更新过程，由k时刻的测量值z_k和测量值的预测值z'_k计算每个粒子的权值，并将其归一化得w，其中i＝1,2…N；

步骤S54：初步状态估计，由粒子和其权重来计算状态的估计值并利用该时刻的粒子和他们的权重来进行重采样，从而得到下一时刻的粒子样本，返回步骤S52；

步骤S55：依据重采样得到的索引去挑选对应的粒子，重构的集合便是滤波的状态集合，对这个状态集合求均值，得到最终的目标状态。

4.根据权利要求1所述的一种基于粒子滤波算法的地磁_INS组合导航方法，其特征在于，所述导数动态时间规整算法是一种趋势匹配算法，DDTW没有对时间序列的第一个数据节点和最后一个数据节点进行估计，而采用第二个节点和倒数第二个节点进行估计。

5.根据权利要求1所述的一种基于粒子滤波算法的地磁_INS组合导航方法，其特征在于，所述惯性导航解算的运动轨迹进行辅助地磁匹配定位解算，需要采用滑动窗口的形式对地磁序列整合，滑动窗口的长度主要由经验值得到，通过轮数计解算的里程信息缩小离散地磁数据库中待匹配的地磁场强序列范围，采用滑动匹配的方式进行定位解算，该方法可以降低匹配过程中的计算复杂度。