CN111964667A - 一种基于粒子滤波算法的地磁_ins组合导航方法 - Google Patents

Abstract

本发明公开了一种基于粒子滤波算法的地磁_INS组合导航方法，包括以下步骤：步骤S2：根据采集的地磁信息建立离散地磁数据库并在地磁图上获取一运动轨迹；步骤S3：利用导数动态时间规整算法进行趋势匹配，得到一定位结果；步骤S4：通过惯性导航解算的运动轨迹进行辅助地磁匹配定位解算，得到另一定位结果；步骤S5：采用粒子滤波算法对步骤S3的定位结果和步骤S4的定位结果进行融合，得到地磁导航最终的定位估计；本发明采用导数动态时间规整方法进行趋势匹配，与点对点匹配的算法相比误差小、精确性更高；惯性导航解算的运动轨迹进行辅助地磁匹配定位解算，采用粒子滤波方法对两者的定位结果融合，定位估计更为精确。

Description

一种基于粒子滤波算法的地磁_INS组合导航方法

技术领域

本发明涉及地磁导航技术领域，尤其涉及一种基于粒子滤波算法的地磁_INS组合导航方法。

背景技术

随着21世纪物联网的不断发展和智能终端的普及，用户对基于位置的服务(LBS)的需求持续增长。用户渴望在各种地理位置中获得具有更高定位精度的位置信息服务。当前，有各种可用的导航方法。卫星导航系统是更受欢迎且非常重要的方法，例如北斗导航系统。GPS导航系统，GLONASS导航系统和其他卫星导航系统可以全天候提供导航和定位服务，从而为用户提供高精度的位置信息。由于智能手机和互联网的全球普及，卫星导航和定位服务已深深影响着人们的日常生活，同时影响了社会和经济发展。但是，在某些特殊环境中，例如深山谷，城市高层建筑集中的区域，百货商店，封闭式体育馆，管道走廊和地下车库，当卫星信号被遮挡时，卫星定位会有更大的错误，甚至无法定位，或者在发生信号干扰和战争以及其他恶劣情况时，卫星可能无法提供正常的定位服务，并且可能会输出错误的定位信息。但是，在上述卫星信号受遮挡影响的背景环境中，通常是用户非常想定位并获得定位服务的区域。为了考虑国防安全和一些民用领域的需求，对新型导航定位技术的研究和探索已成为导航定位领域研究的热点。

地磁导航是目前定位研究热点技术之一，利用地磁场的特征，地磁场是矢量场，是空间位置的函数。目前，全球本地的地磁场数学模型和数据库正在变得越来越完善。高灵敏度，高可靠性，小尺寸和低成本的地磁传感器相继出现。扩展卡尔曼滤波器(EKF)和无痕卡尔曼滤波器(UKF)，粒子滤波和其他滤波技术以及地磁匹配算法正在变得成熟，因此您可以使用近地空间中任意位置的唯一磁场矢量之间的映射关系来实现地磁导航。地磁导航技术不需要大量的设备成本，没有辐射，成本低廉，隐蔽性强，并且误差不会随时间累积。它已逐渐应用于海洋，陆地，空中和天空领域。

由于地磁导航中的地磁匹配相较于地磁滤波易于实现，在各个导航领域的应用前景较好，通常地磁导航往往不是作为一个单一使用的导航技术开展应用，大多数是辅助惯性器件进行导航应用，惯性器件为其提供里程信息和运动方向，所以地磁导航往往与惯性导航一起使用。在地磁导航与惯性器件的组合定位精度相比较于纯粹的惯性导航来说，精度要高很多，这样是有益于导航系统的应用普及的。

在当前的地磁匹配导航中，地磁辅助惯性组合导航具有完全自主，全天候，全区域，无源，无辐射等优点，是一种具有巨大潜力的新型组合导航方法。外部定位和航空导航领域具有广阔的应用前景，地磁辅助惯性组合导航的应用应突破组合导航系统滤波的关键技术。

