CN111256695B - Combined indoor positioning method of UWB/INS based on particle filter algorithm - Google Patents

Abstract

The invention relates to a UWB/INS combined indoor positioning method based on a particle filter algorithm. Aiming at the condition that a UWB (ultra wide band) ranging result is easily interfered by a complex environment to influence the positioning precision in a non-line-of-sight environment, a UWB/INS combined indoor positioning method based on a particle filter algorithm is provided. The distance from a person to be positioned to each reference base station is obtained through a UWB system, and the east direction position and the north direction position of the person are calculated through a UWB position calculating unit. Three-axis acceleration, three-axis angular velocity and three-axis magnetic field intensity of a person in a walking process are obtained through an Inertial Navigation System (INS), and a step length, an eastern walking speed, a northern walking speed, a walking time and an attitude angle of the person in the walking process are calculated through an INS resolving unit. And performing data fusion on the calculation result of the UWB system and the calculation result of the INS system through a particle filter algorithm. Finally, the purposes of reducing the influence of a non-line-of-sight complex environment and improving the positioning precision are achieved.

Description

Combined indoor positioning method of UWB/INS based on particle filter algorithm

technical field

The invention relates to the technical field of indoor positioning, in particular to a UWB/INS combined indoor positioning method based on a particle filter algorithm.

Background technique

Today, when GPS satellite navigation and positioning technology has been developed relatively well, due to the relatively closed and complex environmental characteristics of indoor environment and underground environment, GPS technology is difficult to achieve accurate tracking and positioning indoors and underground. As a result, indoor positioning technology came into being.

Among various indoor positioning schemes, most of them focus on the research of a single positioning technology. A relatively good wireless positioning technology is UWB indoor positioning technology, but UWB positioning is easily affected by the environment. If the indoor environment is open and there is no interference from obstacles, that is, the line-of-sight environment, the UWB positioning accuracy is high; when the indoor environment is complex, that is, the non-line-of-sight environment, the UWB signal is affected by the multipath effect, resulting in the inability to accurately locate the target. Continuous positioning.

Because the INS positioning technology is not easily affected by the environment, it can continuously locate and track the target. However, the INS positioning technology has accumulated errors, which requires continuous position correction.

The indoor positioning technology based on UWB/INS can not only improve the average positioning accuracy of the system, but also realize the continuous positioning and tracking of the target for a long time.

SUMMARY OF THE INVENTION

The present invention proposes a UWB/INS combined indoor positioning method based on a particle filter algorithm, which aims to solve the technical problem of low positioning accuracy of the positioning system caused by complex non-line-of-sight environments.

The technical scheme of the present invention is: a UWB/INS combined indoor positioning method based on a particle filter algorithm. include:

The distance from the person to be located to each reference base station is obtained through the UWB system, and the east and north positions of the person are calculated through the UWB position calculation unit.

The three-axis acceleration, three-axis angular velocity, and three-axis magnetic field strength of the person during the walking process are obtained through the INS system, and the step length, east-direction pace, north-direction pace, and step time are calculated by the INS solution unit. and attitude angle.

The particle filter is constructed using the east and north positions calculated by the UWB system and the east and north pace calculated by the INS system as state vectors, and the step length and attitude angle calculated by the INS system as the observation vectors. Model.

Particle filtering is performed to obtain the best indoor personnel position information at the current moment.

Further, the state equation of the particle filter is:

where [E(k+1) N(k+1) V _E (k+1) V _N (k+1)] and

[E(k) N(k) V _E (k) V _N (k)] are the east position, north position, east pace, The pace in the north direction, T(k) is the stepping time of the person at time k, and ω(k) is the system noise at time k.

Further, the observation equation of the particle filter is:

h ₃ =1/ _VE (k)

Among them, [E(k) N(k) S(k) Y(k)] is the east position, north position, step length and attitude angle calculated by the UWB system and the INS system at time k, [E(k) N(k) V _E (k) V _N (k)] is the east position, north position, east pace, and north pace calculated by the UWB system and the INS system at time k, γ(k) is k System observation noise at time.

Further, a particle filter model is established, and a particle filter algorithm is executed to process the data, specifically:

是第0个采样点的第i个粒子的状态量。Initialization: sample initialization states from the prior distribution

is the state quantity of the ith particle at the 0th sampling point.

Z_k为第1到第k个采样点的观测量。Particle collection sampling:

Z _k is the observation amount of the 1st to kth sampling points.

Calculate particle importance weights:

其中N是粒子滤波器的粒子数目。Normalized weights:

where N is the number of particles in the particle filter.

的大小，对粒子集合

进行复制和淘汰。并重新设置权重

Select resampling: according to normalized weights

the size of the particle set

Duplication and elimination. and reset the weights

Output: The particle filter outputs a set of sample points, approximated as a posterior distribution:

X _k =∫X _k p(X _0:k |Z _1:k )dX _0:k

Finally, the best indoor personnel position estimation at time k is obtained.

The beneficial effects of the invention are: by using the particle filter algorithm to fuse the results obtained by the UWB system and the INS system, the influence of the complex non-line-of-sight environment on the positioning accuracy of the combined positioning system is reduced, and the best indoor personnel position estimation is obtained. . It can be used for indoor high-precision personnel positioning.

Description of drawings

Figure 1 is a schematic diagram of the UWB/INS combined indoor positioning system;

