Cooperative obstacle avoidance trajectory planning method, device and chip

Abstract

Embodiments of the present invention provide a cooperative obstacle avoidance trajectory planning method, apparatus, and chip. The cooperative obstacle avoidance trajectory planning method includes: generating a spatiotemporal calibration point sequence for multiple cooperatively planned vehicles; obtaining a predefined guidance path; determining path following parameters based on the predefined guidance path and the spatiotemporal calibration point sequence; determining inter-vehicle obstacle avoidance parameters to avoid collisions based on the distances between the multiple cooperatively planned vehicles; determining at least one cooperative initial trajectory; determining a safe driving corridor for each vehicle based on the cooperative initial trajectory; identifying at least two conflicting vehicles; determining a relative safe driving corridor between vehicles based on the at least two safe driving corridors for each vehicle; determining a cooperative trajectory based on the safe driving corridor for each vehicle and the relative safe driving corridor between vehicles; and controlling the cooperatively planned vehicles to travel according to the cooperative trajectory. The solution of the present invention rapidly generates safe, smooth, and efficient multi-vehicle cooperative trajectories, significantly improving the reliability of multi-vehicle cooperative passage.

Description

Technical Field

[0001] This invention relates to the field of mining vehicle management technology, and more specifically, to a collaborative obstacle avoidance trajectory planning method, device, and chip. Background Technology

[0002] With the deepening of the intelligent transformation of open-pit mines, autonomous driving of heavy-duty mining truck platoons has become key to improving transportation efficiency and safety in mining areas. In this operating mode, unstructured intersections, as core intersection nodes of the road network, have inherent characteristics such as lack of lane lines, irregular boundaries, and mixed traffic of multiple vehicles. Coupled with the slow dynamic response and strict control constraints of heavy-duty mining trucks, this places extremely high demands on the real-time performance, scenario adaptability, and safety of multi-vehicle collaborative trajectory planning.

[0003] However, in the face of complex and dynamic scenarios where dozens or even hundreds of devices frequently interact in large-scale mining areas, it is difficult to quickly generate safe, smooth and efficient multi-vehicle collaborative trajectories when traffic density at intersections changes dynamically or when sudden situations occur. This leads to a decrease in traffic efficiency and even causes secondary risks, indicating that the reliability of multi-vehicle collaborative trajectory planning is insufficient. Summary of the Invention

[0004] The purpose of this invention is to provide a collaborative obstacle avoidance trajectory planning method, device, and chip that can solve the problem of insufficient reliability in multi-vehicle collaborative trajectory planning in open-pit mines.

[0005] In view of this, an embodiment of the first aspect of the present invention provides a cooperative obstacle avoidance trajectory planning method.

[0006] A second aspect of the present invention provides a cooperative obstacle avoidance trajectory planning device.

[0007] An embodiment of the third aspect of the present invention provides a chip.

[0008] To achieve the above objectives, an embodiment of the first aspect of the present invention provides a cooperative obstacle avoidance trajectory planning method, comprising: generating a spatiotemporal calibration point sequence for multiple cooperative planning vehicles based on a predefined planning time domain and distance step length, wherein the multiple cooperative planning vehicles correspond to the same travel destination; obtaining a predefined guidance path corresponding to each cooperative planning vehicle within a target area; for each cooperative planning vehicle, determining path following parameters based on the predefined guidance path and the spatiotemporal calibration point sequence, wherein the path following parameters are used to guide the cooperative planning vehicle to travel along the predefined guidance path; determining inter-vehicle obstacle avoidance parameters for avoiding inter-vehicle collisions based on the distance between the multiple cooperative planning vehicles; and determining, based on... Path following parameters and inter-vehicle obstacle avoidance parameters determine at least one cooperative initial trajectory; based on the cooperative initial trajectory, a safe driving corridor for each vehicle is determined, which is used to determine the driving space of each cooperatively planned vehicle within the target area, and the driving space does not exceed the map boundary corresponding to the target area; based on the relative positional relationship of multiple safe driving corridors within the time interval corresponding to the spatiotemporal calibration point sequence, at least two conflicting vehicles are determined; for conflicting vehicles, a relative safe driving corridor between vehicles is determined based on at least two safe driving corridors; a cooperative trajectory is determined based on the safe driving corridor for each vehicle and the relative safe driving corridor between vehicles; and the cooperatively planned vehicles are controlled to drive according to the cooperative trajectory.

[0009] This invention generates a unified spatiotemporal calibration point sequence and guidance path for multiple vehicles, calculates obstacle avoidance parameters between vehicles for path following and collaborative planning, and generates a collaborative initial trajectory. Based on the initial trajectory, a safe driving corridor for each vehicle is constructed. Conflicting vehicles are identified by analyzing the spatiotemporal overlap between corridors, and dedicated relative safe driving corridors are allocated for conflicting vehicles. Under the dual constraints of the vehicle corridor and the relative corridor, the collaborative trajectory is optimized and generated to control vehicle movement.

[0010] Understandably, the collaborative obstacle avoidance trajectory planning method provided by this invention can quickly generate safe, smooth and efficient multi-vehicle collaborative trajectories, significantly improving the reliability of multi-vehicle collaborative passage.

[0011] In some technical solutions, optionally, obtaining a predefined guidance path corresponding to each collaboratively planned vehicle within the target area includes: obtaining the entrance and exit positions of multiple intersections corresponding to the target area; determining multiple path direction combinations based on the entrance and exit positions; and matching the corresponding predefined guidance path from the multiple path direction combinations based on the real-time position and destination of the collaboratively planned vehicle. The predefined guidance path includes a preset path and a static boundary.

[0012] In this scheme, when a collaboratively planned vehicle enters the area, the nearest entrance node is matched based on its real-time location, and the corresponding exit node is matched based on its destination, thereby dynamically determining a specific predefined guidance path from the set of path direction combinations.

[0013] Understandably, dynamic and precise vehicle routing guidance can improve the collaborative efficiency of multiple collaboratively planned vehicles.

[0014] In some technical solutions, optionally, for each collaboratively planned vehicle, path following parameters are determined based on a predefined guidance path and a spatiotemporal calibration point sequence, including: obtaining the current position of the collaboratively planned vehicle; determining multiple target guidance points of the collaboratively planned vehicle on the corresponding predefined guidance path based on the current position, wherein the multiple target guidance points correspond to the spatiotemporal calibration point sequence; and determining path following parameters based on the current position and the target guidance points.

[0015] In this scheme, the current projection point of the vehicle is located on the predefined guidance path curve, and a series of target guidance points corresponding to each future time are calculated along the guidance path based on the future time represented by the spatiotemporal calibration point sequence. By calculating the lateral deviation, heading deviation and curvature deviation between the vehicle's predicted trajectory state and this series of target guidance points, a quantitative path following parameter is constructed to minimize the path tracking error in trajectory optimization.

[0016] Understandably, by transforming macroscopic path guidance into precise quantitative tracking targets, the accuracy and stability of trajectory control can be improved.

[0017] In some technical solutions, optionally, at least one cooperative initial trajectory is determined based on path following parameters and inter-vehicle obstacle avoidance parameters, including: determining the net external force acting on each cooperatively planned vehicle based on the path following parameters and inter-vehicle obstacle avoidance parameters; determining the angle mapping component and acceleration mapping component corresponding to the cooperatively planned vehicle based on the net external force; obtaining a vehicle kinematic model; determining multiple trajectory segments corresponding to a spatiotemporal calibration point sequence based on the vehicle kinematic model and the angle mapping component and acceleration mapping component; and determining the cooperative initial trajectory based on the multiple trajectory segments.

[0018] In this scheme, the path following parameters are combined with the obstacle avoidance parameters between vehicles. By calculating the weighted sum of their negative gradient directions, a virtual resultant external force is derived for each vehicle to guide it to simultaneously pursue path following and collision avoidance.

[0019] Furthermore, this resultant external force is decomposed into a lateral component perpendicular to the vehicle body and a longitudinal component parallel to the vehicle body in the vehicle coordinate system, and mapped into executable control commands, namely the desired steering angle mapping component and acceleration mapping component.

[0020] A vehicle kinematics model describing the motion relationship of vehicles is introduced. Starting from the current state and using the mapping component as the control input, the discrete future state points corresponding to the spatiotemporal calibration point sequence are calculated, forming multiple trajectory segments.

[0021] By integrating these trajectory segments arranged in chronological order, a complete state sequence is generated, which is the initial trajectory for multi-vehicle collaboration under preliminary coordination. This efficiently transforms complex constraints into feasible trajectories, laying the foundation for collaboration.

[0022] In some technical solutions, optionally, the safe driving corridor of the autonomous vehicle is determined based on the initial collaborative trajectory, including: obtaining the radius of the vehicle body envelope disk of each collaboratively planned vehicle; expanding the static boundary corresponding to the target area based on the radius of the vehicle body envelope disk to determine the expansion boundary; for each collaboratively planned vehicle, determining the first local box corresponding to each target guidance point along the initial collaborative trajectory; synchronously expanding the first box boundary of all first local boxes with a preset step size until the expanded first box boundary contacts the expansion boundary or reaches the preset maximum expansion range to determine the local safe area; and determining the safe driving corridor of the autonomous vehicle based on multiple local safe areas.

[0023] In this scheme, the radius of the vehicle body envelope disk, including a safety margin, is obtained, and the static environment boundary is expanded accordingly to generate an expanded boundary for distance detection. Initial local boxes with aligned orientations are generated for each target guide point along the cooperative initial trajectory. All boxes synchronously expand their boundaries outwards at fixed steps until they contact the expanded boundary or reach their dimensional limits, thereby determining a maximum local safety area for each trajectory point.

[0024] These discrete local safety zones are smoothly connected and merged to form a spatially continuous three-dimensional volume that is synchronized with the trajectory in time—the vehicle safety corridor. This creates a continuous passage space for each vehicle that ensures static safety, improving the reliability of trajectory generation.

[0025] In some technical solutions, optionally, for conflicting vehicles, a relative safe driving corridor between the vehicles is determined based on at least two autonomous vehicle safe driving corridors, including: for each conflicting vehicle, determining a relative motion trajectory based on a cooperative initial trajectory; determining a relative collision area range based on the radius of the vehicle body envelope disk of the conflicting vehicles; determining a second local box corresponding to each target guide point of the conflicting vehicles based on the relative motion trajectory; synchronously expanding the second box boundaries of all second local boxes by a preset step size until the expanded second box boundaries contact the expansion boundary or reach the maximum expansion range, thereby determining a collision safety zone; and determining a relative safe driving corridor between the vehicles based on multiple collision safety zones.

[0026] In this scheme, for the identified conflicting vehicles, the relative motion trajectory describing the change in their relative positions is first determined through coordinate transformation based on their respective cooperative initial trajectories. Then, the radii of the body envelope disks of the two vehicles are added together to obtain a joint safety distance for collision detection, which is used to define the relative collision area. A collision safety area that avoids collision and environmental interference is determined at each moment, and a corresponding relative safe driving corridor between the vehicles is generated.

[0027] Understandably, defining mutually exclusive safety spaces for conflicting vehicles can eliminate collision risks and improve the safety of actual operation of cooperative trajectories.

[0028] In some technical solutions, optionally, the cooperative trajectory is determined based on the vehicle's safe driving corridor and the relative safe driving corridor of the workshop, including: determining preliminary constraints based on the vehicle's safe driving corridor and the relative safe driving corridor of the workshop; determining optimization parameters based on the preliminary constraints and preset trajectory optimization rules; taking the initial cooperative trajectory as the starting point, iteratively optimizing the initial cooperative trajectory based on the optimization parameters to obtain an optimized trajectory; in each iteration, updating the vehicle's safe driving corridor and the relative safe driving corridor of the workshop based on the currently obtained optimized trajectory to update the optimization parameters; when the optimized trajectory meets the preset convergence condition, stopping the iteration and determining the optimized trajectory that meets the convergence condition as the cooperative trajectory.

[0029] In this scheme, the geometric boundaries of the vehicle's safe driving corridor and the relative safe driving corridor within the workshop are transformed into mathematical inequalities between vehicle position and relative position, establishing preliminary constraints. Using the initial cooperative trajectory as the starting point for iteration, numerical solutions are performed based on the optimization parameters under the constraints to obtain a progressively improved optimized trajectory. When both the improvement in the objective function and the constraint violation of the optimized trajectory are below preset thresholds, convergence is determined, and this trajectory is output as the final cooperative trajectory.

[0030] Understandably, by using closed-loop iterative optimization, a safe, smooth, and globally optimal cooperative trajectory is generated, thereby improving the reliability of multi-vehicle cooperative trajectories.

[0031] In some technical solutions, the cooperative obstacle avoidance trajectory planning method may optionally include: globally synchronizing the cooperative trajectory within the platoon to which multiple cooperative planning vehicles belong; acquiring in real time the status information of the cooperative planning vehicles, the status information of obstacles in the target area, and the status information of other cooperative planning vehicles; determining, based on the status information, whether the dynamic replanning trigger condition is met, wherein the dynamic replanning trigger condition includes at least one of the following: detecting a new obstacle, the deviation between the actual trajectory of the cooperative planning vehicle and the cooperative trajectory exceeding a preset threshold, or communication interruption with at least one other cooperative planning vehicle; when the dynamic replanning trigger condition is met, triggering the dynamic replanning process to redetermine the cooperative trajectory; when the cooperative planning vehicle leaves the core passage area of ​​the target area and meets the preset path return condition, controlling the cooperative planning vehicle to return to the corresponding predefined guidance path and adjusting the vehicle speed to restore the platoon driving state.

[0032] In this scheme, after generating the cooperative trajectory, it is first globally synchronized within the formation to ensure consistent commands. The system monitors the status of its own vehicle, other vehicles, and environmental obstacles in real time, and determines whether the dynamic replanning trigger conditions are met based on preset rules, such as the appearance of new obstacles, excessive trajectory tracking deviation, or communication interruption.

[0033] Once the conditions are met, a dynamic replanning process is immediately triggered to regenerate a collaborative trajectory adapted to the new situation. When the vehicle safely leaves the core collaborative area and meets the path return conditions, it is controlled to smoothly return to the predefined guidance path, and the vehicle speed is adjusted to restore platooning cruising, thus completing the transition from close collaboration to normal driving. Through closed-loop dynamic management throughout the entire process, the robustness and adaptability of the system are improved.

