CN111197983B - Three-dimensional pose measurement method based on human body distribution inertia node vector distance measurement - Google Patents

Abstract

The invention claims a three-dimensional pose measurement method based on human body distributed inertial node vector distance measurement. In this method, distance measuring modules and inertial nodes are installed on the ankle joints of the pedestrian's left and right legs, and each inertial node integrates a three-axis accelerometer, an angular velocity meter, and a magnetometer. Use the ranging module to measure the real-time distance information between pedestrian ankle joints; use the inertial nodes to calculate the attitude angle information at the ankle joint nodes. The vector information in each time segment is obtained by combining the acceleration information and attitude angle information of the three axes during the movement with the previous ranging information. Through vector information and attitude angle information, the position and attitude information of pedestrians in the three-dimensional space domain can be tracked in real time, referred to as pose information. The parameters of the invention are intuitive and reliable, and have the characteristics of independent measurement, no use of estimators, and no restriction of characteristic parameters.

Description

3D Pose Measurement Method Based on Human Body Distributed Inertial Node Vector Ranging

technical field

The invention belongs to the field of inertial navigation and positioning of pedestrians, in particular to a three-dimensional pose measurement method based on human body distributed inertial node vector distance measurement.

Background technique

Gait analysis and step calculation are the key points in the field of inertial navigation and positioning. By analyzing the gait information of pedestrians, the physiological information, motion behavior and health status of the human body can be estimated. By improving the calculation accuracy of the step size, the final navigation positioning accuracy can be improved.

Currently, researchers have done a lot of research on motion estimation systems. It is mainly divided into three categories. One is based on optical methods, such as using the Vicon system to collect human body data. The collected information can be applied to sports analysis and biomedical research, but there are disadvantages of expensive equipment and the need to arrange sensors in advance. One is to use the visual tracking platform to extract the parameter information of human movement through corresponding computer technology and image processing technology, and then analyze the gait information and calculate the stride length, such as Zhang Song et al. .Human action evaluation method based on depth camera[D].Hangzhou Dianzi University, 2018.]The Kinect depth camera is used to collect depth images, and the Kalman-Meanshift tracking method is used to achieve a better depth image human target tracking effect, but there is an algorithm Complicated problems that cannot be applied to real-time operating systems; one is to use wearable devices, such as common inertial devices, to perceive and obtain human body motion parameters, detect pedestrian gait information through parameter extraction and analysis, and use estimators To estimate the step size of pedestrians, the common estimation methods include the linear step size calculation model based on the step frequency calculation step size and the nonlinear step size calculation model based on the acceleration amplitude change calculation step size, Yanshan University Zhang Xiongjie et al. [Document: Zhang Xiong Jie. Research on human motion characteristics based on Xsens MVN inertial motion capture system equipment [D]. Yanshan University, 2016.] Use wearable inertial sensor equipment to obtain personnel's motion parameters, and then study the motion characteristics of human body, Nanchang University Xiong Jian et al. [Document: Xiong Jian, Xu Jiangying, Yang Zuhua, et al. A pedestrian navigation method based on human motion pattern monitoring:.] By installing dual-axis angular velocity sensors on pedestrians' hip joints, knee joints and ankle joints, real-time Measure the angle information of the person's legs during the movement process, combined with the length information of the pedestrian's legs, and finally obtain the movement information, position information and pedestrian step information of each joint, but there are algorithm model parameters that need to be changed manually according to different testers , the disadvantage of poor robustness.

The invention aims at the disadvantages that the optics-based motion estimation system needs to arrange sensors in advance, the vision-based motion estimation system has high algorithm complexity, and the inertial device-based motion estimation system has poor robustness and low precision. A 3D pose measurement method based on human distributed inertial node vector ranging is proposed. This method uses the sensors carried by pedestrians to complete the precise step calculation without prior arrangement of sensors. The algorithm has a small amount of calculation and can be applied to real-time operating systems. Strong Robustness No need to change model parameters according to the differences of testers is extremely practical. Compared with the traditional motion estimation system, the dynamic combination of the ranging module and the inertial node can realize real-time three-dimensional pose measurement without estimated parameters and accumulated errors, and can realize the whole process tracking and reproduction of pedestrian single-step motion.

