Track planning method based on iLQR and particle swarm optimization and electronic equipment

Abstract

The invention provides an iLQR and particle swarm optimization-based trajectory planning method and electronic equipment, and the method comprises the steps: firstly obtaining an optimal initial trajectory through an iLQR algorithm according to a state vector of a vehicle, a kinematic model and a constraint condition, the optimal initial trajectory being a trajectory obtained without considering complex scenes such as obstacle avoidance and U-turn; on the basis, the optimal initial trajectory is used as input information of a particle swarm optimization algorithm model, meanwhile, environmental vehicle data and reference line shape data on a road where the vehicle is located are used as input information of a fitness function of the particle swarm optimization algorithm model, and the optimal initial trajectory is further optimized in a particle swarm optimization mode; at the moment, dynamic factors such as environment vehicles, obstacles and lane center lines are considered, and global optimization of the trajectory in a complex scene is achieved.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving path planning, and in particular to a trajectory planning method and electronic equipment based on iLQR and particle swarm optimization. Background Art

[0002] In an autonomous driving system, trajectory planning is one of the key links, especially in scenarios such as U-turns, obstacle avoidance, and lane changes. Trajectory planning needs to ensure that the vehicle is within a safe area, complies with physical dynamic constraints, avoids obstacles, and completes efficient and safe movement.

[0003] The existing technology mainly uses traditional trajectory planning methods such as the A* algorithm based on static maps, the Dijkstra algorithm, the rapidly expanding random tree, etc. to perform path search, or uses iLQR (iterative linear quadratic regulator) as a local trajectory optimization algorithm to generate a smooth trajectory based on vehicle dynamics constraints. Traditional trajectory planning methods are suitable for simple path planning tasks, but when faced with dynamic environments or complex traffic scenes, they often have problems such as low computational efficiency and insufficient smoothness in the generated trajectory. iLQR is difficult to achieve global optimization when considering dynamic environments (such as obstacles and dynamic vehicles). Therefore, traditional planning methods often generate less smooth trajectories in complex scenarios such as turning, U-turns, and obstacle avoidance, affecting the control stability of the vehicle. When using iLQR or particle swarm optimization (PSO), if there is no effective initial solution as a reference, the algorithm often requires more iterations to converge, which increases the calculation time. Summary of the invention

[0004] The technical problem to be solved by the present invention is that the vehicle trajectory in a dynamic or complex environment is not smooth enough or the optimization efficiency is low. To this end, the present invention provides a U-turn trajectory planning method and electronic equipment based on iLQR and particle swarm optimization.

[0005] In a first aspect, the technical solution of the present application provides a trajectory planning method based on iLQR and particle swarm optimization, comprising:

[0006] Acquire a state vector of the vehicle, wherein the state vector includes a driving speed, a driving direction acceleration, a lateral acceleration, and a steering smoothness;

[0007] Establishing a kinematic model of the vehicle according to the state vector;

[0008] Determine the constraints of the driving trajectory;

[0009] Applying the iLQR algorithm model, updating the control input vector of the kinematic model, and making the output vector of the kinematic model satisfy the constraint condition; repeating this step multiple times until the kinematic model outputs the optimal initial trajectory;

[0010] Using the optimal initial trajectory as input information of a particle swarm optimization algorithm model, and using environmental vehicle data and reference line shape data on the road where the vehicle is located as input information of a fitness function of the particle swarm optimization algorithm model;

[0011] The optimized trajectory is obtained according to the output information of the particle swarm optimization algorithm model.

[0012] In some embodiments, the trajectory planning method based on iLQR and particle swarm optimization, the application of the iLQR algorithm model, updating the control input vector of the kinematic model, and making the output vector of the kinematic model satisfy the constraint condition, includes:

[0013] The forward propagation generates the initial trajectory, including:

[0014] Configuring initial control parameters for the iLQR algorithm model, and using the initial state vector as the initial state of the iLQR algorithm model to simulate the forward propagation process;

[0015] According to the initial state vector and the kinematic model, the state vector of the vehicle in each subsequent stage is updated in sequence, and substituted into the iLQR algorithm model to obtain an initial trajectory;

[0016] Back propagation optimizes the control parameters of the iLQR algorithm model, including:

[0017] Correcting the state vector of each track point in the initial track according to the constraint condition to obtain a corrected track point state vector;

[0018] According to the deviation between the state vector of the corrected trajectory point and the state vector of the corresponding trajectory point in the reference line trajectory, the initial control parameters are revised to finally obtain the corrected control parameters of the iLQR algorithm model;

[0019] iLQR algorithm convergence, including:

[0020] After replacing the initial control parameters of the iLQR algorithm model with the modified control parameters, the initial state vector is used as the initial state of the iLQR algorithm model to simulate the forward propagation process;

[0021] Output the best initial trajectory, including:

[0022] Repeat the above process until the change between two adjacent corrected control parameters is less than a set threshold, and determine the final control parameters of the iLQR algorithm model;

[0023] According to the final control parameters, the initial state vector is used as the initial state of the iLQR algorithm model to simulate the forward propagation process, and the iLQR algorithm model outputs the optimal initial trajectory.

[0024] In some embodiments of the trajectory planning method based on iLQR and particle swarm optimization, in establishing the kinematic model of the vehicle according to the state vector:

[0025] The kinematic model is expressed by the following function:

[0026] x k+1 =f( x k,u k );

[0027] Among them, x k represents the kth state vector, u k represents the state transition corresponding to the kth state vector, x k+1 represents the k+1th state vector.

[0028] In some embodiments, the trajectory planning method based on iLQR and particle swarm optimization includes the following constraints for determining the driving trajectory:

[0029] The driving speed should be within the preset driving speed range, expressed as:

[0030] v represents the driving speed, v min and v max Respectively represent the two end point values ​​of the preset driving speed range;

[0031] The driving acceleration should be within the preset driving acceleration range, expressed as:

[0032] a represents the driving acceleration, acc min and acc max Respectively represent the two end point values ​​of the preset driving acceleration range;

[0033] The lateral acceleration should be within the preset lateral acceleration range, expressed as:

[0034] ω represents the heading angle, latacc min and latacc max Respectively represent the two end point values ​​of the preset lateral acceleration range;

[0035] The vehicle heading angle should be within the preset heading angle range, expressed as:

[0036] ω min and ω max Respectively represent the two end points of the preset heading angle range;

[0037] The deviation between the vehicle trajectory point and the corresponding reference point on the lane reference line is within a preset deviation range, which is represented by the distance dis2lane between the vehicle trajectory point and the lane reference line.

[0038] In some embodiments, the trajectory planning method based on iLQR and particle swarm optimization, wherein the output vector of the kinematic model satisfies the constraint condition comprises:

[0039] Establishing driving speed cost function: J vel =12w veldiff ·(vv target ) 2 ;w veldiff represents the driving speed cost weight, v target Indicates the target driving speed;

[0040] Establish driving acceleration cost function: J acc =w acc ·a 2 ;w acc Represents the driving acceleration cost weight;

[0041] Lateral acceleration cost function: J latacc =w latacc ·(ωv) 2 ;w latacc Represents the lateral acceleration cost weight;

[0042] Heading angle cost function: J yawrate =w yawrate ·ω 2 ;w yawrate Represents the heading angle cost weight;

[0043] Target lane deviation cost function: J lane =w lane ·dis2lane 2 ;w lane represents the lane deviation cost weight;

[0044] Establish the total cost function:

[0045] N represents the total number of state vectors, J total The constraint condition is satisfied when the value is the smallest.

[0046] In some embodiments, the trajectory planning method based on iLQR and particle swarm optimization, wherein the output vector of the kinematic model satisfies the constraint condition, further comprises:

[0047] Adjust the weights in each cost function according to the trajectory optimization goal.

[0048] In some schemes, the trajectory planning method based on iLQR and particle swarm optimization is described, wherein the optimal initial trajectory is used as input information of the particle swarm optimization algorithm model, and the environmental vehicle data and reference line shape data on the road where the vehicle is located are used as input information of the fitness function of the particle swarm optimization algorithm model:

[0049] The input information of the fitness function also includes constraint data corresponding to the constraint condition.

[0050] In a second aspect, an embodiment of the present application provides a computer-readable storage medium having a computer program / instruction stored thereon, which, when executed by a processor, implements the steps of the trajectory planning method based on iLQR and particle swarm optimization described in any technical solution of the first aspect.

[0051] In a third aspect, an embodiment of the present application provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the trajectory planning method based on iLQR and particle swarm optimization described in any technical solution of the first aspect.

[0052] In a fourth aspect, an embodiment of the present application provides a computer program product, including a computer program / instruction, which, when executed by a processor, implements the steps of the trajectory planning method based on iLQR and particle swarm optimization described in any technical solution of the first aspect.

[0053] The present invention first obtains the optimal initial trajectory according to the vehicle's state vector, kinematic model and constraints through the iLQR algorithm. The optimal initial trajectory is the trajectory obtained without considering complex scenarios such as avoiding obstacles and U-turns. On this basis, the optimal initial trajectory is used as the input information of the particle swarm optimization algorithm model, and the environmental vehicle data and reference line shape data on the road where the vehicle is located are used as the input information of the fitness function of the particle swarm optimization algorithm model. The optimal initial trajectory is further optimized using the particle swarm optimization method. At this time, dynamic factors such as environmental vehicles, obstacles, and lane center lines are considered to achieve global optimization of the trajectory in complex scenarios. By combining the two optimization algorithms of iLQR and particle swarm optimization, the efficiency and safety of autonomous driving vehicle trajectory planning are significantly improved, especially in complex U-turn dynamic traffic scenarios. Compared with the prior art, the present invention has the following advantages:

[0054] The computational efficiency is significantly improved: Traditional trajectory planning methods often require a large number of iterations to find a feasible solution, while the present invention generates the best initial trajectory through iLQR and uses it as the input of the PSO algorithm, which greatly reduces the number of invalid solutions. The PSO algorithm is more focused in the search process, effectively shortening the computational time. The simulation test proves that the overall time consumption of this solution is reduced by about one third compared with the existing technical solution.

[0055] Improved trajectory smoothness and safety: The optimal initial trajectory generated by the iLQR algorithm not only meets the vehicle dynamics constraints, but also ensures the smoothness of the trajectory. Further optimization through PSO ensures that the vehicle can travel smoothly in complex scenarios such as U-turns and always maintain a safe distance from surrounding vehicles or obstacles, thereby reducing the risk of collision.

