CN113433947B - Intersection trajectory planning and control method based on obstacle vehicle estimation and prediction - Google Patents

Intersection trajectory planning and control method based on obstacle vehicle estimation and prediction Download PDF

Info

Publication number
CN113433947B
CN113433947B CN202110801915.2A CN202110801915A CN113433947B CN 113433947 B CN113433947 B CN 113433947B CN 202110801915 A CN202110801915 A CN 202110801915A CN 113433947 B CN113433947 B CN 113433947B
Authority
CN
China
Prior art keywords
vehicle
model
state
obstacle
unmanned vehicle
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
CN202110801915.2A
Other languages
Chinese (zh)
Other versions
CN113433947A (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.)
Tianjin University
Original Assignee
Tianjin 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 Tianjin University filed Critical Tianjin University
Priority to CN202110801915.2A priority Critical patent/CN113433947B/en
Publication of CN113433947A publication Critical patent/CN113433947A/en
Application granted granted Critical
Publication of CN113433947B publication Critical patent/CN113433947B/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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • 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/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention discloses an intersection track planning and control method based on obstacle vehicle estimation and prediction, which mainly comprises the following steps: establishing an unmanned vehicle dynamics model, an unmanned vehicle kinematics model, and a movement model and a measurement model of the obstacle vehicle; preliminary prediction of obstacle vehicle trajectory: planning the track of the unmanned vehicle; dynamically decoupling the unmanned vehicle dynamics model; and designing an active disturbance rejection trajectory tracking controller. The method aims at estimating and predicting the motion trail of the barrier vehicle and the motion state of the barrier vehicle in a future period of time on the basis of the known kinematics model of the unmanned vehicle, the barrier vehicle and the road model of the intersection when the unmanned vehicle runs in the intersection containing other traffic participants, completes the trajectory planning on the basis, and finally uses the active disturbance rejection controller based on the dynamic decoupling of the model to track the trajectory, comprehensively considers the performance indexes of the rapidity of the trajectory planning, the accuracy of the trajectory tracking and the like, and provides an efficient and reliable running scheme for the unmanned vehicle to pass through the intersection.

Description

