A trajectory planning method for semi-trailer tractors based on hierarchical iterative search

Abstract

The present invention discloses a semi-trailer tractor trajectory planning method based on hierarchical iterative search, comprising: 1) collecting traffic information data of the semi-trailer tractor in real time through on-board equipment; 2) establishing a road coordinate system and obtaining the position of the vehicle in the road coordinate system; 3) determining the target speed of the vehicle; 4) calculating the horizontal sampling interval and the vertical sampling interval, the sampling time domain, and the number of spline samples; 5) determining the optimal sampling plane in the sampling space by summing the estimated losses of all sampling trajectories in each sampling plane; 6) establishing a coupled kinematic model of the semi-trailer tractor to achieve the complete operating trajectory of the semi-trailer tractor as a whole; 7) evaluating the complete loss of the guessed optimal trajectory; 8) completing the local trajectory planning process by establishing kinematic constraints and safety constraints. The method of the present invention can effectively reduce the incidence of traffic accidents caused by untimely trajectory planning and improve the safety and stability of vehicles during driving.

Description

Technical Field

[0001] The present invention relates to intelligent transportation technology, and in particular to a semi-trailer tractor trajectory planning method based on hierarchical iterative search. Background Art

[0002] Semi-trailer tractors, as a primary means of road transportation, have significant implications for improving traffic safety, promoting logistics industry upgrades, and building smart cities. The Society of Automotive Engineers (SAE) categorizes intelligent vehicles into six levels based on their intelligence: no automation, driver support, partial automation, conditional automation, high automation, and full automation. It is widely believed that autonomous driving technology will rapidly develop over the next few decades. Compared to passenger cars, which operate in more complex environments, autonomous semi-trailer tractors, primarily used in trunk logistics and industrial park dispatching, offer more specific application scenarios and lower implementation costs, making them considered the most suitable candidates for autonomous driving technology.

[0003] Most existing autonomous driving trajectory planning methods are based on standard passenger cars and cannot be directly applied to trajectory planning for autonomous semi-trailer tractors. Furthermore, compared to passenger cars, autonomous semi-trailer tractors have a higher center of gravity, heavier payloads, and more complex driving conditions. Their articulated structures and nonlinear constraints further complicate local path trajectory planning. Existing trajectory planning theories and algorithms for autonomous semi-trailer tractors suffer from excessive computation time and idealized constraints, making real-time dynamic trajectory solution and control difficult. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a semi-trailer tractor trajectory planning method based on hierarchical iterative search in response to the defects in the prior art.

[0005] The technical solution adopted by the present invention to solve the technical problem is: a semi-trailer tractor trajectory planning method based on hierarchical iterative search, comprising the following steps:

[0006] 1) Real-time collection of traffic information data of the semi-trailer tractor through on-board equipment; the traffic information data includes: real-time vehicle speed, vehicle position and posture information, RGB image information and point cloud image information around the vehicle;

[0007] 2) Lane-level high-precision maps construct global road reference lines, establish a road coordinate system, and obtain the vehicle's position in the road coordinate system;

[0008] 3) Determine the target speed of the vehicle by collecting the speed of the preceding vehicle and the distance between the preceding vehicle and the vehicle head in real time;

[0009] 4) Calculate the horizontal and vertical sampling intervals, sampling time domain, and number of spline samples based on the acquired road traffic information and the target speed of the vehicle;

[0010] A typical sampling space is constructed using the horizontal and vertical sampling intervals, sampling time domain, and number of spline samples. With the global road reference line as the sampling center, horizontal and vertical state sampling is performed in the typical sampling space, including horizontal and vertical position, velocity, and acceleration. By constructing a priori estimation loss function for the sampling trajectory, the deviation between the initial and final states of the sampling and the target state is calculated, and the loss value of all trajectories in the sampling space is estimated.

[0011] Horizontal sampling interval d gap , longitudinal sampling interval v gap , the number of sampling time domains n T and the number of spline samples n sample It can be calculated by the following formula:

[0012]

[0013] v gap =v>30?3∶5

[0014]

[0015] Where, ω d and ω T is the adjustment coefficient; w r is the road width; w r is the vehicle width; v now is the current speed of the vehicle; v target is the target speed of the vehicle; v limit is the road speed limit; v gap is the speed sampling interval, usually 5; T max is the maximum sampling time domain; T min is the minimum sampling time domain; w s is the width of the exerciseable area; v gap is the speed sampling interval.

[0016] 5) Determine the optimal sampling plane in the sampling space by calculating the sum of the estimated losses of all sampling trajectories in each sampling plane;

[0017] By estimating the trajectory loss in the sampling plane, the first n trajectories with the smallest loss in each sampling plane are selected to establish the initial guess trajectory solution set in the optimal sampling plane;

[0018] By guessing the initial and final sampling states of the solution, the complete running trajectory of the tractor is solved;

[0019] 6) Establish a coupled kinematic model of the semi-trailer tractor, solve the trajectory of the trailer through the trajectory of the tractor, and realize the complete trajectory of the semi-trailer tractor as a whole (tractor and trailer);

[0020] 7) Evaluate the complete loss of the guessed optimal trajectory by establishing a complete loss function; evaluate the optimality of the optimal trajectory by solving the complete trajectory around the guessed solution and calculating the complete trajectory loss. If the guessed solution is the optimal solution, accept the guessed solution as the optimal solution. Otherwise, calculate the sampling update direction through the gradient method and update the guessed solution set until convergence;

[0021] 8) By establishing kinematic constraints and safety constraints, the feasibility of all trajectories in the guess solution set is checked, and then the feasible guess solution with the smallest loss is taken from the guess solution set as the optimal feasible solution, and the local trajectory planning process is completed according to the optimal feasible solution.

[0022] According to the above scheme, in step 4),

[0023] The prior loss is estimated as C est , as follows:

[0024] C est =C terminal +C continue

[0025] Where C continue is the trajectory continuity loss; C terminal is the terminal offset loss;

[0026] Among them, C continue and C terminal It can be calculated by the following formula

[0027]

[0028] According to the above scheme, in step 5),

[0029] The calculation method of the total loss of the sampling plane is as follows:

[0030]

[0031] Where, Ch is the sampling plane loss,

[0032] C s,d is the estimated loss when the sampling state is (s,d).

[0033] According to the above scheme, in step 5),

[0034] By guessing the initial and final sampling states of the solution, the quintic polynomial and quartic polynomial trajectories are used to connect the initial and final states of the horizontal and longitudinal directions, respectively, to solve the complete running trajectory of the guessed solution towing vehicle.

[0035] The longitudinal quartic polynomial trajectory can be described by the following formula:

[0036] S(t)=c s0 +c s1 t+c s2 t 2 +c s3 t 3 +c s4 t 4

[0037] Where c s0 are the polynomial coefficients,

[0038] T is time,

[0039] S is the longitudinal movement distance.

[0040] The transverse quintic polynomial trajectory described in step 4 can be described by the following equation:

[0041] d(t)=c d0 +c d1 t+c d2 t 2 +c d3 t 3 +c d4 t 4 +c d5 t 5

[0042] Where c is the polynomial coefficient; t is time; and d is the longitudinal movement distance.

[0043] According to the above scheme, in step 5), the complete loss function is described by the following formula:

[0044] C total =C process +C terminal

[0045]

[0046] Where C process is the process loss; C terminal is the terminal loss, w is the weight coefficient, j lat ,j lon represents the lateral and longitudinal acceleration, a lat ,a lat represents the lateral and longitudinal acceleration, and t is the sampling time domain.

[0047] According to the above solution, in step 6), the kinematic model of the semi-trailer tractor can be described by the following formula:

[0048]

