Method for optimizing turning track of trailer

Abstract

The invention discloses a turning trajectory optimization method for a trailer, which comprises the following steps: firstly, judging whether a vehicle needs trajectory optimization, secondly, obtaining an initial input boundary of trajectory optimization based on the current position information of the vehicle, and obtaining an input boundary after an obstacle is processed by combining the longitudinal distance of the center of the obstacle in a frenet coordinate system; based on the input boundary, the turning radius of the trailer during turning and the leftmost end face and the rightmost end face of the trailer are combined, the input boundary considering trailer movement is obtained, and the safety distance of the inner side of the trailer and the safety distance of the outer side of the trailer are obtained through calculation; searching the safe distance between the inner side and the outer side calculated in the previous step on the basis of the curvature, and adding the safe distance into an input boundary after the obstacle is processed; and according to the finally obtained boundary, inputting the boundary into a solver to obtain a final collision-free track considering the trailer motion during turning. The stability, safety and trafficability of the automatic driving system can be greatly improved.

Description

Technical Field

[0001] The invention relates to a method for optimizing a turning trajectory of a trailer, and belongs to the technical field of automatic driving. Background Art

[0002] At present, in modern autonomous driving technology, trajectory planning, as a key link, plays a decisive role in vehicle safety, optimizing path selection, and ensuring passability in narrow scenes. In the field of commercial vehicle autonomous driving, it is necessary to deal with some complex scenarios, such as cornering. Some trajectory optimization methods may not consider the collision relationship between the trailer and surrounding obstacles when the trailer is cornering. There may be a safety risk when the trailer collides with obstacles or stone piers at the corner. In response to the problem of right-angle cornering scenarios of vehicles with trailers, the present invention proposes a cornering trajectory optimization method for vehicles with trailers, the purpose of which is to improve the safety performance and passability of vehicles with trailers when cornering, and effectively improve the adaptability and safety of the autonomous driving system. Summary of the invention

[0003] The technical problem to be solved by the present invention is to overcome the shortcomings of the prior art and provide a method for optimizing the cornering trajectory of a trailer.

[0004] In order to achieve the above objectives, the specific technical solution of the present invention is as follows: A method for optimizing a turning trajectory of a trailer comprises the following steps:

[0005] Step 1: Determine whether the vehicle needs to be optimized. If so, proceed to the next step.

[0006] Step 2: Based on the reference line ref_info(s i ,κ i ) and other information to obtain the initial input boundary init_bound(left i ,right i ),in:

[0007] i represents the index value of the point on the reference line;

[0008] s i Represents the longitudinal position of the i-th point on the reference line in the frenet coordinate system;

[0009] κ i Represents the curvature of the i-th point on the reference line;

[0010] left i Represents the coordinates of the i-th point on the left side of the reference line, and its set constitutes the input left boundary left;

[0011] right iRepresents the coordinates of the i-th point on the right side of the reference line, and its set constitutes the input left boundary right;

[0012] Assuming that the semi-width of the road where the vehicle is located is l, the specific method of obtaining the input boundary is as follows:

[0013] left i = l

[0014] right i =-l

[0015] Step 3: Based on the initial input boundary init_bound obtained in step 2, and combined with the longitudinal distances obstacle_s and obstacle_l of the obstacle center in the frenet coordinate system, the input boundary obstacle_path_bound (left_bound i ,right_bound i );

[0016] Step 4: Based on the obstacle_path_bound input boundary obtained in step 3 after processing the obstacle, combined with the turning radius of the trailer when turning and the leftmost end face and the rightmost end face of the trailer, the input boundary trailer_path_bound considering the movement of the trailer is obtained, and the inner_buffer safety distance inside the trailer and the outer_buffer safety distance outside the trailer are obtained by calculation;

[0017] Step 5: Based on the inner and outer safety distances of the trailer obtained in step 4, find the curvature corresponding to s on the reference line based on the longitudinal distance s of the vehicle on the reference line, and then find the inner and outer safety distances calculated in d in step 4 based on the curvature, and add it to the input boundary obstacle_path_bound after processing the obstacle;

[0018] Step 6: Based on the boundary finally obtained in step 5, input it into the solver to obtain the final collision-free trajectory that takes into account the movement of the trailer when turning.

[0019] Furthermore, in step 1, the vehicle is first observed for the first time, and it is determined whether trajectory optimization for the trailer is required based on the vehicle state and its characteristics, reference lines, obstacle states, and the scene in which the vehicle is located.

[0020] Furthermore, first determine whether the vehicle is in intelligent driving mode, whether the vehicle is a trailer vehicle, whether the vehicle is about to enter a curve, and whether the vehicle has generated a reference line. If it is a trailer vehicle, and the vehicle is in intelligent driving mode, and there is a curve in front of the vehicle, and there is already a reference line in the lane, then it is considered that the basic conditions for the operation of the algorithm are met. Then continue to determine whether there is a change in the curvature of the reference line of the vehicle's driving, and the change in curvature changes from the curvature of a straight line to the curvature of a curve. If the absolute value of the change exceeds 0.03, it is necessary to enter the cornering trajectory optimization.

[0021] Furthermore, the specific content of step 3 is as follows:

[0022] Step 3.1, assuming that the known obstacle ahead is given in the form of a box, the length of the obstacle is length, and the width is width. Combining the length and width of the obstacle, we have:

[0023] The longitudinal distance of the rear end of the obstacle vehicle in the frenet coordinate system is the longitudinal distance of the obstacle in the frenet coordinate system, obstacle_s, minus half of the length of the obstacle vehicle; that is,

[0024] ob_lower_s=obstacle_s-0.5*length

[0025] The longitudinal distance of the front end of the obstacle vehicle in the frenet coordinate system is the longitudinal distance of the obstacle in the frenet coordinate system obstacle_s plus half of the obstacle vehicle length; that is,

[0026] ob_upper_s=obstacle_s+0.5*length

[0027] Step 3.2, combined with the initial input boundary init_bound, the lateral distance obstacle_l and the longitudinal distance obstacle_s of the front and rear ends of the obstacle in the frenet coordinate system, update the input boundary obstacle_path_bound after processing the obstacle. The logic of the left boundary set after processing the obstacle is shown in the following code:

[0028]

[0029] Step 3.2.1, based on the positional relationship between the obstacle and the vehicle, if the obstacle is on the left side of the vehicle or on both sides, take the left reference line boundary left i The minimum value between the two values ​​of obstacle_l is taken as the left boundary of the vehicle left_bound; if the obstacle is on the right side of the vehicle, directly take the left reference line boundary left iAs the left boundary of the vehicle left_bound;

[0030] Step 3.2.2, based on the positional relationship between the obstacle and the vehicle, if the obstacle is on the right side of the vehicle or there are obstacles on both sides, take the right reference line boundary right i The maximum value between the two values ​​of obstacle_l is taken as the right boundary of the vehicle right_bound; if the obstacle is on the left side of the vehicle, directly take the right reference line boundary right i As the left boundary right_bound of the vehicle. The logic of the right boundary set after processing obstacles is shown in the following code:

[0031]

[0032] Furthermore, in step 3.1, if the obstacle is not given in the form of a box but in the form of an impassable point, assuming that the number of impassable points is N, it is only necessary to process the obstacle N times according to the number of impassable points.

[0033] Furthermore, the specific contents of step 4 are as follows:

[0034] Step 4.1, assume that the parameters of the tractor-trailer vehicle are as follows:

[0035] The minimum turning radius of the vehicle: min_turn_radius

[0036] Tractor width: tractor_width

[0037] Trailer width: trailer_width

[0038] Distance from the center of the tractor's rear axle to the front edge: tractor_front_edge_to_ego

[0039] Tractor rear overhang: tractor_rear_edge_to_ego

[0040] Distance from the center of the tractor's rear axle to the articulation point: tractor_ego_to_saddle

[0041] Distance from the center of the trailer's rear axle to the hinge point: trailer_ego_to_saddle

[0042] Trailer front overhang: trailer_front_overhang

[0043] Assume that the number of samples is n, according to the vehicle parameters in the sampling interval sample_range(0,ego_κ max), sampling is performed with sample_ratio as the sampling ratio, and the set of sampling results sample_result can be obtained as the product of the two; that is, sample_result = ego_κ max *sample_ratio

[0044] in:

[0045] i=1, 2, 3, ..., n;

[0046] The maximum curvature of the ego vehicle ego_κ max is the reciprocal of the minimum turning radius,

[0047] ego_κ max =1 / min_turn_radius;

[0048] The sampling ratio sample_ratio is the quotient of the sampling index and the total number of samples, sample_ratio=i / (n-1);

[0049] Step 4.2, based on the result obtained by sampling the maximum curvature of the tractor, for each sampled curvature κ, take the inverse to get the minimum turning radius radius of the ego vehicle. The turning radius radius_inner of the ego vehicle can be calculated by subtracting half of the tractor vehicle width tractor_width from the turning radius radius, and the turning radius radius_outer of the ego vehicle can be calculated by adding half of the tractor vehicle width tractor_width to the turning radius radius; that is, radius = 1 / κ

[0050] radius_inner=radius-0.5*tractor_width

[0051] radius_outer=radius+0.5*tractor_width;

[0052] Step 4.3, assuming that the turning centers of the tractor and trailer are the same, the turning radius difference front_outer_buffer of the front end of the tractor and the turning radius difference rear_outer_buffer of the rear end of the tractor calculated based on the actual vehicle parameters are shown in steps 4.3.1 and 4.3.2 respectively;

[0053] Step 4.3.1, the two right-angled sides are the distance from the center of the rear axle to the front end of the tractor tractor_front_edge_to_ego and the turning radius radius_outer of the outside of the tractor. Using the Pythagorean theorem, it is known that the above two right-angled sides are tractor_front_edge_to_ego and radius_outer. The length of the hypotenuse front_outer_buffer is the turning radius difference of the outer front end of the tractor; the turning radius difference of the outer front end of the tractor is shown in the following code formula:

[0054]

[0055] Step 4.3.2, the two right-angled sides are the rear overhang of the tractor tractor_rear_edge_to_ego and the turning radius radius_outer of the outer side of the tractor. Using the Pythagorean theorem, it is known that the two right-angled sides are tractor_rear_edge_to_ego and radius_outer, and the length of the hypotenuse rear_outer_buffer is the turning radius difference of the outer side of the rear end of the tractor; the turning radius difference of the outer side of the rear end of the tractor is shown in the following code formula:

[0056]

[0057] In order to consider the safety when turning, the largest of the two is taken as the outermost safety distance outer_buffer:

[0058] outer_buffer=max(front_outer_buffer,rear_outer_buffer)

[0059] Step 4.4, based on the vehicle parameters, obtain the minimum turning radius radius_trailer of the trailer, and further obtain the maximum angle max_articulation_angle formed by the tractor-trailer at the turning center and the maximum turning radius max_tractor_radius of the tractor, as shown below:

[0060] Step 4.4.1, according to the Pythagorean theorem, the hypotenuse is the turning radius radius_saddle of the hinge point, and the right angle side is the distance from the center of the rear axle of the trailer to the hinge point trailer_ego_to_saddle. Through the inverse sine function, the maximum angle formed by the trailer at the turning center is obtained as trailer_angle; as shown in the following code formula:

[0061] trailer_angle=asin(trailer_ego_to_saddle / radius_saddle)

[0062] Step 4.4.2, according to the Pythagorean theorem, the hypotenuse is the turning radius radius_saddle, and the right angle side is the distance from the center of the tractor's rear axle to the hinge point tractor_ego_to_saddle. Through the inverse sine function, the maximum angle formed by the tractor at the turning center can be obtained as tractor_angle; as shown in the following code formula:

[0063] tractor_angle=asin(tractor_ego_to_saddle / radius_saddle)

[0064] Step 4.4.3, finally the maximum angle max_articulation_angle formed by the tractor and trailer at the turning center is the difference between the trailer angle and the tractor angle; that is,

[0065] max_articulation_angle=trailer_angle-tractor_angle

[0066] Step 4.4.4, the two right-angled sides are the turning radius radius_saddle at the tractor-trailer hinge point and the distance tractor_ego_to_saddle from the center of the tractor's rear axle to the hinge point. The length of the hypotenuse can be calculated using the Pythagorean theorem, which is the maximum turning radius max_tractor_radius of the tractor; as shown in the following code formula:

[0067]

[0068] in:

[0069] (1) Turning radius outside the trailer:

[0070] tractor_ego_to_saddle=tractor_ego_to_saddle+0.5*trailer_width;

[0071] (2) Given that the two right-angled sides are the outermost turning radius of the trailer: radius_trailer_outer and the distance from the center of the tractor's rear axle to the hinge point tractor_ego_to_saddle, the Pythagorean theorem can be used to obtain the tractor-trailer hinge distance of the hypotenuse:

[0072] The turning radius at the joint is:

[0073]

[0074] Step 4.5, based on the vehicle parameters, further obtain the inner_buffer safety distance inside the trailer and the outer_buffer safety distance outside the trailer under each sampled curvature κ:

[0075] Step 4.5.1, first determine the turning radius radius obtained based on the curvature of the trajectory point and the maximum turning radius of the tractor max_tractor_radius:

[0076] Step 4.5.1.1, if radius is greater than max_tractor_radius, the two right-angled sides are known to be the turning radius radius_saddle at the tractor-trailer hinge point and the distance tractor_ego_to_saddle from the center of the tractor's rear axle to the hinge point. According to the Pythagorean theorem, the hypotenuse can be obtained, which is the turning radius radius_trailer of the trailer.

[0077] Step 4.5.1.2, if radius is less than or equal to max_tractor_radius, the hypotenuse is the turning radius radius_saddle of the hinge point, and the angle angle_saddle formed by the hinge point and the rear axle of the trailer at the turning center, the right angle side can be obtained according to the Pythagorean theorem, which is the turning radius radius_trailer of the trailer; the specific code formula is as follows:

[0078]

[0079] The angle angle_saddle formed by the articulation point and the rear axle of the trailer at the turning center is calculated by the arcsine of the quotient of the maximum angle max_articulation_angle formed by the tractor-trailer at the turning center plus the distance tractor_ego_to_saddle from the center of the tractor rear axle to the articulation point and the turning radius radius_saddle at the tractor-trailer articulation point; that is, angle_saddle = max_articulation_angle + arcsin (tractor_ego_to_saddle / radius_saddle);

[0080] Step 4.5.2, considering the half width of the trailer, the turning radius of the inner side of the trailer can be expressed as the turning radius radius_traile of the trailer minus the half width of the trailer, and the turning radius radius_traile of the outer side of the trailer can be expressed as the turning radius of the trailer plus the half width of the trailer;

[0081] That is radius_trailer_inner=radius_trailer-trailer_half_width;

[0082] radius_trailer_outer=radius_trailer+trailer_half_width;

[0083] Step 4.5.3, calculate again to get the safety distances trailer_inner_buffer and trailer_outer_buffer inside and outside the trailer:

[0084] Step 4.5.3.1, subtract the turning radius radius_trailer_inner on the inner side of the trailer from the turning radius radius_inner on the inner side of the vehicle to obtain the safety distance trailer_inner_buffer on the inner side of the trailer;

[0085] Step 4.5.3.2, the two right-angled sides are the turning radius radius_trailer_outer of the trailer's outer side and the distance trailer_lf from the center of the trailer's rear axle to the front end of the trailer. Given the above two right-angled sides, the hypotenuse is calculated using the Pythagorean theorem to obtain the trailer's outer safety distance trailer_outer_buffer;

[0086] The distance from the center of the rear axle to the front end of the trailer is calculated by adding the product of the turning radius radius_saddle at the tractor-trailer articulation point and the sine value of angle_saddle to obtain trailer_lf.

[0087] Step 4.5.4, through the above steps, finally obtain the safety distance inside the trailer and the safety distance outside the trailer; wherein the safety distance inside the trailer inner_buffer is the maximum value between the two values ​​of inner_buffer and the safety distance inside the trailer trailer_inner_buffer; the safety distance outside the trailer outer_buffer is the maximum value between the two values ​​of outer_buffer and the safety distance outside the trailer trailer_outer_buffer;

[0088] inner_buffer=max(inner_buffer,trailer_inner_buffer);

[0089] outer_buffer=max(outer_buffer,trailer_outer_buffer).

[0090] Furthermore, the specific content of step 5 is as follows:

[0091] Step 5.1: To further improve the passability and safety of the turn, the ego vehicle is allowed to move the boundary forward to the left or right before entering the turn, so that the vehicle tends to deviate from the reference line when entering the turn, which is more in line with normal driving habits. To achieve this effect, firstly, a mapping table between the curve radius and the advance longitudinal distance is established based on experience. Based on the known curve radius, the phase s of the ego vehicle's longitudinal distance that needs to be advanced is obtained by looking up the table. Assuming the advance longitudinal distance is ego_distance_s, then:

[0092] The longitudinal distance s′ of the ego vehicle after the phase advance is the current phase s of the ego vehicle in the Frenet coordinate system plus the advanced longitudinal distance ego_distance_s; that is, s′=s+ego_distance_s;

[0093] Step 5.2, calculate the boundaries on the left and right sides based on s′ after phase advance:

[0094] Step 5.2.1, when the curvature of the reference line is greater than or equal to 0, the new set of the left boundary can be updated as follows: each value on the current left boundary set plus the inner_buffer of the safety distance inside the trailer; otherwise, it is updated as follows: each value on the current left boundary set plus the outer_buffer of the safety distance outside the trailer; the specific code formula is as follows:

[0095] Left border collection:

[0096]

[0097] Step 5.2.2, when the curvature of the reference line is greater than or equal to 0, the new set of the right boundary can be updated as: each value on the current right boundary set plus the outer_buffer of the safety distance on the outside of the trailer; otherwise, it is updated as: each value on the current right boundary set plus the inner_buffer of the safety distance on the inside of the trailer. The specific code formula is as follows:

[0098] Right boundary collection:

[0099]

[0100] Furthermore, in step 4.4.4, the turning radius of the outer side of the trailer is the distance from the center of the rear axle of the tractor to the hinge point tractor_ego_to_saddle plus half of the trailer width trailer_width;

[0101] The two right-angled sides are known to be the outermost turning radius of the trailer: radius_trailer_outer and the distance from the center of the rear axle of the tractor to the hinge point tractor_ego_to_saddle, so the Pythagorean theorem can be used to obtain the turning radius radius_saddle at the tractor-trailer hinge point of the hypotenuse.

[0102] Compared with the prior art, the present invention has the following beneficial effects:

[0103] 1. Greater safety when cornering: By considering the minimum turning radius of the trailer and the width of both sides of the trailer, a phase advance method can be adopted to generate more reasonable left and right boundaries to ensure safety when cornering;

[0104] 2. More human-like when cornering: By using the phase advance method, the planned left and right boundaries can be moved before entering the corner, which is in line with the habits of human driving;

[0105] 3. Enhanced passability in narrow scenes: This method handles the left and right boundaries by considering the minimum turning radius of the tractor and trailer. At the same time, the boundaries are also processed when there are obstacles on the left and right. This can greatly ensure that the vehicle does not collide with obstacles and improve passability.

[0106] Therefore, the present invention has important technical value and application prospects in improving the stability, safety and passability of the automatic driving system. BRIEF DESCRIPTION OF THE DRAWINGS

[0107] Figure 1 This is a system architecture diagram of the invention. DETAILED DESCRIPTION

[0108] The technical solution of the present invention is further described in detail below in conjunction with the accompanying drawings:

[0109] This embodiment proposes a method for optimizing the trajectory of a cornering vehicle with a trailer. According to the vehicle status, reference line, obstacle status, and task management status, it is observed whether a planning module is needed to perform trajectory planning and trajectory optimization. If the upstream status is normal, the condition for the planning submodule-cornering boundary action module to work is that the curvature of the reference line where the vehicle is located changes from a straight line to a curve state, and the vehicle is also required to be a vehicle with a trailer. If all the above conditions are met, the module will perform special boundary processing logic before trajectory optimization. It should be noted that the change of the curvature of the reference line from a curve to a straight line does not fall within the scope of special processing of this submodule. Finally, the boundary after special processing of the submodule will also be sent to the planning module for optimization. Compared with the trajectory without processing by this submodule, the optimized trajectory is more in line with the habit of manual driving, and the adjacent lane will be used before entering the curve.

[0110] The specific steps are as follows:

[0111] Step 1: First observation, based on the vehicle status and its characteristics, reference lines, obstacle status and the scene the vehicle is in, determine whether it is necessary to optimize the trajectory of the vehicle with a trailer. The specific judgment is: whether the vehicle is in intelligent driving mode, whether the vehicle is a vehicle with a trailer, whether the vehicle is about to enter a curve, whether the vehicle generates a reference line, if it is a vehicle with a trailer, and the vehicle is in an intelligent driving state, and there is a curve in front of the vehicle, and there is already a reference line in the lane, then it is considered that the basic conditions for the operation of the algorithm are met, and then continue to judge whether there is a change in curvature of the reference line of the vehicle, and the change in curvature is large. The specific judgment standard is from the curvature of the straight line to the curvature of the curve. If there is a change, it is necessary to use the curve trajectory optimization of the vehicle with a trailer mentioned in this invention.

[0112] Step 2: The first step of the cornering trajectory optimization method. Based on the reference line ref_info(s i ,κ i ) and other information to obtain the initial input boundary init_bound(left i ,right i ),in:

[0113] i represents the index value of the point on the reference line;

[0114] s i Represents the longitudinal position of the i-th point on the reference line in the frenet coordinate system;