Contents of the invention

The present invention aims to solve the above problems of the prior art. A 3D pose measurement method based on human distributed inertial node vector ranging is proposed. Technical scheme of the present invention is as follows:

A three-dimensional pose measurement method based on human body distributed inertial node vector ranging, which comprises the following steps:

Step 1. Install a ranging module and an inertial node at the ankle joints of the pedestrian's left and right legs, where the inertial node collects the acceleration information, angular velocity information, and magnetic field strength information of the installed node in real time, and the ranging module collects the distance information between the two nodes in real time;

Step 2, in the initial stage, use the acceleration information and magnetic field strength information collected by the inertial node to obtain the reference attitude angle information;

Step 3, use the combined acceleration information at the ankle joint node to estimate the motion state of the two nodes, and distinguish between the motion state and the static state;

Step 4. For the nodes in the static state, the error is reduced by zero-speed correction. For the nodes in the moving state, the attitude angle information is updated in real time by the gyroscope to obtain the current attitude information of the node, and the result of the attitude angle change is obtained by combining the reference attitude angle information. ;

Step 5, using the results of the attitude angle change and the acceleration information on the three axes sensed by the accelerometer to obtain the angle information of the vector;

Step 6, according to the distance information between the two nodes provided by the ranging module and the angle information of the vector, obtain the real-time position information of the node;

Step 7. Synthesize the single-step movement information of pedestrians to realize the personnel positioning function.

Further, in the step 1, by adjusting the power of the ranging module, selecting a suitable directional antenna and feeder, etc., the short-distance and high-precision ranging function of the ranging module can be realized, and the distance information between the ankle joint nodes can be measured in real time . The motion information at the node is collected in real time through the inertial node.

Further, in the step 2, in the initial stage, the acceleration information and magnetic field strength information collected by the inertial nodes are used to obtain the reference attitude angle information; is in a static state, therefore, the attitude angle information in this state can be calculated by the accelerometer and magnetometer, and used as the initial attitude angle of the ankle joint node, that is, the reference attitude angle, as shown in formula (1):

Among them, θ ₁ , γ ₁ , and ψ ₁ represent the pitch angle, roll angle, and heading angle respectively; A _x , A _y , and A _z represent the three-axis acceleration information respectively; m _x , my _y , and m _z represent the three-axis magnetometer strength information.

Further, in the step 3, the motion state of the two nodes is estimated by using the combined acceleration information at the ankle joint node, and the motion state and the static state are distinguished, which specifically includes: judging the motion state of the foot through a threshold condition , the judgment method is shown in formula (2):

Among them, A _th represents the threshold for distinguishing motion state from static state, and A _norm represents the combined acceleration. When the combined acceleration A _norm is greater than the threshold A _th , it means that the node is in a moving state; when the combined acceleration A _norm is smaller than the threshold, A _th means that the node is in motion. at rest.

Further, in step 4, when the ankle joint node is in a static state, data fusion is performed through Kalman filter technology, the error of the system is estimated and the system parameters are corrected by using the estimated value of the error, as shown in formula (3) :

Among them, F is the system matrix composed of error model and state quantity, W is the system random process noise sequence, and V is the system observation noise sequence;

When the ankle joint node is in motion, the attitude angle information of the node is updated through the gyroscope, as shown in formula (4):

Among them, q ₀ , q ₁ , q ₂ , and q ₃ are quaternion information, θ ₂ , γ ₂ , and ψ ₂ are real-time attitude angle information, and real-time attitude angle information can be obtained by updating the quaternion number through the gyroscope That is, the attitude information of the node, the difference between the real-time attitude angle information and the reference attitude angle information is obtained to obtain the transformation information of the attitude angle, as shown in formula (5):

Further, 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 on the three axes sensitive to the accelerometer, specifically including: in the actual movement process, the three-axis accelerometer can be sensitive According to the component forces in the three directions of the carrier coordinate system, the angle information of the vector in the carrier coordinate system can be obtained through the proportional relationship of the component forces. Combining the change information of the attitude angle in step 4 with the carrier coordinate system The angle information of the vector is calculated to obtain the 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 the vector relationship, as shown in formula (6) and formula (7):

