CN-121722122-B - AGV multi-source positioning fusion and navigation correction method and system

CN121722122BCN 121722122 BCN121722122 BCN 121722122BCN-121722122-B

Abstract

The invention belongs to the technical field of automatic guided vehicles, and particularly relates to a multi-source positioning fusion and navigation correction method and system for an AGV, wherein the method comprises the following steps: pose estimation is carried out through bidirectional coupling of an extended Kalman filter and a particle filter, wherein the particle filter adopts an adaptive observation likelihood model integrating visual confidence and dynamic interference estimation, and estimation uncertainty is fed back to the extended Kalman filter to dynamically adjust noise parameters of the extended Kalman filter. And further introducing sliding window factor graph optimization to carry out back-end smoothing. In the navigation stage, a dynamic decision maker based on deep reinforcement learning is adopted, and optimal control parameters are generated according to real-time positioning, map semantics and historical performance. The system can also enable the filtering model and the decision strategy to adapt to the current environment through periodical online fine adjustment. Classical state estimation and a leading edge machine learning method are integrated through innovative depth, and the positioning accuracy, environment understanding capability and navigation intelligence of the AGV in a complex dynamic scene are remarkably improved.

Inventors

ZHAO FENG
LUO TAO
LUO YUN

Assignees

莱仕力智能技术（深圳）有限公司

Dates

Publication Date: 20260508
Application Date: 20260211

Claims (8)

