CN104613964A - Pedestrian positioning method and system for tracking foot motion features - Google Patents

Abstract

The invention provides a pedestrian positioning method for tracking foot motion features. The method comprises the following steps: acquiring initial position information of a pedestrian and sensing data of the pedestrian at each moment; computing move position information of the pedestrian at the k-th moment according to the sensing data so as to acquire uncorrected move position information of the pedestrian at the k-th moment, wherein the move position information comprises position information computation error; by using one foot of both feet of the pedestrian as a reference foot, judging whether the reference foot is in the state of rest at the k-th moment according to a Bayesian speculation algorithm; if the reference foot is in the state of rest, continuously executing the next step; otherwise, outputting the uncorrected move position information of the pedestrian at the k-th moment; estimating the information position computation error; correcting the move position information of the pedestrian at the k-th moment, and outputting the corrected move position information of the pedestrian at the k-th moment. The detection efficiency and precision of zero speed are improved, and the error compensation on an inertial navigation pedestrian positioning algorithm is realized.

Description

A kind of pedestrian's localization method and system of following the tracks of foot motion feature

Technical field

The invention belongs to indoor positioning technologies field, relate to a kind of pedestrian's localization method and system, particularly relate to a kind of pedestrian's localization method and system of following the tracks of foot motion feature.

Background technology

Along with the fast development of MEMS (micro electro mechanical system) (MEMS), the pedestrian's navigational system based on microsensor is made to become possibility.But the sensor component of low cost, such as three axis accelerometer, three-axis gyroscope, three axle magnetometers etc., exist and inevitably measure drift, pedestrian's inertial positioning algorithm can be made within the section time to follow the tracks of inefficacy, because the error of raw data makes the site error calculated disperse with the cube of time through twice integration.But, according to human cinology's achievement in research, stride in the cycle a people, there is a foot motion disable time section, in foot motion disable time section, we zero velocity can be used to calibrate EKF (extended Kalman filter) that (ZUPT) and zero angular velocity calibrate (ZARU) auxiliary compensates error, thus make error-reduction that pedestrian's inertial positioning is estimated to tolerance interval.

By a large amount of literature surveys and experimental demonstration, a high-precision zero velocity checks algorithm, great to the Accuracy of final pedestrian location, especially to the Accuracy of high computational.Traditional zero velocity inspection method, based on the raw data of three axis accelerometer and three-axis gyroscope, utilizes associating threshold condition determination methods or Corpus--based Method to gain knowledge and judges.But there is error in judgement point and the interval ill-defined problem of zero velocity in zero velocity interval when high-speed motion in these methods.Therefore, existing pedestrian's localization method calculates not efficient, unstable, and the accuracy that zero velocity detects is not high.

Therefore, how a kind of pedestrian's localization method and system of following the tracks of foot motion feature are provided, to solve pedestrian's localization method of the prior art and system cannot meet real-time calculation requirement, and calculate not efficient, unstable, the not high many disadvantages of zero velocity detection accuracy, has become practitioner in the art's technical matters urgently to be resolved hurrily in fact.

Summary of the invention

The shortcoming of prior art in view of the above, the object of the present invention is to provide a kind of pedestrian's localization method and system of following the tracks of foot motion feature, real-time calculation requirement cannot be met for solving pedestrian's localization method and system in prior art, and calculate not efficient, instability, the problem that zero velocity detection accuracy is not high.

For achieving the above object and other relevant objects, one aspect of the present invention provides a kind of pedestrian's localization method following the tracks of foot motion feature, performed by the locating module and sensing module that are worn on pedestrian upper body and foot respectively, wherein, described sensing module comprises three-axis gyroscope, three axis accelerometer, three axle magnetometers, pedestrian's localization method of described tracking foot motion feature comprises: the initial position message obtaining the described pedestrian provided by described locating module, and the described pedestrian provided by described sensing module is at the sensing data in each moment; Wherein, described sensing data comprises the initial motion directional information of described pedestrian; The mobile location information in described pedestrian's kth moment is calculated to obtain the mobile location information in unregulated described pedestrian's kth moment according to described sensing data; Described mobile location information comprises positional information calculation error; Wherein, k be more than or equal to 1 positive integer; Using a pin in described pedestrian's both feet as with reference to pin, judge whether described reference pin remained static in the kth moment according to Bayesian inference algorithm; If so, continue to perform next step; If not, the mobile location information in unregulated described pedestrian's kth moment is exported; Described positional information calculation error is estimated; Calibrate the mobile location information in described pedestrian's kth moment, export the mobile location information in the described pedestrian's kth moment after calibration.

Alternatively, inertial positioning algorithm is adopted to calculate the mobile location information in described pedestrian's kth moment.

Alternatively, the step that described employing inertial positioning algorithm calculates the mobile location information in described pedestrian's kth moment is based on three-axis gyroscope y-axis data in described sensing module, calculates according to the signal characteristic that strides in three-axis gyroscope y-axis signal the cycle of striding.

Alternatively, judge that described reference pin specifically comprises in the step whether the kth moment remains static according to Bayesian inference algorithm: calculate striding the cycle of described pedestrian based on described sensing data; The described cycle of striding comprises pin contact phase, pin stance, and less touch with the ground stage and pin control the stage; Which in the cycle of striding should to be according to the described pedestrian of pre-defined rule correspondence inquiry in the kth moment in stage, and to obtain Query Result; Arrange described in threshold decision according to the gyroscope prestored and whether remain static with reference to pin, obtain the first judged result; If so, then described first judged result and described Query Result are compared, and judge that whether described first judged result is consistent with described Query Result, if so, then represent there is not potential error point; If not, then represent to there is potential error point, estimate whether described potential error point is in zero velocity interval by naive Bayesian, if, then represent described to remain static in the kth moment with reference to pin, if not, then represent described and be kept in motion in the kth moment with reference to pin.

Alternatively, be based on EKF method, described positional information calculation error is estimated in the step described positional information calculation error estimated.

Alternatively, described positional information calculation error comprises attitude error, and angular velocity is biased, site error, velocity error and acceleration are biased.

Alternatively, the step of calibrating the mobile location information in described pedestrian's kth moment also comprises: adopt attitude error to be revised direction cosine matrix by an attitude error matrix; Adopt angular velocity to be biased to compensate the angular velocity in kth+1 moment; Wherein, the angular velocity+angular velocity in the angular velocity=kth moment in kth+1 moment is biased; Adopt acceleration to be biased to compensate the acceleration in kth+1 moment; Wherein, the acceleration+acceleration in the acceleration=kth moment in kth+1 moment is biased; The position of site error to the kth moment is adopted to compensate; Wherein, the positional information-site error in the mobile location information in position=unregulated described pedestrian's kth moment in described kth moment; The speed of velocity error to the kth moment is adopted to compensate; Wherein, the speed-velocity error in the mobile location information in speed=unregulated described pedestrian's kth moment in described kth moment.

The present invention also provides a kind of pedestrian's positioning system of following the tracks of foot motion feature on the other hand, follow the tracks of pedestrian locate by performing with the locating module and sensing module that are worn on pedestrian upper body and foot respectively, wherein, described sensing module comprises three-axis gyroscope, three axis accelerometer, three axle magnetometers, pedestrian's positioning system of described tracking foot motion feature comprises: data acquisition module, for obtaining the initial position message of the described pedestrian provided by described locating module, and the described pedestrian to be provided by described sensing module is at the sensing data in each moment, wherein, described sensing data comprises the initial motion directional information of described pedestrian, computing module, for calculating the mobile location information in described pedestrian's kth moment to obtain the mobile location information in unregulated described pedestrian's kth moment according to described sensing data, described mobile location information comprises positional information calculation error, wherein, k be more than or equal to 1 positive integer, according to Bayesian inference algorithm, judge module, for using a pin in described pedestrian's both feet as with reference to pin, judges whether described reference pin remained static in the kth moment, if, then call the estimation module for estimating described positional information calculation error, for calibrating the calibration module of the mobile location information in described pedestrian's kth moment, and for the first output module of the mobile location information that exports the described pedestrian's kth moment after calibration, if not, the second output module of the mobile location information for exporting unregulated described pedestrian's kth moment is called.

Alternatively, described computing unit adopts inertial positioning algorithm to calculate the mobile location information in described pedestrian's kth moment.

Alternatively, described judge module comprises: the computation of Period that strides unit, for calculating striding the cycle of described pedestrian based on described sensing data; The described cycle of striding comprises pin contact phase, pin stance, and less touch with the ground stage and pin control the stage; Query unit, for should to be in which in the cycle of striding in the kth moment according to the described pedestrian of pre-defined rule correspondence inquiry, and obtains Query Result in stage; Whether the first judging unit, remaining static with reference to pin for arranging according to the gyroscope prestored described in threshold decision, obtaining the first judged result; If so, then call for described first judged result and described Query Result are compared, judge that whether described first judged result is consistent with described Query Result, if so, then represent there is not potential error point; If not, then represent to there is potential error point, call for being estimated by naive Bayesian whether described potential error point is in second judging unit in zero velocity interval, if, then represent described to remain static in the kth moment with reference to pin, call described estimation module and calibration module, if not, then represent described to be kept in motion in the kth moment with reference to pin, call described second output module.

Alternatively, described estimation module is estimated to obtain attitude error to described positional information calculation error based on extended Kalman filter, and angular velocity is biased, site error, velocity error and acceleration are biased.

Alternatively, described calibration module comprises: the first compensating unit, is revised by an attitude error matrix for adopting attitude error to direction cosine matrix; Second compensating unit, is biased for adopting angular velocity and compensates the angular velocity in kth+1 moment; Wherein, the angular velocity+angular velocity in the angular velocity=kth moment in kth+1 moment is biased; 3rd compensating unit, is biased for adopting acceleration and compensates the acceleration in kth+1 moment; Wherein, the acceleration+acceleration in the acceleration=kth moment in kth+1 moment is biased; 4th compensating unit, compensates for adopting the position of site error to the kth moment; Wherein, the positional information-site error in the mobile location information in position=unregulated described pedestrian's kth moment in described kth moment; 5th compensating unit, compensates for adopting the speed of velocity error to the kth moment; Wherein, the speed-velocity error in the mobile location information in speed=unregulated described pedestrian's kth moment in described kth moment.

As mentioned above, pedestrian's localization method of tracking foot motion feature of the present invention and system, have following beneficial effect:

1, the present invention is based on gyroscope and accelerometer raw data, in conjunction with human cinology's principle, adopt Bayesian network model to carry out zero velocity interval to infer, efficiently solve the interval erroneous judgement problem of traditional zero velocity detection method zero velocity and the interval edge blurring problem of zero velocity, improve detector efficiency and the degree of accuracy of zero velocity.

2, the present invention is based on the zero velocity detected interval, the EKF utilizing ZUPT/ZARU to assist carries out error state to realize the error compensation to inertial navigation pedestrian location algorithm.Pedestrian's localization method of described tracking foot motion feature overcomes location cumulative errors (positional information calculation error) that single pedestrian's inertial positioning algorithm causes with the problem of time 3 power expansion, achieve pedestrian's location navigation function available in real time, take into account counting yield, location progress, use general reliable problem, significant to the practical application of pedestrian's positioning system of high position precision.

Accompanying drawing explanation

Fig. 1 is shown as pedestrian's localization method schematic flow sheet of tracking foot motion feature of the present invention.

Fig. 2 is shown as step S3 in pedestrian's localization method of tracking foot motion feature of the present invention and specifically performs schematic flow sheet.

Fig. 3 is shown as human cinology's model schematic.

Fig. 4 is shown as the theory structure schematic diagram of pedestrian's positioning system of tracking foot motion feature of the present invention

Element numbers explanation

Pedestrian's positioning system of 1 tracking foot motion feature

11 sensing modules

12 locating modules

13 data acquisition modules

14 pretreatment module

15 computing modules

16 judge modules

17 estimation module

18 calibration modules

19 first output modules

20 second output modules

S1 ~ S7 step

Embodiment

Below by way of specific instantiation, embodiments of the present invention are described, those skilled in the art the content disclosed by this instructions can understand other advantages of the present invention and effect easily.The present invention can also be implemented or be applied by embodiments different in addition, and the every details in this instructions also can based on different viewpoints and application, carries out various modification or change not deviating under spirit of the present invention.

Refer to accompanying drawing.It should be noted that, the diagram provided in the present embodiment only illustrates basic conception of the present invention in a schematic way, then only the assembly relevant with the present invention is shown in graphic but not component count, shape and size when implementing according to reality is drawn, it is actual when implementing, and the kenel of each assembly, quantity and ratio can be a kind of change arbitrarily, and its assembly layout kenel also may be more complicated.

Below in conjunction with embodiment and accompanying drawing, the present invention is described in detail.

Embodiment one

The present embodiment provides a kind of pedestrian's localization method following the tracks of foot motion feature, performed by the locating module and sensing module that are worn on pedestrian upper body and foot respectively, wherein, described sensing module comprises three-axis gyroscope, three axis accelerometer, three axle magnetometers.Wherein, described three-axis gyroscope responds to the angular velocity in described each moment of pedestrian, and the acceleration in described each moment of pedestrian responded to by described three axis accelerometer, and the magnetic induction density in described each moment of pedestrian responded to by described three axle magnetometers.Refer to Fig. 1, be shown as the pedestrian's localization method schematic flow sheet following the tracks of foot motion feature.Pedestrian's localization method of described tracking foot motion feature comprises the following steps:

S1, obtains the initial position message of the described pedestrian provided by described locating module, and the described pedestrian provided by described sensing module is at the sensing data in each moment; Wherein, described sensing data comprises the initial motion directional information of described pedestrian.In the present embodiment, described three-axis gyroscope, three axis accelerometer, three axle magnetometers are integrated on sensing module, and described sensing module is arranged on the foot that described walking, such as, on instep, on tiptoe, under heel, after heel, and on ankle, be arranged on any one position and all can meet data acquisition request.In the present embodiment, and the initial position message of described pedestrian is provided by GPS locating module or Big Dipper locating module.Also comprise in this step and utilize low-pass filter to remove the pre-treatment step of the high frequency noise in sensing data to described sensing data.

S2, calculates the mobile location information in described pedestrian's kth moment to obtain the mobile location information in unregulated described pedestrian's kth moment according to described sensing data; Described mobile location information comprises positional information calculation error; Wherein, k be more than or equal to 1 positive integer.In the present embodiment, inertial positioning algorithm is adopted to calculate the mobile location information in described pedestrian's kth moment in step S2, mobile location information comprises the acceleration information in described pedestrian's kth moment, the angular velocity information in described pedestrian's kth moment, the positional information in described pedestrian's kth moment, the velocity information in described pedestrian's kth moment and the angular velocity change information in described pedestrian's kth moment, such as, the computing formula calculating the acceleration information in described pedestrian's kth moment is:

