CN110989625B - Vehicle path tracking control method - Google Patents

Vehicle path tracking control method Download PDF

Info

Publication number
CN110989625B
CN110989625B CN201911360652.5A CN201911360652A CN110989625B CN 110989625 B CN110989625 B CN 110989625B CN 201911360652 A CN201911360652 A CN 201911360652A CN 110989625 B CN110989625 B CN 110989625B
Authority
CN
China
Prior art keywords
vehicle
path
time
point
points
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.)
Active
Application number
CN201911360652.5A
Other languages
Chinese (zh)
Other versions
CN110989625A (en
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.)
Hunan University
Original Assignee
Hunan University
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 Hunan University filed Critical Hunan University
Priority to CN201911360652.5A priority Critical patent/CN110989625B/en
Publication of CN110989625A publication Critical patent/CN110989625A/en
Application granted granted Critical
Publication of CN110989625B publication Critical patent/CN110989625B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Steering Control In Accordance With Driving Conditions (AREA)

Abstract

The invention discloses a vehicle path tracking control method, which comprises the following steps: s1, obtaining a new reference path with denser path points according to the existing reference path points; s2, obtaining vehicle state information; s3, finding out the nearest path point on the new reference path; s4, searching N preview points in front of the vehicle on the new reference path by taking the nearest path point as a starting point; s5, constructing a prediction model, an objective function and system constraints, predicting future dynamics of the vehicle according to current measurement information and the prediction model, solving an optimization problem meeting the objective function and constraint conditions on line, and acquiring an optimal control sequence formed by expected front wheel steering angles corresponding to N pre-aiming points; and S6, controlling the vehicle until the next sampling time arrives according to the optimal control sequence, and repeating the steps S2 to S5 when the next observation time arrives. The method provided by the invention has higher tracking precision, can ensure the comfort in the control process and cannot generate sudden change of the control quantity.

Description

Vehicle path tracking control method
Technical Field
The invention relates to the technical field of vehicle control, in particular to a vehicle path tracking control method.
Background
The unmanned vehicle path tracking control means that the steering angle of the vehicle is controlled by the underlying controller of the unmanned vehicle so that the vehicle always travels along a desired path. The path tracking ability of the unmanned vehicle is therefore an important guarantee for the safe driving of the unmanned vehicle. The aim of the path tracking control is to reduce the deviation of the vehicle from a reference path during driving and simultaneously ensure the smooth steering of the vehicle.
The current main unmanned vehicle path tracking control method mainly comprises methods such as fuzzy control, pre-aiming control, model prediction control and the like.
Such as: the patent application with publication number CN107942663A proposes a fuzzy PID control method, which utilizes a fuzzy control algorithm to establish a fuzzy control rule table, and simultaneously utilizes a PID control method to obtain a specific coefficient, a differential coefficient and an integral coefficient, and calculates to obtain a control value of a steering controller. The control method provided by the patent document combines fuzzy control and PID control, but the method does not use an accurate physical model and does not add vehicle constraint, and good tracking effect is difficult to realize under complex conditions.
For another example: the patent application with publication number CN109318905A proposes two control methods, wherein one of them uses the kinematic constraint of the vehicle to establish the lateral control preview kinematic model of the vehicle, selects a path point with a certain preview distance from the vehicle as the preview point in front of the vehicle, and then combines with the PID control method to perform lateral control on the vehicle. However, the method can only select one preview point, and only the path deviation error near the taken single preview point is small, so that the tracking error of the whole path cannot be considered.
Also for example: patent application with patent publication number CN108973769A proposes a Control method based on Model Predictive Control (MPC), which constructs a dynamic Model for vehicle path tracking, and obtains an optimal Control sequence by solving a Model Predictive controller. However, this method only uses the deviation between the current vehicle and the route as an input condition, and does not fully use the position and direction information of the whole route.
In summary, the methods provided by the prior art cannot solve the problem of path tracking control of a vehicle with a large curvature path, and therefore, a method capable of fully utilizing path information is needed to improve the path tracking accuracy.
Disclosure of Invention
It is an object of the present invention to provide a vehicle path following control method that overcomes or at least mitigates at least one of the above-mentioned disadvantages of the prior art.
To achieve the above object, the present invention provides a vehicle path tracking control method, including:
s1, interpolating the existing reference path points to obtain a new reference path with denser path points;
s2, obtaining vehicle state information;
s3, traversing all path points p on the new reference path obtained in S1i(xi，yi) Find the nearest path point p0；
S4, finding the nearest path point p with S3 on the new reference path obtained in S10As a starting point, N aiming points are searched for in front of the running vehicle according to the vehicle motion speed v (t) acquired in S2；
S5, constructing a prediction model and an objective function shown in the following formula (5), predicting future dynamics of the vehicle according to the current vehicle state information and the prediction model obtained in S2, solving an optimization problem meeting the objective function and constraint conditions on line, and obtaining an expected front wheel steering angle corresponding to each pre-aiming point in N pre-aiming points obtained by searching in S4, wherein the expected front wheel steering angle corresponding to each pre-aiming point in the N pre-aiming points forms an optimal control sequence represented by the formula (20);
Figure GDA0002660294070000021
in formula (5):
q is a weight coefficient;
Δ (k + t | t) is a difference between the vehicle front wheel steering angle at the time k +1+ t predicted at the time t and the vehicle front wheel steering angle at the time k + t predicted immediately before;
e (k + t | t) is the distance between the center of the rear axle of the vehicle at the time k + t predicted at the time t and the tangent of the kth preview point searched at the time t, and is expressed by equation (6):
Figure GDA0002660294070000022
in formula (6):
x (k + t | t) is an abscissa of the center point of the rear axle of the vehicle at the time k + t predicted at the time t;
y (k + t | t) is the ordinate of the vehicle rear axle center point at the time k + t predicted at the time t;
xp(k) the abscissa of the kth pre-aiming point searched in the S4 is obtained;
yp(k) the ordinate of the kth pre-aiming point searched in the S4 is obtained;
θp(k) the heading angle of the k-th pre-aiming point searched in the S4 is obtained;
Figure GDA0002660294070000031
in the formula (20), the reaction mixture is,
Figure GDA0002660294070000032
represents the front wheel steering angle at time k + t of time t;
s6, use
Figure GDA0002660294070000033
And controlling the vehicle until the next sampling time arrives and when the next observation time t +1 arrives, repeating the steps from S2 to S5, refreshing the problem by using the new vehicle state provided by S2, and circulating the steps until the path end point is reached.
Further, the "search for N preview points toward the front of the vehicle in S4" specifically includes the following method:
with the nearest path point p0As a starting point, and at the same time, with v (T) Δ T as a search distance, searching N path points as pre-aiming points (x) in the front of the vehicle running along the new reference pathp(k)，yp(k) K ═ 1, 2, …, N, Δ T are discrete time steps, and the search is stopped when it reaches the end of the new reference path.
Further, the "search for N preview points toward the front of the vehicle in S4" specifically includes the following method:
with the nearest path point p0As a starting point, and at the same time, with v (T) Δ T as a search distance, searching N path points as pre-aiming points (x) in the front of the vehicle running along the new reference pathp(k)，yp(k) K ═ 1, 2, …, N, Δ T are discrete time steps, and the search is stopped when it reaches a set maximum number of predicted steps N.
Further, S5 solves the optimization problem as shown in the following equation (13) in each control cycle:
Figure GDA0002660294070000034
in formula (13):
minJ (k) is the minimum of the objective function provided by equation (5);
x (k +1+ t | t) is an abscissa of the center point of the rear axle of the vehicle at the time k +1+ t predicted at the time t;
y (k +1+ t | t) is the ordinate of the vehicle rear axle center point at the time k +1+ t predicted at the time t;
Figure GDA0002660294070000035
the predicted vehicle body heading angle at the moment k + t at the moment t;
Figure GDA0002660294070000036
the predicted vehicle body heading angle at the moment k +1+ t at the moment t;
v (t) the vehicle speed obtained for S2;
Δ t is the time for each prediction step;
(k + t | t) is the predicted front wheel steering angle at time k + t at time t;
(k +1+ t | t) is the predicted front wheel steering angle at time k + t +1 at time t;
l is the wheelbase of the vehicle;
k is the kth prediction step;
Δmaxthe maximum variation of the steering angle of the front wheels of the vehicle in the adjacent time;
maxthe maximum steering angle of the front wheels of the vehicle.
Further, before the optimization problem shown in equation (13) is solved, a specific calculation method of the initial value of the steering angle of the front wheel in the prediction time domain includes:
s51, using the vehicle state information obtained in the step S2, taking the same step size Delta T as that in the step S4, and obtaining the nearest path point p in the steps S3 and S40And the first N-1 preview points are searched as the nearest path points (x) of the corresponding time of the Stanley methodp(k)，yp(k))，k＝1，2，…，N；
S52, calculating the center (x) of the rear axle of the vehicle at the current time tr(t)，yr(t)) and corresponding nearest path point p0(xp(0)，yp(0) A lateral deviation e offa(t)：
Figure GDA0002660294070000041
In the formula (14), θp(0) Is a path point p0The course angle of (d);
s53, calculating the angle error theta of the vehicle body course angle and the course angle of the nearest path point at the current time te(t)：
Figure GDA0002660294070000042
In the formula (15), the reaction mixture is,
Figure GDA0002660294070000043
is the body heading angle, θp(0) Is a path point p0The course angle of (d);
s54, calculating the expected steering angle' (t) of the front wheels at the current time t:
Figure GDA0002660294070000044
in formula (16):
k is a weight coefficient;
θe(t) an angle error portion for the desired steering angle' (t);
s55, using the equation (16) to obtain the (t) and the known vehicle rear axle center coordinate (x)r(t)，yr(t)), velocity v, body heading angle
Figure GDA0002660294070000051
The front wheel steering angle (T) information and the vehicle kinematics equations (17) to (19) obtain vehicle state information at the next time T + Δ T:
Figure GDA0002660294070000052
Figure GDA0002660294070000053
Figure GDA0002660294070000054
s56, updating the vehicle state by using the vehicle state information obtained by the formulas (17) to (19), and selecting a first preview point (x) by using the T + delta T moment as the current momentp(1)，yp(1) Repeat steps S52 to S55 as the nearest path point, and extrapolate the expected steering angle' (T + k × Δ T) corresponding to each pre-pointing point after that, and use it as the iteration initial value of (k + T | T) in the optimization problem shown in equation (13).
Further, the "closest path point p" in S30"comprises the following steps:
calculation of Path Point p Using equation (1)i(xi，yi) With the vehicle rear axle center (x) at the current time tr(t)，yr(t)) distance squared DiAnd find out the distance squared DiThe smallest path point, which is denoted as the nearest path point p0：
Di＝(xr(t)-xi)2+(yr(t)-yi)2 (1)。
Further, S1 adopts a cubic spline interpolation method, which specifically includes the following steps:
s11, selecting a path point on the reference path, and calculating the rotation direction angle of the coordinate system according to the yaw angle information of the path point adjacent to the selected path point in the geodetic coordinate system;
s12, rotating the geodetic coordinate system, wherein the rotation direction angle is the rotation direction angle of the coordinate system calculated in the step S11, and calculating new coordinates and a course angle of two path points in the rotated S11;
s13, calculating a fitting cubic spline interpolation curve expression between the two path points according to the new coordinates and the course angle of the two path points in the S11 calculated in the S12;
and S14, performing equidistant dispersion on the abscissa of the cubic spline interpolation curve expression obtained in the S13 to obtain high-density interpolation path points, and further obtaining a new reference path. Wherein, the first derivative of each interpolation path point is the course angle information of the point;
s15, rotating the coordinate system of the interpolation path point of S14 to restore the interpolation path point to the geodetic coordinate system, and obtaining the coordinates and course angle information of the original adjacent path points and the interpolation path points thereof under the geodetic coordinate system, wherein: the rotation direction is opposite to the rotation direction of S12, and the rotation angle is the coordinate system rotation direction angle calculated in S11.
Due to the adoption of the technical scheme, the invention has the following advantages: 1. when the path point is selected as the preview point, a resolvable path is not needed, and the preview point can be well found even if the path point is a discrete path point or a non-resolvable path, so that the method is more consistent with the actual working condition in engineering application. 2. Meanwhile, compared with the traditional control method based on preview, only one preview point can be selected, the method adopts a multi-point preview method, and the plurality of preview points can predict longer steps and distances relative to a single preview point and can take the tracking error of the whole path into consideration, so that the tracking precision is improved, the comfort in the control process is ensured, and the sudden change of the control quantity is avoided. 3. By combining the pre-aiming control and the model prediction control, uncertainty caused by model mismatch, time variation, interference and the like can be considered, timely compensation can be carried out, the latest optimization is always established on the actual basis, and the actual optimum is kept.
Drawings
FIG. 1 is a schematic flow chart of a vehicle path tracking control method according to an embodiment of the present invention;
FIG. 2 is a schematic flow chart of a cubic spline interpolation method provided in the step of reference waypoint interpolation in FIG. 1;
FIG. 3 is a schematic diagram of path tracking provided by an embodiment of the present invention;
FIG. 4 is a schematic diagram of the step of calculating nearest waypoints in FIG. 1 providing a schematic diagram of the nearest waypoint acquisition;
fig. 5 is a schematic structural diagram of a path tracking controller provided in the step of calculating the nearest path point in fig. 1.
Detailed Description
The invention is described in detail below with reference to the figures and examples.
As shown in fig. 1, a vehicle path tracking control method provided by an embodiment of the present invention includes the following steps:
and S1, interpolating the existing reference path points to obtain a new reference path with denser path points. The "reference path" can be obtained by recording the path with a GPS system, for example. However, the obtained reference path may be discrete sparse path points, and in order to obtain denser path points to ensure the tracking accuracy, the embodiment uses a cubic spline interpolation method to interpolate all path points between the starting path point and the target path point on the reference path to obtain the coordinates (x, y) and the heading angle θ of each path point on the reference path from the starting point to the target point in the geodetic coordinate systemp(as shown in fig. 2). The cubic spline interpolation method specifically comprises the following steps:
s11, one route point is selected on the reference route, and the coordinate system rotation direction angle is calculated from the yaw angle information of the route point adjacent to the selected route point in the geodetic coordinate system.
And S12, rotating the geodetic coordinate system, wherein the rotating direction angle is the rotating direction angle of the coordinate system calculated in the step S11, and calculating new coordinates and a heading angle of the two path points in the rotated S11.
S13, calculating a fitting cubic spline interpolation curve expression f (x) -a (x-x ') between the two path points according to the new coordinates and the heading angle of the two path points in S11 calculated in S12'i-1)3+b(x-x′i-1)2+c(x-x′i-1) + d, wherein i-1 and i respectively represent the serial numbers, x ', of the two route points in S11'i-1The abscissa of the i-1 st point in the rotated coordinate system is shown, and a, b, c and d are S1 in the rotated coordinate systemThe coefficient of the cubic spline interpolation curve expression between the two path points in 1 is the parameter to be calculated in S13.
And S14, performing equidistant dispersion on the abscissa of the cubic spline interpolation curve expression obtained in the S13 to obtain high-density interpolation path points, and further obtaining a new reference path. And the first derivative of each interpolation path point is the heading angle information of the point.
S15, rotating the coordinate system of the interpolation path point of S14 to restore the interpolation path point to the geodetic coordinate system, and obtaining the coordinates and course angle information of the original adjacent path points and the interpolation path points thereof under the geodetic coordinate system, wherein: the rotation direction is opposite to the rotation direction of S12, and the rotation angle is the coordinate system rotation direction angle calculated in S11.
And S2, obtaining the vehicle state information. FIG. 3 illustrates the position and reference path of a vehicle, and as shown in FIG. 3, the coordinates (x) of the center of the rear axle of the vehicle at the current observation time t are obtained by a GPS sensor, an Inertial Measurement Unit (IMU), and other sensors mounted on the vehicler(t)，yr(t)) and velocity v (t), body heading angle
Figure GDA0002660294070000071
And a front wheel steering angle (t).
And S3, calculating the nearest path point on the new reference path obtained in S1 and the center of the rear axle of the vehicle.
As shown in fig. 4, all path points p on the new reference path are traversedi(xi，yi) Calculating the path point p by using the equation (1)i(xi，yi) With the vehicle rear axle center (x) at the current time tr(t)，yr(t)) distance squared DiAnd find out the distance squared DiThe smallest path point, which is denoted as the nearest path point p0：
Di＝(xr(t)-xi)2+(yr(t)-yi)2 (1)。
S4, finding the nearest path point p with S3 on the new reference path obtained in S10As a starting point, the method comprises the following steps of,and searching N preview points in front of the running vehicle according to the vehicle motion speed v (t) acquired in the step S2. The "forward traveling toward the vehicle" herein refers to the front of the vehicle head.
Wherein: the "search for N preview points toward the front of the vehicle traveling" in S4 specifically includes the following method:
with the nearest path point p0As a starting point, and at the same time, using v (T) Δ T as a search distance, searching N path points in front of the vehicle as pre-aiming points (x)p(k)，yp(k) K ═ 1, 2, …, N. That is, the first preview point (x)p(1)，yp(1) And nearest path point p)0Distance along new reference path v (T) Δ T, second preview point (x)p(2)，yp(2) With a first pre-aiming point (x)p(1)，yp(1) Distance along new reference path v (T) Δ T, and so on, the ith preview point (x)p(j)，yp(j) With the i-1 th preview point (x)p(j-1)，yp(j-1)) a distance v (T) Δ T along the new reference path, the Nth home point (x)p(N)，yp(N)) and the N-1 th preview point (x)p(N-1)，yp(N-1)) is a distance v (T) Δ T along the new reference path, Δ T being a discrete time step.
In the above embodiment, when the search reaches the end of the new reference path, the search is stopped. Of course, the search operation may also be controlled according to a set predicted step number, such as: and stopping searching when the searching reaches the set maximum prediction step number N.
S5, constructing a model prediction controller, wherein the basic schematic diagram of the model prediction controller is shown in FIG. 5, the model prediction controller comprises a prediction model, an objective function and system constraints, predicting the future dynamics of the vehicle according to the current vehicle state information and the prediction model, solving the optimization problem meeting the objective function and the constraint conditions on line, and acquiring each pre-aiming point (x) in N pre-aiming points searched by S4p(k)，yp(k) Optimal control sequence for the corresponding front wheel steering angle.
The prediction model is set to equations (2) to (4) according to a vehicle kinematic formula:
Figure GDA0002660294070000081
Figure GDA0002660294070000082
Figure GDA0002660294070000083
in formulae (2) to (4):
x (k + t | t) is an abscissa of the center point of the rear axle of the vehicle at the time k + t predicted at the time t;
x (k +1+ t | t) is an abscissa of the center point of the rear axle of the vehicle at the time k +1+ t predicted at the time t;
y (k + t | t) is the ordinate of the vehicle rear axle center point at the time k + t predicted at the time t;
y (k +1+ t | t) is the ordinate of the vehicle rear axle center point at the time k +1+ t predicted at the time t;
Figure GDA0002660294070000084
the predicted vehicle body heading angle at the moment k + t at the moment t;
Figure GDA0002660294070000085
the predicted vehicle body heading angle at the moment k +1+ t at the moment t;
v (t) the vehicle speed obtained at S2;
Δ t is the time for each prediction step;
(k + t | t) is the predicted vehicle front wheel steering angle at time k + t at time t;
l is the wheelbase of the vehicle;
k is the kth prediction step.
According to the requirements of vehicle tracking precision and stability, the objective function is set as formula (5):
Figure GDA0002660294070000091
in formula (5):
q is a weight coefficient, which needs to be obtained according to actual experiments and simulations, and in this embodiment, Q is 150.
e (k + t | t) is the distance between the center of the rear axle of the vehicle at the time k + t predicted at the time t and the tangent of the kth preview point searched at the time t, i.e. the distance e shown in FIG. 3faThe distance represents the lateral deviation of the vehicle from the reference path, and e (k + t | t) should be minimized as much as possible in the optimization process to ensure the accuracy of vehicle tracking. e (k + t | t) is calculated by the formula (6):
Figure GDA0002660294070000092
in formula (6):
x (k + t | t) is an abscissa of the center point of the rear axle of the vehicle at the time k + t predicted at the time t;
y (k + t | t) is the ordinate of the vehicle rear axle center point at the time k + t predicted at the time t;
xp(k) the abscissa of the kth pre-aiming point searched in the S4 is obtained;
yp(k) the ordinate of the kth pre-aiming point searched in the S4 is obtained;
θp(k) the heading angle of the k-th pre-aiming point searched in the S4.
Δ (k + t | t) is the difference between the vehicle front wheel steering angle at the time k +1+ t predicted at the time t and the vehicle front wheel steering angle at the time k + t predicted immediately before, and this target is introduced in order to limit the variation range of the front wheel steering angle while ensuring the tracking accuracy, thereby preventing the steering wheel from greatly shaking and ensuring the tracking stability. Δ (k + t | t) is calculated by the formula (7):
Δ(k+t|t)＝(k+1+t|t)-(k+t|t) (7)
in formula (7):
(k + t | t) is the predicted front wheel steering angle at time k + t at time t;
(k +1+ t | t) is the predicted front wheel steering angle at time k + t +1 at time t.
Since the vehicle is used as a mechanical system, there are some structural constraints that the steering wheel steering angle cannot exceed the maximum allowable steering angle, and the vehicle is used as a vehicle, the riding comfort of passengers needs to be ensured, and the steering wheel steering angle change rate is not too large, in view of this, the feasible state and control variable space constraints of equation (6) are set to equations (8) to (12):
Figure GDA0002660294070000101
Figure GDA0002660294070000102
Figure GDA0002660294070000103
-Δmax≤(k+1+t|t)-(k+t|t)≤Δmax (11)
-max≤(k+t|t)≤max (12)
in formulae (8) to (12), ΔmaxThe maximum amount of change in the steering angle of the front wheels at the adjacent time,maxthe maximum steering angle of the front wheels of the vehicle and the mechanical structure.
Solving the optimization problem in each control cycle:
based on the objective function and constraint conditions established in the previous steps, the constrained optimization problem that the model predictive controller needs to solve in each cycle is as follows:
Figure GDA0002660294070000104
in equation (13), minJ (k) is the minimum of the objective function provided by equation (5).
Before solving the optimization problem, firstly using a Stanley method to calculate an initial value of a front wheel steering angle in a prediction time domain through vehicle kinematics model simulation, wherein the specific calculation method is as follows:
s51, using the vehicle state information obtained in the step S2, taking the same step size Delta T as that in the step S4, and obtaining the nearest path point p in the steps S3 and S40And the first N-1 preview points are searched as the nearest path points (x) of the corresponding time of the Stanley methodp(k)，yp(k))，k＝1，2，…，N。
S52, calculating the center (x) of the rear axle of the vehicle at the current time tr(t)，yr(t)) and corresponding nearest path point p0(xp(0)，yp(0) A lateral deviation e offa(t)：
Figure GDA0002660294070000105
In the formula (14), θp(0) Is a path point p0The course angle of (c).
S53, calculating the angle error theta of the vehicle body course angle and the course angle of the nearest path point at the current time te(t)：
Figure GDA0002660294070000111
In the formula (15), the reaction mixture is,
Figure GDA0002660294070000112
is the body heading angle, θp(0) Is a path point p0The course angle of (c).
S54, calculating the expected steering angle' (t) of the front wheels at the current time t:
Figure GDA0002660294070000113
in formula (16):
k is a weight coefficient, which needs to be adjusted according to different situations, and the specific method is to obtain a better weight coefficient by testing and simulation, where K is 1 in this embodiment;
θe(t) is the angular error portion of the desired steering angle' (t), calculated as shown in equation (15);
s55, using the equation (16) to obtain the (t) and the known vehicle rear axle center coordinate (x)r(t)，yr(t)), velocity v (t), body heading angle
Figure GDA0002660294070000118
The front wheel steering angle (T) information and the vehicle kinematics equations (17) to (19) obtain vehicle state information at the next time T + Δ T:
Figure GDA0002660294070000114
Figure GDA0002660294070000115
Figure GDA0002660294070000116
s56, updating the vehicle state by using the vehicle state information obtained by the formulas (17) to (19), and selecting a first preview point (x) by using the T + delta T moment as the current momentp(1)，yp(1) Repeat steps S52 to S55 as the nearest path point, and extrapolate the expected steering angle' (T + k × Δ T) corresponding to each pre-pointing point after that, and use it as the iteration initial value of (k + T | T) in the optimization problem shown in equation (13).
The introduction of the initial value can improve the convergence speed in the solving process and the success rate of obtaining the optimal solution, and avoid the problem from falling into local optimization.
Solving the optimization problem provided by the formula (13) according to the initial value of the steering angle of the front wheel in the prediction time domain obtained by solving in the steps S51 to S56, and further obtaining each pre-aiming point (x) in the N pre-aiming pointsp(k)，yp(k) Correspond toI.e. each of the N preview points (x)p(k)，yp(k) The corresponding desired front wheel steering angle for each of the N home points constitutes an optimal control sequence represented by equation (20):
Figure GDA0002660294070000117
in the formula (20), the reaction mixture is,
Figure GDA0002660294070000121
indicating the predicted front wheel steering angle at time k + t at time t.
In the above-described optimal control sequence, the first element thereof is used as the control quantity for that step of the N steps.
S6, use
Figure GDA0002660294070000122
And controlling the vehicle until the next sampling time arrives and when the next observation time t +1 arrives, repeating the steps from S2 to S5, refreshing the problem by using the new vehicle state provided by S2, and circulating the steps until the path end point is reached.
Finally, it should be pointed out that: the above examples are only for illustrating the technical solutions of the present invention, and are not limited thereto. Those of ordinary skill in the art will understand that: modifications can be made to the technical solutions described in the foregoing embodiments, or some technical features may be equivalently replaced; such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.