经典卡尔曼滤波建立在两个重要的假设的基础上，一是系统的状态方程可以使用线性方程完全表达，二是噪声符合高斯噪声分布，这在工程中并不是一件容易得到满足的事情，首先建立完全准备的状态模型是一件比较困难的事，特别是传感器采样频率较低时，模型误差显得就比较大，而传感器的噪声实际上并不严格符合高斯噪声分布。这种情况下经典卡尔曼滤波的去噪效果就非常有限了。扩展卡尔曼滤波虽然在处理非线性问题时比经典卡尔曼滤波有了很大程度的改进但忽略二阶以上的高阶项会给结果带来影响，同时雅克比矩阵的计算将耗费大量的时间特别是在维数较高的情况下，数据的运算量就更大。这对于导航滤波而言显然是一个非常不利的影响，毕竟导航的实时性也是一项十分重要的要求。为了获得较好的非线性处理能力，并且避免雅克比矩阵的计算，无迹卡尔曼滤波方法被提了出来。无迹卡尔曼(unscented kalman filter,UKF)，是一种基于UnscentedTransformation(UT)变换的非线性滤波方法。

在上述滤波方法中，都需要假设系统噪声和量测噪声是高斯噪声、并且都对线性系统进行了不同形式的线性化，这在实际中不容易实现。

例如，一种在中国专利文献上公开的“一种基于扩展卡尔曼滤波的导航方法及装置”，其公告号：CN102519463A，其申请日：2011年12月13日，该方法通过扩展卡尔曼滤波算法进行导航定位，扩展卡尔曼滤波虽然在处理非线性问题时比经典卡尔曼滤波有了很大程度的改进但忽略二阶以上的高阶项会给结果带来影响，同时雅克比矩阵的计算将耗费大量的时间特别是在维数较高的情况下，数据的运算量就更大。

发明内容

本发明主要解决现有的技术中地磁导航误差大、定位不准确的问题；提供一种基于粒子滤波算法的地磁_INS组合导航方法，采用滤波技术对定位误差进行校正，得到较为精确的定位估计。

本发明的上述技术问题主要是通过下述技术方案得以解决的：一种基于粒子滤波算法的地磁_INS组合导航方法，包括以下步骤：

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

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

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

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

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

所述的三轴地磁传感器模块由于它所测量的地磁场强度是矢量强度，因此在建立离散数据库的过程当中，选定待导航区域，在区域进行网格划分，应该注意地图上测量点的方向应该保持一致，保证地磁信息在空间的唯一性，为在线匹配过程做好地磁信息序列匹配的保障，所述的包含陀螺仪和加速度计的惯性器件模块主要是用于在线地磁匹配定位过程中，在地图上画出运动轨迹为三轴地磁传感器模块所测量的待匹配的地磁信息序列做指引，在初始定位后，根据其所得的方向角和前进加速度做航位推算，在地图上画出惯性导航的轨迹图；地磁数据库采集条件：在采集地磁数据时，要确保测量点与测量点之间的间隔一致，确保该间隔在一定范围内，该间隔不应太大，否则会影响地磁数据库的准确性。同时，在建立库时，必须确保三轴地磁传感器模块的测量方向一致。在收集地磁数据的过程中，在测量点收集一组数据，并用拉伊达准则将总误差去除，将测量点的值取平均后，得到一个测量点的数据，然后利用广义延拓逼近法对网格中的数据点进行最佳分割和细分，建立高精度地磁数据库。

作为优选，所述的步骤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)。

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

作为优选，所述的步骤S5中，采用粒子滤波算法进行地磁导航最终定位估计时，包括以下步骤：

步骤S51：初始化，根据先验条件概率抽取随机样本

其中N为样本个数；

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

得到进一步预测样本

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

步骤S54：初步状态估计，由粒子和其权重来计算状态的估计值

并利用该时刻的粒子和他们的权重来进行重采样，从而得到下一时刻的粒子样本，返回步骤S52；

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