[0056] Strong adaptability to dynamic environments: In the PSO algorithm, the input information of the fitness function includes environmental vehicle data and reference line shape data, that is, it takes into account a variety of dynamic environmental factors. Through the global optimization capability of PSO, the vehicle trajectory can adapt to environmental changes in real time. Compared with traditional static planning algorithms, this provides more flexible and safe response capabilities.

[0057] Reduce invalid solutions and improve convergence speed: The optimal initial trajectory generated by the iLQR algorithm is used as the initial reference solution of the PSO algorithm, which significantly reduces the invalid solutions of the PSO algorithm during the search process, thereby improving the convergence speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 Flow chart of a trajectory planning method based on iLQR and particle swarm optimization according to an embodiment of the present invention;

[0059] Figure 2 A schematic diagram showing the probability density function of particle positions in the particle swarm optimization algorithm using triangular distribution;

[0060] Figure 3 , Figure 4 and Figure 5 A schematic diagram of a simulation scenario for a vehicle U-turn arrangement;

[0061] Figure 6 is the optimal initial trajectory obtained after optimizing the trajectory using the iLQR algorithm;

[0062] Figure 7 To use particle swarm optimization algorithm Figure 6 The output result shown is further optimized to obtain the final output trajectory;

[0063] Figure 8 A schematic diagram of the hardware connection relationship of an electronic device that executes the trajectory planning method based on iLQR and particle swarm optimization according to an embodiment of the present invention. DETAILED DESCRIPTION

[0064] The specific implementation of the present application is further described below with reference to the accompanying drawings.

[0065] It is easy to understand that according to the technical solution of the present application, without changing the essential spirit of the present application, a variety of structural modes and implementation modes that can be replaced by those skilled in the art can be replaced with each other. Therefore, the following specific implementation modes and drawings are only exemplary descriptions of the technical solution of the present application, and should not be regarded as the entirety of the present application or as a limitation or restriction on the technical solution of the application.

[0066] The directional terms such as up, down, left, right, front, back, front, back, top, bottom, etc. mentioned or may be mentioned in this specification are defined relative to the structures shown in the drawings. They are relative concepts and may change accordingly according to different positions and different usage conditions. Therefore, these or other directional terms should not be interpreted as restrictive terms.

[0067] This embodiment provides a trajectory planning method based on iLQR and particle swarm optimization, which is applied to a processor. The processor can be set in the vehicle terminal of the autonomous driving vehicle or in a cloud server, such as Figure 1 As shown, the method comprises the following steps:

[0068] S10: Acquire a state vector of the vehicle, wherein the state vector includes a driving speed, a driving direction acceleration, a lateral acceleration, and a steering smoothness.

[0069] The state vector may include a driving state vector and a control input vector. The driving state vector x k Contains the position, velocity, and direction of the vehicle at each time step k. Usually represented as x k =[x,y,θ,v]; where x and y are the horizontal and vertical position coordinates of the vehicle, θ is the direction angle, and v is the driving speed. Based on the above parameters and time, the driving acceleration, lateral acceleration and steering smoothness of the vehicle can be obtained, where the control input vector u k It can include the driving direction acceleration a and the angular velocity ω.

[0070] S20: Establishing a kinematic model of the vehicle according to the state vector;

[0071] In specific implementation, the kinematic model is expressed by the following function:

[0072] x k+1 =f(x k ,u k );

[0073] The above function is a nonlinear difference equation, where x k represents the kth state vector, u k represents the state transition corresponding to the kth state vector, x k+1The k+1th state vector is shown. Because the state of the vehicle does not change suddenly during its driving, the state vectors obtained in two adjacent time steps should satisfy a certain rule, which can be determined based on the vehicle's driving speed, acceleration, angular velocity, etc. The above kinematic model can ensure that the physical behavior of the vehicle meets the kinematic and dynamic constraints of the vehicle.

[0074] S30: Determine the constraint conditions of the driving trajectory.

[0075] The constraint conditions are used to impose physical constraints on the vehicle's driving process, for example, the vehicle cannot exceed the lane boundary when driving, the vehicle's speed cannot exceed the road speed limit when driving, etc.

[0076] S40: applying the iLQR algorithm model, updating the control input vector of the kinematic model, and making the output vector of the kinematic model satisfy the constraint condition; repeating this step multiple times until the kinematic model outputs the optimal initial trajectory.

[0077] The iLQR algorithm model gradually approaches the optimal initial trajectory by repeatedly and iteratively updating the control input parameters. The control input parameters are the control input vectors, that is, the input of the iLQR algorithm model is data in vector form. In the present application scheme, the output of the iLQR algorithm model can be an acceleration and angular velocity sequence, which serves as the input of the particle swarm optimization algorithm model. The two sequences of acceleration and angular velocity represent the control input of the vehicle during the entire planning time period, and describe how the vehicle should adjust the driving speed and driving direction at each moment. Specifically, the acceleration sequence determines how the vehicle accelerates or decelerates in the longitudinal direction, and the angular velocity sequence determines the steering change of the vehicle. These two control input sequences can be used as the optimization results of the iLQR algorithm model, so that the iLQR algorithm model can generate a physically feasible and relatively smooth optimal initial trajectory.

[0078] S50: using the optimal initial trajectory as input information of a particle swarm optimization algorithm model, and using environmental vehicle data and reference line shape data on the road where the vehicle is located as input information of a fitness function of the particle swarm optimization algorithm model.

[0079] The acceleration and angular velocity sequence output by the iLQR algorithm model is used as the initial input of the particle swarm optimization algorithm model, that is, the "reference solution" of the particle swarm optimization algorithm model. The particle swarm optimization algorithm is further optimized on this basis to cope with complex dynamic environments, such as obstacles, oncoming vehicles, etc. The particle swarm optimization algorithm model uses the acceleration and angular velocity sequence provided by the iLQR algorithm model as the initial solution for its particles, allowing the particle swarm optimization algorithm to search for the optimal solution more centrally and efficiently, reducing invalid solutions in the search space.

[0080] S60: Obtaining an optimized trajectory according to the output information of the particle swarm optimization algorithm model.

[0081] The above scheme utilizes a phased trajectory optimization method to not only improve efficiency but also ensure that the optimized trajectory can be executed safely and smoothly in complex traffic scenarios.

[0082] The solution in this embodiment is based on the combination of iLQR algorithm and particle swarm optimization. When applied to trajectory planning of autonomous driving vehicles, it is particularly suitable for complex scenarios such as U-turns. The optimal initial trajectory is generated by the iLQR algorithm to ensure that the trajectory meets the vehicle dynamics constraints and reduces the number of invalid solutions. Subsequently, the particle swarm optimization algorithm is used for global optimization to generate a safe, smooth trajectory that meets environmental constraints.

[0083] In specific implementation, the iLQR algorithm consists of two parts: forward propagation to generate the initial trajectory, and back propagation to calculate the feedback gain and adjust the control input and algorithm convergence. The specific process is as follows:

[0084] (1) Forward propagation generates the initial trajectory, including:

[0085] In the initial stage, the iLQR algorithm starts with a given initial state vector x0 and propagates forward according to the current control input sequence u0,u1,…,u N-1 Compute the state x at each time step k The specific steps of this process are as follows:

[0086] Initialization: The iLQR algorithm model configures initial control parameters, and uses the initial state vector as the initial state of the iLQR algorithm model to simulate the forward propagation process; the initial control parameters can be set arbitrarily and can be selected based on empirical values.

[0087] State update: According to the initial state vector and the kinematic model, the state vector of the vehicle in each subsequent stage is updated in sequence, and substituted into the iLQR algorithm model to obtain the initial trajectory; that is, the vehicle kinematic model x determined above is used k+1 =f(x k ,u k ), gradually update the state vector x of the vehicle at each time step k , the state vector of any time step can be determined based on the state vector of the previous time step and the control input vector.

[0088] An initial trajectory can be generated at this stage, but the initial trajectory generated at this stage is obtained through a fixed control input vector and is not necessarily the optimal initial trajectory. The next process is performed on this basis.

[0089] (2) Back propagation optimizes the control parameters of the iLQR algorithm model, including:

[0090] In the back propagation phase, the feedback gain matrix K and the feedforward correction term d are calculated by performing a quadratic approximation on the current initial trajectory (including the linearization of the driving state vector and the control input vector), and the control input vector is adjusted based on this information so that the initial trajectory gradually approaches the optimal one. This process mainly includes:

[0091] According to the constraint condition, the state vector of each track point in the initial track is corrected to obtain the corrected track point state vector. In specific implementation, the constraint condition is defined in the form of a cost function, and the cost function is used to define the running cost of each time step. The smaller the running cost, the better the track.

[0092] According to the deviation between the state vector of the corrected trajectory point and the state vector of the corresponding trajectory point in the reference line trajectory, the initial control parameters are revised to finally obtain the corrected control parameters of the iLQR algorithm model. During the back propagation process, the iLQR algorithm will perform quadratic linearization on the kinematic model and the cost function, and calculate the gradient and Hessian matrix of the driving state vector and the control input vector respectively. k and the control input vector u k For example, the second-order Taylor expansion of the cost function is as follows:

[0093]

[0094] Among them, Q xx , Q uu , Q ux , Q xu Represent the second-order derivatives of the driving state vector and the control input vector respectively.

[0095] Feedback gain and feedforward term calculation:

[0096] Through back propagation, the iLQR algorithm calculates the feedback gain matrix K for each step length based on the linearized cost function. k and the feedforward correction term d k These corrections are used to adjust the current control input vector so that the vehicle trajectory approaches the optimal direction. The update formula of the control input vector is as follows:

[0097]

[0098] in, is the deviation between the state vector of the trajectory point and the state vector of the corresponding trajectory point in the reference line trajectory, K k is the feedback gain matrix, d k is the feed-forward correction term.

[0099] (3) iLQR algorithm convergence, including:

[0100] After replacing the initial control parameters of the iLQR algorithm model with the modified control parameters, the initial state vector is used as the initial state of the iLQR algorithm model to simulate the forward propagation process. That is, after the backpropagation is completed, the iLQR algorithm performs forward propagation again to complete the update of the forward propagation, and uses the updated control input sequence u k , forward propagation is performed again to calculate the new initial trajectory.