$a_k^n=C_{b_{k/k-1}}^n\·a_k^b$ Formula (1) wherein, for being engraved in the acceleration of X-axis/Y-axis/Z axis under terrestrial coordinate system during described pedestrian's kth, k represents the kth moment (current time), that is, n represents the X-axis/Y-axis/Z axis under terrestrial coordinate system; for the direction cosine matrix of kth moment (current time), under described sensing data being converted to terrestrial coordinate system from sensor coordinate system by this direction cosine matrix, k-1 is kth-1 moment (previous moment); for being engraved in the acceleration of first direction/second direction/third direction under sensor coordinate system during described pedestrian's kth, k represents the kth moment (current time), and b represents first direction/second direction/third direction under sensor coordinate system.In like manner, calculate the angular velocity information in described pedestrian's kth moment, positional information, velocity information and angular velocity change information, pass through equally under the angular velocity information in described pedestrian's kth moment, positional information, velocity information and angular velocity change information be converted to terrestrial coordinate system by the direction cosine matrix in kth moment (current time) from sensor coordinate system.Wherein, the direction cosine matrix in kth moment (current time) computing formula be:

$C_{b_{k/k-1}}^n=f(C_{b_{k/k-1}}^n,{\mathrm{\ω}}_k^b)=C_{b_{k-1/k-1}}^n\·\frac{{2I}_{3\×3}+\mathrm{\δ}{\mathrm{\Ω}}_K\·\mathrm{\Δt}}{{2I}_{3\×3}-\mathrm{\δ}{\mathrm{\Ω}}_k\·\mathrm{\Δt}}$ Formula (2)

Wherein, under the angular velocity information in described pedestrian's kth moment, positional information, velocity information and angular velocity change information be converted to terrestrial coordinate system by the direction cosine matrix in kth-1 moment (previous moment) from sensor coordinate system; δ Ω _kbe expressed as matrix based on angular velocity in order to represent that little angular transformation is on the impact of direction transformation, δ Ω _kexpression formula be $\mathrm{\δ}{\mathrm{\Ω}}_k=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_k^b\left(3\right)& {\mathrm{\ω}}_k^b\left(2\right)\\ {\mathrm{\ω}}_k^b\left(3\right)& 0& -{\mathrm{\ω}}_k^b\left(1\right)\\ -{\mathrm{\ω}}_k^b\left(2\right)& {\mathrm{\ω}}_k^b\left(1\right)& 0\end{array}\right],$ First direction/second direction/third direction under 1,2,3 expression sensor coordinate systems.Direction cosine matrix as k=1, $C_{b_0}^n=\left[\begin{array}{ccc}\cos \left(\mathrm{pitch}\right)& \sin \left(\mathrm{pitch}\right)& \cos \left(\mathrm{roll}\right)\sin \left(\mathrm{pitch}\right)\\ 0& \cos \left(\mathrm{roll}\right)& -\sin \left(\mathrm{roll}\right)\\ -\sin \left(\mathrm{pitch}\right)& \sin \left(\mathrm{roll}\right)\cos \left(\mathrm{pitch}\right)& \cos \left(\mathrm{roll}\right)\cos \left(\mathrm{pitch}\right)\end{array}\right],$ Wherein, roll represents roll angle, $\mathrm{roll}=\arctan (a_y^{\mathrm{sensor}}/a_z^{\mathrm{sensor}});$ Pitch is the angle of pitch, $\mathrm{pitch}=-\arcsin (a_x^{\mathrm{sensor}}/g),$ Yaw is course angle. refer to the data of the x-axis that acceleration transducer records, refer to the data of the y-axis that acceleration transducer records, refer to the data of the z-axis that acceleration transducer records; G is acceleration of gravity.When there being magnetometer, yaw is the magnetometer number of degrees in the horizontal plane, and when magnetometer, initial value is set to 0.I _3*3be 3 dimension unit matrixs, namely $I_{3*3}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right];$ for the angular velocity information in pedestrian's kth moment described under sensor coordinate system; Δ t represents sampling time interval, the time namely differed between kth moment (current time) with kth-1 moment (previous moment).

According to Bayesian inference algorithm, S3, using a pin in described pedestrian's both feet as with reference to pin, judges whether described reference pin remained static in the kth moment; If so, continue to perform next step, i.e. S4; If not, perform step S7, export the mobile location information in unregulated described pedestrian's kth moment.Refer to Fig. 2, be shown as step S3 and specifically perform schematic flow sheet.As shown in Figure 2, described step S3 comprises the following steps:

S31, calculates striding the cycle of described pedestrian based on described sensing data; The described cycle of striding comprises pin contact phase, pin stance, and less touch with the ground stage and pin control the stage.In the present embodiment, the cycle that strides calculating described pedestrian needs based on three-axis gyroscope y-axis data in described sensing module, calculates according to the signal characteristic that strides in three-axis gyroscope y-axis signal the cycle of striding.At the initial period of pendulum step period, all there is a valley in anyone gyroscope y-axis data, the method to zero crossing arranges threshold value can be utilized to carry out valley detection, but different people signal characteristic may introduce erroneous judgement valley, therefore for the signal characteristic that strides, safeguard a zone bit, when detecting zero velocity interval be, by this mark position true, when saying this mark position false when putting step section and detecting first valley, all ignore to valley in false phase detection afterwards.Simultaneously based on human cinology's information, pendulum step section will continue for 38% time, and all the other valleies detected in section at this moment are all ignored.Experiment proves, these two kinds of mechanism can ensure that the local valley striden in the cycle of 100% calculates accuracy rate.That is, as shown in Figure 2, in each cycle that strides, all can find an obvious valley (local minimum) with this valley for the tiptoe liftoff moment, as the cut-point in judgement one the complete cycle that strides, realize estimating the division of different step motion stage.

S32, should to be in which in the cycle of striding in stage, and to obtain Query Result in the kth moment according to the described pedestrian of pre-defined rule correspondence inquiry.In the present embodiment, described pre-defined rule is human body kinematics model, refers to Fig. 3, is shown as human cinology's model schematic, it is that wall scroll leg contacts to earth from heel that this model defines a cycle of striding, and contacts to earth to sole, to heeloff, tiptoe is liftoff, and pendulum step, last heel contacts to earth again.Comprise erect-position stage and pendulum step section in this cycle, wherein the erect-position stage comprises contact phase, stance, the liftoff stage.It is each stage time scale occupied within the cycle that really strides that human cinology's model defines at proper motion.In this step, the kth moment residing for described pedestrian to correspond in this human cinology's model the described pedestrian of corresponding inquiry and should be in which in the cycle of striding in the kth moment in stage, just obtain Query Result, described Query Result comprises described pedestrian and is in the erect-position stage or described pedestrian is in the kth moment stage of controlling in the kth moment.As shown in Figure 3, if a cycle of striding is divided into 100 parts, A point represents that heel contacts to earth (0%), B point represents that sole contacts to earth (16.8%), C point represents heeloff (41.5%), D point represents tiptoe liftoff (62%), E point represents that heel contacts to earth (100%), heel contacts to earth and contacts to earth for contact phase to sole, sole contacts to earth to heeloff for stance, arriving tiptoe heeloff liftoff is the liftoff stage, and tiptoe is liftoff to contact to earth for controlling the stage to heel.Therefore, can as can be seen from Figure 3, the erect-position stage accounts for 62%, and the stage of controlling accounts for 38%.

