Trajectory optimization method based on vehicle kinematic model predictive control

Abstract

The present invention provides a trajectory optimization method based on vehicle kinematic model predictive control. Starting from a basic vehicle kinematic model (single-vehicle model), the present invention combines model predictive control (MPC) methods to establish an MPC-based trajectory optimization model. Based on the reference trajectory generated by the upstream planning module, an optimization method can be used to calculate a smooth trajectory that meets the vehicle kinematics. The final calculation result meets the vehicle control requirements. At the same time, the invention has a good operating speed and can be applied to intelligent driving vehicles.

Description

Technical Field

[0001] The present invention relates to the field of intelligent driving, and in particular to a trajectory optimization method based on vehicle kinematic model predictive control. Background Art

[0002] With the continuous development of intelligent and autonomous vehicles, more and more planning algorithms are being applied to them. Planning control is a key component of intelligent vehicle technology, and the quality of this algorithm directly affects the comfort, safety, and real-time performance of intelligent vehicles.

[0003] The planning task for intelligent vehicles is to find the optimal trajectory for the vehicle in both space and time. This makes the planning problem NP-hard (Non-deterministic polynomial-hard). In other words, motion planning for intelligent vehicles is a problem for which polynomial-time algorithms are either impossible or difficult to find. Directly solving the trajectory would impose a significant computational burden. As the space required to find the optimal solution increases, the time required to find the optimal solution also increases. This ultimately leads to sluggish responsiveness, which is clearly unacceptable during driving. To resolve this conflict, existing planning algorithms no longer search for the optimal trajectory simultaneously in both time and space. Instead, they divide the planning task into path planning and trajectory planning. Path planning is essentially a search problem. Path planning does not consider the time dimension; its task is to generate a collision-free path from the starting point to the end point in the current free space. Trajectory generation, on the other hand, considers the time dimension and aims to generate a trajectory that can be completed within a certain timeframe based on the given path. To further speed up calculations, existing planning algorithms decouple the intelligent vehicle's lateral and longitudinal directions, calculating lateral and longitudinal trajectories separately and then matching them to form the final trajectory. However, this approach cannot prevent the resulting trajectory from being inapplicable to real-world vehicle motion control or exhibiting severe jitter, such as when encountering a reference line with a large radius of curvature or in areas with complex road conditions. This is because, while most planning algorithms treat the vehicle as a rigid body during collision detection, they fail to consider its kinematic and dynamic constraints. Real-world vehicle motion must comply with the Ackerman steering principle and various constraints. When running existing planning algorithms on experimental vehicles, the chassis' lateral control response in lane change scenarios exhibits significant delays, ranging from 1.2 to 2 seconds. This means that after the lane change trajectory is planned, the chassis response cannot keep up with the lateral plan. This corresponds to a real-world phenomenon where the steering wheel is turned sharply, resulting in an emergency lane change.

[0004] Existing trajectory generation solutions can be divided into those based on polynomial interpolation and curve fitting, those based on optimization algorithms, and those based on deep learning and reinforcement learning. Among them, trajectory generation solutions based on polynomial interpolation and curve fitting have problems such as non-smooth trajectories or higher-order trajectories, and non-compliance with vehicle motion dynamics constraints. Optimization-based trajectory generation solutions require manual definition of the optimization objective function and related constraints, and the final optimization results depend on the quality of these definitions. Trajectory generation solutions based on deep learning and reinforcement learning require extensive offline learning and may be unable to handle scenarios not covered in the learning. Summary of the Invention

[0005] To address these issues, the present invention uses a basic vehicle kinematic model (single-vehicle model) and incorporates Model Predictive Control (MPC) to establish a trajectory optimization model based on vehicle kinematics. Based on the reference trajectory generated by the upstream planning module, an optimization method is used to calculate a smooth trajectory that satisfies the vehicle's kinematics.

[0006] The present invention provides a trajectory optimization method based on vehicle kinematic model predictive control, the process of the trajectory optimization method includes:

[0007] Step S1, retrieving the reference trajectory generated from upstream;

[0008] Step S2, retrieving environmental information;

[0009] Step S3, judging whether the reference trajectory needs algorithm optimization based on the obtained environmental information, if not, proceeding to step S4, if yes, proceeding to step S5;