[0034] A second aspect of the present invention provides a cooperative obstacle avoidance trajectory planning device, comprising: a spatiotemporal anchoring module, configured to generate a spatiotemporal calibration point sequence for multiple cooperative planning vehicles based on a predefined planning time domain and a distance step length, wherein the multiple cooperative planning vehicles correspond to the same travel destination; a path presetting module, configured to obtain a predefined guidance path corresponding to each cooperative planning vehicle within a target area; a gravity construction module, configured to determine path following parameters for each cooperative planning vehicle based on the predefined guidance path and the spatiotemporal calibration point sequence, wherein the path following parameters are used to guide the cooperative planning vehicle to travel along the predefined guidance path; a repulsion construction module, configured to determine inter-vehicle obstacle avoidance parameters for avoiding inter-vehicle collisions based on the distance between the multiple cooperative planning vehicles; and a trajectory generation module, configured to generate a trajectory based on the path following parameters. The system comprises the following modules: a parameter module and vehicle obstacle avoidance parameters to determine at least one cooperative initial trajectory; a corridor construction module to determine a safe driving corridor for each vehicle based on the cooperative initial trajectory, wherein the safe driving corridor determines the driving space of each cooperatively planned vehicle within the target area, and the driving space does not exceed the map boundary corresponding to the target area; a conflict determination module to determine at least two conflicting vehicles based on the relative positional relationship of multiple safe driving corridors within the corresponding time interval of the spatiotemporal calibration point sequence; a corridor optimization module to determine a relative safe driving corridor between vehicles based on at least two safe driving corridors for conflicting vehicles; a trajectory determination module to determine a cooperative trajectory based on the safe driving corridor for each vehicle and the relative safe driving corridor between vehicles; and a cooperative planning module to control the cooperatively planned vehicles to drive according to the cooperative trajectory.

[0035] An embodiment of the third aspect of this application provides a chip including a processor and a communication interface, the communication interface and the processor being coupled together, the processor being used to run a program or instructions to implement the steps of the cooperative obstacle avoidance trajectory planning method as described in the first aspect.

[0036] Additional aspects and advantages of the technical solutions of the present invention will become apparent in the following description or may be learned by practice of the invention. Attached Figure Description

[0037] Figure 1 One of the flowcharts of the collaborative obstacle avoidance trajectory planning method according to this application is shown;

[0038] Figure 2 A second flowchart illustrating the collaborative obstacle avoidance trajectory planning method according to this application is shown.

[0039] Figure 3 The third flowchart of the collaborative obstacle avoidance trajectory planning method according to this application is shown;

[0040] Figure 4 The fourth flowchart of the cooperative obstacle avoidance trajectory planning method according to this application is shown;

[0041] Figure 5 The fifth flowchart of the collaborative obstacle avoidance trajectory planning method according to this application is shown;

[0042] Figure 6 The sixth flowchart of the collaborative obstacle avoidance trajectory planning method according to this application is shown;

[0043] Figure 7 The seventh flowchart of the cooperative obstacle avoidance trajectory planning method according to this application is shown;

[0044] Figure 8 The eighth flowchart of the cooperative obstacle avoidance trajectory planning method according to this application is shown;

[0045] Figure 9 A schematic block diagram of the collaborative obstacle avoidance trajectory planning device according to this application is shown;

[0046] Figure 10 A schematic diagram of the real-time collaborative trajectory planning method for multiple intelligent connected vehicles at unstructured intersections in open-pit mines, according to this application, is shown.

[0047] Figure 11 A schematic diagram of the process for generating the initial trajectory based on the simulation according to this application is shown;

[0048] Figure 12 A schematic diagram illustrating the process of constructing a two-level safe driving corridor according to this application is shown;

[0049] Figure 13 A schematic diagram of the lightweight iterative optimization and optimal trajectory solution according to this application is shown;

[0050] Figure 14 A flowchart illustrating the trajectory tracking execution and dynamic closed-loop control process according to this application is shown.

[0051] Among them, 900: Collaborative obstacle avoidance trajectory planning device; 902: Spatiotemporal anchoring module; 904: Path preset module; 906: Gravity construction module; 908: Repulsion construction module; 910: Trajectory generation module; 912: Corridor construction module; 914: Conflict determination module; 916: Corridor optimization module; 918: Trajectory determination module; 920: Collaborative planning module. Detailed Implementation

[0052] To better understand the above-described objectives, features, and advantages of the embodiments of the present invention, the embodiments of the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be noted that, unless otherwise specified, the embodiments and features described in these embodiments can be combined with each other.

[0053] Many specific details are set forth in the following description in order to provide a full understanding of this application. However, embodiments of the invention may also be implemented in other ways different from those described herein. Therefore, the scope of protection of this application is not limited to the specific embodiments disclosed below.

[0054] With the intelligent transformation of open-pit mines, platooning of heavy-duty mining trucks has become the mainstream operation mode in mining areas. As the core intersection of the mining area road network, unstructured intersections, due to their inherent characteristics of no lane lines, irregular boundaries, and mixed traffic of multiple vehicles, coupled with the characteristics of heavy-duty mining trucks such as slow steering and braking and strict dynamic constraints, place extremely high demands on the real-time performance, adaptability, and safety of multi-vehicle collaborative trajectory planning.

[0055] Many related solutions directly transplant urban road collaborative planning technology, or only achieve basic single-vehicle obstacle avoidance and timing control, which cannot adapt to the operational needs of unstructured mining scenarios and have significant technical shortcomings.

[0056] The related technologies closest to this invention mainly fall into two categories:

[0057] The first category is multi-vehicle collaborative trajectory planning technology for transplanting urban structured roads. It takes centralized optimal control as its core and achieves multi-vehicle collaborative optimization for regular urban intersections. However, it has not been deeply adapted to unstructured mining scenarios and the characteristics of heavy-duty mining trucks. Moreover, it relies on high computing power support from the cloud and cannot meet the real-time computing requirements of mining vehicle terminals without cloud dependence.

[0058] The second category is basic access control and obstacle avoidance technology in the mine itself, which mainly relies on first-come-first-served time-based reservations and passive emergency obstacle avoidance by single vehicles. It can only achieve basic access control and lacks the ability to plan fine-grained trajectories for multi-vehicle collaboration, making it difficult to balance mine access safety and transportation efficiency.

[0059] The collaborative obstacle avoidance trajectory planning method, device, and chip provided in this application will be described in detail below with reference to specific embodiments and application scenarios.

[0060] This embodiment provides a collaborative obstacle avoidance trajectory planning method, such as... Figure 1 As shown, it includes:

[0061] Step S100: Based on the predefined planning time domain and distance step length, generate a spatiotemporal calibration sequence for multiple collaborative planning vehicles, where the driving destinations of the multiple collaborative planning vehicles are the same;

[0062] Step S102: Obtain the predefined guidance path corresponding to each collaboratively planned vehicle within the target area;

[0063] Step S104: For each collaborative planning vehicle, determine the path following parameters based on the predefined guidance path and the spatiotemporal calibration point sequence. The path following parameters are used to guide the collaborative planning vehicle to travel along the predefined guidance path.

[0064] Step S106: Determine the inter-vehicle obstacle avoidance parameters to avoid inter-vehicle collisions based on the distances between multiple collaboratively planned vehicles;

[0065] Step S108: Determine at least one cooperative initial trajectory based on the path following parameters and the obstacle avoidance parameters between vehicles;

[0066] Step S110: Determine the safe driving corridor for each vehicle based on the initial collaborative trajectory. The safe driving corridor is used to determine the driving space of each collaboratively planned vehicle within the target area. The driving space shall not exceed the map boundary corresponding to the target area.

[0067] Step S112: Determine at least two conflicting vehicles based on the relative positional relationship of multiple autonomous vehicle safe driving corridors within the corresponding time intervals of the spatiotemporal calibration point sequence;

[0068] Step S114: For conflicting vehicles, determine the relative safe driving corridor between the vehicles based on at least two vehicle safe driving corridors;

[0069] Step S116: Determine the cooperative trajectory based on the vehicle's safe driving corridor and the relative safe driving corridor between the vehicles;

[0070] Step S118: Control the collaboratively planned vehicle to travel according to the collaborative trajectory.

[0071] This invention generates a unified spatiotemporal calibration point sequence and guidance path for multiple vehicles, calculates obstacle avoidance parameters between vehicles for path following and collaborative planning, and generates a collaborative initial trajectory. Based on the initial trajectory, a safe driving corridor for each vehicle is constructed. By analyzing the spatiotemporal overlap between corridors, conflicting vehicles are identified, and dedicated relatively safe driving corridors are allocated for conflicting vehicles.

[0072] Under the dual constraints of the autonomous vehicle corridor and the relative corridor, a cooperative trajectory is generated to control vehicle movement.

[0073] Understandably, the collaborative obstacle avoidance trajectory planning method provided by this invention can quickly generate safe, smooth and efficient multi-vehicle collaborative trajectories, significantly improving the reliability of multi-vehicle collaborative passage.

[0074] Specifically, based on the predefined planning time domain and distance step length, a spatiotemporal calibration point sequence is generated for multiple collaboratively planned vehicles.

[0075] The planning time domain defines the length of future time for forward planning, while the distance step length is the precision of time division within the time domain. The purpose of generating the spatiotemporal calibration point sequence is to establish a unified future time reference framework for all cooperating vehicles.

[0076] Starting from the current moment, the discrete step length increases until the planned time domain length is reached, thereby generating a series of future discrete time points.

[0077] These time points are the spatiotemporal calibration points, and their sequence provides a unified time coordinate basis for subsequent trajectory parameterization, ensuring that the trajectories of all vehicles can be described, calculated, and coordinated on the same time dimension.

[0078] Obtain the predefined guidance path corresponding to each collaboratively planned vehicle within the target area. The predefined guidance path is a macroscopic reference issued from the global path planning layer, and is usually composed of lane centerlines from a high-precision map or a collision-free coarse path generated based on the start and end points.

[0079] A predefined guidance path indicates the expected travel corridor for vehicles to move from their current location to a common destination, but it does not include precise spatiotemporal speed and obstacle avoidance details. For each vehicle, its guidance path is independent and predetermined.

[0080] For each collaboratively planned vehicle, path following parameters are determined based on its unique predefined guidance path and a shared sequence of spatiotemporal calibration points. The core function of the path following parameters is to quantify the deviation between the vehicle's trajectory and the ideal guidance path, and to guide the vehicle to travel along the guidance path as closely as possible.

[0081] Among them, collaborative planning vehicles refer to a group of intelligent connected vehicles that interact with each other through vehicle networking or collaborative perception systems, with the aim of achieving the optimal driving goal of the group (such as passing through an intersection together or maintaining a convoy to drive towards the same distribution center).

[0082] Collaborative planning of vehicles includes, but is not limited to, heavy-duty trucks operating in open-pit mine scenarios.

[0083] These collaborative planning vehicles do not make independent decisions during the planning process, but rather incorporate their states, intentions, and constraints into a unified optimization framework for collaborative computation.

[0084] The determination process involves mapping spatiotemporal calibration points onto the guidance path, calculating a corresponding path reference point for each future time point, including its position, heading angle, and curvature. Path following parameters are represented by an optimization objective function that penalizes lateral and heading angle deviations between the vehicle's predicted trajectory points and these path reference points. These deviations are typically calculated by determining the normal distance and angle difference between the trajectory point and the nearest point on the guidance path.

[0085] Simultaneously, obstacle avoidance parameters for preventing collisions are determined based on the distances between multiple collaboratively planned vehicles. These parameters aim to translate the requirement of maintaining a safe distance between vehicles into mathematical constraints. The determination process requires real-time acquisition or prediction of the states of all collaborative vehicles, including their positions and profiles. For any two vehicles, the Euclidean distance between their profile polygons at future spatiotemporal calibration points is calculated. The obstacle avoidance parameters represent a constraint requiring that the distance must be greater than a preset safety threshold, which typically includes the vehicle's dimensions and an additional buffer distance.

[0086] Mathematical constraints corresponding to the obstacle avoidance parameters between vehicles are applied to the trajectory optimization problem to ensure that the generated trajectory will not cause geometric overlap between vehicles at any time.

[0087] Determining at least one cooperative initial trajectory based on path following parameters and vehicle obstacle avoidance parameters is achieved by constructing and solving a constrained optimization problem.

[0088] The decision variables in the optimization problem are the state sequences of each vehicle at spatiotemporal calibration points. The objective function minimizes the path tracking deviation represented by the path following parameters, while strictly satisfying the safety distance constraints represented by the obstacle avoidance parameters between vehicles. Solving this problem yields a preliminary set of multi-vehicle trajectories that have considered mutual avoidance, i.e., cooperative initial trajectories. These trajectories are synchronized in time and have initially avoided collisions in space.

[0089] Based on the collaborative initial trajectory, the autonomous vehicle safe driving corridor for each vehicle is further determined. The autonomous vehicle safe driving corridor is a continuous safe space region in time and space centered on the vehicle itself, used to precisely define the drivable space of the vehicle within the target area.

[0090] Using the initial collaborative trajectory as the centerline, and comprehensively considering the vehicle's own contours, control uncertainties, perception errors, and the map boundaries corresponding to the target area, such as lane lines, curbs, and obstacles, the vehicle expands outwards from both sides of the centerline. The width of the expansion must ensure that the vehicle will never collide with the static environment while traveling within the corridor boundaries. Therefore, the driving space is strictly limited to the vehicle's safe driving corridor and never exceeds the map boundaries.

[0091] Subsequently, at least two conflicting vehicles are identified based on the relative positions of multiple autonomous vehicle safe driving corridors within the corresponding time intervals of the spatiotemporal calibration sequence. This step aims to identify potential competition for spatial resources.

[0092] At each spatiotemporal calibration point, check whether the geometric polygons of any two vehicle safety travel corridors overlap. If overlap exists at several consecutive time points, the two corresponding vehicles are determined to have a spatial conflict within the time period and are marked as a pair of conflicting vehicles. The root cause of the conflict is that their initial safety corridors occupied the same physical space at the same time.

[0093] For each pair of conflicting vehicles identified, a relative safe driving corridor between the two vehicles is determined based on their respective safe driving corridors.

[0094] The relative safe driving corridor in the workshop is no longer centered on a single vehicle, but rather specifically describes the spatial division of relative motion between the two conflicting vehicles.

[0095] The spatiotemporal region where the two vehicles overlap is segmented, for example, by allocating a dedicated, non-overlapping sub-channel to each vehicle based on the priority of the initial trajectory, traffic rules, or optimization principles. This sub-channel is the portion of space separated from the original overlapping corridor of the two vehicles, ensuring that they can safely pass each other; it constitutes a strong constraint on the relative motion of the two vehicles.

[0096] Ultimately, a final cooperative trajectory is determined based on each vehicle's own safe driving corridor and the relative safe driving corridors of all vehicles involved. This is a refined trajectory optimization process. During optimization, the vehicle trajectory is constrained to simultaneously lie within its own safe driving corridor and within the relative safe driving corridors of all conflicting vehicles. Under this dual spatial constraint, the trajectory is re-optimized, typically with the goals of smoothness, comfort, or efficiency. The resulting cooperative trajectory ensures the safety of each vehicle with its static environment while completely resolving dynamic spatial conflicts between vehicles.

[0097] Optimizing the trajectory of each vehicle requires simultaneously satisfying two hard constraints: first, it must be completely within its own safe driving corridor; and second, it must be completely within the vehicle-to-vehicle relatively safe driving corridor between itself and all conflicting vehicles.

[0098] With this dual safety space in place, optimization goals can focus on improving trajectory smoothness, comfort, or fuel economy. The resulting coordinated trajectory is the ultimate solution that ensures safe passage and overall performance optimization.

