CN111197983B - Three-dimensional pose measurement method based on human body distribution inertia node vector distance measurement - Google Patents
Three-dimensional pose measurement method based on human body distribution inertia node vector distance measurement Download PDFInfo
- Publication number
- CN111197983B CN111197983B CN202010041983.9A CN202010041983A CN111197983B CN 111197983 B CN111197983 B CN 111197983B CN 202010041983 A CN202010041983 A CN 202010041983A CN 111197983 B CN111197983 B CN 111197983B
- Authority
- CN
- China
- Prior art keywords
- information
- node
- nodes
- vector
- attitude angle
- 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
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Measurement Of The Respiration, Hearing Ability, Form, And Blood Characteristics Of Living Organisms (AREA)
Abstract
The invention relates to a three-dimensional pose measurement method based on human body distribution inertial node vector distance measurement. According to the method, distance measurement modules and inertial nodes are installed on ankle joints of left and right legs of a pedestrian, and a three-axis accelerometer, an angular velocity meter and a magnetometer are integrated in each inertial node. Measuring real-time distance information between ankle joints of the pedestrian by using a distance measuring module; and calculating attitude angle information at the ankle joint nodes by using the inertial nodes. And obtaining vector information in each time segment by combining the acceleration information and the attitude angle information in the three axial directions in the motion process with the previous ranging information. The position and attitude information of the pedestrian in the three-dimensional space domain can be tracked in real time through the vector information and the attitude angle information, and the position and attitude information is called pose information for short. The method has the characteristics of intuitive and reliable parameters, autonomous measurement, no use of estimator and no constraint of characteristic parameters.
Description
Technical Field
The invention belongs to the field of inertial navigation and positioning of pedestrians, and particularly relates to a three-dimensional pose measurement method based on human body distribution inertial node vector distance measurement.
Background
Gait analysis and step calculation are key points in the field of inertial navigation positioning. By analyzing the gait information of the pedestrian, the physiological information, the movement behavior and the health condition of the human body can be estimated. By improving the step length calculation precision, the final navigation positioning precision can be improved.
Currently, researchers have made extensive research into motion estimation systems. The method mainly comprises three categories, one category is based on an optical method, for example, a Vicon system is utilized to collect human body data, and the collected information can be applied to sports analysis and biomedical research, but the method has the defects that the equipment is expensive and a sensor needs to be arranged in advance; one is to extract parameter information of human body movement by using a visual tracking platform through a corresponding computer technology and an image processing technology, further analyze gait information and calculate stride length, for example, a Hangzhou electronic technology university Zhang Song et al [ Zhang Song ] human body action evaluation method [ D ] based on a depth camera, a Hangzhou electronic technology university, 2018 ] collects a depth image through a Kinect depth camera, and a Kalman-Meanshift tracking method is used for realizing a good human body target tracking effect of the depth image, but the method has the problems of complex algorithm and incapability of being suitable for a real-time operating system; one type is to sense and acquire motion parameters of a human body by means of wearable equipment, such as a common inertial device, detect gait information of a pedestrian through parameter extraction and analysis, and estimate a step size of the pedestrian by using an estimation quantity, and a common estimation method includes a linear step size calculation model for calculating the step size based on step frequency and a nonlinear step size calculation model for calculating the step size based on acceleration amplitude change, and people such as yanshan university Zhang Xiongjie [ documents: zhang Xiongjie human motion characteristics research based on Xsens MVN inertial motion capture system equipment [ D ] yanshan university, 2016 ] using wearable inertial sensor devices to obtain human motion parameters and further study human motion characteristics, south chang university Xiong Jian et al [ documents: xiong Jian, xu Jiangying, yang Zuhua, et al, a pedestrian navigation method based on human body motion mode monitoring is that a biaxial angular velocity sensor is installed on a hip joint, a knee joint and an ankle joint of a pedestrian, angle information of legs of the pedestrian in a motion process is measured in real time, and motion information, position information and pedestrian step length information of each joint are finally obtained by combining length information of legs of the pedestrian.
The invention aims at the defects that the motion estimation system based on optics needs to arrange a sensor in advance, the motion estimation system based on vision has high algorithm complexity, and the motion estimation system based on an inertial device has poor robustness and low precision. The method uses a sensor carried by a pedestrian to complete accurate step length calculation without arranging the sensor in advance, has small calculation amount of an algorithm, can be applied to a real-time operating system, has strong robustness, does not need to change model parameters according to the difference of testers, and has strong practicability. Compared with the traditional motion estimation system, the real-time three-dimensional pose measurement without estimation parameters and accumulated errors can be realized through the dynamic combination of the distance measurement module and the inertia nodes, and the tracking and reproduction of the whole process of the single-step motion of the pedestrian can be realized.
Disclosure of Invention
The present invention is directed to solving the above problems of the prior art. A three-dimensional pose measurement method based on human body distribution inertia node vector distance measurement is provided. The technical scheme of the invention is as follows:
a three-dimensional pose measurement method based on human body distributed inertial node vector distance measurement comprises the following steps:
step 1, installing a distance measurement module and an inertia node at ankle joints of left and right legs of a pedestrian, wherein the inertia node acquires acceleration information, angular velocity information and magnetic field intensity information of the installation node in real time, and the distance measurement module acquires distance information between the two nodes in real time;
step 2, in an initial stage, acquiring reference attitude angle information by using the acceleration information and the magnetic field intensity information acquired by the inertial node;
step 3, estimating the motion states of the two nodes by using the joint acceleration information at the ankle joint nodes, and distinguishing the motion states from the static states;
step 4, reducing errors of the nodes in the static state through zero-speed correction, updating the attitude angle information of the nodes in the motion state in real time by using a gyroscope to obtain the current attitude information of the nodes, and obtaining the result of the change of the attitude angle by combining the reference attitude angle information;
step 5, obtaining angle information of the vector by using the change result of the attitude angle and acceleration information in three axial directions sensed by the accelerometer;
step 6, acquiring real-time position information of the nodes according to the distance information between the two nodes and the angle information of the vector provided by the ranging module;
and 7, integrating the motion information of the single step of the pedestrian to realize the function of positioning the pedestrian.
Furthermore, in step 1, the short-distance high-precision distance measurement function of the distance measurement module is realized by adjusting the power of the distance measurement module, selecting a proper directional antenna and a proper feeder line, and the distance information between the ankle joint nodes can be measured in real time. And acquiring motion information at the node in real time through the inertial node.
Further, in the step 2, in an initial stage, reference attitude angle information is obtained by using acceleration information and magnetic field intensity information acquired by the inertial node; at the initial stage, the left foot and the right foot of the human body are both in the support stage, and at this time, the ankle joint node is in a static state, so that the attitude angle information in the state can be calculated through the accelerometer and the magnetometer and is used as the initial attitude angle of the ankle joint node, namely, the reference attitude angle, as shown in formula (1):
wherein, theta 1 、γ 1 、ψ 1 Respectively representing pitch angle, roll angle, course angle, A x 、A y 、A z Respectively representing three-axis acceleration information, m x 、m y 、m z Representing intensity information for a three axis magnetometer.
Further, in step 3, estimating the motion states of the two nodes by using the resultant acceleration information at the ankle joint node, and distinguishing the motion state from the stationary state specifically includes: judging the motion state of the foot through a threshold condition, wherein the judgment method is shown as the formula (2):
wherein A is th Representing a threshold for distinguishing between moving and stationary states, A norm Representing the resultant acceleration, when A norm Greater than a threshold value A th When the node is in the motion state, the representative node is in the motion state; when resultant acceleration A norm Less than threshold A th And the representative node is in a static state.
Further, in step 4, when the ankle joint node is in a static state, data fusion is performed through a kalman filtering technique, an error of the system is estimated, and the system parameter is corrected by using the estimated value of the error, as shown in formula (3):
f is a system matrix formed by the error model and the state quantity, W is a system random process noise sequence, and V is a system observation noise sequence;
when the ankle joint node is in a motion state, the attitude angle information of the node is updated through the gyroscope, as shown in formula (4):
wherein q is 0 、q 1 、q 2 、q 3 Is quaternion information, θ 2 、γ 2 、ψ 2 The method comprises the steps of obtaining real-time attitude angle information, namely attitude information of a node, by updating quaternion through a gyroscope, and obtaining transformation information of the attitude angle by subtracting the real-time attitude angle information from reference attitude angle information, wherein the formula (5) is as follows:
further, in the step 5, obtaining angle information of the vector by using a result of the change of the attitude angle and acceleration information in three axial directions sensed by the accelerometer specifically includes: in the actual movement process, the triaxial accelerometer can sense the component forces of the specific force in three directions of the carrier coordinate system, the angle information of the vector under the carrier coordinate system can be obtained through the proportional relation of the component forces, the final angle information of the vector relative to the initial attitude angle is obtained through calculation by combining the change information of the attitude angle in the step 4 and the angle information of the vector under the carrier coordinate system, and the direction angle information can be obtained through the vector relation in the sagittal plane of a pedestrian, as shown in the formula (6) and the formula (7):
θy＝arccos(Ay/Anorm) (6)
θsum＝θy+θ (7)
wherein, theta y Is the y-axis acceleration A of the carrier coordinate system y And resultant acceleration A norm Theta is information of the change of the current attitude angle from the initial attitude angle, theta sum The angle change information of the vector subjected to the attitude angle change compensation in the pedestrian sagittal plane is obtained.
Further, in step 6, obtaining the real-time location information of the node according to the distance information between the two nodes and the angle information of the vector provided by the ranging module specifically includes: in step 5, angle information of the vector in each time slice can be obtained, all information of the vector can be obtained by combining distance information between two nodes provided by the ranging module, and finally, real-time position information of the nodes can be obtained through a functional relationship, as shown in formula (8):
in the motion process, the process that a motion node swings relative to a static node from back to front to complete a stride can be divided into three stages, the first stage is that the motion node is behind the static node, the second stage is that the motion node swings from the back of the static node to the front of the static node, the third stage is that the motion node is in front of the static node, and the three stages can be distinguished through the angle information of the vector.
Further, in step 7, the motion information of the single step of the pedestrian is integrated to realize the function of locating the person, and the method specifically includes: step length information of the single step and attitude information in the single step can be accurately calculated through the steps 1 to 6, motion information of each single step is integrated, and the positioning function of personnel in a three-dimensional space is realized, wherein the calculation formula is as follows:
wherein x is step Is the horizontal displacement distance, y, of a single stride step Is the vertical displacement distance of a single step, l is the step information of a single step, ψ is the heading information provided by the node, x sum Is relative to the initial seat under the last measurement conditionTarget horizontal displacement distance, y sum Is the vertical displacement distance relative to the initial coordinate under the last measurement condition, x is the horizontal displacement distance relative to the initial coordinate under the current measurement condition, and y is the vertical displacement distance relative to the initial coordinate under the current measurement condition.
The invention has the following advantages and beneficial effects:
the invention monitors the real-time pose information of the pedestrian in the three-dimensional space through the vector information and the pose information. The main innovation points of the invention are the step (5) and the step (6), and the invention has the following benefits:
(1) The autonomy is good: the method only depends on the sensor carried by the pedestrian to complete accurate step length calculation and attitude estimation, and does not need to use an external sensor.
(2) The motion information is rich: the method can realize the tracking and reproduction of the whole process of the single-step movement of the pedestrian, can provide the stepping information and the step height information of each position in the single-step movement process, and provides the attitude information of the position.
(3) The precision is high: the method can complete real-time monitoring of the position information of the pedestrian in the three-dimensional space, the calculation parameters are visual and measurable, and improvement is carried out aiming at the defects of low precision and error accumulation caused by the use of the estimator in step calculation in the pedestrian track calculation algorithm.
(4) The real-time property is as follows: the method has the advantages of small calculated amount, visual and measurable parameters and suitability for a real-time operating system.
(5) The practicability is strong: the method does not need to change the parameters of the calculation model according to the difference of the testers, and has strong robustness.
Drawings
FIG. 1 is a vector relationship representation of a preferred embodiment of the present invention in a carrier coordinate system
FIG. 2 is a representation of the post-attitude angle compensation vector relationship
FIG. 3 is a schematic exploded view of the gait cycle
FIG. 4 is a schematic diagram of step and step height calculation
FIG. 5 is a schematic diagram of a moving node behind a stationary node
FIG. 6 is a schematic diagram of a moving node crossing from back to front of a stationary node
FIG. 7 is a schematic diagram of a moving node in front of a stationary node
FIG. 8 is an algorithm overall framework flow diagram
Detailed Description
The technical solutions in the embodiments of the present invention will be described in detail and clearly with reference to the accompanying drawings. The described embodiments are only some of the embodiments of the present invention.
The technical scheme for solving the technical problems is as follows:
as shown in fig. 8, a three-dimensional pose measurement method based on human body distribution inertial node vector distance measurement. According to the method, distance measurement modules and inertial nodes are installed on ankle joints of left and right legs of a pedestrian, and a three-axis accelerometer, an angular velocity meter and a magnetometer are integrated in each inertial node. Measuring real-time distance information between ankle joints of the pedestrian by using a distance measuring module; and calculating attitude angle information at the ankle joint nodes by using the inertial nodes. And obtaining vector information in each time segment by combining the acceleration information and the attitude angle information in the three axial directions in the motion process with the previous distance measurement information. The position and attitude information of the pedestrian in the three-dimensional space domain can be tracked in real time through the vector information and the attitude angle information, and the position and attitude information is called pose information for short. The method has the characteristics of intuitive and reliable parameters, autonomous measurement, no use of estimator and no constraint of characteristic parameters.
The method comprises the following steps: (1) The method comprises the following steps that a distance measurement module and an inertia node are installed at ankle joints of left and right legs of a pedestrian, wherein the inertia node collects acceleration information, angular velocity information and magnetic field intensity information of the installation node in real time, and the distance measurement module collects distance information between the two nodes in real time; (2) In the initial stage, acquiring reference attitude angle information by using acceleration information and magnetic field intensity information acquired by an inertial node; (3) Estimating the motion states of the two nodes by utilizing the combined acceleration information at the ankle joint nodes, and distinguishing the motion states from the static states; (4) Reducing errors of the nodes in the static state through zero-speed correction, improving the system precision, updating the attitude angle information of the nodes in the motion state in real time by using a gyroscope to obtain the current attitude information of the nodes, and obtaining the result of the change of the attitude angle by combining with the reference attitude angle information; (5) Obtaining angle information of a vector by using a change result of the attitude angle and acceleration information in three axial directions sensed by the accelerometer; (6) The real-time position information of the nodes can be obtained according to the distance information between the two nodes and the angle information of the vector provided by the ranging module; (7) And the movement information of the single step of the pedestrian is integrated to realize the function of positioning the pedestrian. The invention can complete the real-time monitoring of the position information of the pedestrian in the three-dimensional space, can realize the whole-process tracking and reproduction of the single-step movement of the pedestrian, and complete the accurate step length calculation by using the sensor which can be carried by the pedestrian, does not use the estimator, does not have the position calculation and the attitude measurement of the accumulated error, does not need to help other data information, and has strong applicability.
A three-dimensional pose measurement method based on human body distributed inertial node vector distance measurement comprises the following steps:
step (1), installing a distance measurement module and an inertia node at ankle joints of left and right legs of a pedestrian, wherein the inertia node acquires acceleration information, angular velocity information and magnetic field intensity information of the installation node in real time, and the distance measurement module acquires distance information between the two nodes in real time;
step (2), in the initial stage, acquiring reference attitude angle information by using the acceleration information and the magnetic field intensity information acquired by the inertial node;
estimating the motion states of the two nodes by using the combined acceleration information at the ankle joint nodes, and distinguishing the motion states from the static states;
step (4), reducing errors of the nodes in the static state through zero-speed correction, improving the system precision, updating the attitude angle information of the nodes in the motion state in real time by using a gyroscope to obtain the current attitude information of the nodes, and obtaining the result of the change of the attitude angle by combining with the reference attitude angle information;
step (5), obtaining angle information of a vector by using a change result of the attitude angle and acceleration information in three axial directions sensed by the accelerometer;
step (6), the real-time position information of the nodes can be obtained according to the distance information between the two nodes and the angle information of the vector provided by the ranging module;
and (7) synthesizing the motion information of the single step of the pedestrian to realize the function of positioning the pedestrian.
In the step (1), a distance measuring module and an inertia node are installed at the ankle joints of the left leg and the right leg of the pedestrian, wherein the inertia node collects acceleration information, angular velocity information and magnetic field intensity information of the installation node in real time, and the distance measuring module collects distance information between the two nodes in real time. The inertial node is used for providing motion information of the node, the short-distance high-precision distance measurement of the distance measurement module is realized by adopting the modes of reducing the gain of the distance measurement module, reducing the sampling frequency and the like, and the distance information between the ankle joint nodes can be accurately measured.
In the step (2), in the initial stage, reference attitude angle information is obtained by using the acceleration information and the magnetic field intensity information acquired by the inertial node; at the initial stage of program operation, the left foot and the right foot of the human body are both in the support stage, and at the moment, the ankle joint node is in a static state. Therefore, the attitude angle information in this state can be calculated by the accelerometer and the magnetometer as the initial attitude angle (reference attitude angle) of the ankle joint node.
As shown in formula (1):
wherein, theta 1 、γ 1 、ψ 1 Respectively representing pitch angle, roll angle, course angle, A x 、A y 、A z Respectively representing three-axis acceleration information, m x 、m y 、m z Representing intensity information for a three axis magnetometer.
In the step (3), the motion states of the two nodes are estimated by using the resultant acceleration information at the ankle joint nodes, and the motion states and the static states are distinguished. When a person walks, when one foot is in a supporting state, the other foot is in a swinging state, and the states of the two feet show periodic alternate transformation. Therefore, the motion state of the node is changed accordingly. In the process of estimating the position information of the moving node through the vector, the stationary node is required to be used as a reference point, and therefore, the moving node and the stationary node are required to be distinguished in the calculation process. When the node is in a static state, the resultant acceleration (compensated by gravity) changes around a value of 0; when the node is in a motion state, the resultant acceleration is greatly changed. By the threshold condition, the motion state of the foot can be judged. The judgment method is shown as the formula (2):
wherein, A th Representing a threshold for distinguishing between moving and stationary states, A norm Representing the resultant acceleration. When resultant acceleration A norm Greater than a threshold value A th When the node is in the motion state, the representative node is in the motion state; when resultant acceleration A norm When A is less than threshold value th And the representative node is in a static state.
In the step (4), the error of the node in the static state is reduced through zero-speed correction, the system precision is improved, the attitude angle information of the node in the motion state is updated in real time through the gyroscope, the current attitude information of the node is obtained, and the change result of the attitude angle is obtained by combining the reference attitude angle information. The inertia device has the defects of high short-time precision and low long-time precision. Therefore, a proper error correction model needs to be established, and the accuracy of the system is further improved. And when the ankle joint node is in a static state, performing data fusion through a Kalman filtering technology, estimating the error of the system and correcting the system parameters by using the estimated value of the error. As shown in formula (3):
wherein, F is a system matrix formed by the error model and the state quantity, W is a system random process noise sequence, and V is a system observation noise sequence.
When the ankle joint node is in a motion state, the attitude angle information of the node is updated through the gyroscope, as shown in formula (4):
wherein q is 0 、q 1 、q 2 、q 3 Is quaternion information, θ 2 、γ 2 、ψ 2 The quaternion is updated through the gyroscope, and real-time attitude angle information, namely the attitude information of the nodes can be obtained. And subtracting the real-time attitude angle information from the reference attitude angle information to obtain the transformation information of the attitude angle. As shown in formula (5):
in the step (5), the angle information of the vector is obtained by using the result of the attitude angle change and the acceleration information in the three axial directions sensed by the accelerometer. In the actual movement process, the triaxial accelerometer can sense the component forces of the specific force in three directions of the carrier coordinate system, and the angle information of the vector in the carrier coordinate system can be obtained through the proportional relation of the component forces, wherein the vector relation in the carrier coordinate system is shown as the attached drawing 1. And (4) combining the change information of the attitude angle in the step (4) and the angle information of the vector in the carrier coordinate system, and calculating to obtain final angle information of the vector relative to the initial attitude angle. In the sagittal plane of the pedestrian, the direction angle information can be obtained through vector relation, and the geometric representation is shown in figure 2. The calculation formula is shown in formula (6) and formula (7):
θy＝arccos(Ay/Anorm) (6)
θsum＝θy+θ (7)
wherein, theta y Is the y-axis acceleration A of the carrier coordinate system y And resultant acceleration A norm Theta is information of the change of the current attitude angle from the initial attitude angle, theta sum Is the angle of the vector on the sagittal plane of the pedestrian after the attitude angle change compensationDegree change information.
In the step (6), the real-time position information of the node can be obtained according to the distance information between the two nodes and the angle information of the vector provided by the ranging module. The program is executed at a frequency of 200Hz and the human walking frequency is typically about 3-5Hz. The whole walking process of the pedestrian can be divided into a plurality of time segments, and the motion of the nodes in each time segment can be regarded as vector motion. Therefore, the whole walking process can be regarded as the superposition of a plurality of vectors, and the schematic diagram of decomposing the gait cycle to obtain the plurality of vectors is shown in fig. 3. In step (5), angle information of the vector in each time slice can be obtained, and all information of the vector can be obtained by combining the distance information between two nodes provided by the ranging module. Finally, the real-time position information of the node can be obtained through the functional relationship, as shown in fig. 4. As shown in formula (8):
wherein, L is the distance information between two nodes measured by the ranging module, namely the vector length information, alpha is the angle information of the vector in the sagittal plane, namely the angle information of the vector, y is the step distance information of the sagittal plane, and h is the step height information of the sagittal plane. In the whole process, the angle information of the vector, namely alpha, needs to be updated continuously. In the process of movement, the movement node can swing from back to front relative to the static node (the process of completing one stride) into three stages, wherein the first stage is that the movement node is behind the static node, as shown in fig. 5; the second stage is that the motion node swings from the back of the stationary node to the front of the stationary node, as shown in fig. 6; the third stage is that the moving node is in front of the stationary node, as shown in fig. 7. The three stages can be distinguished through the angle information of the vector, and the angle information of the three stages is slightly updated differently. The calculation formula is as follows:
in the first stage:
wherein L is 1 Length information of vectors measured for the previous time, L 2 For length information of the currently measured vector, L 3 For length information of the next measured vector, θ 1 Is the angle information provided by the last measurement of the inertial node, θ 2 Angle information provided for the current measurement of the inertial node, wherein alpha is the included angle between the previous measurement and the current measurement vector, and alpha is 1 Beta and gamma are angle information used in the calculation process, and the vector angle can be updated through the parameters. After the calculation process is finished, the intermediate quantity needs to be updated, so that the step information and the step height information in each time segment in the first stage are obtained.
And a second stage:
wherein L is 1 Length information of vectors measured for the previous time, L 2 For length information of the currently measured vector, θ 1 Is the angle information provided by the current measurement inertia node, alpha is the updated angle information (included angle with the horizontal axis) of the previous vector, alpha 1 For the angle between the vector measured at the previous time and the vector measured at the current time, beta and psi are angle information used in the calculation process, and the vector angle can be updated through the parameters. After the calculation process is finished, the intermediate quantity needs to be updated, so that the step information and the step height information in each time segment in the second stage are obtained.
And a third stage:
wherein L is 1 Length information of vectors measured for the previous time, L 2 For length information of the currently measured vector, θ 1 Is the current measurement inertial nodeThe provided angle information, alpha, is the updated angle information (included angle with the horizontal axis) of the previous vector, alpha 1 For the angle between the vector measured at the previous time and the vector measured at the current time, beta and psi are angle information used in the calculation process, and the vector angle can be updated through the parameters. After the calculation process is finished, the intermediate quantity needs to be updated, so that the stepping information and the step height information in each time segment in the third stage are obtained.
In the step (7), the movement information of the single step of the pedestrian is integrated to realize the function of positioning the pedestrian. Step information of the single step and attitude information in the single step can be accurately calculated through the steps (1) to (6). And the function of positioning personnel in a three-dimensional space can be realized by integrating the motion information of each single step. The calculation formula is as follows:
wherein x is step Is the horizontal displacement distance, y, of a single stride step Is the vertical displacement distance of a single step, l is the step information of a single step, ψ is the heading information provided by the node, x sum Is the horizontal displacement distance, y, from the initial coordinate under the last measurement condition sum Is the vertical displacement distance relative to the initial coordinate under the last measurement condition, x is the horizontal displacement distance relative to the initial coordinate under the current measurement condition, and y is the vertical displacement distance relative to the initial coordinate under the current measurement condition.
The overall framework flow chart of the algorithm is shown in fig. 8.
The above examples are to be construed as merely illustrative and not limitative of the remainder of the disclosure in any way whatsoever. After reading the description of the invention, the skilled person can make various changes or modifications to the invention, and these equivalent changes and modifications also fall into the scope of the invention defined by the claims.
Claims (6)
1. A three-dimensional pose measurement method based on human body distributed inertial node vector distance measurement is characterized by comprising the following steps:
step 1, installing a distance measurement module and an inertia node at ankle joints of left and right legs of a pedestrian, wherein the inertia node acquires acceleration information, angular velocity information and magnetic field intensity information of the installation node in real time, and the distance measurement module acquires distance information between the two nodes in real time;
step 2, in the initial stage, acquiring reference attitude angle information by using the acceleration information and the magnetic field intensity information acquired by the inertial node;
step 3, estimating the motion states of the two nodes by using the combined acceleration information at the ankle joint nodes, and distinguishing the motion states from the static states;
step 4, reducing errors of the nodes in the static state through zero-speed correction, updating the attitude angle information of the nodes in the motion state in real time by using a gyroscope to obtain the current attitude information of the nodes, and obtaining the result of the change of the attitude angle by combining the reference attitude angle information;
step 5, obtaining angle information of the vector by using the change result of the attitude angle and acceleration information in three axial directions sensed by the accelerometer;
step 6, acquiring real-time position information of the nodes according to the distance information between the two nodes and the angle information of the vector provided by the ranging module;
step 7, synthesizing the motion information of the single step of the pedestrian to realize the function of positioning the pedestrian;
in the step 4, when the ankle joint node is in a static state, data fusion is performed through a kalman filtering technology, the error of the system is estimated, and the system parameter is corrected by using the estimated value of the error, as shown in formula (3):
f is a system matrix formed by the error model and the state quantity, W is a system random process noise sequence, and V is a system observation noise sequence;
when the ankle joint node is in a motion state, the attitude angle information of the node is updated through the gyroscope, as shown in formula (4):
wherein q is 0 、q 1 、q 2 、q 3 Is quaternion information, θ 2 、γ 2 、ψ 2 The method comprises the steps of obtaining real-time attitude angle information, namely attitude information of a node, by updating quaternion through a gyroscope, and obtaining transformation information of the attitude angle by subtracting the real-time attitude angle information from reference attitude angle information, wherein the formula (5) is as follows:
in the step 5, the angle information of the vector is obtained by using the result of the change of the attitude angle and the acceleration information in three axial directions sensed by the accelerometer, and the method specifically includes: in the actual movement process, the triaxial accelerometer can sense the component forces of the specific force in three directions of the carrier coordinate system, the angle information of the vector under the carrier coordinate system can be obtained through the proportional relation of the component forces, the final angle information of the vector relative to the initial attitude angle is obtained through calculation by combining the change information of the attitude angle in the step 4 and the angle information of the vector under the carrier coordinate system, and the direction angle information can be obtained through the vector relation in the sagittal plane of a pedestrian, wherein the direction angle information can be shown in the formula (6),
Formula (7):
θ y ＝arccos(A y /A norm ) (6)
θ sum ＝θ y +θ (7)
wherein, theta y Is a carrier seatY-axis acceleration A of the system y And resultant acceleration A norm Theta is the change information of the current attitude angle compared to the initial attitude angle, theta sum The angle change information of the vector subjected to the attitude angle change compensation in the pedestrian sagittal plane is obtained.
2. The three-dimensional pose measurement method based on human body distributed inertial node vector distance measurement according to claim 1, wherein in step 1, a distance measurement module and an inertial node are installed at ankle joints of left and right legs of a pedestrian, wherein the inertial node collects acceleration information, angular velocity information and magnetic field strength information of the installed nodes in real time, and the distance measurement module collects distance information between two nodes in real time, and specifically comprises: the short-distance high-precision distance measurement function of the distance measurement module is realized by adjusting the power of the distance measurement module and selecting a proper directional antenna and a proper feeder line, the distance information between ankle joint nodes can be measured in real time, and the motion information of the nodes is collected in real time through the inertial nodes.
3. The three-dimensional pose measurement method based on human body distributed inertia node vector distance measurement of claim 1, wherein in the step 2, in an initial stage, reference attitude angle information is obtained by using acceleration information and magnetic field strength information acquired by inertia nodes; at the initial stage, the left foot and the right foot of the human body are both in the support stage, and at this time, the ankle joint node is in a static state, so that the attitude angle information in the state can be calculated through the accelerometer and the magnetometer and is used as the initial attitude angle of the ankle joint node, namely, the reference attitude angle, as shown in formula (1):
wherein, theta 1 、γ 1 、ψ 1 Respectively representing pitch angle, roll angle, course angle, A norm Represents the resultant acceleration, A x 、A y 、A z Respectively representing three-axis acceleration information, m x 、m y 、m z Representing intensity information for a three axis magnetometer.
4. The three-dimensional pose measurement method based on human body distributed inertia node vector distance measurement according to claim 3, wherein in the step 3, the motion states of the two nodes are estimated by using the combined acceleration information at the ankle joint nodes, and the motion state is distinguished from the static state, specifically comprising: judging the motion state of the foot through a threshold condition, wherein the judgment method is shown as the formula (2):
wherein A is th Representing a threshold for distinguishing between moving and stationary states, A norm Representing the resultant acceleration, a norm Greater than a threshold value A th When the node is in the motion state, the representative node is in the motion state; when resultant acceleration A norm Less than threshold A th And the representative node is in a static state.
5. The three-dimensional pose measurement method based on human body distributed inertia node vector distance measurement according to claim 4, wherein in the step 6, the real-time position information of the nodes is obtained according to the distance information between two nodes and the angle information of the vector provided by the distance measurement module, and the method specifically comprises the following steps: in step 5, angle information of the vector in each time slice can be obtained, all information of the vector can be obtained by combining distance information between two nodes provided by the ranging module, and finally, real-time position information of the nodes can be obtained through a functional relationship, as shown in formula (8):
in the motion process, the process that a motion node swings relative to a static node from back to front to complete a stride can be divided into three stages, the first stage is that the motion node is behind the static node, the second stage is that the motion node swings from the back of the static node to the front of the static node, the third stage is that the motion node is in front of the static node, and the three stages can be distinguished through the angle information of the vector.
6. The three-dimensional pose measurement method based on human body distributed inertia node vector distance measurement according to claim 5, wherein in the step 7, the motion information of a single step of a pedestrian is integrated to realize a personnel positioning function, and the method specifically comprises the following steps: step length information of the single step and attitude information in the single step can be accurately calculated through the steps 1 to 6, motion information of each single step is integrated, and the positioning function of personnel in a three-dimensional space is realized, wherein the calculation formula is as follows:
wherein x is step Is the horizontal displacement distance, y, of a single stride step Is the vertical displacement distance of a single step, l is the step information of a single step, ψ is the heading information provided by the node, x sum Is the horizontal displacement distance, y, from the initial coordinate under the last measurement condition sum Is the vertical displacement distance relative to the initial coordinate under the last measurement condition, x is the horizontal displacement distance relative to the initial coordinate under the current measurement condition, and y is the vertical displacement distance relative to the initial coordinate under the current measurement condition.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010041983.9A CN111197983B (en) | 2020-01-15 | 2020-01-15 | Three-dimensional pose measurement method based on human body distribution inertia node vector distance measurement |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010041983.9A CN111197983B (en) | 2020-01-15 | 2020-01-15 | Three-dimensional pose measurement method based on human body distribution inertia node vector distance measurement |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111197983A CN111197983A (en) | 2020-05-26 |
| CN111197983B true CN111197983B (en) | 2022-12-27 |
Family
ID=70746499
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010041983.9A Active CN111197983B (en) | 2020-01-15 | 2020-01-15 | Three-dimensional pose measurement method based on human body distribution inertia node vector distance measurement |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111197983B (en) |
Citations (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2004125731A (en) * | 2002-10-07 | 2004-04-22 | Nec Tokin Corp | Attitude angle detector |
| CN103076619A (en) * | 2012-12-27 | 2013-05-01 | 山东大学 | System and method for performing indoor and outdoor 3D (Three-Dimensional) seamless positioning and gesture measuring on fire man |
| CN103776446A (en) * | 2013-10-29 | 2014-05-07 | 哈尔滨工程大学 | Pedestrian autonomous navigation calculation algorithm based on MEMS-IMU |
| CN103916875A (en) * | 2014-04-24 | 2014-07-09 | 山东大学 | Management and planning system of multi-class control terminals based on WIFI wireless network |
| CN104061934A (en) * | 2014-06-10 | 2014-09-24 | 哈尔滨工业大学 | Pedestrian indoor position tracking method based on inertial sensor |
| CN104215238A (en) * | 2014-08-21 | 2014-12-17 | 北京空间飞行器总体设计部 | Indoor positioning method of intelligent mobile phone |
| CN104897155A (en) * | 2015-06-05 | 2015-09-09 | 北京信息科技大学 | Personal portable auxiliary multisource locating information correcting method |
| CN105651281A (en) * | 2016-03-03 | 2016-06-08 | 南京诺导电子科技有限公司 | Space positioning algorithm on basis of multi-direction human movement modes |
| CN106352875A (en) * | 2016-09-21 | 2017-01-25 | 成都创慧科达科技有限公司 | Dead-reckoning-based navigation system and method |
| CN106500691A (en) * | 2016-10-21 | 2017-03-15 | 上海国际汽车城（集团）有限公司 | Vehicle real-time Passive Location and Tracking method and system interior in public places |
| CN107830862A (en) * | 2017-10-13 | 2018-03-23 | 桂林电子科技大学 | A kind of method of the indoor positioning pedestrian tracking based on smart mobile phone |
| CN109680592A (en) * | 2019-01-31 | 2019-04-26 | 河南科技大学 | A kind of vehicle-mounted road surface detection device and method based on inertia measurement and radar range finding |
| CN109682375A (en) * | 2019-01-21 | 2019-04-26 | 重庆邮电大学 | A kind of UWB supplementary inertial localization method based on fault-tolerant decision tree |
| CN109696178A (en) * | 2017-10-20 | 2019-04-30 | 夏普株式会社 | Offset correction device, offset correction program, pedestrian's dead-reckoning analyzer and recording medium |
| CN109764865A (en) * | 2019-01-25 | 2019-05-17 | 北京交通大学 | A kind of indoor orientation method based on MEMS and UWB |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2968395B1 (en) * | 2010-12-01 | 2012-12-28 | Movea | SYSTEM FOR ESTIMATING A TRACK |
| US10190881B2 (en) * | 2015-01-08 | 2019-01-29 | Profound Positioning Inc. | Method and apparatus for enhanced pedestrian navigation based on WLAN and MEMS sensors |
| US9696165B2 (en) * | 2015-02-03 | 2017-07-04 | The Regents Of The University Of Michigan | Last-mile navigation using smartphones |
| CN110017837B (en) * | 2019-04-26 | 2022-11-25 | 沈阳航空航天大学 | Attitude anti-magnetic interference combined navigation method |
-
2020
- 2020-01-15 CN CN202010041983.9A patent/CN111197983B/en active Active
Patent Citations (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2004125731A (en) * | 2002-10-07 | 2004-04-22 | Nec Tokin Corp | Attitude angle detector |
| CN103076619A (en) * | 2012-12-27 | 2013-05-01 | 山东大学 | System and method for performing indoor and outdoor 3D (Three-Dimensional) seamless positioning and gesture measuring on fire man |
| CN103776446A (en) * | 2013-10-29 | 2014-05-07 | 哈尔滨工程大学 | Pedestrian autonomous navigation calculation algorithm based on MEMS-IMU |
| CN103916875A (en) * | 2014-04-24 | 2014-07-09 | 山东大学 | Management and planning system of multi-class control terminals based on WIFI wireless network |
| CN104061934A (en) * | 2014-06-10 | 2014-09-24 | 哈尔滨工业大学 | Pedestrian indoor position tracking method based on inertial sensor |
| CN104215238A (en) * | 2014-08-21 | 2014-12-17 | 北京空间飞行器总体设计部 | Indoor positioning method of intelligent mobile phone |
| CN104897155A (en) * | 2015-06-05 | 2015-09-09 | 北京信息科技大学 | Personal portable auxiliary multisource locating information correcting method |
| CN105651281A (en) * | 2016-03-03 | 2016-06-08 | 南京诺导电子科技有限公司 | Space positioning algorithm on basis of multi-direction human movement modes |
| CN106352875A (en) * | 2016-09-21 | 2017-01-25 | 成都创慧科达科技有限公司 | Dead-reckoning-based navigation system and method |
| CN106500691A (en) * | 2016-10-21 | 2017-03-15 | 上海国际汽车城（集团）有限公司 | Vehicle real-time Passive Location and Tracking method and system interior in public places |
| CN107830862A (en) * | 2017-10-13 | 2018-03-23 | 桂林电子科技大学 | A kind of method of the indoor positioning pedestrian tracking based on smart mobile phone |
| CN109696178A (en) * | 2017-10-20 | 2019-04-30 | 夏普株式会社 | Offset correction device, offset correction program, pedestrian's dead-reckoning analyzer and recording medium |
| CN109682375A (en) * | 2019-01-21 | 2019-04-26 | 重庆邮电大学 | A kind of UWB supplementary inertial localization method based on fault-tolerant decision tree |
| CN109764865A (en) * | 2019-01-25 | 2019-05-17 | 北京交通大学 | A kind of indoor orientation method based on MEMS and UWB |
| CN109680592A (en) * | 2019-01-31 | 2019-04-26 | 河南科技大学 | A kind of vehicle-mounted road surface detection device and method based on inertia measurement and radar range finding |
Non-Patent Citations (3)
| Title |
|---|
| A Foot-mounted PDR System Based On IMU/EKF+HMM+ZUPT+ZARU+HDR+Compass Algorithm;Wenchao Zhang 等;《2017 International Conference on Indoor Positioning and Indoor Navigation (IPIN)》;20170921;第1-5页 * |
| 基于MEMS惯性器件的室内行人定位方法研究;赵玉乐;《中国优秀博硕士学位论文全文数据库(硕士) 信息科技辑》;20190215(第2期);I136-1491 * |
| 基于惯性传感器的动态步长算法研究;刘宇 等;《压电与声光》;20161031;第38卷(第5期);第766-774页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111197983A (en) | 2020-05-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Bai et al. | A high-precision and low-cost IMU-based indoor pedestrian positioning technique | |
| CN104757976B (en) | A kind of Human Body Gait Analysis method and system based on Multi-sensor Fusion | |
| EP2850392B1 (en) | Method for step detection and gait direction estimation | |
| Tian et al. | Accurate human navigation using wearable monocular visual and inertial sensors | |
| Jimenez et al. | A comparison of pedestrian dead-reckoning algorithms using a low-cost MEMS IMU | |
| CN108939512B (en) | Swimming posture measuring method based on wearable sensor | |
| CN103968827B (en) | A kind of autonomic positioning method of wearable body gait detection | |
| US10352959B2 (en) | Method and system for estimating a path of a mobile element or body | |
| CN110095116A (en) | A kind of localization method of vision positioning and inertial navigation combination based on LIFT | |
| Bleser et al. | Using egocentric vision to achieve robust inertial body tracking under magnetic disturbances | |
| CN108836346A (en) | A kind of Human Body Gait Analysis method and system based on inertial sensor | |
| CN107490378B (en) | Indoor positioning and navigation method based on MPU6050 and smart phone | |
| Meng et al. | Self-contained pedestrian tracking during normal walking using an inertial/magnetic sensor module | |
| Li et al. | Real-time human motion capture based on wearable inertial sensor networks | |
| CN105865450A (en) | Zero-speed update method and system based on gait | |
| CN108168548B (en) | Pedestrian inertial navigation system and method assisted by machine learning algorithm and model | |
| CN110579207A (en) | Indoor positioning system and method based on combination of geomagnetic signals and computer vision | |
| Tjhai et al. | Step-size estimation using fusion of multiple wearable inertial sensors | |
| CN110319840A (en) | Conjugate gradient attitude algorithm method towards abnormal gait identification | |
| Woyano et al. | Evaluation and comparison of performance analysis of indoor inertial navigation system based on foot mounted IMU | |
| CN110672095A (en) | Pedestrian indoor autonomous positioning algorithm based on micro inertial navigation | |
| Scapellato et al. | In-use calibration of body-mounted gyroscopes for applications in gait analysis | |
| CN115655265A (en) | Pedestrian navigation indoor positioning method based on inertial sensing | |
| Ji et al. | An adaptive heading correction algorithm for suppressing magnetic interference in inertial navigation system | |
| KR101301462B1 (en) | Pedestiran dead-reckoning apparatus using low cost inertial measurement unit and method thereof |
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 |