[0010] Step S4, sending the reference trajectory to the vehicle controller;

[0011] Step S5, inferring the future motion position based on the vehicle kinematic model, establishing the objective function using the model predictive control method, and taking into account the constraints to obtain the optimal control variable relative to the reference trajectory at the current moment;

[0012] Step S6, optimizing the trajectory using the optimal control amount to obtain the final trajectory;

[0013] Step S7: Send the final trajectory to the vehicle controller.

[0014] In one embodiment, step S5 further comprises:

[0015] Step S51, reading steering delay calibration data;

[0016] Step S52: Initialize model constraint parameters.

[0017] In one embodiment, the environmental information in step S3 includes curvature and geographical information.

[0018] In one embodiment, the constraint conditions in step S5 are determined by actuator performance, driving status, and geographic information of the vehicle platform used;

[0019] The actuator performance refers to the lateral response delay, which can be measured by step signal and sine wave signal experiments;

[0020] The driving state can be obtained by sensors of the vehicle's automatic driving platform;

[0021] The geographic information refers to road markings or curb information, which can be obtained from a high-precision map.

[0022] In one embodiment, in step S1, the reference trajectory is composed of a trajectory point set R obtained by the upstream planning algorithm, and the trajectory point set R includes a number of reference trajectory points, and the reference trajectory points contain the expected information at each moment, and the expected information includes the vehicle position coordinates (x, y) in the geodetic coordinate system, the speed v, the acceleration a, the heading angle Time t and curvature κ, the trajectory point set R is expressed as:

[0023]

[0024] In one embodiment, the final trajectory in step S6 is composed of a trajectory point set S, wherein the trajectory point set S includes a number of final trajectory points, and the final trajectory points include the expected information at each moment, wherein the expected information includes the vehicle position coordinates (x, y) in the geodetic coordinate system, the velocity v, the acceleration a, the heading angle Time t and curvature κ, the trajectory point set S is expressed as:

[0025]

[0026] In one embodiment, the trajectory optimization is to optimize each trajectory point, and the trajectory point set S is obtained by filling the optimized trajectory points into the trajectory point set R. The filling method adopts the time alignment method, and the time t before and after the optimization of each trajectory point remains unchanged.

[0027] In one embodiment, the future motion position is calculated according to the vehicle kinematic model in step S5, and a two-degree-of-freedom kinematic model is obtained based on the vehicle kinematic model, and a state equation is established according to the forward Euler method to update the position coordinates (x, y) of the center of mass of the front wheel of the vehicle in the geodetic coordinate system and the orientation angle of the vehicle. The state quantities of the vehicle's front wheel mass center velocity v and the vehicle's front wheel mass center acceleration a at the next moment are as follows:

[0028]

[0029] Where subscript k represents the current moment, subscript k+1 represents the next moment, δ represents the steering angle of the front wheels of the vehicle, l represents the wheelbase of the vehicle, j represents the impact degree, which refers to the rate of change of acceleration, and Δt is the sampling time within a unit cycle.

[0030] In one embodiment, the objective function and constraints in step S5 are as follows:

[0031]

[0032] Among them, f represents the objective function, w1, w2, w3, w4, and w5 are the weight coefficients of each quadratic term, (x ref ,y ref ),v ref 、a ref They represent the position, velocity, and acceleration of the reference trajectory point in the reference trajectory R at a specific moment in the earth coordinate system, j represents the impact degree, X represents the state vector, A represents the system matrix after linearization near the reference trajectory point, B represents the input matrix, U represents the input vector, subscript k represents the current moment, and subscript k+1 represents the next moment;

[0033] In the constraint conditions of the objective function, Θ represents the chassis response angle set at each sampling moment obtained through experiments under a certain speed condition, k represents the number of sampling times in a unit cycle, Δt represents the sampling time in a unit cycle, and t delay represents the delay of the vehicle's lateral control, δ represents the front wheel steering angle of the vehicle, and δ min , δ max Represent the minimum and maximum values ​​of the current front wheel steering angle constraint, v min 、v max Represent the minimum and maximum values ​​of the current vehicle front wheel mass center speed, a min 、a max Represent the minimum and maximum values ​​of the current vehicle front wheel mass center acceleration, x geofence_min 、x geofence_max 、y geofence_min 、y geofence_max Respectively represent the minimum and maximum values ​​of the area where the car can drive in the x and y directions on the current road.

