High-precision real-time track positioning method and system and computer readable storage medium

Abstract

The invention discloses a high-precision real-time track positioning method and system and a computer readable storage medium, and the method comprises the steps: firstly building a path digital model containing discrete high-precision azimuth angle information, carrying out dead reckoning through employing a low-cost IMU when a mobile carrier runs, and carrying out the dead reckoning through employing a low-cost IMU when the mobile carrier travels for a small fixed distance; and obtaining an accurate azimuth angle at the position from the digital model as an external observed quantity. Through a data fusion algorithm, continuous calculation of the IMU and discrete absolute azimuth information are optimally fused, and the real-time arc length position of the carrier on the path is accurately solved, and then the real-time arc length position is mapped into global coordinates. According to the method, the accumulative error of inertial navigation is effectively inhibited, continuous and high-precision track positioning which does not diverge along with time is realized at low cost, and the method is particularly suitable for scenes of water gate opening and closing, rail transit, AGV (Automatic Guided Vehicle) and the like running along a fixed path.

Description

Technical Field

[0001] This invention relates to the field of navigation and positioning technology for mobile carriers, specifically to a high-precision real-time trajectory positioning method, system, and computer-readable storage medium. Background Technology

[0002] In the navigation of mobile carriers such as rail transit, automated guided vehicles (AGVs), and industrial robots that travel along fixed paths, high-precision real-time positioning is a core requirement for ensuring stable operation and precise control of these carriers. Its performance directly affects the operational efficiency and safety of related equipment. With the rapid development of intelligent manufacturing and intelligent transportation, the comprehensive requirements for positioning technology in terms of cost, accuracy, continuity, and reliability are increasing.

[0003] Currently, positioning solutions for mobile vehicles traveling along fixed paths can be mainly divided into the following three categories: The first category is positioning schemes based on high-precision inertial navigation systems (INS). This scheme uses tactical-grade or navigation-grade inertial measurement units (IMUs) to calculate the position and attitude of the vehicle by integrating the collected angular velocity and acceleration data. Its advantages are that it does not rely on external environmental signals and has strong anti-interference capabilities, but it has significant drawbacks: on the one hand, the hardware cost of tactical-grade or navigation-grade IMUs is extremely high, significantly increasing the deployment threshold of the system; on the other hand, the positioning error of inertial navigation accumulates quadratically or cubically over time, and the error will diverge severely after long-term operation, making it unable to meet the positioning requirements of long-term continuous operation.

[0004] The second category is based on continuous absolute positioning, with typical technologies including Real-time Dynamic Differential (RTK) and Ultra-Wideband (UWB) systems from Global Navigation Satellite Systems (GNSS). These solutions achieve continuous positioning by receiving satellite signals or deploying dedicated base stations, providing a certain level of positioning accuracy in open environments or scenarios with good base station coverage. However, their limitations are significant: in complex environments such as indoors, tunnels, and densely built-up areas, satellite signals are easily blocked, and technologies like UWB face multipath interference, leading to a substantial decrease in positioning stability and reliability. Furthermore, the deployment, debugging, and maintenance of dedicated base stations require substantial human and material resources, further increasing the overall system cost.

[0005] The third category is based on discrete beacon positioning schemes, such as magnetic nails, QR codes, and RFID. This scheme deploys discrete beacon nodes along the path. When the vehicle reaches a beacon location, it acquires absolute position information, and trajectory calculations between points rely on odometers or low-cost IMUs. Its drawback is that it can only provide absolute positioning at specific beacon locations, unable to output continuous trajectory information, making it difficult to meet the vehicle's real-time control requirements for continuous position feedback. Furthermore, within the beacon intervals, errors generated during the calculation process accumulate, leading to a gradual decrease in positioning accuracy, making it impossible to achieve high-precision positioning throughout the entire journey.

[0006] In summary, existing positioning technologies generally suffer from the difficulty of simultaneously achieving cost, accuracy, continuity, and reliability. In particular, how to utilize low-cost sensors to achieve continuous, stable positioning with accuracy that does not drift over time under known path constraints has become a pressing technical problem to be solved in this field. Summary of the Invention