θy=arccos(Ay/Anorm) (6)

θsum=θy+θ (7)

Among them, θ _y is the angle between the y-axis acceleration A _y of the carrier coordinate system and the total acceleration A _norm , θ is the change information of the current attitude angle compared to the initial attitude angle, and θ _sum is the vector after the attitude angle change compensation in Angle change information of pedestrian sagittal plane.

Further, in the step 6, the real-time position information of the node is obtained according to the distance information between the two nodes provided by the ranging module and the angle information of the vector, specifically including: in step 5, the vector of each time segment can be obtained Angle information, combined with the distance information between two nodes provided by the ranging module, can get all the information of the vector, and finally, the real-time position information of the node can be obtained through the functional relationship, as shown in formula (8):

Among them, L is the distance information between two nodes measured by the ranging module, that is, the vector length information, α is the angle information of the vector in the sagittal plane, that is, the angle information of the vector, y is the step distance information of the sagittal plane, h It is the step height information of the sagittal plane. During the whole process, the angle information of the vector needs to be updated continuously. During the movement process, the moving node swings from the back to the front relative to the stationary node to complete a stepping process, which can be divided into Three stages, the first stage is that the moving node is behind the stationary node, the second stage is that the moving node swings from behind the stationary node to the front of the stationary node, and the third stage is that the moving node is in front of the stationary node, through the angle of the vector Information can be distinguished for three phases.

Further, in the step 7, the movement information of the single step of the pedestrian is integrated to realize the personnel positioning function, which specifically includes: through the steps 1 to 6, the step length information of the single step and the posture information in the single step can be accurately calculated, and the information of each step is integrated. Single-step motion information realizes the positioning function of personnel in three-dimensional space, and the calculation formula is as follows:

Among them, x _step is the horizontal displacement distance of a single step, y _step is the vertical displacement distance of a single step, l is the step length information of a single step, ψ is the heading information provided by the node, and x _sum is the last measurement condition The horizontal displacement distance relative to the initial coordinates, y _sum is the vertical displacement distance relative to the initial coordinates under the previous measurement conditions, x is the horizontal displacement distance relative to the initial coordinates under the current measurement conditions, y is the current measurement conditions The vertical displacement distance relative to the initial coordinates.

Advantage of the present invention and beneficial effect are as follows:

The present invention monitors the real-time posture information of pedestrians in three-dimensional space through vector information and posture information. Main innovation of the present invention is step (5) and step (6), has following benefit:

(1) Good autonomy: This method only relies on the sensors carried by pedestrians to complete accurate step calculation and attitude estimation without external sensors.

(2) Rich motion information: This method can track and reproduce the whole process of pedestrian single-step motion, and can provide step information and step height information of each position in the single-step motion process, and provide attitude information at this position .

(3) High precision: This method can complete the real-time monitoring of the pose information of pedestrians in three-dimensional space, and the calculation parameters are intuitive and measurable. In view of the lack of low precision and error accumulation caused by the use of estimators in the calculation of step length in the pedestrian dead reckoning algorithm Make improvements.

(4) Good real-time performance: This method has a small amount of calculation, intuitive and measurable parameters, and is suitable for real-time operating systems.

(5) Strong practicability: This method does not need to change the parameters of the calculation model according to the differences of testers, and has strong robustness.

Description of drawings

Fig. 1 is the vector relationship representation under the carrier coordinate system of the preferred embodiment provided by the present invention

Figure 2 is the representation of the vector relationship after attitude angle compensation

Figure 3 is a schematic diagram of gait cycle decomposition

Figure 4 is a schematic diagram of step and step height calculation

Figure 5 is a schematic diagram of the moving node behind the static node

Figure 6 is a schematic diagram of the moving node crossing from the back of the static node to the front

Figure 7 is a schematic diagram of the moving node in front of the static node

Figure 8 is a flowchart of the overall framework of the algorithm

detailed description

