AGV high-precision positioning method and system based on dynamic reflector feature matching

Abstract

The invention discloses an AGV high-precision positioning method and system based on dynamic reflector feature matching, and relates to the technical field of automatic guided vehicle navigation. An original data acquisition module acquires laser data, odometer data and IMU data; the laser data preprocessing module performs dynamic threshold calculation, intensity filtering and annular scanning compensation; the reflector feature extraction module is used for spatial clustering and weighted centroid calculation; the least square method pose optimization module comprises coordinate transformation, pose optimization and optimal pose iterative solution; and the multi-sensor fusion module is used for performing Kalman filtering fusion on time compensation, an odometer and the optimal pose to obtain a global pose. The AGV positioning method is high in robustness and adaptive to the dynamic environment, and the problem of positioning drift caused by poor feature matching stability and fixed motion model noise is solved.

Description

Technical Field

[0001] The present invention mainly relates to the technical field of automatic guided vehicle navigation, and specifically to an AGV high-precision positioning method and system based on dynamic reflector feature matching. Background Art

[0002] In today's era of rapid development of intelligent manufacturing and smart logistics, automated guided vehicles (AGVs) serve as key execution equipment in automated systems such as smart warehousing and flexible production lines. Their efficient and precise positioning capabilities are the core foundation for achieving automated cargo handling, seamless integration of production processes, and intelligent operation of the entire system. The accuracy of AGV positioning directly determines the efficiency and accuracy of cargo handling, affecting the smoothness of the entire production or logistics process, and ultimately, a company's production efficiency and market competitiveness. Therefore, the development of a high-precision and highly reliable AGV positioning method and system is of great practical significance.

[0003] Currently, positioning methods based on preset maps are widely used in the field of AGV positioning, with the adaptive Monte Carlo positioning algorithm being a typical example. This method works well in scenarios with relatively stable environments and minimal structural changes. However, in actual industrial production and logistics environments, dynamic changes are the norm. For example, in a production workshop, equipment may be moved and adjusted according to the needs of production tasks; in a warehouse environment, the storage location of goods frequently changes with inbound and outbound operations. Positioning methods based on preset maps are extremely sensitive to these dynamic changes. Once the environment changes, deviations will occur between the pre-built map and the actual environment, resulting in large errors in positioning results. This may even cause the AGV to completely lose its position and be unable to perform its tasks normally, seriously affecting the stability and reliability of the AGV system.

[0004] In traditional reflector positioning algorithms, feature matching is a key step. However, in real-world industrial scenarios, there are numerous objects with similar structures, such as similarly shaped shelves in warehouses and equipment with similar structures in production workshops. These similar structures can severely interfere with the reflector feature matching process. Because traditional algorithms lack the ability to effectively distinguish similar structures during feature extraction and matching, they can easily misidentify other objects in the surrounding environment with similar features as reflectors as reflectors, resulting in an increased mismatch rate. Once a mismatch occurs, the calculated AGV position information will deviate from its true position, significantly reducing the AGV's positioning accuracy and potentially causing AGV collisions, posing a threat to equipment and personnel safety.

[0005] Currently, many AGV positioning systems rely solely on a single sensor, such as lidar, vision sensors, or inertial navigation systems. While single-sensor positioning methods offer advantages such as simplicity and low cost, each sensor has its own limitations in complex industrial environments and is susceptible to interference from environmental factors.

[0006] In summary, existing AGV positioning technology has significant shortcomings in adapting to dynamic environments, reducing feature matching mismatch rates, and achieving multi-source data fusion, making it difficult to meet the demand for high-precision AGV positioning in modern industrial production and logistics. Therefore, developing a new AGV high-precision positioning method and system that can overcome these shortcomings is a technical problem that needs to be solved urgently by researchers in this field. Summary of the Invention

[0007] Based on this, the purpose of the present invention is to provide an AGV high-precision positioning method and system based on dynamic reflector feature matching to solve the technical problems raised in the above background technology.

