阅读说明：本技术 一种基于粒子滤波的sins/dr组合导航系统位置跟踪确定方法 (SINS/DR integrated navigation system position tracking determination method based on particle filtering ) 是由 王万征 邓亮 裴兴凯 陈静 庄广琛 于 2018-08-14 设计创作，主要内容包括：本发明属于捷联导航控制技术,具体公开了一种基于粒子滤波的SINS/DR组合导航系统位置跟踪确定方法,首先根据目标的初始状态量及可能出现的误差半径对粒子进行随机分布,然后进行粒子状态预测,计算粒子权值,确定各个粒子与真实位置的近似程度和所有粒子的状态融合成的估计值,当某粒子的权值小于某一阈值时,进行重采样。本方法以粒子滤波理论为指导,以目标位置为特征实现了SINS/DR组合导航系统中的位置跟踪算法,可以避免里程计输出异常时引起的位置跳变,且误差不随时间发散,提高了位置和速度的实时输出精度。(The invention belongs to a strapdown navigation control technology, and particularly discloses a SINS/DR combined navigation system position tracking determination method based on particle filtering. The method takes the particle filter theory as guidance and takes the target position as a characteristic to realize the position tracking algorithm in the SINS/DR combined navigation system, can avoid position jump caused by abnormal output of the odometer, has no error divergence along with time, and improves the real-time output precision of the position and the speed.)

一种基于粒子滤波的SINS/DR组合导航系统位置跟踪确定 方法

技术领域

本发明属于捷联导航控制技术，具体涉及一种导航系统位置跟踪确定方法。

背景技术

在SINS/DR组合导航系统中，通常以里程计进行航位推算所得的位置、速度作为量测量与捷联惯导系统进行组合导航，通过卡尔曼滤波对捷联惯导系统的姿态误差进行估计，从而保持惯导姿态精度。在载体行进过程中，如果里程计测量发生异常情况，如打滑、侧移、离地等，会导致航位推算结果发生异常。在实时处理过程中，无法通过滤波器相关量对航位推算结果进行修正，使得里程计在发生异常情况时，航位推算结果会发生跳变，影响精度。

发明内容

本发明的目的在于提供一种基于粒子滤波的SINS/DR组合导航系统位置跟踪确定方法，能够避免里程计输出异常时引起的位置跳变，且误差不随时间发散。

本发明的技术方案如下：

一种基于粒子滤波的SINS/DR组合导航系统位置跟踪确定方法，该方法包括如下步骤：

1)粒子初始化，确定目标的初始状态量及可能出现的误差半径，然后根据式(1)对粒子进行随机分布

其中，S(i)_t为第i个粒子的航程，ΔS为根据运动速度预估的误差，ΔV为目标预估的速度，random为(-1,1)内的随机数，V(i)_t为第i个粒子的速度，weight(i)为第i个粒子的权重，N为粒子数；

2)粒子状态预测

通过状态转移方程来计算每个粒子在该周期的状态预测值，用来描述目标运动的状态转移方程如式(2)所示：

其中，V_odo为里程计速度；

3)粒子权值计算，确定各个粒子与真实位置的近似程度

其中，weight(i)为每个粒子的权重，S为参考位置；

利用下式确定每个粒子权值占整个累计权值的比重

其中，sum_weight为权值累加值；

4)跟踪估计，确定所有粒子的状态融合成的估计值

5)设置一个权值阈值weight_min，当某粒子的权值小于该阈值时，进行重采样

所述的步骤3)中的参考位置S是根据SINS解算结果和航位推算结果综合计算的，其计算方法如式(4)所示：

S＝0.995×(S+V_SINS×t)+0.005×S_DR (4)

其中，V_SINS为组合导航系统解算的速度，t为计算周期，S_DR为航位推算位置。

所述的权值阈值weight_min为

本发明的显著效果在于：

本方法以粒子滤波理论为指导，以目标位置为特征实现了SINS/DR组合导航系统中的位置跟踪算法，可以避免里程计输出异常时引起的位置跳变，且误差不随时间发散，提高了位置和速度的实时输出精度。

具体实施方式

下面结合具体实施例对本发明作进一步详细说明。

首先要确定粒子模型及其状态转移模型。粒子模型包含两个状态变量：航程S_t和前向速度V_t。

位置跟踪步骤如下。

1)粒子初始化

粒子初始化是通过确定目标的初始状态量及可能出现的误差半径，然后根据式(1)对粒子进行随机分布：

其中，S(i)_t为第i个粒子的航程，ΔS为根据运动速度预估的误差，ΔV为目标预估的速度，random为(-1,1)内的随机数，V(i)_t为第i个粒子的速度，weight(i)为第i个粒子的权重，N为粒子数。

粒子初始化只在开始时执行一次，后续的步骤每个计算周期都执行一次。

2)粒子状态预测

粒子状态预测是通过状态转移方程来计算每个粒子在该周期的状态预测值，用来描述目标运动的状态转移方程如式(2)所示：

其中，V_odo为里程计速度。

3)粒子权值计算

粒子通过状态预测得到了目标可能的位置，权值计算是用来计算各个粒子与真实位置的近似程度。权值计算方法如式(3)所示：

其中，weight(i)为每个粒子的权重，S为参考位置。

参考位置S是根据SINS解算结果和航位推算结果综合计算的，其计算方法如式(4)所示：

S＝0.995×(S+V_SINS×t)+0.005×S_DR (4)

其中，V_SINS为组合导航系统解算的速度，t为计算周期，S_DR为航位推算位置。

计算目标运动状态的估计值时，需要用归一化的加权粒子集来进行计算。权值归一化即求出每个粒子权值占整个累计权值的比重，权值归一化公式如式(5)所示：

其中，sum_weight为权值累加值。

4)跟踪估计

归一化后的权值即可反映出该粒子在所有粒子中能代表跟踪目标运动状态的比重，权值越大则说明该粒子越接近目标真实状态。通过权值即可将所有粒子的状态融合成一个估计值，计算方法如式(6)所示：

5)重采样

随着迭代次数的增多，一些粒子的权值会变得很小，使得这些粒子对于估计出目标运动状态难以起到作用，出现粒子退化现象。在迭代计算中，设置一个权值阈值weight_min，本实施例中设为

当某粒子的权值小于该阈值时，进行重采样，方法如式(7)所示：

通过以上流程对载体的位置进行持续跟踪计算，即可得到航迹。正常情况下，里程计未发生异常情况时，里程计输出的位置增量无误差，SINS系统虽然可以通过航位推算的速度对其速度误差和姿态误差进行修正，但加表零偏引起在一个滤波周期内的速度误差仍会导致位置误差随时间发散。利用本发明中基于粒子滤波的位置跟踪算法，位置误差仍然存在，但在一定时间收敛后，误差不再随着时间发散。

当里程计发生异常时，位置误差发生跳变，在实时处理中会影响该时刻测量精度。利用本方法可以使位置输出在长时间情况下跟踪里程计位置而不发生跳变，且不会随时间发散。

里程计正常情况下速度输出为1m/s，在里程计打滑时，里程计输出的速度跳变为零，通过本发明方法跟踪到的速度在该异常时刻变化值约为0.05m/s，误差减小了一个数量级以上。