CN104061934B - Pedestrian indoor position tracking method based on inertial sensor - Google Patents

Abstract

The invention discloses a pedestrian indoor position tracking method based on an inertial sensor, relates to a pedestrian indoor position tracking method and particularly relates to a PDR (precision depth recorder) method and a PF fusion map information algorithm based on the inertial sensor. The pedestrian indoor position tracking method disclosed by the invention is used for solving the conditions such as great position estimation errors and even wrong estimation, and the like caused by long-time drift performance of the inertial sensor when only the PDR is used for tracking the pedestrian position. The method comprises the following steps of I. detecting steps and estimating step length according to an acceleration sensor; II. estimating a course angle according to the change of three-axle angular velocity in measured data of a gyroscope, correcting the course angle, carrying out course calculation according to the corrected course angle and the step length of the first step; and finally, estimating the position according to the step length and the course angle through the PDR method; and III. fusing map information with the PDR estimation by particle filtering to complete the pedestrian indoor position tracking method based on the inertial sensor. The pedestrian indoor position tracking method disclosed by the invention is applied to the technical field of indoor positioning.

Description

Pedestrian's indoor location tracking based on inertial sensor

Technical field

The present invention relates to a kind of pedestrian's indoor location tracking, and in particular to PDR methods based on inertial sensor and PF merges cartographic information algorithm.

Background technology

In recent years, the development of microelectromechanical systems (Micro-ElectroMechanical Systems, MEMS), made Obtain inertial sensor to be popularized in the application of intelligent mobile terminal.Based on pedestrian's indoor navigation system of inertial sensor, with It need not lay the low-cost advantage of outside plant, be increasingly becoming research hot topic.The general principle of the system is navigated using pedestrian Mark predication method (PDR, Pedestrian Dead-Reckoning), according to the survey of inertial sensor (such as accelerometer, gyroscope) Amount data, are mainly concerned with inspection step, estimate the technologies such as step-length, estimated course angle, therefore this system is otherwise referred to as SHSs (Step-and-Heading Systems).Early stage uses wearable device, and sensor is installed on shoes, the helmet, is put In pocket, waist etc., motion feature can more be reflected in foot in walking, so step can preferably be examined based on Foot sensor, But need additionally to purchase Special Equipment and not Portable belt, it is not suitable for common pedestrian's indoor navigation system.In walking process In, acceleration transducer can export certain walking characteristics, by carrying out peakvalue's checking, zero passage detection to measurement data, from phase Close matching, spectrum analysis etc., one of which or it is several combine to recognize each step, latter two because time-consuming and cause real-time Property is not strong.For step-size estimation, most rough is directly to set step-length as a constant, because pedestrian has one under constant speed walking Individual average step length, but in reality, step-length by the height of a people, build, change the factors such as synchronizing frequency and affected, it is not possible to Yi Gaiwei Average.Researchers generally receive four root sides of a minimax acceleration difference formula calculate, but also it has been proposed that its , as based on cadence and the wired sexual intercourse of step-length, the sensor for dressing foot adopts Zero velocity Updating (ZUPT, Zero-velocity for it UPdaTe), based on pedestrian's height unique step method of estimation.And in course estimation, simplest method of estimation is to consolidate sensor Due to pedestrian, sensor coordinate system and terrestrial coordinate system (compass coordinate system, N-E coordinate systems) three overlapping of axles are made, so sensed The azimuth that device is measured is the course angle of pedestrian, then frequently by the component by gravity on the axle of acceleration transducer three with And magnetometer (electronic compass) is tried to achieve to calculate, and generally, the placement of sensor is that (i.e. attitude is appointed without fixed pose Meaning property), this is to need to come estimated course, such as Principal Component Analysis Method (PCA, Primary Components by other methods Analyze) estimated course angle.

