A vehicle trajectory planning method fusing fast collision detection and rollover prevention

Abstract

The application discloses a vehicle trajectory planning method integrating rapid collision detection and rollover prevention, comprising the following steps: S1, a rapid collision detection strategy is designed; S2, planning setting conditions are acquired, a path planning algorithm with combined bias sampling is designed, the rapid collision detection strategy is called in the path planning algorithm, and a reference path without collision is planned; S3, vehicle dynamics parameters are acquired, and rollover prevention dynamics constraints are solved; S4, a smoothing strategy is designed in combination with the rollover prevention dynamics constraints and intelligent vehicle actuator constraints; and S5, the smoothing strategy is used to perform smoothing processing on the reference path, so that a smooth trajectory of the vehicle is obtained. The application can solve the problems of the prior art, such as the need for large-scale numerical solution operation, the incapability of realizing stable obstacle avoidance in real time, the easy occurrence of false detection and missed detection, and high development cost, and integrates the collision detection and rollover prevention technologies, thereby improving the safety and reliability of vehicle trajectory planning.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automobiles, in particular to a vehicle trajectory planning method integrating rapid collision detection and rollover prevention. BACKGROUND

[0002] With the development of the automobile industry, it is necessary to promote the deep integration of intelligentization, electrification and networking of automobiles, help the automobile industry to transform from mechanization to electrification and intelligentization, and reduce traffic accidents such as collisions and rollovers caused by human errors. Therefore, realizing collision detection and rollover prevention is the focus of the research and development of core technologies such as intelligent vehicle trajectory planning.

[0003] Although current collision detection technologies such as inner angle and area method have been developed and widely applied, the existing collision detection methods often need to perform large-scale numerical solution operations, cannot realize stable obstacle avoidance in real time, are prone to false detection and missed detection, and have high development costs. In addition, there are few reports on the research of rollover prevention technology in trajectory planning, which is a major challenge to the safety and reliability of intelligent vehicles. SUMMARY

[0004] In view of this, the present application provides a vehicle trajectory planning method integrating rapid collision detection and rollover prevention to solve the problems of the prior art, such as the need for large-scale numerical solution operations, the inability to realize stable obstacle avoidance in real time, the tendency to false detection and missed detection, and high development costs. The present application integrates collision detection and rollover prevention technology to improve the safety and reliability of vehicle trajectory planning.

[0005] A vehicle trajectory planning method integrating rapid collision detection and rollover prevention, comprising:

[0006] Step S1, designing a rapid collision detection strategy: based on the coordinates of the two end points of the given path segment to be detected, the coordinates of the four vertices of the obstacle, and the set safety distance , performing primary collision detection, if at least one of the two end points of the path segment to be detected is inside the quadrilateral formed by the coordinates of the four vertices of the obstacle, it is determined that there is a collision, and the detection is directly ended, otherwise, the projection point P of the path segment to be detected to the center point O of the obstacle is calculated, and the nearest vertex of the four vertices of the obstacle to the projection point P is defined as point M, and the coordinates of the auxiliary point Q are calculated based on M, then secondary collision detection is performed to judge the distance from O to P minus the safety distance After, whether still greater than O to Q distance, if yes, it is judged that there is no collision, and directly end detection, if not, the final collision detection is executed, if M is collinear with the two end points of the path segment to be detected, and M is between the two end points of the path segment to be detected, it is judged that there is collision; if M is collinear with the two end points of the path segment to be detected, and M is not between the two end points of the path segment to be detected, it is judged that there is no collision; if M is not collinear with the two end points of the path segment to be detected, and the path segment to be detected passes through the obstacle, it is judged that there is collision; if M is not collinear with the two end points of the path segment to be detected, and the path segment to be detected does not pass through the obstacle, it is judged that there is no collision;

[0007] Step S2, obtain the planning setting condition, design the path planning algorithm combined with the biased sampling, call the fast collision detection strategy in the path planning algorithm, and plan out the reference path without collision;

[0008] Step S3, obtain the vehicle dynamics parameters, and solve the anti-rollover dynamics constraint;

[0009] Step S4, combine the anti-rollover dynamics constraint and the intelligent vehicle actuator constraint, and design the smoothing strategy;

[0010] Step S5, adopt the smoothing strategy to perform smoothing processing on the reference path, and obtain the smooth trajectory of the vehicle.

[0011] The vehicle trajectory planning method fusing fast collision detection and anti-rollover provided by the application has the following beneficial effects:

[0012] (1) The strict geometric relationship of vehicle collision is designed and derived in the application, and the fast collision detection strategy is designed based on the strict geometric relationship, and the collision detection is performed, so that the false detection and the missed detection can be effectively reduced, and the safety of the intelligent vehicle is significantly improved.

[0013] (2) The anti-rollover dynamics constraint is designed in the trajectory planning, the anti-rollover dynamics constraint and the intelligent vehicle actuator constraint are combined, and the smoothing strategy is designed, so that the rollover instability phenomenon of the vehicle tracking trajectory can be effectively prevented, and the safety and reliability of the vehicle trajectory planning are significantly improved.

[0014] (3) The collision detection and the anti-rollover method designed in the application are both in the form of analytical solution, without large-scale numerical solution operation, so that the stable obstacle avoidance can be realized in real time, and the safety and reliability of the intelligent vehicle are significantly improved.

[0015] (4) The collision detection and the anti-rollover method designed in the application are both lightweight calculation, without relying on large-scale parallel calculation, so that the vehicle development cost can be significantly reduced. BRIEF DESCRIPTION OF DRAWINGS

[0016] Figure 1A flowchart of a vehicle trajectory planning method combining fast collision detection and rollover prevention provided by an embodiment of the present application is shown in the figure.

[0017] Figure 2 A case diagram for primary collision detection is shown in the figure.

[0018] Figure 3 A case diagram for secondary collision detection is shown in the figure.

[0019] Figure 4 A case diagram for when the condition of the tertiary collision detection is true is shown in the figure.

[0020] Figure 5 A case diagram for when the condition of the tertiary collision detection is true is shown in the figure.

[0021] Figure 6 A case diagram for when the condition of the tertiary collision detection is true is shown in the figure.

[0022] Figure 7 A case diagram for when the condition of the tertiary collision detection is true is shown in the figure.

[0023] Figure 8 A traditional obstacle avoidance path without using rollover prevention planning is shown in the figure.

[0024] Figure 9 An obstacle avoidance path planned by the method of the present application is shown in the figure. DETAILED DESCRIPTION

[0025] Embodiments of the present application are described in detail below with reference to the accompanying drawings, in which the same or similar components have the same or similar reference numbers throughout. The embodiments described below by way of example with reference to the accompanying drawings are illustrative and are not to be construed as limiting the present application.

[0026] Referring to Figure 1 The vehicle trajectory planning method combining fast collision detection and rollover prevention provided by the present application comprises steps S1 to S5:

[0027] Step S1: design a fast collision detection strategy.

[0028] The fast collision detection strategy is: based on the coordinates of the two end points of a given path segment to be detected, the coordinates of the four vertices of an obstacle, and a set safety distance , primary collision detection is performed, if at least one of the two end points of the path line segment to be detected is inside the quadrilateral formed by the coordinates of the four vertices of the obstacle, it is determined that there is a collision, and the detection is directly ended, otherwise, the projection point P of the path line segment to be detected to the center point O of the obstacle is calculated, and the nearest vertex of the four vertices of the obstacle to the projection point P is defined as point M, and the coordinates of the auxiliary point Q are calculated based on M, and then secondary collision detection is performed, the distance from O to P is subtracted by the safety distance , if yes, it is determined that there is no collision, and the detection is directly ended, if not, tertiary collision detection is performed, if M is collinear with the two end points of the path line segment to be detected, and M is between the two end points of the path line segment to be detected, it is determined that there is a collision; if M is collinear with the two end points of the path line segment to be detected, and M is not between the two end points of the path line segment to be detected, it is determined that there is no collision; if M is not collinear with the two end points of the path line segment to be detected, and the path line segment to be detected passes through the obstacle, it is determined that there is a collision; if M is not collinear with the two end points of the path line segment to be detected, and the path line segment to be detected does not pass through the obstacle, it is determined that there is no collision.

[0029] Specifically, the two end points of the path line segment to be detected are set as end point E and end point F, and the four vertices of the obstacle are A, B, C and D,

