阅读说明：本技术 一种基于粒子滤波的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.)

1. A SINS/DR combined navigation system position tracking determination method based on particle filtering is characterized by comprising the following steps:

1) particle initialization, determining the initial state quantity of the target and the radius of error which may occur, and then randomly distributing the particles according to equation (1)

Wherein, S (i)_tFor the flight path of the ith particle, Δ S is the error estimated from the velocity of motion, Δ V is the estimated velocity of the target, random is a random number in (-1,1), V (i)_tWeight (i) is the weight of the ith particle, and N is the number of particles;

2) particle state prediction

Calculating a predicted state value of each particle in the period through a state transition equation, wherein the state transition equation for describing the target motion is shown as a formula (2):

wherein, V_odoIs the odometer speed;

3) calculating the weight of the particles, and determining the approximation degree of each particle to the real position

Wherein, weight (i) is the weight of each particle, and S is the reference position;

determining the proportion of each particle weight in the total accumulated weight by using the following formula

Wherein sum _ weight is a weight accumulated value;

4) tracking estimation, determining an estimate of the fusion of the states of all particles

5) Setting a weight threshold value weight _ min, and when the weight of a certain particle is less than the threshold value, resampling

2. The method of claim 1, wherein the SINS/DR combined navigation system location tracking determination method based on particle filtering is:

the reference position S in the step 3) is comprehensively calculated according to the SINS calculation result and the dead reckoning result, and the calculation method is shown as the formula (4):

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

wherein, V_SINSSpeed of calculation for integrated navigation system, t is calculation period, S_DRIs a dead reckoning position.

3. The method of claim 1, wherein the SINS/DR combined navigation system location tracking determination method based on particle filtering is: the weight threshold weight _ min is

Technical Field

The invention belongs to a strapdown navigation control technology, and particularly relates to a navigation system position tracking and determining method.

Background

In an SINS/DR integrated navigation system, a position and a speed obtained by dead reckoning with a speedometer are generally used as a measurement value to perform integrated navigation with a strapdown inertial navigation system, and an attitude error of the strapdown inertial navigation system is estimated through kalman filtering, so that the inertial navigation attitude precision is maintained. During the carrier moving process, if abnormal conditions such as slipping, side shifting, lift off and the like occur in the odometer measurement, the dead reckoning result is abnormal. In the real-time processing process, the dead reckoning result cannot be corrected through the related quantity of the filter, so that when the odometer is in an abnormal condition, the dead reckoning result jumps, and the accuracy is influenced.

Disclosure of Invention

The invention aims to provide a SINS/DR combined navigation system position tracking and determining method based on particle filtering, which can avoid position jump caused by abnormal output of a speedometer and prevent errors from dispersing along with time.

The technical scheme of the invention is as follows:

a SINS/DR combined navigation system position tracking determination method based on particle filtering comprises the following steps:

1) particle initialization, determining the initial state quantity of the target and the radius of error which may occur, and then randomly distributing the particles according to equation (1)

Wherein, S (i)_tFor the flight path of the ith particle, Δ S is the error estimated from the velocity of motion, Δ V is the estimated velocity of the target, random is a random number in (-1,1), V (i)_tWeight (i) is the weight of the ith particle, and N is the number of particles;

2) particle state prediction

Calculating a predicted state value of each particle in the period through a state transition equation, wherein the state transition equation for describing the target motion is shown as a formula (2):

wherein, V_odoIs the odometer speed;

3) calculating the weight of the particles, and determining the approximation degree of each particle to the real position

Wherein, weight (i) is the weight of each particle, and S is the reference position;

determining the proportion of each particle weight in the total accumulated weight by using the following formula

Wherein sum _ weight is a weight accumulated value;

4) tracking estimation, determining an estimate of the fusion of the states of all particles

5) Setting a weight threshold value weight _ min, and when the weight of a certain particle is less than the threshold value, resampling

The reference position S in the step 3) is comprehensively calculated according to the SINS calculation result and the dead reckoning result, and the calculation method is shown as the formula (4):

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

wherein, V_SINSSpeed of calculation for integrated navigation system, t is calculation period, S_DRIs a dead reckoning position.

The weight threshold weight _ min is

The invention has the following remarkable effects:

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.

Detailed Description

The present invention will be described in further detail with reference to specific examples.

First, a particle model and its state transition model are determined. The particle model contains two state variables: voyage S_tAnd forward speed V_t。

The position tracking procedure is as follows.

1) Particle initialization

Particle initialization is performed by determining the initial state quantity of the target and the radius of error that may occur, and then randomly distributing the particles according to equation (1):

wherein, S (i)_tFor the flight path of the ith particle, Δ S is the error estimated from the velocity of motion, Δ V is the estimated velocity of the target, random is a random number in (-1,1), V (i)_tWeight (i) is the weight of the ith particle, and N is the number of particles.

Particle initialization is performed only once at the beginning and the subsequent steps are performed once per calculation cycle.

2) Particle state prediction

The particle state prediction is to calculate the state prediction value of each particle in the period through a state transition equation, and the state transition equation for describing the target motion is shown as formula (2):

wherein, V_odoIs the odometer speed.

3) Particle weight calculation

The particles obtain the possible positions of the target through state prediction, and weight calculation is used for calculating the approximation degree of each particle to the real position. The weight calculation method is shown in formula (3):

where weight (i) is the weight of each particle and S is the reference position.

The reference position S is comprehensively calculated according to the SINS calculation result and the dead reckoning result, and the calculation method is shown as the formula (4):

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

wherein, V_SINSSpeed of calculation for integrated navigation system, t is calculation period, S_DRIs a dead reckoning position.

When calculating the estimated value of the motion state of the target, the normalized weighted particle set is used for calculation. The weight normalization can calculate the proportion of each particle weight in the whole accumulated weight, and the weight normalization formula is shown as formula (5):

and sum _ weight is a weight accumulated value.

4) Tracking estimation

The normalized weight value can reflect the proportion of the particle which can represent the motion state of the tracked target in all the particles, and the larger the weight value is, the closer the particle is to the real state of the target is. The states of all particles can be fused into an estimated value through the weight, and the calculation method is shown as the formula (6):

5) resampling

As the number of iterations increases, the weights of some particles become small, so that the particles are difficult to function for estimating the motion state of the target, and a particle degradation phenomenon occurs. In the stackIn the generation calculation, a weight threshold weight _ min is set, which is set in this embodiment

When the weight of a certain particle is smaller than the threshold, resampling is carried out, and the method is shown in formula (7):

and continuously tracking and calculating the position of the carrier through the process to obtain the flight path. Under normal conditions, when the odometer is not abnormal, the position increment output by the odometer is error-free, although the SINS can correct the speed error and the attitude error of the odometer through the speed of dead reckoning, the speed error in one filtering period caused by zero offset of the adding table still causes the position error to be dispersed along with time. By utilizing the particle filter-based position tracking algorithm, the position error still exists, but after the position error is converged for a certain time, the error does not diverge with the time.

When the odometer is abnormal, the position error jumps, and the measurement precision at the moment can be influenced in real-time processing. By using the method, the position output can track the position of the odometer under the condition of long time without jumping and can not diverge along with time.

The speed output of the odometer is 1m/s under the normal condition, when the odometer slips, the speed jump output by the odometer becomes zero, the change value of the speed tracked by the method of the invention at the abnormal moment is about 0.05m/s, and the error is reduced by more than one order of magnitude.