CN116653930A - Path planning method for multiple parking scenes - Google Patents

Abstract

The invention relates to the technical field of unmanned vehicles, in particular to a path planning method for various parking scenes, which comprises the steps of building a whole vehicle kinematics model and designing a multi-stage nonlinear predictive control model; designing a multi-stage nonlinear predictive controller and a constraint function taking a vehicle starting point position, a target point position, a parking speed, a parking steering angle, incomplete kinematic constraint and a minimum obstacle avoidance safety distance as basic constraints; integrating the parking system constraint into an optimal control problem meeting the multi-stage nonlinear predictive control model and the parking system constraint; solving an optimal control problem by adopting an interior point method to obtain an optimal control sequence, and applying a first column element in the sequence to vehicle bottom layer control; the invention can ensure the controllability and the accuracy during parking, can improve the flexibility and the high efficiency of the parking process, and greatly optimizes the architecture of an automatic parking control system.

Description

Path planning method for multiple parking scenes

Technical Field

The invention relates to the technical field of unmanned vehicles, in particular to a path planning method for various parking scenes.

Background

With the development of automobile industry, the popularization of passenger cars, the urban parking space is smaller and smaller, and in order to improve the parking efficiency and save the travel time, the automatic parking technology is more and more important in the automobile intelligent technology. The path planning and the track tracking are key attributes of the unmanned vehicle in the parking process, and the path planning not only finds a collision-free path from a starting point to a target point based on an algorithm, but also requires the unmanned vehicle to complete self-adaptive obstacle avoidance in a complex environment. The track tracking needs to track the planned parking path to finish the autonomous parking process of the vehicle. In the existing automatic parking technology, a path planning algorithm (such as a mixed A-type algorithm and an RRT-type algorithm) and a track tracking algorithm (such as a pure tracking method, a PID algorithm and an LQR algorithm) are separated, so that the parking process algorithm is complex, the parking time is long, the parking process has a pause feeling, and the driving requirement of an unmanned vehicle in a parking scene cannot be met.

Disclosure of Invention

In order to solve the problem of low-efficiency conservative dynamic parking caused by separation of the existing global path planning algorithm and the track tracking algorithm in an automatic parking scene, the invention provides a path planning method for various parking scenes, which specifically comprises the following steps:

building a whole vehicle kinematic model;

based on the whole vehicle kinematics model, designing a multi-stage nonlinear prediction control model;

based on the multi-stage nonlinear predictive control model, constructing a multi-stage nonlinear predictive controller and representing the evolution process of the uncertain parameters in time in the form of a scene tree;

designing a parking system constraint;

based on the constraint of the parking system, designing a cost function of the automatic parking system, and integrating the speed planning, path planning and track tracking of the vehicle into an optimal control problem, wherein the optimal control problem meets the multi-stage nonlinear predictive control model and the constraint of the parking system;

solving the optimal control problem by adopting an interior point method to obtain an optimal control sequence of the system control problem, wherein the control sequence minimizes a cost function of the parking system and meets a constraint condition appointed in a prediction range, and a first element in the sequence is applied to vehicle bottom layer control to realize steering and speed control of a vehicle during parking.

Further, when the whole vehicle kinematic model is constructed, the center point of the rear axle of the vehicle is taken as a whole vehicle state reference point, and the expression of the whole vehicle kinematic model is as follows:

the continuous system state equation for longitudinal motion of a vehicle can be expressed as:

according to a kinematic model of the vehicle, a vehicle state jacobian matrix is obtained:

wherein ,is the course angle of the vehicle, L is the wheelbase of the vehicle, v is the speed of the rear axle, delta is the steering angle of the front wheels, +.> and />The speed components of the vehicle in the X and Y directions, respectively, A being the vehicle state matrix, B being the vehicle control matrix, X being the vehicle state quantity, u being the control input quantity,/->Representing the velocity component in the heading angle direction.

Further, based on the vehicle kinematics model, a multi-stage nonlinear predictive control model is designed, comprising:

vectorizing a vehicle kinematic equation to obtain a time domain prediction model:

wherein ,f_u(t) () Representing the vectorized time domain prediction model;as state variables in the time domain, u (t) =v (t), δ (t)] ^T For the time-domain control input, x (t) and y (t) are the state quantities of the central position of the rear axle of the vehicle in the time domain, +.>V (t) is the rear axle speed representation of the time domain, and delta (t) is the front wheel steering angle of the time domain;

converting the time domain prediction model into a spatial domain prediction model, and representing the spatial domain prediction model as:

wherein , and />Representing the velocity components of the vehicle in the X and Y directions in the spatial domain state, respectively, +.>A vehicle heading angle component representing a spatial domain; s is the distance travelled by the vehicle, +.>The change rate of the distance travelled by the vehicle is represented by L, wherein L represents the wheelbase of the vehicle; the spatial domain prediction model is obtained as follows:

wherein ,f_u(s) (. Cndot.) represents the vectorized spatial domain prediction model; ζ(s) is a state variable of the spatial domain; u(s) is the control input of the spatial domain;

discretizing the spatial domain prediction model to obtain a discretized prediction model, wherein the discretized spatial domain prediction model is as follows:

ξ(k _s +1)＝f _u(ks) (ξ(k _s ),u(k _s ))

wherein ,ξ(k_s +1) represents the relative current sampling point position k _s The position corresponding to the next step is predicted after discretization,representing the discretized predictive model, ζ (k) _s ) Sampling the point position k for the spatial domain _s State variables of u (k) _s ) Sampling the point position k for the spatial domain _s Control input of (a);

converting the discretized predictive model into an incremental predictive model:

wherein ,ξ(k_s +1) is represented at k _s The state variable at the +1 sample time,the method is an incremental prediction model; deltau (k) _s )＝u(k _s )-u(k _s -1) represents the current sampling point position k _s From the sampling point position k at the previous moment _s -1 difference in state variables.

Further, designing a multi-stage nonlinear predictive controller, wherein the evolution process of the uncertain parameters in time is represented by building a scene tree model in the control process, and the method comprises the following steps:

the scene tree setting assumes a discrete time formula of an uncertain nonlinear system expressed as:

wherein ,state variables representing the j-th node on the scene tree at the k+1 sampling instant, +.>A p (j) parent node vehicle state variable representing a k sample time; />Control input representing node j at k sample time, < ->An uncertainty parameter representing a k sampling instant, j representing a state node on the scene tree, P (j) representing a node on the scene tree having the same parent node, r (j) representing a corresponding implementation of the j node uncertainty on the scene tree; f (·) represents an uncertainty nonlinear system expression for discrete time;

assuming that the scene tree has the same number of branches on all nodes, at time step k the scene tree is composed ofGiving S different possible values of uncertainty, S _i Representing slave root node x ₀ An ith scene to one of the leaf nodes.

Further, the optimal control problem of the scene-based multi-stage formula at each time step k can be expressed as:

constraint conditions:

if it isThen->

Wherein I represents all index combinations (j, k) corresponding to nodes covered by the scene tree, and S represents the sceneNumber, omega _i Representing each scene S _i Corresponding weights; and />Representing the state with the same parent node at time k, P (j) and P (l) represent the two nodes j and l, respectively, with the same parent node, +.> and />Representing control inputs having the same parent node; />Representing each scene S _i Is a cost function of (2); the cost function for each of the S scenes is defined as:

wherein ,representing a phase cost function, N _p Representing a prediction time domain;

the model in the process control domain is represented in the continuous time domain by a set of ordinary differential equations, which can be written as:

wherein ,representing the prediction model in the continuous time domain, Φ (·) represents the prediction model specific expression in the continuous time domain,x represents a state quantity, u represents a control input quantity, and d represents an uncertainty factor;

the discrete nature of the scene tree requires a time discrete model of the system, an implicit Euler discretization is selected, and the discretization model can be written as:

by discretizing, each Euler discrete point corresponds to a stage of the scene tree, and to ensure the accuracy of the discretization, several Euler discrete points are used within a stage of the scene tree.

Further, the nonlinear programming problem which can be obtained based on the discretized prediction model is expressed as:

constraint conditions: x is x _l ≤x ^opt ≤x _u

b _l ≤Ax ^opt ≤b _u

c _l ≤c(x ^opt )≤c _u

Where f () represents an objective function; x is x ^opt Representing an augmented optimization vector, expressed as:

wherein ,x₀ The state quantity representing the initial moment in time,represented in the prediction time domain N _p The next Nth state quantity,/->Represented in the prediction time domain N _p An nth control input amount; x is x _l and x_u The representation being applied to the actual systemState and control constraints on b _l and b_u Representing an unexpected constraint of the system, A is an augmentation factor, c _l and c_u Representing the nonlinear constraint of the predictive model, c (x ^opt ) Representing nonlinear constraints under the augmented prediction model.

Further, for an automatic parking system based on a multi-stage nonlinear programming controller, discretizing a model differential equation by using implicit euler, the multi-stage optimization problem of the solution required at each sampling time point can be written as:

constraint conditions:

if it isThen->

Wherein Q and R represent weighting factors; () 'represents a multi-stage optimization problem expression to be solved for each sampling instant, deltau' _k Representing the increment of the control input, R _Δ Representing the increment of the weighting factor,represents the state quantity of the j node at the time of k+1, x _s Representing the state quantity in the spatial domain, +.>Represents the control input quantity of the j node at the time of k+1, u _s Representing the control input quantity in the spatial domain, X representing the state quantity set, U representing the control input quantity set, Δt representing the time increment, Φ (·) representing the prediction model specific expression in the continuous time domain.

Further, constructing constraints of the parking system includes:

designing the constraint of the position and the gesture of a parking target, and constraining the speed and the gesture of a vehicle body at the termination moment:

wherein ,(x_ref ,y _ref ) The position of the parking target point; v (k) _s +N _p ) The vehicle speed at the time of termination is indicated,indicating the heading angle of the vehicle at the termination time, x (k) _s +N _p ) Represents the lateral position of the vehicle at the termination time, y (k) _s +N _p ) Indicating the longitudinal position of the vehicle at the termination time; k (k) _s Represents the current sampling position, N _p Representing a prediction step size;

designing safety constraint, and constraining the transverse position and the longitudinal position of a vehicle in the parking process:

X _left +b≤x(k _s +1)≤X _right -a

y(k _s +1)≥Y _bound +w/2

i＝1,2,3,…,N _p

dist _min ≥dist _safe

wherein ,X_left and X_right The left and right boundary positions of the garage position are respectively, a is the sum of the front overhang length and the wheelbase as well as the safety margin of the vehicle head, b is the sum of the rear overhang length and the safety margin of the vehicle tail, and Y _bound Is the side boundary of the garage, w is the width of the vehicle, dist _min Representing the minimum distance of the vehicle from the obstacle, dist _safe Indicating a parking safety distance;

designing an actuator constraint, and constraining the actuator:

Δv _min ≤Δv≤Δv _max

δ _min ≤δ≤δ _max

Δδ _min ≤Δδ≤Δδ _max

wherein ,Δv_min and Δv_max Is the upper and lower limits of the set speed increment input, delta _min and Δδ_max Is the upper and lower limits of the set front wheel steering angle change quantity input, delta _min and δ_max Is the upper and lower limits of the front wheel corner.