[0101] (4) Output the optimal initial trajectory, including:

[0102] The above process is iterated repeatedly until the change between two adjacent corrected control parameters is less than a set threshold, and the final control parameters of the iLQR algorithm model are determined.

[0103] According to the final control parameters, the initial state vector is used as the initial state of the iLQR algorithm model to simulate the forward propagation process, and the iLQR algorithm model outputs the optimal initial trajectory.

[0104] Through the above scheme, the iLQR algorithm model finally outputs an optimal initial trajectory that is smooth and satisfies the vehicle dynamics constraints.

[0105] In specific implementation, the core of the iLQR algorithm is to construct a reasonable cost function to measure the quality of the output trajectory, that is, determining appropriate constraints and setting the corresponding cost function is the key to trajectory optimization. The constraints for determining the driving trajectory described in the embodiments of the present application include:

[0106] (1) The driving speed should be within the preset driving speed range, expressed as:

[0107] v represents the driving speed, v min and v max They respectively represent the two end points of the preset driving speed range; ensuring that the vehicle's driving speed is within the preset range to avoid overspeeding or underspeeding.

[0108] (2) The driving acceleration should be within the preset driving acceleration range, expressed as:

[0109] a represents the driving acceleration, acc min and acc max They respectively represent the two end point values ​​of the preset driving acceleration range; they limit the driving acceleration of the vehicle so that it varies within the physically feasible range.

[0110] (3) The lateral acceleration should be within the preset lateral acceleration range, expressed as:

[0111] ω represents the heading angle, latacc min and latacc max They respectively represent the two end point values ​​of the preset lateral acceleration range; they control the lateral acceleration of the vehicle to avoid excessive centrifugal force during the turning process.

[0112] (4) The vehicle heading angle should be within the preset heading angle range, expressed as:

[0113] ω min and ω max They respectively represent the two end points of the preset heading angle range; they limit the steering angular velocity of the vehicle to prevent the vehicle from turning too sharply.

[0114] (5) The deviation between the vehicle trajectory point and the corresponding reference point on the lane reference line is within a preset deviation range, represented by the distance dis2lane between the vehicle trajectory point and the lane reference line.

[0115] Furthermore, making the output vector of the kinematic model satisfy the constraint condition includes:

[0116] (1) Establishing the driving speed cost function: J vel =12w veldiff ·(vv target ) 2 ;w veldiff represents the driving speed cost weight, v target Represents the target driving speed; by penalizing the difference between the vehicle's current driving speed and the target driving speed, the vehicle gradually approaches the set target driving speed.

[0117] (2) Establishing driving acceleration cost function: J acc =w acc ·a 2 ;w acc Represents the driving acceleration cost weight; by penalizing the square value of the driving acceleration, the vehicle's driving acceleration is constrained to make it smooth and reasonable.

