Millimeter wave radar-based agv environment perception and prediction avoidance method

Abstract

The application relates to the technical field of automatic guided vehicle (AGV) navigation and obstacle avoidance, and discloses an AGV environment perception and prediction avoidance method based on a millimeter wave radar, which comprises the following steps: step one, multi-modal data acquisition and fusion, and data fusion of the multi-modal data through a unified timestamp and a coordinate system to construct a global environment model; step two, space-time probability graph and trajectory prediction, generation of a plurality of obstacle future possible discrete trajectory data sets based on a space-time probability graph model and a particle filtering algorithm, output of a plurality of possible future trajectory paths, and construction of trajectory prediction; step three, dynamic no-entry area construction, outward expansion to generate a dynamic no-entry area with a timestamp; and step four, cooperative path planning and closed-loop control, calculation of an expected return avoidance action, setting of a safety threshold, real-time closed-loop control of the AGV, and execution of the avoidance action. The application can stably work all day long and has dynamic obstacle speed and trajectory prediction capabilities.

Description

Technical Field

[0001] This invention relates to the technical field of automated guided vehicle (AGV) navigation and obstacle avoidance, specifically to an AGV environmental perception and predictive obstacle avoidance method based on millimeter-wave radar. Background Technology

[0002] With the development of intelligent manufacturing and warehousing logistics, AGVs are increasingly widely used in industrial settings and parks. Existing AGV obstacle avoidance systems largely rely on two mainstream sensors: LiDAR or vision cameras. LiDAR accurately constructs a point cloud map of the surrounding environment by emitting and receiving laser beams, enabling high-precision detection of static obstacles; while vision cameras simulate human vision, using image processing algorithms to identify and classify objects in the environment. These two types of systems play a crucial role in specific scenarios, forming the foundation of current AGV obstacle avoidance solutions. However, LiDAR suffers from drawbacks such as susceptibility to interference in harsh environments like dust and smoke, leading to a rapid drop in accuracy and high cost, and it lacks direct speed measurement capabilities. Vision cameras are easily affected by lighting conditions, and their performance degrades in low or strong light. Therefore, there is an urgent need for an AGV environmental perception and predictive avoidance method based on millimeter-wave radar that can operate stably in all weather conditions and has the ability to predict the speed and trajectory of dynamic obstacles. Summary of the Invention

[0003] The present invention aims to provide an AGV environmental perception and prediction avoidance method based on millimeter-wave radar that can work stably in all weather conditions and has the ability to predict the speed and trajectory of dynamic obstacles.

[0004] To achieve the above objectives, the present invention adopts the following technical solution: 1) An AGV environmental perception and predictive avoidance method based on millimeter-wave radar, including the following steps: Step 1: Multimodal Data Acquisition and Fusion: Millimeter-wave radar mounted on the AGV collects relative distance, relative speed, and angle data of dynamic obstacles. Simultaneously, it combines point cloud clustering feature data of dynamic obstacles collected by lidar and 3D image state data of dynamic obstacles collected by vision camera for spatial alignment and motion distortion compensation. The collected relative distance, relative speed, angle, point cloud clustering feature data, and 3D image state data constitute multimodal data. The multimodal data is then fused using a unified timestamp and coordinate system to construct a global environment model. Step 2, Spatiotemporal Probability Map and Trajectory Prediction: Based on the global environment model, real-time information from fused millimeter-wave radar, lidar, and visual camera, as well as historical behavior data of dynamic obstacles, is acquired to construct a spatiotemporal probability map model; based on the spatiotemporal probability map model, a particle filter algorithm is used to generate a dataset of multiple possible future discrete trajectories of obstacles, outputting multiple possible future trajectory paths to construct trajectory prediction; Step 3: Dynamic restricted area construction: Based on the predicted discrete trajectory dataset obtained from trajectory prediction, construct a two-dimensional or three-dimensional bounding box for the dynamic obstacle, and combine the physical contour size and response time of the AGV to expand outward to generate a dynamic restricted area with timestamps. Step 4, Collaborative Path Planning and Closed-Loop Control: Based on the global environment model, guided by the dynamic restricted area, the local obstacle avoidance path is sampled and optimized in real time within the velocity space using the dynamic window method or model predictive control. During execution, the strategy is iterated based on the Markov decision process model to calculate the expected reward of the avoidance action. A safety threshold is set to enable real-time closed-loop control for the AGV to execute the avoidance action.

[0005] This invention integrates motion parameter data from millimeter-wave radar, point cloud feature data from lidar, and 3D state data from a visual camera. Simultaneously, it achieves spatial alignment and motion distortion compensation. A global environment model is constructed using a unified timestamp and coordinate system, solving the problems of incomplete information and insufficient accuracy when sensing dynamic obstacles with a single sensor. It can completely and accurately reconstruct the motion state and environmental spatial information of dynamic obstacles, providing a reliable data foundation for subsequent obstacle avoidance decisions. Furthermore, leveraging the strong penetration and anti-interference characteristics of millimeter-wave radar, it effectively solves the problem of perception blind spots in harsh environments such as dust and low light. Based on the fused global environment data, a spatiotemporal probability graph model is constructed, and combined with a particle filtering algorithm, multiple possible future trajectories of obstacles are output. This fully covers the motion possibilities of dynamic obstacles, significantly improving the accuracy and robustness of trajectory prediction compared to single trajectory prediction, and is adaptable to complex and ever-changing dynamic operating scenarios.

