A dynamic obstacle avoidance control method for a laser-guided AGV

Abstract

A dynamic obstacle avoidance control method for laser-guided AGVs aims to improve the AGV's obstacle avoidance capability against moving obstacles. This invention relates to the field of autonomous driving. The method includes an obstacle perception module, an obstacle prediction module, an improved TEB planning controller, and an AGV. Specifically, a laser radar sensor on the AGV inputs point cloud information to the obstacle perception module for clustering processing, generating obstacle cluster information. The obstacle prediction module receives the obstacle cluster information and predicts the future trajectory of the obstacles. The improved TEB planning controller integrates the future trajectory of the obstacles and vehicle state information, and outputs angular velocity and linear velocity through multi-objective optimization to control the AGV to perform obstacle avoidance actions, thereby improving the AGV's dynamic obstacle avoidance performance.

Description

Technical fields:

[0001] This invention belongs to the field of autonomous driving, specifically a dynamic obstacle avoidance control method for laser-guided AGVs. Background technology:

[0002] With the continuous upgrading of modern factory production automation and intelligent logistics systems, traditional manual transportation and semi-automated operation modes can no longer meet the demands of efficient, precise, and flexible industrial processes. Against the backdrop of intelligent manufacturing and flexible production becoming mainstream trends, the widespread application of automated control production equipment has not only significantly improved factory production efficiency and logistics speed but has also become a key driving force for high-quality socio-economic development. In this process, Automated Guided Vehicles (AGVs), as core logistics equipment, are gradually replacing traditional transportation methods and are widely used in scenarios such as warehouse management, production line material distribution, and finished product handling, thanks to their autonomous navigation, flexible scheduling, and high reliability. Currently, AGV guidance technologies are mainly divided into three types: magnetic tape guidance, QR code guidance, and laser guidance. Among them, laser-guided AGVs, equipped with high-precision laser radar, can scan the environment in real time and build maps to achieve autonomous path planning and positioning navigation. Compared with traditional guidance methods, they have significant advantages: on the one hand, laser guidance does not rely on physical markers such as magnetic strips and QR codes, which greatly reduces site modification costs and maintenance complexity; on the other hand, the high resolution and fast response characteristics of laser radar enable AGVs to achieve high-precision positioning in complex and ever-changing industrial environments, while also possessing stronger environmental adaptability, providing solid technical support for the intelligent upgrading and flexible production of factories.

[0003] During material handling or other tasks, AGVs inevitably encounter dynamic obstacles such as personnel movement and other mobile devices. Currently, AGVs employ two obstacle avoidance methods: one is to adjust the global path to avoid dynamic obstacles, but this reduces path continuity and increases computational complexity, also affecting the AGV's task completion time and efficiency; the other is to use a local path planning controller for dynamic obstacle avoidance. The Time Elastic Band (TEB) algorithm is a commonly used local path planning controller for AGVs, offering the advantages of dynamically optimizing paths while balancing obstacle avoidance efficiency and kinematic constraints. However, traditional TEB relies on a cost map for dynamic obstacle avoidance. Because dynamic obstacles cannot be updated in real time and data processing is delayed, planning response is lagging, leading to frequent replanning and even collision risks.

[0004] Patent CN120043527A proposes a path planning method combining TEB dynamic obstacle avoidance and an improved bidirectional A* algorithm. It improves global path efficiency through 5-neighborhood bidirectional search and utilizes TEB for local obstacle avoidance, enhancing path coherence in static environments. However, patent CN120043527A does not incorporate obstacle trajectory prediction, relying solely on TEB optimization based on the current position, making it ill-suited for dynamic obstacles. Furthermore, patent CN120043527A employs a global-local decoupling architecture, requiring a restart of planning when the path is blocked, resulting in poor robustness. Patent CN118938924A proposes a dynamic obstacle avoidance method integrating Kalman filtering and an improved dynamic window algorithm (DWA). It can predict obstacle trajectories and optimize the DWA evaluation function, improving dynamic obstacle avoidance performance. However, the prediction results of patent CN118938924A are not converted into spatiotemporal constraints, lacking active obstacle avoidance capabilities. Additionally, the DWA evaluation function primarily relies on geometric distance, neglecting the threat of velocity vector angles, making collisions more likely in dynamic, dense scenes. Summary of the Invention:

[0005] To address the shortcomings of existing technologies, this invention proposes a dynamic obstacle avoidance control method for laser-guided AGVs. This method uses a laser radar point cloud information mounted on the AGV to generate obstacle cluster information via an obstacle perception module; an obstacle prediction module predicts the future trajectory of obstacles based on these obstacle clusters; and an improved TEB planning controller integrates the future trajectory of obstacles with the vehicle state information output by the AGV, solves the local path using a multi-objective optimization method, and outputs the AGV's angular velocity and linear velocity to control the AGV to perform obstacle avoidance actions, thereby improving the AGV's obstacle avoidance capability against dynamic obstacles.

[0006] The technical solution adopted by this invention to solve the technical problem is as follows:

[0007] This invention relates to a dynamic obstacle avoidance control method for laser-guided AGVs, which includes an obstacle perception module, an obstacle prediction module, an improved TEB planning controller, and the AGV.

[0008] The method includes the following steps:

[0009] Step 1: Obstacle Perception Module Design:

[0010] The AGV converts the raw LiDAR data into polar coordinates (r n ,θ n The data is passed to the obstacle perception module, where (r) n ,θ n () represents the distance and angle of the nth laser point. (x) n =r n cosθ n y n =rn sinθ n The polar coordinate data of the LiDAR is converted into Cartesian coordinate data. Furthermore, coordinate transformation is used to map the LiDAR Cartesian point cloud data to the global coordinate system, achieving uniformity in point cloud positions and ensuring consistent obstacle positions.

[0011] Based on the Euclidean distance d(p,q), a density-based clustering algorithm is used to cluster the lidar point cloud information, as shown in equations (1) to (2):

[0012]

[0013] N (p) ={q|d(p,q)≤ε} (2)

[0014] In the formula, p = (x p ,y p ),q=(x q ,y q ) are the coordinates of the two laser beams from the lidar, respectively; N (p) ε is the neighborhood set; ε is the neighborhood radius.

[0015] If a laser point contains at least MinPts points in its neighborhood, then this laser point is considered a core point and expands to form an obstacle cluster, achieving effective obstacle separation. Here, MinPts represents the minimum number of points required to form a high-density region within the laser point's neighborhood.

[0016] Step 2, Obstacle Prediction Module Design:

[0017] To distinguish the temporal positional changes of point cloud data for static and dynamic obstacles detected by lidar, target matching and motion estimation methods are used to track the motion state of the obstacles.

[0018] Assume the obstacle prediction module obtains two frames of point cloud data P at times t1 and t2 respectively. clt1 and P clt2 Then the motion state of the obstacle can be matched by the nearest neighbor search method, as shown in equation (3):

[0019]

[0020] In the formula, p z It is point cloud data P clt1 The center point of the obstacle in the middle, p t2 It is point cloud data P clt2 Any laser point in the array, It is p z In point cloud data P clt2 The nearest obstacle center matching point.

[0021] If the matching error of an obstacle in two frames of data Greater than the set threshold d th As shown in equation (4):

[0022]

[0023] Then, the obstacle is determined to be a dynamic obstacle, and the velocity of the obstacle's center is calculated, as shown in equations (5) and (6):

[0024]

[0025]

[0026] In the formula, (x1, y1) are the coordinates of the obstacle center at time t1, (x2, y2) are the coordinates of the obstacle center at time t2, and v x ,y y These are the velocity components of the obstacle's center on the x and y axes, respectively.

[0027] Based on obstacle matching, Kalman filtering is used to track moving obstacles, so as to smooth the movement trajectory of the obstacles and predict the future movement trajectory of the obstacles. The state vector is defined as shown in equation (7):

[0028] X k =[x k ,y k ,v x ,v y ] T (7)

[0029] The state update equations for the Kalman filter are shown in equations (8) and (9):

[0030] X k =FX k-1 +ω k (8)

[0031] z k =Hx k +v k (9)

[0032] In the formula, X k Z is the state vector of the obstacle at time k, including the position coordinates and velocity information of the obstacle's center; F is the state transition matrix, used to predict the obstacle's position at time k+1; H is the observation matrix; z k ω represents the obstacle state vector measured by the lidar at time k; k and v k These are process noise and measurement noise, respectively.

[0033] By using Kalman filtering, the system can smoothly track obstacle trajectories and suppress measurement noise. Simultaneously, the geometric boundary dimensions of dynamic obstacles are extracted as spatial constraint parameters for the TEB obstacle avoidance model. Based on trajectory estimation, the system predicts the future position of the obstacle using a uniform velocity model, assuming it maintains a constant velocity for a short period. The predicted future trajectory of the obstacle is shown in equations (10) and (11):

[0034] x k+Δk =x k +v x ·Δk (10)

[0035] y k+Δk =y k +y y ·Δk (11)

[0036] In the formula, x k ,y k v is the x-coordinate and y-coordinate of the obstacle at time k. x ,y y These are the horizontal and lateral velocities of the obstacle, respectively, and Δk is the prediction step size.

[0037] Step 3: Design of an improved TEB planning controller:

[0038] Step 3.1, Information Input:

[0039] The improved TEB planning controller performs dynamic obstacle avoidance, mainly relying on the future trajectory of obstacles and the vehicle status information of AGVs.

[0040] The future trajectory of the obstacle: includes the position sequence of all obstacles within the future time window T.

[0041] AGV vehicle status information consists of the AGV vehicle's pose, linear velocity, and angular velocity. A timestamp synchronization strategy is used to eliminate timing discrepancies between sensing and planning, ensuring data timeliness.

[0042] Step 3.2: The improved TEB planning controller constructs a dynamic obstacle avoidance constraint function that integrates distance constraints, time constraints, and velocity direction constraints based on the future trajectory of the obstacle and the AGV vehicle state information. The specific steps are as follows:

[0043] Step 3.2.1: Design of the distance constraint function in the dynamic obstacle avoidance constraint function:

[0044] To improve the obstacle avoidance capability of AGVs, a distance constraint function is introduced to limit the minimum distance between the AGV's waypoints and the future trajectories of obstacles. In the distance constraint function, obstacles are approximated as rectangles, and their width ω is used to define the distance. j and height h jThe distance between the AGV waypoint and the future trajectory of the obstacle is normalized as shown in equation (12):

[0045]

[0046] In the formula, x i ,y i These are the x and y coordinates of the AGV path points; x j (k),y j (k) represents the coordinates of the obstacle's future trajectory at time k; d ij (k) represents the minimum distance between the AGV trajectory point and the future trajectory of the obstacle; [k1,k2] is the time period for predicting the future trajectory of the obstacle; i is the AGV path point index; j is the obstacle future trajectory point index.

[0047] Define distance constraint function E d_p Used to control the safe distance between the AGV and obstacles, as shown in equation (13):

[0048]

[0049] In the formula, λ d This is the penalty weight for the distance constraint, used to adjust the degree of influence of the distance constraint in TEB programming; d safe It is a safe distance; d m It is a safety range adjusted based on the uncertainty of prediction.

[0050] In addition, to improve obstacle avoidance foresight, a dynamic selection mechanism for the future trajectory point index of obstacles is introduced:

[0051] First, calculate the current position p of the AGV. A =(x A ,y A ) and the current position p of the obstacle o =(x o ,y o The distance d) AGV-obs As in equation (14):

[0052]

[0053] Secondly, based on the distance between the AGV and the obstacle, the index j of the obstacle's future trajectory point is dynamically adjusted, as shown in equation (15):

[0054]

[0055] In the formula, m is the total length of the predicted trajectory point sequence, m = T predict / step_size, T predictIt is the prediction duration, step_size is the prediction step size, and d far This is the distance threshold between the AGV and obstacles. The argmin function is used to calculate the distance within the interval [1, m]. The smallest s value.

[0056] Step 3.2.2: Design of the obstacle avoidance time constraint function in the dynamic obstacle constraint function:

[0057] In local path planning, obstacle avoidance requires consideration of not only spatial constraints but also temporal constraints to prevent the AGV and dynamic obstacles from arriving at the same location simultaneously. The system selects the point closest to the AGV from the obstacle's future trajectory as the prediction point P. T Calculate the predicted arrival point P for both the AGV and the obstacle. T Time t A and t o As shown in equations (16) and (17):

[0058]

[0059]

[0060] Calculate the time difference Δt between the AGV and the obstacle reaching the predicted point, as shown in equation (18):

[0061] Δt=|t A -t O | (18)

[0062] In the formula, These are the position vectors of the predicted point, the current position of the AGV, and the current position of the obstacle, respectively. These are the velocity vectors of the AGV and the obstacle, respectively.

[0063] To ensure a sufficient time interval, a collision avoidance time constraint function E is introduced. t As in equation (19):

[0064] E t =λ t ·max(0,t safe -|Δt|) (19)

[0065] In the formula, λ t It is the weighting factor for the time constraint, t safe It is a predefined safety time interval; the max function is used to select 0 and t. safe The maximum value of -|Δt|.

[0066] Step 3.2.3: Design of velocity direction constraint function in dynamic obstacle avoidance constraint function:

[0067] Velocity direction constraints are introduced, including obstacle avoidance speed constraints and target alignment constraints, to ensure obstacle avoidance safety and efficiency. The obstacle avoidance speed constraint improves dynamic obstacle avoidance performance by predicting the future movement range of obstacles. In the obstacle avoidance speed constraint function, obstacles are modeled as ellipses with the current velocity direction as their major axis, covering the potential movement area of ​​the obstacles and providing real-time data for the TEB planner. First, the relative velocity v between the AGV and the obstacle is calculated. rel As in equation (20):

[0068] v rel =v A -v o (20)

[0069] In the formula, v A It is the speed of the AGV, v o It is the speed of the obstacle.

[0070] The obstacle is modeled as an ellipse, with the center of the ellipse being the current estimated position of the obstacle. The semi-major axis a and semi-minor axis b of the ellipse are defined as shown in equations (21) and (22):

[0071]

[0072]

[0073] In the formula, h j It is the initial circumference diameter of the obstacle, k. a It is the semi-major axis expansion coefficient, k b It is the semi-minor axis expansion coefficient.

[0074] To determine the position of the AGV relative to the obstacle modeling ellipse, an obstacle range distance function g(p) is established. AGV ), as in equation (23):

