Automatic driving automobile obstacle avoidance trajectory planning method based on space-time corridor dynamic search in curve scene

Abstract

The invention discloses an automatic driving automobile obstacle avoidance trajectory planning method based on space-time corridor dynamic search in a curve scene, and relates to the field of intelligent driving and traffic safety, and the method comprises the steps: S1, information collection and reference line fitting; s2, coordinate system conversion of the vehicle state parameters; s3, generating an obstacle space in the three-dimensional space-time map S-D-T; s4, generating a subsection passable space-time corridor; s5, generating feasible track points in the space-time corridor area; and S6, generating an optimal track point. According to the method, track point sudden change and sharp turning conditions are avoided, and safety and efficiency are brought to driving.

Description

Technical Field

[0001] The present invention relates to the field of intelligent vehicle technology, and in particular to an obstacle avoidance trajectory planning method for an autonomous driving vehicle based on dynamic search of a spatiotemporal corridor in a curve scenario. Background Art

[0002] Vehicle trajectory planning in curved scenarios must not only account for interference from surrounding obstacles but also consider whether the trajectory satisfies the vehicle dynamics model and road curvature constraints. To prevent excessive curvature in the planned trajectory during curve avoidance, which could lead to vehicle rollover, current obstacle avoidance trajectory planning strategies for autonomous vehicles often focus on conservative, trackable plans, resulting in low driving efficiency. Therefore, research on obstacle avoidance trajectory planning methods for autonomous vehicles in curved scenarios that balance safety, efficiency, and comfort is crucial.

[0003] Existing trajectory planning methods for autonomous vehicles on curved roads can be categorized into two types: time-space decoupling planning and time-space coupling planning. The trajectories generated by time-space decoupling planning methods in curved roads are prone to falling into local optima, failing to balance efficiency and safety. The path curvature generated by time-space coupling planning methods is often discontinuous, making stable tracking difficult and resulting in poor trajectory quality in curved roads.

[0004] Chinese patent application publication number CN110286681B proposes a dynamic autonomous driving lane change trajectory planning method for variable-curvature curves. This method uses an optimal trajectory algorithm that considers both comfort and efficiency to generate an optimal trajectory. Safety constraints are then used to adjust the optimal trajectory, ensuring that the final lane change trajectory is optimally planned while maintaining safety. However, this method fails to consider the impact of the changing motion of surrounding obstacles, making it difficult to meet the requirements of real-world curve scenarios.

[0005] Chinese patent application publication number CN116339320A discloses a vehicle lane change trajectory planning method based on coordinate system transformation for avoiding obstacles on curved lanes. This method, when a stationary obstacle is located ahead of the vehicle, decelerates if the distance between the obstacle and the vehicle is less than a set threshold. The method then determines whether there is space to avoid obstacles on either side of the vehicle, allowing for an obstacle-avoiding lane change. However, this method is relatively conservative, overemphasizing safety and resulting in low vehicle efficiency on curved lanes. Summary of the Invention

[0006] In response to the shortcomings of existing methods, the present invention provides an obstacle avoidance trajectory planning method for an autonomous driving vehicle based on dynamic search of space-time corridors in a curved scenario, so as to solve the problem of ensuring that the curved obstacle avoidance trajectory meets the requirements of safety, efficiency and comfort at the same time.

[0007] The method comprises the following steps:

[0008] S1. Information collection and reference line fitting: Collect the motion state information of the vehicle and surrounding vehicles and the road curvature information, and use the road center curve as the reference line;

[0009] S2. Coordinate system conversion of vehicle state parameters: Combined with road curvature information, the position, velocity, acceleration, and heading angle of the vehicle and surrounding vehicles in the current Cartesian coordinate system are converted to the Frenet coordinate system;

[0010] S3. Generate obstacle space within the 3D spatiotemporal map (SDT): Generate a 3D spatiotemporal map (SDT) based on the vehicle's position, and combine the surrounding vehicle's motion state changes and critical safety distance to generate an obstacle space.

[0011] S4. Generation of segmented traversable space-time corridors: Generate segmented traversable space-time corridors within the planning time domain based on the spatial distribution of obstacles and the vehicle's motion state at the current moment;

[0012] S5. Generation of feasible trajectory points in the space-time corridor area: A cost function is established by comprehensively considering driving safety and traffic efficiency. The hybrid A* trajectory search algorithm is improved from two aspects: dynamic step size design and optimal node iterative optimization. A rough search is performed to obtain feasible trajectory points in the passable space-time corridor area.

[0013] S6. Optimal trajectory point generation: Use a quintic polynomial fitting function to perform segmented fitting on the trajectory points obtained by the coarse search, and repeat two rounds of local weighted regression process to generate the optimal trajectory.

[0014] In step S1, vehicle-mounted sensors such as lidar and cameras are first used to acquire parameters of the road environment and surrounding vehicle status. Simultaneously, a high-precision positioning system is used to acquire parameters of the vehicle's own vehicle status. The road environment parameters refer to road curvature, while the parameters of the surrounding vehicles and the vehicle's own vehicle status include information such as vehicle position, velocity, acceleration, and heading angle. In a Cartesian coordinate system, the vehicle's state is represented as [x, y, v, a, θ, κ], where (x, y) is the vehicle's position, v, a, and θ are velocity, acceleration, and heading angle, respectively, and κ is the road curvature at the current position. A center curve of the vehicle's lane is generated based on the perceived road information and used as a reference line.

