Path planning method oriented to various parking scenarios

Abstract

A path planning method oriented to various parking scenarios, said method relating to the technical field of unmanned vehicles. The method comprises: building a vehicle kinematic model, and designing a multi-stage non-linear prediction control model; designing a multi-stage non-linear prediction controller, and designing a constraint function which takes as basic constraints the pose of a vehicle at a starting point, the pose of the vehicle at a target point, a parking speed, a parking steering angle, a non-holonomic kinematic constraint, and the minimum safe obstacle avoidance distance; integrating constraints of a parking system into an optimization control problem that satisfies the multi-stage non-linear prediction control model and the constraints of the parking system; and using an interior point method to solve the optimization control problem, so as to obtain an optimal control sequence, and applying elements in a first column in the sequence to vehicle low-level control. The present invention can ensure the controllability and accuracy of parking and can also improve the flexibility and efficiency of a parking process, thereby greatly optimizing the architecture of an automatic parking control system.

Description

A path planning method for various parking scenarios Technical Field

[0001] The present invention relates to the technical field of unmanned vehicles, and in particular to a path planning method for various parking scenarios. Background Art

[0002] With the development of the automotive industry and the increasing popularity of passenger cars, urban parking spaces are shrinking. To improve parking efficiency and save travel time, automated parking technology is becoming increasingly important within intelligent automotive technology. Path planning and trajectory tracking during parking are key attributes of autonomous vehicles. Path planning not only relies on algorithms to find a collision-free path from the starting point to the destination, but also requires the autonomous vehicle to perform adaptive obstacle avoidance in complex environments. Trajectory tracking involves tracking the planned parking path to complete the vehicle's autonomous parking process. Currently, existing automated parking technologies use separate path planning algorithms (such as the hybrid A* algorithm and the RRT* algorithm) and trajectory tracking algorithms (such as the pure tracking method, the PID algorithm, and the LQR algorithm). This results in complex parking algorithms, long parking times, and a sense of frustration during parking, which fails to meet the driving requirements of autonomous vehicles in parking scenarios.

[0003] Summary of the Invention

[0004] To address the inefficient and conservative dynamic parking problem caused by the separation of existing global path planning algorithms and trajectory tracking algorithms in automated parking scenarios, the present invention proposes a path planning method for multiple parking scenarios, which specifically includes the following steps:

[0005] Build a kinematic model of the entire vehicle;

[0006] Design a multi-stage nonlinear predictive control model based on the vehicle kinematic model;

[0007] Based on the multi-stage nonlinear predictive control model, a multi-stage nonlinear predictive controller is constructed and the evolution of uncertain parameters over time is represented in the form of a scenario tree.

[0008] Design parking system constraints;

[0009] Based on the parking system constraints, a cost function for the automatic parking system is designed, integrating the vehicle's speed planning, path planning, and trajectory tracking into an optimization control problem that satisfies the aforementioned multi-stage nonlinear predictive control model and the parking system constraints.

[0010] The interior point method is used to solve the optimization control problem and obtain the optimal control sequence of the system control problem. This control sequence minimizes the cost function of the parking system and satisfies the constraints specified within the prediction range. The first column element in this sequence is applied to the vehicle's underlying control to achieve steering and speed control of the vehicle during parking.

[0011] Furthermore, when constructing the vehicle kinematic model, the center point of the vehicle rear axle is used as the vehicle state reference point. The vehicle kinematic model expression is:

[0012] The continuous system state equation of the vehicle longitudinal motion can be expressed as:

[0013] The vehicle state Jacobian matrix is ​​obtained based on the vehicle's kinematic model:

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

[0015] Furthermore, based on the vehicle kinematic model, a multi-stage nonlinear predictive control model is designed, including:

[0016] Vectorize the vehicle kinematic equations to obtain the time domain prediction model:

[0017] Among them, f u(t) (□) represents the vectorized time domain prediction model; 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 variables of the center position of the vehicle rear axle in the time domain, 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;

[0018] The time domain prediction model is converted into a spatial domain prediction model, which is expressed as:

[0019] in, and Respectively represent the velocity components of the vehicle in the X and Y directions in the spatial domain state, represents the vehicle heading angle component in the spatial 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 as follows:

[0020] Among them, f u(s) (□) represents the vectorized spatial domain prediction model; ξ(s) is the state variable in the spatial domain; u(s) is the control input in the spatial domain;

[0021] The spatial domain prediction model is discretized to obtain a discretized prediction model. The discretized spatial domain prediction model is:

[0022] Among them, ξ(k s +1) indicates the relative position k of the current sampling point s The prediction of the discretized position corresponding to the next step size, represents the discretized prediction model, ξ(k s ) is the spatial domain sampling point position k s The state variable, u(k s ) is the spatial domain sampling point position k s Control input;