Figure 2 is a schematic diagram of a combined UWB/INS indoor positioning method based on a particle filter algorithm.

Detailed ways

The present invention is further described below in conjunction with accompanying drawing and specific implementation:

UWB/INS combined indoor positioning method based on particle filter algorithm, including:

Arrange the UWB base station indoors in advance, fix the UWB tag on the person, fix the INS sensor on the person's toes, and then carry out the following steps:

Step 1: Obtain the distance from the person to be located to each reference base station through the UWB system, and calculate the east and north positions of the person through the UWB position calculation unit.

Step 2: Obtain the three-axis acceleration, three-axis angular velocity, and three-axis magnetic field strength of the person during walking through the INS system, and use the INS solution unit to calculate the person's step length, east-direction pace, and north-direction pace. , stride time and attitude angle.

Step 3: Take the east and north positions calculated by the UWB system, the east and north pace calculated by the INS system as the state vector, and the step length and the attitude angle calculated by the INS system as the observation vector, Build a particle filter model. include:

The state equation of the particle filter is:

where [E(k+1) N(k+1) V _E (k+1) V _N (k+1)] and [E(k) N(k) V _E (k) V _N (k) ] are the east position, north position, east pace, and north pace calculated by the UWB system and the INS system at time k+1 and time k respectively, T(k) is the stepping time of the person at time k, ω( k) is the system noise at time k.

The observation equation of the particle filter is:

h ₃ =1/ _VE (k)

Among them, [E(k) N(k) S(k) Y(k)] is the east position, north position, step length and attitude angle calculated by the UWB system and the INS system at time k, [E(k) N(k) V _E (k) V _N (k)] is the east position, north position, east pace, and north pace calculated by the UWB system and the INS system at time k, γ(k) is k System observation noise at time.

Step 4: Perform particle filter processing according to the particle filter model constructed in Step 3, including:

是第0个采样点的第i个粒子的状态量。Initialization: sample initialization states from the prior distribution

is the state quantity of the ith particle at the 0th sampling point.

Z_k为第1到第k个采样点的观测量。Particle collection sampling:

Z _k is the observation amount of the 1st to kth sampling points.

Calculate particle importance weights:

其中N是粒子滤波器的粒子数目。Normalized weights:

where N is the number of particles in the particle filter.

的大小，对粒子集合

进行复制和淘汰。并重新设置权重

Select resampling: according to normalized weights

the size of the particle set

Duplication and elimination. and reset the weights

Output: The particle filter outputs a set of sample points, approximated as a posterior distribution:

X _k =∫X _k p(X _0:k |Z _1:k )dX _0:k

Finally, the best indoor personnel position estimation at time k is obtained.

Claims (2)

1. The UWB/INS combined indoor positioning method based on the particle filter algorithm is characterized by comprising the following steps of:

the method comprises the following steps: acquiring the distance from a person to be positioned to each reference base station through a UWB system, and calculating the east position and the north position of the person through a UWB position calculating unit;

step two: acquiring three-axis acceleration, three-axis angular velocity and three-axis magnetic field intensity of a person in a walking process through an INS system, and calculating a step length, an eastern walking speed, a northern walking speed, a walking time and an attitude angle of the person in the walking process through an INS resolving unit;

step three: the east and north positions calculated by the UWB system and the east and north pace calculated by the INS system are used as state vectors, and the step length and attitude angle calculated by the INS system are used as observation vectors to construct a particle filter model;

the state equation of the particle filter model is:

wherein [ E (k +1) N (k +1) V _E (k+1) V _N (k+1)]And [ E (k) N (k) V _E (k) V _N (k)]The system comprises a UWB system, an INS system, a server and a server, wherein the UWB system and the INS system respectively calculate the east direction position, the north direction position, the east direction pace and the north direction pace at the moment k +1 and the moment k, T (k) is the step time of a person at the moment k, and omega (k) is system noise at the moment k;

the observation equation of the particle filter model is:

h ₃ ＝1/V _E (k)

wherein [ E (k) N (k) S (k) Y (k)]Is east direction position, north direction position, step length and attitude angle calculated by UWB system and INS system at time k, [ E (k) N (k) V _E (k) V _N (k)]The system comprises a UWB system and an INS system at the moment k, wherein the UWB system and the INS system calculate to obtain an east direction position, a north direction position, an east direction pace and a north direction pace, and gamma (k) is system observation noise at the moment k;

step four: and performing particle filtering processing to obtain the best indoor personnel position estimation at the current moment.

2. The UWB/INS combined indoor positioning method based on particle filter algorithm as claimed in claim 1, wherein the particle filter model is established, and the particle filter algorithm is executed to process data, specifically:

initialization: extracting initialization states from a prior distribution

The state quantity of the ith particle which is an initial sampling point;

sampling of a particle set:

Z _1:k for observation of the 1 st to k th sampling pointsAn amount;

calculating the importance weight of the particles:

normalization weight:

wherein N is the number of particles of the particle filter;

selecting resampling: according to the normalized weight

Size of (2) to the particle set

Copy and discard, and reset the weights

And (3) outputting: the particle filter outputs a set of sample points, approximately represented as a posterior distribution:

X _k ＝∫X _k p(X _0:k |Z _1:k )dX _0:k

and finally obtaining the best indoor personnel position estimation at the k moment.

Images