[0115] κ i Represents the curvature of the i-th point on the reference line;

[0116] left i Represents the coordinates of the i-th point on the left side of the reference line, and its set constitutes the input left boundary left;

[0117] right i Represents the coordinates of the i-th point on the right side of the reference line, and its set constitutes the input left boundary right;

[0118] Assuming that the semi-width of the road where the vehicle is located is l, the specific method of obtaining the input boundary is as follows:

[0119] left i = l

[0120] right i =-l

[0121] Step 3: Based on the initial input boundary init_bound in step 2, combined with the longitudinal distances obstacle_s and obstacle_l of the obstacle center in the frenet coordinate system, the input boundary obstacle_path_bound (left_bound i ,right_bound i ), the specific processing is as follows:

[0122] Step 3.1, assuming that the known obstacle ahead is given in the form of a box, the length of the obstacle is length, and the width is width. Considering the length and width of the obstacle, we have:

[0123] The longitudinal distance of the rear end of the obstacle vehicle in the Frenet coordinate system:

[0124] ob_lower_s=obstacle_s-0.5*length

[0125] The longitudinal distance of the front face of the obstacle vehicle in the Frenet coordinate system:

[0126] ob_upper_s=obstacle_s+0.5*length

[0127] in:

[0128] If the obstacle is not in the form of a box but in the form of an inaccessible point, this result still applies. You only need to process the obstacle in multiple loops.

[0129] Step 3.2, combined with the initial input boundary init_bound, the lateral distance obstacle_l and the longitudinal distance obstacle_s of the front and rear ends of the obstacle in the frenet coordinate system, update the input boundary obstacle_path_bound after processing the obstacle:

[0130] The left boundary set after handling obstacles:

[0131]

[0132] The right boundary set after handling obstacles:

[0133]

[0134] Step 4: Based on the obstacle-processing boundary obstacle_path_bound obtained in step 3, the turning radius of the trailer when turning and the leftmost and rightmost end faces of the trailer are considered to obtain the input boundary trailer_path_bound that takes into account the movement of the trailer. The specific method is as follows:

[0135] Step 4.1, assume that the parameters of the tractor-trailer vehicle are as follows:

[0136] The minimum turning radius of the vehicle: min_turn_radius

[0137] Tractor width: tractor_width

[0138] Trailer width: trailer_width

[0139] Distance from the center of the tractor's rear axle to the front edge: tractor_front_edge_to_ego

[0140] Tractor rear overhang: tractor_rear_edge_to_ego

[0141] Distance from the center of the tractor's rear axle to the articulation point: tractor_ego_to_saddle

[0142] Distance from the center of the trailer's rear axle to the hinge point: trailer_ego_to_saddle

[0143] Trailer front overhang: trailer_front_overhang

[0144] Assume that the number of samples is n, according to the vehicle parameters in the sampling interval sample_range(0,ego_κ max ), sampling is performed with sample_ratio as the sampling ratio, and the set of sampling results sample_result is:

[0145] sample_result = ego_κ max *sample_ratio

[0146] in:

[0147] i=1, 2, 3, ..., n;

[0148] The maximum curvature of the ego vehicle ego_κ max =1 / min_turn_radius;

[0149] Sampling ratio sample_ratio = i / (n-1);

[0150] Step 4.2, based on the result obtained by sampling the maximum curvature of the tractor, for each sampled curvature κ, obtain the minimum turning radius radius of the ego vehicle, the turning radius radius_inner of the inner side of the ego vehicle, and the turning radius radius_outer of the outer side of the ego vehicle:

[0151] radius=1 / κ

[0152] radius_inner=radius-0.5*tractor_width

[0153] radius_outer=radius+0.5*tractor_width

[0154] Step 4.3, assuming that the turning centers of the tractor and trailer are the same, the turning radius difference of the front outer side of the tractor calculated based on the actual vehicle parameters is front_outer_buffer and rear_outer_buffer respectively:

[0155]

[0156] In order to consider the safety when turning, the largest of the two is taken as the outermost safety distance outer_buffer:

[0157] outer_buffer=max(front_outer_buffer,rear_outer_buffer)

[0158] Step 4.4, based on the vehicle parameters, the minimum turning radius radius_trailer of the trailer is obtained, and the maximum angle max_articulation_angle formed by the tractor-trailer at the turning center and the maximum turning radius max_tractor_radius of the tractor can be further obtained as follows:

[0159] max_articulation_angle

[0160] =asin(trailer_ego_to_saddle / radius_saddle)-asin(tractor_ego_to_saddle / radius_saddle)

[0161]

[0162] in:

[0163] Turning radius outside the trailer:

[0164] tractor_ego_to_saddle=tractor_ego_to_saddle+0.5*trailer_width;

[0165] The turning radius at the tractor-trailer articulation point is:

[0166]

[0167] Step 4.5, based on the vehicle parameters, further obtain the inner_buffer safety distance inside the trailer and the outer_buffer safety distance outside the trailer under each sampled curvature κ:

[0168] Step 4.5.1, first calculate the turning radius radius_trailer of the trailer. It is necessary to distinguish the turning radius of the curve from that of the vehicle. The specific code formula is as follows:

[0169]

[0170] The angle between the hinge point and the rear axle of the trailer at the turning center is:

[0171] angle_saddle=max_articulation_angle+arcsin(tractor_ego_to_saddle / radius_saddle);

[0172] Step 4.5.2, considering the half width of the trailer, the turning radius inside and outside the trailer can be expressed as:

[0173] radius_trailer_inner=radius_trailer-trailer_half_width;