[0023] Convert the discretized prediction model into an incremental prediction model:

[0024] Among them, ξ(k s +1) means that in k s +1 state variables at sampling time, is the incremental prediction model; Δu(k s )=u(k s )-u(k s -1) indicates the current sampling point position k s and the sampling point position k at the previous moment s -1 difference in state variables.

[0025] Furthermore, a multi-stage nonlinear predictive controller is designed to represent the temporal evolution of uncertain parameters during the control process by establishing a scenario tree model, including:

[0026] The scenario tree assumes a discrete-time formulation of an uncertain nonlinear system, expressed as:

[0027] in, Represents the state variable of the jth node on the scene tree at the k+1 sampling time, represents the vehicle state variable of the parent node p(j) at sampling time k; represents the control input of node j at sampling time k, represents the uncertain parameters at the k sampling moment, j represents the state node on the scene tree, P(j) represents the nodes with the same parent node on the scene tree, r(j) represents the corresponding realization of the uncertainty of node j on the scene tree; f(□) represents the discrete-time uncertain nonlinear system expression;

[0028] Assuming the scene tree has the same number of branches at all nodes, at time step k is given by Given s different possible values ​​of uncertainty, S i Represents the i-th scenario from the root node x0 to one of the leaf nodes.

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

[0030] Constraints:

[0031] Among them, I represents all index combinations (j, k) corresponding to the nodes covered by the scene tree, S represents the number of scenes, ω i Represents each scene S i The corresponding weight; and represents the state with the same parent node at time k, P(j) and P(l) represent the two nodes j and l with the same parent node respectively. and Represents control inputs with the same parent node; Represents each scene S i The cost function of each scene in S scenes is defined as:

[0032] in, represents the stage cost function, N p represents the prediction time domain;

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

[0034] in, represents 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 uncertainty factor;

[0035] The discrete nature of the scene tree requires a time-discrete model of the system. We choose implicit Euler discretization and the discretization model can be written as:

[0036] 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.

[0037] Furthermore, the nonlinear programming problem based on the discretized prediction model can be expressed as: Constraints: x l ≤x opt ≤x u b l ≤Ax opt ≤b u c l ≤c(x opt )≤c u

[0038] Where f(□) represents the objective function; x opt represents the augmented optimization vector, which is expressed as:

[0039] Among them, x0 represents the state quantity at the initial moment, Indicates that in the prediction domain N p The next N state quantity, Indicates that in the prediction domain N p The Nth control input; x l and x u represents the state and control constraints imposed on the actual system, b l and b u represents the unexpected constraints of the system, A is the augmentation coefficient, c l and c u represents the nonlinear constraints of the prediction model, c(x opt ) represents the nonlinear constraints under the augmented prediction model.

[0040] Furthermore, for the automatic parking system based on the multi-stage nonlinear programming controller, the implicit Euler method is used to discretize the model differential equations. The multi-stage optimization problem to be solved at each sampling time point can be written as: Constraints: like but

[0041] Where Q and R represent weighting factors; (□)' represents the expression of the multi-stage optimization problem to be solved at each sampling moment, Δu' kRepresents the increment of control input, R Δ represents the increment of the weighting factor, represents the state of node j at time k+1, x s represents the state quantity in the spatial domain, represents the control input of node j at time k+1, u s Represents the control input in the spatial domain, X represents the state quantity set, U represents the control input set, Δt represents the time increment, and Φ(□) represents the specific expression of the prediction model in the continuous time domain.

[0042] Furthermore, the constraints of the parking system are constructed, including:

[0043] Design parking target position and posture constraints, and constrain the vehicle speed and posture at the end time:

[0044] Among them, (x ref ,y ref ) is the location of the parking target point; v(k s +N p ) represents the vehicle speed at the end time, represents the heading angle of the vehicle at the end time, x(k s +N p ) represents the lateral position of the vehicle at the end time, y(k s +N p ) represents the longitudinal position of the vehicle at the end time; k s Indicates the current sampling position, N p Indicates the prediction step length;

[0045] Design safety constraints to constrain the lateral and longitudinal positions of the vehicle during parking: X left +b≤x(k s +1)≤X right -ay(k s +1)≥Y bound +w / 2 i=1,2,3,…,N p dist min ≥dist safe

[0046] Among them, X left and X right are the left and right boundary positions of the storage location, a is the sum of the front overhang length, wheelbase and front safety margin, b is the sum of the rear overhang length and rear safety margin, Y bound is the side boundary of the garage, w is the width of the vehicle, dist min Indicates the minimum distance from the vehicle to the obstacle, dist safe Indicates safe parking distance;

[0047] Design actuator constraints and constrain the actuator: Δv min ≤Δv≤Δv max δ min ≤δ≤δ max Δδ min ≤Δδ≤Δδ max

