AGV adaptive deviation correction control method and system based on fusion of double magnetic sensors and slam

Abstract

The application provides an AGV adaptive deviation correction control method and system based on fusion of double magnetic sensors and SLAM, comprising: collecting a lateral deviation relative to a center line of a ground magnetic stripe in real time through a first magnetic sensor and a second magnetic sensor; calculating a curvature of a current magnetic stripe path; calculating a feedforward angular velocity; calculating a lateral deviation and an angular deviation of the AGV relative to the magnetic stripe; calculating a lateral deviation and a heading deviation of the AGV relative to a global path; obtaining a fused deviation state; calculating a feedback angular velocity based on the deviation state; obtaining a target angular velocity; converting the target linear velocity and the target angular velocity into target rotating speeds of left and right wheels; monitoring a magnetic sensor signal quality index and a SLAM positioning reliability index in real time, and automatically switching to a pure SLAM navigation mode or a magnetic navigation mode. The application can solve the deficiencies of a traditional scheme in aspects such as curved track tracking, speed adaptability, anti-interference capability and the like, thereby improving the navigation robustness of the AGV in a complex industrial scene.

Description

Technical Field

[0001] This invention relates to the field of automated guided vehicle (AGV) control technology, specifically to an AGV adaptive correction control method and system based on dual magnetic sensors and SLAM fusion. It aims to address the shortcomings of traditional magnetic navigation AGVs in terms of curve tracking, speed adaptability, and anti-interference capabilities, and improve the navigation accuracy and robustness of AGVs in complex industrial scenarios. Background Technology

[0002] Automated Guided Vehicles (AGVs) are core equipment in intelligent logistics and intelligent manufacturing, and their navigation and control accuracy directly affects the system's operational efficiency and safety. Magnetic navigation is widely used in the industrial AGV field due to its advantages such as low cost, high reliability, and strong anti-interference capability.

[0003] Traditional magnetic navigation AGVs typically use a single magnetic sensor to collect lateral deviations and calculate the correction angular velocity using a PID controller. However, this method has the following shortcomings: (1) It relies on only a single deviation signal and cannot sense the angle between the vehicle body and the magnetic strip, which can easily lead to overshoot and oscillation; (2) When encountering curves, pure proportional control will produce lag, resulting in "bending" or "outward deviation" phenomena; (3) The control parameters need to be adjusted with the speed, otherwise the dynamic response will be inconsistent; (4) It is sensitive to sensor noise, and direct mapping may amplify the noise and cause motor vibration.

[0004] To address the aforementioned issues, existing research has proposed employing a dual-magnetic sensor scheme. For instance, patent application CN202010842964.6 discloses a navigation correction method for AGVs based on differential wheel sets, which calculates the AGV's center offset and heading angle using front and rear magnetic sensors. Patent application CN202411779753.7 proposes a dual-magnetic-guided control method, employing dual control target point angle correction to consider both front and rear control points. However, these methods still have the following limitations: they lack geometric feedforward compensation for the curvature of the magnetic strip, resulting in lag in response to continuous curves or scenarios with drastic curvature changes; they do not consider fusion with global positioning information such as SLAM, making it impossible to maintain stable operation when the magnetic strip is partially disturbed or lost; and the control parameters require manual tuning, making it difficult to adapt to the dynamic response requirements at different speeds. Summary of the Invention

[0005] To address the aforementioned issues, the present invention aims to provide an AGV adaptive correction control method and system based on the fusion of dual magnetic sensors and SLAM. By employing high-precision curvature feedforward, fusion of local and global information, adaptive speed control, and intelligent mode switching, the method overcomes the shortcomings of traditional solutions in terms of curve tracking, speed adaptability, and anti-interference capabilities, thereby improving the navigation robustness of AGVs in complex industrial scenarios.

[0006] The present invention achieves the above objectives through the following technical solutions: An adaptive deviation correction control method for AGVs based on the fusion of dual magnetic sensors and SLAM includes: The lateral deviation relative to the center line of the ground magnetic strip is collected in real time by the first and second magnetic sensors installed at the front and rear ends of the AGV body, including the front magnetic sensor deviation A and the rear magnetic sensor deviation B, with the left side of the AGV's forward direction as the positive direction; the global pose information of the AGV is obtained in real time through the SLAM system, including the global coordinate position and heading angle θ_slam; the current linear velocity V of the AGV is obtained through the encoder. Based on the deviations A and B between the front and rear magnetic sensors, and the horizontal installation distance L between the first and second magnetic sensors, the curvature κ of the current magnetic strip path is calculated; based on the curvature κ and the current linear velocity V, the feedforward angular velocity ω_ff is calculated. Based on the deviations A and B of the front and rear magnetic sensors, calculate the lateral deviation e and angular deviation θ of the AGV relative to the magnetic strip; Using the global pose information provided by the SLAM system and combined with the pre-stored map magnetic strip path information, the lateral deviation e_global and heading deviation θ_global of the AGV relative to the global path are calculated; a complementary filter is designed to fuse the local deviation (e,θ) with the global deviation (e_global, θ_global) to obtain the fused deviation state (e_fused, θ_fused). Design a PD feedback controller to calculate the feedback angular velocity ω_fb based on the fused deviation state (e_fused, θ_fused); The feedforward angular velocity ω_ff and the feedback angular velocity ω_fb are combined to obtain the final target angular velocity ω: ω = ω_ff + ω_fb; according to the differential drive kinematic model, the target linear velocity V and the target angular velocity ω are converted into the target rotational speeds V_L and V_R of the left and right wheels; The system monitors the magnetic sensor signal quality index and SLAM positioning confidence index in real time. When the magnetic sensor signal is lost or the signal-to-noise ratio is lower than the threshold, it automatically switches to pure SLAM navigation mode and uses the Stanley controller for path tracking. When the SLAM confidence decreases but the magnetic sensor signal is normal, the magnetic navigation mode is used as the main mode, and the SLAM navigation mode provides slow integration correction.

