Unmanned truck trajectory optimization method based on mining area loading site

Abstract

The invention relates to the technical field of unmanned driving track optimization, in particular to an unmanned truck track optimization method based on a mining area loading site, which comprises the following steps: establishing a grid map based on a mining area loading site map, and obtaining an initial path through path search according to a track starting point and a track ending point; performing discrete sampling on the motion state of the vehicle on the initial path to obtain multiple pieces of sampling point information; based on the grid map and the sampling point information, a driving safety corridor used for representing the maximum movable range of the vehicle is determined; establishing a trajectory optimization model, processing the discretized trajectory optimization model by adopting a multiple targeting method based on the sampling point information, performing iterative optimization based on vehicle kinematics constraint, physical constraint, boundary value constraint, obstacle avoidance constraint and violation degree determined by a target function, and finally obtaining an optimal trajectory; according to the method, the curvature of the planned path can be smooth and stable.

Description

Technical Field

[0001] This invention relates to the field of unmanned driving trajectory optimization technology, specifically to a method for optimizing the trajectory of unmanned trucks based on loading sites in mining areas. Background Technology

[0002] With the rapid development and breakthroughs in autonomous driving technology, unmanned transportation in mining areas has gradually become a focus of industry attention. As an ideal application scenario for autonomous driving technology, mining areas are characterized by relatively simple scene patterns, fixed and highly repetitive transportation routes. Furthermore, mining environments are typically harsh, especially in high-altitude, cold, or extreme weather conditions. Manual driving not only poses safety hazards but also increases labor costs. Introducing autonomous driving technology can significantly reduce labor costs, greatly improve transportation efficiency, and reduce the risks associated with human error.

[0003] However, the complex terrain and irregular obstacles in mining areas also pose significant challenges to the application of autonomous driving technology. Mining roads are typically rugged and may contain dynamically changing obstacles, such as temporarily stored ore, equipment, or other vehicles, which significantly increases the difficulty of path planning. Therefore, providing a stable, safe, and efficient driving path for autonomous vehicles is crucial for the successful implementation of unmanned transportation in mining areas.

[0004] In current autonomous driving applications in mining areas, grid map modeling is typically used to construct the environment model, and Dijkstra's algorithm or A / B algorithm is employed. * The path search algorithm calculates the initial path from the starting point to the destination. However, this initial trajectory cannot be directly sent to the vehicle control module for tracking because the path is not smoothed after the path planning stage. There are still potential problems such as sudden changes in curvature at the junctions of different line types and excessively high speeds when the curvature on the original path is large. Summary of the Invention

[0005] In view of the above problems, the present invention provides a method for optimizing the trajectory of unmanned trucks based on loading sites in mining areas, which solves the technical problems of uneven planned paths and large curvature in the prior art.

[0006] This invention provides a method for optimizing the trajectory of unmanned trucks based on loading sites in mining areas, comprising the following steps:

[0007] Step S1: Establish a grid map based on the loading site map of the mining area; obtain an initial path through path search based on the trajectory start and end points determined in the grid map; and perform discrete sampling of the motion state of vehicles on the initial path to obtain multiple sampling point information.

[0008] Step S2: Determine the driving safety corridor representing the maximum range of vehicle mobility based on the grid map and sampling point information;

[0009] Step S3: Establish a trajectory optimization model, including: determining the vehicle kinematic constraints, physical constraints, and boundary value constraints of the vehicle's motion state; determining obstacle avoidance constraints based on the driving safety corridor; and determining the objective function of the trajectory optimization model.

[0010] Step S4: Based on the sampling point information, the discretized trajectory optimization model is processed using the multi-target method. Iterative optimization is performed based on the degree of violation of vehicle kinematic constraints, physical constraints, boundary value constraints, obstacle avoidance constraints, and objective function to finally obtain the optimal trajectory.

[0011] Preferably, in step S1, the step of obtaining the initial path through path search specifically includes: based on the starting point and the ending point, using Hybrid A * The path search algorithm generates an initial path from the starting point to the end point that conforms to the vehicle's kinematics model and has no collision risk.

[0012] The motion state of the vehicle trajectory points on the initial path is discretely sampled to obtain sampling points; the sampling point information includes the vehicle coordinate position, heading angle and velocity of the sampling point.

[0013] Preferably, step S2 specifically includes,

[0014] Step S2-1: Determine the vehicle's outer contour rectangle based on the vehicle's coordinate position, heading angle, and vehicle dimensions at the sampling points;

[0015] The vehicle's coordinates are used as the center of the rectangle, the vehicle's dimensions are used as the length and width of the rectangle, and the direction of the grid edges of the raster map is used as the direction of the rectangle's edges.

