Unmanned truck cooperative trajectory planning method for strip mine unloading operation scene

Abstract

The invention discloses an unmanned truck collaborative trajectory planning method for an open-pit mine unloading operation scene. The problems of multi-truck path generation and conflict resolution in an unstructured environment are solved. An initial track is generated through a mixed A * algorithm, a risk range judgment mechanism is introduced on the basis, and vehicles with potential interaction relations are divided into the same subgroup. Local path optimization is carried out by solving a simplified nonlinear programming problem in the subgroups, conflicts are coordinated among the subgroups according to a priority ranking rule, and finally global collision-free planning of the multi-vehicle trajectory is realized. According to the unmanned truck collaborative trajectory planning method for the strip mine unloading operation scene, a safe, efficient and intelligent planning scheme can be provided for vehicle operation.

Description

Technical Field

[0001] This invention belongs to the field of unmanned driving in mining, specifically relating to a collaborative trajectory planning method for unmanned trucks in open-pit mine unloading operations. Background Technology

[0002] Mine unloading areas are typical unstructured scenarios, often characterized by irregular spatial boundaries and complex obstacle distribution. Furthermore, the large size, heavy weight, and poor maneuverability of unmanned trucks make traditional trajectory planning methods inadequate for meeting the demands for high safety and real-time performance. In addition, single-vehicle trajectory planning methods lack collaborative mechanisms, easily leading to path congestion and conflicts during high-density vehicle traffic, increasing operational risks. Therefore, researching collaborative trajectory planning methods for unmanned trucks in unloading areas is of great significance for improving system performance.

[0003] Common methods for multi-vehicle cooperative trajectory planning include coupled planning and decoupled planning. Coupled planning methods generate the trajectories of all vehicles at once, but they are difficult to handle large groups of vehicles. Decoupled planning methods break down the original task into several sub-problems, solve them separately, and then combine them into a complete solution, but the result may not be globally optimal. Summary of the Invention

[0004] The purpose of this invention is to provide a collaborative trajectory planning method for unmanned trucks in open-pit mine unloading operations, which solves the coupling problem in a decoupling manner, thereby achieving efficient path generation and conflict resolution in complex multi-vehicle interaction environments.

[0005] The specific technical solution of this invention is as follows: a cooperative trajectory planning method for unmanned trucks for open-pit mine unloading operations, comprising the following steps:

[0006] Step 1: Assume n unmanned trucks are working collaboratively in the unloading area. Determine the initial position, target position, load status, and obstacle positions of all vehicles according to the task, where n≥3. Proceed to Step 2.

[0007] Step 2: Establish the kinematic model of the unmanned truck and approximate the vehicle outline using two disks to simplify the geometric modeling process, then proceed to Step 3.

[0008] Step 3: Based on the above kinematic model, construct the kinematic constraints, boundary value constraints, and collision constraints during the driving process of the unmanned truck, and further formalize it into a nonlinear programming problem, then proceed to step 4.

[0009] Step 4: Independently plan the initial trajectory of each unmanned truck in the unloading area using the hybrid A* algorithm, set the risk range, and proceed to Step 5.

[0010] Step 5: Divide all unmanned trucks into several subgroups based on the initial trajectory, ensuring that the vehicles within the subgroups are interconnected within the current risk range, and that there is no interaction between vehicles in the subgroups. Proceed to Step 6.

[0011] Step 6: For each subgroup, extract the collision constraints and offset constraints of each vehicle in the subgroup within the risk range to construct a simplified nonlinear programming problem, and proceed to Step 7.

[0012] Step 7: For each subgroup, if the simplified nonlinear programming problem is solved successfully, update the collision-free trajectories within that subgroup and proceed to Step 8; if any subgroup fails to be solved, narrow down the risk range and return to Step 5 to start dividing all subgroups again.

[0013] Step 8: For all subgroups, check if there is a collision between two adjacent subgroups: if no collision occurs, output the trajectory; if a collision occurs, sort the two colliding subgroups by priority, fix the trajectory of the higher priority subgroup, treat the trajectory of the higher priority subgroup as an obstacle, and return to step 6 to regenerate the constraints for solving.