[0007] According to the adaptive deviation correction control method for AGV based on dual magnetic sensors and SLAM fusion provided by the present invention, the formula for calculating the curvature κ of the current magnetic strip path is as follows: κ = 4(A - B) / L 2 Among them, coefficient 4 is derived based on two-point curvature geometry; The formula for calculating the feedforward angular velocity ω_ff is: ω_ff= V · κ = 4V(A - B) / L 2 Among them, the feedforward angular velocity is used to compensate in advance for the influence of the magnetic strip path curvature on the AGV's heading.

[0008] According to the present invention, an adaptive deviation correction control method for AGV based on dual magnetic sensors and SLAM fusion is provided. The lateral deviation e is the arithmetic mean of the lateral deviations of the front and rear magnetic sensors, and the calculation formula is as follows: e = (A + B) / 2 The angular deviation θ is the ratio of the difference in lateral deviation between the front and rear magnetic sensors to the installation distance, and the calculation formula is: θ = (A - B) / L The lateral deviation e and the angular deviation θ together constitute the local pose deviation state of the AGV relative to the magnetic strip. The lateral deviation e is used for feedback control of the AGV's lateral position correction, and the angular deviation θ is used for feedback control of the AGV's heading adjustment.

[0009] According to the adaptive deviation correction control method for AGV based on the fusion of dual magnetic sensors and SLAM provided by the present invention, the calculation formula for the fused deviation state (e_fused, θ_fused) is expressed as follows: e_fused = α·e + (1-α)·e_global θ_fused = β·θ + (1-β)·θ_global Wherein, α and β are fusion weighting coefficients, dynamically adjusted according to the magnetic sensor signal quality index Q_mag and the SLAM confidence index Q_slam, specifically expressed as follows: α = Q_mag / (Q_mag + Q_slam) β = Q_mag / (Q_mag + Q_slam) Q_mag is calculated based on the magnetic sensor reading amplitude and noise level, while Q_slam is calculated based on the laser matching score or the number of visual feature points.

[0010] According to the adaptive deviation correction control method for AGV based on dual magnetic sensors and SLAM fusion provided by the present invention, when calculating the magnetic sensor signal quality index Q_mag, it is calculated based on the reading amplitude, signal-to-noise ratio, and signal stability of the first and second magnetic sensors. The calculation formula is expressed as follows: Q_mag = (S_mag / N_mag) × (1 - Var_mag / Max_Var) Where S_mag is the effective signal amplitude of the magnetic sensor, N_mag is the noise amplitude, Var_mag is the signal variance, and Max_Var is the maximum allowable variance; According to the adaptive deviation correction control method for AGV based on dual magnetic sensors and SLAM fusion provided by the present invention, the SLAM confidence index Q_slam is calculated based on the lidar point cloud matching score, the number of visual feature points, or the trace of the localization covariance matrix of the SLAM system. The calculation formula is expressed as follows: Q_slam = (Match_Score / Max_Score) × (Feature_Num / Max_Feature) ×(1 - Trace(Cov) / Max_Trace) Where Match_Score is the current matching score, Max_Score is the maximum possible score, Feature_Num is the current number of feature points, Max_Feature is the maximum number of feature points, Trace(Cov) is the trace of the covariance matrix, and Max_Trace is the maximum allowed trace.

[0011] According to the present invention, an AGV adaptive deviation correction control method based on the fusion of dual magnetic sensors and SLAM is provided. When Q_mag > Q_slam, that is, the magnetic sensor signal quality is better than SLAM, α and β approach 1, and the local deviation (e, θ) of magnetic navigation is dominant; when Q_slam > Q_mag, that is, the SLAM confidence is better than the magnetic sensor, α and β approach 0, and the global deviation (e_global, θ_global) of SLAM is dominant; when the two are equal, α = β = 0.5, and equal weight fusion is achieved.

[0012] When Q_mag drops below the first preset threshold Qth1, it is determined that the magnetic strip is temporarily obstructed. When the magnetic strip is determined to be temporarily obstructed, the fusion weight coefficients α and β corresponding to magnetic navigation are reduced, while the fusion weight coefficients corresponding to SLAM navigation are increased, so that the weight ratio of SLAM global positioning information in fusion control is significantly improved. When the magnetic strip returns to normal and Q_mag rises back to above the second preset threshold Qth2, α and β are restored to normal levels, and fusion control is resumed with the high-precision local deviation of magnetic navigation as the main factor.

[0013] According to the adaptive deviation correction control method for AGV based on dual magnetic sensors and SLAM fusion provided by the present invention, the formula for calculating the feedback angular velocity ω_fb is expressed as follows: ω_fb = K_p · e_fused + K_d · V · θ_fused Where K_p is the proportionality coefficient and K_d is the differential coefficient; Adaptively adjusting the controller gain according to the current linear velocity V, which is expressed as: K_p = K_p0 / (1 + λ·V), K_d = K_d0 · (1 + μ·V) Wherein, K_p0 and K_d0 are reference gain coefficients, and λ and μ are velocity adaptive adjustment coefficients.

[0014] According to the AGV adaptive deviation rectification control method based on the fusion of dual magnetic sensors and SLAM provided by the present invention, based on the kinematic characteristic that the rotational speed difference between the left and right differentially driven wheels generates a steering torque, the target rotational speeds V_L and V_R of the left and right driving wheels satisfy the following linear relationship: V_L = V - (d / 2)·ω V_R = V + (d / 2)·ω Wherein, V_L is the target rotational speed of the left driving wheel, V_R is the target rotational speed of the right driving wheel, and the wheel base d is the lateral distance between the centers of the left and right driving wheels of the AGV; When ω =0, V_L=V_R= V , the AGV travels in a straight line; when ω >0, V_L< V <V_R, the AGV turns left, that is, the rotational speed of the left wheel is lower than that of the right wheel, generating a clockwise steering torque; when ω <0, V_L> V >V_R, the AGV turns right, that is, the rotational speed of the left wheel is higher than that of the right wheel, generating a counterclockwise steering torque.

[0015] An AGV adaptive deviation rectification control system based on the fusion of dual magnetic sensors and SLAM, comprising: A vehicle body, and left and right differentially driven wheels arranged below the vehicle body; A first magnetic sensor, mounted at the front end of the vehicle body, configured to collect a front magnetic sensor deviation A; A second magnetic sensor, mounted at the rear end of the vehicle body, configured to collect a rear magnetic sensor deviation B; A SLAM positioning module, including a lidar or a vision sensor, configured to acquire the global pose information of the AGV; An encoder, mounted on the driving wheel, configured to acquire the current linear velocity V of the AGV; A main controller, connected to the first magnetic sensor, the second magnetic sensor, the SLAM positioning module, the encoder and the motor drivers of the driving wheels respectively, wherein the main controller comprises: A curvature feedforward calculation unit, configured to calculate a feedforward angular velocity ω_ff; The state estimation and fusion unit is used to calculate the local bias (e, θ) and fuse it with the global bias (e_global, θ_global); The feedback control unit is used to calculate the feedback angular velocity ω_fb based on the fused deviation state. A speed adaptive adjustment unit is used to dynamically adjust the controller gain according to the current linear velocity V; The kinematics calculation unit is used to convert the target linear velocity and target angular velocity into the target rotational speed of the left and right wheels; The mode switching unit is used to switch the navigation mode according to the signal quality.

[0016] Therefore, compared with the prior art, the AGV adaptive correction control method and system based on the fusion of dual magnetic sensors and SLAM proposed in this invention has the following beneficial effects: 1. This invention calculates the curvature of the magnetic strip in real time through the geometric relationship of dual magnetic sensors, generates a feedforward angular velocity, and enables the AGV to turn in advance before the curve, significantly reducing tracking error and solving the problem of cutting or deflecting outward in traditional methods, thereby improving the accuracy of the curve trajectory.

[0017] 2. This invention dynamically fuses the local deviation of the high-frequency magnetic sensor with the global pose of the low-frequency SLAM through a complementary filter, taking into account both real-time response and global correction capabilities, avoiding trajectory deviation caused by local interference or global error accumulation, and significantly enhancing the robustness of the system.

[0018] 3. The controller gain of this invention is dynamically adjusted with linear velocity, the proportional coefficient decreases as the speed increases, and the derivative coefficient increases as the speed increases, which solves the problems of low-speed overshoot and high-speed lag, achieves stable response across the entire speed range, and improves the consistency of dynamic performance.

[0019] 4. This invention automatically switches the navigation mode (normal / pure magnetic navigation / pure SLAM navigation / emergency stop) based on the sensor signal quality. It can still operate continuously when the magnetic strip is blocked or SLAM fails. After the fault is recovered, it automatically switches back to the normal mode, which greatly improves the fault tolerance of the system.

[0020] 5. This invention establishes a curvature calculation, deviation fusion, and gain adjustment model based on rigorous geometric kinematic derivation. The physical meaning of the control parameters is clear, avoiding trial and error based on experience, thereby improving engineering debugging efficiency and reducing deployment costs.

[0021] In summary, this invention, through the integration of multiple technologies and innovative design, systematically solves the pain points of traditional AGVs in areas such as curve tracking, speed adaptability, and anti-interference capabilities, significantly improving navigation accuracy, robustness, and engineering practicality, and providing an efficient and low-cost solution for the fields of intelligent logistics and manufacturing.

[0022] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments. Attached Figure Description

[0023] Figure 1 This is a flowchart of an embodiment of an AGV adaptive correction control method based on the fusion of dual magnetic sensors and SLAM according to the present invention.

[0024] Figure 2 This is a flowchart illustrating the adjustment of the fusion weight coefficient in an embodiment of the AGV adaptive correction control method based on the fusion of dual magnetic sensors and SLAM according to the present invention.