[0174] radius_trailer_outer=radius_trailer+trailer_half_width;

[0175] Step 4.5.3, calculate again the safety distances trailer_inner_buffer and trailer_outer_buffer inside and outside the trailer:

[0176] trailer_inner_buffer=radius_inner-radius_trailer_inner;

[0177]

[0178] The distance from the center of the trailer's rear axle to the front end of the trailer is:

[0179] trailer_lf=trailer_front_overhang+sin(angle_saddle)*radius_saddle;

[0180] Step 4.5.4, finally get the safety distance inside the trailer and the safety distance outside the trailer, take the maximum value:

[0181] inner_buffer=max(inner_buffer,trailer_inner_buffer);

[0182] outer_buffer=max(outer_buffer,trailer_outer_buffer);

[0183] Step 5: Based on the inner_buffer and outer_buffer safety distances of the trailer inside and outside under each sampled curvature κ obtained in step 4, find the curvature corresponding to s on the reference line based on the longitudinal distance s of the vehicle on the reference line, and then find the inner and outer safety distances calculated in d in step 4 based on the curvature, and add them to the input boundary obstacle_path_bound after processing the obstacle. The specific method for updating the boundary is as follows:

[0184] Step 5.1: To further improve the passability and safety of the turn, the ego vehicle can move the boundary forward to the left or right before entering the turn, so that the vehicle tends to deviate from the reference line when entering the turn, which is more in line with normal driving habits. To achieve this effect, the phase s of the longitudinal distance of the ego vehicle can be appropriately advanced based on the radius of the curve. Let the advanced longitudinal distance be ego_distance_s, then:

[0185] The longitudinal distance of the ego vehicle after phase advance s′=s+ego_distance_s;

[0186] Step 5.2, calculate the boundaries on the left and right sides based on s′ after phase advance:

[0187] Left border collection:

[0188]

[0189] Right boundary collection:

[0190]

[0191] Step 6: The fifth step of the cornering trajectory optimization method. According to the boundary finally obtained in step 5, it is input into the solver to obtain the final collision-free trajectory that takes into account the movement of the trailer when cornering. The above shows and describes the basic principles, main features and advantages of the present invention.

[0192] Those skilled in the art should understand that the present invention is not limited to the above specific embodiments. The above specific embodiments and the description in the specification are only for further illustrating the principle of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, which fall within the scope of the present invention to be protected. The scope of the present invention to be protected is defined by the claims and their equivalents.

Claims

1. A method for optimizing a turning trajectory of a trailer, characterized in that: The following steps are included: Step 1: Determine whether the vehicle needs to be optimized. If so, proceed to the next step. Step 2: Based on the reference line ref_info(s i ,κ i ) and other information to obtain the initial input boundary init_bound(left i ,right i ),in: i represents the index value of the point on the reference line; s i Represents the longitudinal position of the i-th point on the reference line in the frenet coordinate system; κ i Represents the curvature of the i-th point on the reference line; left i Represents the coordinates of the i-th point on the left side of the reference line, and its set constitutes the input left boundary left; right i Represents the coordinates of the i-th point on the right side of the reference line, and its set constitutes the input left boundary right; Assume that the half-width of the road where the vehicle is located is l, and the boundary is defined as positive on the left and negative on the right; then the left boundary left i That is the value of l, the right boundary right i is -l; Step 3: Based on the initial input boundary init_bound obtained in step 2, and combined with the longitudinal distances obstacle_s and obstacle_l of the obstacle center in the frenet coordinate system, the input boundary obstacle_path_bound (left_bound i ,right_bound i ); Step 4: Based on the obstacle_path_bound input boundary obtained in step 3 after processing the obstacle, combined with the turning radius of the trailer when turning and the leftmost end face and the rightmost end face of the trailer, the input boundary trailer_path_bound considering the movement of the trailer is obtained, and the inner_buffer safety distance inside the trailer and the outer_buffer safety distance outside the trailer are obtained by calculation; Step 5: According to the inner and outer safety distances of the trailer obtained in step 4, based on the longitudinal distance s of the vehicle on the reference line, find the curvature corresponding to s on the reference line, then find the inner and outer safety distances calculated in step 4 based on the curvature, and update them as the final input boundary obstacle_path_bound; Step 6: Based on the obstacle_path_bound obtained in step 5, input it into the solver to obtain the final collision-free trajectory that takes into account the movement of the trailer when turning.

2. The method for optimizing the turning trajectory of a trailer according to claim 1, characterized in that: In step 1, the vehicle is first observed for the first time, and it is determined whether trajectory optimization for the vehicle with trailer is required based on the vehicle state and its characteristics, reference lines, obstacle states, and the scene in which the vehicle is located.

3. The method for optimizing the turning trajectory of a trailer according to claim 2, characterized in that: First, determine whether the vehicle is in intelligent driving mode, whether the vehicle is a trailer vehicle, whether the vehicle is about to enter a curve, and whether the vehicle has generated a reference line. If it is a trailer vehicle, and the vehicle is in intelligent driving mode, and there is a curve in front of the vehicle, and there is already a reference line in the lane, then it is considered that the basic conditions for the operation of the algorithm are met. Then continue to determine whether there is a change in the curvature of the reference line of the vehicle's driving, and the change in curvature changes from the curvature of a straight line to the curvature of a curve. If the absolute value of the change exceeds 0.03, it is necessary to enter the cornering trajectory optimization.