1. The multi-source positioning fusion and navigation correction method for the AGV is characterized by comprising the following steps of: S1, acquiring original data through an inertial measurement unit, a wheel speed encoder and a visual camera which are arranged on an AGV; S2, loading a pre-constructed topological map, wherein the map comprises nodes, edges, associated visual feature descriptors and path semantic information; s3, carrying out time synchronization and noise filtering pretreatment on the original data; S4, based on the preprocessed inertial measurement unit and wheel speed encoder data, fusing through an extended Kalman filter, and outputting a first predicted pose; S5, matching the current image acquired by the visual camera with visual features in the topological map, resolving the visual observation pose, and calculating the visual feature matching confidence coefficient of the current environment and the dynamic obstacle interference evaluation factor; The calculation formula of the visual feature matching confidence coefficient C vis is as follows: ; N match is the number of feature points successfully matched with visual features in the topological map, N map is the total number of visual feature points stored in a current reference node in the topological map, sigma d is the standard deviation of descriptor distances between all successfully matched feature point pairs, and delta is an adjustable parameter for adjusting the sensitivity of confidence level to descriptor difference; The dynamic obstacle interference evaluation factor D obs carries out dynamic object segmentation detection on the current frame image by calculating the distortion degree of the characteristic point optical flow field of the current frame and the previous frame or by a lightweight deep learning model, and calculates the proportion of the detected dynamic object area to the total area of the image; S6, initializing importance suggestion distribution of a particle filter by using a first predicted pose and a covariance matrix thereof, taking visual observation pose, confidence and interference assessment factors as observation input, calculating particle weight by using a self-adaptive observation likelihood model, and outputting the optimal estimated pose after resampling; The self-adaptive observation likelihood model calculation particle weight w i is as follows: ; Wherein z is the visual observation pose, For the observation space corresponding to the particle state x i , R adaptive is an adaptive observation noise covariance matrix: ; Wherein R 0 is a reference noise matrix, alpha and beta are adjustment coefficients, epsilon is a normal number for preventing zero removal; S7, inputting the optimal estimated pose into a navigation parameter dynamic decision maker based on deep reinforcement learning, and outputting the optimized navigation control parameters in real time according to the current pose, path tracking deviation, topological map semantics and historical performance data; and S8, performing navigation control, and collecting positioning error and path tracking error data in a time window as performance evaluation tuples, wherein the positioning error and path tracking error data are used for performing periodic on-line fine adjustment on the self-adaptive observation likelihood model in S6 and the deep reinforcement learning decision maker in S7.
2. The method according to claim 1, wherein in S6, the empirical covariance matrix P pf of the particle sets after resampling of the particle filters is fed back to the extended kalman filter, and is used for dynamically adjusting the process noise covariance matrix Q k at the next moment, and the adjustment rule is as follows: ; wherein Q base is a preset basic process noise covariance matrix, Representing the extraction of the column vector of P pf main diagonal elements, Representing a diagonal matrix of principal diagonal elements with the column vector, η is a learning rate factor between 0 and 1 that is adaptively adjusted based on a long term consistency error between the extended kalman filter predictions and the particle filter estimates.
3. The method of claim 1, further comprising a sliding window factor graph optimization back-end, wherein maintaining a fixed length historical pose sliding window, taking the optimal estimated pose output in step S6 as a factor node, and simultaneously fusing the IMU pre-integration constraint from step S4, the visual re-projection constraint from step S5, and the node position prior constraint from the topological map, to perform local beam adjustment optimization, further smoothing the trajectory and reducing the accumulated error.
4. The method of claim 1, wherein the depth reinforcement learning based navigation parameter dynamic determiner of S7 is a trained depth reinforcement learning network: The state space of the navigation parameter dynamic decision maker comprises the current optimal estimated pose, the lateral deviation and heading deviation between the actual track of the AGV and the planned path, semantic information codes of the front path edge obtained from the topological map and the current speed of the AGV; The action space of the navigation parameter dynamic decision maker is an adjustment quantity of parameters of the bottom navigation controller, wherein the adjustment quantity comprises an increment delta K p 、ΔK i 、ΔK d corresponding to proportional, integral and differential parameters of the steering controller and an increment delta V of a target speed; the reward function R of the navigation parameter dynamic decision maker is designed as a negative weighted comprehensive tracking error, and the expression is as follows: ; Wherein e lateral 、e heading 、e speed represents a lateral tracking error, a heading tracking error and a speed tracking error, w 1 、w 2 、w 3 is a corresponding positive weight coefficient, Δa represents a motion vector [ Δk p ,ΔK i ,ΔK d ,ΔV] T , and λ is a penalty coefficient, respectively.
5. The method of claim 4, wherein the periodic online trimming in S8 is performed as follows: S81, when a preset triggering condition is met, starting to collect data, wherein the triggering condition is that a preset time interval is reached or a preset driving distance is reached; collecting M groups of performance evaluation tuples, wherein each group of tuples comprises the state input by the navigation parameter dynamic decision maker in the step S7, the action output by the navigation parameter dynamic decision maker and the rewarding value actually calculated after the action is executed; s82, utilizing the M groups of performance evaluation tuples collected in S81, and performing fine adjustment on network parameters of the deep reinforcement learning decision-maker through an offline strategy reinforcement learning algorithm; And S83, performing Bayesian optimization on the adjustment coefficients alpha and beta in the self-adaptive observation likelihood model according to the statistical characteristics of the visual characteristic matching confidence coefficient C vis collected in the period corresponding to the triggering condition so as to minimize the positioning error in the period.
6. The method of claim 1, wherein the preprocessing of S3 further comprises a sensor anomaly detection and processing step of: respectively reconstructing real-time sequence data of the inertia measurement unit and the wheel speed encoder by utilizing a pre-trained self-encoder, and calculating a reconstruction error of the real-time sequence data; If the reconstruction error of the sensor exceeds the corresponding preset threshold value in the continuous K sampling periods, judging that the sensor has data abnormality, wherein K is a preset integer; When the extended Kalman filter fusion is performed in step S4, if it is determined that the sensor data is abnormal, one of (a) increasing the noise covariance of the abnormal sensor data in the extended Kalman filter observation model and (b) switching to a stand-by state estimation algorithm independent of the abnormal sensor is performed.
7. The method according to claim 1, wherein in the construction of the topological map, for each node, a convolutional neural network is used to extract and store high-level semantic feature vectors of the same image in addition to the local feature descriptors of the image; and when the S5 is used for visual matching, firstly, calculating the similarity between the high-level semantic feature vector of the current frame image and the high-level semantic feature vector stored by the nodes in the topological map, screening candidate matching nodes according to the similarity, and then, carrying out feature point matching and geometric verification on the local feature description sub-level only aiming at the candidate matching nodes so as to calculate the visual observation pose.
8. The AGV multi-source positioning fusion and navigation correction system is characterized by comprising a vehicle body, a driving unit, a control unit, an inertia measurement unit, a wheel speed encoder, a visual camera and a memory, wherein the memory stores a pre-constructed topological map and a computer program, and the computer program realizes the method according to any one of claims 1-7 when being executed by the control unit.