[0014] Compared with the prior art, the significant advantages of this invention are:

[0015] 1) This invention introduces an objective function that considers factors such as time cost, transportation cost and trajectory smoothness during the trajectory optimization process, so that the generated trajectory not only meets the collision constraints, but also has good execution feasibility, and is suitable for unmanned trucks to travel at low speed and under heavy load in the unloading area.

[0016] 2) This invention achieves adaptive optimization of the vehicle subgroup division and collision detection process by analyzing the solution results of the nonlinear programming problem and dynamically adjusting the risk range.

[0017] 3) This invention constructs a multi-dimensional priority ranking rule, comprehensively considering factors such as load status, target distance, and number of vehicles, thereby rationally determining the vehicle trajectory planning order. High-priority subgroup trajectories are fixed, while low-priority subgroups are treated as obstacles for avoidance planning, thereby reducing the frequency of conflicts between subgroups and the number of trajectory recalculations. Attached Figure Description

[0018] Figure 1 This is a schematic diagram of the overall process of the unmanned truck cooperative trajectory planning method for open-pit mine unloading operations as described in this invention.

[0019] Figure 2 This is a schematic diagram of the kinematic model of the unmanned truck described in this invention. Detailed Implementation

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

[0021] Combination Figure 1 The detailed steps of the unmanned truck cooperative trajectory planning method for open-pit mine unloading operations described in this invention are as follows:

[0022] Step 1: Assume n unmanned trucks are working collaboratively in the unloading area, and determine the initial position, target position, load status, and obstacle positions of all vehicles according to the task, where n≥3.

[0023] Step 2, Combining Figure 2 A kinematic model of the unmanned truck is established, and the vehicle outline is approximated by two disks to simplify the geometric modeling process. Due to the characteristics of the unmanned truck—large size, heavy weight, large turning radius, and low operating speed—its kinematic model is established as follows:

[0024]

[0025] Among them, (x i (t),y i (t) represents the position of the i-th unmanned truck at time t, and v i (t) represents the speed of the i-th unmanned truck at time t, φ i (t) represents the steering wheel angle of the i-th unmanned truck at time t, θ i (t) represents the yaw angle of the i-th unmanned truck at time t, and a i (t) represents the acceleration of the i-th unmanned truck at time t, w i (t) represents the angular velocity of the i-th unmanned truck at time t, L Wi Let t be the wheelbase of the i-th driverless truck. f The time it takes for all vehicles to reach the target location.

[0026] Furthermore, the rectangular body of each autonomous truck is uniformly covered by two disks to approximate its occupied space and simplify collision detection calculations, such as... Figure 2 As shown. To achieve a double-circle approximation of the vehicle's rectangular model, the centers of both the front and rear disks are located on the vehicle's longitudinal axis, offset forward and backward relative to the vehicle's geometric center point. The overall vehicle length is at the specified position. The center position of the front disc of the i-th unmanned truck at time t is: The center position of the rear disk of the i-th autonomous truck at time t is: As shown in the following formula:

[0027]

[0028]

[0029] Among them, L FiL is the front overhang length of the i-th driverless truck. Ri Let be the rear overhang length of the i-th autonomous truck. Let r be the radius of the two disks on the i-th autonomous truck. i All of these are the distances from the center of the corresponding circle to the vertices of the covered rectangular area, as shown in the following formula:

[0030]

[0031] Among them, L Bi Let be the width of the i-th unmanned truck.

[0032] Step 3: Based on the above kinematic model, construct the kinematic constraints, boundary value constraints, and collision constraints during the driving process of the unmanned truck, and further formalize it into a nonlinear programming problem.

[0033] Kinematic constraints: The state variables of the unmanned truck are constrained by their maximum and minimum values, as shown in the following equation:

[0034]

[0035] Among them, a maxi Let v be the maximum acceleration of the i-th unmanned truck. maxi Let w be the maximum speed of the i-th driverless truck. maxi Let φ be the maximum angular velocity of the i-th unmanned truck. maxi Let be the maximum steering wheel angle of the i-th unmanned truck.

[0036] Boundary value constraints: Unmanned truck at t=0 and t=t f The motion stops at a certain point, as shown in the following equation:

[0037]

[0038] Among them, a i (0) represents the acceleration of the i-th unmanned truck at time t=0, v i (0) represents the velocity of the i-th unmanned truck at time t=0, w i (0) represents the angular velocity of the i-th unmanned truck at t=0, φ i (0) represents the steering wheel angle of the i-th unmanned truck at time t=0, a i (t f Let be the i-th driverless truck at t=t f acceleration at time, v i (t f Let be the i-th driverless truck at t=t f The speed of time, w i (t f Let be the i-th driverless truck at t=t f angular velocity at time φ i(t f Let be the i-th driverless truck at t=t f The angle of the steering wheel at any given moment.

[0039] Collision Constraint: Assuming there are m static obstacles within the unloading area, after circularly expanding each obstacle, the j-th obstacle can be approximated as follows: With the center of the circle, The circular area has a radius of [radius value]. To avoid collisions between the vehicle and obstacles, the front and rear discs covering the rectangular body of the vehicle must not overlap with the obstacles, as shown in the following formula:

[0040]

[0041] i∈{1,...,n},j∈{1,...,m},t∈[0,t f ],

[0042] Among them, (X) i (t),Y i Let (t) be the center of any disk of the i-th autonomous truck at time t. To avoid collisions between vehicles, the disks of any two autonomous trucks cannot overlap, as shown in the following equation:

[0043] (X i (t)-X s (t)) 2 +(Y i (t)-Y s (t)) 2 ≥(r i +r s ) 2 ,

[0044]

[0045] i≠s,i∈{1,...,n},s∈{1,...,n},t∈[0,t f ],

[0046] Among them, (X) s (t),Y s (t) is the center of any disk of the s-th unmanned truck at time t. It is the center of the front disc of the s-th unmanned truck at time t. r is the center of the disk behind the s-th unmanned truck at time t. s It is the radius of the disk of the s-th unmanned truck.

[0047] Objective function: Considering the three optimization objectives of the autonomous truck—time cost, transportation cost, and trajectory smoothness—the objective function J can be constructed as follows:

[0048]

[0049] Where λ1, λ2, and λ3 are the weights corresponding to the optimization objective, and the fuel consumption rate of the i-th unmanned truck at time t is... v i Let (t) represent the speed of the i-th unmanned truck at time t. Parameters b0, b1, b2, and b3 need to be estimated using experimental fuel consumption data. Based on the above constraints and objective function, the multi-vehicle cooperative trajectory planning problem is modeled as a continuous-time coupled optimal control problem. Furthermore, the time domain is uniformly discretized into t0, t1, ..., t... f The first-order Runge-Kutta method is used to transform it into a nonlinear programming problem for numerical solution.

[0050] Step 4: Independently plan the initial trajectory of each unmanned truck in the unloading area using the hybrid A* algorithm and set the risk range. The hybrid A* algorithm combines grid search capabilities with feasibility verification of the vehicle kinematics model, supporting basic motion modes such as left turn, straight ahead, and right turn in the forward and backward directions, thereby generating an initial trajectory that satisfies the nonholonomic constraints of the unmanned truck. However, this trajectory only considers collisions between the vehicle and static obstacles, and does not consider collisions between vehicles. In addition, after the trajectory is generated, the risk range [0, d] needs to be set. risk ] ， d risk It represents the upper bound of the risk range, used to determine whether there is a potential collision risk between any unmanned truck and other vehicles or obstacles.

[0051] Step 5: Divide all unmanned trucks into several subgroups based on the initial trajectory, ensuring that vehicles within each subgroup are interconnected within the current risk range, and that there is no interaction between vehicles in different subgroups. If, during the entire driving process, there exists t∈[0,t...] f ] makes d is (t)∈[0,d risk If d , then the i-th autonomous truck and the s-th autonomous truck are said to be interconnected, where d is (t) is the distance between the i-th unmanned truck and the s-th unmanned truck at time t.

[0052] Step 6: For each subgroup, extract the collision constraints and offset constraints for each vehicle within the risk range to construct a simplified nonlinear programming problem. Since the nonlinear programming problem suffers from increased computational complexity as the number of vehicles increases, it is simplified. If other vehicles or obstacles are within the risk range, the collision constraint is considered in the simplified nonlinear programming problem; otherwise, it is removed. Furthermore, the offset constraint is shown in the following equation:

[0053]

[0054] Among them, t k ∈{t0,t1,...,t f} ， For the i-th driverless truck, the front disc at t k The center of time For the i-th driverless truck, the rear disc is at t k The center of time For the i-th unmanned truck in the initial trajectory, the front disk at t k The center of time For the i-th unmanned truck in the initial trajectory, the rear disk is at t k The center of the circle at time δ(t) k ) represents the trajectory at t k The allowable offset range at any given time is shown in the following formula:

[0055]

[0056] Where, δ min The minimum offset range is defined by δ0, which is the initial offset range. To achieve adaptive control of trajectory disturbances, a time-dynamically adjusted offset range is designed. This range enhances trajectory convergence and executability by widening the offset in the initial stage to facilitate path exploration and automatically tightening the offset in the final stage.

[0057] Step 7: For each subgroup, if the simplified nonlinear programming problem is solved successfully, update the collision-free trajectories within that subgroup; if any subgroup fails to solve, narrow down the risk range and return to Step 5 to redistribute all subgroups. The reason for the failure is that the number of collision constraints in the nonlinear programming model is too large, resulting in an excessively high problem dimension that exceeds the solver's processing capacity, hence the need to narrow down the risk range.

[0058] In a preferred embodiment, when a feasible solution cannot be obtained even after gradually narrowing the risk range to a preset lower limit, a backup mechanism can be triggered: first, the initially set risk range is expanded to re-divide the subgroups, thereby exploring new combinations of constraints and improving the solvability of the problem; second, if conflict resolution still cannot be achieved after multiple iterations, an exception handling mechanism is triggered to alert the operator for manual intervention. This optional implementation enhances the robustness of the method under complex operating conditions and avoids infeasible solutions or deadlocks due to excessive narrowing of the risk range.

[0059] Step 8: For all subgroups, check if there is a collision between two adjacent subgroups: if no collision occurs, output the trajectory; if a collision occurs, sort the two colliding subgroups by priority, fix the trajectory of the higher priority subgroup, treat the trajectory of the higher priority subgroup as an obstacle, and return to step 6 to regenerate the constraints for solving.

[0060] The collision subgroups are prioritized according to the following rules:

[0061] 1) Subgroups with a larger number of fully loaded vehicles are given priority over those with a smaller number of fully loaded vehicles. Fully loaded unmanned trucks carrying mineral resources have poor acceleration and deceleration capabilities, so avoidance maneuvers should be minimized.

[0062] 2) Subgroups closer to the target point are given priority over those farther away. Unmanned trucks closer to the target point are usually about to complete their mission and have limited room for trajectory adjustment, so priority should be given to ensuring their passage.

[0063] 3) Subgroups with more vehicles are prioritized over those with fewer vehicles. Subgroups with more vehicles have higher requirements for coordination during planning, are more difficult to adjust, and have higher computational complexity during solution.

[0064] 4) When all the above conditions are the same, randomly determine the priority. When there is no obvious priority order, using a random strategy can effectively prevent the algorithm from getting stuck in local optima or infinite loops.

Claims

1. A method for cooperative trajectory planning of unmanned trucks for open-pit mine unloading operations, characterized in that, Includes the following steps: Step 1: Assume n unmanned trucks are working collaboratively in the unloading area, and determine the initial position, target position, load status, and obstacle positions of all vehicles according to the task, where n≥3, then proceed to Step 2. Step 2: Establish the kinematic model of the unmanned truck and approximate the vehicle outline using two disks to simplify the geometric modeling process, then proceed to Step 3; Step 3: Based on the above kinematic model, construct the kinematic constraints, boundary value constraints, and collision constraints during the driving process of the unmanned truck, and further formalize it into a nonlinear programming problem, then proceed to step 4; Step 4: Independently plan the initial trajectory of each unmanned truck in the unloading area using the hybrid A* algorithm, set the risk range, and proceed to Step 5; Step 5: Divide all unmanned trucks into several subgroups based on the initial trajectory, ensuring that the vehicles within the subgroups are interconnected within the current risk range, and that there is no interaction between vehicles in the subgroups. Proceed to Step 6. Step 6: For each subgroup, extract the collision constraints and offset constraints of each vehicle in the subgroup within the risk range to construct a simplified nonlinear programming problem, and proceed to step 7. Step 7: For each subgroup, if the simplified nonlinear programming problem is solved successfully, update the collision-free trajectories within that subgroup and proceed to Step 8; if any subgroup fails to be solved, narrow down the risk range and return to Step 5 to start dividing all subgroups again. Step 8: For all subgroups, check if there is a collision between two adjacent subgroups: if no collision occurs, output the trajectory; if a collision occurs, sort the two colliding subgroups by priority, fix the trajectory of the higher priority subgroup, treat the trajectory of the higher priority subgroup as an obstacle, and return to step 6 to regenerate the constraints for solving.