The technical solutions in the embodiments of the present invention will be described clearly and in detail below with reference to the drawings in the embodiments of the present invention. The described embodiments are only some of the embodiments of the invention.

The technical scheme that the present invention solves the problems of the technologies described above is:

As shown in Figure 8, a three-dimensional pose measurement method based on human body distributed inertial node vector ranging. In this method, ranging modules and inertial nodes are installed on the ankle joints of the left and right legs of pedestrians, and each inertial node integrates a three-axis accelerometer, an angular velocity meter, and a magnetometer. Use the ranging module to measure the real-time distance information between pedestrian ankle joints; use the inertial nodes to calculate the attitude angle information at the ankle joint nodes. The vector information in each time segment is obtained by combining the acceleration information and attitude angle information of the three axes during the movement process with the previous distance measurement information. Through vector information and attitude angle information, the position and attitude information of pedestrians in the three-dimensional space domain can be tracked in real time, referred to as pose information. The parameters of the invention are intuitive and reliable, and have the characteristics of independent measurement, no use of estimators, and no restriction of characteristic parameters.

It includes the following steps: (1) Install a ranging module and an inertial node at the ankle joints of the pedestrian's left and right legs, where the inertial node collects the acceleration information, angular velocity information, and magnetic field strength information of the installed node in real time, and the ranging module collects the distance between the two nodes in real time. (2) In the initial stage, use the acceleration information and magnetic field strength information collected by the inertial node to obtain the reference attitude angle information; (3) use the combined acceleration information at the ankle joint to estimate the motion state of the two nodes, Distinguish between motion state and static state; (4) For nodes in static state, use zero-speed correction to reduce error and improve system accuracy; for nodes in motion state, use gyroscope to update the attitude angle information in real time to obtain the current attitude information of the node, and Combining the reference attitude angle information to obtain the result of the attitude angle change; (5) using the result of the attitude angle change and the acceleration information on the three axes sensitive to the accelerometer to obtain the angle information of the vector; (6) according to the information provided by the ranging module The distance information between the two nodes and the angle information of the vector can obtain the real-time position information of the node; (7) Synthesize the single-step motion information of pedestrians to realize the personnel positioning function. The present invention can complete the real-time monitoring of the pose information of pedestrians in three-dimensional space, and can realize the whole-process tracking and reproduction of the single-step movement of pedestrians, and complete the accurate step length calculation with the sensors that pedestrians can carry without using estimated quantities , Position calculation and attitude measurement without accumulative error, no need to rely on other data information, with strong applicability.

A three-dimensional pose measurement method based on human body distributed inertial node vector ranging, which comprises the following steps:

Step (1), install a ranging module and an inertial node at the ankle joints of the pedestrian's left and right legs, where the inertial node collects the acceleration information, angular velocity information, and magnetic field strength information of the installed node in real time, and the ranging module collects the distance between the two nodes in real time information;

Step (2), in the initial stage, use the acceleration information and magnetic field strength information collected by the inertial node to obtain the reference attitude angle information;

Step (3), using the combined acceleration information at the ankle joint node to estimate the motion state of the two nodes, and distinguish between the motion state and the static state;

Step (4), for the nodes in the static state, the error is reduced by zero-speed correction, and the system accuracy is improved. For the nodes in the moving state, the attitude angle information is updated in real time by the gyroscope, and the current attitude information of the node is obtained, combined with the reference attitude angle information Obtain the result of attitude angle change;

Step (5), using the result of the attitude angle change and the acceleration information on the three axes that the accelerometer is sensitive to obtain the angle information of the vector;

Step (6), the real-time location information of the node can be obtained according to the distance information between the two nodes provided by the ranging module and the angle information of the vector;

Step (7) Synthesize the single-step motion information of pedestrians to realize the personnel positioning function.

In the step (1), a ranging module and an inertial node are installed at the ankle joints of the left and right legs of the pedestrian, wherein the inertial node collects the acceleration information, angular velocity information and magnetic field strength information of the installed node in real time, and the ranging module collects two nodes in real time distance information. The inertial nodes are used to provide motion information at the nodes, and the short-range high-precision ranging of the ranging module is realized by reducing the gain of the ranging module and reducing the sampling frequency, which can accurately measure the distance information between ankle joint nodes.