4. The method for optimizing the turning trajectory of a trailer according to claim 1, characterized in that: The specific contents of step 3 are as follows: Step 3.1, assuming that the known obstacle ahead is given in the form of a box, the length of the obstacle is length, and the width is width. Combining the length and width of the obstacle, we have: The longitudinal distance of the rear end of the obstacle vehicle in the frenet coordinate system is the longitudinal distance of the obstacle in the frenet coordinate system, obstacle_s, minus half of the obstacle vehicle length; The longitudinal distance of the front end of the obstacle vehicle in the frenet coordinate system is the longitudinal distance of the obstacle in the frenet coordinate system obstacle_s plus half of the obstacle vehicle length; Step 3.2, combined with the initial input boundary init_bound, the lateral distance obstacle_l and the longitudinal distance obstacle_s of the front and rear ends of the obstacle in the frenet coordinate system, update the input boundary obstacle_path_bound after processing the obstacle: Step 3.2.1, based on the positional relationship between the obstacle and the vehicle, if the obstacle is on the left side of the vehicle or on both sides, take the left reference line boundary left i The minimum value between the two values ​​of obstacle_l is taken as the left boundary of the vehicle left_bound; if the obstacle is on the right side of the vehicle, directly take the left reference line boundary left i As the left boundary of the vehicle left_bound; Step 3.2.2, based on the positional relationship between the obstacle and the vehicle, if the obstacle is on the right side of the vehicle or there are obstacles on both sides, take the right reference line boundary right i The maximum value between the two values ​​of obstacle_l is taken as the right boundary of the vehicle right_bound; if the obstacle is on the left side of the vehicle, directly take the right reference line boundary right i As the left boundary of the vehicle right_bound.

5. The method for optimizing the turning trajectory of a trailer according to claim 4, characterized in that: In the step 3.1, if the obstacle is not given in the form of a box but in the form of an impassable point, assuming that the number of impassable points is N, it is only necessary to process the obstacle N times according to the number of impassable points.

6. The method for optimizing the turning trajectory of a trailer according to claim 1, characterized in that: The specific contents of step 4 are as follows: Step 4.1, assume that the parameters of the tractor-trailer vehicle are as follows: The minimum turning radius of the vehicle: min_turn_radius Tractor width: tractor_width Trailer width: trailer_width Distance from the center of the tractor's rear axle to the front edge: tractor_front_edge_to_ego Tractor rear overhang: tractor_rear_edge_to_ego Distance from the center of the tractor's rear axle to the articulation point: tractor_ego_to_saddle Distance from the center of the trailer's rear axle to the hinge point: trailer_ego_to_saddle Trailer front overhang: trailer_front_overhang Assume that the number of samples is n, according to the vehicle parameters in the sampling interval sample_range(0,ego_κ max ), sampling is performed with sample_ratio as the sampling ratio, and the set of sampling results sample_result can be obtained as the product of the two; in: i=1, 2, 3, ..., n; The maximum curvature of the ego vehicle ego_κ max It is the reciprocal of the minimum turning radius; The sampling ratio sample_ratio is the quotient of the sampling index and the total number of samples; Step 4.2, based on the result obtained by sampling the maximum curvature of the tractor, for each sampled curvature κ, take the inverse to get the minimum turning radius radius of the ego vehicle. The turning radius radius_inner of the ego vehicle can be calculated by subtracting half of the tractor vehicle width tractor_width from the turning radius radius, and the turning radius radius_outer of the ego vehicle can be calculated by adding half of the tractor vehicle width tractor_width to the turning radius radius. Step 4.3, assuming that the turning centers of the tractor and trailer are the same, the turning radius difference front_outer_buffer of the front end of the tractor and the turning radius difference rear_outer_buffer of the rear end of the tractor calculated based on the actual vehicle parameters are shown in steps 4.3.1 and 4.3.2 respectively; Step 4.3.1, the two right-angled sides are the distance from the center of the rear axle to the front end of the tractor tractor_front_edge_to_ego and the turning radius radius_outer of the outer side of the tractor. Using the Pythagorean theorem, the two right-angled sides are known to be tractor_front_edge_to_ego and radius_outer. The length of the hypotenuse front_outer_buffer is the turning radius difference of the outer side of the front end of the tractor. Step 4.3.2, the two right-angled sides are the rear overhang of the tractor tractor_rear_edge_to_ego and the turning radius radius_outer of the outer side of the tractor. Using the Pythagorean theorem, it is known that the two right-angled sides are tractor_rear_edge_to_ego and radius_outer, and the length of the hypotenuse rear_outer_buffer is the turning radius difference of the outer side of the rear end of the tractor; In order to consider the safety when turning, the largest of the two is taken as the outermost safety distance outer_buffer; Step 4.4, based on the vehicle parameters, obtain the minimum turning radius radius_trailer of the trailer, and further obtain the maximum angle max_articulation_angle formed by the tractor-trailer at the turning center and the maximum turning radius max_tractor_radious of the tractor, as shown below: Step 4.4.1, according to the Pythagorean theorem, the hypotenuse is the turning radius radius_saddle of the hinge point, and the right angle side is the distance from the rear axle center of the trailer to the hinge point trailer_ego_to_saddle. Through the inverse sine function, the maximum angle formed by the trailer at the turning center is obtained as trailer_angle; Step 4.4.2, according to the Pythagorean theorem, the hypotenuse is the turning radius radius_saddle, and the right angle side is the distance from the center of the tractor's rear axle to the hinge point tractor_ego_to_saddle. Through the inverse sine function, the maximum angle formed by the tractor at the turning center can be obtained as tractor_angle; Step 4.4.3, finally, the maximum angle max_articulation_angle formed by the tractor and trailer at the turning center is obtained as the difference between the trailer angle and the tractor angle; Step 4.4.4, the two right-angled sides are the turning radius radius_saddle at the tractor-trailer hinge point and the distance tractor_ego_to_saddle from the center of the tractor's rear axle to the hinge point. The length of the hypotenuse can be calculated using the Pythagorean theorem, which is the maximum turning radius max_tractor_radius of the tractor. Step 4.5, according to the vehicle parameters, further obtain the inner_buffer safety distance of the inner side of the trailer and the outer_buffer safety distance of the outer side of the trailer under each sampled curvature κ; Step 4.5.1, first determine the turning radius radius obtained according to the curvature on the trajectory point and the maximum turning radius of the tractor max_tractor_radius; Step 4.5.1.1, if radius is greater than max_tractor_radius, the two right-angled sides are known to be the turning radius radius_saddle at the tractor-trailer hinge point and the distance tractor_ego_to_saddle from the center of the tractor's rear axle to the hinge point. According to the Pythagorean theorem, the hypotenuse can be obtained, which is the turning radius radius_trailer of the trailer. Step 4.5.1.2, if radius is less than or equal to max_tractor_radius, the hypotenuse is the turning radius radius_saddle of the hinge point, and the angle angle_saddle formed by the hinge point and the rear axle of the trailer at the turning center, the right angle side can be calculated according to the Pythagorean theorem, which is the turning radius radius_trailer of the trailer; The angle angle_saddle formed by the articulation point and the rear axle of the trailer at the turning center is calculated by the arc sine of the quotient of the maximum angle max_articulation_angle formed by the tractor-trailer at the turning center plus the distance tractor_ego_to_saddle from the center of the tractor's rear axle to the articulation point and the turning radius radius_saddle at the tractor-trailer articulation point; Step 4.5.2, considering the half width of the trailer, the turning radius of the inner side of the trailer can be expressed as the turning radius radius_traile of the trailer minus the half width of the trailer, and the turning radius radius_traile of the outer side of the trailer can be expressed as the turning radius of the trailer plus the half width of the trailer; Step 4.5.3, calculate again to obtain the safety distances trailer_inner_buffer and trailer_outer_buffer inside and outside the trailer; Step 4.5.3.1, subtract the turning radius radius_trailer_inner on the inner side of the trailer from the turning radius radius_inner on the inner side of the vehicle to obtain the safety distance trailer_inner_buffer on the inner side of the trailer; Step 4.5.3.2, the two right-angled sides are the turning radius radius_trailer_outer of the trailer's outer side and the distance trailer_lf from the center of the trailer's rear axle to the front end of the trailer. Given the above two right-angled sides, the hypotenuse is calculated using the Pythagorean theorem to obtain the trailer's outer safety distance trailer_outer_buffer; The distance from the center of the rear axle to the front end of the trailer is calculated by adding the product of the turning radius radius_saddle at the tractor-trailer articulation point and the sine value of angle_saddle to obtain trailer_lf. Step 4.5.4, through the above steps, finally obtain the safety distance inside the trailer and the safety distance outside the trailer; the safety distance inside the trailer inner_buffer is the maximum value between the two values ​​of inner_buffer and the safety distance inside the trailer trailer_inner_buffer; the safety distance outside the trailer outer_buffer is the maximum value between the two values ​​of outer_buffer and the safety distance outside the trailer trailer_outer_buffer.