Intersection trajectory planning and control method based on obstacle vehicle estimation and prediction
Technical Field
The invention relates to the state prediction of barrier vehicles, the trajectory planning and the trajectory tracking control of unmanned vehicles, designs an intersection trajectory planning and control algorithm based on the real-time estimation and prediction of barrier vehicles by taking the passing of the unmanned vehicles in the intersection environment as the background and predicting the motion state of the barrier vehicles in a period of time in the future, is suitable for the unmanned vehicles to rapidly and safely pass through intersections in complex scenes, and belongs to the field of trajectory planning;
background
The unmanned vehicle can efficiently and quickly respond to various complex road conditions, effectively avoids dangerous driving behaviors such as drunk driving and fatigue driving on the basis of ensuring safety, and can also effectively improve the road utilization rate; however, various interferences and constraints exist in an actual road traffic system, and vehicles need to be accurately judged, decided and controlled in the complex scenes to ensure driving safety and traffic efficiency; especially in the intersection, the complexity of the scene is particularly prominent; the intersection collects traffic flows from all directions and is a place where traffic jam and traffic accidents occur frequently; different from a common one-dimensional driving scene, a two-dimensional traffic flow needs to be considered in an intersection scene, and the road geometry is complex, so that the safety is higher;
the control problem of the unmanned vehicle in a complex scene can be divided into two parts, namely trajectory planning and trajectory tracking control; aiming at the problem of trajectory planning, firstly, the obstacle avoidance problem aiming at the obstacle vehicle needs to be considered; in the actual running process of the vehicle, the real-time states of other traffic participants can be obtained through the measurement of the sensor, but the motion states of the other traffic participants in a future period of time cannot be directly obtained; therefore, how to accurately and reliably predict the track of the obstacle vehicle is a key problem of ensuring the road traffic safety and lightening the road congestion and is one of the important and difficult points of track planning; secondly, attention should be paid to avoid collision with roads at intersections in the process of trajectory planning, so how to add road collision constraint in the trajectory planning is also one of important research points; after the expected track is obtained based on the above conditions, the vehicle needs to control the advancing direction, speed and the like of the vehicle by sending a control instruction to an execution mechanism of the vehicle, so that the vehicle can accurately and stably track the expected track to run; however, the vehicle model has the characteristics of strong coupling and strong nonlinearity, how to handle the coupling between model states, and how to design a controller to quickly, accurately and stably complete a tracking task in a complex actual driving scene is also the key point of track tracking control research; therefore, under the condition of considering multiple performance requirements such as obstacle avoidance and collision avoidance, rapid planning, trajectory tracking precision and the like, in order to realize rapid and safe driving of the unmanned vehicle at the intersection, the research on the obstacle vehicle state prediction method and the unmanned vehicle trajectory planning and trajectory tracking method has important theoretical significance and practical significance;
disclosure of Invention
In the process of driving at the intersection, the unmanned vehicle mainly aims to avoid other traffic participants and safely and quickly pass through the intersection; in order to ensure the driving safety of the unmanned vehicle, a reasonable driving track needs to be planned for the unmanned vehicle according to the motion states of other traffic participants and the road condition of the intersection, and the unmanned vehicle can not collide with other traffic participants and the road edge of the intersection; in order to meet the actual situation, the motion state of the obstacle vehicle is assumed to be unknown in the research, so that the motion state of the obstacle vehicle in a future period of time needs to be estimated, and meanwhile, road collision constraint needs to be added in the trajectory planning, which undoubtedly increases the difficulty of the trajectory planning; in order to ensure that the unmanned vehicle can quickly pass through the intersection to avoid congestion, a reasonable control scheme needs to be designed so that the unmanned vehicle can accurately and quickly travel along a planned path;
aiming at the prior art, the invention aims to provide a safe and quick driving scheme for unmanned vehicles to drive in intersections containing other traffic participants, namely an intersection track planning and control algorithm based on real-time estimation and prediction of barrier vehicles; on the basis that a kinematics model of the unmanned vehicle and the barrier vehicle and a road model of the intersection are known, the motion trail of the barrier vehicle and the motion state in a period of time in the future are estimated and predicted, the trail planning is completed on the basis, finally, the trail tracking is performed by using an active disturbance rejection controller based on model dynamic decoupling, the performance indexes of rapidity of the trail planning, accuracy of the trail tracking and the like are comprehensively considered, and an efficient and reliable running scheme is provided for the unmanned vehicle to pass through the intersection.
In order to solve the technical problem, the invention provides an intersection trajectory planning and control method based on obstacle vehicle estimation and prediction, which mainly comprises the following steps:
firstly, establishing an unmanned vehicle dynamic model, an unmanned vehicle kinematic model, and a motion model and a measurement model of an obstacle vehicle;
step two, preliminary prediction of the obstacle vehicle track:
step three, unmanned vehicle track planning;
step four, dynamically decoupling the unmanned vehicle dynamic model;
designing an active disturbance rejection trajectory tracking controller;
the invention comprehensively considers the performance indexes of rapidity of track planning, accuracy of track tracking and the like, thereby providing a safe, rapid, efficient and reliable driving scheme for unmanned vehicles when the unmanned vehicles drive in intersections containing other traffic participants.
The first step comprises the following specific contents:
1) the unmanned vehicle kinematics model with a sampling time T is as follows:
Figure BDA0003164966350000021
in the formula (1), (x, y) represents the position of the unmanned vehicle under a ground inertial coordinate system, v is the speed of the mass center of the vehicle, and an included angle beta exists between the direction of the speed and the advancing direction of the vehicle, which is called as a vehicle sideslip angle;
Figure BDA0003164966350000022
representing the yaw angle, delta, of the vehiclefRepresenting a front wheel slip angle; lfAnd lrFor the vehicle wheelbase, /)wIs the width of the vehicle body; selecting
Figure BDA0003164966350000023
Is a state variable, u ═ a δf]TIs a control variable; formula (1) is represented as:
Figure BDA0003164966350000024
2) the unmanned vehicle dynamics model is as follows:
Figure BDA0003164966350000031
in the formula (2), vxAnd vyAcceleration in a direction corresponding to longitudinal speed and lateral speed of vehiclexAnd ayRepresents; v is the resultant velocity in the longitudinal and transverse directions, α is the tire slip angle, Fl·,*And Fc·,*Respectively representing the longitudinal force and the lateral force of the tire, wherein ∈ { f, r } represents the front and rear axes of the vehicle, and ∈ { l, r } represents the left and right tires of the vehicle; m is the mass of the unmanned vehicle, IzRepresents the moment of inertia of the unmanned vehicle; selecting
Figure BDA0003164966350000032
Is a state variable, ud＝[δf Flf Flr]TIs a control variable;
3) the barrier vehicle has two motion states, which are respectively: the barrier vehicle performs uniform acceleration movement on a straight road and performs cooperative turning movement at a turning position; the motion model of the obstacle vehicle comprises a uniform acceleration motion model and a cooperative turning motion model;
selecting the state variable as
Figure BDA0003164966350000033
Assuming that the sampling period is T, the uniform acceleration motion model in a discrete form is obtained as follows:
Figure BDA0003164966350000034
in the formula (3), wCA(k) Is white Gaussian noise with a covariance matrix of
Figure BDA0003164966350000035
If the speed of the turning angle of the obstacle vehicle is constant omega0The uniform-speed circular motion of the circular motion device,the discrete form cooperative turning motion model is obtained as follows:
Figure BDA0003164966350000036
in the formula (4), wCT(k) Is white Gaussian noise with a covariance matrix of
Figure BDA0003164966350000041
4) In order to obtain the observed quantity of the motion state of the obstacle vehicle, the observed quantity is taken as z ═ x y v]TObtaining a measurement model of the obstacle vehicle as follows:
Figure BDA0003164966350000042
is in (5), wob(k) For measured white Gaussian noise, the covariance matrix is
Figure BDA0003164966350000043
The second step comprises the following specific contents:
1) the method comprises the following steps of adopting a cubature Kalman filtering algorithm to preliminarily predict the state of the unmanned vehicle, and obtaining a state estimation value and a covariance matrix as follows:
Figure BDA0003164966350000044
in the formula (6), the reaction mixture is,
Figure BDA0003164966350000045
the optimal estimated value at the k moment is obtained;
Figure BDA0003164966350000046
the step one predicted value at the kth moment is taken as a step one predicted value; w (k) is the volumetric Kalman filter gain; z (k) is the observed quantity in the measurement model;
Figure BDA0003164966350000047
predicting the measurement value; p (k | k-1) is a covariance matrix of the one-step predicted value at the kth moment; pzz(k | k-1) is the error covariance matrix of the measured values; p (k | k) is the covariance matrix of the state vector;
2) setting: making the barrier vehicle make uniform acceleration linear motion on a straight-going lane, making cooperative turning motion at an intersection, and making the turning direction known, obtaining a state estimation value of the barrier vehicle through a cubature Kalman filtering algorithm, and substituting the state estimation value into a corresponding formula (3) or (4) to obtain a prediction time domain NpModel prediction value of the state of the internal obstacle vehicle;
introducing error correction factors
Figure BDA0003164966350000048
Correcting the error of the model predicted value to predict the time domain NpThe predicted value of the state of the internal obstacle vehicle is as follows:
Figure BDA0003164966350000049
Figure BDA00031649663500000410
in the formulae (7) and (8),
Figure BDA00031649663500000411
and
Figure BDA00031649663500000412
calculated for the kth moment
Figure BDA00031649663500000413
The actual predicted value and the model predicted value of the state of the obstacle vehicle at the moment,
Figure BDA00031649663500000414
and
Figure BDA00031649663500000415
for the actual predicted value of the acceleration and the predicted value of the model,
Figure BDA00031649663500000416
in order to be an acceleration estimation value,
Figure BDA00031649663500000417
predicting an error for the acceleration;
Figure BDA00031649663500000418
the error coefficient is used for measuring the magnitude of the acceleration prediction error, and the selection is related to the acceleration of the obstacle vehicle;
Figure BDA00031649663500000419
and
Figure BDA00031649663500000420
the value of (a) is related to the acceleration of the obstacle vehicle and the predicted time, and as the time is prolonged, the influence of the error of the k time on the subsequent time is smaller and smaller, so that the error of the k time has smaller influence on the subsequent time
Figure BDA0003164966350000051
And
Figure BDA0003164966350000052
value of and
Figure BDA0003164966350000053
in inverse proportion, the proportionality coefficient is selected according to the actual situation and is generally in the interval (0, 10);
and obtaining a state predicted value of the obstacle vehicle i in the prediction time domain through the correction:
Figure BDA0003164966350000054
the third step comprises the following specific contents:
a Model predictive adaptive dynamic programming control (MPADP) algorithm is used to solve the trajectory planning, and the contents are as follows:
1) firstly, obtaining an infinite time domain objective function at each moment based on dynamic programming and a Bellman optimality principle; then, considering the objective functions of all the moments in the prediction time domain, overlapping all infinite time domain objective functions of the prediction time domain to obtain a stacked objective function, which is used as an objective function of the model prediction adaptive dynamic programming control algorithm and is expressed as follows:
Figure BDA0003164966350000055
in the formula (10), V (k + i | k) is the ith objective function predicted at the kth time;
2) designing a parameter approximator for approximating the target function of each prediction moment, wherein the parameter approximator is as follows:
V(k+i|k)＝wT(k+i|k)φ(ξ(k+i|k),u(k+i|k)),i＝1,...,Np (11)
in the formula (11), w is a parameter vector, and phi represents a regression quantity; ξ (k + i | k) and u (k + i | k) are respectively the ith state quantity and the control quantity predicted at the kth moment;
in an evaluation module of a model prediction adaptive dynamic programming control algorithm, aiming at the deviation of performance indexes, the following optimization problems are designed:
Figure BDA0003164966350000056
in formula (12):
Figure BDA0003164966350000061
solving the optimization problem to obtain the optimal parameter vector w*(. k); will w*Substituting (-) k into the formula (12) to obtain the optimization problem of the execution module;
model prediction adaptive dynamic programming control algorithm and obstacle vehicle track predictionThe combination of the detection algorithms, under the condition that the driving state of the obstacle vehicle is unknown, when the unmanned vehicle detects the obstacle vehicle, the obstacle vehicle is predicted in the time domain NpThe position and the speed of the inner part are predicted online;
the optimization problem of the execution module for the straight road section is as follows:
Figure BDA0003164966350000062
in the formula (14), NcIs a control time domain; ξ (k + i | k) and u (k + i | k) are respectively the ith state quantity and the control quantity predicted at the kth moment; a isminAnd amaxUpper and lower bounds of acceleration respectively; n is a radical ofobsThe number of the obstacle vehicles; when detecting the range ddetWhen the vehicle is in obstacle, the upper and lower limits of the acceleration are widened to
Figure BDA0003164966350000063
And
Figure BDA0003164966350000064
δfminand deltafmaxThe upper and lower limits of the front wheel deflection angle are respectively; Δ uminAnd Δ umaxRespectively the upper and lower bounds of the control increment delta u; dsafeIs a safe distance; j is 1,3, dsafeThe value of (A) is related to the position of the unmanned vehicle, chi1,χ3A set of straight-going states is represented,
Figure BDA0003164966350000065
indicating unmanned vehicle and obstacle vehicle
Figure BDA0003164966350000066
The relative distance of the estimated position of (a),
Figure BDA0003164966350000067
indicating unmanned vehicle and obstacle vehicle
Figure BDA0003164966350000068
The relative distance of the predicted position of (2), the calculation formulaThe following were used:
Figure BDA0003164966350000069
Figure BDA0003164966350000071
for the intersection road section, the optimization problem is as follows:
Figure BDA0003164966350000072
in the formula (17), foutAnd finRespectively are nonlinear functions for avoiding collision with the outer edge and the inner edge of the road; b is the lane width, and R is a radius of a quarter circular arc at the joint of the straight road and the intersection; chi shape2Representing a set of unmanned vehicle turning states;
the specific contents included in the step four are as follows:
regarding the dynamic coupling action among the output quantities as an Extended state, observing by using an Extended State Observer (ESO), and feeding back the ESO to the controller for compensation;
aiming at the unmanned vehicle dynamics model shown in the formula (2), the output quantity y is selectedd＝[x y v]TThe dynamic model is a coupling system with 3-dimensional input and 3-dimensional output; simplifying the model by adopting a small angle hypothesis to obtain a simplified model:
Figure BDA0003164966350000073
let u1＝Flf，u2＝δf，u3＝FlrThen, the nonlinear model described by equation (8) is expressed as:
Figure BDA0003164966350000081
in the formula (19), the compound represented by the formula (I),
Figure BDA0003164966350000082
and f3(ξd,ud) Expressed as a non-linear function, as follows:
Figure BDA0003164966350000083
b11，b22and b33Input gain for the corresponding sub-object:
Figure BDA0003164966350000084
through the decoupling process, an Extended State Observer (ESO) is designed to estimate the actual state and the Extended state of the child object as follows:
Figure BDA0003164966350000085
Figure BDA0003164966350000086
Figure BDA0003164966350000087
in the formulae (22) to (24), h is an integration step, βx1，βx2，βx3，βy1，βy2，βy3，βv1And betav2Is the gain factor, δx，δy，δv，αx1，αx2，αy1，αy2，αv1And alphav2Is an adjustable parameter; using the ESO of the state quantity x as an example, the function fal (e)x(k),αx1,δx) Is defined as:
Figure BDA0003164966350000091
the concrete contents included in the step five are as follows:
1) designing an active disturbance rejection controller: the system comprises a Tracking differential controller (TD) of longitudinal and transverse positions and speeds, an extended state observer and a Nonlinear error feedback control law (NLSEF); wherein said tracking derivative controller arranges a transition to produce a smoothed input signal and an input derivative signal; the extended state observer estimates the state and internal and external disturbances of the system through the control quantity and the output quantity; the nonlinear error feedback control law obtains a controlled variable by nonlinear combination of the error between the output of the tracking differential controller and the observed value of the extended state observer;
2) taking the longitudinal state quantity x as an example, the mathematical model of the tracking differential controller is as follows:
Figure BDA0003164966350000092
in the formula (26), r0For fast factors, h is the integration step, xr(k) For the input signal, i.e. the longitudinal reference value, x1(k) Tracking the input signal, x2(k) To track the rate of change of the input signal; function fhan (x)1,x2,r0And h) is defined as:
Figure BDA0003164966350000093
the mathematical expression for the nonlinear error feedback control law is as follows:
Figure BDA0003164966350000101
in the formula (28), the reaction mixture is,
Figure BDA0003164966350000102
kx1and kx2Is a parameter to be set; u. ofx(k) For the actual controlled variable, the controlled variable u is fed back by an error0x(k) And disturbance estimate zx3Obtaining the compensation; the optimal parameters are obtained through continuous testing, and the control effect with strong robustness can be obtained. Similarly, the mathematical model of the tracking differential controller of the transverse state quantity y and the velocity state quantity v and the mathematical expression of the nonlinear error feedback control law can be obtained according to the method.
Compared with the prior art, the invention has the beneficial effects that:
the prediction method provided by the invention can accurately predict the track and the speed of the barrier vehicle, can accurately reflect the future movement trend of the barrier vehicle, can ensure that no collision exists between the unmanned vehicle and the road edge and between the unmanned vehicle and the barrier vehicle, shows that the barrier vehicle state prediction algorithm can provide reliable prediction information for the unmanned vehicle, ensures that the unmanned vehicle can successfully avoid the barrier in the track planning process, and has good control effect and small tracking error. Therefore, the intersection trajectory planning and control algorithm based on real-time estimation and prediction of the obstacle vehicles has feasibility.
Drawings
FIG. 1 is a flow chart of a control system implementing the method of the present invention;
FIG. 2 is a block diagram of an active disturbance rejection controller in the method of the present invention;
FIG. 3 is a schematic view of an intersection scene involved in the present invention;
FIG. 4 is a diagram of the results of a trajectory planning simulation according to an embodiment.
Detailed Description
The invention belongs to the field of trajectory planning, and the design idea is as follows: the method mainly comprises the steps of predicting the state of a barrier vehicle, planning the trajectory of the unmanned vehicle and tracking and controlling the trajectory, so that the unmanned vehicle can rapidly and safely pass through the intersection in a complex scene;
the invention will be further described with reference to the accompanying drawings and specific examples, which are not intended to limit the invention in any way;
the unmanned vehicle can improve driving safety and reduce labor of human in driving. Because the vehicle-mounted sensor cannot predict the driving intention of the obstacle vehicle and cannot obtain the motion state of the obstacle vehicle in a period of time in the future, the method adopts the cubature Kalman filtering to estimate the motion state of the obstacle vehicle, designs an error correction strategy, and completes multi-step prediction of the state by combining with a kinematics model of the obstacle vehicle. And then planning the track of the unmanned vehicle based on the obtained motion track prediction result. Because the unmanned vehicle dynamics model has the characteristics of strong coupling and nonlinearity, the invention adopts a dynamic decoupling method to decouple the model and designs the trajectory tracking controller by utilizing an active disturbance rejection algorithm. The control system flow chart of the invention is shown in fig. 1, and the specific process is as follows:
1. model building
The invention relates to a model with a unmanned vehicle dynamics model, an unmanned vehicle kinematics model, and a motion model and a measurement model of an obstacle vehicle. First, an intersection scene is introduced. FIG. 3 is a schematic view of a cross road scene studied by the present invention, with a lane width of b, and each lane sequentially
Figure BDA0003164966350000111
The nomenclature is used. The junction of the straight road and the intersection is a quarter circular arc with the radius of R, O (n)1,n2) Is the center of the arc. The present invention assumes that the vehicle can turn left, right, and go straight at the intersection without any restriction. R1，R2，R3And R4Is a point located on the center line of the lane, where R1R2And R3R4Representing straight road sections, R2R3Representing an intersection road segment.
The invention adopts an unmanned vehicle kinematics model with sampling time T as follows:
Figure BDA0003164966350000112
Figure BDA0003164966350000113
v(k+1)＝v(k)+T·a(k)
Figure BDA0003164966350000114
in the above formula, (x, y) represents the position of the unmanned vehicle in the ground inertial coordinate system, v is the speed at the centroid of the vehicle, and the direction of v is an included angle β with the advancing direction of the vehicle, which is called the vehicle sideslip angle.
Figure BDA0003164966350000115
Representing the yaw angle, delta, of the vehiclefIndicating the front wheel slip angle. lfAnd lrFor the vehicle wheelbase, /)wIs the width of the vehicle body. Selecting
Figure BDA0003164966350000116
Is a state variable, u ═ a δf]TAre control variables. Thus, the above formula can be expressed as:
Figure BDA0003164966350000117
the kinetic model of the unmanned vehicle is described as:
Figure BDA0003164966350000118
Figure BDA0003164966350000119
Figure BDA00031649663500001110
Figure BDA00031649663500001111
Figure BDA00031649663500001112
wherein v isxAnd vyAcceleration in a direction corresponding to longitudinal speed and lateral speed of vehiclexAnd ayAnd (4) showing. v is the resultant velocity in the longitudinal and transverse directions, α is the tire slip angle, Fl·,*And Fc·,*Represents the longitudinal and lateral forces of the tire, respectively, where ∈ { f, r } represents the vehicle front-rear axis, and ∈ { l, r } represents the vehicle left and right tires. m is the mass of the unmanned vehicle, IzRepresenting the moment of inertia of the unmanned vehicle. Selecting
Figure BDA00031649663500001113
Is a state variable, ud＝[δf Flf Flr]TAre control variables.
In the designed scene, the obstacle vehicle has two motion states, namely uniform acceleration motion on a straight road and cooperative turning motion at a turning position. Selecting the state variable as
Figure BDA0003164966350000121
And setting the sampling period as T to obtain a uniform acceleration motion model in a discrete form:
Figure BDA0003164966350000122
wherein, wCA(k) Is white Gaussian noise with a covariance matrix of
Figure BDA0003164966350000123
The cooperative turning model assumes that the speed of the turning angle of the obstacle vehicle is constant omega0The discrete form cooperative turning motion model is as follows:
Figure BDA0003164966350000124
wherein, wCT(k) Is white Gaussian noise with a covariance matrix of
Figure BDA0003164966350000125
In order to obtain the observed quantity of the motion state of the obstacle vehicle, a measurement model is required to be designed. Taking the observed quantity as z ═ x yv]TThe metrology model can be expressed as:
Figure BDA0003164966350000126
wherein, wob(k) For measured white Gaussian noise, the covariance matrix is
Figure BDA0003164966350000127
2. Multi-step prediction of obstacle vehicle trajectory
Gaussian white noise exists in the motion model and the measurement model of the obstacle vehicle, and in order to perform optimal estimation on the motion state of the obstacle vehicle, a proper filtering algorithm needs to be selected to reduce noise interference so as to obtain a signal with weak interference. Compared with the traditional Kalman filtering algorithm, the volumetric Kalman filtering algorithm can effectively process a nonlinear system with higher complexity, and compared with the extended Kalman filtering algorithm, the accuracy can be effectively improved. Therefore, the invention adopts the cubature Kalman filtering to carry out preliminary prediction on the state of the unmanned vehicle, and the state estimation value and the covariance matrix are obtained as follows:
Figure BDA0003164966350000131
P(k|k)＝P(k|k-1)-W(k)Pzz(k|k-1)WT(k)
wherein the content of the first and second substances,
Figure BDA0003164966350000132
is the optimal estimated value of the k time.
Figure BDA0003164966350000133
And the predicted value is a one-step predicted value at the k-th moment. W (k) is the volumetric Kalman filter gain. z (k) is the observed quantity in the measurement model.
Figure BDA0003164966350000134
The measured value is a predicted value. P (k | k-1) is the covariance matrix of the one-step predictor at time k. Pzz(k | k-1) is the error covariance matrix of the measured values. P (k | k) is the covariance matrix of the state vector.
Assuming that the obstacle vehicle makes uniform acceleration linear motion on a straight lane and makes cooperative turning motion at an intersection, and the turning direction is known, the obstacle vehicle needs to select a corresponding filter model and a prediction model according to the position of the obstacle vehicle, obtains an obstacle vehicle state estimation value through a cubature Kalman filter algorithm, substitutes the state estimation value into the obstacle vehicle motion model, makes uniform acceleration linear motion on the straight lane into the discrete uniform acceleration motion model, and makes cooperative turning motion at the intersection into the discrete cooperative turning motion model, so that a prediction time domain N can be obtainedpAnd predicting the model of the state of the internal obstacle vehicle. However, due to the existence of interference, the model-based prediction method cannot accurately reflect the future movement track of the unmanned vehicle. Further, since the unmanned vehicle cannot directly observe the acceleration of the obstacle vehicle in the prediction time range, the error of the obstacle vehicle state prediction is mainly caused by the acceleration error. Aiming at the problem, the invention designs an error correction strategy, and introduces an error correction factor
Figure BDA0003164966350000135
Correcting the error of the model predicted value to obtainAnd the relatively reliable obstacle vehicle state prediction value is obtained.
Figure BDA0003164966350000136
Figure BDA0003164966350000137
Wherein the content of the first and second substances,
Figure BDA0003164966350000138
and
Figure BDA0003164966350000139
calculated for the kth moment
Figure BDA00031649663500001310
The actual predicted value and the model predicted value of the state of the obstacle vehicle at the moment,
Figure BDA00031649663500001311
and
Figure BDA00031649663500001312
for the actual predicted value of the acceleration and the predicted value of the model,
Figure BDA00031649663500001313
in order to be an acceleration estimation value,
Figure BDA00031649663500001314
the error is predicted for acceleration.
Figure BDA00031649663500001315
The error coefficient is used for measuring the magnitude of the acceleration prediction error.
Figure BDA00031649663500001316
And
Figure BDA00031649663500001317
the value of (b) is related to the acceleration of the obstacle vehicle and should be selected according to actual conditions.
After correction, the state prediction value of the obstacle vehicle i in the prediction time domain can be obtained
Figure BDA00031649663500001318
Figure BDA00031649663500001319
3. Unmanned vehicle trajectory planning
The invention provides a Model predictive adaptive dynamic programming control (MPADP) algorithm for solving a trajectory planning problem. Firstly, based on dynamic programming and a Bellman optimality principle, an infinite time domain objective function at each moment is obtained. Then, considering the target functions of all the moments in the prediction time domain, overlapping all the infinite time domain target functions of the prediction time domain to obtain a stacked target function as a target function of an MPADP algorithm. Is represented as follows:
Figure BDA0003164966350000141
wherein, V (k + i | k) is the ith objective function predicted at the kth time.
Designing a parameter approximator to approximate the target function of each prediction moment, wherein the parameter approximator is as follows:
V(k+i|k)＝wT(k+i|k)φ(ξ(k+i|k),u(k+i|k)),i＝1,...,Np
wherein w is a parameter vector and phi represents a regression quantity. ξ (k + i | k) and u (k + i | k) are the ith state quantity and the controlled quantity predicted at the kth time, respectively.
In an evaluation module of the MPADP algorithm, aiming at the deviation of performance indexes, the following optimization problems are designed:
Figure BDA0003164966350000142
s.t.wT(k+i|k)φ(ξ(k+i|k),u(k+i|k-1))＞0
Figure BDA0003164966350000143
wherein:
Figure BDA0003164966350000144
solving the optimization problem to obtain the optimal parameter vector w*(. k). Will w*And (k) is substituted into the parameter approximator to obtain the optimization problem of the execution module.
The MPADP algorithm is combined with the obstacle trajectory prediction algorithm. Under the condition that the driving state of the obstacle vehicle is unknown, when the unmanned vehicle detects the obstacle vehicle, the obstacle vehicle can be predicted in the time domain NpThe position and velocity within the vessel are predicted online.
The optimization problem of the execution module for the straight road section is as follows:
Figure BDA0003164966350000151
Figure BDA0003164966350000152
Figure BDA0003164966350000153
Figure BDA0003164966350000154
Figure BDA0003164966350000155
Figure BDA0003164966350000156
Figure BDA0003164966350000157
wherein N iscTo control the time domain. ξ (k + i | k) and u (k + i | k) are the ith state quantity and the controlled quantity predicted at the kth time, respectively. a isminAnd amaxRespectively the upper and lower bounds of the acceleration. N is a radical ofobsThe number of obstacle vehicles. When detecting the range ddetWhen the vehicle is in obstacle, the upper and lower limits of the acceleration are widened to
Figure BDA0003164966350000158
And
Figure BDA0003164966350000159
δfminand deltafmaxRespectively the upper and lower bounds of the front wheel slip angle. Δ uminAnd Δ umaxRespectively the upper and lower bounds of the control increment delta u; dsafeIs a safe distance; j is 1,3, dsafeThe value of (A) is related to the position of the unmanned vehicle, chi1,χ3A set of straight-going states is represented,
Figure BDA00031649663500001510
indicating unmanned vehicle and obstacle vehicle
Figure BDA00031649663500001511
The relative distance of the estimated position of (a),
Figure BDA00031649663500001512
indicating unmanned vehicle and obstacle vehicle
Figure BDA00031649663500001513
The relative distance between the predicted positions is calculated as follows:
Figure BDA00031649663500001514
Figure BDA00031649663500001515
for the intersection road section, the optimization problem is as follows:
Figure BDA00031649663500001516
Figure BDA00031649663500001517
Figure BDA00031649663500001518
Figure BDA00031649663500001519
Figure BDA0003164966350000161
Figure BDA0003164966350000162
Figure BDA0003164966350000163
Figure BDA0003164966350000164
Figure BDA0003164966350000165
wherein f isoutAnd finRespectively, non-linear functions for avoiding collision with the outer edge and the inner edge of the road. Chi shape2Representing a set of unmanned vehicle turning conditions.
4. Dynamic decoupling of unmanned vehicle dynamics models
Because the dynamic model of the unmanned vehicle has strong coupling, the dynamic coupling effect between the output quantities is regarded as an Extended state, an Extended State Observer (ESO) is adopted for observation, and the ESO is fed back to the controller for compensation. Selecting output quantity y aiming at the unmanned vehicle dynamics modeld＝[x y v]TThe dynamic model is a coupling system with 3-dimensional input and 3-dimensional output. Then simplifying the model by adopting a small angle hypothesis to obtain a simplified model:
Figure BDA0003164966350000166
Figure BDA0003164966350000167
Figure BDA0003164966350000168
let u1＝Flf，u2＝δf，u3＝FlrThen, the above nonlinear model can be expressed as:
Figure BDA0003164966350000169
wherein the content of the first and second substances,
Figure BDA00031649663500001610
and f3(ξd,ud) Expressed as a non-linear function, as follows:
Figure BDA00031649663500001611
Figure BDA00031649663500001612
Figure BDA00031649663500001613
b11，b22and b33Input gain for the corresponding sub-object:
Figure BDA0003164966350000171
Figure BDA0003164966350000172
Figure BDA0003164966350000173
through the decoupling process, the following ESO estimation sub-object actual state and expansion state are designed:
Figure BDA0003164966350000174
Figure BDA0003164966350000175
Figure BDA0003164966350000176
where h is the integration step, βx1，βx2，βx3，βy1，βy2，βy3，βv1And betav2Is the gain factor, δx，δy，δv，αx1，αx2，αy1，αy2，αv1And alphav2Is an adjustable parameter. Using the ESO of the state quantity x as an example, the function fal (e)x(k),αx1,δx) Is defined as:
Figure BDA0003164966350000177
5. auto-disturbance rejection trajectory tracking controller design
The basic structure of the active disturbance rejection controller designed by the invention is shown in fig. 2, and the controller comprises a Tracking differential controller (TD) for longitudinal and transverse position and speed, an ESO (electronic stability automation) and a Nonlinear error feedback control law (NLSEF). Wherein the TD arranges for a transition to produce a smoothed input signal and an input differential signal; the ESO estimates the state and internal and external disturbance of the system through the control quantity and the output quantity; the NLSEF obtains a control quantity by nonlinear combination of errors between the output of the TD and the observed value of the ESO. Among them, ESO has been described above. Taking the longitudinal state quantity x (when i is 1 in fig. 2) as an example, the mathematical model of TD in the active disturbance rejection controller is as follows:
Figure BDA0003164966350000181
wherein r is0For fast factors, h is the integration step, xr(k) For the input signal, i.e. the longitudinal reference value, x1(k) Tracking the input signal, x2(k) To track the rate of change of the input signal. Function fhan (x)1,x2,r0And h) is defined as:
Figure BDA0003164966350000182
the mathematical expression of NLSEF is as follows:
Figure BDA0003164966350000183
wherein the content of the first and second substances,
Figure BDA0003164966350000184
kx1and kx2Is a parameter to be set. u. ofx(k) For the actual controlled variable, the controlled variable u is fed back by an error0x(k) And disturbance estimate zx3The compensation of (2) to obtain. And the control effect with stronger robustness can be obtained by adjusting the parameters. The mathematical model of the tracking differential controller of the transverse state quantity y and the velocity state quantity v and the mathematical expression of the nonlinear error feedback control law can be obtained by the skilled person according to the method, and are not described herein again.
The implementation mode completes the processes of model establishment, algorithm verification and the like through program compiling, and verifies the effectiveness of the provided barrier vehicle prediction algorithm in the intersection scene trajectory planning and trajectory tracking through a large amount of data; in the invention, the test is carried out by taking an unmanned vehicle running at the intersection and avoiding two obstacle vehicles as backgrounds; after the data processing is completed, the result is shown in fig. 4; fig. 4 is a diagram of a result of trajectory planning, which includes an overall trajectory diagram of the unmanned vehicle, a partially enlarged diagram at an intersection, and a schematic diagram of a distance between the unmanned vehicle and an obstacle vehicle, i.e., a sub-diagram (a).
Simulation results show that the prediction method provided by the invention can accurately predict the track and the speed of the obstacle vehicle, can accurately reflect the future movement trend of the obstacle vehicle, can ensure that no collision exists between the unmanned vehicle and the road edge and between the unmanned vehicle and the obstacle vehicle, shows that the obstacle vehicle state prediction algorithm can provide reliable prediction information for the unmanned vehicle, ensures that the unmanned vehicle successfully avoids obstacles in the track planning process, and has good control effect and small tracking error. The intersection track planning and control algorithm based on real-time estimation and prediction of the obstacle vehicles has feasibility.
While the present invention has been described with reference to the accompanying drawings, the present invention is not limited to the above-described embodiments, which are illustrative only and not restrictive, and various modifications which do not depart from the spirit of the present invention and which are intended to be covered by the claims of the present invention may be made by those skilled in the art.

