CN104061934A

CN104061934A - Pedestrian indoor position tracking method based on inertial sensor

Info

Publication number: CN104061934A
Application number: CN201410255619.7A
Authority: CN
Inventors: 马琳; 邓仲哲; 秦丹阳; 何晨光; 徐玉滨; 崔扬
Original assignee: Harbin Institute of Technology Shenzhen
Current assignee: Hit Robot Group Co ltd
Priority date: 2014-06-10
Filing date: 2014-06-10
Publication date: 2014-09-24
Anticipated expiration: 2034-06-10
Also published as: CN104061934B

Abstract

Indoor pedestrian position tracking method based on inertial sensor. The invention relates to a pedestrian indoor position tracking method, in particular to a PDR method based on an inertial sensor and a PF fusion map information algorithm. The present invention aims to solve the situation that the position estimation error is large or even wrongly estimated due to the long-term drift of the inertial sensor itself when the PDR is only used to track the position of pedestrians. 1. Step detection and step estimation based on the acceleration sensor; 2. Estimation of the heading angle according to the change of the three-axis angular velocity in the gyroscope measurement data, correcting the heading angle, and then according to the corrected heading angle and the step length of the first step Carry out dead reckoning, and finally estimate the position through the PDR method according to the step size and heading angle: 3. The map information and the PDR estimation result are fused through particle filtering, that is, the indoor position tracking method for pedestrians based on inertial sensors is completed. The invention is applied to the technical field of indoor positioning.

Description

Pedestrian indoor position tracking method based on inertial sensor

Technical Field

The invention relates to a pedestrian indoor position tracking method, in particular to a PDR method and a PF fusion map information algorithm based on an inertial sensor.

Background

In recent years, the development of Micro-electro mechanical Systems (MEMS) has led to the popularization of the application of inertial sensors in smart mobile terminals. An inertial sensor-based pedestrian indoor navigation system is becoming popular for research due to its low cost advantage of not needing to lay external facilities. The basic principle of the system is that a Pedestrian track deduction algorithm (PDR) is adopted, and technologies such as Step detection, Step length estimation, Heading angle estimation and the like are mainly involved according to measurement data of an inertial sensor (such as an accelerometer and a gyroscope), so that the system is sometimes called SHSs (Step-and-Heading Systems). The early wearable equipment that adopts, install the sensor in shoes, helmet, place pocket, waist etc. in, the foot can more reflect motion characteristics in the walking, so can be better examine step based on foot sensor, nevertheless need additionally to purchase professional equipment and inconvenient carrying, be not suitable for ordinary pedestrian's indoor navigation. In the walking process, the acceleration sensor can output certain walking characteristics, peak detection, zero-crossing detection, autocorrelation matching, spectrum analysis and the like are carried out on the measured data, one or more of the peak detection, the zero-crossing detection, the autocorrelation matching, the spectrum analysis and the like are combined to identify each step, and the real-time performance of the last two steps is poor due to long time consumption. For the step length estimation, the most rough is to directly set the step length as a constant, because a pedestrian has an average step length under the constant-speed walking, but in reality, the step length cannot be generally the average value because the step length is influenced by the height, the body type, the step-changing frequency and other factors of a person. Researchers generally accept a formula calculation of a quartic root of a maximum and minimum acceleration difference value, but other step length estimation methods such as Zero-velocity UPdaTe (ZUPT) based on linear relation between step frequency and step length, Zero-velocity UPdaTe (Zero-velocity UPdaTe) based on height of a pedestrian and the like are also proposed. In course estimation, the simplest estimation method is to fix the sensor to the pedestrian, so that the coordinate system of the sensor coincides with three axes of a terrestrial coordinate system (compass coordinate system, N-E coordinate system), and thus the azimuth angle measured by the sensor is the course angle of the pedestrian, which is often calculated by using the Components of gravity on the three axes of the acceleration sensor and a magnetometer (electronic compass), while generally, the sensor is placed without a fixed attitude (i.e., attitude randomness), which is needed to estimate the course by using other methods, such as Principal Component Analysis (PCA) to estimate the course angle.

