Space-time joint trajectory planning method of CILQR based on improved reference point acquisition

Abstract

The invention provides a space-time joint trajectory planning method of a CILQR based on improved reference point acquisition. The space-time joint trajectory planning method comprises the following steps: step 1, acquiring a kinematic model of a vehicle; the input of the kinematic model comprises a state vector xi at any moment and a control vector ui at the current moment, and the output of the kinematic model comprises a state vector xi + 1 at the next moment of the vehicle; step 2, based on a kinematic model, obtaining a reference state vector Gir at any moment; step 3, obtaining vehicle driving cost; the vehicle driving cost is determined by Gir, Xi and ui; 4, solving the optimal control vector deviation at any moment by taking the minimum vehicle driving cost as a target; 5, circulating the steps 1-4 until the control vector deviations at all moments are obtained; and 6, applying all the control vector deviations to a nominal track to generate a planned track. According to the invention, the real-time performance and smoothness of the track are improved.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of automatic driving vehicles, and particularly relates to a spatiotemporal joint trajectory planning method based on improved reference point acquisition of CILQR. BACKGROUND

[0002] Trajectory planning is a key link in an automatic driving system and is a core module for realizing autonomous decision and safe driving of a vehicle. The main task is to generate a driving trajectory that can safely avoid obstacles, is smooth and comfortable, and meets real-time requirements, and to issue the planning result to a control layer for tracking and execution. However, the trajectory planning methods commonly used at present still have certain deficiencies in actual application, for example, the planning takes a long time, the processing of vehicle kinematics and constraint conditions is not perfect, and the planning effect and real-time performance are difficult to balance. Therefore, it is of great significance to improve the performance and optimize the real-time performance of the trajectory planning algorithm.

[0003] Therefore, the CILQR (Constrained Iterative Linear Quadratic Regulator) algorithm provides an efficient and feasible solution for the automatic driving trajectory planning problem due to its advantages in optimization and real-time solving of nonlinear systems and the ability to effectively balance the requirements of safe obstacle avoidance and trajectory smoothing. SUMMARY

[0004] The application aims to provide a spatiotemporal joint trajectory planning method based on improved reference point acquisition of CILQR. The method establishes a state space equation based on a continuous curvature vehicle kinematics model and improves real-time performance while ensuring trajectory smoothness and safety through an iterative optimization framework. The specific steps are as follows:

[0005] A spatiotemporal joint trajectory planning method based on improved reference point acquisition of CILQR includes the following steps:

[0006] Step 1, obtaining a kinematics model of a vehicle;

[0007] The input of the kinematics model includes a state vector x i at any time and a control vector u i at the current time, and the output includes a state vector x i+1 of the vehicle at the next time.

[0008] Step 2, obtaining a reference state vector G ir at any time.

[0009] Step 3, obtaining a vehicle driving cost.

[0010] The vehicle driving cost is determined by G ir , x i , and ui determining;

[0011] Step 4, solving the optimal control vector deviation at any time with the minimum vehicle travel cost as the goal

[0012] Step 5, repeating steps 2-4 until the control vector deviation at all times is obtained

[0013] Step 6, all are applied to the nominal trajectory to generate a planned trajectory.

[0014] Preferably, the reference state vector G ir is:

[0015] G ir = (x ir , y ir , θ ir , v ir , k ir ).

[0016] Preferably, the vehicle travel cost is:

[0017]

[0018] L(x i , u i ) = L ref_line + L centri_acc + L centri_jerk + L Δv + L K + L control

[0019] L(x N , u N ) = L ref_line + L centri_acc + L centri_jerk + L Δv + L K

[0020]

[0021] L(x i , u i ) - the vehicle travel cost at the current time;

[0022] the sum of the vehicle travel cost from the next time to the terminal time N.

[0023] Preferably, in step 6:

[0024]

[0025] Compared with the prior art, the application has the advantages that:

[0026] The improved CILQR-based spatiotemporal joint trajectory planning can effectively reduce the calculation complexity and planning time while fully considering the vehicle lateral and longitudinal coupling characteristics; the reference point generated by extrapolation based on the planning information of the previous frame can more accurately reflect the actual motion trend of the vehicle; the centripetal acceleration and centripetal jerk terms introduced in the cost function can help improve the trajectory comfort. It is suitable for automatic driving trajectory planning tasks in complex dynamic environments. BRIEF DESCRIPTION OF DRAWINGS

[0027] Fig. 1 is a vehicle kinematics diagram;

[0028] Fig. 2 is a flowchart of the improved CILQR-based spatiotemporal joint trajectory planning method based on improved reference point acquisition;

[0029] Fig. 3 is a flowchart of the improved CILQR-based spatiotemporal joint trajectory planning method based on improved reference point acquisition. DETAILED DESCRIPTION

[0030] The improved CILQR-based spatiotemporal joint trajectory planning method based on improved reference point acquisition will be described in more detail below with reference to the accompanying drawings, in which a preferred embodiment of the application is shown, it should be understood that those skilled in the art can modify the application described herein while still achieving the advantageous effects of the application. Therefore, the following description should be understood as a broad knowledge to those skilled in the art, and not as a limitation on the application.

[0031] As Figs. 1-3 , the improved CILQR-based spatiotemporal joint trajectory planning method based on improved reference point acquisition includes the following steps:

[0032] Step 1, acquiring the kinematics model f(x i , u i ) of the vehicle.

[0033] The input of the kinematics model includes the state vector x i of the vehicle at any time and the control vector u i at the current time, and the output includes the state vector x i+1 of the vehicle at the next time;

[0034] x i =(x i , y i , θ i );

[0035] Kinematic model f(x i , u i ):

[0036]

[0037] Discretize equation (1) based on forward Euler method to obtain the discretized kinematic model:

[0038]

[0039] where i is the index of the current time; t is the time at the reference point;

[0040] X i is the horizontal coordinate of the nominal trajectory at time i;

[0041] y i is the vertical coordinate of the nominal trajectory at time i;

[0042] v i is the vehicle speed at time i;

[0043] a i is the vehicle acceleration at time i;

[0044] T s is the step size;

[0045] θ i is the vehicle heading angle at time i;

[0046] κ i is the curvature of the trajectory at time i.

[0047] Fig. 1 where Δ represents the turning angle.

[0048] Step 2, obtain any time reference state vector G ir .

[0049] Based on the extrapolation of the previous frame state sequence, obtain the reference point close to the reference line cost, i.e. g ir .

[0050] where the previous frame state = (x i , y i , θ i , v i , k i )

[0051] The reference line is a segment in the global planning result, and only with the reference line can local planning be performed. CILQR is a local planning.

[0052] The value related to the r subscript in the reference line cost is reference, which is the core point of the application. How to obtain the value of reference.

[0053] In common planning based on a point mass model, the longitudinal vehicle speed is often regarded as a constant. The reference states of all states in the prediction time domain are obtained by comparing the integral value of the speed re-planning time domain with the cumulative distance value of the reference points on the reference line.

[0054] However, the planning target in the embodiment contains the vehicle speed. The vehicle speed of the previous frame planning result is taken as the selection basis of the reference point of the current planning.

[0055] The update of the reference trajectory is not directly from the first frame of the planning result of the previous time, but needs to be corrected according to the spatial relationship between the vehicle positioning state of the current period t and the planning trajectory of the previous period t-1.

[0056] Specifically, during the execution of the planning control process of the previous period, the actual position of the vehicle has often advanced several sampling points along the planning trajectory of the previous period.

[0057] Therefore, first, the selection of the first reference point is directly extrapolated by the integral of the positioning speed of the vehicle.

