CN115447615A - Trajectory optimization method based on vehicle kinematics model predictive control - Google Patents

Trajectory optimization method based on vehicle kinematics model predictive control Download PDF

Info

Publication number
CN115447615A
CN115447615A CN202211274770.6A CN202211274770A CN115447615A CN 115447615 A CN115447615 A CN 115447615A CN 202211274770 A CN202211274770 A CN 202211274770A CN 115447615 A CN115447615 A CN 115447615A
Authority
CN
China
Prior art keywords
vehicle
trajectory
track
optimization method
automobile
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211274770.6A
Other languages
Chinese (zh)
Inventor
伍文剑
彭怡凡
卢奕燊
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
SAIC Volkswagen Automotive Co Ltd
Original Assignee
SAIC Volkswagen Automotive Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by SAIC Volkswagen Automotive Co Ltd filed Critical SAIC Volkswagen Automotive Co Ltd
Priority to CN202211274770.6A priority Critical patent/CN115447615A/en
Publication of CN115447615A publication Critical patent/CN115447615A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W60/00—Drive control systems specially adapted for autonomous road vehicles
    • B60W60/001—Planning or execution of driving tasks
    • B60W60/0011—Planning or execution of driving tasks involving control alternatives for a single driving scenario, e.g. planning several paths to avoid obstacles
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W60/00—Drive control systems specially adapted for autonomous road vehicles
    • B60W60/001—Planning or execution of driving tasks
    • B60W60/0015—Planning or execution of driving tasks specially adapted for safety
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W2050/0001—Details of the control system
    • B60W2050/0019—Control system elements or transfer functions
    • B60W2050/0028—Mathematical models, e.g. for simulation
    • B60W2050/0031—Mathematical model of the vehicle
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

The invention provides a track optimization method based on vehicle kinematics model predictive control. The invention starts from a basic vehicle kinematics Model (a bicycle Model), combines a Model Predictive Control Method (MPC) and establishes a track optimization Model based on the MPC. On the basis of receiving the reference track generated by the upstream planning module, the smooth track meeting the vehicle kinematics can be calculated through an optimization method. The final calculation result meets the control requirement of the automobile, and meanwhile, the method has good running speed and can be applied to intelligent driving of the automobile.

Description

