Equipment track real-time re-planning method and system for complex scene

Abstract

The invention relates to the technical field of equipment track real-time planning, and discloses a complex scene-oriented equipment track real-time re-planning method and system, and the method comprises the steps: analyzing a scene obstacle and equipment motion process according to pre-obtained scene environment data and equipment state data, and constructing a real-time scene environment; responding to the equipment moving task, taking the real-time position of the equipment as a starting point, screening out a feasible region planning moving track in the real-time scene environment, and obtaining a first moving track; taking the current position of each device as a center, identifying a track conflict area between the devices, analyzing a device movement intention of a conflict position, and re-planning the first movement track to obtain a second movement track; the moving process of each device is controlled; according to the method, high-complexity scenes such as dynamic obstacles, narrow channels and multiple devices can be effectively processed, and the trafficability and task completion efficiency of the devices in a complex environment are improved.

Description

Technical Field

[0001] This application relates to the field of real-time equipment trajectory planning technology, and more specifically to a real-time equipment trajectory replanning method and system for complex scenarios. Background Technology

[0002] Currently, in complex and dynamic scenarios such as warehousing and logistics, flexible manufacturing, park inspection, and autonomous driving, the collaborative operation of multiple devices, such as AGVs, robots, and unmanned vehicles, has become crucial for improving efficiency. Traditional path planning algorithms are based on static environmental maps. When unexpected dynamic obstacles or other devices appear in the environment, the original planned path becomes invalid, leading to collision problems. Furthermore, the planning phase does not fully consider the dynamic uncertainties during execution, lacks effective online feedback and replanning mechanisms, and has poor adaptability to the environment.

[0003] Existing technologies suffer from the following problems: using static maps or infrequently updated obstacle information makes it difficult to accurately capture and predict the trajectories of fast-moving obstacles, resulting in planning data lagging behind the real environment; independently planned trajectories only discover conflicts during execution, making it difficult to meet the needs of large-scale, real-time scenarios; based on fixed priorities or simple local rules, they cannot adaptively adjust according to task context and device status, leading to long waiting times for low-priority devices or local congestion; to solve at least one of the above problems, this application proposes a real-time device trajectory replanning method and system for complex scenarios. Summary of the Invention

[0004] To address the shortcomings of existing technologies, the purpose of this application is to provide a real-time replanning method and system for device trajectories in complex scenarios, which can effectively solve the problems in the background technology. The specific technical solution of this application is as follows:

[0005] A real-time replanning method for device trajectories in complex scenarios includes:

[0006] Based on the pre-acquired scene environment data and device status data, analyze the scene obstacles and device movement process to construct a real-time scene environment;

[0007] In response to the device movement task, starting from the real-time location of the device, a feasible area is selected in the real-time scene environment, and a movement trajectory is planned according to the feasible area to obtain the first movement trajectory.

[0008] Taking the current position of each device as the center, based on the first movement trajectory, identify the areas where the trajectories of the devices conflict, analyze the movement intention of the devices at the conflict positions, and replan the first movement trajectory to obtain the second movement trajectory;

[0009] The movement of each device is controlled according to the second movement trajectory in order to replan the device trajectory in real time.

[0010] Specifically, the step of analyzing scene obstacles and device movement processes based on pre-acquired scene environment data and device status data to construct a real-time scene environment includes:

[0011] Based on the pre-acquired scene environment data and device status data, calculate the rate of change of corresponding values ​​between adjacent sampling points, and filter out dynamic data with a rate of change greater than a preset rate of change threshold and static data with a rate of change less than or equal to the preset rate of change threshold.

[0012] Based on dynamic and static data, the movement of obstacles and equipment in the scene is analyzed to construct a real-time scene environment.

[0013] Specifically, the process of analyzing scene obstacles and device movement based on dynamic and static data to construct a real-time scene environment includes:

[0014] Based on dynamic data, the features of the corresponding moving objects are extracted, the feature correlation between adjacent positions is analyzed, and the positions with feature correlation greater than a preset correlation threshold are connected to obtain the movement trajectory of the scene obstacles, and an obstacle trajectory set is constructed.

[0015] By combining static data and a preset scene map, the obstacle trajectory set is mapped onto the scene and the device movement process is analyzed to construct a real-time scene environment.

[0016] Specifically, in response to the device movement task, starting from the device's real-time location, a feasible area is selected in the real-time scene environment, and a movement trajectory is planned according to the feasible area to obtain a first movement trajectory, including:

[0017] In response to device movement tasks, starting from the device's real-time location, the region corresponding to the connected path is filtered out in the real-time scene environment, and feasible regions corresponding to the connection between the start and end points in multiple regions are extracted.

[0018] Based on the feasible area, the movement trajectory is planned, and the path with the shortest movement path is selected as the first movement trajectory.

[0019] Specifically, in response to the device movement task, starting from the device's real-time location, the region corresponding to the connected path is filtered out in the real-time scene environment, and feasible regions corresponding to the connection between the start and end points in multiple regions are extracted, including:

[0020] In response to the device movement task, starting from the device's real-time location, the location occupancy probability of each obstacle is calculated according to the real-time scene environment. Locations with a location occupancy probability greater than a preset probability threshold are marked as impassable, and locations with a location occupancy probability less than or equal to the preset probability threshold are marked as passable, thus constructing a passage map.

[0021] Connected regions are constructed by selecting interconnected passable locations in the passage map, the shape of each connected region is analyzed, and the point with the smallest boundary distance is identified as the narrow point, thus constructing a set of narrow points.

[0022] Starting from the connected domain corresponding to the origin, construct the reachable region by analyzing the maximum and minimum distances the device can move based on its speed and acceleration.

[0023] The system filters out the intersecting regions where the reachable area and the traffic map intersect. Within the intersecting regions, it analyzes the traffic conditions at the corresponding narrow points. The intersecting regions are then optimized by adjusting the device speed or boundary optimization. This process is repeated until the system connects to the connected domain where the device's movement task ends, thus obtaining the feasible region.

[0024] Specifically, the step of planning the movement trajectory according to the feasible area and selecting the shortest path for the device movement as the first movement trajectory includes:

[0025] According to the preset time interval, the feasible area is divided into multiple time slices, the geometry of the area corresponding to each time slice is analyzed, and the center of the area is selected.

[0026] Connecting the centers of adjacent time slices yields a set of movement trajectories;

[0027] The path with the shortest movement distance from the set of movement trajectories is selected as the first movement trajectory.

[0028] Specifically, the process involves taking the current position of each device as the center, identifying areas of trajectory conflict between devices based on the first movement trajectory, analyzing the movement intentions of devices at the conflict locations, and replanning the first movement trajectory to obtain a second movement trajectory, including:

[0029] Using the current location of each device as the center, and based on the first movement trajectory, identify the areas where trajectories conflict between devices, and construct a conflict map;

[0030] Based on the conflict map, and combined with the priority and status of the mobile tasks corresponding to the conflicting devices, the intention of the devices to move at the conflict location is analyzed, and the first mobile trajectory is replanned to obtain the second mobile trajectory.

[0031] Specifically, the step of using the current position of each device as the center, identifying areas of trajectory conflict between devices based on the first movement trajectory, and constructing a conflict map includes:

[0032] Taking the current location of each device as the center, based on the first movement trajectory and the real-time status of the devices, simulate the changes in the movement path of each device, and calculate the probability that each location is occupied by the corresponding device according to the changes, and construct the space occupancy map of each device.

[0033] Calculate the product of the probabilities of different devices occupying the same location in the space occupancy map to obtain the joint probability, and select the locations whose joint probability is greater than a preset conflict threshold as conflict points to construct a conflict point set;

[0034] At the point of conflict, analyze the relative speed and relative position of the conflicting devices, and calculate the probability of conflict.

[0035] By treating devices as nodes and conflict probabilities as edge weights, connections are established between corresponding nodes to construct a conflict graph.

[0036] Specifically, based on the conflict map, and combined with the priority and status of the movement tasks corresponding to the conflicting devices, the movement intention of the devices at the conflict locations is analyzed, and the first movement trajectory is replanned to obtain the second movement trajectory, including:

[0037] Based on the priority of the equipment relocation task and the equipment status, the urgency of the task and the matching degree of the status are analyzed to calculate the relocation priority of the equipment.