[0008] To achieve the above object, the present invention provides the following technical solutions:

[0009] The AGV high-precision positioning system based on dynamic reflector feature matching includes a raw data acquisition module, a laser data preprocessing module, a reflector feature extraction module, a least squares pose optimization module, and a multi-sensor fusion module;

[0010] The raw data acquisition module is used to collect raw laser data, odometer motion data and IMU data when the AGV is running;

[0011] The laser data preprocessing module is used to obtain a candidate reflector point set by applying dynamic threshold filtering and annular compensation to the collected original laser data set;

[0012] The reflector feature extraction module is used to perform spatial clustering and weighted centroid calculation on the candidate reflector point set to obtain a reflector polar coordinate combination;

[0013] The least squares pose optimization module is used to read the AMCL rough pose of the previous laser frame, perform coordinate conversion on the reflector polar coordinate combination to obtain a Cartesian coordinate system, and build observation equations, define residuals, set optimization targets and iteratively solve based on the pre-stored map reflector coordinates, and output the optimized pose and covariance matrix;

[0014] The multi-sensor fusion module is used to receive the optimized posture and odometer motion data, first perform time synchronization compensation, then use the extended Kalman filter to update the state, and output the fused global posture and covariance.

[0015] Preferably, the dynamic threshold filtering and ring compensation include the following steps:

[0016] S1. Input the original laser data set: And perform dynamic threshold calculation, the calculation formula is: Among them, k1, k2, k3 are the calibration parameters of the laser equipment; d i : is the distance, θ i is the angle, r i is the reflection intensity;

[0017] S2, intensity filtering, specifically: Output the filtered candidate reflector point set: Among them, D max is the maximum effective detection distance, R min is the minimum reflection intensity threshold at long distance;

[0018] S3. Input S′, perform annular scanning compensation, and output a continuous reflector point set S″.

[0019] Preferably, the annular scan compensation specifically includes head-to-tail continuity detection:

[0020] If the first and last points satisfy the geometric continuity constraints: Copy the tail point to the head of the dataset and correct the angle;

[0021] Among them, Θ full To scan the full angle (360° or 2π radians), Δθ th , Δd th is the angle and distance tolerance threshold.

[0022] Preferably, the spatial clustering and weighted centroid calculation specifically include the following steps:

[0023] S1. Input S″ and perform spatial clustering, constraining the adjacent angle difference to satisfy: |θ″ j+1 -θ″ j |<Δθ cluster ; Distance difference between adjacent points: |d″ j+1 -d″ j |<Δd cluster ; The spatial clustering results are recorded as in

[0024] S2, for each spatial cluster C k , calculate the centroid position weighted by the reflection intensity: Output reflector polar coordinates set:

[0025] Preferably, the pre-stored map reflector is represented by:

[0026] Preferably, the observation equation:

[0027] in, is the distance and angle of the reflector in the map;

[0028] Defining the residual includes defining the distance and angle double residuals:

[0029] Preferably, the optimization objective is to minimize the weighted residual sum of squares: The optimized pose is expressed as: (x opt ,y opt ,φ opt ), the covariance matrix is: ∑ opt ;

[0030] Among them, ω m is the distance residual weight, and λ is the angle residual weight factor.

[0031] Preferably, the synchronous compensation is specifically to perform linear interpolation compensation on the laser processing delay:

[0032] Where Δt = t odom -t opt .

[0033] Preferably, the state model of the extended Kalman filter is: t =f(x t-1 ,u t )+w t ,w t ~N(0,Q); the observation model is: z t =h(x t )+v t ,v t ~N(0,R);

[0034] where x = [x, y, φ] T ,u=[ν,ω] T , Q is the process noise covariance; z t =[x sync ,y sync ,φ sync ] T ,R=∑ opt ;

[0035] The status update is: The global pose after output fusion is: (x final ,y final ,φ final ), the covariance after fusion is: ∑final .