In the step (2), in the initial stage, the acceleration information and the magnetic field strength information collected by the inertial node are used to obtain the reference attitude angle information; in the initial stage of the program operation, the left foot and the right foot of the human body are in the supporting stage. At this time, the ankle Joint nodes are at rest. 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):

Among them, θ ₁ , γ ₁ , and ψ ₁ represent the pitch angle, roll angle, and heading angle respectively; A _x , A _y , and A _z represent the three-axis acceleration information respectively; m _x , my _y , and m _z represent the three-axis magnetometer strength information.

In the step (3), the motion state of the two nodes is estimated by using the combined acceleration information at the ankle joint node, and the motion state and the static state are distinguished. When a person walks, when one foot is in a supporting state, the other foot is in a swinging state, and the state of the two feet presents a periodic alternation. Therefore, the motion state of the node will change accordingly. In the process of estimating the position information of moving nodes through vectors, stationary nodes need to be used as reference points. Therefore, we need to distinguish moving nodes from stationary nodes in the calculation process. When the node is at rest, the resultant acceleration (compensated by gravity) changes around 0; when the node is in motion, the resultant acceleration will change greatly. Through the threshold condition, the motion state of the foot can be judged. The judgment method is shown in formula (2):

Among them, A _th represents the threshold value for distinguishing the motion state from the static state, and A _norm represents the combined acceleration. When the resultant acceleration A _norm is greater than the threshold value A _th , it means that the node is in a moving state; when the resultant acceleration A _norm is smaller than the threshold value A _th , it means that the node is in a static state.

In the step (4), for the nodes in the static state, the error is reduced by zero-speed correction, and the accuracy of the system is improved. For the nodes in the motion state, the attitude angle information is updated in real time using the gyroscope to obtain the current attitude information of the node, and combined with reference The attitude angle information obtains the result of the attitude angle change. Inertial devices have the disadvantages of high short-term accuracy and low long-term accuracy. Therefore, we need to establish a suitable error correction model to improve the accuracy of the system. When the ankle joint is in a static state, the Kalman filter technology is used for data fusion to estimate the system error and use the estimated value of the error to correct the system parameters. As shown in formula (3):

Among them, F is the system matrix composed of error model and state quantity, W is the system random process noise sequence, and V is the system observation noise sequence.

When the ankle joint node is in motion, the attitude angle information of the node is updated through the gyroscope, as shown in formula (4):

Among them, q ₀ , q ₁ , q ₂ , and q ₃ are quaternion information, θ ₂ , γ ₂ , and ψ ₂ are real-time attitude angle information, and real-time attitude angle information can be obtained by updating the quaternion through the gyroscope That is, the attitude information of the node. The difference between the real-time attitude angle information and the reference attitude angle information is obtained 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 on the three axes sensed by the accelerometer. In the actual movement process, the three-axis accelerometer can be sensitive to the component forces of the specific force in the three directions of the carrier coordinate system. Through the proportional relationship of the component forces, the angle information of the vector in the carrier coordinate system can be obtained. The carrier coordinate system The following vector relationship representation is shown in Figure 1. Combining the change information of the attitude angle in step (4) with the angle information of the vector in the carrier coordinate system, the final angle information of the vector relative to the initial attitude angle is calculated. On the sagittal plane of the pedestrian, the direction and angle information can be obtained through the vector relationship, 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)

Among them, θ _y is the angle between the y-axis acceleration A _y of the carrier coordinate system and the total acceleration A _norm , θ is the change information of the current attitude angle compared to the initial attitude angle, and θ _sum is the vector after the attitude angle change compensation in Angle change information of pedestrian sagittal plane.

In the 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. The execution frequency of the program is 200Hz, and the walking frequency of a person is usually about 3-5Hz. The entire walking process of pedestrians can be divided into multiple time segments, and in each time segment, the motion of nodes can be regarded as vector motion. Therefore, the entire walking process can be regarded as the superposition of multiple vectors, and the schematic diagram of multiple vectors obtained by decomposing the gait cycle is shown in Figure 3. In step (5), the angle information of the vector in each time segment can be obtained, combined with the distance information between two nodes provided by the ranging module, all the information of the vector can be obtained. Finally, the real-time location information of the nodes can be obtained through the functional relationship, as shown in Figure 4. As shown in formula (8):