[0030] The formula of the primary collision detection is:

[0031]

[0032]

[0033]

[0034]

[0035]

[0036] wherein, is the coordinate of the end point E of the path line segment to be detected, , , , the coordinates of the four vertices A, B, C and D of the obstacle respectively, is the angle composed of the two rays EA and EB drawn from point E, is the angle composed of the two rays EA and ED drawn from point E, is the angle composed of the two rays EB and EC drawn from point E, is the angle composed of the two rays EC and ED drawn from point E.

[0037] When When it is true, it means that the end point E of the path line segment to be detected is inside the quadrangle surrounded by the coordinates of the four vertices of the obstacle, as shown in the following figure. Figure 2

[0038] When it is not true, it means that the end point E of the path line segment to be detected is not inside the quadrangle surrounded by the coordinates of the four vertices of the obstacle.

[0039] Similarly, by replacing the end point E with the end point F, it can be determined whether the end point F is inside the quadrangle surrounded by the coordinates of the four vertices of the obstacle.

[0040] If at least one of the two end points of the path line segment to be detected is inside the quadrangle surrounded by the coordinates of the four vertices of the obstacle, it is determined that there is a collision, and the detection is directly ended. Otherwise, if neither of the two end points of the path line segment to be detected is inside the quadrangle surrounded by the coordinates of the four vertices of the obstacle, the projection point P of the path line segment to be detected to the center point O of the obstacle is calculated, and the process satisfies the following formula:

[0041]

[0042]

[0043]

[0044]

[0045]

[0046] wherein, is the coordinate of the projection point P, is the coordinate of the end point F of the path line segment to be detected, is the projection coefficient, represents taking the maximum value, represents taking the minimum value, is the coordinate of the center point O of the obstacle;

[0047] In the process of calculating the coordinate of the auxiliary point Q based on M, the following trigonometric relationships are calculated first:

[0048]

[0049]

[0050] wherein, is the angle composed of the two rays OP and OM from the point O, is the angle composed of the two rays PO and PM from the point P, is the coordinate of the point M. ​​

[0051] When the coordinates of the auxiliary point Q are :

[0052]

[0053]

[0054] When the coordinates of the auxiliary point Q are :

[0055]

[0056]

[0057] The formula of the secondary collision detection is:

[0058] ;

[0059] If the formula of the secondary collision detection is established, it is determined that there is no collision, and the detection is directly ended, as shown in Figure 3 .

[0060] If the formula of the secondary collision detection is not established, that is, the distance from O to P minus the safety distance is not greater than the distance from O to Q, the final collision detection is performed.

[0061] In the process of the final collision detection, it is determined whether the conditional expression one and the conditional expression two are established, the expression of the conditional expression one is:

[0062] ;

[0063] The expression of the conditional expression two is:

[0064] ;

[0065] When the conditional expression one is established, it indicates that M is collinear with the two end points of the path segment to be detected, and M is between the two end points of the path segment to be detected, and it is determined that there is a collision, as shown in Figure 4 .

[0066] When the conditional expression two is established, it indicates that M is collinear with the two end points of the path segment to be detected, and M is not between the two end points of the path segment to be detected, and it is determined that there is no collision, as shown in Figure 5 .

[0067] When the conditional expression one is not established, or the conditional expression two is not established, it indicates that M is not collinear with the two end points of the path segment to be detected, and it is further determined whether the conditional expression three is established, the expression of the conditional expression three is:

[0068]

[0069]

[0070] wherein, is the coordinate of the previous vertex of M in the four vertices of the obstacle , is the coordinate of the next vertex of M in the four vertices of the obstacle , is the angle formed by the two rays ME and MF drawn from point M, is the angle formed by the two rays and drawn from point M.

[0071] When condition three is true, it indicates that the path segment to be detected passes through the obstacle, and it is determined that there is a collision, as shown in Figure 6 . When condition three is not true, it indicates that the path segment to be detected does not pass through the obstacle, and it is determined that there is no collision, as shown in Figure 7 .

[0072] Step S2, obtain the planning setting conditions, design a path planning algorithm with joint bias sampling, call the fast collision detection strategy in the path planning algorithm, and plan a collision-free reference path.