[0049] Where, δ f is the steering angle of the front axle of the tractor, l head is the wheelbase of the tractor, dt is the sampling time interval, is the rate of change of the heading angle of the tractor and trailer at time t, are the heading angles of the tractor and trailer at time t, (x t head ,y t head ) is the horizontal and vertical coordinates of the rear axle center of the tractor at time t, is the rate of change of the horizontal and vertical coordinates of the center of the rear axle of the trailer at time t.

[0050] According to the above scheme, in step 8), the kinematic constraints are:

[0051]

[0052] Where v is the vehicle speed; v limit is the road speed limit; a is the acceleration; a min 、a max are the minimum and maximum acceleration respectively; θ head ,θ trailer are the heading angles of the tractor and trailer respectively; Δθ max is the maximum articulation angle.

[0053] According to the above solution, in step 8), the safety constraint condition adopts the safety constraint condition of the multi-circle envelope method, which is specifically divided into two parts: envelope center calculation and collision constraint condition. The envelope center is calculated by the following formula:

[0054]

[0055] The collision constraint is described by the following formula:

[0056]

[0057] Where, is the center of the i-th envelope circle of the tractor, is the center of the i-th enveloping circle of the trailer; (x h ,y h ) is the coordinate of the center of the rear axle of the tractor; (x t ,y t ) is the center position of the trailer's rear axle; r veh 、r obsare the radius of the vehicle envelope and the obstacle respectively; θ h is the heading angle of the tractor; θ t is the heading angle of the trailer.

[0058] The beneficial effects produced by the present invention are:

[0059] The present invention provides a semi-trailer tractor trajectory planning method with high real-time performance, which reduces the incidence of traffic accidents caused by untimely trajectory planning and can effectively improve the safety and stability of the vehicle during driving. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] The present invention will be further described below with reference to the accompanying drawings and embodiments, in which:

[0061] Figure 1 is a flow chart of a method according to an embodiment of the present invention;

[0062] Figure 2 Schematic diagram of a kinematic model according to an embodiment of the present invention. DETAILED DESCRIPTION

[0063] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0064] like Figure 1 As shown, a semi-trailer tractor trajectory planning method based on hierarchical iterative search includes the following steps:

[0065] 1) Real-time collection of traffic information data of the semi-trailer tractor through on-board equipment; the traffic information data includes: real-time vehicle speed, vehicle position and posture information, RGB image information and point cloud image information around the vehicle;

[0066] 2) Lane-level high-precision maps construct global road reference lines, establish a road coordinate system, and obtain the vehicle's position in the road coordinate system;

[0067] 3) Determine the target speed of the vehicle by collecting the speed of the preceding vehicle and the distance between the preceding vehicle and the vehicle head in real time;

[0068] 4) Calculate the horizontal sampling spacing and vertical sampling range, sampling time domain, and number of spline samples based on the acquired road traffic information and the target vehicle speed;

[0069] Among them, the horizontal sampling interval d gap , longitudinal sampling interval v gap , the number of sampling time domains n T and the number of spline samples n sample It can be calculated by the following formula:

[0070]

[0071] v gap =v>30?3∶5

[0072]

[0073] Where, ω d and ω T is the adjustment coefficient; w r is the road width; w r is the vehicle width; v now is the current speed of the vehicle; v target is the target speed of the vehicle; v limit is the road speed limit; v gap is the speed sampling interval, usually 5; T max is the maximum sampling time domain; T min is the minimum sampling time domain; w s is the width of the exerciseable area; v gap is the speed sampling interval. A typical sampling space is constructed using the horizontal and vertical sampling intervals, the sampling time domain, and the number of spline samples. The global road reference line is used as the sampling center, and horizontal and vertical state sampling is performed in the typical sampling space, including horizontal and vertical position, velocity, and acceleration. By constructing a priori estimation loss function for the sampling trajectory, the deviation between the initial and final states of the sampling and the target state is calculated, and the loss value of all trajectories in the sampling space is estimated.

[0074] The prior loss is estimated as C est , which can be calculated by the following formula

[0075] C est =C terminal +C continue