作为优选，所述的导数动态时间规整算法是一种趋势匹配算法，DDTW没有对时间序列的第一个数据节点和最后一个数据节点进行估计，而采用第二个节点和倒数第二个节点进行估计。

作为优选，所述的惯性导航解算的运动轨迹进行辅助地磁匹配定位解算，需要采用滑动窗口的形式对地磁序列整合，滑动窗口的长度主要由经验值得到，通过轮数计解算的里程信息缩小离散地磁数据库中待匹配的地磁场强序列范围，采用滑动匹配的方式进行定位解算，该方法可以降低匹配过程中的计算复杂度。

作为优选，所述的对两者的定位结果融合的滤波算法，主要是在面对数值方法不容易解决的问题时，用类似统计采样的方法来近似的估算出其概率，通过进一步计算得到一个最优近似解。

作为优选，所述的离散地磁数据库的建立方法为：保证在采集地磁数据的过程中，测量点与测量点之间的间隔保持一致，间隔不宜过大，否则会影响到地磁数据库的精度，在建库的同时保证三轴地磁传感器模块的测量方向保持一致，在测量点采集一组数据后，通过拉依达准则去除粗大误差，再对该测量点的数值求平均，就得到一个测量点的数据，最后通过广义延拓逼近的方法将网格中的数据点进行分片最佳逼近，建立高精度的细分地磁数据库。

作为优选，所述的广义延拓逼近方法为：设Ψ为定义域，把其区分成n个互不重叠的单元子域Ψ_e(e＝1,2,…,n)，则有：

把一定程度扩大后的延拓域记为Ψ′_e，则

且有

设Ψ′_e内有m个节点，其中属于Ψ_e的节点有r个(r<m)，在延拓域Ψ'_e上构造逼近函数时，为使单元Ψ_e边界上函数值与各邻近单元函数值相一致，单元Ψ_e的m个边界点应满足边界点上插值限制因素；最后将每个延拓域中求得的逼近函数仅取单元定义域上的那部分，然后拼接起来，构成整个定义域的逼近函数；

设延拓域上Ψ'_e的逼近函数为：

其中，t为逼近函数得的项数，(h₁,h₂,…,h_p)是Ψ_e上的一组基函数；ε_i(i＝1,2,…p)为待定系数；对延拓域Ψ'_e的逼近函数实行在单元域Ψ_e的边界点上符合插值条件，而在其它点作最小二乘拟合处理，即可得单元域内的逼近函数的待定系数；

求出每个单元域内的逼近函数后，拼接可得整个定义域的逼近函数：

作为优选，所述的运动轨迹的获取方法为：在二维地磁图上运动一段距离，得到一段地磁数据序列(m₁,m₂,…,m_k)，以及用惯性器件模块测得一组加速度(a₁,a₂,…,a_k)和方位角数据序列(θ₁,θ₂,…,θ_k)；通过航位推算将运动轨迹在地磁图中画出来；

所述航位推算方法为：假设在t_n时刻，已知位置

和当前时刻运动速度

及航向角

这时通过航位推算求t_n+1时刻位置

计算式为：

本发明的有益效果是:(1)本发明采用导数动态时间规整方法进行趋势匹配，与点对点匹配的算法相比误差小、精确性更高；(2)惯性导航解算的运动轨迹进行辅助地磁匹配定位解算，采用粒子滤波方法对两者的定位结果融合，定位估计更为精确。

附图说明

图1是实施例一的DDTW算法产生的时间序列对齐方式图。

图2是实施例一的在线测量得到的地磁场序列的滑动匹配过程示意图。

图3是实施例一的粒子滤波的流程示意图。

图4是实施例一的EKF_UKF_PF滤波对比效果图。

图5是实施例一的EKF_UKF_PF的估计值误差图。

具体实施方式

下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。

实施例一：一种基于粒子滤波算法的地磁_INS组合导航方法，如图1、图2和图3所示，包括以下步骤：