[0015] In step S2, the reference line starting point is taken as the origin, the reference line tangent direction is set as the S axis, and the normal direction is set as the D axis to establish the Frenet coordinate system SD. In order to simplify the horizontal and vertical trajectory planning and solution process, the vehicle state parameters in the Cartesian coordinate system are converted to the Frenet coordinate system in combination with the road curvature information. The vehicle motion state parameters in the Frenet coordinate system are recorded as Where s is the distance from the projection point of the vehicle on the reference line to the origin, which can also be called longitudinal displacement. is the longitudinal velocity, is the longitudinal acceleration, d is the distance between the vehicle and the projection point on the reference line, which can also be called the lateral displacement, is the lateral velocity, is the lateral acceleration, d′ is the first-order derivative of the lateral displacement with respect to the longitudinal displacement, and d″ is the second-order derivative of the lateral displacement with respect to the longitudinal displacement. The projection point M of the vehicle on the reference line in the Cartesian coordinate system is denoted as (x M ,y M ), the vehicle motion state parameters [x, y, v, a, θ, κ] in the Cartesian coordinate system at the current moment and the corresponding vehicle motion state parameters in the Frenet coordinate system The state transition between the two states is determined by the following process: The vehicle lateral displacement d is calculated as follows:

[0016]

[0017] Vehicle lateral speed for:

[0018]

[0019] Vehicle lateral acceleration for:

[0020]

[0021] The first-order derivative d′ of the lateral displacement d with respect to the reference line is:

[0022]

[0023] Depend on So d′ is:

[0024]

[0025] Squaring both sides and shifting the terms yields:

[0026] d′=(1-κ r d)tan(△θ),

[0027] Vehicle longitudinal speed for:

[0028]

[0029] Longitudinal acceleration The calculation is as follows:

[0030]

[0031] The second-order derivative d″ of the lateral displacement d with respect to the reference line is:

[0032]

[0033] In step S3, the SDT spatiotemporal map is constructed, and the corresponding obstacle space is generated according to whether the surrounding obstacle vehicles are in a uniform motion state, specifically including:

[0034] Step 31) Based on the Frenet coordinate system SD, add the time axis T to form a three-dimensional SDT space-time map. Set the vehicle as a point mass model and plan the time domain T p The three-dimensional space area occupied by the surrounding obstacle vehicles is called the obstacle space, and the space area where the vehicle can safely pass is called the traversable space-time corridor.

[0035] Step 32) Calculate the critical safety distance d at the current moment based on whether the vehicle and the obstacle vehicle are in a uniform speed state. b :

[0036]

[0037] in, and are the longitudinal velocity and acceleration of the vehicle respectively; and are the longitudinal velocity and acceleration of the obstacle vehicle respectively; t s is the braking delay time; d s The safe distance between the two vehicles after they stop is 3m.

[0038] Step 33) Generate the corresponding obstacle space according to the motion state of the obstacle vehicle, which is divided into the following three cases:

[0039] A. Static state: The obstacle vehicle is fixed in the space-time map. The obstacle space extends along the T axis to form a cuboid perpendicular to the SD plane. The obstacle space is represented as:

[0040]

[0041] Where, ((s z (t),d z (t),t) is the three-dimensional space occupied by the obstacle space, s N and d N are the longitudinal position and lateral position of the obstacle vehicle, L A 、W A are the length and width of the bottom surface of the obstacle space, T p To plan the time domain.

[0042] B. Uniform speed state: The obstacle space extends along the S axis, forming an oblique prism inclined to the SD plane. The obstacle space is expressed as:

[0043]

[0044] Where, v N The speed of the obstacle vehicle.

[0045] C. Non-uniform speed state: If the acceleration of the obstacle vehicle is greater than 0 and the speed of the obstacle vehicle continues to increase, the obstacle space will bend forward along the S axis, and the tangent of the side curve will obliquely intersect the SD plane, generating a curved cylinder convex along the T axis; if the acceleration of the obstacle vehicle is less than 0 and the speed of the obstacle decreases, the obstacle space will bend backward along the S axis, and the tangent of the side curve will obliquely intersect the SD plane, generating a curved cylinder concave along the T axis. The obstacle space can be expressed as:

[0046]

[0047] Where a N is the acceleration of the obstacle vehicle.

[0048] In order to avoid the trajectory planning falling into the local optimum under the non-uniform speed state, the ST plane obstacle space bounding box is expanded along the S axis direction to make it into a parallelogram with the smallest envelope area, so that the solution space becomes a convex space. The expanded obstacle space is expressed as:

[0049]

[0050] In step S4, a segmented traversable space-time corridor area in a prismatic convex space in the planning time domain is generated according to the spatial distribution of obstacles, specifically:

[0051] Step 41), plan the time domain T p It is divided into m+1 subintervals, and the time interval corresponding to each subinterval is recorded as [T0, T1],…, [T m ,T m+1 ]. k is the length of the kth time interval, where h k =T k -T k-1 .

[0052] Step 42) Calculate the time interval h according to the spatial distribution of obstacles k The corresponding kth traversable space-time corridor area S k :

[0053]

[0054] Where, s(T k-1 ) , s(T k ) The time T k-1 、T k The corresponding vertical lower bound of the traversable space-time corridor, Time T k-1 、T k The corresponding longitudinal upper bound of the traversable space-time corridor. dr and d l They are respectively the upper and lower horizontal boundaries of the traversable space-time corridor.

[0055] Planning time domain T p The space-time corridor area S is accessible within Ω is the union of the traversable space-time corridor areas of each sub-interval, that is, S Ω =S0∪…∪S m

[0056] In step S5, a cost function is established by comprehensively considering driving safety and traffic efficiency, and the hybrid A* trajectory search algorithm is improved from two aspects: dynamic step size design and optimal node iterative optimization. Specifically,

[0057] Step 51), node cost function design:

[0058] The node state space is recorded as (s, d, θ, v, t), the current position of the vehicle is taken as the initial node N0, and the sub-node expansion is performed by discrete control variables. The sub-node N in this round of expansion is calculated. i The cost function is used to calculate the optimal child node using the node iterative optimization algorithm and use it as the parent node N of the next step. p , repeat this process until the target node is reached and the search is completed.

[0059] Child node cost function f(s i )for:

[0060] f(s i )=G i +εH i ,

[0061] Where G i is the cumulative cost function, H i is the heuristic cost function, and ε is the heuristic weight.

[0062] Cumulative cost function G i The cumulative cost function is the sum of the costs accumulated from the initial node to the current child node. This sum includes the accumulated cost of the parent node and the cost from the parent node to the current node. By introducing a cumulative cost function, we ensure that the desired speed is achieved as much as possible, avoid deviation from the reference line, and penalize close proximity to obstacles.

[0063] The cumulative cost function is expressed as:

[0064] G i =G p +ω i1 ×G vi +ω i2 ×G ci +ω i3 ×G si ,

[0065] G p is the accumulated cost of the parent node; G vi is the difference between the current node speed and the expected speed, G vi = v i -v exp ; G ci is the lateral offset of the current position from the reference line, G ci = d i -d ref ; G si is the shortest Euclidean distance between the current node and the obstacle space, m = 0, 1…, T p , ω i1 , ω i2 , ω i3 are all weight coefficients.

[0066] The heuristic cost function H i is the cost estimate value from the current child node to the target node, which can be expressed as:

[0067] H i = w1(s i -s G ) 2 + w2(d i -d G ) 2 + w3(t i -t G ) 2 ,

[0068] In the formula, the first and second terms are planar distance heuristic function terms; the third term is a time heuristic function term for accelerating the search process and avoiding the search algorithm falling into local optimum; w1, w2, w3 are all weight coefficients, and (s G , d G , t G ) are the target point coordinates of the planning.

[0069] The value of the heuristic weight ε is related to the position of the current child node. The closer the current child node is to the target node, the higher the heuristic weight. The calculation is as follows: ε = 1 + s G -s i / s G -s0 × △ε, where △ε is a variable weight coefficient.

[0070] Step 52), dynamic step size design:

[0071] The neighborhood uniform discrete method is used to discretize the front wheel steering angle at the child node, and the front wheel steering angle discrete range is [δ i -△δ, δ i+△δ], then the vehicle discrete front wheel turning angle set corresponding to the i-th step is for:

[0072]

[0073] Where j = 0, 1, ..., N odd , N odd is the number of child nodes searched by the parent node, which is an odd number; △δ is the discrete neighborhood. Let δ max To satisfy the maximum front wheel turning angle constraint of the dynamic model, if δ i -△δ<-δ max , then set the left boundary of the discrete range to -δ max If δ i -△δ>-δ max , then set the right boundary of the discrete range to δ max .

[0074] The acceleration at the subnode is discretized using the neighborhood uniform discretization method, and the acceleration discretization range is Then the vehicle discrete acceleration set corresponding to the i-th step is Expressed as:

[0075]

[0076] set up is the maximum acceleration that satisfies the constraints of the dynamic model. Then set the left boundary of the discrete range to like Then set the right boundary of the discrete range to

[0077] By parent node N p (s p ,d p ,θ p ,v p ,t p ) starts, based on the discrete front wheel angle set and discrete acceleration sets Perform node expansion, the child node N corresponding to the i-th step i (s i ,d i ,θ i ,v i ,t i ) can be expressed as:

[0078]

[0079] Where: dt is the time step between two adjacent nodes, t p is the time corresponding to the parent node, δi is the front wheel turning angle of the vehicle at the current node, L is the vehicle wheelbase, is the vehicle acceleration at the current node. and acceleration set Traverse and get the set of child nodes corresponding to each step length.

[0080] Step 53), iterative optimization of the optimal node:

[0081] The hybrid A* trajectory search algorithm is further improved by iterative optimization of the optimal node. First, the cost function of each child node is calculated, and the node with the lowest cost function value is replaced. As the first point of the first round of iterative optimization. Control variables The control variables of the four adjacent nodes are discretized by median to obtain the control variables Then, based on the above control variables, the sub-nodes are expanded and the obtained sub-nodes are added to the sub-node set of this step, and the node with the lowest cost function is replaced as the second point of the first round of iterative optimization.

[0082] Assume that the front wheel steering angles of the first and second points in the first round of iterative optimization are α1, α2, and the accelerations are β1, β2 respectively. The control variables of the first and second points are discretized by median to obtain the control variables ((α1+α2) / 2, (β1+β2) / 2). The child nodes obtained by expanding the nodes are The subnode set is added as the first point of the second round of iterative optimization, and the subnode with the lowest cost value in the subnode set is used as the second point of the second round of iterative optimization. The above node iterative optimization process is repeated until the set number of iterations is reached, and then the next step of long node expansion is entered.

[0083] Step 54), generate feasible trajectory points by rough search:

[0084] The node with the lowest cost in the generated child node set of the i-th step is taken as the optimal node. When the node is expanded to the target node, the optimal node set corresponding to all steps is recorded as Ω, which is the trajectory point obtained by the rough search.

[0085] In step S6, the trajectory points obtained by the rough search are segmented and fitted, and two rounds of local weighted regression are repeated to generate the optimal trajectory, avoiding sudden changes and sharp turns in the trajectory points. Specifically:

[0086] Use the fifth-order polynomial to fit the function f s (d) Perform piecewise fitting on the discrete feasible trajectory points obtained by the rough search. The quintic polynomial fitting function is:

[0087]

[0088] Where a j is the coefficient of the jth term. The goal of fitting optimization is the loss function f loss Minimum, that is:

[0089]

[0090] Where, (s i ,d i ) is the initial point of the i-th path segment, K(d,d i ) is the weight function, which is calculated as follows:

[0091]

[0092] Where W(z) is the cube root kernel function. Applying this function to the weight function can speed up the convergence speed. It is calculated as follows:

[0093]

[0094] From this we can get the fitting function f s (d i The path generated by the traditional local weighted regression method through a single regression may contain a curve path with a large curvature, which often does not meet the vehicle kinematic constraints. Therefore, this paper uses a double regression process to avoid the vehicle from making sharp turns. The fitting residual e is calculated based on the results of the first round of local weighted regression. i :

[0095] e i =s i -f s (d i ),

[0096] Since the smooth fitting residual variance of the double square root kernel function is smaller and suitable for the second and subsequent iterations, this function is used to calculate the residual weight η i :

[0097]

[0098] Where ξ is the median of the absolute value of the residuals, κ l Is the outlier factor. The weight function is updated using the residual weight of the trajectory point and substituted into the loss function. After repeating two rounds of local weighted regression process, the segmented longitudinal trajectory fitting optimization function g is obtained. s (d) Applying this function to the weight function can accelerate the convergence speed. Repeat the above method to fit and optimize the lateral trajectory that changes with time, and obtain the segmented lateral trajectory fitting optimization function h d (t). Therefore, the planning time domain T p The optimal trajectory in the traversable space-time corridor is:

[0099] {g s (d),h d (t),t|s0=g s (d0),s G =g s (d G ),d0=h(t0),d G =h(t G ),t0=0,t G =T p},

[0100] Where (s0, d0, t0) and (s G ,d G ,t G ) are the coordinates of the starting point and target point of trajectory planning in the Frenet coordinate system.

[0101] The vehicle trajectory planning scheme in the curved road scenario provided by the present invention not only takes into account the interference of surrounding obstacles, but also takes into account the vehicle dynamics model and road curvature constraints, avoiding the risk of rollover and ensuring driving efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0102] Figure 1 A flow chart of the obstacle avoidance trajectory planning method for an autonomous vehicle based on dynamic search of a spatiotemporal corridor in a curve scenario provided by this application;

[0103] Figure 2 Schematic diagram of the conversion between the Frenet coordinate system and the Cartesian coordinate system provided in this application;

[0104] Figure 3 Schematic diagram of the static obstacle space provided for this application;

[0105] Figure 4 Schematic diagram of the uniform speed obstacle space provided for this application;

[0106] Figure 5 Schematic diagram of the obstacle space with acceleration barriers provided for this application;

[0107] Figure 6 Schematic diagram of the obstacle space with speed reduction barriers provided for this application;

[0108] Figure 7 Schematic diagram of the space-time corridor area provided for this application;

[0109] Figure 8 Schematic diagram of the iterative optimization nodes provided in this application. DETAILED DESCRIPTION

[0110] The technical solution of the present invention is described in detail below with reference to the accompanying drawings and embodiments. Those skilled in the art should know that the following embodiments are not the only limitations on the technical solution of the present invention, and any equivalent changes or modifications made within the spirit of the technical solution of the present invention should be deemed to fall within the scope of protection of the present invention.

[0111] The present invention provides a method for planning obstacle avoidance trajectories for an autonomous vehicle based on dynamic search of a spatiotemporal corridor in a curve scenario. The specific implementation of the method is as follows: Figure 1 As shown, the following steps are included:

[0112] Step 1: Information collection and reference line fitting

[0113] On-board sensors such as lidar and cameras are used to acquire parameters of the road environment and surrounding vehicle states, while a high-precision positioning system is used to acquire parameters of the vehicle's own state. The road environment parameters refer to road curvature, while the parameters of surrounding vehicles and the vehicle's own state include information such as vehicle position, velocity, acceleration, and heading angle. In a Cartesian coordinate system, the vehicle state is represented as [x, y, v, a, θ, κ], where (x, y) is the vehicle's position, v, a, and θ are velocity, acceleration, and heading angle, respectively, and κ is the road curvature at the current position. A center curve of the vehicle's lane is generated based on the perceived road information and used as a reference line.

[0114] Step 2: Coordinate system conversion of vehicle state parameters

[0115] In order to simplify the horizontal and vertical trajectory planning and solution process, the vehicle state parameters in the Cartesian coordinate system are converted to the Frenet coordinate system in combination with the road curvature information. Figure 2 As shown, the projection point M of vehicle P on the reference line in the Cartesian coordinate system is recorded as (x M ,y M ), the current vehicle motion state is recorded as [x, y, v, a, θ, κ], where is the vehicle position vector, v, a, θ are the velocity, acceleration and heading angle respectively, and κ is the road curvature at the current position.

[0116] With the starting point of the reference line as the origin, the tangent direction of the reference line as the S axis and the normal direction as the D axis, the Frenet coordinate system SD is established. The vehicle motion state in the Frenet coordinate system is recorded as Where s is the distance from the projection point of the vehicle on the reference line to the origin, which can also be called longitudinal displacement. is the longitudinal velocity, is the longitudinal acceleration, d is the distance between the vehicle and the projection point on the reference line, which can also be called the lateral displacement, is the lateral velocity, is the lateral acceleration, d′ is the first derivative of the lateral displacement with respect to the longitudinal displacement, and d″ is the second derivative of the lateral displacement with respect to the longitudinal displacement.

[0117] Convert the vehicle motion state parameters [x, y, v, a, θ, κ] in the current Cartesian coordinate system to the corresponding vehicle motion state parameters in the Frenet coordinate system The state transition process is as follows:

[0118] The lateral displacement d is calculated as follows:

[0119]

[0120] d can also be represented by a vector: in, is the unit vector from the projected point M to the point P, is the unit vector from point P to the projected point M.

[0121] Derivative to obtain the vehicle's lateral velocity

[0122]

[0123] Where Δθ=θ-θ r , κ r is the curvature of the reference line at point M, is the unit vector along the vehicle's travel direction at point P, θ r is the angle between the tangent of the reference line at point M and the X-axis.

[0124] The lateral acceleration of the vehicle is:

[0125]

[0126] The first-order derivative d′ of the lateral displacement d with respect to the reference line is:

[0127]

[0128] Depend on So d′ is:

[0129]

[0130] Squaring both sides and shifting the terms yields: d′=(1-κ r d)tan(△θ)

[0131] The longitudinal velocity can be obtained by combining

[0132]

[0133] Longitudinal acceleration It can be calculated as:

[0134]

[0135] Then we have:

[0136]

[0137] The second-order derivative d″ of the lateral displacement d with respect to the reference line is:

[0138]

[0139] Step 3: Generate obstacle space in the 3D space-time map SDT

[0140] Based on the Frenet coordinate system SD, the time axis T is added to form a three-dimensional SDT space-time map. The vehicle is set as a point mass model and the time domain T is planned. p The three-dimensional space area occupied by the internal obstacle vehicle is called the obstacle space, and the space area where the ego vehicle can pass safely is called the traversable space-time corridor.

[0141] When generating the obstacle space, first calculate the critical safety distance d at the current moment based on whether the vehicle and the obstacle vehicle are in a uniform speed state. b :

[0142]

[0143] in, and are the longitudinal velocity and acceleration of the vehicle respectively; and are the longitudinal velocity and acceleration of the obstacle vehicle respectively; t s is the braking delay time; d s The safe distance between the two vehicles after they stop is 3m.

[0144] like Figure 3-Figure 6 As shown, the corresponding obstacle space is generated according to the change of the obstacle vehicle's motion state, which is divided into the following three cases:

[0145] A. Static state: The obstacle vehicle is fixed in the space-time map. The obstacle space extends along the T axis to form a cuboid perpendicular to the SD plane. The obstacle space is:

[0146]