[0025] Figure 3 This is a system block diagram of an embodiment of an AGV adaptive correction control system based on the fusion of dual magnetic sensors and SLAM according to the present invention.

[0026] Figure 4 This is a system schematic diagram of an embodiment of an AGV adaptive correction control system based on the fusion of dual magnetic sensors and SLAM according to the present invention.

[0027] Figure 5 This is a schematic diagram of an embodiment of an AGV adaptive correction control system based on the fusion of dual magnetic sensors and SLAM according to the present invention. Detailed Implementation

[0028] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0029] In this document, the term "embodiment" means that a particular feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0030] An Example of an Adaptive Correction Control Method for AGVs Based on Dual Magnetic Sensors and SLAM Fusion See Figure 1 This embodiment provides an AGV adaptive correction control method based on the fusion of dual magnetic sensors and SLAM, including: Step S1: The first and second magnetic sensors installed at the front and rear ends of the AGV body are used to collect the lateral deviation relative to the center line of the ground magnetic strip in real time, including the deviation A of the front magnetic sensor and the deviation B of the rear magnetic sensor, with the left side of the AGV's forward direction as the positive direction; the global pose information of the AGV is obtained in real time through the SLAM system, including the global coordinate position and heading angle θ_slam; the current linear velocity V of the AGV is obtained through the encoder. Step S2: Based on the deviations A and B between the front and rear magnetic sensors, and the horizontal installation distance L between the first and second magnetic sensors, calculate the curvature κ of the current magnetic strip path; calculate the feedforward angular velocity ω_ff based on the curvature κ and the current linear velocity V. Step S3: Based on the deviations A and B of the front and rear magnetic sensors, calculate the lateral deviation e and angular deviation θ of the AGV relative to the magnetic strip. Step S4: Using the global pose information provided by the SLAM system and combined with the pre-stored map magnetic strip path information, calculate the lateral deviation e_global and heading deviation θ_global of the AGV relative to the global path; design a complementary filter to fuse the local deviation (e, θ) with the global deviation (e_global, θ_global) to obtain the fused deviation state (e_fused, θ_fused). Step S5: Design a PD feedback controller and calculate the feedback angular velocity ω_fb based on the fused deviation state (e_fused, θ_fused); Step S6: Combine the feedforward angular velocity ω_ff and the feedback angular velocity ω_fb to obtain the final target angular velocity ω: ω = ω_ff + ω_fb; Based on the differential drive kinematic model, convert the target linear velocity V and the target angular velocity ω into the target rotational speeds V_L and V_R of the left and right wheels; Step S7: Monitor the magnetic sensor signal quality index and SLAM positioning confidence index in real time. When the magnetic sensor signal is lost or the signal-to-noise ratio is lower than the threshold, automatically switch to pure SLAM navigation mode and use Stanley controller for path tracking. When the SLAM confidence decreases but the magnetic sensor signal is normal, use magnetic navigation mode as the main mode and SLAM navigation mode provides slow integration correction.

[0031] In step S2 above, the formula for calculating the curvature κ of the current magnetic stripe path is: κ = 4(A - B) / L 2 Among them, coefficient 4 is derived based on two-point curvature geometry; The formula for calculating the feedforward angular velocity ω_ff is: ω_ff= V · κ = 4V(A - B) / L 2 Among them, the feedforward angular velocity is used to compensate in advance for the influence of the magnetic strip path curvature on the AGV's heading, so as to reduce the adjustment burden of the feedback controller.

[0032] In step S3 above, the lateral deviation e is the arithmetic mean of the lateral deviations of the front and rear magnetic sensors, and the calculation formula is: e = (A + B) / 2 The geometric meaning of this formula is: the lateral position of the AGV's centroid relative to the center line of the magnetic strip. The average deviation of the front and rear sensors can cancel out the random noise of a single sensor, such as uneven magnetic field of the magnetic strip or sensor drift, thus improving the accuracy of lateral position estimation. The angular deviation θ is the ratio of the difference in lateral deviation between the front and rear magnetic sensors to the installation distance, and the calculation formula is: θ = (A - B) / L This formula is based on the small angle approximation principle (tanθ≈θ, θ unit: rad). Geometrically, it means the angular deviation between the AGV's heading and the tangent direction of the magnetic strip path. The deviation difference between the front and rear sensors reflects the degree of AGV torsion. Dividing it by the installation distance converts it into angular deviation, which directly characterizes the heading error. The lateral deviation e and the angular deviation θ together constitute the local pose deviation state of the AGV relative to the magnetic strip. The lateral deviation e is used for feedback control of the AGV's lateral position correction, such as the proportional term of the PD controller; the angular deviation θ is used for feedback control of the AGV's heading adjustment, such as the derivative term of the PD controller. The combination of the two realizes the local deviation estimation in both position and attitude dimensions. Compared with the single magnetic sensor, which can only provide position deviation, it significantly improves the attitude control accuracy of magnetic navigation, such as reducing the tail-wagging phenomenon of AGV in curves.