Description

AGV multi-source positioning fusion and navigation correction method and system Technical Field The invention relates to the technical field of automatic guided vehicles, in particular to a multi-source positioning fusion and navigation correction method and system for an AGV. Background Automated Guided Vehicles (AGVs) play a vital role in modern logistics, warehousing, and flexible manufacturing systems. Its core capabilities rely on high-precision, high-robustness positioning and navigation techniques. In the prior art, single or limited sensors are mostly adopted for positioning, such as laser radar SLAM, vision SLAM or pure inertial navigation, and problems of precision reduction, accumulated error and even positioning failure are easy to occur in complex, dynamic or feature-missing environments. Patent CN114030805A discloses a warehousing system, a shuttle car for the warehousing system and a navigation method thereof. The scheme focuses on a shuttle car running on a fixed track, and is characterized in that the integration and the switching of a positioning mode are realized by dead reckoning by relying on a positioning sensor such as a wheel speed encoder and the like in a long-distance driving stage, and when the vehicle approaches a target parking space (a sub road section end point), the vehicle is switched to a two-dimensional code label which is arranged in advance based on visual recognition for accurate positioning. The scheme effectively improves the terminal parking precision and control smoothness of the bicycle in the structured track environment. However, the fusion and switching strategy is fixed and passive (depends on a preset distance threshold), and the confidence and weight of different positioning sources cannot be adaptively adjusted according to real-time environment dynamics (such as blocked visual labels, intense illumination changes, instantaneous sensor anomalies and the like), and the deep fusion and online learning correction of multi-source perception information in an unstructured and dynamically-changed environment is not involved. Patent CN115951691a discloses a method and system for planning the track of a shuttle in a dense library under 5G communication, and the scheme focuses on global track planning and conflict coordination of a plurality of shuttles in a grid map environment. The three-dimensional grid map is established, and a clustering algorithm is utilized to calculate the path from each shuttle car to the target point, so that the aim of coordinating the vehicle track and avoiding the conflict is achieved. However, the solution mainly solves the problem of multi-vehicle coordination at the path planning level, the positioning basis of which usually depends on a simple encoder or a fixed tag, and the solution does not relate to how to obtain high-precision and high-robustness real-time pose estimation through depth adaptive fusion of multiple sensors (such as an IMU (inertial measurement unit), vision and wheel speed meter) in a complex dynamic environment. The coupling degree of the navigation control and positioning estimation links is weak, and an intelligent closed loop capable of performing online learning and adjustment according to real-time positioning quality, environment semantics and historical performance is lacking. Therefore, there is a need for an AGV positioning and navigation correction method that can deeply fuse multi-source information, intelligently adapt to environmental changes, achieve real-time accurate correction, and tightly combine with navigation control. Disclosure of Invention The invention provides a multi-source positioning fusion and navigation correction method and system of an AGV, and aims to solve the technical problems that in the prior art, a filtering strategy is fixed, and sensor weights cannot be adjusted in a real-time self-adaptive mode according to environmental changes (such as visual characteristic quality and dynamic obstacle interference), so that positioning reliability is suddenly reduced in a dynamic or characteristic missing scene. In a first aspect, an embodiment of the present invention provides a method for multi-source positioning fusion and navigation correction of an AGV, including: S1, acquiring original data through an inertial measurement unit, a wheel speed encoder and a visual camera which are arranged on an AGV; S2, loading a pre-constructed topological map, wherein the map comprises nodes, edges, associated visual feature descriptors and path semantic information; s3, carrying out time synchronization and noise filtering pretreatment on the original data; S4, based on the preprocessed inertial measurement unit and wheel speed encoder data, fusing through an extended Kalman filter, and outputting a first predicted pose; S5, matching the current image acquired by the visual camera with visual features in the topological map, resolving the visual observation pose, and calculating the visual feat