CN107270896A - A kind of pedestrian's positioning and trace tracking method and system - Google Patents

Abstract

The invention discloses a pedestrian positioning and trajectory tracking method and system, wherein the implementation of the method is: receiving acceleration information, angular velocity information and magnetometer information of pedestrians; filtering and thresholding the acceleration information and angular velocity information to obtain the velocity state Information, when the speed is not 0, the angular velocity information is used to obtain the attitude information, and the acceleration information is used to correct the angular velocity information to obtain the attitude information when the speed is 0; the magnetometer is ellipse corrected, and the attitude information is used to compensate the inclination angle of the magnetometer to obtain the magnetic heading angle; The magnetic course angle and the course angle in the attitude information are fused to obtain the fused course angle. Combining the attitude information and the fused course angle, the velocity information can be obtained by integrating the acceleration. After the velocity information is corrected by zero speed, the displacement in the horizontal direction and the vertical direction can be obtained. Pedestrian positioning and trajectory tracking through displacement and height. The invention can navigate for a long time, has strong adaptability and wide application range.

Description

A pedestrian positioning and trajectory tracking method and system

technical field

The invention belongs to the field of inertial navigation, and more specifically, relates to a pedestrian positioning and trajectory tracking method and system.

Background technique

With the rapid development of science and technology and the rapid advancement of urbanization, positioning technology has become more and more important in people's lives, and people's demand for positioning products, functions and services has increased, thus giving birth to a huge market. Traditional positioning technologies are basically based on GPS and wireless networks (Wifi, ZigBee, UWB, etc.), but this positioning method cannot be used indoors without GPS signals or in places without wireless networks. Therefore, the pedestrian positioning and trajectory tracking technology based on inertial navigation sensors is gradually emerging. Its biggest feature is that it can realize autonomous navigation and real-time positioning in known or unknown environments without basic fixed facilities. Conventional pedestrian positioning devices based on inertial navigation sensors usually only use accelerometers and gyroscopes, and then cooperate with attitude calculation and zero-speed detection algorithms to perform three-dimensional trajectory calculation and positioning. However, in this solution, due to the drift of the gyroscope, the heading angle will eventually diverge, and the positioning will fail in the case of long-term navigation and positioning. At the same time, the conventional positioning scheme based on inertial navigation sensors sometimes adopts the method of estimating the pedestrian’s step length. Personnel positioning scenarios greatly reduce the application range of pedestrian positioning devices based on inertial navigation sensors.

It can be seen that the prior art has the technical problems of being unable to navigate for a long time, poor adaptability and narrow application range.

Contents of the invention

In view of the above defects or improvement needs of the prior art, the present invention provides a pedestrian positioning and trajectory tracking method and system, thereby solving the technical problems of the prior art that cannot navigate for a long time, has poor adaptability and narrow application range.

In order to achieve the above purpose, according to one aspect of the present invention, a pedestrian positioning and trajectory tracking method is provided, including:

(1) Receive the acceleration information and angular velocity information of pedestrians, perform filtering and threshold segmentation on the acceleration information and angular velocity information, and obtain the velocity status information. When the velocity is not 0, use the angular velocity information to obtain the attitude information, and use the acceleration information to correct the angular velocity information when the velocity is 0. Get posture information;

(2) Receive the magnetometer information, perform ellipse correction on the magnetometer, and then use the attitude information to perform inclination compensation to obtain the magnetic heading angle;

(3) The magnetic course angle and the course angle in the attitude information are fused to obtain the fused course angle, combined with the attitude information and the fused course angle, the velocity information is obtained by integrating the acceleration, and after the velocity information is corrected by zero speed, it is integrated again to obtain the The displacement and height in the vertical direction, and finally the pedestrian positioning and trajectory tracking are performed through the displacement and height.

Further, step (1) includes:

(1-1) Carry out filtering and threshold value segmentation to acceleration information and angular velocity information, obtain velocity state information;

(1-2) When the speed is not 0, use the angular velocity information to update the quaternion, and use the quaternion to obtain the attitude information; when the speed is 0, use the proportional integral, use the acceleration information to correct the drift of the angular velocity information, and use the corrected angular velocity information to update the four arity, and then get the attitude information.

