CN117922548A - Automatic parking track planning method based on model predictive control - Google Patents

Automatic parking track planning method based on model predictive control Download PDF

Info

Publication number
CN117922548A
CN117922548A CN202410119397.XA CN202410119397A CN117922548A CN 117922548 A CN117922548 A CN 117922548A CN 202410119397 A CN202410119397 A CN 202410119397A CN 117922548 A CN117922548 A CN 117922548A
Authority
CN
China
Prior art keywords
vehicle
control
track
parking
model
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.)
Granted
Application number
CN202410119397.XA
Other languages
Chinese (zh)
Other versions
CN117922548B (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.)
Harbin Institute of Technology Weihai
Original Assignee
Harbin Institute of Technology Weihai
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 Harbin Institute of Technology Weihai filed Critical Harbin Institute of Technology Weihai
Priority to CN202410119397.XA priority Critical patent/CN117922548B/en
Publication of CN117922548A publication Critical patent/CN117922548A/en
Application granted granted Critical
Publication of CN117922548B publication Critical patent/CN117922548B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/06—Automatic manoeuvring for parking
    • 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/0011—Planning or execution of driving tasks involving control alternatives for a single driving scenario, e.g. planning several paths to avoid obstacles
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W2050/0001—Details of the control system
    • B60W2050/0019—Control system elements or transfer functions
    • B60W2050/0028—Mathematical models, e.g. for simulation
    • B60W2050/0031—Mathematical model of the vehicle
    • 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
    • B60W2520/00—Input parameters relating to overall vehicle dynamics
    • B60W2520/10—Longitudinal speed
    • 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
    • B60W2520/00—Input parameters relating to overall vehicle dynamics
    • B60W2520/12—Lateral speed
    • 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
    • B60W2540/00—Input parameters relating to occupants
    • B60W2540/18—Steering angle

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Control Of Driving Devices And Active Controlling Of Vehicle (AREA)

Abstract

The invention provides an automatic parking track planning method based on model predictive control, which comprises the following steps: inputting a given speed into a vehicle kinematic model, outputting a preset initial position and a preset gesture of the vehicle by the vehicle kinematic model, transmitting the initial position and the gesture to a parking controller, and comparing the initial position and the gesture with a preset parking track to obtain an error signal of the position and the gesture of the vehicle; the preset parking track is designed based on a fifth-order polynomial curve; based on error signals of the position and the posture of the vehicle, the parking controller performs calculation control based on model prediction control, outputs a numerical value of steering angle of the vehicle at the current moment, inputs the numerical value into a vehicle kinematics model, and controls the operation of the vehicle together with a given speed signal, so that the vehicle continuously approaches a preset parking track and automatic parking track planning is realized. The invention greatly improves the accuracy of automatic parking track planning by adopting a path planning design mainly comprising a quintic polynomial path and a track tracking control design mainly comprising model predictive control.

Description

Automatic parking track planning method based on model predictive control
Technical Field
The invention relates to the technical field of automatic parking track planning, in particular to an automatic parking track planning method based on model predictive control.
Background
The ideal automatic parking enables the vehicle to reach the preset point from the starting point, not only the position meets the preset requirement, but also the gesture of the vehicle meets the requirement, so that the automatic parking related to the planning and tracking of the preset track is needed, a reasonable path between the starting point and the target point can be planned before the vehicle is parked, and then a proper control rule is selected to control the vehicle to reach the preset point from the starting point according to the generated track.
In the prior art, the control of automatic parking track tracking is basically based on a vehicle kinematic model, the track of the trolley meeting the kinematic model is composed of a series of circular arcs, and among a plurality of parking tracks, the simplest is two circular arc tracks, as the name implies, the trolley is composed of two sections of tangential circular arcs, and the trolley has the advantages that the track mathematical model is simple and easy to express and write, the defect is that the curvature of the trolley is discontinuous, and the change of the tangent point is too large, in other words, in the practical situation, when the vehicle is positioned at the tangent point, a driver needs to instantly turn the steering wheel from side to side, which brings great operation difficulty, and great error is generated during control, so that the simulation result is quite non-ideal. The track adopted under many circumstances is an arc-straight line-arc track, and the track has a section of straight line between two sections of arcs to transition, so that the defects caused by the two-arc track are better improved.
Model predictive control (ModelPredictive Control), which is a trajectory tracking algorithm widely used in automatic driving, is almost the most popular control method for trajectory tracking at present with PID control, LQR control, sliding mode control and the like, and is a control method for performing mathematical optimization according to a vehicle dynamics model established by us.
Essentially, the model predictive control, which is based on the current state of the vehicle and the desired path, predicts the behavior of the vehicle, i.e., the steering angle of the steering wheel, and transmits a control signal to the actuator to turn the vehicle, can also be referred to as a feedback control. A series of prediction actions are made at each moment to track the target track, but in order to pursue high precision, errors are reduced, the influence of other factors on a control process is avoided, and only the first step of each prediction is taken for control. And the method is repeated continuously, so that the track to be tracked can be approximated continuously with very small errors. Therefore, it is of great technical value to strengthen the application research of model-based predictive control in automatic parking trajectory planning.
Disclosure of Invention
The invention provides an automatic parking track planning method based on model predictive control, which takes a path planning design taking a quintic polynomial path as a main part and a track tracking control design taking model predictive control as a main part, thereby greatly improving the accuracy of automatic parking track planning.
The invention provides an automatic parking track planning method based on model predictive control, which comprises the following steps:
S1, inputting a given speed into a vehicle kinematic model, outputting a preset initial position and a preset gesture of a vehicle by the vehicle kinematic model, transmitting the initial position and the gesture to a parking controller, and comparing the initial position and the gesture with a preset parking track to obtain an error signal of the position and the gesture of the vehicle; the preset parking track is designed based on a fifth-order polynomial curve;
S2, calculating and controlling by a parking controller based on model prediction control based on error signals of the position and the posture of the vehicle, and outputting a numerical value for controlling the steering angle of the vehicle at the current moment;
S3, inputting the value of the steering angle into a vehicle kinematic model, and controlling the running of the vehicle together with a given speed signal;
S4, repeating the steps S1-S3, and enabling the vehicle to continuously approach a preset parking track to realize automatic parking track planning.
Further, the preset parking track is designed based on a fifth-order polynomial curve, and specifically includes:
After the sizes of the vehicle and the garage are determined, the position of the center point of the rear axle of the vehicle in an inertial coordinate system is determined, the coordinates of a starting point are set as (x 0,y0) and the coordinates of an ending point are set as (x 1,y1), the starting time of track planning between the two points of the starting point and the ending point is t 0, and the ending time is t 1;
the locus of the center point of the rear axle is expressed by a function y (x), and the expression adopts a fifth-order polynomial, which is as follows:
y(x)＝a0+a1x+a2x2+a3x3+a4x4+a5x5 (1)
Wherein a 0,a1,a2,a3,a4,a5 is a coefficient of a penta-order polynomial, and constraint conditions of a penta-order polynomial curve are set, including a starting point coordinate and an ending point coordinate of a vehicle, a vehicle body azimuth, and a curvature of the curve, namely, initial conditions meet the following requirements:
y(x0)＝y0，
y(x1)＝y1，
the coefficients can be calculated according to the fifth order polynomial and the first derivative and the second derivative thereof: a 0,a1,a2;
Bringing the obtained a 0,a1,a2 back to the original formula and the first derivative and the second derivative thereof, and calculating to obtain a coefficient a 3,a4,a5;
And (3) rewriting the calculated penta-polynomial track into the relationship between the abscissa x of the central point of the rear axle of the vehicle and the time t and the relationship between the course angle theta and the time t.
Further, the relationship between the heading angle θ and the time t is expressed as follows:
Kinematic model formula of vehicle:
In the method, in the process of the invention, For the steering angle of the vehicle, θ is the heading angle of the vehicle,/>Is the course angular velocity; /(I)And/>The first derivative of the displacement of the vehicle in the X axis and the Y axis is represented by l, the wheelbase of the vehicle is represented by v, and the speed of the center of the rear axle of the vehicle is represented by v;
According to the equation (2), the relation between the transverse displacement x and the heading angle theta is selected to deduce the relation between the theta and the time t:
Further, the step S2 is performed by the parking controller based on the model prediction control based on the error signal of the vehicle position and the attitude, and specifically includes:
The state vector is taken as X= [ X, y, theta ] T, and the control vector is taken as The kinematic model formula (2) of the vehicle is rewritten into a vector form:
Where f= [ f 1,f2,f3]T,f1＝vcosθ,f2 =vsin θ,
The ideal automatic parking path planned by the fifth order polynomial (1) has a state vector of X r＝[xr,yr,θr]T and a control vector of X r＝[xr,yr,θr]T at each moment
Where (x r,yr) is the planned vehicle rear axle center point coordinate, θ r is the planned heading angle,V r is the planned vehicle rear axle center speed;
By performing taylor expansion at f (X r,ur) of equation (4), retaining the first order term, ignoring the higher order term, a linearized error state equation can be obtained:
wherein,
Performing forward Euler discretization on the formula (6), and setting T 0 =0, wherein the current time is t=kT, and T is the sampling time, so that a discrete state equation can be obtained:
In the middle of
For model prediction, intermediate variables are setDeriving a discrete state space expression:
In the middle of I is a unit array;
the predicted value of the output η (k) at the next N p times can be obtained by the following equation:
Y(k)＝ψk(k)ξ(k)+ΘKΔU(k) (9)
In the middle of
N p is the predicted time domain step number, N C is the control time domain step number;
the objective function is expressed as:
Wherein Q is an output weight matrix, R is a control increment weight matrix, ρ is a relaxation factor weight, and ε is a relaxation factor.
Further, regarding the constraint section, the control amount increment error constraint is:
Where ΔU min and ΔU max are the minimum and maximum values of the control increment, respectively, and M is the maximum value of the slack.
The automatic parking track planning method based on model prediction control provided by the invention is based on a vehicle kinematics model, a path planning design mainly comprising a five-order polynomial path and a track tracking control design mainly comprising model prediction control are determined, the smallest order which can realize the best parallel parking effect in a polynomial curve is a five-order polynomial curve, the five-order polynomial curve is used as a track planning curve of an automatic parking system, the accuracy of path planning is improved, the model prediction control can make a series of prediction actions at each moment to track a target track, but in order to pursue high precision, reduce errors and avoid the influence of other factors on a control process, only the first step of each prediction is taken for control, and the control is repeated continuously, so that the track to be tracked can be continuously approximated with a very small error; the steering angle value output by the parking controller is input into a kinematic model of the target vehicle, the running of the vehicle is controlled together with a given speed signal, the vehicle can continuously output the gesture and the position of the vehicle to continuously execute the first step flow of the control method, and the vehicle can continuously approach the planned parking track to achieve the effect of path tracking.
Drawings
In order to more clearly illustrate the invention or the technical solutions of the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described, and it is obvious that the drawings in the description below are some embodiments of the invention, and other drawings can be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a block diagram of an automated parking trajectory tracking control system of the present invention;
FIG. 2 is a diagram of a polynomial trajectory of the present invention, wherein (a) represents a trajectory based on a polynomial of fifth degree; (b) represents track curve speed; (c) represents a trajectory curve acceleration;
FIG. 3 is a schematic illustration of simulation results of a parking process;
Fig. 4 is a parking error analysis chart.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the present invention more apparent, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings, and it is apparent that the described embodiments are some embodiments of the present invention, not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The following describes an automatic parking trajectory planning method based on model predictive control of the present invention with reference to fig. 1 to 4.
FIG. 1 is a block diagram of an automated parking trajectory tracking control system that includes a processor that sets a kinematic model of a vehicle, a parking controller, and an actuator.
As shown in fig. 1, the automatic parking trajectory planning method based on model predictive control provided in this embodiment may be executed by an automatic parking trajectory tracking control system, and at least includes the following steps:
S1, inputting a given speed into a vehicle kinematic model, outputting a preset initial position and a preset gesture of a vehicle by the vehicle kinematic model, transmitting the initial position and the gesture to a parking controller, and comparing the initial position and the gesture with a preset parking track to obtain an error signal of the position and the gesture of the vehicle; the preset parking track is designed based on a fifth-order polynomial curve;
S2, calculating and controlling by a parking controller based on model prediction control based on error signals of the position and the posture of the vehicle, and outputting a numerical value for controlling the steering angle of the vehicle at the current moment;
S3, inputting the value of the steering angle into a vehicle kinematic model, and controlling the running of the vehicle together with a given speed signal;
S4, repeating the steps S1-S3, and enabling the vehicle to continuously approach a preset parking track to realize automatic parking track planning.
Specifically, in step S1, the designing of the preset parking trajectory based on the fifth order polynomial curve includes the following steps:
The vehicle meeting the kinematic model has the advantages that the track is composed of a series of circular arcs, the simplest of the tracks is a two-circular arc track, and the track is composed of two sections of tangential circular arcs as the name implies, the advantages that the track mathematical model is simple, easy to express and write, the disadvantage that the curvature of the track mathematical model is discontinuous, the change of the tangent point is too large, in other words, in practical cases, when the vehicle is positioned at the tangent point, a driver needs to instantly turn the steering wheel from one side to the other side, great operation difficulty is brought, and great error is generated during control, so that simulation results are quite unsatisfactory. The track adopted under many circumstances is an arc-straight line-arc track, and the track has a section of straight line between two sections of arcs to transition, so that the defects caused by the two-arc track are better improved. The invention selects the fifth order polynomial curve as the track planning curve of the automatic parking system.
In actual running, the motion track of the vehicle is very close to the arc curve, the curve of the fifth order polynomial is very smooth and the curvature is continuous, so that the curve of the fifth order polynomial can be used as a very good track planning path, and in fact, the curve of the fifth order polynomial and other polynomials can be widely applied to track planning of a plurality of robots and mechanical arms, and very good effects are achieved.
The smallest order that can realize the best parallel parking effect in the polynomial curve is a five-order polynomial curve, the curvature of the curve is k, the curve is a function of x, and the constraint conditions of the curve include the position of a starting point, an end point, the vehicle body orientation, the curvature of the curve and other conditions.
After the sizes of the vehicle and the garage are determined, the position of the center point of the rear axle of the vehicle in an inertial coordinate system is determined, the coordinates of a starting point are set as (x 0,y0) and the coordinates of an ending point are set as (x 1,y1), the starting time of track planning between the two points of the starting point and the ending point is t 0, and the ending time is t 1;
The locus of the center point of the rear axle can be expressed by a function y (x) expressed as:
y(x)＝a0+a1x+a2x2+a3x3+a4x4+a5x5 (1)
Where a 0,a1,a2,a3,a4,a5 is the coefficient of the fifth degree polynomial. Fitting the curve to the parking arc track needs to meet the requirement that the slope between the starting point of the vehicle to be parked and the target parking position in the garage is zero, and the curvature also needs to meet certain requirements. The constraint conditions of the quintic polynomial curve are set, wherein the constraint conditions comprise the starting point coordinates and the ending point coordinates of the vehicle, the vehicle body azimuth and the curvature of the curve, namely the initial conditions meet the following requirements:
y(x0)＝y0，
y(x1)＝y1，
In the design process, after the vehicle and the garage are designed, the coordinates of the starting point and the ending point of the vehicle are determined, the coordinates of the starting point and the ending point are known, the starting time of one section of track planning is t 0, and the ending time is t 1. In general, when the starting point is default to t 0 =0, the coefficients can be obtained by calculating the fifth order polynomial and the first derivative and the second derivative thereof: a 0,a1,a2. Bringing the obtained a 0,a1,a2 back to the original expression, and the first derivative and the second derivative thereof, and calculating to obtain a 3,a4,a5.
For example, the size of a parking space selected for track following automatic parking is 6m×2m, and the size of a vehicle is 2.48m×1.66m, so that the vehicle can be accurately parked in a garage for convenience. The selected starting point is (0, 3), the selected ending point is (9, 8), the constraint condition of the track is completely determined, the known starting point and ending point and the planned constraint are used for obtaining the track equation to be tracked, and the track equation to be tracked is:
y(x)＝3+2.89×10-2x3+3.6×10-3x4+1.2056×10-4x5 (1-2)
the position of the polynomial trajectory equation of fifth order, the change of the curve speed and acceleration are shown in fig. 2.
In order to write the planned path into the program, the planned path is compared with the position generated in real time by the vehicle dynamics model, error amount is generated for control, and the vehicle dynamics model outputs x, y and theta coordinates of each moment, so that the calculated penta-polynomial track needs to be rewritten into the relation between the x and t of the central point of the rear axle of the vehicle and the relation between the x and t of the course angle theta and the time t.
The specific rewriting method is that a certain time, for example, 10 seconds, is selected, the abscissa and the ordinate of the starting point and the ending point are taken as constraints, the time ranges from 0 to 10 seconds, and the relation between the abscissa x and the time t can be written out first:
x(t)＝0.1x3-0.015x4+6×10-4x5 (1-3)
when calculating the relation between the coordinates y and t of the center point of the rear axle, the following formula (1-4) can be used for obtaining:
y(t)＝y(x(t)) (1-4)
the relationship between heading angle θ and time t is formulated by the kinematic model of the vehicle:
In the method, in the process of the invention, For the steering angle of the vehicle, θ is the heading angle of the vehicle,/>Is the course angular velocity; /(I)And/>The first derivative of the displacement of the vehicle in the X axis and the Y axis is represented by l, the wheelbase of the vehicle is represented by v, and the speed of the center of the rear axle of the vehicle is represented by v;
According to the equation (2), the relation between the transverse displacement x and the heading angle theta is selected to deduce the relation between the theta and the time t:
All parameters and formulas related to the planned penta-polynomial trajectory are obtained, and when the controller is designed, the designed trajectory equation can be called to park.
Specifically, in step S2, based on the error signals of the vehicle position and posture, the parking controller performs calculation control based on model prediction control, and the specific contents are as follows:
Many parameters are included in designing model predictive control, such as: sampling time, prediction step size, controller range, constraint weights, all of which are chosen in conjunction with the specific implementation at design time. The design of model predictive control takes into account many aspects.
Model 1 predictive control
The state vector is taken as X= [ X, y, theta ] T, and the control vector is taken asThe kinematic model formula (2) of the vehicle is rewritten into a vector form:
Where f= [ f 1,f2,f3]T,f1＝vcosθ,f2 =vsin θ,
Assuming that the preset vehicle parking system completely passes through the expected path, the state quantity and the control quantity of the ideal vehicle parking system at each moment in the path can be obtained, and for the ideal automatic parking path tracking, the state quantity and the control quantity at each moment in the ideal automatic parking path planned by the quintuple polynomial (1) can meet the equation (5), and the state vector at each moment in the ideal automatic parking path is X r＝[xr,yr,θr]T, and the control vector is
Where (x r,yr) is the planned vehicle rear axle center point coordinate, θ r is the planned heading angle,V r is the planned vehicle rear axle center speed;
By performing taylor expansion at f (X r,ur) of equation (4), retaining the first order term, ignoring the higher order term, a linearized error state equation can be obtained:
wherein,
Performing forward Euler discretization on the formula (6), and setting T 0 =0, wherein the current time is t=kT, and T is the sampling time, so that a discrete state equation can be obtained:
In the middle of
For model prediction, intermediate variables are setDeriving a discrete state space expression:
In the middle of I is a unit array;
the predicted value of the output η (k) at the next N p times can be obtained by the following equation:
Y(k)＝ψk(k)ξ(k)+ΘKΔU(k) (9)
In the middle of
N p is the predicted time domain step number, N C is the control time domain step number;
the objective function is expressed as:
Wherein Q is an output weight matrix, R is a control increment weight matrix, ρ is a relaxation factor weight, and ε is a relaxation factor.
The vehicle is dynamically changed in the running process, the situation that the solution is not solved possibly is caused in the process of solving the objective function under the limit of the constraint condition, so that a relaxation factor is added, which is the requirement of numerical calculation robustness, and the calculation can be continued if the situation of violation of the constraint or the situation of unsatisfied constraint occurs in the calculation process.
Regarding the constraint section, the control amount increment error constraint is:
Where ΔU min and ΔU max are the minimum and maximum values of the control increment, respectively, and M is the maximum value of the slack.
2 Parameter selection
For model predictive control, the selection of parameters is very critical, which not only affects the accuracy and robustness of control, but also directly determines whether the control result is accurate, but the selection of parameters often cannot be directly selected, and in most cases, a parameter value is drawn up, and then the simulation result is observed through simulation to continuously optimize and select the optimal parameter combination. The significance of several parameters of the model predictive control and their impact on the control process is described below.
(1) Sampling time T
In all the controls, the selection of the sampling time is very critical, because the degree of the sampling time is controlled, that is, the sampling time cannot be too large or too small, the system generates a large error due to the too large sampling time, the system is too large in calculation amount due to the too small sampling time, and a large load is brought to calculation.
(2) Predicting time domain N p
The prediction horizon is also a very important parameter of model prediction control, and may represent the number of predicted steps of the controller, that is, the longer the prediction horizon is, the farther the prediction horizon is from the rear distance, the more popular it has to have a more distant view for the parking system, so when the prediction horizon is too large, it is not good to have a more distant prediction view, because if the parking stall obstacle is just started, it is inappropriate to react to the more distant parking stall obstacle, of course, the prediction horizon is too small, and if the prediction horizon is selected to be smaller, the vehicle may only concentrate on the more near environment, and cannot react quickly to the complex environment that is about to be encountered. This can lead to a failure to follow the planned path, degrading control. The selection of the prediction time domain does not have an explicit criterion, and repeated attempts are necessary to find the optimal parameter value selection.
(3) Control time domain N C
The control time domain indicates how much control quantity the system needs to solve in each rolling optimization, and as one of parameters, the influence of the control time domain on the control of the system is not great, and the system only takes a first group of predicted control increment in each optimization process, so that the selection of the control time domain is not complex.
Model predictive control parking simulation
A parking environment is built in MATLAB/Simulink, the model predictive control algorithm designed above is led into the built MATLAB/Simulink environment, the parking environment comprises a vehicle kinematic model, an S-function module used as a model predictive controller for tracking a parking path, a drawing MATLAB function module for displaying the vehicle and a parking space in real time, and a stop simulation module is added to stop the motion of the vehicle after the vehicle is parked in a preset parking space.
The Simulink can better represent each part of the automatic parking control system by each module in the software, and the S-function module in the Simulink better meets various requirements on the design of the controller. The S-function module can edit the input and output of the module to a certain extent, and a controller meeting the requirement of the user is designed. Although Simulink is not professional automobile simulation software, the automobile model only uses the kinematic model of an automobile, the mathematical relationship is relatively simple, the vehicle model can be better represented in Simulink, and meanwhile, parking paths can be displayed by means of other modules, so that simulation of a designed automatic parking control system by MATLAB/Simulink is completely sufficient and quite visual.
Firstly, starting and ending points of a vehicle are determined according to set parameters, a quintic polynomial track equation of parking is calculated, and then the equation is decomposed into functions of input variables x, y, theta and time t. Model predictive control is then introduced and designed. The meaning and selection method of each parameter are introduced. Selecting proper parameters, writing codes in MATLAB, leading the codes into S-function as a controller for path tracking, leading the MATLAB function as a real-time display module of vehicle track, then building a parking simulation environment in Simulink, running simulation, and observing the parking simulation process and result. And running a Simulink simulation, and obtaining a parking process as shown in figure 3.
Although the output of the kinematic model of the vehicle is only the center point of the rear axle of the vehicle, the coordinate relationship between the center point of the rear axle of the vehicle and the four wheels of the vehicle can be deduced easily when the vehicle is subjected to kinematic modeling, and then the MATLAB function module is used for drawing the coordinates of the output part of the vehicle, namely the center point of the rear axle, and marking the coordinate positions of the parking space and the obstacle vehicle, so that the real-time parking track of the vehicle and the position relationship between the parking space and the vehicle can be seen after the simulation is run. After the simulation is run, the vehicle can be seen to automatically park in the planned garage according to the track of the preset penta polynomial, and the automatic parking movement is well completed.
In order to study whether the parking path tracking is accurate, writing codes in MATLAB, writing preset tracks into the codes, calling real-time tracks of vehicles in a working space, and drawing the real-time tracks in the same coordinate system. Thus we can observe errors in vehicle trajectory tracking as shown in figure 4. It can be seen that there is a slight error between the reference trajectory and the actual trajectory, but the error is mainly derived from the fact that the penta-polynomial function is decomposed in time when the trajectory is traced and simulated, and in the mathematical decomposition process, the relation between x and t is also formulated according to the initial and ending condition constraints, and does not necessarily completely represent the relation between x and t, that is, the decomposed equation and the actual equation are not overlapped. Therefore, the actual track and the reference track have certain errors, although the actual track and the reference track have certain errors, the partial errors can be allowed in the simulation because of the parking movement, and the error reduction method is to adopt a tighter mathematical decomposition method, so that the calculated equation and the actual planning equation are completely overlapped, and the errors are greatly reduced.
The invention carries out a complete design on an automatic parking system, explains some common tracks and some common control methods in the aspect of path planning, determines the design to be based on path planning and track tracking, and firstly determines specific parameters of vehicle parking spaces and parking environments. And determining a path planning design mainly comprising a five-degree polynomial path and a track tracking control design mainly comprising model predictive control, simulating the design in MATLAB/Simulink, observing the parking process of the vehicle, comparing the actual path of the vehicle with a reference path to observe errors, and explaining error sources and possible improvement methods.
Finally, it should be noted that: the above embodiments are only for illustrating the technical solution of the present invention, and are not limiting; although the invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims (5)

1. An automatic parking track planning method based on model predictive control is characterized by comprising the following steps:
S1, inputting a given speed into a vehicle kinematic model, outputting a preset initial position and a preset gesture of a vehicle by the vehicle kinematic model, transmitting the initial position and the gesture to a parking controller, and comparing the initial position and the gesture with a preset parking track to obtain an error signal of the position and the gesture of the vehicle; the preset parking track is designed based on a fifth-order polynomial curve;
S2, calculating and controlling by a parking controller based on model prediction control based on error signals of the position and the posture of the vehicle, and outputting a numerical value for controlling the steering angle of the vehicle at the current moment;
S3, inputting the value of the steering angle into a vehicle kinematic model, and controlling the running of the vehicle together with a given speed signal;
S4, repeating the steps S1-S3, and enabling the vehicle to continuously approach a preset parking track to realize automatic parking track planning.
2. The automatic parking trajectory planning method based on model predictive control according to claim 1, wherein the preset parking trajectory is designed based on a quintuple polynomial curve, specifically comprising:
After the sizes of the vehicle and the garage are determined, the position of the center point of the rear axle of the vehicle in an inertial coordinate system is determined, the coordinates of a starting point are set as (x 0,y0) and the coordinates of an ending point are set as (x 1,y1), the starting time of track planning between the two points of the starting point and the ending point is t 0, and the ending time is t 1;
the locus of the center point of the rear axle is expressed by a function y (x), and the expression adopts a fifth-order polynomial, which is as follows:
y(x)＝a0+a1x+a2x2+a3x3+a4x4+a5x5 (1)
Wherein a 0,a1,a2,a3,a4,a5 is a coefficient of a penta-order polynomial, and constraint conditions of a penta-order polynomial curve are set, including a starting point coordinate and an ending point coordinate of a vehicle, a vehicle body azimuth, and a curvature of the curve, namely, initial conditions meet the following requirements:
the coefficients can be calculated according to the fifth order polynomial and the first derivative and the second derivative thereof: a 0,a1,a2;
Bringing the obtained a 0,a1,a2 back to the original formula and the first derivative and the second derivative thereof, and calculating to obtain a coefficient a 3,a4,a5;
And (3) rewriting the calculated penta-polynomial track into the relationship between the abscissa x of the central point of the rear axle of the vehicle and the time t and the relationship between the course angle theta and the time t.
3. The model predictive control-based automatic parking trajectory planning method according to claim 2, wherein the relationship between the heading angle θ and the time t is expressed as follows:
Kinematic model formula of vehicle:
In the method, in the process of the invention, For the steering angle of the vehicle, θ is the heading angle of the vehicle,/>Is the course angular velocity; /(I)And/>The first derivative of the displacement of the vehicle in the X axis and the Y axis is represented by l, the wheelbase of the vehicle is represented by v, and the speed of the center of the rear axle of the vehicle is represented by v;
According to the equation (2), the relation between the transverse displacement x and the heading angle theta is selected to deduce the relation between the theta and the time t:
4. the method for planning an automatic parking trajectory based on model predictive control according to claim 3, wherein said step S2 of calculating and controlling by the parking controller based on the model predictive control based on the error signal of the vehicle position and posture specifically comprises:
The state vector is taken as X= [ X, y, theta ] T, and the control vector is taken as The kinematic model formula (2) of the vehicle is rewritten into a vector form:
where f= [ f 1,f2,f3]T,f1＝v cosθ,f2 =vsin θ,
The ideal automatic parking path planned by the fifth order polynomial (1) has a state vector of X r＝[xr,yr,θr]T and a control vector of X r＝[xr,yr,θr]T at each moment
Where (x r,yr) is the planned vehicle rear axle center point coordinate, θ r is the planned heading angle,V r is the planned vehicle rear axle center speed;
By performing taylor expansion at f (X r,ur) of equation (4), retaining the first order term, ignoring the higher order term, a linearized error state equation can be obtained:
wherein,
Performing forward Euler discretization on the formula (6), and setting T 0 =0, wherein the current time is t=kT, and T is the sampling time, so that a discrete state equation can be obtained:
In the middle of
For model prediction, intermediate variables are setDeriving a discrete state space expression:
In the middle of I is a unit array;
the predicted value of the output η (k) at the next N p times can be obtained by the following equation:
Y(k)＝ψk(k)ξ(k)+ΘKΔU(k) (9)
In the middle of
N p is the predicted time domain step number, N C is the control time domain step number;
the objective function is expressed as:
Wherein Q is an output weight matrix, R is a control increment weight matrix, ρ is a relaxation factor weight, and ε is a relaxation factor.
5. The model predictive control-based automatic parking trajectory planning method of claim 4, wherein, with respect to the constraint section, the control amount increment error constraint is:
Where ΔU min and ΔU max are the minimum and maximum values of the control increment, respectively, and M is the maximum value of the slack.
CN202410119397.XA 2024-01-29 2024-01-29 Automatic parking track planning method based on model predictive control Active CN117922548B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202410119397.XA CN117922548B (en) 2024-01-29 2024-01-29 Automatic parking track planning method based on model predictive control

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202410119397.XA CN117922548B (en) 2024-01-29 2024-01-29 Automatic parking track planning method based on model predictive control

Publications (2)

Publication Number Publication Date
CN117922548A true CN117922548A (en) 2024-04-26
CN117922548B CN117922548B (en) 2024-07-16

Family

ID=90750539

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202410119397.XA Active CN117922548B (en) 2024-01-29 2024-01-29 Automatic parking track planning method based on model predictive control

Country Status (1)

Country Link
CN (1) CN117922548B (en)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102007008624A1 (en) * 2007-02-22 2008-08-28 Hella Kgaa Hueck & Co. Motor vehicle i.e. train, steering method, involves forming control input signal for controller i.e. proportional integral controller, from feedback vector, and determining desired speed from plan speed, web guiding vector and variable
WO2022095814A1 (en) * 2020-11-05 2022-05-12 长沙智能驾驶研究院有限公司 Automatic vehicle reversing control method and apparatus, vehicle and storage medium
CN114523959A (en) * 2022-04-08 2022-05-24 南昌智能新能源汽车研究院 Automatic parking trajectory planning method based on vehicle kinematics model
CN115489515A (en) * 2022-08-29 2022-12-20 武汉理工大学 Automatic parking trajectory planning method for narrow vertical parking space and electronic equipment
CN116653930A (en) * 2023-07-10 2023-08-29 重庆邮电大学 Path planning method for multiple parking scenes

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102007008624A1 (en) * 2007-02-22 2008-08-28 Hella Kgaa Hueck & Co. Motor vehicle i.e. train, steering method, involves forming control input signal for controller i.e. proportional integral controller, from feedback vector, and determining desired speed from plan speed, web guiding vector and variable
WO2022095814A1 (en) * 2020-11-05 2022-05-12 长沙智能驾驶研究院有限公司 Automatic vehicle reversing control method and apparatus, vehicle and storage medium
CN114523959A (en) * 2022-04-08 2022-05-24 南昌智能新能源汽车研究院 Automatic parking trajectory planning method based on vehicle kinematics model
CN115489515A (en) * 2022-08-29 2022-12-20 武汉理工大学 Automatic parking trajectory planning method for narrow vertical parking space and electronic equipment
CN116653930A (en) * 2023-07-10 2023-08-29 重庆邮电大学 Path planning method for multiple parking scenes

Also Published As

Publication number Publication date
CN117922548B (en) 2024-07-16

Similar Documents

Publication Publication Date Title
CN111273551B (en) Double-steering wheel automatic guided vehicle trajectory tracking control method and system
Ollero et al. Stability analysis of mobile robot path tracking
US20100114338A1 (en) Multi-goal path planning of welding robots with automatic sequencing
Zhang et al. Automatic vehicle parallel parking design using fifth degree polynomial path planning
CN108153310B (en) Mobile robot real-time motion planning method based on human behavior simulation
CN114967676A (en) Model prediction control trajectory tracking control system and method based on reinforcement learning
CN111930112A (en) Intelligent vehicle path tracking control method and system based on MPC
CN113467470B (en) Trajectory tracking control method of unmanned autonomous trolley
CN111752274A (en) Laser AGV path tracking control method based on reinforcement learning
Yue et al. Path tracking control of skid-steered mobile robot on the slope based on fuzzy system and model predictive control
Kim et al. Model predictive control method for autonomous vehicles using time-varying and non-uniformly spaced horizon
CN117922548B (en) Automatic parking track planning method based on model predictive control
Bacha et al. Autonomous vehicle path tracking using nonlinear steering control and input-output state feedback linearization
Huang Control approach for tracking a moving target by a wheeled mobile robot with limited velocities
CN110562245B (en) Automatic parking space searching path following control method and system, storage medium and vehicle
CN115675459A (en) Intelligent vehicle and control method, device and storage medium for path tracking of intelligent vehicle
CN114879698A (en) Robot-driven vehicle obstacle avoidance method based on improved artificial potential field and MPC
Huang et al. Dynamic model construction and simulation based on intelligent driving vehicle
CN117163050B (en) Predictive control algorithm based on motion model
Sasiadek et al. Path tracking of an autonomous LHD articulated vehicle
Pérez-Morales et al. Information-theoretic sensor-based predictive control for autonomous vehicle navigation: A proof of concept
CN116578100B (en) Two-degree-of-freedom power model edge cleaning control method based on brushing error
Sun et al. Urban Pilot Motion Planning and Control Deployment Via Real-Time Multi-Core Multi-Thread Prototyping
Naya et al. Real-time multi-body formulation for virtual-reality-based design and evaluation of automobile controllers
CN118131628A (en) Mobile robot tracking control method based on multi-target point information fusion

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