Claims (1)

1. An intersection trajectory planning and control method based on obstacle vehicle estimation and prediction is characterized by comprising the following steps of:
step one, establishing an unmanned vehicle dynamics model, an unmanned vehicle kinematics model and a motion model and a measurement model of the obstacle vehicle:
1) the unmanned vehicle kinematics model with a sampling time T is as follows:
Figure FDA0003164966340000011
in the formula (1), (x, y) represents the position of the unmanned vehicle under a ground inertial coordinate system, v is the speed of the mass center of the vehicle, and an included angle beta exists between the direction of the speed and the advancing direction of the vehicle, which is called as a vehicle sideslip angle;
Figure FDA0003164966340000012
representing the yaw angle, delta, of the vehiclefRepresenting a front wheel slip angle; lfAnd lrFor the vehicle wheelbase, /)wIs the width of the vehicle body; selecting
Figure FDA0003164966340000013
Is a state variable, u ═ a δf]TIs a control variable; formula (1) is represented as:
Figure FDA0003164966340000014
2) the unmanned vehicle dynamics model is as follows:
Figure FDA0003164966340000015
in the formula (2), vxAnd vyAcceleration in a direction corresponding to longitudinal speed and lateral speed of vehiclexAnd ayRepresents; v is the resultant velocity in the longitudinal and transverse directions, α is the tire slip angle, Fl·,*And Fc·,*Respectively representing the longitudinal force and the lateral force of the tire, wherein ∈ { f, r } represents the front and rear axes of the vehicle, and ∈ { l, r } represents the left and right tires of the vehicle; m is the mass of the unmanned vehicle, IzRepresents the moment of inertia of the unmanned vehicle; selecting
Figure FDA0003164966340000016
Is a state variable, ud＝[δf Flf Flr]TIs a control variable;
3) the barrier vehicle has two motion states, which are respectively: the barrier vehicle performs uniform acceleration movement on a straight road and performs cooperative turning movement at a turning position; the motion model of the obstacle vehicle comprises a uniform acceleration motion model and a cooperative turning motion model;
selecting the state variable as
Figure FDA0003164966340000017
Assuming that the sampling period is T, the uniform acceleration motion model in a discrete form is obtained as follows:
Figure FDA0003164966340000021
in the formula (3), wCA(k) Is white Gaussian noise with a covariance matrix of
Figure FDA0003164966340000022
If the speed of the turning angle of the obstacle vehicle is constant omega0The obtained discrete form cooperative turning motion model is as follows:
Figure FDA0003164966340000023
in the formula (4), wCT(k) Is white Gaussian noise with a covariance matrix of
Figure FDA0003164966340000024
4) In order to obtain the observed quantity of the motion state of the obstacle vehicle, the observed quantity is taken as z ═ x y v]TObtaining a measurement model of the obstacle vehicle as follows:
Figure FDA0003164966340000025
is in (5), wob(k) For measured white Gaussian noise, the covariance matrix is Qob(k)；
Step two, preliminary prediction of the obstacle vehicle track:
1) the method comprises the following steps of adopting a cubature Kalman filtering algorithm to preliminarily predict the state of the unmanned vehicle, and obtaining a state estimation value and a covariance matrix as follows:
Figure FDA0003164966340000026
in the formula (6), the reaction mixture is,
Figure FDA0003164966340000027
the optimal estimated value at the k moment is obtained;
Figure FDA0003164966340000028
the step one predicted value at the kth moment is taken as a step one predicted value; w (k) is the volumetric Kalman filter gain; z (k) is the observed quantity in the measurement model;
Figure FDA0003164966340000031
predicting the measurement value; p (k | k-1) is one at the k-th timeStep one, a covariance matrix of the predicted values; pzz(k | k-1) is the error covariance matrix of the measured values; p (k | k) is the covariance matrix of the state vector;
2) setting: making the barrier vehicle make uniform acceleration linear motion on a straight-going lane, making cooperative turning motion at an intersection, and making the turning direction known, obtaining a state estimation value of the barrier vehicle through a cubature Kalman filtering algorithm, and substituting the state estimation value into a corresponding formula (3) or (4) to obtain a prediction time domain NpModel prediction value of the state of the internal obstacle vehicle;
introducing error correction factors
Figure FDA0003164966340000032
Correcting the error of the model predicted value to predict the time domain NpThe predicted value of the state of the internal obstacle vehicle is as follows:
Figure FDA0003164966340000033
Figure FDA0003164966340000034
in the formulae (7) and (8),
Figure FDA0003164966340000035
and
Figure FDA0003164966340000036
calculated for the kth moment
Figure FDA0003164966340000037
The actual predicted value and the model predicted value of the state of the obstacle vehicle at the moment,
Figure FDA0003164966340000038
and
Figure FDA0003164966340000039
for the actual predicted value of the acceleration and the predicted value of the model,
Figure FDA00031649663400000310
in order to be an acceleration estimation value,
Figure FDA00031649663400000311
predicting an error for the acceleration;
Figure FDA00031649663400000312
the error coefficient is used for measuring the magnitude of the acceleration prediction error, and the selection is related to the acceleration of the obstacle vehicle;
Figure FDA00031649663400000313
Figure FDA00031649663400000314
the value of (d) is related to the acceleration of the obstacle vehicle and the predicted time, and as time goes on, the influence of the error at time k on the subsequent time is smaller and smaller, so that
Figure FDA00031649663400000315
And
Figure FDA00031649663400000316
value of and
Figure FDA00031649663400000317
inversely proportional, with the proportionality coefficient within the interval (0, 10);
and obtaining a state predicted value of the obstacle vehicle i in the prediction time domain through the correction:
Figure FDA00031649663400000318
step three, unmanned vehicle track planning
A Model predictive adaptive dynamic programming control (MPADP) algorithm is used to solve the trajectory planning, and the contents are as follows:
1) firstly, obtaining an infinite time domain objective function at each moment based on dynamic programming and a Bellman optimality principle; then, considering the objective functions of all the moments in the prediction time domain, overlapping all infinite time domain objective functions of the prediction time domain to obtain a stacked objective function, which is used as an objective function of the model prediction adaptive dynamic programming control algorithm and is expressed as follows:
Figure FDA00031649663400000319
in the formula (10), V (k + i | k) is the ith objective function predicted at the kth time;
2) designing a parameter approximator for approximating the target function of each prediction moment, wherein the parameter approximator is as follows:
V(k+i|k)＝wT(k+i|k)φ(ξ(k+i|k),u(k+i|k)),i＝1,...,Np (11)
in the formula (11), w is a parameter vector, and phi represents a regression quantity; ξ (k + i | k) and u (k + i | k) are respectively the ith state quantity and the control quantity predicted at the kth moment;
in an evaluation module of a model prediction adaptive dynamic programming control algorithm, aiming at the deviation of performance indexes, the following optimization problems are designed:
Figure FDA0003164966340000041
in formula (12):
Figure FDA0003164966340000042
solving the optimization problem to obtain the optimal parameter vector w*(. k); will w*(k) is substituted into equation (12) to obtain the executionOptimizing the module;
the model prediction self-adaptive dynamic programming control algorithm is combined with the barrier vehicle track prediction algorithm, and when the driving state of the barrier vehicle is unknown and no-man vehicle detects the barrier vehicle, the barrier vehicle is subjected to the prediction time domain NpThe position and the speed of the inner part are predicted online;
the optimization problem of the execution module for the straight road section is as follows:
Figure FDA0003164966340000043
Figure FDA0003164966340000051
in the formula (14), NcFor controlling the time domain, w is a parameter vector, and phi represents a regression quantity; ξ (k + i | k) and u (k + i | k) are respectively the ith state quantity and the control quantity predicted at the kth moment; a isminAnd amaxUpper and lower bounds of acceleration respectively; n is a radical ofobsThe number of the obstacle vehicles; when detecting the range ddetWhen the vehicle is in obstacle, the upper and lower limits of the acceleration are widened to
Figure FDA0003164966340000059
And
Figure FDA00031649663400000510
δfminand deltafmaxThe upper and lower limits of the front wheel deflection angle are respectively; Δ uminAnd Δ umaxRespectively the upper and lower bounds of the control increment delta u; dsafeIs a safe distance; j is 1,3, dsafeThe value of (A) is related to the position of the unmanned vehicle, chi1,χ3A set of straight-going states is represented,
Figure FDA0003164966340000052
indicating unmanned vehicle and obstacle vehicle
Figure FDA0003164966340000053
The relative distance of the estimated position of (a),
Figure FDA0003164966340000054
indicating unmanned vehicle and obstacle vehicle
Figure FDA0003164966340000055
The calculation formula is as follows:
Figure FDA0003164966340000056
Figure FDA0003164966340000057
for the intersection road section, the optimization problem is as follows:
Figure FDA0003164966340000058
Figure FDA0003164966340000061
Figure FDA0003164966340000062
in the formula (17), foutAnd finRespectively are nonlinear functions for avoiding collision with the outer edge and the inner edge of the road; b is the lane width, and R is a radius of a quarter circular arc at the joint of the straight road and the intersection; chi shape2Representing a set of unmanned vehicle turning states;
step four, dynamic decoupling of the unmanned vehicle dynamic model
Regarding the dynamic coupling action among the output quantities as an Extended state, observing by using an Extended State Observer (ESO), and feeding back the ESO to the controller for compensation;
aiming at the unmanned vehicle dynamics model shown in the formula (2), the output quantity y is selectedd＝[x y v]TThe dynamic model is a coupling system with 3-dimensional input and 3-dimensional output; simplifying the model by adopting a small angle hypothesis to obtain a simplified model:
Figure FDA0003164966340000063
let u1＝Flf，u2＝δf，u3＝FlrThen, the nonlinear model described by equation (8) is expressed as:
Figure FDA0003164966340000064
in the formula (19), the compound represented by the formula (I),
Figure FDA0003164966340000065
and f3(ξd,ud) Expressed as a non-linear function, as follows:
Figure FDA0003164966340000066
b11，b22and b33Input gain for the corresponding sub-object:
Figure FDA0003164966340000071
through the decoupling process, an Extended State Observer (ESO) is designed to estimate the actual state and the Extended state of the child object as follows:
Figure FDA0003164966340000072
Figure FDA0003164966340000073
Figure FDA0003164966340000074
in the formulae (22) to (24), h is an integration step, βx1，βx2，βx3，βy1，βy2，βy3，βv1And betav2Is the gain factor, δx，δy，δv，αx1，αx2，αy1，αy2，αv1And alphav2Is an adjustable parameter; using the ESO of the state quantity x as an example, the function fal (e)x(k),αx1,δx) Is defined as:
Figure FDA0003164966340000075
step five, designing an active disturbance rejection trajectory tracking controller
1) Designing an active disturbance rejection controller: the system comprises a Tracking differential controller (TD) of longitudinal and transverse positions and speeds, an extended state observer and a Nonlinear error feedback control law (NLSEF); wherein said tracking derivative controller arranges a transition to produce a smoothed input signal and an input derivative signal; the extended state observer estimates the state and internal and external disturbances of the system through the control quantity and the output quantity; the nonlinear error feedback control law obtains a controlled variable by nonlinear combination of the error between the output of the tracking differential controller and the observed value of the extended state observer;
2) taking the longitudinal state quantity x as an example, the mathematical model of the tracking differential controller is as follows:
Figure FDA0003164966340000081
in the formula (26), r0For fast factors, h is the integration step, xr(k) For the input signal, i.e. the longitudinal reference value, x1(k) Tracking the input signal, x2(k) To track the rate of change of the input signal; function fhan (x)1,x2,r0And h) is defined as:
Figure FDA0003164966340000082
the mathematical expression for the nonlinear error feedback control law is as follows:
Figure FDA0003164966340000083
in the formula (28), the reaction mixture is,
Figure FDA0003164966340000084
kx1and kx2Is a parameter to be set; u. ofx(k) For the actual controlled variable, the controlled variable u is fed back by an error0x(k) And disturbance estimate zx3Obtaining the compensation; optimal parameters are obtained through continuous testing, and a control effect with strong robustness can be obtained;
and similarly, obtaining a mathematical model of the tracking differential controller of the transverse state quantity y and the velocity state quantity v and a mathematical expression of the nonlinear error feedback control law.
CN202110801915.2A 2021-07-15 2021-07-15 Intersection trajectory planning and control method based on obstacle vehicle estimation and prediction Active CN113433947B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110801915.2A CN113433947B (en) 2021-07-15 2021-07-15 Intersection trajectory planning and control method based on obstacle vehicle estimation and prediction

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110801915.2A CN113433947B (en) 2021-07-15 2021-07-15 Intersection trajectory planning and control method based on obstacle vehicle estimation and prediction