[0033] In step S4 above, the formula for calculating the fused deviation state (e_fused, θ_fused) is as follows: e_fused = α·e + (1-α)·e_global θ_fused = β·θ + (1-β)·θ_global Wherein, α and β are fusion weighting coefficients, dynamically adjusted according to the magnetic sensor signal quality index Q_mag and the SLAM confidence index Q_slam, specifically expressed as follows: α = Q_mag / (Q_mag + Q_slam) β = Q_mag / (Q_mag + Q_slam) Q_mag is calculated based on the magnetic sensor reading amplitude and noise level, while Q_slam is calculated based on the laser matching score or the number of visual feature points.

[0034] When calculating the signal quality index Q_mag of the magnetic sensor, it is based on the reading amplitude of the first and second magnetic sensors (such as the magnetic strip magnetic field strength), the signal-to-noise ratio (the ratio of signal to noise), and the signal stability (such as the variance within 100ms). The calculation formula is expressed as follows: Q_mag = (S_mag / N_mag) × (1 - Var_mag / Max_Var) Where S_mag is the effective signal amplitude of the magnetic sensor, N_mag is the noise amplitude, Var_mag is the signal variance, and Max_Var is the maximum allowable variance; When calculating the SLAM confidence index Q_slam, it is calculated based on the lidar point cloud matching score of the SLAM system (such as the matching error of the ICP algorithm), the number of visual feature points (such as the effective number of ORB feature points), or the trace of the localization covariance matrix (reflecting localization uncertainty). The calculation formula is expressed as follows: Q_slam = (Match_Score / Max_Score) × (Feature_Num / Max_Feature) ×(1 - Trace(Cov) / Max_Trace) Where Match_Score is the current matching score, Max_Score is the maximum possible score, Feature_Num is the current number of feature points, Max_Feature is the maximum number of feature points, Trace(Cov) is the trace of the covariance matrix, and Max_Trace is the maximum allowed trace.

[0035] like Figure 2 As shown, when Q_mag > Q_slam, meaning the magnetic sensor signal quality is better than SLAM, α and β approach 1, and the local bias (e, θ) of magnetic navigation is dominant; when Q_slam > Q_mag, meaning the SLAM confidence is better than the magnetic sensor, α and β approach 0, and the global bias (e_global, θ_global) of SLAM is dominant; when the two are equal, α = β = 0.5, achieving equal-weight fusion.

[0036] When Q_mag drops below the first preset threshold Qth1, it is determined that the magnetic strip is temporarily obstructed, such as by foreign objects or magnetic field interference causing signal quality degradation. When it is determined that the magnetic strip is temporarily obstructed, the fusion weight coefficients α and β corresponding to magnetic navigation are reduced, such as α decreasing from 0.8 to 0.2, and β being adjusted synchronously. At the same time, the fusion weight coefficient corresponding to SLAM navigation is increased, i.e., 1. α、1 β, for example, increasing from 0.2 to 0.8, significantly raises the weight proportion of the SLAM global positioning information in the fusion control, and the robustness of SLAM is preferentially relied on to maintain AGV operation; when the magnetic stripe returns to normal, such as the occluder is removed and the magnetic field signal is stabilized, and Q_mag rises back above a second preset threshold Qth2 (Qth2>Qth1, and Qth2 is not lower than the minimum signal quality value when the magnetic sensor works normally), α and β are restored to normal levels, that is, the weight coefficients when the magnetic stripe is not occluded, for example, α=0.8, β=0.8, and the fusion control is re-performed mainly based on the high-precision local deviation of magnetic navigation; It can be seen that the above adaptive fusion strategy solves the problems of the conventional fusion system that "magnetic navigation fails but the SLAM weight is not increased in time" or "the accuracy decreases due to still relying on SLAM after the magnetic stripe is restored" through the closed-loop logic of occlusion detection - weight tilting - recovery calibration, and ensures the navigation robustness of the AGV under various working conditions such as magnetic stripe occlusion and recovery, for example, no loss of control during occlusion and high precision after recovery.

[0037] In the above step S5, the calculation formula of the feedback angular velocity ω_fb is expressed as: ω_fb = K_p · e_fused + K_d · V · θ_fused where K_p is a proportional coefficient, and K_d is a differential coefficient; Adaptive adjustment is performed on the controller gain according to the current linear velocity V, which is expressed as: K_p = K_p0 / (1 + λ·V), K_d = K_d0 · (1 + μ·V) where K_p0 and K_d0 are reference gain coefficients, and λ and μ are velocity adaptive adjustment coefficients.

[0038] In the above step S6, based on the kinematic characteristic that the steering torque is generated by the rotational speed difference between the left and right wheels of differential drive, the target rotational speeds V_L and V_R of the left and right drive wheels satisfy the following linear relationship: V_L = V- (d / 2)·ω V_R = V+ (d / 2)·ω where V_L is the target rotational speed of the left drive wheel, V_R is the target rotational speed of the right drive wheel, and the wheel base d is the lateral distance between the centers of the left and right drive wheels of the AGV; When ω =0, V_L=V_R= V , the AGV travels straight; when ω >0, V_L< V <V_R, the AGV turns left, that is, the rotational speed of the left wheel is lower than that of the right wheel, generating a clockwise steering torque; when ω <0, V_L>V >V_R, the AGV turns right, that is, the left wheel rotates faster than the right wheel, generating a counterclockwise steering torque.