[0099] For example, controlling a collaboratively planned vehicle to travel along a collaborative trajectory involves converting the planned discrete spatiotemporal point sequence into control commands and sending them to the drive-by-wire actuators of the collaboratively planned vehicle. The controller tracks the position, speed, and heading information of each point in the trajectory sequence and calculates specific commands for throttle, braking, and steering angle through underlying control algorithms, ensuring that the vehicle's actual motion state remains consistent with the planned collaborative trajectory, thereby achieving safe collaborative driving for multiple vehicles.

[0100] In some embodiments, the predefined planning time domain and distance step length are not fixed, but can be dynamically adjusted according to the real-time traffic flow density and the average driving speed of the collaboratively planned vehicles within the target area.

[0101] In areas with traffic congestion or high vehicle density, the planning time domain is automatically shortened and the distance step length is reduced to generate more refined and responsive short-term collaborative strategies; in areas with smooth traffic or sparse vehicles, the planning time domain is automatically extended to support more forward-looking and smoother long-term trajectory planning.

[0102] This adaptive spatiotemporal discretization mechanism enables the generation of spatiotemporal calibration point sequences to match complex and ever-changing real-world driving scenarios, achieving a dynamic balance between computational efficiency and planning foresight.

[0103] In some embodiments, the determination of the vehicle's safe driving corridor is optionally based not only on the cooperative initial trajectory and static map boundaries, but also on the real-time perceived dynamic obstacle occupancy grid information.

[0104] Non-cooperative traffic participants, such as pedestrians, bicycles, or other vehicles not included in the cooperative planning, are represented by the perception module as occupants of the grid with spatiotemporal probabilities.

[0105] When constructing the safe driving corridor for autonomous vehicles, these dynamically occupied grids are treated as no-entry zones and excluded from the corridor space. The resulting safe driving corridor for autonomous vehicles is a spatially and temporally safe passage tunnel that avoids both static boundaries and dynamic obstacles, providing a more comprehensive safety guarantee for subsequent trajectory optimization.

[0106] In some embodiments, optionally, such as Figure 2 As shown, step S102: Obtain the predefined guidance path corresponding to each collaboratively planned vehicle within the target area, including:

[0107] Step S1020: Obtain the entrance and exit locations of multiple intersections corresponding to the target area;

[0108] Step S1022: Determine multiple path direction combinations based on the entrance and exit locations;

[0109] Step S1024: Based on the real-time location and destination of the collaboratively planned vehicle, match the corresponding predefined guidance path from multiple path direction combinations. The predefined guidance path includes preset paths and static boundaries.

[0110] In this embodiment, when a collaboratively planned vehicle enters the area, the nearest entrance node is matched based on its real-time location, and the corresponding exit node is matched based on its destination, thereby dynamically determining a specific predefined guidance path from the set of path direction combinations.

[0111] For example, the predefined guidance path includes a geometrically preset path along the centerline of the lane and a static driving boundary determined by physical elements such as lane lines and curbs, which together constitute a reference channel that combines guidance and safety constraints.

[0112] Understandably, dynamic and precise vehicle routing guidance can improve the collaborative efficiency of multiple collaboratively planned vehicles.

[0113] Specifically, the target area usually refers to a road network area consisting of multiple intersections, such as a square block containing multiple intersections.

[0114] The entrance location refers to the starting point where a vehicle legally enters a road within the target area from outside or other road sections. It usually corresponds to the beginning of a lane or the stop line position at an intersection.

[0115] The exit location refers to the endpoint where a vehicle legally leaves the target area from a certain road within the area, usually corresponding to the end of a lane or the exit boundary of an intersection.

[0116] Location information is extracted from lane-level topology data of high-precision maps that describe road connectivity. The process of determining these locations essentially abstracts the physical road network into a connected graph consisting of entrance and exit nodes, laying the data foundation for subsequent generation of traffic directions.

[0117] Determining multiple path direction combinations based on entrance and exit locations involves enumerating all possible macroscopic driving directions within the target area. A path direction combination represents an ordered set of road segments and turning maneuvers necessary to reach a specific exit from a particular entrance location.

[0118] The process of determining these combinations involves path searching on the aforementioned connected graph. Based on the connectivity between lanes, the system traverses all legal connected paths from each entrance to each exit. Legal connectivity means that the path must follow the lane's direction of travel and comply with traffic rules; for example, intersections where left turns are prohibited will not generate path direction combinations that include left-turn actions. The resulting multiple path direction combinations collectively constitute a complete set of travel options within the target area, from any possible origin to any possible destination.

[0119] Based on the real-time location and destination of the collaboratively planned vehicle, matching the corresponding predefined guidance path from multiple path direction combinations is the process of dynamically assigning a specific reference path to a particular vehicle.

[0120] The vehicle's real-time location coordinates are linked to a specific lane in the target area's road network using map matching technology, thereby determining its current most likely entrance location or the nearest road segment node.

[0121] Simultaneously, based on the task objectives, the vehicle's destination within the target area is defined, with each destination corresponding to a specific exit location. The determination process involves using the vehicle's current entrance location and destination exit location as start and end conditions, and searching for a match within the previously generated set of path direction combinations. Successfully matched path direction combinations are selected as the predefined guidance path for collaboratively planned vehicles.

[0122] The predefined guidance path includes a preset path and static boundaries. The preset path is the specific geometric expression of the aforementioned path direction combination, typically refined into a smooth curve extending along the lane centerline provided by a high-precision map. This curve precisely reflects the trajectory that the ideal center of mass of the cooperatively planned vehicle should traverse when following the path direction combination.

[0123] Static boundaries are rigid restrictions on the vehicle's driving space, consisting of physical or regulatory boundaries on both sides of a pre-defined path. These boundaries typically include precise geometric descriptions of insurmountable static elements such as lane lines, curbs, medians, and guardrails.

[0124] When determining static boundaries, the system uses a preset path as the centerline and extends to both sides to these physical or regulatory constraints, thus forming a spatial corridor that allows vehicle passage. The preset path provides a guiding centerline, while the static boundaries define the safe area. Together, they constitute a complete reference channel with safe area information, serving as the basic framework for subsequent trajectory optimization.

[0125] In some embodiments, optionally, such as Figure 3 As shown, step S104: For each collaboratively planned vehicle, determine the path following parameters based on the predefined guidance path and spatiotemporal calibration sequence, including:

[0126] Step S1040: Obtain the current location of the collaboratively planned vehicle;

[0127] Step S1042: Determine multiple target guidance points for the collaboratively planned vehicle on the corresponding predefined guidance path based on the current location. The multiple target guidance points correspond to a spatiotemporal calibration point sequence.

[0128] Step S1044: Determine the path following parameters based on the current location and the target guide point.

[0129] In this embodiment, the current projection point of the vehicle is located on the predefined guidance path curve, and a series of target guidance points corresponding to each future time are calculated along the guidance path based on the future time represented by the spatiotemporal calibration point sequence. By calculating the lateral deviation, heading deviation and curvature deviation between the vehicle's predicted trajectory state and this series of target guidance points, a quantitative path following parameter is constructed to minimize the path tracking error in trajectory optimization.

[0130] Understandably, by transforming macroscopic path guidance into precise quantitative tracking targets, the accuracy and stability of trajectory control can be improved.

[0131] Specifically, the current position of the collaboratively planned vehicle is obtained. The current position is usually obtained in real time in a high-precision map coordinate system by fusing perception data from the global satellite navigation system, inertial measurement unit, and lidar or vision sensor, with the rear axle center or center of mass of the vehicle as the reference point. It is a three-dimensional coordinate containing latitude, longitude and elevation, or a two-dimensional state containing planar coordinates and heading angle.

[0132] The location information has sufficiently high accuracy and real-time performance, which directly determines the accuracy of the starting point for all subsequent guide point mappings.

[0133] Determining multiple target guidance points for the collaboratively planned vehicle on the corresponding predefined guidance path based on the current location is a key step in establishing the vehicle's expected future state sequence.

[0134] The determination process is as follows: First, on the predefined guide path geometric curve, find the point closest to the vehicle's current position and use it as the path projection point at the current moment.

[0135] By combining the future time series defined by the spatiotemporal calibration point sequence, the expected driving state of the vehicle is projected forward.

[0136] For example, based on the vehicle's current speed and acceleration, the arc length it should travel along the guide path at each future spatiotemporal calibration point is estimated. Then, starting from the current path projection point, these calculated arc lengths are intercepted along the guide path to obtain a series of future path points.

[0137] These waypoints are multiple target guidance points. Each point corresponds precisely to a specific future time point and contains the position coordinates of the future time point on the guidance path, the tangent direction (i.e., the heading angle), and the path curvature information of the future time point.

[0138] The number of target guidance points is exactly the same as the length of the spatiotemporal calibration point sequence, thus establishing a discrete reference state sequence for future path tracking in the time dimension.

[0139] Determining path-following parameters based on the current location and the target guide point transforms the desired tracking behavior into mathematical indicators that can be directly processed and minimized in the trajectory optimization problem.

[0140] The path following parameter is a cost function used to penalize vehicles for deviations from the expected guide point in their predicted trajectory.

[0141] For each future spatiotemporal calibration point, the predicted state of the vehicle at the corresponding time is compared with the state of its corresponding target guidance point, and the deviation between the two is calculated.

[0142] These deviations mainly include three aspects: first, lateral deviation, which is the distance from the vehicle's predicted position to the path normal at the target guide point, measuring whether the vehicle deviates from the path center; second, heading angle deviation, which is the angle difference between the vehicle's predicted heading angle and the path tangent at the target guide point, measuring whether the vehicle's attitude is aligned with the path direction; and third, curvature deviation, which is the difference between the curvature of the vehicle's predicted trajectory and the inherent curvature of the path at the target guide point, measuring whether the vehicle's steering matches the degree of path curvature. The specific mathematical form of the path following parameters is usually a weighted sum of the squares of all these deviations.

[0143] The above series of geometric comparisons and calculations are ultimately combined into a scalar value, the magnitude of which directly and quantitatively reflects the overall degree to which the vehicle's future trajectory deviates from the ideal guidance path.

[0144] In subsequent trajectory optimization, one of the goals of the optimization algorithm is to minimize this path following parameter, thereby driving the vehicle's generated trajectory to closely follow the predefined guide path.

[0145] In some embodiments, when determining multiple target guidance points on a predefined guidance path based on the current location, instead of using a simple equal time interval arc length calculation, an adaptive look-ahead distance adjustment mechanism based on the vehicle's current motion state is introduced.

[0146] The system dynamically adjusts the arc length for searching the target guide point based on the vehicle's real-time longitudinal velocity and lateral acceleration. When the vehicle speed is high or on a curve, the look-ahead distance is automatically increased to obtain path reference information further away, ensuring a smooth and stable trajectory. When the vehicle speed is low or on a straight road, the look-ahead distance is appropriately reduced to improve the immediacy of tracking. This dynamic mapping method matches the distribution of target guide points with the vehicle's dynamic response characteristics, providing a more reasonable sequence of reference states for generating a comfortable and accurate tracking trajectory.

[0147] In some embodiments, the path following parameters optionally incorporate a dynamic weighting coefficient related to road curvature when calculating lateral and heading deviations.

[0148] In curved areas with a large curvature of the guide path, the weight of lateral position deviation is increased to more strictly constrain vehicles to travel along the center of the curve and prevent them from deviating from the lane; in straight areas with a small curvature of the guide path, the weight of heading angle deviation is appropriately increased to enable vehicles to align with the direction of travel more quickly.

[0149] The weighting adjustment function is correlated with the absolute value of the path curvature. This transforms the path following parameter from a uniform error penalty term into an adaptive evaluation index that can intelligently adjust the emphasis based on road geometry, thereby achieving different control balances on curves and straight sections.

[0150] In some embodiments, the path following parameters are optionally calculated based on a sequence of predicted states within a rolling optimization window.

[0151] In each planning cycle, instead of calculating the immediate deviation at the current moment, the cumulative deviation between all predicted states and their corresponding target guidance points within a finite window encompassing multiple future spatiotemporal calibration points is calculated and used as the path following parameter value for that cycle. The window length is adjustable. This method makes parameter determination forward-looking, enabling the detection of future deviation trends and the application of corrective forces before the vehicle actually deviates from the path, thus achieving predictive path tracking and enhancing trajectory stability and robustness.

[0152] In some embodiments, optionally, such as Figure 4 As shown, step S108: Determine at least one cooperative initial trajectory based on path following parameters and inter-vehicle obstacle avoidance parameters, including:

[0153] Step S1080: Determine the net external force acting on each cooperatively planned vehicle based on the path following parameters and the obstacle avoidance parameters between vehicles;

[0154] Step S1082: Determine the angle mapping component and acceleration mapping component corresponding to the collaboratively planned vehicle based on the resultant external force;

[0155] Step S1084: Obtain the vehicle kinematics model;

[0156] Step S1086: Based on the vehicle kinematics model, determine multiple trajectory segments corresponding to the spatiotemporal calibration point sequence according to the angle mapping component and the acceleration mapping component;

[0157] Step S1088: Determine the cooperative initial trajectory based on multiple trajectory segments.

[0158] In this embodiment, the path following parameters are combined with the obstacle avoidance parameters between vehicles. By calculating the weighted sum of their negative gradient directions, a virtual resultant external force is derived for each vehicle to guide it to simultaneously pursue path following and collision avoidance.

[0159] Furthermore, this resultant external force is decomposed into a lateral component perpendicular to the vehicle body and a longitudinal component parallel to the vehicle body in the vehicle coordinate system, and mapped into executable control commands, namely the desired steering angle mapping component and acceleration mapping component.

[0160] A vehicle kinematics model describing the motion relationship of vehicles is introduced. Starting from the current state and using the mapping component as the control input, the discrete future state points corresponding to the spatiotemporal calibration point sequence are calculated through iterative forward integration, forming multiple trajectory segments.

[0161] Finally, these trajectory segments arranged in chronological order are integrated to generate a complete state sequence, which is the initial trajectory for multi-vehicle collaboration under preliminary coordination. This efficiently transforms complex constraints into feasible trajectories and lays the foundation for collaboration.

[0162] Specifically, determining the net external force acting on each collaboratively planned vehicle based on path following parameters and obstacle avoidance parameters between vehicles is a process of transforming the optimization objective into a virtual guiding force.

[0163] The net external force here is not a real physical force, but a mathematical quantity constructed within an optimization framework to guide vehicle motion. Its direction points towards the state space direction that can simultaneously reduce path following error and collision risk. The path following parameters quantify the deviation between the vehicle state and the ideal path, and their negative gradient direction indicates the state adjustment trend required to reduce the deviation.

[0164] The obstacle avoidance parameters between vehicles quantify the degree to which the distance between vehicles violates the safety threshold, and their negative gradient direction indicates the trend of state adjustment required to increase the distance and avoid collisions.

[0165] Determining the net external force involves calculating the weighted vector sum of the two negative gradient directions. This vector sum defines a virtual net force that comprehensively expresses the instantaneous motion tendency that the vehicle should follow at its current position in order to simultaneously better track the path and avoid collisions.

[0166] Determining the angle and acceleration mapping components corresponding to the collaboratively planned vehicle based on the net external force decomposes the overall motion trend into two independent control dimensions that the vehicle can execute. The vehicle's motion control ultimately adjusts lateral movement through steering wheel angle and longitudinal acceleration through throttle and brake. The angle and acceleration mapping components are precisely the projections of the net external force onto these two control dimensions.