[0007] The purpose of this section is to outline some aspects of the embodiments of the present invention and to briefly describe some preferred embodiments. Simplifications or omissions may be made in this section, as well as in the abstract and title of this application, to avoid obscuring the purpose of these documents; however, such simplifications or omissions should not be construed as limiting the scope of the invention.

[0008] Therefore, the purpose of this invention is to provide a high-precision real-time trajectory positioning method, system, and computer-readable storage medium to solve the problems mentioned in the background art.

[0009] To address the aforementioned technical problems, according to one aspect of the present invention, the present invention provides the following technical solution: A high-precision real-time trajectory positioning method, comprising the following steps: S1. Construct and store a digital model of a known path, which includes at least a continuous geometric curve of the path centerline and discrete path point information serialized at fixed intervals on the curve. S2. When the mobile carrier is stationary at the starting point of the path, the system is initialized, the current position of the carrier is aligned with the starting arc length value in the digital model of the path, and the initial heading is loaded. S3. During the operation of the carrier, the angular velocity and acceleration data of the on-board inertial measurement unit are collected in real time at a preset frequency; S4. Use the collected real-time angular velocity and acceleration data to perform inertial dead reckoning and obtain the predicted motion state of the carrier. S5. When the carrier moves close to the discrete distribution point, the precise azimuth angle of the corresponding point is obtained from the digital model as an observation. S6. By integrating the predicted motion state with the precise azimuth angle observation, the precise arc length position of the carrier on the path is solved by the estimation algorithm. S7. Based on the precise arc length position and the digital model, output the real-time trajectory coordinates of the carrier in the global coordinate system.

[0010] As a preferred embodiment of the high-precision real-time trajectory positioning method of the present invention, each path point information includes at least its arc length value on the path curve, the corresponding two-dimensional or three-dimensional coordinates in the global coordinate system, and the azimuth angle value of the path tangent at that point, wherein the accuracy of the azimuth angle value is less than 0.2°.

[0011] In a preferred embodiment of the high-precision real-time trajectory positioning method of the present invention, in step S1, the preset fixed interval is 5-20 cm.

[0012] In a preferred embodiment of the high-precision real-time trajectory positioning method of the present invention, in step S3, the preset frequency is ≥10Hz.

[0013] In a preferred embodiment of the high-precision real-time trajectory positioning method of the present invention, step S4 includes predicting the motion state of the carrier, the change in heading angle, and the predicted value of the current heading.

[0014] As a preferred embodiment of the high-precision real-time trajectory positioning method of the present invention, in step S5, when the carrier moves close to the discrete distribution point, the specific steps for obtaining the precise azimuth angle of the corresponding point from the digital model as an observation are as follows: based on the cumulative displacement increment of the carrier, determine whether the carrier has moved to a position close to the next discrete path point; when the determination is yes, obtain the azimuth angle of the current nearest path point from the digital model as a high-precision azimuth angle observation.

[0015] As a preferred embodiment of the high-precision real-time trajectory positioning method of the present invention, in step S6, the specific steps of fusing the predicted motion state and the precise azimuth angle observation to solve the precise arc length position of the carrier on the path by the estimation algorithm are as follows: the heading and position information predicted by inertial dead reckoning are fused with the high-precision azimuth angle observation, and the precise arc length, along-track speed, and sensor error bias of the on-board inertial measurement unit of the carrier on the path curve are optimally estimated by the estimation algorithm.

[0016] As a preferred embodiment of the high-precision real-time trajectory positioning method of the present invention, the specific steps of outputting the real-time trajectory coordinates of the carrier in the global coordinate system in step S7, based on the precise arc length position and combined with the digital model, are as follows: based on the precise arc length, interpolation calculation is performed in the path digital model to output the high-precision position coordinates and attitude of the carrier in the global coordinate system in real time, forming a continuous motion trajectory.

[0017] A system for implementing a high-precision real-time trajectory positioning method, comprising: The path database module constructs and stores a digital model of a known path, which includes at least a continuous geometric curve of the path centerline and discrete path point information serialized at fixed intervals on the curve. An inertial measurement unit, mounted on a moving vehicle, is used to measure the angular velocity and acceleration of the vehicle in real time. The data processing and fusion module is configured to execute steps S3-S7 in a high-precision real-time trajectory positioning method to achieve inertial estimation, data fusion and trajectory calculation. The output module outputs the real-time trajectory coordinates of the carrier in the global coordinate system.