[0073] wherein, step S2 specifically comprises:

[0074] obtain the planning setting conditions, including the initial node n init , the corresponding coordinates of the initial node n init , , the target node n goal , the corresponding coordinates of the target node n goal , , the maximum iteration number iter_max of planning, the iteration radius R i , the target radius R g , the safety radius R s , the distance ratio dist_ratio and the obstacle set obsList, based on the above setting conditions, design the following flow path planning algorithm:

[0075] 1) Algorithm initialization:

[0076] initialize the random tree T with the initial node n init as the root node, and initialize the parent node index list parent, set the starting point parent node index to -1, and calculate the maximum safety neighborhood radius maxR, that is, the distance from the target node to the nearest obstacle boundary point in the obstacle set, the formula is:

[0077]

[0078] wherein, is the number of obstacles, is the coordinate of the i-th vertex of the j-th obstacle;

[0079] 2) Iterative search and node expansion:

[0080] The following operations are iteratively performed until the target node n goal is found or the maximum number of iterations iter_max is reached:

[0081] 2.1) Generate a random sampling point: generate a random sampling point n rand within the search space, whose corresponding coordinate is rand . .

[0082] 2.2) Find the nearest node of the random sampling point n rand in the random tree T: traverse the random tree T, calculate the Euclidean distance between each node and n rand , and select the node with the smallest Euclidean distance as the nearest node n near from the random tree T, where the k-th node has the Euclidean distance d rand between it and n k , and the calculation formula is:

[0083]

[0084] wherein, is the coordinate corresponding to the k-th node, k ∈ [1, K], and K is the number of nodes in the random tree T;

[0085] 2.3) Collision detection: call the fast collision detection strategy designed in step S1 to determine whether the line L rand connecting n near and n rand-near collides, where the line L rand connecting n near and n rand-near is regarded as the path line segment to be detected, n rand and n near are regarded as the end points E and F in step S1, and each obstacle in the obstacle set obsList is subjected to collision detection with L rand-near . If the collision detection between the line L rand-near and any obstacle in the obstacle set obsList shows collision, the process returns to the step of generating a random sampling point and a new iteration is started. If the collision detection between the line L rand-near and all obstacles in the obstacle set obsList shows no collision, the process continues.

[0086] 2.4) Target connection judgment: Calculate the distance Rt from n rand to n goal using the following formula:

[0087]

[0088] If Rt ≤ R g , add n goal to the random tree T, list n near as the parent node of n goal in the parent node index list parent, and mark the search as successful and end the iteration; if R g <Rt < maxR, first add n rand to the random tree T, list n near as the parent node of n rand , then add n goal to the random tree T, list n rand as the parent node of n goal , and mark the search as successful and end the iteration; if Rt ≥ maxR, continue to execute;

[0089] 2.5) Generation of initial new node: Control the expansion step size according to the distance ratio dist_ratio to generate the initial new node n new using the following formula:

[0090]

[0091] where L is the expansion step size, L = dist_ratio × d near-rand , d near-rand is the distance between n near and n rand ;

[0092] 2.6) Adaptive heuristic adjustment: Calculate the distance D1 from the nearest node n near to the target node n goal and the distance D2 from the initial new node n new to the nearest node n near using the following formula:

[0093]

[0094]

[0095] where, is the coordinate corresponding to the initial new node n new , For the latest node n near The corresponding coordinates;

[0096] Calculate the unit heuristic vector from n near to n goal :

[0097]

[0098] Let the distance gain coefficient be p, according to the size relationship between D1 and p x D2, dynamically adjust the heuristic function , the expression is:

[0099]

[0100] Then generate an adaptive heuristic new node HA new , HA new =n new +H, call the fast collision detection strategy to verify whether the connection line of HA new and n new has collision, if not, let n new =HA new ;

[0101] 2.7) Neighborhood optimization: calculate the distance of n new from all nodes in the random tree T, and screen out the neighborhood node set n i with a distance ≤R near-set , for each neighborhood node, calculate the sum of the distance from the root node to the neighborhood node and the distance from the neighborhood node to n new , select the node with the smallest distance sum and no collision connection line as the parent node of n new , update the parent;

[0102] 2.8) Target accessibility judgment: calculate the distance d new from n goal to n new-goal , if d new-goal ≤Rg or d new-goal ≤maxR, add n goal to the random tree T, list n new as the parent node of n goal in the parent node index list parent, and mark the search success and end the iteration; if d new-goal >Rg and d new-goal >maxR, add n new to the random tree T, then return to the step of generating random sampling points and start a new iteration.

[0103] 3) Path generation:

[0104] ​When the search is successful, the original path is obtained by backtracking from n goal to n init through the parent index list parent, and then the node order is reversed to obtain the collision-free reference path from n init to n goal .

[0105] Step S3, obtain the vehicle dynamics parameters, and solve the anti-rollover dynamics constraint.

[0106] The vehicle rollover process can be simply and intuitively described as follows: one side of the wheel starts to leave the ground, and the wheel off-the-ground distance increases over time. Once the vehicle center of mass crosses the wheel grounding center on the side without the wheel off-the-ground, the vehicle will irreversibly roll over. Therefore, an effective method to prevent vehicle rollover is to avoid one side of the wheel off-the-ground, that is, the following formula must be met:

[0107]

[0108] wherein, is the vertical force of the right side wheel of the vehicle, is the vertical force of the left side wheel of the intelligent vehicle.

[0109] For wheel lateral force calculation, three reasonable assumptions are made: (1) the vehicle unsprung mass is much smaller than the sprung mass; (2) the vehicle unsprung mass center height is very low; (3) the vehicle roll center height is much smaller than the roll radius. Combined with the two-degree-of-freedom single-track vehicle dynamics model, the formula of the equivalent model is as follows:

[0110]

[0111] wherein, is the height of the vehicle center of mass off the ground under static state, is the wheelbase of the vehicle, is the roll stiffness of the vehicle, is the front wheel steering angle of the vehicle, is the sprung mass of the vehicle, is the gravitational acceleration, is the longitudinal vehicle speed.

[0112] Further, when the intelligent vehicle key dynamics parameters such as the sprung mass, the earth's gravitational acceleration, and the longitudinal vehicle speed are measured by the vehicle-mounted sensor, the expression of the anti-rollover dynamics constraint can be derived as follows:

[0113]

[0114] Step S4, combine the anti-rollover dynamics constraint and the intelligent vehicle actuator constraint to design a smoothing strategy.

[0115] Where, since the installation of steering, driving, braking actuators on the vehicle can not output infinite or infinitesimal force, that is, the physical constraints of the vehicle actuators, the expression of the intelligent vehicle actuator constraints is:

[0116]

[0117]

[0118]

[0119]

[0120] Wherein, is the front wheel steering angle control variable, , is the maximum value and the minimum value of the front wheel steering angle control variable, is the first derivative of the front wheel steering angle control variable, , is the maximum value and the minimum value of the first derivative of the front wheel steering angle control variable, is the front wheel speed control variable, , is the maximum value and the minimum value of the front wheel speed control variable, is the front wheel acceleration control variable, , is the maximum value and the minimum value of the front wheel acceleration control variable;

[0121] Joint the above formula, further get the steering constraint under the joint constraint of rollover prevention and actuator is:

[0122] .

[0123] The , , and the above constraints are used as the smoothing constraints of the smoothing strategy.

[0124] Step S5, using the smoothing strategy, smoothing the reference path to obtain the smooth trajectory of the vehicle.

[0125] Wherein, the kinematic model of the vehicle is as follows:

[0126]

[0127] Combined with the smoothing strategy of the physical constraint, the smoothing equation can be obtained as:

[0128]

[0129] Wherein, a first derivative of the displacement control quantity of the vehicle in the x-axis direction, an angle between a forward direction of the vehicle and the x-axis, a first derivative of the displacement control quantity of the vehicle in the y-axis direction, a first derivative of a first derivative of a vehicle length control quantity, a front wheel steering angle of the vehicle, a first derivative of a first derivative of a first derivative of a first derivative of a front wheel steering angle velocity control quantity of the vehicle.

[0130] Based on the smoothing equation, a pure tracking control method is used to track the reference path planned in step S2, and a smooth trajectory of the vehicle anti-rollover is output.