S33, arrange described in threshold decision according to the gyroscope prestored and whether remain static with reference to pin, obtain the first judged result, the angular velocity information in the described pedestrian's kth moment namely got by three-axis gyroscope is arranged threshold value with the gyroscope prestored and compares, if the angular velocity information in described pedestrian's kth moment is less than the gyroscope prestored arrange threshold value, then represent described to remain static with reference to pin, if the angular velocity information in described pedestrian's kth moment is greater than the gyroscope prestored arrange threshold value, then represent described to be kept in motion with reference to pin, perform step S7, remain static if described first judged result is described reference pin, continue to perform next step.

S34, judge described first judged result whether consistent with described Query Result (with reference to pin remain static corresponding described reference pin be in the erect-position stage, namely represent that the first judged result is consistent with described Query Result, or to be kept in motion with reference to pin namely represent described and be in reference to pin the stage of controlling and namely represent that the first judged result is consistent with described Query Result), if so, then represent there is not potential error point; If not, then represent to there is potential error point, perform step S35.

By naive Bayesian, S35, estimates whether described potential error point is in zero velocity interval, if, then represent described to remain static in the kth moment with reference to pin, continue to perform step S4, if not, then represent described to be kept in motion in the kth moment with reference to pin, then perform step S7.In this step, adopt following naive Bayesian estimation formulas to estimate, naive Bayesian estimation formulas is:

Zv=argmaxP (s|zv) P (a|zv) P (zv) formula (3)

Wherein, P (s|zv) represents the conditional probability of the finish time on foot contact ground,

formula (4)

$P\left(s\right|\mathrm{zv})=\left[\begin{array}{cc}\frac{1}{2}[1+\mathrm{erf}\left(\frac{t-(t_0+T_{\mathrm{gait}}*0.452)}{\mathrm{\σ}\sqrt{2}}\right)]& 1-\frac{1}{2}[1+\mathrm{erf}\left(\frac{t-(t_0+T_{\mathrm{gait}}*0.452)}{\mathrm{\σ}\sqrt{2}}\right)]\\ 1-\frac{1}{2}[1+\mathrm{erf}\left(\frac{t-(t_0+T_{\mathrm{gait}}*0.452)}{\mathrm{\σ}\sqrt{2}}\right)]& \frac{1}{2}[1+\mathrm{erf}\left(\frac{t-(t_0+T_{\mathrm{gait}}*0.452)}{\mathrm{\σ}\sqrt{2}}\right)]\end{array}\right]$ Formula (5)

Wherein, σ identifies the variance of normal distribution, σ ²=C × T _gait, t ₀for the initial time controlled, t is kth moment (current time), and C is a constant, T _gaitrepresent the time span in the whole cycle that strides.P (a|zv) represents the conditional probability of the acceleration under known zero velocity Probability Condition.

formula (6)

Wherein, a _nrepresent the acceleration under terrestrial coordinate system.P (zv) expression obtains based on kniesiological probability statistics the probability that the kth moment (current time) is zero velocity,

$P\left(\mathrm{zv}\right)=\left\{\begin{array}{cc}0.548,& \mathrm{is\; ZV}\\ 0.452,& \mathrm{not\; ZV}\end{array}\right.$ Formula (7)

S4, estimates described positional information calculation error based on EKF method.In this step, calculate the mobile location information in described pedestrian's kth moment by step S2 according to described sensing data and comprise positional information calculation error with the mobile location information obtaining unregulated described pedestrian's kth moment.In the present embodiment, adopt ZUPT (Zero velocityUpdaTe, zero velocity upgrades) the auxiliary error state EKF method of/ZART (Zero Angular Rate Update, zero angular velocity upgrade) estimates described positional information calculation error.

Described positional information calculation error comprises attitude error, and angular velocity is biased, site error, velocity error and acceleration are biased, and namely the positional information calculation error in described pedestrian's kth moment can be labeled as δ x _k, this positional information calculation error delta x _kfollowing error vector is adopted to represent:

Wherein, represent the attitude error in described pedestrian's kth moment, represent that the angular velocity in described pedestrian's kth moment is biased, δ r _krepresent the site error in described pedestrian's kth moment, δ v _krepresent the velocity error in described pedestrian's kth moment, represent that the acceleration in described pedestrian's kth moment is biased, these 5 error vectors are all corresponding 3 orthogonal axes.

The positional information calculation error delta x in described pedestrian's kth moment _kcomputing formula be

δ x _k=F _kδ x _k-1+ w _k-1formula (8)

Wherein, F _krepresent state-transition matrix, δ x _k-1represent the positional information calculation error in described pedestrian's kth-1 moment, w _k-1represent the noise of the positional information calculation error in described pedestrian's kth moment.In the present embodiment, because needs adopt Kalman filtering method to estimate error, then the measurement model of Kalman filtering is:

Z _k=H δ x _k|k+ n _kformula (9)

When step remains static time, acceleration and angular velocity should be 0, but the data of actual acquisition exist noise, and this noise obtains velocity error after integration, another one is exactly the angular velocity error that actual measurement is arrived, these two the measured value Z be worth as error state _k, namely h is error in measurement, $H=\left[\begin{array}{ccccc}{0}_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×3}\end{array}\right],$ I _{3 × 3}for unit matrix.

The positional information calculation error in the described pedestrian's kth moment after renewal

δ x _k=δ x _k-1+ K _k[Z _k-H δ x _k-1] formula (10)

Wherein, K _kfor kalman gain, kalman gain K _k=P _k-1h ^t(HP _kh ^t+ R _k) ^-1middle P _kfor described pedestrian's kth moment is error state covariance matrix, P _k=(I _{15 × 15}-K _kh) P _k-1(I _{15 × 15}-K _kh) ^t+ R _k, P _k-1for described pedestrian's kth-1 moment is error state covariance matrix.In this step, employing formula (10) estimates attitude error respectively, and angular velocity is biased, site error, velocity error and acceleration are biased, specific as follows:

δr _k＝δr _k-1+K _k·[Z _k-Hδr _k-1]

δv _k＝δv _k-1+K _k·[Z _k-Hδv _k-1]

$\mathrm{\δ}{a}_k^b=\mathrm{\δ}{a}_{k-1}^b+K_k\·[Z_k-H{\mathrm{\δa}}_{k-1}^b]$

S5, calibrate the mobile location information in described pedestrian's kth moment, described step S5 specifically comprises the following steps:

Adopt attitude error, the i.e. attitude error in described pedestrian's kth moment by an attitude error matrix delta Ω _kto direction cosine matrix $C_{b_{k/k-1}}^n=f(C_{b_{k/k-1}}^n,{\mathrm{\ω}}_k^b)=C_{b_{k-1/k-1}}^n\×\frac{{2I}_{3\×3}+\mathrm{\δ}{\mathrm{\Ω}}_K\·\mathrm{\Δt}}{{2I}_{3\×3}-\mathrm{\δ}{\mathrm{\Ω}}_k\·\mathrm{\Δt}}$ Revise, wherein attitude error matrix delta Ω _kfor

Employing angular velocity is biased, and namely the angular velocity in described pedestrian's kth moment is biased the angular velocity in kth+1 moment is compensated; Wherein, the angular velocity+angular velocity in the angular velocity=kth moment in kth+1 moment is biased.

Employing acceleration is biased, and namely the acceleration in described pedestrian's kth moment is biased the acceleration in kth+1 moment is compensated; Wherein, the acceleration+acceleration in the acceleration=kth moment in kth+1 moment is biased;

Adopt site error, i.e. the site error δ r in described pedestrian's kth moment _kthe position in kth moment is compensated; Wherein, the positional information-site error in the mobile location information in position=unregulated described pedestrian's kth moment in described kth moment;

