CN109855620B - Indoor pedestrian navigation method based on self-backtracking algorithm - Google Patents

Indoor pedestrian navigation method based on self-backtracking algorithm Download PDF

Info

Publication number
CN109855620B
CN109855620B CN201811596821.0A CN201811596821A CN109855620B CN 109855620 B CN109855620 B CN 109855620B CN 201811596821 A CN201811596821 A CN 201811596821A CN 109855620 B CN109855620 B CN 109855620B
Authority
CN
China
Prior art keywords
pedestrian
filtering
walking
algorithm
navigation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201811596821.0A
Other languages
Chinese (zh)
Other versions
CN109855620A (en
Inventor
梁源
徐兵
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hunan Ordnance Science And Technology Research Institute Co Ltd
Hunan Weidao Technology Co ltd
Original Assignee
Beijing Yiqing Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Yiqing Technology Co ltd filed Critical Beijing Yiqing Technology Co ltd
Priority to CN201811596821.0A priority Critical patent/CN109855620B/en
Publication of CN109855620A publication Critical patent/CN109855620A/en
Application granted granted Critical
Publication of CN109855620B publication Critical patent/CN109855620B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Navigation (AREA)

Abstract

The invention discloses an indoor pedestrian navigation method based on a self-backtracking algorithm, wherein a navigated pedestrian is worn with an MEMS sensor, and the navigation method comprises the following steps: carrying out pedestrian gait calculation according to an MEMS gyroscope and an MEMS accelerometer to obtain walking information of a pedestrian for a period of time, carrying out linear detection on the walking information in the period of time, judging whether a walking route of the pedestrian in the period of time is a straight line, if the walking route of the pedestrian is in a straight line state, constructing a pseudo observed quantity according to course angle information, and carrying out navigation error detection and correction compensation by using a forward and reverse adaptive Kalman filtering algorithm; and outputting the indoor navigation and positioning result of the pedestrian. The invention fully explores the gait characteristics of the pedestrian when walking and utilizes the walking constraint conditions to estimate and compensate the navigation error of the pedestrian position and posture information, thereby realizing the purpose of improving the positioning and posture-fixing precision of the indoor pedestrian navigation scheme based on the motion sensor.

Description

Indoor pedestrian navigation method based on self-backtracking algorithm
Technical Field
The invention relates to indoor pedestrian navigation and positioning, in particular to an indoor pedestrian navigation method based on a self-backtracking algorithm.
Background
Location-based Services (LBS) are in urgent need in military, industrial and people's life fields. Particularly, with the advent of the mobile internet era, location services play an increasingly important role in daily life, and well-known location service providers include Google maps, high-grade maps and the like, which bring convenience to people going out. The indoor navigation technology is a popular direction of current research, and has important commercial value, for example, the indoor environment of a shopping center provides position information of shops for consumers, and the shopping is faster; the system is applied to a large parking lot, and can help a parking user to quickly search a parking space or reversely search a vehicle; and has important significance in providing public service for blind people for indoor guidance and emergency help. Due to the shielding of the building on the satellite signal, the GPS cannot realize accurate positioning in an indoor complex environment. Currently, developing an indoor positioning technology with low cost and high positioning accuracy is a hot topic of many scientific research institutions and commercial companies.
Pedestrian indoor positioning and navigation technologies are mainly classified into positioning methods based on radio frequency technologies (including WIFI, low-power Bluetooth (BLE), Zigbee (Zigbee), Ultra Wideband (UWB), global system for mobile communications (e.g., 4G), etc.), positioning based on geographic features (including geomagnetic positioning, map matching, barometer height measurement,
ultrasonic ranging, etc.), positioning based on motion sensors (dead reckoning using Micro-Electro-Mechanical systems (MEMS) gyros, accelerometers), etc. The positioning method based on the radio frequency technology and the positioning method based on the geographic features are indoor positioning means which have entered into commercial projects primarily at present, mainly adopt the relation between the wireless signal strength and the distance and the indoor geographic features as reference bases for positioning, collect a large number of signals, establish a comparison database, and estimate the positioning position by algorithms such as a fingerprint algorithm and the like. However, in the method, a large number of wireless transmitting devices need to be arranged in an indoor environment in advance and corresponding landmark information needs to be obtained, and meanwhile, corresponding acquisition/mapping tool software and geomagnetic measurement equipment need to finish acquisition of indoor map information and geomagnetic information. The scheme has high cost and complex operation flow. The positioning scheme based on the motion sensor has the advantages of low cost, strong expandability, little environmental influence and poor long-time precision. The reason that the scheme based on the motion sensor has poor long-time precision is limited by the manufacturing process, and the low-cost MEMS sensor has the defects of low precision, large random error and the like. Therefore, research of many researchers at home and abroad focuses on researching the error detection compensation technology of the MEMS sensor.
Disclosure of Invention
The invention provides an indoor pedestrian navigation method based on a self-backtracking algorithm.
In order to achieve the purpose, the technical scheme of the invention is as follows:
indoor pedestrian navigation method based on self-backtracking algorithm, wherein the navigated pedestrian is worn with MEMS
A sensor, wherein the steps of the navigation method include:
step 1: the pedestrian gait calculation is carried out according to the MEMS gyroscope and the MEMS accelerometer to obtain the walking information of the pedestrian for a period of time, the walking information in the period of time is subjected to linear detection,
step 2: judging whether the walking route of the pedestrian in the time period is a straight line, if the walking of the pedestrian is in a straight line state, executing the step 3, otherwise, returning to the step 1, wherein the walking information comprises the position, the course angle and the step length of the pedestrian;
and step 3: constructing a pseudo observed quantity according to the course angle information, and detecting, correcting and compensating the navigation error by using a forward and reverse adaptive Kalman filtering algorithm;
and 4, step 4: and (4) repeating the steps 1 to 3, continuously carrying out navigation error detection and correction compensation, and simultaneously outputting the indoor navigation positioning result of the pedestrian.
The scheme is further as follows: the length of time is the length of time that the pedestrian walks 10 steps.
The scheme is further as follows: the straight line detection and determination of whether the walking route of the pedestrian in the time period is a straight line is as follows: and calculating the difference value between the maximum value and the minimum value of the course angle between the beginning and the end of the duration, if the absolute value of the difference value is less than 20 x pi/180 radians, the pedestrian is considered to walk linearly in the time period, otherwise, the pedestrian does not walk linearly.
The scheme is further as follows: the steps of constructing the pseudo observed quantity and utilizing the forward and reverse adaptive Kalman filtering algorithm to carry out navigation error detection and correction compensation are as follows:
step one, calculating and respectively acquiring course angle information of each step in the duration by using a PDR (product data Rate):
Figure BDA0001921506060000031
acquiring step length information of each step in the duration:
Figure BDA0001921506060000032
secondly, constructing a pseudo observation sequence:
Figure BDA0001921506060000033
thirdly, establishing an expression model associated with the course angle information of each step, the step length information of each step and the pseudo observation sequence information:
Figure BDA0001921506060000034
H＝[0 0 0 0 1]，
a (k) is a system state transition matrix at the time k, and H is a measurement matrix;
fourthly, navigation error detection and correction compensation are carried out on the expression model by using a self-adaptive Kalman filtering algorithm, and the specific algorithm is as follows:
Figure BDA0001921506060000035
Pk,k-1＝A(k)Pk-1(A(k))T+Q
Figure BDA0001921506060000036
Figure BDA0001921506060000037
Pk＝[I-KkH]Pk,k-1
wherein:
Figure BDA0001921506060000038
represents the result of the filtering of the previous step,
Figure BDA0001921506060000039
one-step prediction of the representative state, Pk,k-1Representing the one-step prediction error variance, KkWhich represents the gain of the filtering, is,
Figure BDA00019215060600000310
is the main course, PkRepresenting the current step filter error variance, Pk-1Represents the filtering error variance of k-1 steps, and I representsThe identity matrix of the corresponding dimension is,
Figure BDA00019215060600000311
represents the adaptive measurement noise variance matrix:
Figure BDA0001921506060000041
wherein std (Z)pseduo) Represents the sequence ZpseduoStandard deviation of (d); wherein the content of the first and second substances,
Figure BDA0001921506060000042
the self-adaptive online adjustment can be carried out along with the change of the walking mode and the walking state of the pedestrian.
And fifthly, detecting and correcting the navigation error by using a forward and reverse adaptive Kalman filtering algorithm on the basis of the fourth step, wherein the specific algorithm is as follows:
first forward adaptive filtering:
setting the initial value of the forward filtering as follows:
Figure BDA0001921506060000043
starting to perform forward adaptive filtering:
Figure BDA0001921506060000044
Pk,k-1＝A(k)Pk-1(A(k))T+Q
Figure BDA0001921506060000045
Figure BDA0001921506060000046
Pk＝[I-KkH]Pk,k-1
wherein k is increased from 1 to 10; and obtaining a forward filtering result:
Figure BDA0001921506060000047
and
Figure BDA0001921506060000048
and (3) carrying out reverse self-adaptive filtering:
will be provided with
Figure BDA0001921506060000049
And
Figure BDA00019215060600000410
as an initial value of the filtering, there is,
Figure BDA00019215060600000411
Pk-1,k＝A(k-1)-1Pk(A(k-1)-1)T+Q
Figure BDA00019215060600000412
Figure BDA00019215060600000413
Pk-1＝[I-KkH]Pk-1,k
wherein, A (k-1)-1Represents the inverse matrix of A (k-1), k decreasing from 10 to 1 during the inverse filtering; obtaining a backward filtering result
Figure BDA0001921506060000051
And
Figure BDA0001921506060000052
at this time, one round of forward and reverse self-adaptive Kalman filtering is finished, and when the next round of operation is started, the next round of operation is carried out
Figure BDA0001921506060000053
And
Figure BDA0001921506060000054
and (3) providing the forward adaptive filtering algorithm to continuously repeat the operation process, and after two rounds of loop iteration, finally obtaining the result of the forward adaptive filtering algorithm:
Figure BDA0001921506060000055
and
Figure BDA0001921506060000056
the invention has the beneficial effects that:
(1) under the premise of not increasing hardware cost and not needing additional auxiliary information, the invention extracts accurate and effective walking constraint conditions from the gait characteristics of pedestrians during walking by fully exploring the gait characteristics, and estimates and compensates the navigation error of the pedestrian position and posture information by utilizing the walking constraint conditions, thereby realizing the purpose of improving the positioning and posture-fixing precision of the indoor pedestrian navigation scheme based on the motion sensor.
(2) The invention adopts a forward and reverse Kalman filtering algorithm to realize the detection of the navigation error, and the traditional one-way Kalman filtering algorithm can not obtain a better filtering result under the conditions of insufficient data volume and short filtering time, and even has the condition of larger error of the filtering result under individual conditions. And the forward and reverse Kalman filtering algorithm is adopted, and the Kalman filtering algorithm is carried out for multiple times in forward and reverse directions, so that the available information in the data is maximized, the useful information in the data is fully explored, and the filtering precision and the more stable filtering effect which are better than those of the traditional one-way Kalman filtering algorithm are obtained.
(3) The self-adaptive Kalman filtering algorithm designed by the invention does not depend on fixed filtering parameters any more, but adjusts corresponding filtering parameters dynamically on line according to the real-time change condition of the walking characteristics of pedestrians. Because different pedestrians have different walking gait characteristics and the gait characteristics can change along with the time change, at the moment, if a fixed single filtering parameter is adopted, all walking gait characteristics cannot be adapted, so that the Kalman filtering with the unchanged filtering parameter cannot obtain a satisfactory result under certain conditions. In the invention, by designing the self-adaptive Kalman filtering, the filtering parameters are self-adaptively adjusted on line according to the change of the walking characteristics of the pedestrian, so that the application range and the stability of the algorithm are effectively improved
The invention is described in detail below with reference to the figures and examples.
Drawings
FIG. 1 is a flow chart of an indoor pedestrian navigation scheme based on an online self-backtracking algorithm;
FIG. 2 is a flow chart of a walking information acquisition process
FIG. 3 is an algorithm flow diagram of a line detection algorithm;
FIG. 4 is an algorithm flow diagram of a forward and reverse adaptive Kalman filtering algorithm;
FIG. 5 is a schematic diagram of the real walking track of a pedestrian in a test area;
FIG. 6 is a graph of the variation of the pedestrian course angle calculated by the algorithm in this patent;
FIG. 7 is a graph of a pedestrian walking route calculated by the algorithm of the present patent.
Detailed Description
Indoor pedestrian navigation method based on self-backtracking algorithm, wherein the navigated pedestrian is worn with MEMS
A sensor, wherein the steps of the navigation method include:
step 1: the pedestrian gait calculation is carried out according to the MEMS gyroscope and the MEMS accelerometer to obtain the walking information of the pedestrian for a period of time, the walking information in the period of time is subjected to linear detection,
step 2: judging whether the walking route of the pedestrian in the time period is a straight line, if the walking of the pedestrian is in a straight line state, executing the step 3, otherwise, returning to the step 1, wherein the walking information comprises the position, the course angle and the step length of the pedestrian;
and step 3: constructing a pseudo observed quantity according to the course angle information, and detecting, correcting and compensating the navigation error by using a forward and reverse adaptive Kalman filtering algorithm;
and 4, step 4: and (4) repeating the steps 1 to 3, continuously carrying out navigation error detection and correction compensation, and simultaneously outputting the indoor navigation positioning result of the pedestrian.
Wherein the length of time is the length of time that the pedestrian is walking 10 steps.
In the examples: the straight line detection and determination of whether the walking route of the pedestrian in the time period is a straight line is as follows: and calculating the difference value between the maximum value and the minimum value of the course angle between the beginning and the end of the duration, if the absolute value of the difference value is less than 20 x pi/180 radians, the pedestrian is considered to walk linearly in the time period, otherwise, the pedestrian does not walk linearly.
In the examples: the steps of constructing the pseudo observed quantity and utilizing the forward and reverse adaptive Kalman filtering algorithm to carry out navigation error detection and correction compensation are as follows:
step one, calculating and respectively acquiring course angle information of each step in the duration by using a PDR (product data Rate):
Figure BDA0001921506060000061
acquiring step length information of each step in the duration:
Figure BDA0001921506060000062
secondly, constructing a pseudo observation sequence:
Figure BDA0001921506060000071
thirdly, establishing an expression model associated with the course angle information of each step, the step length information of each step and the pseudo observation sequence information:
Figure BDA0001921506060000072
H＝[0 0 0 0 1]，
a (k) is a system state transition matrix at the time k, and H is a measurement matrix;
fourthly, navigation error detection and correction compensation are carried out on the expression model by using a self-adaptive Kalman filtering algorithm, and the specific algorithm is as follows:
Figure BDA0001921506060000073
Pk,k-1＝A(k)Pk-1(A(k))T+Q
Figure BDA0001921506060000074
Figure BDA0001921506060000075
Pk＝[I-KkH]Pk,k-1
wherein:
Figure BDA0001921506060000076
represents the result of the filtering of the previous step,
Figure BDA0001921506060000077
one-step prediction of the representative state, Pk,k-1Representing the one-step prediction error variance, KkRepresenting the filter gain, PkRepresenting the current step filter error variance, Pk-1Represents the filtering error variance of step k-1, I represents the identity matrix of the corresponding dimension,
Figure BDA0001921506060000078
represents the adaptive measurement noise variance matrix:
Figure BDA0001921506060000079
wherein std (Z)pseduo) Represents the sequence ZpseduoStandard deviation of (d); wherein the content of the first and second substances,
Figure BDA00019215060600000710
the self-adaptive online adjustment can be carried out along with the change of the walking mode and the walking state of the pedestrian.
And fifthly, detecting and correcting the navigation error by using a forward and reverse adaptive Kalman filtering algorithm on the basis of the fourth step, wherein the specific algorithm is as follows:
first forward adaptive filtering:
setting the initial value of the forward filtering as follows:
Figure BDA0001921506060000081
starting to perform forward adaptive filtering:
Figure BDA0001921506060000082
Pk,k-1＝A(k)Pk-1(A(k))T+Q
Figure BDA0001921506060000083
Figure BDA0001921506060000084
Pk＝[I-KkH]Pk,k-1
wherein k is increased from 1 to 10; and obtaining a forward filtering result:
Figure BDA0001921506060000085
and
Figure BDA0001921506060000086
and (3) carrying out reverse self-adaptive filtering:
will be provided with
Figure BDA0001921506060000087
And
Figure BDA0001921506060000088
as an initial value of the filtering, there is,
Figure BDA0001921506060000089
Pk-1,k＝A(k-1)-1Pk(A(k-1)-1)T+Q
Figure BDA00019215060600000810
Figure BDA00019215060600000811
Pk-1＝[I-KkH]Pk-1,k
wherein, A (k-1)-1Represents the inverse matrix of A (k-1), k decreasing from 10 to 1 during the inverse filtering; obtaining a backward filtering result
Figure BDA00019215060600000812
And
Figure BDA00019215060600000813
at this time, one round of forward and reverse self-adaptive Kalman filtering is finished, and when the next round of operation is started, the next round of operation is carried out
Figure BDA00019215060600000814
And
Figure BDA00019215060600000815
and (3) providing the forward adaptive filtering algorithm to continuously repeat the operation process, and after two rounds of loop iteration, finally obtaining the result of the forward adaptive filtering algorithm:
Figure BDA00019215060600000816
and
Figure BDA00019215060600000817
the principle of the embodiment is as follows: carrying out linear detection by using the gait information of the pedestrian in the walking process; when detecting that the pedestrian is in a linear motion state in the last time period, firstly determining the main course of the pedestrian in the time period, further constructing a corresponding pseudo-observation information sequence by using the main course information, and providing the pseudo-observation information sequence for a forward and reverse adaptive Kalman (Kalman) filtering algorithm, wherein the calculation process comprises the following steps: firstly, carrying out forward self-adaptive Kalman filtering once, providing a forward filtering result for a backward filtering algorithm to be used as an initial value of the backward filtering algorithm, carrying out backward filtering operation, providing the backward filtering result for the forward filtering algorithm to be used as the initial value of the forward filtering algorithm, repeatedly iterating a number of rounds to obtain a final navigation error estimation result, and carrying out compensation correction on the navigation error according to the result so as to achieve the purpose of improving the navigation precision. The overall flow chart of the method is shown in fig. 1 and fig. 2.
It should be noted that the pedestrian gait estimation PDR (pedestrian gait estimation) algorithm in this embodiment is a known technique, and this embodiment utilizes a gait result provided by the PDR algorithm and corrects the PDR result to achieve the purpose of improving the accuracy of the PDR algorithm, and the PDR algorithm is equivalent to providing original data for the specific implementation of this embodiment. The core function of the PDR is to convert the MEMS gyroscope and MEMS accelerometer into the heading information of the pedestrian and the step length information of the pedestrian, and the PDR algorithm will not be described in detail here.
The MEMS (microelectromechanical systems) related to the embodiment is called a micromechanical electromechanical system in Chinese. The driver and the sensor made by the traditional electromechanical process are widely applied to the industry, scientific research and even military industry. It is known to integrate drivers with sensors and integrated circuit chips.
For a clearer understanding, the above examples are described further below:
the specific process of the method comprises the following steps:
step 1: and (2) carrying out Pedestrian gait calculation (PDR) according to the MEMS gyroscope and the MEMS accelerometer to obtain walking history information (including position, course and step length) of the Pedestrian for a period of time, carrying out linear detection on the walking history information in the period of time, judging whether a walking route of the Pedestrian in the period of time is a straight line, if the walking route of the Pedestrian is in a linear state, executing the step 2, otherwise, continuously executing the step 1.
The main function of the straight line detection is to judge whether the pedestrian is in a straight line walking state in the previous time period; the feasibility analysis of this detection algorithm is as follows: although the accuracy of the MEMS device cannot be guaranteed for a long time, the MEMS device has a short-time accuracy meeting the requirement in a short time, that is, by selecting a short period of time (in the embodiment, the selected detection time is the time length of 10 steps of walking of the pedestrian), the navigation result obtained by the MEMS device has a high confidence.
The principle of the linear detection method is introduced as follows: and extracting the maximum value and the minimum value of the course angle information of the MEMS device in a set historical time period (the time point of the time length that the pedestrian walks for 10 steps backwards (in the direction of time reduction) with the current time as the time period end point) and calculating a difference value, wherein if the absolute value of the difference value is less than 20 x pi/180 radians, the pedestrian is considered to walk in a straight line in the time period, otherwise, the pedestrian does not walk in a straight line. The straight line detection algorithm flow is shown in fig. 3, and the specific flow is as follows:
1. storing the MEMS device information in the previous time period and the PDR information obtained by resolving;
2. extracting the maximum value and the minimum value of course angle information in the PDR information in the period of time, and simultaneously calculating the absolute value of the difference value between the maximum value and the minimum value;
3. judging whether the absolute value is less than 20 x pi/180 radians or not, and if the absolute value is less than 20 x pi/180 radians, the walking is a straight line; if it is greater than 20 x pi/180 radians, it is not a straight line.
Step 2: and determining the main course angle information of the pedestrian walking in the time period on the basis of determining that the pedestrian is in the straight walking state in the last time period. Considering the device characteristics of the MEMS device and the walking characteristics of pedestrians, the method selects the course information of the starting point and the starting moment of the time period as the main course, and defines the main course as the main course
Figure BDA0001921506060000101
And step 3: and on the basis of obtaining the main course angle information, constructing a pseudo observed quantity, and further performing navigation error detection and correction compensation by using a forward and reverse self-adaptive Kalman algorithm.
For convenience of description hereinafter, the variables used are defined herein:
Figure BDA0001921506060000102
representing course angle information of each step obtained by resolving by the PDR in the time period;
Figure BDA0001921506060000103
representing step length information of each step obtained by resolving by the PDR in the time period;
by using
Figure BDA0001921506060000104
And
Figure BDA0001921506060000105
construction of pseudo-Observation sequences, pseudo-Observation sequences ZpseduoConstructed as follows
Figure BDA0001921506060000106
Based on the pseudo-observation sequence, forward and backward adaptive Kalman filtering algorithm operation is performed, and firstly, the system model (forward) is introduced as follows:
X(k+1)＝A(k)X(k)+W(k)
Z(k)＝HX(k)+V(k)
wherein k represents a system time ranging from 1 to 10, and x (k) is a discrete system state variable, specifically:
X(k)＝[pXpY△Roll△Pitch△Heading]T
pX，pYthe position errors in the X direction and the Y direction in the navigation coordinate system, △Roll，△Pitch，△HeadingRoll angle error, pitch angle error and course angle error are respectively, and the unit of position error measurement is meter, and the unit of angle error measurement is radian. Z (k) is an observed value of the system, where Z (k) ═ Zpseduo{k}(Zpseduo{ k } represents the sequence ZpseduoThe k-th element of (1). A (k) is the system state transition matrix at time k, and W (k) is the system processNoise, H is the measurement matrix, and V (k) is the system measurement noise. W (k) -N (0, Q) and V (k) -N (0, R) are white Gaussian noise, and Q and R are positive definite covariance matrixes. In this patent, the specific forms of A (k) and H are as follows:
Figure BDA0001921506060000111
H＝[0 0 0 0 1]
the forward and reverse adaptive Kalman filter algorithm designed in the present invention is introduced based on the above model, and for convenience of introduction, the adaptive Kalman filter algorithm part is first introduced here: applying an adaptive Kalman filter algorithm to the model described above is:
Figure BDA0001921506060000112
Pk,k-1＝A(k)Pk-1(A(k))T+Q
Figure BDA0001921506060000113
Figure BDA0001921506060000114
Pk＝[I-KkH]Pk,k-1
wherein the content of the first and second substances,
Figure BDA0001921506060000121
represents the result of the filtering of the previous step,
Figure BDA0001921506060000122
one-step prediction of the representative state, Pk,k-1Representing the one-step prediction error variance, KkRepresenting the filter gain, PkRepresenting the current step filter error variance, Pk-1Represents the filtering error variance of step k-1, I represents the identity matrix of the corresponding dimension,
Figure BDA00019215060600001215
represents the adaptive measurement noise variance matrix:
Figure BDA0001921506060000123
wherein std (Z)pseduo) Represents the sequence ZpseduoStandard deviation of (d); wherein the content of the first and second substances,
Figure BDA0001921506060000124
the self-adaptive online adjustment can be carried out along with the change of the walking mode and the walking state of the pedestrian.
On the basis of the adaptive Kalman filtering algorithm, a forward and reverse adaptive Kalman filtering algorithm is further introduced, the algorithm is divided into a forward adaptive Kalman filtering part and a reverse adaptive Kalman filtering part, the algorithm operation flow is shown as a figure 4, and the specific flow is as follows:
first forward adaptive filtering:
setting the initial value of the forward filtering as follows:
Figure BDA0001921506060000125
starting forward adaptive Kalman filtering:
Figure BDA0001921506060000126
Pk,k-1＝A(k)Pk-1(A(k))T+Q
Figure BDA0001921506060000127
Figure BDA0001921506060000128
Pk＝[I-KkH]Pk,k-1
wherein k is increased from 1 to 10; and obtaining a forward filtering result:
Figure BDA0001921506060000129
and
Figure BDA00019215060600001210
will be provided with
Figure BDA00019215060600001211
And
Figure BDA00019215060600001212
and as a filtering initial value, performing reverse self-adaptive Kalman filtering:
Figure BDA00019215060600001213
Pk-1,k＝A(k-1)-1Pk(A(k-1)-1)T+Q
Figure BDA00019215060600001214
Figure BDA0001921506060000131
Pk-1＝[I-KkH]Pk-1,k
wherein, A (k-1)-1Represents the inverse matrix of A (k-1), k decreasing from 10 to 1 during the inverse filtering; obtaining a backward filtering result
Figure BDA0001921506060000132
And
Figure BDA0001921506060000133
at this time, a round of forward and backward adaptive Kalman filtering is finished. When the next round of operation begins, the next round of operation will be
Figure BDA0001921506060000134
And
Figure BDA0001921506060000135
and providing the forward adaptive filtering algorithm to continuously repeat the operation process. After two rounds of loop iteration, the result of the forward and reverse self-adaptive Kalman filtering algorithm is finally obtained:
Figure BDA0001921506060000136
and
Figure BDA0001921506060000137
wherein the content of the first and second substances,
Figure BDA0001921506060000138
the method comprises the steps of carrying out error correction compensation on a navigation result by utilizing an error item, including the position error and the angle error of pedestrian navigation at the current moment, and achieving the purpose of improving the navigation positioning precision.
And 4, step 4: and (4) repeating the steps 1-3, continuously carrying out navigation error on-line estimation and correction compensation, and simultaneously outputting a pedestrian indoor navigation positioning result. The pedestrian walking schematic diagram of the indoor pedestrian navigation method based on the online self-backtracking algorithm is shown in fig. 5 to 7, wherein fig. 5 represents a real walking route of the pedestrian, a white area is a corridor, a black area is an impassable office area, the pedestrian starts from one end of the corridor, walks to the other end and then turns back to a starting point; fig. 6 represents a course angle variation graph calculated by the algorithm in this patent for the pedestrian walking situation shown in fig. 5, and as can be seen from the walking schematic diagram of the pedestrian in fig. 5, the walking mode of the pedestrian can be divided into the following three sections: 1. straight going; 2. turning (180 degrees); 3. straight going; as can be seen from FIG. 6, the variation of the course angle calculated by the navigation algorithm designed in the present patent is very consistent with the pedestrian walking mode; fig. 7 represents a graph of the walking route of the pedestrian calculated by the algorithm in the patent, and a comparison between the walking schematic diagram (fig. 5) of the pedestrian and the graph (fig. 7) of the walking route calculated by the algorithm in the patent shows that the navigation algorithm designed by the patent has good and stable navigation positioning accuracy.
In summary, the above description is only a preferred embodiment of the present invention, and is not intended to limit the scope of the present invention. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (3)