Further, in the automatic parking process, for the multi-order nonlinear MPC controller, the number of uncertain parameters and corresponding value ranges are determined (uncertain factors in the invention include speed steering and external uncertain factors in the parking process, and uncertain factors in the invention refer to emergencies, such as factors that a vehicle detects that a pedestrian or an obstacle suddenly appears on a running path and needs to brake and change the running path during running), so as to construct an initial uncertainty setThe uncertainty set D is scaled down using a multi-order nonlinear MPC algorithm, comprising the following stages:

(1) Initializing the weight P of each scene _(0,j) =1/S, j e {1, …, S }, where S is the total number of scenes of the scene tree, initializing the uncertainty set D;

(2) Calculating model predicted values y corresponding to S scenes in current time step k _(k,j) ：

Then according to the model predictive value y of the current time step k _(k,j) And process measurement y _k I.e. by the vehicle at presentPredicting parking state at next moment by three items of state, control input and uncertainty factors, and calculating residual epsilon _(k,j) Expressed as:

ε _(k,j) ＝y _k -y _(k,j)

wherein ,ε_(k,j) ＝[ε ₁ ，…ε _n ] ^T ,ε ₁ ，…ε _n Residual values for the corresponding n states;

(3) Using residual information epsilon _(k,j) And each scene weight P of the previous time step _(k-1,j) Calculating Bayesian probability weight P of current time step k _(k,j) Expressed as:

where K is a weight matrix expressed as:

wherein cov (·) represents the computational covariance;

(4) According to the Bayesian probability weight P (k, j) of each scene in the current time step k, finding the scene P corresponding to the largest weight in the S scenes _max Scene p corresponding to the smallest weight _min ；

(5) By combining the least probable scenes p _min Move to the most likely scene p _max To update p _min and D_k+1 The updating process comprises the following steps:

p _min ＝p _min +β(p _max -p _min )

D _k+1 ∈D _k ∈…∈D ₁

wherein, beta is the self-adaptive step length;

(6) At the next time step k+1, solving a multi-stage nonlinear MPC optimal control problem according to the scene tree constructed by the new uncertain set to obtain an optimal control actionComprising the following steps:

constraint conditions:

if it isThen->

(7) Reinitializing the initial weights P for each scene _(0,j) ＝1/S,j∈{1,…,S}；

(8) Repeating the steps (2) - (7) until the automatic parking process is finished, and realizing automatic parking.

Further, solving the control problem by adopting an interior point method to obtain an optimal open-loop control sequence, and selecting a first column element in the optimal open-loop control sequence for controlling a vehicle to realize automatic parking, wherein the method comprises the following steps:

solving the control problem by adopting an interior point method to obtain an optimal open-loop control sequence delta u, and using a first column element in the optimal open-loop control sequence for controlling a vehicle to realize automatic parking, wherein the optimal open-loop control sequence delta u is expressed as:

wherein ,v_ref For a desired vehicle longitudinal speed, deltau is the optimal open loop control sequence.

The invention is based on the design of a multi-order nonlinear MPC algorithm, can fully consider the constraint and uncertain parameters of the system, and has good robustness; in addition, the multi-order nonlinear MPC controller designed by the invention integrates path planning and track tracking into an optimization problem to solve, so that the controllability and accuracy in parking can be ensured, the flexibility and high efficiency in the parking process can be improved, and the architecture of an automatic parking control system is greatly optimized.

Drawings

FIG. 1 is a schematic diagram 1 of a vehicle kinematic model in a path planning method for multiple parking scenes according to the present invention;

FIG. 2 is a schematic diagram 2 of a vehicle kinematic model in a path planning method for multiple parking scenarios according to the present invention;

FIG. 3 is a schematic view of a scene tree of a multi-order nonlinear MPC in a path planning method for multiple parking scenes;

fig. 4 is a schematic diagram of an updating method of an MSNMPC scene in a path planning method for multiple parking scenes according to the present invention;

FIG. 5 is a schematic view of a vertical parking scene in a path planning method for multiple parking scenes according to the present invention;

FIG. 6 is a schematic diagram of parallel parking scenes in a path planning method for multiple parking scenes according to the invention;

fig. 7 is a schematic diagram of an inclined parking scene in a path planning method for multiple parking scenes according to the present invention.

Detailed Description

The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but 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 invention provides a path planning method for various parking scenes, which specifically comprises the following steps:

building a whole vehicle kinematic model;

based on the whole vehicle kinematics model, designing a multi-stage nonlinear prediction control model;

based on a multi-stage nonlinear prediction control model, designing a multi-stage nonlinear prediction controller and a constraint function taking a vehicle starting point pose, a target point pose, a parking speed, a parking steering angle, incomplete kinematic constraint and a minimum obstacle avoidance safety distance as basic constraints, and simultaneously expressing the evolution process of uncertain parameters in time in a scene tree form; designing a cost function of an automatic parking system, and integrating speed planning, path planning and track tracking of a vehicle into an optimal control problem, wherein the optimal control problem meets the multi-stage nonlinear predictive control model and the constraint of the parking system;

solving an optimization control problem by adopting an interior point method to obtain an optimal control sequence which is integrated by path planning, parking speed and system constraint, wherein the control sequence minimizes a cost function of a parking system and meets a constraint condition appointed in a prediction range, and a first row of elements in the sequence are applied to vehicle bottom layer control to realize steering and speed control of a vehicle during parking.

In this embodiment, in order to implement a path planning method for multiple parking scenarios, the method specifically includes the following steps:

building a whole vehicle kinematic model;

based on the whole vehicle kinematics model, a Multi-stage nonlinear model predictive control (Multi-stage Nonlinear Model Predictive Control, MSNMPC) model is designed;