[0038] By combining the conflict map and movement priority, the movement path of the device with higher movement priority remains unchanged, while the movement path of the device with lower movement priority is adjusted at the conflict point. The first movement trajectory is then replanned to obtain the second movement trajectory.

[0039] A real-time equipment trajectory replanning system for complex scenarios, used to implement the aforementioned real-time equipment trajectory replanning method for complex scenarios, includes:

[0040] The complex scene analysis module analyzes scene obstacles and equipment movement processes based on pre-acquired scene environment data and device status data, and constructs a real-time scene environment;

[0041] The device trajectory planning module responds to the device movement task, takes the real-time location of the device as the starting point, selects a feasible area in the real-time scene environment, and plans a movement trajectory according to the feasible area to obtain the first movement trajectory.

[0042] The device trajectory replanning module takes the current position of each device as the center, identifies the area of ​​trajectory conflict between devices based on the first movement trajectory, analyzes the movement intention of the devices at the conflict position, and replans the first movement trajectory to obtain the second movement trajectory.

[0043] The equipment control module controls the movement of each device according to the second movement trajectory in order to replan the device trajectory in real time.

[0044] The beneficial effects of this application are as follows: By calculating the rate of change of data to distinguish between dynamic and static information, and by constructing an obstacle trajectory set through feature correlation, a static map and predicted trajectories of dynamic obstacles can be provided, offering environmental information for trajectory planning. Based on the constructed accessibility map, narrow points in the connected domain are identified, and reachable areas are determined by combining equipment speed and acceleration. Optimization of intersecting areas ensures that the planned feasible areas are not only connected but also meet the physical constraints of actual equipment passage, optimizing passability in congested and narrow scenarios. By simulating the equipment movement process to identify conflict points and constructing corresponding conflict maps, a structural foundation is provided for subsequent trajectory optimization. Calculating movement priorities based on the priority of equipment movement tasks and the real-time status of the equipment, and combining this with trajectory optimization adjustments using the conflict map, improves the efficiency of trajectory replanning. This reduces the probability of collisions between equipment and dynamic obstacles or other equipment, effectively handling highly complex scenarios such as dynamic obstacles, narrow passages, and multiple devices, improving equipment passability and task completion efficiency in complex environments. Attached Figure Description

[0045] Figure 1 This is a flowchart illustrating a real-time replanning method for device trajectories in complex scenarios, as described in Embodiment 1 of this application.

[0046] Figure 2 This is a schematic diagram of the reachable area of ​​the device in Embodiment 1 of this application;

[0047] Figure 3 This is a schematic diagram of the conflict map in Embodiment 1 of this application;

[0048] Figure 4 This is a schematic diagram of the structure of a real-time replanning system for device trajectories in complex scenarios, as described in Embodiment 1 of this application. Detailed Implementation

[0049] The present application will be further described in detail below with reference to the accompanying drawings and embodiments.

[0050] In the embodiments of this application, the terms "exemplary" or "for example" are used to indicate that something is an example, illustration, or description. Any embodiment or design that is described as "exemplary" or "for example" in the embodiments of this application should not be construed as being more preferred or advantageous than other embodiments or design. Specifically, the use of the terms "exemplary" or "for example" is intended to present the relevant concepts in a specific manner.

[0051] Hereinafter, the terms "first," "second," and other generic terms are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Therefore, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of the embodiments of this application, unless otherwise stated, "multiple" means two or more.

[0052] Example 1:

[0053] refer to Figure 1 The image shows a specific implementation of a real-time device trajectory replanning method for complex scenarios according to this application, including:

[0054] S101. Based on the pre-acquired scene environment data and device status data, analyze the scene obstacles and device movement process to construct a real-time scene environment;

[0055] S102. In response to the device movement task, starting from the real-time location of the device, a feasible area is selected in the real-time scene environment, and a movement trajectory is planned according to the feasible area to obtain the first movement trajectory.

[0056] S103. Taking the current position of each device as the center, based on the first movement trajectory, identify the area of ​​trajectory conflict between devices, analyze the movement intention of the devices at the conflict position, replan the first movement trajectory, and obtain the second movement trajectory.

[0057] S104. Control the movement of each device according to the second movement trajectory to perform real-time replanning of the device trajectory.

[0058] In this embodiment, the system synchronously collects multi-sensor data at fixed intervals and aligns them on the coordinate system. It calculates the rate of position change of each observation point in the environmental data between two consecutive sampling times. Based on the rate of change, it identifies dynamic and static data; dynamic data reflects moving objects, while static data represents fixed environmental structures. For dynamic data, it is clustered into different motion clusters, each cluster representing a candidate moving object. Between consecutive time frames, by comparing the appearance and motion characteristics of these clusters, clusters belonging to the same physical entity are associated and tracked to obtain the continuous trajectory of each moving object. The current velocity and acceleration are estimated based on the historical trajectory of the moving object, and its future position is predicted to obtain the predicted trajectory. The historical trajectory and predicted trajectory of the dynamic obstacle are overlaid and fused with a static map combining the real-time status of the controlled device to obtain the real-time scene environment, including static feasible and infeasible area information, and the predicted spatial positions of the dynamic obstacle and device at various future times are marked in the time dimension.

[0059] It should be noted that by constructing a real-time scene environment, environmental perception capabilities can be improved, overcoming the limitations of traditional environmental modeling that only reflects the current instantaneous state. Through the trajectory prediction of dynamic obstacles, changes in the environment within the next few seconds can be predicted, providing accurate spatiotemporal information for subsequent trajectory planning. This enables proactive avoidance of the future paths of dynamic obstacles, reducing collision risks and improving the stability and safety of equipment operation.

[0060] Specifically, based on the real-time scene environment, the probability of each spatial location being occupied by obstacles at every moment within the future planning time domain is calculated. This probability is compared with a preset probability threshold; locations with a probability higher than the threshold are considered impassable, while those with a probability lower are considered passable, generating a corresponding passage map. Starting from the device's origin, spatiotemporal connectivity is analyzed. Based on the device's current speed, maximum acceleration, and deceleration, the reachable spatial range within each short time interval is calculated. The intersection of the passable and reachable areas is taken as the feasible region connecting the origin and destination. Within the feasible region, a smooth geometric path is fitted by selecting the safest path point on each time slice. The path with the shortest total length or the shortest total time is selected as the device's first movement trajectory.

[0061] It should be noted that by considering safety and dynamic feasibility in trajectory planning, the reliability and executability of the first movement trajectory can be improved. The trajectory is restricted to the feasible area, which can ensure that the trajectory does not collide with any known and predicted obstacles. Through path smoothing and speed optimization, the generated trajectory has continuous curvature and limited acceleration, enabling the equipment to move smoothly and efficiently. This avoids equipment shaking, frequent start-stop and stop and energy waste caused by sharp bends or sudden speed changes in the path, thereby improving the stability and efficiency of equipment movement.

[0062] Furthermore, based on the first movement trajectory, the movement trajectory of each device is simulated, and the frequency of each spatiotemporal location occupied by the device in the simulation results is statistically analyzed to obtain the probability occupancy distribution of the device. For any two devices, the product of their occupancy probabilities at the same spatiotemporal location is calculated to obtain the joint probability. Locations where the joint probability exceeds a preset conflict threshold are marked as conflict points. The relative speed and direction of the two devices are analyzed at the conflict points to calculate the conflict probability. Devices are treated as nodes, and if conflict points exist, connecting edges are established between nodes, with the edge weight being the corresponding conflict probability, to construct a conflict graph. Combining the urgency of the tasks performed by the devices and the device's own state, the movement priority of each device is calculated. Based on the conflict graph and the device movement priority, the trajectory of high-priority devices remains unchanged, while the trajectory of low-priority devices is locally adjusted near the conflict points. Adjustment strategies include inserting waiting periods, local detours, or fine-tuning the speed sequence. After adjusting a trajectory, it is necessary to reassess whether it will trigger new conflicts until all conflict probabilities drop below the conflict threshold. The set of coordinated trajectories of all devices is used as the second movement trajectory.