Further, the fusion heading angle is:

Among them, 0≤i≤n, c _i is the magnetic heading angle at time i, y _i is the heading angle in the attitude information at time i, h _i is the fusion heading angle at time i, E _i is the heading angle deviation at time i Shift, k _P is the proportional coefficient, and k _I is the integral coefficient.

Further, step (3) includes:

(3-1) Fusion of the magnetic heading angle and the heading angle in the attitude information to obtain the fusion heading angle;

(3-2) After the speed information is corrected by zero speed, the displacement in the horizontal direction can be directly integrated by the speed in the horizontal direction, and the speed in the vertical direction needs to be further judged by going up and down the stairs. After height compensation, the displacement in the vertical direction can be obtained high;

(3-3) Use displacement and height for pedestrian positioning and trajectory tracking.

According to another aspect of the present invention, a pedestrian positioning and trajectory tracking system is provided, including: an accelerometer, a gyroscope, a magnetometer and a control module,

The accelerometer is used to receive the acceleration information of pedestrians and transmit it to the control module;

The gyroscope is used to receive the angular velocity information of the pedestrian and transmit it to the control module;

The magnetometer is used to receive the magnetometer information of the pedestrian and transmit it to the control module;

The control module is used to obtain speed state information and attitude information by using acceleration information and angular velocity information, obtain magnetic heading angle by using magnetometer information, fuse magnetic heading angle and heading angle in attitude information to obtain fusion heading angle, use displacement and Altitude positioning and trajectory tracking of pedestrians.

Further, the control module includes a gait submodule, a fusion submodule and a navigation submodule.

Further, the gait sub-module is used to filter and threshold the acceleration information and angular velocity information to obtain the velocity state information, and obtain the velocity information by integrating the acceleration. After the velocity information is corrected by zero velocity, the displacement in the horizontal direction can be determined by the horizontal direction The speed is directly integrated to get the speed in the vertical direction. The speed in the vertical direction needs to be further judged by going up and down the stairs. After height compensation, the height in the vertical direction is obtained.

Further, the fusion sub-module is used to obtain the attitude information by using the angular velocity information when the pedestrian speed is non-zero, and use the acceleration information to correct the angular velocity information to obtain the attitude information when the pedestrian speed is 0. After the ellipse correction is performed on the magnetometer, the attitude information is used to Perform tilt angle compensation to obtain the magnetic heading angle, and fuse the magnetic heading angle with the heading angle in the attitude information to obtain the fused heading angle.

Further, the navigation sub-module is used for pedestrian positioning and trajectory tracking using displacement and height.

Generally speaking, compared with the prior art, the above technical solutions conceived by the present invention can achieve the following beneficial effects:

(1) When the speed is not 0, the acceleration information is affected by the external force and cannot accurately reflect the attitude information. At this time, if the attitude information is obtained by using the acceleration information and angular velocity information, the calculation accuracy of the attitude information will be reduced; when the speed is 0 , the acceleration information can reflect the attitude information more accurately. At this time, the acceleration information is used to correct the offset of the angular velocity information, which can effectively improve the calculation accuracy of the attitude information. When the velocity is not zero, the angular velocity information is used to obtain the attitude information, and the velocity is At 0 o'clock, the acceleration information is used to correct the angular velocity information to obtain the attitude information, and the attitude information of the present invention has high precision.

(2) The present invention fuses the magnetic heading angle and the heading angle in the attitude information, and the obtained fusion heading angle is a feature-level fusion. Since the magnetometer information is easily disturbed, if in the data layer, the magnetometer information and the acceleration information are combined , angular velocity information are fused together to calculate the attitude angle, it is easy to spread the error of the magnetometer information to the pitch angle and roll angle, thereby reducing the accuracy of the attitude information, and after adopting the feature-level fusion scheme of the present invention, without reducing the pitch angle and roll angle Under the premise of angular accuracy, the magnetometer information can be effectively used to improve the drift of the fused heading angle, thereby further improving the accuracy of the fused heading angle.

