CN116653930A - A Path Planning Method for Various Parking Scenarios - 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

A Path Planning Method for Various Parking Scenarios

technical field

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

Background technique

With the development of the automobile industry and the popularization of passenger cars, the parking space in cities is getting smaller and smaller. In order to improve parking efficiency and save travel time, automatic parking technology is becoming more and more important in automotive intelligent technology. Path planning and trajectory tracking in the parking process are the key attributes of unmanned vehicles. Path planning is not only based on algorithms to find a collision-free path from the starting point to the target point, but also requires unmanned vehicles to complete adaptive avoidance in complex environments. barrier. Trajectory tracking needs to track the planned parking path to complete the autonomous parking process of the vehicle. In the existing automatic parking technology, path planning algorithms (such as hybrid A* algorithm, RRT* algorithm) and trajectory tracking algorithms (such as pure tracking method, PID algorithm, LQR algorithm) are separated, resulting in complex parking process algorithms , Parking takes a long time, and the parking process is frustrating, which cannot meet the driving needs of unmanned vehicles in parking scenarios.

Contents of the invention

In order to solve the inefficient and conservative dynamic parking problem caused by the separation of the existing global path planning algorithm and the trajectory tracking algorithm in the automatic parking scene, the present invention proposes a path planning method for various parking scenes, which specifically includes the following steps :

Build the vehicle kinematics model;

Based on the vehicle kinematics model, design a multi-stage nonlinear predictive control model;

Based on the multi-stage nonlinear predictive control model, a multi-stage nonlinear predictive controller is constructed and the evolution process of uncertain parameters in time is expressed in the form of scene tree;

Design parking system constraints;

Based on the constraints of the parking system, the cost function of the automatic parking system is designed, and the speed planning, path planning and trajectory tracking of the vehicle are integrated into an optimal control problem, which satisfies the above multi-stage nonlinear predictive control model and the constraints of the parking system ;

The interior point method is used to solve the optimization control problem, and the optimal control sequence of the system control problem is obtained. This control sequence minimizes the cost function of the parking system and satisfies the specified constraints within the prediction range. The first column in the sequence The element is applied to the underlying control of the vehicle to realize the steering and speed control of the vehicle when parking.

Further, when constructing the kinematics model of the whole vehicle, the center point of the rear axle of the vehicle is taken as the reference point of the vehicle state, and the expression of the kinematics model of the whole vehicle is:

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

According to the kinematic model of the vehicle, the Jacobian matrix of the vehicle state is obtained:

in, is the heading angle of the vehicle, L is the wheelbase of the vehicle, v is the speed of the rear axle, δ is the steering angle of the front wheel, /> and /> are the velocity components of the vehicle in the X and Y directions, A is the vehicle state matrix, B is the vehicle control matrix, x is the vehicle state quantity, u is the control input quantity, /> Indicates the velocity component in the direction of the heading angle.

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

Vectorize the vehicle kinematics equation to obtain the time domain prediction model:

Among them, f _u(t) () represents the time-domain prediction model after vectorization; is the state variable in the time domain, u(t)=v(t),δ(t)] ^T is the control input in the time domain, x(t) and y(t) are the state of the center position of the rear axle of the vehicle in the time domain amount, /> is the vehicle heading angle in the time domain, v(t) is the rear axle speed in the time domain, and δ(t) is the front wheel steering angle in the time domain;

The time domain prediction model is transformed into a space domain prediction model, expressed as:

in, and /> respectively represent the velocity components of the vehicle in the X and Y directions in the space domain state, /> Indicates the vehicle heading angle component in the space domain; s is the distance traveled by the vehicle, /> is the rate of change of the distance traveled by the vehicle, and L represents the wheelbase of the vehicle; the spatial domain prediction model is obtained as follows:

Among them, f _u(s) ( ) represents the spatial domain prediction model after vectorization; ξ(s) is the 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, the discretized spatial domain prediction model:

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

Among them, ξ(k _s +1) represents the prediction after the discretization of the position corresponding to the next step of the current sampling point position k _s , Represents the discretized prediction model, ξ(k _s ) is the state variable of the sampling point k _s in the spatial domain, u(k _s ) is the control input of the sampling point k _s in the spatial domain;

Convert a discretized forecasting model to an incremental forecasting model:

Among them, ξ(k _s +1) represents the state variable at the sampling moment of k _s +1, is an incremental prediction model; Δu(k _s )=u(k _s )-u(k _s -1) represents the difference between the current sampling point position k _s and the previous sampling point position k _s -1 state variable.

Furthermore, a multi-stage nonlinear predictive controller is designed, and the evolution process of uncertain parameters in time is expressed by establishing a scene tree model during the control process, including:

The scene tree setting assumes a discrete-time formulation of an uncertain nonlinear system, expressed as:

in, Indicates the state variable of the jth node on the scene tree at k+1 sampling time, /> Represents the p(j) parent node vehicle state variable at k sampling time; /> Indicates the control input of node j at sampling time k, /> Indicates the uncertain parameter at k sampling time, j indicates the state node on the scene tree, P(j) indicates the node with the same parent node on the scene tree, r(j) indicates the corresponding realization of the uncertainty of node j on the scene tree; f( ) represents the discrete-time uncertain nonlinear system expression;

Assuming the scene tree has the same number of branches on all nodes, at time step k by Given s different possible values of uncertainty, S _i represents the ith scenario from the root node _x0 to one of the leaf nodes.

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

Restrictions:

like Then />

Among them, I represents all index combinations (j, k) corresponding to the nodes covered by the scene tree, S represents the number of scenes, and ω _i represents the weight corresponding to each scene S _i ; and /> Indicates the state with the same parent node at time k, P(j) and P(l) respectively represent two nodes j and l with the same parent node, /> and /> Indicates a control input with the same parent node; /> Represents the cost function of each scene S _i ; the cost function of each scene in S scenes is defined as:

in, Represents the stage cost function, N _p represents the prediction time domain;

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

in, Indicates the prediction model in the continuous time domain, Φ( ) represents the specific expression of the prediction model in the continuous time domain, x represents the state quantity, u represents the control input quantity, and d represents the uncertain factor;

The discrete nature of the scene tree requires a time-discrete model of the system. If the implicit Euler discretization is selected, the discretization model can be written as:

Through discretization, each Euler discrete point corresponds to a stage of the scene tree. To ensure the accuracy of discretization, several Euler discrete points are used in one stage of the scene tree.

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

Constraints: x _l ≤ x ^opt ≤ x _u

b _l ≤Ax ^opt ≤b _u

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

Among them, f() represents the objective function; x ^opt represents the augmented optimization vector, expressed as:

Among them, x ₀ represents the state quantity at the initial moment, Indicates the Nth state quantity in the prediction time domain N _p , /> Indicates the Nth control input in the prediction time domain N _p ; x _l and x _u indicate the state and control constraints imposed on the actual system, b _l and b _u indicate the unexpected constraints of the system, A is the augmentation coefficient, c _l and c _u represent the nonlinear constraints of the prediction model, and c(x ^opt ) represents the nonlinear constraints under the augmented prediction model.

Furthermore, for an automatic parking system based on a multi-stage nonlinear programming controller, implicit Euler is used to discretize the model differential equation, and the multi-stage optimization problem to be solved at each sampling time point can be written as:

Restrictions:

like Then />

Among them, Q and R represent the weighting factors; ()' represents the multi-stage optimization problem expression to be solved at each sampling moment, Δu' _k represents the increment of the control input, R _Δ represents the increment of the weighting factor, Indicates the state quantity of node j at time k+1, x _s represents the state quantity in the space domain, /> Indicates the control input of node j at time k+1, u _s represents the control input in the space domain, X represents the set of state quantities, U represents the set of control inputs, Δt represents the time increment, Φ(·) represents The specific expression of the prediction model under the domain.

Further, construct the constraints of the parking system, including:

Design the parking target position and attitude constraints, and constrain the vehicle speed and body attitude at the end time:

Among them, (x _ref , y _ref ) is the position of the parking target point; v(k _s +N _p ) represents the vehicle speed at the end time, Indicates the heading angle of the vehicle at the time of termination, x(k _s +N _p ) represents the lateral position of the vehicle at the time of termination, y(k _s +N _p ) represents the longitudinal position of the vehicle at the time of termination; k _s represents the current sampling position, N _p represents the prediction step long;

Design safety constraints to constrain the lateral and longitudinal positions of the vehicle during parking:

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

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

i=1,2,3,..., _Np