based on a multi-stage nonlinear model prediction controller, expressing the evolution process of the uncertain parameters in time in the form of a scene tree;

designing a parking system constraint;

based on the constraint of the parking system, designing a cost function of the automatic parking system, and integrating the speed planning, path planning and track tracking of the vehicle into an optimal control problem, wherein the optimal control problem meets the multi-stage nonlinear predictive control model and the constraint of the parking system;

solving the optimal control problem by adopting an interior point method to obtain an optimal control sequence of the system control problem, wherein the control sequence minimizes a cost function of the parking system and meets a constraint condition appointed in a prediction range, and a first element in the sequence is applied to vehicle bottom layer control to realize steering and speed control of a vehicle during parking.

Based on the multi-stage nonlinear MPC controller, the controller generates an optimal parking path according to the position of the starting point and the position of the target point, then the vehicle posture at the next moment in the track tracking process is predicted in real time according to the vehicle posture at the last moment, the current posture and the posture at the next moment are changed in real time, the vehicle prediction step length at the adjacent moment is the same, and the automatic parking process can be further optimized.

The multi-stage nonlinear MPC provides a thought of closed-loop optimization by using feedback information, and the method enables the parking process to be safe and reliable by regarding the parking process as a closed-loop system, representing uncertainty propagation in a prediction time domain by using a discrete scene tree, and then calculating different control input tracks for different scenes.

The vehicle kinematic model is shown in figures 1-2, and v in figure 1 is based on the whole vehicle kinematic model _f Indicating the central speed of the front axle of the vehicle,representing the lateral speed of the front axle of the vehicle, from which the speed of the rear axle of the vehicle can be calculated, in fig. 2 +.>Representing a desired steering angle of a vehicle, constructing a parkKinematic constraints in a scene, the process comprising in particular:

the midpoint of the rear axle of the vehicle is taken as a whole vehicle state reference point, and the pose state vector of the whole vehicle is taken asThe whole vehicle kinematic model expression is as follows:

wherein, (x, y) is the midpoint coordinates of the rear axis,the vehicle course angle is the vehicle course angle, L is the wheelbase, v is the rear axle speed, and delta is the front wheel steering angle; /> and />The speed components of the vehicle in the X and Y directions, respectively,/->Representing the velocity component in the heading angle direction.

The continuous system state equation for longitudinal motion of a vehicle can be expressed as:

according to a kinematic model of the vehicle, a vehicle state jacobian matrix is obtained:

wherein u represents a control input amount;

the multi-order nonlinear MPC controller adopts a track tracking method based on prediction to convert the transverse dynamics state space of the vehicle into a discrete time form:

wherein ,f_u(t) (. Cndot.) represents the vectorized time domain prediction model;as state variables in the time domain, u (t) = [ v (t), δ (t)] ^T For the time-domain control input, x (t) and y (t) are the state quantities of the central position of the rear axle of the vehicle in the time domain, +.>V (t) is a time domain rear axle vehicle speed representation, δ (t) is a time domain front wheel steering angle, and (t) is a time domain.

Converting the time domain prediction model into a space domain prediction model by adopting a formula (6), namely deriving a state variable of a time domain in the space domain to obtain the space domain prediction model, wherein the conversion process specifically comprises the following steps:

wherein , and />Representing the velocity components of the vehicle in the X and Y directions in the spatial domain state, respectively, +.>A vehicle heading angle component representing a spatial domain; s is the distance travelled by the vehicle, +.>The change rate of the distance travelled by the vehicle is represented by L, wherein L represents the wheelbase of the vehicle; (s) in the present embodiment each represents a spatial domain.

The spatial domain prediction model is obtained as follows:

wherein ,f_u(s) (-) represents a fourier transform operation; ζ(s) is a state variable of the spatial domain; u(s) is the control input of the spatial domain; discretizing the spatial domain prediction model to obtain a discretized prediction model, wherein the discretized spatial domain prediction model is as follows:

wherein ,(k_s +1) represents the relative current sampling point position (k _s ) The position corresponding to the next step, ζ (k _s +1) represents the relative current sampling point position k _s Predicting the position discretization corresponding to the next step;representing discrete operations, ζ (k) _s ) Sampling the point position k for the spatial domain _s State variables of u (k) _s ) Sampling the point position k for the spatial domain _s Control input of (a);

converting the discretized spatial domain prediction model into an incremental prediction model:

wherein ,ξ(k_s +1) is represented at k _s The state variable at the +1 sample time,for the incremental predictive model, i.e. the spatial domain sampling point position k in the formula (8) _s The control input of (a) is replaced by deltau (k) _s ) Obtaining an incremental prediction model; deltau (k) _s )＝u(k _s )-u(k _s -1) represents the current sampling point position k _s From the sampling point position k at the previous moment _s -1 difference in state variables.

A multi-stage NMPC controller was designed to build a suitable scene tree model as shown in fig. 3. The scene tree model in FIG. 3 represents the process of transitioning to one scene under the drive of control vectors and uncertainty factors, e.g., the vehicle state at time k (current time) is represented as x _k Control vector in scene 1 at time kAnd uncertainty factor in scene 1 at time k +.>The driving of (1) to transition to the 1 st scene at the time of k+1 +.>And so on to build a scene tree model. In fig. 3, the vehicle state x at time k _k The control variables (the control variables in the present invention refer to the variables that control the speed and steering angle of the vehicle, etc. to drive the vehicle to the target position) and the uncertainty factors (the uncertainty factors in the present invention refer to the unexpected factors that the vehicle can encounter in a given track during the driving of the control variables, such as obstacles, pedestrians or unexpected factors that need to make a vehicle track change or brake, etc. when the vehicle is in a departure from the road, a meeting, etc.), under the driving of 3 scenes, may have three states at time k+1, taking the 1 st scene as an example, i.e., the state obtained at time k+1 is->The state can also generate three different state quantities at time k+2 under the driving of control variables and uncertain factors in three scenes, and scene trees are generated by analogy.