[0039] Perform speed limiting processing on the converted V_L and V_R: limit the target speed within the rated speed range of the drive wheel motor (e.g., (200rpm≤V_L,V_R≤200rpm) to avoid motor overload or AGV loss of control due to excessive speed; the speed command after limiting is directly sent to the motor driver of the left and right drive wheels to achieve precise motion control of the AGV.

[0040] In practical applications, this embodiment details the derivation process of the curvature feedforward formula in step S2, including its derivation and verification. Assume the AGV travels along a circular magnetic strip, and the vehicle's direction is always tangent to the magnetic strip. Let the radius of the arc be R, the distance between the two sensors be L, the front sensor be located L / 2 in front of the center of the vehicle, and the rear sensor be located L / 2 behind the center of the vehicle.

[0041] According to the geometric properties of a circle, the distance from the midpoint of the chord to the center of the circle is R - e_center, where e_center is the lateral deviation of the AGV center. From the perpendicular bisector theorem and the Pythagorean theorem, we can obtain: R² = (R - e_center)² + (L / 2)² Expanding and ignoring higher-order minor quantities e_center², we get: e_center ≈ L² / (8R) Similarly, the front sensor deviation A = e_center + Δ, and the rear sensor deviation B = e_center - Δ, where Δ = L² / (8R).

[0042] Therefore, A - B = 2Δ = L² / (4R) From the definition of curvature κ = 1 / R, we get κ = 4(A - B) / L².

[0043] When the AGV runs at a linear velocity V, the feedforward angular velocity ω_ff = V·κ = 4V(A - B) / L².

[0044] Regarding the tuning of the speed adaptive parameters, this embodiment provides a specific parameter example for the speed adaptive gain adjustment in step S6: The parameters of a certain type of AGV are: wheel track d=0.5m, sensor spacing L=0.8m, and maximum linear speed V_max=1.5m / s.

[0045] The baseline gain was obtained through debugging: K_p0=2.5, K_d0=0.8.

[0046] Speed ​​adaptive coefficients: λ=0.3, μ=0.5.

[0047] The gain values ​​at different speeds are: When V = 0.3 m / s: K_p = 2.5 / (1 + 0.3 × 0.3) = 2.29, K_d = 0.8 × (1 + 0.5 × 0.3) = 0.92 When V = 0.8 m / s: K_p = 2.5 / (1 + 0.3 × 0.8) = 2.02, K_d = 0.8 × (1 + 0.5 × 0.8) = 1.12 When V = 1.2 m / s: K_p = 2.5 / (1 + 0.3 × 1.2) = 1.84, K_d = 0.8 × (1 + 0.5 × 1.2) = 1.28 It can be seen that as the speed increases, the proportional coefficient is appropriately reduced to prevent overshoot, and the differential coefficient is appropriately increased to enhance the damping effect, thus ensuring the consistency of dynamic response at different speeds.

[0048] Regarding the fusion of local and global information, this embodiment provides a specific implementation of the fusion of local and global deviations in step S4: Assume the magnetic sensor sampling frequency is 100Hz and the SLAM pose update frequency is 10Hz. At time t, the magnetic sensor measures the local deviation (e, θ), and the SLAM system calculates the global deviation (e_global, θ_global) based on the current global pose and the pre-stored path.

[0049] The fusion weighting coefficients α and β are dynamically adjusted based on the magnetic sensor signal quality index Q_mag and the SLAM confidence index Q_slam. α = Q_mag / (Q_mag + Q_slam) β = Q_mag / (Q_mag + Q_slam) Q_mag is calculated based on the magnetic sensor reading amplitude and noise level, while Q_slam is calculated based on the laser matching score or the number of visual feature points.

[0050] When the magnetic strip is temporarily blocked, causing Q_mag to decrease, α and β decrease accordingly, and the weight of SLAM information increases; when the magnetic strip is restored and Q_mag recovers, α and β return to normal levels. It is evident that this adaptive fusion strategy ensures the robustness of the system under various operating conditions.

[0051] Regarding the mode switching logic, this embodiment provides the specific logic for mode switching in step S8: The failure condition for the magnetic sensor is defined as follows: within N consecutive sampling periods, |A| > threshold T_A and |B| > threshold T_B, or the signal amplitude is lower than the detection threshold. N is set to 5, and T_A = T_B = 50 mm.

[0052] Define SLAM failure condition as follows: within M consecutive sampling periods, the localization covariance is greater than the threshold P_max, or the laser matching score is lower than the threshold S_min, where M is 3.

[0053] The mode switching logic is as follows: Normal mode (magnetic navigation as the main method): When the magnetic sensor is effective and SLAM is effective or ineffective, the method of this invention is used (ω = ω_ff + ω_fb).

[0054] Degradation Mode 1 (Pure Magnetic Navigation): When the magnetic sensor is effective and SLAM fails, the fusion process is disabled, α=β=1 is set, and only local deviations are used for feedback control.

[0055] Degradation Mode 2 (Pure SLAM Navigation): When the magnetic sensor fails and SLAM is active, switch to the Stanley controller and calculate the angular velocity based on the global deviation.

[0056] Emergency stop mode: When the magnetic sensor fails and SLAM fails, the AGV decelerates and stops and an alarm is triggered.

[0057] An embodiment of an AGV adaptive deviation correction control system based on dual magnetic sensors and SLAM fusion like Figures 3 to 5 As shown, this embodiment provides an AGV adaptive deviation correction control system based on the fusion of dual magnetic sensors and SLAM, comprising: The vehicle body, and the left and right differential drive wheels located under the vehicle body; The first magnetic sensor is installed at the front of the vehicle body and is used to collect the deviation A of the front magnetic sensor; The second magnetic sensor is installed at the rear of the vehicle body and is used to collect the deviation B of the rear magnetic sensor. SLAM positioning module, including LiDAR or vision sensor, is used to acquire global pose information of AGV; An encoder, mounted on the drive wheel, is used to obtain the current linear velocity V of the AGV; The main controller is connected to the first magnetic sensor, the second magnetic sensor, the SLAM positioning module, the encoder, and the motor driver of the drive wheel. The main controller includes: The curvature feedforward calculation unit is used to calculate the feedforward angular velocity ω_ff; The state estimation and fusion unit is used to calculate the local bias (e, θ) and fuse it with the global bias (e_global, θ_global); The feedback control unit is used to calculate the feedback angular velocity ω_fb based on the fused deviation state. A speed adaptive adjustment unit is used to dynamically adjust the controller gain according to the current linear velocity V; The kinematics calculation unit is used to convert the target linear velocity and target angular velocity into the target rotational speed of the left and right wheels; The mode switching unit is used to switch the navigation mode according to the signal quality.

[0058] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0059] The above embodiments are merely preferred embodiments of the present invention and should not be construed as limiting the scope of protection of the present invention. Any non-substantial changes and substitutions made by those skilled in the art based on the present invention shall fall within the scope of protection claimed by the present invention.

