CN115179935A - Path tracking method and device, electronic equipment and storage medium - Google Patents

Path tracking method and device, electronic equipment and storage medium Download PDF

Info

Publication number
CN115179935A
CN115179935A CN202211108443.3A CN202211108443A CN115179935A CN 115179935 A CN115179935 A CN 115179935A CN 202211108443 A CN202211108443 A CN 202211108443A CN 115179935 A CN115179935 A CN 115179935A
Authority
CN
China
Prior art keywords
vehicle
equation
error
path
augmentation
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
CN202211108443.3A
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.)
Haomo Zhixing Technology Co Ltd
Original Assignee
Haomo Zhixing Technology 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 Haomo Zhixing Technology Co Ltd filed Critical Haomo Zhixing Technology Co Ltd
Priority to CN202211108443.3A priority Critical patent/CN115179935A/en
Publication of CN115179935A publication Critical patent/CN115179935A/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
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
    • B60W30/10—Path keeping
    • 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/0025—Planning or execution of driving tasks specially adapted for specific operations
    • 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
    • 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/0043—Signal treatments, identification of variables or parameters, parameter estimation or state estimation

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Steering Control In Accordance With Driving Conditions (AREA)

Abstract

The application is applicable to the technical field of vehicles, and provides a path tracking method, a path tracking device, an electronic device and a storage medium, wherein the method comprises the following steps: the method comprises the steps of constructing a dynamic vehicle model and a vehicle error model, obtaining a discretization state equation and a current cost equation, carrying out augmentation processing by using the curvature of each pre-aiming point of an expected path to obtain a pre-aiming augmentation state equation and an augmentation cost equation, obtaining feedforward gain and feedback gain of an optimal control law, obtaining the number of the pre-aiming points based on the current vehicle speed and the feedforward gain of the optimal control law of a first preset condition, obtaining the vehicle transverse error of each pre-aiming point of the expected path, obtaining the front wheel rotating angle corresponding to each pre-aiming point by using the optimal front wheel rotating angle calculation formula of the vehicle and controlling the vehicle to track the path based on the feedforward gain, the feedback gain, the vehicle transverse error of each pre-aiming point and the curvature of each pre-aiming point, and therefore, when the vehicle is driven automatically or unmanned, the vehicle path tracking can take the smoothness and the real-time of the control into account.

Description