Trajectory optimization method based on vehicle kinematics model predictive control
Technical Field
The invention relates to the field of intelligent driving, in particular to a track optimization method based on vehicle kinematics model predictive control.
Background
With the continuous development of intelligent driving automobiles and unmanned automobiles, more and more planning algorithms are applied to the intelligent driving automobiles. The planning control is an important ring in the intelligent automobile driving technology, and the comfort, the safety and the real-time performance of the intelligent automobile are directly influenced by the quality of the algorithm.
The planning task of the intelligent driving automobile is to seek the optimal track which can be driven by the current intelligent driving automobile in two dimensions of space and time, so that the planning problem of the intelligent driving automobile is an NP hard (Non-deterministic polymeric-hard) problem. That is, the motion planning of an intelligent drive vehicle is a problem in that it is impossible or difficult to find an algorithm that satisfies polynomial time. If the track is directly solved, the algorithm complexity of the method brings huge calculation burden, and as the space for seeking the optimal solution increases, the time for seeking the optimal solution becomes large, which finally causes the intelligent driving automobile to react slowly, which is obviously unacceptable in driving. In order to solve the conflict, the existing planning algorithm does not search for the optimal trajectory simultaneously in time and space, but divides the planning task into two parts of path planning and trajectory planning. Path planning is inherently a search problem. The path planning does not consider the dimension of time, and the task of the path planning is to generate a collision-Free path from a starting point to an end point in the current Free Space (Free Space). The track generation takes the time dimension into consideration, and the task of the track generation is to generate a track which can be completed within a certain time on the basis of the current given path. In order to further increase the calculation speed, the existing planning algorithm can decouple the intelligent automobile transversely and longitudinally, then respectively calculate transverse and longitudinal tracks, and then match the transverse and longitudinal tracks to form a final track. However, it is inevitable that the obtained trajectory in some cases cannot be applied to real vehicle motion control or has problems of serious jitter and the like, such as encountering a reference line with a large curvature radius or an area with a complex road surface condition. This is because most planning algorithms treat the vehicle as a rigid body during collision detection, but do not consider kinematic and dynamic constraints thereof, and the real vehicle motion needs to satisfy ackermann steering principle and various constraints. When the experimental vehicle runs the existing regulation and control algorithm, the response delay of the chassis transverse control under the lane changing scene is larger and is between 1.2 and 2 seconds. Namely, after the lane change track is planned, the chassis response cannot keep up with the transverse planning. The corresponding reality phenomenon is that the steering wheel is driven very quickly, and an emergency lane change is generated.
The existing trajectory generation scheme can be divided into trajectory generation based on polynomial interpolation and curve fitting, trajectory generation based on an optimization algorithm, and trajectory generation based on deep learning and reinforcement learning. The track generation scheme based on polynomial interpolation and curve fitting has the problems that the track or the high order of the track is not smooth, the constraint of vehicle motion dynamics is not met, and the like. The optimization-based trajectory generation scheme needs to artificially define an optimization objective function and related constraints, and the final optimization result depends on the quality of the definitions. The track generation scheme based on deep learning and reinforcement learning requires a great amount of offline learning, and there is a risk that scenes not covered in learning cannot be processed.
Disclosure of Invention
In order to solve the existing problems, the invention establishes a track optimization Model based on vehicle kinematics Model Predictive Control by combining a Model Predictive Control Method (MPC) from a basic vehicle kinematics Model (a single vehicle Model). On the basis of receiving the reference track generated by the upstream planning module, calculating a gentle track meeting the automobile kinematics through an optimization method.
The invention provides a trajectory optimization method based on vehicle kinematics model predictive control, which comprises the following steps:
step S1, retrieving a reference track generated from an upstream;
s2, retrieving environmental information;
s3, judging whether the reference track needs algorithm optimization or not according to the obtained environment information, if not, entering a step S4, and if so, entering a step S5;
s4, sending the reference track to a vehicle controller;
s5, calculating a future motion position according to the vehicle kinematic model, establishing a target function by using a model predictive control method, and considering constraint conditions to obtain the optimal control quantity of the current moment relative to a reference track;
s6, optimizing the track by using the optimal control quantity to obtain a final track;
and S7, sending the final track to a vehicle controller.
In one embodiment, the step S5 further includes:
step S51, reading steering delay calibration data;
in step S52, model constraint parameters are initialized.
In one embodiment, the environment information in step S3 includes curvature and geographic information.
In one embodiment, the constraint conditions in step S5 are determined by the actuator performance, the driving state and the geographic information of the vehicle platform used;
wherein the actuator performance refers to a lateral response delay, which can be experimentally measured by a step signal and a sine wave signal;
the driving state can be obtained by a sensor of the automatic driving platform of the vehicle;
the geographic information refers to road marking or curb information and can be obtained from a high-precision map.
In one embodiment, in step S1, the reference track is composed of a track point set R obtained by an upstream planning algorithm, where the track point set R includes a plurality of reference track points, the reference track points include expected information at each time, and the expected information includes position coordinates (x, y), speed v, acceleration a, and heading angle (a) of the vehicle in a geodetic coordinate system
Figure BDA0003896022240000031
Time t and curvature k, the set of trajectory points R is represented as:
Figure BDA0003896022240000032
in one embodiment, the final track in step S6 is composed of a track point set S, the track point set S includes a plurality of final track points, the final track points include expected information at each time, and the expected information includes position coordinates (x, y), speed v, acceleration a, and orientation angle of the automobile in a geodetic coordinate system
Figure BDA0003896022240000033
Timet and a curvature κ, the set of trajectory points S being represented as:
Figure BDA0003896022240000034
in one embodiment, the track optimization is realized by optimizing each track point, the track point set S is obtained by filling the optimized track points into the track point set R, the filling mode adopts a time alignment method, and the time t before and after the optimization of each track point is unchanged.
In one embodiment, the step S5 of calculating the future motion position according to the vehicle kinematic model is to obtain a two-degree-of-freedom kinematic model based on the vehicle kinematic model, and establish a state equation according to the forward euler method to update the position coordinates (x, y) of the center of mass of the front wheel of the automobile in the geodetic coordinate system and the orientation angle of the automobile
Figure BDA0003896022240000035
The state quantities of the vehicle front wheel mass center speed v and the vehicle front wheel mass center acceleration a at the next moment are represented by the following formula:
Figure BDA0003896022240000041
wherein, subscript k represents the current time, subscript k +1 represents the next time, δ represents the steering angle of the front wheels of the automobile, l represents the wheel base of the automobile, j represents the impact degree and refers to the change rate of the acceleration, and Δ t is the sampling time in a unit period.
In one embodiment, the objective function and constraint conditions in step S5 are as follows:
Figure BDA0003896022240000042
wherein f represents an objective function, w 1 、w 2 、w 3 、w 4 、w 5 The weight coefficients of the respective quadratic terms, (x) ref ，y ref )、v ref 、a ref Respectively representing the position, the speed and the acceleration of a reference track point in a reference track R under a specific moment in a geodetic coordinate system, wherein j represents the impact degree, X represents a state vector, A represents a linearized system matrix near the reference track point, B represents an 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, theta identifies a chassis response angle set at each sampling moment obtained through experiments under a certain speed condition, k represents the sampling times in a unit period, delta t represents the sampling time in the unit period, and t represents delay Representing the delay in the lateral control of the vehicle, delta representing the angle of rotation of the front wheels of the vehicle, delta min 、δ max Respectively representing the minimum and maximum values of the current vehicle front wheel steering angle constraint, v min 、v max Respectively representing the minimum value and the maximum value of the mass center speed of the front wheel of the current automobile, a min 、a max Respectively representing the minimum value and the maximum value, x, of the acceleration of the mass center of the front wheel of the current automobile geofence_min 、x geofence_max 、y geofence_min 、y geofence_max Which represent the minimum and maximum values of the travelable area of the car in x and y direction on the current road, respectively.
In one embodiment, the optimized trajectory in step S6 is:
calculating position coordinates (x, y) of the mass center of the front wheel of the automobile under a geodetic coordinate system, the mass center speed v of the front wheel of the automobile, the mass center acceleration a of the front wheel of the automobile and the impact degree j according to the formula (2) to be used as optimal control quantity;
and substituting the optimal control quantity into an equation (1) to generate a final track.
The invention has the following beneficial effects:
1. the track optimization method based on the vehicle kinematic model predictive control considers the response delay of the chassis transverse control, applies the steering data calibrated by the real vehicle, and optimizes a predictive planning track. The generated optimized track smoothes the impact degree of the vehicle during steering, and dangerous behaviors are avoided.
2. According to the method, a proper cost function is used as a target function of model prediction control, the expected speed, the expected acceleration and the track impact degree of the vehicle are considered, and the finally output track can be directly transmitted to a lower control layer.
3. The optimization algorithm of the patent is based on transverse and longitudinal coupling solution. The problem that the pain point based on the horizontal and vertical decoupling planning can not completely meet the Ackerman steering principle and the automobile kinematics is solved. The trajectory optimization method based on vehicle kinematics model predictive control is based on the kinematics of the automobile, and avoids repeated calculation on the premise of ensuring that the finally generated trajectory meets the kinematic constraint. The optimization algorithm is based on optimal control and a state equation, and can generate smooth transverse and longitudinal tracks at the same time, so that the stable and smooth running of the automatic driving automobile is ensured.
Drawings
FIG. 1 discloses a flow chart of a trajectory optimization method based on vehicle kinematics model predictive control according to an embodiment of the present invention;
FIG. 2 discloses a schematic diagram of a vehicle kinematics model of a trajectory optimization method based on vehicle kinematics model predictive control according to an embodiment of the present invention;
FIG. 3 discloses a schematic diagram of a lane-changing scene of a trajectory optimization method based on vehicle kinematics model predictive control according to an embodiment of the present invention; and
fig. 4 discloses a comparison graph of trajectory data before and after algorithm optimization of the trajectory optimization method based on vehicle kinematics model predictive control according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and do not limit the invention.
The invention provides a track optimization method based on vehicle kinematics model predictive control. The optimization algorithm receives a planned track sent from the upstream, and judges whether algorithm optimization is needed or not according to conditions such as track curvature, geographic information and the like. If the optimization is needed, calculating a future motion position according to the automobile motion model, optimizing a planned control track by taking a proper cost function as an optimization target and a model predictive control method into consideration of the working constraint range and road constraint of an actuator, and sending the control track to a vehicle controller; and if the optimization is not needed, jumping out of the optimization algorithm and directly sending the optimization algorithm to the vehicle controller. The algorithm flow described below will run at a fixed frequency in the specific scenario described above.
The specific flow of the trajectory optimization method based on vehicle kinematics model predictive control according to an embodiment of the present invention is shown in fig. 1, and includes:
step S1, retrieving a reference track generated from an upstream;
s2, retrieving environmental information;
s3, judging whether the reference track needs algorithm optimization or not according to the obtained environment information, if not, entering a step S4, and if so, entering a step S5;
s4, sending the reference track to a vehicle controller;
s5, calculating a future motion position according to the vehicle kinematic model, establishing a target function by using a model predictive control method, and considering constraint conditions to obtain the optimal control quantity of the current moment relative to a reference track;
s6, optimizing the track by using the optimal control quantity to obtain a final track; and
and S7, sending the final track to a vehicle controller.
In this embodiment, the step S5 further includes:
step S51, reading steering delay calibration data;
in step S52, model constraint parameters are initialized.
In this embodiment, the environment information in step S3 includes curvature and geographical information. The constraint conditions in step S5 are determined by the actuator performance, the driving state, and the geographic information of the vehicle platform used. The actuator performance refers to the transverse response delay, and the transverse response delay can be measured through a step signal and a sine wave signal experiment. The driving state may be obtained by a sensor of the vehicle autopilot platform. The geographic information refers to road marking or curb information and can be obtained from a high-precision map.
In the present embodiment, the reference trajectory in step S1 is composed of a set of trajectory points R obtained by an upstream planning algorithm. The track point set R comprises a plurality of reference track points, and the reference track points contain expected information at each moment. The expected information includes the position coordinates (x, y), velocity v, acceleration a, and orientation angle of the vehicle in the geodetic coordinate system
Figure BDA0003896022240000071
Time t and curvature k. The set of trajectory points R is represented as:
Figure BDA0003896022240000072
in this embodiment, the final track in step S6 is composed of a track point set S, where the track point set S includes a plurality of final track points, and the final track points include expected information at each time. The expected information comprises the position coordinates (x, y), the speed v, the acceleration a and the orientation angle of the automobile in the geodetic coordinate system
Figure BDA0003896022240000073
Time t and curvature k. The set of trajectory points S is represented as:
Figure BDA0003896022240000074
in the embodiment, the track is optimized to each track point, the track point set S is obtained by filling the optimized track points into the track point set R, the filling mode adopts a time alignment method, and the time t before and after each track point is optimized is unchanged.
As shown in fig. 2, in step S5, the future kinematic position is calculated according to the vehicle kinematic model, and a two-degree-of-freedom kinematic model is obtained based on the vehicle kinematic model, and the two-degree-of-freedom kinematic model is calculated according to the forward eulerian methodEstablishing a state equation to update the position coordinates (x, y) of the mass center of the front wheel of the automobile in a geodetic coordinate system and the orientation angle of the automobile
Figure BDA0003896022240000075
The state quantities of the speed v of the mass center of the front wheel of the automobile and the acceleration a of the mass center of the front wheel of the automobile at the next moment.
The equation of state is as follows:
Figure BDA0003896022240000076
wherein, subscript k represents the current time, subscript k +1 represents the next time, delta represents the steering angle of the front wheels of the automobile, l represents the wheel base of the automobile, j represents the impact degree and represents the change rate of the acceleration, delta t is the sampling time in a unit period,
Figure BDA0003896022240000077
the orientation angle representing the current time k of the vehicle is 0.
In the present embodiment, the objective function and constraint conditions in step S5 are as follows:
Figure BDA0003896022240000081
wherein f represents an objective function, w 1 、w 2 、w 3 、w 4 、w 5 The weight coefficients of the respective quadratic terms, (x) ref ，y ref )、v ref 、a ref Respectively represent the position, the speed and the acceleration of a reference point in a reference track R under a certain moment in a geodetic coordinate system, j represents the impact degree, X represents a state vector, A represents a system matrix after linearization near a reference track point, B represents an input matrix, U represents an input vector, subscript k represents the current moment, and subscript k +1 represents the next moment.
Within the constraints of the objective function, Θ identifies the experimentally obtained chassis response angle at each sampling instant at a certain speedSet, k represents the number of samples in a unit period, Δ t represents the sampling time in a unit period, t delay Representing the delay of the vehicle lateral control, the product of the sampling times k and the sampling time delta t in the unit period should not be larger than the delay t of the vehicle lateral control delay And delta denotes the angle of rotation of the front wheel of the vehicle, delta min 、δ max Minimum and maximum values, v, representing the current vehicle front wheel steering angle constraint min 、v max Represents the minimum and maximum values of the current vehicle front wheel centroid velocity, a min 、a max Represents the minimum value and the maximum value of the acceleration of the mass center of the front wheel of the current automobile, x geofence_min 、x geofence_max 、y geofence_min 、y geofence_max Which represent the minimum and maximum values of the travelable area of the car in x and y direction on the current road, respectively.
In this embodiment, the optimized trajectory in step S6 is: and (3) calculating the position coordinates (x, y) of the mass center of the front wheel of the automobile in the geodetic coordinate system, the mass center speed v of the front wheel of the automobile, the mass center acceleration a of the front wheel of the automobile and the impact degree j according to the formula (2) to be used as the optimal control quantity. The optimal control amount is substituted into equation (1) to generate a final trajectory.
Under the lane change scenario shown in fig. 3, the simulation result of the optimization model according to an embodiment of the present invention is shown in fig. 4. In fig. 3, a broken line M represents a vehicle travel track line. In fig. 4, line a represents the reference trajectory data of the upstream planning algorithm, while line B is the final trajectory data of the optimization model output.
In FIG. 4 (a), the abscissa represents time in seconds and the ordinate represents time in seconds
Figure BDA0003896022240000082
Representing the orientation angle in radians. As shown by line a, the vehicle steering does not take into account the response delay of the chassis lateral control before the algorithm is optimized, so no advance is reserved, steering occurs at about 0.6 seconds, so the rate of change of heading angle is large resulting in oversteer. As shown by line B, the vehicle will turn ahead with predictability after algorithm optimization and be more stable toward the rate of angular change, so the vehicle will be smoother when turning a lane change.
In fig. 4 (b), the abscissa represents time in seconds, and the ordinate v represents velocity in meters per second. As shown by line A, vehicle speed drops due to vehicle oversteer before algorithm optimization. After the algorithm is optimized, the automobile is turned ahead, and the speed track smoothly rises, as shown by a line B.
In fig. 4 (c), the abscissa represents time in seconds, and the ordinate a represents acceleration in meters per second squared. As shown by line a, acceleration may suddenly change many times before algorithm optimization, thereby affecting somatosensory comfort. As shown by line B, the acceleration smoothly rises after the algorithm optimization, resulting in a relatively stable jerk (i.e., a rate of change of acceleration) and a more comfortable body feeling.
The invention has the following beneficial effects:
1. the track optimization method based on the vehicle kinematic model predictive control considers the response delay of the chassis transverse control, applies the steering data calibrated by the real vehicle, and optimizes a predictive planning track. The generated optimized track smoothes the impact degree of the vehicle during steering, and dangerous behaviors are avoided.
2. According to the method, a proper cost function is used as a target function of model prediction control, the expected speed, the expected acceleration and the track impact degree of the vehicle are considered, and the finally output track can be directly transmitted to the lower control layer.
3. The optimization algorithm of the patent is based on transverse and longitudinal coupling solution. The problem that the pain point based on the horizontal and vertical decoupling planning cannot completely meet the Ackerman steering principle and the automobile kinematics is solved. The trajectory optimization method based on the vehicle kinematics model predictive control is based on the kinematics of the automobile, and avoids repeated calculation on the premise of ensuring that the finally generated trajectory meets the constraint of the kinematics. The optimization algorithm is based on optimal control and a state equation, and can generate smooth transverse and longitudinal tracks at the same time, so that the stable and smooth running of the automatic driving automobile is ensured.
The above-mentioned embodiments are merely illustrative of the present invention, and are not intended to limit the present invention in any way, and various other embodiments are possible. Various modifications and changes may be made by those skilled in the art without departing from the spirit and substance of the invention, and these modifications and changes are intended to fall within the scope of the invention.