[0048] Where Δv min and Δv max It is the upper and lower limits of the speed increment input, Δδ min and Δδ max is the upper and lower limits of the front wheel steering angle change input, δ min and δ max These are the upper and lower limits of the front wheel turning angle.

[0049] Furthermore, during the automatic parking process, for the multi-order nonlinear MPC controller, the number of uncertain parameters and their corresponding value ranges are determined (the uncertain factors in the present invention include speed and steering, as well as external uncertainties during the parking process. The uncertain factors in the present invention refer to emergencies, such as the sudden appearance of pedestrians or obstacles on the driving path during the vehicle's driving process, which requires braking or changing the driving path). The initial uncertainty set is constructed. The uncertainty set D is reduced using a multi-order nonlinear MPC algorithm, which includes the following stages:

[0050] (1) Initialize the weight P of each scene (0,j) =1 / S,j∈{1,…,S}, where S is the total number of scenes in the scene tree, and the uncertainty set D is initialized;

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

[0052] Then the model prediction value y is calculated based on the current time step k. (k,j) and process measurement value y k , that is, predict the parking state at the next moment through the current state of the vehicle, control input and uncertain factors, and calculate the residual ε (k,j) , expressed as: ε (k,j) =y k -y (k,j)

[0053] Among them, ε (k,j) =[ε1,…ε n ] T ,ε1,…ε n is the residual value of the corresponding n states;

[0054] (3) Using residual information ε (k,j) and each scene weight P at the previous time step (k-1,j) Calculate the Bayesian probability weight P for the current time step k (k,j) , expressed as:

[0055] Among them, K is a weight matrix, expressed as:

[0056] Among them, cov(□) means calculating covariance;

[0057] (4) According to the Bayesian probability weight P(k, j) of each scene at the current time step k, find the scene p corresponding to the largest weight among the S scenes max The scene p corresponding to the minimum weight min ;

[0058] (5) By putting the most unlikely scenario p min Move to the most likely scenario p max To update p min and D k+1 The update process includes: min =p min +β(p max -p min ) D k+1 ∈D k ∈…∈D1

[0059] Among them, β is the adaptive step size;

[0060] (6) At the next time step k+1, the scenario tree constructed based on the new uncertainty set is used to solve the multi-stage nonlinear MPC optimal control problem and obtain the optimal control action include: Constraints: like but

[0061] (7) Reinitialize the initial weight P of each scene (0,j) =1 / S,j∈{1,…,S};

[0062] (8) Repeat steps (2) to (7) until the automatic parking process is completed and automatic parking is achieved.

[0063] Furthermore, an 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 to control the vehicle to achieve automatic parking, including:

[0064] The control problem is solved using the interior point method to obtain the optimal open-loop control sequence Δu. The first column element in the optimal open-loop control sequence is used to control the vehicle to achieve automatic parking. The optimal open-loop control sequence Δu is expressed as:

[0065] Among them, v ref is the desired vehicle longitudinal velocity, and Δu is the optimal open-loop control sequence.

[0066] The present invention is based on a multi-order nonlinear MPC algorithm design, which can fully consider the system constraints and uncertain parameters and has good robustness. Moreover, the multi-order nonlinear MPC controller designed in the present invention integrates path planning and trajectory tracking into a single optimization problem to solve, which can ensure controllability and accuracy during parking, while improving the flexibility and efficiency of the parking process, greatly optimizing the architecture of the automatic parking control system. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] FIG1 is a schematic diagram 1 of a vehicle kinematic model in a path planning method for various parking scenarios according to the present invention;

[0068] FIG2 is a schematic diagram 2 of a vehicle kinematic model in a path planning method for various parking scenarios according to the present invention;

[0069] FIG3 is a schematic diagram of a scenario tree of a multi-order nonlinear MPC in a path planning method for various parking scenarios according to the present invention;

[0070] FIG4 illustrates an MSNMPC scenario updating method in a path planning method for multiple parking scenarios according to the present invention;

[0071] FIG5 is a schematic diagram of a perpendicular parking scenario in a path planning method for multiple parking scenarios according to the present invention;

[0072] FIG6 is a schematic diagram of a parallel parking scenario in a path planning method for multiple parking scenarios according to the present invention;

[0073] FIG7 is a schematic diagram of an inclined parking scenario in a path planning method for multiple parking scenarios according to the present invention. DETAILED DESCRIPTION

[0074] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

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

[0076] Build a kinematic model of the entire vehicle;

[0077] Design a multi-stage nonlinear predictive control model based on the vehicle kinematic model;

[0078] Based on a multi-stage nonlinear predictive control model, a multi-stage nonlinear predictive controller is designed, along with a constraint function based on the vehicle's starting point position, target point position, parking speed, parking steering angle, nonholonomic kinematic constraints, and minimum obstacle avoidance safety distance. The temporal evolution of the uncertain parameters is represented in the form of a scenario tree. A cost function for the automated parking system is designed, integrating the vehicle's speed planning, path planning, and trajectory tracking into an optimization control problem that satisfies the multi-stage nonlinear predictive control model and parking system constraints.