[0018] A computer-readable storage medium having a computer program stored thereon that, when executed by a processor, implements a high-precision real-time trajectory positioning method.

[0019] Compared with the prior art, the beneficial effects of the present invention are: 1. By introducing discrete but absolutely accurate azimuth angles on a known path as observations, the heading drift of the IMU can be continuously corrected at fixed intervals, fundamentally suppressing the quadratic or cubic increase of inertial navigation error over time, so that the overall positioning accuracy does not deteriorate with the increase of operating distance or time.

[0020] 2. It does not require expensive navigation-grade IMUs or densely deployed continuous absolute positioning base stations (such as UWB). It only requires low-cost MEMS IMUs and pre-mapped digital maps of the path, which greatly reduces the system hardware cost and deployment complexity. It does not rely on external wireless signals (such as GNSS) and has strong anti-interference capabilities. At the same time, it provides continuous position output rather than discrete point positioning, which better meets the continuity requirements of control and navigation.

[0021] 3. The fusion algorithm has low computational load and can achieve high-frequency (≥100Hz) trajectory updates on low-power processors, meeting the real-time control requirements of high-speed mobile carriers. Attached Figure Description

[0022] To more clearly illustrate the technical solutions of the embodiments of the present invention, the present invention will be described in detail below with reference to the accompanying drawings and detailed embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort. Wherein: Figure 1 This is a flowchart of a high-precision real-time trajectory positioning method according to the present invention; Figure 2 This is a schematic diagram of the principle of a high-precision real-time trajectory positioning system of the present invention. Detailed Implementation

[0023] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0024] Secondly, the present invention is described in detail with reference to the schematic diagrams. When detailing the embodiments of the present invention, for ease of explanation, the cross-sectional views illustrating the device structure may be partially enlarged, not according to the usual scale. Furthermore, the schematic diagrams are merely examples and should not limit the scope of protection of the present invention. In addition, actual fabrication should include three-dimensional spatial dimensions of length, width, and depth.

[0025] To make the objectives, technical solutions, and advantages of the present invention clearer, the embodiments of the present invention will be described in further detail below with reference to the accompanying drawings.

[0026] This invention provides a high-precision real-time trajectory positioning method, such as... Figure 1 As shown, the specific steps of this high-precision real-time trajectory positioning method are as follows: S1. Construct and store a digital model of a known path. The model includes at least a continuous geometric curve of the path centerline and discrete path point information serialized on the curve at fixed intervals. Each path point information includes at least its arc length on the path curve, its corresponding two-dimensional or three-dimensional coordinates in the global coordinate system, and the azimuth angle of the path tangent at that point. The accuracy of the azimuth angle is less than 0.2°, and the fixed interval is 5-20 cm.

[0027] S2. When the mobile carrier is stationary at the starting point of the path, the system is initialized, the current position of the carrier is aligned with the starting arc length value in the digital model of the path, and the initial heading is loaded. S3. During the operation of the carrier, the angular velocity and acceleration data of the on-board inertial measurement unit are collected in real time at a frequency of ≥10Hz; S4. Using the collected real-time angular velocity and acceleration data, perform inertial dead reckoning to obtain the predicted motion state of the vehicle. The predicted motion state includes the displacement increment, heading angle change, and the predicted value of the current heading. S5. When the carrier moves close to the discrete distribution point, the precise azimuth angle of the corresponding point is obtained from the digital model as an observation. More specifically, based on the cumulative displacement increment of the carrier, it is determined whether the carrier has moved to a position close to the next discrete path point. When it is determined to be so, the azimuth angle of the current nearest path point is obtained from the digital model as a high-precision azimuth angle observation. S6. By fusing the predicted motion state with the precise azimuth angle observation, the precise arc length position of the carrier on the path is calculated using an estimation algorithm. More specifically, the specific steps for fusing the predicted motion state with the precise azimuth angle observation to calculate the precise arc length position of the carrier on the path are as follows: the heading and position information predicted by inertial dead reckoning are fused with the high-precision azimuth angle observation, and the precise arc length, along-track speed, and sensor error bias of the on-board inertial measurement unit of the carrier on the path curve are optimally estimated using an estimation algorithm. S7. Based on the precise arc length position and the digital model, output the real-time trajectory coordinates of the carrier in the global coordinate system. More specifically, based on the precise arc length, perform interpolation calculations in the digital path model to output the high-precision position coordinates and attitude of the carrier in the global coordinate system in real time, forming a continuous motion trajectory.