Particle Filtering (PF), a Filter tracking algorithm developed in the case where gaussian, linear assumptions of the Kalman filtered state model are not satisfied, approximates the bayesian optimal estimate by sampling the maximum posterior estimate using the monte carlo method. At present, various indoor positioning such as WLAN indoor positioning, ultra-wideband indoor positioning, vision-based indoor positioning, inertial sensor indoor positioning and the like adopt particle filtering to improve positioning accuracy and solve the problem of multi-target tracking, and the particle filtering is used for fusing data of different positioning systems, so that the positioning results are mutually assisted and improved. The particle filtering is often adopted to add the map information into the position estimation, and the parameters in the particle filtering are adjusted according to the map information, so that the precision is improved, and the tracking target is prevented from passing through the wall and other special conditions.

Disclosure of Invention

The invention provides a pedestrian indoor position tracking method based on an inertial sensor, aiming at solving the problems of large position estimation error, even error estimation and the like caused by long-term drift of the inertial sensor when a pedestrian position is tracked by using PDR (pulse width modulation) only.

The pedestrian indoor position tracking method based on the inertial sensor is realized by the following steps:

firstly, detecting steps and estimating step length according to an acceleration sensor;

estimating a course angle according to the three-axis angular speed change in the gyroscope measurement data, correcting the course angle, carrying out track calculation according to the corrected course angle and the first step length, and estimating the position according to the step length and the course angle by a PDR method:

wherein,representing the estimated location of the PDR for the k-th steph_kRepresenting the heading angle, sLen, estimated at step k_kRepresents the step size of the k step;

and thirdly, fusing the map information and the PDR estimation result through particle filtering, thus completing the pedestrian indoor position tracking method based on the inertial sensor.

The invention has the following effects:

the pedestrian position tracking method based on the inertial sensor initially estimates the position by means of an inertial sensor module in a smart phone and a pedestrian track pushing algorithm, and then provides real-time position information for a user by fusing map information through a particle filter algorithm. In the implementation of a pedestrian track estimation algorithm, a PZVT step detection algorithm based on an acceleration value and time is provided, the characteristic that the angular integral of a gyroscope has long-term drift is estimated through step length, the three-axis angle change is mutually corrected, the test is carried out in an experimental environment, and the position estimation error is relatively reduced. The estimated position is filtered by using particle filtering and combining map information, so that the position estimation accuracy is improved to a certain extent.

Drawings

FIG. 1 is a schematic block diagram of an inertial sensor based PDR;

FIG. 2 is a diagram of a mobile phone sensor coordinate system, an electronic compass coordinate system;

FIG. 3 is a schematic diagram of an experimental environment for pedestrian position tracking based on inertial sensors;

FIG. 4 is a Z-axis acceleration value and a step detection result;

FIG. 5 is a chart comparing uncorrected and corrected course angles;

fig. 6 is a position tracking comparison graph using PF and DR only.

Detailed Description

The first embodiment is as follows: the pedestrian indoor position tracking method based on the inertial sensor is realized by the following steps:

The second embodiment is as follows: the first difference between the present embodiment and the specific embodiment is: the specific process of the step one is as follows:

detecting steps by adopting a peak value-zero value-valley value-time interval method, namely, each step of walking comprises 1 maximum acceleration, 2 zero values and 1 minimum acceleration, the time interval is reasonable, the lower limit of the time interval is set to be 250 milliseconds and S is set to be S according to the walking speed of the adult at the normal speed of 2-4 steps per second₀Indicating start, S_iI-1 … 9 denotes the ith step, and the step size estimate is obtained by the following equation

Where sLen is the estimated step size, acc_i、acc_AveRespectively representing the acceleration value and the average acceleration value in each step, wherein N represents the number of the data collected in each step.

Other steps and parameters are the same as those in the first embodiment.

The third concrete implementation mode: the present embodiment differs from the first or second embodiment in that: the course angle estimation method in the second step comprises the following steps:

firstly, time integration is carried out on the three-axis angular velocity around x, y and z to respectively obtain a Pitch angle, a Roll angle and an Azimuth angle which are recorded as Pitch, Roll and Azimuth;

and (3) correcting the course angle in the first step:

heading＝c₁·Pitch+c₂·Roll+c₃·Azimuth