[0076] Where C continue is the trajectory continuity loss; C terminal is the terminal offset loss;

[0077] Among them, C continue and C terminal It can be calculated by the following formula

[0078]

[0079] Where d is the horizontal sampling offset; v is the sampling speed; t is the sampling time domain; C d 、C v 、C time and C riskare the lateral deviation sub-item, speed sub-item, time sub-item and obstacle sub-item of the terminal deviation loss; w i is the corresponding weight coefficient; d max is the maximum sampling offset; d min is the minimum sampling offset; is the horizontal sampling offset at time t; z and σ0 are adjustment coefficients; d obs is the distance between the obstacle and the reference line; d est is the estimated lateral displacement from the closest position s of the obstacle.

[0080] 5) Determine the optimal sampling plane in the sampling space by calculating the sum of the estimated losses of all sampling trajectories in each sampling plane;

[0081] The calculation method of sampling plane loss is as follows:

[0082]

[0083] Where C h is the sampling plane loss,

[0084] C s,d is the estimated loss when the sampling state is (s,d).

[0085] The optimal sampling plane is determined by estimating the loss of the trajectory, and the top n trajectories with the smallest loss on each optimal sampling plane are taken to establish the initial guess trajectory solution set within the optimal sampling plane.

[0086] By guessing the initial and final sampling states of the solution, the quintic polynomial and quartic polynomial trajectories are used to connect the initial and final states of the horizontal and longitudinal directions, respectively, to solve the complete running trajectory of the guessed solution towing vehicle.

[0087] The longitudinal quartic polynomial trajectory can be described by the following formula:

[0088] S(t)=c s0 +c s1 t+c s2 t 2 +c s3 t 3 +c s4 t 4

[0089] Where c is the polynomial coefficient,

[0090] T is time,

[0091] S is the longitudinal movement distance.

[0092] The transverse quintic polynomial trajectory described in step 4 can be described by the following equation:

[0093] d(t)=cd0 +c d1 t+c d2 t 2 +c d3 t 3 +c d4 t 4 +c d5 t 5

[0094] Where c is the polynomial coefficient;

[0095] t is time;

[0096] d is the longitudinal movement distance;

[0097] 6) Establish a kinematic model of the semi-trailer tractor, such as Figure 2 , through the running trajectory of the semi-trailer tractor, the running trajectory of the semi-trailer tractor is further deduced;

[0098] The kinematic model of the semi-trailer tractor can be described by the following formula:

[0099]

[0100] Where, δ f is the steering angle of the tractor front axle,

[0101] l head is the wheelbase of the tractor,

[0102] dt is the sampling time interval,

[0103] is the rate of change of the heading angle of the tractor and trailer at time t,

[0104] are the heading angles of the tractor and trailer at time t,

[0105] (x t head ,y t head ) is the horizontal and vertical coordinates of the rear axle center of the tractor at time t,

[0106] is the rate of change of the horizontal and vertical coordinates of the center of the rear axle of the trailer at time t.

[0107] 7) Evaluate the complete loss of the guessed optimal trajectory by establishing a complete loss function; evaluate the optimality of the optimal trajectory by solving the complete trajectory around the guessed solution and calculating the complete trajectory loss. If the guessed solution is the optimal solution, accept the guessed solution as the optimal solution. Otherwise, calculate the sampling update direction through the gradient method and update the guessed solution set until convergence;

[0108] The complete loss function can be described by the following formula:

[0109] C total =C process +C terminal

[0110]

[0111] Where C process Is process loss

[0112] and C terminal is the terminal loss,

[0113] w jerk and w acc w is the corresponding weight coefficient,

[0114] j lat ,j lon Represents the lateral and longitudinal accelerations,

[0115] a lat ,a lat represents the lateral and longitudinal acceleration, and t is the sampling time domain.

[0116] The sampling update direction is calculated by the gradient method and is updated according to the maximum gradient direction.