[0075]

[0076] The gradient of the obstacle range distance function is calculated for path adjustment, and an obstacle avoidance coefficient λ is added. avoid Construct the obstacle avoidance speed constraint function E avoid As in equation (24):

[0077]

[0078] In the formula, λ avoid It is the obstacle avoidance coefficient. Represents the function g(p) AGV The gradient vector of the function points in the direction of the fastest increase; the max function is used to find the values ​​of 0 and 1. The maximum value.

[0079] The target alignment constraint is used to ensure that the AGV moves towards the target and avoids retreating or deviating from its direction. Further, the AGV velocity vector is calculated. and the direction of the target The cosine value is given by equation (25):

[0080]

[0081] The target coefficient λ is introduced. align Design the target alignment constraint function E align As in equation (26):

[0082] E align =λ align ·(1-cosθ) 2 (26)

[0083] Step 3.3, Multi-objective optimization solution:

[0084] TEB uses a multi-objective optimizer to solve the cost function, which includes dynamic obstacle avoidance constraints, in real time. It also optimizes the path control point sequence and time intervals, ultimately outputting the AGV's linear and angular velocities to control the AGV and complete obstacle avoidance maneuvers.

[0085] The beneficial effects of this invention are as follows: This invention relates to a dynamic obstacle avoidance control method for laser-guided AGVs. This method is based on the laser radar on the AGV, which extracts point cloud information into obstacle clusters in real time through an obstacle perception module. It integrates the future movement trajectory of obstacles output by an obstacle prediction module, and adopts an improved TEB planning controller with integrated dynamic obstacle avoidance constraint functions. It combines the vehicle state information of the AGV to perform multi-objective optimization to determine the angular velocity and linear velocity of the AGV, and controls the AGV to perform obstacle avoidance, thereby improving the dynamic obstacle avoidance capability of the AGV. Attached image description:

[0086] Figure 1 This is a schematic diagram of a dynamic obstacle avoidance control method for a laser-guided AGV according to the present invention. Detailed implementation method:

[0087] The present invention will now be described in detail with reference to the accompanying drawings.

[0088] like Figure 1As shown, this invention is a dynamic obstacle avoidance control method for laser-guided AGVs, aiming to significantly improve the AGV's obstacle avoidance capability against moving obstacles. The method includes an obstacle perception module, an obstacle prediction module, an improved TEB planning controller, and an AGV. Specifically, a laser radar installed on the AGV inputs point cloud information to the obstacle perception module for clustering processing, generating obstacle clusters. The obstacle prediction module receives the obstacle clusters and predicts the future trajectory of the obstacles. The improved TEB planning controller, based on the future trajectory of the obstacles and the AGV's vehicle state information, optimizes and outputs the AGV's angular velocity and linear velocity through multi-objective solution, controlling the AGV to perform obstacle avoidance actions, thereby improving the AGV's dynamic obstacle avoidance performance. The method specifically includes the following steps:

[0089] Step 1: Obstacle Perception Module Design:

[0090] Step 1.1, LiDAR data preprocessing:

[0091] The measurement data of lidar can be expressed in polar coordinates, as shown in equation (27):

[0092] p clt ={(r n ,θ n )|n=1,2,....,N} (27)

[0093] In the formula, p clt r represents the set of point clouds detected by the lidar. n θ is the ranging value of the nth laser point. n It is the angle corresponding to the laser point.

[0094] Furthermore, the polar coordinate data of the lidar is converted into rectangular coordinate data, as shown in equation (28):