[0167] In the vehicle's current motion coordinate system, the resultant external force vector is decomposed into a lateral component perpendicular to the current vehicle body direction and a longitudinal component along the vehicle body direction.

[0168] After the magnitude of the lateral component is standardized by the vehicle dynamics parameters, it is mapped to a desired steering wheel angle or yaw rate increment, i.e., the steering angle mapping component.

[0169] The magnitude of the longitudinal component is then mapped to a desired acceleration or deceleration value, i.e., the acceleration mapping component.

[0170] Obtaining a vehicle kinematic model is the mathematical foundation for establishing the relationship between control commands and state change prediction.

[0171] Vehicle kinematic models describe the kinematic characteristics of a vehicle. Taking a bicycle model with front-wheel steering as an example, the bicycle model is a system of differential or difference equations that includes state variables such as vehicle position coordinates, heading angle, longitudinal velocity, and front wheel steering angle.

[0172] The vehicle kinematics model defines how the vehicle's state will evolve in the next short time interval after being given the current state and after applying a steering command and an acceleration command.

[0173] Based on the vehicle kinematics model, multiple trajectory segments corresponding to the spatiotemporal calibration point sequence are determined according to the angle mapping component and the acceleration mapping component. This process generates discrete trajectory points through forward integration.

[0174] Using the vehicle's real-time state as the initial condition, and the calculated steering angle and acceleration mapping components as the control inputs at the current moment, the vehicle's kinematics model is substituted into the kinematics model for integral calculation to extrapolate the vehicle's state a certain time after the walk.

[0175] This new state corresponds to the first future time point in the spatiotemporal calibration point sequence. Then, using this new state as a new starting point, based on the newly calculated mapping components for the next time point, the above integration process is repeated to extrapolate to the next time point.

[0176] This iterative process calculates a corresponding predicted vehicle state for each future time point in the spatiotemporal calibration point sequence. Each continuous state change from the current moment to a future moment constitutes a trajectory segment. Multiple trajectory segments together form a discrete sequence of future states covering the entire planning time domain.

[0177] The cooperative initial trajectory is a complete data structure that contains, in chronological order, the complete state information of the vehicle at all spatiotemporal calibration points, including position, heading, speed, and acceleration. Determining the trajectory involves connecting and encapsulating the state points calculated in the above steps, corresponding to each discrete time point, in timestamp order, into a continuous trajectory object.

[0178] The initial collaborative trajectory is a set of trajectories calculated in parallel by all collaboratively planned vehicles, considering only the initial path following and obstacle avoidance constraints. It provides a preliminary draft trajectory with basic collaborative characteristics for subsequent more refined driving space division and conflict resolution.

[0179] In some embodiments, optionally, the step of decomposing the resultant external force into angular mapping components and acceleration mapping components involves first performing amplitude limiting processing on the resultant external force based on the vehicle dynamics feasible domain before vector projection. Based on the vehicle's physical limits, such as maximum tire lateral force, maximum driving and braking forces, the system pre-calculates a joint feasible envelope of lateral and longitudinal acceleration achievable under the current vehicle speed and road adhesion conditions.

[0180] When determining the mapping components, the original resultant external force vector is first decomposed according to the vehicle coordinate system. Then, the resulting lateral and longitudinal components are constrained to the maximum values ​​allowed in the corresponding directions by the joint feasible envelope. The constrained components are then standardized and mapped to obtain the angle mapping components and acceleration mapping components. This ensures that the control commands obtained from the decomposition are always within the range that vehicle dynamics can execute, avoiding the generation of infeasible control commands from the source.

[0181] In some embodiments, the acquired vehicle kinematic model is optionally not a single fixed model, but a model library that can be dynamically selected based on the vehicle type and current motion state in collaborative planning. The model library contains a variety of models suitable for different scenarios, such as a simplified kinematic model suitable for low-speed parking scenarios, a dynamic model that considers tire side slip characteristics suitable for medium- and high-speed driving, and an articulated vehicle model suitable for special vehicles such as trailers.

[0182] When determining the specific model to be used, the system automatically selects the model with the highest matching degree from the model library based on the vehicle's real-time speed, vehicle configuration information, and task scenario. Using a dynamically selected model that better reflects the vehicle's current actual characteristics to extrapolate trajectory segments significantly improves the accuracy of trajectory prediction, laying a more solid theoretical foundation for generating reliable cooperative initial trajectories.

[0183] In some embodiments, optionally, such as Figure 5 As shown, step S110: Determine the safe driving corridor of the vehicle based on the cooperative initial trajectory, including:

[0184] Step S1100: Obtain the radius of the vehicle body envelope disk for each collaboratively planned vehicle;

[0185] Step S1102: Based on the radius of the vehicle body envelope disk, expand the static boundary corresponding to the target area to determine the expansion boundary;

[0186] Step S1104: For each collaboratively planned vehicle, along the initial collaborative trajectory, determine the first local box corresponding to each target guidance point;

[0187] Step S1106: Expand the first box boundary of all first local boxes synchronously with a preset step size until the expanded first box boundary contacts the expansion boundary or reaches the preset maximum expansion range, and determine the local safe area.

[0188] Step S1108: Determine the safe driving corridor for the vehicle based on multiple local safety zones.

[0189] In this embodiment, the radius of the vehicle body envelope disk, including a safety margin, is obtained, and the static environment boundary is expanded accordingly to generate an expanded boundary for distance detection. Initial local boxes with aligned directions are generated for each target guide point along the cooperative initial trajectory. All boxes synchronously expand their boundaries outward at fixed steps until they contact the expanded boundary or reach the size limit, thereby determining a maximum local safety area for each trajectory point.

[0190] These discrete local safety zones are smoothly connected and merged to form a spatially continuous three-dimensional volume that is synchronized with the trajectory in time—the vehicle safety corridor. This creates a continuous passage space for each vehicle that ensures static safety, improving the reliability of trajectory generation.

[0191] Specifically, the vehicle body envelope disk is a conservative geometric model used to simplify vehicle collision detection in path planning. It encloses the irregular shape of the vehicle within a disk, and the radius of the disk is the radius of the smallest circle that ensures no part of the vehicle exceeds the boundary of the disk.

[0192] The outer contour of the vehicle is obtained from its parameters, typically based on the vehicle length and width, and the radius of its circumcircle is used as the base value. An additional buffer distance is then added to compensate for tracking errors and state estimation errors in the vehicle control system, and to provide extra safety margin.

[0193] The final obtained body envelope disk radius is a comprehensive radius value that includes physical dimensions and safety redundancy. The body envelope disk radius defines the minimum circular space that the vehicle as a whole needs to occupy at any time.

[0194] Based on the radius of the vehicle body's envelope disk, the static boundary corresponding to the target area is expanded to determine the expanded boundary, which transforms environmental constraints into a restricted area map for spatial search.

[0195] Static boundaries are the geometric representations of insurmountable physical elements such as lane lines, curbs, and medians, and are typically composed of a series of continuous line segments or polygons.

[0196] Dilation is a computational geometry operation. The process involves taking each point on the static boundary as the center and the radius of the vehicle body's envelope disk as the radius, and expanding outward in a circular fashion. The outer contour of the union of all these circular regions constitutes a new, thicker boundary, namely the dilated boundary.

[0197] Once the center point of any vehicle (i.e., the center of the disk) touches this expanded boundary, it is equivalent to the vehicle's actual outline touching the original static boundary. Through this process, the original linear or planar constraints are transformed into a no-go zone centered on the vehicle's center point, which facilitates distance detection.

[0198] For each collaboratively planned vehicle, along the initial collaborative trajectory, determine the first local box corresponding to each target guidance point.

[0199] The first local box is a rectangular area with an initial size centered on the target guide point, and its direction is aligned with the tangent direction of the trajectory at the point (i.e., the vehicle heading).

[0200] When determining the initial dimensions of the local box, its width is usually set to be slightly larger than the width of the vehicle, and its length is set to a small fixed value.

[0201] This initial box represents an initial spatial range aligned with the vehicle's orientation, allowing the vehicle to move freely without considering environmental boundaries. Generating such an aligned local box for each target guidance point on the trajectory forms a series of initial discrete spatial units distributed along the trajectory.

[0202] With a preset step size, the first box boundary of all first local boxes is expanded synchronously until the expanded first box boundary contacts the expansion boundary or reaches the preset maximum expansion range. The local safe area is determined by iteratively searching for the maximum safe space for each trajectory point.

[0203] The preset step size defines the incremental expansion of the box in the width and length directions during each iteration.

[0204] The iterative process for determining the local safe region is as follows: for each first local box, its four boundaries are expanded outward synchronously and uniformly. After each expansion, it is immediately checked whether the expanded box boundary has geometric interference with the aforementioned expansion boundary, or whether the box size has reached a preset limit value. As long as no contact occurs and the limit is not reached, the next round of expansion continues with a step size.

[0205] Once the boundary in a certain direction touches the expansion boundary or reaches its limit, expansion in that direction stops, while expansion in other directions continues until it stops completely. When expansion stops in all directions, the region enclosed by the box boundaries at this moment is the maximum feasible subspace at the trajectory point, called the local safe region.

[0206] This area ensures that when the vehicle's center point is located within it, the entire vehicle body envelope disk will never collide with the original static boundary.

[0207] Determining a safe driving corridor for a vehicle based on multiple local safety zones involves connecting and merging the maximum safety space of discrete points into a continuous safety passage.

[0208] Since each target guidance point corresponds to a local safe zone, and these zones are distributed along the trajectory sequence, determining the safe driving corridor for the vehicle involves smoothly connecting these discrete local safe zones, which may vary in shape and size, in space and time.

[0209] Using the boundary points of these local safe regions as constraints, a smooth, continuously varying boundary is generated by polygon interpolation or by constructing a minimum convex hull. This boundary encloses the local safe region of each point on the trajectory.

[0210] The resulting three-dimensional structure, which is continuous in space and synchronized with the trajectory in time, is the safe driving corridor for autonomous vehicles.

[0211] The vehicle safety corridor precisely describes the complete spatial range in which a vehicle can safely travel from its starting point to its destination, provided that all static environmental obstacles are strictly avoided.

[0212] In some embodiments, optionally, the radius of the vehicle body envelope disk for each collaboratively planned vehicle is obtained. This radius is not a fixed value determined before the task begins, but a variable that is dynamically adjusted based on the real-time motion state of the vehicle. The system dynamically calculates the required additional control buffer distance based on the vehicle's real-time longitudinal velocity and lateral acceleration.

[0213] When determining the dynamic radius, a dynamic margin proportional to the square of the current vehicle speed is added above the vehicle's basic geometric envelope radius. This allows the safety disk radius to automatically increase at high speeds to accommodate greater control errors and braking distances, while a smaller radius is used at low speeds or when stationary to retain more drivable space, thus achieving an adaptive balance between safety and space utilization.

[0214] In some embodiments, the vehicle safety driving corridor is not kept fixed after it is first generated, but rather a mechanism is established to fine-tune it online based on the real-time tracking error of the vehicle.

[0215] The system monitors the lateral deviation between the vehicle's actual trajectory and the initial cooperative trajectory in real time. If the system continuously detects that the vehicle's actual position is consistently deviating from one side boundary of the corridor, it triggers online adaptive adjustment of the corridor.

[0216] Based on the direction and magnitude of the deviation, the geometric centerline of the vehicle's safe driving corridor is slightly shifted towards the actual driving trend of the vehicle within a predicted future period, while the corridor width is flexibly adjusted. This allows the safe driving corridor to adapt to minor changes in the vehicle's actual control characteristics and road conditions to a certain extent, avoiding unnecessary emergency braking or planning failures caused by overly strict fixed corridor constraints.

[0217] In some embodiments, optionally, such as Figure 6 As shown, step S114: For conflicting vehicles, determine the relative safe driving corridor between the vehicles based on at least two vehicle safe driving corridors, including:

[0218] Step S1140: For each conflicting vehicle, determine the relative motion trajectory based on the cooperative initial trajectory;

[0219] Step S1142: Determine the relative collision area range based on the radius of the vehicle body envelope disk of the conflicting vehicles;

[0220] Step S1144: For the relative motion trajectory, determine the second local box corresponding to each target guidance point of the conflicting vehicles;

[0221] Step S1146: Expand the second box boundary of all second local boxes synchronously with a preset step size until the expanded second box boundary contacts the expansion boundary or reaches the maximum expansion range, and determine the collision safety zone.

[0222] Step S1148: Determine the relatively safe driving corridor of the workshop based on multiple collision safety zones.

[0223] In this embodiment, for the identified conflicting vehicles, the relative motion trajectory describing the change in their relative positions is first determined by coordinate transformation based on their respective cooperative initial trajectories. Then, the radii of the vehicle body envelope disks of the two vehicles are added together to obtain a joint safety distance for collision detection, which is used to define the relative collision area range. A collision safety area that avoids collision and environmental interference is determined at each moment, and a relative safe driving corridor between the vehicles and the collision safety area is generated.

[0224] Understandably, defining mutually exclusive safety spaces for conflicting vehicles eliminates the risk of collision and improves the safety of actual operation of cooperative trajectories.

[0225] Specifically, for each conflicting vehicle, determining the relative motion trajectory based on the initial collaborative trajectory is an analytical framework for observing the movement of the other party from the perspective of one side of the conflict.

[0226] The relative motion trajectory describes the change in the positional relationship between the two vehicles in a conflict vehicle pair over time.

[0227] When determining the relative motion trajectory, a reference vehicle must first be selected from the two vehicles. Usually, the vehicle with higher priority or more stable motion state in the conflict space-time region is selected.

[0228] Then, the absolute position coordinates of the other vehicle, i.e. the target vehicle, at each spatiotemporal calibration point in the collaborative initial trajectory are transformed into a relative coordinate system with the reference vehicle as the origin.

[0229] This relative coordinate system is usually based on the center of mass of the reference vehicle at the current moment as the origin, and the direction of its front as the vertical axis.

[0230] Through this coordinate transformation, a series of displacement vector sequences of the target vehicle relative to the reference vehicle are obtained, which constitute the relative motion trajectory observed from the perspective of the reference vehicle.

[0231] Determining the relative collision area range based on the radius of the vehicle body envelope disk of the conflicting vehicles defines a joint safety zone for collision detection.

[0232] The relative collision zone is a region surrounding the relative motion trajectory, and its core idea is to combine the safety disk radii of the two vehicles.

[0233] The specific method for determining the relative collision zone range is to obtain the radius of the body envelope disk of each vehicle in the collision vehicle pair, and then add the two radii together to obtain a joint safety distance.

[0234] At any point on the relative trajectory, a circular area is formed with that point as the center and the joint safety distance as the radius. This indicates that if the center of the reference vehicle is located at the origin of the relative coordinate system and the center of the target vehicle falls within this circular area, the solid discs of the two vehicles will inevitably overlap geometrically, resulting in a collision. Therefore, this circular area defines the collision hazard zone in relative space, and its boundary is the critical line that needs to be avoided.

[0235] For the relative motion trajectory, the second local box corresponding to each target guidance point of the conflicting vehicles is determined, which is to establish an initial analysis unit for the conflict relationship at each moment in the relative space.