步骤一：建立离散地磁数据库，保证在采集地磁数据的过程中，测量点与测量点之间的间隔保持一致，间隔不宜过大，否则会影响到地磁数据库的精度。在建库的同时保证三轴地磁传感器模块的测量方向保持一致，在测量点采集一组数据后，通过拉依达准则去除粗大误差，再对该测量点的数值求平均，就得到一个测量点的数据，最后通过广义延拓逼近的方法将网格中的数据点进行分片最佳逼近，建立高精度的细分地磁数据库。

广义延拓逼近方法是将每个子单元域向附近邻近单元作一些延伸，形成延拓域。这样能充分利用单元外的信息，使单元内的逼近函数能充分吸收邻近单元的信息，从而保证单元域上逼近函数能与邻近单元逼近函数相协调。

原理：设Ψ为定义域，把其区分成n个互不重叠的单元子域Ψ_e(e＝1,2,…,n)，则有：

把一定程度扩大后的延拓域记为Ψ'_e，则

且有

设Ψ'_e内有m个节点，其中属于Ψ_e的节点有r个(r<m)，在延拓域Ψ'_e上构造逼近函数时，为使单元Ψ_e边界上函数值与各邻近单元函数值相一致，单元Ψ_e的m个边界点应满足边界点上插值限制因素。最后将每个延拓域中求得的逼近函数仅取单元定义域上的那部分，然后拼接起来，构成整个定义域的逼近函数。

设延拓域上Ψ'_e的逼近函数为：

其中，t为逼近函数得的项数，(h₁,h₂,…,h_p)是Ψ_e上的一组基函数；ε_i(i＝1,2,…p)为待定系数。对延拓域Ψ'_e的逼近函数实行在单元域Ψ_e的边界点上符合插值条件，而在其它点作最小二乘拟合处理，即可得单元域内的逼近函数的待定系数。

求出每个单元域内的逼近函数后，拼接可得整个定义域的逼近函数：

步骤二：在二维地磁图上运动一段距离，得到一段地磁数据序列(m₁,m₂,…,m_k)，以及用惯性器件模块测得一组加速度(a₁,a₂,…,a_k)和方位角数据序列(θ₁,θ₂,…,θ_k)。通过航位推算将运动轨迹在地磁图中画出来。

航位推算原理：假设在t_n时刻，已知位置

和当前时刻运动速度

及航向角

这时通过航位推算求t_n+1时刻位置

计算公式为：

步骤三、利用导数动态时间规整(DDTW)算法将运动轨迹上里程信息对应的数据库中地磁序列与在线所测得的地磁序列进行整合，如附图1所示。在地磁匹配中，通过考虑序列的一阶导数来获取形状的相关信息，将序列的形态特征考虑进来。导数动态时间规整方法(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)。

在实际环境中，长时间的地磁数据的采集量比较大，单独采用DDTW方式不利于实时定位的效果，采用滑动窗口的形式对地磁序列整合，滑动窗口的长度主要由经验值得到，通过轮数计解算的里程信息缩小离散地磁数据库中待匹配的地磁场强序列范围，采用滑动匹配的方式进行定位解算，该方法可以降低匹配过程中的计算复杂度。在计划定位区域划分为等间隔的网格，采集每个网格点的地磁特征量，再将每个点的坐标p_i(x_i,y_i)与对应的地磁特征量组成一条指纹向量MP[m,p]，而后按照时间顺序交叉截取滑动窗口，根据定位时间和精度设置滑动窗口大小k，建立基于滑动窗口大小的地磁模值时间序列B_i＝{MP₁,MP₂,MP₃,…,MP_i}，图2中为滑动窗口长度为4的时间序列模型，并根据滑动窗口大小将待定位数据建立同等规格的地磁场模值指纹序列A_i＝{M₁,M₂,M₃,…,M_j}，在定位过程中，待匹配的地磁时间序列与地磁指纹数据库中的每一个地磁时间序列进行匹配获得最佳匹配结果。