[0131] Simulation test: set a 3-lane, 4-obstacle obstacle avoidance scene, and generate obstacle avoidance paths respectively by using the traditional method without anti-rollover planning and the method of the present application, as shown in Figure 8 and Figure 9 The starting point P0 of the planning is set at the position of (0, -1.75), and the end point P g is set at the position of (55, -1.75), Figure 8 is an obstacle avoidance path generated by the traditional method without anti-rollover planning, Figure 9 is an obstacle avoidance path generated by the method of the present application. As can be seen from Figure 8 , the traditional method has too large a turning change and has a rollover risk, and as can be seen from Figure 9 , the path of the method of the present application is smoother and has a smaller turning change, which shows that the method of the present application can significantly reduce the rollover risk during obstacle avoidance planning.

[0132] In summary, according to the vehicle trajectory planning method of the fusion of rapid collision detection and anti-rollover according to the above embodiment, the following beneficial effects are obtained:

[0133] (1) The present application designs and deduces the strict geometric relationship of vehicle collision, and designs a rapid collision detection strategy based on this, which can effectively reduce false detection and missed detection, and significantly improve the safety of intelligent vehicles.

[0134] (2) The present application designs an anti-rollover dynamics constraint in trajectory planning, combines the anti-rollover dynamics constraint and the intelligent vehicle actuator constraint, and designs a smoothing strategy, which can effectively prevent the vehicle from following the trajectory and appearing rollover instability phenomenon, and significantly improves the safety and reliability of the vehicle trajectory planning.

[0135] (3) The collision detection and rollover prevention method designed by the application adopts an analytical solution, does not need large-scale numerical solution operation, can realize stable obstacle avoidance in real time, and significantly improves the safety and reliability of the intelligent vehicle.

[0136] (4) The collision detection and rollover prevention method designed by the application is lightweight calculation, does not need to rely on large-scale parallel calculation, and can significantly reduce the vehicle development cost.

[0137] The above-mentioned embodiments only express several embodiments of the application, and the description is more specific and detailed, but it cannot be understood as a limitation on the scope of the patent. It should be pointed out that for ordinary skilled persons in the art, without departing from the concept of the application, some modifications and improvements can be made, which belong to the protection scope of the application. Therefore, the protection scope of the patent of the application should be subject to the appended claims.

Claims

