Shoulder hanging type self-positioning method, device and system and storage medium

Abstract

The invention discloses a shoulder-hanging type autonomous positioning method, device and system and a storage medium, and the method comprises the steps: S1, obtaining pedestrian course data according to the three-axis acceleration of an inertial sensor and the measurement data of a gyroscope, the pedestrian course data comprising a speed matrix, a position matrix and a first course angle; wherein a first course angle is obtained based on measurement data of the gyroscope; s2, obtaining a second course angle according to measurement data of the three-axis magnetometer; and S3, correcting the accumulated error of the gyroscope by comparing the first course angle with the second course angle. By adopting the technical scheme of the invention, the stability and reliability of indoor navigation positioning are improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of indoor positioning, and particularly relates to a shoulder-mounted autonomous positioning method, device, system, and storage medium. Background Art

[0002] GNSS has been widely used in fields such as personnel positioning, vehicle navigation, map drawing, and climate monitoring. Indoors, affected by factors such as wall occlusion and multipath effects, the positioning accuracy drops sharply or even fails, unable to meet the seamless indoor and outdoor positioning requirements of personnel.

[0003] Pedestrian inertial positioning is autonomous. Its principle is based on an inertial navigation system (INS), using inertial sensors such as accelerometers and gyroscopes to estimate the speed and attitude of pedestrians. Usually, the inertial sensors can be placed on the feet or shoulders. However, the error accumulation of pure inertial positioning is serious, growing in proportion to the cube of time. Currently, the pedestrian inertial positioning mainly uses the foot PDR method, using the characteristic that the speed is zero when the foot touches the ground to constrain the accumulated error, that is, zero velocity update (ZUPT) is used to improve the inertial positioning accuracy. Domestic universities such as Beihang University and Wuhan University and other research institutes have made representative achievements in this regard. For shoulder-mounted inertial positioning, since the shoulders are mostly moving during walking and there is no periodic zero-velocity state similar to that of the feet, shoulder-mounted inertial positioning cannot use ZUPT to suppress error growth like foot-mounted inertial navigation, and the positioning diverges rapidly.

[0004] The geomagnetic field exists widely, and the geomagnetic heading has no accumulated error and can be used to correct shoulder-mounted inertial positioning. Indoors, the geomagnetic field is easily affected by environmental steel structures and iron products, etc., and the geomagnetic heading error is large. How to fuse the data of magnetometers and gyroscopes to reduce the heading angle error is of great significance for improving shoulder-mounted inertial positioning. Summary of the Invention

[0005] The technical problem to be solved by the present invention is to provide a shoulder-mounted autonomous positioning method, device, system, and storage medium.

[0006] To achieve the above object, the present invention adopts the following technical solutions:

[0007] A shoulder-mounted autonomous positioning method includes:

[0008] Step S1: Obtain pedestrian heading data according to the three-axis acceleration of the inertial sensor and the measurement data of the gyroscope. The pedestrian heading data includes: a velocity matrix, a position matrix, and a first heading angle; wherein, the first heading angle is obtained based on the measurement data of the gyroscope;

[0009] Step S2: Obtain a second heading angle according to the measurement data of the three-axis magnetometer;

[0010] Step S3: Correct the cumulative error of the gyroscope by comparing the first heading angle and the second heading angle.

[0011] Preferably, in step S2, the calculation formula for the second heading angle is:

[0012]

[0013] where are the magnetic field intensities of the x, y, and z axes of the inertial sensor in the b system at time k, respectively, are the magnetic field intensities in the x and y directions in the n system at time k, respectively, and ψ mk is the second heading angle obtained from the magnetometer data at time k; θ k is the pitch angle at time k, and φ k is the roll angle at time k.

[0014] Preferably, in step S3, fuse the first heading angle and the second heading angle according to the factor graph to correct the cumulative error of the gyroscope.

[0015] The present invention also provides a shoulder-mounted autonomous positioning device, including:

[0016] A first calculation module, configured to obtain pedestrian heading data according to the triaxial acceleration of the inertial sensor and the measurement data of the gyroscope, where the pedestrian heading data includes: a velocity matrix, a position matrix, and a first heading angle; among them, the first heading angle is obtained based on the measurement data of the gyroscope;

[0017] A second calculation module, configured to obtain a second heading angle according to the measurement data of the triaxial magnetometer;

[0018] A third calculation module, configured to correct the cumulative error of the gyroscope by comparing the first heading angle and the second heading angle.

[0019] Preferably, the calculation formula for the second heading angle of the second calculation module is:

[0020]

