CN111307148A - Pedestrian positioning method based on inertial network - Google Patents

Pedestrian positioning method based on inertial network Download PDF

Info

Publication number
CN111307148A
CN111307148A CN202010257889.7A CN202010257889A CN111307148A CN 111307148 A CN111307148 A CN 111307148A CN 202010257889 A CN202010257889 A CN 202010257889A CN 111307148 A CN111307148 A CN 111307148A
Authority
CN
China
Prior art keywords
waist
pedestrian
foot
inertial
course
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.)
Granted
Application number
CN202010257889.7A
Other languages
Chinese (zh)
Other versions
CN111307148B (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.)
Beihang University
Original Assignee
Beihang University
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 Beihang University filed Critical Beihang University
Priority to CN202010257889.7A priority Critical patent/CN111307148B/en
Publication of CN111307148A publication Critical patent/CN111307148A/en
Application granted granted Critical
Publication of CN111307148B publication Critical patent/CN111307148B/en
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation

Abstract

The invention discloses a pedestrian positioning method based on an inertial network, which is based on the fact that in the walking process of pedestrians, the waist and the feet of the pedestrians have different motion characteristics, inertial sensors are respectively installed on the waist and the feet of a human body to respectively sense the operation information of the human body, the motion characteristics of different parts of the human body are integrated and utilized, the special angle and distance constraint relation between the waist and the feet in the walking process of the pedestrians is utilized, and the course angle and distance measurement error in a pedestrian dead reckoning method are restrained based on an inequality constraint Kalman filtering method, so that more accurate pedestrian positioning is realized. The pedestrian positioning method based on the inertial network is provided by combining the special physiological characteristics of the waist and the feet of the pedestrian during movement, overcomes the limitation of the existing pedestrian inertial navigation technology, and has practical application significance for indoor pedestrian navigation with low cost and high precision.

Description