[0236] The second local box is a rectangular region established in a relative coordinate system and aligned with the tangent direction of any point on the relative motion trajectory.

[0237] When determining its initial dimensions, its length is along the direction of relative motion, and its width is perpendicular to the direction of relative motion. The initial width is usually set to a multiple slightly larger than the aforementioned joint safety distance, and the initial length is set to a smaller value.

[0238] By generating such an aligned second local box for each target guidance point on the relative motion trajectory, a series of discretized units are formed for analyzing the spatial relationship between the two vehicles at each time step. Unlike the first local box in the construction of the autonomous vehicle safety corridor, the second local box analyzes the relative position space between the two vehicles.

[0239] With a preset step size, the second box boundary of all second local boxes is expanded synchronously until the expanded second box boundary contacts the expansion boundary or reaches the maximum expansion range. The collision safety zone is determined by iteratively searching for a feasible relative position space that can avoid collision for each relative trajectory point.

[0240] The expansion boundary here is directly adopted from the conservative no-entry zone generated in the absolute coordinate system when constructing the safe driving corridor for autonomous vehicles, which combines the static environment and vehicle size.

[0241] When expanding a box in a relative coordinate system, the box boundary in the relative coordinate system needs to be converted back to the absolute coordinate system in real time to detect whether it is in contact with the expansion boundary.

[0242] The preset step size defines the increment for each expansion. The iterative process for determining the collision safety zone is as follows: for each second local box, its boundary is expanded outward synchronously. After each expansion, the new boundary is transformed to absolute coordinates, and it is checked whether it interferes with the expanded boundary or has reached a maximum size range preset for the relative corridor. Expansion is carried out independently in four directions, and stops when it touches the boundary or reaches the limit in any direction.

[0243] When all directions are at rest, the area defined by the collision box in the relative coordinate system at that moment is a set of safe positions for the target vehicle relative to the reference vehicle at a specific instant, called the collision safety zone. As long as the relative position of the target vehicle falls within this zone, it can be guaranteed that the two vehicles will not collide with each other or with the environment in absolute space.

[0244] Determining the relative safe driving corridor in the workshop based on multiple collision safety zones involves merging the independent relative safe positions at each moment into a continuous, time-evolving relative safe passage.

[0245] Since each target guidance point corresponds to a collision safety zone, these zones describe how the relative safety position changes over time. Determining the relative safe driving corridor in the workshop involves smoothly connecting and integrating these discrete zones in the spatiotemporal dimensions to form a continuous, meandering safety tunnel in relative space.

[0246] This corridor clearly defines the set of all possible positions where the target vehicle's center of mass relative to the reference vehicle can exist during the duration of the conflict. By mapping this relatively safe corridor back to an absolute coordinate system and combining it with the vehicle's own safe driving corridor, each vehicle can be allocated its own exclusive absolute passage space that does not overlap with the conflicting party, thereby completing the conflict resolution.

[0247] In some embodiments, the process of determining the relative collision area range based on the radius of the vehicle body envelope disk of the conflicting vehicles may use a joint safety distance that is not simply the sum of radii, but rather introduces a dynamic scaling factor related to the relative speed of the two vehicles.

[0248] The dynamic scaling factor is calculated based on the expected approach speed and relative direction of the two vehicles during the collision period. When the relative speed of the two vehicles is high, the factor is greater than one, effectively increasing the joint safety distance and reserving more space for braking and reaction; when the relative speed is low, the factor is approximately equal to one, using basic geometric superposition. This transforms the relative collision area from a static geometric circle into a dynamic risk field that reflects the severity of the potential collision, thus providing stronger safety assurance in high-risk scenarios such as high-speed oncoming vehicle approach.

[0249] In some embodiments, the relative safety corridor is optionally smooth and continuously changing in both time and space, avoiding abrupt changes in the position or shape of the safety area at adjacent moments. The optimization process uses discrete collision safety areas as keyframe constraints and aims at the smoothness of motion at the corridor boundary points to solve for a smooth boundary curve that passes through all keyframes and satisfies the condition of continuous differentiability. The resulting relative safe driving corridor provides the vehicle with stable and predictable spatial boundary guidance, which helps the vehicle controller generate smooth tracking control commands, improving ride comfort and system stability.

[0250] In some embodiments, optionally, such as Figure 7 As shown, step S116: Determine the cooperative trajectory based on the vehicle's safe driving corridor and the relative safe driving corridor between the vehicles, including:

[0251] Step S1160: Determine preliminary constraints based on the vehicle's safe driving corridor and the relative safe driving corridor between the vehicles;

[0252] Step S1162: Determine the optimization parameters based on the preliminary constraints and the preset trajectory optimization rules;

[0253] Step S1164: Starting from the initial cooperative trajectory, iteratively optimize the initial cooperative trajectory according to the optimization parameters to obtain the optimized trajectory;

[0254] Step S1166: In each iteration, update the vehicle's safe driving corridor and the relative safe driving corridor between the vehicle and the vehicle based on the currently obtained optimized trajectory, so as to update the optimization parameters;

[0255] Step S1168: When the optimized trajectory meets the preset convergence condition, stop the iteration and determine the optimized trajectory that meets the convergence condition as the cooperative trajectory.

[0256] In this embodiment, the geometric boundaries of the vehicle's safe driving corridor and the relative safe driving corridor within the workshop are transformed into mathematical inequalities between the vehicle's position and its relative position, establishing preliminary constraints. Using the initial cooperative trajectory as the starting point for iteration, numerical solutions are performed based on the optimization parameters under the constraints to obtain a progressively improved optimized trajectory. When both the improvement in the objective function and the constraint violation of the optimized trajectory are below a preset threshold, convergence is determined, and this trajectory is output as the final cooperative trajectory.

[0257] Understandably, by using closed-loop iterative optimization, a safe, smooth, and globally optimal cooperative trajectory is generated, thereby improving the reliability of multi-vehicle cooperative trajectories.

[0258] Specifically, determining the preliminary constraints based on the vehicle's safe driving corridor and the relative safe driving corridor of the vehicle and the workshop is to transform the spatial safety geometric model into executable mathematical constraints in the trajectory optimization problem.

[0259] The initial constraints include two main categories: first, the vehicle's static safety constraints, which require that the vehicle's position at each spatiotemporal calibration point must be within the boundary polygon of its own vehicle's safe driving corridor; and second, the relative safety constraints between vehicles, which require that for each pair of conflicting vehicles, their relative position coordinates at the corresponding time point must satisfy the geometric relationship specified by the relative safe driving corridor between the vehicles.

[0260] The process of determining these constraints involves expressing the boundary polygons or relative positional relationships of the corridor as a set of linear or nonlinear inequalities concerning vehicle state variables. These inequalities collectively constitute the hard boundary of the feasible region of the trajectory, and any feasible trajectory must strictly satisfy all of these inequalities.

[0261] Determining the optimization parameters based on the initial constraints and pre-defined trajectory optimization rules involves setting the objective function and adjustable weights for the optimization problem. The optimization parameters are the mathematical expression of these qualitative rules. When determining the optimization parameters, a scalar objective function needs to be constructed, typically a weighted sum of the aforementioned optimization objectives.

[0262] For example, the square integral of the trajectory's acceleration can be considered as a smoothness cost, the total time as an efficiency cost, and the rate of change of the steering wheel angle as a comfort cost. The weighting coefficients preceding each cost term are the key optimization parameters, and their values ​​determine the trade-offs between conflicting objectives in the optimization process. These parameters can be preset based on task priorities or experience.

[0263] Starting with the cooperative initial trajectory, iterative optimization of the cooperative initial trajectory based on the optimization parameters is performed to obtain the optimized trajectory, which is the core computational process for solving constrained optimization problems.

[0264] Cooperative initial trajectories provide optimized initial guesses, which can significantly improve convergence speed.

[0265] Iterative optimization typically employs numerical optimization algorithms such as sequential quadratic programming, interior-point methods, or augmented Lagrange methods. In each iteration, based on the current trajectory guess, the algorithm calculates the gradient of the objective function with respect to the trajectory variables and the Jacobian matrix of the constraints. Then, it solves a local approximation subproblem to obtain a corrected direction and step size for the trajectory, thereby updating the trajectory and obtaining a new trajectory that is better than the previous one, i.e., the optimized trajectory.

[0266] Since the optimized trajectory may have deviated from the initial cooperative trajectory, it is more accurate to reassess the safety boundary centered on it.

[0267] Using the optimized trajectory obtained in the current iteration as the new centerline, the step of determining the safe driving corridor of the vehicle is re-executed. However, the previously calculated expansion boundary can be reused to quickly obtain a safe driving corridor of the vehicle with a higher degree of matching with the current optimized trajectory.

[0268] Secondly, for conflicting vehicles, based on their updated optimized trajectories, the relative motion trajectories and the relative safe driving corridors between the vehicles are recalculated.

[0269] Then, these newly generated corridors, which better fit the current trajectory prediction, are used to update the initial constraints in the optimization problem.

[0270] The weights in the optimization parameters can also be fine-tuned based on information such as the width of the new corridor. This allows the constraints to be dynamically adjusted as the optimized trajectory evolves, forming a closed loop where the trajectory and safety boundary mutually correct each other and jointly approach the optimal solution.

[0271] When the optimized trajectory meets the preset convergence condition, the iteration stops and the optimized trajectory that meets the convergence condition is identified as the cooperative trajectory. This is the criterion for determining that the calculation is complete and outputting the final result.

[0272] The convergence condition is to stop the calculation in time when the numerical solution reaches sufficient accuracy, so as to avoid unnecessary iterations.

[0273] Pre-defined convergence conditions typically include two parts: first, the improvement in the objective function is less than a very small threshold, indicating that the optimization effect is negligible; second, the degree to which the current trajectory violates the constraints is less than an allowable tolerance, indicating that the trajectory fully satisfies all safety constraints. A maximum number of iterations can also be set as a protective stopping condition. The determination process involves calculating the difference between the objective function value of the current optimized trajectory and the previous value after each iteration, and calculating the degree to which it violates the constraints.

[0274] Convergence is determined only when both numerical indicators are simultaneously below their respective preset thresholds. The final output trajectory, which satisfies all safety constraints and whose objective function value cannot be significantly improved numerically, is the optimal cooperative trajectory that can be delivered for execution.

[0275] In some embodiments, optionally, such as Figure 8 As shown, the cooperative obstacle avoidance trajectory planning method also includes:

[0276] Step S1200: Globally synchronize the cooperative trajectory within the formations of multiple cooperatively planned vehicles;

[0277] Step S1202: Real-time acquisition of the status information of collaborative planning vehicles, the status information of obstacles within the target area, and the status information of other collaborative planning vehicles;

[0278] Step S1204: Based on the status information, determine whether the dynamic replanning trigger condition is met. The dynamic replanning trigger condition includes at least one of the following: a new obstacle is detected, the deviation between the actual trajectory of the collaborative planning vehicle and the collaborative trajectory exceeds a preset threshold, or communication with at least one other collaborative planning vehicle is interrupted.

[0279] Step S1206: When the dynamic replanning trigger condition is met, the dynamic replanning process is triggered to redetermine the cooperative trajectory;

[0280] Step S1208: When the collaboratively planned vehicle leaves the core passage area of ​​the target area and meets the preset path return conditions, control the collaboratively planned vehicle to return to the corresponding predefined guide path and adjust the vehicle speed to restore the platoon driving state.

[0281] In this scheme, after generating the cooperative trajectory, it is first globally synchronized within the formation to ensure consistent commands. The system monitors the status of its own vehicle, other vehicles, and environmental obstacles in real time, and determines whether the dynamic replanning trigger conditions are met based on preset rules, such as the appearance of new obstacles, excessive trajectory tracking deviation, or communication interruption.

[0282] Once the conditions are met, a dynamic replanning process is immediately triggered to regenerate a collaborative trajectory adapted to the new situation. When the vehicle safely leaves the core collaborative area and meets the path return conditions, it is controlled to smoothly return to the predefined guidance path, and the vehicle speed is adjusted to restore platooning cruising, thus completing the transition from close collaboration to normal driving. Through closed-loop dynamic management throughout the entire process, the robustness and adaptability of the system are improved.

[0283] Specifically, global synchronization occurs at the end of the planning cycle when the vehicle or roadside cooperative unit responsible for the main planning simultaneously broadcasts a data packet containing the complete cooperative trajectories of all vehicles to every cooperative planning vehicle in the platoon via a highly reliable, low-latency vehicle-to-everything (V2X) communication link. The synchronized content includes not only the spatiotemporal trajectory sequence of each vehicle but also the relative temporal relationships between vehicles and necessary cross-reference information.

[0284] The completion of synchronization is confirmed when all vehicles send acknowledgment feedback for the planned cycle trajectory data packet to the main control unit within the specified communication window. Global synchronization ensures that all vehicles have a consistent understanding at the same time, thereby achieving precise coordinated actions.

[0285] The collaborative planning vehicle's status information includes its motion status, such as position, speed, heading, and acceleration, measured in real time by its own sensors, as well as the status of the vehicle controller and the health status of the actuators.

[0286] The status information of obstacles within the target area is provided by the fusion of vehicle-mounted sensors and roadside perception facilities, including the precise geometric position of static obstacles, as well as the real-time position, speed, and heading of dynamic obstacles, and prediction of their future short-term trajectories.

[0287] The status information of other collaboratively planned vehicles is mainly shared in real time through direct communication between vehicles. The content is similar to the status of the individual vehicles, but it is formatted according to the communication protocol.

[0288] The process of determining this state information involves the spatiotemporal alignment and confidence fusion of multi-source heterogeneous data, ultimately forming a unified, real-time, and consistent global situation map that describes the current and predicted states of all key entities within the entire target area.

[0289] Determining whether the dynamic replanning trigger conditions are met based on the status information is a process of continuously diagnosing the normal operating status of the system based on preset rules.

[0290] Dynamic replanning trigger conditions are a set of logical judgment rules, each of which is determined by a quantitative definition of a specific risk source.

[0291] The first item, "Detecting New Obstacles," refers to identifying obstacles that were not previously present or included, and whose predicted trajectories would conflict with the current collaborative trajectory, by comparing real-time perception information with the environmental model used in the previous planning cycle. The criterion is that the minimum distance between the obstacle and the future trajectory of any collaboratively planned vehicle is less than a safety threshold.

[0292] The second item, the deviation between the actual trajectory of the collaboratively planned vehicle and the collaborative trajectory exceeds a preset threshold, refers to the calculation of lateral, longitudinal, or heading deviations by comparing the real-time reported position of the vehicle with the planned position at the corresponding time. When the absolute value or comprehensive deviation index of any deviation exceeds a limit value dynamically calculated based on control accuracy and corridor width, it is triggered.

[0293] The third condition, communication interruption with at least one other collaborative planning vehicle, refers to the loss of a reliable direct communication connection with a critical collaborative vehicle due to the absence of heartbeat packets or periodic status updates, and the interruption lasting for more than a timing threshold set to ensure collaborative security. Meeting any one of these conditions triggers a replanning process.