[0016] Step S2-2: Establish a vehicle coordinate system with the direction of the grid edge of the raster map as the x-axis and y-axis. Repeatedly expand the vehicle's outer contour rectangle in the order of positive x-axis direction, negative x-axis direction, positive y-axis direction, and negative y-axis direction, with an expansion width of d.

[0017] During the expansion process, the expansion range is determined. If no obstacles are encountered within the expansion range or the expansion width along the current coordinate axis does not exceed the maximum expansion width d, then... max If the expansion is successful, the expansion will be retained; otherwise, it will no longer expand along the current coordinate axis; ultimately forming the maximum movable area of ​​the sampling point.

[0018] Step S2-3: The maximum movable area of ​​each sampling point together constitutes the driving safety corridor.

[0019] Preferably, step S3 specifically includes:

[0020] Step S3-1: Determine the vehicle kinematic constraints, which are used to determine the vehicle's abscissa, ordinate, heading angle, velocity, and rate of change of the front wheel steering angle from the vehicle's velocity, heading angle, steering angle, acceleration, and front wheel steering angle angular velocity.

[0021] Step S3-2: Determine the physical constraints, which are used to limit the range of vehicle acceleration, speed, front wheel steering angle angular velocity, front wheel steering angle, and curvature.

[0022] Step S3-3: Determine boundary value constraints, which are used to limit the abscissa, ordinate, heading angle, speed, front wheel steering angle, and curvature of the vehicle path start and end states;

[0023] Step S3-4: Determine obstacle avoidance constraints based on the driving safety corridor, whereby the obstacle avoidance constraints are used to limit the vehicle's movement range;

[0024] Step S3-5: Determine the objective function, which is used to estimate the trajectory travel time, the degree of speed deviation, and the degree of acceleration change.

[0025] Preferably, step S3-3 specifically includes:

[0026] Boundary constraints are determined by the current planning type. If the current planning type is to enter the loading point, the initial state of the horizontal coordinate, vertical coordinate, heading angle, speed, front wheel angle and curvature is set to zero, and the final state is set to the corresponding value when entering the loading point.

[0027] If the current planning type is departure from the site, the actual change in heading angle is determined based on whether the vehicle circles during the departure process; the initial states of the horizontal coordinate, vertical coordinate, speed, front wheel angle, and curvature are set to zero, and the final state is set to the corresponding values ​​when leaving the site; the initial state of the heading angle is set to zero, and the final state is set to the actual change in heading angle.

[0028] Preferably, step S3-4 specifically includes:

[0029] The driving safety corridor is defined as the area within which vehicles can travel, and the obstacle avoidance constraint is expressed as follows:

[0030]

[0031] Among them, Ω vehicle Ω represents the drivable range of the vehicle. obstacle Ω represents the space occupied by the obstacle. edge The space encompassing the area on and outside the site boundaries.

[0032] Preferably, in steps S3-5, the objective function expression is:

[0033]

[0034] Among them, J o Let w1, w2, w3 be the objective function, and t be the weight coefficients. f Let v(τ) be the travel time, and v(τ) be the vehicle speed at time τ. ref (τ) represents the planning velocity of the previous sampling point at time τ, |·| 2 j represents the square of the absolute value. 2 (τ) represents the vehicle's acceleration at time τ.

[0035] Preferably, step S4 specifically includes,

[0036] Step S4-1: Discretize the vehicle kinematic constraints to obtain discrete state variables, discrete control variables, and discrete objective functions;

[0037] Step S4-2: Initialize the iterative optimization, including setting the iteration counter iter = 0 and the degree of nonlinear constraint violation ε. infeasibility =+∞, the velocity of each sampling point in the sampling point information is determined as the initial velocity;

[0038] Step S4-3: Employing a multi-targeting method, the violation degree ε is iteratively updated based on discrete state variables, discrete control variables, vehicle kinematic constraints, physical constraints, boundary value constraints, and obstacle avoidance constraints. infeasibility and optimized trajectory T opt ;

[0039] Step S4-4: When iter reaches the maximum number of iterations... max or ε infeasibility The iteration ends when the deviation is less than the maximum permissible violation ε, and the current optimized trajectory T is changed. opt As the optimal trajectory.

[0040] Preferably, step S4-1 specifically includes:

[0041] Define N+1 sampling times t k k = 0, 1, ..., N, where k is the sampling time index, and the time domain [0, t] is... f Divide the time intervals evenly into N segments;