[0095] x n =r n cosθ n y n =r n sinθ n (28)

[0096] Furthermore, coordinate transformation is used to map the LiDAR Cartesian coordinate point cloud data to the global coordinate system, achieving uniformity in point cloud positions and ensuring consistent obstacle locations. In addition, to reduce noise interference, the obstacle perception module employs a local density filtering method to remove outliers, retaining only high-density point clouds within the forward range, thus improving obstacle detection accuracy.

[0097] Step 1.2, Clustering and Identification of Dynamic Obstacles:

[0098] The density-based spatial clustering algorithm DBSCAN is used for point cloud clustering, grouping points that are close together into the same obstacle cluster and removing noise points. The core calculation formulas are shown in formulas (1) and (2) in the invention description. Among them, the number of MinPts points required to form a high-density region within the laser point neighborhood set is 5.

[0099] If a laser point contains no fewer than 5 laser points in its neighborhood set, then this laser point is considered a core point and expands to form an obstacle cluster, thereby achieving effective extraction of obstacles.

[0100] Step 2, Obstacle Prediction Module Design:

[0101] Step 2.1, Dynamic Obstacle Tracking:

[0102] The main difference between static and dynamic obstacles lies in the temporal positional changes of the point cloud. To distinguish between static and dynamic obstacles, target matching and motion estimation methods are employed to analyze and compare LiDAR point cloud data from multiple consecutive time points, thereby tracking the motion state of the obstacles.

[0103] Suppose that two frames of point cloud data P are obtained at times t1 and t2 respectively. clt1 and P clt2 Then the motion state of each obstacle can be matched using the nearest neighbor search method, as shown in formula (3) in the invention. If the matching error of an obstacle in two frames of data is greater than the distance threshold d, th If the obstacle is determined to be a dynamic obstacle, then for the center point of the matched obstacle, the speed is calculated according to formulas (5) and (6) in the invention. Wherein, the distance threshold d... th It is set to 0.2 meters.

[0104] Based on obstacle matching, this module uses Kalman filtering to track moving obstacles, smoothing their trajectory and predicting their future trajectory. The state vector is defined as shown in formula (7) in the invention description, and the state update equations for Kalman filtering are shown in formulas (8) and (9) in the invention description. Through Kalman filtering, the system can smoothly track the trajectory of obstacles and effectively suppress errors caused by measurement noise. At the same time, the obstacle prediction module also extracts the geometric boundary dimensions of each dynamic obstacle point cloud as spatial constraint parameters for constructing the TEB dynamic obstacle avoidance constraint function.

[0105] Step 2.2, Prediction of the future trajectory of the obstacle:

[0106] After estimating the trajectory of the obstacle, this module further predicts the future trajectory of the obstacle. To predict the trajectory of the obstacle, this invention adopts a uniform motion model, assuming that the obstacle maintains a constant speed for a short period of time, and predicts the future trajectory of the obstacle according to formulas (10) and (11) in the invention. The prediction step size Δk is set to 0.2 seconds.

[0107] Step 3: Design of an improved TEB planning controller:

[0108] Step 3.1, Information Input:

[0109] The improved TEB planning controller relies primarily on the future trajectory of obstacles and the vehicle status information of the AGV for dynamic obstacle avoidance.

[0110] The future trajectory of the obstacle includes the spatiotemporal position sequence of all obstacles within a future time window T, where T is set to 3 seconds in this invention.

[0111] The vehicle status information of AGVs includes the AGV's pose, linear velocity, and angular velocity. A timestamp synchronization strategy is used to eliminate timing discrepancies between sensing and planning, ensuring data timeliness.

[0112] Step 3.2: The improved TEB planning controller constructs a dynamic obstacle avoidance constraint function that integrates distance constraints, time constraints, and velocity direction constraints based on the predicted future trajectory of obstacles and vehicle state information. This includes the following steps:

[0113] Step 3.2.1: Design of distance constraint function in dynamic obstacle avoidance function:

[0114] To improve obstacle avoidance capabilities, a distance constraint function is introduced to constrain the distance between AGV path points and the future trajectory points of obstacles.

[0115] In the distance constraint function, the AGV needs to approximate the geometry of the obstacle using rectangles based on the estimated point cloud geometric boundary dimensions. Further, the AGV trajectory point p is calculated. i With the predicted future trajectory point q of the obstacle j The minimum distance d between them ij (k), and through the width ω j and height h j The distance between the AGV pathpoint and the future trajectory of the obstacle is normalized, and the normalization formula is shown in formula (12) in the invention description. Wherein, the width ω j Set to 0.3 meters, height h j Set to 0.5 meters.

[0116] To ensure AGV path point pi The future trajectory point q of the obstacle j To maintain a sufficient safe distance between them, define a distance constraint function E. d_p Specifically, as shown in formula (13) in the invention description. Wherein, the penalty weight λ for the distance constraint... d The safe distance d between the AGV and obstacles is set to 2.0. safe The safety range d is set at 0.5 meters and adjusted based on the predicted uncertainty. m Set to 0.1 meters.

[0117] In addition, to improve obstacle avoidance foresight, a dynamic selection mechanism for the future trajectory point index of obstacles is introduced: First, based on the current position p of the AGV... A =(x A ,y A ) and the current position p of the obstacle o =(x o ,y o The distance d between the AGV and the obstacle is calculated according to formula (14) in the invention. AGV-obs Secondly, based on the distance between the AGV and the obstacle, the index j of the obstacle's future trajectory point is dynamically adjusted. The specific selection method is described in formula (15) within the invention description. In formula (15), the prediction duration T... predict The prediction time is 3.0 seconds, the prediction step size is 0.2 seconds, and the distance threshold d between the AGV and the obstacle is... far Set to 0.5 meters.

[0118] Step 3.2.2: Design of the obstacle avoidance time constraint function in the dynamic obstacle avoidance constraint function:

[0119] In AGV local path planning, safe obstacle avoidance involves not only spatial obstacle avoidance but also temporal prediction and avoidance. Time constraints aim to predict the probability of the AGV and the obstacle arriving at the same location simultaneously, ensuring a sufficient time interval between them to avoid potential collision risks. The core of time constraints is calculating the estimated time for the AGV and the dynamic obstacle to reach the same location and ensuring a sufficient time difference between them to avoid potential collisions.

[0120] First, select a predicted point from the predicted future trajectories of dynamic obstacles that is closest to the AGV's position, and then calculate the arrival time P of the AGV and the obstacle at the predicted point. T Time t A and t o For details, see formulas (16) and (17) in the invention description.

[0121] To ensure sufficient time intervals, a time constraint penalty function E is introduced. tFor details, please refer to formula (19) in the invention description. Wherein, the weighting factor λ for the time constraint... t The predefined safety time interval t is 1.5. safe It takes 2.0 seconds.

[0122] Step 3.2.3: Design of velocity direction constraint function in dynamic obstacle avoidance constraint function:

[0123] To further improve obstacle avoidance performance, a velocity direction constraint is introduced in addition to the distance and time constraint functions. The velocity direction constraint function includes obstacle avoidance speed constraints and target alignment constraints to ensure that the AGV safely and efficiently avoids dynamic obstacles.

[0124] Step 3.2.3.1, Design of obstacle avoidance speed constraint function:

[0125] Obstacle avoidance velocity constraints not only consider the current position of obstacles but also predict their potential future movement range, thus achieving more effective dynamic obstacle avoidance. Since elliptical models can accurately simulate the motion trends of obstacles and their potential impact on the environment, they are chosen for obstacle modeling when designing the obstacle avoidance velocity constraint function. The major axis of the ellipse is aligned with the direction of obstacle movement, and its length reflects the obstacle's velocity. This configuration allows the ellipse shape to effectively cover the area the obstacle might reach in the future, providing real-time obstacle avoidance data for the planning algorithm.

[0126] First, calculate the relative velocity v between the AGV's movement and the obstacle's movement. rel As shown in formula (20) in the invention description. Next, the obstacle is modeled as an ellipse, with the center of the ellipse being the current estimated position of the obstacle, and the semi-major axis a and semi-minor axis b of the ellipse are defined, as shown in formulas (21) and (22) in the invention description. Wherein, the initial circumference diameter h of the obstacle... j Set to 0.3 meters, semi-major axis expansion coefficient k a The value is set to 1.5, which is the semi-minor axis expansion coefficient k. b Set to 1.0.

[0127] To determine the position of the AGV relative to the modeled ellipse, the obstacle range distance function g(p) is calculated. AGV For details, see formula (23) in the invention description.

[0128] The gradient of the obstacle range distance function is calculated and can be used to adjust the path to avoid entering high-risk areas. An obstacle avoidance coefficient λ is also included. avoid Obstacle avoidance speed constraint penalty function E avoid See formula (24) in the invention description for details. The obstacle avoidance coefficient λ is also mentioned. avoid Set to 3.0.

[0129] Step 3.2.3.2, Target Alignment Constraint Design:

[0130] Target alignment constraints ensure that the AGV moves towards the target. Without this constraint, the AGV may deviate from the target direction on its trajectory, or even back up towards the target before adjusting its orientation. This unnecessary back up not only reduces trajectory efficiency but can also lead to danger. Target alignment constraints are applied to the AGV based on its velocity vector. and the direction of the target The cosine value is calculated as shown in formula (25) in the invention.

[0131] At the same time, the target coefficient λ is introduced. align Then the target alignment constraint function E align As shown in formula (26) in the invention description. Wherein, the target coefficient λ align Set to 2.0.

[0132] Step 3.3, Multi-objective optimization solution:

[0133] The TEB algorithm adjusts the trajectory in space and time by constructing an elastic band model. The core idea of ​​the TEB algorithm is to represent the robot's trajectory as a discrete pose sequence with added time intervals, incorporating time as a variable into trajectory optimization. This allows the robot's local path to optimize not only the spatial trajectory but also the temporal dimension, thus enabling direct optimization of dynamic constraints such as velocity and acceleration. The local path planned by TEB is a series of pose points P. i =[x i ,y i ,θ i ] T And define a time interval ΔT for adjacent poses, then the pose sequence S = {P} i} i＝0,...,n Time series τ={ΔT} i＝1,...,n-1 With trajectory sequence B * =(S,τ). The improved TEB planning controller considers the dynamic obstacle avoidance constraint function that integrates the above-mentioned distance constraint, time constraint, and velocity direction constraint during the solution process. Specifically, it includes the following steps:

[0134] Step 3.3.1, Design the TEB objective function, as shown in equations (29) and (30):

[0135]

[0136] B * =argmin B f(B) (30)

[0137] In the formula, f(B) is the overall objective function, f k(B) A single objective function, specifically including the distance constraint function, obstacle avoidance time constraint function, and velocity direction constraint function in the dynamic obstacle constraint function, γ k To constrain the weights, B * Output trajectory for TEB.

[0138] Step 3.3.2, Real-time optimization solution:

[0139] Within each control cycle, the improved TEB planning controller constructs a TEB objective function based on the current AGV pose, target point, dynamic obstacle state, and historical trajectory information. It then uses a sparse nonlinear graph optimization method to efficiently iterate and solve for the variables. The optimization result outputs a time-labeled optimal trajectory, which consists of a series of discrete control points and includes corresponding linear and angular velocity commands.

[0140] Step 3.3.3, Speed ​​Command Generation and AGV Control:

[0141] Extract the linear velocity and angular velocity of the first time step from the optimal trajectory with time tags in the optimization results, and use them as the control command for the current moment. Publish this command to the AGV via ROS message to control the AGV to perform obstacle avoidance actions.

[0142] Step 3.3.4, Feedback Loop and Update Mechanism:

[0143] In the next control cycle, the improved TEB planning controller reacquires the vehicle status information output by the AGV and the future trajectory of the obstacle output by the obstacle prediction module, constructs a multi-objective optimization problem, optimizes and solves the linear velocity and angular velocity of the AGV to control the AGV motion and ensure the safety of the AGV's motion.

[0144] In summary, this invention proposes a dynamic obstacle avoidance control method for laser-guided AGVs. The laser radar on the AGV inputs point cloud information into the obstacle perception module for clustering processing, outputting obstacle clusters. This information is then input into the obstacle prediction module to predict the future trajectory of the obstacles. The improved TEB planning controller receives the future trajectory of the obstacles and the vehicle status information of the AGV, plans a local path through multi-objective optimization, and outputs the angular velocity and linear velocity of the AGV, controlling the AGV to perform obstacle avoidance actions, thereby improving the dynamic obstacle avoidance capability of the AGV.

Claims