[0036] Preferably, the system further includes a storage module for storing all data; an execution module for reading the global posture to control the real-time positioning of the AGV; and a display module for displaying the posture information of the AGV in real time.

[0037] In summary, the present invention mainly has the following beneficial effects:

[0038] This invention enhances positioning robustness: Facing dynamic interference factors such as human movement and temporary obstacles that frequently occur in industrial scenarios, this invention accurately captures and matches the characteristics of dynamic reflectors to eliminate the impact of these interferences on positioning. Just like in a busy logistics warehouse, where workers and forklifts are constantly shuttling back and forth, the system can still stably provide the AGV with accurate position information based on reflector information, ensuring normal operation of the AGV and avoiding mission interruptions or collisions caused by positioning errors.

[0039] The high-precision positioning system of this invention ensures that the AGV reaches its target position accurately, with errors kept within a very small range. This system meets the stringent requirements of modern industry for AGV positioning accuracy, improving production efficiency and product quality. The invention integrates data from multiple sensors, including LiDAR. LiDAR provides precise distance and angle information, while the inertial navigation system reflects the AGV's motion state and posture changes. The adaptive Kalman filter algorithm fuses these multi-source data, fully leveraging the advantages of each sensor to compensate for the shortcomings of a single sensor and ensuring the continuity and accuracy of positioning.

[0040] Because the present invention offers improved adaptability to dynamic environmental changes and high positioning accuracy, it reduces the occurrence of AGV collisions and mission failures caused by positioning errors, lowering the risk of equipment damage and mission delays, thereby reducing system maintenance and operating costs. Furthermore, the system's high reliability and stability reduce the need for manual intervention, further improving the system's automation level and overall efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 It is the algorithm flow framework diagram of the present invention;

[0042] Figure 2 A scene diagram for arranging the reflector of the present invention;

[0043] Figure 3 It is the positioning effect diagram of the present invention. DETAILED DESCRIPTION

[0044] The technical solutions in the embodiments of the present invention will be described clearly and completely below with reference to the accompanying drawings in the embodiments of the present invention.

[0045] Example

[0046] It should be noted that, in this embodiment, the hardware deployment:

[0047] Sensor configuration: Pepperl+Fuchs R2000 laser sensor, encoder and IMU included with forklift AGV.

[0048] Processor configuration: Main control device equipped with i5 processor.

[0049] Reflector arrangement: In a 30m*30m industrial warehouse scenario, dynamically arrange 50 reflective columns with a diameter of 75mm at a spacing of 1.5-4.0m, such as Figure 2 shown.

[0050] like Figure 1 and Figure 3 As shown, 1. Laser data preprocessing

[0051] Input: raw laser scan dataset, where d i is the distance, θ i is the angle, r i is the reflection intensity.

[0052] Processing process:

[0053] 1.1 Dynamic threshold calculation: Based on the nonlinear threshold model of distance, the laser intensity threshold is dynamically adjusted to suppress environmental noise: Where k1, k2, and k3 are the calibration parameters of the laser equipment.

[0054] 1.2 Intensity Filtering: Filter the laser data set based on the dynamic threshold curve and the scanning distance and reflection intensity threshold of the laser sensor device: Among them, D max is the maximum effective detection distance, R min is the minimum reflection intensity threshold at long distance.

[0055] Output: Filtered candidate reflector point set

[0056] 1.3 Annular Scan Compensation

[0057] To address the discontinuity problem of the lidar's head and tail scanning data, a geometric continuity verification mechanism is designed. When the conditions are met, the tail data is copied to the head of the scanning sequence and the angle value is corrected to form a closed-loop data flow.

[0058] Input: candidate reflector point set S′.

[0059] Processing process:

[0060] Start-end continuity detection:

[0061] If the first and last points satisfy the geometric continuity constraints: Among them, Θ full To scan the full angle (360° or 2π radians), Δθ th , Δd th is the angle and distance tolerance threshold.

[0062] Data compensation: copy the tail point to the head of the data set and correct the angle: S″=S′∪{(d′ M ,θ′1+Θ full , r′ M )};