Particle filter (PF, Particle Filter) is Gauss, the linear hypothesis bar of the state model in Kalman filter In the case of part is ungratified rise filter tracking algorithm, it by using Monte Carlo method sample MAP estimation, so as to Approach Bayes Optimum estimation.At present various indoor positionings such as WLAN indoor positionings, UWB indoor positioning, in view-based access control model room Positioning, inertial sensor indoor positioning etc. are all improved positioning precision using particle filter and solve the problems, such as multiple target tracking, And merge the data of different alignment systems using particle filter, so as to mutually aid in improving positioning result.Jing is frequently with grain Son filtering is added to cartographic information in location estimation, the parameter during according to the map information is to adjust particle filter, so as to not Precision is only improved, the generation that the special circumstances such as through walls occurs in tracking target is also prevent.

The content of the invention

The present invention is to solve when being tracked to pedestrian position using PDR merely, because what inertial sensor itself had Drift when long and cause position estimation error big or even situations such as mistake estimates, and provide based on the pedestrian of inertial sensor Indoor location tracking.

Realized according to the following steps based on pedestrian's indoor location tracking of inertial sensor：

First, carried out examining step and step-size estimation according to acceleration transducer, detailed process is：Using peak value-null value-valley- The method of time interval carries out inspection step, i.e., often row makes a move comprising 1 peak acceleration, 2 null values, 1 minimum acceleration, and And meet time interval rationally, and it is that 2～4 steps are per second according to the speed of adult's constant speed walking, the lower limit that sets interval is 250 millis Second, S₀Represent starting, S_i, i=1 ... 9 represents the i-th step, and step-size estimation obtained by following formula

Wherein sLen be estimate step-length, acc_i、acc_AveAccekeration, the average acceleration in each step is represented respectively Value, N represents the data amount check collected in often step；

2nd, the three axis angular rates change in gyroscope measurement data carries out course angle estimation, and to course angle school is carried out Just, then reckoning carried out according to the course angle after correction and first step step-length, finally according to step-length, course angle PDR is passed through Method estimated location：

Wherein,Represent the PDR estimated locations of kth steph_kThe course angle that kth step is estimated is represented, sLen_kRepresent the step-length of kth step；

3rd, cartographic information and PDR estimated results are merged by particle filter, that is, is completed based on the row of inertial sensor People's indoor location tracking.

Invention effect：

Based on the pedestrian position tracking of inertial sensor, by the inertial sensor module in smart mobile phone, pedestrian Dead reckoning position according to a preliminary estimate, then merges cartographic information using particle filter algorithm, provides the user real time position letter Breath.In pedestrian's dead reckoning is realized, a kind of PZVT inspection step algorithms based on accekeration and time are proposed, estimated by step-length Meter, for gyroscope angle integration have long when Excursion Characteristic, corrected mutually using three axle angle changes, enter in experimental situation Test is gone, position estimation error is relatively reduced.Estimated location is filtered using particle filter combining cartographic information, one Determine the degree of accuracy that position estimation is improve in degree.

Description of the drawings

Fig. 1 is based on the PDR theory diagrams of inertial sensor；

Fig. 2 is mobile phone sensor coordinate system, electronic compass coordinate system diagram；

Fig. 3 is based on inertial sensor pedestrian position tracking test environment schematic；

Fig. 4 is Z axis accekeration and inspection step result；

Fig. 5 be do not correct and correct after course angle comparison diagram；

Fig. 6 is the position tracking comparison diagram for using PF and only using DR.

Specific embodiment

Specific embodiment one：Pedestrian's indoor location tracking based on inertial sensor of present embodiment is by following Step is realized：

First, carried out examining step and step-size estimation according to acceleration transducer；

2nd, the three axis angular rates change in gyroscope measurement data carries out course angle estimation, and to course angle school is carried out Just, then reckoning carried out according to the course angle after correction and first step step-length, finally according to step-length, course angle PDR is passed through Method estimated location：

