CN117250860A - Automatic driving vehicle path planning and tracking control system and method - Google Patents
Automatic driving vehicle path planning and tracking control system and method Download PDFInfo
- Publication number
- CN117250860A CN117250860A CN202311224283.3A CN202311224283A CN117250860A CN 117250860 A CN117250860 A CN 117250860A CN 202311224283 A CN202311224283 A CN 202311224283A CN 117250860 A CN117250860 A CN 117250860A
- Authority
- CN
- China
- Prior art keywords
- path
- vehicle
- tracking
- planning
- curvature
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 230000001133 acceleration Effects 0.000 claims abstract description 31
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 11
- 230000002068 genetic effect Effects 0.000 claims abstract description 10
- 238000012545 processing Methods 0.000 claims description 17
- 210000000349 chromosome Anatomy 0.000 claims description 13
- 230000008569 process Effects 0.000 claims description 10
- 230000008859 change Effects 0.000 claims description 7
- 238000013459 approach Methods 0.000 claims description 6
- 230000007423 decrease Effects 0.000 claims description 3
- 238000006467 substitution reaction Methods 0.000 claims description 2
- 238000005457 optimization Methods 0.000 description 10
- 230000000694 effects Effects 0.000 description 7
- 238000010586 diagram Methods 0.000 description 6
- 238000004458 analytical method Methods 0.000 description 3
- 238000012937 correction Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 230000004888 barrier function Effects 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 108090000623 proteins and genes Proteins 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000013016 damping Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 239000000725 suspension Substances 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses an automatic driving vehicle path planning and tracking control system and method, which are used for acquiring target path information, real-time pose information and vehicle motion state information of a vehicle input by a reference path in real time; finding the nearest reference point and the real-time pose and curvature of the vehicle corresponding to the reference point in the reference path according to the current position of the vehicle; respectively calculating vehicle motion state information and reference path tracking error of the three-degree-of-freedom dynamics discrete model according to the current position of the vehicle, the vehicle motion state information and the information of the reference point, and updating unit data of the MPC controller in real time; judging whether to re-plan the path based on the actual speed, acceleration and tracking error, solving the path for the re-planned path by adopting a genetic algorithm, and fitting the planned path by utilizing a Bezier curve; the invention improves the path tracking precision of the intelligent vehicle and simultaneously gives consideration to the stability and smoothness of the vehicle when the vehicle runs on the re-planned path road.
Description
Technical Field
The invention relates to the field of automatic driving automobile decision control, in particular to an automatic driving automobile path planning and tracking control system and method.
Background
Along with the rapid progress of the automatic driving technology in recent years, the automatic driving automobile gradually rises in the travel, the automatic driving automobile also becomes the key field of vehicle engineering research in the aspect of scientific research, and the path tracking technology of the automatic driving automobile is one of the core control technologies of the automatic driving of the automobile and is popular and focused by expert students at home and abroad. However, the current path tracking research is mostly limited to improving the tracking precision, so that the stability of the vehicle is ignored, the optimization of the path tracking precision is mostly realized by reducing the error of a control algorithm, and meanwhile, the improvement of the precision can lead to the complexity of the control algorithm, thereby reducing the calculation force and the efficiency and further reducing the feasibility of the actual path tracking of the automatic driving vehicle. On one hand, the stability of a vehicle system is influenced by the path tracking process, the stability of the vehicle can be deteriorated due to the improvement of the tracking precision, and the safety of a driver can be even influenced under extreme working conditions; on the other hand, the existing path tracking optimization algorithm is generally limited to the change and the complexity of a control method, and the influence of the road curvature on the path tracking precision is ignored.
The path tracking control algorithm of the automatic driving vehicle is an important step for realizing the automatic driving function of the vehicle, and is used as a control link of a vehicle actuator, so that the path tracking control theory is improved along with the continuous development of the automatic driving field. The high-precision automatic driving needs to be capable of adapting to the change of the road environment and the change of the vehicle, so that the high-precision automatic driving has higher requirements on the tracking precision, stability, safety and the like of the vehicle path tracking control.
Disclosure of Invention
In order to solve the defects in the prior art, the application provides an automatic driving vehicle path planning and tracking control system and method, which improve the tracking effect from the angles of curvature optimization, vehicle body comfort and control stability, improve the path tracking precision and improve the running stability and smoothness of an intelligent vehicle.
The technical scheme adopted by the invention is as follows:
an automatic driving vehicle path planning and tracking control method comprises the following steps:
s1, acquiring target path information, real-time pose information and motion state information of a vehicle, which are input by a reference path, in real time;
s2: finding the nearest reference point and the real-time pose and curvature of the vehicle corresponding to the reference point in the reference path according to the current position of the vehicle;
s3: respectively calculating vehicle motion state information and reference path tracking error of the three-degree-of-freedom dynamics discrete model according to the current position of the vehicle, the vehicle motion state information and the information (coordinates, transverse errors and curvature) of the reference point, and updating unit data of the MPC controller in real time;
s4: setting a tracking precision threshold, a speed threshold and an acceleration threshold, and taking the tracking precision threshold, the speed threshold and the acceleration threshold as judging standards of whether path re-planning is needed or not; based on the data obtained in S3, the actual vehicle speed v is determined f Acceleration a f Tracking error e y Respectively corresponding to the set speed threshold v default Acceleration threshold a default And a tracking accuracy threshold e default Comparing; if any index exceeds the limit value, turning to step S5 to re-plan the path; if all the three indexes do not exceed the limit value, outputting the current planned path;
s5: carrying out path re-planning, and solving a path planning problem by a genetic algorithm;
s6: and constructing a Bezier curve, fitting the planned path and outputting a new path.
Further, the step of S2 is as follows:
s2.1: searching a point closest to the current position of the vehicle on the reference path as a reference point, and extracting the coordinates of the reference point; respectively calculating course angles of the reference points according to the coordinates of the reference pointsPose information of the current vehicle, reference point initial coordinates (x 0 ,y 0 )；
S2.2: calculating a transverse error based on the selected reference point coordinates;
s2.3: based on the selected reference point coordinates, the curvature of the reference point is calculated.
Further, the step of S3 includes:
s3.1: constructing a three-degree-of-freedom vehicle dynamics model;
s3.2: calculating state quantity of a three-degree-of-freedom dynamic discrete model which does not consider the road tracking error, wherein the state quantity comprises the road curvature, the reference path tracking error and the course deviation change rate of a vehicle;
s3.3: discretizing the constructed three-degree-of-freedom vehicle dynamics model to obtain a three-degree-of-freedom dynamics discrete model.
Further, the judgment process in S4 is as follows:
s4.1: acquiring the actual speed v of the vehicle f Acceleration a f Calculate tracking error e y 。
S4.2: will be the actual vehicle speed v f Acceleration a f Tracking error e y Respectively with a set speed threshold v default Acceleration threshold a default And a tracking accuracy threshold e default Comparing;
if any index exceeds the limit value, path re-planning is needed; and if all the three indexes do not exceed the limit value, outputting the current planned path.
Further, the step of solving the path plan by using the genetic algorithm in S5 is as follows:
s5.1: decoding and encoding, adopting binary encoding, and optimizing parameters as curvature K of a reference path;
s5.2: decoding and outputting the optimized parameters;
s5.3: initializing the population, and randomly generating an initial population H= { gamma 1 ,γ 2 ,......,γ n A scheme comprising n chromosomes, each of which is re-routed for a path, the structure of the chromosome being denoted gamma i ＝{u 1 ,u 2 ,......,u p }，u p 0 or 1; optimizing the selected chromosomes, and calculating the difference degree between different chromosomes in each iteration population; increasing judgment criteria: in each iteration, only the population is similarWhen the index is lower than the threshold mu, the chromosomes can be subjected to the next iteration; the initial value of μ is a random value within the (0, 1) interval, the value μ decreases linearly from the initial value to 0 during the substitution, the attenuation factor ω=0.99 represents the attenuation rate, i.e., the attenuation formula of μ is: mu (mu) n+1 ＝ωμ t Mu approaches 0 when t approaches infinity;
s5.4: and selecting an adaptability function, and finding a target path closest to the reference path for tracking.
Further, the fitness function is expressed as:
wherein K is f To optimize the curvature of the path e y For tracking error, K is the reference path curvature.
Further, optimizing the curvature should meet the requirements of vehicle body stability:
wherein a is y A is the lateral acceleration of the vehicle body default For the system built-in acceleration threshold value, K default A curvature threshold is built into the system.
Further, the process of S6 is as follows:
s6.1: processing the environment map by binarization to construct a grid map with a plurality of pixel points; when the grid is a control point of the Bezier curve, the value is taken as 1, and when the grid is not the control point of the Bezier curve, the value is taken as 0; if the obstacle covers the grid, the value is set to be-1; if the path passes through the obstacle, namely, the path point with the value of-1, the path point is removed through obstacle avoidance operation in subsequent genetic operation;
s6.2: defining low-order continuity criteria: one line segment connecting the starting points has 0-order continuity; at the joint of two line segments, an equivalent tangent is used for ensuring the first-order continuity; the continuity of the three-order or more is ensured by a Bezier curve;
s6.3: on the premise of meeting the continuity, a third-order Bezier curve is adopted, and the three-order Bezier curve is expressed as:
F(t)＝B 0 (1-t) 3 +3B 1 (1-t) 2 t+3B 2 (1-t)t 2 +B 3 t 3
wherein t is a parameter variable, t= … …, b i Is the ith Bezier curve control point.
S6.4: and outputting the generated path and providing a newly planned path for the next tracking control.
An automatic driving vehicle path planning and tracking control system comprises a data acquisition module, an MPC controller, an error processing unit and a path re-planning module;
the data acquisition module acquires a reference path, and acquires target path information, real-time pose information and motion state information of a vehicle which are input by the reference path;
the MPC controller comprises an MPC transverse controller and an MPC longitudinal controller, wherein in the MPC controller, MPC controller unit data updated in real time are input to the MPC transverse controller; outputting a front wheel corner by an MPC transverse controller, inputting the front wheel corner into an MPC longitudinal controller, and calculating the longitudinal speed and the longitudinal acceleration of the automobile;
the error processing unit is respectively connected with the data acquisition module and the MPC controller, and the error processing unit carries out tracking precision judgment, speed judgment and curvature judgment on the output results of the data acquisition module and the MPC controller;
and the path re-planning module judges whether the path re-planning needs to be executed according to the judging result of the error processing unit.
Further, the vehicle motion state information includes a vehicle yaw rate ω and a vehicle longitudinal rate v x Lateral speed v of vehicle y The method comprises the steps of carrying out a first treatment on the surface of the The reference path is a discrete set of reference path points; the real-time pose information of the vehicle comprises the centroid position of the vehicle under a vehicle body coordinate system and the course angle of the vehicleThe target path information is a discrete set of reference path points, including abscissa and ordinate and curvature information of the path reference points in the geodetic coordinate system.
The invention has the beneficial effects that:
the invention aims to provide a curvature-optimized automatic driving vehicle path re-planning and path tracking control method, which improves tracking effect from the angles of curvature optimization, vehicle body comfort and control stability, improves path tracking precision and simultaneously improves running stability and smoothness of an intelligent vehicle.
Drawings
FIG. 1 is a schematic diagram of an autonomous vehicle path planning and tracking control method according to the present invention;
FIG. 2 is a block diagram of an autonomous vehicle path planning and tracking control system according to the present invention;
FIG. 3 is a schematic diagram of a three degree of freedom based vehicle dynamics model analysis;
FIG. 4 and FIG. 5 are schematic diagrams of Bezier curves;
FIG. 6 is a schematic diagram of a path trace simulation test effect;
fig. 7 is a schematic diagram of a path-tracking longitudinal velocity tracking effect.
Detailed Description
The present invention will be described in further detail with reference to the drawings and examples, in order to make the objects, technical solutions and advantages of the present invention more apparent. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the invention.
The automatic driving vehicle path planning and tracking control system shown in the attached figure 2 comprises a data acquisition module, an MPC controller, an error processing unit and a path re-planning module;
the data acquisition module acquires a reference path, and acquires target path information, real-time pose information and motion state information of a vehicle which are input by the reference path; wherein the vehicle motion state information includes a vehicle yaw rate ω and a vehicle longitudinal rate v x Lateral speed v of vehicle y . The reference path is offA scattered set of reference path points; the real-time pose information of the vehicle comprises the centroid position of the vehicle under a vehicle body coordinate system and the course angle of the vehicleThe target path information is a discrete set of reference path points, including abscissa and ordinate and curvature information of the path reference points in the geodetic coordinate system.
The MPC controller comprises an MPC transverse controller and an MPC longitudinal controller, wherein in the MPC controller, MPC controller unit data updated in real time are input to the MPC transverse controller; the MPC transverse controller outputs the front wheel rotation angle, and inputs the front wheel rotation angle into the MPC longitudinal controller to calculate the longitudinal speed and the longitudinal acceleration of the automobile.
The error processing unit is respectively connected with the data acquisition module and the MPC controller, and the error processing unit carries out tracking precision judgment, speed judgment and curvature judgment on the output results of the data acquisition module and the MPC controller;
and the path re-planning module judges whether the path re-planning needs to be executed according to the judging result of the error processing unit.
In this embodiment, if the output results of the data acquisition module and the MPC controller satisfy the tracking accuracy determination, the speed determination, and the curvature determination, then path re-planning is not performed; otherwise, if the output results of the data acquisition module and the MPC controller meet any one of the tracking precision judgment, the speed judgment and the curvature judgment, the path re-planning is executed.
Based on the system, the invention also provides an automatic driving vehicle path planning and tracking control method, which is shown in figures 1, 2 and 3, and comprises the following steps:
s1, acquiring target path information, real-time pose information and motion state information of a vehicle, which are input by a reference path, in real time, wherein the target path information, the real-time pose information and the motion state information are respectively initial reference point coordinates (X 0 ,Y 0 ) Actual vehicle speed v f Acceleration a f ；
S2: finding the real-time pose and curvature of the vehicle corresponding to the nearest reference point in the reference path according to the current position of the vehicle;
specifically, the specific steps of the step S2 include:
s2.1: searching a point closest to the current position of the vehicle on the reference path as a reference point, extracting the coordinates of the reference point, and respectively calculating the course angle of the reference point according to the coordinates of the reference pointPose information of the current vehicle, reference point initial coordinates (x 0 ,y 0 )；
S2.2: based on the selected reference point coordinates, calculating a transverse error, simplifying the transverse error into a shortest linear distance, and calculating the linear distance;
s2.3: calculating the curvature K of the reference point based on the selected reference point coordinates ref ；
S3: and respectively calculating the vehicle motion state information and the reference path tracking error of the three-degree-of-freedom dynamics discrete model according to the current position of the vehicle, the vehicle motion state information and the information (coordinates, transverse errors and curvature) of the reference point, and updating the unit data of the MPC controller in real time.
Specifically, the specific steps of the step S3 include:
s3.1: construction of three degree of freedom vehicle dynamics model
Taking the earth as a reference system, establishing a natural coordinate system XOY, taking the vehicle as the reference system, establishing a coordinate system o-xyz, and then carrying out stress analysis on each axis of the vehicle x, y and z by using the Dallange principle to respectively obtain the force and moment balance equations as follows:
considering that the tire side deflection angle and the vehicle side acceleration of the automatic driving vehicle are not large in the path tracking process, a linear tire model is selected and used, and analysis is carried out according to the roll motion of the dynamics model, so that the tire side deflection angle model of the linear roll motion is obtained as follows:
meter eta f ，η r The roll correction coefficients of the front wheel and the rear wheel are respectively:
defining the equivalent stiffness of the front wheel and the rear wheel as follows:
meanwhile, the vehicle lateral acceleration is:
the above equation is brought into a balance equation to obtain a three-degree-of-freedom vehicle dynamics differential equation set as follows:
wherein a and b are the distances between the front axle and the rear axle and the mass center of the automobile respectively; delta is the front wheel corner; omega r Is yaw rate; h is a s Distance from the roll axis is the sprung mass centroid; phi is the roll angle; f (F) y Lateral forces for each tire; v y Is the transverse speed of the vehicle; m, m s The total mass and the sprung mass of the vehicle are respectively; a, a y Is the transverse acceleration; f (F) f 、F r The lateral forces of the front wheel and the rear wheel are respectively; i x 、I Z Moment of inertia about the x-axis and z-axis, respectively; K. c is equivalent roll stiffness and damping, respectively; k (k) f 、k r The lateral deflection rigidity of the front wheel and the rear wheel respectively; alpha f 、α r Respectively the front and rear wheel slip angles; e (E) f 、E r Respectively the equivalent sides of the front axle and the rear axleA tilt-turn coefficient; η (eta) f 、η r The front wheel side-tipping correction coefficients and the rear wheel side-tipping correction coefficients are respectively; e (E) 1 、E 2 The front and rear suspensions are respectively provided with deformation steering equivalent coefficients.
S3.2: calculating state quantities of a three-degree-of-freedom dynamic discrete model without considering road tracking errors, comprising:
(1) Calculating the curvature of the road:
K＝K ref ；
(2) Calculating a reference path tracking error, and expressing the reference path tracking error by using errors of a Y axis and an X axis in a geodetic coordinate system:
(3) According to the yaw rate omega and the ideal course angle of the vehicleCalculating the course deviation change rate of the vehicle>
S3.3: discretizing the constructed three-degree-of-freedom vehicle dynamics model to obtain a three-degree-of-freedom dynamics discrete model, wherein the specific process is as follows:
s3.31: discretizing a three-degree-of-freedom vehicle dynamics model by utilizing a forward Euler method, wherein T is sampling time:
the linear time-varying equation adopts first-order forward Euler discretization:
and (3) decomposing to obtain:
note a=ta (t) +e, b=tb (t),
therefore, the vehicle dynamics model discretized linear state space equation and the output equation:
χ(k+1|t)＝Aχ(k|t)+Bu(k|t)+d k,t ；
wherein,u(k|t)＝δ-δ ref ，d k,t and neglected.
S3.32: calculating a discrete system matrix A and a control matrix B of a vehicle dynamics model taking path tracking into account and a reference vehicle front wheel steering angle delta ref ：
δ ref ＝arctan(LK)；
dT is discrete change time, a is the distance from the front axle to the mass center of the vehicle, b is the distance from the rear axle to the mass center of the vehicle, C cf C is the cornering stiffness of the front wheel cr For the cornering stiffness of the rear wheels, delta is the front wheel turning angle, delta ref For reference of front wheel angle, I Z For the moment of inertia of the vehicle,for course angle->The cornering acceleration is L, the wheelbase is L, and K is the road curvature.
S4: and (3) delivering the data obtained in the step (S3) to a built-in error processing unit for judgment, wherein a tracking precision threshold value, a speed threshold value and a curvature threshold value are set in the error processing unit, and the speed threshold value and the curvature threshold value are respectively used as judgment standards for whether path re-planning is needed. The specific process is as follows:
s4.1: acquiring the actual speed v of the vehicle f Acceleration a f Calculate tracking error e y 。
S4.2: will be the actual vehicle speed v f Acceleration a f Tracking error e y Respectively with a set speed threshold v default Acceleration threshold a default And a tracking accuracy threshold e default Comparing;
if any index exceeds the limit value, turning to step S5 to re-plan the path; and if all the three indexes do not exceed the limit value, outputting the current planned path.
S5: the path re-planning is carried out, and the specific process is as follows:
solving a path planning problem by using a genetic algorithm, comprising the following basic steps:
s5.1: the invention adopts binary coding, the optimization parameter is the curvature K of the reference path, and the binary length L has the calculation formula:wherein a and b are the optimization parameter ranges, which are respectively set to 0.07 and 0.2, eps is the precision required by optimizing, and is set to 0.01;
s5.2: decoding and outputting the optimized parameters, wherein the formula is as follows:wherein M is two of optimization parametersAnd (3) carrying out decimal expression, wherein m is the decimal expression of the final optimization parameter.
S5.3: initializing the population, and randomly generating an initial population H= { gamma 1 ,γ 2 ,......,γ n A scheme comprising n chromosomes, each of which is re-planned for a path, the structure of which can be represented as gamma i ＝{u 1 ,u 2 ,......,u p }. Optimizing the selected chromosomes, and calculating the difference degree between different chromosomes in each iteration population. Adding a judgment criterion: in each iteration, these chromosomes can only be subjected to the next iteration if the similarity index of the population is below the threshold μ. Wherein the initial value of μ is a random value within the (0, 1) interval. The value μ during the selection process decreases linearly from the initial value to 0, and the attenuation factor ω=0.99 represents the attenuation rate, i.e., the attenuation formula for μ is: mu (mu) n+1 ＝ωμ t . Wherein, when t approaches infinity, μ approaches 0;
s5.4: selecting a fitness function, searching a target path closest to a reference path for tracking, and solving a minimum problem, wherein the formula is as follows:wherein K is f To optimize the curvature of the path. Meanwhile, the optimized curvature should meet the requirement of vehicle body stability: />
S6: and constructing a Bezier curve processing unit, wherein the specific steps of the S6 comprise:
s6.1: the environment map is processed in a binarization mode, and a grid map with a plurality of pixel points is constructed. Each grid corresponds to a gene, and its value may be 0 or 1, and when the grid is a control point of the bezier curve, its value is taken as 1, and when it is not a control point of the bezier curve, its value is taken as 0. If the barrier covers the grid, the value is set to be-1, and the barrier cannot be used as a control point; a gene value of 0 or 1 represents whether the path point is a candidate control point. If the path passes through the obstacle, namely, the path point with the value of-1, the path point is removed through obstacle avoidance operation in subsequent genetic operation;
s6.2: while guaranteeing continuity of path planning, a low-order continuity criterion is defined here as follows:
one line segment connecting the starting points has 0-order continuity; at the joint of two line segments, an equivalent tangent is used for ensuring the first-order continuity; the continuity of the three-order or more is ensured by a Bezier curve;
s6.3: on the premise of meeting the continuity, a third-order Bezier curve is adopted, wherein the three-order Bezier curve contains 4 control points N 0 ，N 1 ，N 2 ，N 3 As shown in fig. 4 and 5, α and β respectively represent the vector N 0 N 1 ，N 1 N 2 ，N 2 N 3 An included angle between the two. The third-order Bezier curve formula is: f (t) =b 0 (1-t) 3 +3B 1 (1-t) 2 t+3B 2 (1-t)t 2 +B 3 t 3 ；
S6.4: and outputting the generated path and providing a newly planned path for the next tracking control.
According to the automatic driving vehicle path planning and tracking control method based on the genetic algorithm curvature optimization, on the basis of selection of a basic path tracking controller, the advantages of high tracking precision, better robustness, quicker control feedback and the like of model prediction control compared with a traditional controller are considered, the model prediction control is selected as a basic controller, and meanwhile, a path re-planning module is redesigned, curvature optimization is carried out, the tracking precision is improved, and meanwhile, the stability of the vehicle is improved.
In order to better verify and explain the technical effects adopted in the method, the embodiment selects a simulation platform for testing, and verifies the actual effects of the method. With reference to fig. 6 and fig. 7, it can be seen that the tracking accuracy effect in the simulation experiment result is better, the error is smaller, the set re-planning path is more reasonable, and the robustness is higher, and meanwhile, the automatic driving vehicle can respond to the longitudinal acceleration behavior in time to reach the target speed.
The above embodiments are merely for illustrating the design concept and features of the present invention, and are intended to enable those skilled in the art to understand the content of the present invention and implement the same, the scope of the present invention is not limited to the above embodiments. Therefore, all equivalent changes or modifications according to the principles and design ideas of the present invention are within the scope of the present invention.
Claims (10)
1. An automatic driving vehicle path planning and tracking control method is characterized by comprising the following steps:
s1, acquiring target path information, real-time pose information and motion state information of a vehicle, which are input by a reference path, in real time;
s2: finding the nearest reference point and the real-time pose and curvature of the vehicle corresponding to the reference point in the reference path according to the current position of the vehicle;
s3: respectively calculating vehicle motion state information and reference path tracking error of the three-degree-of-freedom dynamics discrete model according to the current position of the vehicle, the vehicle motion state information and the information of the reference point, and updating unit data of the MPC controller in real time;
s4: setting a tracking precision threshold, a speed threshold and an acceleration threshold, and taking the tracking precision threshold, the speed threshold and the acceleration threshold as judging standards of whether path re-planning is needed or not; based on the data obtained in S3, the actual vehicle speed v is determined f Acceleration a f Tracking error e y Respectively corresponding to the set speed threshold v default Acceleration threshold a default And a tracking accuracy threshold e default Comparing; if any index exceeds the limit value, turning to step S5 to re-plan the path; if all the three indexes do not exceed the limit value, outputting the current planned path;
s5: carrying out path re-planning, and solving a path planning problem by a genetic algorithm;
s6: and constructing a Bezier curve, fitting the planned path and outputting a new path.
2. The method for automatically driving a vehicle path planning and tracking control according to claim 1, wherein the step of S2 is as follows:
s2.1: searching a point closest to the current position of the vehicle on the reference path as a reference point, and extracting the coordinates of the reference point; respectively calculating course angles of the reference points according to the coordinates of the reference pointsPose information of the current vehicle, reference point initial coordinates (x 0 ,y 0 )；
S2.2: calculating a transverse error based on the selected reference point coordinates;
s2.3: based on the selected reference point coordinates, the curvature of the reference point is calculated.
3. The method for automatically driving a vehicle path planning and tracking control according to claim 1, wherein the step of S3 comprises:
s3.1: constructing a three-degree-of-freedom vehicle dynamics model;
s3.2: calculating state quantity of a three-degree-of-freedom dynamic discrete model which does not consider the road tracking error, wherein the state quantity comprises the road curvature, the reference path tracking error and the course deviation change rate of a vehicle;
s3.3: discretizing the constructed three-degree-of-freedom vehicle dynamics model to obtain a three-degree-of-freedom dynamics discrete model.
4. The method for planning and tracking control of an automatically driven vehicle according to claim 1, wherein the determining process in S4 is as follows:
s4.1: acquiring the actual speed v of the vehicle f Acceleration a f Calculate tracking error e y 。
S4.2: will be the actual vehicle speed v f Acceleration a f Tracking error e y Respectively with a set speed threshold v default Acceleration threshold a default And a tracking accuracy threshold e default Comparing;
if any index exceeds the limit value, path re-planning is needed; and if all the three indexes do not exceed the limit value, outputting the current planned path.
5. The method for path planning and tracking control of an autonomous vehicle according to claim 1, wherein the step of solving the path plan in S5 by using a genetic algorithm comprises the steps of:
s5.1: decoding and encoding, adopting binary encoding, and optimizing parameters as curvature K of a reference path;
s5.2: decoding and outputting the optimized parameters;
s5.3: initializing the population, and randomly generating an initial population H= { gamma 1 ,γ 2 ,......,γ n -comprising n chromosomes, each being a path re-planning scheme; optimizing the selected chromosomes, and calculating the difference degree between different chromosomes in each iteration population; increasing judgment criteria: in each iteration, the chromosomes can only be subjected to the next iteration when the similarity index of the population is lower than the threshold mu; the initial value of μ is a random value within the (0, 1) interval, the value μ decreases linearly from the initial value to 0 during the substitution, the attenuation factor ω=0.99 represents the attenuation rate, i.e., the attenuation formula of μ is: mu (mu) n+1 ＝ωμ t Mu approaches 0 when t approaches infinity;
s5.4: and selecting an adaptability function, and finding a target path closest to the reference path for tracking.
6. The method of claim 5, wherein the fitness function is expressed as:
wherein K is f To optimize the curvature of the path e y For tracking error, K is the reference path curvature.
7. The method of automatically driven vehicle path planning and tracking control of claim 6, wherein optimizing the curvature should meet the vehicle body stability requirement:
wherein a is y A is the lateral acceleration of the vehicle body default For the system built-in acceleration threshold value, K default A curvature threshold is built into the system.
8. The method for automatically driving vehicle path planning and tracking control according to claim 1, wherein the process of S6 is as follows:
s6.1: processing the environment map by binarization to construct a grid map with a plurality of pixel points; when the grid is a control point of the Bezier curve, the value is taken as 1, and when the grid is not the control point of the Bezier curve, the value is taken as 0; if the obstacle covers the grid, the value is set to be-1; if the path passes through the obstacle, namely, the path point with the value of-1, the path point is removed through obstacle avoidance operation in subsequent genetic operation;
s6.2: defining low-order continuity criteria: one line segment connecting the starting points has 0-order continuity; at the joint of two line segments, an equivalent tangent is used for ensuring the first-order continuity; the continuity of the three-order or more is ensured by a Bezier curve;
s6.3: on the premise of meeting the continuity, a third-order Bezier curve is adopted, and the three-order Bezier curve is expressed as:
F(t)＝B 0 (1-t) 3 +3B 1 (1-t) 2 t+3B 2 (1-t)t 2 +B 3 t 3
wherein t is a parameter variable, t= … …, b i Is the ith Bezier curve control point.
S6.4: and outputting the generated path and providing a newly planned path for the next tracking control.
9. The automatic driving vehicle path planning and tracking control system is characterized by comprising a data acquisition module, an MPC controller, an error processing unit and a path re-planning module;
the data acquisition module acquires a reference path, and acquires target path information, real-time pose information and motion state information of a vehicle which are input by the reference path;
the MPC controller comprises an MPC transverse controller and an MPC longitudinal controller, wherein in the MPC controller, MPC controller unit data updated in real time are input to the MPC transverse controller; outputting a front wheel corner by an MPC transverse controller, inputting the front wheel corner into an MPC longitudinal controller, and calculating the longitudinal speed and the longitudinal acceleration of the automobile;
the error processing unit is respectively connected with the data acquisition module and the MPC controller, and the error processing unit carries out tracking precision judgment, speed judgment and curvature judgment on the output results of the data acquisition module and the MPC controller;
and the path re-planning module judges whether the path re-planning needs to be executed according to the judging result of the error processing unit.
10. An autonomous vehicle path planning and tracking control system as defined in claim 1, wherein the vehicle motion state information includes vehicle yaw rate ω, vehicle longitudinal rate v x Lateral speed v of vehicle y The method comprises the steps of carrying out a first treatment on the surface of the The reference path is a discrete set of reference path points; the real-time pose information of the vehicle comprises the centroid position of the vehicle under a vehicle body coordinate system and the course angle of the vehicleThe target path information is a discrete set of reference path points, including abscissa and ordinate and curvature information of the path reference points in the geodetic coordinate system.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311224283.3A CN117250860A (en) | 2023-09-21 | 2023-09-21 | Automatic driving vehicle path planning and tracking control system and method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311224283.3A CN117250860A (en) | 2023-09-21 | 2023-09-21 | Automatic driving vehicle path planning and tracking control system and method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117250860A true CN117250860A (en) | 2023-12-19 |
Family
ID=89136313
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311224283.3A Pending CN117250860A (en) | 2023-09-21 | 2023-09-21 | Automatic driving vehicle path planning and tracking control system and method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117250860A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117850413A (en) * | 2023-12-26 | 2024-04-09 | 上海联适导航技术股份有限公司 | Novel vehicle control method based on broken line path |
-
2023
- 2023-09-21 CN CN202311224283.3A patent/CN117250860A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117850413A (en) * | 2023-12-26 | 2024-04-09 | 上海联适导航技术股份有限公司 | Novel vehicle control method based on broken line path |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Tang et al. | An improved kinematic model predictive control for high-speed path tracking of autonomous vehicles | |
| CN113320542B (en) | Tracking control method for automatic driving vehicle | |
| CN107561942A (en) | Intelligent vehicle track following model predictive control method based on model compensation | |
| Xu et al. | Model predictive control for lane keeping system in autonomous vehicle | |
| CN110267866B (en) | Apparatus for controlling vehicle trajectory | |
| CN111930112A (en) | Intelligent vehicle path tracking control method and system based on MPC | |
| CN112578672B (en) | Unmanned vehicle trajectory control system based on chassis nonlinearity and trajectory control method thereof | |
| CN110147628A (en) | Consider the tire cornering stiffness zoning method for calculating of multifactor variation | |
| CN116627044B (en) | Travel track prediction control method | |
| CN114572251A (en) | High-speed automatic driving automobile track tracking method based on predictive control | |
| CN113581201A (en) | Potential field model-based collision avoidance control method and system for unmanned automobile | |
| CN116048081A (en) | Automatic driving vehicle decision and regulation method considering safety boundary constraint | |
| CN116184835A (en) | Unmanned vehicle tracking control method considering input time delay and speed time variation | |
| Kim et al. | Vehicle path tracking control using pure pursuit with MPC-based look-ahead distance optimization | |
| Leng et al. | Deep reinforcement learning-based drift parking control of automated vehicles | |
| Kone | Lateral and longitudinal control of an autonomous racing vehicle. | |
| CN115525054B (en) | Method and system for controlling tracking of edge path of unmanned sweeper in large industrial park | |
| Xu et al. | Linear time-varying MPC vehicle trajectory tracking controller considering driving road surface factors | |
| Németh et al. | Mechanical analysis and control design of a variable-geometry McPherson suspension | |
| CN117250860A (en) | Automatic driving vehicle path planning and tracking control system and method | |
| CN114435371B (en) | Road gradient estimation method and device | |
| Li et al. | Path tracking control based on the prediction of tire state stiffness using the optimized steering sequence | |
| Chen et al. | Path tracking controller design of automated parking systems via NMPC with an instructible solution | |
| CN114684109A (en) | Electric automobile yaw stability control method and device | |
| Reda et al. | Model-based control strategy for autonomous vehicle path tracking task |
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 |