1. A dynamic obstacle avoidance control method for a laser-guided AGV, the method comprising an obstacle perception module, an obstacle prediction module, an improved TEB planning controller, and the AGV; The obstacle perception module receives point cloud information output by the lidar on the AGV and performs coordinate transformation to eliminate spatial reference deviation of the point cloud information caused by changes in the AGV's pose. Secondly, a local density filtering method is used to remove outliers in the point cloud information. Finally, an obstacle cluster is generated based on Euclidean distance and neighborhood density constraints through a density clustering algorithm, including centroid and obstacle contour information. The obstacle prediction module analyzes and identifies dynamic obstacles based on the temporal matching and motion characteristics of obstacle clusters at adjacent time points, and uses Kalman filtering to predict the motion state of obstacles and suppress noise; at the same time, it extracts the spatial size parameters of obstacles and predicts the future motion trajectory of obstacles. The improved TEB planning controller constructs a dynamic obstacle avoidance constraint function that integrates distance constraints, obstacle avoidance time constraints, and velocity direction constraints based on the predicted future trajectory of obstacles and the vehicle state information output by the AGV. Through multi-objective optimization, it outputs angular velocity and linear velocity to control the AGV to perform obstacle avoidance actions; specifically as follows: The distance constraint function is used to limit the minimum distance between the AGV pathpoint and the future trajectory of the obstacle; in the distance constraint function, the obstacle is modeled and represented as a rectangle, and its width ω is used to define the distance. j and height h j The distance between the AGV path points and the future trajectories of obstacles is normalized; the minimum distance between the AGV path points and the future trajectories of obstacles is denoted by d. ij (k) is characterized by the following formula: In the formula, x i ,y i x represents the x and y coordinates of the AGV path point; j (k), y j (k) represents the coordinates of the obstacle's future trajectory at time k; [k1,k2] represents the time period for predicting the obstacle's future trajectory; i represents the AGV path point index; j represents the obstacle's future trajectory point index; Define distance constraint function E d_p Used to control the safe distance between the AGV and obstacles: In the formula, λ d This is the penalty weight for the distance constraint, used to adjust the degree of influence of the distance constraint in the TEB planning controller; d safe The set safe distance; d m The safety range is adjusted based on the uncertainty of the forecast; In addition, to improve the foresight of obstacle avoidance, a dynamic selection mechanism for the future trajectory point index of obstacles is introduced: First, calculate the current position p of the AGV. A =(x A ,y A ) and the current position p of the obstacle o =(x o ,y o The distance d between) AGV-obs : Secondly, based on the distance between the current position of the AGV and the current position of the obstacle, the index j of the obstacle's future trajectory point is dynamically adjusted: In the formula, m is the total length of the future trajectory sequence of the obstacle, m = T predict / step_size, T predict It is the prediction duration, step_size is the prediction step size, and d far The argmin function represents the distance threshold between the AGV and obstacles, calculated within the interval [1, m] that makes the function... The smallest s value; The obstacle avoidance time constraint is used to prevent the AGV and dynamic obstacles from arriving at the same position simultaneously; the system selects the point closest to the AGV from the obstacle's future trajectory as the prediction point P. T Calculate the predicted arrival point P for both the AGV and the obstacle. T Time t A and t O : Calculate the time difference Δt between the AGV and the obstacle reaching the predicted point: Δt=|t A -t O | In the formula, These are the position vectors of the predicted point, the current position of the AGV, and the current position of the obstacle, respectively. These are the velocity vectors of the AGV and the obstacle, respectively. To ensure the time interval between the AGV and the obstacle reaching the predicted point, a collision avoidance time constraint function E is designed. t : E t =λ t ·max(0,t safe -|Δt|) In the formula, λ t It is the weighting factor for the time constraint, t safe It is a predefined safety time interval; the max function is used to select 0 and t. safe -The maximum value of |Δt|; The speed direction constraint includes obstacle avoidance speed constraint and target alignment constraint; the obstacle avoidance speed constraint models the obstacle as an ellipse with the current speed direction as the major axis, and improves the dynamic obstacle avoidance effect of the AGV by predicting the future movement range of the obstacle; First, calculate the relative velocity v between the AGV and the obstacle. rel : v rel =v A -v o In the formula, v A It is the speed of the AGV, v o It is the speed of the obstacle; The obstacle is modeled as an ellipse, with the center of the ellipse representing the obstacle's current estimated position. The semi-major axis 'a' and semi-minor axis 'b' of the ellipse are defined based on the obstacle's velocity. In the formula, h j It is the initial circumference diameter of the obstacle, k. a It is the semi-major axis expansion coefficient, k b It is the semi-minor axis expansion coefficient; To determine the position of the AGV relative to the obstacle ellipse, an obstacle range distance function g(p) is established. AGV ): The gradient of the obstacle range distance function is calculated for path adjustment, and an obstacle avoidance coefficient λ is added. avoid Construct the obstacle avoidance speed constraint function E avoid : In the formula, λ avoid It is the obstacle avoidance coefficient. Represents the function g(p) AGV The gradient vector of the function points in the direction of the fastest increase; the max function is used to find the values ​​of 0 and 1. The maximum value; The target alignment constraint is used to ensure that the AGV moves towards the target and avoids retreating or deviating from the direction; AGV velocity vector and the direction of the target The formula for calculating the cosine value is as follows: Add target coefficient λ align Design the target alignment constraint function E align for: E align =λ align ·(1-cosθ) 2 。

Images