[0021] where are the magnetic field intensities of the x, y, and z axes of the inertial sensor in the b system at time k, respectively, are the magnetic field intensities in the x and y directions in the n system at time k, respectively, and ψ mk is the second heading angle obtained from the magnetometer data at time k; θ k is the pitch angle at time k, and φ k is the roll angle at time k.

[0022] Preferably, the third calculation module is configured to fuse the first heading angle and the second heading angle according to the factor graph to correct the cumulative error of the gyroscope.

[0023] A computer program run by the processor is stored on the memory, and the computer program executes a shoulder-mounted autonomous positioning method when run by the processor.

[0024] An embodiment of the present invention also provides a storage medium, on which a computer program is stored, and the computer program executes a shoulder-mounted autonomous positioning method when running.

[0025] The present invention has the following beneficial effects:

[0026] 1. Improve the heading angle accuracy of shoulder-mounted inertial pedestrian navigation: By combining the data of the magnetometer and the gyroscope, the advantages that the magnetometer has no cumulative error and the gyroscope is relatively accurate in a short time are fully utilized, and the problem of rapid divergence of the heading angle in shoulder-mounted navigation is solved, so that the heading angle accuracy is greatly improved.

[0027] 2. Greatly improve the accuracy of shoulder-mounted inertial pedestrian autonomous positioning: By correcting the error of the heading angle through an algorithm with low complexity, the drift of the heading angle is greatly reduced, and on the premise of meeting low power consumption, the accuracy of shoulder-mounted inertial pedestrian autonomous positioning is significantly improved.

[0028] 3. Improve the stability and reliability of pedestrian position services: By combining the data of the magnetometer and the gyroscope, the disadvantages that the magnetometer is inaccurate indoors and the gyroscope has a large cumulative error are effectively avoided, the stability and reliability of indoor navigation positioning are improved, and a more accurate and reliable indoor-outdoor seamless navigation service is provided for users. Description of the Drawings

[0029] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the drawings in the following description are only the embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained according to the provided drawings without creative efforts.

[0030] Figure 1 It is a flowchart of the shoulder-mounted autonomous positioning method according to an embodiment of the present invention;

[0031] Figure 2 It is a heading angle diagram of shoulder-mounted inertial pedestrian autonomous positioning based on inertial sensors provided by an embodiment of the present invention;

[0032] Figure 3 It is a trajectory diagram of shoulder-mounted inertial pedestrian autonomous positioning based on inertial sensors provided by an embodiment of the present invention. Detailed Embodiments

[0033] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0034] To make the above objects, features, and advantages of the present invention more obvious and understandable, the present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments.

[0035] Embodiment 1:

[0036] As Figure 1 shown, an embodiment of a shoulder-mounted autonomous positioning method of the present invention includes:

[0037] Step S1: Obtain pedestrian heading data based on the measurement data of the three-axis acceleration of the inertial sensor and the gyroscope. The pedestrian heading data includes: a velocity matrix, a position matrix, and a first heading angle; wherein, the first heading angle is obtained based on the measurement data of the gyroscope.

[0038] Step S2: Obtain a second heading angle based on the measurement data of the three-axis magnetometer.

[0039] Step S3: Correct the cumulative error of the gyroscope by comparing the first heading angle and the second heading angle.

[0040] Further, in step S1, the original data of the accelerometer and the gyroscope output by the shoulder-mounted inertial sensor in the carrier coordinate system (b system) at time k are respectively denoted as and

[0041] In the step S1, the specific method for obtaining the pedestrian position data is as follows:

[0042] Define b as the carrier coordinate system and n as the navigation coordinate system;

[0043] Construct a rotation matrix from the b system to the n system Satisfy:

[0044]

[0045] Combined with the sampling of the inertial sensor at time k, the output of the inertial navigation system at time k is calculated by the following formula:

[0046]

[0047] Wherein, and respectively represent the rotation matrix, velocity matrix, and position matrix at time k, Denotes the acceleration at time k in the n - frame, [·] × is the skew - symmetric matrix of the vector, T is the sampling period, ψ gk is the first heading angle obtained from the gyroscope data at time k.

[0048] Furthermore, the acceleration in the n - frame is calculated by the following formula:

[0049]

[0050] where g n is the compensation value of the gravity component in the navigation coordinate system.

[0051] As an implementation manner of the embodiment of the present invention, further, in step S2, geomagnetic calibration is performed on the magnetometer data to eliminate the interference of the hard - iron effect and the soft - iron effect on the magnetic field, and the second heading angle is obtained according to the acceleration data and the calibrated magnetometer data. Specifically:

[0052] The pitch angle and the roll angle are obtained according to the acceleration data at time k. The specific method is as follows:

[0053]

[0054]

[0055] where are the accelerations of the x, y, and z axes of the inertial sensor in the b - frame at time k, θ k is the pitch angle at time k, φ k is the roll angle at time k.

[0056] According to the pitch angle and the roll angle at time k and the magnetometer data at time k, the second heading angle can be obtained. The specific method is as follows:

[0057]

[0058] where are the magnetic field intensities of the x, y, and z axes of the inertial sensor in the b - frame at time k, are the magnetic field intensities in the x and y directions in the n - frame at time k, ψ mk is the second heading angle obtained from the magnetometer data at time k. The obtained second heading angle is as Figure 2 shown, Figure 2 where the gyro - integrated heading is the first heading angle, and the corrected heading is the second heading angle.

[0059] As an implementation manner of an embodiment of the present invention, in step S3, the first heading angle and the second heading angle are fused according to the factor graph to correct the cumulative error of the gyroscope. Specifically as follows:

[0060] First, the initial magnetic heading angle is assigned as the initial value to the gyroscope heading angle, so that the heading angles obtained by the two methods are synchronized initially. After synchronizing the heading angles, it is more convenient to compare the heading angles.

[0061] By detecting the moment when the magnetic heading change is small, a relatively stable heading angle within a period of time is found, and the average values of the heading angles of the magnetometer and the gyroscope within this time window are recorded. Selecting a stable heading angle is to exclude the problem that the rapid change of the heading angle during the turning process of pedestrians affects the heading angle calibration result. Selecting a stable heading angle can obtain a better calibration result. The specific method is as follows:

[0062] max(ψ mk ,···,ψ mk+n ) - min(ψ mk ,···,ψ mk+n ) < σ1 (10)

[0063] When the magnetic heading angle within a certain period of time satisfies the above formula, it is considered that the change of the heading angle within this period of time is small, where n is the size of the time window and σ1 is the judgment threshold for the stable heading angle. At this time, record the average value of ψ g and ψ m , and use the average value to represent the heading angle within this time window:

[0064]

[0065] where are respectively the average value of the magnetic heading angle and the average value of the heading angle obtained from the gyroscope data within this time window, and store them into two arrays of the average values of the two heading angles for convenient use later, are respectively the arrays for recording the average value of the magnetic heading and the average value of the heading angle obtained from the gyroscope data.

[0066] When the next stable heading angle is detected, that is, when the formula (10) is satisfied, detect the next stable heading angle and judge whether its magnetic heading is close to the already recorded magnetic heading and whether it is close to a certain item in the array. The specific judgment method is as follows:

[0067]

[0068] where k represents the k - th detection of the stable heading angle, i represents the i - th element in the array, and σ2 represents the judgment threshold for the past recorded heading angle.

[0069] If the formula (13) is not satisfied, it means that the difference between the current heading angle and the previously recorded heading angle is large, that is, there is no approximate heading angle in the historical record. Then, calculate (11) and (12) repeatedly and store them in the two arrays of the average values of the heading angles.

[0070] If the formula (13) is satisfied, it means that the current heading angle is close to the previously recorded heading angle, and the cumulative error of the gyroscope can be corrected. The specific correction method is as follows:

[0071]

[0072] Among them, represents the heading angle obtained by factor graph fusion and . Δψ represents the error accumulated by the gyroscope integration from the previous correction to this correction, m represents the serial number of the previous correction, and n represents the serial number of this correction.

[0073] By obtaining the nine-axis data of the shoulder-mounted sensor, ψ g and ψ m are obtained respectively through the motion gyroscope integration algorithm and the magnetometer calculation model. Finally, the first heading angle and the second heading angle are fused by the factor graph to obtain a more accurate corrected heading angle, providing a better indoor and outdoor spatio-temporal positioning service for users. The specific effect is as Figure 3 shown. Compared with the original trajectory, the corrected trajectory has significantly reduced heading error and closed trajectory error.

[0074] The present invention makes full use of the advantages that the magnetometer has no cumulative error, the relative value of the indoor magnetic heading angle is relatively accurate, and the gyroscope integration is relatively accurate in a short time, and cleverly avoids the disadvantages of large indoor magnetic heading angle error and gyroscope error accumulation. The combination of the two kinds of data can provide more accurate indoor and outdoor autonomous positioning for users.

[0075] Embodiment 2:

[0076] The embodiment of the present invention also provides a shoulder-mounted autonomous positioning device, including:

[0077] A first calculation module, configured to obtain pedestrian heading data according to the three-axis acceleration of the inertial sensor and the measurement data of the gyroscope. The pedestrian heading data includes: a speed matrix, a position matrix, and a first heading angle; wherein, the first heading angle is obtained based on the measurement data of the gyroscope;