Claims

1. An adaptive deviation correction control method for AGVs based on the fusion of dual magnetic sensors and SLAM, characterized in that, include: The lateral deviation relative to the center line of the ground magnetic strip is collected in real time by the first and second magnetic sensors installed at the front and rear ends of the AGV body, including the front magnetic sensor deviation A and the rear magnetic sensor deviation B, with the left side of the AGV's forward direction as the positive direction; the global pose information of the AGV is obtained in real time through the SLAM system, including the global coordinate position and heading angle θ_slam; the current linear velocity V of the AGV is obtained through the encoder. Based on the deviations A and B between the front and rear magnetic sensors, and the horizontal installation distance L between the first and second magnetic sensors, the curvature κ of the current magnetic strip path is calculated; based on the curvature κ and the current linear velocity V, the feedforward angular velocity ω_ff is calculated. Based on the deviations A and B of the front and rear magnetic sensors, calculate the lateral deviation e and angular deviation θ of the AGV relative to the magnetic strip; Using the global pose information provided by the SLAM system and combined with the pre-stored map magnetic strip path information, the lateral deviation e_global and heading deviation θ_global of the AGV relative to the global path are calculated; a complementary filter is designed to fuse the local deviation (e, θ) with the global deviation (e_global, θ_global) to obtain the fused deviation state (e_fused, θ_fused). Design a PD feedback controller to calculate the feedback angular velocity ω_fb based on the fused deviation state (e_fused, θ_fused); The feedforward angular velocity ω_ff and the feedback angular velocity ω_fb are combined to obtain the final target angular velocity ω: ω = ω_ff + ω_fb; according to the differential drive kinematic model, the target linear velocity V and the target angular velocity ω are converted into the target rotational speeds V_L and V_R of the left and right wheels; The system monitors the magnetic sensor signal quality index and SLAM positioning confidence index in real time. When the magnetic sensor signal is lost or the signal-to-noise ratio is lower than the threshold, it automatically switches to pure SLAM navigation mode and uses the Stanley controller for path tracking. When the SLAM confidence decreases but the magnetic sensor signal is normal, the magnetic navigation mode is used as the main mode, and the SLAM navigation mode provides slow integration correction.

2. The method according to claim 1, characterized in that: The formula for calculating the curvature κ of the current magnetic stripe path is: κ = 4(A - B) / L 2 Among them, coefficient 4 is derived based on two-point curvature geometry; The formula for calculating the feedforward angular velocity ω_ff is: ω_ff= V · κ = 4V(A - B) / L 2 Among them, the feedforward angular velocity is used to compensate in advance for the influence of the magnetic strip path curvature on the AGV's heading.

3. The method according to claim 2, characterized in that: The lateral deviation 'e' is the arithmetic mean of the lateral deviations of the front and rear magnetic sensors, calculated using the following formula: e = (A + B) / 2 The angular deviation θ is the ratio of the difference in lateral deviation between the front and rear magnetic sensors to the installation distance, and the calculation formula is: θ = (A - B) / L The lateral deviation e and the angular deviation θ together constitute the local pose deviation state of the AGV relative to the magnetic strip. The lateral deviation e is used for feedback control of the AGV's lateral position correction, and the angular deviation θ is used for feedback control of the AGV's heading adjustment.

4. The method according to claim 3, characterized in that: The formula for calculating the fused bias state (e_fused, θ_fused) is as follows: e_fused = α·e + (1-α)·e_global θ_fused = β·θ + (1-β)·θ_global Wherein, α and β are fusion weighting coefficients, dynamically adjusted according to the magnetic sensor signal quality index Q_mag and the SLAM confidence index Q_slam, specifically expressed as follows: α = Q_mag / (Q_mag + Q_slam) β = Q_mag / (Q_mag + Q_slam) Q_mag is calculated based on the magnetic sensor reading amplitude and noise level, while Q_slam is calculated based on the laser matching score or the number of visual feature points.