Publications (2)

Publication Number Publication Date
CN113433947A CN113433947A (en) 2021-09-24
CN113433947B true CN113433947B (en) 2022-04-01

Family

ID=77760617

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110801915.2A Active CN113433947B (en) 2021-07-15 2021-07-15 Intersection trajectory planning and control method based on obstacle vehicle estimation and prediction

Country Status (1)

Country Link
CN (1) CN113433947B (en)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113870316B (en) * 2021-10-19 2023-08-15 青岛德智汽车科技有限公司 Front vehicle path reconstruction method under GPS-free following scene
CN114387786B (en) * 2022-01-26 2023-03-21 青岛海信网络科技股份有限公司 Method and device for predicting running state of vehicle at intersection
CN116956978B (en) * 2023-05-22 2024-06-04 杭州彩熊医疗科技有限责任公司 Biological sample traceability system based on RFID label technology
CN117133140A (en) * 2023-07-27 2023-11-28 北京工业大学 Dual-mode optimal speed track control method for intelligent network-connected vehicle at signalized intersection
CN116700319B (en) * 2023-08-04 2023-10-20 西安交通大学 Autonomous take-off and landing system and method for aerial robot based on micro radar array

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104933228A (en) * 2015-05-27 2015-09-23 西安交通大学 Unmanned ground vehicle real-time track planning method based on speed impediment
CN109345875A (en) * 2018-09-28 2019-02-15 哈尔滨工程大学 A kind of estimation method improving ship automatic identification system measurement accuracy
CN110334087A (en) * 2019-06-04 2019-10-15 大连理工大学 A kind of data cleaning method based on improvement robust volume Kalman filtering
CN110647042A (en) * 2019-11-11 2020-01-03 中国人民解放军国防科技大学 Robot robust learning prediction control method based on data driving
CN111381597A (en) * 2020-05-14 2020-07-07 吉林大学 Vehicle obstacle avoidance trajectory planning method based on Gaussian pseudo-spectral method
CN111766879A (en) * 2020-06-24 2020-10-13 天津大学 Intelligent vehicle formation system based on autonomous collaborative navigation
CN112068545A (en) * 2020-07-23 2020-12-11 哈尔滨工业大学（深圳） Method and system for planning driving track of unmanned vehicle at crossroad and storage medium

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104933228A (en) * 2015-05-27 2015-09-23 西安交通大学 Unmanned ground vehicle real-time track planning method based on speed impediment
CN109345875A (en) * 2018-09-28 2019-02-15 哈尔滨工程大学 A kind of estimation method improving ship automatic identification system measurement accuracy
CN110334087A (en) * 2019-06-04 2019-10-15 大连理工大学 A kind of data cleaning method based on improvement robust volume Kalman filtering
CN110647042A (en) * 2019-11-11 2020-01-03 中国人民解放军国防科技大学 Robot robust learning prediction control method based on data driving
CN111381597A (en) * 2020-05-14 2020-07-07 吉林大学 Vehicle obstacle avoidance trajectory planning method based on Gaussian pseudo-spectral method
CN111766879A (en) * 2020-06-24 2020-10-13 天津大学 Intelligent vehicle formation system based on autonomous collaborative navigation
CN112068545A (en) * 2020-07-23 2020-12-11 哈尔滨工业大学（深圳） Method and system for planning driving track of unmanned vehicle at crossroad and storage medium

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
A Novel Local Motion Planning Framework for Autonomous Vehicles Based on Resistance Network and Model Predictive Control;Yanjun Huang等;《IEEE Transactions on Vehicular Technology》;20191028;全文 *
Autonomous Planning and Control for;you changxi等;《IEEE Transactions on Intelligent Transportation Systems》;20190404;全文 *
Distributed synchronous cooperative tracking algorithm for ground moving target in urban by UAVs;Chaofang Hu 等;《International Journal of Systems Science》;20201108;全文 *
Spacecraft output feedback attitude control based on extended state observer and adaptive dynamic programming;Li-Gang Gong等;《journal of the franklin institute》;20190731;全文 *
Steering control based on model predictive control for obstacle avoidance of unmanned ground vehicle;Chaofang Hu等;《MEASUREMENT AND CONTROL》;20200131;全文 *
基于滑模自抗扰的智能车路径跟踪控制;吴艳等;《控制与决策》;20191031;全文 *