[0006] Meanwhile, this invention combines the predicted trajectory to generate bounding boxes for obstacles, and then expands them with the AGV's own contour size and system response time to generate a dynamic restricted area with timestamps. This incorporates the uncertainty of obstacle movement, the physical characteristics of the AGV itself, and the lag in system response into safety considerations, overcoming the shortcomings of traditional static obstacle avoidance areas that cannot adapt to dynamic scenarios. It effectively reserves safety redundancy and improves obstacle avoidance safety. Guided by the dynamic restricted area, it combines dynamic window method or model predictive control to complete real-time optimization of local paths. At the same time, it iteratively calculates the optimal avoidance action based on Markov decision process and achieves real-time closed-loop control through safety thresholds. It can quickly complete feasible path search within the speed space and adjust the avoidance strategy in real time according to scene changes, realizing local dynamic obstacle avoidance under the global path framework. This ensures the efficiency and safety of AGV operation in dynamic environments and adapts to the autonomous operation requirements of AGVs in dynamic industrial scenarios.

[0007] 2) According to the AGV environmental perception and predictive avoidance method based on millimeter-wave radar described in 1), wherein: In step one, a millimeter-wave radar mounted on the AGV is used to collect relative distance data, relative speed data, and angle data of dynamic obstacles, including the following steps: Based on the millimeter radar polar coordinate observation formula, the relative distance data, relative velocity data, and angle data of the dynamic obstacle in polar coordinate state are obtained as follows: Let the actual state of the j-th target at time k during AGV operation be:

[0008] This invention utilizes millimeter-wave radar mounted on an AGV to directly acquire three core motion observation data of dynamic obstacles in a coordinate system: relative distance, relative angle, and radial relative velocity. This data matches the core requirements of AGV operational status perception and directly supports subsequent obstacle motion state estimation without additional coordinate transformation. When calculating using polar coordinate observation formulas, the invention directly integrates the millimeter-wave radar's installation position and the coordinate difference information between the dynamic obstacle and the radar. The derivation process closely matches the actual operating scenario of the AGV, restoring the true position and velocity characteristics of the dynamic obstacle, resulting in stronger data adaptability. Simultaneously, the formula incorporates a millimeter-wave radar measurement noise term, reserving noise correction space to reduce the impact of random interference during observation on data accuracy and improve the accuracy of obstacle motion state perception.

[0009] 3) According to the AGV environmental perception and predictive avoidance method based on millimeter-wave radar described in 1), wherein: In step one, spatial alignment and motion distortion compensation are performed by combining the point cloud clustering feature data of dynamic obstacles acquired by LiDAR and the 3D state image data of dynamic obstacles acquired by visual camera, including the following steps:

[0010] This invention achieves precise conversion and alignment of LiDAR point cloud clustering data and visual camera 3D image data to the AGV coordinate system through coordinate transformation formulas. This solves the problem of inconsistent coordinate systems among different sensors and effectively integrates dynamic obstacle information collected by different sensors, providing a complete and unified data foundation for subsequent obstacle state perception. At the same time, by adding compensation formulas for pure translation compensation and angular velocity rotation correction, it corrects the translational and rotational position deviations caused by the time difference in acquisition and the AGV's own movement, improving the accuracy of dynamic obstacle position state detection and ensuring the precision of AGV's perception of dynamic obstacles.

[0011] 4) According to the AGV environmental perception and predictive avoidance method based on millimeter-wave radar described in 3), wherein:

[0012] This invention solves the problems of asynchronous data acquisition from multiple sensors and positional shift of observation points during AGV movement by performing incremental data compensation and kinematic compensation for observation points at synchronous and asynchronous times respectively. It unifies the observation data of LiDAR, vision camera and millimeter-wave radar at different timestamps to the same synchronous time coordinate system. It combines the original observation data of AGV coordinate system and introduces the angular velocity and linear velocity data collected by inertial measurement unit (IMU) to decompose translational offset terms and rotational offset terms to construct compensation formula. The compensation logic fits the actual motion characteristics of AGV and can complete the correction without complex iterative calculations, thus reducing the computational complexity while ensuring compensation accuracy.

[0013] 5) According to the AGV environmental perception and predictive avoidance method based on millimeter-wave radar described in 1), wherein: In step one, the collected relative distance data, relative velocity data, angle data, point cloud clustering feature data, and image 3D state data are combined to form multimodal data. This multimodal data is then fused using a unified timestamp and coordinate system to construct a global environment model, including the following steps: Based on Doppler velocity information and three-dimensional spatial information obtained from millimeter-wave radar, the Doppler velocity information is used as the velocity component of the dynamic obstacle relative to the millimeter-wave radar beam direction, and the three-dimensional spatial information is used as the position coordinates of the dynamic obstacle relative to the millimeter-wave radar in a three-dimensional Cartesian coordinate system. The Doppler velocity information and three-dimensional spatial information are then input into a Kalman filter algorithm for optimal state estimation.

[0014] Wherein: H kThe observation matrix is ​​used to map the state space to the observation space. R k This is the observation noise covariance matrix after multimodal fusion. The identity matrix is ​​used as the basis for the concatenation or fusion of multimodal data, including relative distance data, relative velocity data, angle data, point cloud clustering feature data, and 3D image state data. The Kalman gain K is then calculated. k Furthermore, the updated global state formula integrates the velocity information from millimeter-wave radar with the spatial accuracy of visual / LiDAR, outputting the most accurate global environmental state estimate.

[0015] This invention employs a two-step recursive estimation using Kalman filtering. First, it performs an initial prediction of the current state based on the global optimal state from the previous time step, clarifying the initial predicted state and error range that are not integrated into the current observation data. Then, it incorporates the fused multimodal observation data and updates the global state by calculating the Kalman gain, achieving complementarity of the advantages of different sensors: it integrates the precise Doppler velocity information acquired by millimeter-wave radar and combines it with the high-precision spatial information provided by visual / LiDAR, ultimately outputting a more accurate and less error-prone global environment state estimation result, thus improving the accuracy and reliability of obstacle state and global environment modeling in dynamic environments.