Path tracking method and device, electronic equipment and storage medium
Technical Field
The present application belongs to the field of vehicle technologies, and in particular, to a method and an apparatus for path tracking, an electronic device, and a storage medium.
Background
Currently, automatic driving or unmanned driving is rapidly developing as a new technology capable of improving traffic safety and reducing traffic congestion.
A Linear Quadratic Regulator (LQR) in the prior art based on a vehicle dynamics model and an optimal control theory is a commonly used path tracking algorithm. However, the linear quadratic regulator algorithm cannot take into account future road information, and the control accuracy and smoothness of the linear quadratic regulator algorithm are reduced when a straight road enters a curve and the curvature of the road changes in the future. If all online optimization methods are adopted, for example, an online model is adopted to predict future path information and control a vehicle to track a path, a large amount of calculation is increased, and the real-time performance of vehicle path tracking control cannot be realized.
When the vehicle is driven automatically or unmanned, the path tracking method in the prior art has the problem that the smoothness and the real-time performance of the vehicle path tracking control cannot be simultaneously considered.
Disclosure of Invention
The embodiment of the application provides a path tracking method, a path tracking device, electronic equipment and a storage medium, and aims to solve the problem that smoothness and real-time performance of vehicle path tracking control of the path tracking method in the prior art cannot be considered at the same time during automatic driving or unmanned driving.
In a first aspect, an embodiment of the present application provides a method for path tracking, including:
constructing a two-degree-of-freedom dynamic vehicle model based on the center of mass of the vehicle and a vehicle error model of an expected path;
obtaining a discretization state equation and a current cost equation based on the dynamic vehicle model and the vehicle error model;
carrying out augmentation processing on the discretization state equation and the current cost equation by using the curvature of each pre-aiming point of the expected path to obtain a pre-aiming augmentation state equation and an augmentation cost equation;
obtaining a feedforward gain and a feedback gain of an optimal control law based on the preview augmentation state equation and the augmentation cost equation;
obtaining the number of the preview points based on the current vehicle speed and the feedforward gain of the optimal control law under a first preset condition;
obtaining a vehicle lateral error of each preview point of the prospective path based on the vehicle error model and the number of preview points;
obtaining a front wheel rotating angle corresponding to each preview point through an optimal front wheel rotating angle calculation formula of the vehicle based on the feedforward gain, the feedback gain, the vehicle transverse error of each preview point and the curvature of each preview point;
and controlling the vehicle to track the path based on the front wheel turning angles corresponding to the pre-aiming points of the expected path.
In one embodiment, the constructing the two-degree-of-freedom dynamic vehicle model based on the vehicle center of mass and the vehicle error model of the expected path comprises:
constructing a two-degree-of-freedom dynamic vehicle model based on the vehicle mass center based on a second preset condition;
constructing a vehicle error model of the expected path based on the dynamic vehicle model, wherein the vehicle error model comprises a first vehicle error equation, a second vehicle error equation, a third vehicle error equation, and a fourth vehicle error equation.
In one embodiment, the obtaining a discretized state equation and a current cost equation based on the dynamic vehicle model and the vehicle error model includes:
obtaining a fifth vehicle error equation and a sixth vehicle error equation based on the dynamic vehicle model and the vehicle error model;
converting the fifth vehicle error equation and the sixth vehicle error equation to obtain a continuous state equation;
discretizing through bilinear transformation based on the continuous state equation to obtain the discretization state equation;
obtaining the current cost equation based on the discretized state equation.
In one embodiment, the fifth vehicle error equation is:
Figure 100002_DEST_PATH_IMAGE001
wherein, the first and the second end of the pipe are connected with each other,
Figure 100002_DEST_PATH_IMAGE002
a second derivative of the lateral error of the vehicle from the center of mass to the expected path;
Figure 100002_DEST_PATH_IMAGE003
is the first derivative of the lateral error of the vehicle from the center of mass to the intended path;
Figure 100002_DEST_PATH_IMAGE004
is the first derivative of the course angle error;
Figure 100002_DEST_PATH_IMAGE005
a first derivative of a heading angle at a home point of the intended path;
C af is vehicle front wheel cornering stiffness; c ar Is vehicle rear wheel cornering stiffness;
l f the wheelbase from the front wheel to the center of mass; l. the r The wheelbase from the front wheel to the center of mass;
v x a speed in the longitudinal direction; m is the mass of the vehicle;
δ is the front wheel corner of the desired path;
the sixth vehicle error equation is:
Figure 100002_DEST_PATH_IMAGE006
wherein the content of the first and second substances,
Figure 100002_DEST_PATH_IMAGE007
is the second derivative of the course angle error;
Figure 100002_DEST_PATH_IMAGE008
is the vehicle steering moment of inertia;
Figure 100002_DEST_PATH_IMAGE009
a second derivative of a heading angle for a predicted point of the desired path;
the continuous state equation is:
Figure 100002_DEST_PATH_IMAGE010
wherein, c R Is the curvature of the current path;
σ 1 is a first coefficient, σ 2 Is the second coefficient, σ 3 For the third coefficient, the following is set:
Figure 100002_DEST_PATH_IMAGE011
x is the vehicle lateral error of each preview point of the prospective path;
Figure 100002_DEST_PATH_IMAGE012
a first derivative of a vehicle lateral error for each pre-sight point of the prospective path;
the discretization state equation is as follows:
Figure 100002_DEST_PATH_IMAGE013
wherein k is the time of the current moment;
a is a first coefficient matrix, B is a second coefficient matrix, D is a third coefficient matrix, and
Figure 100002_DEST_PATH_IMAGE014
；
the current cost equation is:
Figure 100002_DEST_PATH_IMAGE015
wherein J is a current cost function;
t is a symbol of matrix transposition;
Figure 100002_DEST_PATH_IMAGE016
is a first positively determined weight matrix, R is a second positively determined weight matrix, and
Figure 100002_DEST_PATH_IMAGE017
。
in one embodiment, the obtaining of the feedforward gain and the feedback gain of the optimal control law by offline solving based on the preview augmented state equation and the augmented cost equation includes:
obtaining an optimal control strategy equation by adopting a dynamic programming method based on the augmentation cost equation;
obtaining an analytical equation of an optimal control law based on the optimal control strategy equation;
and obtaining the feedforward gain and the feedback gain of the optimal control law based on the analytic equation and the preview augmentation state equation of the optimal control law.
In one embodiment, the optimal control strategy equation is:
Figure 100002_DEST_PATH_IMAGE018
wherein, J * An optimal control strategy in the cost function is adopted;
x is the vehicle lateral error of each preview point of the prospective path;
x is the augmented vehicle lateral error at each pre-aiming point of the expected path;
δ is the front wheel steering angle of the intended path; t is a symbol of matrix transposition; k is the time of the current moment;
Figure 100002_DEST_PATH_IMAGE019
a weight matrix that is positively determined for the first augmentation,
Figure 100002_DEST_PATH_IMAGE020
a weight matrix positively determined for the second augmentation, an
Figure 100002_DEST_PATH_IMAGE021
The settings are as follows:
Figure 100002_DEST_PATH_IMAGE022
the analytical equation for the optimal control law is:
Figure 100002_DEST_PATH_IMAGE023
wherein, delta * Is the optimal front wheel corner;
b is a second coefficient matrix, and B belongs to \8477 4 ；
Figure 100002_DEST_PATH_IMAGE024
The augmentation matrix is B and is a second augmentation coefficient matrix;
Figure 100002_DEST_PATH_IMAGE025
for the third augmented positive definite weight matrix, the following is set:
Figure 100002_DEST_PATH_IMAGE026
beta is the slip angle of the vehicle's center of mass;
Figure 100002_DEST_PATH_IMAGE027
is a first matrix of augmentation coefficients, and
Figure 100002_DEST_PATH_IMAGE028
。
in one embodiment, the preview augmented state equation is:
Figure 100002_DEST_PATH_IMAGE029
wherein X (k + 1) is the augmented vehicle lateral error of the next home point at the current time k of the prospective path;
x (k) is the augmented vehicle lateral error for the home point at the current time k of the prospective path, set to:
Figure 100002_DEST_PATH_IMAGE030
C R to increase the curvature, the settings were:
Figure 100002_DEST_PATH_IMAGE031
t is a symbol of matrix transposition; c. C R Is the curvature of the current path;
k is the current time;
Figure 663951DEST_PATH_IMAGE027
is a first matrix of the amplification coefficients,
Figure 331692DEST_PATH_IMAGE024
is a second matrix of augmentation coefficients, and
Figure 100002_DEST_PATH_IMAGE032
the settings are as follows:
Figure 100002_DEST_PATH_IMAGE033
l and D are middle matrixes,
Figure 100002_DEST_PATH_IMAGE034
the augmentation matrix is D, and the third augmentation coefficient matrix is D;
the augmentation cost equation is:
Figure 100002_DEST_PATH_IMAGE035
wherein J' is an augmentation cost function;
δ is the front wheel corner of the desired path;
Figure 485724DEST_PATH_IMAGE019
a weight matrix that is positively determined for the first augmentation,
Figure 947930DEST_PATH_IMAGE020
a weight matrix positively determined for the second augmentation, an
Figure 100002_DEST_PATH_IMAGE036
The settings are as follows:
Figure 100002_DEST_PATH_IMAGE037
o is a zero matrix, I is a unit matrix, and\8477and \8469;
the optimal front wheel steering angle calculation formula is as follows:
Figure 100002_DEST_PATH_IMAGE038
wherein, K b For feedback gain, set to:
Figure 100002_DEST_PATH_IMAGE039
Figure 512379DEST_PATH_IMAGE020
is an augmented matrix of R, P is a first intermediate matrix,
Figure 718232DEST_PATH_IMAGE027
an augmentation matrix of A;
K f,i the feed forward gain for the ith preview point is set as:
Figure 100002_DEST_PATH_IMAGE040
ζ is a second intermediate matrix, set to:
Figure 100002_DEST_PATH_IMAGE041
δ * the optimal front wheel turning angle is obtained.
In one embodiment, the first preset condition is that the last value of the feedforward gain of the optimal control law is close to 0, and the sampling interval time is less than or equal to 0.02 seconds.
In a second aspect, an embodiment of the present application provides an apparatus for path tracking, including:
the vehicle model building module is used for building a two-degree-of-freedom dynamic vehicle model based on the mass center of the vehicle and a vehicle error model of an expected path;
the current state equation obtaining module is used for obtaining a discretization state equation and a current cost equation based on the dynamic vehicle model and the vehicle error model;
the acquisition and amplification state equation module is used for carrying out amplification processing on the discretization state equation and the current cost equation by using the curvature of each pre-aiming point of the expected path to obtain a pre-aiming and amplification state equation and an amplification cost equation;
the optimal control law obtaining module is used for obtaining feedforward gain and feedback gain of the optimal control law based on the preview augmentation state equation and the augmentation cost equation;
the number obtaining module is used for obtaining the number of the preview points according to the current vehicle speed and the feedforward gain of the optimal control law based on a first preset condition;
the vehicle error obtaining module is used for obtaining the vehicle transverse error of each pre-aiming point of the expected path based on the vehicle error model and the number of the pre-aiming points;
the front wheel rotation angle obtaining module is used for obtaining a front wheel rotation angle corresponding to each pre-aiming point through an optimal front wheel rotation angle calculation formula of the vehicle based on the feedforward gain, the feedback gain, the vehicle transverse error of each pre-aiming point and the curvature of each pre-aiming point;
and the path tracking module is used for controlling the vehicle to track the path based on the front wheel turning angles corresponding to the pre-aiming points of the expected path.
In a third aspect, an embodiment of the present application provides an electronic device, which includes a memory, a processor, and a computer program stored in the memory and executable on the processor, and the processor, when executing the computer program, implements the method according to any one of the contents of the first aspect.
In a fourth aspect, the present application provides a computer-readable storage medium, which stores a computer program, where the computer program is implemented to implement the method according to any one of the contents of the first aspect when executed by a processor.
It is understood that the beneficial effects of the second to fourth aspects can be seen from the description of the first aspect, and are not described herein again.
Compared with the prior art, the embodiment of the application has the advantages that:
the method comprises the steps that a two-degree-of-freedom dynamic vehicle model based on the mass center of a vehicle and a vehicle error model of an expected path are built, a discretization state equation and a current cost equation are obtained, the discretization state equation and the current cost equation are subjected to amplification processing through the curvature of each pre-aiming point of the expected path to obtain a pre-aiming amplification state equation and an amplification cost equation, then feedforward gain and feedback gain of an optimal control law are obtained, the number of the pre-aiming points is obtained based on the feedforward gain of the optimal control law of a first preset condition, the vehicle transverse error of each pre-aiming point of the expected path is obtained based on the vehicle error model and the number of the pre-aiming points, and front wheel rotating angles corresponding to each pre-aiming point are obtained through an optimal front wheel rotating angle calculation formula of the vehicle based on the feedforward gain, the feedback gain, the vehicle transverse error of each pre-aiming point and the curvature of each pre-aiming point; the vehicle is controlled to track the path based on the front wheel steering angles corresponding to the pre-aiming points of the expected path, so that the accuracy of vehicle path tracking is improved and the calculated amount of the optimal control law of the vehicle path tracking is reduced during automatic driving or unmanned driving, and the smoothness and the real-time performance of the vehicle path tracking can be considered at the same time.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed to be used in the embodiments or the prior art descriptions will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present application, and it is obvious for those skilled in the art to obtain other drawings without creative efforts.
Fig. 1 is a schematic flowchart of a method for path tracking according to an embodiment of the present application;
FIG. 2 is a schematic diagram of a two-degree-of-freedom two-wheel bicycle dynamic vehicle model provided by an embodiment of the present application;
FIG. 3 is a schematic flow chart illustrating steps for obtaining a discretized state equation and a current cost equation based on a dynamic vehicle model and a vehicle error model according to an embodiment of the present disclosure;
FIG. 4 is a schematic flow chart of the feedforward gain and the feedback gain step for obtaining the optimal control law based on the preview augmented state equation and the augmented cost equation according to an embodiment of the present application;
fig. 5 is a schematic diagram illustrating a result of calculating the number of preview points at different speed values of the current vehicle speed according to a method for path tracking provided in an embodiment of the present application;
fig. 6 is a schematic structural diagram of a path tracking apparatus according to an embodiment of the present application.
Detailed Description
In the following description, for purposes of explanation and not limitation, specific details are set forth such as particular system structures, techniques, etc. in order to provide a thorough understanding of the embodiments of the present application. It will be apparent, however, to one skilled in the art that the present application may be practiced in other embodiments that depart from these specific details. In other instances, detailed descriptions of well-known systems, devices, circuits, and methods are omitted so as not to obscure the description of the present application with unnecessary detail.
It will be understood that the terms "comprises" and/or "comprising," when used in this specification and the appended claims, specify the presence of stated features, integers, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and/or groups thereof.
It should also be understood that the term "and/or" as used in this specification and the appended claims refers to and includes any and all possible combinations of one or more of the associated listed items.
Furthermore, in the description of the present application and the appended claims, the terms "first," "second," "third," and the like are used for distinguishing between descriptions and not necessarily for describing or implying relative importance.
Reference throughout this specification to "one embodiment" or "some embodiments," or the like, means that a particular feature, structure, or characteristic described in connection with the embodiment is included in one or more embodiments of the present application. Thus, appearances of the phrases "in one embodiment," "in some embodiments," "in other embodiments," or the like, in various places throughout this specification are not necessarily all referring to the same embodiment, but rather "one or more but not all embodiments" unless specifically stated otherwise. The terms "comprising," "including," "having," and variations thereof mean "including, but not limited to," unless otherwise specifically stated.
Path tracking control of a vehicle, i.e. controlling the steering wheel of the vehicle to make the vehicle track a desired path, is an important part of autonomous or unmanned driving technology. The multipoint preview is a path tracking control algorithm which adds curvature information of a plurality of path preview points in a preview window of a predicted path in the control process of a Linear Quadratic Regulator (LQR) and processes curvature change of the predicted path.
The accuracy and the smoothness are two important performance indexes in the path tracking of the vehicle, the accuracy means that the path tracking error of the vehicle is small, and the smoothness means that the vehicle does not have large steering action in the path tracking. The accuracy and smoothness of the path tracking of the vehicle directly affect the safety of the vehicle and the user experience of the passengers. In addition, the calculation time required for the vehicle to calculate the amount of route tracking also needs to be shortened as much as possible, so that the vehicle control is real-time. However, the path tracking of the vehicle in the prior art cannot achieve both smoothness and real-time performance.
The Cost Function (Cost Function) is a Function representing the error between the predicted value and the actual value.
The embodiment provides a path tracking method, which includes incorporating curvatures of all pre-aiming points in a pre-aiming window in an expected path into a state quantity, converting a path tracking control problem into an augmented linear quadratic regulator control problem, solving an optimal problem controlled by the augmented linear quadratic regulator to obtain an optimal steering control law, wherein the optimal steering control law increases the influence of the curvatures of all the pre-aiming points in the pre-aiming window in the expected path, and online optimization is not needed. Therefore, the accuracy of vehicle path tracking is improved and the calculated amount of the optimal control law of the vehicle path tracking is reduced during automatic driving or unmanned driving, so that the smoothness and the real-time performance of the vehicle path tracking can be considered.
The technical solution of the present application will be described below by specific examples.
In a first aspect, as shown in fig. 1, the present embodiment provides a method for path tracking, including:
s100, constructing a two-degree-of-freedom dynamic vehicle model based on the vehicle center of mass and a vehicle error model of an expected path.
And constructing a two-degree-of-freedom dynamic vehicle model and a vehicle error model of the expected path on the basis of the vehicle mass center, and calculating and correcting the vehicle transverse error generated when the vehicle tracks the expected path according to the dynamic vehicle model.
In one embodiment, constructing a two degree of freedom dynamic vehicle model based on a vehicle center of mass and a vehicle error model of a prospective path comprises:
and S110, constructing a two-degree-of-freedom dynamic vehicle model based on the vehicle mass center based on a second preset condition.
As shown in FIG. 2, in one embodiment, a two degree of freedom dynamic vehicle model based on lateral and yaw motion is constructed based on the vehicle center of mass o of the two-wheel bicycle model to control the vehicle for path tracking based on various parameters of the vehicle in the desired path. In FIG. 2, A is the front wheel of the vehicle, B is the rear wheel of the vehicle, XOY is the geodetic coordinate system, XOY is the coordinate system of the vehicle with the centroid o of the vehicle as the origin, x is the forward direction of the vehicle, y is the lateral movement direction of the vehicle, and phi is the heading angle,βis the slip angle of the center of mass, e y For lateral error of the vehicle from the centre of mass to the intended path, e φ Is the heading angle error.
Since it is necessary that the path tracking of the expected path has smoothness, the smaller the heading angle error of the center of mass of the vehicle is, the better, the heading angle error is set to be a small angle, the yaw angle of the center of mass is a small angle, so that sin β ≈ 0 and cos β ≈ 1, so that the second preset condition is set that the lateral acceleration of the vehicle is less than or equal to 0.3g, and the yaw force of the tire is proportional to the yaw angle.
Therefore, the constructed two-degree-of-freedom dynamic vehicle model is as follows:
Figure DEST_PATH_IMAGE042
wherein m is the mass of the vehicle; a is a y Is the lateral acceleration of the vehicle;
F yf for the front wheel side biasing force, it is set to:
Figure DEST_PATH_IMAGE043
C af for the vehicle front wheel cornering stiffness, α f Is a front wheel side slip angle;
F yr for the rear wheel side biasing force, it is set to:
Figure DEST_PATH_IMAGE044
C ar for vehicle rear wheel cornering stiffness, α r Is a rear wheel side slip angle;
l f the wheelbase from the front wheel to the center of mass; l r The wheelbase from the front wheel to the center of mass;
Figure DEST_PATH_IMAGE045
a steering moment of inertia for the vehicle;
Figure DEST_PATH_IMAGE046
is the heading angular velocity.
In one embodiment, the front wheel side slip angle α f And rear wheel side slip angle alpha r Respectively set as follows:
Figure DEST_PATH_IMAGE047
wherein δ is a front wheel steering angle of the intended path; theta.theta. f A front wheel corner being an actual direction;
θ r a rear wheel steering angle that is an actual direction; v. of x A speed in the longitudinal direction;
v y in the transverse directionSpeed;
Figure DEST_PATH_IMAGE048
the first derivative of the heading angle.
And S120, constructing a vehicle error model of the expected path based on the dynamic vehicle model.
In one embodiment, a vehicle error model in the prospective path tracking is constructed according to various parameters of the vehicle in the dynamic vehicle model, so that the transverse error of the vehicle can be corrected at any time in the path tracking, and the accuracy of the prospective path tracking is improved.
In one embodiment, the vehicle error model includes a first vehicle error equation, a second vehicle error equation, a third vehicle error equation, and a fourth vehicle error equation.
The first vehicle error equation is:
Figure DEST_PATH_IMAGE049
the second vehicle error equation is:
Figure DEST_PATH_IMAGE050
the third vehicle error equation is:
Figure DEST_PATH_IMAGE051
the fourth vehicle error equation is:
Figure DEST_PATH_IMAGE052
wherein e is φ The course angle error is obtained;
Figure 214853DEST_PATH_IMAGE048
is the first derivative of the heading angle;
Figure 333113DEST_PATH_IMAGE005
a first derivative of a heading angle that is a pre-pointing point of the intended path;
Figure 246842DEST_PATH_IMAGE003
is the first derivative of the lateral error of the vehicle from the center of mass to the intended path;
Figure DEST_PATH_IMAGE053
is the first derivative of the course angle error;
Figure DEST_PATH_IMAGE054
the second derivative of the course angle error;
Figure DEST_PATH_IMAGE055
is the second derivative of the course angle;
Figure DEST_PATH_IMAGE056
a second derivative of a heading angle at a pre-target point of the desired path;
Figure DEST_PATH_IMAGE057
a second derivative of the lateral error of the vehicle from the center of mass to the expected path;
Figure DEST_PATH_IMAGE058
the first derivative of the velocity in the transverse direction.
S200, obtaining a discretization state equation and a current cost equation based on the dynamic vehicle model and the vehicle error model.
In one embodiment, a vehicle error model is used as a basis, a continuous state equation is obtained by combining a dynamic vehicle model, discretization is carried out on the continuous state equation to obtain a discretization state equation, and a current cost equation with the vehicle transverse error and the front wheel rotation angle as variables is obtained, so that path tracking control can be carried out according to the state of the vehicle at each moment in the expected path.
In one embodiment, as shown in fig. 3, obtaining a discretized state equation and a current cost equation based on a dynamic vehicle model and a vehicle error model comprises:
s210, obtaining a fifth vehicle error equation and a sixth vehicle error equation based on the dynamic vehicle model and the vehicle error model.
In one embodiment, a fifth vehicle error equation and a sixth vehicle error equation are obtained by transforming based on the calculations of the dynamic vehicle model and the calculations of the vehicle error model to correlate the lateral error, the heading angle error, the front wheel steering angle, and the speed in the longitudinal direction of the vehicle.
The fifth vehicle error equation is:
Figure DEST_PATH_IMAGE059
wherein the content of the first and second substances,
Figure 987878DEST_PATH_IMAGE002
a second derivative of the lateral error of the vehicle from the center of mass to the expected path;
Figure 303453DEST_PATH_IMAGE003
is the first derivative of the lateral error of the vehicle from the center of mass to the expected path;
Figure 327035DEST_PATH_IMAGE053
is the first derivative of the course angle error;
Figure DEST_PATH_IMAGE060
a first derivative of a heading angle at a home point of the intended path;
C af is a vehicle front wheel cornering stiffness; c ar Is vehicle rear wheel cornering stiffness;
l f the wheelbase from the front wheel to the center of mass; l r From the front wheel to the centre of massDistance;
v x a speed in the longitudinal direction; m is the mass of the vehicle;
δ is the front wheel corner of the desired path;
the sixth vehicle error equation is:
Figure DEST_PATH_IMAGE061
wherein, the first and the second end of the pipe are connected with each other,
Figure 446169DEST_PATH_IMAGE007
is the second derivative of the course angle error;
Figure 993825DEST_PATH_IMAGE008
is the vehicle steering moment of inertia;
Figure 960644DEST_PATH_IMAGE009
the second derivative of the heading angle of the predicted point for the desired path.
And S220, transforming the fifth vehicle error equation and the sixth vehicle error equation to obtain a continuous state equation.
And transforming the curvature according to a curvature calculation formula to obtain the curvature, wherein the curvature calculation formula is as follows:
Figure DEST_PATH_IMAGE062
in one embodiment, to reduce the computational load of path tracking, the curvature c of the current trajectory R The numerical value of the curvature of the current track is obtained in advance through methods such as a map and sensing, and the curvature outside the sensing range is directly taken as 0.
Respectively setting the first coefficients sigma 1 A second coefficient sigma 2 A third coefficient σ 3 As follows
Figure 576301DEST_PATH_IMAGE011
Is provided with
Figure DEST_PATH_IMAGE063
。
And combining the fifth vehicle error equation and the sixth vehicle error equation to transform so as to obtain a continuous state equation.
The continuous state equation is:
Figure 182731DEST_PATH_IMAGE010
wherein, c R Is the curvature of the current path;
σ 1 is a first coefficient, σ 2 Is the second coefficient, σ 3 Is the third coefficient;
x is the vehicle lateral error of each preview point of the expected path;
Figure DEST_PATH_IMAGE064
the first derivative of the vehicle lateral error for each pre-line point of the intended path.
And S230, discretizing through bilinear transformation based on the continuous state equation to obtain a discretized state equation.
In one embodiment, the continuous state equation is subjected to bilinear transformation, and discretization processing is carried out according to sampling time, so that a discretization state equation is obtained, and vehicle transverse error processing of each pre-aiming point in the expected path is facilitated.
The discretization state equation is:
Figure 753652DEST_PATH_IMAGE013
wherein k is the time of the current moment;
a is a first coefficient matrix, B is a second coefficient matrix, D is a third coefficient matrix, and
Figure 574978DEST_PATH_IMAGE014
。
and S240, obtaining a current cost equation based on the discretization state equation.
In one embodiment, a current cost equation with the vehicle lateral error x of each pre-aiming point of the expected path and the front wheel rotation angle delta of the expected path as variables is obtained according to the discretization state equation and the requirements of accuracy and smoothness of path tracking.
The current cost equation is:
Figure 986367DEST_PATH_IMAGE015
wherein J is a current cost function; t is a symbol of matrix transposition;
Figure 893143DEST_PATH_IMAGE016
is a first positively determined weight matrix, R is a second positively determined weight matrix, and
Figure 31870DEST_PATH_IMAGE017
。
and S300, carrying out augmentation processing on the discretization state equation and the current cost equation by using the curvature of each preview point of the expected path to obtain a preview augmented state equation and an augmented cost equation.
In one embodiment, the curvature of each pre-aiming point of the expected path in a future timing domain is used as a disturbance quantity to be added into the optimal control law, the curvature of each pre-aiming point of the expected path is used for carrying out augmentation processing on the discrete state equation, and a new state quantity augmented vehicle transverse error X and an augmented curvature C are obtained R Thus, a preview augmentation state equation and an augmentation cost equation of the augmented Linear Quadratic Regulator (LQR) control system are constructed. This allows the Linear Quadratic Regulator (LQR) to improve the accuracy and smoothness of the Linear Quadratic Regulator (LQR) path tracking algorithm when entering a curve from a straight road and when the curvature of the path is expected to change.
Augmented vehicle lateral error for the point of preview at the current time k of the intended path
Figure DEST_PATH_IMAGE065
The setting is as follows:
Figure DEST_PATH_IMAGE066
increasing curvature C R The method is set as follows:
Figure DEST_PATH_IMAGE067
wherein the curvature C is increased R Has a dimension of N +1, including the curvature c of the current path of the vehicle R And curvatures at N preview points after the current path in the prospective path.
And setting a first augmentation coefficient matrix
Figure 534133DEST_PATH_IMAGE027
And a second matrix of augmentation coefficients
Figure 631271DEST_PATH_IMAGE024
The settings are as follows:
Figure 25343DEST_PATH_IMAGE033
wherein L and D are intermediate matrixes,
Figure 718493DEST_PATH_IMAGE034
an augmentation matrix of D, a third augmentation coefficient matrix,Ois a matrix of zero values, and is,Iis a unit matrix, \8477and \8469.
And combining the new state quantity and coefficient matrix with the original discretization state equation and the current cost equation for transformation to obtain the preview augmented state equation and the augmented cost equation.
The preview augmentation state equation is as follows:
Figure DEST_PATH_IMAGE068
wherein X (k + 1) is the augmented vehicle lateral error of the next preview point of the current time k of the prospective path;
x (k) is the augmented vehicle lateral error of the pre-aiming point at the current time k of the expected path;
t is a symbol of matrix transposition; k is the current time;
Figure 468405DEST_PATH_IMAGE027
is a first matrix of the amplification coefficients,
Figure 221597DEST_PATH_IMAGE024
is a second matrix of augmentation coefficients, an
Figure 102966DEST_PATH_IMAGE032
，
The cost of augmentation equation is:
Figure 599806DEST_PATH_IMAGE035
wherein J' is an augmentation cost function; δ is the front wheel steering angle of the intended path;
Figure 968339DEST_PATH_IMAGE019
a weight matrix that is positively determined for the first augmentation,
Figure 830116DEST_PATH_IMAGE020
a weight matrix positively determined for the second augmentation, an
Figure 686863DEST_PATH_IMAGE036
The settings are as follows:
Figure 925078DEST_PATH_IMAGE037
o is a zero matrix, I is an identity matrix, and v 8477and v 8469.
And S400, acquiring the feedforward gain and the feedback gain of the optimal control law based on the preview augmentation state equation and the augmentation cost equation.
In one embodiment, as shown in fig. 4, the off-line solving to obtain the feedforward gain and the feedback gain of the optimal control law based on the preview augmented state equation and the augmented cost equation includes:
and S410, obtaining an optimal control strategy equation by adopting a dynamic programming method based on the augmentation cost equation.
And at the current moment k, carrying out iterative calculation by adopting a dynamic programming method based on an augmented cost equation by taking the minimum front wheel corner angle and the minimum vehicle transverse error as principles until the front wheel corner angle and the vehicle transverse error meet a threshold value, thereby obtaining an optimal control strategy equation.
The optimal control strategy equation is as follows:
Figure DEST_PATH_IMAGE069
wherein, J * An optimal control strategy in the cost function is adopted;
x is the vehicle lateral error of each pre-aiming point of the expected path; δ is the front wheel corner of the desired path;
t is a symbol of matrix transposition; k is the time of the current moment;
Figure DEST_PATH_IMAGE070
a weight matrix that is positively determined for the first augmentation,
Figure DEST_PATH_IMAGE071
a weight matrix positively determined for the second augmentation, an
Figure 351380DEST_PATH_IMAGE036
The settings are as follows:
Figure 197107DEST_PATH_IMAGE037
o is a zero matrix, L is an intermediate matrix,
Figure 53068DEST_PATH_IMAGE019
is an augmented matrix of Q.
And S420, obtaining an analytical equation of the optimal control law based on the optimal control strategy equation.
And (3) leading the optimal control strategy equation to the front wheel corner delta of the expected path, and setting the derivative as 0 so as to obtain an analytic intermediate equation of the optimal control law.
The analytical intermediate equation of the optimal control law is as follows:
Figure DEST_PATH_IMAGE072
and transforming the analytic intermediate equation of the optimal control law to obtain the analytic equation of the optimal control law.
The analytical equation of the optimal control law is as follows:
Figure DEST_PATH_IMAGE073
wherein, delta * The optimal front wheel turning angle is obtained; k is an optimal control law;
b is a second coefficient matrix, and B belongs to \8477 4 ；
Figure 639514DEST_PATH_IMAGE024
An augmentation matrix of B;
Figure 717060DEST_PATH_IMAGE020
a weight matrix that is positive for the second augmentation;
Figure 186219DEST_PATH_IMAGE025
the weight matrix, which is positively determined for the third augmentation, is set as follows:
Figure DEST_PATH_IMAGE074
beta is the slip angle of the vehicle's center of mass;
Figure 889995DEST_PATH_IMAGE027
is a first matrix of augmentation coefficients, and
Figure 781596DEST_PATH_IMAGE028
。
and S430, acquiring the feedforward gain and the feedback gain of the optimal control law based on the analytic equation and the preview augmentation state equation of the optimal control law.
And substituting the analytical equation of the optimal control law into a preview augmentation state equation to obtain a calculation formula of the augmented vehicle transverse error X (k + 1) of the optimal control law.
The calculation formula of the vehicle lateral error X (k + 1) of the optimal control law is:
Figure DEST_PATH_IMAGE075
in one embodiment, the optimal control law K includes a feedback gain K b And a feedforward gain K f,i Wherein the feedback gain K b The setting is as follows:
Figure 729961DEST_PATH_IMAGE039
feedforward gain K of ith preview point f,i The method is set as follows:
Figure 432337DEST_PATH_IMAGE040
in the formula (I), the compound is shown in the specification,
Figure 942517DEST_PATH_IMAGE025
is an augmented matrix of P and is,
Figure 575492DEST_PATH_IMAGE034
and zeta is a second intermediate matrix, and zeta is an augmentation matrix of D and is set as:
Figure 581625DEST_PATH_IMAGE041
substituting the data of each parameter into the calculation formula of the optimal control law for increasing the vehicle transverse error X (K + 1) and the feedback gain K b And the feedforward gain K of the ith preview point f,i In the calculation formula (c), thereby obtaining the optimum
The feedforward gain and the feedback gain of the control law.
Because the optimal control law K and the optimal front wheel corner delta are directly combined * Direct correlation eliminates the need to obtain the optimum nose wheel steering angle delta from other intermediate variables * Therefore, a large amount of intermediate calculation amount is saved, and the calculation time of the path tracking calculation amount is reduced.
And S500, acquiring the number of the preview points based on the current vehicle speed and the feedforward gain of the optimal control law of the first preset condition.
In one embodiment, since the optimal control law K is a vector with a dimension N +5, where N is the number of preview points in a preview window of the desired path, and the first 4 dimensions of the other 5 dimensions are sequentially the lateral error e y Rate of change of lateral error
Figure 408898DEST_PATH_IMAGE003
(i.e., the first derivative of the lateral error), heading angle error e φ And rate of change of course angle error
Figure 599184DEST_PATH_IMAGE004
(i.e., the first derivative of the heading angle error), the last 1 of the 5 dimensions is the curvature of each of the waypoints of the prospective path.
Giving the current vehicle speed V of the vehicle at the current moment K, and setting the first preset condition as the feedforward gain K of the optimal control law, wherein the vehicle speed V is approximately equal to the speed vx in the longitudinal direction due to the second preset condition f The last value tends to 0, and the sampling interval time is less than or equal to 0.02 s, for the feedforward gain K f The calculation formula of (2) is operated, thereby obtaining the number of the preview points. Optionally, based on the current vehicle speed and a first preset barAnd obtaining the number of the preview points by the feedforward gain of the optimal control law of the element, and solving the number of the preview points off line.
For example, as shown in FIG. 5, the current vehicle speeds of the vehicle are set to 15m/s, 8m/s and 2.7m/s, the sampling interval time is equal to 0.02 second, and the feed forward gain K is set f When the last value is close to 0, and the number of the aiming points is 100, the feedforward gain K f Already towards 0. In the present embodiment, the current vehicle speed of the vehicle is not particularly limited, and is set according to the vehicle speed limit of various road scenes. Optionally, the current vehicle speed of the vehicle is equal to or less than 60m/s.
The number of the preview points is obtained by setting the feedforward gain of the optimal control law under the first preset condition, so that the number of the preview points of the expected path is greatly reduced, the calculation amount of the path tracking algorithm is further reduced, and the calculation time of the calculation amount of the path tracking algorithm is reduced.
S600, obtaining the vehicle transverse error of each pre-aiming point of the expected path based on the vehicle error model and the number of the pre-aiming points.
In one embodiment, according to the first vehicle error model, the second vehicle error model, the third vehicle error model and the fourth vehicle error model of the vehicle error models, parameters of the vehicle are substituted into the vehicle error models, so that the vehicle transverse error x (k) of each pre-aiming point of the expected path at the current time k is obtained.
And S700, obtaining the front wheel rotating angle corresponding to each pre-aiming point through an optimal front wheel rotating angle calculation formula of the vehicle based on the feedforward gain, the feedback gain, the vehicle transverse error of each pre-aiming point and the curvature of each pre-aiming point.
In one embodiment, the curvature c of the preview point according to the current time k R (k) Obtaining an augmented curvature C R (k) Then, the feedforward gain K obtained in the above step is used f Feedback gain K b A vehicle lateral error x (k) of each preview point at the current time k and an augmentation curvature C of each preview point R (k) Substituting the optimal front wheel steering angle calculation formula to obtain the front wheel steering angle corresponding to each preview point, thereby reducing the calculation process of obtaining the optimal front wheel steering angle through other intermediate variables and reducing the pathThe calculation time of the calculation amount of the path tracking is further reduced.
The optimal front wheel steering angle calculation formula is:
Figure 6156DEST_PATH_IMAGE038
wherein, delta * The optimal front wheel turning angle is obtained; x is the vehicle lateral error of each preview point of the expected path;
C R to increase the curvature; k is b Is the feedback gain; k is f Is the feed forward gain.
And S800, controlling the vehicle to track the path based on the front wheel steering angles corresponding to the pre-aiming points of the expected path.
The vehicle is controlled to track the path through the front wheel turning angles corresponding to the preview points of the expected path, so that the path tracking algorithm of the embodiment can quickly react to the curvature change of the preview points and can perform off-line operation.
Compared with the prior art, the embodiment has the beneficial effects that:
the method comprises the steps of obtaining a discretization state equation and a current cost equation of path tracking by constructing a dynamic vehicle model and a vehicle error model, then converting the discretization state equation into a pre-aiming augmented state equation by taking the curvature of each pre-aiming point as a state quantity, solving the augmented cost equation of the augmented path tracking to obtain an analytical equation of an optimal control law, solving according to the current vehicle speed to obtain the number of the pre-aiming points in a pre-aiming window in an expected path, and controlling the vehicle to track the path based on front wheel rotation angles corresponding to the pre-aiming points of the expected path, so that the accuracy of vehicle control is improved and the calculated quantity of optimal control of vehicle path tracking is reduced when the vehicle is driven automatically or in an unmanned mode, and the smoothness and the real-time of control can be considered in the vehicle path tracking.
In a second aspect, as shown in fig. 6, an embodiment of the present application provides an apparatus for path tracking, including:
a build vehicle error model module 100 builds a vehicle error model of the expected path based on the centroid of the vehicle.
And a current state equation obtaining module 200, configured to obtain a discretized state equation and a current cost equation based on the vehicle error model.
And an obtaining augmented state equation module 300, configured to perform augmentation processing on the discretization state equation and the current cost equation by using the curvature of each pre-aiming point of the expected path to obtain a pre-aiming augmented state equation and an augmented cost equation.
And an optimal control law obtaining module 400, configured to obtain a feedforward gain and a feedback gain of the optimal control law based on the preview augmented state equation and the augmented cost equation.
The number of preview points obtaining module 500 is configured to obtain the number of preview points based on the current vehicle speed and the feedforward gain of the optimal control law under the first preset condition.
The get vehicle error module 600 is configured to obtain a vehicle lateral error at each pre-aiming point of the expected path based on the vehicle error model and the number of pre-aiming points.
And the front wheel steering angle obtaining module 700 is used for obtaining the front wheel steering angle corresponding to each preview point through an optimal front wheel steering angle calculation formula of the vehicle based on the feedforward gain, the feedback gain, the vehicle transverse error of each preview point and the curvature of each preview point.
And a path tracking module 800, configured to control the vehicle to track a path based on the front wheel turning angles corresponding to the respective pre-aiming points of the expected path.
In a third aspect, an embodiment of the present application provides an electronic device, which includes a memory, a processor, and a computer program stored in the memory and executable on the processor, and when the processor executes the computer program, the method according to any one of the first aspect is implemented.
In a fourth aspect, the present application provides a computer-readable storage medium, where a computer program is stored, where the computer program is executed by a processor to implement the method according to any one of the contents in the first aspect.
It is understood that the beneficial effects of the second to fourth aspects can be seen from the description of the first aspect, and are not described herein again.
It should be noted that, for the information interaction, execution process, and other contents between the above-mentioned devices/units, the specific functions and technical effects thereof are based on the same concept as those of the embodiment of the method of the present application, and specific reference may be made to the part of the embodiment of the method, which is not described herein again.
It should be clear to those skilled in the art that, for convenience and simplicity of description, the foregoing division of the functional units and modules is only used for illustration, and in practical applications, the above function distribution may be performed by different functional units and modules as needed, that is, the internal structure of the apparatus may be divided into different functional units or modules to perform all or part of the above described functions. Each functional unit and module in the embodiments may be integrated in one processing unit, or each unit may exist alone physically, or two or more units are integrated in one unit, and the integrated unit may be implemented in a form of hardware, or in a form of software functional unit. In addition, specific names of the functional units and modules are only used for distinguishing one functional unit from another, and are not used for limiting the protection scope of the present application. The specific working processes of the units and modules in the system may refer to the corresponding processes in the foregoing method embodiments, and are not described herein again.
The method for path tracking provided by the embodiment of the application can be applied to terminal devices such as a mobile phone, a tablet personal computer, a wearable device, a vehicle-mounted device, an Augmented Reality (AR)/Virtual Reality (VR) device, a notebook computer, a super-mobile personal computer (UMPC), a netbook, a Personal Digital Assistant (PDA), and the like, and the embodiment of the application does not limit the specific types of the terminal devices at all.
The integrated unit, if implemented in the form of a software functional unit and sold or used as a stand-alone product, may be stored in a computer readable storage medium. Based on such understanding, all or part of the processes in the methods of the embodiments described above can be implemented by a computer program, which can be stored in a computer-readable storage medium and can implement the steps of the embodiments of the methods described above when the computer program is executed by a processor. Wherein the computer program comprises computer program code, which may be in the form of source code, object code, an executable file or some intermediate form, etc.
The computer-readable medium may include at least: any entity or device capable of carrying computer program code to a photographing apparatus/terminal device, recording medium, computer Memory, read-Only Memory (ROM), random Access Memory (RAM), electrical carrier wave signals, telecommunication signals, and software distribution medium. Such as a usb-disk, a removable hard disk, a magnetic or optical disk, etc. In certain jurisdictions, computer-readable media may not be an electrical carrier signal or a telecommunications signal in accordance with legislative and patent practice.
In the above embodiments, the description of each embodiment has its own emphasis, and reference may be made to the related description of other embodiments for parts that are not described or recited in any embodiment.
Those of ordinary skill in the art will appreciate that the various illustrative elements and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware or combinations of computer software and electronic hardware. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the technical solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present application.
In the embodiments provided in the present application, it should be understood that the disclosed apparatus/network device and method may be implemented in other ways. For example, the above-described apparatus/network device embodiments are merely illustrative, and for example, the division of the modules or units is only one logical division, and there may be other divisions when actually implementing, for example, a plurality of units or components may be combined or integrated into another system, or some features may be omitted, or not implemented. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be through some interfaces, indirect coupling or communication connection of devices or units, and may be in an electrical, mechanical or other form.
The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.
The above-mentioned embodiments are only used for illustrating the technical solutions of the present application, and not for limiting the same; although the present application has been described in detail with reference to the foregoing embodiments, it should be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; such modifications and substitutions do not substantially depart from the spirit and scope of the embodiments of the present application and are intended to be included within the scope of the present application.