Adopt velocity error, i.e. the velocity error δ v in described pedestrian's kth moment _kthe speed in kth moment is compensated; Wherein, the speed-velocity error in the mobile location information in speed=unregulated described pedestrian's kth moment in described kth moment.

S6, exports the mobile location information in the described pedestrian's kth moment after calibration.

Pedestrian's localization method of the tracking foot motion feature described in the present embodiment, by being bound to the data sensor module of foot, first carries out pre-service and initialization self calibration to acquired original data.And then use know clearly a kind of versatility, high-precision zero velocity detection method, the method is based on gyroscope and accelerometer raw data, in conjunction with human cinology's principle, adopt Bayesian network model to carry out zero velocity interval to infer, efficiently solve the interval erroneous judgement problem of traditional zero velocity detection method zero velocity and the interval edge blurring problem of zero velocity, improve detector efficiency and the degree of accuracy of zero velocity.And interval based on the zero velocity detected, the EKF utilizing ZUPT/ZARU to assist carries out error state to realize the error compensation to inertial navigation pedestrian location algorithm.Pedestrian's localization method of described tracking foot motion feature overcomes location cumulative errors (positional information calculation error) that single pedestrian's inertial positioning algorithm causes with the problem of time 3 power expansion, achieve pedestrian's location navigation function available in real time, take into account counting yield, location progress, use general reliable problem, significant to the practical application of pedestrian's positioning system of high position precision.

Embodiment two

The present embodiment provides a kind of pedestrian's positioning system 1 of following the tracks of foot motion feature, follow the tracks of pedestrian locate by performing with the locating module and sensing module that are worn on pedestrian upper body and foot respectively, wherein, described sensing module comprises three-axis gyroscope, three axis accelerometer, three axle magnetometers.Refer to Fig. 4, be shown as the theory structure schematic diagram of pedestrian's positioning system of following the tracks of foot motion feature.Pedestrian's positioning system of described tracking foot motion feature comprises: sensing module 11, locating module 12, data acquisition module 13, pretreatment module 14, computing module 15, judge module 16, estimation module 17, calibration module 18, first output module 19 and the second output module 20.

In the present embodiment, described three-axis gyroscope, three axis accelerometer, three axle magnetometers are integrated on sensing module 11, and described sensing module 11 is arranged on the foot that described walking, such as, on instep, on tiptoe, under heel, after heel, and on ankle, be arranged on any one position and all can meet data acquisition request.

In the present embodiment, and the initial position message of described pedestrian is provided by GPS locating module 12 or Big Dipper locating module 12.

The described data acquisition module 13 be connected with described sensing module 11 and locating module 12 is respectively for obtaining the initial position message of the described pedestrian provided by described locating module, and the described pedestrian provided by described sensing module is at the sensing data in each moment; Wherein, described sensing data comprises the initial motion directional information of described pedestrian.

The pretreatment module 14 be connected with described data acquisition module 13 removes high frequency noise in sensing data for utilizing low-pass filter to described sensing data.

The computing module 15 be connected with described pretreatment module 14 is for calculating the mobile location information in described pedestrian's kth moment to obtain the mobile location information in unregulated described pedestrian's kth moment according to described sensing data; Described mobile location information comprises positional information calculation error; Wherein, k be more than or equal to 1 positive integer.In the present embodiment, described computing module 15 adopts the inertial positioning algorithm be pre-stored in wherein to calculate the mobile location information in described pedestrian's kth moment, mobile location information comprises the acceleration information in described pedestrian's kth moment, the angular velocity information in described pedestrian's kth moment, the positional information in described pedestrian's kth moment, the velocity information in described pedestrian's kth moment and the angular velocity change information in described pedestrian's kth moment, such as, the computing formula calculating the acceleration information in described pedestrian's kth moment is:

$a_k^n=C_{b_{k/k-1}}^n\·a_k^b$

Wherein, for being engraved in the acceleration of X-axis/Y-axis/Z axis under terrestrial coordinate system during described pedestrian's kth, k represents the kth moment (current time), that is, n represents the X-axis/Y-axis/Z axis under terrestrial coordinate system; for the direction cosine matrix of kth moment (current time), under described sensing data being converted to terrestrial coordinate system from sensor coordinate system by this direction cosine matrix, k-1 is kth-1 moment (previous moment); for being engraved in the acceleration of first direction/second direction/third direction under sensor coordinate system during described pedestrian's kth, k represents the kth moment (current time), and b represents first direction/second direction/third direction under sensor coordinate system.In like manner, calculate the angular velocity information in described pedestrian's kth moment, positional information, velocity information and angular velocity change information, pass through equally under the angular velocity information in described pedestrian's kth moment, positional information, velocity information and angular velocity change information be converted to terrestrial coordinate system by the direction cosine matrix in kth moment (current time) from sensor coordinate system.Wherein, the direction cosine matrix in kth moment (current time) computing formula be:

$C_{b_{k/k-1}}^n=f(C_{b_{k/k-1}}^n,{\mathrm{\ω}}_k^b)=C_{b_{k-1/k-1}}^n\·\frac{{2I}_{3\×3}+\mathrm{\δ}{\mathrm{\Ω}}_K\·\mathrm{\Δt}}{{2I}_{3\×3}-\mathrm{\δ}{\mathrm{\Ω}}_k\·\mathrm{\Δt}}$

Wherein, under the angular velocity information in described pedestrian's kth moment, positional information, velocity information and angular velocity change information be converted to terrestrial coordinate system by the direction cosine matrix in kth-1 moment (previous moment) from sensor coordinate system; δ Ω _kbe expressed as matrix based on angular velocity in order to represent that little angular transformation is on the impact of direction transformation, δ Ω _kexpression formula be $\mathrm{\δ}{\mathrm{\Ω}}_k=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_k^b\left(3\right)& {\mathrm{\ω}}_k^b\left(2\right)\\ {\mathrm{\ω}}_k^b\left(3\right)& 0& -{\mathrm{\ω}}_k^b\left(1\right)\\ -{\mathrm{\ω}}_k^b\left(2\right)& {\mathrm{\ω}}_k^b\left(1\right)& 0\end{array}\right],$ First direction/second direction/third direction under 1,2,3 expression sensor coordinate systems.Direction cosine matrix as k=1, $C_{b_0}^n=\left[\begin{array}{ccc}\cos \left(\mathrm{pitch}\right)& \sin \left(\mathrm{pitch}\right)& \cos \left(\mathrm{roll}\right)\sin \left(\mathrm{pitch}\right)\\ 0& \cos \left(\mathrm{roll}\right)& -\sin \left(\mathrm{roll}\right)\\ -\sin \left(\mathrm{pitch}\right)& \sin \left(\mathrm{roll}\right)\cos \left(\mathrm{pitch}\right)& \cos \left(\mathrm{roll}\right)\cos \left(\mathrm{pitch}\right)\end{array}\right],$ Wherein, roll represents roll angle, $\mathrm{roll}=\arctan (a_y^{\mathrm{sensor}}/a_z^{\mathrm{sensor}});$ Pitch is the angle of pitch, $\mathrm{pitch}=-\arcsin (a_x^{\mathrm{sensor}}/g),$ Yaw is course angle. refer to the data of the x-axis that acceleration transducer records, refer to the data of the y-axis that acceleration transducer records, refer to the data of the z-axis that acceleration transducer records; G is acceleration of gravity.When there being magnetometer, yaw is the magnetometer number of degrees in the horizontal plane, and when magnetometer, initial value is set to 0.I _3*3be 3 dimension unit matrixs, namely $I_{3*3}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right];$ for the angular velocity information in pedestrian's kth moment described under sensor coordinate system; Δ t represents sampling time interval, the time namely differed between kth moment (current time) with kth-1 moment (previous moment).