dist _min ≥ dist _safe

Among them, X _left and X _right are the left and right boundary positions of the warehouse respectively, a is the sum of the front overhang length, wheelbase and front safety margin, b is the sum of the rear overhang length and the rear safety margin, and Y _bound is the garage The side boundary of , w is the width of the vehicle, dist _min represents the minimum distance from the vehicle to the obstacle, and dist _safe represents the parking safety distance;

Design actuator constraints and constrain the actuators:

Δv _min ≤ Δv ≤ Δv _max

δ _min ≤ δ ≤ _{δ max}

Δδ _min ≤ Δδ ≤ Δδ _max

Among them, Δv _min and Δv _max are the upper and lower limits of the set speed increment input, Δδ _min and Δδ _max are the upper and lower limits of the set front wheel steering angle change input, and δ _min and δ _max are the front wheel angle upper and lower limits.

Further, in the automatic parking process, for the multi-order nonlinear MPC controller, determine the quantity of uncertain parameters and the corresponding value range (uncertain factors in the present invention include speed steering and external uncertainties in the parking process Factors, uncertain factors in the present invention refer to unexpected events, for example, the vehicle detects that pedestrians or obstacles suddenly appear on the driving path during driving, and it is necessary to perform braking and change the driving path), and the structure is initially uncertain. deterministic set The uncertainty set D is reduced using the multi-order nonlinear MPC algorithm, including the following stages:

(1) Initialize the weight of each scene P _{(0, j)} = 1/S, j∈{1,...,S}, where S is the total number of scenes in the scene tree, and initialize the uncertainty set D;

(2) Calculate the model prediction value y _(k,j) corresponding to the S scenes in the current time step k:

Then, according to the model prediction value y _(k,j) of the current time step k and the process measurement value y _k , that is, the parking state at the next moment is predicted through the current vehicle state, control input and uncertain factors, and the residual error is calculated ε _(k,j) , expressed as:

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

Among them, ε _(k,j) = [ε ₁ ,…ε _n ] ^T ,ε ₁ ,…ε _n are the residual values of the corresponding n states;

(3) Calculate the Bayesian probability weight P _{(k,j) of the current time step k by using the residual information ε (k,j)} and each scene weight P _{(k-1,j) of} the previous time step, expressing for:

Among them, K is a weighting matrix expressed as:

Among them, cov( ) means to calculate the covariance;

(4) According to the Bayesian probability weight P(k, j) of each scene of the current time step k, find the scene p _max corresponding to the maximum weight and the scene p _min corresponding to the minimum weight among the S scenes;

(5) Update _pmin and Dk ₊₁ by moving the least likely scenario _pmin to the most probable scenario _pmax . The update process includes:

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

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

Among them, β is the adaptive step size;

(6) At the next time step k+1, solve the multi-stage nonlinear MPC optimal control problem based on the scene tree constructed by the new uncertain set to obtain the optimal control action include:

Restrictions:

like Then />

(7) Reinitialize the initial weight of each scene P _{(0, j)} = 1/S, j∈{1,...,S};

(8) Steps (2) to (7) are repeated until the automatic parking process ends, and automatic parking is realized.

Further, the interior point method is used to solve the control problem to obtain the optimal open-loop control sequence, and the first column element in the optimal open-loop control sequence is selected to control the vehicle to realize automatic parking, including:

The interior point method is used to solve the control problem, and the optimal open-loop control sequence Δu is obtained, and the first column elements in the optimal open-loop control sequence are used to control the vehicle to realize automatic parking. The optimal open-loop control sequence Δu is represented by for:

Among them, v _ref is the desired vehicle longitudinal speed, Δu is the optimal open-loop control sequence.

The present invention is based on multi-order nonlinear MPC algorithm design, can fully take into account the constraints and uncertain parameters of the system, and has good robustness; moreover, the multi-order nonlinear MPC controller designed by the present invention combines path planning and trajectory Tracking is integrated into an optimization problem solution, which can ensure the controllability and accuracy of parking, and at the same time improve the flexibility and efficiency of the parking process, greatly optimizing the architecture of the automatic parking control system.

Description of drawings

1 is a schematic diagram 1 of a vehicle kinematics model in a path planning method for various parking scenarios according to the present invention;

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

