Probabilistic nonlinear predictive controller and method based on uncertainty propagation via Gaussian distribution hypothesis density filter
Patent Information
- Application Number
- JP2024517607
- Authority / Receiving Office
- JP · JP
- Patent Type
- Patents
- Current Assignee / Owner
- Priority Date
- 2021-07-01
- Filing Date
- 2022-02-18
- Publication Date
- 2024-11-22
- Estimated Expiration
- 2042-02-18
Smart Images

Figure 0007588761000075 
Figure 0007588761000076 
Figure 0007588761000077
Abstract
Description
[Technical field]
[0001] The present invention relates generally to predictive control, and more particularly to a method and apparatus for high accuracy propagation of mean and covariance information of predicted state variables for stochastic predictive control of nonlinear dynamic systems in the presence of uncertainty. [Background technology]
[0002] Nonlinear model predictive control (NMPC) is mature and has shown the ability to handle relatively complex constrained processes. Predictive controllers such as NMPC can be used in many applications to control complex dynamical systems described by a set of nonlinear differential equations, i.e. systems of ordinary differential equations (ODEs) or differential-algebraic equations (DAEs). Examples of such systems include production lines, vehicles, satellites, engines, robots, generators and other (semi-)automated controlled machines. Although NMPC exhibits inherent robustness due to feedback, such controllers do not directly consider uncertainty and therefore cannot guarantee the satisfaction of safety-critical constraints in the presence of model uncertainty and / or external disturbances. An alternative approach is robust NMPC, which relies on optimizing the control policy in worst-case scenarios in the presence of uncertainty and / or bounded disturbances. However, robust NMPC can be conservative in its control performance because the worst-case scenarios occur with extremely low probability.
[0003] Stochastic NMPC (SNMPC) aims to reduce the conservativeness of robust NMPC by incorporating a probabilistic description of uncertainties directly into the formulation of the optimal control problem (OCP). In SNMPC, constraints are required to be satisfied with a certain probability, i.e. by formulating so-called chance constraints that consider a specified but non-zero probability of constraint violation. Furthermore, stochastic NMPC is advantageous in settings where high performance is achieved in closed-loop operation near the boundaries of the plant's feasible region. In the general case, chance constraints are difficult to compute and can be easily solved, e.g. through nonlinear system dynamics. propagation In this paper, we propose an approximate formulation based on approximating the probability distribution of the state variables to be estimated.
[0004] Sampling techniques characterize stochastic system dynamics using a finite set of random realizations of uncertainty, but can be computationally expensive due to the often large number of samples required for uncertainty propagation. Scenario-based methods exploit an appropriate representation of the probability distribution, but the task of determining the number of scenarios leads to a trade-off between robustness and computational efficiency. Gaussian mixture approximations can be used to describe the state transition probability distributions, but adapting the weights is often computationally expensive. Another approach uses polynomial chaos (PC), replacing the implicit mapping with an expansion of orthogonal polynomial basis functions, but for time-varying uncertainties, PC-based stochastic NMPC requires many expansion terms.
[0005] Alternatively, for example, an explicit linearization of the nonlinear system dynamics based on Taylor series approximation can be used to approximate the stochastic chance constraints. However, the resulting linearization-based covariance propagation may not be accurate enough for nonlinear system dynamics. Moreover, the latter approach requires accurate estimation of the mean values of the state variables. propagationand may differ from the nominal values of the nonlinear system dynamics (i.e., values corresponding to zero uncertainty and / or zero disturbance). Therefore, in stochastic predictive control of nonlinear dynamical systems under uncertainty, a direct tractable yet accurate estimation of the mean and covariance information is required to formulate the stochastic chance constraints. propagation is necessary.
[0006] Direct optimal control methods rely on discretization of continuous-time differential equations based on a discretization of the control horizon and a corresponding parameterization of the control action over the prediction horizon. In addition, for stochastic predictive control applications, the direct OCP formulation can include a set of discrete-time or discretized equations to propagate the uncertainty of the nonlinear system dynamics based on a parameterization of the control feedback over the prediction horizon. The resulting large-scale nonlinear optimization problem or nonlinear program (NLP) can be solved by any nonlinear optimization solver. However, for real-time applications of predictive control of nonlinear systems, this nonlinear optimization problem needs to be solved under strict timing constraints, on embedded hardware with limited computational power, and limited available memory.
[0007] Stochastic predictive control of systems described by nonlinear differential equations requires the solution of a nonlinear stochastic optimal control problem at each control time step. Instead of solving each problem exactly, one real-time iteration of sequential quadratic programming (SQP) can be performed to update a guess at the solution from one time point to the next. Such Newton-type SQP algorithms require linearization of the nonlinear constraint and objective functions at each iteration of the algorithm. This linearization is particularly useful for accounting for uncertainties in the nonlinear system dynamics. propagationFor the set of equations describing the nonlinear differential equations, this can be costly, requiring Jacobian evaluations when using explicit integration methods, and in the case of implicit integration methods to discretize the nonlinear differential equations, which can require additional matrix decompositions, matrix-matrix multiplications, and / or iterative procedures to solve the nonlinear simultaneous equations.
[0008] Therefore, there is a need to increase the accuracy of propagating the mean and covariance information of state variables through nonlinear system dynamics to improve the closed-loop performance of the resulting SNMPC controller, and to reduce the computational cost of numerical optimization algorithms in real-time applications of stochastic predictive control for nonlinear dynamic systems under uncertainty. Summary of the Invention
[0009] The objective of some embodiments is to discretize the nonlinear differential equations describing the dynamic model of the system and to perform discrete-time analysis of the uncertainties of the nonlinear system dynamics. propagation The present invention provides a system and method for controlling a system under uncertainty by solving an inequality-constrained nonlinear dynamic optimization problem that includes probabilistic chance constraints based on: (a) a probability threshold for a given inequality constraint that is greater than or equal to a certain probability threshold;
[0010] Some embodiments of the present invention use a formulation of probabilistic chance constraints based on reinforcement for each inequality constraint with terms that depend on backoff coefficient values, constraint Jacobian matrix, and first and / or higher order moment integrals of predicted state values at a particular time step. In some embodiments of the present invention, the first and / or higher order moment integrals include a first moment integral that is the mean and a second moment integral that is the covariance. The covariance matrix can be efficiently calculated for state values at each time step within the control range using approximate linearization-based covariance propagation. Linearization-based covariance equations can lead to inaccurate propagation of mean and covariance information of state variables through nonlinear system dynamics. Therefore, in some embodiments of the present invention, a Gaussian assumed density filter (ADF) is used to provide highly accurate propagation of mean and covariance information of state variables through nonlinear system dynamics. propagation More accurate propagation of mean and covariance information of state variables can lead to improved performance of stochastic predictive control of nonlinear dynamic systems under uncertainty.
[0011] According to some embodiments of the present invention, a predictive controller for controlling a system under uncertainty subject to constraints on the states and control variables of the system may be provided. The predictive controller may include at least one processor and a memory storing instructions. The instructions, when executed by the at least one processor, cause the predictive controller to solve an inequality-constrained nonlinear dynamic optimization problem including a probabilistic chance constraint representing the uncertainty to generate a control signal at each control step of the predictive controller. The predictive controller solves a discrete-time approximation based on Gaussian Hypothesis Density Filtering (ADF) based on the probabilistic chance constraint to generate a control signal at each control step of the predictive controller. propagationThe equations are used to solve a direct optimal control structured nonlinear program (NLP) to perform approximate predictions of first and / or higher order moment integrals of the state probability distribution from one time step to the next time step within the prediction horizon as a function of the first and / or higher order moment integrals of the probability distribution of one or more of the time-varying modeling uncertainties and / or external disturbances until a termination condition is met. The instructions further cause the predictive controller to control the operation of the system using the control signal.
[0012] Some embodiments of the present invention are based on the recognition that when the constraint functions of the probabilistic chance constraints are linear or slightly nonlinear, linearization-based augmentation for each inequality constraint may be exact or sufficiently accurate, taking into account the covariance matrix of the predicted state values at each time step within the control range. Some embodiments of the present invention are based on the recognition that when the constraint functions of the probabilistic chance constraints are highly nonlinear, linearization-based augmentation may be inaccurate, but instead, ADFs can be used to directly compute high-precision mean and covariance information for each constraint function that depends nonlinearly on the state variables and / or control input variables.
[0013] For example, for nonlinear system dynamics, unscented Kalman filtering (UKF) is known to be more accurate than linearization-based propagation of mean and covariance information using, for example, extended Kalman filtering (EKF). Some embodiments of the present invention are based on the recognition that UKF is a special case of the more general family of linear-regression Kalman filtering (LRKF), which is part of the more general family of Gaussian distribution assumption density filters (ADFs). Some embodiments of the present invention are based on the recognition that ADF uses statistical linearization based on approximate matching of one and / or multiple higher-order moment integrals instead of explicit linearization based on Taylor series approximation (e.g., in EKF). Thus, while EKF is a first-order method based on explicit linearization for handling nonlinearities, the family of ADFs based on statistical linearization can achieve second-order or better accuracy in propagating mean and covariance information of state variables through nonlinear system dynamics.
[0014]
number
[0015] In some embodiments of the present invention, ADF is applied to continuous-time system dynamics to obtain continuous-time equations for the propagation of mean and covariance information of state variables. The resulting continuous-time OCP can be solved based on the time discretization and optimal control parameterization to arrive at a tractable nonlinear optimization problem that can be solved with a numerical optimization algorithm, for example, using direct single shooting, multiple shooting, direct collocation or pseudospectral methods.
[0016] In some embodiments of the present invention, ADF is applied to the discrete-time or discretized system dynamics to obtain discrete-time equations for the propagation of mean and covariance information of state variables. Some embodiments use discrete-time covariance propagation This is based on the realization that the equation can reduce computational cost and preserve the positive definiteness of the covariance matrix at each control time step. Some embodiments can include nonlinearity bounds in the covariance propagation to ensure that the covariance matrix is an overestimate of the exact covariance for the predicted state values at each time step, i.e., to yield consistent predictions, such that each of the probabilistic chance constraints is guaranteed to have a probability of violation below a certain threshold.
[0017] Some embodiments of the present invention are based on the recognition that feedback control actions should be taken into account in the forward propagation of uncertainty information of the mean and covariance of predicted state values. Some embodiments pre-stabilize the nonlinear system dynamics using a time-invariant or time-varying sequence of affine feedback gains to pre-stabilize the nonlinear system dynamics, resulting in state mean and covariance propagation equations that directly consider the effect of feedback control actions on uncertainties and / or disturbances in the future. For example, a linear-quadratic regulator with infinite range of linearized system dynamics at reference steady states and input values can be used to pre-stabilize the system dynamics in a stochastic nonlinear OCP formulation.
[0018] Using an approximate formulation of the stochastic chance constraints based on separate reinforcement of each inequality constraint, the resulting inequality-constrained nonlinear dynamic optimization problem can be solved using a Newton-type optimization algorithm based on sequential linearization of optimality and feasibility conditions. Examples of such Newton-type optimization algorithms include the interior point method (IPM) and sequential quadratic programming (SQP). Some embodiments of the present invention are characterized in that the SQP algorithm performs, at each iteration of the SQP optimization algorithm, a quadratic programming (QP) approximation of the stochastic nonlinear OCP for each inequality constraint and for each reinforced stochastic chance constraint, using a linear-quadratic approximation of the objective function, a linearization-based approximation of the discretized system dynamics, a discrete-time covariance propagation It is based on the recognition that the solution is based on equations and linearization-based approximations.
[0019] If the original dynamic model of the system is described by a set of continuous-time differential equations, some embodiments of the invention discretize the system dynamics using an explicit or implicit numerical integration method, e.g., an explicit or implicit Runge-Kutta method, to construct discrete-time or discretized ADF-based state-average and covariance propagation equations. Some embodiments are based on the recognition that at each iteration of a Newton-type optimization algorithm, the linearization of the state-average and covariance propagation equations requires evaluation of first and / or higher order derivatives for the nonlinear system dynamics, which may form a computationally expensive step when the dynamics are high-dimensional and / or computationally complex, e.g., when the dynamics include long nonlinear expressions and / or are described by a set of rigid or implicitly defined differential equations.
[0020] In some embodiments of the present invention, the optimization variables in the stochastic OCP formulation include factors of a matrix decomposition for the covariance matrix of the state variables at each time step of the control horizon, for example using a forward or inverse Cholesky decomposition of the state covariance matrix. Some embodiments of the present invention are based on the recognition that the ADF-based state mean and covariance propagation equations can be reformulated directly in terms of Cholesky factors, for example using a Cholesky decomposition operator, so that only the Cholesky factors and not the state covariance matrix need to be included in the optimization variables. Furthermore, some embodiments of the present invention are based on the recognition that the trajectory of the state covariance matrix can be calculated from the corresponding trajectory of the Cholesky factors, and therefore the state covariance matrix is guaranteed to always be positive definite at each control time step and each iteration of any optimization algorithm.
[0021] Some embodiments of the present invention are based on the realization that the Cholesky decomposition (forward or backward) is only defined for positive definite matrices, so it may be necessary to add a small regularization term to the ADF-based covariance propagation equation to ensure that the Cholesky decomposition operator is defined everywhere. First and higher order derivatives of the Cholesky decomposition operator can be computed by the forward or backward modes of algorithmic differentiation techniques.
[0022]
number
[0023]
number
[0024] Some embodiments of the present invention are based on the recognition that although an inexact derivative-based optimization algorithm converges to a feasible stochastic nonlinear OCP solution with respect to the system dynamics, the covariance propagation equation, the inequality constraints, and the probabilistic chance constraints, the solution may be suboptimal due to inexact derivative calculation. Instead, some embodiments of the present invention are based on an inexact derivative-based optimization algorithm with an adjoint-based gradient calculation to compensate for the use of inexact derivatives, which converges to a feasible and locally optimal stochastic nonlinear OCP solution. Note that the adjoint calculation of the covariance propagation equation requires evaluation of first and / or higher order derivatives of the system dynamics, which corresponds to a single gradient rather than a full Jacobian matrix. In some embodiments, the latter adjoint gradient calculation can be efficiently performed using a single sweep of the adjoint mode of algorithmic or automatic differentiation.
[0025] In some embodiments of the present invention, the proposed inexact optimization algorithm for SNMPC (possibly with adjoint-based gradient calculation) consists of three main computational steps. The first step prepares a linear-quadratic objective function, calculates vectors and mean and Jacobian matrices to prepare linearized equality and inequality constraints, evaluates an adjoint-based gradient calculation, and numerically removes the covariance matrix and / or Cholesky factor from each of the objective and constraint functions given the current trajectories of the predicted mean state and control values and the current values of the covariance matrix and / or Cholesky factor. The second step consists of solving the resulting block-structured QP sub-problem with one or more strengthened inequality constraints to approximate each of the probabilistic chance constraints. The third and final step includes the evolution of the corresponding updates of the Lagrange multipliers and updates of the trajectories of the covariance matrix and / or Cholesky factor over the control range in addition to the Newton-type updates for the predicted state and control value trajectories.
[0026] Some embodiments of the present invention are based on the recognition that a globalization technique may be necessary to improve the convergence behavior of a derivative-based optimization algorithm for solving a stochastic OCP problem, for example due to the highly nonlinear ADF-based state mean and covariance propagation equations. An example of such a globalization technique is a line search method for calculating step sizes in a Newton-type update of the primal and / or dual optimization variables to ensure that a sufficient reduction condition of a merit function that depends on both optimality and feasibility conditions can be satisfied at each iteration of the derivative-based optimization algorithm, for example using an SQP optimization method. In some embodiments of the present invention, the latter line search-based step size selection for a Newton-type update of the primal and / or dual optimization variables may be part of the third step in the optimization algorithm of the SNMPC controller.
[0027] Some embodiments of the present invention are based on the recognition that in addition to the covariance and / or Cholesky factors, the average state variables in the stochastic optimal control problem can also be numerically eliminated at each iteration based on a compression procedure, which uses discrete-time system dynamics to specify the state variables of each stage in the forecast horizon as a function of the initial state values and the control variables at all previous stages in the forecast horizon. This full or partial compression procedure results in a smaller but overall denser optimization problem, in which there are fewer or no equality constraints and the same amount of inequality constraints and probabilistic chance constraints, which are accounted for in terms of the remaining optimization variables in the OCP. Some embodiments of the present invention are based on the recognition that the same inexact derivative-based optimization algorithm can be used in combination with such a compression procedure. More specifically, the numerical elimination of the average state variables is additionally performed in a first step, the dense QP solution is performed in a second step, while the expansion of the compressed state variables is additionally performed in a third step of the inexact optimization algorithm for SNMPC.
[0028] Some embodiments of the present invention use a real-time iterative method for solving nonlinear stochastic OCPs by performing one iteration of the proposed exact or inexact derivative-based optimization algorithm at each control time step in the stochastic predictive controller. That is, at each control time step, only one preparation, solving, and evolving step needs to be performed for a block-structured local (convex) QP approximation of the nonlinear stochastic optimization problem. The preparation of the QP includes linearization of the nonlinear equations imposing the discretized nonlinear system dynamics, linearization of the nonlinear inequality constraints, compression or deletion of the covariance matrix and / or Cholesky factors, and optional evaluation options of the adjoint gradient calculation. Based on this preparation, the resulting block-structured QP is solved, followed by an evolving step that updates all the main optimization variables and Lagrange multiplier values to generate the control solution used to control the system at each step of the stochastic nonlinear predictive controller.
[0029] The presently disclosed embodiments are further described with reference to the accompanying drawings, in which: The drawings shown are not necessarily to scale, with emphasis generally being placed upon illustrating the principles of the presently disclosed embodiments. [Brief description of the drawings]
[0030] [Figure 1A] FIG. 2 is a block diagram illustrating a predictive controller and feedback loop for systems with uncertainty according to some embodiments. [Figure 1B] FIG. 2 is a block diagram illustrating a probabilistic predictive controller and feedback loop for systems with uncertainty according to some embodiments. [Figure 1C] FIG. 2 is a block diagram illustrating a controller implemented with a CPU processor and memory and a feedback system according to some embodiments of the present invention. [Figure 2A]FIG. 2 is a schematic diagram illustrating prediction of state probability distribution from one time step to the next using a Gaussian Assumption Density Filter (ADF) for nonlinear system dynamics according to an embodiment of the present invention. [Figure 2B] FIG. 2 is a schematic diagram illustrating prediction of state probability distributions including time-varying uncertainties and / or disturbances affecting nonlinear system dynamics according to some embodiments of the present invention. [Figure 2C] FIG. 2 is a block diagram illustrating discrete-time or discretization propagation of state mean and covariance information based on explicit linearization of nonlinear system dynamics according to some embodiments of the present invention. [Figure 2D] FIG. 2 is a block diagram illustrating discrete-time or discretization propagation of state mean and covariance information based on statistical linearization of nonlinear system dynamics according to some embodiments of the present invention. [Figure 3A] FIG. 1 is a block diagram illustrating a stochastic nonlinear model predictive control (SNMPC) method for implementing a stochastic predictive controller for a controlled system under uncertainty, according to some embodiments. [Figure 3B] FIG. 2 is a block diagram illustrating an SNMPC method for solving direct optimal control structured nonlinear programs (NLPs) based on discrete-time system dynamics and stochastic chance constraints, according to some embodiments. [Figure 3C] FIG. 2 is a block diagram illustrating an SNMPC method for solving direct optimal control structured NLP based on state mean and covariance explicit linearization based propagation equations according to some embodiments. [Figure 3D] FIG. 2 is a block diagram illustrating an SNMPC method for solving direct optimal control structured NLP based on statistical linearization of state mean and covariance based propagation equations according to some embodiments. [Figure 4A] FIG. 2 is a block diagram illustrating approximation of probabilistic chance constraints in a probabilistic predictive controller according to some embodiments. [Figure 4B] FIG. 1 illustrates the idea behind the formulation and approximation of probabilistic chance constraints in a stochastic predictive controller. [Figure 5A] FIG. 2 is a block diagram illustrating state mean and covariance propagation of nonlinear system dynamics in continuous time according to some embodiments. [Figure 5B] FIG. 2 is a block diagram illustrating state mean and covariance propagation of nonlinear system dynamics in discrete time according to some embodiments. [Figure 5C] FIG. 2 is a block diagram illustrating explicit linearization-based state mean and covariance propagation of pre-stabilized nonlinear system dynamics in discrete time according to some embodiments. [Figure 5D] FIG. 2 is a block diagram illustrating statistical linearization-based state mean and covariance propagation of pre-stabilized nonlinear system dynamics in discrete time according to some embodiments. [Figure 5E] FIG. 2 is a block diagram illustrating the formulation and approximation of control boundaries as probabilistic chance constraints resulting from state-feedback control actions according to some embodiments of the present invention. [Figure 6A] FIG. 1 is a block diagram illustrating an iterative differential-based optimization procedure for solving a constrained nonlinear optimal control problem at each time step in a stochastic predictive controller. [Figure 6B] FIG. 1 shows a compact formulation of the optimal control structured NLP that needs to be solved by a stochastic predictive controller. [Figure 6C] FIG. 2 is a block diagram illustrating an exact Jacobian-based local quadratic programming (QP) approximation of optimal control structured NLP in a stochastic predictive controller according to some embodiments of the present invention. [Figure 7A] FIG. 2 is a block diagram illustrating the explicit, sequential calculation of state mean values and covariance matrix values given current average state values and control values over a prediction time horizon according to some embodiments. [Figure 7B] FIG. 1 is a block diagram illustrating an iterative and inexact SQP optimization algorithm for efficiently implementing a stochastic predictive controller in accordance with some embodiments of the present invention without requiring evaluation of one or more first and / or higher order derivatives of a nonlinear equality constraint function. [Figure 8A] FIG. 2 is a block diagram illustrating Jacobian matrix approximation and corresponding adjoint-based gradient correction, condensed constraint evaluation, and Lagrange multiplier expansion steps for an implementation of the adjoint-based inexact SQP optimization algorithm according to some embodiments. [Figure 8B] FIG. 1 is a block diagram illustrating an iterative and inexact SQP optimization algorithm for efficient realization of a stochastic predictive controller based on adjoint-based gradient correction to improve convergence properties. [Figure 8C] FIG. 2 is an algorithmic diagram illustrating a real-time variant of an adjoint-based inexact SQP optimization algorithm for implementing stochastic nonlinear model predictive control, according to some embodiments of the present invention. [Figure 8D] FIG. 2 illustrates an example of a search procedure for a desired step size selection such that a sufficient reduction condition is met for the step size value of a real-time adjoint-based SQP optimization algorithm in a stochastic predictive controller according to some embodiments of the present invention. [Figure 9A] FIG. 1 is a block diagram illustrating the exploitation of block-structured sparsity of the constraint Jacobian matrix in the forward recursion to compute an update sequence of the Cholesky factor of the state covariance matrix in an adjoint-based SQP optimization algorithm. [Figure 9B] FIG. 2 is a block diagram illustrating the exploitation of block-structured sparsity of the constraint Jacobian matrix in the forward recursion to compute condensed inequality constraint values in the adjoint-based SQP optimization algorithm. [Figure 9C] FIG. 2 is a block diagram illustrating the exploitation of block-structured sparsity of the constraint Jacobian matrix in the forward recursion to compute condensed equality constraint values in the adjoint-based SQP optimization algorithm. [Figure 9D] FIG. 2 is a block diagram illustrating the exploitation of block-structured sparsity of the constraint Jacobian matrix in the backward recursion to compute updated Lagrangian multiplier values in the adjoint-based SQP optimization algorithm. [Figure 10A]FIG. 1 is a schematic diagram illustrating a vehicle including a stochastic predictive controller employing the principles of some embodiments. [Figure 10B] FIG. 10 is a schematic diagram illustrating the interaction between a stochastic predictive controller employing principles of some embodiments and a controller of a vehicle 1001 according to some embodiments. [Figure 10C] FIG. 1 is a schematic diagram illustrating a method for motion planning and / or probabilistic predictive control of a controlled vehicle under uncertainty employing principles of some embodiments of the present invention. [Figure 10D] FIG. 1 is a schematic diagram illustrating a method for motion planning and / or probabilistic predictive control of a controlled vehicle under uncertainty employing principles of some embodiments of the present invention. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS
[0031] Description of the embodiments Various embodiments of the present invention are described below with reference to the figures. It should be noted that the figures are not drawn to scale, and elements of similar structure or function are represented by similar reference numerals throughout the figures. Furthermore, the figures are merely intended to facilitate the description of certain embodiments of the present invention. They are not intended as an exhaustive description of the present invention or as a limitation of the scope of the present invention. In addition, aspects described in conjunction with a specific embodiment of the present invention are not necessarily limited to that embodiment, but may be implemented in any other embodiment of the present invention.
[0032] The following description provides exemplary embodiments only and is not intended to limit the scope, applicability, or configuration of the present disclosure. Rather, the following description of exemplary embodiments provides those skilled in the art with an enabling description for implementing one or more exemplary embodiments. Contemplated are various changes that may be made in the function and arrangement of elements without departing from the spirit and scope of the disclosed subject matter as set forth in the appended claims.
[0033] Some embodiments of the present invention provide systems and methods for controlling the operation of a system involving uncertainty or using a stochastic predictive controller, an example of which is stochastic model predictive control (SMPC), which determines control inputs based on a model of the controlled system and a model of uncertainty.
[0034] FIG. 1A illustrates an example of a system 120 with uncertainties 125 connected to a predictive controller 110 via a state estimator 131, according to some embodiments. In some implementations, the predictive controller is a model predictive controller (MPC) programmed according to a dynamic model 140 of the system. The model may be a set of equations that represent the change in state and output 103 of the system 120 over time as a function of current and previous inputs 111 and previous outputs 103. The model may include constraints 142 that represent physical and operational limitations of the system. In operation, the controller receives commands 101 that indicate a desired behavior of the system. The commands are, for example, motion commands. In response to receiving the commands 101, the controller generates control signals 111 that serve as inputs for the real system 120 with uncertainties 125. In response to the inputs, the system 120 updates the outputs 103 of the system 120. Based on measurements of the outputs 103 of the system, the state estimator 131 updates the estimated state 121 of the system 120. This estimated state 121 of the system provides state feedback to the controller 110. In some cases, measurements of the output 103 may be provided by sensors (not shown) located on the real system 120 or actuator(s) / circuit(s) of the real system 120. Additionally, the uncertainties 125 may be observable / measurable physical quantities (signals), including external disturbances indicated by temperature, pressure, or air flow, or one or more combinations thereof, at one or more locations located on or around the real system 120, currents, forces, or torques acting on the system 120, unmodeled dynamics, or uncertainties in physical quantities such as uncertain friction coefficients, masses of objects, or uncertain coefficients and parameters measured by sensors or other sensors located on / in the real system 120.
[0035] A system 120 as referred to herein may be any machine or device that is controlled by certain operational input signals 111 (inputs), which may be related to physical quantities such as voltage, pressure, force, torque, etc., and returns some controlled output signals 103 (outputs), which may be related to physical quantities such as current, flow rate, speed, position indicating a transition of the state of the system from a previous state to a current state. The output values are partly related to previous output values of the system and partly related to previous and current input values. The dependency on previous inputs and previous outputs is encoded in the state of the system. The operation of the system, for example the movement of a component of the system, may include a sequence of output values produced by the system after application of certain input values.
[0036] The uncertainties 125 may be any time-varying uncertainties including external disturbances, forces or torques acting on the system 120, unmodeled dynamics, or uncertainties in physical quantities such as uncertain friction coefficients, masses of objects, or uncertain coefficients and parameters in the dynamic model equations that describe the physical behavior of the real system 120. Most realizations of MPC controllers use simplified dynamic models 140 to reduce the computational complexity of the controller or because some of the physical behavior is too complex to model, resulting in most of the physical behavior of the real system remaining unmodeled. Note that the time-invariant uncertainties can be estimated or learned either online or offline as part of the state and parameter estimator 131.
[0037] The dynamic model of the system 140 may include a set of mathematical equations, equations that may be time-invariant or time-varying, and may be linear or nonlinear, to describe how the output of the system changes over time as a function of current and previous inputs and previous outputs. The state of the system is generally any collection of time-varying information, e.g., a proper subset of current and previous inputs and outputs, that, together with the dynamic model of the system and future inputs, can uniquely (but approximately) define the future motion of the system. The real system 120 may be subject to physical limitations and specification constraints 142 that limit the range over which the outputs, inputs, and possibly the state of the system can operate.
[0038] The controller 110 may be implemented in hardware or as a software program running on a processor (e.g., a microprocessor) that receives the estimated state and desired motion commands 101 of the system 121 at fixed or variable control period sampling intervals and uses this information to determine inputs, e.g., control signals 111, for operating the system.
[0039] The state estimator 131 and the uncertainty estimator 132 (in FIG. 1B ) can be implemented in hardware or as a software program running either on the same or a different processor as the controller 110 that receives the output of the system 103 at fixed or variable control period sampling intervals and uses new and previous output measurements to determine an estimated state 121 of the system 120.
[0040] 1B illustrates an example of a system 120 with uncertainties 125 connected to a probabilistic predictive controller 150 via a state estimator 131 and an uncertainty estimator 132, according to some embodiments. In some implementations, the probabilistic predictive controller 150 is a probabilistic model predictive controller (SMPC) programmed according to a real system 120 and a dynamic model 140 of the uncertainties. The dynamic model 141 includes an uncertainty model 141 for modeling the relationship between the uncertainties 125 and the behavior of the system 120. The uncertainty model includes a model of linear and / or nonlinear relationships between the uncertainties and dynamic model equations that describe the dynamic behavior of the system. Additionally, the uncertainty model includes a model of probability distributions for each time-varying uncertainty in the dynamic model.
[0041] In some embodiments of the present invention, the dynamic model 140 of the probabilistic predictive controller 150 may include one or more probabilistic chance constraints 143. Any of the physical limitations and specification constraints of the system can be formulated as one or more probabilistic chance constraints 143 that aim to enforce that the probability of violating the corresponding constraint is below a certain probability threshold.
[0042] In some embodiments of the invention, the uncertainty estimator 132 provides estimates of the uncertainties 122, e.g., estimates of the first and / or higher order moments of the probability distribution for one or more of the time-varying uncertainties in the dynamic model 140 used by the probabilistic predictive controller 150. In some embodiments of the invention, the state estimator 131 and the uncertainty estimator 132 are implemented together in a single state uncertainty parameter estimator component 130, which receives the output of the system 103 at fixed or variable control period sampling intervals and uses new and previous output measurements to determine the estimated state 121 and estimated uncertainty 122 of the system 120 and uncertainty 125, as well as possibly one or more additional estimated parameter values.
[0043] 1C is a block diagram illustrating a probabilistic predictive controller 150 according to some embodiments, which operates the system such that the estimated states 121 and outputs 103 of the system follow the commands 101 given the estimated uncertainties 122. The probabilistic predictive controller 150 may include an input interface 151, an output interface 152, and a computer, for example in the form of a single central processing unit (CPU) or multiple CPU processors 160, connected to a memory 165 for storing a dynamic model 140, the uncertainty model 141, the constraints 142, and the probabilistic chance constraints 143 for the operation of the real system 120 with uncertainties 125. The processor(s) 160 are configured to obtain / accept the estimated states 121 and the commands 101 via the input interface 151 and to send control signals 111 to the real system 120 via the output interface 152. The processor(s) 160 may be a single core microprocessor, a multi-core processor, a computing cluster, a network with multiple processors connected, or any number of other configurations. Memory 165 may include random access memory (RAM), read only memory (ROM), flash memory, or any other suitable memory system.
[0044]
number
[0045]
number
[0046]
number
[0047]
number
[0048]
number
[0049]
number
[0050]
number
[0051]
number
[0052]
number
[0053] Finally, an explicit linearization-based method for discrete-time or discretized nonlinear system dynamics according to some embodiments of the present invention is described. propagation Discrete time or discretization of state mean and covariance information using equations propagation In step 255, one or more additional steps may be performed.
[0054]
number
[0055] Some embodiments of the present invention are based on the recognition that Unscented Kalman Filtering (UKF) can be used to compute more accurate propagation of mean and covariance information for nonlinear system dynamics than explicit linearization-based propagation of mean and covariance information, for example, using Extended Kalman Filtering (EKF). Some embodiments of the present invention are based on the recognition that UKF is a special case of a more general family of Linear Regression Kalman Filtering (LRKF), which is part of an even more general family of Gaussian-distributed Assumption Density Filters (ADFs) that can be used in the realization of a stochastic predictive controller of a controlled system under uncertainty. Some embodiments of the present invention are based on the recognition that ADFs use a statistical linearization based on approximate matching of one and / or more higher-order moment integrals, instead of an explicit linearization based on Taylor series approximation (e.g., in EKF). Thus, while EKF is a first-order technique based on explicit linearization for dealing with nonlinearities, the family of ADFs based on statistical linearization is a first-order technique based on explicit linearization for dealing with nonlinearities, while the family of ADFs based on statistical linearization is a first-order technique based on discrete-time or discretized propagation of mean and covariance information of state variables through nonlinear system dynamics. propagation In this way, second order or higher accuracy can be achieved.
[0056] Some embodiments of the present invention are based on the recognition that for certain classes of problems, statistical linearization based on matching of one and / or more higher order moment integrals can be performed analytically, which further improves the accuracy of the propagation of mean and covariance information and hence the performance of a stochastic predictive controller of a controlled system under uncertainty.
[0057]
number
[0058]
number
[0059] Finally, in accordance with some embodiments of the present invention, discrete-time or discretized representations of state mean and covariance information are propagation In this paper, we propose a statistical linearization-based method for the discrete-time or discretized nonlinear system dynamics of a controlled system under uncertainty. propagation The equation can be used to perform one or more additional steps 275. In some embodiments of the present invention, a statistical linearization-based propagation A different set of integration points and weights can be used in one or more steps of the equation.
[0060]
number
[0061]
number
[0062] 3A is a block diagram illustrating a stochastic nonlinear model predictive control (SNMPC) system and method for implementing a stochastic predictive controller 150 that calculates a control signal 111 taking into account a current state estimate 121 of the system and taking into account estimation uncertainties 122 and control commands 101, according to some embodiments of the present invention. Specifically, SNMPC calculates 360 a control solution, e.g., a solution vector 365 that includes a sequence of future optimal or near-optimal control inputs over the system's forecast time horizon, by solving a constrained optimization problem 350 at each control time step. The objective function, equality constraints, and inequality constraints data 345 in this optimization problem 350 depend on the dynamic model and system constraints 340, the current state estimate 121 of the system, the estimation uncertainties 122, and the control commands 101.
[0063] The embodiments of the present invention use direct optimal control methods to formulate the continuous-time SNMPC problem as an inequality-constrained nonlinear dynamic optimization problem. Some embodiments of the present invention use derivative-based optimization algorithms to solve the inequality-constrained optimization problem 350 exactly or approximately using an iterative procedure based on Newton's method and the sequential linearization of the feasibility and optimality conditions of the optimization problem. Examples of such Newton-type optimization algorithms include the interior point method (IPM) and sequential quadratic programming (SQP). Some embodiments of the present invention are based on the recognition that the inequality-constrained optimization problem 350 has the form of an optimal control structured nonlinear program (NLP) such that a structure that utilizes the realization of a derivative-based optimization algorithm can be used to calculate the solution vector 365 at each control time step.
[0064] In some embodiments of the invention, the solution of the inequality constrained optimization problem 350 uses exact or approximate control input, state mean and / or covariance values over a forecast time range from a previous control time step 310 as a solution guess, which can be read from memory to reduce the computational effort of solving the inequality constrained optimization problem 350 at the current control time step. This concept of computing a solution guess from the solution information at the previous control time step 310 is referred to as a warm start or hot start of the optimization algorithm, and in some embodiments of the invention, can reduce the required computational effort of the SNMPC controller. In a similar manner, the corresponding solution vector 365 can be used to update and store 360 a sequence of exact or approximate control input, state mean and / or covariance values for the next control time step. In some embodiments of the invention, a time shifting procedure can be used to compute a more accurate solution guess of the inequality constrained optimization problem 350 at the current control time step, taking into account the control input, state mean and / or covariance values over a forecast time range from the previous control time step 310.
[0065]
number
[0066]
number
[0067]
number
[0068]
number
[0069]
number
[0070]
number
[0071]
number
[0072]
number
[0073]
number
[0074] Using an approximate formulation 375 or 385 of the probabilistic chance constraints 355 based on individual reinforcement of each inequality constraint, the resulting inequality constrained nonlinear dynamic optimization problem can be solved using a Newton-type optimization algorithm based on a sequential linearization of the optimality and feasibility conditions. Examples of such Newton-type optimization algorithms include the interior point method (IPM) and sequential quadratic programming (SQP). Some embodiments of the invention are described in which the SQP algorithm uses a linear-quadratic approximation of the objective function and a linear-quadratic approximation of the discretized system dynamics and discrete-time covariances at each iteration of the SQP optimization algorithm. propagation It is based on the recognition that we solve a quadratic programming (QP) approximation of a stochastic nonlinear OCP based on a linearization-based approximation of the equations, a linearization-based approximation for each inequality constraint, and an enhanced stochastic chance constraint.
[0075] In some embodiments of the invention, the stage costs and / or terminal costs in the objective function 351, 371 or 381 can be defined by any linear, linear-quadratic and / or nonlinear smooth functions, including either convex and / or non-convex functions. The objective function 351, 371 or 381 of the stochastic optimal control problem may include cost terms corresponding to each time point in the forecast time horizon. In some embodiments, the objective function includes imposing a (nonlinear) least-squares type penalty of the deviation of a particular output function of the system from a sequence of reference output values at each time point in the forecast time horizon, resulting in a reference tracking type formulation of the cost function in the stochastic predictive controller 150.
[0076]
number
[0077]
number
[0078]
number
[0079]
number
[0080]
number
[0081]
number
[0082]
number
[0083]
number
[0084]
number
[0085] 5C is a block diagram to illustrate pre-stabilization of nonlinear system dynamics to take into account feedback control actions as part of an explicit linearization-based state mean and covariance propagation of a stochastic predictive controller. Some embodiments are based on a parameterization of the feedback control actions to formulate a pre-stabilized nonlinear system dynamics 560 taking into account the discrete-time dynamics 520 and the time-varying disturbances 525.
[0086]
number
[0087]
number
[0088]
number
[0089]
number
[0090]
number
[0091] Note that the feedback control actions in Figures 5C, 5D, and 5E are predictions of future feedback control actions over a forecast time horizon in a dynamic optimization problem at one control step, and therefore these feedback control actions should not be confused with the actual feedback of the estimated state of system 121 to the predictive controller 110 of Figure 1A.
[0092] FIG. 6A is a block diagram illustrating an iterative differential-based optimization procedure for solving a constrained optimal control structured nonlinear program (NLP) 350 at each control time step in a stochastic predictive controller by using a sequential local linearization-based convex approximation 605. A guess for the solution of the NLP 601 is used to build this local convex approximation, and the solution of the local (convex) approximation of the constrained NLP 610 is used to update (615) the current sequence of control inputs, state mean values and covariance values over the prediction time horizon, so that at each iteration of the algorithm procedure, a guess for the current solution of the constrained NLP 601 is updated. Each iteration of the optimization procedure checks (607) whether a solution of the constrained NLP has been found and / or whether a maximum number of iterations has been reached. If the termination condition 607 is met, then the control solution 365 has been found, otherwise an evaluation (620) of the (approximation of) the constraint Jacobian matrix is performed to build the local linearization-based approximation 605 at the next iteration of the optimization algorithm. The control inputs, state mean values and covariance values from the previous control time step 310 can be used to form an initial solution guess and linearization points for the constrained NLP 601 at each time step of the stochastic predictive controller.
[0093] Based on the nonlinear objective and constraint functions 345, a local (convex) approximation to the NLP 605 is constructed at each iteration of the algorithm procedure using guesses of the current solution, including the trajectories of the control inputs, state mean values and covariance values over the forecast time horizon as linearization points 601. For this purpose, it is necessary to calculate or approximate (620) the constraint Jacobian matrix in order to form the linearization of the discretized system of complex nonlinear system dynamics, the linearization of the state mean and covariance propagation equations, and / or the linearization of the nonlinear inequality constraints. If the solution of the local approximation forms a sufficiently accurate solution for the NLP 607, an optimal control solution 365 is obtained. Instead, when the maximum number of iterations 607 is reached, a non-optimal and / or infeasible solution is obtained (365). If a sufficiently accurate solution of the NLP has not yet been found and the maximum number of iterations has not yet been reached (607), the solution of the local approximation 610 is used to update the trajectories of the control inputs, state mean values and covariance values over the forecast time horizon 615 and to update the guesses of the solution of the NLP 601.
[0094] By using the successive local approximations 605, different types of optimization algorithms can be used to solve the inequality-constrained optimal control structured nonlinear program (NLP) 350 at each control time step. Some embodiments are based on the successive quadratic programming (SQP) method, in which a (convex) quadratic program (QP) is constructed and solved as a local approximation to the original NLP at each iteration. Some embodiments of the invention are based on the recognition that a convex QP can generally be solved at a computational cost significantly less than that required to solve the original NLP 350. Instead, some embodiments are based on the interior point (IP) method, in which each local approximation is a linearization of the first-order necessary condition of optimality of the NLP, in which the complementarity conditions corresponding to the inequality constraints are smoothed based on a relaxation procedure. In some embodiments, a barrier function is used to iteratively enforce the inequality constraints, and each iteration constructs and solves a local approximation of the barrier reformulation problem.
[0095] The derivative-based optimization algorithm may use different Newton-type approximation techniques for the constraint Jacobian and Hessian matrices when constructing (605) and solving (610) the local sub-problems at each iteration. Some embodiments are based on exact linearization of some or all of the constraint functions by computing (620) the exact Jacobian matrix. Some embodiments instead iteratively update an approximation to the constraint Jacobian matrix by low-rank update techniques using a quasi-Newton type update formula. Similarly, different Newton-type approximation techniques may be used for the Lagrangian-Hessian matrix of the NLP. Some embodiments are based on evaluation of the Lagrangian's exact Hessian when constructing each local approximation of the NLP. Some embodiments instead iteratively update an approximation of the Hessian matrix by symmetric low-rank update techniques using a quasi-Newton type update formula. If the NLP objective function includes a (nonlinear) least-squares type cost term, some embodiments are based instead on a Gauss-Newton type Hessian approximation.
[0096]
number
[0097]
number
[0098]
number
[0099] 6C is a block diagram illustrating a strict Jacobian-based quadratic programming (QP) 640 forming a local (convex) approximation 605 of an optimal control structured NLP 630 according to some embodiments based on sequential quadratic programming (SQP) for realizing a stochastic predictive controller. The linear equality constraints 642 in the QP subproblem correspond to the linearization of the discrete-time system dynamics 632 and the state covariance propagation equation 633 based on the evaluation of a fully constrained Jacobian matrix 652. Furthermore, a local linearization 643 is required for the inequality constraints 634 in the original NLP formulation, for which the strict Jacobian matrix 653 needs to be evaluated for each nonlinear inequality constraint.
[0100]
number
[0101] Some embodiments of the present invention are based on the recognition that due to the separable objective function 381, the individual inequality constraints 384-385 per stage, and the stage-wise coupling between the states and covariance matrix variables in the subsequent stages over the forecast time horizon in the equality constraints 382-383 of the constrained NLP 380, the Hessian matrix 651, the equality constraint Jacobian matrix 652, and the inequality constraint Jacobian matrix 653 exhibit block-structured sparsity. Thus, in some embodiments of the present invention, a block-sparse structure exploiting optimization algorithm can be used to solve 610 each local (convex) QP approximation 640 of the optimal control structured NLP 630 in an SQP optimization algorithm for implementing a stochastic predictive controller. Examples of block-sparse structures that the QP optimization algorithm utilizes include primal, dual, or primal-dual active set methods, interior point methods, projected gradient methods, forward-backward splitting methods, or alternating direction method of multipliers (ADMM).
[0102] In some embodiments of the invention, one or more of the nonlinear inequality constraints 634 can be locally approximated by one or more nonlinear but convex inequality constraints, resulting in a local convex program (CP) approximation 605 that needs to be solved 610 in a sequential convex programming (SCP) realization of the stochastic predictive controller. For example, in some embodiments, one or more stochastic chance constraints can be locally approximated by convex quadratic cone constraints and / or convex quadratic inequality constraints. Each of the convex cone constraints imposes that a linear combination of one or more control inputs, state mean, and / or state covariance matrix variables is restricted to be inside a convex cone. Examples of convex cones can include a positive quadrant, a set of positive semidefinite matrices, and / or a quadratic cone. Some embodiments of the present invention are based on the recognition that the locally convex program approximation 605 of the optimal control structured constrained NLP 350 may be a linear program (LP), a quadratic program (QP), a quadratically constrained quadratic program (QCQP), a second-order cone program (SOCP), or a semidefinite program (SDP), and each of these classes of problems can be solved by a structure that utilizes a convex optimization algorithm.
[0103]
number
[0104]
number
[0105]
number
[0106]
number
[0107]
number
[0108]
number
[0109]
number
[0110]
number
[0111]
number
[0112]
number
[0113]
number
[0114]
number
[0115]
number
[0116]
number
[0117]
number
[0118]
number
[0119]
number
[0120]
number
[0121]
number
[0122]
number
[0123]
number
[0124]
number
[0125]
number
[0126]
number
[0127]
number
[0128]
number
[0129]
number
[0130]
number
[0131]
number
[0132]
number
[0133] FIG. 10A is a schematic diagram illustrating a vehicle 1001 including a stochastic predictive controller 1002 employing principles of some embodiments. As used herein, the vehicle 1001 may be any type of wheeled vehicle, such as a car, a bus, or a rover. The vehicle 1001 may also be an autonomous or semi-autonomous vehicle. For example, some embodiments control the motion of the vehicle 1001. An example of the motion is the lateral motion of the vehicle, which is controlled by a steering system 1003 of the vehicle 1001. In one embodiment, the steering system 1003 is controlled by the controller 1002. Additionally or alternatively, the steering system 1003 may be controlled by a driver of the vehicle 1001.
[0134] The vehicle 1001 may also include an engine 1006 that may be controlled by the controller 1002 or by other components of the vehicle 1001. The vehicle 1001 may also include one or more sensors 1004 that sense the surrounding environment. Examples of the sensors 1004 include range finders, radar, lidar, and cameras. The vehicle 1001 may also include one or more sensors 1005 that sense its current momentum and internal state. Examples of the sensors 1005 include a global positioning system (GPS), an accelerometer, an inertial measurement unit, a gyroscope, a shaft rotation sensor, a torque sensor, a deflection sensor, a pressure sensor, and a flow sensor. The sensors 1005 provide information to the controller 1002. In some embodiments of the invention, the sensors are used to estimate the current state of the vehicle, including one or more of the following quantities: vehicle position, longitudinal and lateral velocity, yaw angle and yaw rate, angular velocity or slip angle at one or more of the vehicle's wheels, and other related quantities. According to some embodiments of the present invention, the predictive controller 1002 calculates control inputs including one or more of the following quantities: steering wheel angle or steering rate, angular velocity or torque at one or more of the vehicle's wheels, and other related quantities. The vehicle 1001 may include a transceiver 1007 that enables communication functions of the controller 1002 via wired or wireless communication channels.
[0135] 10B illustrates a schematic of the interaction between the stochastic predictive controller 1002 and the controller 1020 of the vehicle 1001, according to some embodiments. For example, in some embodiments, the controller 1020 of the vehicle 1001 is a steering controller 1025 and a brake / throttle controller 1030 that control the turning and acceleration of the vehicle 1001. In such a case, the stochastic predictive controller 1002 outputs control inputs to the controllers 1025 and 1030 to control the state of the vehicle. The controller 1020 may also include a higher level controller, e.g., a lane keeping assist controller 1035, that further processes the control inputs of the stochastic predictive controller 1002. In either case, the controller 1020 uses the output of the stochastic predictive controller 1002 to control at least one actuator of the vehicle, such as the steering wheel and / or brakes of the vehicle, to control the motion of the vehicle.
[0136] 10C is a schematic diagram showing an autonomous or semi-autonomous controlled vehicle 1050 that can dynamically calculate a realizable and often optimal trajectory 1055 by using an embodiment of the present invention. The generated trajectory is intended to keep the vehicle within a certain road boundary 1052 and avoid other non-controlled vehicles, i.e. obstacles 1051 for the controlled vehicle 1050. In some embodiments, each of the obstacles 1051 can be represented by one or more inequality constraints in a time or space formulation of the constrained optimal control problem. Based on an embodiment configured to realize, for example, a probabilistic model predictive controller, the autonomous or semi-autonomous controlled vehicle 1050 can make decisions in real time, such as, for example, to overtake another vehicle on the left or right side, or to instead stay behind another vehicle in the current lane of the road 1052. An embodiment of the present invention is based on an SNMPC controller that directly accounts for uncertainties regarding the current and predicted states of the vehicle 1050, uncertainties regarding the parameters in the vehicle model, and uncertainties regarding the current and predicted states of the environment (including, for example, obstacles 1051 within a certain distance from the current position of the autonomous or semi-autonomous controlled vehicle 1050).
[0137] 10D is a schematic diagram illustrating a vehicle 1065 controlled by an SNMPC controller that is intended to track a dynamically feasible and optimal trajectory 1070 of an abrupt lane change maneuver within an upper road boundary 1060 and a lower road boundary 1061 by using an embodiment of the present invention. FIG. 10D illustrates a vehicle position 1065 including predicted state trajectory uncertainty propagation by the SNMPC controller 1071 at a first time point, a vehicle position 1066 and corresponding predicted state uncertainty propagation 1072 at a second time point, and a vehicle position 1067 and corresponding predicted state uncertainty propagation 1073 at a third time point. Using a probabilistic predictive controller with probabilistic chance constraints according to some embodiments of the present invention allows the probability of the controlled vehicle violating the road boundary constraints 1060 and / or 1061 to be below a certain probability threshold. More specifically, for example, FIG. 10D shows that the stochastic tube of the predicted state trajectory 1072 at a second time reaches (1075) the upper road boundary constraint 1060, illustrating the behavior of a probabilistic predictive controller that attempts to satisfy both deterministic and probabilistic chance constraints on the controlled system under uncertainty.
[0138] Examples of uncertainties of the system and its environment include any time-varying parameters related to the friction behavior between the tires of the vehicle and the road surface, such as parameters of the Pacejka tire-force model that can be learned or estimated either offline and / or online while controlling the vehicle. The estimated parameter values and estimation uncertainties can be defined as time-varying and uncertain disturbance variables in the direct optimal control problem formulation of the stochastic nonlinear model predictive controller according to an embodiment of the present invention.
[0139] The embodiments of the invention described above may be implemented in any of numerous ways. For example, the embodiments may be implemented using hardware, software, or a combination thereof. If implemented in software, the software code may be implemented on any suitable processor or collection of processors, whether located on a single computer or distributed across multiple computers. Such a processor may be implemented as an integrated circuit, with one or more processors included within an integrated circuit component. However, the processor may be implemented using any suitable form of circuitry.
[0140] Also, embodiments of the invention may be embodied as methods, examples of which are provided. The acts performed as part of the method may be ordered in any suitable manner. Thus, while acts are shown in the example embodiments as sequential, embodiments may be constructed in which acts are performed in a different order than shown, which may include performing some acts simultaneously.
[0141] The use of ordinal terms such as "first" and "second" in the claims to modify claim elements does not, in and of itself, imply a priority, precedence, or order of a claim element relative to other claim elements, or the temporal order in which actions of a method are performed, but is merely used as a label to distinguish a claim element having a certain name from other claim elements having the same name (but using ordinal terms) to distinguish among the claim elements.
Claims
1. 1. A predictive controller for controlling a system under uncertainty subject to constraints on states and control variables of the system, comprising: At least one processor; and a memory storing instructions that, when executed by the at least one processor, cause the prediction controller to: and solving an inequality-constrained nonlinear dynamic optimization problem including a probabilistic chance constraint representing the uncertainties to generate a control signal at each control step of the predictive controller, the predictive controller solving a direct optimal control structured nonlinear program (NLP) using a discrete-time approximate propagation equation based on Gaussian distribution assumption density filtering (ADF) based on the probabilistic chance constraint to perform approximate prediction of first and / or higher order moment integrals of state probability distributions from one time step to the next time step within a prediction range as a function of the first and / or higher order moment integrals of one or more probability distributions of time-varying modeling uncertainties and / or external disturbances, until a termination condition of whether a direct optimal control structured nonlinear program (NLP) solution has been found and / or a maximum number of iterations has been reached is met, the instructions further comprising: Controlling operation of the system using the control signal; a predictive controller, wherein constraints on state and control variables of the system include one or more inequality constraints, and wherein the probabilistic chance constraints are approximated based on one or more inequality constraints reinforced by constraint bounds having terms that depend on backoff factor values, a constraint Jacobian matrix, and the first and / or higher order moment integrals of predicted state variables at each time step within the prediction horizon to ensure that the probability of violating each corresponding inequality constraint is below a probability threshold.
2. The predictive controller of claim 1 , wherein the first and / or higher order moment integrals include a first moment integral that is a mean and a second moment integral that is a covariance.
3. The predictive controller of claim 1 , wherein the backoff factor values are calculated using the Cantelli-Chebyshev inequality, which holds regardless of the underlying state probability distribution.
4. The predictive controller of claim 1 , wherein the back-off factor values are calculated using an inverse Gaussian error function that assumes approximately normally distributed state trajectories.
5. 2. The predictive controller of claim 1, wherein the backoff factor value is calculated using a cumulative density function of a multivariate Pearson VII probability distribution based on a first moment integral, a second moment integral, and a fourth moment integral of the state probability distribution.
6. 3. The predictive controller of claim 2, wherein propagation of mean state values and state covariance matrices for each time step in the prediction horizon is performed by approximating nonlinear state mean and nonlinear covariance propagation equations based on Gaussian distribution assumption density filtering (ADF), taking into account initial state estimates and uncertainties, taking into account one or more means and covariances of the time-varying modeling uncertainties and / or external disturbances, and taking into account current values of control input variables that are optimized for the prediction horizon.
7. 7. The predictive controller of claim 6, wherein the Gaussian distribution hypothesis density filter is an extended Kalman filter (EKF) that uses explicit linearization of nonlinear system dynamics based on evaluation of state dynamic equations and corresponding Jacobian matrices given current values of control input variables that are optimized over the prediction horizon.
8. 7. The predictive controller of claim 6, wherein the Gaussian distribution assumption density filter is a Linear Regression Kalman Filter (LRKF) that is used for statistical linearization of the state probability distribution based on evaluation of state dynamic equations at one or more integration points given current values of control input variables that are optimized for the prediction horizon.
9. The predictive controller of claim 8 , wherein the set of integration points is selected according to a spherical cubature formula or an unscented transformation.
10. The predictive controller of claim 6 , wherein the nonlinear covariance propagation equation reformulates the state covariance matrix using a Cholesky decomposition operator to preserve positive definiteness of the state covariance matrix at each control time step.
11. The predictive controller of claim 10 , wherein a regularization term is used in the nonlinear covariance propagation equation to ensure that a Cholesky decomposition of the state covariance matrix exists at each control time step.
12. The propagation of the average state values and state covariance matrices for each time step within the prediction horizon is performed for a pre-stabilized system of nonlinear system dynamics to take into account future feedback control actions in forward propagation of the uncertainties of the predicted state variables within the prediction horizon; The predictive controller of claim 6 , wherein the pre-stabilization system of the nonlinear system dynamics uses a time-invariant or time-varying sequence of affine feedback gains.
13. 13. The predictive controller of claim 12, wherein the inequality constrained nonlinear dynamic optimization problem includes one or more probabilistic chance constraints on one or more inequality constraints for subsequent feedback control actions within the prediction horizon to ensure feasibility of a pre-stabilized system of the nonlinear system dynamics.
14. 2. The predictive controller of claim 1, wherein the predictive controller computes an (approximate) optimal solution of the direct optimal control structured nonlinear program (NLP) using a sequential quadratic programming (SQP) optimization algorithm that solves a block-structured sparsity-preserving quadratic programming (QP) sub-problem, and computes updates to values of state mean and covariance variables, as well as updates to values of the control variables, at each iteration of the SQP optimization algorithm until a termination condition is met.
15. 15. The predictive controller of claim 14, wherein the SQP optimization algorithm is based on an inexact evaluation of one or more of first and / or higher order derivatives of the NLP objective and / or constraint functions, thereby resulting in a QP sub-problem that can be solved at significantly lower computational cost.
16. 16. The predictive controller of claim 15, wherein each iteration of the inexact SQP optimization algorithm employs numerical elimination of deviation variables of the Cholesky factors of the state covariance matrix, thereby resulting in a QP sub-problem that preserves block-structured sparsity and can be solved at significantly lower computational cost.
17. 17. The predictive controller of claim 16, wherein each iteration of the inexact SQP optimization algorithm uses one or more adjoint-based gradient calculations as a correction for inexact evaluations of one or more of first and / or higher order derivatives of the NLP objective function and / or constraint functions.
18. 17. The predictive controller of claim 16, wherein each iteration of the inexact SQP optimization algorithm uses an evolution step based on primal and dual solutions of the QP sub-problems to compute updates to deviation variables of Cholesky factors of the state covariance matrix and updates to Lagrange multiplier values of state covariance propagation equations at each time step within the prediction horizon.
19. 15. The predictive controller of claim 14, wherein updating the values of the state mean and covariance variables and updating the values of the control variables requires a globalization-based step size selection that may be based on a line search procedure in combination with a merit function to ensure sufficient progress in each iteration of the SQP optimization algorithm to compute a (near-)optimal solution of a direct optimal control structured NLP.
20. The predictive controller of claim 14, wherein the optimization of the average state values and control variables numerically reduces the average state variables at each time step within the prediction range in a compression routine as a function of initial values of the average state variables and control variables at all previous time steps within the prediction range, so that after solving the QP sub-problem, the linearized system dynamics are simulated starting from initial state values and the values of the average state variables can be updated by using the updated control values at each time step within the prediction range.
21. 2. The predictive controller of claim 1, wherein the predictive controller uses only one or a predetermined number of iterations of a derivative-based optimization algorithm to solve the inequality constrained nonlinear dynamic optimization problem at each control step, starting from an optimal sequence of state mean and covariance matrix values and control input values over the prediction horizon at a previous control step.
22. 22. The predictive controller of claim 21 , wherein the derivative-based optimization algorithm is an adjoint-based SQP optimization method that uses inexact evaluations of one or more of first and / or higher order derivatives of objective and / or constraint functions, and which solves only one or a predetermined number of block-structured QP sub-problems at each control step to update an optimal sequence of state mean and covariance matrix values and control input values over the prediction horizon.
23. The predictive controller of claim 1 , wherein the controlled system is configured to output a control input to a controller of the vehicle.
24. 24. The predictive controller of claim 23, wherein the vehicle state includes one or a combination of values of position, orientation, velocity, angular velocity, slip ratio and slip angle of the vehicle, the control input includes one or a combination of values of acceleration, brake torque, steering angle and steering rate, and the uncertainties include time-varying disturbances including one or a combination of uncertainties in mass values, inertia values, or both, in a model of the vehicle, uncertainties in a steering model of the vehicle, uncertainties in values of one or more parameters indicative of friction between the vehicle's tires and a road surface.
Citation Information
Patent Citations
Apparatus and method for controlling a system
JP2020535562A
Control system and method for controlling system operation
JP2021504815A
Method and system for simultaneous receiver calibration and object localisation for multilateration
US20140368387A1