7. The method for optimizing the turning trajectory of a trailer according to claim 1, characterized in that: The specific contents of step 5 are as follows: Step 5.1: In order to further improve the passability and safety of the turn, the ego vehicle is allowed to advance the boundary to the left / right before entering the turn, so that the vehicle tends to deviate from the reference line when entering the turn, which is more in line with normal driving habits. In order to achieve this effect, a mapping table between the curve radius and the advance longitudinal distance is first established based on experience. Based on the known curve radius, the phase s of the ego vehicle's longitudinal distance that needs to be advanced is obtained by looking up the table. Let the advanced longitudinal distance be ego_distance_s, then: the ego vehicle's longitudinal distance s′ after the phase advance is the current phase s of the ego vehicle in the frenet coordinate system plus the advanced longitudinal distance ego_distance_s; Step 5.2, calculating the boundaries of the left and right sides based on s′ after phase advance; Step 5.2.1, when the curvature of the reference line is greater than or equal to 0, the new set of the left boundary can be updated as: each value on the current left boundary set plus the inner_buffer of the safety distance inside the trailer; otherwise, it is updated as: each value on the current left boundary set plus the outer_buffer of the safety distance outside the trailer; Step 5.2.2, when the curvature of the reference line is greater than or equal to 0, the new set of the right boundary can be updated as: each value on the current right boundary set plus the safety distance outer_buffer on the outside of the trailer; otherwise, it is updated as: each value on the current right boundary set plus the safety distance inner_buffer on the inside of the trailer.

8. The method for optimizing the turning trajectory of a trailer according to claim 7, characterized in that: In step 4.4.4, the turning radius of the outer side of the trailer is the distance from the center of the rear axle of the tractor to the hinge point tractor_ego_to_saddle plus half of the trailer width trailer_width; The two right-angled sides are known to be the outermost turning radius of the trailer: radius_trailer_outer and the distance from the center of the rear axle of the tractor to the hinge point tractor_ego_to_saddle, so the Pythagorean theorem can be used to obtain the turning radius radius_saddle at the tractor-trailer hinge point of the hypotenuse.

Images