2. The unmanned truck cooperative trajectory planning method for open-pit mine unloading operations according to claim 1, characterized in that, In step 2, a kinematic model of the unmanned truck is established, and the vehicle outline is approximated by two disks, as follows: The kinematic model of an unmanned truck can be represented as: Among them, (x i (t),y i (t) represents the position of the i-th unmanned truck at time t, and v i (t) represents the speed of the i-th unmanned truck at time t, φ i (t) represents the steering wheel angle of the i-th unmanned truck at time t, θ i (t) represents the yaw angle of the i-th unmanned truck at time t, and a i (t) represents the acceleration of the i-th unmanned truck at time t, w i (t) represents the angular velocity of the i-th unmanned truck at time t, L Wi Let t be the wheelbase of the i-th driverless truck. f The time it takes for all vehicles to reach the target location; Furthermore, the rectangular body of each autonomous truck is uniformly covered by two disks to approximate its occupied space and simplify collision detection calculations. To achieve the double-circle approximation of the vehicle's rectangular model, the centers of both the front and rear disks are located on the vehicle's longitudinal axis, offset forward and backward relative to the vehicle's geometric center point. The total length of the vehicle is at the location, and the radius r of both disks is the distance from their respective centers to the vertices of the covered rectangular area.

3. The unmanned truck cooperative trajectory planning method for open-pit mine unloading operations according to claim 2, characterized in that, The constraints and nonlinear programming problem in step 3 are as follows: In irregular unloading areas, the cooperative trajectory planning of unmanned trucks needs to consider kinematic constraints, boundary value constraints, and collision constraints. Kinematic constraints: The state variables of the unmanned truck are constrained by their maximum and minimum values, as shown in the following equation: Among them, a maxi Let v be the maximum acceleration of the i-th unmanned truck. maxi Let w be the maximum speed of the i-th driverless truck. maxi Let φ be the maximum angular velocity of the i-th unmanned truck. maxi Let be the maximum steering wheel angle of the i-th unmanned truck; Boundary value constraints: Unmanned truck at t=0 and t=t f The motion stops at a certain point, as shown in the following equation: Among them, a i (0) represents the acceleration of the i-th unmanned truck at time t=0, v i (0) represents the velocity of the i-th unmanned truck at time t=0, w i (0) represents the angular velocity of the i-th unmanned truck at t=0, φ i (0) represents the steering wheel angle of the i-th unmanned truck at time t=0, a i (t f Let be the i-th driverless truck at t=t f acceleration at time, v i (t f Let be the i-th driverless truck at t=t f The speed of time, w i (t f Let be the i-th driverless truck at t=t f angular velocity at time φ i (t f Let be the i-th driverless truck at t=t f Steering wheel angle at any given moment; Collision Constraint: Assuming there are m static obstacles within the unloading area, after circularly expanding each obstacle, the j-th obstacle can be approximated as follows: With the center of the circle, The circular area has a radius of [radius value]. To avoid collisions between the vehicle and obstacles, the front and rear discs covering the rectangular vehicle body must not overlap with the obstacles, as shown in the following formula: i∈{1,...,n},j∈{1,...,m},t∈[0,t f ], Among them, (X) i (t),Y i (t) is the center of any disk of the i-th unmanned truck at time t. Let be the center of the front disc of the i-th driverless truck at time t. r is the center of the rear disk of the i-th driverless truck at time t. i It is the radius of the disk of the i-th unmanned truck; To avoid collisions between vehicles, the disks between any two driverless trucks cannot overlap, as shown in the following formula: (X i (t)-X s (t)) 2 +(Y i (t)-Y s (t)) 2 ≥(r i +r s ) 2 , i≠s,i∈{1,...,n},s∈{1,...,n},t∈[0,t f ], Among them, (X) s (t),Y s (t) is the center of any disk of the s-th unmanned truck at time t. It is the center of the front disc of the s-th unmanned truck at time t. r is the center of the disk behind the s-th unmanned truck at time t. s It is the radius of the disk of the s-th unmanned truck; Objective function: Considering the three optimization objectives of the autonomous truck—time cost, transportation cost, and trajectory smoothness—the objective function J can be constructed as follows: Where λ1, λ2, and λ3 are the weights corresponding to the optimization objective, and λ3 represents the fuel consumption rate of the i-th unmanned truck at time t. v i Let (t) be the speed of the i-th unmanned truck at time t. Parameters b0, b1, b2, and b3 need to be estimated using experimental fuel consumption data. Based on the above constraints and objective function, the multi-vehicle cooperative trajectory planning problem is modeled as a continuous-time coupled optimal control problem. On this basis, the time domain is uniformly discretized into t0, t1, ..., t... f The first-order Runge-Kutta method is used to transform it into a nonlinear programming problem for numerical solution.

4. The unmanned truck cooperative trajectory planning method for open-pit mine unloading operations according to claim 3, characterized in that, The scope of risks in step 4 is as follows: The risk range is represented as [0, d] risk ] ， d risk It represents the upper bound of the risk range, used to determine whether there is a potential collision risk between any unmanned truck and other vehicles or obstacles.

5. The unmanned truck cooperative trajectory planning method for open-pit mine unloading operations according to claim 4, characterized in that, The vehicles in step 5 are interconnected, as detailed below: If during the entire driving process, there exists t∈[0,t] f ] makes d is (t)∈[0,d risk If d , then the i-th autonomous truck and the s-th autonomous truck are said to be interconnected, where d is (t) is the distance between the i-th unmanned truck and the s-th unmanned truck at time t.

6. The unmanned truck cooperative trajectory planning method for open-pit mine unloading operations according to claim 5, characterized in that, The offset constraints in step 6 are as follows: The offset constraint is shown in the following formula: Wherein, time t k ∈{t0,t1,...,t f } ， For the i-th driverless truck, the front disc at t k The center of time For the i-th driverless truck, the rear disc is at t k The center of time For the i-th unmanned truck in the initial trajectory, the front disk at t k The center of time For the i-th unmanned truck in the initial trajectory, the rear disk is at t k The center of the circle at time δ(t) k ) represents the trajectory at t k The allowable offset range at any given time is shown in the following formula: Where, δ min δ0 is the minimum offset range and δ0 is the initial offset range. To achieve adaptive control of trajectory disturbances, an offset range based on time dynamic adjustment is designed. This range promotes path exploration by widening the offset in the initial stage and automatically tightening the offset in the final stage to enhance trajectory convergence and executability.

7. The unmanned truck cooperative trajectory planning method for open-pit mine unloading operations according to claim 6, characterized in that, In step 8, the two colliding subgroups are prioritized as follows: 1) Subgroups with a larger number of fully loaded vehicles are given priority over subgroups with a smaller number of fully loaded vehicles; 2) Subgroups closer to the target point are prioritized over those farther away; 3) Subgroups with more vehicles are prioritized over those with fewer vehicles; 4) When all the above conditions are the same, the priority is determined randomly.

Images