步骤四、将地磁匹配定位的结果与INS定位解算的位置信息定位结果通过粒子滤波算法进行滤波处理，滤除误匹配造成的定位误差，得到一个稳定、连续、高精度的定位结果。

粒子滤波算法是一种基于贝叶斯估计的滤波方法，其基本思想是通过寻找一组在状态空间中传播的随机样本来描述概率分布，这些样本成为“粒子”，然后再测量的基础上，通过调节各粒子的权值大小和样本的位置，来近似实际的概率分布，并以样本的均值作为系统的估计值，从而获得最小方差意义上的状态估计。假设连续型时变非线性系统可描述为：

上式中x_t表示t时刻的状态量，y_t表示t时刻的观测量，f_t，h_t表示t时刻的状态转移函数和观测函数，y_t，n_t表示t时刻的状态噪声和观测噪声向量。在粒子滤波算法中，并不直接求所需要的预测向量和状态向量，而是求其概率分布，假设状态的初始概率密度函数

为已知，那么状态的预测量就可以经贝叶斯递推表达为：

p(x_t|y_tx-1)＝∫p(x_t|x_t-1)p(x_t-1|y_tx-1)dx_t-1

且状态的观测更新为：

上式中：

p(y_t|y_tx-1)＝∫p(y_t|x_t)p(x_t|y_tx-1)dx_t

其中：y_tx＝{y₁,y₂,...,y_t}。显然，式中的积分运算会给解算带来不便，不利于实现p(x_0x|y_0x)的递推，为此粒子滤波引入了Monte Carlo方法，将积分转化为有限点上的样本加权值，而这些样本和对应的权值，被称为粒子。假设采集到N个独立分布的样本，那么后验概率可近似的表示为：

由于目标密度函数p(x)实际上往往比较复杂，所以从中获取粒子权值是比较难实现的，于是常使用重要性密度函数进行采样，将这个重要性密度函数表达为q(x_0x|y_tx)，并且q(x_0x|y_tx)需包含p(x_0x|y_tx)所有的支承点集，重要性密度函数的求取方法为：

其中w表示对应于样本的权值，其表达式为：

同时定义：

在这里表示归一化权重，并且满足：

经后验概率密度递推和样本权值递推，可得：

从而得到状态的估计值为：

由上面的推导过程可以看出，需要首先从重要性分布中获取粒子，进而确定权值更新方式，结合观测向量递推获得状态估计值，在跟踪精度可信的基础上依次递推获取所需状态量的预报值，解算思路即为：根据实际需求选定粒子集，并对其初值进行设置，之后计算重要性权值并归一化权值，然后进行重新采样和重新分配权值，逐步迭代输出状态估计量。

根据粒子滤波的基本解算思想，可知该方法是一种随机抽样方法，但由于粒子滤波存在退化现象，所以不能进行简单的重复采样。为了解决这个问题抽样计划不能是固定不变的，需要根据上次采样粒子和相应权值来进行重采样。

第一步、初始化，根据先验条件概率抽取随机样本

其中N为样本个数；第二步、预测过程。由系统的状态方程和随机样本

得到进一步预测样本

第三步、更新过程，由k时刻的测量值z_k和测量值的预测值z'_k计算每个粒子的权值，并将其归一化得w，其中i＝1,2…N；第四步、状态估计。由粒子和其权重来计算状态的估计值

并利用该时刻的粒子和他们的权重来进行重采样，从而得到下一时刻的粒子样本，返回第二步。

建立滤波模型如下,其中系统状态方程为：

将系统状态方程进一步简化为：

系统的观测方程可以表示为：

在上述模型中，W_k，V_k分别表示系统状态方程和观测方程的高斯白噪声，系统状态噪声和观测噪声协方差可以表示为Q_k和R_k。(x_k,y_k)为真实运动轨迹，l_k、θ_k为运动的距离和方向角，(x'_k,y'_k)为k时刻的地磁定位结果。