The main assumption of multi-stage NMPC is that the uncertainty factor can be represented by a discrete scene tree. The scene tree setting assumes a discrete time formula of an uncertain nonlinear system, which can be written as:

wherein ,state variables representing the j-th node on the scene tree at the k+1 sampling instant, +.>A p (j) parent node vehicle state quantity representing k sampling time; />Control input representing node j at k sample time, < ->An uncertain parameter representing k sampling moments, j representing a state node on a scene tree; p (j) represents a node on the scene tree with the same parent node, e.g., in FIG. 3, +.>Representing different vehicle states obtained in 3 scenarios, which have the same parent +.>r (j) represents the corresponding realization of j node uncertainty on the scene tree, namely the realization of one control vector and one uncertainty factor under a certain scene at a certain moment; f () is a state transfer function, in this embodiment state transfer is achieved by a finite state machineMapping, i.e. p (j) parent node vehicle state quantity at k moment +.>Uncertainty parameter of k sampling instants +.>And the control variable +.>As input to the finite state machine, the finite state machine outputs the vehicle state quantity +.>

Assuming that the scene tree has the same number of branches on all nodes, at time step k the scene tree is composed ofGiving S different possible values of uncertainty, S _i Representing slave root node x ₀ An ith scene to one of the leaf nodes.

According to the constructed scene tree model, the scale of the optimization problem increases exponentially with the increase of the prediction time domain, by assuming that the uncertainty remains unchanged after a specific robust time domain. The optimal control problem of the scene-based multi-stage formula at each time step k can be expressed as:

constraint conditions:

wherein I represents all index combinations (j, k) corresponding to the nodes covered by the scene tree, S represents the number of scenes, ω _i Representing each fieldScene S _i Corresponding weights; and />Representing the state with the same parent node at time k, P (j) and P (l) represent the two nodes j and l, respectively, with the same parent node, +.> and />Representing control inputs having the same parent node; />Representing each scene S _i The cost function of each of the S scenarios is defined as:

wherein ,N_p Representing a prediction time domain;the phase cost function is represented as:

the discrete nature of the scene tree requires a time-discrete model of the system, but most models in the process control domain are represented in the continuous time domain by a set of ordinary differential equations, which can be written as:

the above equation shows that both control inputs and states are discretized in the system discretization process and included as optimization variables in the resulting nonlinear programming problem (NLP), so discretization of differential equations is necessary. Implicit euler discretization is chosen here because it provides sufficient accuracy and simplicity. The discretized model can be written as:

by discretizing, each Euler discrete point corresponds to a stage of the scene tree, and to ensure the accuracy of the discretization, several Euler discrete points are used within a stage of the scene tree.

The NLP problem in the prediction model is designed, and NLP which can be obtained based on the discretization prediction model is as follows:

constraint conditions: x is x _l ≤x ^opt ≤x _u (17)

b _l ≤Ax ^opt ≤b _u (18)

c _l ≤c(x ^opt )≤c _u (19)

Wherein f (·) represents the objective function, which in this embodiment refers tox ^opt Representing an augmented optimization vector, expressed as:

wherein ,x₀ The state quantity representing the initial moment in time,represented in the prediction time domain N _p The following NStatus quantity->Represented in the prediction time domain N _p An nth control input amount; x is x _l and x_u Representing the state and control constraints imposed on the actual system, b _l and b_u Representing an unexpected constraint of the system, A is an augmentation factor, c _l and c_u Representing the nonlinear constraint of the predictive model, c (x ^opt ) Representing nonlinear constraints under the augmented prediction model.

The objective function f in the formula (16) contains all terms required to create the objective function in the form of the formula (11); the constraints in equation (17) represent the state and control constraints imposed on the actual system; the linear (equal) constraint (18) comprises an unexpected constraint and the nonlinear constraint (19) comprises a discrete model of all nodes in the tree.

In this embodiment, for an automatic parking system based on a multi-stage NMPC, discretizing the model differential equation using implicit euler, the multi-stage optimization problem of the solution required at each sampling time point can be written as:

constraint conditions:

wherein Q and R represent weighting factors; (. Cndot.) 'represents a multi-stage optimization problem expression to be solved for each sampling instant, deltau' _k Representing the increment of the control input, R _Δ Representing the increment of the weighting factor,represents the state quantity of the j node at the time of k+1, x _s Representing the state quantity in the spatial domain, +.>Represents the control input quantity of the j node at the time of k+1, u _s Representing the control input quantity in the spatial domain, X representing the state quantity set, U representing the control input quantity set, Δt representing the time increment, Φ (·) representing the prediction model specific expression in the continuous time domain.

The cost function (21) comprises the sum of the costs of each scene multiplied by its probability omega _i In this case, the cost of each scene includes three different terms weighted with three different tuning parameters (Q, R and R _Δ ) The first term is a penalty term for tracking the set point on the state, the second term is a penalty term for tracking the set point in the control input, and the third term includes a regularization term to penalize the control motion to avoid oscillatory behavior of the control input; constraints include boundaries of states (22) and inputs (23), unexpected constraints (24), and discretization dynamics (25).

The process of designing constraints of the parking system in the present embodiment specifically includes:

designing the constraint of the position and the gesture of a parking target, and constraining the speed and the gesture of a vehicle body at the termination moment:

wherein ,(x_ref ,y _ref ) The position of the parking target point; v (k) _s +N _p ) The vehicle speed at the time of termination is indicated,indicating the heading angle of the vehicle at the termination time, x (k) _s +N _p ) Represents the lateral position of the vehicle at the termination time, y (k) _s +N _p ) Indicating the longitudinal position of the vehicle at the termination time; k (k) _s Represents the current sampling position, N _p Representing a prediction step size;

designing safety constraint, and constraining the transverse position and the longitudinal position of a vehicle in the parking process:

wherein ,X_left and X_right The left and right boundary positions of the garage position are respectively, a is the sum of the front overhang length and the wheelbase as well as the safety margin of the vehicle head, b is the sum of the rear overhang length and the safety margin of the vehicle tail, and Y _bound Is the side boundary of the garage, w is the width of the vehicle, dist _min Representing the minimum distance of the vehicle from the obstacle, dist _safe Indicating a parking safety distance;

designing an actuator constraint, and constraining the actuator:

wherein ,Δv_min and Δv_max Is the upper and lower limits of the set speed increment input, delta _min and Δδ_max Is the upper and lower limits of the set front wheel steering angle change quantity input, delta _min and δ_max Is the upper and lower limits of the front wheel corner.

In the automatic parking process, for a multi-order nonlinear MPC controller, firstly determining the number of uncertain parameters and corresponding value ranges, and constructing an initial uncertainty set wherein /> and />An uncertainty parameter representing the L U range taken at the nth state node, n _d An uncertainty parameter representing an nth state node; the uncertainty set D is then scaled down using a multi-order nonlinear MPC algorithm, comprising the following stages:

(1) Initializing the weight P of each scene _(0,j) =1/S, j e {1, …, S }, where S is the total number of scenes of the scene tree, initializing the uncertainty set D;

(2) Calculating model predictive value y of the j-th scene in the current time step k _(k,j) Expressed as:

then according to the model predictive value y of the current time step k _(k,j) And process measurement y at time k _k Calculating residual epsilon _(k,j) Expressed as:

ε _(k,j) ＝y _k -y _(k,j) (30)

wherein ,ε_(k,j) ＝[ε ₁ ，…ε _n ] ^T ,ε ₁ ，…ε _n Residual values for the corresponding n states; the superscript T denotes a transpose operation;

(3) Using residual information epsilon _(k,j) And each scene weight P of the previous time step _(k-1,j) Calculating Bayesian probability weight P of current time step k _(k,j) Expressed as:

where K is a weight matrix expressed as:

wherein cov (·) represents the computational covariance;

(4) According to the Bayesian probability weight P (k, j) of each scene in the current time step k, finding the scene P corresponding to the largest weight in the S scenes _max Scene p corresponding to the smallest weight _min ；

(5) By combining the least probable scenes p _min Move to the most likely scene p _max To update p _min and D_k+1 The updating process comprises the following steps:

wherein, beta is the self-adaptive step length;

(6) At the next time step k+1, solving a multi-stage nonlinear MPC optimal control problem according to the scene tree constructed by the new uncertain set to obtain an optimal control actionComprising the following steps:

constraint conditions:

(7) Reinitializing the initial weights P for each scene _(0,j) ＝1/S,j∈{1,…,S}；

(8) Repeating the steps (2) - (7) until the automatic parking process is finished, and realizing automatic parking.

The multi-stage NMPC scene updating method is shown in fig. 4, and performs optimization calculation according to the set value and the output of the prediction model, where the optimization calculation obtains the output of the optimization calculation, i.e. the optimal control action, according to the performance index and the constraint conditionAccording to the optimal control action->And calculating a deviation for each scene from the different scenes; at the same time according to the optimal control action->Performing rolling control, and obtaining y according to the uncertainty of the controlled object _k According to y _k Inputting scene deviation into a prediction model to obtain a predicted value of the model; in this embodiment, the input set value is the real-time speed and steering angle of the vehicle, the constraint condition is the constraint of the parking system, when the embodiment performs online scene update, the scene deviation is calculated according to the predicted value and the actual value, the bayesian probability weight is calculated by using the scene deviation, the scene is updated online according to the weight information in a fixed step length to predict the actual implementation of the uncertainty value, and the scene tree modeling is made to approach the actual value of the uncertainty.

Solving the control problem by adopting an interior point method to obtain an optimal open-loop control sequence, and selecting a first column element in the optimal open-loop control sequence for controlling a vehicle to realize automatic parking, wherein the method comprises the following steps:

solving the control problem by adopting an interior point method to obtain an optimal open-loop control sequence delta u, and using a first column element in the optimal open-loop control sequence for controlling a vehicle to realize automatic parking, wherein the optimal open-loop control sequence delta u is expressed as:

wherein ,v_ref For a desired vehicle longitudinal speed, deltau is the optimal open loop control sequence.

Through the control method, the embodiment provides parking simulation diagrams in three scenes of fig. 5-6, including vertical parking, parallel parking and inclined parking.

Although embodiments of the present invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made therein without departing from the principles and spirit of the invention, the scope of which is defined in the appended claims and their equivalents.

Claims (10)

1. A path planning method for various parking scenes is characterized by comprising the following steps:

building a whole vehicle kinematic model;

based on the whole vehicle kinematics model, designing a multi-stage nonlinear prediction control model;

based on the multi-stage nonlinear predictive control model, constructing a multi-stage nonlinear predictive controller and representing the evolution process of the uncertain parameters in time in the form of a scene tree;

designing a parking system constraint;

based on the constraint of the parking system, designing a cost function of the automatic parking system, and integrating the speed planning, path planning and track tracking of the vehicle into an optimal control problem, wherein the optimal control problem meets the multi-stage nonlinear predictive control model and the constraint of the parking system;