Claims (10)

1. A trajectory optimization method based on vehicle kinematic model prediction control is characterized in that the flow of the trajectory optimization method comprises the following steps:
step S1, retrieving a reference track generated from an upstream;
s2, retrieving environmental information;
s3, judging whether the reference track needs algorithm optimization or not according to the obtained environment information, if not, entering a step S4, and if so, entering a step S5;
s4, sending the reference track to a vehicle controller;
s5, calculating a future motion position according to the vehicle kinematic model, establishing a target function by using a model predictive control method, and considering constraint conditions to obtain the optimal control quantity of the current moment relative to a reference track;
s6, optimizing the track by using the optimal control quantity to obtain a final track;
and S7, sending the final track to a vehicle controller.
2. The trajectory optimization method based on vehicle kinematic model predictive control according to claim 1, wherein the step S5 further comprises:
step S51, reading steering delay calibration data;
in step S52, model constraint parameters are initialized.
3. The vehicle kinematic model prediction control-based trajectory optimization method according to claim 1, wherein the environmental information in step S3 includes curvature and geographic information.
4. The trajectory optimization method based on vehicle kinematic model predictive control according to claim 3, wherein the constraint conditions in step S5 are determined by actuator performance, driving state and geographic information of the vehicle platform used;
wherein, the actuator performance refers to the transverse response delay which can be experimentally measured by a step signal and a sine wave signal;
the driving state can be obtained by a sensor of the automatic driving platform of the vehicle;
the geographic information refers to road marking or curb information and can be obtained from a high-precision map.
5. The vehicle kinematics model predictive control-based track optimization method according to claim 1, wherein in the step S1, the reference track is composed of a track point set R obtained by an upstream planning algorithm, the track point set R includes a plurality of reference track points, the reference track points include expected information at each time, and the expected information includes vehicle position coordinates (x, y), speed v, acceleration a, and orientation angle in a geodetic coordinate system
Figure FDA0003896022230000021
Time t and curvature k, the set of trajectory points R is represented as:
Figure FDA0003896022230000022
6. the vehicle kinematics model predictive control-based trajectory optimization method according to claim 5, wherein the final trajectory in step S6 is composed of a set S of trajectory points, the set S of trajectory points comprises a plurality of final trajectory points, the final trajectory points comprise expected information at each time, and the expected information comprises position coordinates (x, y), velocity v, acceleration a and orientation angle of the vehicle in a geodetic coordinate system
Figure FDA0003896022230000023
Time t and curvature κ, the set S of trajectory points being represented as:
Figure FDA0003896022230000024
7. the vehicle kinematics model predictive control-based trajectory optimization method according to claim 6, wherein the trajectory optimization is performed by optimizing each trajectory point, the set S of trajectory points is obtained by filling the optimized trajectory points into the set R of trajectory points, the filling mode is a time alignment method, and the time t before and after optimization of each trajectory point is constant.
8. The vehicle kinematics model predictive control-based trajectory optimization method according to claim 1, wherein in step S5, the future motion position is calculated according to the vehicle kinematics model, a two-degree-of-freedom kinematics model is obtained based on the vehicle kinematics model, and a state equation is established according to a 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
Figure FDA0003896022230000025
The state quantities of the vehicle front wheel mass center speed v and the vehicle front wheel mass center acceleration a at the next moment are represented by the following formula:
Figure FDA0003896022230000031
wherein, subscript k represents the current time, subscript k +1 represents the next time, δ represents the steering angle of the front wheels of the automobile, l represents the wheel base of the automobile, j represents the impact degree and refers to the change rate of the acceleration, and Δ t is the sampling time in a unit period.
9. The trajectory optimization method based on vehicle kinematic model predictive control according to claim 8, characterized in that the objective function and constraint conditions in step S5 are as follows:
Figure FDA0003896022230000032
wherein f represents an objective function, w 1 、w 2 、w 3 、w 4 、w 5 The weight coefficients of the respective quadratic terms, (x) ref ,y ref )、v ref 、a ref Respectively representing the position, the speed and the acceleration of a reference track point in a reference track R under a geodetic coordinate system at a specific moment, j represents the impact degree, X represents a state vector, A represents a linearized system matrix near the reference track point, B represents an 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, theta identifies a chassis response angle set at each sampling moment obtained through experiments under a certain speed condition, k represents the sampling times in a unit period, delta t represents the sampling time in the unit period, and t represents delay Representing the delay of the lateral control of the vehicle, δ representing the angle of rotation of the front wheels of the vehicle, δ min 、δ max Respectively representing the minimum and maximum values of the current vehicle front wheel steering angle constraint, v min 、v max Respectively representing the minimum value and the maximum value of the mass center speed of the front wheel of the current automobile, a min 、a max Respectively representing the minimum value and the maximum value, x, of the acceleration of the mass center of the front wheel of the current automobile geofence_min 、x geofence_max 、y geofence_min 、y geofence_max Which represent the minimum and maximum values of the travelable area of the car in x and y direction on the current road, respectively.
10. The vehicle kinematics model predictive control-based trajectory optimization method according to claim 9, wherein the optimized trajectory in step S6 is:
calculating position coordinates (x, y) of the mass center of the front wheel of the automobile in a geodetic coordinate system, the mass center speed v of the front wheel of the automobile, the mass center acceleration a of the front wheel of the automobile and the impact degree j according to the formula (2) to serve as optimal control quantity;
and substituting the optimal control quantity into an equation (1) to generate a final track.
CN202211274770.6A 2022-10-18 2022-10-18 Trajectory optimization method based on vehicle kinematics model predictive control Pending CN115447615A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211274770.6A CN115447615A (en) 2022-10-18 2022-10-18 Trajectory optimization method based on vehicle kinematics model predictive control

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211274770.6A CN115447615A (en) 2022-10-18 2022-10-18 Trajectory optimization method based on vehicle kinematics model predictive control