观测矩阵为：

根据粒子的预测量与预测量之间的关系计算粒子的重要性权值并进行归一化处理：

根据权重对粒子集合进行复制和淘汰，粒子i复制的粒子数计为δ_i，重新设置权重

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

利用上述地磁匹配定位的结果与INS定位解算的位置信息定位结果通过粒子滤波算法进行滤波处理，滤除误匹配造成的定位误差，得到一个稳定、连续、高精度的定位结果。

由于各导航传感器都是在特额定的物理原理之下工作的敏感器件，且先进的导航仪器几乎都是电子器件，气信号采集、检测、处理与传输过程中都收到外围的设备或者外部的环境影响干扰，是的各导航传感器的测量结果都是包含噪声的，测量出来的数据也是偏离真实的数据的。所以滤波算法是减少各种误差影响的有效方法，使得我们测量端的值更接近真实状态。运用比较广泛的滤波方法是卡尔曼滤波，但是卡尔曼滤波的框架只适合解决线性滤波问题，现实中遇到的问题一般都是非线性的，为了解决这个问题，引入了扩展卡尔曼滤波(extended kalman filter,EKF)的概念。这一思想的基础是在滤波方法上使用了泰勒展开的原理。

经典卡尔曼滤波建立在两个重要的假设的基础上，一是系统的状态方程可以使用线性方程完全表达，二是噪声符合高斯噪声分布，这在工程中并不是一件容易得到满足的事情，首先建立完全准备的状态模型是一件比较困难的事，特别是传感器采样频率较低时，模型误差显得就比较大，而传感器的噪声实际上并不严格符合高斯噪声分布。这种情况下经典卡尔曼滤波的去噪效果就非常有限了。扩展卡尔曼滤波虽然在处理非线性问题时比经典卡尔曼滤波有了很大程度的改进但忽略二阶以上的高阶项会给结果带来影响，同时雅克比矩阵的计算将耗费大量的时间特别是在维数较高的情况下，数据的运算量就更大。这对于导航滤波而言显然是一个非常不利的影响，毕竟导航的实时性也是一项十分重要的要求。为了获得较好的非线性处理能力，并且避免雅克比矩阵的计算，无迹卡尔曼滤波方法被提了出来。无迹卡尔曼(unscented kalman filter,UKF),是一种基于UnscentedTransformation(UT)变换的非线性滤波方法。

在上述滤波方法中，都需要假设系统噪声和量测噪声是高斯噪声、并且都对线性系统进行了不同形式的线性化，这在实际中不容易实现。解决这一问题的有效方法之一就是建立在蒙特卡洛模拟思想上的粒子滤波方法，粒子滤波算法是一种从根本上不同于卡尔曼滤波的状态估计方法，是于近些年才提出来的一种在贝叶斯估计理论的基础上建立起来的序贯蒙特卡洛模拟方法这一方法的核心是利用一些随机样本，(对于这些随机样本，我们也可以形象的称之为粒子)，及其对应的权值，来表示系统随机变量的后验概率密度，从而能够得到基于状态模型的最优解由于摒弃了传统滤波方法中的模型近似原理，粒子滤波更适用于非高斯噪声非线性系统的场合。

对三种滤波方法进行了一个简单的对比，如图4和图5所示，将三种滤波方法滤波过程中的滤波估计值与真实值，三种滤波器的估计值误差进行对比，可以看出粒子滤波(PF)比扩展卡尔曼滤波(EKF)和无迹卡尔曼滤波(UKF)估计误差小，滤波结果更平滑。

以上所述的实施例只是本发明的一种较佳的方案，并非对本发明作任何形式上的限制，在不超出权利要求所记载的技术方案的前提下还有其它的变体及改型。