3 is a schematic diagram of a scene tree of a multi-order nonlinear MPC in a path planning method for multiple parking scenarios according to the present invention;

Fig. 4 is MSNMPC scene update method in a kind of path planning method facing multiple parking scenes of the present invention;

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

6 is a schematic diagram of parallel parking scenarios in a path planning method for multiple parking scenarios according to the present invention;

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

Detailed ways

The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some, not all, embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by persons of ordinary skill in the art without making creative efforts belong to the protection scope of the present invention.

The present invention proposes a path planning method for various parking scenarios, which specifically includes the following steps:

Build the vehicle kinematics model;

Based on the vehicle kinematics model, design a multi-stage nonlinear predictive control model;

Based on the multi-stage nonlinear predictive control model, design a multi-stage nonlinear predictive controller and design a vehicle starting point pose, target point pose, parking speed, parking steering angle, nonholonomic kinematic constraints and minimum obstacle avoidance safety Distance is the constraint function of the basic constraints, and at the same time, the evolution process of uncertain parameters in time is expressed in the form of a scene tree; the cost function of the automatic parking system is designed, and the speed planning, path planning and trajectory tracking of the vehicle are integrated into An optimal control problem that satisfies the aforementioned multi-stage nonlinear predictive control model and parking system constraints;

The interior point method is used to solve the optimization control problem, and the optimal control sequence is obtained by integrating path planning, parking speed and system constraints. The control sequence minimizes the cost function of the parking system and satisfies the specified constraints within the prediction range. Conditions, apply the elements in the first column of the sequence to the bottom layer control of the vehicle to realize the steering and speed control of the vehicle when parking.

In this embodiment, in order to realize a path planning method for various parking scenarios, the following steps are specifically included:

Build the vehicle kinematics model;

Based on the vehicle kinematics model, design a multi-stage nonlinear model predictive control (Multi-stage Nonlinear Model Predictive Control, MSNMPC) model;

Based on the multi-stage nonlinear model predictive controller, the evolution process of uncertain parameters in time is expressed in the form of scene tree;

Design parking system constraints;

Based on the constraints of the parking system, the cost function of the automatic parking system is designed, and the speed planning, path planning and trajectory tracking of the vehicle are integrated into an optimal control problem, which satisfies the above multi-stage nonlinear predictive control model and the constraints of the parking system ;

The interior point method is used to solve the optimization control problem, and the optimal control sequence of the system control problem is obtained. This control sequence minimizes the cost function of the parking system and satisfies the specified constraints within the prediction range. The first column in the sequence The element is applied to the underlying control of the vehicle to realize the steering and speed control of the vehicle when parking.

Based on the multi-stage nonlinear MPC controller, the controller generates an optimal parking path according to the starting point position and the target point position planning, and then the vehicle attitude at the next moment in the trajectory tracking process is obtained according to the real-time prediction of the vehicle attitude at the previous moment , the current attitude and the attitude of the next moment change in real time, and the vehicle prediction steps at adjacent moments are the same, which can further optimize the automatic parking process.

Multi-stage nonlinear MPC provides a closed-loop optimization idea using feedback information. This method regards the parking process as a closed-loop system, and expresses the uncertainty propagation in the prediction time domain as a discrete scene tree, and then for Different scenarios calculate different control input trajectories to make the parking process safe and reliable.

The vehicle kinematics model is shown in Figure 1-2, based on the vehicle kinematics model, v _f in Figure 1 represents the center velocity of the front axle of the vehicle, Indicates the lateral velocity of the front axle of the vehicle, and the velocity of the rear axle of the vehicle can be calculated by passing the two velocities, as shown in Figure 2 Indicates the expected steering angle of the vehicle and constructs the kinematic constraints in the parking scene. The process specifically includes:

Taking the midpoint of the rear axle of the vehicle as the reference point of the vehicle state, the pose state vector of the vehicle is The vehicle kinematics model expression is:

Among them, (x, y) is the coordinates of the midpoint of the rear axis, is the heading angle of the vehicle, L is the wheelbase, v is the speed of the rear axle, δ is the steering angle of the front wheel; /> and /> are the velocity components of the vehicle in the X and Y directions, respectively, /> Indicates the velocity component in the direction of the heading angle.

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

According to the kinematic model of the vehicle, the Jacobian matrix of the vehicle state is obtained:

Among them, u represents the control input quantity;