5. The method according to claim 4, characterized in that: When calculating the magnetic sensor signal quality index Q_mag, it is based on the reading amplitude, signal-to-noise ratio, and signal stability of the first and second magnetic sensors. The calculation formula is expressed as follows: Q_mag = (S_mag / N_mag) × (1 - Var_mag / Max_Var) Where S_mag is the effective signal amplitude of the magnetic sensor, N_mag is the noise amplitude, Var_mag is the signal variance, and Max_Var is the maximum allowable variance.

6. The method according to claim 5, characterized in that: When calculating the SLAM confidence index Q_slam, it is calculated based on the lidar point cloud matching score, the number of visual feature points, or the trace of the localization covariance matrix of the SLAM system. The calculation formula is expressed as follows: Q_slam = (Match_Score / Max_Score) × (Feature_Num / Max_Feature) × (1 -Trace(Cov) / Max_Trace) Where Match_Score is the current matching score, Max_Score is the maximum possible score, Feature_Num is the current number of feature points, Max_Feature is the maximum number of feature points, Trace(Cov) is the trace of the covariance matrix, and Max_Trace is the maximum allowed trace.

7. The method according to claim 6, characterized in that: When Q_mag > Q_slam, meaning the magnetic sensor signal quality is better than SLAM, α and β approach 1, and the local bias (e, θ) of magnetic navigation is dominant; when Q_slam > Q_mag, meaning the SLAM confidence is better than the magnetic sensor, α and β approach 0, and the global bias (e_global, θ_global) of SLAM is dominant; when the two are equal, α = β = 0.5, achieving equal-weight fusion. When Q_mag drops below the first preset threshold Qth1, it is determined that the magnetic strip is temporarily blocked. When it is determined that the magnetic strip is temporarily blocked, the fusion weight coefficients α and β corresponding to magnetic navigation are reduced, while the fusion weight coefficients corresponding to SLAM navigation are increased, so that the weight ratio of SLAM's global positioning information in fusion control is significantly improved. When the magnetic stripe returns to normal and Q_mag rises above the second preset threshold Qth2, α and β are restored to normal levels, and fusion control is resumed based on the high-precision local deviation of magnetic navigation.

8. The method according to claim 4, characterized in that: The formula for calculating the feedback angular velocity ω_fb is expressed as: ω_fb = K_p · e_fused + K_d · V · θ_fused Where K_p is the proportionality coefficient and K_d is the differential coefficient; The controller gain is adaptively adjusted based on the current linear velocity V, as follows: K_p = K_p0 / (1 + λ·V), K_d = K_d0 · (1 + μ·V) Where K_p0 and K_d0 are the reference gain coefficients, and λ and μ are the speed adaptive adjustment coefficients.

9. The method according to any one of claims 1 to 8, characterized in that: Based on the kinematic characteristics of the steering torque generated by the speed difference between the left and right wheels in differential drive, the target speeds V_L and V_R of the left and right drive wheels satisfy the following linear relationship: V_L = V - (d / 2)·ω V_R = V + (d / 2)·ω Where V_L is the target speed of the left drive wheel, V_R is the target speed of the right drive wheel, and the wheel track d is the lateral distance between the centers of the left and right drive wheels of the AGV; When ω =0, V_L=V_R= V , the AGV travels straight; when ω >0, V_L< V <V_R, the AGV turns left, that is, the rotation speed of the left wheel is lower than that of the right wheel, generating a clockwise steering torque; when ω <0, V_L> V >V_R, the AGV turns right, that is, the rotation speed of the left wheel is higher than that of the right wheel, generating a counterclockwise steering torque.

10. An AGV adaptive deviation correction control system based on the fusion of dual magnetic sensors and SLAM, characterized in that, include: The vehicle body, and the left and right differential drive wheels located under the vehicle body; The first magnetic sensor is installed at the front of the vehicle body and is used to collect the deviation A of the front magnetic sensor; The second magnetic sensor is installed at the rear of the vehicle body and is used to collect the deviation B of the rear magnetic sensor. SLAM positioning module, including LiDAR or vision sensor, is used to acquire global pose information of AGV; An encoder, mounted on the drive wheel, is used to obtain the current linear velocity V of the AGV; The main controller is connected to the first magnetic sensor, the second magnetic sensor, the SLAM positioning module, the encoder, and the motor driver of the drive wheel. The main controller includes: The curvature feedforward calculation unit is used to calculate the feedforward angular velocity ω_ff; The state estimation and fusion unit is used to calculate the local bias (e, θ) and fuse it with the global bias (e_global, θ_global); The feedback control unit is used to calculate the feedback angular velocity ω_fb based on the fused deviation state. A speed adaptive adjustment unit is used to dynamically adjust the controller gain according to the current linear velocity V; The kinematics calculation unit is used to convert the target linear velocity and target angular velocity into the target rotational speed of the left and right wheels; The mode switching unit is used to switch the navigation mode according to the signal quality.

Images