(3) The present invention does not depend on the average walking step length of a specific person, and the calculation amount of the algorithm is small, which can meet the requirements of most applications. The present invention can navigate for a long time, has strong adaptability and wide application range.

(4) Preferably, after the velocity information is corrected at zero velocity, the displacement in the horizontal direction can be obtained by directly integrating the velocity in the horizontal direction, and the velocity in the vertical direction needs to be further judged by going up and down the stairs, and then combined with height compensation to compensate for the height In this way, the accuracy of three-dimensional positioning can be improved while ensuring the accuracy of two-dimensional positioning.

Description of drawings

Fig. 1 is a flow chart of a pedestrian positioning and trajectory tracking method provided by an embodiment of the present invention;

FIG. 2 is a structural diagram of a pedestrian positioning and trajectory tracking system provided in Embodiment 1 of the present invention;

Fig. 3 is a structural design and topological relationship of a pedestrian positioning and trajectory tracking system provided by Embodiment 1 of the present invention;

FIG. 4 is a flow chart of an attitude calculation algorithm provided by Embodiment 1 of the present invention.

detailed description

In order to make the object, technical solution and advantages of the present invention clearer, the present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described here are only used to explain the present invention, not to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not constitute a conflict with each other.

As shown in Figure 1, a pedestrian positioning and trajectory tracking method includes:

(1) Receive the acceleration information and angular velocity information of pedestrians, perform filtering and threshold segmentation on the acceleration information and angular velocity information, and obtain the velocity status information. When the velocity is not 0, use the angular velocity information to obtain the attitude information, and use the acceleration information to correct the angular velocity information when the velocity is 0. Get posture information;

(2) Receive the magnetometer information, perform ellipse correction on the magnetometer, and then use the attitude information to perform inclination compensation to obtain the magnetic heading angle;

(3) The magnetic course angle and the course angle in the attitude information are fused to obtain the fused course angle, combined with the attitude information and the fused course angle, the velocity information is obtained by integrating the acceleration, and after the velocity information is corrected by zero speed, it is integrated again to obtain the The displacement and height in the vertical direction, and finally the pedestrian positioning and trajectory tracking are performed through the displacement and height.

Further, step (1) includes:

(1-1) Carry out filtering and threshold value segmentation to acceleration information and angular velocity information, obtain velocity state information;

(1-2) When the speed is not 0, use the angular velocity information to update the quaternion, and use the quaternion to obtain the attitude information; when the speed is 0, use the proportional integral, use the acceleration information to correct the drift of the angular velocity information, and use the corrected angular velocity information to update the four arity, and then get the attitude information.

Further, the fusion heading angle is:

Among them, 0≤i≤n, c _i is the magnetic heading angle at time i, y _i is the heading angle in the attitude information at time i, h _i is the fusion heading angle at time i, E _i is the heading angle deviation at time i shift, k _P is the proportional coefficient, k _I is the integral coefficient, T ₁ and T ₂ are the proportional limit value and the integral limit value respectively.

Further, step (3) includes:

(3-1) merging the magnetic heading angle and the attitude heading angle to obtain the fusion heading angle;

(3-2) After the speed information is corrected by zero speed, the displacement in the horizontal direction can be directly integrated by the speed in the horizontal direction, and the speed in the vertical direction needs to be further judged by going up and down the stairs. After height compensation, the displacement in the vertical direction can be obtained high;

(3-3) Use displacement and height for pedestrian positioning and trajectory tracking.

Example 1

As shown in Figure 2, Embodiment 1 of the present invention provides a schematic structural diagram of a pedestrian positioning and trajectory tracking system, which is mainly composed of a power supply, an accelerometer, a gyroscope, a magnetometer, a control module, and Bluetooth, and its connection mode is: power supply It is connected with accelerometer, gyroscope, magnetometer, control module and Bluetooth, and the control module is bidirectionally connected with accelerometer, gyroscope, magnetometer and Bluetooth.