Publications (1)

Publication Number Publication Date
CN115447615A true CN115447615A (en) 2022-12-09

Family

ID=84311171

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211274770.6A Pending CN115447615A (en) 2022-10-18 2022-10-18 Trajectory optimization method based on vehicle kinematics model predictive control

Country Status (1)

Country Link
CN (1) CN115447615A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115848365A (en) * 2023-02-03 2023-03-28 北京集度科技有限公司 Vehicle controller, vehicle and vehicle control method

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115848365A (en) * 2023-02-03 2023-03-28 北京集度科技有限公司 Vehicle controller, vehicle and vehicle control method

Similar Documents

Publication Publication Date Title
Li et al. Development of a new integrated local trajectory planning and tracking control framework for autonomous ground vehicles
CN110377039B (en) Vehicle obstacle avoidance track planning and tracking control method
CN109669461B (en) Decision-making system for automatically driving vehicle under complex working condition and track planning method thereof
Erlien et al. Shared steering control using safe envelopes for obstacle avoidance and vehicle stability
JP6715899B2 (en) Collision avoidance device
Hernandez et al. Steering control of automated vehicles using absolute positioning GPS and magnetic markers
CN110877611B (en) Obstacle avoidance device and obstacle avoidance path generation device
Awad et al. Model predictive control with fuzzy logic switching for path tracking of autonomous vehicles
CN110703754A (en) Path and speed highly-coupled trajectory planning method for automatic driving vehicle
CN112429004B (en) Automatic lane changing control method for vehicle
CN112947469A (en) Automobile track-changing track planning and dynamic track tracking control method
Roselli et al. H∞ control with look-ahead for lane keeping in autonomous vehicles
CN113619574A (en) Vehicle avoidance method and device, computer equipment and storage medium
Yuan et al. Mixed local motion planning and tracking control framework for autonomous vehicles based on model predictive control
CN115285145A (en) Unmanned curve collision avoidance trajectory planning and tracking control method
Khalifa et al. Vehicles platooning in urban environment: Consensus-based longitudinal control with limited communications capabilities
Peng et al. Lane-change model and tracking control for autonomous vehicles on curved highway sections in rainy weather
CN116654017A (en) Intelligent automobile collision avoidance decision and path planning method and system based on peripheral automobile track prediction under emergency working condition
CN115447615A (en) Trajectory optimization method based on vehicle kinematics model predictive control
CN114228690A (en) Automatic driving vehicle roll control method based on DDPG and iterative control
Chen et al. Ribbon model based path tracking method for autonomous ground vehicles
Kim et al. Hierarchical Motion Planning and Control Algorithm of Autonomous Racing Vehicles for Overtaking Maneuvers
US20230030368A1 (en) Vehicle control device
Tominaga et al. GNSS-Based Lane Keeping Assist System Using Model Predictive Control and Time Delay Compensation
Skrickij et al. Review of Integrated Chassis Control Techniques for Automated Ground Vehicles

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination