An agv autonomous mapping and path planning method and system based on multi-sensor fusion
By using multi-sensor fusion and improved path planning algorithms, the problems of perception blind spots and uneven paths in complex environments of AGVs have been solved, achieving efficient and robust autonomous navigation capabilities.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- JIANGXI MFG POLYTECHNIC COLLEGE
- Filing Date
- 2026-04-28
- Publication Date
- 2026-07-24
Smart Images
Figure CN122448211A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of robot navigation technology, specifically to a method and system for autonomous mapping and path planning of AGVs based on multi-sensor fusion, which is particularly suitable for autonomous navigation applications of Mecanum wheel AGVs in industrial scenarios. Background Technology
[0002] With the rapid development of intelligent manufacturing and Industry 4.0, Automated Guided Vehicles (AGVs) have been widely used in logistics warehousing, flexible manufacturing, and other fields. Autonomous navigation is one of the core functions of AGVs, mainly including key technologies such as environmental perception, autonomous positioning, path planning, and motion control. Among these, autonomous mapping and path planning are the foundation and key to realizing the intelligent and flexible operation of AGVs.
[0003] Currently, AGV navigation mainly employs methods such as magnetic strip navigation, QR code navigation, and laser SLAM navigation. Magnetic strip navigation and QR code navigation require the laying of guide markers on the ground, resulting in poor flexibility, high modification costs, and difficulty in adapting to dynamically changing production environments. While laser SLAM navigation offers high flexibility, a single laser radar has blind spots in complex environments, struggles to effectively detect transparent or highly reflective objects, and cannot acquire color and texture information about the environment, limiting the application capabilities of AGVs in complex scenarios.
[0004] In path planning, while the traditional A-Star algorithm can find the optimal path, its search efficiency is low, and the generated paths suffer from numerous bends and lack smoothness, making them unsuitable for the actual motion control of AGVs. Furthermore, it often ignores the kinematic constraints of the AGV, resulting in paths that are difficult for the AGV to accurately track and execute. This is especially true for omnidirectional AGVs using Mecanum wheels, whose unique kinematic characteristics need to be fully considered in path planning to leverage their flexibility and maneuverability. In addition, in localization, traditional Monte Carlo localization methods use a fixed number of particles, which struggles to guarantee accuracy when there is significant uncertainty, while wasting computational resources when localization is highly deterministic. Moreover, during mapping, the keyframe selection strategy directly affects map quality and update efficiency. Existing methods often use fixed intervals or simple thresholds for keyframe selection, making it difficult to adapt to the needs of different scenarios.
[0005] Therefore, there is an urgent need for AGV autonomous mapping and path planning methods and systems that can integrate information from multiple sensors, consider the kinematic constraints of AGVs, and achieve efficient and accurate positioning and path planning, so as to improve the autonomous navigation capability and operational efficiency of AGVs in complex industrial environments. Summary of the Invention
[0006] The purpose of this invention is to overcome the shortcomings of the prior art and provide an AGV autonomous mapping and path planning method and system based on multi-sensor fusion. It achieves high-precision environmental perception through multi-sensor fusion of LiDAR and depth camera, and uses an improved multi-step A-Star algorithm combined with the kinematic constraints of Mecanum wheel AGV for path planning, thereby realizing efficient autonomous navigation of AGV in complex industrial environments.
[0007] To achieve the above objectives, the present invention adopts the following technical solution:
[0008] The first aspect of this invention provides a method for autonomous mapping and path planning of AGVs based on multi-sensor fusion, comprising the following steps:
[0009] S1. Multi-sensor data acquisition steps: Acquire two-dimensional point cloud data of the environment through LiDAR, and simultaneously acquire depth and color images of the environment through a depth camera. Perform coordinate transformation on the depth images to generate three-dimensional point cloud data.
[0010] S2. Data fusion and mapping steps: The extended Kalman filter algorithm is used to perform spatiotemporal registration and fusion of LiDAR point cloud data and depth camera point cloud data. An environmental raster map is constructed based on the fused point cloud data. At the same time, a dynamic keyframe selection strategy is used to optimize the map update process.
[0011] S3, AGV positioning steps: Based on the constructed environmental grid map, the Monte Carlo positioning algorithm combined with an adaptive particle sampling strategy is used to achieve real-time positioning of the AGV in the environment;
[0012] S4. Path planning steps: Global path planning is performed based on the improved multi-step A-Star algorithm. The improved multi-step A-Star algorithm incorporates the kinematic constraints of the Mecanum wheel AGV into the heuristic function and achieves smooth path optimization by setting multiple step length parameters.
[0013] S5. Path tracking and obstacle avoidance steps: Based on the planned global path, the fuzzy PID control algorithm is used to control the AGV to travel along the planned path, while dynamic obstacle detection and local obstacle avoidance are performed based on real-time sensor data.
[0014] Further, in step S1, the specific method for generating 3D point cloud data by performing coordinate transformation on the depth image is as follows: obtain the intrinsic parameter matrix K and extrinsic parameter matrix [R|T] of the depth camera, where K contains the focal lengths fx and fy and the optical center coordinates cx and cy, R is the rotation matrix, and T is the translation vector; for each pixel (u,v) in the depth image, its corresponding depth value is d(u,v), then the 3D coordinates of the pixel in the camera coordinate system are: Xc = (u-cx)×d / fx, Yc = (v-cy)×d / fy, Zc =d; the points in the camera coordinate system are transformed to the world coordinate system through the extrinsic parameter matrix to obtain the 3D point cloud data for fusion.
[0015] Further, in step S2, the specific method for data fusion using the extended Kalman filter algorithm is as follows: Establish the AGV motion state equation and observation equation. The state equation is X(k+1) = F(k)X(k) + B(k)U(k) + W(k), and the observation equation is Z(k) = H(k)X(k) + V(k), where X(k) is the state vector, F(k) is the state transition matrix, B(k) is the control input matrix, U(k) is the control vector, W(k) is the process noise, Z(k) is the observation vector, H(k) is the observation matrix, and V(k) is the observation noise. The AGV's pose state is iteratively estimated through prediction and update steps to achieve multi-sensor data fusion.
[0016] Further, in step S2, the specific method of the dynamic keyframe selection strategy is as follows: calculate the relative pose change between the current frame and the nearest keyframe, including the translation distance Δd and the rotation angle Δθ; set the translation threshold Td and the rotation threshold Tθ, and when Δd > Td or Δθ > Tθ, mark the current frame as a candidate keyframe; calculate the feature matching degree between the candidate keyframe and the existing keyframe, and when the feature matching degree is lower than the set threshold, confirm the candidate keyframe as a new keyframe and add it to the map update; at the same time, redundant keyframes are removed based on the timestamp and spatial distribution of the keyframes to maintain the compactness of the map.
[0017] Further, in step S3, the specific method of the adaptive particle sampling strategy is as follows: set the particle number range [Nmin, Nmax], and dynamically adjust the particle number according to the AGV positioning uncertainty; when the positioning uncertainty increases, increase the particle number to improve the positioning accuracy; when the positioning uncertainty decreases, reduce the particle number to reduce the computational load; adopt the importance resampling method, filter and copy particles according to the particle weight distribution, retain high-weight particles, and remove low-weight particles.
[0018] Further, in step S4, the specific implementation method of the improved multi-step A-Star algorithm is as follows: the single fixed step size of the traditional A-Star algorithm is extended into multiple step sizes {s1, s2, ..., sn}, each step size corresponds to a motion mode of the AGV; a heuristic function H(n) = α·D(n) + β·M(n) is designed to integrate kinematic constraints, where D(n) is the Euclidean distance from the current node to the target node, M(n) is the kinematic constraint cost, and α and β are weight coefficients; the kinematic constraint cost M(n) is calculated based on the omnidirectional movement characteristics of the Mecanum wheel AGV, including lateral movement cost, rotation cost, and acceleration constraint cost; during the path search process, the comprehensive cost F(n) = G(n) + H(n) is calculated for each extended node, where G(n) is the actual cost from the starting point to the current node, and the node with the smallest comprehensive cost is selected as the next extended node.
[0019] Furthermore, the kinematic constraint cost M(n) is calculated as follows: M(n) = w1·|θdiff| + w2·|v_change| + w3·|ω_change|, where θdiff is the deviation between the AGV's heading angle and the desired heading angle, v_change is the velocity change, ω_change is the angular velocity change, and w1, w2, and w3 are the heading angle deviation weight, velocity change weight, and angular velocity change weight, respectively; the weights are adaptively adjusted according to the AGV's dynamic characteristics and operating environment.
[0020] Further, in step S5, the specific implementation method of the fuzzy PID control algorithm is as follows: the lateral deviation e and heading angle deviation θe between the AGV and the planned path are used as inputs to the fuzzy controller, and the output of the fuzzy controller is the parameter adjustment amount ΔKp, ΔKi, and ΔKd of the PID controller; a fuzzy rule base is designed, and the parameter adjustment amount is determined according to the magnitude and rate of change of the deviation; the PID controller calculates the control output u(t) = Kp·e(t) + Ki·∫e(t)dt + Kd·de(t) / dt based on the adjusted parameters, and controls the linear velocity and angular velocity of the AGV to achieve path tracking.
[0021] Furthermore, in step S5, the specific method for dynamic obstacle detection and local obstacle avoidance is as follows: real-time environmental information within a preset range in front of the AGV is acquired using LiDAR and depth camera; point cloud data is segmented using a clustering algorithm to identify dynamic obstacles; the speed and direction of the dynamic obstacles are calculated to predict their position at the time of AGV arrival; when the predicted position conflicts with the AGV's planned path, a dynamic window method is used to plan an obstacle avoidance path within a local range, bypassing the dynamic obstacles and returning to the global path.
[0022] This invention achieves high-precision environmental perception by fusing multiple sensors, including lidar and depth cameras, fully leveraging the high ranging accuracy of lidar and the rich information provided by depth cameras. Lidar can acquire precise distance information, while depth cameras provide color texture information and three-dimensional spatial information about the environment. The complementary fusion of these two sensors effectively improves the completeness and accuracy of environmental perception, especially in complex lighting conditions and scenes with transparent objects.
[0023] This invention employs an extended Kalman filter algorithm for multi-sensor data fusion, effectively addressing the time synchronization and spatial registration issues of sensor data, thereby improving the accuracy and efficiency of data fusion. Simultaneously, the dynamic keyframe selection strategy adaptively selects keyframes based on environmental changes, avoiding the accumulation of redundant information and improving map update efficiency and map quality.
[0024] This invention employs a Monte Carlo localization algorithm combined with an adaptive particle sampling strategy, which can dynamically adjust the number of particles according to localization uncertainty. This reduces computational load while ensuring localization accuracy, thereby improving the real-time performance and robustness of the localization system.
[0025] The improved multi-step A-Star algorithm proposed in this invention incorporates the kinematic constraints of the Mecanum wheel AGV into the heuristic function, fully considering the motion characteristics of omnidirectional AGVs. This results in a smoother path that is more suitable for actual AGV execution. The multi-step strategy effectively reduces the number of search nodes, improves search efficiency, and generates paths with fewer turning points, which is beneficial for the smooth operation of the AGV.
[0026] This invention employs a fuzzy PID control algorithm for path tracking, which adaptively adjusts control parameters based on path deviation, improving the adaptability and robustness of the control system. Simultaneously, dynamic obstacle detection and local obstacle avoidance functions effectively address dynamic changes in the environment, ensuring the safe operation of the AGV.
[0027] The second aspect of this invention provides an AGV autonomous mapping and path planning system based on multi-sensor fusion, comprising: a data acquisition module for acquiring two-dimensional point cloud data of the environment via LiDAR, and simultaneously acquiring depth and color images of the environment via a depth camera, and performing coordinate transformation on the depth images to generate three-dimensional point cloud data; a data fusion and mapping module for performing spatiotemporal registration and fusion of LiDAR point cloud data and depth camera point cloud data using an extended Kalman filter algorithm, constructing an environmental grid map based on the fused point cloud data, and simultaneously optimizing the map update process using a dynamic keyframe selection strategy; a positioning module for achieving real-time positioning of the AGV in the environment based on the constructed environmental grid map using a Monte Carlo positioning algorithm combined with an adaptive particle sampling strategy; a path planning module for performing global path planning based on an improved multi-step A-Star algorithm; and a path tracking and obstacle avoidance module for controlling the AGV to travel along the planned global path using a fuzzy PID control algorithm, and simultaneously performing dynamic obstacle detection and local obstacle avoidance based on real-time sensor data. Attached Figure Description
[0028] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the embodiments will be briefly introduced below. 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.
[0029] Figure 1 This is a flowchart illustrating the AGV autonomous mapping and path planning method based on multi-sensor fusion provided in an embodiment of the present invention.
[0030] Figure 2 This is a schematic diagram of the multi-sensor data acquisition and fusion process provided in an embodiment of the present invention;
[0031] Figure 3 This is a schematic diagram of the improved multi-step A-Star algorithm provided in an embodiment of the present invention;
[0032] Figure 4 This is a schematic diagram of the dynamic keyframe selection strategy provided in an embodiment of the present invention;
[0033] Figure 5 This is a schematic diagram of the AGV autonomous mapping and path planning system provided in the embodiments of the present invention. Detailed Implementation
[0034] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0035] Example 1
[0036] Please see Figure 1 , Figure 1 This is a flowchart illustrating the AGV autonomous mapping and path planning method based on multi-sensor fusion provided in this embodiment of the invention. Figure 1 As shown, the method includes the following steps:
[0037] Step S1, Multi-sensor Data Acquisition: This step acquires environmental information using two sensors: LiDAR and depth camera. The LiDAR, either single-line or dual-line, is installed on the top or front of the AGV to horizontally scan and acquire two-dimensional point cloud data of the environment. The depth camera, an RGB-D camera (such as Kinect or RealSense), is installed at the front of the AGV to acquire depth and color images of the environment. For depth images, coordinate transformation is required to generate 3D point cloud data. The specific method is as follows: obtain the intrinsic parameter matrix K and extrinsic parameter matrix [R|T] of the depth camera, where K contains the focal lengths fx and fy and the optical center coordinates cx and cy, R is the rotation matrix, and T is the translation vector; for each pixel (u,v) in the depth image, its corresponding depth value is d(u,v), then the 3D coordinates of the pixel in the camera coordinate system are: Xc = (u-cx)×d / fx, Yc = (v-cy)×d / fy, Zc =d; transform the points in the camera coordinate system to the world coordinate system through the extrinsic parameter matrix to obtain the 3D point cloud data for fusion.
[0038] Step S2, Data Fusion and Mapping: (e.g.) Figure 2As shown, this step employs the extended Kalman filter algorithm to perform spatiotemporal registration and fusion of LiDAR point cloud data and depth camera point cloud data. First, the data from both sensors need to be synchronized in time to ensure consistency in data acquisition timing. Then, spatial registration is performed using the calibrated sensor extrinsic parameters to transform the data from both sensors to the same coordinate system. The specific implementation of Extended Kalman Filter (EKF) is as follows: Establish the AGV's motion state equation and observation equation. The state equation is X(k+1) = F(k)X(k) + B(k)U(k) + W(k), and the observation equation is Z(k) = H(k)X(k) + V(k). Here, X(k) is the state vector, containing the AGV's position and attitude information; F(k) is the state transition matrix, determined based on the AGV's kinematic model; B(k) is the control input matrix; U(k) is the control vector, containing the AGV's velocity and angular velocity commands; W(k) is the process noise, following a Gaussian distribution; Z(k) is the observation vector, containing observation data from the LiDAR and depth camera; H(k) is the observation matrix; and V(k) is the observation noise. The AGV's pose state is iteratively estimated through prediction and update steps, achieving multi-sensor data fusion.
[0039] Figure 3 is a flowchart of the improved multi-step A-Star algorithm provided in this embodiment. As shown in Figure 3, the improved multi-step A-Star algorithm first initializes the starting point, ending point, and open list of the path planning; it then checks if the open list is empty. If it is not empty, it returns the initialized starting point, ending point, and open list of the path planning; if it is empty, it retrieves the node n with the smallest comprehensive cost F(n) from the open list; it then checks if node n is the target node. If it is not the target node, it returns the initialized starting point, ending point, and open list of the path planning; if it is the target node, it expands the neighboring nodes according to the preset multi-step strategy and updates the open list according to the heuristic function that incorporates the kinematic constraints of the Mecanum wheel AGV. The above steps are repeated until the search is completed.
[0040] During map building, a dynamic keyframe selection strategy is employed to optimize map updates. For example... Figure 4 As shown, the specific method is as follows: Calculate the relative pose change between the current frame and the nearest keyframe, including translation distance Δd and rotation angle Δθ; set translation threshold Td and rotation threshold Tθ. When Δd > Td or Δθ > Tθ, the current frame is marked as a candidate keyframe; calculate the feature matching degree between the candidate keyframe and existing keyframes. When the feature matching degree is lower than the set threshold, the candidate keyframe is confirmed as a new keyframe and added to the map update; simultaneously, redundant keyframes are removed based on the keyframe's timestamp and spatial distribution to maintain map compactness. This strategy can adaptively select keyframes according to environmental changes, avoid the accumulation of redundant information, and improve the efficiency of map updates.
[0041] Step S3, AGV Localization: This step utilizes the constructed environmental grid map and employs the Monte Carlo localization algorithm to achieve real-time AGV localization. Monte Carlo localization uses a set of weighted particles to represent the probability distribution of AGV pose. To improve localization efficiency and accuracy, an adaptive particle sampling strategy is adopted: a particle number range [Nmin, Nmax] is set, and the particle number is dynamically adjusted according to the AGV localization uncertainty; when localization uncertainty increases, the particle number is increased to improve localization accuracy; when localization uncertainty decreases, the particle number is reduced to reduce computational load; an importance resampling method is used to filter and replicate particles according to their weight distribution, retaining high-weight particles and discarding low-weight particles. Localization uncertainty can be measured by the variance or entropy of the particle distribution.
[0042] Step S4, Path Planning: This step performs global path planning based on the improved multi-step A-Star algorithm. The traditional A-Star algorithm uses a single fixed step size for node expansion, resulting in low search efficiency and insufficiently smooth paths. The improved multi-step A-Star algorithm proposed in this invention incorporates the kinematic constraints of the Mecanum wheel AGV into a heuristic function, such as... Figure 4 As shown, the specific implementation is as follows: the single fixed step size of the traditional A-Star algorithm is extended to multiple step sizes {s1, s2, ..., sn}, each step size corresponding to a movement mode of the AGV, such as forward, backward, lateral movement, diagonal movement, etc.; a heuristic function H(n) = α·D(n) + β·M(n) is designed to integrate kinematic constraints, where D(n) is the Euclidean distance from the current node to the target node, M(n) is the kinematic constraint cost, and α and β are weight coefficients; the kinematic constraint cost M(n) is calculated based on the omnidirectional movement characteristics of the Mecanum wheel AGV, including lateral movement cost, rotation cost, and acceleration constraint cost, and the specific calculation formula is: M(n) = w1·|θdiff| + w2·|v_change| + w3·|ω_change|, where θdiff is the deviation between the AGV's heading angle and the desired heading angle, v_change is the speed change, ω_change is the angular velocity change, and w1, w2, and w3 are the heading angle deviation weight, speed change weight, and angular velocity change weight, respectively. During the path search process, the comprehensive cost F(n) = G(n) + H(n) is calculated for each extended node, where G(n) is the actual cost from the starting point to the current node, and the node with the minimum comprehensive cost is selected as the next extended node.
[0043] Step S5, Path Tracking and Obstacle Avoidance: This step uses a fuzzy PID control algorithm based on the planned global path to control the AGV to travel along the planned path. The specific implementation of fuzzy PID control is as follows: the lateral deviation e and heading angle deviation θe between the AGV and the planned path are used as inputs to the fuzzy controller, and the outputs of the fuzzy controller are the parameter adjustments ΔKp, ΔKi, and ΔKd of the PID controller. A fuzzy rule base is designed to determine the parameter adjustments based on the magnitude and rate of change of the deviation. For example, when the deviation is large, Kp is increased to improve the response speed, and when the deviation is small, Kp is decreased to avoid overshoot. The PID controller calculates the control output u(t) = Kp·e(t) + Ki·∫e(t)dt + Kd·de(t) / dt based on the adjusted parameters, controlling the linear and angular velocities of the AGV to achieve path tracking.
[0044] Simultaneously, dynamic obstacle detection and local obstacle avoidance are performed based on real-time sensor data. Specifically, environmental information within a preset range (e.g., 3 meters) in front of the AGV is acquired in real-time using LiDAR and a depth camera. A clustering algorithm (e.g., DBSCAN) is used to segment the point cloud data and identify dynamic obstacles. The speed and direction of the dynamic obstacles are calculated to predict their positions at the time of AGV arrival. When the predicted position conflicts with the AGV's planned path, a dynamic window method is used to plan an obstacle avoidance path within a local area, bypassing the dynamic obstacle and returning to the global path. The dynamic window method considers the kinematic and dynamic constraints of the AGV, searching for the optimal speed command in the velocity space, enabling the AGV to safely and smoothly avoid obstacles.
[0045] Example 2
[0046] Please see Figure 5 A schematic diagram of the AGV autonomous mapping and path planning system provided in this embodiment of the invention. The system includes:
[0047] The data acquisition module is used to acquire 2D point cloud data of the environment via LiDAR, and simultaneously acquire depth and color images of the environment via a depth camera. It then performs coordinate transformation on the depth images to generate 3D point cloud data. The data acquisition module includes a LiDAR unit, a depth camera unit, and a coordinate transformation unit. The LiDAR unit controls the LiDAR to scan the environment and acquire 2D point cloud data. The depth camera unit controls the depth camera to acquire depth and color images. The coordinate transformation unit is responsible for converting the depth images into 3D point cloud data and performing coordinate system unification.
[0048] The data fusion and mapping module performs spatiotemporal registration and fusion of LiDAR point cloud data and depth camera point cloud data using the Extended Kalman Filter (EKF) algorithm. Based on the fused point cloud data, it constructs an environmental raster map and employs a dynamic keyframe selection strategy to optimize the map update process. The module includes a time synchronization unit, a spatial registration unit, a Kalman filtering unit, a raster map construction unit, and a keyframe selection unit. The time synchronization unit aligns the timestamps of the multi-sensor data. The spatial registration unit transforms data from different sensors to the same coordinate system. The Kalman filtering unit executes the EKF algorithm for data fusion. The raster map construction unit constructs an environmental raster map based on the fused point cloud data. The keyframe selection unit executes a dynamic keyframe selection strategy.
[0049] The localization module, based on a constructed environmental grid map, employs a Monte Carlo localization algorithm combined with an adaptive particle sampling strategy to achieve real-time localization of the AGV within the environment. The localization module includes a particle initialization unit, a motion prediction unit, an observation update unit, a weight calculation unit, and an adaptive resampling unit. The particle initialization unit initializes the particle swarm. The motion prediction unit predicts the particle states based on the motion model. The observation update unit updates the particle weights based on observation data. The weight calculation unit calculates the weight of each particle. The adaptive resampling unit adaptively adjusts the number of particles and performs resampling based on localization uncertainty.
[0050] The path planning module is used for global path planning based on an improved multi-step A-Star algorithm. The module includes an environment modeling unit, a heuristic function computation unit, a node expansion unit, and a path backtracking unit. The environment modeling unit is responsible for converting the raster map into a graph structure usable for path planning. The heuristic function computation unit is responsible for calculating the heuristic function values that incorporate kinematic constraints. The node expansion unit is responsible for expanding the search nodes according to a multi-step strategy. The path backtracking unit is responsible for generating the final path by backtracking from the target node.
[0051] The path tracking and obstacle avoidance module is used to control the AGV along a planned global path using a fuzzy PID control algorithm, while simultaneously performing dynamic obstacle detection and local obstacle avoidance based on real-time sensor data. The module includes a deviation calculation unit, a fuzzy controller unit, a PID controller unit, an obstacle detection unit, and a local obstacle avoidance unit. The deviation calculation unit calculates the lateral and heading angle deviations between the AGV and the planned path. The fuzzy controller unit calculates the PID parameter adjustments based on the deviations. The PID controller unit calculates the control output. The obstacle detection unit detects dynamic obstacles in the environment. The local obstacle avoidance unit plans local obstacle avoidance paths.
[0052] Example 3
[0053] This embodiment uses a logistics AGV in a smart factory as an application scenario to describe in detail the specific implementation process of the present invention. The AGV uses a Mecanum wheel chassis and is equipped with a single-line LiDAR (scanning frequency 15Hz, ranging range 0.1-30m) and an RGB-D depth camera (resolution 640×480, frame rate 30fps) for material transportation within the factory workshop.
[0054] During the multi-sensor data acquisition phase, the LiDAR is horizontally mounted on top of the AGV, with a scanning angle range of 270° and an angular resolution of 0.25°, acquiring 1080 point cloud data points per frame. A depth camera is mounted at the front of the AGV with a pitch angle of 15°, used to acquire environmental information within a 1-4 meter range in front of the AGV. The extrinsic parameter matrix between the LiDAR and depth camera is obtained through calibration and used for subsequent data fusion. When converting the depth image into a 3D point cloud, the camera intrinsic parameters fx=fy=525 pixels, cx=319.5 pixels, and cy=239.5 pixels are used.
[0055] In the data fusion and mapping phase, the extended Kalman filter algorithm is used for data fusion. The state vector X(k) contains the AGV's two-dimensional position (x, y) and heading angle θ. The state transition matrix F(k) is determined based on the AGV's kinematic model, and the process noise covariance matrix Q and observation noise covariance matrix R are obtained through experimental calibration. The grid map resolution is 0.05m, and the translation threshold Td = 0.5m and rotation threshold Tθ = 15° are selected for dynamic keyframes.
[0056] During the AGV localization phase, the particle count for Monte Carlo localization ranges from [Nmin=500, Nmax=3000]. Initially, 3000 particles are used, gradually reduced to 500 particles after localization convergence. Resampling employs a systematic resampling method, with an effective particle count threshold set at Neff = 0.5N. Resampling is performed when the effective particle count falls below this threshold.
[0057] In the path planning phase, the improved multi-step A-Star algorithm uses five step sizes {s1=0.1m, s2=0.2m, s3=0.3m, s4=0.4m, s5=0.5m}, corresponding to the AGV's fine movement, normal movement, and fast movement modes, respectively. The heuristic function weights are α=0.6 and β=0.4. The kinematic constraint cost weights are w1=0.4, w2=0.3, and w3=0.3. Compared to the standard A-Star algorithm, the improved algorithm reduces the number of search nodes by approximately 35%, the number of path turning points by approximately 50%, and the path length by approximately 8%.
[0058] During the path tracking and obstacle avoidance phase, the fuzzy sets of the input variables lateral deviation e and heading angle deviation θe of the fuzzy PID controller are {negative large, negative medium, negative small, zero, positive small, positive medium, positive large}, and the fuzzy sets of the output variables ΔKp, ΔKi, and ΔKd are {very small, small, medium, large, very large}. The dynamic obstacle detection range is set to 3 meters in front of the AGV. The DBSCAN clustering algorithm is used to identify obstacles, with clustering parameters ε=0.2m and MinPts=5. When a dynamic obstacle is detected, a dynamic window method is used for local obstacle avoidance. The velocity search space is vx∈[-1, 1]m / s, vy∈[-0.5, 0.5]m / s, ω∈[-π / 2, π / 2]rad / s.
[0059] Using the method of this invention, the AGV achieves high-precision autonomous navigation in a factory workshop, with a positioning accuracy of ±2cm and a path tracking error of less than ±3cm. It can effectively avoid dynamic obstacles, meeting the needs of industrial applications. Compared with existing technologies, this invention significantly improves positioning accuracy, path planning efficiency, and path tracking performance.
[0060] The foregoing has provided a detailed description of an AGV autonomous mapping and path planning method and system based on multi-sensor fusion, as provided in the embodiments of the present invention. Specific examples have been used to illustrate the principles and implementation methods of the present invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of the present invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of the present invention. Therefore, the content of this specification should not be construed as a limitation of the present invention.
Claims
1. A method for autonomous mapping and path planning of AGVs based on multi-sensor fusion, characterized in that, Includes the following steps: S1. Multi-sensor data acquisition: Two-dimensional point cloud data of the environment is acquired through LiDAR, and depth and color images of the environment are acquired through depth camera. The coordinate transformation of the depth image is performed to generate three-dimensional point cloud data. S2. Data Fusion and Mapping: The extended Kalman filter algorithm is used to perform spatiotemporal registration and fusion of LiDAR point cloud data and depth camera point cloud data. An environmental raster map is constructed based on the fused point cloud data. At the same time, a dynamic keyframe selection strategy is used to optimize the map update process. S3, AGV positioning: Based on the constructed environmental grid map, the Monte Carlo positioning algorithm combined with an adaptive particle sampling strategy is used to achieve real-time positioning of AGV in the environment; S4. Path planning: Global path planning is performed based on the improved multi-step A-Star algorithm. The improved multi-step A-Star algorithm incorporates the kinematic constraints of the Mecanum wheel AGV into the heuristic function and achieves smooth path optimization by setting multiple step length parameters. S5. Path tracking and obstacle avoidance: Based on the planned global path, the fuzzy PID control algorithm is used to control the AGV to travel along the planned path, while dynamic obstacle detection and local obstacle avoidance are performed based on real-time sensor data.
2. The AGV autonomous mapping and path planning method based on multi-sensor fusion according to claim 1, characterized in that, In step S1, the specific method for generating 3D point cloud data by coordinate transformation of the depth image is as follows: obtain the intrinsic parameter matrix K and extrinsic parameter matrix [R|T] of the depth camera, where K contains the focal lengths fx and fy and the optical center coordinates cx and cy, R is the rotation matrix, and T is the translation vector; for each pixel (u,v) in the depth image, its corresponding depth value is d(u,v), then the 3D coordinates of the pixel in the camera coordinate system are: Xc = (u-cx)×d / fx, Yc = (v-cy)×d / fy, Zc = d; transform the points in the camera coordinate system to the world coordinate system through the extrinsic parameter matrix to obtain the 3D point cloud data for fusion.
3. The AGV autonomous mapping and path planning method based on multi-sensor fusion according to claim 1, characterized in that, In step S2, the specific method for data fusion using the extended Kalman filter algorithm is as follows: Establish the AGV motion state equation and observation equation. The state equation is X(k+1) = F(k)X(k) + B(k)U(k) + W(k), and the observation equation is Z(k) = H(k)X(k) + V(k), where X(k) is the state vector, F(k) is the state transition matrix, B(k) is the control input matrix, U(k) is the control vector, W(k) is the process noise, Z(k) is the observation vector, H(k) is the observation matrix, and V(k) is the observation noise. The AGV's pose state is iteratively estimated through prediction and update steps to achieve multi-sensor data fusion.
4. The AGV autonomous mapping and path planning method based on multi-sensor fusion according to claim 1, characterized in that, In step S2, the specific method of the dynamic keyframe selection strategy is as follows: calculate the relative pose change between the current frame and the nearest keyframe, including the translation distance Δd and the rotation angle Δθ; set the translation threshold Td and the rotation threshold Tθ, and when Δd > Td or Δθ > Tθ, mark the current frame as a candidate keyframe; calculate the feature matching degree between the candidate keyframe and the existing keyframe, and when the feature matching degree is lower than the set threshold, confirm the candidate keyframe as a new keyframe and add it to the map update; at the same time, redundant keyframes are removed based on the timestamp and spatial distribution of the keyframes to maintain the compactness of the map.
5. The AGV autonomous mapping and path planning method based on multi-sensor fusion according to claim 1, characterized in that, In step S3, the specific method of the adaptive particle sampling strategy is as follows: set the particle number range [Nmin, Nmax], and dynamically adjust the particle number according to the AGV positioning uncertainty; when the positioning uncertainty increases, increase the particle number to improve the positioning accuracy; when the positioning uncertainty decreases, reduce the particle number to reduce the computational load; use the importance resampling method to filter and copy particles according to the particle weight distribution, retain high-weight particles, and remove low-weight particles.
6. The AGV autonomous mapping and path planning method based on multi-sensor fusion according to claim 1, characterized in that, In step S4, the specific implementation method of the improved multi-step A-Star algorithm is as follows: the single fixed step size of the traditional A-Star algorithm is extended into multiple step sizes {s1, s2, ..., sn}, each step size corresponds to a motion mode of the AGV; a heuristic function H(n) = α·D(n) + β·M(n) is designed to integrate kinematic constraints, where D(n) is the Euclidean distance from the current node to the target node, M(n) is the kinematic constraint cost, and α and β are weight coefficients; the kinematic constraint cost M(n) is calculated based on the omnidirectional movement characteristics of the Mecanum wheel AGV, including lateral movement cost, rotation cost, and acceleration constraint cost; during the path search process, the comprehensive cost F(n) = G(n) + H(n) is calculated for each extended node, where G(n) is the actual cost from the starting point to the current node, and the node with the smallest comprehensive cost is selected as the next extended node.
7. The AGV autonomous mapping and path planning method based on multi-sensor fusion according to claim 6, characterized in that, The kinematic constraint cost M(n) is calculated as follows: M(n) = w1·|θdiff| + w2·|v_change| + w3·|ω_change|, where θdiff is the deviation between the AGV's heading angle and the desired heading angle, v_change is the speed change, ω_change is the angular velocity change, and w1, w2, and w3 are the heading angle deviation weight, speed change weight, and angular velocity change weight, respectively; the weights are adaptively adjusted according to the AGV's dynamic characteristics and operating environment.
8. The AGV autonomous mapping and path planning method based on multi-sensor fusion according to claim 1, characterized in that, In step S5, the specific implementation method of the fuzzy PID control algorithm is as follows: the lateral deviation e and heading angle deviation θe between the AGV and the planned path are used as the input of the fuzzy controller, and the output of the fuzzy controller is the parameter adjustment amount ΔKp, ΔKi, and ΔKd of the PID controller. A fuzzy rule base is designed to determine the parameter adjustment amount based on the magnitude and rate of change of the deviation. The PID controller calculates the control output u(t) = Kp·e(t) + Ki·∫e(t)dt + Kd·de(t) / dt based on the adjusted parameters, and controls the linear speed and angular speed of the AGV to achieve path tracking.
9. The AGV autonomous mapping and path planning method based on multi-sensor fusion according to claim 1, characterized in that, In step S5, the specific method for dynamic obstacle detection and local obstacle avoidance is as follows: real-time environmental information within a preset range in front of the AGV is acquired using LiDAR and depth camera; point cloud data is segmented using a clustering algorithm to identify dynamic obstacles; the speed and direction of the dynamic obstacles are calculated to predict their position at the time of AGV arrival; when the predicted position conflicts with the AGV's planned path, a dynamic window method is used to plan an obstacle avoidance path within a local range, bypassing the dynamic obstacles and returning to the global path.
10. An AGV autonomous mapping and path planning system based on multi-sensor fusion, characterized in that, include: The data acquisition module is used to acquire two-dimensional point cloud data of the environment through LiDAR, and simultaneously acquire depth and color images of the environment through a depth camera, and perform coordinate transformation on the depth images to generate three-dimensional point cloud data. The data fusion and mapping module is used to perform spatiotemporal registration and fusion of LiDAR point cloud data and depth camera point cloud data using the extended Kalman filter algorithm, and to build an environmental raster map based on the fused point cloud data. At the same time, a dynamic keyframe selection strategy is used to optimize the map update process. The positioning module is used to achieve real-time positioning of the AGV in the environment based on the constructed environmental grid map, using the Monte Carlo positioning algorithm combined with an adaptive particle sampling strategy. The path planning module is used for global path planning based on the improved multi-step A-Star algorithm. The improved multi-step A-Star algorithm incorporates the kinematic constraints of the Mecanum wheel AGV into the heuristic function and achieves smooth path optimization by setting multiple step length parameters. The path tracking and obstacle avoidance module is used to control the AGV to travel along the planned global path using a fuzzy PID control algorithm, while performing dynamic obstacle detection and local obstacle avoidance based on real-time sensor data.