Claims (6)

1. A vehicle path tracking control method characterized by comprising:
s1, interpolating the existing reference path points to obtain a new reference path with denser path points;
s2, obtaining vehicle state information;
s3, traversing all paths on the new reference path obtained in S1Point pi(xi，yi) Find the nearest path point p0；
S4, finding the nearest path point p with S3 on the new reference path obtained in S10As a starting point, searching N preview points in front of the running vehicle according to the vehicle motion speed v (t) in the vehicle state information acquired in S2;
s5, constructing a prediction model represented by formulas (2) to (4) and an objective function represented by the following formula (5), predicting the future dynamics of the vehicle according to the current vehicle state information and the prediction model obtained in S2, solving an optimization problem meeting the objective function and constraint conditions on line, obtaining an expected front wheel steering angle corresponding to each of N pre-aiming points obtained by searching in S4, and forming an optimal control sequence represented by the formula (20) by the expected front wheel steering angle corresponding to each of the N pre-aiming points;
Figure FDA0002660294060000011
Figure FDA0002660294060000012
Figure FDA0002660294060000013
Figure FDA0002660294060000014
in formula (5):
q is a weight coefficient;
Δ (k + t | t) is a difference between the vehicle front wheel steering angle at the time k +1+ t predicted at the time t and the vehicle front wheel steering angle at the time k + t predicted immediately before;
e (k + t | t) is the distance between the center of the rear axle of the vehicle at the time k + t predicted at the time t and the tangent of the kth preview point searched at the time t, and is expressed by equation (6):
Figure FDA0002660294060000015
in formula (6):
x (k + t | t) is an abscissa of the center point of the rear axle of the vehicle at the time k + t predicted at the time t;
y (k + t | t) is the ordinate of the vehicle rear axle center point at the time k + t predicted at the time t;
xp(k) the abscissa of the kth pre-aiming point searched in the S4 is obtained;
yp(k) the ordinate of the kth pre-aiming point searched in the S4 is obtained;
θp(k) the heading angle of the k-th pre-aiming point searched in the S4 is obtained;
Figure FDA0002660294060000021
in the formula (20), the reaction mixture is,
Figure FDA0002660294060000022
indicating the front wheel steering angle at the predicted k + t time at t time;
solving an optimization problem as shown in the following formula (13) in each control cycle:
Figure FDA0002660294060000023
in formula (13):
minJ (k) is the minimum of the objective function provided by equation (5);
x (k +1+ t | t) is an abscissa of the center point of the rear axle of the vehicle at the time k +1+ t predicted at the time t;
y (k +1+ t | t) is the ordinate of the vehicle rear axle center point at the time k +1+ t predicted at the time t;
Figure FDA0002660294060000025
the predicted vehicle body heading angle at the moment k + t at the moment t;
Figure FDA0002660294060000026
the predicted vehicle body heading angle at the moment k +1+ t at the moment t;
v (t) the vehicle speed obtained for S2;
Δ t is the time for each prediction step;
(k + t | t) is the predicted front wheel steering angle at time k + t at time t;
(k +1+ t | t) is the predicted front wheel steering angle at time k + t +1 at time t;
l is the wheelbase of the vehicle;
k is the kth prediction step;
Δmaxthe maximum variation of the steering angle of the front wheels of the vehicle in the adjacent time;
maxthe maximum steering angle of the front wheels of the vehicle;
s6, use
Figure FDA0002660294060000024
And controlling the vehicle until the next sampling time arrives and when the next observation time t +1 arrives, repeating the steps from S2 to S5, refreshing the problem by using the new vehicle state provided by S2, and circulating the steps until the path end point is reached.
2. The vehicle path tracking control method according to claim 1, wherein the "search for N preview points toward the front of the vehicle travel" in S4 specifically includes a method of:
with the nearest path point p0As a starting point, and at the same time, with v (T) Δ T as a search distance, searching N path points as pre-aiming points (x) in the front of the vehicle running along the new reference pathp(k)，yp(k) K ═ 1, 2, …, N, Δ T are discrete time steps, and the search is stopped when it reaches the end of the new reference path.
3. The vehicle path tracking control method according to claim 1, wherein the "search for N preview points toward the front of the vehicle travel" in S4 specifically includes a method of:
with the nearest path point p0As a starting point, and at the same time, with v (T) Δ T as a search distance, searching N path points as pre-aiming points (x) in the front of the vehicle running along the new reference pathp(k)，yp(k) K ═ 1, 2, …, N, Δ T are discrete time steps, and the search is stopped when it reaches a set maximum number of predicted steps N.
4. The vehicle path tracking control method according to claim 2 or 3, wherein the specific calculation method of the initial value of the front wheel steering angle solution iteration in the prediction time domain before the optimization problem solution shown in equation (13) includes:
s51, using the vehicle state information obtained in the step S2, taking the same step size Delta T as that in the step S4, and obtaining the nearest path point p in the steps S3 and S40And the first N-1 preview points are searched as the nearest path points (x) of the corresponding time of the Stanley methodp(k)，yp(k))，k＝0，2，…，N-1；
S52, calculating the center (x) of the rear axle of the vehicle at the current time tr(t)，yr(t)) and corresponding nearest path point p0(xp(0)，yp(0) A lateral deviation e offa(t)：
Figure FDA0002660294060000031
In the formula (14), θp(0) Is a path point p0The course angle of (d);
s53, calculating the angle error theta of the vehicle body course angle and the course angle of the nearest path point at the current time te(t)：
Figure FDA0002660294060000041
In the formula (15), the reaction mixture is,
Figure FDA0002660294060000042
is the body heading angle, θp(0) Is a path point p0The course angle of (d);
s54, calculating the expected steering angle' (t) of the front wheels at the current time t:
Figure FDA0002660294060000043
in formula (16):
k is a weight coefficient;
θe(t) an angle error portion for the desired steering angle' (t);
s55, using the equation (16) to obtain the (t) and the known vehicle rear axle center coordinate (x)r(t)，yr(t)), velocity v, body heading angle
Figure FDA0002660294060000044
The front wheel steering angle (T) information and the vehicle kinematics equations (17) to (19) obtain vehicle state information at the next time T + Δ T:
Figure FDA0002660294060000045
Figure FDA0002660294060000046
Figure FDA0002660294060000047
s56, updating the vehicle state by using the vehicle state information obtained by the formulas (17) to (19), and selecting a first preview point (x) by using the T + delta T moment as the current momentp(1)，yp(1) ) as the nearest route point, repeating the steps S52 to S55, and forwardingThe expected steering angle' (T + k × Δ T) corresponding to each preview point is derived after the estimation, and is used as the iteration initial value of (k + T | T) in the optimization problem shown in formula (13).
5. The vehicle path following control method according to claim 1, 2 or 3, wherein the "closest path point p" in S30"comprises the following steps:
calculation of Path Point p Using equation (1)i(xi，yi) With the vehicle rear axle center (x) at the current time tr(t)，yr(t)) distance squared DiAnd find out the distance squared DiThe smallest path point, which is denoted as the nearest path point p0：
Di＝(xr(t)-xi)2+(yr(t)-yi)2 (1)。
6. A vehicle path tracking control method according to claim 1, 2 or 3, wherein S1 employs a cubic spline interpolation method, which specifically includes the steps of:
s11, selecting a path point on the reference path, and calculating the rotation direction angle of the coordinate system according to the yaw angle information of the path point adjacent to the selected path point in the geodetic coordinate system;
s12, rotating the geodetic coordinate system, wherein the rotation direction angle is the rotation direction angle of the coordinate system calculated in the step S11, and calculating new coordinates and a course angle of two path points in the rotated S11;
s13, calculating a fitting cubic spline interpolation curve expression between the two path points according to the new coordinates and the course angle of the two path points in the S11 calculated in the S12;
s14, performing equidistant dispersion on the abscissa of the cubic spline interpolation curve expression obtained in the S13 to obtain high-density interpolation path points, and further obtaining a new reference path, wherein the first derivative of each interpolation path point is course angle information of the point;
s15, rotating the coordinate system of the interpolation path point of S14 to restore the interpolation path point to the geodetic coordinate system, and obtaining the coordinates and course angle information of the original adjacent path points and the interpolation path points thereof under the geodetic coordinate system, wherein: the rotation direction is opposite to the rotation direction of S12, and the rotation angle is the coordinate system rotation direction angle calculated in S11.
CN201911360652.5A 2019-12-25 2019-12-25 Vehicle path tracking control method Active CN110989625B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911360652.5A CN110989625B (en) 2019-12-25 2019-12-25 Vehicle path tracking control method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911360652.5A CN110989625B (en) 2019-12-25 2019-12-25 Vehicle path tracking control method

