Inertial navigation system drift suppression method based on distance constraint

Abstract

The invention discloses an inertial navigation system drift suppression method based on distance constraint. The method comprises the following steps: step 1, data acquisition; step 2, coordinate transformation; 3, calculating an initial distance; step 4, UWB module distance measurement; step 5, data filtering; step 6, position calculation; 7, converting the coordinates back to the longitude and latitude; and step 8, result feedback. The inertial navigation system drift suppression method based on distance constraint provided by the invention has the beneficial effects that the position of the inertial navigation system is corrected in real time by using distance measurement between equipment so as to suppress the inertial navigation position drift, and high-precision inertial navigation and distance measurement equipment are utilized; and the drift of the low-precision inertial navigation is effectively inhibited.

Description

Technical Field

[0001] The present invention relates to a method for suppressing the drift of an inertial navigation system, and particularly to a method for suppressing the drift of an inertial navigation system based on distance constraint. Background Art

[0002] At present, in complex electromagnetic environments and building shielding scenarios, traditional satellite positioning technologies face significant challenges: The global navigation satellite system (GNSS) includes systems such as the Global Positioning System (GPS), the Beidou satellite navigation system (BDS), the Galileo satellite positioning system, and the GLONASS positioning system. Its positioning performance will seriously degrade in strong shielding outdoor environments such as tunnels and dense forests and indoor scenarios with significant multipath effects. Against this background, the inertial navigation system (INS) based on the inertial measurement unit (IMU) has become an important technical path to solve the positioning problem in complex environments due to its environmental universality, strong scene adaptability, and cost-effective advantages.

[0003] The inertial navigation system is a navigation method based on the inertial measurement principle, which derives the position, velocity, and direction information of a moving object by measuring and integrating physical quantities such as acceleration and angular velocity. It does not rely on external reference points, so it has the unique advantages of good concealment and being unaffected by external electromagnetic interference. The inertial navigation system processes the motion parameters output by the accelerometer and gyroscope through double integration, resulting in an error accumulation effect in pose solution, and its trajectory estimation accuracy shows a divergent characteristic with the increase of the motion duration. The current inertial navigation error correction system mainly includes the following methods: One is the error compensation mechanism based on kinematic modeling, which uses zero velocity update (ZUPT) and motion constraints to reset the velocity error in a stationary or regular motion state; the other uses a multi-source information fusion strategy, combining multi-sensor fusion technologies (such as GNSS, photoelectric positioning), and using external high-precision observation data to correct the accumulated error of the inertial navigation. It should be noted that the method based on motion feature modeling has significant application limitations - rigid motion carriers such as vehicles and unmanned aerial vehicles lack periodic stationary features similar to pedestrian gaits and it is difficult to build a universal motion model.

[0004] However, neither the error compensation mechanism based on kinematic modeling nor the multi-source information fusion strategy can completely meet people's requirements for high-precision positioning. Therefore, the work of suppressing inertial navigation drift needs further research. Based on this, this patent proposes an algorithm for suppressing inertial navigation drift using ultra-wideband ranging technology to achieve dynamic correction of the errors of the inertial navigation system. Summary of the Invention

[0005] The main object of the present invention is to solve the problem that the existing error compensation mechanism based on kinematic modeling and the multi-source information fusion strategy cannot completely meet people's requirements for high-precision positioning;

[0006] Another object of the present invention is to achieve dynamic correction of the errors of an inertial navigation system;

[0007] In order to achieve the above object and solve the above problems, the present invention provides a method for suppressing the drift of an inertial navigation system based on distance constraint.

[0008] The method for suppressing the drift of an inertial navigation system based on distance constraint provided by the present invention includes the following steps:

[0009] Assemble device A and device B as two mobile devices. Device A is equipped with a global satellite navigation system GNSS, a high-precision inertial navigation system, and an ultra-wideband UWB ranging terminal, and can obtain the longitude, latitude, and elevation X of device A in real time A (x A , y A , z A ); Device B is equipped with an inertial navigation system that needs to suppress drift and an ultra-wideband ranging terminal. The inertial navigation system on device B can obtain the three-axis acceleration, yaw angle, pitch angle, and roll angle of device B in real time, and obtain the real-time longitude, latitude, and elevation X of two points by performing double integration on the acceleration B (x B , y B , z B ), and device A and device B can measure the distance d between AB in real time through the ranging terminal;

[0010] Correct the inertial navigation data of point B through the measured distance d between device A and device B to obtain the calibrated position X of device B B' (x B' , y B' , z B' ), and the formula is as follows:

[0011]

[0012] The specific steps are as follows:

[0013] Step 1, data acquisition: Device A reads the real-time longitude and latitude information through the UART serial port, and device B acquires the longitude and latitude data of the inertial navigation system.

[0014] Step 2, coordinate conversion: Convert the longitude and latitude information of device A and device B into coordinates in the CGCS2000 plane coordinate system.

[0015] Step 3, initial distance calculation: Use the converted coordinates and substitute them into the distance formula to calculate the likelihood distance |AB| between device A and device B.

[0016] Step 4, UWB module ranging: Use the UWB ranging terminal between device A and device B to measure the distance between AB. The ranging is initiated by device B to obtain real-time distance information at a frequency of 2 Hz;

[0017] Step 5, Data filtering: Apply Kalman filtering to the measured distance d to reduce measurement noise and improve the accuracy of the distance measurement d;

[0018] Step 6, Position calculation: According to the filtered distance information, combined with the likelihood distance |AB|, use formula (1) to calculate the calibrated position of device B;