[0016] Therefore, in this invention, multi-source heterogeneous data fusion is achieved by using a unified timestamp and coordinate system. This integrates multi-modal data such as relative distance, relative velocity, angle, point cloud clustering features, and 3D state data of images into a unified observation input. By recursively updating the covariance matrix, the uncertainty of state estimation can be quantified in real time, and estimation errors can be dynamically corrected. This effectively filters out the interference of process noise and multi-source observation noise on the modeling results and improves the robustness of the global environment model.

[0017] 6) According to the AGV environmental perception and predictive avoidance method based on millimeter-wave radar described in 1), wherein: In step two, a spatiotemporal probabilistic graphical model is constructed by acquiring real-time information from the fused millimeter-wave radar, lidar, and visual camera, along with historical behavior data of dynamic obstacles, based on a global environment model. This includes the following steps: Based on the rationality and prediction accuracy of dynamic obstacle state estimation, and by integrating the historical motion patterns of the target with real-time environmental constraint information, a spatiotemporal probabilistic graphical model based on spatiotemporal correlation characteristics is constructed to characterize the posterior probability distribution of the target state. The specific formula is as follows:

[0018] in: It provides prior knowledge of the historical behavior of dynamic obstacles, including the target's past movement trajectory, speed change patterns, and temporal characteristics of movement preferences, which can accurately reflect the dynamic movement habits and historical behavior inertia of obstacles. It provides real-time environmental constraint information, covering the spatial constraint features of scene map topology, lane distribution, road boundaries, and traffic areas, and can characterize the external environmental constraints on target movement.

[0019] In this invention, the spatiotemporal probability density function in the spatiotemporal probability model overcomes the limitations of single-moment state estimation, deeply coupling temporal historical behavioral features with spatial environmental constraints to construct a spatiotemporally constrained target state probabilistic representation system. On the one hand, it can effectively capture the temporal evolution patterns, motion trends, and potential behavioral features of obstacle dynamics, avoiding the random errors of single-frame detection; on the other hand, it can combine real road environment rules to constrain the target state output, ensuring that the state estimation conforms to the motion logic of the actual scene.

[0020] 7) According to the AGV environmental perception and predictive avoidance method based on millimeter-wave radar described in 1), wherein: In step one, based on the spatiotemporal probabilistic graphical model, a particle filter algorithm is used to generate a dataset of multiple possible future discrete trajectories of obstacles, outputting multiple possible future trajectory paths to construct trajectory prediction, including the following steps:

[0021] This invention generates multiple discrete future possible trajectories for obstacles based on a spatiotemporal probabilistic graphical model combined with a particle filtering algorithm. It can fully cover the various possibilities of the obstacle's future motion, solving the deficiency of single trajectory prediction in not being able to cover uncertain motion scenarios. It can accurately characterize the probability distribution characteristics of the obstacle's future motion. It supports two motion models, the constant velocity model (CV) and the constant turning rate velocity model (CTRV), to adapt to different motion states of obstacles. It can handle various typical dynamic driving scenarios such as straight-line movement and turning, and conforms to the motion law of obstacles in actual traffic scenarios, thus improving the accuracy of state transition prediction.

[0022] Meanwhile, by fully integrating the observation data from three types of sensors on the AGV—millimeter-wave radar, lidar, and vision camera—the degree of matching between particle state and actual observation is quantified through observation likelihood calculation. The particle weight is dynamically adjusted based on the observation results, allowing particles with higher weights to more closely approximate the actual state of the obstacle. This effectively reduces the interference of single sensor observation errors on prediction results and improves the accuracy of obstacle state estimation.

[0023] By introducing a residual resampling strategy, only high-weight particles are retained. This not only eliminates invalid particles that deviate from the true state, ensuring that the key state space is effectively covered and avoiding particle degradation issues that affect prediction stability, but also controls the overall number of particles, reducing the computational load of the algorithm and balancing prediction accuracy and operational efficiency. The final state estimate is obtained by weighted aggregation of the updated particle set, and multiple probabilistic future trajectory paths are output. This provides complete and accurate uncertainty prediction information for the AGV's subsequent path planning and obstacle avoidance decisions, supporting the AGV to make safer and more reasonable motion decisions in dynamic environments.

[0024] 8) According to the AGV environmental perception and predictive avoidance method based on millimeter-wave radar described in 1), wherein: In step three, the dynamic restricted area is constructed: based on the predicted discrete trajectory dataset obtained from trajectory prediction, a two-dimensional or three-dimensional bounding box is constructed for the dynamic obstacle, and combined with the physical contour size and response time of the AGV, a dynamic restricted area with a timestamp is generated by expanding outward, including the following steps: Based on the discrete trajectory dataset generated by particle filtering, the geometric boundaries of dynamic obstacles are extracted to construct the basic bounding box of dynamic obstacles. Then, the spatial safety redundancy is obtained by expanding outward with the physical contour size of the AGV. Temporal safety redundancy is obtained by dynamically compensating with the response time. On the basis of spatial and temporal safety redundancy, timestamps are added to form dynamic restricted areas. After the dual expansion of spatial and temporal safety redundancy, each generated safe area is marked with a precise timestamp.

[0025] This invention accurately extracts the geometric boundaries of dynamic obstacles from discrete trajectory datasets output by particle filtering, enabling rapid construction of basic bounding boxes for dynamic obstacles and providing accurate obstacle positioning for dynamic region generation. It further expands spatial safety redundancy by incorporating the AGV's own physical contour dimensions and supplements temporal safety redundancy by combining AGV response time. Through this dual spatial and temporal safety expansion, it eliminates blind spots caused by the AGV's own size and braking response during obstacle avoidance, thus improving the safety of dynamic obstacle avoidance. Simultaneously, it adds precise timestamps to the final generated restricted areas, matching the restricted areas with the dynamic movement of obstacles, successfully constructing a temporally sequenced dynamic restricted area that conforms to the movement patterns of dynamic obstacles. This provides accurate dynamic restriction constraints for AGV path planning, adapting to the AGV's driving needs in dynamic scenarios.

[0026] 9) According to the AGV environmental perception and predictive avoidance method based on millimeter-wave radar described in 1), wherein: In step four, based on the global environment model, guided by the dynamic restricted area, the local obstacle avoidance path is sampled and optimized in real time within the velocity space using either the dynamic window method or model predictive control. The specific steps of employing the dynamic window method are as follows: Under the kinematic constraints of the AGV, a locally optimal trajectory is generated based on the global environment model and the dynamic entry region, and then in the velocity space... Feasible local trajectories for mid-sample, and evaluation function based on dynamic window method. To select the optimal speed command: Where: heading is the heading angle evaluation function, used to measure the fit between the trajectory and the global target path; dist is the dynamic obstacle distance evaluation function, used to measure the distance between the trajectory and the dynamic restricted area; vel is the speed evaluation function, used to encourage the AGV to maintain a high speed; α, β, γ are weighting coefficients.

[0027] This invention performs local path planning based on a global environment model, using dynamic restricted areas as the planning guide. It can adapt to dynamically changing work scenarios, solving the problem that global paths cannot cope with changes in dynamic obstacles in the environment, and ensuring the path availability of AGVs in dynamic environments. At the same time, feasible trajectory sampling and optimization are completed in the speed space, and the optimal speed command is selected through multi-dimensional weighted evaluation: heading angle evaluation constraints can ensure that the local path conforms to the global planning goal and avoid the path deviating from the task requirements; dynamic obstacle distance evaluation can ensure that the AGV always maintains a safe distance from dynamic restricted areas and dynamic obstacles, avoiding collision risks; speed evaluation can guide the AGV to maintain a high travel speed, ensuring the operational efficiency of the AGV.

[0028] By introducing AGV kinematic constraints into the planning process, the sampled local trajectories all conform to the AGV's own motion capabilities, and the output speed commands are executable, thus avoiding the problem of mismatch between the planned trajectory and the AGV's actual motion capabilities and improving the stability of path tracking. The local path planning is completed by using a real-time sampling optimization method in velocity space. The calculation logic is simple and adaptable to real-time requirements, which can meet the needs of AGV to adjust the obstacle avoidance path in real time during dynamic driving.

[0029] 10) According to the AGV environmental perception and predictive avoidance method based on millimeter-wave radar described in 1), wherein: In step four, based on the global environment model, guided by the dynamic restricted area, the local obstacle avoidance path is sampled and optimized in real time within the velocity space using the dynamic window method or model predictive control. The specific steps of using model predictive control are as follows: Under the kinematic constraints of the AGV, a local optimal trajectory is generated based on the global environment model and the dynamic entry area. Model predictive control is constructed using constrained rolling time-domain optimization. Within the prediction time domain N_p, the cost function including state error and control quantity is minimized.

[0030] in: x Let the state vector be the position and velocity. u For control vectors, Q,R,P This is the weight matrix. This is a set of dynamically restricted areas.

[0031] This invention optimizes paths based on dynamic restricted areas, adapting to dynamically changing restricted conditions in the environment. It solves the problem that traditional static path planning is unable to cope with obstacles in dynamic environments, ensuring the safety of AGV passage in dynamic scenarios. It generates locally optimal trajectories based on the AGV's own kinematic constraints, and the planned paths conform to the actual movement capabilities of the AGV, avoiding planning results that exceed the performance limits of the AGV and cannot be executed, thus ensuring the executability of the trajectory.

[0032] By constructing model predictive control through constrained rolling time-domain optimization, real-time sampling optimization of local paths can be completed within the prediction time domain. It can respond to dynamic changes in the environment and realize online real-time adjustment of obstacle avoidance paths, taking into account both global path requirements and local dynamic obstacle avoidance needs. At the same time, the constructed cost function incorporates both state error terms and control terms. In the process of minimizing the cost function, it can simultaneously ensure trajectory tracking accuracy and smooth optimization of control quantities. This allows the AGV to accurately track the reference trajectory while avoiding excessive fluctuations in control output, thus improving the stability of AGV operation.

[0033] 11) According to the AGV environmental perception and predictive avoidance method based on millimeter-wave radar described in 1), wherein: In step four, during execution, the strategy is iterated based on the Markov decision process model to calculate the expected reward for the avoidance action. A safety threshold is set to enable real-time closed-loop control of the AGV to execute the avoidance action, including the following steps: Step S1: Model the AGV obstacle avoidance process as a Markov decision process, iterate the strategy using the Bellman equation, and select the optimal avoidance action that maximizes the expected reward using the Bellman equation formula, which is as follows:

[0034] Compared with the prior art, the present invention also has the following technical effects: 1. All-weather robustness: Utilizing the strong penetration and anti-interference characteristics of millimeter-wave radar, it effectively solves the problem of perception blind spots in harsh environments such as dust and low light.

[0035] 2. Predictive obstacle avoidance: By introducing Doppler velocity measurement and particle filter trajectory prediction, the traditional "passive obstacle avoidance" is upgraded to "active predictive obstacle avoidance", which greatly improves the passage efficiency and safety of AGV in scenarios where people and forklifts frequently cross.

[0036] 3. Decision optimization: Combining Markov decision and MPC algorithm, the AGV's avoidance actions in complex environments are smoother and more efficient, avoiding frequent sudden stops. Attached Figure Description

[0037] Figure 1 This is a framework diagram of the AGV environmental perception and predictive avoidance method based on millimeter-wave radar according to the present invention. Figure 2 This is a schematic diagram illustrating the principle of multi-sensor data fusion and coordinate system transformation in the AGV environmental perception and predictive avoidance method based on millimeter-wave radar of the present invention. Figure 3 This is a schematic diagram of obstacle trajectory prediction based on particle filtering in the AGV environmental perception and prediction avoidance method based on millimeter-wave radar of the present invention. Figure 4 This is a schematic diagram of the dynamic restricted area generation and local path planning in the AGV environmental perception and prediction avoidance method based on millimeter-wave radar of the present invention. Detailed Implementation

[0038] The following detailed description illustrates the specific implementation method: See the example. Figure 1 As shown, the AGV environmental perception and predictive avoidance method based on millimeter-wave radar in this embodiment includes the following steps: Step 1: Multimodal Data Acquisition and Fusion: Millimeter-wave radar mounted on the AGV collects relative distance, relative speed, and angle data of dynamic obstacles. Simultaneously, it combines point cloud clustering feature data of dynamic obstacles collected by lidar and 3D image state data of dynamic obstacles collected by vision camera for spatial alignment and motion distortion compensation. The collected relative distance, relative speed, angle, point cloud clustering feature data, and 3D image state data constitute multimodal data. The multimodal data is then fused using a unified timestamp and coordinate system to construct a global environment model. Step 2, Spatiotemporal Probability Map and Trajectory Prediction: Based on the global environment model, real-time information from fused millimeter-wave radar, lidar, and visual camera, as well as historical behavior data of dynamic obstacles, is acquired to construct a spatiotemporal probability map model; based on the spatiotemporal probability map model, a particle filter algorithm is used to generate a dataset of multiple possible future discrete trajectories of obstacles, outputting multiple possible future trajectory paths to construct trajectory prediction; Step 3: Dynamic restricted area construction: Based on the predicted discrete trajectory dataset obtained from trajectory prediction, construct a two-dimensional or three-dimensional bounding box for the dynamic obstacle, and combine the physical contour size and response time of the AGV to expand outward to generate a dynamic restricted area with timestamps. Step 4, Collaborative Path Planning and Closed-Loop Control: Based on the global environment model, guided by the dynamic restricted area, the local obstacle avoidance path is sampled and optimized in real time within the velocity space using the dynamic window method or model predictive control. During execution, the strategy is iterated based on the Markov decision process model to calculate the expected reward of the avoidance action. A safety threshold is set to enable real-time closed-loop control for the AGV to execute the avoidance action.

[0039] Combination Figure 2 The diagram illustrates the principle of multi-sensor data fusion and coordinate system transformation. In this embodiment, the multiple sensors are millimeter-wave radar, lidar, and a vision camera. The data fusion and coordinate system transformation using millimeter-wave radar, lidar, and the vision camera are detailed below: In step one, the millimeter-wave radar mounted on the AGV collects relative distance data, relative speed data, and angle data of dynamic obstacles, including the following steps: Based on the millimeter radar polar coordinate observation formula, the relative distance data, relative velocity data, and angle data of the dynamic obstacle in polar coordinate state are obtained as follows: Let the actual state of the j-th target at time k during AGV operation be:

[0040] Where r represents the relative distance data; θ represents the relative angle data (azimuth). This is relative velocity data (radial velocity measured based on the Doppler effect); The installation position of the millimeter radar in the AGV coordinate system; Measurement noise for millimeter radar; The difference in lateral coordinates between the dynamic obstacle and the millimeter radar. The difference in longitudinal coordinates between the dynamic obstacle and the millimeter radar is calculated using the above millimeter radar polar coordinate observation formula. and The relative distance data of the j-th target at time k in the AGV coordinate system were calculated respectively. Relative angle data and relative velocity data .

[0041]

[0042] Based on the above, the relative distance data, relative velocity data, angle data, point cloud clustering feature data, and 3D image state data collected in step one are used to construct multimodal data. This multimodal data is then fused using a unified timestamp and coordinate system to build a global environment model, including the following steps: Based on Doppler velocity information and three-dimensional spatial information obtained from millimeter-wave radar, the Doppler velocity information is used as the velocity component of the dynamic obstacle relative to the millimeter-wave radar beam direction, and the three-dimensional spatial information is used as the position coordinates of the dynamic obstacle relative to the three-dimensional Cartesian coordinate system of the millimeter-wave radar (the azimuth, elevation, and range of the target are obtained by the radar through multiple antenna channels or scanning mechanisms, and then its spatial position in the X, Y, and Z dimensions is calculated). The Doppler velocity information and three-dimensional spatial information are input into the Kalman filter algorithm for optimal state estimation.

[0043] in: This is the observation matrix, used to map the state space to the observation space. The observation noise covariance matrix after multimodal fusion. The identity matrix is ​​used as the basis for the concatenation or fusion of multimodal data, including relative distance data, relative velocity data, angle data, point cloud clustering feature data, and 3D image state data. The Kalman gain is then calculated. Furthermore, the updated global state formula integrates the velocity information from millimeter-wave radar with the spatial accuracy of visual / LiDAR, outputting the most accurate global environmental state estimate.

[0044] See Figure 3 The diagram shown illustrates obstacle trajectory prediction based on particle filtering. In step two, building upon step one, and more specifically, step two involves acquiring real-time information from the fused millimeter-wave radar, lidar, and visual camera, along with historical behavior data of dynamic obstacles, based on a global environment model. This process constructs a spatiotemporal probabilistic graphical model, including the following steps: Based on the rationality and prediction accuracy of dynamic obstacle state estimation, and by integrating the historical motion patterns of the target with real-time environmental constraint information, a spatiotemporal probabilistic graphical model based on spatiotemporal correlation characteristics is constructed to characterize the posterior probability distribution of the target state. The specific formula is as follows: in: It provides prior knowledge of the historical behavior of dynamic obstacles, including the target's past movement trajectory, speed change patterns, and temporal characteristics of movement preferences, which can accurately reflect the dynamic movement habits and historical behavior inertia of obstacles. It provides real-time environmental constraint information, covering the spatial constraint features of scene map topology, lane distribution, road boundaries, and traffic areas, and can characterize the external environmental constraints on target movement.