Wherein,Represent the PDR estimated locations of kth steph_kThe course angle that kth step is estimated is represented, sLen_kRepresent the step-length of kth step；

3rd, cartographic information and PDR estimated results are merged by particle filter, that is, is completed based on the row of inertial sensor People's indoor location tracking.

Specific embodiment two：Present embodiment from unlike specific embodiment one：Step one detailed process It is as follows：

Inspection step is carried out using the method for peak value-null value-valley-time interval, i.e., often row makes a move comprising 1 maximum acceleration Degree, 2 null values, 1 minimum acceleration, and meet time interval rationally, it is 2～4 steps according to the speed of adult's constant speed walking Per second, the lower limit that sets interval is 250 milliseconds, S₀Represent starting, S_i, the i-th steps of expression of i=1 ... 9, step-size estimation is by following formula To obtain

Wherein sLen be estimate step-length, acc_i、acc_AveAccekeration, the average acceleration in each step is represented respectively Value, N represents the data amount check collected in often step.

Other steps and parameter are identical with specific embodiment one.

Specific embodiment three：Present embodiment from unlike specific embodiment one or two：The step 2 Air China It is to angular estimation method：

First time integral is carried out to three axis angular rates around x, y, z, respectively obtains the angle of pitch, roll angle, azimuth, It is denoted as Pitch, Roll, Azimuth；

First step correction is carried out to course angle using (3)：

Heading=c₁·Pitch+c₂·Roll+c₃·Azimuth (3)

Wherein, angle of pitch Pitch represents the rotation amount around x-axis, and roll angle Roll represents the rotation around y-axis Amount, azimuth Azimuth represents the rotation amount around the axles of z tri-, wherein c₁,c₂,c₃For corresponding weight coefficient；

Second step correction is carried out to the course angle after first step correction, i.e., is gradually smoothed, calculated especially by following formula：

Mean () is represented and averaged.

Other steps and parameter are identical with specific embodiment one or two.

Specific embodiment four：Unlike one of present embodiment and specific embodiment one to three：The step 3 It is by particle filter that cartographic information and PDR estimated result fusion methods is as follows：

Particle filter is a kind of Bayesian filter method that posterior probability is approached using monte carlo method, does not require fortune Dynamic system is linear or Gauss；

The state equation of the particle filter for the, adopting for：

Wherein, x_k,y_kPosition coordinates when representing that kth is walked, x_k-1,y_k-1Represent the position coordinates of the step of kth -1, h_kIt is to estimate The course angle for going out,For step-size estimation error,For course angle estimation error, l_kThe step-length that expression is estimated, sin (), cos () represents respectively sine value, the cosine value for seeking corresponding angle；

2nd, value w is weighted to all particles of kth step_iCalculate：

WhereinRepresent the PDR estimated locations of kth step Represent i-th particle of kth step Position, σ represents the standard deviation of PDR location estimations, when kth step in generate certain particle not in the range of active position when, then w_i0 value is taken, otherwise weights is calculated according to condition_1；

3rd, cartographic information and PDR estimated results are merged by particle filter：

In all particle weighted values w for obtaining kth step_iAfterwards, according to w_iCalculating has used the estimation that the kth of particle filter is walked Position：

Wherein, Np represents the particle number in kth step,Represent the estimated location to i-th particle in kth stepIt is calculated according to formula (5), is normalization weighted value, is calculated by following formula：

The position for constantly estimating to PDR carries out particle filter, obtains new estimated location.

The state estimation of dynamical system is typically described by two equations, respectively

State equation：s_k=F (s_k-1,n_k-1) (9)

Observational equation：m_k=G (s_k,v_k) (10)

Wherein F (), G () are system modes s_k-1,s_kFunction, s_kFor k moment state variables, m_kFor s_kObservation Value, system noise n_kWith observation noise v_kIt is separate known to probability density, and with the stochastic variable of system mode independence；

Other steps and parameter are identical with one of specific embodiment one to three.