[0042] Sub-time interval t∈[t k , t k+1 On the [top], the control quantity u(t) is interpolated, and the expression is:

[0043]

[0044] in, This represents the value of the control variable in the i-th dimension at the k-th time interval. φ represents the value of the interpolated i-th dimension control variable in the k-th time interval. k (t) represents the basis function corresponding to the k-th time interval;

[0045] The discrete state quantities are set to satisfy the continuity constraint, that is, the discrete state quantity at the end of each sub-time interval is equal to the discrete state quantity at the beginning of the next sub-time interval.

[0046] Compared with the prior art, the present invention has at least the following beneficial effects:

[0047] (1) This invention firstly utilizes a Hybrid path search algorithm to fully consider the kinematic characteristics of vehicles and the actual constraints of complex terrain in mining areas during the initial path generation stage, effectively avoiding the infeasible path problem caused by neglecting dynamic constraints in traditional methods such as Dijkstra's algorithm and A* algorithm. By discretizing the initial collision-free path, a more feasible and safer foundation is provided for subsequent trajectory optimization, greatly improving the smoothness and rationality of the path.

[0048] (2) This invention provides a strategy for constructing a safe driving corridor based on a grid map and sampling points, transforming traditional complex obstacle avoidance constraints into dynamic upper and lower bound constraints on trajectory point coordinates. This enables unmanned trucks to maintain movement within a safe area away from obstacles and site boundaries during trajectory optimization, improving the system's obstacle avoidance capability and safety in complex dynamic environments.

[0049] (3) This invention provides a multi-targeting method for discretizing the model, transforming the high-dimensional, highly nonlinear constrained trajectory optimization problem into a finite-dimensional nonlinear programming problem that can be solved efficiently. This significantly reduces computational complexity and improves solution efficiency. Finally, by substituting the reasonable initial trajectory solution obtained from the search within the loading area of ​​the mining area into the trajectory optimization solution architecture proposed in this invention, a better optimized trajectory can be obtained, thereby significantly improving the transportation efficiency and safety of unmanned trucks in the mining area and providing reliable technical support for the large-scale implementation of unmanned transportation in the mining area. Attached Figure Description

[0050] The accompanying drawings are for illustrative purposes only and are not intended to limit the scope of the invention.

[0051] Figure 1 The flowchart shows the unmanned truck trajectory optimization method based on the loading site in the mining area provided by the present invention.

[0052] Figure 2 The flowchart illustrates the iterative solution of the trajectory optimization problem after conversion using the multiple firing method provided by this invention.

[0053] Figure 3This is a schematic diagram illustrating the circling phenomenon of the unmanned truck leaving the site, as provided by the present invention.

[0054] Figure 4 This is a schematic diagram illustrating the expansion method of the driving safety corridor provided by the present invention. Detailed Implementation

[0055] To better understand the above-described objectives, features, and advantages of the present invention, the 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 of the present invention and the features thereof can be combined with each other. Furthermore, the present invention can be implemented in other ways different from those described herein; therefore, the scope of protection of the present invention is not limited to the specific embodiments disclosed below.

[0056] This invention is based on a trajectory optimization model for optimal control problems. It establishes constraints for the problem model according to the characteristics of the loading area in the mining area and redesigns the objective function. During the solution process, the original optimal control problem is transformed into a nonlinear programming problem, and the multiple-shot method from the direct method is used for solving it. The final result is a smooth, stable, and safe unmanned truck driving path.

[0057] To illustrate the effectiveness of the method proposed in this invention, the following detailed description of the above technical solution is provided through a specific embodiment, such as... Figure 1 As shown, a method for optimizing the trajectory of unmanned trucks based on loading sites in mining areas is disclosed. The specific implementation steps are as follows:

[0058] Step S1: Establish a grid map based on the loading site map of the mining area, determine the starting point and ending point of the trajectory in the grid map, and obtain an initial path through path search based on the starting point and ending point; perform discrete sampling on the motion state of the vehicle on the initial path to obtain multiple sampling point information;

[0059] The sampling point information includes the vehicle coordinates, heading angle, and speed of the sampling point.

[0060] In this step, the first step is to acquire a map of the loading area of ​​the unmanned truck in the mining area. The acquired map is then rasterized, dividing the entire work area into multiple square grid cells to facilitate subsequent path planning and area analysis. Road boundaries and static obstacles within the mining area can be marked on the raster map, and appropriate environmental parameters (such as lane width, grid resolution, and obstacle safety distance) can be set according to actual application requirements, providing basic data support for subsequent path search and motion state sampling.

[0061] Subsequently, the starting and ending points of the autonomous truck's journey are determined, and the corresponding starting and target positions are marked on a grid map. Based on these starting and ending points, the Hybrid A... * The path search algorithm generates an initial path from the starting point to the ending point that conforms to the vehicle's kinematics model and has no collision risk. The initial path contains trajectory points, each containing information such as the vehicle's coordinate position, heading angle, and vehicle speed at each path point.

[0062] The initial path is obtained by using a fixed-interval discrete sampling method to perform multi-point discrete sampling of the vehicle's motion trajectory points along the path, thus obtaining multiple sampling points. Each sampling point contains information such as the vehicle's coordinate position, heading angle, and vehicle speed.

[0063] Step S2: Determine the driving safety corridor representing the maximum range of vehicle mobility based on the grid map and sampling point information;

[0064] In this step, a driving safety corridor is constructed based on the coverage area of ​​the vehicle's own driving area, such as... Figure 4 As shown, the specific steps are as follows.

[0065] Step S2-1: Determine the vehicle's outer contour rectangle based on the vehicle's coordinate position, heading angle, and vehicle dimensions at the sampling points.

[0066] The vehicle's coordinates are used as the center of the rectangle, the vehicle's dimensions are used as the length and width of the rectangle, and the direction of the grid edges of the raster map is used as the direction of the rectangle's edges.

[0067] Step S2-2: Establish a vehicle coordinate system with the direction of the grid edge of the raster map as the x-axis and y-axis. Repeatedly expand the vehicle's outer contour rectangle in the order of positive x-axis direction, negative x-axis direction, positive y-axis direction, and negative y-axis direction, with an expansion width of d.

[0068] During the expansion process, the expansion range is determined. If no obstacles are encountered within the expansion range or the expansion width along the current coordinate axis does not exceed the maximum expansion width d, then... max If the expansion is successful, the expansion will be retained; otherwise, it will no longer expand along the current coordinate axis; ultimately forming the maximum movable area of ​​each sampling point.

[0069] Step S2-3: The maximum movable area of ​​each sampling point together constitutes the driving safety corridor.

[0070] Each sampling point is assigned an upper and lower bound range for its x and y coordinates, expressed as follows:

[0071]

[0072] Where, x min (t), xmax (t) represents the movable upper and lower bounds of the x-coordinate x(t) of the trajectory point at time t within the driving safety corridor; y min (t), y max (t) represents the movable upper and lower bounds of the ordinate y(t) of the trajectory point at time t within the driving safety corridor, where t f This refers to the travel time.

[0073] By constructing a safe driving corridor, each sampling point on the trajectory is assigned an upper and lower bound range for its x and y coordinates, which can be used for subsequent obstacle avoidance constraint planning.

[0074] Step S3: Establish a trajectory optimization model, including: determining the vehicle kinematic constraints, physical constraints, and boundary value constraints of the vehicle's motion state; determining obstacle avoidance constraints based on the driving safety corridor; and determining the objective function of the trajectory optimization model.

[0075] This invention first defines the optimal control problem for trajectory optimization, which is expressed as: given a controlled system in the time domain t[0, t] f The above satisfies:

[0076]

[0077] Where X(t) and u(t) are the state variable and control variable of the controlled system, respectively. Let f(·) be the rate of change of the state variable, and let f(·) represent the control function.

[0078] State variables and control variables must satisfy:

[0079] L e (X(t),u(t),t)=0

[0080] L i (X(t),u(t),t)≤0

[0081] Among them, L e (·) and L i (·) represent the equality and inequality constraints that the state variables, control variables, and time variables of the controlled system need to satisfy.

[0082] By finding a suitable control variable, the system can reach the target set at the end time and minimize the performance index J0.

[0083] The above describes the optimal control problem. This invention concretizes the optimal control problem based on various constraints during the operation of an unmanned truck and establishes it as a trajectory optimization model, as described in detail below.

[0084] (1) Vehicle kinematic constraints

[0085] Based on the vehicle kinematics model based on the bicycle model, the vehicle kinematic constraints applicable to loading sites in mining areas are obtained as follows:

[0086]

[0087] Where the state variables X(t) = [x(t), y(t), θ(t), v(t), δ(t)] T , Let v(t), θ(t), δ(t), a(t), and ω(t) represent the abscissa, ordinate, heading angle, velocity, and front wheel steering angle of the vehicle at time t, respectively. Let L represent the velocity, heading angle, steering angle, acceleration, and front wheel steering angular velocity at time t, respectively.

[0088] (2) Physical constraints

[0089] Due to inherent motion characteristics, the physical constraints on vehicle kinematics are expressed as follows:

[0090]

[0091] Where κ(t) represents the curvature at time t, a min a max v min v max ω min ω max δ min δ max κ min κ max These represent the maximum and minimum values ​​of acceleration, velocity, front wheel steering angle angular velocity, front wheel steering angle, and curvature, respectively.

[0092] (3) Boundary value constraints

[0093] The trajectory planning tasks within the loading area of ​​the mining area are divided into two categories: trajectory planning for parking and entering the loading point, and trajectory planning for leaving the site.

[0094] For the trajectory of the vehicle entering the loading point, the endpoint is a stationary state; therefore, the endpoint of the parking trajectory is taken as the starting point for trajectory optimization. The endpoint of trajectory optimization is determined by the trajectory point S on the empty vehicle main road; therefore, the boundary value constraints are expressed as follows:

[0095]

[0096] Where, x init Indicates the initial state, [·] T Indicates transpose, x goal Indicates the final state, x s y s θ sv s δ s κ s These represent the x-coordinate, y-coordinate, heading angle, speed, front wheel angle, and curvature of the trajectory points on the main road for empty vehicles, respectively.

[0097] The starting point of the trajectory for leaving the site is a stationary state. The ending point is determined by the trajectory merging point M where the loaded vehicle leaves the main lane. However, because the vehicle is not allowed to shift gears, the trajectory leaving the loading point may exhibit a circular pattern in some narrow loading areas. The change in the heading angle of the vehicle's trajectory is not the difference (θ) between the heading angle at the loading point and the heading angle at the trajectory merging point. M -θ s ), but (θ M -θ s -π, such as Figure 3 As shown. Therefore, before optimizing the departure trajectory, it is necessary to traverse all trajectory points of the departure site in advance and record the actual heading change Δθ. The boundary value constraint is expressed as:

[0098]

[0099] Where, x M ,y M ,Δθ,v M ,δ M ,κ M These represent the x-coordinate and y-coordinate of the trajectory merging point, the actual change in heading angle, speed, front wheel angle, and curvature, respectively.

[0100] (4) Obstacle Avoidance Constraints

[0101] Obstacle avoidance constraints refer to the range of movement allowed for each trajectory point when its position changes due to trajectory optimization, in order to ensure safety. The expression is:

[0102]

[0103] Among them, Ω vehicle Ω represents the drivable range of the vehicle. vehicle Ω represents the space occupied by the obstacle. edge The space encompassing the area on and outside the site boundaries.

[0104] The present invention determines the obstacle avoidance constraint based on the driving safety corridor as the vehicle driving range in the obstacle avoidance constraint.

[0105] Based on the trajectory planning task within the loading area of ​​the mining zone, and combined with the performance index calculation function in the established optimal control model, the objective function designed in this invention is as follows:

[0106]

[0107] Among them, J o Let w1, w2, w3 be the objective function, and t be the weight coefficients. f Let v(τ) be the travel time, and v(τ) be the vehicle speed at time τ. ref (τ) represents the planning velocity of the previous sampling point at time τ, |·| 2 j represents the square of the absolute value. 2 (τ) represents the vehicle's acceleration at time τ.

[0108] In the objective function, the first term w1t f This represents the travel time t for the entire trajectory. f The consideration is to complete the task in a shorter time and improve driving efficiency in the narrow loading area of ​​the mine.

[0109] Second item This indicates consideration of the degree of deviation between the vehicle's speed and the planned speed obtained in the previous section, to prevent excessive differences from the speed planning results;

[0110] Third item This indicates consideration of the mechanical losses of the power system and braking system during the vehicle's travel along the trajectory, in order to reduce sudden changes in vehicle acceleration and reduce the frequent switching between acceleration and braking.

[0111] Step S4: Based on the sampling point information, the discretized trajectory optimization model is processed using the multi-target method. Iterative optimization is performed based on the degree of violation of vehicle kinematic constraints, physical constraints, boundary value constraints, obstacle avoidance constraints, and objective function to finally obtain the optimal trajectory.

[0112] In this step, the trajectory optimization model is first discretized.

[0113] Since the curvature constraint included in the trajectory optimization model is a nonlinear constraint, the multiple-shot method in nonlinear programming is chosen to solve the established model. By discretizing the control and state variables in the optimal control problem, the original optimal control problem is transformed into determining the control parameter σ. k and auxiliary parameter s k In the time domain t∈[t k , t k+1 The transformation process of the finite-dimensional nonlinear programming problem on [ ] is as follows:

[0114] Define N+1 sampling times {t k |k=0,1,...,N}, where k is the sampling time index, which will divide the time domain [0,t f Divide the segments evenly into N segments. Then we can obtain:

[0115] 0 = t0 <t1<…<tN =t f

[0116]

[0117] (1) Discretization of control quantity

[0118] In the optimization problem described above, the control variable is u(t) = [a(t), ω(t)]. T In the sub-time interval t∈[t k , t k+1 Above, for n u The control quantity u(t) of dimension is interpolated, and its curve satisfies:

[0119]

[0120] in, To control component u i (t) in the sub-time interval t∈[t k ,t k+1 The value of χ on ] k (t) is the switching function.

[0121]

[0122] Introducing the control vector σ k :

[0123]

[0124] in, This represents the value of the control variable in the i-th dimension at the k-th time interval. At this point, It can be achieved through the basis function φ k (t) is approximated by interpolation, and the expression is:

[0125]

[0126] in, φ represents the value of the interpolated i-th dimension control variable in the k-th time interval. k (t) represents the basis function corresponding to the k-th time interval.

[0127] If the basis functions can be linear interpolation, then:

[0128]

[0129] (2) Discretization of state variables

[0130] Based on the multiple-target shooting method, an auxiliary vector is introduced. Components s k (k = 0, 1, ..., N)

[0131] Let x(t) be the initial value of the state variable in each sub-time interval, then we have:

[0132] s k =x(t) k )

[0133] Let x be the state variable in the k-th time interval. k (t), then we have:

[0134] x k (t)=f(x k (t),σ k (t),t)

[0135] To ensure the continuity of state variables throughout the entire time domain, a continuity constraint is introduced as an equality constraint:

[0136] x k (t k )-x k+1 (t k+1 ) = 0

[0137] Furthermore, it can be written as:

[0138] x k (t k )-s k =0

[0139] Where, x k (t k ) represents the state quantity within the k-th time interval.

[0140] By introducing auxiliary variable s in this way k This approach ensures the continuity of the starting state in each sub-interval. The vehicle dynamic equations are discretized into integral constraints within each sub-interval. The original optimal control problem is transformed into determining the control parameter σ. k and auxiliary parameter s k In the time domain t∈[t k ,t k+1 This is a finite-dimensional NLP problem on a time scale, where each sub-time interval is independent of the others, and the state variables within each sub-time interval can be calculated separately.

[0141] The objective function of the established trajectory optimization model is transformed into:

[0142]

[0143] Where k is the sub-time interval index, N is the total number of sub-time intervals, and t k , t k+1 These are the beginning and end times of sub-time interval k, respectively.

[0144] The steps for optimizing the discretized trajectory optimization model using the multiple-shot method are as follows: Figure 2 As shown, it includes:

[0145] (1) Initialize the iteration counter iter = 0, and the degree of nonlinear constraint violation ε infeasibility =+∞, the velocity planning result is used as the initial velocity value, which is combined with the original path point information to form the initial planning result. This result is then input into the established trajectory optimization model. The multi-shot method is used to solve the transformed trajectory optimization problem. In each iteration, the solver returns the degree of violation ε of the new optimization result relative to the nonlinear constraints. infeasibility And the new optimized trajectory T opt The iteration count increases by one unit after each iteration.

[0146] In some embodiments, the degree of violation of nonlinear constraints ε infeasibility This is used to quantify the degree to which the curvature in an optimized trajectory exceeds the allowable range. Its calculation is based on sampled points after trajectory discretization, and the specific logic is as follows:

[0147] ① Discretize the optimized trajectory into N sampling points based on time or distance, denoted as t1, t2, ..., t N ;

[0148] ② Calculate the actual curvature κ(s) at each sampling point. k );

[0149] ③ For each sampling point, if κ(s) k )>κ max or κ(s) k )<κ min If so, then there is a violation at that point, and the violation quantity is |κ(s) k )-κ max |or|κ(s) k )-κ min |;|·| indicates the calculation of the absolute value; if it does not exceed the curvature constraint range, the violation amount is 0;