[0063] It should be noted that by quantifying conflicts and dynamic prioritization, equipment is intelligently scheduled according to collision avoidance. Compared with fixed avoidance rules, right-of-way can be flexibly allocated according to real-time production needs and equipment status. For example, fully loaded equipment performing urgent tasks can pass without obstruction, while unloaded equipment performing regular tasks will perform slight deceleration to give way. While ensuring the smooth execution of critical tasks, the impact on the overall system throughput is minimized, avoiding global congestion and efficiency decline caused by rigid avoidance, and improving the operating efficiency of equipment in complex environments.

[0064] Specifically, the system deploys a trajectory tracking controller for each device. The controller's inputs include the current device's status feedback and the target status on the second movement trajectory corresponding to the current time. The status feedback includes, but is not limited to, the actual position, speed, and heading angle obtained through the encoder, IMU, and positioning system. The target status includes, but is not limited to, the target position, target speed, and target heading angle. The controller typically employs a model predictive control algorithm.

[0065] Preferably, in each control cycle (e.g., 50ms), the model predictive control algorithm uses the device's current precise state as initial conditions. Under the constraints of the device's differential drive model, it performs rolling optimization calculations over the next 20 control cycles to determine a series of optimal control quantities, including but not limited to left and right wheel speeds or front wheel steering angles and accelerations. The optimization objective is to minimize the positional error between the predicted trajectory and the reference path of the second movement trajectory, while ensuring smooth control changes. After optimization, the optimal control quantity for the first control cycle is sent to the device's motor driver or steering servo mechanism for execution. This process is repeated in the next control cycle, forming a closed-loop feedback control. While the device executes along the current second movement trajectory, the system begins a new round of planning and coordination based on the latest environmental data, achieving continuous response to dynamic environments and unexpected events, and real-time trajectory replanning.

[0066] It should be noted that by combining the optimized and coordinated second movement trajectory with closed-loop trajectory tracking control, the accurate implementation of the planning intent can be ensured. The model predictive controller can actively predict equipment dynamics and compensate for model errors and external disturbances, enabling the equipment to smoothly transition when faced with minor trajectory adjustments. The second movement trajectory has fully considered feasibility and coordination, which can reduce the pressure on the control layer to handle extreme abnormal situations. The system can operate at a stable frequency, improving the system's ability to continuously, smoothly, and safely respond to complex dynamic scenarios.

[0067] This application distinguishes between dynamic and static information by calculating the rate of change of data, and constructs an obstacle trajectory set through feature correlation. This provides both static maps and predicted trajectories for dynamic obstacles, offering environmental information for trajectory planning. Based on the constructed accessibility map, it identifies narrow points in connected domains, determines reachable areas by combining device speed and acceleration, and optimizes intersecting areas. This ensures that the planned feasible areas are not only connected but also meet the physical constraints of actual device passage, optimizing accessibility in congested and narrow scenarios. By simulating device movement, it identifies conflict points and constructs corresponding conflict maps, providing a structural foundation for subsequent trajectory optimization. Calculating movement priorities based on the priority of device movement tasks and real-time device status, and combining this with the conflict map for trajectory optimization and adjustment, improves trajectory replanning efficiency. This reduces the probability of collisions between devices and dynamic obstacles or other devices, effectively handling highly complex scenarios such as dynamic obstacles, narrow passages, and multiple devices, improving device accessibility and task completion efficiency in complex environments.

[0068] Furthermore, based on the pre-acquired scene environment data and device status data, the movement processes of scene obstacles and devices are analyzed to construct a real-time scene environment, including:

[0069] S201. Based on the pre-acquired scene environment data and device status data, calculate the rate of change of corresponding values ​​between adjacent sampling points, and filter out dynamic data with a rate of change greater than a preset rate of change threshold and static data with a rate of change less than or equal to the preset rate of change threshold.

[0070] S202. Based on dynamic and static data, analyze the movement process of obstacles and equipment in the scene to construct a real-time scene environment.

[0071] In this embodiment, data from multiple sensors are synchronously collected at a fixed sampling period. For environmental data, including point cloud data from 2D or 3D LiDAR, the processing unit aligns two consecutive frames of point cloud data in the same global map coordinate system. For each point in the point cloud, clustering is used to obtain corresponding spatial units. Within each spatial unit, the change in the observed value is calculated, and the Euclidean coordinates of the spatial unit's center point are removed and the change rate is obtained by time interval. Simultaneously, the system receives and stores status data reported by each device controller, including device ID, real-time position, speed, heading angle, etc., calculated through encoder and IMU fusion. A change rate threshold is set based on sensor noise and minor environmental disturbances that need to be ignored in the scenario; for example, in an indoor AGV scenario, the change rate threshold is set to 0.15 m / s. All data points are traversed, and the calculated change rate is compared with the change rate threshold. Data with a change rate greater than the threshold is considered dynamic data, while data with a change rate less than or equal to the preset change rate threshold is considered static data.

[0072] It should be noted that by setting a change rate threshold, interference signals caused by sensor noise, changes in lighting, or slight swaying of non-rigid objects can be effectively filtered out, preventing irrelevant changes from being misjudged as dynamic objects that need to be avoided, thus improving the stability and reliability of environmental perception; the range of dynamic data that requires complex tracking processing is narrowed down to truly fast-moving objects, providing an accurate data foundation for obstacle trajectory tracking and improving the efficiency and real-time performance of building real-time scene environments.

[0073] Specifically, based on dynamic and static data, this method analyzes scene obstacles and equipment movement processes to construct a real-time scene environment. Traditional real-time maps only reflect obstacles at the current instant. This method combines accurate static structures with predicted trajectories of dynamic obstacles and collaborative equipment in the next few seconds to construct a real-time scene environment. The planned paths can proactively avoid areas that will be occupied in the future, either in time or space, avoiding emergency braking and replanning based on instantaneous reactions. This makes equipment movement smoother, more efficient, and safer, and improves the collaborative operation capabilities of equipment in complex dynamic scenes.

[0074] Furthermore, based on dynamic and static data, the movement of obstacles and equipment in the scene is analyzed to construct a real-time scene environment, including:

[0075] S301. Extract the features of the corresponding moving objects based on the dynamic data, analyze the feature correlation between adjacent positions, connect the positions with feature correlation greater than the preset correlation threshold to obtain the movement trajectory of the scene obstacle, and construct the obstacle trajectory set.

[0076] S302. Combining static data and a preset scene map, the obstacle trajectory set is mapped onto the scene and the device movement process is analyzed to construct a real-time scene environment.

[0077] In this embodiment, for each frame of dynamic data, spatially neighboring dynamic point clouds are aggregated into an independent cluster by spatial clustering based on Euclidean distance, and each cluster is regarded as a candidate moving object. For each cluster, corresponding features are extracted, including but not limited to the bounding box size of its point cloud, the length, width and height of the minimum bounding box, the eigenvalue of the covariance matrix of the point cloud distribution, and the direction and magnitude of the average velocity vector of the point cloud within the cluster. The features are arranged in order to obtain the feature vector.

[0078] The system includes a pre-defined list of active trajectories, which contains the latest state estimate, corresponding feature vector, and historical position sequence for each trajectory. For each candidate cluster feature vector extracted in a new frame, the association cost between the candidate cluster feature vector and the last known feature vector of each trajectory is calculated to determine whether it belongs to an existing trajectory in the list. Specifically, the cosine similarity between the candidate cluster feature vector and the last known feature vector of each trajectory is calculated as the corresponding association cost. An association threshold is set based on the statistical distribution of historical tracking data; for example, by analyzing the cost distribution of a large number of correctly and incorrectly associated samples offline, the 95th percentile is selected as the association threshold.

[0079] If the association cost exceeds the association threshold, the association is considered successful. The observed location and features of the cluster are then updated in the corresponding trajectory, and the trajectory is smoothed using a Kalman filter, with the state estimate updated. The set of all active trajectories is treated as the obstacle trajectory set. Each trajectory contains a unique ID, a sequence of location points arranged in chronological order, and a current motion state estimate, including velocity and acceleration.