[0034] In one embodiment, the optimization trajectory in step S6 is:

[0035] According to formula (2), the position coordinates (x, y) of the center of mass of the front wheel of the vehicle in the geodetic coordinate system, the speed v of the center of mass of the front wheel of the vehicle, the acceleration a of the center of mass of the front wheel of the vehicle, and the impact degree j are calculated as the optimal control quantity;

[0036] Substitute the optimal control amount into equation (1) to generate the final trajectory.

[0037] The present invention has the following beneficial effects:

[0038] 1. This vehicle kinematic model-based trajectory optimization method takes into account the response delay of chassis lateral control and utilizes steering data calibrated from real vehicles to optimize a predictable planned trajectory. The resulting optimized trajectory smooths the impact of vehicle steering and avoids dangerous maneuvers.

[0039] 2. The present invention uses a suitable cost function as the objective function of the model predictive control, taking into account the vehicle's expected speed, expected acceleration and trajectory impact. The final output trajectory can be directly transmitted to the lower control layer.

[0040] 3. The optimization algorithm of this patent is based on the solution of lateral and longitudinal coupling. It solves the pain point mentioned above that the planning based on lateral and longitudinal decoupling cannot fully meet the Ackerman steering principle and vehicle kinematics. The trajectory optimization method based on vehicle kinematic model predictive control of the present invention is based on the kinematics of the car, ensuring that the final generated trajectory meets the kinematic constraints while avoiding repeated calculations. The optimization algorithm is based on optimal control and state equations, and can simultaneously generate smooth lateral and longitudinal trajectories, ensuring that the autonomous vehicle can travel stably and smoothly. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 A flow chart of a trajectory optimization method based on vehicle kinematic model predictive control according to an embodiment of the present invention is disclosed;

[0042] Figure 2 A schematic diagram of a vehicle kinematic model is disclosed, which illustrates a trajectory optimization method based on vehicle kinematic model predictive control according to an embodiment of the present invention;

[0043] Figure 3 A schematic diagram of a lane change scenario of a trajectory optimization method based on vehicle kinematic model predictive control according to an embodiment of the present invention is disclosed; and

[0044] Figure 4 A comparison diagram of trajectory data before and after algorithm optimization of a trajectory optimization method based on vehicle kinematic model predictive control according to an embodiment of the present invention is disclosed. DETAILED DESCRIPTION

[0045] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the invention and are not intended to limit the invention.

[0046] The present invention proposes a trajectory optimization method based on vehicle kinematic model predictive control. The optimization algorithm of the present invention receives the planned trajectory sent from upstream and determines whether algorithm optimization is required based on conditions such as trajectory curvature and geographic information. If optimization is required, the future motion position is calculated based on the vehicle motion model, and a suitable cost function is used as the optimization target. The planned control trajectory is optimized using the model predictive control method, taking into account the operating constraints of the actuator and road constraints, and then sent to the vehicle controller. If optimization is not required, the optimization algorithm is exited and sent directly to the vehicle controller. The algorithm process described below will run at a fixed frequency in the above-mentioned specific scenario.

[0047] The specific process of the trajectory optimization method based on vehicle kinematic model predictive control according to one embodiment of the present invention is as follows: Figure 1 Shown, including:

[0048] Step S1, retrieving the reference trajectory generated from upstream;

[0049] Step S2, retrieving environmental information;

[0050] Step S3: Determine whether the reference trajectory requires algorithm optimization based on the obtained environmental information. If not, proceed to step S4; if yes, proceed to step S5.

[0051] Step S4, sending the reference trajectory to the vehicle controller;

[0052] Step S5, inferring the future motion position based on the vehicle kinematic model, establishing the objective function using the model predictive control method, and taking into account the constraints to obtain the optimal control variable relative to the reference trajectory at the current moment;

[0053] Step S6, optimizing the trajectory using the optimal control amount to obtain the final trajectory; and

[0054] Step S7: Send the final trajectory to the vehicle controller.

[0055] In this embodiment, the above step S5 further includes:

[0056] Step S51, reading steering delay calibration data;