Publications (2)

Publication Number Publication Date
CN110989625A CN110989625A (en) 2020-04-10
CN110989625B true CN110989625B (en) 2020-11-27

Family

ID=70077025

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911360652.5A Active CN110989625B (en) 2019-12-25 2019-12-25 Vehicle path tracking control method

Country Status (1)

Country Link
CN (1) CN110989625B (en)

Families Citing this family (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111487966B (en) * 2020-04-13 2022-09-09 哈尔滨工程大学 Self-adaptive path tracking control method for unmanned surface vehicle based on waypoints
CN111504340B (en) * 2020-05-22 2022-04-29 北京汽车研究总院有限公司 Vehicle path planning method and device and vehicle
CN112000090B (en) * 2020-07-03 2023-06-27 中国北方车辆研究所 Track queue management method for unstructured road
CN114384902B (en) * 2020-10-19 2024-04-12 中车株洲电力机车研究所有限公司 Automatic tracking control method and system thereof
CN113110045B (en) * 2021-03-31 2022-10-25 同济大学 Model prediction control real-time optimization parallel computing method based on computation graph
CN113204236B (en) * 2021-04-14 2022-05-20 华中科技大学 Intelligent agent path tracking control method
CN113276833B (en) * 2021-05-11 2022-07-29 上汽通用五菱汽车股份有限公司 Lateral motion control method for vehicle, control terminal, and storage medium
CN113390431B (en) * 2021-06-17 2022-09-30 广东工业大学 Method and device for dynamically generating reference line, computer equipment and storage medium
CN113306573B (en) * 2021-06-23 2022-08-16 湖南大学 Learning type path tracking prediction control method for automatic driving vehicle
CN113253741B (en) * 2021-06-29 2021-11-23 浙江华睿科技股份有限公司 AGV forklift control method and device and computer readable medium
CN113635892B (en) * 2021-10-18 2022-02-18 禾多科技（北京）有限公司 Vehicle control method, device, electronic equipment and computer readable medium
CN113848953B (en) * 2021-10-25 2023-12-08 济南大学 Unmanned path tracking control method for large-scale hydraulic plant protection machine
CN113985895B (en) * 2021-11-29 2024-05-03 佛山市毕佳索智能科技有限公司 AGV path tracking method based on optimization
CN113978550B (en) * 2021-12-30 2022-03-25 北京慧拓无限科技有限公司 Reversing control method for unmanned mine car

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0788044A1 (en) * 1996-02-01 1997-08-06 MANNESMANN Aktiengesellschaft An automated guided vehicle navigation steering control system
CN102358287A (en) * 2011-09-05 2012-02-22 北京航空航天大学 Trajectory tracking control method used for automatic driving robot of vehicle
US8437890B2 (en) * 2009-03-05 2013-05-07 Massachusetts Institute Of Technology Integrated framework for vehicle operator assistance based on a trajectory prediction and threat assessment
CN104977933A (en) * 2015-07-01 2015-10-14 吉林大学 Regional path tracking control method for autonomous land vehicle
CN107161207A (en) * 2017-05-08 2017-09-15 江苏大学 A kind of intelligent automobile Trajectory Tracking Control System and control method based on active safety
CN107942663A (en) * 2017-11-21 2018-04-20 山东省计算中心（国家超级计算济南中心） Agricultural machinery automatic steering control method based on fuzzy PID algorithm
CN108248605A (en) * 2018-01-23 2018-07-06 重庆邮电大学 The transverse and longitudinal control method for coordinating that a kind of intelligent vehicle track follows
CN108732921A (en) * 2018-04-28 2018-11-02 江苏大学 A kind of autonomous driving vehicle, which can laterally be opened up, pre- takes aim at method for handover control
CN108973769A (en) * 2018-06-15 2018-12-11 吉林大学 A kind of all fronts control electric car path tracking control method
CN109318905A (en) * 2018-08-22 2019-02-12 江苏大学 A kind of intelligent automobile path trace mixing control method
CN109795502A (en) * 2018-09-27 2019-05-24 吉林大学 Intelligent electric automobile path trace model predictive control method
CN109857098A (en) * 2018-12-11 2019-06-07 东南大学 A kind of automatic Pilot agricultural machinery Trajectory Tracking System and method based on MPC
CN110598311A (en) * 2019-09-06 2019-12-20 广东工业大学 Automatic driving vehicle track tracking method

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0788044A1 (en) * 1996-02-01 1997-08-06 MANNESMANN Aktiengesellschaft An automated guided vehicle navigation steering control system
US8437890B2 (en) * 2009-03-05 2013-05-07 Massachusetts Institute Of Technology Integrated framework for vehicle operator assistance based on a trajectory prediction and threat assessment
CN102358287A (en) * 2011-09-05 2012-02-22 北京航空航天大学 Trajectory tracking control method used for automatic driving robot of vehicle
CN104977933A (en) * 2015-07-01 2015-10-14 吉林大学 Regional path tracking control method for autonomous land vehicle
CN107161207A (en) * 2017-05-08 2017-09-15 江苏大学 A kind of intelligent automobile Trajectory Tracking Control System and control method based on active safety
CN107942663A (en) * 2017-11-21 2018-04-20 山东省计算中心（国家超级计算济南中心） Agricultural machinery automatic steering control method based on fuzzy PID algorithm
CN108248605A (en) * 2018-01-23 2018-07-06 重庆邮电大学 The transverse and longitudinal control method for coordinating that a kind of intelligent vehicle track follows
CN108732921A (en) * 2018-04-28 2018-11-02 江苏大学 A kind of autonomous driving vehicle, which can laterally be opened up, pre- takes aim at method for handover control
CN108973769A (en) * 2018-06-15 2018-12-11 吉林大学 A kind of all fronts control electric car path tracking control method
CN109318905A (en) * 2018-08-22 2019-02-12 江苏大学 A kind of intelligent automobile path trace mixing control method
CN109795502A (en) * 2018-09-27 2019-05-24 吉林大学 Intelligent electric automobile path trace model predictive control method
CN109857098A (en) * 2018-12-11 2019-06-07 东南大学 A kind of automatic Pilot agricultural machinery Trajectory Tracking System and method based on MPC
CN110598311A (en) * 2019-09-06 2019-12-20 广东工业大学 Automatic driving vehicle track tracking method

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
Vehicle tracking control on piecewise-clothoidal trajectories by MPC with guaranteed error bounds;S. Di Cairano等;《2016 IEEE 55th Conference on Decision and Control (CDC)》;20161214;第709-714页 *
基于MPC算法的预判型ESP控制策略研究;李萧良;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》;20190215;第C035-237页 *
基于多点预瞄最优控制的智能车辆路径跟踪;黄海洋;《汽车技术》;20181231(第10期);第6-9页 *
基于模型预测控制的移动机器人路径跟踪控制;刘洋;《中国优秀硕士学位论文全文数据库 信息科技辑》;20160930(第09(2016)期);第I140-87页 *
智能汽车横向轨迹跟踪的离散时间模型预测控制;高洪波，等;《指挥与控制学报》;20181231;第4卷(第4期);第297-305页 *

Also Published As

Publication number Publication date
CN110989625A (en) 2020-04-10

Similar Documents

Publication Publication Date Title
CN110989625B (en) Vehicle path tracking control method
CN110955250B (en) Vehicle reversing path tracking control method
CN110568758B (en) Parameter self-adaptive transverse motion LQR control method for automatically driving automobile
CN112733270B (en) System and method for predicting vehicle running track and evaluating risk degree of track deviation
CN108614426B (en) Multi-mobile-robot formation robust control method based on disturbance observer
CN112693449B (en) Transverse and longitudinal coupling control method under limit working condition of unmanned vehicle
CN110398969A (en) Automatic driving vehicle adaptive prediction time domain rotating direction control method and device
CN108710294B (en) Unmanned vehicle path tracking feedforward control method
CN109683614B (en) Vehicle path control method and device for unmanned mining vehicle
JP2008143269A (en) Vehicular control device
CN111538328B (en) Priority hierarchical prediction control method for obstacle avoidance trajectory planning and tracking control of autonomous driving vehicle
CN107289938B (en) Local path planning method for ground unmanned platform
CN111258218B (en) Intelligent vehicle path tracking method based on maximum correlation entropy criterion
Kebbati et al. Lateral control for autonomous wheeled vehicles: A technical review
CN113467231A (en) Unmanned ship path tracking method based on sideslip compensation ILOS guidance law
US20240067185A1 (en) Technology for dead time compensation during transverse and longitudinal guidance of a motor vehicle
CN114940164B (en) Unmanned vehicle driving track optimization method and system for parking scene
CN114942642A (en) Unmanned automobile track planning method
CN106647264A (en) Unmanned aerial vehicle control method based on control constraint extended robust H<infinite>
CN113311845B (en) Pure tracking control error compensation method and device based on path curvature
Lorenz et al. A decoupled approach for trajectory generation for an unmanned rotorcraft
CN111045428B (en) Obstacle avoidance method, mobile robot, and computer-readable storage medium
CN113306573B (en) Learning type path tracking prediction control method for automatic driving vehicle
CN116691668A (en) Transverse control method for large intelligent vehicle
CN114740845A (en) Vehicle tracking control method based on immersion and invariant manifold

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
GR01 Patent grant
GR01 Patent grant