[0019] Step 7, Coordinate conversion back to longitude and latitude: Convert the calculated calibrated position of device B from the CGCS2000 plane coordinate system back to the longitude and latitude format;

[0020] Step 8, Result feedback: Feed back the calibrated position information of device B to the system or the user interface.

[0021] Advantages of the present invention:

[0022] The method for suppressing the drift of an inertial navigation system based on distance constraint provided by the present invention uses the distance measurement between devices to perform real-time correction on the position of the inertial navigation system to suppress the drift of the inertial navigation position. The present invention utilizes a high-precision inertial navigation and a distance measurement device, effectively suppressing the drift of the low-precision inertial navigation. Brief description of the drawings

[0023] Figure 1 It is a schematic diagram of the set positions of device A and device B described in the present invention.

[0024] Figure 2 It is a schematic flow diagram of the method for suppressing the drift of the inertial navigation system described in the present invention. Detailed implementation manners

[0025] Please refer to Figures 1 to 2 as shown:

[0026] The method for suppressing the drift of an inertial navigation system based on distance constraint provided by the present invention includes the following steps:

[0027] Assemble device A and device B as two mobile devices. Among them, device A is equipped with a global satellite navigation system GNSS, a high-precision inertial navigation system, and an ultra-wideband UWB ranging terminal, and can obtain the longitude, latitude, and elevation X of device A in real time A (x A ,y A ,z A); Device B is equipped with an inertial navigation system that requires drift suppression and an ultra-wideband ranging terminal. The inertial navigation system on Device B can obtain the three-axis acceleration, yaw angle, pitch angle, and roll angle of Device B in real time, and obtain the real-time longitude, latitude, and elevation X of two points by performing double integration on the acceleration. B (x B ,y B ,z B ), and Devices A and B can measure the distance d between A and B in real time through the ranging terminal.

[0028] The inertial navigation data of point B is corrected by the measured distance d between Devices A and B to obtain the calibrated position X of Device B. B' (x B' ,y B' ,z B' ), and the formula is as follows:

[0029]

[0030] The specific steps are as follows:

[0031] Step 1, Data acquisition: Device A reads the real-time longitude and latitude information through the UART serial port, and Device B collects the longitude and latitude data of the inertial navigation system.

[0032] Step 2, Coordinate transformation: The longitude and latitude information of Devices A and B is transformed into the coordinates in the CGCS2000 plane coordinate system.

[0033] Step 3, Initial distance calculation: Using the transformed coordinates, substitute them into the distance formula to calculate the likelihood distance |AB| between Device A and Device B.

[0034] Step 4, UWB module ranging: Use the UWB ranging terminal between Devices A and B to measure the distance between A and B. The ranging is initiated by Device B to obtain the real-time distance information, with a frequency of 2 Hz.

[0035] Step 5, Data filtering: Apply Kalman filtering to the measured distance d to reduce the measurement noise and improve the accuracy of the measured distance d.

[0036] Step 6, Position calculation: According to the filtered distance information, combined with the likelihood distance |AB|, use formula (1) to calculate the calibrated position of Device B.

[0037] Step 7, Coordinate transformation back to longitude and latitude: Transform the calculated calibrated position of Device B from the CGCS2000 plane coordinate system back to the longitude and latitude format.

[0038] Step 8, Result feedback: Feed back the calibrated position information of Device B to the system or the user interface.

Claims

1. A method for suppressing drift of an inertial navigation system based on distance constraints, characterized in that: The method comprises the following steps: Device A and device B are two mobile devices, where device A is equipped with a global satellite navigation system GNSS, a high-precision inertial navigation system and an ultra-wideband UWB ranging terminal, which can obtain the latitude, longitude and altitude of device A in real time. A (x A ,y A ,z A ); Device B is equipped with an inertial navigation system that needs to be drift-suppressed and an ultra-wideband ranging terminal. The inertial navigation system on device B can obtain the three-axis acceleration, yaw angle, pitch angle and roll angle of device B in real time, and obtain the real-time latitude and longitude and altitude of two points by double integration of acceleration. B (x B ,y B ,z B ), device A and device B can measure the distance d between AB in real time through the ranging terminal; The inertial navigation data of point B is corrected by the measured distance d between device A and device B to obtain the calibration position X of device B. B' (x B' ,y B' ,z B' ), the formula is as follows: The specific steps are as follows: Step 1, data collection: Device A reads the real-time longitude and latitude information through the UART serial port, and device B collects the longitude and latitude data of the inertial navigation system; Step 2, coordinate conversion: convert the latitude and longitude information of device A and device B into the coordinates of the CGCS2000 plane coordinate system; Step 3: Initial distance calculation: Use the converted coordinates and substitute them into the distance formula to calculate the likelihood distance |AB| between device A and device B. Step 4, UWB module ranging: Use the UWB ranging terminal between device A and device B to measure the distance between AB. The ranging is initiated by device B to obtain real-time distance information at a frequency of 2Hz. Step 5, data filtering: Apply Kalman filtering on the measured distance d to reduce measurement noise and improve the accuracy of the distance measurement d; Step 6, position calculation: Based on the filtered distance information and the likelihood distance |AB|, the calibration position of device B is calculated using formula (1); Step 7, coordinate conversion back to longitude and latitude: Convert the calculated calibration position of device B from the CGCS2000 plane coordinate system back to longitude and latitude format; Step 8: Feedback the result: Feedback the calibration position information of device B to the system or user interface.

Images