1. An indoor pedestrian navigation method based on a self-backtracking algorithm, a navigated pedestrian is worn with an MEMS sensor, and the navigation method is characterized by comprising the following steps:
step 1: the pedestrian gait calculation is carried out according to the MEMS gyroscope and the MEMS accelerometer to obtain the walking information of the pedestrian for a period of time, the walking information in the period of time is subjected to linear detection,
step 2: judging whether the walking route of the pedestrian in the time period is a straight line, if the walking of the pedestrian is in a straight line state, executing the step 3, otherwise, returning to the step 1, wherein the walking information comprises the position, the course angle and the step length of the pedestrian;
and step 3: constructing a pseudo observed quantity according to the course angle information, and detecting, correcting and compensating the navigation error by using a forward and reverse adaptive Kalman filtering algorithm;
and 4, step 4: repeating the steps 1 to 3, continuously carrying out navigation error detection and correction compensation, and simultaneously outputting a pedestrian indoor navigation positioning result;
the steps of constructing the pseudo observed quantity and utilizing the forward and reverse adaptive Kalman filtering algorithm to carry out navigation error detection and correction compensation are as follows:
step one, calculating and respectively acquiring course angle information of each step in the duration by using a PDR (product data Rate):
Figure FDA0002635574800000011
acquiring step length information of each step in the duration:
Figure FDA0002635574800000012
secondly, constructing a pseudo observation sequence:
Figure FDA0002635574800000013
thirdly, establishing an expression model associated with the course angle information of each step, the step length information of each step and the pseudo observation sequence information:
Figure FDA0002635574800000014
H＝[0 0 0 0 1]，
a (k) is a system state transition matrix at the time k, and H is a measurement matrix;
fourthly, navigation error detection and correction compensation are carried out on the expression model by using a self-adaptive Kalman filtering algorithm, and the specific algorithm is as follows:
Figure FDA0002635574800000021
Pk,k-1＝A(k)Pk-1(A(k))T+Q
Figure FDA0002635574800000022
Figure FDA0002635574800000023
Pk＝[I-KkH]Pk,k-1
wherein:
Figure FDA0002635574800000024
represents the result of the filtering and is then,
Figure FDA0002635574800000025
representing the filtering result of the previous step, Q represents a covariance matrix corresponding to model noise,
Figure FDA0002635574800000026
representing the state of the filtering result as a prediction of the previous step, Pk,k-1Representing the variance of the prediction filtering error of the previous step,
Figure FDA0002635574800000027
is the main course, KkRepresenting the filter gain, PkFiltering on behalf of current stepError variance, Pk-1Represents the filtering error variance of step k-1, I represents the identity matrix of the corresponding dimension,
Figure FDA0002635574800000028
represents the adaptive measurement noise variance matrix:
Figure FDA0002635574800000029
wherein std (Z)pseduo) Represents the sequence ZpseduoStandard deviation of (d); wherein the content of the first and second substances,
Figure FDA00026355748000000210
self-adaptive online adjustment can be carried out along with the change of the walking mode and the walking state of the pedestrian;
and fifthly, detecting and correcting the navigation error by using a forward and reverse adaptive Kalman filtering algorithm on the basis of the fourth step, wherein the specific algorithm is as follows:
first forward adaptive filtering:
setting the initial value of the forward filtering as follows:
Figure FDA00026355748000000211
starting to perform forward adaptive filtering:
Figure FDA0002635574800000031
Figure FDA0002635574800000032
Figure FDA0002635574800000033
Figure FDA0002635574800000034
Pk＝[I-KkH]Pk,k-1
wherein k is increased from 1 to 10; and obtaining a forward filtering result:
Figure FDA0002635574800000035
and
Figure FDA0002635574800000036
and (3) carrying out reverse self-adaptive filtering:
will be provided with
Figure FDA0002635574800000037
And
Figure FDA0002635574800000038
as an initial value of the filtering, there is,
Figure FDA0002635574800000039
Pk-1,k＝A(k-1)-1Pk(A(k-1)-1)T+Q
Figure FDA00026355748000000310
Figure FDA00026355748000000311
Pk-1＝[I-KkH]Pk-1,k
wherein, A (k-1)-1Represents the inverse matrix of A (k-1), k decreasing from 10 to 1 during the inverse filtering; obtaining a backward filtering result
Figure FDA00026355748000000312
And P1 2；
At this time, one round of forward and reverse self-adaptive Kalman filtering is finished, and when the next round of operation is started, the next round of operation is carried out
Figure FDA00026355748000000313
And P1 2And (3) providing the forward adaptive filtering algorithm to continuously repeat the operation process, and after two rounds of loop iteration, finally obtaining the result of the forward adaptive filtering algorithm:
Figure FDA00026355748000000314
and
Figure FDA00026355748000000315
2. the navigation method according to claim 1, wherein the time period is a time length for which the pedestrian walks by 10 steps.
3. The navigation method according to claim 1, wherein the straight line detection and determination of whether the pedestrian's walking route in the time period is a straight line is: and calculating the difference value between the maximum value and the minimum value of the course angle between the beginning and the end of the duration, if the absolute value of the difference value is less than 20 x pi/180 radians, the pedestrian is considered to walk linearly in the time period, otherwise, the pedestrian does not walk linearly.
CN201811596821.0A 2018-12-26 2018-12-26 Indoor pedestrian navigation method based on self-backtracking algorithm Active CN109855620B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811596821.0A CN109855620B (en) 2018-12-26 2018-12-26 Indoor pedestrian navigation method based on self-backtracking algorithm

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811596821.0A CN109855620B (en) 2018-12-26 2018-12-26 Indoor pedestrian navigation method based on self-backtracking algorithm