solving the optimal control problem by adopting an interior point method to obtain an optimal control sequence of the system control problem, wherein the control sequence minimizes a cost function of the parking system and meets a constraint condition appointed in a prediction range, and a first element in the sequence is applied to vehicle bottom layer control to realize steering and speed control of a vehicle during parking.

2. The path planning method for multiple parking scenes according to claim 1, wherein when a vehicle kinematic model is constructed, a vehicle rear axle center point is taken as a vehicle state reference point, and the vehicle kinematic model expression is:

the continuous system state equation for longitudinal motion of a vehicle can be expressed as:

according to a kinematic model of the vehicle, a vehicle state jacobian matrix is obtained:

wherein ,is the course angle of the vehicle, L is the wheelbase of the vehicle, v is the speed of the rear axle, delta is the steering angle of the front wheels, +.> and />The speed components of the vehicle in the X and Y directions, respectively, A being the vehicle state matrix, B being the vehicle control matrix, X being the vehicle state quantity, u being the control input quantity,/->Representing the velocity component in the heading angle direction.

3. The method for path planning for multiple parking scenarios according to claim 1, wherein designing a multi-stage nonlinear predictive control model based on a vehicle kinematic model comprises:

vectorizing a vehicle kinematic equation to obtain a time domain prediction model:

wherein ,f_u(t) (. Cndot.) represents the vectorized time domain prediction model;as state variables in the time domain, u (t) = [ v (t), δ (t)] ^T For the time-domain control input, x (t) and y (t) are the state quantities of the central position of the rear axle of the vehicle in the time domain, +.>V (t) is the rear axle speed representation of the time domain, and delta (t) is the front wheel steering angle of the time domain;

converting the time domain prediction model into a spatial domain prediction model, and representing the spatial domain prediction model as:

wherein , and />Representing the velocity components of the vehicle in the X and Y directions in the spatial domain state, respectively, +.>A vehicle heading angle component representing a spatial domain; s is the distance travelled by the vehicle, +.>The change rate of the distance travelled by the vehicle is represented by L, wherein L represents the wheelbase of the vehicle; the spatial domain prediction model is obtained as follows:

wherein ,f_u(s) (. Cndot.) represents the vectorized spatial domain prediction model; ζ(s) is a state variable of the spatial domain; u(s) is the control input of the spatial domain;

discretizing the spatial domain prediction model to obtain a discretized prediction model, wherein the discretized spatial domain prediction model is as follows:

wherein ,ξ(k_s +1) represents the relative current sampling point position k _s The position corresponding to the next step is predicted after discretization,representing the discretized predictive model, ζ (k) _s ) Sampling the point position k for the spatial domain _s State variables of u (k) _s ) Sampling the point position k for the spatial domain _s Control input of (a);

converting the discretized predictive model into an incremental predictive model:

wherein ,ξ(k_s +1) is represented at k _s The state variable at the +1 sample time,the method is an incremental prediction model; deltau (k) _s )＝u(k _s )-u(k _s -1) represents the current sampling point position k _s From the sampling point position k at the previous moment _s -1 difference in state variables.

4. The method for path planning for multiple parking scenarios according to claim 1, wherein designing a multi-stage nonlinear predictive controller, the evolution of uncertain parameters over time during control is represented by building a scenario tree model, comprises:

the scene tree setting assumes a discrete time formula of an uncertain nonlinear system expressed as:

wherein ,j-th section on scene tree representing k+1 sampling timeState variables of points>A p (j) parent node vehicle state variable representing a k sample time; />Control input representing node j at k sample time, < ->An uncertainty parameter representing a k sampling instant, j representing a state node on the scene tree, P (j) representing a node on the scene tree having the same parent node, r (j) representing a corresponding implementation of the j node uncertainty on the scene tree; f (·) represents an uncertainty nonlinear system expression for discrete time;

assuming that the scene tree has the same number of branches on all nodes, at time step k the scene tree is composed ofGiving S different possible values of uncertainty, S _i Representing slave root node x ₀ An ith scene to one of the leaf nodes.

5. The method of claim 4, wherein the optimal control problem of the multi-stage scene-based formula at each time step k can be expressed as:

constraint conditions:

wherein I represents all index combinations (j, k) corresponding to the nodes covered by the scene tree, S represents the number of scenes, ω _i Representing each scene S _i Corresponding weights; and />Representing the state with the same parent node at time k, P (j) and P (l) represent the two nodes j and l, respectively, with the same parent node, +.> and />Representing control inputs having the same parent node;representing each scene S _i Is used for the cost function of (a),

the cost function for each of the S scenes is defined as:

wherein ,representing a phase cost function, N _p Representing a prediction time domain;

the model in the process control domain is represented in the continuous time domain by a set of ordinary differential equations, which can be written as:

wherein ,representing a prediction model in a continuous time domain, wherein phi (·) represents a specific expression of the prediction model in the continuous time domain, x represents a state quantity, u represents a control input quantity, and d represents an uncertainty factor;

the discrete nature of the scene tree requires a time discrete model of the system, an implicit Euler discretization is selected, and the discretization model can be written as:

by discretizing, each Euler discrete point corresponds to a stage of the scene tree, and to ensure the accuracy of the discretization, several Euler discrete points are used within a stage of the scene tree.

6. The path planning method for multiple parking scenarios according to claim 5, characterized in that the nonlinear programming problem available based on the discretized prediction model is expressed as:

constraint conditions:

wherein f (·) represents the objective function; x is x ^opt Representing an augmented optimization vector, expressed as:

wherein ,x₀ The state quantity representing the initial moment in time,represented in the prediction time domain N _p The next Nth state quantity,/->Represented in the prediction time domain N _p An nth control input amount; x is x _l and x_u Representing the state and control constraints imposed on the actual system, b _l and b_u Representing an unexpected constraint of the system, A is an augmentation factor, c _l and c_u Representation predictionNonlinear constraint of model, c (x ^opt ) Representing nonlinear constraints under the augmented prediction model.

7. The method for path planning for multiple parking scenarios according to claim 5, characterized in that for an automatic parking system based on a multi-stage nonlinear programming controller, discretizing the model differential equation with implicit euler, the multi-stage optimization problem of the solution required at each sampling time point can be written as:

constraint conditions:

if it isThen->

Wherein Q and R represent weighting factors; () 'represents a multi-stage optimization problem expression to be solved for each sampling instant, deltau' _k Representing the increment of the control input, R _Δ Representing the increment of the weighting factor,represents the state quantity of the j node at the time of k+1, x _s Representing the state quantity in the spatial domain, +.>Represents the control input quantity of the j node at the time of k+1, u _s Representing the control input quantity in the spatial domain, X representing the state quantity set, U representing the control input quantity set, Δt representing the time increment, Φ (·) representing the prediction model specific expression in the continuous time domain.

8. The method of path planning for multiple parking scenarios of claim 1, wherein constructing constraints for the parking system comprises:

designing the constraint of the position and the gesture of a parking target, and constraining the speed and the gesture of a vehicle body at the termination moment:

wherein ,(x_ref ,y _ref ) The position of the parking target point; v (k) _s +N _p ) The vehicle speed at the time of termination is indicated,indicating the heading angle of the vehicle at the termination time, x (k) _s +N _p ) Represents the lateral position of the vehicle at the termination time, y (k) _s +N _p ) Indicating the longitudinal position of the vehicle at the termination time; k (k) _s Represents the current sampling position, N _p Representing a prediction step size;

designing safety constraint, and constraining the transverse position and the longitudinal position of a vehicle in the parking process:

X _left +b≤x(k _s +1)≤X _right -a

y(k _s +1)≥Y _bound +w/2

i＝1,2,3,…,N _p

dist _min ≥dist _safe

wherein ,X_left and X_right The left and right boundary positions of the garage position are respectively, a is the sum of the front overhang length and the wheelbase as well as the safety margin of the vehicle head, b is the sum of the rear overhang length and the safety margin of the vehicle tail, and Y _bound Is the side boundary of the garage, w is the width of the vehicle, dist _min Representing the minimum distance of the vehicle from the obstacle, dist _safe Indicating a parking safety distance;

designing an actuator constraint, and constraining the actuator:

Δv _min ≤Δv≤Δv _max

δ _min ≤δ≤δ _max

Δδ _min ≤Δδ≤Δδ _max

wherein ,Δv_min and Δv_max Is the upper and lower limits of the set speed increment input, delta _min and Δδ_max Is the upper and lower limits of the set front wheel steering angle change quantity input, delta _min and δ_max Is the upper and lower limits of the front wheel corner.

9. The method for planning a path for multiple parking scenarios according to claim 1, wherein, during automatic parking, for the multi-stage nonlinear MPC controller, the number of uncertainty parameters and corresponding value ranges are determined, and an initial uncertainty set is constructedThe uncertainty set D is scaled down using a multi-order nonlinear MPC algorithm, comprising the following stages:

(1) Initializing the weight P of each scene _(0,j) =1/S, j e {1, …, S }, where S is the total number of scenes of the scene tree, initializing the uncertainty set D;

(2) Calculating model predicted values y corresponding to S scenes in current time step k _(k,j) ：

And then according to the currentModel predictive value y of time step k _(k,j) And process measurement y _k Calculating residual epsilon _(k,j) Expressed as:

ε _(k,j) ＝y _k -y _(k,j)

wherein ,ε_(k,j) ＝[ε ₁ ，…ε _n ] ^T ,ε ₁ ，…ε _n Residual values for the corresponding n states;

(3) Using residual information epsilon _(k,j) And each scene weight P of the previous time step _(k-1,j) Calculating Bayesian probability weight P of current time step k _(k,j) Expressed as:

where K is a weight matrix expressed as:

wherein cov (·) represents the computational covariance;

(4) According to the Bayesian probability weight P (k, j) of each scene in the current time step k, finding the scene P corresponding to the largest weight in the S scenes _max Scene p corresponding to the smallest weight _min ；

(5) By combining the least probable scenes p _min Move to the most likely scene p _max To update p _min and D_k+1 The updating process comprises the following steps:

p _min ＝p _min +β(p _max -p _min )

D _k+1 ∈D _k ∈…∈D ₁

wherein, beta is the self-adaptive step length;

(6) At the next time step k+1, solving a multi-stage nonlinear MPC optimal control problem according to the scene tree constructed by the new uncertain set to obtain an optimal control actionComprising the following steps:

constraint conditions:

(7) Reinitializing the initial weights P for each scene _(0,j) ＝1/S,j∈{1,…,S}；

(8) Repeating the steps (2) - (7) until the automatic parking process is finished, and realizing automatic parking.

10. The method for planning paths for multiple parking scenes according to claim 1, wherein solving the control problem by using an interior point method to obtain an optimal open-loop control sequence, selecting a first element in the optimal open-loop control sequence for controlling a vehicle, and realizing automatic parking, comprises:

solving the control problem by adopting an interior point method to obtain an optimal open-loop control sequence delta u, and using a first column element in the optimal open-loop control sequence for controlling a vehicle to realize automatic parking, wherein the optimal open-loop control sequence delta u is expressed as:

wherein ,v_ref For a desired vehicle longitudinal speed, deltau is the optimal open loop control sequence.