According to the Bayesian inference algorithm prestored wherein, the described judge module 16 be connected with described computing module 15, for using a pin in described pedestrian's both feet as with reference to pin, judges whether described reference pin remained static in the kth moment; If, then call the estimation module 17 for estimating described positional information calculation error, for calibrating the calibration module 18 of the mobile location information in described pedestrian's kth moment, and for the first output module 19 of the mobile location information that exports the described pedestrian's kth moment after calibration; If not, the second output module 20 of the mobile location information for exporting unregulated described pedestrian's kth moment is called.Wherein, described judge module 16 specifically comprises the computation of Period unit that strides, query unit, the first judging unit and the second judging unit.

Wherein, the computation of Period that strides described in unit is for calculating striding the cycle of described pedestrian based on described sensing data; The described cycle of striding comprises pin contact phase, pin stance, and less touch with the ground stage and pin control the stage.In the present embodiment, the cycle that strides calculating described pedestrian needs based on three-axis gyroscope y-axis data in described sensing module, calculates according to the signal characteristic that strides in three-axis gyroscope y-axis signal the cycle of striding.At the initial period of pendulum step period, all there is a valley in anyone gyroscope y-axis data, the method to zero crossing arranges threshold value can be utilized to carry out valley detection, but different people signal characteristic may introduce erroneous judgement valley, therefore for the signal characteristic that strides, safeguard a zone bit, when detecting zero velocity interval be, by this mark position true, when saying this mark position false when putting step section and detecting first valley, all ignore to valley in false phase detection afterwards.Simultaneously based on human cinology's information, pendulum step section will continue for 38% time, and all the other valleies detected in section at this moment are all ignored.Experiment proves, these two kinds of mechanism can ensure that the local valley striden in the cycle of 100% calculates accuracy rate.That is, as shown in Figure 2, in each cycle that strides, all can find an obvious valley (local minimum) with this valley for the tiptoe liftoff moment, as the cut-point in judgement one the complete cycle that strides, realize estimating the division of different step motion stage.

Described query unit is used for which that should to be in the kth moment according to the described pedestrian of pre-defined rule correspondence inquiry in the cycle of striding in the stage, and obtains Query Result.In the present embodiment, described pre-defined rule is human body kinematics model, human cinology's model as shown in Figure 3, it is that wall scroll leg contacts to earth from heel that this model defines a cycle of striding, and contacts to earth to sole, to heeloff, tiptoe is liftoff, and pendulum step, last heel contacts to earth again.Comprise erect-position stage and pendulum step section in this cycle, wherein the erect-position stage comprises contact phase, stance, the liftoff stage.It is each stage time scale occupied within the cycle that really strides that human cinology's model defines at proper motion.In described query unit, the kth moment residing for described pedestrian to correspond in this human cinology's model the described pedestrian of corresponding inquiry and should be in which in the cycle of striding in the kth moment in stage, just obtain Query Result, described Query Result comprises described pedestrian and is in the erect-position stage or described pedestrian is in the kth moment stage of controlling in the kth moment.As shown in Figure 3, if a cycle of striding is divided into 100 parts, A point represents that heel contacts to earth (0%), B point represents that sole contacts to earth (16.8%), C point represents heeloff (41.5%), D point represents tiptoe liftoff (62%), E point represents that heel contacts to earth (100%), heel contacts to earth and contacts to earth for contact phase to sole, sole contacts to earth to heeloff for stance, arriving tiptoe heeloff liftoff is the liftoff stage, and tiptoe is liftoff to contact to earth for controlling the stage to heel.Therefore, can as can be seen from Figure 3, the erect-position stage accounts for 62%, and the stage of controlling accounts for 38%.

Whether described first judging unit is used for arranging described in threshold decision according to the gyroscope prestored remaining static with reference to pin, obtains the first judged result; If so, then call for described first judged result and described Query Result are compared, judge that whether described first judged result is consistent with described Query Result, if so, then represent there is not potential error point; If not, then represent to there is potential error point, call for being estimated by naive Bayesian whether described potential error point is in second judging unit in zero velocity interval, if, then represent described to remain static in the kth moment with reference to pin, call described estimation module and calibration module, if not, then represent described to be kept in motion in the kth moment with reference to pin, call described second output module.The angular velocity information in described pedestrian's kth moment that described first judging unit is got by three-axis gyroscope arranges threshold value with the gyroscope prestored and compares, if the angular velocity information in described pedestrian's kth moment is less than the gyroscope prestored arrange threshold value, then represent described to remain static with reference to pin, if the angular velocity information in described pedestrian's kth moment is greater than the gyroscope prestored arrange threshold value, then represents described and be kept in motion with reference to pin; Remain static if described first judged result is described reference pin, continue judge described first judged result whether consistent with described Query Result (reference pin remain static corresponding described reference pin be in the erect-position stage, namely represent that the first judged result is consistent with described Query Result, or to be kept in motion with reference to pin namely represent described and be in reference to pin the stage of controlling and namely represent that the first judged result is consistent with described Query Result), if so, then represent there is not potential error point; If not, then represent to there is potential error point, estimate whether described potential error point is in zero velocity interval by naive Bayesian, if, then represent described to remain static in the kth moment with reference to pin, if not, then represent described and be kept in motion in the kth moment with reference to pin.Described first judging unit adopts following naive Bayesian estimation formulas to estimate, naive Bayesian estimation formulas is:

zv＝argmaxP(s|zv)P(a|zv)P(zv)

Wherein, P (s|zv) represents the conditional probability of the finish time on foot contact ground.

$P\left(s\right|\mathrm{zv})=\left[\begin{array}{cc}\frac{1}{2}[1+\mathrm{erf}\left(\frac{t_0-t}{\mathrm{\σ}\sqrt{2}}\right)]& 1-\frac{1}{2}[1+\mathrm{erf}\left(\frac{t_0-t}{\mathrm{\σ}\sqrt{2}}\right)]\\ 1-\frac{1}{2}[1+\mathrm{erf}\left(\frac{t_0-t}{\mathrm{\σ}\sqrt{2}}\right)]& \frac{1}{2}[1+\mathrm{erf}\left(\frac{t_0-t}{\mathrm{\σ}\sqrt{2}}\right)]\end{array}\right]$

$P\left(s\right|\mathrm{zv})=\left[\begin{array}{cc}\frac{1}{2}[1+\mathrm{erf}\left(\frac{t-(t_0+T_{\mathrm{gait}}*0.452)}{\mathrm{\σ}\sqrt{2}}\right)]& 1-\frac{1}{2}[1+\mathrm{erf}\left(\frac{t-(t_0+T_{\mathrm{gait}}*0.452)}{\mathrm{\σ}\sqrt{2}}\right)]\\ 1-\frac{1}{2}[1+\mathrm{erf}\left(\frac{t-(t_0+T_{\mathrm{gait}}*0.452)}{\mathrm{\σ}\sqrt{2}}\right)]& \frac{1}{2}[1+\mathrm{erf}\left(\frac{t-(t_0+T_{\mathrm{gait}}*0.452)}{\mathrm{\σ}\sqrt{2}}\right)]\end{array}\right]$

Wherein, σ identifies the variance of normal distribution, σ ²=C × T _gait, t ₀for the initial time controlled, t is kth moment (current time), and C is a constant, T _gaitrepresent the time span in the whole cycle that strides.P (a|zv) represents the conditional probability of the acceleration under known zero velocity Probability Condition.