Claims (11)

1. A method of path tracking, comprising:
constructing a two-degree-of-freedom dynamic vehicle model based on the vehicle mass center and a vehicle error model of an expected path;
obtaining a discretization state equation and a current cost equation based on the dynamic vehicle model and the vehicle error model;
carrying out augmentation processing on the discretization state equation and the current cost equation by using the curvature of each preview point of the expected path to obtain a preview augmentation state equation and an augmentation cost equation;
obtaining a feedforward gain and a feedback gain of an optimal control law based on the preview augmentation state equation and the augmentation cost equation;
obtaining the number of the preview points based on the current vehicle speed and the feedforward gain of the optimal control law under a first preset condition;
obtaining a vehicle lateral error of each preview point of the prospective path based on the vehicle error model and the number of preview points;
obtaining a front wheel rotating angle corresponding to each preview point through an optimal front wheel rotating angle calculation formula of the vehicle based on the feedforward gain, the feedback gain, the vehicle transverse error of each preview point and the curvature of each preview point;
and controlling the vehicle to track the path based on the front wheel turning angles corresponding to the pre-aiming points of the expected path.
2. The method of claim 1, wherein constructing a two degree of freedom vehicle model of dynamics based on vehicle center of mass and a vehicle error model of the expected path comprises:
constructing a two-degree-of-freedom dynamic vehicle model based on the vehicle mass center based on a second preset condition;
constructing a vehicle error model of the expected path based on the dynamic vehicle model, wherein the vehicle error model comprises a first vehicle error equation, a second vehicle error equation, a third vehicle error equation, and a fourth vehicle error equation.
3. The method of claim 1, wherein obtaining a discretized state equation and a current cost equation based on the dynamic vehicle model and the vehicle error model comprises:
obtaining a fifth vehicle error equation and a sixth vehicle error equation based on the dynamical vehicle model and the vehicle error model;
converting the fifth vehicle error equation and the sixth vehicle error equation to obtain a continuous state equation;
discretizing through bilinear transformation based on the continuous state equation to obtain a discretized state equation;
obtaining the current cost equation based on the discretized state equation.
4. The method of claim 3, wherein the fifth vehicle error equation is:
Figure DEST_PATH_IMAGE001
wherein the content of the first and second substances,
Figure DEST_PATH_IMAGE002
a second derivative of the lateral error of the vehicle from the center of mass to the expected path;
Figure DEST_PATH_IMAGE003
is the first derivative of the lateral error of the vehicle from the center of mass to the intended path;
Figure DEST_PATH_IMAGE004
is the first derivative of the course angle error;
Figure DEST_PATH_IMAGE005
a first derivative of a heading angle that is a pre-pointing point of the intended path;
C af is vehicle front wheel cornering stiffness;
C ar is vehicle rear wheel cornering stiffness;
l f the wheelbase from the front wheel to the center of mass;
l r the wheelbase from the front wheel to the center of mass;
v x a speed in the longitudinal direction;
m is the mass of the vehicle;
δ is the front wheel steering angle of the intended path;
the sixth vehicle error equation is:
Figure DEST_PATH_IMAGE006
wherein the content of the first and second substances,
Figure DEST_PATH_IMAGE007
is the second derivative of the course angle error;
Figure DEST_PATH_IMAGE008
is the vehicle steering moment of inertia;
Figure DEST_PATH_IMAGE009
a second derivative of a heading angle for a predicted path preview point;
the continuous state equation is:
Figure DEST_PATH_IMAGE010
wherein, c R Is the curvature of the current path;
σ 1 is the first coefficient, σ 2 Is the second coefficient, σ 3 For the third coefficient, the following is set:
Figure DEST_PATH_IMAGE011
x is the vehicle lateral error of each preview point of the prospective path;
Figure DEST_PATH_IMAGE012
a first derivative of a vehicle lateral error for each pre-sight point of the prospective path;
the discretized state equation is:
Figure DEST_PATH_IMAGE013
wherein k is the time of the current moment;
a is a first coefficient matrix, B is a second coefficient matrix, D is a third coefficient matrix, and
Figure DEST_PATH_IMAGE014
；
the current cost equation is:
Figure DEST_PATH_IMAGE015
wherein J is a current cost function;
t is a symbol of matrix transposition;
q is a first positively determined weight matrix, R is a second positively determined weight matrix, and
Figure DEST_PATH_IMAGE016
。
5. the method of claim 1, wherein the obtaining of the feedforward gain and the feedback gain of the optimal control law based on the preview augmented state equation and the augmented cost equation by off-line solving comprises:
obtaining an optimal control strategy equation by adopting a dynamic programming method based on the augmentation cost equation;
obtaining an analytical equation of an optimal control law based on the optimal control strategy equation;
and obtaining the feedforward gain and the feedback gain of the optimal control law based on the analytic equation and the preview augmentation state equation of the optimal control law.
6. The method of claim 5,
the optimal control strategy equation is as follows:
Figure DEST_PATH_IMAGE017
wherein, J * An optimal control strategy in the cost function is adopted;
x is the vehicle lateral error of each preview point of the prospective path;
x is the augmented vehicle lateral error at each pre-aiming point of the expected path;
δ is the front wheel steering angle of the intended path;
t is a symbol of matrix transposition;
k is the time of the current moment;
Figure DEST_PATH_IMAGE018
a weight matrix that is positively determined for the first augmentation,
Figure DEST_PATH_IMAGE019
a weight matrix positively determined for the second augmentation, an
Figure DEST_PATH_IMAGE020
The settings are as follows:
Figure DEST_PATH_IMAGE021
the analytical equation for the optimal control law is:
Figure DEST_PATH_IMAGE022
wherein, delta * Is the optimal front wheel corner;
b is a matrix of second coefficients, and
Figure DEST_PATH_IMAGE023
；
Figure DEST_PATH_IMAGE024
the augmentation matrix is B and is a second augmentation coefficient matrix;
Figure DEST_PATH_IMAGE025
for the third augmented positive definite weight matrix, the following is set:
Figure DEST_PATH_IMAGE026
beta is the slip angle of the vehicle centroid;
Figure DEST_PATH_IMAGE027
is a first matrix of augmentation coefficients, and
Figure DEST_PATH_IMAGE028
。
7. the method of claim 1,
the preview augmentation state equation is as follows:
Figure DEST_PATH_IMAGE029
wherein X (k + 1) is the augmented vehicle lateral error of the next pre-aiming point at the current time k of the prospective path;
x (k) is the augmented vehicle lateral error for the home point at the current time k of the prospective path, set to:
Figure DEST_PATH_IMAGE030
C R to increaseWide curvature set as:
Figure DEST_PATH_IMAGE031
t is a symbol of matrix transposition;
c R is the curvature of the current path;
k is the current time;
Figure 350887DEST_PATH_IMAGE027
is a first matrix of the amplification coefficients,
Figure 487602DEST_PATH_IMAGE024
is a second matrix of augmentation coefficients, and
Figure DEST_PATH_IMAGE032
the settings are as follows:
Figure DEST_PATH_IMAGE033
l and D are intermediate matrixes,
Figure DEST_PATH_IMAGE034
the augmentation matrix is D and is a third augmentation coefficient matrix;
the augmentation cost equation is:
Figure DEST_PATH_IMAGE035
wherein J' is an augmentation cost function;
δ is the front wheel steering angle of the intended path;
Figure 860814DEST_PATH_IMAGE018
a weight matrix that is positively determined for the first augmentation,
Figure 724865DEST_PATH_IMAGE019
a weight matrix positively determined for the second augmentation, an
Figure DEST_PATH_IMAGE036
The settings are as follows:
Figure DEST_PATH_IMAGE037
o is a zero matrix, I is a unity matrix, and v 8477and v 8469;
the optimal front wheel steering angle calculation formula is as follows:
Figure DEST_PATH_IMAGE038
wherein, K b For feedback gain, set to:
Figure DEST_PATH_IMAGE039
Figure 83778DEST_PATH_IMAGE019
is an augmented matrix of R, P is a first intermediate matrix,
Figure 375082DEST_PATH_IMAGE027
an augmentation matrix of A;
K f,i the feed forward gain for the ith preview point is set as:
Figure DEST_PATH_IMAGE040
ζ is a second intermediate matrix, set to:
Figure DEST_PATH_IMAGE041
δ * the optimal front wheel turning angle is obtained.
8. The method of claim 1,
the first preset condition is that the last value in the feedforward gain of the optimal control law tends to be 0, and the sampling interval time is less than or equal to 0.02 second.
9. An apparatus for path tracking, comprising:
the vehicle model building module is used for building a two-degree-of-freedom dynamic vehicle model based on the vehicle center of mass and a vehicle error model of an expected path;
the current state equation obtaining module is used for obtaining a discretization state equation and a current cost equation based on the dynamic vehicle model and the vehicle error model;
the acquisition and amplification state equation module is used for carrying out amplification processing on the discretization state equation and the current cost equation by using the curvature of each pre-aiming point of the expected path to obtain a pre-aiming and amplification state equation and an amplification cost equation;
an optimal control law module is obtained and used for obtaining feedforward gain and feedback gain of an optimal control law based on the preview augmentation state equation and the augmentation cost equation;
the pre-aiming point number obtaining module is used for obtaining the number of the pre-aiming points based on the current vehicle speed and the feedforward gain of the optimal control law under the first preset condition;
the vehicle error obtaining module is used for obtaining the vehicle transverse error of each pre-aiming point of the expected path based on the vehicle error model and the number of the pre-aiming points;
the front wheel rotation angle obtaining module is used for obtaining a front wheel rotation angle corresponding to each pre-aiming point through an optimal front wheel rotation angle calculation formula of the vehicle based on the feedforward gain, the feedback gain, the vehicle transverse error of each pre-aiming point and the curvature of each pre-aiming point;
and the path tracking module is used for controlling the vehicle to track the path based on the front wheel turning angles corresponding to the pre-aiming points of the expected path.
10. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the method of any of claims 1 to 8 when executing the computer program.
11. A computer-readable storage medium, in which a computer program is stored which, when being executed by a processor, carries out the method according to any one of claims 1 to 8.
CN202211108443.3A 2022-09-13 2022-09-13 Path tracking method and device, electronic equipment and storage medium Pending CN115179935A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211108443.3A CN115179935A (en) 2022-09-13 2022-09-13 Path tracking method and device, electronic equipment and storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211108443.3A CN115179935A (en) 2022-09-13 2022-09-13 Path tracking method and device, electronic equipment and storage medium