The multi-order nonlinear MPC controller uses a prediction-based trajectory tracking method to transform the vehicle lateral dynamics state space into a discrete-time form:

Among them, f _u(t) ( ) represents the time-domain prediction model after vectorization; is the state variable in the time domain, u(t)=[v(t),δ(t)] ^T is the control input in the time domain, x(t) and y(t) are the center position of the rear axle of the vehicle in the time domain state quantity, /> is the vehicle heading angle in the time domain, v(t) is the rear axle speed in the time domain, δ(t) is the front wheel steering angle in the time domain, and (t) is the time domain.

Using formula (6), the time domain prediction model is converted into a space domain prediction model, and the state variables of the time domain are derived in the space domain to obtain the space domain prediction model. The conversion process specifically includes:

in, and /> respectively represent the velocity components of the vehicle in the X and Y directions in the space domain state, /> Indicates the vehicle heading angle component in the space domain; s is the distance traveled by the vehicle, /> is the rate of change of the distance traveled by the vehicle, L represents the wheelbase of the vehicle; (s) both represent the space domain in this embodiment.

The spatial domain prediction model is obtained as follows:

Among them, f _u(s) ( ) represents the Fourier transform operation; ξ(s) is the state variable in the space domain; u(s) is the control input in the space domain; discretize the prediction model in the space domain to obtain Discretized prediction model, discretized spatial domain prediction model:

Among them, (k _s +1) represents the position corresponding to the next step relative to the current sampling point position (k _s ), that is, ξ(k _s +1) represents the discrete position corresponding to the next step relative to the current sampling point position k _s The predicted forecast; Indicates a discrete operation, ξ(k _s ) is the state variable of the sampling point k _s in the spatial domain, u(k _s ) is the control input of the sampling point k _s in the spatial domain;

Transform the discretized spatial domain forecasting model into an incremental forecasting model:

Among them, ξ(k _s +1) represents the state variable at the sampling moment of k _s +1, is an incremental prediction model, that is, the control input of the sampling point position k _s in the space domain in formula (8) is replaced by Δu(k _s ) to obtain an incremental prediction model; Δu(k _s )=u(k _s )-u(k _s -1) represents the difference between the current sampling point position k _s and the previous sampling point position k _s -1 state variable.