1. A vehicle trajectory planning method fusing fast collision detection and rollover prevention, characterized in that, Comprise: Step S1, design a fast collision detection strategy: based on the given to be detected path line segment of two end point coordinates, the coordinates of the four vertices of the obstacle and the set of safety distance , the primary collision detection, if the two end points of the to-be-detected path line segment at least one end point in the four vertices of the coordinates of the obstacle four vertices of the quadrilateral inside, then determine that there is a collision, and directly end the detection, otherwise, continue to calculate the projection point P of the to-be-detected path line segment to the center point O of the obstacle, and define the projection point P to the nearest vertex of the four vertices of the obstacle as point M, and calculate the coordinates of the auxiliary point Q based on M, then perform secondary collision detection, judge the distance from O to P minus the safety distance , whether it is still greater than the distance from O to Q, if yes, it is determined that there is no collision, and the detection is directly ended, if not, the final collision detection is executed, if M is collinear with the two end points of the to-be-detected path line segment, and M is between the two end points of the to-be-detected path line segment, then it is determined that there is a collision; if M is collinear with the two end points of the to-be-detected path line segment, and M is not between the two end points of the to-be-detected path line segment, then it is determined that there is no collision; if M is not collinear with the two end points of the to-be-detected path line segment, and the to-be-detected path line segment passes through the obstacle, then it is determined that there is a collision; if M is not collinear with the two end points of the to-be-detected path line segment, and the to-be-detected path line segment does not pass through the obstacle, then it is determined that there is no collision; Step S2, obtain planning setting conditions, design joint bias sampling path planning algorithm, call fast collision detection strategy in path planning algorithm, plan out collision-free reference path; Step S3, obtain vehicle dynamics parameters, solve anti-rollover dynamics constraints; Step S4, joint anti-rollover dynamics constraints and intelligent vehicle actuator constraints, design smoothing strategy; Step S5, using smoothing strategy, smoothing processing reference path, get vehicle smooth trajectory; In step S1, the formula of primary collision detection is: wherein are the coordinates of the end point E of the path segment to be detected, , , , are the coordinates of the four vertices A, B, C, D of the obstacle, is the angle formed by the two rays EA and EB emanating from the point E, is the angle formed by the two rays EA and ED emanating from the point E, is the angle formed by the two rays EB and EC emanating from the point E, is the angle formed by the two rays EC and ED emanating from the point E; When is true, it means that the end point E of the path segment to be detected is inside the quadrilateral whose four vertices are the coordinates of the four corners of the obstacle; In the process of obtaining the projection point P of the to-be-detected path segment to the obstacle center point O, the following formula is satisfied: wherein, is the coordinate of the projection point P, is the coordinate of the end point F of the path segment to be detected, is the projection coefficient, denotes taking the maximum value, denotes taking the minimum value, is the coordinate of the obstacle center point O; In the process of obtaining the coordinates of the auxiliary point Q based on M, the following trigonometric relationships are calculated: wherein is an angle formed by two rays OP and OM emanating from point O, is an angle formed by two rays PO and PM emanating from point P, is the coordinate of point M; When the coordinates of the auxiliary point Q are: ​ When the coordinates of the auxiliary point Q are: : The formula of secondary collision detection is: ； If the formula of secondary collision detection is established, it is determined that there is collision, and the detection is directly ended; In the process of final collision detection, it is judged whether condition formula one and condition formula two are established, the expression of condition formula one is: ; The expression of condition formula two is: ; When condition formula one is established, it means that M is collinear with the two end points of the to-be-detected path segment, and M is between the two end points of the to-be-detected path segment; When condition formula two is established, it means that M is collinear with the two end points of the to-be-detected path segment, and M is not between the two end points of the to-be-detected path segment; When the conditional expression one is not true, or the conditional expression two is not true, it indicates that M is not collinear with the two end points of the path segment to be detected. Further, it is determined whether the conditional expression three is true. The expression of the conditional expression three is: wherein is the coordinate of the next vertex of the obstacle four vertices M is the coordinate of the next vertex of the obstacle four vertices M is the angle formed by the two rays ME and MF drawn from the point M, is the angle formed by the two rays and drawn from the point M;​​ When condition formula three is established, it means that the to-be-detected path segment passes through the obstacle, and it is determined that there is collision; When condition formula three is not established, it means that the to-be-detected path segment does not pass through the obstacle, and it is determined that there is no collision.

