Disclosure of Invention
Aiming at the problem of divergence caused by the navigation position of a pedestrian wearing an MEMS-INS sensor under an indoor environment condition, the invention provides a three-dimensional map matching method for indoor pedestrian navigation. The method comprises the steps of collecting indoor motion information of pedestrians by using an MEMS-INS sensor bound on the instep of the pedestrian, resolving the speed, position and course of the pedestrian, analyzing an indoor structure, creating a state point, and establishing a conditional random field model according to navigation output position information and state point position information to achieve indoor three-dimensional positioning of the pedestrian.
In order to achieve the technical purpose, the invention adopts the technical scheme that the indoor three-dimensional map matching method for the pedestrian comprises the following steps:
step 1: and acquiring data, and preliminarily calculating the three-dimensional position and the course of the indoor pedestrian.
Step 1.1, a pedestrian collects pedestrian movement data by wearing an MEMS-INS sensor, wherein the pedestrian movement data comprises: three-axis acceleration data and three-axis gyro data.
And step 1.2, solving the three-dimensional position and course information of the collected pedestrian motion data by using a strapdown calculation algorithm.
Step 2: and extracting the observation points of the conditional random field model.
And 2.1, extracting horizontal two-dimensional position information of the indoor pedestrian according to the fixed-length walking distance, extracting pedestrian height information at the zero-speed moment, acquiring an observation point of the CRF model, and respectively recording the two-dimensional position of the indoor pedestrian and the sampling moment of the height information.
And step 3: and establishing an indoor electronic map, creating a state point according to the indoor structure information, and storing the coordinate of the state point.
And 3.1, creating an indoor electronic map by using the known indoor map, and storing the indoor electronic map in the navigation computer.
And 3.2, solving coordinate points with the minimum and maximum numerical values in the electronic map as the value range of the state points, covering the whole indoor range with the equally spaced state points, and storing the state point information.
And 3.3, adding a state point on the height information by taking the step height as a standard.
And 4, performing a two-dimensional position map matching algorithm based on the conditional random field algorithm.
Step 4.1, establishing a characteristic equation according to the relationship between the two-dimensional position observation point coordinates and each state point coordinate;
and 4.2, establishing a characteristic equation according to the azimuth angle between the azimuth information of the observation point and the state point at the corresponding moment.
And 4.3, establishing a two-dimensional map matching mathematical model based on the conditional random field, and obtaining the maximum probability of the state sequence under the condition that the two-dimensional position is taken as the observation sequence, wherein the maximum probability sequence is the optimal state matching of the position.
And 5, a height information map matching algorithm based on the conditional random field algorithm.
And 5.1, dividing the walking height of each step of the pedestrian into different states according to the height of the step and the limit value of the step of the pedestrian.
And 5.2, establishing a characteristic equation which takes the height as an observation point and a state point corresponding to the observation point.
And 5.3, solving the mean square error between the height information of all the previous adjacent observation points and the height of the matched state point.
And 5.4, establishing a characteristic equation by taking the mean square error as another characteristic according to the relation between the heights of the adjacent state points and the height difference of each state point.
And 5.5, establishing a height map matching mathematical model based on the conditional random field, and obtaining the maximum probability of the state sequence under the condition that the height is taken as the observation sequence, wherein the maximum probability sequence is the optimal state matching of the height.
Step 6: two-dimensional position and height information fusion
And 6.1, inquiring the sampling time of the corresponding observation sequence by using the state sequence with the best two-dimensional position matching for storage, and inquiring the sampling time of the corresponding observation sequence by using the state sequence with the best height matching for storage.
And 6.2, combining the two-dimensional optimal matching point and the height optimal matching point by using a method of adjacent time.
And 6.3, correcting the three-dimensional position information output by the inertial navigation system according to the matched three-dimensional position.
Compared with the prior art, the invention has the following beneficial effects:
firstly, positioning of the three-dimensional position of the pedestrian is realized, and the algorithm precision is high; secondly, a method of separately matching two-dimensional position and height information is adopted, so that the complexity of an algorithm is simplified; thirdly, the two-dimensional position and the height information are fused by adopting the approaching moment, so that the accuracy of map matching is improved.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings.
The frame structure of the method of the invention is shown in figure 1 and comprises the following steps:
step 1: and acquiring data, and preliminarily calculating the three-dimensional position and the course of the indoor pedestrian.
Step 1.1, the pedestrian collects pedestrian movement data by wearing the MEMS-INS sensor, and the mode that the pedestrian wears the sensor is shown in figure 2. The pedestrian motion data includes: three-axis acceleration data and three-axis gyro data.
Step 1.2, solving the three-dimensional position and heading information of the collected pedestrian motion data by adopting a strapdown calculation algorithm, wherein a system flow chart of inertial navigation calculation is shown in fig. 3.
Based on physics, under the condition that the sampling interval is very short, the speed position relation satisfies:
wherein p is
nThree-dimensional position coordinates representing a pedestrian; t denotes the sampling time, d
nRepresenting the distance traveled by the pedestrian; t is a sampling interval; v. of
nRepresenting speed information of a pedestrian, a
nRepresents a pedestrian acceleration;
a coordinate transformation matrix representing a transformation from the carrier coordinate system b to the navigation coordinate system n; g
nIs the acceleration of gravity.
Step 2: and extracting the observation points of the conditional random field model.
And 2.1, extracting horizontal two-dimensional position information according to a fixed-length walking distance, extracting pedestrian height information at a zero-speed moment, acquiring observation points of the CRF model, and respectively recording the horizontal two-dimensional position of the pedestrian and the sampling moment of the height information.
(1) The horizontal two-dimensional position observation point extraction model is as follows, and when the following conditions are met:
recording the coordinates of the position point at the moment as a two-dimensional position observation point, and simultaneously recording the corresponding time:
time1(tob1)=t
wherein, Distance represents the Euclidean Distance between the current time and the sampling point coordinate of the last time; p
ob1(t
ob1) Represents t
ob1Observing two-dimensional coordinates of the points;
when represents tPosition coordinates of navigation output are carved; threshold is the set walking distance Threshold; time1 represents the time of day for all observation points.
(2) The height observation point extraction model is as follows when the condition is satisfied:
if v(t)==0
recording the height information at the moment as a height observation point, and simultaneously recording the corresponding time:
time2(tob2)=t
wherein H
ob2(t
ob2) Denotes the t-th
ob2An individual height observation point; time2 represents the time at which the altitude observation point corresponds;
height information indicating the navigation output at time t.
And step 3: and establishing an indoor electronic map, creating a state point according to the indoor structure information, and storing the coordinate of the state point.
And 3.1, creating an indoor electronic map by using the known indoor map, wherein the electronic map is as shown in figure 4, and storing the electronic map in the navigation computer.
And 3.2, solving coordinate points with the minimum and maximum numerical values in the electronic map as the value range of the state points, covering the whole value range with the equally spaced state points, and storing coordinate information (X, Y) of all the state points.
(1) Solving coordinate points with minimum and maximum numerical values in the electronic map:
pmin=(xmin,ymin)=min(X,Y)
pmax=(xmax,ymax)=max(X,Y)
wherein < pmin,pmaxThe minimum position point and the maximum position point of the coverage range of the state point are represented.
(2) The distribution range of the state points obtained according to the maximum position point and the minimum position point is as follows:
r1=pmin=(xmin,ymin)
r2=(xmin,ymax)
r3=(xmax,ymin)
r4=pmax=(xmax,ymax)
wherein < r1,r2,r3,r4The four vertex coordinates of the state point range matrix.
(3) And (3) solving the coordinates of the state points, namely selecting the minimum coordinate point as a first state point, selecting the Threshold length as the interval between the state points, wherein the state point model is as follows:
state1(0,0)=r1
state1=(is,js)=r1+(is×Threshold,js×Threshold)<r4
wherein state1 is a collective term for all state points; (i)s,js) Indicating the state of the state point store.
Step 3.3, because the height variation of each step is integral multiple of the height of the step when the pedestrian walks the corridor, the height of the state point in the stair adding area is based on the height of the step, the rule that the step height standing _ high is combined with the state point distribution of the two-dimensional position is set, and the operation model of the step height is as follows:
State2(N)=0+N×stair_high
wherein, State2 is the set of all step height State points, N represents the number, and State2(N) is determined by the number of steps. And combining the State1 and the State2 to obtain the three-dimensional coordinates of the State points, and storing the coordinate information of all the State points. The three-dimensional state point and the digital map are as shown in fig. 5.
And 4, a flow chart of a two-dimensional position map matching algorithm, a matching and fusing method based on the conditional random field algorithm is shown in FIG. 6.
Step 4.1, establishing a characteristic method according to the relation between the two-dimensional position observation point coordinates and each state point coordinatesA process; the state point corresponding to the two-dimensional position is represented as Sp。
Wherein f is
cRepresenting the relationship between the observation point coordinates and the state point coordinates; s
p(t
ob1) Represents t
ob1(x, y) coordinates of the time of day state point, and
P
ob1(t
ob1) Represents t
ob1The (x, y) coordinates of the observation point at the moment of time, and
σ
crepresenting the covariance of the range error of the state point from the observation point.
And 4.2, calculating the azimuth information of the observation point, and establishing a characteristic equation according to the azimuth angle between the azimuth information of the observation point and the state point at the moment corresponding to the azimuth information of the observation point.
Wherein sigmaθA covariance representing an observed azimuth error; b (S)p(tob1-1),Sp(tob1) ) represents tob1-1 and tob1Functions between the time of day state points; theta (S)p(tob1-1),Sp(tob1) Represents tob1-1 and tob1And an azimuth angle function between the time state points, wherein the function takes the positive direction of the X axis of the map coordinate system as a reference.
And 4.3, establishing a two-dimensional map matching mathematical model based on the conditional random field, and obtaining the maximum probability of the state sequence under the condition that the two-dimensional position is taken as the observation sequence, wherein the maximum probability sequence is the optimal state matching of the position.
Calculating a maximum probability state point sequence S by using a Viterbi algorithm
P*。
Wherein λ isp,μpRespectively representing the weight corresponding to each feature in the two-dimensional map matching mathematical model, wherein all the weights are set to be 1; i, l represents the number of characteristic functions; zob1Is a normalization factor.
And 5, a height information map matching algorithm based on the conditional random field algorithm.
And 5.1, estimating the state corresponding to each step of walking of the pedestrian according to the height of the step and the limit value of the step of walking of the pedestrian.
By using the limit of the step crossed by the pedestrian every time walking, the pedestrian is supposed to cross N at most every timeTThe step number is one, the height variation of each pedestrian walking should be (-N)T,NT) Within this range. Thus, the pedestrian is high for each walkThe state of the degree is:
Sh=(((-NT)×stair_high),((1-NT)×stair_high),…,(NT×stair_high))
and 5.2, establishing a characteristic equation which takes the height as an observation point and the state point corresponding to the observation point.
Wherein S ish(tob2) Represents tob2The height of the time status point; hob2(tob2) Represents tob2The height of the observation point at the moment; g represents the functional relation between the observation point and the height of the state point; sigmahRepresenting the covariance between the heights between the state point and the observation point; STAIR _ HIGH represents the height of each step; b (S)h(tob2-1),Sh(tob2) ) represents tob2-1 and tob2Functions between the time of day state points; h (S)h(tob2-1),Sh(tob2) ) represents tob2-1 and tob2The relative height function between the state points at the time.
And 5.3, solving the mean square error between the height information of all the previous observation points and the height of the matched state point.
δHob2(tob2)=Hob2(tob2)-Sh*(tob2)
δH
ob2The error vector between the observation point height and the matching state point height,
the average error vector is represented.
And 5.4, establishing a characteristic equation by taking the mean square error as another characteristic according to the relation between the heights of the adjacent state points and the height difference of each state point.
gc(Sh(tob2),Hob2(tob2),S_Hob2(tob2))=(Hob2(tob2)-Sh(tob2))-S_Hob2(tob2)
Wherein σsRepresents the height error covariance; s _ Hob2(tob2) Represents tob2Covariance of all observed errors prior to time of day.
And 5.5, establishing a height map matching mathematical model based on the conditional random field, and obtaining the maximum probability of the state sequence under the condition that the height is taken as the observation sequence, wherein the maximum probability sequence is the optimal state matching of the height.
Calculating a maximum probability state point sequence S by using a Viterbi algorithm
h*。
Wherein λ ish,μhAnd representing the weight values corresponding to the features, wherein the weight values are all set to be 1. i, l represents the number of characteristic functions; zob2Is a normalization factor.
Step 6: two-dimensional position and height information fusion
And 6.1, inquiring the sampling time of the corresponding observation sequence by using the state sequence with the best two-dimensional position matching for storage, and inquiring the sampling time of the corresponding observation sequence by using the state sequence with the best height matching for storage.
And 6.2, combining the two-dimensional optimal matching point and the height optimal matching point by using a method of adjacent time.
time=|time1(tob1)-time2(kob2)|kob2=1…tob2
Wherein the requirements are as follows: 0 < t
ob1<t
ob2. Finding out the minimum value in time and corresponding k
ob2Marking as
S*(tob1)=<SP*(tob1),S(tob1)>
Wherein, S (t)ob1) Represents tob1Pedestrian height information obtained by adopting near point fusion is adopted at any moment; s*Is the final pedestrian trajectory information.
And 6.3, correcting and feeding back the three-dimensional position information output by the inertial navigation system according to the matched three-dimensional position. The mathematical model of the correction feedback is as follows:
pn(t)=S*(tob1)
to verify the validity of the algorithm, experimental verification was performed. Taking a certain indoor office environment as an example, the experimental place comprises two indoor environments, namely a corridor and a corridor. Fig. 7 shows three-dimensional position information calculated by the inertial navigation system. Therefore, errors exist in navigation output no matter two-dimensional track or height information, and positioning accuracy is not accurate. A three-dimensional indoor map matching algorithm based on conditional random fields is shown in fig. 8. The experimental result shows that the matching result of the method has high accuracy and effectiveness.