[0080] It should be noted that through feature extraction and correlation analysis, multiple unknown dynamic obstacles in a scene can be accurately tracked. This method can effectively distinguish multiple adjacent obstacles with similar appearance or movement patterns, reducing false correlations and loss of trajectories. For example, two handcarts traveling side by side in a warehouse aisle would be misidentified as the same obstacle by traditional methods that rely solely on position prediction. This method, however, combines geometric features such as size and outline to analyze their independent trajectories. The constructed obstacle trajectory set provides the past and present positions of the obstacles, and through Kalman filtering, their short-term future positions can be reliably predicted, providing accurate dynamic environmental information for real-time path planning of the equipment.

[0081] Specifically, the preset scene map is a high-precision static reference map, which marks fixed structures such as passageways, walls, shelves, and workstations. The static data is registered with the reference map, and the current static point cloud is aligned with the map point cloud, unifying all dynamic observations and known device locations into the global map coordinate system. All trajectory points in the obstacle trajectory set are transformed from their original sensor observation coordinate system to the global map coordinate system, ensuring that the trajectory of each dynamic obstacle can be accurately marked on its corresponding physical location on the static reference map.

[0082] Specifically, the system analyzes and integrates the movement process of the equipment, including autonomous mobile units with known states under system control, such as AGVs. The system obtains the real-time status reported by each device through its internal communication network, including its ID, precise location on the global map, speed, heading, and its currently planned movement path, i.e., the first or second movement trajectory output by the planning module of the previous cycle. This information is overlaid with a static baseline map and the mapped dynamic obstacle trajectories to construct a real-time scene environment.

[0083] It should be noted that by constructing a real-time scene environment, comprehensive dynamic situational awareness is provided for subsequent equipment trajectory planning. When planning paths, it not only considers avoiding static obstacles in space, but also proactively schedules itself in time to avoid predicted paths of dynamic obstacles, and coordinates with the planned paths of other known equipment. This elevates the obstacle avoidance strategy from passive, reactive emergency avoidance to proactive, predictive coordination, improving the overall smoothness, safety, and efficiency of equipment operation in complex dynamic scenarios.

[0084] Furthermore, in response to the device movement task, starting from the device's real-time location, a feasible area is selected in the real-time scene environment, and a movement trajectory is planned according to the feasible area to obtain a first movement trajectory, including:

[0085] S401. In response to the device movement task, starting from the device's real-time location, filter out the regions corresponding to the connected paths in the real-time scene environment, and extract the feasible regions corresponding to the connection between the start and end points in multiple regions.

[0086] S402. Plan the movement trajectory according to the feasible area, and select the path with the shortest movement path of the equipment as the first movement trajectory.

[0087] In this embodiment, in response to the device movement task, starting from the device's real-time location, the region corresponding to the connected path is filtered out in the real-time scene environment, and feasible regions corresponding to the connection between the start and end points in multiple regions are extracted. By analyzing spatiotemporal accessibility and connectivity, the filtered feasible regions can ensure the safety of the planning, ensuring that subsequent trajectory planning is carried out within this safe area, and eliminating the possibility of collisions with known and predicted obstacles. For example, in an intersection scenario, by identifying the safe time window that is staggered from the predicted vehicle passage and its corresponding spatial region, the risk of colliding with lateral vehicles in the center of the intersection caused by pure spatial shortest path planning can be avoided.

[0088] Specifically, the movement trajectory is planned according to the feasible area, and the shortest path of the equipment movement is selected as the first movement trajectory. The optimal path is selected within the feasible area to improve the safety of the initial trajectory. Through smoothing optimization and kinematic constraints, it can be ensured that the curvature of the generated trajectory is continuous and the acceleration is limited, so that it can be accurately and smoothly tracked by the corresponding underlying tracking controller. This avoids the frequent start-stop and sharp turns of the equipment caused by the broken line path, and improves the movement efficiency and the life of the equipment's mechanical components.

[0089] Furthermore, in response to device movement tasks, starting from the device's real-time location, regions corresponding to connected paths are filtered out in the real-time scene environment, and feasible regions corresponding to the connection between the start and end points are extracted from multiple regions, including:

[0090] S501. In response to the device movement task, starting from the device's real-time location, calculate the location occupancy probability of each obstacle according to the real-time scene environment, mark the location with the location occupancy probability greater than the preset probability threshold as impassable, and mark the location with the location occupancy probability less than or equal to the preset probability threshold as passable, and construct a passage map.

[0091] S502. Select the passable locations that are interconnected in the passage map to construct a connected region, analyze the shape of each connected region, identify the point with the smallest boundary distance as a narrow point, and construct a set of narrow points.

[0092] S503. Starting from the connected domain corresponding to the starting point, analyze the maximum and minimum distances the device can move according to the device's speed and acceleration, and construct the reachable region.

[0093] S504. Filter out the intersecting regions where the reachable area and the passage map intersect. Analyze the passage situation of the corresponding narrow points in the intersecting region. Optimize the intersecting region by adjusting the device speed or boundary optimization. Repeat the intersecting region optimization process until the connection to the connected domain where the device movement task ends is located, and obtain the feasible region.

[0094] In this embodiment, based on the real-time scene environment, for each spatiotemporal unit within a future period, for example, a 0.1m × 0.1m grid in the future second t, the corresponding position occupancy probability value is calculated by combining the deterministic occupancy of the static reference map, the uncertainty of the dynamic obstacle trajectory prediction, and the sensor observation noise. For example, for a position, if it is inside a static wall, its position occupancy probability value is 1.0; if it is on the center line of a predicted dynamic obstacle trajectory, its position occupancy probability value is 0.9; if it is in the edge uncertainty area of ​​the trajectory prediction, its position occupancy probability value is 0.5; if it is a completely open area, its position occupancy probability value is 0.

[0095] Preferably, a probability threshold is set according to the system's security and accessibility requirements, for example, 0.7. All relevant spatiotemporal units within the planning time domain are traversed, and the location occupancy probability of each unit is compared with the preset probability threshold. Any unit whose location occupancy probability is greater than 0.7 is marked as impassable and assigned a value of 0 in the accessibility map; if the location occupancy probability is less than or equal to 0.7, it is marked as accessible and assigned a value of 1. By performing this operation throughout the entire planning spatiotemporal scope, a series of binary-labeled accessibility maps on time slices are obtained.

[0096] It should be noted that by setting probability thresholds to construct a travel map, allowing devices to traverse areas with low probability of interference but which are actually safe, more alternative paths can be retained, unnecessary detours can be avoided, and compared to a strategy that completely ignores uncertainty, high-probability occupied areas can be avoided, improving the safety of devices during movement and providing a safe search space for subsequent path search processes.

[0097] Specifically, on the passage map of each time slice, a scan-line based labeling algorithm is used to group all spatially adjacent passable units into the same region as a connected component. Each connected component represents a continuous space where movement is possible without encountering obstacles at that moment. The Euclidean distance from each passable unit within the connected component to its nearest impassable boundary is calculated. Points whose calculated Euclidean distance values ​​are local minima and whose absolute distance values ​​are less than the width of the device body are selected as the narrow points of the connected component. All narrow points in all connected components constitute the narrow point set, reflecting the critical locations in the passage map where the passage is narrowest and most difficult to traverse.

[0098] It should be noted that by screening potential risk points for equipment passage through regional morphology analysis, special attention can be paid to narrow points in subsequent trajectory planning. For example, the passing posture and speed can be adjusted in advance, or avoidance can be planned in advance. This avoids the problem that the equipment will find that the passage is too narrow to pass when it reaches this point, resulting in a deadlock or the need for a large backtracking and replanning. This improves the robustness of trajectory planning and the success rate of passing through on the first attempt.

[0099] Specifically, based on the current motion state of the equipment, the spatial range that the equipment can reach within the next planning cycle or time step is calculated. The current motion state of the equipment includes, but is not limited to, its current position, current velocity vector, known maximum acceleration, and maximum deceleration. According to the time slice interval of the travel map, the farthest displacement that the equipment can reach within that time with its current velocity direction and maximum acceleration is calculated as the maximum distance; the displacement required for the equipment to immediately apply maximum deceleration until its velocity reaches zero is calculated as the minimum distance.

[0100] like Figure 2 As shown, since the device will turn, the reachable area is a fan-shaped area or a ring-shaped area in the velocity direction. With the current position as the origin, the minimum distance as the inner radius, and the maximum distance as the outer radius, within the reachable heading angle range that takes into account the maximum lateral acceleration limit, the fan-shaped area is constructed as the reachable area, reflecting the boundary of the spatial position that the device can reach in the next moment.