[0118] (3) Lateral acceleration cost function: J latacc =w latacc ·(ωv) 2 ;w latacc Represents the lateral acceleration cost weight; by penalizing the lateral acceleration (i.e., the product of the vehicle's steering angular velocity and driving speed), the vehicle's steering is ensured to be smooth and lateral vibration is reduced.

[0119] (4) Heading angle cost function: J yawrate =w yawrate ·ω2 ;w yawrate Represents the heading angle cost weight; controls the vehicle's steering acceleration and ensures that the vehicle's steering does not change excessively drastically by penalizing the square of the steering angular velocity.

[0120] (5) Target lane deviation cost function: J lane =w lane ·dis2lane 2 ;w lane Represents the lane deviation cost weight; ensures that the vehicle travels along the target lane as much as possible and reduces the degree to which the vehicle deviates from the center line of the lane.

[0121] Based on the above cost functions, the total cost function is established: N represents the total number of state vectors, J total The constraint is satisfied when the value is minimum. The iLQR algorithm optimizes the trajectory by minimizing the total cost function of the vehicle in the entire trajectory through multiple iterations while satisfying all constraints. In the present application, the cost function not only takes into account the smoothness of the trajectory and the vehicle dynamics constraints, but also introduces road constraints. By defining multiple cost functions, each cost function reflects different optimization goals when the vehicle trajectory is generated. These cost functions are repeatedly iterated by the iLQR algorithm during the trajectory optimization process to balance the smoothness of the vehicle's motion trajectory and the degree of goal achievement.

[0122] Furthermore, making the output vector of the kinematic model satisfy the constraint condition also includes: adjusting the weights in each cost function according to the trajectory optimization goal. In the definition of the cost function, each cost term has a corresponding weight parameter. These weights reflect the importance of different goals and are usually adjusted through experiments or experience. The size of the weight directly affects the balance strategy in the optimization process of the iLQR algorithm. For example: increasing w acc Prioritizes the smoothness of longitudinal acceleration, but may increase the risk of the vehicle deviating from the lane; increasing w lane This will force the vehicle to stay in the center of the lane, but may cause the vehicle to slow down or turn frequently.

[0123] On the basis of the above scheme, the optimal initial trajectory is used as the input information of the particle swarm optimization algorithm model, and the environmental vehicle data and reference line shape data on the road where the vehicle is located are used as the input information of the fitness function of the particle swarm optimization algorithm model: the input information of the fitness function also includes the constraint data corresponding to the constraint condition. That is, the particle swarm optimization algorithm model further considers the constraint conditions defined in the iLQR algorithm to further improve the smoothness of the optimized trajectory in complex scenarios.

[0124] In this embodiment, after the iLQR algorithm model generates the optimal initial trajectory, the particle swarm optimization algorithm takes over and performs global optimization on the optimal initial trajectory. This process gradually searches for the final optimized trajectory through trajectory evaluation and control input vector update. The main process of the particle swarm optimization algorithm includes:

[0125] (1) Particle swarm initialization: When each particle in particle swarm optimization is initialized, the optimal initial trajectory generated by the iLQR algorithm model is used as the reference initial solution. This greatly reduces the invalid solutions in the search space of the particle swarm optimization algorithm and ensures that the particles can search effectively in the solution space. In the particle initialization process of particle swarm optimization, taking driving acceleration as an example, the optimal initial trajectory generated by the iLQR algorithm is used as a reference. Figure 2 The triangular distribution shown in the figure generates a new solution. Specifically, the driving acceleration position of the particle is not generated completely randomly, but takes the initial driving acceleration calculated by the iLQR algorithm as the expected value, and sets an interval containing the minimum acceleration a, the expected acceleration c, and the maximum acceleration b. This interval defines the possible range of values ​​of the particle acceleration, where the expected acceleration represents a reasonable control target for vehicle dynamics. In order to make the generated particle acceleration closer to this expected value, the probability density function shown in the triangular distribution is applied. The characteristic of this distribution is that the probability density is the highest at the expected value, while the probability density at both ends of the interval is lower. In this way, the acceleration value of the particle is more likely to be close to the initial solution provided by the iLQR algorithm, ensuring that during the particle swarm optimization search process, the particles are mainly concentrated near the expected trajectory, reducing the generation of invalid solutions that do not meet physical constraints or are unreasonable, and accelerating the convergence speed of the optimization.

[0126] (2) Fitness function evaluation: The trajectory of each particle is evaluated by the fitness function, which takes into account the following aspects:

[0127] Safety: The obstacle detection function is used to evaluate whether the vehicle trajectory will collide with an obstacle. If a collision occurs, the fitness will be greatly reduced.

[0128] Dynamic constraints: whether the particle's acceleration and angular velocity comply with the vehicle's physical constraints.

[0129] Smoothness: The smoothness of the trajectory is measured by the rate of change of acceleration and angular velocity. Excessive changes will reduce fitness.

[0130] (3) Particle update and convergence: The particle swarm optimization algorithm updates the position of particles, i.e., the control input vector, through three main items:

[0131] Inertia term: maintains the particle's current search direction.

[0132] Cognitive item: Modification based on the particle’s own historical optimal solution.

[0133] Social term: Modified according to the global optimal solution of the particle swarm.

[0134] The updated particles will recalculate the trajectory according to the fitness function. After multiple iterations, the particle swarm gradually converges to the optimal solution, that is, the optimized trajectory.

[0135] In the above embodiment, it is only necessary to determine the optimal initial trajectory and the input information of the fitness function to obtain the optimal trajectory.

[0136] In order to prove the effect of the above solution of the present invention, a simulation verification was carried out. Figures 3 to 5 As shown in the figure, the verification scenario is set under complex traffic conditions with dynamic environment vehicles, simulating the process of an autonomous driving vehicle (green vehicle) performing a U-turn in a narrow U-turn lane in the middle of a main road. The main goal of the test verification is to evaluate the performance of the method in actual scenarios, including trajectory smoothness, safety, satisfaction of vehicle dynamics constraints, and performance in a dynamic environment. In the simulation scenario, there is a narrow lane in the middle of the main road that is only used for U-turns. The autonomous driving vehicle (green vehicle) is at the entrance of the U-turn lane and is preparing to make a U-turn. There is an environment vehicle (yellow vehicle) on the opposite lane driving at a slower speed, which requires the autonomous driving vehicle to maintain a safe distance from the environment vehicle while completing the U-turn and complete the operation at a reasonable speed.

[0137] Initial solution generation: First, the iLQR algorithm is used to generate a rough optimal initial trajectory. This optimal initial trajectory only considers the kinematic constraints of the road and the vehicle, and does not include factors such as obstacles, road speed limits, or lane centerlines. In the generated optimal initial trajectory, the autonomous vehicle slows down and enters the U-turn lane, but because the dynamic environment is not fully considered, the distance between the vehicle and the oncoming vehicle after the U-turn is completed is too close, that is, the distance between the green vehicle and the yellow vehicle is too close, such as Figure 4 As shown, the speed increase is insufficient, which poses a safety hazard. This situation is very dangerous in actual driving.

[0138] Particle swarm optimization: Particle swarm optimization performs global optimization based on the optimal initial trajectory generated by iLQR. By introducing constraint information such as the environment vehicle, lane shape, lateral acceleration, and turning radius as the input of the fitness function, the particle swarm optimization algorithm optimizes the optimal initial trajectory. The speed and position of the vehicle in the U-turn area are adjusted, so that the autonomous driving vehicle can complete efficient and smooth U-turn operations while ensuring safety. The final trajectory generated by the particle swarm optimization algorithm ensures that the vehicle always maintains a safe distance from the oncoming vehicle. When the U-turn is about to be completed, the vehicle accelerates and smoothly drives out of the U-turn lane, always maintaining a safe distance from the oncoming vehicle. Figure 5As shown in the figure, the distance between the green vehicle and the yellow vehicle is significantly increased, and the dangerous situation of too low speed is avoided. The optimized trajectory not only meets the vehicle dynamics constraints, but also keeps close to the center line of the lane as much as possible, and the trajectory is smooth, avoiding the occurrence of dangerous situations.

[0139] Figure 6 and Figure 7 The trajectory of the autonomous driving vehicle (blue line), the trajectory of the oncoming vehicle (yellow line), the centerline of the U-turn lane (light purple surface), and the scattered points on the centerline (red points, the input of the algorithm) are given in the space-time three-dimensional space. From the comparison in the figure, it can be determined that in the optimal initial trajectory, the distance between the autonomous driving vehicle and the oncoming vehicle is very close before the intersection, and the speed is not increased in time. After particle swarm optimization, the autonomous driving vehicle always maintains a safe distance from the oncoming vehicle and the speed is faster, thus achieving a safe and efficient U-turn operation.

[0140] The simulation results show that by introducing the optimal initial trajectory generated by the iLQR algorithm before particle swarm optimization, the computational efficiency of the algorithm has been significantly improved, and the overall time consumption has been reduced by about one third. This is because the optimal initial trajectory generated by iLQR reduces the number of invalid solutions in the particle swarm algorithm and provides a higher quality initial solution for the particle swarm algorithm, thereby accelerating the convergence speed and improving the quality of the trajectory. Through simulation verification, the trajectory planning method of the present invention can achieve safe and efficient trajectory generation in complex U-turn scenarios, significantly reduce invalid solutions, improve overall computational efficiency, and has high practical value.

[0141] In some embodiments of the present invention, a computer-readable storage medium is provided, on which a computer program / instruction is stored. When the computer program / instruction is executed by a processor, the steps of the trajectory planning method based on iLQR and particle swarm optimization described in any of the above solutions are implemented.

[0142] In some embodiments of the present invention, a computer program product is provided, including a computer program / instruction, characterized in that when the computer program / instruction is executed by a processor, the steps of the trajectory planning method based on iLQR and particle swarm optimization described in any of the above schemes are implemented.

[0143] The present application also provides an electronic device, such as Figure 8As shown, the electronic device includes at least one processor 81 and at least one memory 82, at least one of the memories 82 stores program information, and at least one of the processors 81 reads the program information and executes the trajectory planning method based on iLQR and particle swarm optimization described in any of the above method embodiments. The device may also include: an input device 83 and an output device 84. The processor 81, the memory 82, the input device 83 and the output device 84 can be connected in communication. The memory 82, as a non-volatile computer-readable storage medium, can be used to store non-volatile software programs, non-volatile computer executable programs and modules. The processor 81 executes various functional applications and data processing by running the non-volatile software programs, instructions and modules stored in the memory 82, that is, realizing the trajectory planning method based on iLQR and particle swarm optimization provided by any of the above schemes. The memory 82 may include a program storage area and a data storage area, wherein the program storage area may store an operating system and an application required by at least one function; the data storage area may store data created according to the use of the trajectory planning method based on iLQR and particle swarm optimization, etc. In addition, the memory 82 may include a high-speed random access memory, and may also include a non-volatile memory, such as at least one disk storage device, a flash memory device, or other non-volatile solid-state storage devices. In some embodiments, the memory 82 may optionally include a memory remotely arranged relative to the processor 81, and these remote memories may be connected to a device for executing a trajectory planning method based on iLQR and particle swarm optimization through a network. Examples of the above-mentioned network include but are not limited to the Internet, an intranet, a local area network, a mobile communication network, and a combination thereof. The input device 83 may receive input user clicks, and generate signal inputs related to user settings and function controls of the trajectory planning method based on iLQR and particle swarm optimization. The output device 84 may include a display device such as a display screen. When the one or more modules are stored in the memory 82, when the one or more processors 81 are run, the trajectory planning method based on iLQR and particle swarm optimization in any of the above-mentioned method embodiments is executed.

[0144] As needed, the above technical solutions can be combined to achieve the best technical effect.

[0145] The above are only the principles and preferred embodiments of the present application. It should be noted that, for ordinary technicians in this field, on the basis of the principles of the present application, several other modifications can be made, which should also be considered as the protection scope of the present application.

Claims

1. A trajectory planning method based on iLQR and particle swarm optimization, characterized in that: include: Acquire a state vector of the vehicle, wherein the state vector includes a driving speed, a driving direction acceleration, a lateral acceleration, and a steering smoothness; Establishing a kinematic model of the vehicle according to the state vector; Determine the constraints of the driving trajectory; Applying the iLQR algorithm model, updating the control input vector of the kinematic model, and making the output vector of the kinematic model satisfy the constraint condition; Repeat this step multiple times until the kinematic model outputs the best initial trajectory; Using the optimal initial trajectory as input information of a particle swarm optimization algorithm model, and using environmental vehicle data and reference line shape data on the road where the vehicle is located as input information of a fitness function of the particle swarm optimization algorithm model; The optimized trajectory is obtained according to the output information of the particle swarm optimization algorithm model.

2. The trajectory planning method based on iLQR and particle swarm optimization according to claim 1, characterized in that: The applying the iLQR algorithm model to update the control input vector of the kinematic model and make the output vector of the kinematic model satisfy the constraint condition includes: The forward propagation generates the initial trajectory, including: Configuring initial control parameters for the iLQR algorithm model, and using the initial state vector as the initial state of the iLQR algorithm model to simulate the forward propagation process; According to the initial state vector and the kinematic model, the state vector of the vehicle in each subsequent stage is updated in sequence, and substituted into the iLQR algorithm model to obtain an initial trajectory; Back propagation optimizes the control parameters of the iLQR algorithm model, including: Correcting the state vector of each track point in the initial track according to the constraint condition to obtain a corrected track point state vector; According to the deviation between the state vector of the corrected trajectory point and the state vector of the corresponding trajectory point in the reference line trajectory, the initial control parameters are revised to finally obtain the corrected control parameters of the iLQR algorithm model; iLQR algorithm convergence, including: After replacing the initial control parameters of the iLQR algorithm model with the modified control parameters, the initial state vector is used as the initial state of the iLQR algorithm model to simulate the forward propagation process; Output the best initial trajectory, including: Repeat the above process until the change between two adjacent corrected control parameters is less than a set threshold, and determine the final control parameters of the iLQR algorithm model; According to the final control parameters, the initial state vector is used as the initial state of the iLQR algorithm model to simulate the forward propagation process, and the iLQR algorithm model outputs the optimal initial trajectory.

3. The trajectory planning method based on iLQR and particle swarm optimization according to claim 2, characterized in that: In establishing the kinematic model of the vehicle according to the state vector: The kinematic model is expressed by the following function: x k+1 =f(x k ,u k ); Among them, x k represents the kth state vector, u k represents the state transition corresponding to the kth state vector, x k+1 represents the k+1th state vector.

4. The trajectory planning method based on iLQR and particle swarm optimization according to claim 2, characterized in that: The constraints for determining the driving trajectory include: The driving speed should be within the preset driving speed range, expressed as: v represents the driving speed, v min and v max Respectively represent the two end point values ​​of the preset driving speed range; The driving acceleration should be within the preset driving acceleration range, expressed as: a represents the driving acceleration, acc min and acc max Respectively represent the two end point values ​​of the preset driving acceleration range; The lateral acceleration should be within the preset lateral acceleration range, expressed as: ω represents the heading angle, latacc min and latacc max Respectively represent the two end point values ​​of the preset lateral acceleration range; The vehicle heading angle should be within the preset heading angle range, expressed as: ω min and ω max Respectively represent the two end points of the preset heading angle range; The deviation between the vehicle trajectory point and the corresponding reference point on the lane reference line is within a preset deviation range, which is represented by the distance dis2lane between the vehicle trajectory point and the lane reference line.

5. The trajectory planning method based on iLQR and particle swarm optimization according to claim 4, characterized in that: The step of making the output vector of the kinematic model satisfy the constraint condition comprises: Establish the driving speed cost function: J vel =12w veldiff ·(v-v target ) 2 ; w veldiff represents the driving speed cost weight, v target Indicates the target driving speed; Establish the driving acceleration cost function: J acc =w acc a 2 ; w acc Represents the driving acceleration cost weight; Lateral acceleration cost function: J latacc =w latacc ·(ωv) 2 ; w latacc Represents the lateral acceleration cost weight; Heading angle cost function: J yawrate =w yawrate ·oh 2 ; w yawrate Represents the heading angle cost weight; Target lane deviation cost function: J lane =in lane ·dis2lane 2 ; w lane represents the lane deviation cost weight; Establish the total cost function: N represents the total number of state vectors, J total The constraint condition is satisfied when the value is the smallest.

6. The trajectory planning method based on iLQR and particle swarm optimization according to claim 5, characterized in that: The step of making the output vector of the kinematic model satisfy the constraint condition further includes: Adjust the weights in each cost function according to the trajectory optimization goal.

7. The trajectory planning method based on iLQR and particle swarm optimization according to any one of claims 1 to 6, characterized in that: The optimal initial trajectory is used as input information of the particle swarm optimization algorithm model, and the environmental vehicle data and reference line shape data on the road where the vehicle is located are used as input information of the fitness function of the particle swarm optimization algorithm model: The input information of the fitness function also includes constraint data corresponding to the constraint condition.

8. A computer-readable storage medium having a computer program / instruction stored thereon, characterized in that: When the computer program / instruction is executed by a processor, the steps of the trajectory planning method based on iLQR and particle swarm optimization described in any one of claims 1 to 7 are implemented.

9. An electronic device comprising a memory, a processor and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the trajectory planning method based on iLQR and particle swarm optimization according to any one of claims 1 to 7.

10. A computer program product comprising a computer program / instructions, characterized in that When the computer program / instruction is executed by a processor, the steps of the trajectory planning method based on iLQR and particle swarm optimization described in any one of claims 1 to 7 are implemented.

Images