System and Method for Adaptive Control of Vehicle Dynamics

Abstract

A vehicle dynamics control system receives a feedback state signal including values of a roll rate and a roll angle of the motion of the vehicle and updates parameters of a model of roll dynamics of the vehicle by fitting the received values into the roll dynamics model. The roll dynamics model explains the evolution of the roll rate and the roll angle based on the parameters including a center of gravity (CoG) parameter modeling a location of a CoG of the vehicle, and a spring constant and a damping coefficient modeling suspension dynamics of the vehicle. The system determines a control command for controlling at least one actuator of the vehicle using a motion model including the updated CoG parameter and submits the control command to the vehicle controller to control the motion of the vehicle.

Claims

1. A vehicle dynamics control system, comprising: at least one processor; and memory having instructions stored thereon that, when executed by the at least one processor, cause the vehicle dynamics control system to: receive a feedback state signal indicative of measurements of a state of a motion of a vehicle controlled by a vehicle controller, the state includes values of a roll rate and a roll angle in the vehicle; update one or multiple parameters of a model of roll dynamics of the vehicle by fitting the received values of the roll rate and the roll angle into the roll dynamics model, wherein the roll dynamics model is a spring-damper model that explains an evolution of the roll rate and the roll angle based on the parameters including a center of gravity (CoG) parameter modeling a location of a CoG of the vehicle, and a spring constant and a damping coefficient modeling suspension dynamics of the vehicle, and wherein the updated parameters include the CoG parameter; determine, based on the state of the vehicle, a control command for controlling at least one actuator of the vehicle using a motion model including the updated CoG parameter; and submit the control command to the vehicle controller to control the motion of the vehicle based on the control command.

2. The vehicle dynamics control system of claim 1, wherein the updated parameters of the vehicle roll dynamics model include the spring constant and the damping coefficient.

3. The vehicle dynamics control system of claim 2, wherein the motion model does not include the spring constant and the damping coefficient of the roll dynamics model.

4. The vehicle dynamics control system of claim 1, wherein the motion model includes planar dynamics of the vehicle.

5. The vehicle dynamics control system of claim 4, wherein the motion model includes the roll dynamics of the vehicle.

6. The vehicle dynamics control system of claim 1, wherein the CoG parameter is estimated probabilistically as a probabilistic distribution using a Bayesian filter, and wherein the motion model includes first and second moments of the probabilistic distribution of the CoG parameter capturing an uncertainty of estimation of the CoG parameter, such that the control command is determined based on the uncertainty of the CoG estimation.

7. The vehicle dynamics control system of claim 6, wherein the Bayesian filter is a Kalman filter assuming the probabilistic distribution of the CoG parameter being a Gaussian distribution.

8. The vehicle dynamics control system of claim 6, wherein the Bayesian filter is a particle filter.

9. The vehicle dynamics control system of claim 8, wherein the particle filter maintains a set of particles, each particle represents a weighted estimation of the probability distribution of the CoG parameter, wherein the set of particles collectively represents the first and the second order moments of the estimated probability distribution of the CoG parameter, wherein to update the particles, the processor is configured to determine a relation between the roll angle and the roll rate estimated using the roll dynamics model having the uncertainty of the CoG parameter and the received roll angle and the roll rate; and update the probability distribution of the CoG parameter based on the determined relation to reduce the relation between the roll angle and the roll rate and the received values of the roll angle and the roll rate.

10. The vehicle dynamics control system of claim 1, wherein the vehicle dynamics control system updates the CoG parameter iteratively starting from initial values of the parameters of the roll dynamics model, wherein the initial values are determined by minimizing a cost function of a difference between a sequence of historical values of the roll rate and the roll angle and a sequence of estimated values of the roll rate and the roll angle estimated based on values of the parameters of the roll dynamics model selected from a range of possible values of the parameters. [This was originally to initialize the CoG parameter, but it isn't reflected in the claim. Maybe it is OK anyway and covered by the formulation.]

11. The vehicle dynamics control system of claim 1, wherein the cost function is a Gaussian-process regressor optimized by a Bayesian optimization [not clear]

12. The vehicle dynamics control system of claim 1, wherein the control command is submitted to override control signals generated by the vehicle controller.

13. The vehicle dynamics control system of claim 1, wherein the control command is submitted as a feedforward input to the vehicle controller.

14. The vehicle dynamics control system of claim 13, wherein the control command is determined to reduce a likelihood of a rollover of the vehicle

15. The vehicle dynamics control system of claim 14, wherein the controller includes includes a constraint-admissible reference trajectory adjusted from a desired reference signal by an adaptive reference governor (ARG) for the states of the vehicle according to the motion model with the updated CoG parameter.

16. The vehicle dynamics control system of claim 14, wherein the controller is a stochastic model-predictive controller (SMPC) wherein the SMPC performs an optimization subject to constraints including a rollover avoidance constraint, wherein the SMPC uses the the second moment of the estimated probability distribution of CoG parameter to determine the control command.

17. The vehicle dynamics control system of claim 15, wherein the ARG is configured to update the reference trajectory and compute a CoG parameter-robust constraint admissible set based on the updated reference trajectory and the system states, wherein the ARG generates and transmits a reference trajectory to the system based on the parameter-robust constraint admissible set.

18. The vehicle dynamics control system of claim 17, wherein the ARG includes a constraint-admissible invariant set (CAIS) learner configured to generate constraint-admissible sets using the estimated probability distribution of the CoG parameter.

19. The vehicle dynamics control system of claim 18, wherein the CAIS learner is configured to leverage CoG parameter estimates and stored state features and constraint-admissibility labels to learn an updated CAIS.

20. The vehicle dynamics control system of claim 1, wherein the CoG parameter is estimated numerically using a particle filter, and wherein the roll rate and roll angle of the vehicle are estimated analytically based on the numerical estimation of the CoG parameter.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0024] FIG. 1A shows a schematic of the model of vehicle planar dynamics.

[0025] FIG. 1B shows a model, wherein four wheels are modeled, as well as the dynamics for the roll and pitch.

[0026] FIG. 1C shows an alternative schematic of a model only including the vehicle roll dynamics.

[0027] FIG. 1D shows an example of interactions of different vehicle models according to some embodiments.

[0028] FIG. 1E shows the interaction of various vehicle motion models according to one embodiment.

[0029] FIG. 2A shows the interaction of a vehicle dynamics control system with other system components of a vehicle according to some embodiments.

[0030] FIG. 2B shows a general structure of the vehicle-dynamics control system according to one embodiment of the invention.

[0031] FIG. 3A shows a block diagram of a method for controlling the vehicle dynamics including a CoG parameter of a vehicle according to some embodiments.

[0032] FIG. 3B shows a schematic of fitting the received values of the roll rate and the roll angle into the roll dynamics model to update one or multiple parameters of a model of roll dynamics of the vehicle according to some embodiments.

[0033] FIG. 4A shows a flowchart of a method for estimating one or multiple parameters including a CoG parameter of the vehicle by fitting measurements of at least one or a combination of a roll rate and a roll angle in the vehicle roll dynamics using a Bayesian filter according to some embodiments.

[0034] FIG. 4B shows a flowchart of a method for determining initial parameters for the estimator according to some embodiments.

[0035] FIG. 4C shows a flowchart of one iteration of a method for estimating the CoG parameter using a particle filter according to one embodiment.

[0036] FIG. 4D shows a flowchart of one exemplar implementation of updating the probability distribution of the CoG parameter according to one embodiment.

[0037] FIG. 4E shows a simplified schematic of the result of three iterations of steps and when five particles are generated for each iteration.

[0038] FIG. 4F shows possible assigned probabilities of the five different CoG parameters at the first iteration in FIG. 4E.

[0039] FIG. 5A shows an example of a vehicle controlled by a previously designed legacy controller capable of tracking desired references, according to some embodiments.

[0040] FIG. 5B illustrates a schematic of components of the vehicle-dynamics control system interfaced with a vehicle controlled by a legacy control system.

[0041] FIG. 5C provides a block representation of the CAIS learner.

[0042] FIG. 5D shows a flowchart of a method for generating offline features and labels used in CAIS according to some embodiments.

[0043] FIG. 6A shows a schematic of the reference generation module in the adaptive reference governor.

[0044] FIG. 7 shows examples of updated CAIS based on gradually contracting bounds around the point estimate.

[0045] FIG. 8A shows another example of a schematic of components of the vehicle-dynamics control system according to some embodiments.

[0046] FIG. 8B shows a block diagram of an exemplar implementation of a two-level optimization procedure to solve the inequality constrained nonlinear dynamic optimization problem at each control step of the SMPC.

[0047] FIG. 9A shows a block diagram to implement the SMPC using the control model that includes the motion model including the CoG parameter.

[0048] FIG. 9B shows a block diagram of an SNMPC that solves a constrained optimal control structured nonlinear programming problem (OCP-NLP) to compute the control signal at each control time step.

DETAILED DESCRIPTION

[0049] FIG. 1A shows a schematic of the model of vehicle planar dynamics used by some embodiments. The vehicle planar dynamics model describes the time evolution of the vehicle longitudinal motion, lateral motion, and heading, i.e., the vehicle planar dynamics model is a particular example of a motion model of a vehicle. The heading ψ describes the rotation of the vehicle about an axis that is perpendicular to the vehicle's forward axis x and lateral axis y. In the case of FIG. 1A, the vehicle model, i.e., the vehicle planar dynamics, combines the two wheels on the same axle into one, effectively making it a bicycle model. However, the same illustration applies to a four-wheel vehicle model. The model in FIG. 1A illustrates a model that is widely used in vehicle control. However, it does not capture the roll dynamics of the vehicle and is therefore not useful as a stand-alone model for vehicle-dynamics control systems that aim to control the vehicle under nonnegligible roll motion of the vehicle, e.g., the controlling of a sports utility vehicle (SUV) having aggressive steering maneuvers.

[0050] FIG. 1B shows a more complex model, wherein four wheels are modeled, as well as the dynamics for the roll ϕ and pitch θ. The mathematical equations underlying the schematic in FIG. 1B includes the roll dynamics and can therefore be used to estimate the CoG height in a recursive fashion, e.g., using a Kalman filter-type method or a recursive least-squares type method, or any other recursive method applicable to the particular estimation problem.

[0051] FIG. 1C shows an alternative schematic of a model only including the vehicle roll dynamics, wherein the model governing the roll motion is a torsional spring-damper system modeling the CoG height h, spring constant K, and damping coefficient D. As used herein, the roll motion is described by a vehicle state including the roll rate of the vehicle and the roll angle of the vehicle. For the model in FIG. 1C there are in total three parameters, the CoG height h, spring constant K, and damping coefficient D, of main interest for control of the vehicle roll dynamics, e.g., to avoid vehicle rollover.

[0052] The model in FIG. 1C is substantially simplified compared to FIG. 1B. For instance, the model illustrated by FIG. 1B necessitates the need for modeling the vehicle tire to road interaction, because the tire forces are essentially what govern the planar dynamics. Hence, if excluding the pitch dynamics, the number of parameters needed to estimate increases to at least seven compared to three for the model illustrated in FIG. 1C.

[0053] Different vehicle dynamics models capture different aspects of vehicle dynamics and are therefore suitable for different purposes. For instance, when controlling the heading rate of a vehicle and assuming that the vehicle is aligned with the road, a planar dynamics model is sufficient for controlling the vehicle. However, the assumption of a vehicle being aligned with the road is sometimes difficult to verify a priori. For instance, when driving on dry asphalt the friction between tire and road is large such that steering maneuvers intended for controlling the heading rate of a vehicle lead to large roll angles such that using a vehicle planar dynamics model alone as a motion model when controlling the vehicle can be insufficient.

[0054] In cases of roll angles being large such that the CoG location of a vehicle changes more than marginally, it is beneficial for a control system controlling the dynamics of the vehicle, i.e., a vehicle dynamics control system, to also include a parameter modeling CoG location of the vehicle.

[0055] Besides, different vehicle models can have different purposes. FIG. 1D shows an example of interactions of different vehicle models according to some embodiments and how they can fit different purposes in a vehicle-dynamics control system for controlling a vehicle 200.

[0056] For instance, some embodiments recognize that while a complex vehicle model, such as in FIG. 1B, inevitably can lead to accurate modeling of the vehicle, such a model increases complexity while not necessarily adding much modeling improvement. For instance, when controlling a vehicle 200 using a controller 120d, one embodiment uses a motion model of vehicle dynamics that include the roll dynamics of the vehicle and the planar dynamics of the vehicle, wherein the roll dynamics model is a spring-damper model having the CoG location as a parameter. However, when updating 110d the CoG location 111d for control 120d, one embodiment uses only a model of roll dynamics that explains the evolution of the roll rate and the roll angle based on the parameters including a CoG parameter modeling a location of a CoG of the vehicle and a spring constant and a damping coefficient modeling suspension dynamics of the vehicle. In other words, updating one or multiple parameters of a model of roll dynamics of the vehicle can be done with a simple roll-dynamics model, whereas the updated parameters can be sent 111d to the vehicle controller 120d that can use such parameters for determining control commands 121d to the vehicle.

[0057] Different models can be used for different purposes and need different information from the sensors measuring aspects of the vehicle operation. For instance, for updating the CoG parameter some embodiments use a lateral acceleration measurement, a roll rate measurement, and a roll angle measurement, which are sensed by sensors mounted on the vehicle 200 and whose measurements 201d are used when updating the CoG parameter. However, for the purpose of controlling 120d the vehicle, some embodiments use measurements 202d that includes other aspects of vehicle dynamics, such as measurements of heading rate, vehicle velocity, wheel speeds, and vehicle position.

[0058] Other embodiments are based on the recognition that there is not only an interaction between different models relating to the estimation of CoG parameters and the controlling of the vehicle but also different models are used for different aspects of estimation.

[0059] E.g., vehicle sensor setups can measure the relevant states needed for control, e.g., a gyroscope included in an inertial measurement unit (IMU) can measure the heading rate of a vehicle, a global positioning system (GPS) receiver can measure a vehicle position. However, GPS can only measure vehicle position within a few meters precision, and automotive-grade IMUs are imprecise. Hence, some embodiments utilize different estimators for estimating various aspects of vehicle operation.

[0060] FIG. 1E shows the interaction of various vehicle motion models according to one embodiment. For example, the controller 120d determining the control command 121d can include a combination of a vehicle planar dynamics model 121e of the vehicle and a model of roll dynamics 122e of the vehicle. Although shown in FIG. 1E as two different vehicle models, the vehicle model illustrated in FIG. 1B including joint modeling of planar dynamics and vehicle roll dynamics including a CoG parameter can be included in the controller 120d. The determining 110d of CoG parameter can include a vehicle roll dynamics model as a spring-damper model where the CoG parameter includes a CoG location, spring constant, and a damping coefficient. At the same time, the vehicle state, such as a vehicle velocity, vehicle position, vehicle heading, and heading rate of a vehicle, can be estimated 130d using a planar model 121e of vehicle dynamics.

[0061] FIG. 2A shows the interaction of a vehicle dynamics control system 199 with other system components of a vehicle 200 according to some embodiments. The vehicle can be any type of moving vehicles, such as a four-wheeled passenger car or a mobile robot. The vehicle can be entirely human-operated, semi-autonomous, or fully autonomous. E.g., the vehicle can receive external inputs 210 from a driver of the vehicle generating a reference steering command and/or braking/throttle command to the vehicle. E.g., the vehicle can receive external inputs 210 from an autonomous-driving system of the vehicle generating a reference steering command and/or braking/throttle command to the vehicle. The vehicle dynamics control system 199 can be connected to a sensing system 230 that outputs various sensor measurements 231, such as lateral acceleration, steering command, velocity, or sensing of the suspension system of the vehicle. The sensing system 230 can include GPS and/or an IMU. E.g., the IMU can comprise a 3-axis accelerometer(s), 3-axis gyroscope(s), and/or magnetometer(s). The IMU can provide velocity, orientation, and/or other position-related information to other components of the vehicle dynamics control system 199. For example, the sensing system 230 can include a sensor for measuring or estimating the roll angle of the vehicle and the roll rate of the vehicle.

[0062] The vehicle dynamics control system 199 determines a control command 241 to low-level actuators and controllers 260 of the vehicle. The vehicle-dynamics control system can actuate various actuators of the vehicle. The control command 241 can be modified steering command modifying the drivers' 210 reference input to avoid violating the constraints of the vehicle, or it can be the braking commands of the individual tires. The command 241 is used as an input, or reference, to the vehicle controllers 260 to compute vehicle commands, such as steering, brake, and throttle. Those commands are submitted to the actuators of the vehicle to move the vehicle according to command 241.

[0063] If the motion model of the vehicle contained in the vehicle-dynamics control system 199 uses the same control inputs as the actual vehicle, the vehicle can be controlled directly using the computed inputs. E.g., if the vehicle 200 is controlled by applying steering and engine torque, and if the control model used in the vehicle dynamics control system 199 also uses steering and engine torque as control inputs, these can be applied directly to the vehicle, thereby overriding the vehicle-control system 260. However, because the mathematical description of the vehicle 200 used, e.g., mathematical description corresponding to the model in FIG. 1B, in the vehicle dynamics control system 199 often is a simplification of the true motion of the vehicle, the control inputs 241 can instead be used as feedforward, or nominal, control signals together with references as inputs to the vehicle-control system 260. E.g., accelerations can be modeled to be proportional to the engine torque. Hence, if accelerations are used as inputs in the motion model of the vehicle employed in the vehicle dynamics control system 199, the accelerations can be used as scaled feedforward inputs to the vehicle-control system 260.

[0064] In some implementations, the vehicle-dynamics control system 260 can include a steering controller, a brake controller, and a throttle controller, wherein the steering controller takes a steering angle as input, the brake controller takes a reference deceleration or wheel slip as input, the engine controller takes a nominal velocity or acceleration as input, all controllers output torques, and the vehicle-dynamics control system 199 includes all of these entities. Then, the torques computed by the vehicle-dynamics control system 199 can either circumvent the vehicle controllers 260, or the torques 241 can be used as zero-level values of the vehicle controllers 260, and the steering angle, the reference deceleration or wheel slip, and the nominal velocity or acceleration can be used as references 241 to the vehicle controllers 260.

[0065] Some embodiments use a motion model of the vehicle for controlling the vehicle, wherein the motion model does not include the spring constant and the damping coefficient of the roll dynamics model. This is because the CoG location is the key parameter for controlling a vehicle to avoid rollover, and a motion model controlling the roll of the vehicle must include a CoG location.

[0066] In other embodiments, the motion model used when controlling the vehicle includes the planar dynamics of the vehicle. E.g., when a rollover is the only control objective, a vehicle roll dynamics model is sufficient for control so some embodiments include the roll dynamics into the motion model. However, when controlling the other states of a vehicle, such as position, heading rate, or velocity, other models are included. E.g., in one embodiment the motion model used for controlling the vehicle includes the planar dynamics model of the vehicle.

[0067] Some embodiments are based on the understanding that sensor measurements are noisy, and such noise is probabilistic, i.e., it can be described by statistics. To this end, some embodiments use probabilistic, i.e., Bayesian, filters, to update one or multiple parameters of a model of roll dynamics of the vehicle. Using Bayesian filters enable to extract of various moments, e.g., first and second moments, of the determined probability distribution, and such moments can be used when determining the control command. E.g., the second moment, i.e., the variance, of the updated CoG location can be used to determine control commands such that the vehicle is behaving safely for CoG locations within the variation of the possible CoG locations.

[0068] FIG. 2B shows a general structure of the vehicle-dynamics control system 199 according to one embodiment. The vehicle-dynamics control system 199 includes at least one processor 270 for executing instructions of the vehicle-dynamics control system 199 stored in a memory 280. The processor 270 is connected 271 to a memory 280 for storing a Bayesian filter 281 that uses sensing information 231 and a model of vehicle roll dynamics. The memory also stores 282 an initial guess of the distribution of one or more parameters of the vehicle roll dynamics, e.g., including a spring stiffness constant of the dynamics, a damping coefficient, and a CoG location of the vehicle, and 283 a motion model including the CoG parameter. The memory also stores 284 the constraints of the vehicle, and a controller with components 285 for determining control commands satisfying such constraints. For example, the memory can store 285 constraint-admissible invariant sets that, when used in a controller, can guarantee constraint satisfaction. In one embodiment, the vehicle-dynamics control system determines such sets based on the estimates from the Bayesian filter according to various embodiments of the invention.

[0069] FIG. 3A shows a block diagram of a method for controlling the vehicle dynamics including a CoG parameter of a vehicle according to some embodiments, wherein the method is executed by at least one processor 270 of the vehicle. The method estimates 310a one or multiple parameters of a vehicle roll dynamics model 350a including the CoG parameter modeling a location of a CoG of the vehicle and a spring constant and a damping coefficient modeling suspension dynamics of the vehicle by fitting measurements of at least one or a combination of a roll rate and a roll angle in the vehicle roll dynamics model. In some embodiments, the method uses a Bayesian filter that also estimates the confidence intervals of the parameters. In other embodiments, the Bayesian filter also estimates a probability distribution of the uncertainty of the parameters. Using the estimated parameter 312a and the estimated uncertainty 311a of the internal states, the method determines 320a a control command 321a for controlling at least one actuator of the vehicle using a motion model 360a including the updated CoG parameter; and submits 330a the control command 321a by virtue of a control signal 331a to the vehicle. Finally, using the vehicle controller 260, the method controls 340a the motion of the vehicle according to the control command. In some embodiments, the purpose of control is to reduce a likelihood of a rollover of a vehicle having the estimated CoG.

[0070] In other embodiments, the Bayesian filter estimates not only the CoG parameter but also an estimate of the uncertainty of the CoG parameter. In some embodiments, the estimate of the CoG parameter is a general probability distribution, in other embodiments, the estimate of the CoG parameter is done by assuming the CoG parameter uncertainty is following a Gaussian distribution. In other embodiments, the Bayesian filter uses measurements of the roll rate and the roll angle of the vehicle using the sensing system 230.

[0071] Some embodiments are based on the realization that the parameters including a CoG parameter modeling a location of a CoG of the vehicle and a spring constant and a damping coefficient modeling suspension dynamics of the vehicle can be estimated based only on a model of the vehicle roll dynamics, which is simpler than also having a model of vehicle planar dynamics. The realization is based on the understanding that while including a planar vehicle dynamics model, accuracy is improved slightly. However, the complexity of the algorithm needed to include such a model is increased exponentially. Hence, only including the vehicle roll dynamics leads to a small reduction of accuracy while gaining a substantial reduction in computational and algorithmic complexity.

[0072] One embodiment is based on the realization that the vehicle roll dynamics can be represented as a torsional spring-damper model having three unknown parameters: the CoG location, and spring and damping coefficients. Another embodiment is based on the understanding that while the CoG location is the main parameter of interest to vehicle-dynamics control systems focusing on rollover prevention, the suspension dynamics of a vehicle heavily influence the motion of the CoG location. As a result, for accurate CoG estimation also parameters of the suspension dynamics are essential to be estimated.

[0073] FIG. 3B shows a schematic of fitting the received values of the roll rate and the roll angle into the roll dynamics model to update one or multiple parameters of a model of roll dynamics of the vehicle according to some embodiments. The roll dynamics model includes parameters 310b and the relationships 320b connecting the parameters 310b with outputs of the roll dynamics. In various embodiments, parameters 310b include one or a combination of a CoG parameter modeling a location of a CoG of the vehicle, and a spring constant and a damping coefficient modeling suspension dynamics of the vehicle. The outputs of the roll dynamics in the roll dynamics model include the roll rate and the roll angle of the vehicle. Hence, knowing the parameters 310b of the roll dynamic model it is possible to estimate the values of the roll rate and the roll angle 340b according to the relationship 320b. Examples of the relationships 320b include a roll dynamics model having the parameters inserted into the model, wherein the values are the outputs of the roll dynamics model with the parameters inserted into the model.

[0074] To that end, the update 370b of the parameters 310b leads to different values of the estimated roll rate and roll angle 340b, which can be compared with the values of the roll rate and roll angle 360b received in a feedback signal to determine a difference 350b between the estimated 340b and the received 360b values of the roll rate and roll angle. Knowing the difference 350b, various embodiments update 370b one or multiple of the parameters 310b to reduce the difference 350b. Various techniques can be used to estimate the direction and the magnitude of the update 370b. For example, some embodiments determine the update 370b of the parameters using a Bayesian filter, by fitting measurements of at least one or a combination of a roll rate and a roll angle in the vehicle roll dynamics. Other embodiments use optimization-based techniques, such as gradient descent, to minimize the error between the outputs of the roll dynamics model and measurements of the roll rate and roll angle.

[0075] FIG. 4A shows a flowchart of a method for estimating 310a one or multiple parameters including a CoG parameter of the vehicle by fitting measurements of at least one or a combination of a roll rate and a roll angle in the vehicle roll dynamics using a Bayesian filter, wherein the CoG parameters is estimated probabilistically as a probabilistic distribution using a Bayesian filter, and wherein the motion model includes first and second-order moments of the probabilistic distribution of the CoG parameter capturing an uncertainty of estimation of the CoG parameter, such that the control command is determined based on the uncertainty of the CoG estimation. At the first time step, the method initializes 410a the estimates of parameters using input data 409a acquired according to other embodiments. Then, using measurements 419a from the sensing system 230, the method updates 420a the estimates of one or multiple parameters by fitting the measurements in the vehicle roll dynamics model that connects the measurements to the parameters. Using the updated estimates 421a of parameters, the method predicts 430a the parameters to the next time step and sends the predicted parameters 432a. Additionally, the method outputs 431a the parameters for using when determining a control command for controlling the vehicle.

[0076] Initialization of the estimate of the parameters can be done in numerous ways. For instance, if the vehicle dynamics control system is to be used for a specific vehicle model, or a specific type of vehicle, ranges of possible values of the parameters can be determined and bounded beforehand, from which initialization can be done, e.g., by sampling a value from a probability distribution that models the a priori range. However, there are two problems with such an approach. First, the range of values can be very large such that testing all values is impossible for computational reasons. Second, while it is possible to sample a finite set of values by modeling a probability distribution over the possible range of values, how to model such a distribution that captures the underlying true uncertainty of parameters is difficult.

[0077] One embodiment recognizes that if there are prior measurements available, e.g., collected using a vehicle driven by a human operator for the same type of vehicle, these measurements can be used in combination with a model of the roll dynamics of the vehicle and test different values of internal states to compare the fit to the measurements.

[0078] Some embodiments determine the initial guess by optimizing a cost function minimizing the difference between the measurement sequence and a set of estimated measurements using a model of the roll dynamics of the vehicle having hypotheses of the internal states.

[0079] FIG. 4B shows a flowchart of a method for determining initial parameters 410a for the estimator 310a according to some embodiments. Using a measured sequence 409b, wherein measurements include a roll angle of the vehicle and a roll rate of the vehicle, a model 408b of the vehicle roll dynamics having the measurements as outputs of said model, a range 407b of possible values of the parameters, the method determines 410b a predicted measurement sequence as an output of the model of the vehicle roll dynamics using a set of values of parameters determined from the range of values 407b. Using the predicted measurements 411b, the method determines 420b a value of a cost function that reflects the difference between the measurement sequence and the predicted measurement sequence. Then, the method updates 430b the parameters using the history of a combination of values of parameters and values of cost function resulting from the set of values of parameters. If a termination threshold 440b is met, the updated set of parameters are used to determine 450b a probability distribution of the set of possible parameters, from which an initial set of parameters are determined. If the termination threshold 450b is not met, the method determined a new set of predicted measurements using the updated parameters.

[0080] In some embodiments, the method 410a is implemented using Bayesian optimization. Bayesian optimization methods are used to solve optimization problems of the form

[00001]$\underset{\theta \in \Theta }{\min }\mathrm{��}\left(\theta \right),$

where the objective function (θ) is usually too expensive to evaluate for all set of the possible range of values Θ of internal states and an analytical form of the function is unavailable, making it impossible to derive analytical gradients. To prevent prohibitive expenditure in evaluating the cost, the Bayesian optimization framework models the uncertainty on the cost function at values of the parameters that have not yet been evaluated, as a probability distribution. In one embodiment, the Bayesian optimization uses Gaussian-process regression to construct a replica of the true cost function that can be evaluated relatively cheaply, and, therefore, evaluated often. At each iteration of the method, the Gaussian-process regressor is used to update 430b the CoG parameter to be used in the next iteration, and the Gaussian process updates its probability distribution by determining 420b the value of the true cost function using the updated 430b CoG parameter θ* from the previous iteration.

[0081] The determining 430b of the updated CoG parameter is based on the surrogate cost function. In one embodiment, this is done by introducing an acquisition function that is designed to result in the CoG parameter in the range of possible values where the best CoG parameter (the optimal solution), is likely to be located.

[0082] One embodiment employs the expected improvement function to update 430b the CoG parameter, wherein the expected improvement function is .sub.EI(θ)={tilde over (σ)}(θ)(zΦ.sub.G(z)+ϕ.sub.G(z)), wherein Φ.sub.G is the cumulative distribution function and ϕ.sub.G is the probability distribution of the standard Gaussian distribution,

[00002]$z=\frac{\tilde{\mu }\left(\theta \right)-\mathrm{��}\left({\theta }^{+}\right)}{\tilde{\sigma }\left(\theta \right)},$

and {tilde over (μ)}(θ) and {tilde over (σ)}(θ) are the mean and variance from the surrogate function. In one embodiment, the expected improvement is used to determine the next CoG parameter by minimizing the expected improvement.

[0083] In some embodiments, not only one CoG parameter but several different possible values of CoG parameters are determined from sampling the probability distribution determined by 410a.

[0084] Other embodiments of the invention implement the Bayesian filter using a particle filter, wherein the particle filter maintains a set of particles, each particle represents a weighted estimation of the probability distribution of the CoG parameter, wherein the set of particles collectively can represent the first and the second moments of the estimated probability distribution of the CoG parameter. A particle filter particle also includes an estimate of the vehicle state based on the CoG parameter for said particle and weight of probability of the combination of CoG parameter and vehicle state. Using a particle filter avoids the need for the linear or near-linear assumption of the model of vehicle dynamics and its dependence on unknown parameters estimated by the particle filter.

[0085] In some embodiments, the particle filter uses a vehicle model of roll dynamics to update the states. Some embodiments define the CoG parameter as the CoG location, spring stiffness, and damping coefficient, θ=[K D h].sup.T. In some embodiments, the particle filter also updates additional states x of the vehicle, wherein the additional state includes a roll rate of the vehicle and a roll angle of the vehicle. Doing in such a manner ensures a connection between measurements and the CoG parameter because the CoG parameter is not directly measured, whereas the additional states are.

[0086] One embodiment models the CoG parameter as nearly constant, i.e., it is modeled as a random walk θ.sub.k+1=θ.sub.k+w.sub.θ,k, wherein w.sub.θ,k is a Gaussian distribution. In some embodiments, the vehicle roll dynamics is expressed as

[00003]$\stackrel{.}{x}=\left[\begin{array}{cc}0& 1\\ -\frac{K-mgh}{J}& -\frac{D}{J}\end{array}\right]x+\left[\begin{array}{c}0\\ \frac{mh}{J}\end{array}\right]a_y,$

wherein J=I.sub.x+mh.sup.2 and a.sub.y is the measured lateral acceleration. Some embodiments convert the continuous-time roll dynamics model to a discrete-time model.

[0087] Accordingly, some embodiments write the complete model of vehicle roll dynamics model used in estimating the CoG parameter on the form

θ.sub.k+1=Fθ.sub.k+w.sub.0,k,

x.sub.k+1=A(θ.sub.k)x.sub.k+B(θ.sub.k)a.sub.y,

y.sub.k=Cx.sub.k+e.sub.k,

[0088] where the measurement y.sub.k relates to the state and CoG parameter.

[0089] Using the model of vehicle roll dynamics and a measurement model that relates the model of roll dynamics including the estimated CoG parameter with the measurement, the particle filter determines the probability distribution of the CoG parameter recursively as a weighted combination of the CoG parameter of each particle at each time step of control k,

[00004]$p\left(x_{0:k}|y_{0:k}\right)\approx \underset{i=1}{\overset{N}{.Math.}}{q}_k^i\delta \left(x_{0:k}^i-x_{0:k}\right),$

wherein δ(⋅) is the Dirac delta mass and q.sub.k.sup.i is the weight. Using the model of vehicle roll dynamics including the CoG parameter, one embodiment propagates the CoG parameter forward in time and determines the weight according to q.sub.k.sup.i∝q.sub.k−1.sup.ip(y.sub.k|x.sub.k.sup.i), wherein p(y.sub.k|x.sub.k.sup.i) is the measurement model written as a probability distribution, the likelihood.

[0090] In some embodiments, the prediction 430a of the CoG parameter is done concurrently with predicting vehicle states. For example, one embodiment samples a set of process noise w.sub.θ,k, and propagates a new CoG parameter using the model θ.sub.k+1=θ.sub.k+w.sub.θ,k. Concurrently, another embodiment predicts the vehicle state by inserting updated 421a CoG parameter using the model of vehicle roll dynamics x.sub.k+1=A(θ.sub.k)x.sub.k+B(θ.sub.k)a.sub.y and the measured lateral acceleration of the vehicle.

[0091] FIG. 4C shows a flowchart of one iteration of a method for estimating 420a the CoG parameter using a particle filter according to one embodiment, wherein the particle filter is executed by a processor of the vehicle. The particle filter estimates the probability distribution of the CoG parameter by a set of weighted particles, wherein each particle represents a weighted estimation of the probability distribution of the CoG parameter, wherein the set of particles collectively represents the first and the second-order moments of the estimated probability distribution of the CoG parameter, wherein the weights are determined according to a fit of a predicted measurement using a measurement model to the measurement. Accordingly, the particle filter determines 410c a relation between the roll angle and the roll rate estimated using the roll dynamics model having the uncertainty of the CoG parameter and the received 409c roll angle and the roll rate. Using the determined relation 411c, the particle filter updates 420c the probability distribution of the CoG parameter based on the determined relation to reducing the difference between the roll angle and roll rate and the received values 409c of the roll angle and roll rate.

[0092] FIG. 4D shows a flowchart of one exemplar implementation of updating 420c the probability distribution of the CoG parameter according to one embodiment. Using the determined relation 411c, the method determines 410d weights by determining the fit of the roll angle and the roll rate estimated using the roll dynamics model having the uncertainty of the CoG parameter for the particle to the measurement. Then, the weights are normalized to one to make sure that the sum of weights reflects a probability distribution.

[0093] Using the determined weights, the most likely, i.e., the best, particles are resampled 420d, to remove particles that are less likely. Using the resampled particles 421d, the probability distribution of the CoG parameter is determined 430d based on the weights.

[0094] FIG. 4E shows a simplified schematic of the result of three iterations of steps 420a and 430a when five particles are generated for each iteration. The initial state 410e is predicted forward in time 411e using the model of roll dynamics and five sampled CoG parameters, one for each particle, to produce five next states 421e, 422e, 423e, 424e, and 425e. The probabilities are determined as a function of the fit to the measurement 426e and the probabilistic measurement model 427e of the measurement 426e according to the relation between the roll angle and the roll rate estimated using the roll dynamics model having the uncertainty of the CoG parameter and the received roll angle and the roll rate. At each time step, i.e., at each iteration, an aggregate of the probabilities is used to produce a first moment and a second moment of the CoG parameter 420e.

[0095] FIG. 4F shows possible assigned probabilities of the five different CoG parameters at the first iteration in FIG. 4E. Those probabilities 421f, 422f, 423f, 424f, and 425f are reflected in selecting the sizes of the dots illustrating the CoG parameters 421e, 422e, 423e, 424e, and 425e. Referring back to FIG. 4E, the most likely particles become the initial CoG parameters for the next iteration that again produces five sampled CoG parameters.

[0096] In some embodiments, the number of particles varies with the overall fit to the received values of the roll rate and the roll angle. Particle filters do not need the assumption of a linear model and have convergence rates that are inversely proportional to the square root of the number of particles. However, a finite number of particles is always needed and it is not always straightforward to choose this finite number. To this end, some embodiments adapt the number of particles as a function of the variation of the probabilities according to N.sub.k=ceil(N*(1+Var(q.sub.k))), where q.sub.k.sup.i is the weight of the ith particle, N* is the minimum allowed number of particles.

[0097] Some embodiments are based on the understanding that the particle filter is a computationally heavy Bayesian filter, and for implementation in time-sensitive tasks, such as the control of a vehicle, efficient implementations are necessary. Another embodiment recognizes that the roll angle and roll rate enters linearly into the model of roll dynamics of the vehicle, whereas the CoG parameter enters nonlinearly. Hence, updating the roll angle and roll rate can be done efficiently and analytically using a Kalman filter update for each particle. Doing in such a manner ensures that computational complexity is reduced.

[0098] In some embodiments, the estimate of CoG parameter, as well as the confidence of estimate of CoG parameter, are used to control the vehicle to reduce the likelihood of rollover of a vehicle. Any control method for controlling the vehicle based on the probabilistic estimation of the CoG parameter can be used in the vehicle dynamics control system to control the vehicle using a motion model including the CoG parameter.

[0099] In some embodiments, the control command determined to reduce the likelihood of rollover is an adjustment of a control command determined by a legacy controller of the vehicle, e.g., a steering angle reference or a braking behavior reference. In other embodiments, the control command determined to reduce the likelihood of rollover is an adjustment of the steering behavior of the driver of the vehicle. In yet other embodiments, the control command determined to reduce the likelihood of rollover is done autonomously without a driver operating the nominal behavior of the vehicle.

[0100] FIG. 5A shows an example of a vehicle 501a controlled by a previously designed legacy controller capable of tracking desired references 509a, according to some embodiments. To enforce constraint satisfaction, in some implementations, an adaptive reference governor (ARG) 503a adjusts the desired reference 509a to a constraint-admissible reference 507a using the measurements 505a.

[0101] FIG. 5B illustrates a schematic of components of the vehicle-dynamics control system 199 interfaced with a vehicle controlled by a legacy control system 501b. The desired reference signal 509b can be a steering profile generated by a human operator of the vehicle, or it can be an automatic steering or braking profile generated by the legacy controller. In some embodiments, the vehicle-dynamics control system 199 has the following modules: a Bayesian filter 531b, a constraint-admissible invariant set (CAIS) learning module 521b, and a reference generation module 511b.

[0102] The Bayesian filter 531b estimates the CoG parameter and associated uncertainty, e.g., expressed as the second moment, according to some other embodiments. The vehicle 501b and control system is designed to satisfy constraints 527b that arise from safety considerations, physical limitations, and/or specifications, limiting the range where the outputs, the inputs, and also possibly the states of the vehicle are allowed to operate. E.g., a constraint can be a maximum roll angle of the vehicle, or it can be a maximum allowed load transfer of a vehicle, where values beyond said constraint the vehicle risks rollover. The constraints 528b are defined in continuous space. For example, state constraints are defined in the continuous state space of the vehicle, and reference input constraints are defined in continuous reference input space. The CAIS learning module 521b takes estimates of CoG parameter from the Bayesian filter 531b, along with features and labels stored in the memory 523b to generate constraint-admissible sets using the CPU/GPU or for use in the reference generator module 511b.

[0103] Given a desired reference input 509b such as a commanded steering profile from a driver of the vehicle, the reference generator unit 511b adjusts this desired reference input r to an adjusted reference input v using the CAIS generated by the CAIS learner 521b. The adjusted reference input belongs to a set of admissible references 517b, such as limits of steering angles of a vehicle. The reference generator accesses its memory 513b to find the adjusted reference in the previous time instant and updates that to compute the new adjusted reference. The adjusted reference input is connected with the vehicle via the interface 507b and is guaranteed to enforce constraints 527b without altering the behavior or structure of the legacy control system or, in the case of a fully human-operated vehicle, the behavior of the driver.

[0104] FIG. 5C provides a block representation of the CAIS learner 521c that leverages the CoG parameter estimate bound 513c from the Bayesian filter and stored state features and constraint-admissibility labels 527c to learn an updated CAIS 531b given the constraints 528c, according to some embodiments.

[0105] Some embodiments use parameter-robust CAISs to enforce constraints of the vehicle. Some embodiments let H={(x,v)∈X×V:h(x,v)∈Y} denote the set of state and reference inputs for which the output Y satisfies the constraints. Then the parameter-robust constraint admissible set is the set O({circumflex over (Θ)})⊂H if, for every initial condition (x,v)∈O({circumflex over (Θ)}), when x.sub.0=x and v.sub.t=v for all t≥0, (x.sub.t,v.sub.t)∈H for every θ∈{circumflex over (Θ)} and for all t>0, the set O({circumflex over (Θ)}) is invariant.

[0106] In one embodiment a load transfer ratio related constraint is checked. The load transfer ratio measures how much of the vertical load of the vehicle is concentrated on one of the vehicle sides according to

[00005]$LTR=\frac{F_{z,R}-F_{z,l}}{mg},$

where F.sub.z is the vertical load. Wheel liftoff, hence imminent rollover, happens when the LTR becomes higher than 1 or lower than −1. Hence, in one embodiment the rollover avoidance constraint is determined as LTR∈[−LTR.sub.lim,LTR.sub.lim],0<LTR.sub.lim<1

[0107] To generate estimates of parameter-robust constraint admissible sets, some embodiments employ an offline sampling-driven approach to collect data for learning the sets online as measurements become available.

[0108] Consequently, some embodiments use an estimate of a parameter-robust constraint admissible set to evaluate the control law

v.sub.t=G(v.sub.t−1,x.sub.t,{circumflex over (Θ)}.sub.t,r.sub.t)

=v.sub.t−1+G(v.sub.t−1x.sub.t,{circumflex over (Θ)}.sub.t,r.sub.t)(r.sub.t−v.sub.t−1),

by solving for

G(v.sub.t−1,x.sub.t,{circumflex over (Θ)}.sub.t,r.sub.t):=argmin.sub.γ.sub.t(v.sub.t−r.sub.t).sup.2

subject to: (v.sub.t,x.sub.t)∈O({circumflex over (Θ)}.sub.t−1),

v.sub.t=v.sub.t−1+γ.sub.t(r.sub.t−v.sub.t−1),

0≤γ.sub.t≤1,

v.sub.t∈V.sub.ε({circumflex over (Θ)}.sub.t)  (8)

at each time instant t. Note that V.sub.ε({circumflex over (Θ)}.sub.t) denotes the set of references v such that a ball of radius ε>0 centered at the corresponding steady-state x.sup.ss(v,θ) and v lies inside O({circumflex over (Θ)}.sub.t).

[0109] Some embodiments simulate trajectories of the vehicle using a motion model of the dynamics of the vehicle offline including a CoG parameter, from different initial states sampled from , reference inputs sampled from , and CoG parameter within Θ. At the end of each offline simulation, if an initial condition x.sub.i ∈ tracks the desired reference input v.sub.i ∈ without violating the constraints at any time in the simulation, for a CoG parameter θ.sub.i sampled within Θ, then the feature (x.sub.i,v.sub.i) is labeled ‘+1’ to indicate it resides within the parameter-robust constraint admissible set (θ.sub.i). Contrarily, if the constraint is violated at any time point in the simulation, the feature is labeled ‘-1’ to indicate it resides outside (θ.sub.i). This sets up a binary classification problem that is solved via supervised machine learning by one embodiment.

[0110] FIG. 5D shows a flowchart of a method for generating offline features and labels used in CAIS according to some embodiments. In this case, the method extracts N.sub.x unique samples from and construct grids (not necessarily equidistantly spaced) on and Θ with and N.sub.θ nodes 510d, respectively. Let x.sub.i denote the i-th sampled state, v.sub.j the j-th sampled reference input, and θ.sub.k the k-th sampled CoG parameter. For each (x.sub.i,v.sub.j,θ.sub.k) the method simulates 520d the motion model including the CoG parameter forward in time over a finite horizon T.sub.s with a constant reference v.sub.j and CoG parameter θ.sub.k. The horizon T.sub.s is chosen long enough such that the tracking error is small enough according to predefined criteria by the end of the simulation. One embodiment checks for each simulation whether the rollover avoidance constraint is satisfied for every simulation time-point 530d.

[0111] One embodiment sets the corresponding label of the sample x.sub.i as

[00006]${\ell }_i^{j,k}=\{\begin{array}{cc}+1,& {\mathrm{ify}}_t\in \mathrm{Yforeveryt}\in \left\{0,1,\mathrm{.Math.},T_s\right\},\\ -1,& \mathrm{otherwise}\end{array}$

[0112] At the end of the offline data generation procedure, the method includes a fixed collection of initial {x.sub.i}.sub.i=1.sup.N.sup.x, and each initial condition x.sub.i has a corresponding N.sub.v×N.sub.θ matrix of labels 540d

[00007]${\ell }_i=\left[\begin{array}{ccc}{\ell }_i^{1,1}& \mathrm{.Math.}& {\ell }_i^{1,N_{\theta }}\\ \mathrm{.Math.}& \ddots & \mathrm{.Math.}\\ {\ell }_i^{N_v,1}& \mathrm{.Math.}& {\ell }_i^{N_v,N_{\theta }}\end{array}\right],$

from which a labeled set will be generated online for robust invariant set estimation using supervised learning. Some embodiments require an estimate of the set O({circumflex over (Θ)}.sub.t), which can be obtained using machine learning. In some embodiments, the learning problem is set up as follows. At a time instant t, consider a CoG parameter estimate {circumflex over (Θ)}.sub.t provided by the Bayesian filter. Then, for each v.sub.j∈{tilde over (V)}.sub.t, and each x.sub.i ∈{x.sub.i}.sub.i=1.sup.N.sup.x sampled offline, one embodiment assigns the label

[00008]$z_{i,j}\left({\hat{\Theta }}_t\right)=\underset{k\in I_{i,j}\left({\hat{\Theta }}_t\right)}{\min }{\ell }_i^{j,k},\mathrm{where}$$I_{i,j}\left({\hat{\Theta }}_t\right):=\left\{k:{\theta }_k\in {\hat{\Theta }}_t\right\}$

is the index set of parameters contained in the current confidence interval {circumflex over (Θ)}.sub.t. Taking the minimum ensures that the estimated set is robust to all parameters within {circumflex over (Θ)}.sub.t. That is, if even one θ.sub.t is infeasible for the particular v.sub.j and x.sub.i, then x.sub.i does not belong to the robust parameter invariant set corresponding to {circumflex over (Θ)}.sub.t.

[0113] With the training data D:={(x.sub.i,v.sub.j),z.sub.i,j}, one embodiment constructs classifiers ψ.sub.j, where j=1, . . . , |{tilde over (V)}.sub.t|. For each v.sub.j, a classifier is trained on features {x.sub.i} and their corresponding labels {z.sub.i,j}. These classifiers need to represent inner approximations of the robust parameter invariant sets; to this end, one may select sub-level sets of the decision boundary ψ.sub.k=0 of the classifier until no infeasible sample is contained in the interior of the sub-level set.

[0114] FIG. 6A shows a schematic of the reference generation module 630a in the adaptive reference governor. The updated CAIS 610a obtained from the learning module and the set of admissible references 620a is used to computed adjusted references that satisfy the output constraints.

[0115] One embodiment computes a solution to (8) efficiently using machine learning and gridding . Placing a grid on , along with the constraints, imply that the solution to (8) is contained within the sub-grid of defined by

{tilde over (V)}.sub.t:=[min{r.sub.t,k,v.sub.t,k},max{r.sub.t,k,v.sub.t,k}].  (10)

[0116] Consequently, one embodiment recasts the problem (8) as a grid search,

v.sub.t:=argmin.sub.v∈{tilde over (V)}.sub.t(v−r.sub.t).sup.2

subject to: (v,x.sub.t)∈O({circumflex over (Θ)}.sub.t),

v∈V.sub.ε({circumflex over (Θ)}.sub.t).

[0117] Solving the grid search then becomes identical to selecting the node v.sub.j on the grid {tilde over (V)}.sub.t that minimizes the cost while ensuring that ψ.sub.j(x.sub.t)>0; that is, the current state is predicted by the j-th classifier to belong to the robust parameter invariant set induced by {circumflex over (Θ)}.sub.t.

[0118] FIG. 7 shows examples of updated CAIS based on gradually contracting bounds 725 around the CoG parameter estimate 723, e.g., the first moment, of the CoG parameter at increasing time instants 740, 742, 744. Since the uncertainty around the time instant 740 is higher (interval is larger), the corresponding invariant set is tighter 741, but as the bounds shrink from 740 to 742 to 744, the corresponding CAIS expands from 741 to 743 to 745. In some embodiments, the expansion may completely alter the shape of the CAIS such as 743 to 745.

[0119] Various different controllers are used by different embodiments of the vehicle dynamics control system for reducing the likelihood of rollover of the vehicle, as long as the controller accounts for aspects of the probability distribution of CoG parameter, e.g., the second moment.

[0120] FIG. 8A shows another example of a schematic of components of the vehicle-dynamics control system 199 according to some embodiments. In this example, the Bayesian filter 830 including the CoG estimator 831 using the model of roll dynamics 850 is connected 821 to a stochastic model-predictive controller (SMPC) 810 for controlling the vehicle. The SMPC uses the motion model 860 including the CoG parameter in a control model of the vehicle for controlling the vehicle. The SMPC can include constraints 842 that represent the physical and operational limits of the vehicle. E.g., the rollover avoidance constraint is included in constraints 842. E.g., maximum limits on the steering angle is included in constraints 842, a maximum allowed lateral deviation on the road. During the operation, the SMPC 810 receives a command, e.g., a steering command 801 indicating the desired behavior of the system. In response to receiving the command 801, the SMPC generates a control signal 811 that serves as an input for the vehicle 820 through an interface 812.

[0121] In some embodiments, the control model 840 for SMPC 810 can include one or multiple probabilistic chance constraints 843. As used herein, a chance constraint is a constraint defining physical or geometric limitations of the vehicle accounting for the uncertainty of different aspects of the control model. E.g., a chance constraint can define constraints on the lateral displacement of the vehicle on the road, or it can include the rollover avoidance constraint. Different embodiments can formulate one or multiple of any of the physical limitations and specification constraints for the vehicle as one or multiple probabilistic chance constraints 843, which aim to enforce that the probability of violating the corresponding constraint is below a certain probability threshold value.

[0122] In various embodiments, the control model 840 of the system includes a motion model of the vehicle including the CoG parameter with first and second moments of the estimated probability distribution of the CoG parameter. E.g., the first and second moments of the estimated probability distribution of uncertainty of the CoG parameter are included in the control model.

[0123] FIG. 8B shows a block diagram of an exemplar implementation of a two-level optimization procedure to solve the inequality constrained nonlinear dynamic optimization problem at each control step of the SMPC 810, including probabilistic chance constraints representing the uncertainty of CoG parameter to produce a control command reducing the likelihood of a rollover. The two-level optimization procedure alternates propagation of second moments, i.e., covariance matrices, 850b extracted from the probability distribution of the uncertainty of the CoG parameter of the probabilistic chance constraints within the prediction horizon for fixed values of the state, and control variables 865b with optimization of the state and control variables 860b within the prediction horizon for fixed values of the CoG parameter covariance matrices 855b. The control signal 811 is computed, once a termination condition is met for the two-level optimization procedure.

[0124] In some embodiments of the invention, the two-level optimization procedure 810 consists of three main computational steps. The first step prepares the linear-quadratic objective function, computes the Jacobian matrices to prepare the linearized equality and inequality constraints and it propagates a trajectory of covariance matrices 850b, which represent the uncertainty for the predicted state values over the control horizon influenced by the second moment of the CoG parameter, by evaluating the nonlinear covariance propagation equations for a current trajectory of state, CoG parameter, and control values 865b. The second step consists of the solution of the resulting block-structured QP subproblem, with one or multiple tightened inequality constraints to approximate each of the chance constraints 870b. The third and final step includes the Newton-type update for the current trajectories of optimal state and control values 875b.

[0125] In some embodiments, adjoint gradient computations are used to correct inexact Jacobian information in the SMPC and the resulting two-level optimization procedure consists of three main computational steps. The first step prepares the linear-quadratic objective function, computes the Jacobian matrices with respect to state and control variables to prepare the linearized equality and inequality constraints, computes the adjoint-based gradient evaluation, and numerically eliminates the covariance matrices from each of the objective and constraint functions by propagating a trajectory of covariance matrices, i.e., second moment, 850b for a current trajectory of predicted state and control values 865b using the motion model including CoG parameter. The second step consists of the solution of the resulting block-structured QP subproblem, with one or multiple tightened inequality constraints to approximate each of the chance constraints 870b. The third and final step includes the Newton-type update for the trajectories of optimal state and control values 875b, as well as an expansion of the corresponding update of Lagrange multipliers.

[0126] FIG. 9A shows a block diagram to implement the SMPC using the control model 840 that includes the motion model 860 including the CoG parameter. Specifically, SMPC computes a control solution, e.g., a solution vector 965a that includes a sequence of future optimal or approximately optimal control inputs over a prediction time horizon of the system 960a, by solving a constrained optimization problem 950a at each control time step. The data 945a of the objective function, equality, and inequality constraints in this optimization problem 950a depend on the control model and system constraints 940a, determined from the current state estimate of the system, the estimated CoG parameter, and the control command.

[0127] Embodiments of the invention use a direct optimal control method to formulate the continuous-time SMPC problem as an inequality constrained nonlinear dynamic optimization problem. Some embodiments of the invention use a derivative-based optimization algorithm to solve the inequality constrained optimization problem 950a either exactly or approximately, using an iterative procedure that is based on a Newton-type method and the successive linearization of feasibility and optimality conditions for the optimization problem. Examples of such Newton-type optimization algorithms include interior point methods (IPM) and sequential quadratic programming (SQP). Some embodiments of the invention are based on the realization that the inequality constrained optimization problem 950a has the form of an optimal control structured optimization problem (OCP), such that a structure exploiting implementation of a derivative-based optimization algorithm can be used to compute the solution vector 965a at each control time step.

[0128] In some embodiments of the invention, the solution of the inequality constrained optimization problem 950a uses the exact or approximate state and/or control values over the prediction time horizon from the previous control time step 910a, which can be read from the memory, as a solution guess in order to reduce the computational effort of solving the inequality constrained optimization problem 950a at the current control time step. This concept of computing a solution guess from the solution information at the previous control time step 910a is called warm-starting or hot-starting of the optimization algorithm and it can reduce the required computational effort of the SNMPC in some embodiments of the invention. In a similar fashion, the corresponding solution vector 965a can be used to update and store a sequence of exact or approximate state and/or control values for the next control time step 960a.

[0129] FIG. 9B shows a block diagram of an SNMPC that solves a constrained optimal control structured nonlinear programming problem (OCP-NLP) 950a to compute the control signal 811 at each control time step, given the current state estimate of the system and the estimated CoG parameter that provides the inserted updated model 509a and the command 801. The OCP-NLP 950a includes the state variables x=[x.sub.0,x.sub.1, . . . , x.sub.N], the state covariance matrix variables P=[P.sub.0,P.sub.1, . . . , P.sub.N] predicted using the second moment of the CoG parameter and the control input variables u=[u.sub.0, u.sub.1, . . . , u.sub.N−1] over the prediction time horizon as variables in the optimization problem that needs to be solved at each control time step:

[00009]$\underset{x,u,P}{\min }\underset{k=0}{\overset{N-1}{.Math.}}l\left(x_k,u_k+{\mathrm{Kx}}_k\right)+m\left(x_N\right)$$s.t.\{\begin{array}{cc}\forall k\in \left\{0,\mathrm{.Math.},N-1\right\},& \\ 0=x_{k+1}-f\left(x_k,u_k+{\mathrm{Kx}}_k,0\right),& x_0={\hat{x}}_t,\\ 0=P_{k+1}-\left({\tilde{A}}_k{P}_k{\tilde{A}}_k^{\top }+{\tilde{B}}_k.Math.{\tilde{B}}_k^{\top }\right),& P_0={\hat{P}}_t,\\ u_{\min }\le u_k+{\mathrm{Kx}}_k\le u_{\max },& \\ 0\ge h_i\left(x_k,u_k+{\mathrm{Kx}}_k\right)+{\alpha }_i\sqrt{C_{k,i}{P}_k{C}_{k,i}^{\top }},\forall i,& \end{array}$

wherein the probability distribution of the CoG parameter is used to determine the state covariance matrix over the prediction horizon.

[0130] Some embodiments are based on the current state estimate in an initial state value constraint x.sub.0={circumflex over (x)}.sub.t, a motion model of the vehicle including the CoG parameter as a first moment that results in linear and/or nonlinear equality constraints 952a, a linearization-based approximation of the uncertainty propagation through the motion model resulting in linear and/or nonlinear covariance propagation equations 953a including the covariance matrix Σ of the probability distribution of the CoG parameter, bounds 954a on control inputs and linear and/or nonlinear inequality constraints 955a and a linear-quadratic or nonlinear objective function 951a, each of which are defined over the prediction time horizon in the optimal control structured optimization problem. The OCP-NLP data 945a of the objective function, equality, and inequality constraints in this optimization problem 950a depend on the motion model and constraints 940a, the current state of the vehicle, the estimated first moment of CoG parameter, and the control command 801. Examples of the OCP-NLP data 945a include objective functions, e.g., l(⋅) and m(⋅) and constraint functions, e.g., f(⋅) and h.sub.i(⋅). Other examples of the OCP-NLP data 945a include constraint vectors, e.g., {circumflex over (x)}.sub.t, vec({circumflex over (P)}.sub.t), u.sub.min and u.sub.max and matrices, e.g., Ã.sub.k, {tilde over (B)}.sub.k, K and C.sub.k,i.

[0131] In some embodiments, one or multiple of the inequality constraints 955a can be defined as probabilistic chance constraints that aim to ensure that the probability of violating the corresponding inequality constraint is below a certain probability threshold value, i.e., the back-off coefficient value α.sub.i>0 for probabilistic chance constraints and α.sub.i=0 for standard deterministic inequality constraints. Note that deterministic inequality constraints aim to ensure that the corresponding inequality constraint is satisfied for the expected value of the trajectory of state and control values.

[0132] Using an approximate formulation of the probabilistic chance constraints 955a, based on an individual tightening for each of the inequality constraints, the resulting inequality constrained nonlinear dynamic optimization problem can be solved using a Newton-type optimization algorithm that is based on successive linearization of the optimality and feasibility conditions. Examples of such Newton-type optimization algorithms include interior point methods (IPM) and sequential quadratic programming (SQP). Some embodiments of the invention are based on the realization that an SQP algorithm solves a quadratic program (QP) approximation for the stochastic nonlinear OCP at each iteration of the SQP optimization algorithm, based on a linear-quadratic approximation of the objective function and a linearization-based approximation for the discretized system dynamics and the discrete-time covariance propagation equations and a linearization-based approximation for each of the inequality constraints and for each of the tightened probabilistic chance constraints.