[0294] When the conditions for dynamic replanning are met, the dynamic replanning process is triggered to redetermine the collaborative trajectory. This is the system's emergency response mechanism for dealing with unexpected situations.

[0295] The urgency and scope of planning vary depending on the triggering conditions. For example, with a new obstacle, only local trajectory replanning may be needed for the affected vehicles; with a communication interruption, a fault-tolerant collaborative strategy reconstruction may be required for all vehicles in the formation.

[0296] The step of redetermining the cooperative trajectory is essentially to re-execute the core planning process from generating the spatiotemporal calibration point sequence to optimizing and determining the cooperative trajectory. However, its initial input is the latest global situation, and it may use a shorter planning time domain and different optimization weights to quickly generate a new safe trajectory to deal with the current emergency situation.

[0297] When the collaboratively planned vehicle leaves the core traffic area of ​​the target area and meets the preset path return conditions, the collaboratively planned vehicle is controlled to return to the corresponding predefined guide path and the speed is adjusted to restore the platooning state. This is the disbanding and recovery phase after the collaborative task is completed.

[0298] The core traffic area of ​​the target area usually refers to the area where multi-vehicle interaction is most complex and requires the most close coordination, such as the central area of ​​a large intersection.

[0299] The criteria for determining whether a vehicle has left the area are that the distance between the vehicle's center point and the geometric center of the area exceeds a set radius.

[0300] Path regression conditions are a set of constraints that ensure a safe and smooth regression operation. These include the lateral distance between the vehicle and the predefined guidance path being less than a threshold for safe lane merging, no rapidly approaching vehicles behind the target lane, and the vehicle's speed matching the reference speed of the guidance path. When both the exit from the core area and path regression conditions are met, the vehicle controller receives a higher-order instruction, smoothly transitioning its control objective from tracking a complex cooperative trajectory to tracking the initially issued, macroscopic predefined guidance path.

[0301] At the same time, by adjusting the throttle and brakes, the vehicle's speed is synchronized with the speed of the vehicle in front or the lead vehicle in the formation, thereby rejoining the formation, ending this close coordination, and returning to the normal convoy driving state.

[0302] In one specific embodiment, optionally, in order to address the shortcomings of existing open-pit mine unstructured intersection multi-vehicle cooperative planning technology, such as poor scenario adaptability, complex solution leading to insufficient real-time performance of the vehicle end, disconnect between multi-vehicle cooperation and mine platooning operations, and system robustness.

[0303] This invention provides a real-time collaborative trajectory planning method for multiple intelligent connected vehicles at unstructured intersections in open-pit mines. The entire process is independent of cloud computing power, deeply adapted to unstructured mining operation scenarios and the dynamic characteristics of heavy-duty mining trucks, achieving real-time, robust, and safe planning of multi-vehicle collaborative trajectories, balancing mine traffic safety and transportation efficiency.

[0304] Real-time collaborative trajectory planning method for multiple intelligent connected vehicles at unstructured intersections in open-pit mines, such as Figure 10 As shown, it includes:

[0305] Step S200: Obstacle avoidance triggering and spatiotemporal reference anchoring;

[0306] Step S202: Generate the initial trajectory;

[0307] Step S204: Construct a two-layer security corridor;

[0308] Step S206: Solve for the optimal trajectory using lightweight iterative optimization;

[0309] Step S208: Trajectory tracking execution and dynamic closed-loop control;

[0310] Step S210: Determine if replanning is triggered;

[0311] If the result of step S210 is negative, proceed to step S212; if it is positive, return to step S200.

[0312] Step S212: Determine whether passage is complete;

[0313] If the judgment result of step S212 is yes, the process ends; if it is no, the process returns to step S208.

[0314] Specifically:

[0315] 1. Obstacle Avoidance Triggering and Spatiotemporal Reference Anchoring: This is a fundamental prerequisite for obstacle avoidance planning. Its core objective is to anchor a rigid spatiotemporal reference for subsequent path planning, resolving the fundamental issue of spatiotemporal coupling in path planning. It is specifically divided into 6 sub-items:

[0316] a. Obstacle avoidance triggering and control range definition: Based on the high-precision map of the mine, the entire obstacle avoidance control range of the entire road section is predefined, covering the preparation area of ​​the mine intersection, curves, slopes, narrow bridges, mining faces and the surrounding work area of ​​the unloading point; after the vehicle enters the control range, the on-board obstacle avoidance system is automatically triggered to start, and the multi-sensor acquisition module and the vehicle-to-vehicle direct communication module are activated simultaneously.

[0317] b. Construction of the kinematic model for mining trucks:

[0318] i: For the low-speed operation scenario of heavy-duty mining trucks in mines, a monorail bicycle model is adopted as the vehicle kinematic description model to avoid the surge in computational load caused by complex dynamic models, while fully covering the nonholonomic constraints of the vehicle.

[0319] ii: Define the vehicle state vector and control vector:

[0320] State vector ,in , The coordinates of the rear axle center of the vehicle. For driving speed, For the front wheel steering angle, This refers to the vehicle's heading angle;

[0321] Control Vector ,in To accelerate the vehicle, This represents the rate of change of the front wheel steering angle;

[0322] iii: Kinematic differential equations, serving as vehicle motion constraints for all subsequent trajectory planning:

[0323] ;

[0324] ;

[0325] ;

[0326] In the formula The wheelbase is determined based on the actual parameters of heavy-duty mining trucks in the mine. To plan the time domain endpoint;

[0327] Subscript i=1,..., The definition applies to vehicles from the 1st to the 2nd. All vehicles.

[0328] c. Calibration of hard boundary constraints for vehicle status and control variables:

[0329] Based on the physical execution limits of heavy-duty mining trucks, the upper and lower limits of calibrated states and control variables are constrained as follows:

[0330] ;

[0331] ;

[0332] ;

[0333] in, For minimum linear acceleration, Minimum driving speed, This is the lower limit of angular velocity. This is the lower limit of the rate of change of angle. For maximum linear acceleration, For maximum driving speed, This is the upper limit of angular velocity. This represents the upper limit of the rate of change of the angle.

[0334] d. Time block segmentation and spatiotemporal calibration point generation:

[0335] Fixed planning total time domain =40s, ensuring that all vehicles participating in collaborative planning have a consistent planning time domain, thus solving the time synchronization problem of multi-vehicle collaboration;

[0336] The overall planning time domain is uniformly discretized, divided into segments with a fixed step size Δt = 0.2s. =200 consecutive time blocks to ensure a balance between planning accuracy and computational efficiency;

[0337] The end point of each time block corresponds to a unique spacetime calibration point. ,in (k=1,2,…,N) are fixed timestamps. The longitudinal coordinates along the driving direction (obtained by linear interpolation of the guide path, and remain fixed) are used, while only the lateral coordinates are used. As an optimization variable for subsequent planning, it not only anchors the spatiotemporal benchmark but also significantly reduces the optimization dimensionality.

[0338] e. Definition of two-point boundary constraints:

[0339] For the i-th mining truck, the complete form of the boundary constraints at the planning start time t=0 is:

[0340] ;

[0341] ;

[0342] in, and Let i be the initial X-axis position of vehicle i in a specific coordinate system. and Let i be the initial Y-axis position of vehicle i. and Let i be the initial linear velocity of vehicle i. and Let i be the initial linear acceleration. and Let i be the initial heading angle. and Let i be the initial angular velocity of vehicle i. and Let i be the initial front wheel steering angle of vehicle i.

[0343] Subscript i=1,..., The definition applies to vehicles from the 1st to the 2nd. All vehicles.

[0344] End-time boundary constraints: Plan the end-time t= By using ray constraints instead of fixed-position constraints, vehicle slowdown and waiting are avoided, thus improving traffic efficiency.

[0345] State constraints: At the final moment, acceleration, front wheel steering angle, and rate of change of steering angle are all zero to ensure that the vehicle is in a stable straight-ahead state when leaving the intersection.

[0346] ;

[0347] in, and For vehicle i at the terminal time The final linear velocity; Let linear acceleration be the linear acceleration at the terminal moment. The heading angle at the terminal moment. ω is the angular velocity at the terminal moment.

[0348] Position constraint: The destination position must fall on the guide path ray of the target departure direction to ensure that the vehicle leaves the intersection in the correct direction.

[0349] f. Standardized encapsulation of the cooperative obstacle avoidance optimal control problem (OCP):

[0350] Construct a multi-objective optimization cost function in weighted summation form:

[0351] ;

[0352] In the formula: The total number of vehicles participating in the collaborative planning. For smoothness weights, The i-th car is acceleration at any moment The i-th car is angular velocity at time t, To plan the time domain endpoint, The i-th car is The horizontal coordinate of time, The i-th car is The vertical coordinate of time, The lateral coordinates of the virtual point far away in the target direction of the i-th vehicle. The longitudinal coordinates of the virtual point far away from the target direction of the i-th vehicle.

[0353] The four major constraint systems for the OCP problem are defined as follows: vehicle kinematic differential equation constraints; hard boundary constraints on vehicle state and control variables; two-point boundary constraints at the planning start and end points; and static boundary constraints and collision avoidance constraints on the dynamic vehicle.

[0354] The standardized encapsulation of the OCP problem is completed, and the optimization variables are clearly defined as the sequence of states and control variables of all vehicles, providing a unified mathematical framework for subsequent initial trajectory generation, constraint simplification, and iterative solution.

[0355] 2. Zero-degree-of-freedom simulation initial trajectory generation based on a path generation algorithm fusion of high-precision maps and kinematic models:

[0356] This stage is the core of initial trajectory generation. The core objective is to quickly generate kinematically feasible and collision-free multi-vehicle cooperative initial trajectories, providing high-quality initial guesses for subsequent corridor construction and OCP (Optical Characteristic) solving. This is broken down into 6 sub-execution steps, such as... Figure 11 As shown:

[0357] Step S300: Load the offline boot path;

[0358] Step S302: Construct a gravity / repulsion model;

[0359] Step S304: Zero-degree-of-freedom simulation initialization;

[0360] Step S306: Determine if the simulation step size has reached 200;

[0361] If the judgment result of step S306 is yes, proceed to step S308: calculate the resultant external force and map the control quantity;

[0362] Step S310: The Longgekuta method updates the vehicle status;

[0363] Step S312: K = K + 1, return to step S306;

[0364] If the judgment result of step S306 is negative, proceed to step S316: Initial trajectory generation;

[0365] Step S318: Determine feasibility and perform collision-free verification;

[0366] If the verification result of step S318 is yes, output the initial trajectory;

[0367] If the verification result of step S318 is negative, proceed to step S314: fine-tune the gravity and repulsion weights, and return to step S304.

[0368] Among them, a. offline pre-calculation of guidance paths for each driving direction:

[0369] i. For each pair of inbound and outbound directions at unstructured intersections in the mine, the corresponding guidance path is pre-calculated offline. The guidance path is obtained by solving a simple OCP problem containing only static boundary obstacle avoidance constraints. It is only necessary to ensure that the path does not collide with the static boundary of the intersection and meets the vehicle kinematic constraints, without considering other dynamic vehicles.

[0370] ii. Generate a sequence of guiding points at equal intervals for each guiding path, with the spacing between guiding points matching the driving distance corresponding to the planned time-domain step length; for each vehicle participating in collaborative planning, match the guiding path corresponding to its entry and exit directions, and use the second guiding point in front of the vehicle's current position as the gravity source point to ensure that the vehicle can quickly realign when it deviates from the guiding path, thus avoiding trajectory oscillation.

[0371] b. Refined construction of categorized gravity / repulsion models:

[0372] i. Gravity Model Construction: The gravitational source is a predefined guiding point. The magnitude of the gravitational force is proportional to the distance between the vehicle and the gravitational source, and its direction points towards the gravitational source.

[0373] ;

[0374] In the formula The vehicle's current location. Location of the gravitational source point, This is the gravity weighting coefficient.

[0375] ii. Construction of workshop repulsion model: The repulsion source is other cooperating vehicles. The magnitude of the repulsion is inversely proportional to the relative distance between the two vehicles, and the direction is away from the other vehicle.

[0376] ;

[0377] In the formula It is the relative distance between the two vehicles. Let be the radius of the enveloping disk of the two vehicles. , This is the repulsion force weighting coefficient.

[0378] iii. Boundary repulsion model construction: The repulsion source is the static boundary and obstacles at the intersection. The magnitude of the repulsion force is inversely proportional to the distance of the vehicle from the boundary, and the direction is away from the boundary.

[0379] ;

[0380] In the formula Let the radius of the vehicle's envelope disk be . This is the distance from the vehicle to the nearest boundary.

[0381] iv. Dynamic adaptation of parameters for different scenarios: Adjust the weight coefficients differently for different mining scenarios: increase the repulsion weight by 20% in the core area of ​​high-risk intersections, reduce the repulsion weight by 30% in long straight sections, and increase the repulsion weight by 15% for heavy-load mining trucks, so as to achieve differentiated management of spatiotemporal risks.

[0382] c. Composition of the vehicle's net external force vector and kinematic constraint mapping:

[0383] i. Vector synthesis of the gravitational force, all inter-vehicle repulsive forces, and boundary repulsive forces acting on the vehicle to obtain the net external force on the vehicle: ;

[0384] in, For the combined external force on the vehicle, The gravitational force acting on the vehicle. For repulsive forces in all workshops, This is the boundary repulsion force.

[0385] ii. Map the resultant external force to vehicle control variables:

[0386] Front wheel steering angle mapping: The difference between the direction of the net external force and the vehicle's current heading angle, multiplied by the steering coefficient. This yields the front wheel steering angle adjustment amount, while simultaneously limiting the amplitude through the maximum front wheel steering angle constraint:

[0387] ;

[0388] in, This represents the desired rate of change of the steering angle, i.e., the commanded value of the steering wheel rotation speed. To represent the component of the net external force on the vehicle along the X-axis, To determine the component of the net external force on the vehicle along the Y-axis, This is the vehicle's current heading angle. This is the steering ratio coefficient. The upper limit of angular velocity is defined by `x`. `clip()` is a clipping function that limits the input value `x` to a range, ensuring that the output rate of change instruction does not exceed the physical limit.

[0389] Acceleration mapping: The component of the net external force in the vehicle's direction of travel is mapped to the vehicle's acceleration adjustment amount, while the amplitude is limited by the maximum acceleration and deceleration constraint.

[0390] d. Zero-DOF forward simulation initialization:

[0391] i. Initialize the simulation environment: Take the planning start time t=0 as the simulation zero point and Δt=0.2s as the simulation step size, which is completely consistent with the discrete step size in the planning time domain, to ensure that the trajectory points output by the simulation correspond one-to-one with the spatiotemporal calibration points;

[0392] ii. Initialize the simulation initial state of all cooperative vehicles to be completely consistent with the initial boundary constraints of the OCP problem, including vehicle position, speed, heading angle, front wheel steering angle, etc.

[0393] iii. Define zero-degree-of-freedom simulation rules: All vehicles complete state updates synchronously within the same simulation step, without priority distinction, fully simulating the real scenario of multiple vehicles driving simultaneously. Each update strictly follows vehicle kinematic constraints, eliminating the need for complex search iterations, and the computational efficiency is far higher than traditional search methods.