[0058] The acquisition of the subsequent reference points is obtained by linear interpolation. The nearest projection point of the current vehicle position on the planning trajectory of the previous period is determined, and the arc lengths of the point and its adjacent planning points are denoted as and The corresponding planning speed is and The arc length of the current vehicle position is s ego .

[0059] Then, the speed of the reference starting point of the current time i is obtained by linear interpolation of the arc length:

[0060]

[0061] The target result of the planning is a trajectory. Here, it refers to the starting point of the reference trajectory planned at the current time. A trajectory can be found by extrapolating from this starting point. This trajectory is the starting value of the planning, which is iterated from the starting value to the optimal planning trajectory.

[0062] Then, based on the starting point speed, the arc length position and the reference point of the corresponding arc length end in the prediction time domain are recursively calculated at the sampling time interval T s .

[0063]

[0064] For period t, through the above recursive and interpolation calculation, all reference speeds and arc length positions in the planning time domain can be obtained step by step, so as to generate a new reference point sequence

[0065]

[0066] G ir =(X ir ,y ir ,θ ir ,v ir ,k ir ) (5)

[0067] Wherein, r-ref, reference.

[0068] Through the speed extrapolation, the arc length finds all the reference points, and the information carried by these reference points is the state quantity mentioned at the beginning.

[0069] The method realizes the dynamic generation of the reference points by using the speed distribution characteristics of the last period, avoids the prediction inaccuracy problem caused by the traditional constant speed assumption, and improves the continuity and smoothness of the planning trajectory in time and space.

[0070] Step 3, obtaining the vehicle driving cost.

[0071] The vehicle driving cost is determined by G ir , x i and u i ;

[0072]

[0073] L(x i ,u i )=L ref_line +L centri_acc +L centri_jerk +L Δv +L K +L control (7)

[0074] L(x N ,u N )=L ref_line +L centri_acc +L centri_jerk +L Δv +L K (8)

[0075]

[0076] Wherein, q x , q y , q θ , qacc , q jeck , q v , q i , q G are weights in sequence.

[0077] L(x i , u i ) - current time vehicle travel cost;

[0078] The sum of vehicle travel cost from next time vehicle travel cost to terminal time N, that is, the sum of cost to go.

[0079] Definition is the cost expression to be optimized from the current time i to the terminal time N;

[0080] is the optimized cost expression, defined as cost to go.

[0081] Formula (9) includes: reference line close cost L ref_line , lateral acceleration L centri_acc and lateral jerk cost L centri_jerk , speed close reference speed cost L Δv , trajectory curvature cost L K , control amount rate of change cost L control , to ensure smooth, safe and comfortable trajectory.

[0082] Further, the constraints (control amount increment cost, obstacle avoidance cost) will be included in the cost function (formula (7) and formula (8)) in the form of barrier function, to realize the constraint control of vehicle dynamics feasible region and collision safety boundary.

[0083] Trajectory planning is essentially a constrained optimization problem, but since ILQR cannot directly handle constraints, constraints are converted into penalty terms of cost function by constructing barrier function.

[0084] Barrier function should have the following characteristics: when the index meets the constraint, the function value is very close to 0, and once the constraint is violated, it will produce a region infinite penalty value; In addition, it also needs to ensure that the constraint boundary is differentiable.

[0085] This patent considers the physical limit of the control execution control mechanism to be designed as a logarithmic barrier function, and the obstacle avoidance constraint to be designed as an exponential barrier function. Specifically as follows:

[0086] Designed as an exponential barrier function. Specifically as follows:

[0087] The control amount increment cost is:

[0088]

[0089] The cost of obstacle avoidance is:

[0090]

[0091] σ min -Minimum rate of change of curvature;

[0092] σ max - Maximum rate of change of curvature;

[0093] σ-curvature;

[0094] q avoid - The coefficient of obstacle avoidance cost;

[0095] obs - obstacles;

[0096] k-Automobile;

[0097] h-coefficient, which is a constant;

[0098] a - The distance from the vehicle's center of gravity to the front axle;