[0079] The interior point method is used to solve the optimization control problem, and an optimal control sequence is obtained, which integrates path planning, parking speed, and system constraints. This control sequence minimizes the cost function of the parking system and satisfies the constraints specified within the prediction range. The first column element in this control sequence is applied to the vehicle's underlying control to achieve steering and speed control of the vehicle during parking.

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

[0081] Build a kinematic model of the entire vehicle;

[0082] Design a multi-stage nonlinear model predictive control (MSNMPC) model based on the vehicle kinematic model;

[0083] Based on the multi-stage nonlinear model predictive controller, the temporal evolution of uncertain parameters is represented in the form of a scenario tree;

[0084] Design parking system constraints;

[0085] Based on the parking system constraints, a cost function for the automatic parking system is designed, integrating the vehicle's speed planning, path planning, and trajectory tracking into an optimization control problem that satisfies the aforementioned multi-stage nonlinear predictive control model and the parking system constraints.

[0086] The interior point method is used to solve the optimization control problem and obtain the optimal control sequence of the system control problem. This control sequence minimizes the cost function of the parking system and satisfies the constraints specified within the prediction range. The first column element in this sequence is applied to the vehicle's underlying control to achieve steering and speed control of the vehicle during parking.

[0087] Based on a multi-stage nonlinear MPC controller, the controller plans and generates an optimal parking path based on the starting point position and the target point position. Then, during the trajectory tracking process, the vehicle posture at the next moment is predicted in real time based on the vehicle posture at the previous moment. The current posture and the posture at the next moment change in real time, and the vehicle prediction step size at adjacent moments is the same, which can further optimize the automatic parking process.

[0088] Multi-stage nonlinear MPC provides an idea for closed-loop optimization using feedback information. This method treats the parking process as a closed-loop system and represents the uncertainty propagation in the prediction domain using a discrete scenario tree. It then calculates different control input trajectories for different scenarios to make the parking process safe and reliable.

[0089] The vehicle kinematic model is shown in Figures 1 and 2. Based on the vehicle kinematic model, the v in Figure 1 is f represents the center speed of the vehicle's front axle, Indicates the lateral speed of the front axle of the vehicle. The speed of the rear axle of the vehicle can be calculated by the two speeds. In Figure 2 Represent the desired steering angle of the vehicle and construct the kinematic constraints for the parking scenario. The process specifically includes:

[0090] Taking the midpoint of the rear axle of the vehicle as the reference point of the vehicle state, the posture state vector of the vehicle is The vehicle kinematic model expression is:

[0091] Among them, (x,y) is the coordinate of the midpoint of the rear axle, is the vehicle heading angle, L is the wheelbase, v is the rear axle speed, and δ is the front wheel steering angle; and are the vehicle's velocity components in the X and Y directions, Represents the velocity component in the heading angle direction.

[0092] The continuous system state equation of the vehicle longitudinal motion can be expressed as:

[0093] The vehicle state Jacobian matrix is ​​obtained based on the vehicle's kinematic model:

[0094] Wherein, u represents the control input;

[0095] 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:

[0096] Among them, f u(t) (□) represents the vectorized time domain prediction model; 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 variables of the center position of the vehicle rear axle in the time domain, 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) represents the time domain.

[0097] Formula (6) is used to transform the time domain prediction model into a spatial domain prediction model, that is, the state variables in the time domain are derived in the spatial domain to obtain the spatial domain prediction model. The conversion process specifically includes:

[0098] in, and Respectively represent the velocity components of the vehicle in the X and Y directions in the spatial domain state, represents the vehicle heading angle component in the spatial 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) in this embodiment represents the spatial domain.

[0099] The spatial domain prediction model is obtained as follows:

[0100] Among them, f u(s) (□) represents the Fourier transform operation; ξ(s) is the state variable in the spatial domain; u(s) is the control input in the spatial domain; the spatial domain prediction model is discretized to obtain a discretized prediction model, the discretized spatial domain prediction model:

[0101] Among them, (k s +1) indicates the relative position of the current sampling point (k s )The position corresponding to the next step, that is, ξ(k s +1) indicates the relative position k of the current sampling point s The prediction of the discretized position corresponding to the next step size; represents a discrete operation, ξ(k s ) is the spatial domain sampling point position k s The state variable, u(k s ) is the spatial domain sampling point position k s Control input;

[0102] Convert the discretized spatial domain prediction model into an incremental prediction model:

[0103] Among them, ξ(k s +1) means that in k s +1 state variables at sampling time, It is an incremental prediction model, that is, the spatial domain sampling point position k in formula (8) is s The control input is replaced by Δu(k s ) to obtain the incremental prediction model; Δu(k s )=u(k s )-u(k s -1) indicates the current sampling point position k s and the sampling point position k at the previous moment s -1 difference in state variables.

[0104] Design a multi-stage NMPC controller and establish a suitable scenario tree model, as shown in Figure 3. The scenario tree model in Figure 3 represents the process of transferring to another scenario under a certain scenario driven by the control vector and uncertain factors. For example, the vehicle state at time k (current time) is represented by x k , the control vector for the first scenario at time k and the uncertainty factors in the first scenario at time k The vehicle state of the first scene at time k+1 is transferred to the vehicle state of the first scene at time k+1. The scene tree model is established in this way. In Figure 3, the vehicle state x at time k is k , under the control variables (the control variables in this invention refer to the variables that control the vehicle's speed and steering angle to drive the vehicle to the target position) and uncertain factors (the uncertain factors in this invention refer to the unexpected factors that the vehicle may encounter in the given track during the driving process of the control variables, such as the sudden appearance of obstacles and pedestrians on the road, or the need to pass or meet oncoming vehicles, which may cause the vehicle's track to change or brake, etc.) in the three scenarios, there may be three states at time k+1. Taking the first scenario as an example, the state obtained at time k+1 is This state will also produce three different state quantities at time k+2 driven by the control variables and uncertain factors in the three scenarios, and the scenario tree will be generated in the same way.

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

[0106] in, Represents the state variable of the jth node on the scene tree at the k+1 sampling time, Represents the vehicle state of the parent node p(j) at sampling time k; represents the control input of node j at sampling time k, represents the uncertain parameter at the k sampling moment, j represents the state node on the scene tree; P(j) represents the node with the same parent node on the scene tree, for example, in Figure 3, Indicates the 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 of a control vector and an uncertainty factor in a certain scene at a certain moment; f(□) is the state transfer function. In this embodiment, the state transfer mapping is realized by a finite state machine, that is, the vehicle state quantity of the parent node p(j) at time k is converted to Uncertain parameters at k sampling time And the control variables under the jth scenario at time k As the input of the finite state machine, the finite state machine outputs the vehicle state quantity in the jth scenario at the next moment

[0107] Assuming the scene tree has the same number of branches at all nodes, at time step k is given by Given s different possible values ​​of uncertainty, S i Represents the i-th scenario from the root node x0 to one of the leaf nodes.

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

[0109] Constraints: like but

[0110] Among them, I represents all index combinations (j, k) corresponding to the nodes covered by the scene tree, S represents the number of scenes, ω i Represents each scene S i The corresponding weight; and represents the state with the same parent node at time k, P(j) and P(l) represent the two nodes j and l with the same parent node respectively. and Represents control inputs with the same parent node; Represents each scene S i The cost function of each scene in S scenes is defined as:

[0111] Among them, N p represents the prediction time domain; Represents the stage cost function, which is expressed as:

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

[0113] The above equation shows that in the system discretization process, both the control input and the state are discretized and included as optimization variables in the resulting nonlinear programming problem (NLP). Therefore, 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:

[0114] 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.

[0115] The NLP problem in the design 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)

[0116] Wherein, f(□) represents the objective function. In this embodiment, the objective function is represents the augmented optimization vector, which is expressed as:

[0117] Among them, x0 represents the state quantity at the initial moment, Indicates that in the prediction domain N p The next N state quantity, Indicates that in the prediction domain N p The Nth control input; x l and x u represents the state and control constraints imposed on the actual system, b l and b u represents the unexpected constraints of the system, A is the augmentation coefficient, c l and c u represents the nonlinear constraints of the prediction model, c(x opt ) represents the nonlinear constraints under the augmented prediction model.

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

[0119] In this embodiment, for the automatic parking system based on multi-stage NMPC, the model differential equation is discretized using implicit Euler. The multi-stage optimization problem to be solved at each sampling time point can be written as: Constraints: like but

[0120] Where Q and R represent weighting factors; (□)' represents the expression of the multi-stage optimization problem to be solved at each sampling moment, Δu' k Represents the increment of control input, R Δ represents the increment of the weighting factor, represents the state of node j at time k+1, x s represents the state quantity in the spatial domain, represents the control input of node j at time k+1, u s Represents the control input in the spatial domain, X represents the state quantity set, U represents the control input set, Δt represents the time increment, and Φ(□) represents the specific expression of the prediction model in the continuous time domain.

[0121] The cost function (21) consists of the sum of the costs 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 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; the constraints include boundaries on the state (22) and input (23), unexpected constraints (24), and discretized dynamics (25).

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

[0123] Design parking target position and posture constraints, and constrain the vehicle speed and posture at the end time:

[0124] Among them, (x ref ,y ref ) is the location of the parking target point; v(k s +N p ) represents the vehicle speed at the end time, represents the heading angle of the vehicle at the end time, x(k s +N p ) represents the lateral position of the vehicle at the end time, y(k s +N p ) represents the longitudinal position of the vehicle at the end time; k s Indicates the current sampling position, N p Indicates the prediction step length;

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

[0126] Among them, X left and X right are the left and right boundary positions of the storage location, a is the sum of the front overhang length, wheelbase and front safety margin, b is the sum of the rear overhang length and rear safety margin, Y bound is the side boundary of the garage, w is the width of the vehicle, dist min Indicates the minimum distance from the vehicle to the obstacle, dist safe Indicates safe parking distance;

[0127] Design actuator constraints and constrain the actuator:

[0128] Where Δv min and Δv max It is the upper and lower limits of the speed increment input, Δδ min and Δδ max is the upper and lower limits of the front wheel steering angle change input, δ min and δ max These are the upper and lower limits of the front wheel turning angle.

[0129] In the process of automatic parking, for the multi-order nonlinear MPC controller, the number of uncertain parameters and their corresponding value ranges are first determined, and the initial uncertainty set is constructed. in and Indicates the uncertainty parameter in the range of L×U in the nth state node, n d represents the uncertain parameters of the nth state node; then the uncertainty set D is reduced using a multi-order nonlinear MPC algorithm, which includes the following stages:

[0130] (1) Initialize the weight P of each scene (0,j) =1 / S,j∈{1,…,S}, where S is the total number of scenes in the scene tree, and the uncertainty set D is initialized;

[0131] (2) Calculate the model prediction value corresponding to the S scenes in the current time step k, and the model prediction value y for the jth scene in time step k (k,j) Expressed as:

[0132] Then the model prediction value y is calculated based on the current time step k. (k,j) and the process measurement value y at time k k Calculate the residual ε (k,j) , expressed as: ε (k,j) =y k -y (k,j) (30)

[0133] Among them, ε (k,j) =[ε1,…ε n ] T ,ε1,…ε n is the residual value of the corresponding n states; the superscript T represents the transposition operation;

[0134] (3) Using residual information ε (k,j) and each scene weight P at the previous time step (k-1,j) Calculate the Bayesian probability weight P for the current time step k (k,j) , expressed as:

[0135] Among them, K is a weight matrix, expressed as:

[0136] Among them, cov(□) means the calculated covariance;

[0137] (4) According to the Bayesian probability weight P(k, j) of each scene at the current time step k, find the scene p corresponding to the largest weight among the S scenes maxThe scene p corresponding to the minimum weight min ;

[0138] (5) By putting the most unlikely scenario p min Move to the most likely scenario p max To update p min and D k+1 The update process includes:

[0139] Among them, β is the adaptive step size;

[0140] (6) At the next time step k+1, the scenario tree constructed based on the new uncertainty set is used to solve the multi-stage nonlinear MPC optimal control problem and obtain the optimal control action include: Constraints: like but

[0141] (7) Reinitialize the initial weight P of each scene (0,j) =1 / S,j∈{1,…,S};

[0142] (8) Repeat steps (2) to (7) until the automatic parking process is completed and automatic parking is achieved.

[0143] The multi-stage NMPC scenario update method is shown in Figure 4. The optimization calculation is performed based on the set value and the output of the prediction model. The optimization calculation is based on the performance indicators and constraints, 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 Perform rolling control and obtain y according to the controlled object and its uncertainty k , according to y k And the scene deviation is input into the prediction model to obtain the predicted value of the model; in this embodiment, the set values ​​​​as input are the real-time speed and steering angle of the vehicle, and the constraints are the constraints of the parking system. When this embodiment performs online scene update, the scene deviation is calculated based on the predicted value and the actual value, and the Bayesian probability weight is calculated using the scene deviation. The scene is updated online with a fixed step size based on the weight information to predict the true realization of the uncertainty value, so that the scene tree modeling approaches the true value of the uncertainty.

[0144] Solving the control problem using an interior point method to obtain an optimal open-loop control sequence, selecting the first column element in the optimal open-loop control sequence to control the vehicle to achieve automatic parking, including:

[0145] The control problem is solved using the interior point method to obtain the optimal open-loop control sequence Δu. The first column element in the optimal open-loop control sequence is used to control the vehicle to achieve automatic parking. The optimal open-loop control sequence Δu is expressed as:

[0146] Among them, v ref is the desired vehicle longitudinal velocity, and Δu is the optimal open-loop control sequence.

[0147] Through the above control method, this embodiment provides schematic diagrams of parking simulation under three scenarios as shown in Figures 5 to 6, including perpendicular parking, parallel parking, and inclined parking.

[0148] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions, and variations may be made to these embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the appended claims and their equivalents.

Claims