Pedestrian positioning method based on inertial network
Technical Field
The invention relates to the technical field of indoor positioning, in particular to a pedestrian positioning method based on an inertial network.
Background
The indoor positioning technology is a core technology of smart cities, Internet of things, emergency rescue and the like, and is closely related to the daily life of people. More than 80% of the time for human beings to move in indoor environments leads to the explosion of the indoor positioning market. The rapidly increasing industrial demand puts new requirements on the positioning technology beyond the traditional performance indexes such as positioning accuracy, response speed and stability, and particularly puts forward the demand on the universal and autonomous indoor positioning technology. Various indoor positioning technologies come into the spotlight, including Wi-Fi, RFID, UWB based wireless communication, ultrasonic positioning technologies, and technologies based on visual, geomagnetic, inertial sensors of MEMS sensors, etc. Besides the inertial positioning technology, other existing indoor positioning technologies generally require external facilities or a priori database, and cannot meet the requirements for universality and autonomy. However, the inertial navigation technique with autonomy and universality mainly aims at the motion of a rigid body, and when the inertial navigation technique is applied to a human body with a special physiological motion state, not only the inherent accumulated error of the inertial navigation exists, but also an additional error is added, so that the accuracy of pedestrian positioning is reduced.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: the pedestrian positioning method based on the inertial network overcomes the limitation of the traditional inertial navigation which is directly used for positioning the pedestrian, combines the special physiological characteristics of the waist and the feet when the pedestrian moves, and has important practical significance for realizing the ubiquitous application of low-cost and high-precision indoor pedestrian navigation.
The technical scheme adopted by the invention for solving the technical problems is as follows: a pedestrian positioning method based on an inertial network comprises the following steps:
step 1, respectively fixing 2-5 MEMS inertial sensors on the waist and the foot of a human body, and receiving the acquisition information of each MEMS inertial sensor by an upper computer through Bluetooth in real time;
step 2, respectively acquiring the motion characteristics of the waist and the feet in the walking process of the pedestrian, including course angles, step lengths and positions, based on angle information and speed information acquired by the waist and foot MEMS inertial sensors in real time;
step 3, acquiring the course change of the pedestrian according to the information of the waist based on the motion characteristic that the waist moves relatively stably in the walking process of the pedestrian, simultaneously carrying out straight-ahead detection on the acquired course angle of the waist according to the motion characteristic that the pedestrian walks along a straight line within a few seconds, and correcting the course of a straight-ahead motion state by using an averaging method;
step 4, further correcting and restraining a waist course angle and a foot distance measurement error based on an inequality constraint Kalman filtering method by utilizing an inherent angle constraint relation between the waist and the foot and a distance constraint relation between the waist and the foot in the walking process of the pedestrian;
and 5, carrying out dead reckoning on the basis of the acquired waist course angle and the acquired foot distance and outputting the position information of the pedestrian in real time.
Further, in the step 1, the MEMS inertial sensors are respectively arranged on the waist and the feet of the human body and form an inertial network measuring system together with the Bluetooth communication module; the inertial sensors of the waist and the feet are respectively nodes of an inertial network measuring system, and information is acquired and shared in real time through Bluetooth.
Further, the step 2 of acquiring the motion characteristics of the waist and the feet comprises the following steps:
(2.1) according to the waist movement characteristics in the walking process of the pedestrian, selecting an acceleration signal of an MEMS sensor along the direction opposite to the gravity acceleration, based on the time interval between two adjacent acceleration maximum points as a gait cycle, in the gait cycle, resolving the waist course angle of the pedestrian walking based on an angular velocity signal of the MEMS sensor taking the gravity acceleration direction as an axis, and calculating the current waist step length by using a waist vertical acceleration step length model;
and (2.2) selecting the time interval between two adjacent zero-speed state points of the MEMS sensor as a gait cycle according to the motion characteristics of the foot in the walking process of the pedestrian, acquiring the current course angle and step length of the pedestrian in the gait cycle based on a strapdown inertial navigation system resolving method, and calculating the pedestrian position information from the foot by using the track based on the current course angle and step length information of the foot in the walking process of the pedestrian.
Further, the step 3 of performing the straight-ahead detection and correction on the waist course angle includes the following steps:
based on the fact that the foot moves violently and the waist moves stably relatively in the walking process of the pedestrian, the course obtained by a waist sensor in an inertial network measurement system is selected for the course of the walking process of the pedestrian, then the change amount of the course angle of the waist of 3-5 continuous adjacent gait cycles is observed by combining the movement characteristics of linear movement within a few seconds in the walking process of the pedestrian, and the straight-moving state of the pedestrian is judged according to the generalized likelihood ratio; when the pedestrian is in a straight-going state, correcting the heading of the state by using an averaging method, and correcting the heading of the pedestrian for the first time.
Further, the step 4 of correcting and suppressing the error based on the inequality constraint kalman filtering method includes the following steps:
(4.1) constructing space angle inequality constraint by presetting an included angle threshold value based on that the waist and the feet of the pedestrian belong to the same human body and the movement direction of the human body has an inherent included angle in the walking process of the pedestrian;
(4.2) then, constructing inequality constraints between the waist and the feet based on inherent correlation of waist displacement and foot displacement, waist course and foot course in each gait cycle of the pedestrian;
and (4.3) finally, information of the waist and the feet is fused for complementation of advantages and disadvantages, and measurement errors of the course angle of the waist and the distance between the feet in the gait cycle are further corrected and restrained based on an inequality constraint Kalman filtering method.
Compared with the prior art, the invention has the advantages that:
(1) the invention adopts a pedestrian positioning method based on an inertial network, and the two inertial measurement sensors respectively collect information of the waist and the feet to form an inertial network measurement system, thereby not only realizing effective utilization of pedestrian movement characteristics, inhibiting the inherent error of the traditional inertial navigation, but also overcoming the extra error generated in the physiological movement process of the pedestrian and further improving the accuracy of pedestrian positioning.
(2) The invention adopts a pedestrian positioning method based on an inertial network, is a universal autonomous positioning method, is an improvement on the existing pedestrian dead reckoning algorithm, can adapt to gait changes in the human motion process, and realizes more accurate distance and course measurement.
Drawings
FIG. 1 is a schematic diagram illustrating the steps of a pedestrian positioning method based on an inertial network according to the present invention;
FIG. 2 is a schematic diagram of the human body installation of the inertial network measurement system of the pedestrian positioning method based on the inertial network according to the present invention;
FIG. 3 is a reference trajectory diagram of a walking experiment of a field-shaped path based on the pedestrian positioning method of the inertial network;
FIG. 4 is a flow chart of a pedestrian positioning method based on an inertial network for initially correcting a waist course angle by straight-ahead detection;
FIG. 5 is a schematic diagram of the foot and waist angle constraint relationship of a pedestrian positioning method based on an inertial network according to the present invention;
FIG. 6 is a flow chart of an inequality constraint Kalman filtering method for further correcting course angle and distance according to the present invention;
Detailed Description
The technical solutions in the embodiments of the present invention will be described clearly and completely with reference to the accompanying drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, rather than all embodiments, and all other embodiments obtained by a person skilled in the art based on the embodiments of the present invention belong to the protection scope of the present invention without creative efforts.
According to one embodiment of the invention, the pedestrian positioning method based on the inertial network comprises the following steps:
step 1, respectively fixing 2-5 MEMS inertial sensors on the waist and the foot of a human body, and receiving the acquired information of each MEMS inertial sensor in real time by an upper computer (a PC, a notebook computer, a tablet personal computer or a mobile phone) through wireless communication (Bluetooth);
step 2, respectively acquiring the motion characteristics of the waist and the feet in the walking process of the pedestrian, including course angles, step lengths and positions, based on angle information and speed information acquired by the waist and foot sensors in real time;
step 3, acquiring the course change of the pedestrian according to the information of the waist based on the motion characteristic that the waist moves relatively stably in the walking process of the pedestrian, simultaneously carrying out straight-ahead detection on the acquired course angle of the waist according to the motion characteristic that the pedestrian walks along a straight line within a few seconds, and correcting the course of a straight-ahead motion state by using an averaging method;
step 4, further correcting and restraining a waist course angle and a foot distance measurement error based on an inequality constraint Kalman filtering method by utilizing an inherent angle constraint relation between the waist and the foot and a distance constraint relation between the waist and the foot in the walking process of the pedestrian;
and 5, carrying out dead reckoning real-time output based on the acquired waist course angle and the acquired foot distance to obtain the position information of the pedestrian.
In the step 1, the MEMS inertial sensors are respectively placed on the waist and the foot of the human body, and form an inertial network measurement system together with the bluetooth communication module, and the installation manner is shown in fig. 2. The inertial sensors of the waist and the feet are respectively nodes of an inertial network measuring system, and information is acquired and shared in real time through Bluetooth.
The step 2 of obtaining the motion characteristics of the waist and the feet refers to obtaining the course angle and the step length from the waist and the course angle, the step length and the position from the feet by utilizing a Pedestrian Dead Reckoning (PDR) method based on information collected by an inertial network in real time.
Firstly, according to the waist movement characteristics of pedestrians in the walking process, an acceleration signal (hereinafter referred to as vertical acceleration) of an MEMS sensor along the direction opposite to the gravity acceleration is selected, and a gait cycle is set based on the time interval between two adjacent maximum points of the vertical acceleration. In a gait cycle, the waist course angle of pedestrian walking is calculated based on an angular velocity signal of the MEMS sensor with the gravity acceleration direction as an axis
Figure BDA0002438119940000041
Finding out the maximum value a of the vertical acceleration of the waist part in the gait cycle based on the step length model of the vertical acceleration of the waist part of the pedestrianverticalmaxAnd a minimum value averticalminCalculating the forward displacement of the waist, namely the step length SL, and the step length calculation formula is as follows:
Figure BDA0002438119940000042
wherein K represents a parameter which needs to be calibrated in advance.
Secondly, the feet of the pedestrians can periodically go through a static section in the walking process, and the time interval between two adjacent zero-speed state points of the MEMS sensor is selected as a gait cycle. In a gait cycle, acquiring the current foot step length d of the pedestrian walking based on a resolving method of a strapdown inertial navigation systemFootAnd foot course angle
Figure BDA0002438119940000043
And finally, calculating the pedestrian position information of the foot by using the track based on the current step length and course angle information of the pedestrian in the walking process acquired by the foot sensor.
And 3, performing straight-ahead detection and correction on the waist course angle in the step 3, wherein the course obtained by a waist sensor in the inertial network measurement system is selected based on the most violent foot movement and relatively stable waist movement from the whole human body angle in the walking process of the pedestrian. And then, the inherent error of the traditional inertial navigation is restrained by combining the motion characteristic that the waist moves along a straight line within a few seconds in the walking process of the pedestrian, and the waist course angle obtained by the PDR is corrected preliminarily.
The heading angle of the pedestrian is approximately constant during the straight-ahead traveling, so that the heading drift of the waist can be suppressed by detecting the straight-ahead traveling state of the pedestrian. The traveling state of the pedestrian in the room may be classified into two kinds according to the motion state: straight going and turning. The physical quantity directly influenced by the two states is the waist course angle, and simultaneously, the waist course angle is caused by the movementThe turning of the pedestrian is usually completed by multiple gaits, so to judge the advancing state of the pedestrian, the change delta A of the waist course angle of n continuous gaits cycles is firstly observedi(i ═ 1,2,. cndot., n). Since the course angle of the pedestrian is almost constant during straight walking, the corresponding Δ AiThe theoretical value is 0, and then the pedestrian straight-going state is judged according to the Generalized Likelihood Ratio (GLRT). When the pedestrian is in a straight-going state, correcting the course angle of the waist in the state as follows:
Figure BDA0002438119940000051
the method based on the inequality constraint Kalman filtering in the step 4 is to combine the inherent angle and distance constraint relationship between the waist and the foot in the walking process, fuse the information of the waist and the foot to complement the advantages and the disadvantages, and realize the error estimation and inhibition of course angle and distance measurement through the inequality constraint Kalman filtering.
An angular inequality constraint between the waist and the foot is constructed. Based on that the waist and the feet of the pedestrian belong to the same human body, as shown in fig. 5, the motion directions of the waist and the feet of the pedestrian have inherent included angles in the walking process, spatial angle inequality constraints are constructed by presetting the threshold values of the included angles, and the spatial angle constraint conditions are as follows:
ψWaist-ψFoot≤Δψ (2)
phi in formula (2)FootIndicating the foot heading angle, psiWaistRepresents the lumbar heading angle and Δ ψ represents the foot angle preset threshold. Wherein psiFootThe following relationship is satisfied:
Figure BDA0002438119940000052
in the formula (3)
Figure BDA0002438119940000053
For directly resolving the resulting foot heading angle, delta psiFootAnd the error is the foot course angle error. And (3) driving the formula (2) to obtain inequality constraints of the foot course angle error:
Figure BDA0002438119940000054
a horizontal position inequality constraint between the waist and the foot is constructed. Based on the waist displacement being about 0.5 times the foot displacement, a positional constraint relationship between the waist and the foot is obtained:
pFoot(k-1)-pFoot(k)≤2·SL+γ (5)
p in formula (5)Foot(k-1)And pFoot(k)(k 2,3, ·, n) represent the position of the foot at the k-1 st and kth resting stage of the foot, respectively, which results from the calculation of the position of the foot in step 2. SL is the step size measurement of the waist in step 2, and γ represents a threshold for the difference between the step size from the lumbar measurement sensor and the step size from the foot measurement sensor.
Selecting the ENU coordinate system as the navigation coordinate system, equation (5) can be expressed as:
Figure BDA0002438119940000061
p in formula (6)(E)(k-1)，p(N)(k-1)Respectively represents the east position and the north position of the foot corresponding to the k-1 th resting stage of the foot, p(E)(k)，p(N)(k)Indicating the east and north positions of the foot, psi, respectively, for the kth resting stage of the footWaistRepresenting the waist heading angle. Wherein p is(E)(k)，p(N)(k)The following relationship is satisfied:
Figure BDA0002438119940000062
in the formula (7)
Figure BDA0002438119940000063
And
Figure BDA0002438119940000064
respectively, the east direction of the kth resting stage of the foot calculated by the PDR in step 2Position and north position, δ p(E)(k)And δ p(N)(k)Respectively, showing the east and north position errors for the kth resting stage of the foot. And (5) bringing the formula (7) into the formula (6) to obtain inequality constraint of the error of the horizontal position of the foot:
Figure BDA0002438119940000065
information of the waist and the feet is fused for complementation of advantages and disadvantages, course angle and distance measurement errors are further corrected and restrained based on an inequality constraint Kalman filtering method, inherent errors of traditional inertial navigation are restrained, and extra errors generated in the pedestrian physiological motion process are overcome. First, the
Figure BDA0002438119940000066
Respectively representing three-dimensional position error, three-dimensional speed error and three-dimensional attitude error of the kth static stage of the foot, and constructing state variables
Figure BDA0002438119940000067
And secondly, obtaining optimized system state variable inequality constraints by combining an angle and distance inequality constraint relation:
Figure BDA0002438119940000068
in the formula (9), x is the state estimation based on the unconstrained Kalman filtering, r1，r2And r3H is a matrix for extracting the state constraint quantity. And finally, calculating an optimal solution of the inequality constraint Kalman filtering to obtain an error estimation value from the constraint Kalman filtering so as to correct the distance between the waist course angle and the foot of the inertial network system.
More specifically, the method of the present invention can be implemented as follows:
1. the 2-5 MEMS inertial sensors are fixed on the waist and the feet of a human body respectively, and the upper computer receives the acquisition information of the MEMS inertial sensors in real time through Bluetooth.
The MEMS inertial measurement sensor selected in this embodiment is a high-precision inertial sensor module WT901BLE5.0C, which includes a three-axis MEMS accelerometer, a three-axis MEMS gyroscope, and a bluetooth wireless transmission module. In this embodiment, 2 WT901BLE5.0C modules are fixed to the waist and feet of a human body, respectively, in the manner shown in fig. 2. For a waist-mounted module, the coordinates are defined as follows: close to the spine direction is the z-axis, pointing to the left side of the body is the x-axis, and the y-axis points following the right hand rule. For a foot-mounted module, the coordinates are defined as follows: the direction along the tiptoe is a y-axis, the direction vertical to the tiptoe surface is a z-axis, and the direction of the x-axis follows the right-hand rule; the upper computer (a PC, a notebook computer, a tablet personal computer or a mobile phone) receives the acquired information of each MEMS inertial sensor in real time through Bluetooth, and has the functions of data preprocessing, navigation resolving, data storage and the like.
The method comprises the following specific steps:
and a, initially calibrating the WT901BLE5.0C module by using an upper computer.
And b, wearing the calibrated sensors on the waist and the feet of the human body respectively to form an inertial network measuring system together with the Bluetooth communication module.
And c, carrying out a field-shaped path walking experiment, and selecting a field-shaped track of 40m multiplied by 44m as a reference track, wherein the reference track is shown in figure 3. And in the walking process of the pedestrian, the upper computer receives and processes the acquired information in real time.
2. Based on the angle information and the speed information acquired by the waist and foot sensors in real time, the motion characteristics of the waist and the feet in the walking process of the pedestrian, including course angle, step length and position, are acquired respectively.
The method comprises the following specific steps:
and a, setting the cut-off frequency of a low-pass filter to be 4.2Hz, and performing low-pass filtering processing on the acceleration signal and the angular velocity signal acquired by the inertial network measurement system.
B, finding out the maximum value a of the acceleration in the z-axis direction (shown in figure 2) in each gait cycle from the low-pass filtered waist acceleration and angular velocity signalsverticalmaxAnd a minimum value averticalminTo, forCalculating the course corresponding to each step of the waist by the angular velocity signal between the maximum acceleration points of every two adjacent z-axis directions through a strapdown inertial navigation system calculation method
Figure BDA0002438119940000071
And simultaneously calculating the forward displacement of the waist in each gait cycle, namely the step length SL, wherein the step length calculation formula is as follows:
Figure BDA0002438119940000072
where K is a pre-calibrated parameter.
And c, calculating a threshold value of the zero-speed state based on the acceleration and angular velocity signals of the foot, and detecting the static stage of each gait cycle of the foot by using the threshold value, wherein the number of the static stages represents the step frequency of the foot.
Step d, respectively carrying out strapdown resolving on the acceleration signal and the angular velocity signal between every two adjacent foot zero-speed state points based on the filtered foot signals to calculate the step length d of each step of the footFootAnd corresponding course angle
Figure BDA0002438119940000081
Step e. preliminary calculation of pedestrian position from the foot. The initial position is (X)0,Y0) And obtaining the course angle and the step length of each step of the foot through the steps, and calculating the current pedestrian position by using the dead reckoning.
3. Based on the motion characteristic that the waist moves relatively stably in the walking process of the pedestrian, the course change of the pedestrian is obtained according to the information of the waist, meanwhile, the straight-going detection is carried out on the obtained course angle of the waist according to the motion characteristic that the pedestrian walks along a straight line within a few seconds, and the course of the straight-going motion state is corrected by using an averaging method.
The flow chart of straight-going detection and course correction is shown in fig. 4, and the specific steps are as follows:
step a, calculating the following observation information according to the waist course angle of n continuous gait cycles:
Figure BDA0002438119940000082
in the formula (2), Δ Ai(i ═ 1,2, ·, n) denotes the amount of change in lumbar heading angle between two adjacent gait cycles,
Figure BDA0002438119940000083
representing the lumbar heading angle for n consecutive gait cycles.
And b, obtaining a judgment condition of the straight-going state of the pedestrian according to the Generalized Likelihood Ratio (GLRT):
Figure BDA0002438119940000084
λ in the formula (3) is a straight-ahead determination threshold.
C, when the inequality relation in the formula (3) is satisfied, the pedestrian is in a straight-going state, and the corresponding waist course angle is corrected to be
Figure BDA0002438119940000085
i ═ 1,2, ·, n (in this specific implementation step, n ═ 4); when the inequality relation in the formula (3) is not satisfied, the pedestrian is in a non-straight-going state, and the corresponding waist course angle is
Figure BDA0002438119940000086
4. And further restraining and correcting the measurement errors of the course angle of the waist and the distance of the feet by using the inherent angle and distance constraint relation between the waist and the feet in the walking process and based on an inequality constraint Kalman filtering method.
The flow chart of correcting the course and the distance by the inequality constraint Kalman filtering is shown in FIG. 6, and the specific steps are as follows:
step a. constructing an angular inequality constraint between the waist and the foot. As shown in fig. 5, the direction of the foot and the walking direction of the pedestrian have a certain included angle (hereinafter referred to as foot angle), and the foot angle is basically kept stable during walking. The space angle constraint condition is psiWaist-ψFoot≤Δψ，ψFootIndicating the foot heading angle, psiWaistRepresents the lumbar heading angle and Δ ψ represents the foot angle preset threshold. Wherein psiFootSatisfy the requirement of
Figure BDA0002438119940000087
For the foot heading angle, δ ψ, calculated in step 2-dFootAnd the error is the foot course angle error. Therefore, the inequality constraint of the foot course angle error is obtained by combining the two formulas:
Figure BDA0002438119940000088
and b, constructing a horizontal position inequality constraint between the waist and the foot. The lumbar displacement is about 0.5 times the foot displacement in each gait cycle, thus obtaining a position constraint relationship between the foot and the lumbar:
pFoot(k-1)-pFoot(k)≤2·SL+γ (4)
p in formula (4)Foot(k-1)And pFoot(k)(k 2,3, ·, n) represent the position of the foot at the k-1 st and kth resting stage of the foot, respectively, which results from the calculation of the position of the foot in step 2-e. SL is the waist step size measurement in step 2-b and γ represents the threshold for the difference between the step size from the waist measurement sensor and the step size from the foot measurement sensor.
An ENU coordinate system is selected as a navigation coordinate system, the y-axis direction is the east direction, the x-axis direction is the north direction, and the z-axis direction is the sky direction, and formula (4) can be expressed as follows:
Figure BDA0002438119940000091
p in formula (5)(E)(k-1)，p(N)(k-1)Respectively represents the east position and the north position of the foot corresponding to the k-1 th resting stage of the foot, p(E)(k)，p(N)(k)Indicating the east and north positions of the foot, psi, respectively, for the kth resting stage of the footWaistRepresenting the waist heading angle. Wherein p is(E)(k)，p(N)(k)The following relationship is satisfied:
Figure BDA0002438119940000092
in the formula (6)
Figure BDA0002438119940000093
And
Figure BDA0002438119940000094
respectively representing the east and north positions, δ p, corresponding to the kth resting stage of the foot, obtained from the position of the foot in step 2-e(E)(k)And δ p(N)(k)Respectively, showing the east and north position errors for the kth resting stage of the foot. By taking equation (6) into equation (5), the inequality constraint of the error of the horizontal position of the foot can be obtained:
Figure BDA0002438119940000095
and c, restraining and correcting the measurement errors of the course angle of the waist and the distance of the feet based on an inequality constraint Kalman filtering algorithm. First, the
Figure BDA0002438119940000096
Respectively representing three-dimensional position error, three-dimensional speed error and three-dimensional attitude error of the kth static stage of the foot, and constructing state variables
Figure BDA0002438119940000097
Secondly, combining the inequality constraint relation in the step 4-a and the step 4-b to obtain system state variable inequality constraint:
Figure BDA0002438119940000101
in the formula (8), x is the state estimation based on the unconstrained Kalman filtering, r1，r2And r3H is a matrix for extracting the state constraint quantity. Then, calculating an optimal solution of inequality constraint Kalman filtering:
Figure BDA0002438119940000102
in the formula (9), the reaction mixture is,
Figure BDA0002438119940000103
and P is a covariance matrix in the unconstrained Kalman filtering resolving process. And finally, correcting the distance between the waist course angle and the feet of the inertial network system by the constraint Kalman filtering state estimation value.
5. And carrying out dead reckoning on the corrected acquired waist course angle and foot distance in real time to obtain the position information of the pedestrian.
The initial position is (X)0,Y0) The course angle psi and step length d of each step after correction are substituted into formula (10) to carry out dead reckoning, thus obtaining the current position (X)k,Yk) Wherein ψiAnd diRespectively representing the course angle and the step length of the ith step of the pedestrian, and finally outputting the obtained position of the pedestrian in real time through an upper computer.
Figure BDA0002438119940000104
In order to verify the application effect of the pedestrian positioning method based on the inertial network, five groups of experiments are carried out. Five experimental error statistics are shown in table 1:
TABLE 1 statistics of walking experimental results for field shaped path
Figure BDA0002438119940000105
Figure BDA0002438119940000111
In the existing inertial pedestrian positioning method, a zero-speed correction (ZUPT) pedestrian positioning method based on a foot inertial sensor has the problem of course error rapid accumulation due to the violent motion of feet, the course error is 37.8 degrees, and the distance error is 7.2 percent. As can be seen from the table 1, when each group of field-shaped path walking experiments are finished, the pedestrian positioning method based on the inertial network can fuse the waist and foot information of pedestrians, and has better distance precision and heading precision, the heading error is 6.3 degrees at most, and the distance error is 4.4 percent at most.
Although illustrative embodiments of the present invention have been described above to facilitate the understanding of the present invention by those skilled in the art, it should be understood that the present invention is not limited to the scope of the embodiments, but various changes may be apparent to those skilled in the art, and it is intended that all inventive concepts utilizing the inventive concepts set forth herein be protected without departing from the spirit and scope of the present invention as defined and limited by the appended claims.