With reference to being described in detail to specific embodiment, the pedestrian position tracking based on inertial sensor leads to Cross following step realization：

First, carried out examining step and step-size estimation according to acceleration transducer：

Specific embodiment one is described in detail with reference to Fig. 2, Fig. 3.Fig. 3 is Harbin Institute of Technology's research park 2A 12 layers of plan is illustrated, experiment (the dark filling in figure in the corridor of the floor based on the tracking of inertial sensor pedestrian position Region), wide 3 meters or so of corridor is long about 90 meters.It is shown in Fig. 2 in experiment test, the appearance of tester's handheld mobile phone State, and mobile phone inner sensor coordinate system (Sensor coordinate systems), navigational coordinate system (E-N coordinate systems).

People in the process of walking, the acceleration change that the different parts of body can be different, but it is all regular, so can Identify whether walking, or the action such as static therefrom to extract information.Using peak value-null value-valley-time interval (PZVT) examining step, i.e., often row makes a move comprising 1 peak acceleration, 2 null values, 1 minimum acceleration (such as Fig. 4 method It is shown), and meet time interval rationally, it is that 2-4 steps are per second according to the speed of adult's constant speed walking, set interval lower limit For 250 milliseconds.Effective step that the summit box indicating of waveform is detected, S₀Represent starting, S_i, the i-th steps of expression of i=1 ... 9.

Actual measurement PZVT inspection step algorithms understand that the inspection that present embodiment is adopted walks arithmetic accuracy substantially 100%, that is, miss Inspection, missing inspection, the situation of many inspections seldom occur.

Step-length is not only relevant with the height of people, and also relevant with the factor such as the cadence in walking, same person is in walking Each step is all variant, so step-size estimation is a very challenging problem.This patent adopts formula

Wherein sLen be estimate step-length, acc_i、acc_AveAccekeration, the average acceleration in each step is represented respectively Value, k is the corresponding coefficient of each method, and value of the present invention 1.07, N represents the data amount check collected in a step.

2nd, course angle estimation is carried out according to gyroscope measurement data.It is not always all in view of mobile phone in actual walking In above-mentioned basic poses, such as walk process, meeting is slight as pedestrian's step alternately has to swing, with And due to angular disturbance etc. caused by center of gravity dipping and heaving, so carrying out first step correction to it using (2)：

Heading=c₁·Pitch+c₂·Roll+c₃·Azimuth (2)

Wherein Pitch, Roll, Azimuth are corresponded to shown in Fig. 2 respectively, wherein the rotation amount around the axle of x, y, z three is distinguished For the angle of pitch, roll angle, azimuth, Pitch, Roll, Azimuth, c are denoted as₁,c₂,c₃For corresponding weight coefficient, need Test in actual scene is obtained, and c is entered as in this experiment₁=1, c₂=0.3, c₃=0.1.Angle to obtaining on this basis Second step correction is carried out, i.e., is gradually smoothed, primarily to the boat in preventing jagged value from occurring so as to affect a certain step To angular accuracy, but straight line moving or the more stable process of being only limitted to is smoothed, the horizontal segment of the curve in such as Fig. 5, specifically Calculated by following formula

Wherein, mean () is represented and averaged.

3rd, cartographic information and PDR estimated results are merged by particle filter.Relative to Kalman filtering (KF, Kalman Filtering) simplify motion model, so as to try to achieve optimal solution, particle filter does not simplify to model, but directly calculate multiple The suboptimal solution of miscellaneous motion model, so as to gradually approach optimal solution.Particle filter is after one kind is approached using monte carlo method Test the Bayesian filter method of probability, it is not necessary to set motion model as primary condition such as linear models.

The state estimation of dynamical system is typically described by two equations, respectively

State equation：s_k=F (s_k-1,n_k-1) (9)

Observational equation：m_k=G (s_k,v_k) (10)