[0117] 8) By establishing kinematic constraints and safety constraints based on the multi-circle envelope method, the feasibility of all trajectories in the guess solution set is checked; by using the loss size as the evaluation criterion, the guess solution with the smallest loss is selected from the guess solution set as the optimal feasible solution, and the local trajectory planning process is completed.

[0118] The kinematic constraints can be described by the following equation:

[0119]

[0120] Where v is the velocity,

[0121] v limit It's the speed limit on the road.

[0122] a is the acceleration,

[0123] a min 、a max are the minimum and maximum acceleration respectively;

[0124] θ head ,θ trailer are the heading angles of the tractor and trailer respectively;

[0125] Δθ max is the maximum articulation angle.

[0126] The safety constraints of the multi-circle envelope method can be divided into two parts: envelope center calculation and collision constraints. The envelope center can be calculated using the following formula:

[0127]

[0128] The collision constraint can be described by the following formula:

[0129]

[0130] Where, is the center of the i-th envelope circle of the tractor;

[0131] is the center of the i-th enveloping circle of the trailer;

[0132] (x h ,y h ) is the coordinate of the center of the rear axle of the tractor;

[0133] (x t ,y t ) is the center position of the trailer's rear axle,

[0134] r veh 、r obs are the radii of the vehicle envelope and the obstacle respectively;

[0135] θ h is the heading angle of the tractor;

[0136] θ t is the heading angle of the trailer.

[0137] The extraction of the optimal trajectory can be described by the following formula:

[0138] traj best =argmin(C guess )

[0139] Where, traj best is the optimal trajectory,

[0140] C guess is the set of guessed optimal trajectories.

[0141] The extraction of the optimal trajectory can be described by the following formula:

[0142] traj best =argmin(C guess )

[0143] Where, traj best is the optimal trajectory; C guess is the set of guessed optimal trajectories.

[0144] It should be understood that those skilled in the art can make improvements or changes based on the above description, and all such improvements and changes should fall within the scope of protection of the appended claims of the present invention.

Claims

1. A semi-trailer tractor trajectory planning method based on hierarchical iterative search, characterized in that: The following steps are involved: 1) Real-time collection of traffic information data of semi-trailer tractors through on-board equipment; The traffic information data includes: real-time vehicle speed, vehicle position and posture information, RGB image information and point cloud image information around the vehicle; 2) Construct a global road reference line based on the lane-level HD map, establish a road coordinate system, and obtain the vehicle's position in the road coordinate system; 3) Determine the target speed of the vehicle by collecting the speed of the preceding vehicle and the distance between the preceding vehicle and the vehicle head in real time; 4) Calculate the horizontal and vertical sampling intervals, sampling time domain, and number of spline samples based on the acquired road traffic information and the target vehicle speed; A typical sampling space is constructed using the horizontal and vertical sampling intervals, sampling time domain, and number of spline samples. With the global road reference line as the sampling center, horizontal and vertical state sampling is performed in the typical sampling space. State sampling includes horizontal and vertical position, velocity, and acceleration. By constructing a priori estimation loss function for the sampled trajectory, the deviation between the initial and final states of the sample and the target state is calculated, and the loss value of all trajectories in the sampling space is estimated. 5) Determine the optimal sampling plane in the sampling space by calculating the sum of the estimated losses of all sampling trajectories in each sampling plane; By estimating the trajectory loss in the sampling plane, the first n trajectories with the smallest loss in each sampling plane are selected to establish the initial guess trajectory solution set in the optimal sampling plane; By guessing the initial and final sampling states of the solution, the complete running trajectory of the tractor is solved; 6) Establish a coupled kinematic model of the semi-trailer tractor, and solve the trajectory of the trailer through the trajectory of the tractor to achieve the complete trajectory of the semi-trailer tractor as a whole; 7) Evaluate the complete loss of the guessed optimal trajectory by establishing a complete loss function; evaluate the optimality of the optimal trajectory by solving the complete trajectory around the guessed solution and calculating the complete trajectory loss. If the guessed solution is the optimal solution, accept it as the optimal solution. Otherwise, calculate the sampling update direction using the gradient method and update the guessed solution set until convergence; 8) By establishing kinematic constraints and safety constraints, the feasibility of all trajectories in the guess solution set is checked, and then the feasible guess solution with the smallest loss is taken from the guess solution set as the optimal feasible solution, and the local trajectory planning process is completed based on the optimal feasible solution.