[0101] It should be noted that constructing an reachable region ensures the physical feasibility of trajectory planning, preventing the planning of path points that require the equipment to complete rapid acceleration, deceleration, or lateral movement in a very short time, which are impossible to track in actual control. By restricting the trajectory search process to the intersection of the reachable region and the travel map, it can be ensured that each planned movement is executable by the equipment under its current capabilities, avoiding the disconnect between planning and control, and improving the stability of the system and the accuracy of trajectory tracking.

[0102] Specifically, starting from the connected domain where the starting point is located, the reachable region of the device at the current moment is obtained. The geometric intersection of the reachable region and the passable region in the next moment's travel graph is calculated to obtain the intersection region. If the intersection region is empty, it means that it is not safe to enter the free space of the next moment under the current motion state, and adjustments must be made. The intersection region is analyzed to see if it contains the identified narrow point of the connected domain in the next moment. If it does, the space profile required for the device to pass through the narrow point at the current expected speed is evaluated to determine whether it is completely contained within the intersection region and whether a safety clearance is left. If not, the size and shape of the reachable region are changed by adjusting the speed to slow down in advance, and the intersection is recalculated to obtain a larger intersection region that can fully accommodate the device to safely pass through the narrow point.

[0103] After completing one adjustment process, the intersecting regions are re-examined. If optimization is successful, the intersecting region is considered feasible and is used as a cross-section of the spatiotemporal corridor at the next moment. The optimized state of the device at this cross-section is then used as the new starting point for the next iteration. This process is repeated until the intersecting regions connect to the final connected domain where the task endpoint is located. All optimized intersecting regions generated during the iteration process are connected in chronological order to obtain a complete feasible region from the starting point to the endpoint. Through iterative optimization, reliable channels can be obtained in complex scenarios. By actively adjusting the motion strategy to filter out passable paths, the device's mobility can be maintained even in extremely crowded or structurally complex scenarios, improving the environmental adaptability and robustness of the device's movement process.

[0104] Furthermore, based on the feasible area, the movement trajectory is planned, and the path with the shortest movement distance is selected as the first movement trajectory, including:

[0105] S601. Divide the feasible area into multiple time slices according to the preset time interval, analyze the regional geometry corresponding to each time slice, and filter out the regional center.

[0106] S602. Connect the centers of adjacent time slices to obtain a set of movement trajectories;

[0107] S603. Select the shortest path for device movement from the set of movement trajectories and use it as the first movement trajectory.

[0108] In this embodiment, the feasible region is uniformly sliced ​​along the time dimension at preset time intervals, such as 0.1 seconds. The selection of the time interval needs to balance computational accuracy and real-time performance, typically being less than the device control cycle but greater than the execution cycle of the underlying controller. For each time slice, a two-dimensional, irregular polygonal region is obtained, representing the set of all safe spaces that the device is allowed to occupy at that moment. The geometry of each time slice region is analyzed, and the center of the largest inscribed circle in the time slice region is selected as the region center point. The region centers corresponding to each time slice are selected and arranged in chronological order to obtain a sequence of region centers.

[0109] It should be noted that by extracting the center of the largest inscribed circle within the safe area of ​​each time slice, a critical path point with the largest safety margin can be provided for trajectory planning. This ensures that each path point is as far away from all dangerous boundaries as possible. In subsequent trajectory generation, sufficient fault tolerance is provided for trajectory optimization and tracking control, which enhances the robustness of the equipment's motion process to control errors, sensor noise and dynamic disturbances, and reduces the risk of scratches or collisions.

[0110] Specifically, using the region center sequence as a constraint, a smooth curve is fitted using a fifth-order polynomial spline curve. For each segment of the trajectory between adjacent center points, the position coordinates are represented by a fifth-order polynomial function in terms of time. When fitting the spline curve, in addition to the requirement to pass through the given region center points, these center points are treated as soft constraints, allowing the trajectory to be fine-tuned within a small neighborhood around the center point. The neighborhood is defined as the region bounded by the radius of the largest inscribed circle of that point. By setting different neighborhood weights for each center point, multiple candidate trajectories that also satisfy the constraint of passing through the safe region can be generated, but with slight differences in smoothness, energy consumption, or time allocation, resulting in a set of movement trajectories.

[0111] It should be noted that by fitting parametric curves, safe path points are transformed into continuous motion trajectories, which ensures the smoothness and trackability of the trajectory. This avoids the discontinuous movement caused by traditional broken-line paths, where equipment needs to stop, turn, and accelerate again at inflection points. This improves motion efficiency, reduces task completion time, and lowers mechanical wear and energy consumption. Generating a set of motion trajectories provides a basis for optimization and comparison in selecting the shortest path, ensuring that the final selected first motion trajectory is the optimal result after considering multiple factors such as safety, smoothness, and efficiency.

[0112] Specifically, the total path length, estimated travel time, integral of the square of acceleration, and integral of the square of trajectory curvature are weighted and summed to calculate the corresponding path cost. The path with the lowest cost is then selected as the first movement path from the set of movement trajectories. The weighting coefficients can be set according to the needs of specific application scenarios. For example, time efficiency is more important in warehousing and logistics, while smoothness and comfort are more important in passenger-carrying scenarios.

[0113] Preferably, the total path length is obtained by integrating the trajectory parametric equation with arc length. The estimated travel time can be directly derived from the trajectory's time parameters, while the integrals of acceleration and curvature are calculated by differentiating the trajectory function. All candidate trajectories are traversed, and their respective path costs are calculated. The trajectory with the lowest path cost is selected as the first moving trajectory. Calculating the path cost ensures that the first moving trajectory is safe and smooth, avoiding the problem of longer actual travel time and higher energy consumption due to a short path but sharp curves requiring frequent acceleration and deceleration. Selecting a path that is more balanced and efficient in terms of speed, acceleration, and curvature enables the device to perform the moving task with better overall performance, improving the overall system performance.

[0114] Furthermore, taking the current position of each device as the center, based on the first movement trajectory, areas of trajectory conflict between devices are identified, the movement intentions of devices at conflict locations are analyzed, and the first movement trajectory is replanned to obtain a second movement trajectory, including:

[0115] S701. Taking the current position of each device as the center, based on the first movement trajectory, identify the areas of trajectory conflict between devices and construct a conflict map;

[0116] S702. Based on the conflict map, combined with the priority and status of the mobile tasks corresponding to the conflicting devices, analyze the device's mobile intention at the conflict location, replan the first mobile trajectory, and obtain the second mobile trajectory.

[0117] In this embodiment, the current position of each device is taken as the center, and the area of ​​trajectory conflict between devices is identified based on the first movement trajectory, thus constructing a conflict map. By analyzing the spatiotemporal overlap and relative motion relationship to construct the conflict map, the complex multi-body conflict problem is transformed into a clearly structured map, which can identify conflicts caused by motion trends earlier and more accurately, and quantify the urgency of the conflict. For example, at the intersection of warehouse passages, the paths of two AGVs will intersect in the next 3 seconds. This provides accurate data input for the subsequent intelligent scheduling process, improving the efficiency and effectiveness of conflict resolution.

[0118] Specifically, based on the conflict map, combined with the priority of the moving tasks corresponding to the conflicting devices and the device status, the movement intentions of the devices at the conflict locations are analyzed, and the first movement trajectory is replanned to obtain the second movement trajectory. By analyzing the dynamic priority reflecting the movement intentions of the devices and coordinating the replanning of the device trajectories, device conflicts can be quickly resolved, and intelligent scheduling can be performed according to real-time task requirements and device status. For example, a device with sufficient power and executing an urgent order will pass first due to its higher priority, while a device with low power and executing a routine task will actively give way or detour. This can ensure that critical tasks are completed in a timely manner, while balancing device load and energy consumption. By locally modifying the trajectories of low-priority devices, conflicts are eliminated while minimizing the impact of avoidance on the efficiency of individual devices, thus optimizing the overall throughput and operational efficiency of the multi-device system at the global level.

[0119] Furthermore, taking the current position of each device as the center, and based on the first movement trajectory, regions of trajectory conflict between devices are identified, and a conflict map is constructed, including:

[0120] S801. Taking the current position of each device as the center, simulate the change of the movement path of each device according to the first movement trajectory and the real-time status of the device, and count the probability that each position is occupied by the corresponding device according to the change, and construct the space occupancy map of each device.

[0121] S802. Calculate the product of the occupancy probabilities of different devices occupying the same position in the space occupancy map to obtain the joint probability, and select the positions with the joint probability greater than the preset conflict threshold as conflict points to construct a conflict point set.

[0122] S803. At the point of conflict, analyze the relative speed and relative position of the conflicting devices and calculate the probability of conflict.

[0123] S804. Using devices as nodes and conflict probabilities as edge weights, establish connections between corresponding nodes to construct a conflict graph.

[0124] In this embodiment, based on the device's first moving trajectory and real-time state, the control error and state estimation noise present in the actual movement of the device are analyzed. Monte Carlo simulation is used to analyze the uncertainty of the device's current state and the uncertainty of the control process, generating a large number of particles, each representing a possible future motion. These particles are obtained by forward integration of the differential driving model of the device's motion, with random disturbances conforming to noise statistics injected at each time step during the integration process. The simulation covers the entire planning time domain, outputting the spatiotemporal position of each particle. The entire planning space is discretized into a spatiotemporal grid, for example, a grid size of 0.1 m × 0.1 m, with a time slice interval of 0.1 seconds. For each spatiotemporal grid, the number of particles falling into that grid at that moment is counted, and this number is divided by the total number of particles to obtain the device's occupancy probability in that spatiotemporal grid. After traversing all grids, a spatial occupancy map of the device within the planning time domain is obtained.

[0125] It should be noted that by simulating the future movement of the equipment to construct the corresponding space occupancy map, an accurate data foundation for conflict detection can be provided. This can identify in advance edge risks where the trajectories do not conflict, but the actual position may intrude into the other party's safe area. For example, when two vehicles are parallel in a narrow passage, even if the nominal trajectories of the two vehicles maintain a distance, after modeling the lateral control error, their space occupancy maps may overlap at the edges, providing a data foundation for subsequent trajectory collaborative planning.

[0126] Specifically, based on the spatial occupancy maps of two devices, such as device A and device B, each spatiotemporal grid is traversed. The occupancy probability of device A and device B in that grid is read, and their product is calculated to obtain the joint probability of that grid. This reflects the estimated probability of the two devices appearing simultaneously at that specific time and location. A conflict threshold is set according to the system trajectory planning accuracy requirements; for example, a conflict threshold of 0.1 is set. The joint probability is compared with the conflict threshold, and locations with a joint probability greater than the preset conflict threshold are selected as conflict points, thus constructing a set of conflict points.

[0127] It should be noted that by combining joint probability calculation with threshold judgment, key spatiotemporal points with actual collision risks can be automatically and accurately extracted from the probability distribution, avoiding the limitations of collision detection methods based on fixed safety distances when dealing with devices at different speeds and in different directions of motion. For example, when two devices are moving towards each other, even if the current distance is still far, the joint probability of their future trajectories quickly exceeds the collision threshold, and they can be identified as collision points in advance. When two devices are moving in the same direction at a constant speed, even if the distance is relatively short, if their speeds are matched and the uncertainty is low, the joint probability will always be lower than the collision threshold, and unnecessary collision alarms will not be generated. This improves the accuracy and timeliness of collision detection.

[0128] Specifically, the risk of conflict depends not only on the probability of spatial overlap but also on the relative motion state of the equipment. Based on the spatiotemporal information of the conflict point and the real-time states of equipment A and B involved at that point, the relative velocity is calculated, and the relative distance between the two equipment at the predicted location of the conflict point is also calculated. The conflict probability is calculated by integrating the relative velocity and relative distance using exponential decay and a linear combination. The conflict probability increases as the relative distance decreases and as the relative velocity increases. Calculating the conflict probability provides accurate data support for subsequent prioritization decisions and resource reallocation, improving decision-making efficiency.

[0129] like Figure 3 As shown, each device is treated as a node in the graph. For any two devices, if there is a non-empty set of conflict points between them, an undirected edge is established between the corresponding two nodes. The weight of the edge is set to the maximum conflict probability value calculated from all conflict points of that device pair. Figure 3 The numbers in the graph represent the maximum conflict probability values ​​for corresponding device pairs, and a conflict graph is constructed. By constructing a conflict graph, complex conflict scenarios in which multiple devices influence each other can be quickly discovered. Conflicts are globally sorted according to the weight of the edges, allowing for the priority resolution of conflicts with the highest weights, thereby improving the overall coordination ability and global optimality of decision-making in multi-device systems when facing conflicts.

[0130] Furthermore, based on the conflict map, and combined with the priority and status of the movement tasks corresponding to the conflicting devices, the movement intentions of the devices at the conflict locations are analyzed. The first movement trajectory is then replanned to obtain the second movement trajectory, including:

[0131] S901. Based on the priority of the equipment relocation task and the equipment status, analyze the task urgency and status matching degree, and calculate the equipment relocation priority.

[0132] S902. Combining the conflict map and movement priority, keep the movement path of the device with high movement priority unchanged, adjust the movement path of the device with low movement priority at the conflict point, and replan the first movement trajectory to obtain the second movement trajectory.

[0133] In this embodiment, task attributes include task ID and type. Types include emergency production replenishment, routine warehouse handling, empty return, and automatic charging. Equipment status includes, but is not limited to, the remaining battery percentage provided by the battery management system and the load status determined by the load sensor. Based on the task attributes and equipment status, the task urgency and status matching degree are analyzed. Task attributes are mapped to a value between 0 and 1 according to a preset rule set; for example, an emergency replenishment task is assigned a value of 0.9, and routine handling is assigned a value of 0.5. The suitability of the equipment's current physical state for efficiently and reliably completing its current task is quantified as a value between 0 and 1; for example, a matching degree score of 1.0 is given when the battery level is above 80%, and only 0.3 is given when it is below 20%. The movement priority is calculated by summing the task urgency and the status matching degree.

[0134] It should be noted that by comprehensively considering the urgency of the task and the health and adaptability of the equipment itself, decisions that are more in line with the system's state can be made. For example, an AGV with sufficient power that is executing a high-value order nearing its deadline will be given a very high priority to ensure its smooth operation; while an AGV with low power that is executing a non-urgent empty return task will have a lower priority and must yield in the event of a conflict. This can ensure the smooth operation of critical production links, avoid delays in critical tasks due to resource competition, prevent low-power equipment from running out of energy due to competing for paths, and optimize operational efficiency and resource utilization.

[0135] Specifically, based on the conflict graph and the movement priorities of each device, all edges in the conflict graph are sorted in descending order of their weight values, prioritizing the processing of the most probable and urgent conflict relationships. For each edge to be processed, the movement priorities of the two devices are compared, and a priority difference threshold is set, for example, 0.15. If the difference between the two priorities is greater than the priority difference threshold, the device with the higher priority is given the right of way; if the difference is less than or equal to the priority difference threshold, they are considered to have equal priorities, and the remaining distance from their current positions to their respective task endpoints can be compared, allowing the device closer to the endpoint to pass first.

[0136] Preferably, the local trajectory replanning for low-priority equipment includes the following adjustment strategies: Within the time window before the conflict point occurs, plan a trajectory for the low-priority equipment to decelerate to a stop or travel at a low speed, waiting for the high-priority equipment to completely pass through the conflict area before accelerating again; near the spatial location of the conflict point, replan a local path for the low-priority equipment, such as a smooth arc, to bypass the high-priority equipment from the side, ensuring that the new path always remains within the equipment's own safe and feasible area; combining slight early deceleration and minor path offset. After adjustment, use spline curves to smoothly connect the new avoidance segment with the equipment's original first movement trajectory, and verify that the new trajectory still satisfies the corresponding constraints.

[0137] Whenever a low-priority device's trajectory is successfully adjusted to resolve a conflict with a high-priority device, the change in the device's trajectory affects its conflict relationships with other devices in the conflict graph. The conflict graph is dynamically updated, resolving conflict edges are removed or reduced, and the new trajectory of the adjusted device is reassessed to determine if it will create new conflicts with other devices. Based on the updated conflict graph, the remaining most urgent conflicts are processed, and this process is repeated until the weights of all edges in the conflict graph have decreased to below an acceptable safety threshold. The set of trajectories adjusted by all devices through this collaborative process is used as the second movement trajectory.