1. A path planning method for various parking scenarios, characterized in that: The specific steps include: Build a kinematic model of the whole vehicle; Design a multi-stage nonlinear predictive control model based on the vehicle kinematic model; Based on the multi-stage nonlinear predictive control model, a multi-stage nonlinear predictive controller is constructed and the evolution of uncertain parameters over time is represented in the form of a scenario tree. Design parking system constraints; Based on the parking system constraints, the cost function of the automatic parking system is designed to integrate the vehicle's speed planning, path planning and trajectory tracking into an optimization control problem that satisfies the above 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 of the system control problem is obtained. This control sequence minimizes the cost function of the parking system and satisfies the constraints specified within the prediction range. The first column element in the sequence is applied to the vehicle's underlying control to achieve steering and speed control of the vehicle during parking.

2. A path planning method for multiple parking scenarios according to claim 1, characterized in that: When constructing the vehicle kinematic model, the center point of the vehicle rear axle is taken as the vehicle state reference point. The vehicle kinematic model expression is: The continuous system state equation of the vehicle longitudinal motion can be expressed as: The vehicle state Jacobian matrix is ​​obtained based on the vehicle's kinematic model: in, is the vehicle heading angle, L is the vehicle wheelbase, v is the rear axle speed, δ is the front wheel steering angle, and are the velocity components of the vehicle in the X and Y directions respectively, A represents the vehicle state matrix, B represents the vehicle control matrix, x represents the vehicle state quantity, u represents the control input quantity, Represents the velocity component in the heading angle direction.

3. The path planning method for multiple parking scenarios according to claim 1, characterized in that: Based on the vehicle kinematics model, a multi-stage nonlinear predictive control model is designed, including: Vectorize the vehicle kinematic equations to obtain the time domain prediction model: Among them, f u(t) (□) represents the vectorized time domain prediction model; 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 quantities of the center position of the vehicle rear axle in the time domain, 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, which is expressed as: in, and Respectively represent the velocity components of the vehicle in the X and Y directions in the spatial domain state, represents the vehicle heading angle component in the spatial 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 as follows: Among them, f u(s) (□) represents the vectorized spatial domain prediction model; ξ(s) is the state variable in the spatial domain; u(s) is the control input in the spatial domain; The spatial domain prediction model is discretized to obtain a discretized prediction model, the discretized spatial domain prediction model: Among them, ξ(k s +1) indicates the relative position k of the current sampling point s The prediction of the discretized position corresponding to the next step size, represents the discretized prediction model, ξ(k s ) is the spatial domain sampling point position k s The state variable, u(k s ) is the spatial domain sampling point position k s Control input of Convert the discretized prediction model into an incremental prediction model: Among them, ξ(k s +1) indicates that in k s +1 state variables at sampling time, is the incremental prediction model; Δu(k s )=u(k s )-u(k s -1) indicates the current sampling point position k s The sampling point position k at the previous moment s -1 difference in state variables.

4. The path planning method for multiple parking scenarios according to claim 1, characterized in that: Design a multi-stage nonlinear predictive controller to represent the evolution of uncertain parameters over time during the control process by establishing a scenario tree model, including: The scenario tree assumes a discrete-time formulation of an uncertain nonlinear system, expressed as: in, Represents the state variable of the jth node on the scene tree at the k+1 sampling time, represents the vehicle state variable of the parent node p(j) at sampling time k; represents the control input of node j at sampling time k, represents the uncertain parameters at the k sampling time, j represents the state node on the scene tree, P(j) represents the nodes with the same parent node on the scene tree, r(j) represents the corresponding realization of the uncertainty of the j node on the scene tree; f(□) represents the discrete-time uncertain nonlinear system expression; Assuming the scene tree has the same number of branches at all nodes, at time step k, Given s different possible values ​​of uncertainty, S i Represents the i-th scene from the root node x0 to one of the leaf nodes.

5. The path planning method for multiple parking scenarios according to claim 4, characterized in that: The optimal control problem based on the scenario-based multi-stage formulation at each time step k can be expressed as: Constraints: like but Where 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 each scene S i The corresponding weights; and represents the state with the same parent node at time k, P(j) and P(l) represent the two nodes j and l with the same parent node respectively. and Represents control inputs with the same parent node; Represents each scene S i The cost function of 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; The model in the field of process control is represented by a set of ordinary differential equations in the continuous time domain, which can be written as: in, represents 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 uncertainty factor; The discrete nature of the scene tree requires a time-discrete model of the system. We choose implicit Euler discretization and 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.

6. A path planning method for multiple parking scenarios according to claim 5, characterized in that: 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 Where f(□) represents the objective function; x opt represents the augmented optimization vector, expressed as: Among them, x0 represents the state quantity at the initial moment, Indicates that in the prediction time domain N p The next Nth state quantity, Indicates that in the prediction time domain N p The Nth control input; x l and x u represents the state and control constraints imposed on the actual system, b l and b u represents the unexpected constraints of the system, A is the augmentation coefficient, c l and c u represents the nonlinear constraints of the prediction model, c(x opt ) represents the nonlinear constraints under the augmented prediction model.