[0150] ④ε infeasibility This represents the maximum value among all curvature constraint violations at all sampling points.

[0151] The objective function of the trajectory optimization model for σ and s after the transformation is:

[0152]

[0153] All constraints are transformed into algebraic constraints on σ and s:

[0154] ① Vehicle kinematic constraints

[0155]

[0156] ②Physical constraints

[0157]

[0158] ③ Boundary value constraints

[0159] Initial state: s0 = x init

[0160] End point state: s N =x goal

[0161] ④ Obstacle avoidance constraints

[0162]

[0163] Where x min (t k ), y min (t k ), x max (t k ), y max (t k ) is the safety corridor at time t k The boundary.

[0164] (2) Configure iter max The parameter ε represents the maximum number of iterations, and iter represents the maximum permissible degree of violation of the nonlinear constraint. When the number of iterations is less than the maximum number of iterations... max At that time, the trajectory T generated in the previous iteration will be... opt The input is used in a nonlinear programming solver for optimization. After optimization, the degree of violation ε produced in this iteration is compared. infeasibility The relationship between the magnitude of ε and the parameter ε, if ε infeasibility If the current trajectory is less than or equal to ε, then the current trajectory is considered to meet the optimization conditions, and the current optimization result T is returned. opt If the trajectory is optimal, the algorithm terminates; otherwise, the iteration counter iter = iter + 1 is incremented, and the next iteration continues.