[0045] Simultaneously, based on a spatiotemporal probabilistic graphical model and a particle filtering algorithm, multiple discrete trajectories of obstacles are generated, outputting multiple possible future trajectory paths. Trajectory prediction is constructed, including the following steps: The particle sets are initialized using a particle filtering algorithm based on a spatiotemporal probabilistic graphical model. At time t=0, N particle sets are initialized according to the prior distribution generated by the spatiotemporal probabilistic graphical model. and assign initial weights For each particle And assign initial weights For each particle Based on the constant velocity model (CV) or the constant turning rate velocity model (CTRV), the state of the dynamic obstacle at time k is predicted using interactive multi-model filtering, with process noise added. in: State transition function, To control input or environmental constraints; Furthermore, in this embodiment, the state vector of the constant velocity model (CV) is typically [y, x, vx, vy], or the state vector of the constant turning rate velocity model (CTRV) is typically extended to [x, y, θ, v, ω], where θ is the heading angle and ω is the turning rate. The state transition equation is nonlinear and requires the use of an extended Kalman filter (EKF). Meanwhile, in this embodiment, the interactive multi-model filtering is an adaptive multi-model fusion algorithm. Its core idea is: instead of pre-setting the target's current motion mode, it runs multiple model filters (such as CV and CTRV) in parallel, dynamically calculates the "posterior probability" of each model based on real-time observation data, and finally fuses the estimation results of each model according to probability weights to output the optimal state estimate.

[0046] Then, using the observation data Z_k obtained from the millimeter-wave radar, lidar, and vision camera in the AGV at time k, the matching degree between each particle and the actual observation is evaluated by calculating the observation likelihood, and the particle weights are updated accordingly.

[0047] See Figure 4 The diagram illustrates the dynamic restricted area generation and local path planning. In step three, building upon step two, the dynamic restricted area is constructed using the predicted discrete trajectory dataset obtained from trajectory prediction. A two-dimensional or three-dimensional bounding box is constructed for the dynamic obstacle, and combined with the AGV's physical contour dimensions and response time, it is expanded outwards to generate a timestamped dynamic restricted area. This includes the following steps: Based on the discrete trajectory dataset generated by particle filtering, the geometric boundaries of dynamic obstacles are extracted to construct the basic bounding box of dynamic obstacles. Then, the spatial safety redundancy is obtained by expanding outward with the physical contour size of the AGV. Temporal safety redundancy is obtained by dynamically compensating with the response time. On the basis of spatial and temporal safety redundancy, timestamps are added to form dynamic restricted areas. After the dual expansion of spatial and temporal safety redundancy, each generated safe area is marked with a precise timestamp.

[0048] Building upon step three, step four, based on the global environment model from step one, uses a dynamic restricted area as a guide and combines dynamic windowing or model predictive control to sample and optimize local obstacle avoidance paths in real time within the velocity space. The specific steps employing the dynamic windowing method are as follows: Under the kinematic constraints of the AGV, a locally optimal trajectory is generated based on the global environment model and the dynamic entry region, and then in the velocity space... Feasible local trajectories for mid-sample, and evaluation function based on dynamic window method. To select the optimal speed command: Where heading is the heading angle evaluation function, used to measure the degree of fit between the trajectory and the global target path; dist is the dynamic obstacle distance evaluation function, used to measure the distance between the trajectory and the dynamic restricted area; vel is the speed evaluation function, used to encourage the AGV to maintain a high speed; α, β, γ are weighting coefficients.

[0049] or: The specific steps of its model predictive control are as follows: Under the kinematic constraints of the AGV, a local optimal trajectory is generated based on the global environment model and the dynamic entry area. Model predictive control is constructed using constrained rolling time-domain optimization. Within the prediction time domain N_p, the cost function including state error and control quantity is minimized.

[0050] Where: x is the state vector, which consists of position and velocity; u is the control vector; and Q, R, and P are weight matrices. This is a set of dynamically restricted areas.

[0051] During execution, the strategy is iterated based on the Markov decision process model to calculate the expected reward for the avoidance action. A safety threshold is set to enable real-time closed-loop control of the AGV to execute the avoidance action, including the following steps: Step S1: Model the AGV obstacle avoidance process as a Markov decision process, iterate the strategy using the Bellman equation, and select the optimal avoidance action that maximizes the expected reward using the Bellman equation formula, which is as follows: set up This refers to the current state of the AGV, which includes the AGV's pose, the relative position and velocity of dynamic obstacles. To avoid the action, based on following strategy π Perform state transition, state value function V π Satisfies the Bellman equation:

[0052] in: Let R be the state transition probability, and R be the immediate reward, where a safe passage is positive and a collision is negative infinity. Discount factor; Step S2: Building upon step S1, through iterative policy evaluation and policy improvement, the optimal policy is eventually converged. In real-time control, the AGV selects to use the state-action value function. Biggest movement:

[0053] in: The optimal action is the specific action that, given a condition, can bring the greatest expected benefit. Let a mathematical operator be used to find the parameter 'a', which belongs to the action space A. This means finding the action with the largest function value among all possible actions of 'aEA'. This represents the optimal action value function; Indicates the current state; This indicates that the action to be performed at time step t is set to a; Represents the mathematical expectation; This represents the summation of an infinite series. The discount factor raised to the power of k; Indicates future rewards; Step S3: Based on step S2, monitor the minimum distance between the AGV and obstacles in real time, and calculate the shortest Euclidean distance between the AGV and the boundary of the dynamic restricted area in real time. :

[0054] pass Using a safety threshold determination logic, at any time t, the shortest Euclidean distance between the AGV and the boundary of the dynamic obstacle is calculated for real-time obstacle avoidance or safety assessment. In extreme working conditions, emergency braking is triggered to achieve highly reliable real-time closed-loop control.