Also Published As

Publication number Publication date
CN113433947A (en) 2021-09-24

Similar Documents

Publication Publication Date Title
CN113433947B (en) Intersection trajectory planning and control method based on obstacle vehicle estimation and prediction
Li et al. Real-time trajectory planning for autonomous urban driving: Framework, algorithms, and verifications
CN107085424A (en) Pre- for automatic Pilot takes aim at crosswise joint
Betz et al. A software architecture for the dynamic path planning of an autonomous racecar at the limits of handling
Zhang et al. Automatic vehicle parallel parking design using fifth degree polynomial path planning
Ritschel et al. Nonlinear model predictive path-following control for highly automated driving
JP7315039B2 (en) How to find an avoidance route for a car
Ben-Messaoud et al. Smooth obstacle avoidance path planning for autonomous vehicles
Benloucif et al. A new scheme for haptic shared lateral control in highway driving using trajectory planning
Huang et al. Parallel auto-parking of a model vehicle using a self-organizing fuzzy controller
Hegedüs et al. Motion planning for highly automated road vehicles with a hybrid approach using nonlinear optimization and artificial neural networks
Na et al. Experimental evaluation of a game-theoretic human driver steering control model
CN114442630A (en) Intelligent vehicle planning control method based on reinforcement learning and model prediction
Park et al. Path generation algorithm based on crash point prediction for lane changing of autonomous vehicles
Althoff et al. Online verification of cognitive car decisions
Hegedüs et al. Hybrid trajectory planning for autonomous vehicles using neural networks
Osman et al. Combined road following control and automatic lane keeping for automated guided vehicles
Hegedüs et al. Real-time optimal motion planning for automated road vehicles
Zheng et al. Model predictive control for intelligent vehicle lane change
Chen et al. Automated vehicle path planning and trajectory tracking control based on unscented kalman filter vehicle state observer
Reda et al. Model-based control strategy for autonomous vehicle path tracking task
Kulikov et al. Using real world data in virtual development and testing of a path tracking controller for an autonomous vehicle
Szͩcs et al. Experimental verification of a control system for autonomous navigation
Nejad et al. Real time localization, path planning and motion control for autonomous parking in cluttered environment with narrow passages
Jian et al. An Optimal Controller for Trajectory Tracking of Automated Guided Vehicle

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