Wherein F (), G () are system modes s_k-1,s_kFunction, s_kFor k moment state variables, m_kFor s_kObservation Value, system noise n_kWith observation noise v_kIt is separate known to probability density, and with the stochastic variable of system mode independence.

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

Wherein, x_k,y_kPosition coordinates when representing that kth is walked, h_kIt is the course angle estimated according to specific embodiment two,For step-size estimation error,For course angle estimation error, sin (), cos () represent respectively ask corresponding angle sine value, Cosine value.

WhereinRepresent the PDR estimated locations of kth step Represent i-th particle of kth step Position, σ represents the standard deviation of PDR location estimations.When kth step in generate certain particle not in the range of active position when (ratio Such as situations such as through walls), then w_i0 value is taken, otherwise weights is calculated according to condition_1.

The process of implementing is：Give first initial position (0,0), then detect whether it is an effectively step, such as Fruit is, it estimates that long, course angle step by step, according to step-length, course angle by PDR estimation rough positions, with reference to what is randomly generated 100 particles (being required for regenerating in each step), calculate the position of particle, then calculate each according to the condition of (8) All particle position weighted sums are obtained the filtered positions of PF by the weights of particle, then repeatedly above procedure carry out it is next Location estimation, so computes repeatedly each step and puts, and realizes the position to pedestrian and tracks.

4th, the smart mobile phone gathered datas of NEXUS 5 are passed through (according in Fig. 2 in last experimental situation shown in figure 3 Mobile phone places attitude, tester's handheld mobile phone capable record data that takes a round in experiment corridor), the data for collecting have acceleration Value, magnitude of angular velocity and sampling time, simulation calculation is then carried out in MATLAB 2012b, it is final according to the algorithm for being proposed Obtain curve as shown in Figure 6.Can see, through PZVT the pedestrian that step, course angle correction and PF merge cartographic information is examined Location estimation method is compared to and simply uses PDR estimated locations, improves estimation accuracy.

Give first initial position (0,0), then detect whether it is an effectively step, if it is, estimating the step Step-length, course angle, estimate rough position, with reference to 100 particles for randomly generating (each according to step-length, course angle by PDR It is required for regenerating in step), the position of particle is calculated, the weights of each particle are then calculated according to the condition of (8), to all Particle position weighted sum obtains the filtered positions of PF, and then repeatedly above procedure carries out the next position estimation, so repeats Calculate each step to put, realize the position to pedestrian and track.

Claims (3)

1. pedestrian's indoor location tracking of inertial sensor is based on, it is characterised in that it is realized according to the following steps：

First, carried out examining step and step-size estimation according to acceleration transducer, detailed process is：Using peak value-null value-valley-time The method at interval carries out inspection step, i.e., often row makes a move comprising 1 peak acceleration, 2 null values, 1 minimum acceleration, and full Sufficient time interval is that 2～4 steps are per second according to the speed of adult's constant speed walking rationally, and the lower limit that sets interval is 250 milliseconds, S₀Represent starting, S_i, i=1 ... 9 represents the i-th step, and step-size estimation obtained by following formula

$sLen=1.07\·\sqrt[3]{{\mathrm{acc}}_{Ave}},{\mathrm{acc}}_{Ave}=\frac{\underset{i=1}{\overset{N}{\Σ}}\left|{\mathrm{acc}}_i\right|}{N}---\left(2\right)$

Wherein sLen be estimate step-length, acc_i、acc_AveAccekeration, the average acceleration value in each step, N tables are represented respectively Show the data amount check collected in every step；

2nd, the three axis angular rates change in gyroscope measurement data carries out course angle estimation, course angle is corrected, so Afterwards reckoning is carried out according to the course angle after correction and first step step-length, estimated by PDR methods finally according to step-length, course angle Meter position：

${\mathrm{pos}}_k^{PDR}=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right]=\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\end{array}\right]+{\mathrm{sLen}}_k\·\left[\begin{array}{c}cos\left(h_k\right)\\ sin\left(h_k\right)\end{array}\right]---\left(1\right)$