Claims (5)

1. A pedestrian positioning method based on an inertial network is characterized by comprising the following steps:
step 1, respectively fixing 2-5 MEMS inertial sensors on the waist and the foot of a human body, and receiving the acquisition information of each MEMS inertial sensor by an upper computer through Bluetooth in real time;
step 2, respectively acquiring the motion characteristics of the waist and the feet in the walking process of the pedestrian, including course angles, step lengths and positions, based on angle information and speed information acquired by the waist and foot MEMS inertial sensors in real time;
step 3, acquiring the course change of the pedestrian according to the information of the waist based on the motion characteristic that the waist moves relatively stably in the walking process of the pedestrian, simultaneously carrying out straight-ahead detection on the acquired course angle of the waist according to the motion characteristic that the pedestrian walks along a straight line within a few seconds, and correcting the course of a straight-ahead motion state by using an averaging method;
step 4, further correcting and restraining a waist course angle and a foot distance measurement error based on an inequality constraint Kalman filtering method by utilizing an inherent angle constraint relation between the waist and the foot and a distance constraint relation between the waist and the foot in the walking process of the pedestrian;
and 5, carrying out dead reckoning on the basis of the acquired waist course angle and the acquired foot distance and outputting the position information of the pedestrian in real time.
2. The inertial network-based pedestrian positioning method of claim 1, characterized in that: in the step 1, the MEMS inertial sensors are respectively arranged on the waist and the feet of a human body and form an inertial network measuring system together with the Bluetooth communication module; the inertial sensors of the waist and the feet are respectively nodes of an inertial network measuring system, and information is acquired and shared in real time through Bluetooth.
3. The inertial network-based pedestrian positioning method of claim 1, wherein the step 2 of acquiring the motion characteristics of the waist and the feet comprises the steps of:
(2.1) according to the waist movement characteristics in the walking process of the pedestrian, selecting an acceleration signal of an MEMS sensor along the direction opposite to the gravity acceleration, based on the time interval between two adjacent acceleration maximum points as a gait cycle, in the gait cycle, resolving the waist course angle of the pedestrian walking based on an angular velocity signal of the MEMS sensor taking the gravity acceleration direction as an axis, and calculating the current waist step length by using a waist vertical acceleration step length model;
and (2.2) selecting the time interval between two adjacent zero-speed state points of the MEMS sensor as a gait cycle according to the motion characteristics of the foot in the walking process of the pedestrian, acquiring the current course angle and step length of the pedestrian in the gait cycle based on a strapdown inertial navigation system resolving method, and calculating the pedestrian position information from the foot by using the track based on the current course angle and step length information of the foot in the walking process of the pedestrian.
4. The inertial network-based pedestrian positioning method of claim 1, wherein the straight-ahead detection and correction of the waist heading angle in the step 3 comprises the following steps:
based on the fact that the foot moves violently and the waist moves stably relatively in the walking process of the pedestrian, the course obtained by a waist sensor in an inertial network measurement system is selected for the course of the walking process of the pedestrian, then the change amount of the course angle of the waist of 3-5 continuous adjacent gait cycles is observed by combining the movement characteristics of linear movement within a few seconds in the walking process of the pedestrian, and the straight-moving state of the pedestrian is judged according to the generalized likelihood ratio; when the pedestrian is in a straight-going state, correcting the heading of the state by using an averaging method, and correcting the heading of the pedestrian for the first time.
5. The pedestrian positioning method based on the inertial network as claimed in claim 1, wherein the step 4 of correcting and suppressing the error based on the inequality constraint kalman filtering method comprises the steps of:
(4.1) constructing space angle inequality constraint by presetting an included angle threshold value based on that the waist and the feet of the pedestrian belong to the same human body and the movement direction of the human body has an inherent included angle in the walking process of the pedestrian;
(4.2) then, constructing inequality constraints between the waist and the feet based on inherent correlation of waist displacement and foot displacement, waist course and foot course in each gait cycle of the pedestrian;
and (4.3) finally, information of the waist and the feet is fused for complementation of advantages and disadvantages, and course angle and distance measurement errors in a gait cycle are further corrected and restrained based on an inequality constraint Kalman filtering method.
CN202010257889.7A 2020-04-03 2020-04-03 Pedestrian positioning method based on inertial network Expired - Fee Related CN111307148B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010257889.7A CN111307148B (en) 2020-04-03 2020-04-03 Pedestrian positioning method based on inertial network

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010257889.7A CN111307148B (en) 2020-04-03 2020-04-03 Pedestrian positioning method based on inertial network