(3) wherein Pitch angle Pitch represents the amount of rotation about the x-axis, Roll angle Roll represents the amount of rotation about the y-axis, and Azimuth angle Azimuth represents the amount of rotation about the z-axis, where c₁,c₂,c₃Is the corresponding weighting coefficient;

and performing a second step of correction on the course angle after the first step of correction, namely performing successive smoothing, and specifically calculating by the following formula:

<math><mrow> <msub> <mi>heading</mi> <mi>i</mi> </msub> <mo>=</mo> <mi>mean</mi> <mrow> <mo>(</mo> <munderover> <mi>Σ</mi> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>i</mi> </munderover> <msub> <mi>heading</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow></math>

mean (-) indicates the mean.

Other steps and parameters are the same as those in the first or second embodiment.

The fourth concrete implementation mode: the difference between this embodiment mode and one of the first to third embodiment modes is: the method for fusing the map information and the PDR estimation result through particle filtering in the third step is as follows:

particle filtering is a bayesian filtering method that approximates the posterior probability by using the monte carlo method, without requiring the motion system to be linear or gaussian;

firstly, the state equation of the adopted particle filter is as follows:

wherein x is_k,y_kDenotes the position coordinate at step k, x_k-1,y_k-1Position coordinates representing the k-1 st step, h_kIs the estimated heading angle of the vehicle,in order to estimate the error for the step size,for course angle estimation error, l_kIs the representation? Sin (·), cos (·) respectively represents solving sine value and cosine value of corresponding angle;

secondly, weighting values w for all the particles in the k step_iAnd (3) calculating:

<math><mrow> <msub> <mi>w</mi> <mi>i</mi> </msub> <mo>=</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <mi>exp</mi> <mrow> <mo>(</mo> <mo>-</mo> <mfrac> <msup> <mrow> <mo>|</mo> <mo>|</mo> <msubsup> <mi>pos</mi> <mi>k</mi> <mi>PDR</mi> </msubsup> <mo>-</mo> <msubsup> <mi>pos</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>|</mo> <mo>|</mo> </mrow> <mn>2</mn> </msup> <mrow> <mn>2</mn> <msup> <mi>σ</mi> <mn>2</mn> </msup> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>,</mo> <mi>condition</mi> <mo>_</mo> <mn>1</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> <mo>,</mo> <mi>condition</mi> <mo>_</mo> <mn>2</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow></math>

whereinRepresenting the estimated location of the PDR for the k-th step Denotes the position of the ith particle in the k-th step, σ denotes the standard deviation of the PDR position estimate, and w is the position of the particle generated in the k-th step when it is not within the valid position range_iTaking a value of 0, otherwise, calculating a weight according to condition _ 1;

thirdly, fusing the map information and the PDR estimation result through particle filtering:

obtaining all the weight values w of the particles in the k step_iThen according to w_iThe estimated position of the k-th step using particle filtering is calculated:

wherein Np represents the number of particles in the k-th step,representing the estimated position of the ith particle in the k stepCalculated according to equation (5), to normalize the weight values, by the following equation:

and continuously carrying out particle filtering on the position estimated by the PDR to obtain a new estimated position.

The state estimation of a dynamic system is generally described by two equations, respectively: s_k＝F(s_k-1,n_k-1) (9)

The observation equation: m is_k＝G(s_k,v_k) (10)

Wherein F (-), G (-) are the system states s_k-1,s_kFunction of s_kIs a state variable at time k, m_kIs s is_kOf the observed value, system noise n_kAnd observation noise v_kRandom variables with known probability density, which are independent of each other and the system state;

other steps and parameters are the same as those in one of the first to third embodiments.

In the following detailed description of the embodiments, the pedestrian position tracking method based on the inertial sensor is implemented by the following steps:

firstly, step detection and step length estimation are carried out according to an acceleration sensor:

the first embodiment will be described in detail with reference to fig. 2 and 3. Fig. 2 is a plan view illustration of 12 levels of a 2A campus of harbin university of industry, experimented in an inertial sensor based pedestrian location tracking in a corridor (dark filled area in the figure) of the floor, the corridor being about 3 meters wide and about 90 meters long. In fig. 3 is shown the posture of the tester holding the handset in the experimental test, as well as the Sensor coordinate system (Sensor coordinate system), the navigation coordinate system (E-N coordinate system) in the handset.

In the walking process of a person, different parts of the body can change at different accelerated speeds, but the different parts are regular, so that information can be extracted from the acceleration, and whether the person walks or stops and other actions can be identified. The step is detected by adopting a peak-zero value-valley value-time interval (PZVT) method, namely, each step of walking comprises 1 maximum acceleration, 2 zero values and 1 minimum acceleration (as shown in figure 4), the time interval is reasonable, the walking speed at the constant speed of the adult is 2-4 steps per second, and the lower limit of the time interval is set to be 250 milliseconds. The apex box of the waveform represents the detected valid step, S₀Indicating start, S_iAnd i is 1 … 9, indicating the ith step.

The actual measurement PZVT step detection algorithm shows that the accuracy of the step detection algorithm adopted by the embodiment is basically 100%, namely the situations of false detection, missed detection and multiple detection rarely occur.

The step length is not only related to the height of a person, but also related to factors such as the step frequency in walking, and the like, and the same person has a difference in each step in walking, so that the step length estimation is a very challenging problem. This patent uses the formula

Where sLen is the estimated step size, acc_i、acc_AveThe method respectively represents an acceleration value and an average acceleration value in each step, k is a coefficient corresponding to each method, the value of the method is 1.07, and N represents the number of data acquired in one step.

And secondly, estimating a course angle according to the data measured by the gyroscope. Considering that the mobile phone is not always in the aforementioned basic posture during actual walking, such as slight left-right swing along with the alternation of the steps of the pedestrian during walking, and angular disturbance caused by the up-down fluctuation of the center of gravity, etc., the first step of correction is performed by using (3):

heading＝c₁·Pitch+c₂·Roll+c₃·Azimuth (2)

wherein Pitch, Roll, Azimuth correspond to FIG. 2, wherein the rotation amounts around the three axes x, y, z are Pitch angle, Roll angle, Azimuth angle, respectively, and are denoted as Pitch, Roll, Azimuth, c₁,c₂,c₃The corresponding weighting coefficients are obtained by testing in an actual scene, and are assigned as c in the experiment₁＝1,c₂＝0.3,c₃0.1. On the basis, the obtained angle is subjected to second-step correction, namely successive smoothing, mainly to prevent a burr value from appearing to influence the course angle precision in a certain step, but the smoothing is only limited to a straight line walking or a relatively stable process, such as a horizontal segment of a curve in fig. 5, and the calculation is specifically carried out by the following formula

Where mean (-) means taking the average.

And thirdly, fusing the map information and the PDR estimation result through particle filtering. And simplifying the motion model relative to Kalman Filtering (KF, Kalman Filtering) so as to obtain an optimal solution, wherein the particle Filtering does not simplify the model but directly calculates a suboptimal solution of the complex motion model so as to gradually approach the optimal solution. The particle filtering is a Bayes filtering method for approximating the posterior probability by adopting a Monte Carlo method, and initial conditions such as setting a motion model as a linear model and the like are not needed.

The state estimation of a dynamic system is generally described by two equations, one for each

The state equation is as follows: s_k＝F(s_k-1,n_k-1) (9)

The observation equation: m is_k＝G(s_k,v_k) (10)

Wherein F (-), G (-) are the system states s_k-1,s_kFunction of s_kIs a state variable at time k, m_kIs s is_kOf the observed value, system noise n_kAnd observation noise v_kAre random variables whose probability densities are known, independent of each other, and independent of the system state.

The state equation of the particle filter adopted in the present embodiment is

Wherein x is_k,y_kDenotes the position coordinates at step k, h_kIs the estimated heading angle according to embodiment two,in order to estimate the error for the step size,and (3) estimating an error, sin (·), cos (·) for the course angle, wherein the sin value and the cosine value of the corresponding angle are solved respectively.

WhereinRepresenting the estimated location of the PDR for the k-th stepDenotes the position of the ith particle at step k, and σ denotes the standard deviation of the PDR position estimate. When a particle generated in the k step is not in the effective position range (such as wall penetration, etc.), w_iAnd taking a value of 0, otherwise, calculating the weight according to condition _ 1.

The specific implementation process is as follows: firstly, an initial position (0, 0) is given, whether an effective step is detected, if yes, the step length and the course angle are estimated, a rough position is estimated through PDR according to the step length and the course angle, the positions of the particles are calculated by combining 100 randomly generated particles (regeneration is needed in each step), then the weight of each particle is calculated according to the condition (8), the positions of all the particles are weighted and summed to obtain the position after PF filtering, then the next position estimation is carried out by repeating the above process, and thus, the position tracking of the pedestrian is realized.

And fourthly, finally, acquiring data through a NEXUS5 smart phone in the experimental environment shown in the figure 3 (according to the posture of the mobile phone in the figure 2, a tester holds the mobile phone to walk around the experimental corridor to record the data), wherein the acquired data comprise acceleration values, angular velocity values and sampling time, then performing simulation calculation in MATLAB2012b, and finally obtaining a curve shown in the figure 6 according to the proposed algorithm. It can be seen that the pedestrian position estimation method through the PZVT step detection, the course angle correction and the PF fusion map information improves the estimation accuracy compared with the method of estimating the position only by using the PDR.

Firstly, an initial position (0, 0) is given, whether an effective step is detected, if yes, the step length and the course angle are estimated, a rough position is estimated through PDR according to the step length and the course angle, the positions of the particles are calculated by combining 100 randomly generated particles (regeneration is needed in each step), then the weight of each particle is calculated according to conditions, the positions of all the particles are weighted and summed to obtain the position after PF filtering, then the process is repeated to carry out next position estimation, and therefore the position of each step is repeatedly calculated, and the position tracking of pedestrians is achieved.

Claims

1. The pedestrian indoor position tracking method based on the inertial sensor is characterized by comprising the following steps of:

2. The inertial sensor-based pedestrian position tracking method according to claim 1, characterized in that said step one is as follows:

3. The inertial sensor-based pedestrian position tracking method according to claim 2, wherein the second heading angle estimation method is:

and (3) correcting the course angle in the first step:

heading＝c₁·Pitch+c₂·Roll+c₃·Azimuth (3)

wherein Pitch angle Pitch represents the amount of rotation about the x-axis, Roll angle Roll represents the amount of rotation about the y-axis, and Azimuth angle Azimuth represents the amount of rotation about the z-axis, where c₁,c₂,c₃Is the corresponding weighting coefficient;

<math> <mrow> <msub> <mi>heading</mi> <mi>i</mi> </msub> <mo>=</mo> <mi>mean</mi> <mrow> <mo>(</mo> <munderover> <mi>Σ</mi> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>i</mi> </munderover> <msub> <mi>heading</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow> </math>

mean (-) indicates the mean.

4. The inertial sensor-based pedestrian position tracking method according to claim 3, wherein the fusing of the map information and the PDR estimation result by particle filtering in step three is as follows:

firstly, the state equation of the adopted particle filter is as follows:

<math> <mrow> <msub> <mi>w</mi> <mi>i</mi> </msub> <mo>=</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <mi>exp</mi> <mrow> <mo>(</mo> <mo>-</mo> <mfrac> <msup> <mrow> <mo>|</mo> <mo>|</mo> <msubsup> <mi>pos</mi> <mi>k</mi> <mi>PDR</mi> </msubsup> <mo>-</mo> <msubsup> <mi>pos</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>|</mo> <mo>|</mo> </mrow> <mn>2</mn> </msup> <mrow> <mn>2</mn> <msup> <mi>σ</mi> <mn>2</mn> </msup> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>,</mo> <mi>condition</mi> <mo>_</mo> <mn>1</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> <mo>,</mo> <mi>condition</mi> <mo>_</mo> <mn>2</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow> </math>

whereinRepresenting the estimated location of the PDR for the k-th stepDenotes the position of the ith particle in the k-th step, σ denotes the standard deviation of the PDR position estimate, and w is the position of the particle generated in the k-th step when it is not within the valid position range_iTaking a value of 0, otherwise, calculating a weight according to condition _ 1;