As shown in Figure 3, the structural design and topological relationship of a pedestrian positioning and trajectory tracking system provided by Embodiment 1 of the present invention: preferably, the magnetometer is a three-axis magnetometer, the accelerometer is a three-axis accelerometer, and the gyroscope is Three-axis gyroscope, the three-axis magnetometer is calibrated by the magnetometer, and the magnetic heading angle is calculated; the three-axis accelerometer and the three-axis gyroscope, through AHRS attitude calculation, not only calculate the attitude angle but also calculate the pedestrian in the world coordinate system Acceleration of walking; attitude angle includes heading angle, pitch angle and roll angle. After the heading angle calculated by the attitude solution is fused with the magnetic heading angle, the walking acceleration of pedestrians in the world coordinate system is updated; using the three-axis gyroscope and the world coordinate system The walking acceleration of pedestrians under the following conditions is segmented by gait extraction, and the segmentation results are fed back to the AHRS attitude calculation and used for zero-speed correction; after zero-speed correction, the displacement in the horizontal direction can be directly integrated by the velocity in the horizontal direction It is concluded that the speed in the vertical direction needs to be further judged by going up and down the stairs, and after height compensation, the height in the vertical direction is calculated.

As shown in Figure 4, the attitude solution is calculated using the quaternion method. First, the quaternion q is initialized. When the gait is judged to be non-zero speed, the gyroscope is used to update the quaternion; and when the gait is judged to be zero speed When using the PI (proportional integral) algorithm, the accelerometer is used to correct the drift of the gyroscope, and the corrected gyroscope is used to update the quaternion, and finally the three attitude angles are solved by the quaternion: attitude heading angle y, roll angle r and the pitch angle p, the magnetic heading angle c is calculated using the magnetometer, and the heading angle fusion is divided into two stages: real-time fusion and timing fusion. In the real-time fusion stage, the magnetometer is used to correct the heading angle in real time. In the timing fusion stage, the fused heading angle is used to update the quaternion regularly, so that the integral drift of the gyroscope is periodically cleared.

Compared with the prior art, the present invention does not require basic fixed facilities and external auxiliary equipment, adopts a fully autonomous positioning method, and can be conveniently used for pedestrian positioning and track tracking; and in the same hardware solution, an improved The algorithm structure does not depend on the average walking step length of a specific person, and the calculation amount of the algorithm is small, which can meet the requirements of most applications.

Using the algorithm of merging the heading angle at the feature level, while effectively improving the heading angle drift, it does not affect the calculation accuracy of the pitch angle and roll angle, and uses the zero-speed judgment result to feed back to the AHRS attitude calculation to avoid acceleration In the case of external force interference, it is still used to correct the zero drift of the gyroscope, thereby further improving the accuracy of the attitude angle calculation.

Using the zero-speed correction method, not only the second smoothing estimation is made for the horizontal speed, but also the second smoothing estimation of the variable parameters is made for the vertical direction using the walking speed feature, which further improves the three-dimensional positioning accuracy of the device.

It is easy for those skilled in the art to understand that the above descriptions are only preferred embodiments of the present invention, and are not intended to limit the present invention. Any modifications, equivalent replacements and improvements made within the spirit and principles of the present invention, All should be included within the protection scope of the present invention.

Claims (9)