2. The semi-trailer tractor trajectory planning method based on hierarchical iterative search according to claim 1 is characterized in that: In the step 4), The prior loss is estimated to be , as follows: Where, C continue is the loss of trajectory continuity; C terminal is the terminal offset loss; in, C continue and C terminal Specifically calculated by the following formula ； Where, d is the lateral sampling offset; v is the sampling rate; t is the sampling time domain; 、 、 and They are the lateral deviation sub-item, speed sub-item, time sub-item and obstacle sub-item of terminal deviation loss; w i is the corresponding weight coefficient; d max is the maximum sampling offset; d min is the minimum sampling offset; yes t Horizontal sampling offset at time; z、 is the adjustment factor; d obs is the distance of the obstacle from the reference line; d est is the estimated lateral displacement from the closest position s of the obstacle.

3. The semi-trailer tractor trajectory planning method based on hierarchical iterative search according to claim 1 is characterized in that: In the step 5), The calculation method of the total loss of the sampling plane is as follows: Where, is the sampling plane loss, The sampling state is ( s , d ) is the estimated loss when d is the horizontal sampling offset; t is the sampling time domain.

4. The semi-trailer tractor trajectory planning method based on hierarchical iterative search according to claim 1, characterized in that: In the step 5), By guessing the initial and final sampling states of the solution, the quintic polynomial and quartic polynomial trajectories are used to connect the initial and final states of the horizontal and longitudinal directions, respectively, to solve the complete running trajectory of the guessed solution towing vehicle. The longitudinal quartic polynomial trajectory is described by the following formula: Where, c are the polynomial coefficients, t It's time, S is the longitudinal movement distance; The transverse quintic polynomial trajectory described in step 4 is described by the following formula: Where, c are the polynomial coefficients; t For time; d is the lateral movement distance.

5. The semi-trailer tractor trajectory planning method based on hierarchical iterative search according to claim 1, characterized in that: In the step 5), The complete loss function is described by the following formula: C total = C process + C terminal Where, C process is the process loss, C terminal is the terminal loss, w is the weight coefficient, , Represent the lateral and longitudinal accelerations, , Represent the lateral and longitudinal accelerations, t is the sampling time domain.

6. The semi-trailer tractor trajectory planning method based on hierarchical iterative search according to claim 1, characterized in that: In step 8), the kinematic constraints are: Where, v is the vehicle speed; v limit It is the speed limit of the road; a is the acceleration; a min 、 a max are the minimum and maximum acceleration respectively; θ head 、 θ trailer are the heading angles of the tractor and trailer respectively; is the maximum articulation angle.

7. The semi-trailer tractor trajectory planning method based on hierarchical iterative search according to claim 1, characterized in that: In step 8), the safety constraint condition adopts the safety constraint condition of the multi-circle envelope method, which is specifically divided into two parts: envelope center calculation and collision constraint condition. The envelope center is calculated by the following formula: The collision constraint is described by the following formula: Where, It is the tractor i The center of the enveloping circle, It is a trailer i The center of the enveloping circle; is the coordinate of the center of the rear axle of the tractor; It is the center position of the trailer's rear axle; r veh 、 r obs are the radii of the vehicle envelope and the obstacle respectively; is the heading angle of the tractor; is the heading angle of the trailer.

8. An electronic device, characterized in that: include: one or more processors; as well as a storage device for storing one or more programs, When the one or more programs are executed by the one or more processors, the one or more processors are enabled to perform the method according to any one of claims 1 to 7.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 7 is implemented.

Images