Publications (2)

Publication Number Publication Date
CN109855620A CN109855620A (en) 2019-06-07
CN109855620B true CN109855620B (en) 2020-10-09

Family

ID=66892351

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811596821.0A Active CN109855620B (en) 2018-12-26 2018-12-26 Indoor pedestrian navigation method based on self-backtracking algorithm

Country Status (1)

Country Link
CN (1) CN109855620B (en)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110514226B (en) * 2019-09-04 2021-04-27 电子科技大学 Course correction method for pedestrian inertial navigation
CN111141279B (en) * 2019-12-20 2022-07-01 北京小马慧行科技有限公司 Method and device for processing driving track
CN111189443B (en) * 2020-01-14 2022-11-11 电子科技大学 Pedestrian navigation method for online step length calibration, motion deviation angle correction and adaptive energy management
CN112729282B (en) * 2020-12-21 2022-03-25 杭州电子科技大学 Indoor positioning method integrating single anchor point ranging and pedestrian track calculation
CN112923919B (en) * 2021-01-21 2023-05-16 湖南云箭格纳微信息科技有限公司 Pedestrian positioning method and system based on graph optimization
CN113203416B (en) * 2021-03-19 2022-07-12 电子科技大学 Pedestrian dead reckoning method for swing arm pedestrian
CN113091748A (en) * 2021-04-12 2021-07-09 北京航空航天大学 Indoor self-calibration navigation positioning method
CN114111833B (en) * 2021-10-09 2024-03-29 湘潭大学 Pedestrian step length estimation method

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6826477B2 (en) * 2001-04-23 2004-11-30 Ecole Polytechnique Federale De Lausanne (Epfl) Pedestrian navigation method and apparatus operative in a dead reckoning mode
DE102010031350A1 (en) * 2010-07-14 2012-01-19 Bayerische Motoren Werke Aktiengesellschaft Method for determining position of person in e.g. pedestrian navigation, involves determining value of motion based on determined motion class, and using value of motion parameter for determining estimated actual position of person
US9863784B2 (en) * 2014-02-23 2018-01-09 PNI Sensor Corporation Orientation estimation utilizing a plurality of adaptive filters
CN104613965B (en) * 2015-03-02 2017-10-17 大连理工大学 A kind of step-by-step movement pedestrian navigation method based on bidirectional filtering smoothing technique
CN105043385B (en) * 2015-06-05 2018-10-26 北京信息科技大学 A kind of method for adaptive kalman filtering of pedestrian's Camera calibration
CN107655476B (en) * 2017-08-21 2021-04-20 南京航空航天大学 Pedestrian high-precision foot navigation method based on multi-information fusion compensation
CN107677267A (en) * 2017-08-22 2018-02-09 重庆邮电大学 Indoor pedestrian navigation course feedback modifiers method based on MEMS IMU
CN108007477B (en) * 2017-11-29 2020-09-25 哈尔滨工程大学 Inertial pedestrian positioning system error suppression method based on forward and reverse filtering
CN108645406A (en) * 2018-04-19 2018-10-12 北京理工大学 A kind of indoor autonomic positioning method based on score field pedestrian movement perception