1. A pedestrian positioning and trajectory tracking method, characterized in that, comprising: (1) Receive the acceleration information and angular velocity information of pedestrians, perform filtering and threshold segmentation on the acceleration information and angular velocity information, and obtain the velocity status information. When the velocity is not 0, use the angular velocity information to obtain the attitude information, and use the acceleration information to correct the angular velocity information when the velocity is 0. Get posture information; (2) Receive the magnetometer information, perform ellipse correction on the magnetometer, and then use the attitude information to perform inclination compensation to obtain the magnetic heading angle; (3) The magnetic course angle and the course angle in the attitude information are fused to obtain the fused course angle, combined with the attitude information and the fused course angle, the velocity information is obtained by integrating the acceleration, and after the velocity information is corrected by zero speed, it is integrated again to obtain the The displacement and height in the vertical direction, and finally the pedestrian positioning and trajectory tracking are performed through the displacement and height. 2. A kind of pedestrian location and track tracking method as claimed in claim 1, is characterized in that, described step (1) comprises: (1-1) Carry out filtering and threshold value segmentation to acceleration information and angular velocity information, obtain velocity state information; (1-2) When the speed is not 0, use the angular velocity information to update the quaternion, and use the quaternion to obtain the attitude information; when the speed is 0, use the proportional integral, use the acceleration information to correct the drift of the angular velocity information, and use the corrected angular velocity information to update the four arity, and then get the pose information. 3. A kind of pedestrian location and track tracking method as claimed in claim 1, is characterized in that, described fused course angle is: <mfenced open = "{" close = ""><mtable><mtr><mtd><mrow><msub><mi>E</mi><mi>i</mi></msub><mo>=</mo><msub><mi>c</mi><mi>i</mi></msub><mo>-</mo><msub><mi>h</mi><mi>i</mi></msub></mrow></mtd></mtr><mtr><mtd><mrow><msub><mi>h</mi><mi>i</mi></msub><mo>=</mo><msub><mi>y</mi><mi>i</mi></msub><mo>+</mo><msub><mi>E</mi><mi>i</mi></msub><mo>&amp;times;</mo><msub><mi>k</mi><mi>P</mi></msub><mo>+</mo><munderover><mi>&amp;Sigma;</mi><mrow><mi>i</mi><mo>=</mo><mn>0</mn></mi>mrow><mi>n</mi></munderover><msub><mi>E</mi><mi>i</mi></msub><mo>&amp;times;</mo><msub><mi>k</mi><mi>I</mi></msub></mrow></mtd></mtr></mtable></mfenced> Among them, 0≤i≤n, c _i is the magnetic heading angle at time i, y _i is the heading angle in the attitude information at time i, h _i is the fusion heading angle at time i, E _i is the heading angle deviation at time i Shift, k _P is the proportional coefficient, and k _I is the integral coefficient. 4. A kind of pedestrian location and trajectory tracking method as claimed in claim 1, is characterized in that, described step (3) comprises: (3-1) Fusion of the magnetic heading angle and the heading angle in the attitude information to obtain the fusion heading angle; (3-2) After the speed information is corrected by zero speed, the displacement in the horizontal direction can be directly integrated by the speed in the horizontal direction, and the speed in the vertical direction needs to be further judged by going up and down the stairs. After height compensation, the displacement in the vertical direction can be obtained high; (3-3) Use the displacement in the horizontal direction and the height in the vertical direction to locate and track pedestrians. 5. A pedestrian positioning and trajectory tracking system, comprising: accelerometer, gyroscope, magnetometer and control module, The accelerometer is used to receive the acceleration information of the pedestrian and transmit it to the control module; The gyroscope is used to receive the angular velocity information of the pedestrian and transmit it to the control module; The magnetometer is used to receive the magnetometer information of the pedestrian and transmit it to the control module; The control module is used to obtain speed state information and attitude information by using acceleration information and angular velocity information, obtain magnetic heading angle by using magnetometer information, fuse magnetic heading angle and heading angle in attitude information to obtain fusion heading angle, use displacement and Altitude positioning and trajectory tracking of pedestrians. 6. A pedestrian positioning and trajectory tracking system according to claim 5, wherein the control module includes a gait submodule, a fusion submodule and a navigation submodule. 7. A kind of pedestrian positioning and trajectory tracking system as claimed in claim 6, wherein the gait sub-module is used for filtering and thresholding the acceleration information and the angular velocity information to obtain the velocity state information, and by analyzing the acceleration Integrate to obtain velocity information. After zero-speed correction, the displacement in the horizontal direction can be directly integrated from the velocity in the horizontal direction. The velocity in the vertical direction needs to be further judged by going up and down the stairs. After height compensation, the height in the vertical direction can be obtained. 8. A pedestrian positioning and trajectory tracking system according to claim 6, wherein the fusion sub-module is used to obtain attitude information by using angular velocity information when the speed is not zero, and to correct angular velocity information by using acceleration information when the speed is 0 Then obtain attitude information, perform ellipse correction and inclination compensation on the magnetometer to obtain the magnetic heading angle, and fuse the magnetic heading angle and the heading angle in the attitude information to obtain the fused heading angle. 9. A pedestrian positioning and trajectory tracking system according to claim 6, wherein the navigation sub-module is used for pedestrian positioning and trajectory tracking by using displacement and height.