Publications (1)

Publication Number Publication Date
CN115179935A true CN115179935A (en) 2022-10-14

Family

ID=83524259

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211108443.3A Pending CN115179935A (en) 2022-09-13 2022-09-13 Path tracking method and device, electronic equipment and storage medium

Country Status (1)

Country Link
CN (1) CN115179935A (en)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107153420A (en) * 2017-05-25 2017-09-12 广州汽车集团股份有限公司 Path tracking control method, device and intelligent automobile
CN110001637A (en) * 2019-04-10 2019-07-12 吉林大学 A kind of pilotless automobile path following control device and control method based on multiple spot tracking
CN111399380A (en) * 2020-03-24 2020-07-10 湖南大学 Multi-point preview L QR transverse control method based on Fiala brush tire model
CN111610780A (en) * 2019-02-25 2020-09-01 广州汽车集团股份有限公司 Automatic driving vehicle path tracking control method and device
WO2021238747A1 (en) * 2020-05-26 2021-12-02 三一专用汽车有限责任公司 Method and apparatus for controlling lateral motion of self-driving vehicle, and self-driving vehicle
WO2022160196A1 (en) * 2021-01-28 2022-08-04 浙江吉利控股集团有限公司 Vehicle driving control method and apparatus, and vehicle and storage medium

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107153420A (en) * 2017-05-25 2017-09-12 广州汽车集团股份有限公司 Path tracking control method, device and intelligent automobile
CN111610780A (en) * 2019-02-25 2020-09-01 广州汽车集团股份有限公司 Automatic driving vehicle path tracking control method and device
CN110001637A (en) * 2019-04-10 2019-07-12 吉林大学 A kind of pilotless automobile path following control device and control method based on multiple spot tracking
CN111399380A (en) * 2020-03-24 2020-07-10 湖南大学 Multi-point preview L QR transverse control method based on Fiala brush tire model
WO2021238747A1 (en) * 2020-05-26 2021-12-02 三一专用汽车有限责任公司 Method and apparatus for controlling lateral motion of self-driving vehicle, and self-driving vehicle
WO2022160196A1 (en) * 2021-01-28 2022-08-04 浙江吉利控股集团有限公司 Vehicle driving control method and apparatus, and vehicle and storage medium