Publications (2)

Publication Number Publication Date
CN111307148A true CN111307148A (en) 2020-06-19
CN111307148B CN111307148B (en) 2021-09-03

Family

ID=71146202

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010257889.7A Expired - Fee Related CN111307148B (en) 2020-04-03 2020-04-03 Pedestrian positioning method based on inertial network

Country Status (1)

Country Link
CN (1) CN111307148B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114173412A (en) * 2021-11-30 2022-03-11 广州致远电子有限公司 Vehicle positioning method and device based on UWB vehicle-mounted key

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104729507A (en) * 2015-04-13 2015-06-24 大连理工大学 Gait recognition method based on inertial sensor
CN106643785A (en) * 2016-12-28 2017-05-10 北京航空航天大学 Method for multi-source information self-adaption step number detection based on MEMS inertial measurement unit
CN106705968A (en) * 2016-12-09 2017-05-24 北京工业大学 Indoor inertial navigation algorithm based on posture recognition and step length model
CN108180923A (en) * 2017-12-08 2018-06-19 北京理工大学 A kind of inertial navigation localization method based on human body odometer
US20190099134A1 (en) * 2015-08-18 2019-04-04 University Of Miami Method and system for adjusting audio signals based on motion deviation
CN109843371A (en) * 2016-08-17 2019-06-04 洛桑联邦理工学院 Equipment including the support system for user and its operation under gravity auxiliary mode
CN110274592A (en) * 2019-07-18 2019-09-24 北京航空航天大学 A kind of zero-speed section of waist foot Inertial Measurement Unit information fusion determines method
CN110553646A (en) * 2019-07-30 2019-12-10 南京林业大学 Pedestrian navigation method based on inertia, magnetic heading and zero-speed correction

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104729507A (en) * 2015-04-13 2015-06-24 大连理工大学 Gait recognition method based on inertial sensor
US20190099134A1 (en) * 2015-08-18 2019-04-04 University Of Miami Method and system for adjusting audio signals based on motion deviation
CN109843371A (en) * 2016-08-17 2019-06-04 洛桑联邦理工学院 Equipment including the support system for user and its operation under gravity auxiliary mode
CN106705968A (en) * 2016-12-09 2017-05-24 北京工业大学 Indoor inertial navigation algorithm based on posture recognition and step length model
CN106643785A (en) * 2016-12-28 2017-05-10 北京航空航天大学 Method for multi-source information self-adaption step number detection based on MEMS inertial measurement unit
CN108180923A (en) * 2017-12-08 2018-06-19 北京理工大学 A kind of inertial navigation localization method based on human body odometer
CN110274592A (en) * 2019-07-18 2019-09-24 北京航空航天大学 A kind of zero-speed section of waist foot Inertial Measurement Unit information fusion determines method
CN110553646A (en) * 2019-07-30 2019-12-10 南京林业大学 Pedestrian navigation method based on inertia, magnetic heading and zero-speed correction

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
CHUN ZHU 等: "Human Daily Activity Recognition in Robot-assisted Living Using Multi-sensor Fusion", 《2009 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION》 *
ZHUANGSHENG ZHU 等: "A Novel Step Length Estimator Based on Foot-Mounted MEMS Sensors", 《SENSORS》 *
时伟 等: "基于不等式约束卡尔曼滤波的双MIMU导航位置校正方法", 《中国惯性技术学报》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114173412A (en) * 2021-11-30 2022-03-11 广州致远电子有限公司 Vehicle positioning method and device based on UWB vehicle-mounted key
CN114173412B (en) * 2021-11-30 2023-05-12 广州致远电子股份有限公司 Vehicle positioning method and device based on UWB vehicle-mounted key