[0138] It should be noted that by adjusting local trajectories based on priority and iteratively, efficient and collaborative global conflict resolution and system efficiency optimization can be achieved. This can ensure that high-priority critical tasks are executed without interruption while only making local and minimal modifications to the trajectories of low-priority tasks, minimizing the impact on the overall system throughput and improving the overall task completion rate and response speed of the system.

[0139] like Figure 4 As shown, a real-time replanning system for equipment trajectories in complex scenarios is used to implement a real-time replanning method for equipment trajectories in complex scenarios, including:

[0140] The complex scene analysis module analyzes scene obstacles and equipment movement processes based on pre-acquired scene environment data and device status data, and constructs a real-time scene environment;

[0141] The device trajectory planning module responds to the device movement task, takes the real-time location of the device as the starting point, selects a feasible area in the real-time scene environment, and plans a movement trajectory according to the feasible area to obtain the first movement trajectory.

[0142] The device trajectory replanning module takes the current position of each device as the center, identifies the area of ​​trajectory conflict between devices based on the first movement trajectory, analyzes the movement intention of the devices at the conflict position, and replans the first movement trajectory to obtain the second movement trajectory.

[0143] The equipment control module controls the movement of each device according to the second movement trajectory in order to replan the device trajectory in real time.

[0144] In this embodiment, the complex scene analysis module integrates and processes raw environmental data from various sensors, such as LiDAR and cameras, as well as status data from the device's own controllers, such as PLCs and onboard computers. Through the separation of dynamic and static data, feature extraction of moving objects, and cross-frame correlation tracking, a real-time scene environment is constructed. Trajectory planning based on a dynamic map that reflects future environmental evolution trends improves the system's predictability and safety in dealing with sudden obstacles and complex interactions. The device trajectory planning module receives the device's movement task instruction and, starting from the device's current position, selects a feasible area from the starting point to the end point within the real-time scene environment through access map construction, narrow point identification, kinematic reachability analysis, and iterative region optimization. It then generates an optimal initial movement trajectory as the first movement trajectory. By combining safety, feasibility, and efficiency in trajectory planning, the efficiency and smoothness of the device's movement process are improved.

[0145] Specifically, the equipment trajectory replanning module addresses path conflicts that occur when multiple devices operate simultaneously. It constructs a graph model quantifying conflict relationships and dynamically calculates the movement priority of each device based on factors such as task urgency and device status. The module adjusts the initial trajectories of conflicting devices, generating conflict-free coordinated trajectories as the second movement trajectory. Through intelligent collaboration and optimal conflict resolution among multiple devices, it dynamically makes decisions based on the global operation status, ensuring smooth execution of high-priority tasks while minimizing the impact on low-priority tasks, thus improving equipment movement efficiency and task completion efficiency. The equipment control module receives the second movement trajectory as control commands. Using a high-precision trajectory tracking controller and considering the device's real-time pose, it calculates the corresponding motor control quantities, driving the device to move precisely and smoothly along the planned path. This ensures accurate implementation of upper-level commands and provides real-time feedback to compensate for errors and external disturbances, guaranteeing the stability and accuracy of equipment movement even in dynamic environments.

[0146] Example 2:

[0147] This embodiment uses a specific intelligent warehousing and logistics center scenario to illustrate the complete workflow of the technical solution. This scenario includes static facilities such as high-bay racks, picking workstations, and charging areas, while multiple automated guided vehicles (AGVs) perform handling tasks, and personnel or other mobile devices act as dynamic obstacles.

[0148] Specifically, upon system startup and initialization, the complex scene analysis module acquires raw point cloud and image data through a LiDAR scanning array deployed on the warehouse roof, visual cameras, and sensors on each AGV. Simultaneously, it receives real-time status reports from all AGVs via a wireless network, including their ID, location, speed, load, and battery level. At a certain moment, the system acquires a set of environmental point clouds and status data for two AGVs (AGV-001 and AGV-002). AGV-001 is located at coordinates (10.0, 5.0), with a speed of 0.8 m / s, facing east, and its task is to transport urgent materials from area A to workstation B. AGV-002 is located at coordinates (5.0, 12.0), with a speed of 0.5 m / s, facing south, and its task is routine replenishment. The module processes two consecutive frames of point cloud data (100 milliseconds apart), calculating the displacement rate of change for each spatial point. Points with a rate of change greater than 0.15 m / s are classified as dynamic data, while the rest are static data. From the dynamic data, the module identified three moving clusters using a clustering algorithm: one corresponding to AGV-001, one to AGV-002, and a smaller, faster-moving cluster identified as a worker walking in the aisle. Features of each cluster, including bounding boxes and velocity vectors, were extracted and correlated with features from the previous frame. A similarity threshold of 0.7 was set, successfully tracking these three moving targets. A Kalman filter was then used to predict their trajectories for the next 3 seconds. Static data was used to fine-tune a pre-set high-precision warehouse grid map. The module outputs a real-time scene environment, including the latest static map, the three dynamic trajectories (AGV-001 trajectory, AGV-002 trajectory, and worker trajectory), and their predictions.

[0149] The equipment trajectory planning module responds to the task request. At this time, AGV-001 receives a new movement instruction, requiring it to move from its current position (10.0, 5.0) to the loading platform located at (25.0, 5.0). Using this starting point as a reference, and based on the real-time scene environment, the occupancy probability of each spatiotemporal unit is calculated within the planning time domain over the next 8 seconds. Areas with an occupancy probability exceeding 0.65 are marked as impassable, generating a series of binary access maps on time slices. Analyzing the connected components of the access map at each moment, a narrow point near coordinates (18.0, 5.0) is identified, formed due to the small distance between the shelves on both sides. Based on AGV-001's current speed of 0.8 m / s and maximum acceleration, the module calculates the reachable area for the next moment as a forward fan shape. The module iteratively searches for the intersection of the reachable region and the future travel path, and fine-tunes the expected speed of AGV-001 at narrow points, for example, slightly reducing it to 0.6 m / s in advance to ensure its contour can pass safely, ultimately forming a spatiotemporal safety corridor from the starting point to the ending point as a feasible region. Within this feasible region, the module selects a series of path points at 0.2-second intervals and fits them with a quintic spline curve to generate a smooth first movement trajectory with a total length of approximately 15.5 meters that conforms to the AGV's dynamic constraints. Similarly, AGV-002 also generates its corresponding first movement trajectory using its own trajectory planning module.

[0150] The equipment trajectory replanning module coordinates potential conflicts among multiple devices. This module simulates the forward motion of AGV-001 and AGV-002 separately, considering control uncertainties, and generates a spatial occupancy probability map of their future trajectories. Calculations reveal that approximately 4.2 seconds into the future, near coordinates (15.0, 8.0), the joint probability of the two trajectories exceeds the preset conflict threshold of 0.2, marking it as a conflict point. At this point, the relative speeds of the two devices are relatively high and their movement directions intersect, resulting in a conflict probability of 0.35. A conflict graph is constructed, with AGV-001 and AGV-002 as nodes, and an edge weight of 0.35 connecting these two nodes. The module calculates movement priorities: AGV-001 performs an emergency delivery task, with a task urgency assessment of 0.9; its battery is sufficient, and its state matching degree is 0.95; its overall priority P1 is calculated to be 0.9. AGV-002 performs a regular task, with a task urgency assessment of 0.4; its battery is moderate, and its state matching degree is 0.7; its overall priority P2 is 0.5. Since P1 is significantly higher than P2, the module decides to keep AGV-001's first moving trajectory unchanged. For AGV-002, the module inserts a local adjustment within the time window of its trajectory's conflict point: AGV-002 begins to decelerate approximately 1 second before the conflict point, causing it to pass through the conflict area 0.8 seconds later than originally planned, thus allowing AGV-001 to proceed first. The adjusted trajectory of AGV-002 is then smoothed again to form its second moving trajectory, and AGV-001's second moving trajectory is its unchanged first moving trajectory.