2. The vehicle trajectory planning method of claim 1, wherein Step S2 specifically includes: Acquire planning setting conditions, including initial node n init , initial node n init , corresponding coordinates , target node n goal , target node n goal , corresponding coordinates , maximum iteration number iter_max of planning, iteration radius R i , target radius R g , safety radius R s , distance ratio dist_ratio and obstacle set obsList, based on the above setting conditions, the path planning algorithm of the following process is designed: 1) Algorithm initialization: Initialize a random tree T with an initial node n init as the root node, while initializing a parent node index list parent, and calculating the maximum safety neighborhood radius maxR, i.e. the distance from the target node to the nearest obstacle boundary point in the obstacle set, according to the formula: wherein, is the number of obstacles, is the coordinate of the i-th vertex of the j-th obstacle; 2) Iterative search and node expansion: The following is iteratively performed until the target node n is found goal or the maximum number of iterations iter_max is reached: 2.1) Generate random sampling points: Generate random sampling points n within the search space rand , n rand The corresponding coordinates are ; 2.2) Find the random sampling point n in the random tree T rand the nearest node: traverse the random tree T, calculate the Euclidean distance between the node and n rand , and select the node with the smallest Euclidean distance from the random tree T as the nearest node n near , wherein the Euclidean distance d rand between the kth node and n k The calculation formula of d k is: wherein, is the coordinate corresponding to the kth node, k ∈ [1, K], K is the number of nodes in the random tree T; 2.3) Collision Detection: Invoke the fast collision detection strategy designed in step S1 to determine n rand With n near The line L rand-near Whether there is a collision, where n rand With n near The line L rand-near As the path segment to be detected, n rand With n near Considered as endpoints E and F in step S1, each obstacle in the obstacle set obsList must be connected to L. rand-near Perform collision detection if the connection L rand-near If a collision detection is detected with any obstacle in the obstacle set obsList, then return to the step of generating random sampling points and start a new iteration; if the connecting line L... rand-near If collision detection with all obstacles in the obstacle set obsList shows no collision, then continue execution. 2.4) Target connection judgment: Calculate n rand to the distance Rt of n goal , the formula is as follows: If Rt≤ R g , then n goal is added to the random tree T, n near is listed as the parent node of n goal in the parent node index list parent, and it is marked that the search is successful and the iteration is ended; if R g <Rt < maxR, then n rand is added to the random tree T, n near is listed as the parent node of n rand in the parent node index list parent, then n goal is added to the random tree T, n rand is listed as the parent node of n goal in the parent node index list parent, and it is marked that the search is successful and the iteration is ended; if Rt ≥ maxR, then it is continued to be executed downwards. 2.5) Initial new node generation: control the expansion step size according to the distance ratio dist_ratio, generate initial new node n new The formula is as follows: wherein L is an extension step, L = dist_ratio x d near-rand , d near-rand is the distance between n near and n rand ; 2.6) Adaptive heuristic adjustment: compute the distance D1 of the nearest node n near to the target node n goal and the distance D2 of the initial new node n new to the nearest node n near , as follows: wherein, is the initial new node n new corresponding coordinates, is the nearest node n near corresponding coordinates; Compute unit heuristic vector from n near to n goal :​ Let the distance gain coefficient be p, according to the size relationship between D1 and p x D2, the heuristic function is dynamically adjusted , the expression is: Then generate adaptive heuristic new node HA new , HA new = n new + H, call fast collision detection strategy to verify whether the connection of HA new and n new has collision, if no collision, then n new = HA new ; 2.7) Neighborhood optimization: compute n new the distance to all nodes in the random tree T, and filter out the neighborhood node set n i with distance ≤ R near-set For each neighborhood node, compute the sum of the distance from the root node to the neighborhood node and the distance from the neighborhood node to n new , select the node with the smallest sum of distances and no collision in the connection as the parent node of n new , and update parent; 2.8) Goal reachability judgment: Calculate n new to n goal distance d new-goal If d new-goal ≤Rg or d new-goal If n ≤ maxR, then n goal Add n to the random tree T, and add n to the parent node index list parent. new Column n goal The parent node of d is marked as a successful search and the iteration ends; if d new-goal >Rg and d new-goal If n > maxR, then n new Add it to the random tree T, then return to the step of generating random sampling points and start a new iteration; 3) Path generation: When the search is successful, the original path is obtained by backtracking from n goal to n init via the parent index list parent, and the order of the nodes is reversed to obtain the collision-free reference path from n init to n goal .

3. The vehicle trajectory planning method of claim 1, wherein In step S3, the expression of anti-rollover dynamics constraints is: wherein, is a vehicle front wheel angle constraint, is a wheel base of the vehicle, is a static vehicle center of mass height off the ground, is a vehicle roll stiffness, is a vehicle sprung mass, is a gravitational acceleration, is a longitudinal vehicle speed.

4. The vehicle trajectory planning method of claim 3, wherein, In step S4, the smoothing constraints of smoothing strategy are: wherein is a front wheel steering angle control amount, , are a maximum value and a minimum value of the front wheel steering angle control amount, is a first derivative of the front wheel steering angle control amount, , are a maximum value and a minimum value of the first derivative of the front wheel steering angle control amount, is a front wheel speed control amount, , are a maximum value and a minimum value of the front wheel speed control amount, is a front wheel acceleration control amount, , are a maximum value and a minimum value of the front wheel acceleration control amount.

5. The vehicle trajectory planning method of claim 4, wherein, In step S5, in the process of smoothing processing reference path, the smoothing processing equation is: wherein, is a first derivative of the displacement control amount of the vehicle in the x-axis direction, is an angle control amount of a vehicle advancing direction and the x-axis, is a first derivative of the displacement control amount of the vehicle in the y-axis direction, is a first derivative of , is a vehicle length control amount, is a vehicle front wheel steering angle, is a first derivative of , is a first derivative of , is a vehicle front wheel steering angle velocity control amount.

Images