[0063] Output: continuous reflector point set S″.

[0064] 2. Reflector feature extraction

[0065] Input: continuous point set S″.

[0066] Processing process:

[0067] 2.1 Spatial Clustering:

[0068] Angle continuity: adjacent angle difference: |θ″ j+1 -θ″ j |<Δθ cluster .

[0069] Distance consistency: distance difference between adjacent points: |d″ j+1 -d″ j |<Δd cluster .

[0070] The clustering results are recorded as in

[0071] 2.2 Weighted centroid calculation:

[0072] For each cluster C k , calculate the centroid position weighted by the reflection intensity:

[0073] Output: reflector polar coordinates set:

[0074] 3. Least Squares Pose Optimization

[0075] Input: reflector polar coordinate set and pre-stored map reflector coordinates;

[0076]

[0077] The laser estimated pose of the previous frame processed by the adaptive Monte Carlo positioning algorithm (pose x ,posey ,pose θ ).

[0078] Processing process:

[0079] Convert polar coordinates to Cartesian coordinates in the AGV coordinate system:

[0080] That is, the coordinates of the local observation reflector are obtained

[0081] 3.1 Construction of observation equation: from the local observation reflector coordinate set The current pose (x, y, φ) is obtained from the equation, that is, each reflector satisfies:

[0082] in, is the distance and angle of the reflector in the map.

[0083] 3.2 Residual definition:

[0084] Define distance and angle dual residuals:

[0085]

[0086] 3.3 Optimization Objectives

[0087] Minimize the weighted sum of squared residuals:

[0088]

[0089] Among them, ω m is the distance residual weight, and λ is the angle residual weight factor.

[0090] 3.4 Iterative Solution

[0091] Use the Levenberg-Marquardt algorithm to iteratively update the pose:

[0092] (J T WJ+μdiag(J T WJ))Δp=-J T Wr

[0093] in,

[0094] J is the Jacobian matrix of the residual to pose; W is the weight diagonal matrix; μ is the damping factor, which is dynamically adjusted to ensure convergence.

[0095] Output: Optimized pose (x opt ,y opt ,φ opt ) and covariance matrix ∑ opt .

[0096] 4. Multi-sensor filtering and fusion

[0097] Input: Optimized pose (x opt ,y opt ,φ opt ) and odometer motion data (v,ω,t).

[0098] Processing process:

[0099] 4.1 Time synchronization compensation:

[0100] Linear interpolation compensation for laser processing delay:

[0101] Where Δt = t odom -t opt .

[0102] 4.2 Extended Kalman Filter

[0103] State Model: x t =f(x t-1 ,u t )+w t ,w t ~N(0,Q); where, x=[x,y,φ] T ,u=[v,ω] T , Q is the process noise covariance.

[0104] Observation model: z t =h(x t )+v t ,v t ~N(0,R); where z t =[x sync ,y sync ,φ sync ] T ,R=∑ opt .

[0105] Kalman gain: Among them, H t is the Jacobian matrix of the observation equation.

[0106] Status Update:

[0107] Output: fused global pose (x final ,y final ,φ final ) and covariance ∑ final .

[0108] The above embodiments are only for illustrating the technical idea of ​​the present invention and cannot be used to limit the protection scope of the present invention. Any changes made on the basis of the technical solution in accordance with the technical idea proposed by the present invention shall fall within the protection scope of the present invention.

Claims