Among them, L is the distance information between two nodes measured by the ranging module, that is, the vector length information, α is the angle information of the vector in the sagittal plane, that is, the angle information of the vector, y is the step distance information of the sagittal plane, h is the step height information of the sagittal plane. During the whole process, it is necessary to continuously update the angle information of the vector, that is, α. During the movement process, the moving node swings from back to front relative to the stationary node (completes a stepping process) can be divided into three stages. The first stage is that the moving node is behind the stationary node, as shown in Figure 5; The second stage is that the moving node swings from behind the stationary node to the front of the stationary node, as shown in Figure 6; the third stage is that the moving node is in front of the stationary node, as shown in Figure 7. The three stages can be distinguished by the angle information of the vector, and the update of the angle information of the three stages is slightly different. Calculated as follows:

The first stage:

Among them, L ₁ is the length information of the previous measured vector, L ₂ is the length information of the current measured vector, L ₃ is the length information of the next measured vector, θ ₁ is the angle information provided by the last measurement inertial node , θ ₂ is the angle information provided by the current measurement inertial node, α is the angle between the previous measurement and the current measurement vector, α ₁ is the angle between the current measurement and the next measurement vector, β and γ are the calculation process The angle information used in , through these parameters, the update of the vector angle can be completed. When the calculation process is over, the intermediate quantity needs to be updated, so as to obtain the step information and step height information in each time segment in the first stage.

second stage:

Among them, L ₁ is the length information of the previous measured vector, L ₂ is the length information of the current measured vector, θ ₁ is the angle information provided by the current measurement inertial node, and α is the updated angle information of the previous vector ( Angle with the horizontal axis), α ₁ is the angle between the previous measurement and the current measurement vector, β and ψ are the angle information used in the calculation process, and the update of the vector angle can be completed through these parameters. When the calculation process is over, the intermediate quantity needs to be updated, so as to obtain the step information and step height information in each time segment in the second stage.

The third stage:

Among them, L ₁ is the length information of the previous measured vector, L ₂ is the length information of the current measured vector, θ ₁ is the angle information provided by the current measurement inertial node, and α is the updated angle information of the previous vector ( Angle with the horizontal axis), α ₁ is the angle between the previous measurement and the current measurement vector, β and ψ are the angle information used in the calculation process, and the update of the vector angle can be completed through these parameters. When the calculation process is over, the intermediate quantity needs to be updated, so as to obtain the step information and step height information in each time segment in the third stage.

In the step (7), the single-step motion information of pedestrians is integrated to realize the personnel positioning function. Through steps (1) to (6), the step length information of a single step and the attitude information within a single step can be accurately calculated. Combining the motion information of each single step, the positioning function of personnel in three-dimensional space can be realized. Calculated as follows:

Among them, x _step is the horizontal displacement distance of a single step, y _step is the vertical displacement distance of a single step, l is the step length information of a single step, ψ is the heading information provided by the node, and x _sum is the last measurement condition The horizontal displacement distance relative to the initial coordinates, y _sum is the vertical displacement distance relative to the initial coordinates under the previous measurement conditions, x is the horizontal displacement distance relative to the initial coordinates under the current measurement conditions, y is the current measurement conditions The vertical displacement distance relative to the initial coordinates.

The overall framework flow chart of the algorithm is shown in Figure 8.

The above embodiments should be understood as only for illustrating the present invention but not for limiting the protection scope of the present invention. After reading the contents of the present invention, skilled persons can make various changes or modifications to the present invention, and these equivalent changes and modifications also fall within the scope defined by the claims of the present invention.

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 ₀ 、q ₁ 、q ₂ 、q ₃ Is quaternion information, θ ₂ 、γ ₂ 、ψ ₂ 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 ₁ 、γ ₁ 、ψ ₁ 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.

Images