[0078] A second calculation module, configured to obtain a second heading angle according to the measurement data of the three-axis magnetometer;

[0079] A third calculation module corrects the cumulative error of the gyroscope by comparing the first heading angle and the second heading angle.

[0080] As an implementation manner of an embodiment of the present invention, the calculation formula of the second heading angle of the second calculation module is:

[0081]

[0082] Where are respectively the x, y, and z-axis magnetic field intensities of the inertial sensor at time k in the b system, are respectively the magnetic field intensities in the x and y directions at time k in the n system, ψ mk is the second heading angle obtained according to the magnetometer data at time k; θ k is the pitch angle at time k, φ k is the roll angle at time k.

[0083] As an implementation manner of an embodiment of the present invention, a third calculation module is used to fuse the first heading angle and the second heading angle according to the factor graph to correct the cumulative error of the gyroscope.

[0084] Embodiment 3:

[0085] An embodiment of the present invention further provides a shoulder-mounted autonomous positioning system, including: a memory and a processor, where a computer program run by the processor is stored on the memory, and the computer program executes the shoulder-mounted autonomous positioning method when run by the processor.

[0086] Embodiment 4:

[0087] An embodiment of the present invention further provides a storage medium, where a computer program is stored on the storage medium, and the computer program executes the shoulder-mounted autonomous positioning method when running.

[0088] The above-described embodiments are only descriptions of the preferred embodiments of the present invention, and do not limit the scope of the present invention. Without departing from the design spirit of the present invention, various deformations and improvements made by those of ordinary skill in the art to the technical solutions of the present invention shall fall within the protection scope determined by the claims of the present invention.

Claims

1. A shoulder-mounted autonomous positioning method, characterized in that, Including: Step S1: Obtain pedestrian heading data based on the triaxial acceleration of the inertial sensor and the measurement data of the gyroscope. The pedestrian heading data includes: a velocity matrix, a position matrix, and a first heading angle. Among them, the first heading angle is obtained based on the measurement data of the gyroscope. Step S2: Obtain a second heading angle according to the measurement data of the triaxial magnetometer. Step S3: Correct the cumulative error of the gyroscope by comparing the first heading angle and the second heading angle.

2. The shoulder-mounted autonomous positioning method according to claim 1, wherein In Step S2, the calculation formula for the second heading angle is: Among them, are the magnetic field intensities of the three axes of the inertial sensor in the c system at time l in the x, y, and z directions, respectively. are the magnetic field intensities in the x and y directions in the n system at time k, and ψ mk is the second heading angle obtained from the magnetometer data at time k; θ k is the pitch angle at time k, and φ k is the roll angle at time k.

3. The shoulder-mounted autonomous positioning method according to claim 2, wherein, In Step S3, fuse the first heading angle and the second heading angle according to the factor graph to correct the cumulative error of the gyroscope.

4. A shoulder-mounted autonomous positioning device, characterized in that, Including: A first calculation module for obtaining pedestrian heading data based on the triaxial acceleration of the inertial sensor and the measurement data of the gyroscope. The pedestrian heading data includes: a velocity matrix, a position matrix, and a first heading angle. Among them, the first heading angle is obtained based on the measurement data of the gyroscope. A second calculation module for obtaining a second heading angle according to the measurement data of the triaxial magnetometer. A third calculation module for correcting the cumulative error of the gyroscope by comparing the first heading angle and the second heading angle.

5. The shoulder-mounted autonomous positioning device according to claim 4, wherein The calculation formula for the second heading angle of the second calculation module is: Among them, are respectively the magnetic field intensities of the three axes of the inertial sensor in the b coordinate system at the k-th moment, are respectively the magnetic field intensities in the x and y directions in the n coordinate system at the k-th moment, and ψ mk is the second heading angle obtained from the magnetometer data at the k-th moment; θ k is the pitch angle at the k-th moment, and φ k is the roll angle at the k-th moment.

6. The shoulder-mounted autonomous positioning device as described in claim 5, wherein, The third calculation module fuses the first heading angle and the second heading angle according to the factor graph to correct the cumulative error of the gyroscope.

7. A shoulder-mounted autonomous positioning system, characterized in that, Including: A memory and a processor. A computer program is stored on the memory and run by the processor. When the computer program is run by the processor, it executes the shoulder-mounted autonomous positioning method according to any one of claims 1 to 3.

8. A storage medium, characterized in that, A computer program is stored on the storage medium. When the computer program runs, it executes the shoulder-mounted autonomous positioning method according to any one of claims 1 to 3.

Images