[0147] Where, ((s z (t),d z (t),t) is the three-dimensional space occupied by the obstacle space, s N and d N are the longitudinal position and lateral position of the obstacle vehicle, LA 、W A are the length and width of the bottom surface of the obstacle space, T p To plan the time domain.

[0148] B. Uniform speed state: The obstacle space extends along the S axis, forming an oblique prism inclined to the SD plane. The obstacle space is expressed as:

[0149]

[0150] Where, v N The speed of the obstacle vehicle.

[0151] C. Non-uniform speed state: If the obstacle vehicle's acceleration is greater than 0 and its speed increases, the obstacle space bends forward along the S axis, and the tangent of the side curve intersects the SD plane obliquely, generating a curved cylinder convex along the T axis. When the obstacle acceleration is less than 0 and its speed decreases, the obstacle space bends backward along the S axis, and the tangent of the side curve intersects the SD plane obliquely, generating a curved cylinder concave along the T axis. The obstacle space can be expressed as:

[0152]

[0153] Where a N is the acceleration of the obstacle vehicle.

[0154] In order to avoid the trajectory planning falling into the local optimum under the non-uniform speed state, the ST plane obstacle space bounding box is expanded along the S axis direction to make it into a parallelogram with the smallest envelope area, so that the solution space becomes a convex space. The expanded obstacle space is:

[0155]

[0156] Step 4: Generation of segmented traversable space-time corridors

[0157] First, the planning time domain T p It is divided into m+1 subintervals, and the time interval corresponding to each subinterval is recorded as [T0, T1],…, [T m ,T m+1 ]. k is the length of the kth time interval, where h k =T k -T k-1 .

[0158] Secondly, the time interval h is calculated based on the spatial distribution of obstacles k The corresponding kth traversable space-time corridor area S k :

[0159]

[0160] in, s(T k-1 ) , s(T k ) The time T k-1 、T k The corresponding vertical lower bound of the traversable space-time corridor, The time T k-1 、T k The corresponding longitudinal upper bound of the traversable space-time corridor, d r and d l They are respectively the upper and lower horizontal boundaries of the traversable space-time corridor.

[0161] Planning time domain T p The space-time corridor area S is accessible within Ω is the union of the traversable space-time corridor areas of each sub-interval, that is, S Ω =S0∪…∪S m ,like Figure 7 shown.

[0162] Step 5: Generate feasible trajectory points in the space-time corridor area

[0163] Considering driving safety and traffic efficiency, a cost function is set. The hybrid A* trajectory point search algorithm is improved through dynamic step size design and optimal node iterative optimization. A rough search obtains feasible trajectory points within the traversable space-time corridor area. The following sub-steps are included:

[0164] Sub-step 1: Node cost function design

[0165] First, the node state space is set to: (s, d, θ, v, t), the current position of the vehicle is taken as the initial node N0, and the sub-nodes are expanded by discrete control variables. The sub-nodes N in this round of expansion are calculated. i The cost function is used to calculate the optimal child node using the node iterative optimization algorithm and use it as the parent node N of the next step. p , repeat this process until the target node is reached and the search is completed.

[0166] Child node cost function f(s i )for:

[0167] f(s i )=G i +εH i

[0168] Where: G i is the cumulative cost function, ε is the heuristic weight, H i is the heuristic cost function.

[0169] The accumulated cost function refers to the accumulated cost from the initial node to the current sub-node, which includes the accumulated cost of the parent node and the cost from the parent node to the current node. By introducing the accumulated cost function, it is ensured that the desired speed is achieved as much as possible, and the behavior of deviating from the reference line and approaching the obstacle is punished.

[0170] The accumulated cost function is expressed as:

[0171] G i = G p + ω i1 × G vi + ω i2 × G ci + ω i3 × G si

[0172] In the formula, G p is the accumulated cost of the parent node; G vi is the difference between the current node speed and the desired speed, G vi = v i -v exp ; G ci is the lateral offset of the current position from the reference line, G ci = d i -d ref ; G si is the shortest Euclidean distance between the current node and the obstacle space, m = 0, 1…, T p , ω i1 , ω i2 , ω i3 are weight coefficients.

[0173] The heuristic cost function refers to the cost estimate from the current sub-node to the target node, which is expressed as:

[0174] H i = w1(s i -s G ) 2 +w2(d i -d G ) 2 +w3(t i -t G ) 2

[0175] In the formula, the first and second terms are plane distance heuristic function terms; the third term is a time heuristic function term, which is used to speed up the search process and avoid the search algorithm falling into local optimum; w1, w2, w3 are weight coefficients, (s G , d G , t G ) are the coordinates of the planned target point.

[0176] The value of the heuristic weight ε is related to the position of the current sub-node, the closer the current sub-node is to the target node, the higher the heuristic weight, and is calculated as follows: ε = 1 + s G -s i / s G -s0×△ε, wherein △ε is a variable weight coefficient.

[0177] Sub-step 2: Dynamic step design

[0178] The front wheel steering angle at the sub-node is discretized by the neighborhood uniform discretization method, and the discretization range of the front wheel steering angle is [δ i -△δ, δ i +△δ], then the discrete front wheel steering angle set of the vehicle corresponding to the i-th step is represented as:

[0179]

[0180] In the formula, j = 0, 1, …, N odd , N odd is the number of sub-nodes searched by the parent node, and is an odd number; △δ is the discrete neighborhood. Let δ max be the maximum value of the front wheel steering angle that satisfies the dynamics model constraint, if δ i -△δ<-δ max , then the left boundary of the discretization range is set to -δ max ; if δ i -△δ>-δ max , then the right boundary of the discretization range is set to δ max .

[0181] The acceleration at the sub-node is discretized by the neighborhood uniform discretization method, and the acceleration range interval , then the discrete acceleration set of the vehicle corresponding to the i-th step is represented as:

[0182]

[0183] Let be the maximum value of the acceleration that satisfies the dynamics model constraint. If , then the left boundary of the discretization range is set to If , then the right boundary of the discretization range is set to

[0184] Starting from the parent node N p (s p , d p , θ p , v p , t p ), based on the discrete front wheel steering angle set ​​and discrete acceleration sets Perform node expansion, the child node N corresponding to the i-th step i (s i ,d i ,θ i ,v i ,t i ) can be expressed as:

[0185]

[0186] Where: dt is the time step between two adjacent nodes, t p is the time corresponding to the parent node, L is the vehicle wheelbase, δ i is the front wheel turning angle of the vehicle at the current node, is the vehicle acceleration at the current node. and acceleration set Traverse and get the set of child nodes corresponding to each step length.

[0187] Sub-step 3: Iterative optimization of the optimal node

[0188] like Figure 8 As shown, first calculate the cost function of each search sub-node, and take the node with the lowest cost function value As the first point of the first round of iterative optimization. Control variables The control variables of the four adjacent nodes are discretized by median to obtain the control variables Then, based on the above control variables, the sub-nodes are expanded and the obtained sub-nodes are added to the sub-node set of this step, and the node with the lowest cost function is replaced as the second point of the first round of iterative optimization.

[0189] Assume that the front wheel steering angles of the first and second points in the first round of iterative optimization are α1, α2, and the accelerations are β1, β2 respectively. The control variables of the first and second points are discretized by median to obtain the control variables ((α1+α2) / 2, (β1+β2) / 2). The child nodes obtained by expanding the nodes are The child node set is added as the first point of the second round of iterative optimization, and the child node with the lowest cost value in the child node set is used as the second point of the second round of iterative optimization. Repeat the above node iterative optimization process until the set number of iterations is reached, and then proceed to the next step of long node expansion.

[0190] Sub-step 4: Coarse search for feasible trajectory point generation

[0191] The node with the lowest cost in the generated child node set of the i-th step is taken as the optimal node. When the node is expanded to the target node, the optimal node set corresponding to all steps is recorded as Ω, which is the feasible trajectory point obtained by the rough search.

[0192] Step 6: Optimal trajectory point generation based on double local weighted regression

[0193] Use the fifth-order polynomial to fit the function f s (d) Perform segmented fitting on the discrete feasible trajectory points obtained by coarse search:

[0194]

[0195] Where a j is the coefficient of the jth term.

[0196] The goal of fitting optimization is the loss function f loss Minimum, that is:

[0197]

[0198] Where, (s i ,d i ) is the initial point of the i-th path segment, K(d,d i ) is the weight function, which is calculated as follows:

[0199]

[0200] Where W(z) is the cube root kernel function. Applying this function to the weight function can speed up the convergence speed. It is calculated as follows:

[0201]

[0202] From this we can get the fitting function f s (d i The path generated by the traditional local weighted regression method through a single regression may contain a curve path with a large curvature, which often does not meet the vehicle kinematic constraints. Therefore, the present invention introduces a double regression process to avoid the vehicle from making sharp turns. The fitting residual e is calculated based on the results of the first round of local weighted regression. i :

[0203] e i =s i -f s (d i )

[0204] Since the smooth fitting residual variance of the double square root kernel function is smaller, it is suitable for the second and subsequent iterations, so this function is used to calculate the residual weight η i :

[0205]

[0206] Where ξ is the median of the absolute value of the residuals, κ l Is the outlier factor. The weight function is updated using the residual weight of the trajectory point and substituted into the loss function. After repeating two rounds of local weighted regression process, the segmented longitudinal trajectory fitting optimization function g is obtained. s (d) Applying this function to the weight function can accelerate the convergence speed. Repeat the above method to fit and optimize the lateral trajectory that changes with time, and obtain the segmented lateral trajectory fitting optimization function h d (t). Therefore, the planning time domain T p The optimal trajectory in the traversable space-time corridor is:

[0207] {g s (d),h d (t),t|s0=g s (d0),s G =g s (d G ),d0=h d (t0),d G =h d (t G ),t0=0,t G =T p}

[0208] Where (s0, d0, t0) and (s G ,d G ,t G ) are the coordinates of the starting point and target point of trajectory planning in the Frenet coordinate system.

[0209] The present application also provides a computing device, which may be a terminal, comprising: a processor, a communication interface, a memory, a display screen, and an input device. The processor is configured to provide computing and control capabilities, and the processor can call logic instructions stored in the memory. The memory includes a non-volatile storage medium and an internal memory, wherein the non-volatile storage medium stores an operating system and a computer program, and when the computer program is executed by the processor, the method described in the above embodiment is implemented. The internal memory provides an environment for the operation of the operating system and computer program stored in the non-volatile storage medium. The communication interface is configured to communicate with an external terminal in a wired or wireless manner, and the wireless manner may be implemented via WIFI, a management network, NFC (near field communication), or other technologies. The display screen may be a liquid crystal display or an electronic ink display screen, and the input device may be a touch layer covering the display screen, or a key, trackball, or touchpad provided on the housing of the computing device, or an external keyboard, touchpad, or mouse.

[0210] The present application is described with reference to the processes of the methods, devices (systems) and computer program products of the embodiments. It should be understood that one or more processes in the flowcharts can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device produce a device for implementing the functions specified in the flowchart.

Claims

1. A method for obstacle avoidance trajectory planning for an autonomous vehicle based on dynamic search of a spatiotemporal corridor in a curve scenario, characterized by: The following steps are involved: S1. Information collection and reference line fitting: The vehicle and surrounding vehicles' motion status information and road curvature information are collected, and the center curve of the lane where the vehicle is located is used as the reference line. S2. Coordinate system conversion of vehicle state parameters: convert the vehicle state parameters in the current Cartesian coordinate system to the Frenet coordinate system; S3. Generate obstacle space within the three-dimensional spatiotemporal map (SDT): Generate a three-dimensional spatiotemporal map (SDT) based on the vehicle's position, and generate an obstacle space based on the changing trend of surrounding vehicle motion states and the critical safety distance. S4. Generation of segmented traversable space-time corridors: Generate segmented traversable space-time corridors within the planning time domain based on the spatial distribution of obstacles and the vehicle's motion state at the current moment; S5. Generation of feasible trajectory points in the space-time corridor area: Establishing a node cost function, based on dynamic step size design and optimal node iterative optimization, and obtaining feasible trajectory points in the passable space-time corridor area through a coarse search using a hybrid A* trajectory search algorithm; S6. Optimal trajectory point generation: Use a quintic polynomial fitting function to perform segmented fitting on the trajectory points obtained by the coarse search, and repeat two rounds of local weighted regression process to generate the optimal trajectory.

2. The method for obstacle avoidance trajectory planning for an autonomous vehicle based on dynamic search of a spatiotemporal corridor in a curve scenario according to claim 1, characterized in that: In step S1, the road environment and surrounding vehicle state parameters are acquired using a laser radar and a camera, and the vehicle state parameters are acquired using a high-precision positioning system; The road environment parameters include road curvature, and the state parameters of the surrounding vehicles and the vehicle include vehicle position, speed, acceleration and heading angle information. The vehicle state in the Cartesian coordinate system is expressed as [x, y, v, a, θ, κ], where (x, y) is the vehicle position, v, a, θ are the speed, acceleration and heading angle respectively, and κ is the road curvature at the current position.

3. The obstacle avoidance trajectory planning method for an autonomous vehicle based on dynamic search of a spatiotemporal corridor in a curved road scenario according to claim 1, characterized in that: In step S2, the starting point of the reference line is taken as the origin, the tangent direction of the reference line is set as the S axis, and the normal direction is set as the D axis, to establish the Frenet coordinate system SD; The vehicle state parameters in the frenet coordinate system are expressed as: Where s is the distance from the projection point of the vehicle on the reference line to the origin, which is called the longitudinal displacement. is the longitudinal velocity, is the longitudinal acceleration, d is the distance between the vehicle and the projection point on the reference line, called the lateral displacement, is the lateral velocity, is the lateral acceleration, d′ is the first derivative of the lateral displacement with respect to the longitudinal displacement, and d″ is the second derivative of the lateral displacement with respect to the longitudinal displacement.

4. The method for obstacle avoidance trajectory planning for an autonomous vehicle based on dynamic search of a spatiotemporal corridor in a curved road scenario according to claim 1, characterized in that: In step S3, the time axis T is added to the Frenet coordinate system SD to obtain the SDT spatiotemporal map, and the corresponding obstacle space is generated according to the motion state of the surrounding obstacle vehicles; When the obstacle vehicle is stationary, the obstacle space is a cuboid perpendicular to the SD plane, which can be expressed as: Where, ((s z (t),d z (t),t) is the three-dimensional space occupied by the obstacle space, s N and d N are the longitudinal position and lateral position of the obstacle vehicle, L A 、W A are the length and width of the bottom surface of the obstacle space, T p To plan the time domain; When the obstacle vehicle is in a uniform motion state, the obstacle space is calculated in combination with the critical safety distance. At this time, the obstacle space is an oblique prism inclined to the SD plane, which is expressed as: Among them, v N The speed of the obstructing vehicle; When the obstacle vehicle is in a non-uniform motion state, the tangent line of the side curve of the obstacle space is obliquely intersected with the SD plane. At this time, the obstacle space is a curved cylinder, which can be expressed as: Among them, a N is the acceleration of the obstacle vehicle.

5. The obstacle avoidance trajectory planning method for an autonomous vehicle based on dynamic search of a spatiotemporal corridor in a curve scenario according to claim 1 is characterized in that: In step S4, the planning time domain T p It is divided into m+1 subintervals, and the time interval corresponding to each subinterval is recorded as [T0, T1],…, [T m ,T m+1 ], h k is the length of the kth time interval, h k =T k -T k-1 ; Calculate the time interval h based on the spatial distribution of obstacles k The corresponding kth traversable space-time corridor area S k : in: s(T k-1 ) , s(T k ) The time T k-1 、T k The corresponding vertical lower bound of the traversable space-time corridor; The time T k-1 、T k The corresponding longitudinal upper bound of the traversable space-time corridor; d r and d l are the upper and lower horizontal boundaries of the traversable space-time corridor respectively; the planning time domain T p Internal segmented traversable space-time corridor area S Ω is the union of the traversable space-time corridor areas of each sub-interval S Ω , that is, S Ω =S0∪…∪S m .

6. The method for obstacle avoidance trajectory planning for an autonomous vehicle based on dynamic search of a spatiotemporal corridor in a curve scenario according to claim 1, characterized in that: In step S5, a cost function is established by comprehensively considering driving safety and traffic efficiency, and the hybrid A* trajectory search algorithm is improved from two aspects: dynamic step size design and optimal node iterative optimization. Specifically, (1) Set the node state space to (s, d, v, θ, t), take the current vehicle position as the search initial node N0, and calculate the sub-node N in this round of expansion i The cost function is used to calculate the optimal child node using the node iterative optimization algorithm and use it as the parent node N of the next step. p , repeat the process until the target node is reached and the search is completed. The sub-node cost function is calculated as follows: f(s i )=G i +εH i , G i is the cumulative cost function, H i is the heuristic cost function, ε is the heuristic weight, the cumulative cost function includes the driving safety cost term, and the heuristic cost function includes the traffic efficiency cost term; (2) The front wheel angle and acceleration at the child node are discretized using the neighborhood uniform discretization method, and the dynamic step node expansion is performed based on the discrete front wheel angle and discrete acceleration. The expansion process is performed by the parent node N p (s p ,d p ,θ p ,v p ,t p ) Child node N corresponding to the i-th step i (s i ,d i ,θ i ,v i ,t i ) is expressed as: Where: dt is the time step between two adjacent nodes, t p is the time corresponding to the parent node, L is the vehicle wheelbase, δ i and Belong to the discrete front wheel angle set and discrete acceleration sets (3) Calculate the cost function of each child node and replace the node with the lowest cost function value As the first point of the first round of iterative optimization, Control variables The control variables of the four adjacent nodes are discretized by median to obtain the control variables Based on the above control variables, the sub-nodes are expanded and added to the sub-node set of this step. The node with the lowest cost function is replaced as the second point of the first round of iterative optimization. The front wheel angles of the first and second points of the first round of iterative optimization are α1, α2, and the accelerations are β1, β2 respectively. The control variables of the first and second points are median discretized to obtain the control variables ((α1+α2) / 2, (β1+β2) / 2). The sub-nodes obtained by node expansion are Put the child node set as the first point of the second round of iterative optimization method, and take the child node with the lowest cost value in the child node set as the second point of the second round of iterative optimization method. Repeat the above node iterative optimization process until the set number of iterations is reached and enter the next round of node expansion; (4) The node with the lowest cost value in the child node set of the i-th step is taken as the optimal node. When the node is expanded to the target node, the optimal node set corresponding to all steps is recorded as Ω, which is the trajectory point obtained by the rough search.

7. The method for obstacle avoidance trajectory planning for an autonomous vehicle based on dynamic search of a spatiotemporal corridor in a curve scenario according to claim 1, characterized in that: In step S6, a fifth-order polynomial fitting function f is used. s (d) Perform segmented fitting on the discrete feasible trajectory points obtained by coarse search: Where a j is the coefficient of the jth term, and the goal of fitting optimization is the loss function f loss Minimum, that is: Where, (s i ,d i ) is the initial point of the i-th path segment, K(d,d i ) is the weight function, which is calculated as follows: Where W(z) is the cube root kernel function, The weight function is updated using the residual weight of the trajectory point and substituted into the loss function. After repeating two rounds of local weighted regression process, the segmented longitudinal trajectory fitting optimization function g is obtained. s (d) Repeat the above method to fit and optimize the lateral trajectory that changes with time, and obtain the segmented lateral trajectory fitting optimization function h d (t), so the planning time domain T p The optimal trajectory in the traversable space-time corridor is: {g s (d),h d (t),t|s0=g s (d0),s G =g s (d G ),d0=h(t0),d G =h(t G ),t0=0,t G =T p } Where (s0, d0, t0) and (s G ,d G ,t G ) are the coordinates of the starting point and target point of trajectory planning in the Frenet coordinate system.

8. A computing device, characterized in that include: One or more processors, a memory, and one or more computer programs, wherein the one or more computer programs are stored in the memory and configured to be executed by the one or more processors, and the one or more computer programs include instructions for executing any one of the methods according to any one of claims 1 to 7.

Images