Wherein,Represent the PDR estimated locations of kth steph_kRepresent the course angle that kth step is estimated, sLen_kGeneration The step-length of table kth step；

3rd, cartographic information and PDR estimated results are merged by particle filter, that is, is completed based on the pedestrian room of inertial sensor Interior location tracking method.

2. according to the pedestrian's indoor location tracking based on inertial sensor described in claim 1, it is characterised in that institute Stating course angle estimation method in step 2 is：

First time integral is carried out to three axis angular rates around x, y, z, respectively obtain the angle of pitch, roll angle, azimuth, be denoted as Pitch,Roll,Azimuth；

First step correction is carried out to course angle using (3)：

Heading=c₁·Pitch+c₂·Roll+c₃·Azimuth (3)

Wherein, angle of pitch Pitch represents the rotation amount around x-axis, and roll angle Roll represents the rotation amount around y-axis, side Parallactic angle Azimuth represents the rotation amount around the axles of z tri-, wherein c₁,c₂,c₃For corresponding weight coefficient；

Second step correction is carried out to the course angle after first step correction, i.e., is gradually smoothed, calculated especially by following formula：

${\mathrm{heading}}_i=mean\left(\underset{k=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{heading}}_k\right)---\left(4\right)$

Mean () is represented and averaged.

3. according to the pedestrian's indoor location tracking based on inertial sensor described in claim 2, it is characterised in that institute State the by particle filter that cartographic information and PDR estimated result fusion methods is as follows of step 3：

Particle filter is a kind of Bayesian filter method that posterior probability is approached using monte carlo method, does not require kinetic system System is linear or Gauss；

The state equation of the particle filter for the, adopting for：

$s_k=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right]=\left[\begin{array}{ccc}1& 0& (l_k+n_k^l)\·cos(\·)\\ 0& 1& (l_k+n_k^l)\·sin(\·)\end{array}\right]\·(\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\\ h_k\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ n_k^h\end{array}\right])---\left(5\right)$

Wherein, x_k,y_kPosition coordinates when representing that kth is walked, x_k-1,y_k-1Represent the position coordinates of the step of kth -1, h_kEstimate Course angle,For step-size estimation error,For course angle estimation error, l_kThe step-length that expression is estimated, sin (), cos () Sine value, the cosine value for seeking corresponding angle is represented respectively；

2nd, value w is weighted to all particles of kth step_iCalculate：

$w_i=\left\{\begin{array}{c}\exp (-\frac{\left|\right|{\mathrm{pos}}_k^{PDR}-{\mathrm{pos}}_k^i|{|}^2}{2{\mathrm{\σ}}^2}),condition\_1\\ 0,condition\_2\end{array}\right.---\left(6\right)$

WhereinRepresent the PDR estimated locations of kth step The position of i-th particle of kth step is represented, σ represents the standard deviation of PDR location estimations, when certain particle generated in kth step not in the range of active position when, then w_iTake 0 Value, otherwise calculates weights according to condition_1；

3rd, cartographic information and PDR estimated results are merged by particle filter：

In all particle weighted values w for obtaining kth step_iAfterwards, according to w_iCalculating has used the estimated location that the kth of particle filter is walked：

${\mathrm{pos}}_k^{PF}=\underset{i=1}{\overset{Np}{\Σ}}{\tilde{w}}_i\·{\mathrm{pos}}_k^i---\left(7\right)$

Wherein, Np represents the particle number in kth step,Represent the estimated location to i-th particle in kth step It is calculated according to formula (5), is normalization weighted value, is calculated by following formula：

${\tilde{w}}_i=\frac{w_i}{\underset{i=1}{\overset{Np}{\Σ}}{w}_i}---\left(8\right)$

The position for constantly estimating to PDR carries out particle filter, obtains new estimated location.