Also Published As

Publication number Publication date
CN109855620A (en) 2019-06-07

Similar Documents

Publication Publication Date Title
CN109855620B (en) Indoor pedestrian navigation method based on self-backtracking algorithm
US8751151B2 (en) System and method for localizing a trackee at a location and mapping the location using inertial sensor information
Poulose et al. Performance analysis of sensor fusion techniques for heading estimation using smartphone sensors
US9591458B2 (en) Method and apparatus for adaptive positioning
US20070282565A1 (en) Object locating in restricted environments using personal navigation
US20090192708A1 (en) Method and system for estimating step length pedestrian navigation system
CN109164411B (en) Personnel positioning method based on multi-data fusion
CN108387236B (en) Polarized light SLAM method based on extended Kalman filtering
KR101473653B1 (en) Pedestrian Dead-Reckoning apparatus based on pedestrian motion recognition and method thereof
CN106441302B (en) Indoor positioning method in large-scale open area
KR20150094434A (en) System for filtering location of Mobile terminal by fusing wi-fi location and sensing information
Yu et al. Precise 3D indoor localization and trajectory optimization based on sparse Wi-Fi FTM anchors and built-in sensors
Herrera et al. Improving data fusion in personal positioning systems for outdoor environments
CN109001789B (en) Unmanned vehicle positioning fusion method based on cross-correlation entropy registration
EP3415869B1 (en) Electronic device for improving dead reckoning-based positioning accuracy
CN112729301A (en) Indoor positioning method based on multi-source data fusion
CN116567531A (en) Sensor fusion indoor positioning method and system based on particle filter algorithm
Islam et al. An effective approach to improving low-cost GPS positioning accuracy in real-time navigation
US11029415B2 (en) Systems and methods for estimating initial heading at start-up of navigation
CN109737957B (en) INS/LiDAR combined navigation method and system adopting cascade FIR filtering
Lategahn et al. Robust pedestrian localization in indoor environments with an IMU aided TDoA system
CN115451946A (en) Indoor pedestrian positioning method combining MEMS-IMU and Wi-Fi
Mahida et al. Indoor positioning framework for visually impaired people using Internet of Things
Chirakkal et al. Exploring smartphone-based indoor navigation: A QR code assistance-based approach
Liu et al. A Novel Minimum Distance Constraint Method Enhanced Dual-Foot-Mounted Inertial Navigation System for Pedestrian Positioning

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20220124