[0028] like Figure 2 As shown, the present invention also provides a system for implementing the above-described high-precision real-time trajectory positioning method, comprising: The path database module constructs and stores a digital model of a known path, which includes at least a continuous geometric curve of the path centerline and discrete path point information serialized at fixed intervals on the curve. An inertial measurement unit, mounted on a moving vehicle, is used to measure the angular velocity and acceleration of the vehicle in real time. The data processing and fusion module is configured to execute steps S3-S7 in a high-precision real-time trajectory positioning method to achieve inertial estimation, data fusion and trajectory calculation. The output module outputs the real-time trajectory coordinates of the carrier in the global coordinate system.

[0029] A computer-readable storage medium having a computer program stored thereon that, when executed by a processor, implements a high-precision real-time trajectory positioning method.

[0030] To verify the technical effects of the above-described solution of the present invention, the following specific embodiment is provided: 1. System Construction First, the AGV's running track is precisely mapped to obtain a parametric curve of its centerline. Points are taken along the curve at 10-centimeter intervals, and the arc length S, coordinates (X, Y), and tangent azimuth angle ψ (accuracy up to 0.1°) of each point are recorded to form a path database file. A low-cost six-axis MEMS IMU (such as IMU-406) is installed at the center of the AGV body, equipped with an embedded microprocessor (such as STM32F4 series or higher performance chip) as a data processing and fusion module.

[0031] 2. Operational Process The AGV is electrically powered and stationary at the starting point of the track. The system initializes by setting the current arc length to 0 and reading the starting point azimuth angle from the database as the initial heading ψ0.

[0032] The AGV starts running. The IMU outputs the triaxial angular velocity ω and triaxial acceleration a at a frequency of 200Hz.

[0033] The data processing module performs the following real-time loop: (1) Inertial prediction: Integrate ω and a, and combine with the state at the previous moment to predict the current heading ψ. pred and the displacement increment Δs along the track pred .

[0034] (2) Triggered observation: When the predicted total displacement reaches approximately 10 cm since the last observation, an azimuth observation is triggered. This is based on the currently predicted arc length S. pred Find the closest path point in the path database and read the precise azimuth angle ψ of that point. map .

[0035] (3) Fusion Update: Construct a system with arc length S, along-track velocity v, and heading error δ ψ IMU gyroscope zero bias b g For the error state Kalman filter (ESKF) of the state variables, ψ pred With ψ map The difference is used as the observation input filter. The filter performs an update step, optimally estimating the state correction, especially for accurate correction of arc length S and heading, and estimating the IMU's zero bias b online. g .

[0036] (4) Trajectory output: using the corrected optimal arc length estimate S opt Spline interpolation is performed on the curves in the path database to calculate the precise global coordinates (X, Y) of the AGV at the current moment. opt , Y opt) The corrected state is then fed back to the inertial prediction step at the next moment, forming a closed loop.

[0037] 3. Performance Effect Through the above process, the AGV's heading is calibrated with a 0.1° accuracy landmark every 10 centimeters it travels. Therefore, the IMU's own heading drift is strictly limited to the interval between two observations (approximately 10 centimeters). Tests have shown that, after repeated operation on a 200-meter track, the absolute position error compared to the true value measured by the laser tracker primarily depends on the coordinate accuracy of the path database (5 millimeters), while the repeatability error remains stable within 1 centimeter, with no divergence throughout. In contrast, if only the same IMU is used for pure inertial navigation, the position error can reach tens of meters after 2 minutes.