[0394] e. Time-step vehicle state update and initial trajectory generation:

[0395] i. Within the k-th simulation step, synchronously update the current position of all vehicles, recalculate the gravitational force of each vehicle and the repulsive force of all repulsive sources, and complete the resultant external force vector synthesis;

[0396] ii. Following the mapping logic in Section 1c, the net external force is converted into the vehicle's front wheel steering angle and acceleration control quantities to complete the boundary limiting;

[0397] iii. Based on the kinematic model of a monorail bicycle, the first-order explicit Runge-Kutta method is used to update the vehicle's position, velocity, heading angle, and other states for the next simulation step.

[0398] iv. Repeat the above steps until all 200 simulation steps have been calculated, and the initial trajectory sequence of each vehicle is obtained, which includes the position, speed and control information of each spatiotemporal calibration point.

[0399] f. Initial trajectory feasibility verification and correction:

[0400] i. Kinematic feasibility verification: Verify point by point whether the initial trajectory's velocity, acceleration, front wheel angle, and rate of change of angle are all within the hard constraint range. For trajectory points that exceed the constraints, correct them through local smoothing filtering to ensure that the trajectory can be stably executed by the vehicle.

[0401] ii. Collision-free verification: Verify two aspects step by step: ① The distance between the vehicle and the static boundary of the intersection is greater than or equal to the vehicle's envelope radius; ② The relative distance between the two vehicles is greater than or equal to the sum of their envelope radii, ensuring no collision risk in the entire time domain;

[0402] iii. For trajectories that fail verification, fine-tune the gravity and repulsion weight coefficients and re-execute the forward simulation until an initial trajectory that meets the verification requirements is generated. The method can generate an initial trajectory with a success rate of 100%, and the simulation time per round is ≤100ms, which fully meets the real-time requirements.

[0403] 3. Construction of a two-tiered safe driving corridor and linearization of collision constraints:

[0404] This stage is the core of constraint simplification. The core objective is to transform the original high-dimensional, non-convex, and nonlinear collision avoidance constraints into simple linear box constraints, significantly reducing the difficulty of solving the OCP problem. This is broken down into seven subdivided execution steps, such as... Figure 12 As shown, it includes:

[0405] Step S400: Model the single-disc envelope of the vehicle;

[0406] Step S402: Map inflation processing;

[0407] Step S404: Construct a safe corridor for the vehicle;

[0408] Step S406: Detect overlap of the vehicle safety corridor between vehicles;

[0409] Step S408: Determine if there is a potential collision risk;

[0410] If the judgment result of step S408 is yes, then execute step S410: construct a relatively safe corridor;

[0411] Step S412: Linearization transformation of collision constraints;

[0412] Step S414: Introduce relaxation variables;

[0413] Step S416: Reconstruct the lightweight OCP subproblem;

[0414] After step S416, the linearization constraints and OCP are output;

[0415] If the result of step S408 is negative, proceed directly to step S412.

[0416] a. Construction of the vehicle's single-disc envelope model:

[0417] i. A single-disc model is used to enclose the body of the mining truck, with the vehicle's geometric center as the center and the minimum radius encompassing the entire vehicle body as the disk radius, completely covering the vehicle's length and width dimensions:

[0418] Vehicle geometric center coordinate calculation:

[0419] ;

[0420] ;

[0421] In the formula This refers to the rear overhang length of the vehicle. This refers to the front overhang length of the vehicle. Let X be the X-axis coordinate of the geometric center point of the vehicle at time t. Let Y be the Y-coordinate of the geometric center point of the vehicle at time t. Let X be the X-axis coordinate of the vehicle at time t. Let Y be the Y-axis coordinate of the vehicle at time t. This is the total length of the vehicle. Let be the heading angle of the vehicle at time t.

[0422] Calculation of the radius of the envelope disk:

[0423] ;

[0424] ii. The model can not only fully cover all areas of the vehicle body, but also simplify the complex vehicle collision detection to the distance detection between the center of the circle and the obstacle, which greatly reduces the computational complexity and is fully adapted to the low-speed operation scenario in the mine.

[0425] b. Map dilation processing and equivalent collision constraint transformation:

[0426] i. Based on the radius of the vehicle envelope disk The high-precision map of the mine intersection is expanded: the static boundary and fixed obstacles of the intersection are uniformly expanded outward by one... The distance is used to generate the expanded map boundary;

[0427] ii. Complete the equivalent collision constraint transformation: The original constraint of "the vehicle body does not collide with the static boundary" is equivalently transformed into a new constraint of "the geometric center point of the vehicle does not exceed the expanded map boundary", laying the foundation for the subsequent construction of the safe driving corridor.

[0428] c. Generation of Safe Driving Corridor (STC):

[0429] i. The initial trajectory generated by the path generation algorithm that integrates high-precision maps and kinematic models is used as the baseline route. The baseline route is divided into 200 continuous local intervals according to spatiotemporal calibration points. An axis-aligned rectangular local box is constructed for each interval. The set of all local boxes constitutes the safe driving corridor (STC) of the vehicle.

[0430] ii. Employ a multi-vehicle synchronous corridor generation algorithm to synchronously expand each local box in four directions: up, down, left, and right.

[0431] Initialize all local boxes as zero-size rectangles, with the center being the corresponding calibration point of the initial trajectory;

[0432] All local boxes are expanded synchronously in four directions at a fixed step size. If the expanded area does not overlap with the boundary of the expanded map, the expansion result is retained; if an overlap occurs, the expansion in each direction is stopped.

[0433] To set a maximum expansion limit and avoid ineffective space caused by excessive corridor expansion, the upper and lower limits of the lateral coordinates of each local box are ultimately obtained. Vertical coordinate upper and lower limits ;

[0434] iii. After STC generation is completed, two core checks are performed: ① All calibration points of the initial trajectory are within the STC range; ② All regions of the STC do not overlap with the boundaries of the expanded map, and the standardized STC constraints are finally output.

[0435] d. Detection of potential collision risks in the workshop:

[0436] i. For all vehicles participating in collaborative planning, detect the overlap of STC between the two vehicles step by step: if there is spatial overlap between the local boxes of STC of the two vehicles in the same time interval, it is determined that there is a potential collision risk between the two vehicles, and a relatively safe driving corridor needs to be built for the two vehicles.

[0437] ii. For vehicles without overlapping STCs, there is no need to construct a relatively safe driving corridor in the workshop. Collisions can be guaranteed directly through STC constraints, which greatly reduces the number of unnecessary constraints and improves the solution efficiency.

[0438] e. Construction of a relatively safe driving corridor in the workshop:

[0439] i. For two vehicles i and j with potential collision risk, construct a relative motion coordinate system, using vehicle j as the reference frame, and calculate the relative trajectory of vehicle i:

[0440] ;

[0441] In the formula The initial trajectory sequences of the two vehicles are given. Let i be the absolute state quantity of vehicle i. Let be the absolute state quantity of vehicle j.

[0442] ii. Construct a relative collision model for the two vehicles: The relative collision area is a circular region with a radius equal to the sum of the radii of the enclosing disks of the two vehicles. As long as the relative trajectories do not enter the circular region, the two vehicles will not collide.

[0443] ;

[0444] in, Let be the geometric set of potential collision regions between vehicle i and vehicle j.

[0445] iii. Based on the relative initial trajectories of the two vehicles, the same expansion algorithm as STC is used to construct a relative safe driving corridor in the relative coordinate system, ensuring that there is no overlap between the relative collision areas. Finally, the upper and lower limits of the coordinates of the relative trajectory in each time interval are output. , .

[0446] f. Collision constraint linearization transformation and relaxation variable design:

[0447] i. STC constraint linearization transformation: Transform static boundary collision constraints into linear box constraints:

[0448] ;

[0449] ;

[0450] ;

[0451] , ;

[0452] in, Let X be the X-axis coordinate of the geometric center point of vehicle i at time t. Let be the Y-coordinate of the geometric center point of vehicle i at time t. and Let be the minimum and maximum allowable positions of the geometric center of vehicle i in the axial direction during the k-th time segment, respectively. and Let be the minimum and maximum allowed positions of the geometric center of vehicle i in the Y-axis direction during the k-th time segment, respectively. To plan the time domain endpoint, This represents the number of time discretization segments.

[0453] ii. Linearization transformation of workshop relative safe driving corridor construction constraints: Transform the workshop dynamic collision constraints into linear box-shaped constraints in a relative coordinate system:

[0454] ;

[0455] ;

[0456]

[0457] ;

[0458] ;

[0459] in, Let X be the X-axis coordinate of the geometric center point of vehicle j at time t. Let be the Y-coordinate of the geometric center point of vehicle j at time t. and Let $\mathbf{i}$ be the minimum and maximum relative positions of vehicle $i$ relative to vehicle $j$ in the X-axis direction during the $k$-th time segment. and Let represent the minimum and maximum relative positions of vehicle i and vehicle j in the Y-axis direction during the k-th time segment, respectively. This represents another number of time-discretionary segments.

[0460] iii. Slack Variable Design: To resolve the constraint conflicts and the problem of an empty feasible region between the STC and the construction of the relative safe driving corridor in the workshop, slack variables are introduced for the constraints of the construction of the relative safe driving corridor in the workshop. This allows for a slight expansion of the corridor boundary; at the same time, a penalty term for the slack variables is added to the cost function:

[0461] ;

[0462] ;

[0463] in, To relax the penalty term, Let J be the general objective term for optimization, and J be the overall objective function. The number of constraints that require the introduction of slack variables. This represents the number of time discretization segments.

[0464] To relax the penalty weights, we can ensure that a feasible solution can still be obtained when there is a constraint conflict, while minimizing the relaxation amount to ensure collision avoidance safety.

[0465] g. Lightweight OCP subproblem reconstruction:

[0466] i. Using the basic cost function as the core, add slack variable penalty terms to construct a new composite cost function;

[0467] ii. Use STC and the workshop relative safe driving corridor to construct linear box constraints to replace the original nonlinear collision avoidance constraints;

[0468] iii. By retaining the vehicle kinematic constraints, boundary constraints, and two-point boundary constraints, a lightweight OCP subproblem containing only linear constraints is finally reconstructed, solving the core pain points of the original OCP problem, such as high dimension, non-convexity, and difficulty in solving.

[0469] 4. Lightweight iterative optimization and optimal trajectory solution based on LIOM:

[0470] This stage is the core of optimal trajectory solution. The core objective is to gradually resolve the conflict between corridor constraints and vehicle kinematic constraints through lightweight iteration, ensuring that the OCP problem converges stably to a near-optimal solution. This is broken down into three sub-steps, as follows: Figure 13 As shown,

[0471] Step S500: Construct the composite cost function;

[0472] Step S502: Initialize iteration parameters;

[0473] Step S504: Calculate the infeasibility degree of the current trajectory;

[0474] Step S506: Iterative loop judgment;

[0475] If the judgment result of step S506 is yes, then execute step S508: update the corridor constraints based on the current trajectory;

[0476] Step S510: Construct the lightweight OCP subproblem for the current round;

[0477] Step S512: Using the previous solution as a hot start, the solver solves the problem;

[0478] Step S514: Obtain the current intermediate solution trajectory;

[0479] Step S516: Recalculate the infeasibility degree;

[0480] Then return to step S506: iterative loop judgment;

[0481] If the judgment result of step S506 is negative, proceed to step S518: trajectory fit-out judgment;

[0482] If the judgment result of step S518 is yes, execute step S520: output the final optimal trajectory;

[0483] If the result of step S518 is negative, proceed to step S522: trigger the fallback mechanism for extreme scenarios.

[0484] Step S524: Output the initial trajectory as an emergency execution plan.

[0485] a. Cost function construction:

[0486] i. Construct the final composite cost function:

[0487] ;

[0488] in, To relax the penalty term, For routine optimization of target items, This is a composite cost item.

[0489] ii. The original OCP problem, which included nonlinear kinematic constraints, is further transformed into a quadratic programming problem with only linear box constraints, which greatly reduces the difficulty of solving the problem and lays the foundation for lightweight iterative optimization.

[0490] b. Iterative corridor update and solution of OCP subproblems:

[0491] i. If the current infeasibility is greater than the convergence threshold and the number of iterations has not reached the maximum value, then enter the iteration loop;

[0492] Based on the trajectory obtained in the previous iteration The process of constructing the safe driving corridor between STC and the workshop was re-executed, the corridor constraints were updated, the corridor was iteratively refined, the feasible region was gradually narrowed, and the constraints were tightened.

[0493] ii. Based on the updated corridor constraints, construct the lightweight OCP subproblem for the current round, using the trajectory of the previous round as the hot start point, and solve the OCP subproblem using the IPOPT interior point method solver to obtain the intermediate solution trajectory for the current round;

[0494] iii. Based on the intermediate solution trajectory of the current round, recalculate the infeasibility degree, update the iteration count, and complete one round of iteration.

[0495] c. Emergency Response Mechanism:

[0496] If the trajectory infeasibility still exceeds the safety threshold after the iteration terminates, a fallback mechanism is triggered, and the initial trajectory generated by the path generation algorithm that integrates high-precision map and kinematic model is output as the emergency execution trajectory. The trajectory 100% meets the requirements of no collision and kinematic feasibility, which can ensure driving safety in extreme scenarios.

[0497] 5. Trajectory tracking execution and dynamic closed-loop management:

[0498] This stage is the core of the planned trajectory implementation. The core objective is to implement the optimal planned trajectory and achieve dynamic closed-loop management. This is broken down into three detailed implementation steps, as follows: Figure 14 As shown, it includes:

[0499] Step S600: Global broadcast synchronization within the optimal trajectory formation;

[0500] Step S602: The lead vehicle in the convoy adjusts its following distance and driving sequence;

[0501] Step S604: Refresh the scene status and trajectory tracking deviation every 0.2 seconds;

[0502] Step S606: Determine if dynamic replanning is triggered?

[0503] If the judgment result of step S606 is yes, proceed to step S608: re-execute the full process planning, update the optimal trajectory, and then return to step S600;

[0504] If the result of step S606 is negative, proceed to step S610: determine whether the path regression triggering condition is met.

[0505] If the judgment result of step S610 is yes, execute step S612: generate a smooth regression trajectory and a stable regression guide path;

[0506] Step S614: Smoothly restore the platoon base speed;

[0507] Step S616: Send a rejoining request to the lead vehicle;

[0508] Step S618: The lead vehicle passes verification and is reinstated into the formation timing control;

[0509] Step S620: Resume normal platooning, completing the entire closed-loop process;

[0510] If the result of step S610 is negative, return to step S604.

[0511] a. Global synchronization within the formation of the optimal trajectory:

[0512] i. After the collaborative trajectory planning within the cluster is completed, the optimal trajectory, driving sequence, and obstacle avoidance actions of each vehicle are broadcast globally within the formation through vehicle-to-vehicle direct communication, ensuring that all vehicles in the formation have a unified understanding of the collaborative planning results;

[0513] ii. Based on the planning results, the lead vehicle in the convoy adjusts the following distance and driving sequence within the convoy to reserve sufficient safety space for obstacle avoidance maneuvers of vehicles within the convoy, ensuring that obstacle avoidance by a single vehicle will not cause secondary risks such as convoy sequence disorder or following too closely.