[0057] Step S52: Initialize model constraint parameters.

[0058] In this embodiment, the environmental information in step S3 includes curvature and geographic information. The constraints in step S5 are determined by the actuator performance, driving state, and geographic information of the vehicle platform used. Actuator performance refers to lateral response delay, which can be measured experimentally using step and sine wave signals. Driving state can be obtained from sensors on the vehicle's autonomous driving platform. Geographic information refers to road markings or curb information, which can be obtained from high-precision maps.

[0059] In this embodiment, the reference trajectory in step S1 is composed of a trajectory point set R obtained by the upstream planning algorithm. The trajectory point set R includes a number of reference trajectory points, which contain the expected information at each moment. The expected information includes the vehicle position coordinates (x, y) in the geodetic coordinate system, velocity v, acceleration a, heading angle Time t and curvature κ. The trajectory point set R is expressed as:

[0060]

[0061] In this embodiment, the final trajectory in step S6 is composed of a trajectory point set S, which includes several final trajectory points. The final trajectory points contain the expected information at each moment. The expected information includes the vehicle position coordinates (x, y) in the geodetic coordinate system, the speed v, the acceleration a, the heading angle Time t and curvature κ. The trajectory point set S is expressed as:

[0062]

[0063] In this embodiment, the trajectory optimization is the optimization of each trajectory point. The trajectory point set S is obtained by filling the optimized trajectory points into the trajectory point set R. The filling method adopts the time alignment method. The time t before and after the optimization of each trajectory point remains unchanged.

[0064] like Figure 2 As shown, in step S5, the future motion position is calculated based on the vehicle kinematic model. A two-degree-of-freedom kinematic model is derived based on the vehicle kinematic model. The state equation is established according to the forward Euler method to update the position coordinates (x, y) of the center of mass of the front wheel of the vehicle in the geodetic coordinate system and the orientation angle of the vehicle. The state quantities of the car's front wheel center of mass velocity v and the car's front wheel center of mass acceleration a at the next moment.

[0065] The equation of state is as follows:

[0066]

[0067] Wherein, subscript k represents the current moment, subscript k+1 represents the next moment, δ represents the steering angle of the front wheels of the car, l represents the wheelbase of the car, j represents the impact degree, which refers to the rate of change of acceleration, and Δt is the sampling time within a unit cycle. It means that the orientation angle of the car at the current moment k is 0.

[0068] In this embodiment, the objective function and constraints in step S5 are as follows:

[0069]

[0070] Among them, f represents the objective function, w1, w2, w3, w4, and w5 are the weight coefficients of each quadratic term, (x ref ,y ref ),v ref 、a ref They represent the position, velocity, and acceleration of the reference point in the reference trajectory R at a specific moment in the geodetic coordinate system, respectively. j represents the impact degree, X represents the state vector, A represents the system matrix after linearization near the reference trajectory point, B represents the input matrix, U represents the input vector, the subscript k represents the current moment, and the subscript k+1 represents the next moment.

[0071] In the constraints of the objective function, Θ represents the chassis response angle set at each sampling moment obtained through experiments under certain speed conditions, k represents the number of sampling times in a unit cycle, Δt represents the sampling time in a unit cycle, and t delay Represents the delay of vehicle lateral control. The product of the number of sampling times k and the sampling time Δt in a unit cycle should not be greater than the delay t of vehicle lateral control. delay , δ represents the front wheel turning angle of the car, δ min , δ max Represents the minimum and maximum values ​​of the current car's front wheel angle constraint, v min 、v max Represents the minimum and maximum values ​​of the current vehicle's front wheel mass center speed, a min 、a max Represents the minimum and maximum values ​​of the current vehicle's front wheel center of mass acceleration, x geofence_min 、x geofence_max 、y geofence_min 、y geofence_max Respectively represent the minimum and maximum values ​​of the area where the car can drive in the x and y directions on the current road.

[0072] In this embodiment, the optimized trajectory in step S6 is as follows: the position coordinates (x, y) of the center of mass of the vehicle's front wheel in the geodetic coordinate system, the velocity v of the center of mass of the vehicle's front wheel, the acceleration a of the center of mass of the vehicle's front wheel, and the impact degree j are calculated according to equation (2) as the optimal control variables. The optimal control variables are substituted into equation (1) to generate the final trajectory.

[0073] In such Figure 3 In the lane change scenario shown in FIG. 1 , the simulation results of the optimization model of an embodiment of the present invention are as follows: Figure 4 As shown. Among them, Figure 3 The dotted line M in the figure is the vehicle's driving trajectory. Figure 4 In the figure, line A represents the reference trajectory data of the upstream planning algorithm, while line B is the final trajectory data output by the optimization model.

[0074] Figure 4 In (a), the horizontal axis represents time in seconds, and the vertical axis represents time in seconds. represents the heading angle, measured in radians. As shown by line A, before algorithm optimization, the vehicle's steering did not take into account the response delay of the chassis' lateral control, resulting in no lead time. Turning occurred at approximately 0.6 seconds, resulting in a large rate of change in the heading angle and an abrupt turn. As shown by line B, after algorithm optimization, the vehicle proactively turns ahead, and the rate of change in the heading angle is more stable, resulting in smoother lane changes.

[0075] Figure 4 In (b), the horizontal axis represents time in seconds, and the vertical axis v represents speed in meters per second. As shown by line A, before algorithm optimization, the car's speed dropped due to an overly abrupt turn. As shown by line B, after algorithm optimization, the car turns earlier, and the speed trajectory increases smoothly.

[0076] Figure 4 In (c), the horizontal axis represents time in seconds, and the vertical axis a represents acceleration in meters per second squared. As shown by line A, before algorithm optimization, acceleration experienced multiple sudden changes, affecting perceived comfort. As shown by line B, after algorithm optimization, acceleration increased smoothly, resulting in a more stable jerkiness (i.e., the rate of change of acceleration) and a more comfortable feel.

[0077] The present invention has the following beneficial effects:

[0078] 1. This vehicle kinematic model-based trajectory optimization method takes into account the response delay of chassis lateral control and utilizes steering data calibrated from real vehicles to optimize a predictable planned trajectory. The resulting optimized trajectory smooths the impact of vehicle steering and avoids dangerous maneuvers.

[0079] 2. The present invention uses a suitable cost function as the objective function of the model predictive control, taking into account the vehicle's expected speed, expected acceleration and trajectory impact. The final output trajectory can be directly transmitted to the lower control layer.

[0080] 3. The optimization algorithm of this patent is based on the solution of lateral and longitudinal coupling. It solves the pain point mentioned above that the planning based on lateral and longitudinal decoupling cannot fully meet the Ackerman steering principle and vehicle kinematics. The trajectory optimization method based on vehicle kinematic model predictive control of the present invention is based on the kinematics of the car, ensuring that the final generated trajectory meets the kinematic constraints while avoiding repeated calculations. The optimization algorithm is based on optimal control and state equations, and can simultaneously generate smooth lateral and longitudinal trajectories, ensuring that the autonomous vehicle can travel stably and smoothly.

[0081] The above embodiments are merely further explanations of the present invention and are not intended to limit the present invention in any other manner. The present invention may also have various other embodiments. Those skilled in the art may make various corresponding modifications and variations based on the present invention without departing from the spirit and substance of the present invention, and such corresponding modifications and variations shall fall within the scope of protection of the present invention.

Claims

1. A trajectory optimization method based on vehicle kinematic model predictive control, characterized in that: The process of the trajectory optimization method includes: Step S1: Retrieve the reference trajectory generated from the upstream. The reference trajectory consists of a trajectory point set R obtained by the upstream planning algorithm. The trajectory point set R includes several reference trajectory points. The reference trajectory points contain the expected information at each moment. The expected information includes the vehicle position coordinates (x, y) in the geodetic coordinate system, velocity v, acceleration a, and heading angle. Time t and curvature k, the trajectory point set R is expressed as: Step S2, retrieving environmental information; Step S3, judging whether the reference trajectory needs algorithm optimization based on the obtained environmental information, if not, proceeding to step S4, if yes, proceeding to step S5; Step S4, sending the reference trajectory to the vehicle controller; Step S5, calculate the future motion position according to the vehicle kinematic model, establish the objective function using the model predictive control method, and consider the constraints to obtain the optimal control amount relative to the reference trajectory at the current moment. In step S5, the future motion position is calculated according to the vehicle kinematic model. A two-degree-of-freedom kinematic model is derived based on the vehicle kinematic model, and the state equation is established according to the forward Euler method to update the position coordinates (x, y) of the center of mass of the front wheel of the vehicle in the geodetic coordinate system and the orientation angle of the vehicle. The state quantities of the vehicle's front wheel mass center velocity v and the vehicle's front wheel mass center acceleration a at the next moment are as follows: Wherein, subscript k represents the current moment, subscript k+1 represents the next moment, δ represents the steering angle of the front wheels of the vehicle, l represents the wheelbase of the vehicle, j represents the impact degree, which refers to the rate of change of acceleration, and Δt is the sampling time within a unit cycle; Step S6, optimizing the trajectory using the optimal control amount to obtain the final trajectory; Step S7, sending the final trajectory to the vehicle controller; Wherein, the step S5 further includes: Step S51, reading steering delay calibration data; Step S52: Initialize model constraint parameters.

2. The trajectory optimization method based on vehicle kinematic model predictive control according to claim 1, characterized in that: The environmental information in step S3 includes curvature and geographical information.

3. The trajectory optimization method based on vehicle kinematic model predictive control according to claim 2, characterized in that: The constraint conditions in step S5 are determined by the actuator performance, driving status, and geographic information of the vehicle platform used; The actuator performance refers to the lateral response delay, which can be measured by step signal and sine wave signal experiments; The driving state can be obtained by sensors of the vehicle's automatic driving platform; The geographic information refers to road markings or curb information, which can be obtained from a high-precision map.

4. The trajectory optimization method based on vehicle kinematic model predictive control according to claim 1, characterized in that: The final trajectory in step S6 is composed of a trajectory point set S, which includes several final trajectory points. The final trajectory points include the expected information at each moment, including the vehicle position coordinates (x, y), velocity v, acceleration a, and heading angle in the geodetic coordinate system. Time t and curvature κ, the trajectory point set S is expressed as:

5. The trajectory optimization method based on vehicle kinematic model predictive control according to claim 4 is characterized in that: The trajectory optimization is the optimization of each trajectory point. The trajectory point set S is obtained by filling the optimized trajectory points into the trajectory point set R. The filling method adopts the time alignment method. The time t before and after the optimization of each trajectory point remains unchanged.

6. The trajectory optimization method based on vehicle kinematic model predictive control according to claim 1, characterized in that: The objective function and constraints in step S5 are as follows: Among them, f represents the objective function, w1, w2, w3, w4, and w5 are the weight coefficients of each quadratic term, (x ref ,y ref ),v ref 、a ref They represent the position, velocity, and acceleration of the reference trajectory point in the reference trajectory R at a specific moment in the earth coordinate system, j represents the impact degree, X represents the state vector, A represents the system matrix after linearization near the reference trajectory point, B represents the input matrix, U represents the input vector, subscript k represents the current moment, and subscript k+1 represents the next moment; In the constraint conditions of the objective function, Θ represents the chassis response angle set at each sampling moment obtained through experiments under a certain speed condition, k represents the number of sampling times in a unit cycle, Δt represents the sampling time in a unit cycle, and t delay represents the delay of the vehicle's lateral control, δ represents the front wheel steering angle of the vehicle, and δ min , δ max Represent the minimum and maximum values ​​of the current front wheel steering angle constraint, v min 、v max Represent the minimum and maximum values ​​of the current vehicle front wheel mass center speed, a min 、a max Represent the minimum and maximum values ​​of the current vehicle front wheel mass center acceleration, x geofence_min 、x geofence_max 、y geofence_min 、y geofence_max Respectively represent the minimum and maximum values ​​of the area where the car can drive in the x and y directions on the current road.

7. The trajectory optimization method based on vehicle kinematic model predictive control according to claim 6, characterized in that: The optimization trajectory in step S6 is: According to formula (2), the position coordinates (x, y) of the center of mass of the front wheel of the vehicle in the geodetic coordinate system, the speed v of the center of mass of the front wheel of the vehicle, the acceleration a of the center of mass of the front wheel of the vehicle, and the impact degree j are calculated as the optimal control quantity; Substitute the optimal control amount into equation (1) to generate the final trajectory.

Images