Wherein, a _nrepresent the acceleration under terrestrial coordinate system.P (zv) expression obtains based on kniesiological probability statistics the probability that the kth moment (current time) is zero velocity.

$P\left(\mathrm{zv}\right)=\left\{\begin{array}{cc}0.548,& \mathrm{is\; ZV}\\ 0.452,& \mathrm{not\; ZV}\end{array}\right.$

Described estimation module 17 is for estimating described positional information calculation error based on EKF method.Described estimation module 17 is for passing through the sensing data after pretreatment module 13 process, and the mobile location information calculating described pedestrian's kth moment according to described sensing data comprises positional information calculation error with the mobile location information obtaining unregulated described pedestrian's kth moment.In the present embodiment, adopt ZUPT (Zero velocity UpdaTe, zero velocity upgrades) the auxiliary error state EKF method of/ZART (Zero AngularRate Update, zero angular velocity upgrade) estimates described positional information calculation error.

Described positional information calculation error comprises attitude error, and angular velocity is biased, site error, velocity error and acceleration are biased, and namely the positional information calculation error in described pedestrian's kth moment can be labeled as δ x _k, this positional information calculation error delta x _kfollowing error vector is adopted to represent:

Wherein, represent the attitude error in described pedestrian's kth moment, represent that the angular velocity in described pedestrian's kth moment is biased, δ r _krepresent the site error in described pedestrian's kth moment, δ v _krepresent the velocity error in described pedestrian's kth moment, represent that the acceleration in described pedestrian's kth moment is biased, these 5 error vectors are all corresponding 3 orthogonal axes.

The positional information calculation error delta x in described pedestrian's kth moment _kcomputing formula be

δx _k＝F _kδx _k-1+w _k-1

Wherein, F _krepresent state-transition matrix, δ x _k-1represent the positional information calculation error in described pedestrian's kth-1 moment, w _k-1represent the noise of the positional information calculation error in described pedestrian's kth moment.In the present embodiment, because needs adopt Kalman filtering method to estimate error, then the measurement model of Kalman filtering is:

Z _k＝Hδx _k|k+n _k

When step remains static time, acceleration and angular velocity should be 0, but the data of actual acquisition exist noise, and this noise obtains velocity error after integration, another one is exactly the angular velocity error that actual measurement is arrived, these two the measured value Z be worth as error state _k, namely h is error in measurement, $H=\left[\begin{array}{ccccc}{0}_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×3}\end{array}\right],$ I _{3 × 3}for unit matrix.

The positional information calculation error in the described pedestrian's kth moment after renewal

δx _k＝δx _k-1+K _k·[Z _k-Hδx _k-1]

Wherein, K _kfor kalman gain, kalman gain K _k=P _k-1h ^t(HP _kh ^t+ R _k) ^-1middle P _kfor described pedestrian's kth moment is error state covariance matrix, P _k=(I _{15 × 15}-K _kh) P _k-1(I _{15 × 15}-K _kh) ^t+ R _k, P _k-1for described pedestrian's kth-1 moment is error state covariance matrix.In this step, employing formula (10) estimates attitude error respectively, and angular velocity is biased, site error, velocity error and acceleration are biased, specific as follows:

δr _k＝δr _k-1+K _k·[Z _k-Hδr _k-1]

δv _k＝δv _k-1+K _k·[Z _k-Hδv _k-1]

$\mathrm{\δ}{a}_k^b=\mathrm{\δ}{a}_{k-1}^b+K_k\·[Z_k-H{\mathrm{\δa}}_{k-1}^b]$

The described calibration module 18 be connected with described pretreatment module 13 and estimation module 17 is for calibrating the mobile location information in described pedestrian's kth moment.Described calibration module 18 comprises the first compensating unit, the second compensating unit, the 3rd compensating unit, the 4th compensating unit and the 5th compensating unit.

Described first compensating unit for adopting attitude error, i.e. the attitude error in described pedestrian's kth moment by an attitude error matrix delta Ω _kto direction cosine matrix $C_{b_{k/k-1}}^n=f(C_{b_{k/k-1}}^n,{\mathrm{\ω}}_k^b)=C_{b_{k-1/k-1}}^n\×\frac{{2I}_{3\×3}+\mathrm{\δ}{\mathrm{\Ω}}_K\·\mathrm{\Δt}}{{2I}_{3\×3}-\mathrm{\δ}{\mathrm{\Ω}}_k\·\mathrm{\Δt}}$ Revise, wherein attitude error matrix delta Ω _kfor

Described second compensating unit is biased for adopting angular velocity, and namely the angular velocity in described pedestrian's kth moment is biased the angular velocity in kth+1 moment is compensated; Wherein, the angular velocity+angular velocity in the angular velocity=kth moment in kth+1 moment is biased.

Described 3rd compensating unit is biased for adopting acceleration, and namely the acceleration in described pedestrian's kth moment is biased the acceleration in kth+1 moment is compensated; Wherein, the acceleration+acceleration in the acceleration=kth moment in kth+1 moment is biased.

Described 4th compensating unit for adopting site error, i.e. the site error δ r in described pedestrian's kth moment _kthe position in kth moment is compensated; Wherein, the positional information-site error in the mobile location information in position=unregulated described pedestrian's kth moment in described kth moment.

Described 5th compensating unit for adopting velocity error, i.e. the velocity error δ v in described pedestrian's kth moment _kthe speed in kth moment is compensated; Wherein, the speed-velocity error in the mobile location information in speed=unregulated described pedestrian's kth moment in described kth moment.

Eventually pass the mobile location information in the described pedestrian's kth moment after the first output module 19 or the second output module 20 output calibration or export the mobile location information in unregulated described pedestrian's kth moment.

Pedestrian's localization method of tracking foot motion feature of the present invention and system, by being bound to the data sensor module of foot, first carry out pre-service and initialization self calibration to acquired original data.And then use know clearly a kind of versatility, high-precision zero velocity detection method, the method is based on gyroscope and accelerometer raw data, in conjunction with human cinology's principle, adopt Bayesian network model to carry out zero velocity interval to infer, efficiently solve the interval erroneous judgement problem of traditional zero velocity detection method zero velocity and the interval edge blurring problem of zero velocity, improve detector efficiency and the degree of accuracy of zero velocity.And interval based on the zero velocity detected, the EKF utilizing ZUPT/ZARU to assist carries out error state to realize the error compensation to inertial navigation pedestrian location algorithm.Pedestrian's localization method of described tracking foot motion feature and system overcome location cumulative errors (positional information calculation error) that single pedestrian's inertial positioning algorithm causes with the problem of time 3 power expansion, achieve pedestrian's location navigation function available in real time, take into account counting yield, location progress, use general reliable problem, significant to the practical application of pedestrian's positioning system of high position precision.

In sum, the present invention effectively overcomes various shortcoming of the prior art and tool high industrial utilization.

Above-described embodiment is illustrative principle of the present invention and effect thereof only, but not for limiting the present invention.Any person skilled in the art scholar all without prejudice under spirit of the present invention and category, can modify above-described embodiment or changes.Therefore, such as have in art usually know the knowledgeable do not depart from complete under disclosed spirit and technological thought all equivalence modify or change, must be contained by claim of the present invention.

Claims (12)

1. follow the tracks of pedestrian's localization method of foot motion feature for one kind, performed by the locating module and sensing module that are worn on pedestrian upper body and foot respectively, wherein, described sensing module comprises three-axis gyroscope, three axis accelerometer, three axle magnetometers, it is characterized in that, pedestrian's localization method of described tracking foot motion feature comprises:

Obtain the initial position message of the described pedestrian provided by described locating module, and the described pedestrian provided by described sensing module is at the sensing data in each moment; Wherein, described sensing data comprises the initial motion directional information of described pedestrian;

The mobile location information in described pedestrian's kth moment is calculated to obtain the mobile location information in unregulated described pedestrian's kth moment according to described sensing data; Described mobile location information comprises positional information calculation error; Wherein, k be more than or equal to 1 positive integer;

Using a pin in described pedestrian's both feet as with reference to pin, judge whether described reference pin remained static in the kth moment according to Bayesian inference algorithm; If so, continue to perform next step; If not, the mobile location information in unregulated described pedestrian's kth moment is exported;

Described positional information calculation error is estimated;

Calibrate the mobile location information in described pedestrian's kth moment, export the mobile location information in the described pedestrian's kth moment after calibration.

2. pedestrian's localization method of tracking foot motion feature according to claim 1, is characterized in that: adopt inertial positioning algorithm to calculate the mobile location information in described pedestrian's kth moment.

3. pedestrian's localization method of tracking foot motion feature according to claim 2, it is characterized in that: the step that described employing inertial positioning algorithm calculates the mobile location information in described pedestrian's kth moment is based on three-axis gyroscope y-axis data in described sensing module, calculates according to the signal characteristic that strides in three-axis gyroscope y-axis signal the cycle of striding.

4. pedestrian's localization method of tracking foot motion feature according to claim 1, is characterized in that: judge that described reference pin specifically comprises in the step whether the kth moment remains static according to Bayesian inference algorithm:

Striding the cycle of described pedestrian is calculated based on described sensing data; The described cycle of striding comprises pin contact phase, pin stance, and less touch with the ground stage and pin control the stage;

Which in the cycle of striding should to be according to the described pedestrian of pre-defined rule correspondence inquiry in the kth moment in stage, and to obtain Query Result;

Arrange described in threshold decision according to the gyroscope prestored and whether remain static with reference to pin, obtain the first judged result; If so, then described first judged result and described Query Result are compared, and judge that whether described first judged result is consistent with described Query Result, if so, then represent there is not potential error point; If not, then represent to there is potential error point, estimate whether described potential error point is in zero velocity interval by naive Bayesian, if, then represent described to remain static in the kth moment with reference to pin, if not, then represent described and be kept in motion in the kth moment with reference to pin.

5. pedestrian's localization method of tracking foot motion feature according to claim 1, is characterized in that: be estimate described positional information calculation error based on EKF method in the step estimate described positional information calculation error.

6. pedestrian's localization method of tracking foot motion feature according to claim 5, is characterized in that: described positional information calculation error comprises attitude error, and angular velocity is biased, site error, velocity error and acceleration are biased.

7. pedestrian's localization method of tracking foot motion feature according to claim 6, is characterized in that: the step of calibrating the mobile location information in described pedestrian's kth moment also comprises:

Attitude error is adopted to be revised direction cosine matrix by an attitude error matrix;

Adopt angular velocity to be biased to compensate the angular velocity in kth+1 moment; Wherein, the angular velocity+angular velocity in the angular velocity=kth moment in kth+1 moment is biased;

Adopt acceleration to be biased to compensate the acceleration in kth+1 moment; Wherein, the acceleration+acceleration in the acceleration=kth moment in kth+1 moment is biased;

The position of site error to the kth moment is adopted to compensate; Wherein, the positional information-site error in the mobile location information in position=unregulated described pedestrian's kth moment in described kth moment;

The speed of velocity error to the kth moment is adopted to compensate; Wherein, the speed-velocity error in the mobile location information in speed=unregulated described pedestrian's kth moment in described kth moment.

8. follow the tracks of pedestrian's positioning system of foot motion feature for one kind, follow the tracks of pedestrian locate by performing with the locating module and sensing module that are worn on pedestrian upper body and foot respectively, wherein, described sensing module comprises three-axis gyroscope, three axis accelerometer, three axle magnetometers, it is characterized in that, pedestrian's positioning system of described tracking foot motion feature comprises:

Data acquisition module, for obtaining the initial position message of the described pedestrian provided by described locating module, and the described pedestrian provided by described sensing module is at the sensing data in each moment; Wherein, described sensing data comprises the initial motion directional information of described pedestrian;

Computing module, for calculating the mobile location information in described pedestrian's kth moment to obtain the mobile location information in unregulated described pedestrian's kth moment according to described sensing data; Described mobile location information comprises positional information calculation error; Wherein, k be more than or equal to 1 positive integer;

According to Bayesian inference algorithm, judge module, for using a pin in described pedestrian's both feet as with reference to pin, judges whether described reference pin remained static in the kth moment; If, then call the estimation module for estimating described positional information calculation error, for calibrating the calibration module of the mobile location information in described pedestrian's kth moment, and for the first output module of the mobile location information that exports the described pedestrian's kth moment after calibration; If not, the second output module of the mobile location information for exporting unregulated described pedestrian's kth moment is called.

9. pedestrian's positioning system of tracking foot motion feature according to claim 8, is characterized in that: described computing unit adopts inertial positioning algorithm to calculate the mobile location information in described pedestrian's kth moment.

10. pedestrian's positioning system of tracking foot motion feature according to claim 8, is characterized in that: described judge module comprises:

The computation of Period that strides unit, for calculating striding the cycle of described pedestrian based on described sensing data; The described cycle of striding comprises pin contact phase, pin stance, and less touch with the ground stage and pin control the stage;

Query unit, for should to be in which in the cycle of striding in the kth moment according to the described pedestrian of pre-defined rule correspondence inquiry, and obtains Query Result in stage;

Whether the first judging unit, remaining static with reference to pin for arranging according to the gyroscope prestored described in threshold decision, obtaining the first judged result; If so, then call for described first judged result and described Query Result are compared, judge that whether described first judged result is consistent with described Query Result, if so, then represent there is not potential error point; If not, then represent to there is potential error point, call for being estimated by naive Bayesian whether described potential error point is in second judging unit in zero velocity interval, if, then represent described to remain static in the kth moment with reference to pin, call described estimation module and calibration module, if not, then represent described to be kept in motion in the kth moment with reference to pin, call described second output module.

Pedestrian's positioning system of 11. tracking foot motion features according to claim 8, it is characterized in that: described estimation module is estimated to obtain attitude error to described positional information calculation error based on extended Kalman filter, angular velocity is biased, site error, velocity error and acceleration biased.

Pedestrian's positioning system of 12. tracking foot motion features according to claim 11, is characterized in that: described calibration module comprises:

First compensating unit, is revised direction cosine matrix by an attitude error matrix for adopting attitude error;

Second compensating unit, is biased for adopting angular velocity and compensates the angular velocity in kth+1 moment; Wherein, the angular velocity+angular velocity in the angular velocity=kth moment in kth+1 moment is biased;

3rd compensating unit, is biased for adopting acceleration and compensates the acceleration in kth+1 moment; Wherein, the acceleration+acceleration in the acceleration=kth moment in kth+1 moment is biased;

4th compensating unit, compensates for adopting the position of site error to the kth moment; Wherein, the positional information-site error in the mobile location information in position=unregulated described pedestrian's kth moment in described kth moment;

5th compensating unit, compensates for adopting the speed of velocity error to the kth moment; Wherein, the speed-velocity error in the mobile location information in speed=unregulated described pedestrian's kth moment in described kth moment.