Design a multi-stage NMPC controller and establish a suitable scene tree model, as shown in Figure 3. The scene tree model in Figure 3 represents the process of transferring to another scene driven by the control vector and uncertain factors in a certain scene. For example, the vehicle state at time k (current time) is expressed as x _k , The control vector in the first scene at time and the uncertain factors in the first scenario at time k/> Transfer to the vehicle state of the first scene at time k+1 under the drive of Build a scene tree model by analogy. In Figure 3, the vehicle state x _k at time k, the control variables in three scenarios (the control variables in the present invention refer to the variables that control the speed and steering angle of the vehicle to drive the vehicle to reach the target position) and uncertain Factors (uncertain factors in the present invention refer to the accident factor that vehicle can run into in the predetermined track in the driving process of control variable, such as obstacles, pedestrians suddenly appearing in the road, or need to pass the car with passing vehicles, meet cars, etc. Driven by unexpected factors that may cause vehicle track changes or braking, there may be three states at time k+1. Take the first scene as an example, that is, the state obtained at time k+1 is /> Driven by the control variables and uncertain factors in the three scenarios, this state will also generate three different state quantities at k+2 time, and so on to generate the scenario tree.

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

in, Indicates the state variable of the jth node on the scene tree at k+1 sampling time, /> Indicates the p(j) parent node vehicle state quantity at k sampling time; /> Indicates the control input of node j at sampling time k, /> Indicates the uncertain parameter at k sampling time, j indicates the state node on the scene tree; P(j) indicates the node with the same parent node on the scene tree, for example, in Figure 3, /> Represents different vehicle states obtained in three scenarios, which have the same parent node /> r(j) represents the corresponding realization of the uncertainty of node j on the scene tree, that is, the realization composed of a control vector and an uncertainty factor in a certain scene at a certain moment; The state machine realizes the mapping of state transition, that is, the state quantity of p(j) parent node vehicle at time k/> Uncertain parameter at k sampling instant /> And the control variable in the jth scene at time k/> As the input of the finite state machine, the finite state machine outputs the vehicle state quantity in the j scene at the next moment />

Assuming the scene tree has the same number of branches on all nodes, at time step k by Given s different possible values of uncertainty, S _i represents the ith scenario from the root node _x0 to one of the leaf nodes.

According to the constructed scene tree model, the size of the optimization problem increases exponentially with the increase of the forecast time domain, by assuming that the uncertainty remains constant after a certain robust time domain. The optimal control problem of the scenario-based multi-stage formulation at each time step k can be formulated as:

Restrictions:

Among them, I represents all index combinations (j, k) corresponding to the nodes covered by the scene tree, S represents the number of scenes, and ω _i represents the weight corresponding to each scene S _i ; and /> Indicates the state with the same parent node at time k, P(j) and P(l) respectively represent two nodes j and l with the same parent node, /> and /> Indicates a control input with the same parent node; /> Represents the cost function of each scene S _i , and the cost function of each scene in S scenes is defined as:

Among them, N _p represents the prediction time domain; Represents the stage cost function, expressed 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 formula shows that in the discretization process of the system, both the control input and the state are discretized and included as optimization variables in the resulting nonlinear programming problem (NLP), so the discretization of the differential equation is necessary. Implicit Euler discretization is chosen here because it provides sufficient accuracy and simplicity. The discretization model can be written as:

Through discretization, each Euler discrete point corresponds to a stage of the scene tree. To ensure the accuracy of discretization, several Euler discrete points are used in one stage of the scene tree.

To design the NLP problem in the prediction model, the NLP that can be obtained based on the above discrete prediction model is:

Constraints: x _l ≤ x ^opt ≤ x _u (17)

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

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

Among them, f( ) represents the objective function, and in this embodiment the objective function refers to x ^opt represents the augmented optimization vector, expressed as:

Among them, x ₀ represents the state quantity at the initial moment, Indicates the Nth state quantity in the prediction time domain N _p , /> Indicates the Nth control input in the prediction time domain N _p ; x _l and x _u indicate the state and control constraints imposed on the actual system, b _l and b _u indicate the unexpected constraints of the system, A is the augmentation coefficient, c _l and c _u represent the nonlinear constraints of the prediction model, and c(x ^opt ) represents the nonlinear constraints under the augmented prediction model.

The objective function f in Equation (16) contains all the terms needed to create the objective function in the form of Equation (11); the constraints in Equation (17) represent the state and control constraints imposed on the actual system; the linear (equal) constraints ( 18) includes unexpected constraints, nonlinear constraints (19) include discrete models of all nodes in the tree.

In this embodiment, for the automatic parking system based on multi-stage NMPC, implicit Euler is used to discretize the model differential equation, and the multi-stage optimization problem to be solved at each sampling time point can be written as:

Restrictions:

Among them, Q and R represent the weighting factors; ( )' represents the multi-stage optimization problem expression to be solved at each sampling moment, Δu' _k represents the increment of the control input, R _Δ represents the increment of the weighting factor, Indicates the state quantity of node j at time k+1, x _s represents the state quantity in the space domain, /> Indicates the control input of node j at time k+1, u _s represents the control input in the space domain, X represents the set of state quantities, U represents the set of control inputs, Δt represents the time increment, Φ(·) represents The specific expression of the prediction model under the domain.

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

In this embodiment, the process of designing the constraints of the parking system specifically includes:

Design the parking target position and attitude constraints, and constrain the vehicle speed and body attitude at the end time:

Among them, (x _ref , y _ref ) is the position of the parking target point; v(k _s +N _p ) represents the vehicle speed at the end time, Indicates the heading angle of the vehicle at the time of termination, x(k _s +N _p ) represents the lateral position of the vehicle at the time of termination, y(k _s +N _p ) represents the longitudinal position of the vehicle at the time of termination; k _s represents the current sampling position, N _p represents the prediction step long;

Design safety constraints to constrain the lateral and longitudinal positions of the vehicle during parking:

Among them, X _left and X _right are the left and right boundary positions of the warehouse respectively, a is the sum of the front overhang length, wheelbase and front safety margin, b is the sum of the rear overhang length and the rear safety margin, and Y _bound is the garage The side boundary of , w is the width of the vehicle, dist _min represents the minimum distance from the vehicle to the obstacle, and dist _safe represents the parking safety distance;

Design actuator constraints and constrain the actuators:

Among them, Δv _min and Δv _max are the upper and lower limits of the set speed increment input, Δδ _min and Δδ _max are the upper and lower limits of the set front wheel steering angle change input, and δ _min and δ _max are the front wheel angle upper and lower limits.

In the process of automatic parking, for the multi-order nonlinear MPC controller, first determine the number of uncertain parameters and the corresponding value range, and construct the initial uncertainty set where /> and /> Indicates that the uncertain parameter in the range of L×U is taken in the nth state node, and n _d represents the uncertain parameter of the nth state node; then the uncertainty set D is reduced by using the multi-order nonlinear MPC algorithm, including the following stages:

(1) Initialize the weight of each scene P _{(0, j)} = 1/S, j∈{1,...,S}, where S is the total number of scenes in the scene tree, and initialize the uncertainty set D;

(2) Calculate the model prediction values corresponding to the S scenes in the current time step k, and the model prediction value y _{(k, j)} of the jth scene in the time step k is expressed as:

Then calculate the residual ε _{(k,j) according to the model prediction value y (k,j)} at the current time step k and the process measurement value y _k at time k _, expressed as:

ε _{(k, j)} = y _k - y _{(k, j)} (30)

Among them, ε _{(k, j)} = [ε ₁ ,...ε _n ] ^T , ε ₁ ,...ε _n are the residual values of the corresponding n states; the superscript T represents the transpose operation;

(3) Calculate the Bayesian probability weight P _{(k,j) of the current time step k by using the residual information ε (k,j)} and each scene weight P _{(k-1,j) of} the previous time step, expressing for:

Among them, K is a weighting matrix expressed as:

Among them, cov( ) means to calculate the covariance;

(4) According to the Bayesian probability weight P(k,j) of each scene at the current time step k, find the scene p _max corresponding to the largest weight and the scene p _min corresponding to the smallest weight among the S scenes;

(5) Update _pmin and Dk ₊₁ by moving the least likely scenario _pmin to the most probable scenario _pmax . The update process includes:

Among them, β is the adaptive step size;

(6) At the next time step k+1, solve the multi-stage nonlinear MPC optimal control problem based on the scene tree constructed by the new uncertain set to obtain the optimal control action include:

Restrictions:

(7) Reinitialize the initial weight of each scene P _{(0, j)} = 1/S, j∈{1,...,S};

(8) Steps (2) to (7) are repeated until the automatic parking process ends, and automatic parking is realized.

The multi-stage NMPC scene update method is shown in Figure 4, and the optimization calculation is performed according to the set value and the output of the prediction model. The optimization calculation is based on the performance index and constraint conditions, and the output of the optimization calculation is the optimal control action. According to the optimal control action /> And calculate the deviation of each scene in different scenes; at the same time, according to the optimal control action /> Carry out rolling control, obtain y _k according to the controlled object and its uncertainty, input the prediction model according to y _k and the scene deviation, and obtain the predicted value of the model; in this embodiment, the set value as the input is the real-time value of the vehicle Speed and steering angle, the constraint conditions are the constraints of the parking system. When the present embodiment performs online scene update, the scene deviation is calculated according to the predicted value and the actual value, and the Bayesian probability weight is calculated by using the scene deviation. The scene is updated online to predict the real realization of the uncertainty value, so that the scene tree modeling approaches the real value of the uncertainty.

The interior point method is used to solve the control problem to obtain an optimal open-loop control sequence, and the first column element in the optimal open-loop control sequence is selected for controlling the vehicle to realize automatic parking, including:

The interior point method is used to solve the control problem, and the optimal open-loop control sequence Δu is obtained, and the first column elements in the optimal open-loop control sequence are used to control the vehicle to realize automatic parking. The optimal open-loop control sequence Δu is represented by for:

Among them, v _ref is the desired vehicle longitudinal speed, Δu is the optimal open-loop control sequence.

Through the above control method, this embodiment provides schematic diagrams of parking simulation in the three scenarios shown in FIGS. 5-6 , including vertical parking, parallel parking, and inclined parking.

Although the embodiments of the present invention have been shown and described, those skilled in the art can understand that various changes, modifications and substitutions can be made to these embodiments without departing from the principle and spirit of the present invention. and modifications, the scope of the invention is defined by 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.