Also Published As

Publication number Publication date
CN111307148B (en) 2021-09-03

Similar Documents

Publication Publication Date Title
CN107655476B (en) Pedestrian high-precision foot navigation method based on multi-information fusion compensation
CN105043385B (en) A kind of method for adaptive kalman filtering of pedestrian's Camera calibration
Liu et al. Stereo visual-inertial odometry with multiple Kalman filters ensemble
CN102538781B (en) Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method
US10352959B2 (en) Method and system for estimating a path of a mobile element or body
CN103759730B (en) The collaborative navigation system of a kind of pedestrian based on navigation information two-way fusion and intelligent mobile carrier and air navigation aid thereof
CN109186597B (en) Positioning method of indoor wheeled robot based on double MEMS-IMU
CN109099913B (en) Wearable navigation device and method based on MEMS inertial device
CN112697138B (en) Bionic polarization synchronous positioning and composition method based on factor graph optimization
CN108180923B (en) Inertial navigation positioning method based on human body odometer
CN110672099A (en) Course correction method and system for indoor robot navigation
CN109959374B (en) Full-time and full-range reverse smooth filtering method for pedestrian inertial navigation
CN106123900A (en) Indoor pedestrian navigation magnetic heading calculation method based on modified model complementary filter
CN112254729A (en) Mobile robot positioning method based on multi-sensor fusion
CN109959375A (en) A kind of acoustics amendment localization method based on error triggering calibration
CN104316058B (en) Interacting multiple model adopted WSN-INS combined navigation method for mobile robot
CN104897158A (en) Indoor double-layered positioning method and indoor double-layered positioning system for walker
CN110986941B (en) Method for estimating installation angle of mobile phone
CN112362057A (en) Inertial pedestrian navigation algorithm based on zero-speed correction and attitude self-observation
CN111307148B (en) Pedestrian positioning method based on inertial network
CN112904884B (en) Method and device for tracking trajectory of foot type robot and readable storage medium
CN113008229A (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN117053782A (en) Combined navigation method for amphibious robot
CN109737957B (en) INS/LiDAR combined navigation method and system adopting cascade FIR filtering
CN115451946A (en) Indoor pedestrian positioning method combining MEMS-IMU and Wi-Fi

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
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20210903

CF01 Termination of patent right due to non-payment of annual fee