7. The path planning method for multiple parking scenarios according to claim 5, characterized in that: For the automatic parking system based on multi-stage nonlinear programming controller, implicit Euler is used to discretize the model differential equation. The multi-stage optimization problem to be solved at each sampling time point can be written as: Constraints: like but Where Q and R represent weighting factors; (□)' represents the expression of the multi-stage optimization problem to be solved at each sampling time, Δu' k represents the increment of control input, R Δ represents the increment of the weighting factor, represents the state of node j at time k+1, x s represents the state quantity in the space domain, represents the control input of node j at time k+1, u s represents the control input in the spatial domain, X represents the state quantity set, U represents the control input quantity set, Δt represents the time increment, and Φ(□) represents the specific expression of the prediction model in the continuous time domain.

8. The path planning method for multiple parking scenarios according to claim 1, characterized in that: Constraints for building a parking system include: Design the parking target position and posture constraints, and constrain the vehicle speed and body posture at the end time: Among them, (x ref ,y ref ) is the location of the parking target point; v(k s +N p ) represents the vehicle speed at the end time, represents the heading angle of the vehicle at the end time, x(k s +N p ) represents the lateral position of the vehicle at the end time, y(k s +N p ) represents the longitudinal position of the vehicle at the end time; k s Indicates the current sampling position, N p represents the prediction step length; 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,…,N p dist min ≥dist safe Among them, X left and X right are the left and right boundary positions of the storage location, a is the sum of the front overhang length, wheelbase and front safety margin, b is the sum of the rear overhang length and rear safety margin, Y bound is the side boundary of the garage, w is the width of the vehicle, dist min Indicates the minimum distance from the vehicle to the obstacle, dist safe Indicates safe parking distance; Design actuator constraints and constrain the actuator: Δv min ≤Δv≤Δv max d min ≤δ≤δ max Dd min ≤Δδ≤Δδ max Where Δv min and Δv max is the upper and lower limits of the set speed increment input, Δδ min and Δδ max is the upper and lower limits of the input of the front wheel steering angle change, δ min and δ max Are the upper and lower limits of the front wheel turning angle.

9. The path planning method for multiple parking scenarios according to claim 1, characterized in that: In the automatic parking process, for the multi-order nonlinear MPC controller, the number of uncertain parameters and their corresponding value ranges are determined, and the initial uncertainty set is constructed. The uncertainty set D is reduced using a multi-order nonlinear MPC algorithm, which includes the following stages: (1) Initialize the weight P of each scene (0,j) =1 / S,j∈{1, … ,S}, where S is the total number of scenes in the scene tree, and the uncertainty set D is initialized; (2) Calculate the model prediction value y corresponding to the S scenarios in the current time step k (k,j) : Then, the model predicts the value y based on the current time step k (k,j) and process measurement value y k Calculate the residual ε (k,j) , expressed as: ε (k,j) =and k -and (k,j) Among them, ε (k,j) =[ε1,…ε n ] T ,ε1,…ε n is the residual value of the corresponding n states; (3) Using residual information ε (k,j) and each scene weight P at the previous time step (k-1,j) Calculate the Bayesian probability weight P for the current time step k (k,j) , expressed as: Among them, K is a weight matrix, expressed as: Among them, cov(□) means calculating covariance; (4) Based on the Bayesian probability weight P(k,j) of each scene at the current time step k, find the scene p corresponding to the largest weight among the S scenes max The scene p corresponding to the minimum weight min ; (5) By putting the most unlikely scenario p min Move to the most likely scenario p max To update p min and D k+1 The update process includes: p min =p min +β(p max -p min ) D k+1 ∈D k ∈…∈D1 Among them, β is the adaptive step size; (6) At the next time step k+1, the scenario tree constructed based on the new uncertainty set is used to solve the multi-stage nonlinear MPC optimal control problem and obtain the optimal control action include: Constraints: like but (7) Reinitialize the initial weight P of each scene (0,j) =1 / S,j∈{1,…,S}; (8) Repeat steps (2) to (7) until the automatic parking process is completed and automatic parking is achieved.

10. The path planning method for multiple parking scenarios according to claim 1, characterized in that: The control problem is solved by an interior point method to obtain an 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 control problem is solved by the interior point method to obtain the optimal open-loop control sequence Δu. The first column element in the optimal open-loop control sequence is used to control the vehicle to achieve automatic parking. The optimal open-loop control sequence Δu is expressed as: Among them, v ref is the desired vehicle longitudinal velocity, and Δu is the optimal open-loop control sequence.

Images