[0151] The equipment control module executes the following steps: For each AGV, the module receives a second movement trajectory as input. Taking AGV-001 as an example, its underlying controller operates at a frequency of 100 Hz. In each control cycle, the controller reads the precise pose fed back from the real-time encoder and IMU of AGV-001 and compares it with the target pose and speed of the second movement trajectory at the current moment. The controller calculates the optimal left and right wheel speed commands and sends them to the drive motors for execution, accurately tracking the planned trajectory to safely and efficiently avoid moving personnel and pass through potential conflict points within the coordinated time, ultimately reaching the target loading platform. The entire perception-planning-control cycle repeats at a fixed period, such as 500 milliseconds, achieving real-time replanning of the equipment trajectory and continuous adaptation to the dynamic environment.

[0152] The above description is merely a preferred embodiment of this application. The scope of protection of this application is not limited to the above embodiments. All technical solutions falling within the scope of this application's concept are within the scope of protection of this application. It should be noted that for those skilled in the art, any improvements and modifications made without departing from the principles of this application should also be considered within the scope of protection of this application.

Claims

1. A real-time replanning method for device trajectories in complex scenarios, characterized in that, include: Based on the pre-acquired scene environment data and device status data, analyze the scene obstacles and device movement process to construct a real-time scene environment; In response to the device movement task, starting from the real-time location of the device, a feasible area is selected in the real-time scene environment, and a movement trajectory is planned according to the feasible area to obtain the first movement trajectory. Taking the current position of each device as the center, based on the first movement trajectory, identify the areas where the trajectories of the devices conflict, analyze the movement intention of the devices at the conflict positions, and replan the first movement trajectory to obtain the second movement trajectory; The movement of each device is controlled according to the second movement trajectory in order to replan the device trajectory in real time.

2. The real-time replanning method for device trajectories in complex scenarios according to claim 1, characterized in that, The step of analyzing scene obstacles and device movement processes based on pre-acquired scene environment data and device status data to construct a real-time scene environment includes: Based on the pre-acquired scene environment data and device status data, calculate the rate of change of corresponding values ​​between adjacent sampling points, and filter out dynamic data with a rate of change greater than a preset rate of change threshold and static data with a rate of change less than or equal to the preset rate of change threshold. Based on dynamic and static data, the movement of obstacles and equipment in the scene is analyzed to construct a real-time scene environment.

3. The method for real-time replanning of device trajectories in complex scenarios according to claim 2, characterized in that, The process of analyzing scene obstacles and equipment movement based on dynamic and static data to construct a real-time scene environment includes: Based on dynamic data, the features of the corresponding moving objects are extracted, the feature correlation between adjacent positions is analyzed, and the positions with feature correlation greater than a preset correlation threshold are connected to obtain the movement trajectory of the scene obstacles, and an obstacle trajectory set is constructed. By combining static data and a preset scene map, the obstacle trajectory set is mapped onto the scene and the device movement process is analyzed to construct a real-time scene environment.

4. The real-time replanning method for device trajectories in complex scenarios according to claim 1, characterized in that, The response to the device movement task, starting from the device's real-time location, filters out feasible areas in the real-time scene environment, plans a movement trajectory according to the feasible areas, and obtains a first movement trajectory, including: In response to device movement tasks, starting from the device's real-time location, the region corresponding to the connected path is filtered out in the real-time scene environment, and feasible regions corresponding to the connection between the start and end points in multiple regions are extracted. Based on the feasible area, the movement trajectory is planned, and the path with the shortest movement path is selected as the first movement trajectory.

5. The real-time replanning method for device trajectories in complex scenarios according to claim 4, characterized in that, In response to the device movement task, starting from the device's real-time location, the region corresponding to the connected path is filtered out in the real-time scene environment, and feasible regions corresponding to the connection between the start and end points in multiple regions are extracted, including: In response to the device movement task, starting from the device's real-time location, the location occupancy probability of each obstacle is calculated according to the real-time scene environment. Locations with a location occupancy probability greater than a preset probability threshold are marked as impassable, and locations with a location occupancy probability less than or equal to the preset probability threshold are marked as passable, thus constructing a passage map. Connected regions are constructed by selecting interconnected passable locations in the passage map, the shape of each connected region is analyzed, and the point with the smallest boundary distance is identified as the narrow point, thus constructing a set of narrow points. Starting from the connected domain corresponding to the origin, construct the reachable region by analyzing the maximum and minimum distances the device can move based on its speed and acceleration. The system filters out the intersecting regions where the reachable area and the traffic map intersect. Within the intersecting regions, it analyzes the traffic conditions at the corresponding narrow points. The intersecting regions are then optimized by adjusting the device speed or boundary optimization. This process is repeated until the system connects to the connected domain where the device's movement task ends, thus obtaining the feasible region.

6. The real-time replanning method for device trajectories in complex scenarios according to claim 4, characterized in that, The step of planning the movement trajectory according to the feasible area and selecting the shortest path for the equipment movement as the first movement trajectory includes: According to the preset time interval, the feasible area is divided into multiple time slices, the geometry of the area corresponding to each time slice is analyzed, and the center of the area is selected. Connecting the centers of adjacent time slices yields a set of movement trajectories; The path with the shortest movement distance from the set of movement trajectories is selected as the first movement trajectory.

7. The real-time replanning method for device trajectories in complex scenarios according to claim 1, characterized in that, The process involves taking the current position of each device as the center, identifying areas of trajectory conflict between devices based on the first movement trajectory, analyzing the movement intentions of devices at the conflict locations, and replanning the first movement trajectory to obtain the second movement trajectory, including: Using the current location of each device as the center, and based on the first movement trajectory, identify the areas where trajectories conflict between devices, and construct a conflict map; Based on the conflict map, and combined with the priority and status of the mobile tasks corresponding to the conflicting devices, the intention of the devices to move at the conflict location is analyzed, and the first mobile trajectory is replanned to obtain the second mobile trajectory.

8. The real-time replanning method for device trajectories in complex scenarios according to claim 7, characterized in that, The step of identifying areas of trajectory conflict between devices, centered on the current position of each device and based on the first movement trajectory, and constructing a conflict map includes: Taking the current location of each device as the center, based on the first movement trajectory and the real-time status of the devices, simulate the changes in the movement path of each device, and calculate the probability that each location is occupied by the corresponding device according to the changes, and construct the space occupancy map of each device. Calculate the product of the probabilities of different devices occupying the same location in the space occupancy map to obtain the joint probability, and select the locations whose joint probability is greater than a preset conflict threshold as conflict points to construct a conflict point set; At the point of conflict, analyze the relative speed and relative position of the conflicting devices, and calculate the probability of conflict. By treating devices as nodes and conflict probabilities as edge weights, connections are established between corresponding nodes to construct a conflict graph.

9. A real-time replanning method for device trajectories in complex scenarios according to claim 8, characterized in that, The process involves analyzing the movement intentions of devices at conflict locations based on a conflict map, combined with the priority and status of the movement tasks corresponding to the conflicting devices, and replanning the first movement trajectory to obtain a second movement trajectory, including: Based on the priority of the equipment relocation task and the equipment status, the urgency of the task and the matching degree of the status are analyzed to calculate the relocation priority of the equipment. By combining the conflict map and movement priority, the movement path of the device with higher movement priority remains unchanged, while the movement path of the device with lower movement priority is adjusted at the conflict point. The first movement trajectory is then replanned to obtain the second movement trajectory.

10. A real-time replanning system for device trajectories in complex scenarios, characterized in that, A method for real-time replanning of device trajectories in complex scenarios as described in any one of claims 1 to 9 includes: The complex scene analysis module analyzes scene obstacles and equipment movement processes based on pre-acquired scene environment data and device status data, and constructs a real-time scene environment; The device trajectory planning module responds to the device movement task, takes the real-time location of the device as the starting point, selects a feasible area in the real-time scene environment, and plans a movement trajectory according to the feasible area to obtain the first movement trajectory. The device trajectory replanning module takes the current position of each device as the center, identifies the area of ​​trajectory conflict between devices based on the first movement trajectory, analyzes the movement intention of the devices at the conflict position, and replans the first movement trajectory to obtain the second movement trajectory. The equipment control module controls the movement of each device according to the second movement trajectory in order to replan the device trajectory in real time.

Images