Address after: 410000 3rd floor, unit 103, No.1 small instrument assembly plant, Jinzhuo Industrial Park, 118 Qingzhuhu Road, Qingzhuhu street, Kaifu District, Changsha City, Hunan Province

Patentee after: Hunan ordnance Technology Center Co.,Ltd.

Patentee after: Hunan Weidao Technology Co.,Ltd.

Address before: Room 315, 3 / F, building 10, yard 1, JinFang Road, Chaoyang District, Beijing

Patentee before: BEIJING YIQING TECHNOLOGY Co.,Ltd.

CP03 Change of name, title or address
CP03 Change of name, title or address

Address after: Room 919, 9th Floor, No. 9, Lixiang East Road, Changsha Economic and Technological Development Zone, Changsha, Hunan Province, 410000

Patentee after: Hunan Ordnance Science and Technology Research Institute Co., Ltd.

Patentee after: Hunan Weidao Technology Co.,Ltd.

Address before: 410000 3rd floor, unit 103, No.1 small instrument assembly plant, Jinzhuo Industrial Park, 118 Qingzhuhu Road, Qingzhuhu street, Kaifu District, Changsha City, Hunan Province

Patentee before: Hunan ordnance Technology Center Co.,Ltd.

Patentee before: Hunan Weidao Technology Co.,Ltd.