1. The AGV high-precision positioning system based on dynamic reflector feature matching is characterized by: It includes raw data acquisition module, laser data preprocessing module, reflector feature extraction module, least squares pose optimization module, and multi-sensor fusion module; The raw data acquisition module is used to collect raw laser data, odometer motion data and IMU data when the AGV is running; The laser data preprocessing module is used to obtain a candidate reflector point set by applying dynamic threshold filtering and annular compensation to the collected original laser data set; The reflector feature extraction module is used to perform spatial clustering and weighted centroid calculation on the candidate reflector point set to obtain a reflector polar coordinate combination; The least squares pose optimization module is used to read the AMCL rough pose of the previous laser frame, perform coordinate conversion on the reflector polar coordinate combination to obtain a Cartesian coordinate system, and build observation equations, define residuals, set optimization targets and iteratively solve based on the pre-stored map reflector coordinates, and output the optimized pose and covariance matrix; The multi-sensor fusion module is used to receive the optimized posture and odometer motion data, first perform time synchronization compensation, then use the extended Kalman filter to update the state, and output the fused global posture and covariance.

2. The AGV high-precision positioning system based on dynamic reflector feature matching according to claim 1 is characterized in that: The dynamic threshold filtering and ring compensation include the following steps: S1. Input the original laser data set: And perform dynamic threshold calculation, the calculation formula is: Among them, k1, k2, k3 are the calibration parameters of the laser equipment; d i : is the distance, θ i is the angle, r i is the reflection intensity; S2, intensity filtering, specifically: Output the filtered candidate reflector point set: Among them, D max is the maximum effective detection distance, R min is the minimum reflection intensity threshold at long distance; S3. Input S′, perform annular scanning compensation, and output a continuous reflector point set S″.

3. The AGV high-precision positioning system based on dynamic reflector feature matching according to claim 2 is characterized in that: The annular scan compensation specifically includes the head-to-tail continuity detection: If the first and last points satisfy the geometric continuity constraints: Copy the tail point to the head of the dataset and correct the angle; Among them, Θ full To scan the full angle (360° or 2π radians), Δθ th , Δd th is the angle and distance tolerance threshold.

4. The AGV high-precision positioning system based on dynamic reflector feature matching according to claim 1 is characterized in that: The spatial clustering and weighted centroid calculation specifically include the following steps: S1. Input S″ and perform spatial clustering, constraining the adjacent angle difference to satisfy: |θ″ j+1 -θ″ j |<Δθ cluster ; Distance difference between adjacent points: |d″ j+1 -d″ j |<Δd cluster ; The spatial clustering results are recorded as in S2, for each spatial cluster C k , calculate the centroid position weighted by the reflection intensity: Output reflector polar coordinates set:

5. The AGV high-precision positioning method and system based on dynamic reflector feature matching according to claim 4 is characterized in that: The pre-stored map reflector is represented by:

6. The AGV high-precision positioning system based on dynamic reflector feature matching according to claim 5 is characterized in that: The observation equation: in, is the distance and angle of the reflector in the map; Defining the residual includes defining the distance and angle double residuals:

7. The AGV high-precision positioning system based on dynamic reflector feature matching according to claim 6, characterized in that: The optimization objective is to minimize the weighted residual sum of squares: The optimized pose is expressed as: (x opt ,y opt ,φ opt ), the covariance matrix is: ∑ opt ; Among them, ω m is the distance residual weight, and λ is the angle residual weight factor.

8. The AGV high-precision positioning system based on dynamic reflector feature matching according to claim 7, characterized in that: The synchronous compensation is specifically to perform linear interpolation compensation on the laser processing delay: Where Δt = t odom -t opt .

9. The AGV high-precision positioning system based on dynamic reflector feature matching according to claim 7, characterized in that: The state model of the extended Kalman filter is: t =f(x t-1 ,u t )+w t ,w t ~N(0,Q); the observation model is: z t =h(x t )+v t ,v t ~N(0,R); where x = [x, y, φ] T ,u=[ν,ω] T , Q is the process noise covariance; z t =[x sync ,y sync ,φ sync ] T ,R=∑ opt ; The status update is: The global pose after output fusion is: (x final ,y final ,φ final ), the covariance after fusion is: ∑ final .

10. The AGV high-precision positioning system based on dynamic reflector feature matching according to claim 1, characterized in that: The system also includes a storage module for storing all data; an execution module for reading the global posture control AGV real-time positioning; and a display module for displaying the posture information of the AGV in real time.

Images