Claims (10)

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

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

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

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

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

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

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所述的一种基于粒子滤波算法的地磁_INS组合导航方法，其特征在于，所述步骤S4中，通过INS解算的运动轨迹进行辅助地磁匹配定位解算的具体方法为：采用滑动窗口的形式对地磁序列进行整合，滑动窗口的长度主要由经验值得到，通过轮数计解算的里程信息缩小离散地磁数据库中待匹配的地磁场强序列范围，采用滑动匹配的方式进行定位解算，令表示序列之间匹配的相似度，代表匹配相似度的阈值，主要由序列间匹配相似度的经验值得到，当匹配条件成立的时候，即输出匹配的定位位置，否则地磁数据库场强序列的窗口向左或向右滑动。

4.根据权利要求1或2或3所述的一种基于粒子滤波算法的地磁_INS组合导航方法，其特征在于，

所述步骤S5中，采用粒子滤波算法进行地磁导航最终定位估计时，包括以下步骤：

步骤S51：初始化，根据先验条件概率抽取随机样本

其中N为样本个数；

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

得到进一步预测样本

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

步骤S54：初步状态估计，由粒子和其权重来计算状态的估计值

并利用该时刻的粒子和他们的权重来进行重采样，从而得到下一时刻的粒子样本，返回步骤S52；

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

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

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

7.根据权利要求1所述的一种基于粒子滤波算法的地磁_INS组合导航方法，其特征在于，所述对两者的定位结果融合的滤波算法，主要是在面对数值方法不容易解决的问题时，用类似统计采样的方法来近似的估算出其概率，通过进一步计算得到一个最优近似解。

8.根据权利要求1所述的一种基于粒子滤波算法的地磁_INS组合导航方法，其特征在于，所述离散地磁数据库的建立方法为：保证在采集地磁数据的过程中，测量点与测量点之间的间隔保持一致，间隔不宜过大，否则会影响到地磁数据库的精度，在建库的同时保证三轴地磁传感器模块的测量方向保持一致，在测量点采集一组数据后，通过拉依达准则去除粗大误差，再对该测量点的数值求平均，就得到一个测量点的数据，最后通过广义延拓逼近的方法将网格中的数据点进行分片最佳逼近，建立高精度的细分地磁数据库。

9.根据权利要求8所述的一种基于粒子滤波算法的地磁_INS组合导航方法，其特征在于，所述广义延拓逼近方法为：设Ψ为定义域，把其区分成n个互不重叠的单元子域Ψ_e(e＝1,2,...,n)，则有：

把一定程度扩大后的延拓域记为Ψ′_e，则

且有

设Ψ′_e内有m个节点，其中属于Ψ_e的节点有r个(r<m)，在延拓域Ψ'_e上构造逼近函数时，为使单元Ψ_e边界上函数值与各邻近单元函数值相一致，单元Ψ_e的m个边界点应满足边界点上插值限制因素；最后将每个延拓域中求得的逼近函数仅取单元定义域上的那部分，然后拼接起来，构成整个定义域的逼近函数；

设延拓域上Ψ'_e的逼近函数为：

其中，t为逼近函数得的项数，(h₁,h₂,…,h_p)是Ψ_e上的一组基函数；ε_i(i＝1,2,...p)为待定系数；对延拓域Ψ'_e的逼近函数实行在单元域Ψ_e的边界点上符合插值条件，而在其它点作最小二乘拟合处理，即可得单元域内的逼近函数的待定系数；

求出每个单元域内的逼近函数后，拼接可得整个定义域的逼近函数：

10.根据权利要求1所述的一种基于粒子滤波算法的地磁_INS组合导航方法，其特征在于，所述运动轨迹的获取方法为：在二维地磁图上运动一段距离，得到一段地磁数据序列(m₁,m₂,…,m_k)，以及用惯性器件模块测得一组加速度(a₁,a₂,…,a_k)和方位角数据序列(θ₁,θ₂,…,θ_k)；通过航位推算将运动轨迹在地磁图中画出来；

所述航位推算方法为：假设在t_n时刻，已知位置

和当前时刻运动速度

及航向角

这时通过航位推算求t_n+1时刻位置

计算式为：

Images