[0514] b. Real-time status refresh and dynamic replanning trigger:

[0515] i. During vehicle operation, the status information of obstacles and surrounding vehicles is refreshed every 0.2 seconds to verify the deviation between the current driving status and the planned trajectory in real time, as well as the collision risk caused by scene changes;

[0516] ii. When any of the following unexpected scenarios occur, dynamic replanning will be immediately triggered, the entire planning method will be re-executed, and the optimal trajectory will be updated:

[0517] Sudden changes in the scene: the addition of static / dynamic obstacles, and sudden lane changes by manually driven vehicles;

[0518] Track tracking deviation: Lateral deviation > 0.5m, velocity deviation > 1m / s, exceeding the tracking control correction capability;

[0519] Communication interruption: Trajectory information of surrounding vehicles has been lost, posing a potential collision risk.

[0520] c. Path regression and formation repositioning closed loop:

[0521] i. When a vehicle leaves the core traffic area of ​​an intersection and enters the departure phase, and simultaneously meets all of the following conditions, the original path regression process is triggered:

[0522] ① The distance from all obstacles and vehicles is greater than or equal to the upper limit of the safe distance;

[0523] ② Overall risk value ≤ 0.3;

[0524] ③ It is within the allowed range of formation timing.

[0525] ii. Based on the vehicle's current position and the predefined guidance path, ensure that the vehicle smoothly returns to the guidance path, while restoring the platooning reference speed with a gentle acceleration of ≤0.1g;

[0526] iii. After the vehicle returns to its original path and baseline speed, it sends a rejoining request to the lead vehicle in the platoon. Once the lead vehicle verifies that the vehicle's driving status and following distance meet the platooning requirements, it reintegrates the vehicle into the platooning sequence control, restores the normal platooning driving mode, and completes the entire closed loop of this collaborative obstacle avoidance plan.

[0527] In one specific embodiment, optionally, an improved artificial potential field method is used instead of a social force model to generate the initial trajectory. By superimposing the potential fields of the target point's attraction, obstacle repulsion, and road boundary repulsion, a collision-free feasible trajectory is quickly generated. Simultaneously, a convex feasible set is used instead of a double-layer safety corridor, transforming collision constraints into convex polygon feasible region constraints, thus completing the linearization of nonlinear constraints. Finally, the optimal trajectory is solved through quadratic programming. The entire calculation process can be completed locally on the vehicle, adapting to the kinematic characteristics of heavy-duty mining trucks, realizing multi-vehicle cooperative obstacle avoidance planning at unstructured intersections, and achieving the core objective of the invention.

[0528] In one specific embodiment, optionally, a rolling time-domain optimization framework based on model predictive control is adopted to replace fixed full-time-domain planning. This framework performs rolling optimization only on trajectories within a short future time window, further reducing the computational load per iteration. Simultaneously, priority-decoupled planning replaces vehicle-to-vehicle synchronous coordination. Planning priorities are allocated based on the truck's load and direction of travel. High-priority vehicles complete trajectory planning first, while low-priority vehicles incorporate the trajectories of high-priority vehicles as dynamic obstacles, completing planning sequentially. This enables real-time on-board solution without cloud dependency, adapting to mine platooning operations, ensuring the safety and efficiency of multi-vehicle traffic, and achieving the core objective of the invention.

[0529] like Figure 9As shown in the figure, this application embodiment also provides a cooperative obstacle avoidance trajectory planning device 900, including: a spatiotemporal anchoring module 902, used to generate a spatiotemporal calibration point sequence for multiple cooperative planning vehicles according to a predefined planning time domain and distance step length, wherein the multiple cooperative planning vehicles correspond to the same driving destination; a path preset module 904, used to obtain a predefined guidance path corresponding to each cooperative planning vehicle within the target area; a gravity construction module 906, used to determine path following parameters for each cooperative planning vehicle according to the predefined guidance path and spatiotemporal calibration point sequence, wherein the path following parameters are used to guide the cooperative planning vehicle to travel along the predefined guidance path; a repulsion construction module 908, used to determine vehicle-to-vehicle obstacle avoidance parameters for avoiding vehicle-to-vehicle collisions based on the distance between the multiple cooperative planning vehicles; and a trajectory generation module 910, used to generate a trajectory based on the path following parameters. The parameters and obstacle avoidance parameters between vehicles determine at least one cooperative initial trajectory; the corridor construction module 912 is used to determine the safe driving corridor of the vehicle based on the cooperative initial trajectory, and the safe driving corridor of the vehicle is used to determine the driving space of each cooperatively planned vehicle in the target area, and the driving space does not exceed the map boundary corresponding to the target area; the conflict determination module 914 is used to determine at least two conflicting vehicles based on the relative positional relationship of multiple vehicle safe driving corridors in the time interval corresponding to the spatiotemporal calibration point sequence; the corridor optimization module 916 is used to determine the relative safe driving corridor between vehicles for conflicting vehicles based on at least two vehicle safe driving corridors; the trajectory determination module 918 is used to determine the cooperative trajectory based on the vehicle safe driving corridor and the relative safe driving corridor between vehicles; the cooperative planning module 920 is used to control the cooperatively planned vehicles to drive according to the cooperative trajectory.

[0530] This application also provides a chip, which includes a processor and a communication interface. The communication interface and the processor are coupled. The processor is used to run programs or instructions to implement the various processes of the above-described cooperative obstacle avoidance trajectory planning method embodiments, and can achieve the same technical effect. To avoid repetition, it will not be described again here. In addition, the chip improves the data processing speed corresponding to the cooperative obstacle avoidance trajectory planning method in this application.

[0531] It should be understood that the chip mentioned in the embodiments of this application may also be referred to as a system-on-a-chip, system chip, chip system, or system-on-a-chip, etc.

[0532] In this invention, the terms "first," "second," and "third" are used for descriptive purposes only and should not be construed as indicating or implying relative importance; the term "multiple" refers to two or more unless otherwise explicitly defined. The terms "install," "connect," "link," and "fix" should be interpreted broadly. For example, "connect" can be a fixed connection, a detachable connection, or an integral connection; "link" can be a direct connection or an indirect connection through an intermediate medium. Those skilled in the art can understand the specific meaning of the above terms in this invention according to the specific circumstances.

[0533] In the description of this invention, it should be understood that the terms "upper," "lower," "left," "right," "front," "rear," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or unit referred to must have a specific orientation or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on this invention.

[0534] In the description of this specification, the terms "one embodiment," "some embodiments," "specific embodiment," etc., refer to specific features, structures, materials, or characteristics described in connection with an embodiment or example that are included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0535] The above are merely preferred embodiments of the present invention and are not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A cooperative obstacle avoidance trajectory planning method, characterized in that, include: Based on the predefined planning time domain and distance step length, a spatiotemporal calibration point sequence is generated for multiple collaborative planning vehicles, and the multiple collaborative planning vehicles have the same driving destination. Obtain the predefined guidance path corresponding to each of the collaboratively planned vehicles within the target area; For each of the collaboratively planned vehicles, path following parameters are determined based on the predefined guidance path and the spatiotemporal calibration sequence. The path following parameters are used to guide the collaboratively planned vehicle to travel along the predefined guidance path. Based on the distances between multiple collaboratively planned vehicles, inter-vehicle obstacle avoidance parameters are determined to prevent inter-vehicle collisions. At least one cooperative initial trajectory is determined based on the path following parameters and the inter-vehicle obstacle avoidance parameters; The autonomous vehicle safe driving corridor is determined based on the cooperative initial trajectory. The autonomous vehicle safe driving corridor is used to determine the driving space of each of the cooperatively planned vehicles in the target area. The driving space does not exceed the map boundary corresponding to the target area. At least two conflicting vehicles are determined based on the relative positional relationship of the multiple vehicle safety driving corridors within the time interval corresponding to the spatiotemporal calibration point sequence. For the conflicting vehicles, a relative safe driving corridor between the vehicles is determined based on at least two of the vehicle's safe driving corridors; The cooperative trajectory is determined based on the vehicle's safe driving corridor and the workshop's relative safe driving corridor; Control the vehicle to travel according to the cooperative trajectory.

2. The cooperative obstacle avoidance trajectory planning method according to claim 1, characterized in that, The acquisition of the predefined guidance path corresponding to each of the collaboratively planned vehicles within the target area includes: Obtain the entrance and exit locations of multiple intersections corresponding to the target area; Multiple path direction combinations are determined based on the entrance location and the exit location; Based on the real-time location and destination of the collaboratively planned vehicle, a corresponding predefined guidance path is matched from multiple combinations of path directions. The predefined guidance path includes a preset path and a static boundary.

3. The cooperative obstacle avoidance trajectory planning method according to claim 1, characterized in that, For each of the collaboratively planned vehicles, the path following parameters are determined based on the predefined guidance path and the spatiotemporal calibration sequence, including: Obtain the current location of the collaboratively planned vehicle; Based on the current location, multiple target guidance points are determined for the collaborative planning vehicle on the corresponding predefined guidance path, and the multiple target guidance points correspond to the spatiotemporal calibration point sequence; The path following parameters are determined based on the current location and the target guide point.

4. The cooperative obstacle avoidance trajectory planning method according to claim 1, characterized in that, Determine at least one cooperative initial trajectory based on the path following parameters and the inter-vehicle obstacle avoidance parameters, including: The net external force acting on each of the cooperatively planned vehicles is determined based on the path following parameters and the inter-vehicle obstacle avoidance parameters. The turning angle mapping component and acceleration mapping component corresponding to the collaboratively planned vehicle are determined based on the resultant external force. Obtain the vehicle kinematics model; Based on the vehicle kinematics model, multiple trajectory segments corresponding to the spatiotemporal calibration point sequence are determined according to the angle mapping component and the acceleration mapping component. A cooperative initial trajectory is determined based on multiple trajectory segments.

5. The cooperative obstacle avoidance trajectory planning method according to claim 1, characterized in that, Determining the safe driving corridor of the vehicle based on the cooperative initial trajectory includes: Obtain the radius of the vehicle body envelope disk for each of the collaboratively planned vehicles; Based on the radius of the vehicle body envelope disk, the static boundary corresponding to the target area is expanded to determine the expanded boundary. For each of the cooperatively planned vehicles, along the cooperative initial trajectory, a first local box corresponding to each target guidance point is determined; With a preset step size, the first box boundary of all the first local boxes is expanded synchronously until the expanded first box boundary contacts the expansion boundary or reaches the preset maximum expansion range, thereby determining the local safe area. The vehicle's safe driving corridor is determined based on multiple local safety zones.

6. The cooperative obstacle avoidance trajectory planning method according to claim 5, characterized in that, For the conflicting vehicles, a relative safe driving corridor between the vehicles is determined based on at least two of the vehicle's safe driving corridors, including: For each of the conflicting vehicles, the relative motion trajectory is determined based on the cooperative initial trajectory; The relative collision area range is determined based on the radius of the vehicle body envelope disk of the conflicting vehicles; For the relative motion trajectory, determine a second local box corresponding to each target guidance point of the conflicting vehicles; With a preset step size, the second box boundary of all the second local boxes is expanded synchronously until the expanded second box boundary contacts the expansion boundary or reaches the maximum expansion range, thereby determining the collision safety zone; The relative safe driving corridor of the workshop is determined based on multiple collision safety zones.

7. The cooperative obstacle avoidance trajectory planning method according to claim 1, characterized in that, Determining a cooperative trajectory based on the vehicle's safe driving corridor and the relative safe driving corridor of the vehicle within the workshop includes: Preliminary constraints are determined based on the vehicle's safe driving corridor and the workshop's relative safe driving corridor. The optimization parameters are determined based on the preliminary constraints and the preset trajectory optimization rules. Starting from the initial cooperative trajectory, the initial cooperative trajectory is iteratively optimized according to the optimization parameters to obtain the optimized trajectory; In each iteration, the vehicle's safe driving corridor and the vehicle-to-vehicle safe driving corridor are updated based on the currently obtained optimized trajectory to update the optimization parameters; When the optimized trajectory meets the preset convergence condition, the iteration stops and the optimized trajectory that meets the convergence condition is determined as the cooperative trajectory.

8. The cooperative obstacle avoidance trajectory planning method according to any one of claims 1 to 7, characterized in that, Also includes: The cooperative trajectory is globally synchronized within the platoons to which multiple cooperatively planned vehicles belong; Real-time acquisition of the status information of the collaborative planning vehicle, the status information of obstacles in the target area, and the status information of other collaborative planning vehicles; Based on the status information, it is determined whether the dynamic replanning trigger condition is met. The dynamic replanning trigger condition includes at least one of the following: a new obstacle is detected, the deviation between the actual trajectory of the cooperative planning vehicle and the cooperative trajectory exceeds a preset threshold, or communication with at least one other cooperative planning vehicle is interrupted. When the aforementioned dynamic replanning triggering condition is met, the dynamic replanning process is triggered to redetermine the cooperative trajectory; When the collaboratively planned vehicle leaves the core traffic area of ​​the target area and meets the preset path return conditions, the collaboratively planned vehicle is controlled to return to the corresponding predefined guidance path and the vehicle speed is adjusted to restore the platooning state.

9. A cooperative obstacle avoidance trajectory planning device, characterized in that, include: The spatiotemporal anchoring module is used to generate a spatiotemporal calibration point sequence for multiple collaborative planning vehicles based on a predefined planning time domain and distance step length, wherein the multiple collaborative planning vehicles correspond to the same driving destination. The path pre-setting module is used to obtain the predefined guidance path corresponding to each of the collaboratively planned vehicles within the target area; The gravity construction module is used to determine path following parameters for each of the cooperative planning vehicles based on the predefined guidance path and the spatiotemporal calibration point sequence. The path following parameters are used to guide the cooperative planning vehicles to travel along the predefined guidance path. The repulsion construction module is used to determine inter-vehicle obstacle avoidance parameters to avoid inter-vehicle collisions based on the distance between multiple collaboratively planned vehicles. The trajectory generation module is used to determine at least one cooperative initial trajectory based on the path following parameters and the inter-vehicle obstacle avoidance parameters; The corridor construction module is used to determine the safe driving corridor of the autonomous vehicle based on the cooperative initial trajectory. The safe driving corridor of the autonomous vehicle is used to determine the driving space of each of the cooperatively planned vehicles in the target area. The driving space does not exceed the map boundary corresponding to the target area. The conflict determination module is used to determine at least two conflicting vehicles based on the relative positional relationship of the multiple vehicle safe driving corridors within the time interval corresponding to the spatiotemporal calibration point sequence; The corridor optimization module is used to determine a relative safe driving corridor between the vehicles in the conflict based on at least two vehicle safe driving corridors. The trajectory determination module is used to determine a cooperative trajectory based on the vehicle's safe driving corridor and the workshop's relative safe driving corridor; The collaborative planning module is used to control the collaboratively planned vehicle to travel according to the collaborative trajectory.

10. A chip, characterized in that, The chip includes a processor and a communication interface, the communication interface being coupled to the processor, the processor being used to run programs or instructions to implement the steps of the cooperative obstacle avoidance trajectory planning method as described in any one of claims 1 to 8.

Images