[0165] (3) If the number of iterations reaches the maximum value, iter max However, the optimization condition ε is still not met. infeasibility If ≤ε, then return the current optimization result T. opt The algorithm terminates once the optimal trajectory is determined.

[0166] The final optimal trajectory T opt It contains complete trajectory information such as position, heading, and speed. This trajectory satisfies vehicle dynamics, physical constraints, and boundary conditions, as well as obstacle avoidance requirements based on the safety corridor transformation, while optimizing travel time and vehicle smoothness.

[0167] While the specific embodiments of the present invention depict actions or steps in a particular order, this should be understood as requiring such actions or steps to be performed in the specific order shown or in sequential order, or requiring all illustrated actions or steps to be performed to achieve the desired result. In certain environments, multitasking and parallel processing may be advantageous. Similarly, although several specific implementation details are included in the above discussion, these should not be construed as limiting the scope of this disclosure. Certain features described in the context of individual embodiments may also be implemented in combination in a single implementation. Conversely, various features described in the context of a single implementation may also be implemented individually or in any suitable sub-combination in multiple implementations.

[0168] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for optimizing the trajectory of unmanned trucks based on loading areas in mining areas, characterized in that, Includes the following steps: Step S1: Establish a raster map based on the loading site map of the mining area. Based on the trajectory start and end points determined in the raster map, obtain the initial path through path search. The motion state of the vehicles on the initial path is discretely sampled to obtain information from multiple sampling points; Step S2: Determine the driving safety corridor representing the maximum range of vehicle mobility based on the grid map and sampling point information; Step S3: Establish a trajectory optimization model, including: determining the vehicle kinematic constraints, physical constraints and boundary value constraints of the vehicle's motion state, and determining obstacle avoidance constraints by the driving safety corridor; Determine the objective function of the trajectory optimization model; Step S4: Based on the sampling point information, the discretized trajectory optimization model is processed using the multi-target method. Iterative optimization is performed based on the degree of violation of vehicle kinematic constraints, physical constraints, boundary value constraints, obstacle avoidance constraints, and objective function to finally obtain the optimal trajectory.