[0055] In practice, our company applies the AGV environmental perception and predictive avoidance method described in this embodiment to the predictive avoidance system of warehouse logistics AGVs. This predictive avoidance system includes a perception layer, a prediction layer, a decision layer, and a control layer. The perception layer utilizes millimeter-wave radar modules that support multiple frequency bands (77GHz-81GHz, 60GHz, and 24GHz) in the warehouse logistics AGVs to meet the actual functional requirements of different testing scenarios. The millimeter-wave radar interacts with the AGV's underlying system (the system to which the AGV environmental perception and predictive avoidance method is applied in this embodiment) via communication interfaces such as CAN, UDP Ethernet, or RS485, and is compatible with common power supply voltages of 3.3V-5V DC. The AGV's underlying system acquires the radial velocity and position information of obstacles ahead in real time at a data update rate of up to 50Hz. Within the detection range (maximum detection distance ≥30m, ≥50m for personnel; minimum detection distance ≤0.1m), when an object with a relative velocity greater than 0.5m / s is detected, the predictive mechanism of the AGV environmental perception and predictive avoidance method in this embodiment is triggered.

[0056] Meanwhile, in the prediction layer, the AGV's underlying system utilizes the high-precision ranging capability (±5mm accuracy within 2m, ±2cm accuracy within 5m) and high resolution (distance resolution ≤0.2m, angular resolution ≤1.2°) of millimeter-wave radar to accurately extract the centroid coordinates of dynamic obstacles. The Kalman filter algorithm is used to calculate the optimal state and match the observed point trace with the predicted trajectory. Combined with a constant velocity model (CV) or a constant turning rate velocity model (CTRV), interactive multi-model filtering (IMM) is used to predict its motion trajectory within the next 3 seconds.

[0057] At the decision-making level, the AGV's underlying system generates a dynamic no-entry zone in front of the AGV based on the predicted trajectory. Using a model predictive control algorithm, and while satisfying the AGV's kinematic constraints, it combines precise angle measurements provided by millimeter-wave radar (accuracy ±1°, horizontal field of view ≥ ±45°, pitch field of view ±10°) to generate a smooth detour curve. Simultaneously, at the control level, the controller within the AGV's underlying system receives the planned trajectory. If a dynamic obstacle suddenly accelerates and intrudes into the safety threshold (e.g., minimum safety distance 1.5 meters), the AGV's underlying system immediately triggers emergency braking logic to ensure human-machine collaboration safety. Furthermore, the millimeter-wave radar module has an IP67 protection rating, weighs ≤300g, and consumes ≤6W, enabling it to adapt to the complex environment of warehousing and logistics sites and meet the lightweight and low-power design requirements of AGVs.

[0058] Therefore, this AGV environmental perception and predictive avoidance method includes constructing a multimodal data acquisition and fusion system. Millimeter-wave radar is used to acquire the target's distance, relative speed, and angle. Combined with laser / visual data, a high-precision global environment model is constructed using Kalman filtering. Simultaneously, a spatiotemporal probability map and trajectory prediction are established. Based on historical behavior and environmental information, a spatiotemporal probability map is constructed, and a particle filtering algorithm (including state prediction, weight update, and resampling) is used to generate multiple possible future trajectories for obstacles. Secondly, a dynamic no-entry zone is established. Based on the predicted trajectory and the AGV's physical dimensions, a timestamped dynamic no-entry zone is generated. Finally, through collaborative path planning and closed-loop control, a local avoidance trajectory is generated using Dynamic Window (DWA) or Model Predictive Control (MPC), and a Markov Decision Process is used for strategy iteration. Emergency braking is triggered when a safety threshold is exceeded.

[0059] The above are merely embodiments of this solution. Commonly known technical solutions and / or characteristics are not described in detail here. It should be noted that those skilled in the art can make various modifications and improvements without departing from the technical solution of this solution. These modifications and improvements should also be considered within the scope of protection of this solution, and will not affect the effectiveness of the implementation of this solution or the practicality of the patent. The scope of protection claimed in this application shall be determined by the content of its claims, and the specific embodiments described in the specification can be used to interpret the content of the claims.

Claims

1. An AGV environmental perception and predictive avoidance method based on millimeter-wave radar, characterized in that, Includes the following steps: Step 1: Multimodal Data Acquisition and Fusion: Millimeter-wave radar mounted on the AGV collects relative distance, relative speed, and angle data of dynamic obstacles. Simultaneously, it combines point cloud clustering feature data of dynamic obstacles collected by lidar and 3D image state data of dynamic obstacles collected by vision camera for spatial alignment and motion distortion compensation. The collected relative distance, relative speed, angle, point cloud clustering feature data, and 3D image state data constitute multimodal data. The multimodal data is then fused using a unified timestamp and coordinate system to construct a global environment model. Step 2, Spatiotemporal Probability Map and Trajectory Prediction: Based on the global environment model, real-time information from fused millimeter-wave radar, lidar, and visual camera, as well as historical behavior data of dynamic obstacles, is acquired to construct a spatiotemporal probability map model; based on the spatiotemporal probability map model, a particle filter algorithm is used to generate a dataset of multiple possible future discrete trajectories of obstacles, outputting multiple possible future trajectory paths to construct trajectory prediction; Step 3: Dynamic restricted area construction: Based on the predicted discrete trajectory dataset obtained from trajectory prediction, construct a two-dimensional or three-dimensional bounding box for the dynamic obstacle, and combine the physical contour size and response time of the AGV to expand outward to generate a dynamic restricted area with timestamps. Step 4, Collaborative Path Planning and Closed-Loop Control: Based on the global environment model, guided by the dynamic restricted area, the local obstacle avoidance path is sampled and optimized in real time within the velocity space using the dynamic window method or model predictive control. During execution, the strategy is iterated based on the Markov decision process model to calculate the expected return of the avoidance action, and a safety threshold is set to enable real-time closed-loop control of the AGV to execute the avoidance action.

