[发明专利]一种人工物理优化粒子滤波的重力梯度辅助定位方法

摘要

本发明提供的是一种人工物理优化粒子滤波的重力梯度辅助定位方法。通过GPS确定载体的初始位置参数，将它们装订至导航计算机中；捷联惯导系统进行预热准备，采集光纤陀螺仪和石英加速度计输出的数据并对数据进行处理；通过引入人工物理优化，改善粒子分布，克服粒子滤波的粒子退化问题，减少计算量和所需的迭代次数。人工物理优化粒子滤波算法通过在改善粒子分布的过程中引入了排斥力，避免粒子集过分重叠或拥挤，保证粒子多样性和优化后的粒子集对后验概率密度形成均匀的覆盖能力。人工物理优化粒子滤波通过不断的更新和递推，估计出惯导系统的位置误差后进行校正，从而使重力梯度辅助惯导系统的位置误差逐渐趋于零。本发明提出的人工物理优化粒子滤波算法在提高系统估计精度的同时，使系统具有更好的稳定性。

主权项

1.一种人工物理优化粒子滤波的重力梯度辅助定位方法，其特征在于包括以下步骤：(1)通过GPS确定载体的初始位置参数，将它们装订至导航计算机中；(2)捷联惯导系统进行预热准备，采集光纤陀螺仪和石英加速度计输出的数据并对数据进行处理；(3)人工物理优化粒子滤波算法的实现；1)初始化从后验概率密度分布中采样，权重置为1/N。同时设置感知半径r_s，引力和斥力的阈值D_th，虚拟力系数K_a1、K_a2，迭代次数等参数，感知矩阵c、虚拟力矩阵F和当前迭代次数置零。2)预测从采样的重要性密度函数中采样新粒子集并计算粒子权值。3)优化粒子分布a)计算粒子s_i与s_j之间的欧氏距离d(s_i，s_j)，i，j＝1，2，...Nb)计算s_i对s_j的感知矩阵c(si,sj)=1d(si,sj)≤rs0d(si,sj)>rs,]]>i，j＝1，2，...N，r_s为感知半径。c)计算粒子s_i的虚拟力F(s_i，s_j)F(si,sj)=Ka1[d(si,sj)-Dth]xj-xid(si,sj)c(si,sj)=0Ka2[Dth-d(si,sj)]xj-xid(si,sj)c(si,sj)=1d(si,sj)>DthKa2[d(si,sj)-Dth]xj-xid(si,sj)c(si,sj)=1d(si,sj)<Dth]]>d)计算粒子s_i受到的合力F(i)如果F(i)＞最大位移限制L_max，则F(i)＝L_max；如果F(i)＜最小位移限制L_min，则F(i)＝L_min。e)更新粒子s_i的位置x′_i＝x_i+F_i其中，x′_i为更新后的位置，受到最小位移和最大位移的限制，即x′_i∈[L_min，L_max]。满足优化条件或迭代次数后，优化算法终止。f)计算新粒子权值，更新权值最大的粒子4)获得新粒子集，迭代优化结束。重新计算新粒子权值并归一化5)重采样如果有效粒子数小于设定的阈值，粒子滤波算法进行重采样6)状态估计计算完单个粒子所受的合虚拟力后进行粒子位置的更新，以此减少计算量和所需的迭代次数，提高算法的收敛速度。人工物理优化粒子算法通过在改善粒子分布的过程中引入排斥力，避免粒子集过分重叠或拥挤，以此实现粒子多样性和对后验概率密度的分布。(4)重力梯度辅助的组合滤波模型的建立；在惯性导航系统误差方程基础上推导基于重力梯度辅助定位的组合滤波模型，选取经纬度误差、速度误差、姿态误差、加速度计零偏以及陀螺漂移作为状态变量。重力梯度辅助定位与惯性导航系统的紧组合过程是将重力梯度作为观测量。考虑重力梯度观测值是载体真实位置的函数，基于重力梯度辅助定位观测方程可表示为：其中，y_jk(j，k＝x，y，z)为重力梯度观测值；表示真实载体位置处的重力梯度；v表示观测噪声。重力梯度与载体真实位置为典型的非线性关系，由于惯性导航系统不能直接给出载体真实位置因此，常用线性近似滤波方法将非线性向量函数在当前时刻滤波估值附近取一阶泰勒级数展开：得到近似线性化的误差观测方程：由于重力梯度辅助定位的基准梯度图一般采用离散格网的存储形式，重力梯度值和位置坐标存在对应关系，因此基于重力梯度辅助定位观测方程难以用解析函数表示。通过给定的系统状态初值和初始方差阵，粒子滤波器可生成初始时刻位置误差的后验分布和因此，重力梯度辅助定位的观测方程中的两个重要参数通过粒子滤波方法获得。经过一步预测，粒子滤波就可从基准梯度图中直接计算出作为预测概率密度。当最新观测值y_ik到来时，粒子滤波再通过重要性密度函数，利用预测梯度值和观测梯度值之间的差值，更新粒子权值得到系统状态。如果粒子集不能代表系统状态的真实后验分布，那么基准梯度图中读出的预测梯度值和观测梯度值之间的差值较大，权值则较小，反之亦然。粒子滤波通过不断的更新和递推，估计出惯导系统的位置误差后进行校正，从而使系统位置误差逐渐趋于零。