[0099] b - The distance from the vehicle's center of gravity to the rear axle;

[0100] u - control vector.

[0101] Step 4: With the goal of minimizing the vehicle's driving cost, solve for the optimal control vector deviation at any given time.

[0102] The purpose of step 4 is to use the Bellman optimality principle to backpropagate from the terminal time and recursively calculate the optimal control increment deviation for each time step.

[0103] The fundamental purpose of the algorithm is to minimize the increase in cost. Minimum.

[0104] Define the terminal cost at time N as:

[0105] Performing backpropagation, the cost of optimization. Based on the second-order Taylor expansion, we get:

[0106]

[0107] The coefficients are expressed as follows:

[0108]

[0109] In equation (11),

[0110]

[0111] Setting the gradient of the control increment bias to 0, we obtain the expression for the optimal control vector bias at the current time, as follows:

[0112]

[0113] In equation (13):

[0114]

[0115] Formula (14) is also known as the control strategy.

[0116] Step 5: Repeat steps 1-4 until the control vector deviations at all times are obtained.

[0117] Step 6, Apply to the nominal trajectory to generate the planned trajectory.

[0118] The control strategy obtained in step 4 is applied to the nominal trajectory of the previous cycle to generate the planned trajectory of the current cycle, which is then used as the nominal trajectory of the next cycle. This iterative optimization continues until convergence.

[0119] During forward propagation, a path search is required along the descent direction to ensure sufficient cost reduction. We use parameter α to employ a simple backtracking search, defining the nominal trajectory as... The search process is as follows:

[0120]

[0121] Where x0 is the state vector at the first time step, x start - The first point in the nominal trajectory of the previous plan.

[0122] The function of formula (15) is to apply the control strategy obtained in step 4 to the nominal trajectory of the previous cycle to generate the planned trajectory of the current cycle.

[0123] The acceptance of candidate trajectories and the termination conditions for ILQR iterations are not elaborated here; these are standard procedures and not part of the core of this invention.

[0124] The above are merely preferred embodiments of the present invention and do not constitute any limitation on the present invention. Any equivalent substitutions or modifications made by those skilled in the art to the technical solutions and content disclosed in the present invention without departing from the scope of the present invention shall be deemed to have remained within the protection scope of the present invention.

Claims

1. A spatiotemporal joint trajectory planning method based on CILQR with improved reference point acquisition, characterized in that, Includes the following steps: Step 1: Obtain the kinematic model of the vehicle; The input to the kinematic model includes the state vector x at any given time. i and the control vector u at the current time i Its output includes the vehicle's next-time state vector x. i+1 ; Step 2: Obtain the reference state vector G at any given time. ir ; Step 3: Obtain the vehicle's driving cost; The vehicle's operating cost is determined by G. ir x i and u i Sure; Step 4: With the goal of minimizing the vehicle's driving cost, solve for the optimal control vector deviation at any given time. Step 5: Repeat steps 2-4 until the control vector deviation at all times is obtained. Step 6, put all of them Apply to the nominal trajectory to generate the planned trajectory.

2. The spatiotemporal joint trajectory planning method based on improved reference point acquisition CILQR according to claim 1, characterized in that, Reference state vector G at any time ir for: G ir =(x ir ,y ir ,θ ir ,v ir ,k ir )。 3. The spatiotemporal joint trajectory planning method based on improved reference point acquisition CILQR according to claim 1, characterized in that, The cost of vehicle operation is: L(x i ,u i )=L ref_line +L centri_acc +L centri_jerk +L Δv +L K +L control L(x N ,u N )=L ref_line +L centri_acc +L centri_jerk +L Δv +L K L(x i u i - The cost of vehicle travel at the current moment; - The sum of vehicle travel costs from the next time step to the terminal time N.

4. The spatiotemporal joint trajectory planning method based on improved reference point acquisition CILQR according to claim 1, characterized in that, In step 6:

Images