[0038] Although the present invention has been described above with reference to embodiments, various modifications can be made and components can be replaced with equivalents without departing from the scope of the invention. In particular, as long as there is no structural conflict, the features in the disclosed embodiments can be combined with each other in any manner. The lack of an exhaustive description of these combinations in this specification is merely for the sake of brevity and resource conservation. Therefore, the present invention is not limited to the specific embodiments disclosed herein, but includes all technical solutions falling within the scope of the claims.

Claims

1. A high-precision real-time trajectory positioning method, characterized in that, The steps are as follows: S1. Construct and store a digital model of a known path, which includes at least a continuous geometric curve of the path centerline and discrete path point information serialized at fixed intervals on the curve. S2. When the mobile carrier is stationary at the starting point of the path, the system is initialized, the current position of the carrier is aligned with the starting arc length value in the digital model of the path, and the initial heading is loaded. S3. During the operation of the carrier, the angular velocity and acceleration data of the on-board inertial measurement unit are collected in real time at a preset frequency; S4. Use the collected real-time angular velocity and acceleration data to perform inertial dead reckoning and obtain the predicted motion state of the carrier. S5. When the carrier moves close to the discrete distribution point, the precise azimuth angle of the corresponding point is obtained from the digital model as an observation. S6. By integrating the predicted motion state with the precise azimuth angle observation, the precise arc length position of the carrier on the path is solved by the estimation algorithm. S7. Based on the precise arc length position and the digital model, output the real-time trajectory coordinates of the carrier in the global coordinate system.

2. The high-precision real-time trajectory positioning method according to claim 1, characterized in that, Each path point information includes at least its arc length on the path curve, its corresponding two-dimensional or three-dimensional coordinates in the global coordinate system, and the azimuth angle of the path tangent at that point, wherein the accuracy of the azimuth angle is less than 0.2°.

3. The high-precision real-time trajectory positioning method according to claim 1, characterized in that, In step S1, the fixed interval is 5-20 centimeters.

4. The high-precision real-time trajectory positioning method according to claim 1, characterized in that, In step S3, the preset frequency is ≥10Hz.

5. The high-precision real-time trajectory positioning method according to claim 1, characterized in that, In step S4, the predicted motion state includes the displacement increment of the carrier, the change in heading angle, and the predicted value of the current heading.

6. The high-precision real-time trajectory positioning method according to claim 5, characterized in that, In step S5, when the carrier moves close to the discrete distribution point, the specific steps for obtaining the precise azimuth angle of the corresponding point from the digital model as an observation measure are as follows: based on the cumulative displacement increment of the carrier, determine whether the carrier has moved to a position close to the next discrete path point; when the determination is yes, obtain the azimuth angle of the current nearest path point from the digital model as a high-precision azimuth angle observation measure.

7. The high-precision real-time trajectory positioning method according to claim 1, characterized in that, In step S6, the specific steps for fusing the predicted motion state and the precise azimuth angle observation to solve the precise arc length position of the carrier on the path using an estimation algorithm are as follows: the heading and position information predicted by inertial dead reckoning are fused with the high-precision azimuth angle observation, and the precise arc length, along-track speed, and sensor error bias of the on-board inertial measurement unit of the carrier on the path curve are optimally estimated using an estimation algorithm.

8. The high-precision real-time trajectory positioning method according to claim 1, characterized in that, In step S7, the specific steps for outputting the real-time trajectory coordinates of the carrier in the global coordinate system based on the precise arc length position and the digital model are as follows: based on the precise arc length, interpolation calculation is performed in the digital path model to output the high-precision position coordinates and attitude of the carrier in the global coordinate system in real time, forming a continuous motion trajectory.

9. A system for implementing the high-precision real-time trajectory positioning method according to any one of claims 1-8, characterized in that, include: The path database module constructs and stores a digital model of a known path, which includes at least a continuous geometric curve of the path centerline and discrete path point information serialized at fixed intervals on the curve. An inertial measurement unit, mounted on a moving vehicle, is used to measure the angular velocity and acceleration of the vehicle in real time. The data processing and fusion module is configured to execute steps S3-S7 of the high-precision real-time trajectory positioning method according to any one of claims 1-8, to realize inertial estimation, data fusion and trajectory calculation; The output module outputs the real-time trajectory coordinates of the carrier in the global coordinate system.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements a high-precision real-time trajectory positioning method as described in any one of claims 1-8.

Images