2. The AGV environmental perception and prediction avoidance method based on millimeter-wave radar according to claim 1, characterized in that: In step one, a millimeter-wave radar mounted on the AGV collects relative distance, relative speed, and angle data of dynamic obstacles. This includes the following steps: Based on the millimeter-wave radar polar coordinate observation formula, the relative distance, relative speed, and angle data of the dynamic obstacles in polar coordinates are obtained to reflect the actual state of the AGV's operation. Specifically, let the actual state of the j-th target at time k during AGV operation be:

3. The AGV environmental perception and prediction avoidance method based on millimeter-wave radar according to claim 1, characterized in that: In step one, spatial alignment and motion distortion compensation are performed by combining the point cloud clustering feature data of dynamic obstacles acquired by LiDAR and the 3D state image data of dynamic obstacles acquired by visual camera, including the following steps:

4. The AGV environmental perception and prediction avoidance method based on millimeter-wave radar according to claim 1, characterized in that: In step one, the collected relative distance data, relative velocity data, angle data, point cloud clustering feature data, and image 3D state data are combined to form multimodal data. This multimodal data is then fused using a unified timestamp and coordinate system to construct a global environment model, including the following steps: Based on Doppler velocity information and three-dimensional spatial information obtained from millimeter-wave radar, the Doppler velocity information is used as the velocity component of the dynamic obstacle relative to the millimeter-wave radar beam direction, and the three-dimensional spatial information is used as the position coordinates of the dynamic obstacle relative to the millimeter-wave radar in a three-dimensional Cartesian coordinate system. The Doppler velocity information and three-dimensional spatial information are then input into a Kalman filter algorithm for optimal state estimation.

5. The AGV environmental perception and prediction avoidance method based on millimeter-wave radar according to claim 1, characterized in that: In step two, based on the global environment model, real-time information from the fused millimeter-wave radar, lidar, and visual camera, along with historical behavior data of dynamic obstacles, is acquired in real time. A spatiotemporal probabilistic graphical model is then constructed, including the following steps: Based on the rationality and prediction accuracy of the dynamic obstacle state estimation, the historical motion patterns of the target and real-time environmental constraint information are fused to construct a spatiotemporal probabilistic graphical model based on spatiotemporal correlation characteristics. This model is used to characterize the posterior probability distribution of the target state. The specific formula is as follows: Where: y history This provides prior knowledge of the dynamic obstacle's historical behavior, including the target's past trajectory, speed variation patterns, and temporal characteristics of its movement preferences. It accurately reflects the obstacle's dynamic movement habits and historical behavioral inertia. environment The current real-time environmental constraint information covers the spatial constraint characteristics of the scene map topology, lane line distribution, road boundaries, and traffic areas, which can characterize the external environmental constraints on the target's movement.

6. The AGV environmental perception and prediction avoidance method based on millimeter-wave radar according to claim 1, characterized in that: In step one, based on the spatiotemporal probabilistic graphical model, a particle filter algorithm is used to generate a dataset of multiple possible future discrete trajectories of obstacles, outputting multiple possible future trajectory paths to construct trajectory prediction, including the following steps:

7. The AGV environmental perception and prediction avoidance method based on millimeter-wave radar according to claim 1, characterized in that: In step three, the dynamic restricted area is constructed: based on the predicted discrete trajectory dataset obtained from trajectory prediction, a two-dimensional or three-dimensional bounding box is constructed for the dynamic obstacle, and combined with the physical contour size and response time of the AGV, a dynamic restricted area with a timestamp is generated by expanding outward, including the following steps: Based on the discrete trajectory dataset generated by particle filtering, the geometric boundaries of dynamic obstacles are extracted to construct the basic bounding box of dynamic obstacles. Then, the spatial safety redundancy is obtained by expanding outward with the physical contour size of the AGV. Temporal safety redundancy is obtained by dynamically compensating with the response time. On the basis of spatial and temporal safety redundancy, timestamps are added to form dynamic restricted areas. After the dual expansion of spatial and temporal safety redundancy, each generated safe area is marked with a precise timestamp.

8. The AGV environmental perception and prediction avoidance method based on millimeter-wave radar according to claim 1, characterized in that: In step four, based on the global environment model, guided by the dynamic restricted area, and combined with the dynamic window method or model predictive control, local obstacle avoidance paths are sampled and optimized in real time within the velocity space. The specific steps using the dynamic window method are as follows: Under the kinematic constraints of the AGV, a locally optimal trajectory is generated based on the global environment model and the dynamic entry area. Feasible local trajectories are sampled in the velocity space (υ,ω), and the dynamic window method evaluation function G( To select the optimal speed command: Where: heading is the heading angle evaluation function, used to measure the fit between the trajectory and the global target path; dist is the dynamic obstacle distance evaluation function, used to measure the distance between the trajectory and the dynamic restricted area; vel is the speed evaluation function, used to encourage the AGV to maintain a high speed; α, β, γ are weighting coefficients.

9. The AGV environmental perception and prediction avoidance method based on millimeter-wave radar according to claim 1, characterized in that: In step four, based on the global environment model, guided by the dynamic restricted area, the local obstacle avoidance path is sampled and optimized in real time within the velocity space using the dynamic window method or model predictive control. The specific steps of using model predictive control are as follows:

10. The AGV environmental perception and predictive avoidance method based on millimeter-wave radar according to claim 1, characterized in that: In step four, during execution, the strategy is iterated based on the Markov decision process model to calculate the expected reward for the avoidance action. A safety threshold is set to enable real-time closed-loop control of the AGV to execute the avoidance action, including the following steps: Step S1: Model the AGV obstacle avoidance process as a Markov decision process, iterate the strategy using the Bellman equation, and select the optimal avoidance action that maximizes the expected reward using the Bellman equation formula, which is as follows:

Images