Similar Documents

Publication Publication Date Title
Tang et al. An improved kinematic model predictive control for high-speed path tracking of autonomous vehicles
CN107415939B (en) Steering stability control method for distributed driving electric automobile
Kosecka et al. Vision-based lateral control of vehicles
CN109017984B (en) Track following control method and system for unmanned vehicle and related device
CN111873991B (en) Vehicle steering control method, device, terminal and storage medium
CN112346465B (en) IALOS guide law-based adaptive fuzzy control method for under-actuated unmanned ship
Xu et al. Model predictive control for lane keeping system in autonomous vehicle
CN112622903A (en) Longitudinal and transverse control method for autonomous vehicle in vehicle following driving environment
CN111123701B (en) Automatic driving path tracking anti-interference control method based on pipeline prediction model
CN113110511B (en) Intelligent ship course control method based on generalized fuzzy hyperbolic model
Kebbati et al. Lateral control for autonomous wheeled vehicles: A technical review
CN111717282A (en) Man-machine shared driving auxiliary steering method based on model predictive control
Lee et al. Multirate active steering control for autonomous vehicle lateral maneuvering
CN115534925A (en) Vehicle control method, device, equipment and computer readable medium
CN113009829A (en) Longitudinal and transverse coupling control method for intelligent internet motorcade
CN114148320B (en) Path tracking control method and equipment
CN116184835A (en) Unmanned vehicle tracking control method considering input time delay and speed time variation
CN115248553A (en) Event triggering adaptive PID track tracking fault-tolerant control method for under-actuated ship
CN113467231A (en) Unmanned ship path tracking method based on sideslip compensation ILOS guidance law
CN113525366A (en) Transverse control method for hydraulic transverse controller of steel-wheel road roller
CN110723207B (en) Intelligent automobile model prediction steering controller based on model reconstruction and control method thereof
CN115179935A (en) Path tracking method and device, electronic equipment and storage medium
CN115447615A (en) Trajectory optimization method based on vehicle kinematics model predictive control
Raffone et al. Optimal look-ahead vehicle lane centering control design and application for mid-high speed and curved roads
CN115092176A (en) Automobile, pre-aiming control method and device thereof and storage medium

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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20221014