2. The method for optimizing the trajectory of unmanned trucks based on loading sites in mining areas according to claim 1, characterized in that, In step S1, the step of obtaining the initial path through path search specifically includes: based on the starting point and the ending point, using Hybrid A * The path search algorithm generates an initial path from the starting point to the end point that conforms to the vehicle's kinematics model and has no collision risk. The motion state of the vehicle trajectory points on the initial path is discretely sampled to obtain sampling points; the sampling point information includes the vehicle coordinate position, heading angle and velocity of the sampling point.

3. The method for optimizing the trajectory of unmanned trucks based on loading sites in mining areas according to claim 2, characterized in that, Step S2 specifically includes, Step S2-1: Determine the vehicle's outer contour rectangle based on the vehicle's coordinate position, heading angle, and vehicle dimensions at the sampling points; The vehicle's coordinates are used as the center of the rectangle, the vehicle's dimensions are used as the length and width of the rectangle, and the direction of the grid edges of the raster map is used as the direction of the rectangle's edges. Step S2-2: Establish a vehicle coordinate system with the direction of the grid edge of the raster map as the x-axis and y-axis. Repeatedly expand the vehicle's outer contour rectangle in the order of positive x-axis direction, negative x-axis direction, positive y-axis direction, and negative y-axis direction, with an expansion width of d. During the expansion process, the expansion range is determined. If no obstacles are encountered within the expansion range or the expansion width along the current coordinate axis does not exceed the maximum expansion width d, then... max If the expansion is successful, the expansion will be retained; otherwise, it will no longer expand along the current coordinate axis; ultimately forming the maximum movable area of ​​the sampling point. Step S2-3: The maximum movable area of ​​each sampling point together constitutes the driving safety corridor.

4. The method for optimizing the trajectory of unmanned trucks based on loading sites in mining areas according to claim 3, characterized in that, Step S3 specifically includes: Step S3-1: Determine the vehicle kinematic constraints, which are used to determine the vehicle's abscissa, ordinate, heading angle, velocity, and rate of change of the front wheel steering angle from the vehicle's velocity, heading angle, steering angle, acceleration, and front wheel steering angle angular velocity. Step S3-2: Determine the physical constraints, which are used to limit the range of vehicle acceleration, speed, front wheel steering angle angular velocity, front wheel steering angle, and curvature. Step S3-3: Determine boundary value constraints, which are used to limit the abscissa, ordinate, heading angle, speed, front wheel steering angle, and curvature of the vehicle path start and end states; Step S3-4: Determine obstacle avoidance constraints based on the driving safety corridor, whereby the obstacle avoidance constraints are used to limit the vehicle's movement range; Step S3-5: Determine the objective function, which is used to estimate the trajectory travel time, the degree of speed deviation, and the degree of acceleration change.

5. The method for optimizing the trajectory of unmanned trucks based on loading sites in mining areas according to claim 4, characterized in that, Step S3-3 specifically includes: Boundary constraints are determined by the current planning type. If the current planning type is to enter the loading point, the initial state of the horizontal coordinate, vertical coordinate, heading angle, speed, front wheel angle and curvature is set to zero, and the final state is set to the corresponding value when entering the loading point. If the current planning type is departure from the site, the actual change in heading angle is determined based on whether the vehicle circles during the departure process; the initial states of the horizontal coordinate, vertical coordinate, speed, front wheel angle, and curvature are set to zero, and the final state is set to the corresponding values ​​when leaving the site; the initial state of the heading angle is set to zero, and the final state is set to the actual change in heading angle.

6. The method for optimizing the trajectory of unmanned trucks based on loading sites in mining areas according to claim 5, characterized in that, Steps S3-4 specifically include: The driving safety corridor is defined as the area within which vehicles can travel, and the obstacle avoidance constraint is expressed as follows: Among them, Ω vehicle Ω represents the drivable range of the vehicle. obstacle Ω represents the space occupied by the obstacle. edge The space encompassing the area on and outside the site boundaries.

7. The method for optimizing the trajectory of unmanned trucks based on loading sites in mining areas according to claim 6, characterized in that, In step S3-5, the objective function expression is: Among them, J o Let w1, w2, w3 be the objective function, and t be the weight coefficients. f Let v(τ) be the travel time, and v(τ) be the vehicle speed at time τ. ref (τ) represents the planning velocity of the previous sampling point at time τ, |·| 2 j represents the square of the absolute value. 2 (τ) represents the vehicle's acceleration at time τ.

8. The method for optimizing the trajectory of unmanned trucks based on loading sites in mining areas according to claim 7, characterized in that, Step S4 specifically includes, Step S4-1: Discretize the vehicle kinematic constraints to obtain discrete state variables, discrete control variables, and discrete objective functions; Step S4-2: Initialize the iterative optimization, including setting the iteration counter iter = 0 and the degree of nonlinear constraint violation ε. infeasibility =+∞, the velocity of each sampling point in the sampling point information is determined as the initial velocity; Step S4-3: Employing a multi-targeting method, the violation degree ε is iteratively updated based on discrete state variables, discrete control variables, vehicle kinematic constraints, physical constraints, boundary value constraints, and obstacle avoidance constraints. infeasibility and optimized trajectory T opt ; Step S4-4: When iter reaches the maximum number of iterations... max or ε infeasibility The iteration ends when the deviation is less than the maximum permissible violation ε, and the current optimized trajectory T is changed. opt As the optimal trajectory.

9. The method for optimizing the trajectory of unmanned trucks based on loading sites in mining areas according to claim 8, characterized in that, Step S4-1 specifically includes: Define N+1 sampling times t k k = 0, 1, ..., N, where k is the sampling time index, and the time domain [0, t] is... f Divide the time intervals evenly into N segments; Sub-time interval t∈[t k ,t k+1 On the [top], the control quantity u(t) is interpolated, and the expression is: in, This represents the value of the control variable in the i-th dimension at the k-th time interval. φ represents the value of the interpolated i-th dimension control variable in the k-th time interval. k (t) represents the basis function corresponding to the k-th time interval; The discrete state quantities are set to satisfy the continuity constraint, that is, the discrete state quantity at the end of each sub-time interval is equal to the discrete state quantity at the beginning of the next sub-time interval.

Images