Model-based control with uncertain motion models

Abstract

A stochastic feedback controller for controlling the behavior of a robotic system using a stochastic filter subject to structural constraints on the behavior of the robotic system is configured to execute a stochastic filter that recursively estimates a distribution of a current state of the robotic system, taking into account previous states of the robotic system, based on a kinematic model of the robotic system perturbed by stochastic process noise and a measurement model of the robotic system perturbed by stochastic measurement noise, with uncertainty modeled as a time-varying Gaussian process represented as a weighted combination of time-varying basis functions with weights determined by corresponding Gaussian distributions. The stochastic filter recursively updates both the distribution of the current state of the robotic system and the Gaussian distribution of the weights of the basis functions, the basis functions being selected to satisfy the structural constraints implied by the measurements of the states of the robotic system.

Description

[Technical field]

[0001] The present invention relates generally to model-based control, and more particularly to stochastic control for controlling the motion of robotic systems using constrained stochastic filters. [Background technology]

[0002] Optimization-based control, such as model predictive control (MPC) or state feedback control, enables a model-based design framework that can directly consider system dynamics as well as state and input constraints. MPC is used in many applications to control dynamic systems of various complexities. Examples of such systems include production lines, automobile engines, robots, numerically controlled machining, satellites, and power generators.

[0003] For example, MPC is based on real-time finite horizon optimization of a model of a system. MPC has the ability to predict future events and take appropriate control actions. This is achieved by optimizing the system's behavior over a finite future time horizon subject to state and input constraints and exercising control only over the current time step.

[0004] MPC can predict changes in the state variables of a modeled system caused by changes in the control variables. The state variables define the state of the system; that is, the state of a controlled system is the minimum set of state variables in the state-space representation of the control system that can represent the entire state of the system at any given time. For example, if the controlled system is a robotic system such as an autonomous vehicle, the state variables may include the position, speed, and heading of the vehicle. Control variables are inputs to the system designed to change the state of the machine. For example, in chemical processes, the controlled variables are often pressure, flow rate, temperature, valve opening, and damper stiffness. State variables in these processes are other measurements that represent either the control objective or the process constraints.

[0005] MPC uses a model of the system, current system measurements, the current dynamic state of the process, and state and control constraints to calculate future changes in state variables. These changes are calculated to keep the state variables near their goals, subject to constraints on both the control and state variables. MPC typically sends out only the first change of each control variable to implement, and then repeats the calculation when the next change is needed.

[0006] The performance of model-based control necessarily depends on the quality of the predictive model used in the optimal control computation. A predictive model describes the dynamics of a system, i.e., the evaluation of the system over time. For example, a predictive model is a nonlinear function that describes the dynamics of the system, linking the previous and current states of the controlled system based on the control inputs to the system.

[0007] However, in many applications, the kinematic model of the controlled system is partially unknown or uncertain, and in addition, the unknown parts of the controlled system are often subject to structural constraints. In such cases, applying control to the uncertain model can lead to suboptimal performance or even instability of the controlled system. For example, in some situations, some parameters of the kinematic model are not accurately measured. Therefore, the controller may have to estimate unknown parameters of the machine model. Conventional approaches to address such problems include adaptive or learning-based MPC, where the MPC control problem is enhanced with a closed-loop identification scheme to learn the unknown machine parameters. Learning the unknown parameters improves the machine behavior achieved by the controller. See, for example, US2011 / 0022193. [Prior art documents] [Patent documents]

[0008] [Patent Document 1] US2011 / 0022193 Summary of the Invention [Problem to be solved by the invention]

[0009] However, in addition to or instead of uncertainties in the parameters of the motion model, in some situations the dynamics of the system, i.e. the functional relationships between successive states of the controlled system, are changing, in which case adaptive or learning-based methods for estimating the parameters of the model may be insufficient.

[0010] In addition, as the dynamics of the system are changing, the associated learning of the controller must be done recursively, i.e., in real time. However, if the controlled system is subject to structural constraints and such constraints are not incorporated into the learning process, the learning will be inefficient or will result in learning a system that is not physical and therefore cannot be used for subsequent control.

[0011] Therefore, there is a need for a model-based controller that can learn a kinematic model of the control system dynamics in real time by incorporating structural constraints into the learning process.

[0012] An objective of some embodiments is to provide probabilistic control of a robotic system using a kinematic model describing the dynamics of the system. Additionally or alternatively, another objective of some embodiments is to provide model-based control of a system using a kinematic model with uncertainties. Additionally or alternatively, another objective of some embodiments is to provide model-based control of a system using a kinematic model with uncertainties in the dynamics of the system, the motion of the robotic system being subject to structural constraints. Additionally or alternatively, another objective of some embodiments is to estimate a kinematic model of a controlled robotic system when the parameters and / or dynamics of the system's motion are unknown and / or partially known and incorporate structural constraints into the estimation of the kinematic model. [Means for solving the problem]

[0013] Typically, a robotic system can be modeled with at least two models (equations). The first model is a kinematic model of the robotic system that relates the state of the system to previous states of the system and to the inputs to the system. The kinematic model captures the dynamics of the system. The kinematic model typically includes noise or disturbances that represent the uncertainty of the kinematic model and / or disturbances of the kinematic model. This uncertainty is referred to herein as process noise. The second model is a measurement model that relates the available measurements of the system to the state of the system. The measurement model also includes measurement noise and / or other uncertainties that are referred to herein as measurement noise. These uncertainties are referred to herein as model uncertainties.

[0014] In addition, the state of the system is also subject to uncertainties, referred to herein as state uncertainty. In particular, process noise and measurement noise combine with model uncertainty to cause state uncertainty. However, while state and model uncertainties are closely coupled, they are distinct from process noise and measurement noise. Specifically, state uncertainty is internal to the system's state and model values, while process noise and measurement noise are disturbances to the state.

[0015] If the process noise, the measurement noise, and the model are known, i.e., the shape and parameters of the distribution of the process noise and the measurement noise, and the nonlinear functions describing the model are known, then both the state and the state uncertainty of the system can be estimated by various techniques, for example using Kalman filters or particle filters. Both the state and the state uncertainty of the system are important for some control applications. For example, the state of the system can be used to determine the control inputs to the system to achieve the control objective, and the state uncertainty can be used to adjust the control inputs to ensure the feasibility of the control.

[0016] For example, if the distributions of process and measurement noise are known and the motion and measurement models are known, a particle filter can be used to represent the state and state uncertainty of the system as a set of particles, where the state of the system is a weighted combination of the particles, with the weights being determined according to the fit of the particles with the measurement model.

[0017] However, in some applications the motion model is not fully known before run-time, for example for a vehicle driving on a road the surface cannot be sensed unless the vehicle is moving, and any controller is based on a motion model that is uncertain or even fully unknown during the start-up phase. Additionally or alternatively, the dynamics of the system may be unknown, partially known or changing over time.

[0018] Thus, in some applications, it is necessary not only to estimate the distribution of the current state of the system, but also to estimate the motion model of the system. Therefore, some embodiments estimate the state and motion model of the system taking into account previous states, the measurement model, and the process and measurement noise that perturb the motion and measurement models, respectively.

[0019] The task of estimating a kinematic model of a system can be viewed as a task of reducing the uncertainty of the kinematic model. The kinematic model relates the parameters of the robotic system, which represent the structural and operational configuration of the robotic system, to variables that represent the inputs and outputs of the robotic system's operation with these parameters. Reducing the uncertainty of the kinematic model is thus a matter of reliably learning the parameters of the kinematic model. For example, learning the parameters may be the task of learning the actual mass of a load carried by a robotic arm. This task is difficult in itself, but what further complicates the problem is that the parameters of the kinematic model may be functions that vary over time. For example, friction between the road and the vehicle varies over time based on several different factors, such as the speed and acceleration of the vehicle and / or the type of road surface. Thus, an objective of some embodiments is to learn functions of parameters that affect the operation of the robotic system.

[0020] Some embodiments are based on the realization that the uncertainty of the parameters of the kinematic model, or the uncertainty of the functions of the parameters that affect the behavior of the robotic system, can be represented as a time-varying Gaussian process. Some embodiments are based on the realization that due to the time-varying nature of the functions of the parameters, updating the kinematic model requires updating the Gaussian process, which is a computationally challenging problem. However, some embodiments are based on the realization that the Gaussian process can be modeled as a set of weighted basis functions, where the weights are distributed according to a Gaussian distribution. In this way, the learning problem is greatly simplified, since the problem of learning the model is reduced to learning the weights of each basis function. In other words, to update the Gaussian process, some embodiments only need to update these Gaussian distributions of the weights, and to determine the kinematic model, some embodiments only need to determine N scalar weights from the Gaussian distribution.

[0021] Indeed, by considering the motion model as a weighted combination of basis functions, the computational requirements for probabilistically estimating tire friction are significantly reduced. For example, in some embodiments, a Gaussian process is represented as a weighted combination of time-varying basis functions, where the weights are defined by the corresponding Gaussian distributions, such that the time-varying mean of the Gaussian process is a function of the basis functions modified by the mean of the corresponding Gaussian distribution, and the time-varying variance of the Gaussian process is a function of the basis functions modified by the variance of the corresponding Gaussian distribution.

[0022] However, while representing a Gaussian process using basis functions simplifies the computation, this representation introduces additional problems. Specifically, in some situations, the function of the parameters has a structure that reflects the nature of the parameters and / or the robotic system. For example, the function of the parameters may be symmetric, which indicates the symmetry of the effect of the parameter variations on the performance of the system. The structure of the function of the parameters may be asymmetric, such as in the tire friction example, which may affect, for example, the Lyapunov stability of the robotic system's motion. Therefore, the update of the Gaussian process of the parameter function uncertainty needs to be done under constraints on the shape of the function. Such constraints are referred to herein as structural constraints.

[0023] The types of structural constraints are evident from measurements of the state of the robotic system. Specifically, a series of measurements of the state of the robotic system at different times indicates structural constraints on the shape of the functions of the parameters that affect the behavior of the robotic system. This dependency is probabilistic, but exists and differs between different kinds of robotic systems.

[0024] Thus, an objective of some embodiments is to update a Gaussian process that represents the uncertainty of a function of parameters subject to structural constraints on the shape of the function of parameters affecting the behavior of the robotic system. Additionally or alternatively, an objective of some embodiments is to use these dual updates to track the state of the system, i.e., to use a kinematic and measurement model of the robotic system to update the state of the robotic system and to update a time-varying Gaussian process of the possibly time-varying parameters of the kinematic model.

[0025] Unfortunately, constrained updates of Gaussian processes subject to structural constraints are computationally intractable, making such constrained updates impractical for some applications.

[0026] Some embodiments are based on the realization that when a Gaussian process is represented by a weighted combination of basis functions, structural constraints may be imposed on the selection of the shape of the basis functions. That is, if the basis functions are selected in a predetermined manner based on the structural constraint, any weighted combination of these basis functions will also satisfy the structural constraint. As a simple example, if the structural constraint on the shape of the function of the parameters is that the shape is asymmetric, the basis functions need to be odd. Conversely, if the shape is symmetric, an even number of basis functions need to be selected. In this way, the constrained update is replaced by a selection of basis functions based on structural constraints indicated by a series of measurements of the state of the robotic system, followed by an unconstrained update of the Gaussian process represented by these selected basis functions, which is much more computationally efficient than the constrained update.

[0027] Accordingly, one embodiment discloses a stochastic feedback controller for controlling a behavior of a robotic system using a stochastic filter subject to structural constraints on the behavior of the robotic system, the stochastic feedback controller comprising at least one processor and a memory having instructions stored thereon, which when executed by the at least one processor cause the controller to: collect digital representations of a series of measurements of a state of the robotic system at different times, the digital representations being indicative of structural constraints on shapes of functions of parameters affecting the behavior of the robotic system; and execute a stochastic filter to recursively estimate a distribution of a current state of the robotic system, taking into account previous states of the robotic system, based on a kinematic model of the robotic system perturbed by stochastic process noise and a measurement model of the robotic system perturbed by stochastic measurement noise. wherein one or a combination of the kinematic model, the process noise, the measurement model, and the measurement noise include parameters with uncertainty modeled as a time-varying Gaussian process represented as a weighted combination of time-varying basis functions with weights determined by corresponding Gaussian distributions, the time-varying mean of the Gaussian process is a function of the basis functions modified with the mean of the corresponding Gaussian distribution, and the time-varying variance of the Gaussian process is a function of the basis functions modified with the variance of the corresponding Gaussian distribution, the stochastic filter recursively updates both a distribution of a current state of the robotic system and a Gaussian distribution of the weights of the basis functions, each of the basis functions being selected to satisfy a structural constraint implied by a sequence of measurements of the state of the robotic system, and the instructions further cause the controller to perform a control action determined based on an estimate of the distribution of the current state of the robotic system to alter the current state of the robotic system in accordance with a control objective.

[0028] Another embodiment discloses a method for controlling the operation of a robotic system using a stochastic filter subject to structural constraints on the operation of the robotic system, the method using a processor in combination with stored instructions implementing the method, the instructions when executed by the processor performing steps of the method including collecting digital representations of a series of measurements of a state of the robotic system at different time points indicative of structural constraints on shapes of functions of parameters affecting the operation of the robotic system, and executing a stochastic filter, the stochastic filter configured to recursively estimate a distribution of a current state of the robotic system taking into account previous states of the robotic system and based on a kinematic model of the robotic system perturbed by stochastic process noise and a measurement model of the robotic system perturbed by stochastic measurement noise. the step of: one or a combination of the motion model, the process noise, the measurement model, and the measurement noise includes parameters with uncertainty modeled as a time-varying Gaussian process represented as a weighted combination of time-varying basis functions with weights determined by corresponding Gaussian distributions; a time-varying mean of the Gaussian process is a function of the basis functions modified with the mean of the corresponding Gaussian distribution; a time-varying variance of the Gaussian process is a function of the basis functions modified with the variance of the corresponding Gaussian distribution; the step of: recursively updating both a distribution of a current state of the robotic system and a Gaussian distribution of weights of the basis functions, each of the basis functions being selected to satisfy a structural constraint implied by a series of measurements of the state of the robotic system; and performing a control action determined based on an estimate of the distribution of the current state of the robotic system to modify the current state of the robotic system in accordance with a control objective.

[0029] Another embodiment discloses a non-transitory computer-readable storage medium including a program executable by a processor to perform a method, the method comprising: collecting digital representations of a series of measurements of a state of a robotic system at different time instants, the digital representations being indicative of structural constraints on shapes of functions of parameters affecting a behavior of the robotic system; and executing a stochastic filter configured to recursively estimate a distribution of a current state of the robotic system, taking into account previous states of the robotic system, based on a kinematic model of the robotic system perturbed by stochastic process noise and a measurement model of the robotic system perturbed by stochastic measurement noise, the kinematic model being indicative of a linear distribution of the current state of the robotic system, the process noise being indicative of a linear distribution of the current state of the robotic system, the process noise being indicative of a linear distribution of the current state of the robotic system, the measurement model ... the fitting includes parameters with uncertainties modeled as a time-varying Gaussian process represented as a weighted combination of time-varying basis functions with weights determined by corresponding Gaussian distributions, a time-varying mean of the Gaussian process is a function of the basis functions modified with the mean of the corresponding Gaussian distributions, a time-varying variance of the Gaussian process is a function of the basis functions modified with the variance of the corresponding Gaussian distributions, a stochastic filter recursively updates both a distribution of a current state of the robotic system and a Gaussian distribution of weights of the basis functions, each of the basis functions being selected to satisfy a structural constraint implied by a series of measurements of the state of the robotic system, and further includes performing a control action determined based on an estimate of the distribution of the current state of the robotic system to modify the current state of the robotic system in accordance with a control objective. [Brief description of the drawings]

[0030] [Figure 1A] FIG. 2 shows a schematic diagram of the operation of a Kalman Filter (KF) according to some embodiments. [Figure 1B] FIG. 1 shows a schematic diagram of a particle filter (PF) used by some embodiments. [Diagram 2] 1 shows an illustration of the principle of dual updating according to some embodiments; [Figure 3A]FIG. 1 shows a schematic diagram of a probabilistic filter that performs a double update recursively in accordance with some embodiments. [Figure 3B] 1 shows a schematic diagram of a particular example of a probabilistic filter, namely a Particle Filter (PF). [Figure 4A] 1 illustrates an iterative flowchart of a method for stochastic feedback control to control the motion of a robotic system using a stochastic filter subject to structural constraints on the motion of the robotic system, according to some embodiments. [Figure 4B] 1 illustrates a flowchart of an execution of one iteration of a probabilistic filter according to one embodiment. [Figure 4C] FIG. 1 shows a block diagram of an apparatus for controlling a system according to some embodiments. [Figure 5A] FIG. 1 shows an illustration of the use of weighted basis functions according to one embodiment. [Figure 5B] 4 shows a diagram of weight averages and weight covariances associated with different basis functions according to some embodiments; [Figure 5C] 5 shows a diagram of the effect of basis function weights on a subset of functions of parameters 540c, according to some embodiments. [Figure 5D] 4 shows a block diagram of one iteration of a method for updating 440b the probability distributions of basis function weights according to one embodiment. [Figure 5E] FIG. 2 illustrates a block diagram of a method for determining a combination of states for updating weights of a weighted combination of basis functions according to one embodiment. [Figure 6A] 1 illustrates a flowchart of a method for identifying a type of shape of a function of parameters that preserves structural constraints that influence the behavior of a robotic system, according to some embodiments. [Figure 6B] 1 illustrates a flowchart of a method for determining structural constraints offline, according to one embodiment. [Figure 6C] 1 illustrates a flowchart of one iteration of a method for determining structural constraints online, according to one embodiment. [Figure 6D]1 shows a table illustrating some of the most common structural constraints used in some embodiments. [Figure 6E] 1 shows a set of basis functions that satisfy these constraints and a table of example basis functions according to some embodiments. [Figure 7A] 1 shows a flowchart of a method for selecting basis functions according to an embodiment. [Figure 7B] 1 illustrates fitting a shape to measurements according to some embodiments. [Figure 7C] 1 illustrates fitting a shape to measurements according to one embodiment. [Figure 7D] 1 shows an illustration of the reduction of error due to imposing structural constraints, according to one embodiment. [Figure 8A] 8 shows a block diagram of a stochastic feedback controller 850a according to some embodiments. [Figure 8B] FIG. 2 illustrates a block diagram of discrete-time or discretized propagation of state mean and covariance information according to one embodiment. [Figure 8C] FIG. 13 shows a block diagram of discrete-time or discretized propagation of state mean and covariance information according to another embodiment. [Figure 9A] FIG. 1 illustrates a block diagram of a system and method for stochastic nonlinear model predictive control (SNMPC) according to some embodiments. [Figure 9B] FIG. 1 illustrates a block diagram of an SNMPC controller for solving constrained optimal control structured nonlinear programs (OCP-NLPs), according to one embodiment. [Figure 9C] FIG. 1 illustrates a block diagram of an SNMPC controller for solving constrained optimal control structured nonlinear programs (OCP-NLPs) according to another embodiment. [Figure 9D] FIG. 1 illustrates a block diagram of an SNMPC controller that uses statistical linearization-based state mean and covariance propagation to solve constrained optimal control-structured nonlinear programming problems (OCP-NLP), according to some embodiments. [Figure 10A]1 shows a schematic diagram of a vehicle including a stochastic controller employing the principles of some embodiments. [Figure 10B] 1 illustrates a schematic diagram of the interaction between a predictive controller and a vehicle controller according to some embodiments. [Figure 10C] 1 shows a schematic diagram of an autonomous or semi-autonomous controlled vehicle that can be dynamically realised and often times an optimal trajectory can be calculated by using some embodiments. [Figure 10D] FIG. 1 shows a schematic diagram of a vehicle controlled by an SNMPC controller that aims to track a dynamically feasible and optimal trajectory using an embodiment of the present invention. [Figure 10E] 1 illustrates an example of a vehicle traveling on a road network according to some embodiments. [Figure 11] 1 illustrates an example of a vehicle traveling on a road network according to some embodiments. [Figure 12A] FIG. 1 illustrates a schematic diagram of a controller for controlling a drone, according to some embodiments. [Figure 12B] 1 illustrates a multi-device motion planning problem according to some embodiments of the present disclosure. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0031] An objective of some embodiments is to provide probabilistic control of a robotic system using a kinematic model describing the dynamics of the system. Additionally or alternatively, another objective of some embodiments is to provide model-based control of a robotic system using a kinematic model with uncertainties. Additionally or alternatively, another objective of some embodiments is to provide model-based control of a system using a kinematic model with uncertainties in the dynamics of the system, the motion of the robotic system being subject to structural constraints. Additionally or alternatively, another objective of some embodiments is to estimate a kinematic model of a controlled robotic system when the parameters and / or dynamics of the system's motion are unknown and / or partially known and incorporate structural constraints into the estimation of the kinematic model.

[0032] A robotic system, as used herein, may refer to any mechanical system capable of operating autonomously, such as a road vehicle, a stationary robotic manipulator, a mobile robot, a drone, a set of the aforementioned entities, or a combination of the aforementioned entities.

[0033] The behavior of a robotic system can be modeled using at least two models, each represented by a set of equations. The first model is a kinematic model of the robotic system that relates the state of the system to previous states of the system and inputs to the system. The kinematic model captures the dynamics of the system. The kinematic model typically includes noise or disturbances that represent the uncertainty of the kinematic model and / or disturbances of the kinematic model. This uncertainty is referred to herein as process noise. The second model is a measurement model that relates the available measurements of the system to the state of the system. The measurement model also includes measurement noise and / or other uncertainties that are referred to herein as measurement noise. These uncertainties are referred to herein as model uncertainties.

[0034]

number

[0035]

number

[0036] If the process noise, the measurement noise, and the model are known, i.e., the shape and parameters of the distribution of the process noise and the measurement noise, and the nonlinear functions describing the model are known, then both the state and the state uncertainty of the system can be estimated by various techniques, for example using Kalman filters or particle filters. Both the state and the state uncertainty of the system are important for some control applications. For example, the state of the system can be used to determine the control inputs to the system to achieve the control objective, and the state uncertainty can be used to adjust the control inputs to ensure the feasibility of the control.

[0037] For example, if the distributions of process and measurement noise are known and the motion and measurement models are known, a particle filter can be used to represent the state and state uncertainty of the system as a set of particles, where the state of the system is a weighted combination of the particles, with the weights being determined according to the fit of the particles with the measurement model.

[0038] However, in some applications the motion model is not known before run-time, for example in the case of a vehicle driving on a road, the surface cannot be sensed unless the vehicle is moving, and any controller is based on a motion model that is uncertain at the start-up phase, since the road surface that affects tire friction is not fully known. Additionally or alternatively, the dynamics of the system may be unknown, partially known, or changing over time.

[0039] Thus, in some applications, it is necessary not only to estimate the distribution of the current state of the system, but also to estimate functions of the parameters contained in the motion model of the system. Therefore, some embodiments estimate states and functions of the parameters contained in the motion model of the system, taking into account previous states, the measurement model, and process and measurement noise that perturb the motion and measurement models, respectively.

[0040] Some embodiments are based on the realization that functions of parameters that affect the behavior of a robotic system can be expressed as time-varying Gaussian processes. Some embodiments are based on the realization that updating a kinematic model requires updating a Gaussian process, which is a computationally difficult problem, because updating a Gaussian process using a set of measurements describing a robotic system, with or without structural constraints, grows with the length of the sequence of measurements, making recursive updates of Gaussian processes for real-world systems such as robotic systems computationally difficult to perform. Some embodiments are based on the realization that to update a Gaussian process efficiently, structural constraints on the shape of the function of parameters need to be incorporated into the Gaussian process formulation.

[0041] Therefore, other embodiments are based on the understanding that a Gaussian process can be modeled as a set of weighted basis functions, where the weights are distributed according to a Gaussian distribution. In this way, the learning problem is greatly simplified, since the problem of learning a model is reduced to learning the weights of each basis function. In other words, to update a Gaussian process, some embodiments only need to update these Gaussian distributions of weights, and to determine a motion model, some embodiments only need to determine N scalar weights from a Gaussian distribution. Indeed, considering the motion model as a weighted combination of basis functions greatly reduces the computational requirements for estimating unknown functions of parameters in a probabilistic manner.

[0042] Another embodiment is based on the realization that if structural constraints on the behavior of the robotic system exist, and are not taken into account during the learning process, the learning problem is computationally intractable, even if the Gaussian process is represented as a set of weighted basis functions, with the weights distributed according to a Gaussian distribution. Moreover, the learned functions of the parameters may not satisfy the underlying structural constraints.

[0043] Therefore, one embodiment determines the basis functions used in the basis function representation based on ensuring satisfaction of the structural constraints, such that the learned shape of the function of the parameters affecting the behavior of the robotic system satisfies the structural constraints.

[0044] In some embodiments, the probabilistic filter recursively updates both the distribution of the current state of the robotic system and a Gaussian distribution of the weights of the basis functions, i.e., the distribution of the weights, where each of the basis functions is selected to satisfy the structural constraints implied by the sequence of measurements of the state of the robotic system. Such a probabilistic filter can be realized, for example, by Kalman filtering, particle filtering, or other nonlinear filtering methods, where the probabilistic filter performs a dual update of the state and the Gaussian distribution of the weights of the basis functions.

[0045] FIG. 1A shows a schematic diagram of the operation of a Kalman filter (KF) according to some embodiments. The KF is a particular example of a probabilistic filter. The KF is a tool for state estimation in a linear state space model and is an optimal estimator when the noise sources are known and Gaussian, in which case the state estimates are also Gaussian distributed. The KF estimates the mean and variance of the Gaussian distribution, since the mean and variance are two quantities necessary and sufficient statistics to describe the Gaussian distribution.

[0046] The KF starts with initial knowledge 110a of the states to determine the mean of the states and their variances 111a. The KF then uses a model of the system to predict 120a the states and the variances to the next time step, thereby obtaining updated means and variances 121a of the states. The KF then determines updated means and variances 141a of the states by using measurements 130a in an update step 140a that uses a measurement model of the system. An output 150a is then obtained and the procedure is repeated for the next time step 160a.

[0047] Some embodiments use probabilistic filters including various variants of KFs, such as extended KFs (EKFs), linear-regression KFs (LRKFs), such as unscented KFs (UKFs), and particle filters. Although multiple variants of KFs exist, they conceptually function as illustrated in FIG. 1E. Importantly, KFs use measurements 130a described by a probabilistic measurement model to update the first and second moments, i.e., the mean and covariance, of the probability distribution of interest.

[0048] FIG. 1B shows a schematic diagram of a particle filter (PF) used by some embodiments. PF is a tool for state estimation in nonlinear state space models with stochastic, possibly non-Gaussian processes and measurement noise. PF recursively estimates the distribution of states using measurements at each time step by propagating particles forward in time, which include state hypotheses and weights that use a measurement model to determine the state's consistency with the measurements. FIG. 1B shows a simplified schematic diagram of the results of three iterations generating state trajectories when five particles are generated for each iteration. An initial state 110b is predicted forward in time (111b) using a model of motion and inputs to the system to generate five next states 121b, 122b, 123b, 124b, and 125b. Probabilities are determined as a function of measurements 126b and models of noise sources. At each time step, i.e., at each iteration, an ensemble of probabilities is used to generate an aggregate state 120b.

[0049] Some embodiments are based on the understanding that probabilistic filters such as KF, PF, and their variants typically operate under the assumption of a known motion model and a known measurement model. Using the motion model, we predict from a previous state to a current state based on a control input, and using the measurement model, we compare the predicted state with the measurement and update the state by inserting the predicted state into the measurement model, taking the difference, and minimizing the difference. However, if either the motion model or the measurement model has an uncertainty different from the process and / or measurement noise, the one-time update shown in Figures 1A and 1B is not applicable.

[0050] One embodiment allows the application of KF and PF type methods by incorporating a dual update embedded in the probabilistic filter, i.e. state update and function update of parameters with uncertainties affecting the model.

[0051] FIG. 2 shows an illustration of the principle of dual updating according to some embodiments. First, the received measurements 210a are used for an update 215a to the current state. Then, the updated state 219a is used together with the received measurements 210a to update 217a a function of the parameters of the robotic system. The updated function of the parameters 229a is then used when predicting 225a a state using previous states and control inputs. Then, the function of the parameters is predicted 227a, outputting at least one of the state 235a and the function of the parameters 237a from the probabilistic filter. In some embodiments, the entire model is unknown and thus the function of the parameters 217a is updated. In other embodiments, at least one function of the parameters of the model is updated 217a.

[0052] 3A shows a schematic diagram of a stochastic filter that performs a double update recursively, according to some embodiments. Some embodiments are based on the recognition that stochastic filters such as KF and PF can estimate the distribution of states by including the state 312a and the correction term 314a in the filter 310a and determining the fitting of the state 312a to the measurement value using a measurement model 324a that is affected by the measurement noise 326a and correcting it. However, when the state is included in the filter, only the state can be updated, and not the uncertainty that is part of the motion model 320a, because in such a configuration the motion model 320a is external to the estimator 310a, i.e., external to the estimation equation, and the prediction step is driven by a deterministic known motion model 320a, and the probabilistic uncertainty comes from the process noise 322a. In this way, the outside of the filter motion model 320a does not change.

[0053] Some embodiments are based on the understanding that the probabilistic filter may also include a motion model of the robotic system with parameters that affect the robotic system that are uncertain or unknown (330a). In this way, the motion model goes inside the filter, i.e., the filter has its own beliefs about the motion model and the uncertainties it contains. For example, the filter 340a may include a motion model 347a with a distribution of a function of the unknown parameters 344a, which effectively means a motion model with a probability distribution. In fact, since some of the motion models in the filter are uncertain, the modified probabilistic filter may update not only the state of the system, but also functions of the parameters that affect the motion model of the system.

[0054] Some embodiments are based on the realization that in order to include a kinetic model in a filter, the filter must include the states 342a, the parameters 344a included in the kinetic model 347a, measures of uncertainty of the parameters that affect the kinetic model, and corrections 346a that determine the joint fitting of the states and the kinetic model to the measurement model 324a. This ensures that the corrections in the filter 340a reflect the quality of the estimated parameters and thereby indeed reflect the kinetic model.

[0055] Some embodiments are based on the realization that the uncertainty of the function of parameters that influence the kinetic model can be modeled as a Gaussian process over possible functions of the parameters. The kinetic model is a function, often nonlinear, that describes the evolution of the state over time. Each sample for the Gaussian process over the function of the parameters is a sampled function of the parameters.

[0056] FIG. 3B shows a schematic diagram of a particular example of a probabilistic filter, i.e., a particle filter (PF). Some embodiments are based on the recognition that the PF can estimate the distribution of states by including in each particle 310b a state 312b and a weight 314b, and determining the fitting of the state 312b to the measurement using a measurement model 324b affected by the measurement noise 126c, where the weight 314b is a correction term 314a. However, if the state is included in the particle, only the state can be updated, and not the function of the parameters 320b, because in such a configuration, the motion model 320b is external to the particle 310b, the parameters are included in the motion model, all particles are driven by the same motion model 320b, and the probabilistic uncertainty comes from the process noise 322b. In this way, the outside of the particle motion model 320b does not change.

[0057] Some embodiments are based on the understanding that the PF may also include a kinetic model with uncertain parameters for each particle (330b). In this way, the parameters are internal to the particle, i.e., each particle will have its own parameters and therefore its own kinetic model. For example, particle 340b may include parameters 344b included in kinetic model 347b, and particle 350b may include parameters 354b included in kinetic model 357b. In fact, since the kinetic models of different particles may be different, the modified PF may update not only the state of the system, but also the parameters that affect the kinetic model of the system.

[0058] Some embodiments are based on the realization that to include a kinetic model for each particle, the particle must include states 342b and 352b, uncertain parameters 344b and 354b, a kinetic model 347b and 357b, and weights 346b and 356b that determine the joint fitting of the states and kinetic model with the uncertain parameters to the measurement model 324b. In this way, it is ensured that the weighting of each particle 340b and 350b reflects the quality of the parameter estimates.

[0059] Some embodiments are based on the realization that the uncertainty of the function of the parameters can be modeled as a Gaussian process over the possible functions of the parameters affecting the robotic system. The parameters are often nonlinear functions, describing the dependence of the parameters as a function of the state. Each sample of the Gaussian process over the possible functions of the parameters is a sampled function of the parameters, which in effect represents a new sample of the kinematic model. The parameters of the Gaussian process, such as the mean and the transformation of the Gaussian process, capture the uncertainty of the function of the parameters, so the sampled function represents the uncertainty. Therefore, when the parameters of the kinematic model are unknown, some embodiments model the parameters using a Gaussian process, which is a distribution over the functions of the state.

[0060] Thus, in some embodiments, parameters 344b and 354b are included in different particles as distributions of parameters, i.e., parameters with uncertainties modeled as Gaussian processes over the possible parameters affecting the system. Different particles may thus have different parameters with different distributions. For example, in some embodiments, the Gaussian process distribution of the parameter of one particle is different from the Gaussian process distribution of the parameter of at least one other particle.

[0061] Some embodiments are based on the recognition that the unknown parameters can be considered as probabilistic uncertainties in a model of the system's motion. In addition, an embodiment recognizes that there are typically other disturbances acting on the system's motion, for example due to uncertainties in the actuators generating the control inputs, or other disturbances.

[0062] One embodiment is based on the realization that the distribution of parameters can be determined as a weighted combination of the possible parameters for each particle, weighted according to the particle's weight.

[0063] An embodiment is based on the recognition that a function can be expressed as a weighted sum of basis functions, for example as a weighted sum of trigonometric functions or a weighted sum of polynomial functions. A Gaussian process is a generalization of the Gaussian distribution to the distribution of functions. Thus, some embodiments recognize that a Gaussian process can be expressed as a weighted sum of basis functions, where the weights have a Gaussian distribution.

[0064] 4A shows an iterative flowchart of a method for stochastic feedback control to control the behavior of a robotic system using a stochastic filter subject to structural constraints on the behavior of the robotic system, according to some embodiments. The feedback control can be implemented using various controllers in the literature, such as model predictive control, linear-orthogonal control, or any other controller that can utilize the estimated information from the stochastic filter. The feedback controller comprises at least one processor and a memory having instructions stored thereon, which when executed by the at least one processor, performs a method of collecting a series of measurements of a state of the robotic system at different time points that are indicative of structural constraints on the shape of a function of a parameter that affects the behavior of the robotic system. The method searches a memory configured to store a probabilistic filter and executes 420a the probabilistic filter, which takes into account inputs 414a to the system and previous states 416a of the robotic system, and recursively estimates a distribution 425a of current states of the robotic system based on a kinematic model 415a of the robotic system perturbed by stochastic process noise and a measurement model 415a of the robotic system perturbed by stochastic measurement noise, and further taking into account the collected measurements 411a. One or a combination of the motion model, the process noise, the measurement model, and the measurement noise include parameters with uncertainties modeled as a time-varying Gaussian process represented as a weighted combination of time-varying basis functions with weights determined by corresponding Gaussian distributions, the time-varying mean of the Gaussian process is a function of the basis functions modified with the mean of the corresponding Gaussian distribution, and the time-varying variance of the Gaussian process is a function of the basis functions modified with the variance of the corresponding Gaussian distribution, and the probabilistic filter recursively updates both the distribution 425a of the current state of the robotic system and the Gaussian distribution of the weights of the basis functions, i.e., the distribution of the weights, and each of the basis functions is selected to satisfy a structural constraint implied by a series of measurements of the state of the robotic system.The method then performs 430a a control action based on the estimate of the distribution 425a of the current state of the robotic system and a function of the parameters of the robotic system to modify the current state of the robotic system according to a control objective.

[0065] FIG. 4B illustrates a flow chart of the execution of one iteration of the stochastic filter according to one embodiment. Using a motion model 405b of the robotic system with uncertain parameters determined in a previous iteration, control inputs 406b determined during a previous iteration, and states 407b, the stochastic filter updates 410b the state according to the estimated parameters of the vehicle. In some embodiments, the function of the parameters included in the motion model 405b is represented by a distribution over the states. In these embodiments, the method samples the distribution to generate a sampled function of the parameters and updates the state with the corresponding sampled parameters. The stochastic filter then measures 420b the state to generate a measured state 425b. Using the measured state 425b and a measurement relationship 427b relating the state to the measured state, the stochastic filter updates 430b the state distribution to generate an updated state 435b. The method 420a then updates 440b the parameters included in the motion model of the robotic system to generate updated parameters 445b.

[0066] The update 440b of the parameter function, i.e., the Gaussian process distribution, can be done in a number of ways: In one embodiment, the update is done by updating the parameters that define the Gaussian process distribution based on a combination of states with parameter uncertainty and states determined without parameter uncertainty, and parameters determined in previous iterations.

[0067] 4C shows a block diagram of an apparatus 410c for controlling a system according to some embodiments. The apparatus includes a memory 420c for storing a probabilistic filter, the filter including a model 421c of the system with uncertainty, a state 422c of the system determined using the uncertainty of the motion model, and an uncertainty measure 423c that determines a joint fitting of the model and the state to measurements.

[0068] The apparatus 410c also includes at least a sensor 450c that measures a signal and generates a digital representation of a series of measurements indicative of a state of the system, and a processor 430c configured as follows: the processor executes a probabilistic filter that recursively estimates a distribution of a current state taking into account previous states and parameters; updates a distribution of a function of the parameters based on a difference between a state determined using uncertainties in the parameters of the particle and a state determined when inserted into the measurement model, the current estimate of the function of the parameters being a weighted combination of time-varying basis functions having weights defined by a Gaussian distribution; generates values ​​of control inputs based on the current estimated distributions of the functions of the parameters included in the motion model and the current estimate of the state of the system; and executes a controller configured to control the system using the values ​​of the control inputs.

[0069] Some embodiments are based on the realization of a scheme that simplifies the estimation of Gaussian processes for determining functions of parameters. Specifically, in some embodiments, the Gaussian process is considered as a weighted combination of a finite number of basis functions, each basis function being a function of the system state, and the Gaussian process for functions of parameters is captured by a Gaussian distribution of the weights of the basis functions. In other words, to update the Gaussian process, some embodiments only need to update these Gaussian distributions of weights, and to determine the motion model, some embodiments can determine a weighted combination using these N scalar weights from the Gaussian distribution. Indeed, considering the function of parameters as a weighted combination of basis functions significantly reduces the computational requirements for probabilistically estimating the function of parameters, which is important for real-time implementation.

[0070] Figure 5A shows an illustration of the use of weighted basis functions according to one embodiment. In this figure, there are three basis functions 510, 520a, and 530a. Also shown is the true function of the parameters 340a that maps previous states to the current state along with the kinetic model. By combining the basis functions and using different weights for each basis function, they can be combined to reproduce the true function of the parameters and therefore the true kinetic model.

[0071] 5B shows a diagram of weight means and weight covariances associated with different basis functions, according to some embodiments. For example, basis function 530a has weight distribution 531a, basis function 520a has weight distribution 521a, and basis function 510a has distribution 511a.

[0072] 5C shows a diagram of the effect of basis function weights on a subset of functions of parameter 540c, according to some embodiments. By weighting functions 520c, 530c very lightly and weighting 510c very heavily, basis function expansion can reproduce the function of parameter 540c with one basis function. While Figures 5A-5C are simplifications of reality, they illustrate the principles of basis functions and the computational efficiencies they can have.

[0073]

number

[0074]

number

[0075]

number

[0076] This can be inserted into the motion model of the system.

[0077] 5D illustrates a block diagram of one iteration of a method for updating the probability distribution of basis function weights 440b, according to one embodiment, which updates 510d the weights of a weighted combination of basis functions using a combination of determined current and previous states 520d, and updates 520d the probability distribution of a function of parameters according to the weighted combination of weighted basis functions using the updated weights.

[0078] FIG. 5E illustrates a block diagram of a method for determining a combination of states for updating weights of a weighted combination of basis functions according to one embodiment.

[0079] The method uses sampled current states 509e and previous states 508e inserted into basis functions 507e to map the sampled current states 509e and previous states 508e inserted into basis functions 507e to a set of numerical values ​​515e stored in a matrix. The method then uses the determined numerical values ​​515e and a probability function 520e that maps the numerical values ​​515e to a distribution of weights.

[0080]

number

[0081] Some embodiments are based on the realization that the satisfaction of structural constraints on the behavior of the robotic system is achieved by enforcing constraints on the basis functions themselves: in other words, by selecting a particular set of basis functions, the structural constraints are enforced on the basis functions and thus also on the Gaussian processes that model functions of parameters affecting the robotic system.

[0082] Various embodiments are based on the recognition that a set of measurements is indicative of structural constraints that affect the behavior of a robotic system. In particular, a set of measurements of a state of a robotic system is related to values ​​of parameters that affect the behavior of the robotic system through one or a combination of a kinematic model and a measurement model, and thus a set of measurements of a state of a robotic system is related to values ​​of parameters that statistically preserve structural constraints on the shape of a function of the parameters.

[0083] Another embodiment is based on the idea that in order to determine basis functions that satisfy a structural constraint, the structural constraint itself must be determined. In some cases, what the structural constraint is will be obvious from the physical configuration of the robot system, but in most cases, the structural constraint is unknown and must be determined.

[0084] FIG. 6A shows a flowchart of a method for identifying a type of shape of a function of parameters that preserves structural constraints that affect the behavior of a robotic system, according to some embodiments. The method predicts (610a) a series of measurements 618a using a model 608a of the robotic system, including a parameter-free kinematic model and a measurement model, and inputs 609a to the system. The method then compares (620a) measurement data using the predicted series of measurements 618a and the series of measurements 619a to generate compared data 625a. As an example, the compared data 625a may be the difference between the measured values ​​and the predicted estimates for a series of measurements from different starts. Finally, the method determines (630a) a shape 635a of a function of parameters that preserves structural constraints based on the data difference 625a.

[0085] In various embodiments, the structural constraints are determined offline, while in other embodiments, the structural constraints are determined online.

[0086] FIG. 6B shows a flow chart of a method for determining structural constraints offline, according to one embodiment, where basis functions are selected offline in response to the determination of structural constraints. Using inputs 609b to the system, the method runs 610b the robotic system using the inputs 609b, resulting in a state motion 618b of the system. The inputs must be diverse enough to stimulate the state space effectively enough. The method then measures 620b the system, resulting in a digital representation of a series of measurements 625b. Using a nominal model 628b without a weighted combination of basis functions inserted into the model, the measurements 625b are compared 630b, resulting in a predicted output using the nominal model 628b. Using the resulting difference 635b, the method determines 640b structural constraints 645b and selects 650b basis functions 655b that satisfy the structural constraints 645b.

[0087] Some embodiments determine the structural constraints 640b by characterizing the deltas 635b relative to the inputs to the system. Other embodiments classify the constraints relative to previously tabulated structural constraints.

[0088] FIG. 6C illustrates a flow chart of one iteration of a method for determining structural constraints online, according to one embodiment, where basis functions are selected online during operation of a robotic system in response to determining structural constraints. Initially, the method starts 610c the robotic system using a range of control inputs 605c. Using the control inputs 605c, the method collects 620c a series of measurements 625c in response to starting the robotic system 615c using the control inputs. Using the inputs 605c to the system, the method compares 630c the measurements 625c to predicted outputs using a nominal model 618c using a nominal model 615c that does not have a weighted combination of basis functions inserted into the model. Using the resulting differences 635c, the method updates 640c the structural constraints 645c and selects 650c basis functions 655c that satisfy the structural constraints 645c.

[0089] In some embodiments, various probabilistic filters are employed to estimate the system with different structural constraints on the shape of the function of the parameters that affect the behavior of the robotic system. As an example, some embodiments employ PFs with various structural constraints on the shape of the function of the parameters, while other embodiments employ KF type filters.

[0090] 6D shows a table illustrating some of the most common structural constraints used in some embodiments. For example, one embodiment uses a control input to start the system and measures the response, resulting in a difference 635b as a function of the previous state and the input to the system, resulting in a mapping as a function of state. This mapping is compared and matched with each constraint, and if the behavior is similar to the category of the constraint, the structural constraint is determined to belong to this category.

[0091] FIG. 6E shows a set of basis functions that satisfy these constraints and a table of example basis functions, according to some embodiments.

[0092] Linear operator constraints, such as those shown in Fig. 6D, include various constraints, e.g., divergence free vector fields, curl free vector fields, derivatives, integrals, etc. For such complex constraints, a systematic procedure for determining the basis functions is needed.

[0093]

number

[0094]

number

[0095] Therefore, one embodiment discloses a method for offline selection of basis functions according to a predetermined rule and a solution of a linear system of equations. FIG. 7A shows a flow chart of a method for selecting basis functions according to one embodiment. The method is executed using a processor. First, the method selects a nominal set of basis functions (710a). For example, one embodiment selects a nominal set of triangular basis functions (710a). Using the selected (710a) basis functions 715a, the method determines (720a) operators that satisfy the structure constraints. Then, using the operators 725a, the method selects (730a) basis functions 735a.

[0096] FIG. 7B illustrates fitting a shape to measurements, according to some embodiments. In this figure, the parameter is tire friction, which can be described by a function of slip. For longitudinal tire friction, which is tire friction in the forward direction of the tire, to determine the shape, one embodiment starts the vehicle using different brake and acceleration inputs. With an acceleration sensor, it is possible to measure acceleration, which is friction if the accelerometer is located on the wheel, and a scaled and rotated version of tire friction if not. The result is some measurements 710b that only show friction due to noise in the sensor. However, by collecting a series of measurements, it is possible to estimate that the tire friction function is asymmetric.

[0097] 7C illustrates fitting a shape to measurements according to one embodiment, where measurements 710b and their noise characteristics have been used to find a distribution from which the friction value can be obtained. For example, distributions 740b and 750b have already been found. By performing this procedure on a series of measurements, a distribution 720c can be determined that statistically preserves the shape of the constraint.

[0098] Some embodiments are based on the realization that by imposing structural constraints on the shape of the functions of the parameters, the difference between the predicted state of the robotic system using a kinematic model having the parameters and the measured state of the robotic system is reduced.

[0099] 7D shows an illustration of the reduction of error due to imposing structural constraints, according to one embodiment, where a function of parameters 720d has a symmetric structural constraint and its distribution 730d is estimated with and without the symmetry constraint. When predicting a state according to a function of parameters 740d, introducing the symmetry constraint improves the fit to measurements 710d when imposing the constraints.

[0100] 8A shows a block diagram of a stochastic feedback controller 850a according to some embodiments, which initiates the system such that the estimated state 821a and output 803a of the system follow the command 801a, taking into account the estimated uncertain parameters 822a. The stochastic feedback controller 850a includes a computer, e.g., in the form of a single central processing unit (CPU) or multiple CPU processor 860a, connected to a memory 865a, the memory for storing a model 840a, e.g., an uncertainty model 841a in the form of a weighted set of basis functions, constraints 842a on the behavior of a real system 820a with uncertainty 825a, and probabilistic chance constraints 843a. The processor 860a may be a single-core microprocessor, a multi-core processor, a computing cluster, a network of multiple connected processors, or any number of other configurations. The memory 865a may include a random access memory (RAM), a read-only memory (ROM), a flash memory, or any other suitable memory system.

[0101] 8B shows a block diagram of discrete-time or discretized propagation of state mean and covariance information according to some embodiments. The propagation is performed using an explicit linearization-based propagation equation for the discrete-time or discretized nonlinear system dynamics, taking into account the initial state estimates and corresponding uncertainties 840, and considering Gaussian approximations of the probability density functions for the (time-varying) uncertainties and / or disturbances in the controlled system. In some embodiments of the invention, the discrete-time or discretized nonlinear system dynamics is k+1 =f(x k ,u k ,w k ) where x k ,u k and w k is the time step t k denote the state variables, control inputs, and disturbance variables in, respectively, and x k+1 is the time step t k+1In some embodiments of the invention, the mechanical model of the controlled system may consist of a set of continuous-time differential equations, e.g., explicit and / or implicit ordinary differential equations (ODEs), or explicit and / or implicit differential algebraic equations (DAEs). In some embodiments of the invention, the mechanical model of the controlled system may consist of a discretization of the set of continuous-time differential equations using a numerical integration method, e.g., a linear multistep method, an explicit or implicit Runge-Kutta method, a backward differential equation, or a finite element method.

[0102]

number

[0103]

number

[0104] Finally, in accordance with some embodiments of the present invention, one or more additional steps 855 may be performed in discrete-time or discretized propagation of state mean and covariance information using explicit linearization-based propagation equations of the discrete-time or discretized nonlinear system dynamics.

[0105]

number

[0106] Some embodiments of the present invention are based on the recognition that the use of unscented Kalman filtering (UKF) can compute more accurate propagation of mean and covariance information for nonlinear system dynamics than explicit 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 the UKF is a special case of a more comprehensive class of linear regression Kalman filtering (LRKF) that is part of a more comprehensive class of Gaussian assumed density filters (ADFs) that can be used to implement probabilistic predictive controllers for controlled systems under uncertainty. Some embodiments of the present invention are based on the recognition that the ADF uses a statistical linearization based on approximated matching of one and / or more high-order moment integrals instead of an explicit linearization based on Taylor series approximation (e.g., in the EKF). Thus, while the EKF is a first-order method based on explicit linearization for handling nonlinearities, the class of ADFs based on statistical linearization can achieve second-order or better accuracy in discrete-time or discretized propagation of mean and covariance information of state variables through nonlinear system dynamics.

[0107] Some embodiments of the present invention are based on the recognition that for certain problems, a statistical linearization based on matching of one and / or multiple higher order moment integrals can be analytically performed, which further improves the accuracy of the propagation of mean and covariance information and thus the performance of a stochastic predictive controller for a controlled system under uncertainty.

[0108]

number

[0109]

number

[0110] Finally, according to some embodiments of the invention, one or more additional steps 875 may be performed in the discrete-time or discretized propagation of state mean and covariance information using statistical linearization-based propagation equations for the discrete-time or discretized nonlinear system dynamics of the controlled system under uncertainty. In some embodiments of the invention, a different set of integration points and weights may be used in one or more steps of the statistical linearization-based propagation equations.

[0111]

number

[0112]

number

[0113] 9A illustrates a block diagram of a system and method for stochastic nonlinear model predictive control (SNMPC) that implements a stochastic predictive controller 850a that takes into account the current state estimate 821a of the system, and calculates a control signal 811a taking into account estimated uncertainties 822a and control commands 801a, according to some embodiments. Specifically, SNMPC calculates a control solution by solving a constrained optimization problem 950 at each control time step, e.g., a solution vector 965 that includes a set of future optimal or near-optimal control inputs over a forecast time horizon of the system 960. The objective function, equality and inequality constraints data 945 in this optimization problem 950 depend on the dynamic model and system constraints 940, the current state estimate 821a of the system, the estimated uncertainties 822a, and the control commands 801a.

[0114] 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 950 exactly or approximately using an iterative procedure based on Newton-type methods and continuous 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 present invention are based on the recognition that the inequality-constrained optimization problem 950 has the form of an optimal control structured nonlinear program (NLP) and can be used to calculate the solution vector 965 at each control time step using a structure that utilizes an implementation of a derivative-based optimization algorithm.

[0115] In some embodiments of the present invention, the solution of the inequality constrained optimization problem 950 uses exact or approximate control input, state average, and / or covariance values ​​over the forecast time horizon from the previous control time step 910, which can be read from memory, as a guess of the solution, thereby reducing the computational effort of solving the inequality constrained optimization problem 950 at the current control time step. This concept of computing a guess of the solution from the solution information at the previous control time step 910 is called a warm start or hot start of the optimization algorithm, which can reduce the required computational effort of the SNMPC controller in some embodiments of the present invention. Similarly, the corresponding solution vector 965 can be used to update and store a set of exact or approximate control input, state average, and / or covariance values ​​for the next control time step 960. In some embodiments of the present invention, a more accurate guess of the solution of the inequality constrained optimization problem 950 at the current control time step can be computed by using a time shifting procedure, taking into account the control input, state average, and / or covariance values ​​over the forecast time horizon from the previous control time step 910.

[0116]

number

[0117]

number

[0118]

number

[0119]

number

[0120]

number

[0121]

number

[0122]

number

[0123]

number

[0124]

number

[0125] Using the approximation formula 975 or 985 of the probabilistic chance constraints 955 and based on the individual strengthening of each of the inequality constraints, the resulting inequality-constrained nonlinear dynamic optimization problem can be solved using a Newton-type optimization algorithm based on successive linearization of 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 present invention are based on the recognition that the SQP algorithm solves a quadratic programming (QP) approximation of the stochastic nonlinear OCP at each iteration of the SQP optimization algorithm, which is based on a linear-quadratic and linearization-based approximation of the objective function for the discretized system dynamics and the discrete-time covariance propagation equation, and a discrete-time covariance propagation equation and linearization-based approximation for each of the inequality constraints and each of the strengthened probabilistic chance constraints.

[0126] In some embodiments of the invention, the stage and / or terminal costs in the objective functions 951, 971 or 981 can be defined by any linear, linear-quadratic and / or nonlinear smooth functions, including either convex and / or non-convex functions. The objective functions 951, 971 or 981 of the stochastic optimal control problem may include cost terms corresponding to each time point of the prediction time horizon. In some embodiments, the objective function includes a (nonlinear) least-squares type penalty of the deviation of a particular output function of the system from a set of reference output values ​​at each time point of the prediction time horizon, resulting in a reference tracking type formulation of the cost function in the stochastic feedback controller 850a.

[0127] FIG. 10A illustrates a schematic diagram of a vehicle 1001 including a stochastic controller 1002 employing the 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 this motion includes 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 controllable by a driver of the vehicle 1001.

[0128] The vehicle may also include an engine 1006 that may be controlled by the controller 1002 or by other components of the vehicle 1001. The vehicle may also include one or more sensors 1004 for sensing the surrounding environment. Examples of the sensors 1004 include distance range finders, radar, lidar, and cameras. The vehicle 1001 may also include one or more sensors 1005 for sensing its current momentum and internal status. 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 provide information to the controller 1002. The vehicle may also include a transceiver 1006 that enables communication functions of the controller 1002 through wired or wireless communication channels.

[0129] FIG. 10B illustrates a schematic diagram of the interaction between the 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, which control the rotation and acceleration of the vehicle 1020. In such a case, the 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, for example a lane keep assist controller 1035, that further processes the control inputs of the predictive controller 1002. In either case, the controller 1020 maps using the output of the predictive controller 1002 to control the motion of the vehicle by controlling at least one actuator of the vehicle, such as the steering wheel and / or brakes of the vehicle.

[0130] FIG. 10C shows a schematic diagram of an autonomous or semi-autonomous controlled vehicle 1050 that can use some embodiments to calculate a dynamically realizable and often optimal trajectory 1055. The generated trajectory aims to keep the vehicle within a certain road boundary 1052 and avoid other non-controlled vehicles, i.e., obstacles 1051 of 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 a mixed integer optimal control problem, including one or more additional discrete variables for each obstacle. For example, based on an embodiment configured to implement a mixed integer model predictive controller, the autonomous or semi-autonomous controlled vehicle 1050 can make discrete decisions in real time, such as to overtake another vehicle on the left or right side, or alternatively to stay behind another vehicle in the current lane of the road 1052. An embodiment of the invention is based on an SNMPC controller that directly takes into account uncertainties about the current and predicted states of the vehicle 1050, uncertainties about the parameters in the vehicle model, and uncertainties about 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.

[0131] 10D shows a schematic diagram of a vehicle 1065 controlled by an SNMPC controller that aims 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 using an embodiment of the present invention. FIG. 10D shows 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. By using a probabilistic predictive controller with probabilistic chance constraints according to some embodiments of the present invention, the controlled vehicle may violate the road boundary constraints 1060 and / or 1061 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 road upper constraint 1060, illustrating the behavior of a stochastic predictive controller that aims to satisfy both deterministic and probabilistic chance constraints on a controlled system under uncertainty.

[0132] Examples of uncertainties of the system and its environment may include any time-varying parameters related to the friction behavior between the tires of the vehicle and the road surface, for example, parameters in the Pacejka tire force model that can be learned online while controlling the vehicle. The estimation function and estimation uncertainty of the parameters can be defined as time-varying and uncertain disturbance variables in a direct optimal control problem formulation of a stochastic nonlinear model predictive controller according to an embodiment of the present invention. In some embodiments, the estimation parameter is the friction of the tires with the surface of the road on which the vehicle travels, described by a nonlinear friction function with a structural constraint of being asymmetric, and the basis functions are selected to be only odd functions.

[0133] In some embodiments, for controlling the vehicle, the control inputs include commands specifying values ​​of one or a combination of steering angles of the vehicle's wheels and rotational speeds of the wheels, and the measurements include values ​​of one or a combination of vehicle rotational speed and vehicle acceleration. Each state of the vehicle includes a vehicle speed and a heading speed, and the motion model relates values ​​of the control inputs to first values ​​of the vehicle states through the dynamics of the vehicle at successive time steps, and the measurement model relates the measurements to second values ​​of the vehicle states at the same time steps.

[0134] 10E illustrates multiple vehicles traveling simultaneously on roads in a road network. For example, the vehicles can be controlled to maximize throughput or traffic flow 1090 on the roads. For example, in one embodiment, traffic flow is estimated by a weighted set of basis functions, and the estimated traffic flow on the road network is used to suggest a route for a particular vehicle that can increase the estimated traffic flow on the road network.

[0135] FIG. 11 illustrates an example of a vehicle traveling on a road network, according to some embodiments.

[0136] In this example, multiple vehicles 1100, 1110, 1120 are moving on a given road 1101. Each vehicle can perform multiple maneuvers. For example, a vehicle can stay on the same route (1150, 1190, 1180) or it can change route (or lane) (1160, 1170). Each vehicle has its own sensing capabilities, e.g., lidar, camera, etc. Each vehicle can send and receive information to and from its neighbors (1130, 1140). , and / or indirectly through other vehicles via a remote server. For example, vehicles 1100 and 1180 can exchange information through vehicle 1110. In this type of communication network, information can be transmitted over most of the road 1101. In addition, traffic information such as red lights can be sent and received.

[0137] Some embodiments are based on the realization that for a certain road, it is equally likely that the number of vehicles traveling in a particular direction on the road is similar to the number of vehicles traveling in the opposite direction on the road, which implies an antisymmetric structural constraint. Other embodiments are based on the understanding that if there is a red light or stop sign on the road, this implies a boundary condition, i.e., zero flow at the boundary of that road segment.

[0138] FIG. 12A illustrates a schematic diagram of a controller 1211 for controlling a drone 1200 according to some embodiments. FIG. 12A illustrates a schematic diagram of a quadcopter drone as an example of the drone 1200 according to an embodiment of the present disclosure. The drone 1200 includes an actuator for causing the motion of the drone 1200 and a sensor for perceiving the environment and the position of the device 1200. The rotor 1201 may be an actuator, and the sensor for perceiving the environment may include a light detection and ranging (LIDAR) 1202 and a camera 1203. Furthermore, the sensor for localization may include a GPS 1204. Such sensors may be integrated with an inertial measurement unit (IMU) that can measure acceleration, rotational speed, and magnetic fields. The drone 1200 also includes a communication transceiver 1205 for transmitting and receiving information, and a control unit 1206 for processing data obtained from the sensors and the transceiver 1205, computing commands to the actuators 1401, and computing data to be transmitted via the transceiver 1205. Additionally, it may include an estimator 1207 that tracks the state of the drone.

[0139] Further, based on the information transmitted by drone 1200, controller 1211 is configured to control the motion of drone 1200 by calculating a motion plan for drone 1200. The motion plan for drone 1200 may include one or more trajectories along which the drone moves. In some embodiments, there are one or more devices (drones, such as drone 2400) whose motion is coordinated and controlled by controller 1211. Controlling and coordinating the motion of the one or more devices corresponds to solving a mixed integer optimization problem.

[0140] In different embodiments, the controller 1211 obtains parameters of the task from the drone 1200 and / or a remote server (not shown). The parameters of the task include the state of the drone 1200, but may include more information. In some embodiments, the parameters may include one or a combination of an initial position of the drone 1200, a target position of the drone 1200, a geometry of one or more stationary obstacles that define at least a portion of the constraints, and a geometry and motion of moving obstacles that define at least a portion of the constraints. The parameters are submitted to a motion planner to obtain an estimated motion trajectory for performing the task, and the motion planner is configured to output the estimated motion trajectory for performing the task.

[0141] FIG. 12B illustrates a multi-device motion planning problem according to some embodiments of the present disclosure. In FIG. 12B, multiple devices (e.g., drone 1201b, drone 1201a, drone 1201c, drone 1201d) are shown that are required to reach assigned final positions 1202c, 1202b, 1202b, and 1202d. Additionally, obstacles 1203a, 1203b, 1203c, 1203d, 1203e, and 1403f in the surrounding environment of drones 1201a-1201d are shown. Drones 1201a-1201d are required to reach their assigned final positions 1202a-1202d while avoiding obstacles 1203a-1203f in the surrounding environment. Simple trajectories (such as trajectory 1204 shown in FIG. 142) may cause collisions. Thus, embodiments of the present disclosure calculate trajectories 1205 that avoid obstacles 1203a-1203f and avoid collisions between drones 1201a-1201d, which can be achieved by avoiding trajectory overlap or, if multiple trajectories overlap 1206, by ensuring that corresponding drones reach the overlapping point at times in the future planning time horizon that are sufficiently separated.

[0142] Drones need to navigate indoors using various sensing information. Some embodiments are based on the fact that the indoor environment creates magnetic field anomalies that can be used to navigate indoors. Other embodiments recognize that positioning using magnetic fields does not require additional infrastructure other than an IMU, and line of sight is not required, such as when using radar / lidar.

[0143]

number

[0144] The above-described embodiments of the present disclosure 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 executed 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 being components of the integrated circuit. However, the processor may be implemented using circuits in any suitable format.

[0145] Also, the various methods or processes outlined herein may be coded as software executable on one or more processors using any one of a variety of operating systems or platforms. In addition, such software may be written using any of a number of suitable programming languages ​​and / or programming or scripting tools, and may be compiled as executable machine language code or intermediate code that runs on a framework or virtual machine. Typically, the functionality of the program modules may be combined or distributed as desired in various embodiments.

[0146] Also, embodiments of the present disclosure may be implemented as a method, an example of which is provided. The order of operations performed as part of the method may be determined in any suitable manner. Thus, embodiments may be configured to perform operations in an order different from that illustrated, which may include performing some operations simultaneously, although in the illustrated embodiment they are shown as a sequence of operations.

[0147] Although the disclosure has been described with examples of preferred embodiments, it will be understood that various other adaptations and modifications can be made within the spirit and scope of the disclosure. Therefore, it is the object of the appended claims to cover all such variations and modifications that come within the true spirit and scope of the disclosure.

Claims

1. 1. A stochastic feedback controller for controlling a motion of a robotic system using a stochastic filter subject to structural constraints on the motion of the robotic system, the stochastic feedback controller comprising at least one processor and a memory having instructions stored thereon, the instructions, when executed by the at least one processor, causing the controller to: collecting a digital representation of a series of measurements of the state of the robotic system at different times that indicate construction constraints on the shapes of functions of parameters that affect the behavior of the robotic system; and executing a stochastic filter, the stochastic filter being configured to recursively estimate a distribution of a current state of the robotic system taking into account previous states of the robotic system and based on a kinematic model of the robotic system perturbed by stochastic process noise and a measurement model of the robotic system perturbed by stochastic measurement noise, wherein one or a combination of the kinematic model, the process noise, the measurement model, and the measurement noise are represented as a weighted combination of time-varying basis functions with weights determined by corresponding Gaussian distributions. the parameters having uncertainty modeled as a time-varying Gaussian process, the time-varying mean of the Gaussian process being a function of the basis functions modified by the mean of the corresponding Gaussian distribution, the time-varying variance of the Gaussian process being a function of the basis functions modified by the variance of the corresponding Gaussian distribution, the probabilistic filter recursively updates both a distribution of the current state of the robotic system and the Gaussian distribution of the weights of the basis functions, each of the basis functions being selected to satisfy the structural constraints implied by the sequence of measurements of the state of the robotic system, and the instructions to the controller further include: a stochastic feedback controller that executes a control action determined based on an estimate of a distribution of the current state of the robotic system to modify the current state of the robotic system in accordance with a control objective.

2. 2. The stochastic feedback controller of claim 1, wherein each measurement in the set of measurements of a state of the robotic system is related to a value of the parameter through one or a combination of the kinematic model and the measurement model, and the set of measurements of a state of the robotic system is related to a set of values ​​of the parameter that statistically preserves the structural constraint on a shape of a function of the parameter.

3. The stochastic feedback controller of claim 2 , wherein a smoothed representation of the data fitted to the range of values ​​of the parameters comprises a function having a shape that preserves the structural constraints.

4. The stochastic feedback controller of claim 2 , wherein statistical characteristics of the sequence of values ​​of the parameters depend on the structural constraints.

5. 3. The stochastic feedback controller of claim 2, wherein imposing the structural constraint on the shape of the function of the parameters reduces a difference between a predicted state of the robotic system using the kinematic model having the parameters and a measured state of the robotic system.

6. The stochastic feedback controller of claim 1 , wherein the structural constraints are determined offline based on experimental or simulation data of the operation of the robotic system, and the basis functions are selected offline in response to the determination of the structural constraints.

7. The stochastic feedback controller of claim 1 , wherein the structural constraints are determined online during control of the robotic system, and the basis functions are selected during operation of the robotic system in response to determining the structural constraints.

8. To select the basis functions, the processor: actuating the robotic system using a range of control inputs to collect training measurements of states of the robotic system; estimating states of the robotic system resulting from actuation by the range of control inputs using different probabilistic filters with different structural constraints on the shapes of functions of the parameters affecting the motion of the robotic system; selecting the structural constraint from the probabilistic filter that optimally estimates the training measurements according to a cost function; Selecting the basis functions that comply with the selected structural constraints The stochastic feedback controller of claim 7 , configured as follows:

9. To select the basis functions, the processor: actuating the robotic system using a range of control inputs to collect training measurements of states of the robotic system; estimating states of the robotic system resulting from actuation with control inputs in the range using a probabilistic filter without a nominal function of the parameters; determining a pattern of distribution of data points of differences between the training measurements and corresponding estimates of states of the robotic system; Transforming the pattern into the structural constraints The stochastic feedback controller of claim 7 , configured as follows:

10. The stochastic feedback controller of claim 9 , wherein the pattern is represented as a system of equations that depends on the parameters, and the basis functions that enable a solution of the system of equations satisfy the structural constraints.

11. 2. The stochastic feedback controller of claim 1, wherein the stochastic filter is a Kalman filter, and the motion model including the parameters inserted into the Kalman filter enables the stochastic filter to perform a dual update of the state of the robotic system and the time-varying Gaussian process.

12. 2. The stochastic feedback controller of claim 1, wherein the stochastic filters are particle filters, and the motion model including the parameters associated with each particle filter enables the stochastic filters to perform a dual update of the state of the robotic system and the time-varying Gaussian process.

13. The stochastic feedback controller of claim 1 , wherein each of the basis functions is selected to be an odd function to satisfy the structure constraints.

14. The stochastic feedback controller of claim 1 , wherein each of the basis functions is selected to be an even function to satisfy the structure constraints.

15. 2. The stochastic feedback controller of claim 1, wherein the robotic system includes a vehicle traveling on a road having uncertain road conditions modeled as the time-varying Gaussian process represented by a weighted combination of odd basis functions.

16. The stochastic feedback controller of claim 15 , wherein the function of the parameter is a friction function, and the shape of the friction function is asymmetric.

17. 2. The stochastic feedback controller of claim 1, wherein the robotic system includes a plurality of vehicles forming an uncertain traffic flow modeled as the time-varying Gaussian process across different segments, each segment being represented by a weighted combination of odd or even basis functions.

18. 2. The stochastic feedback controller of claim 1, wherein the robotic system includes a set of drones controlled by a magnetic field represented by a nonlinear function with a structural constraint of no rotation, and the basis functions are selected as a combination of sine and cosine basis functions.

19. 1. A method for controlling a motion of a robotic system using a probabilistic filter subject to structural constraints on the motion of the robotic system, the method using a processor in combination with stored instructions implementing the method, the instructions, when executed by the processor, perform steps of the method, the steps of the method including: collecting a digital representation of a series of measurements of the state of the robotic system at different times that indicate construction constraints on the shapes of functions of parameters that affect the behavior of the robotic system; and executing a stochastic filter configured to recursively estimate a distribution of a current state of the robotic system taking into account previous states of the robotic system and based on a kinematic model of the robotic system perturbed by stochastic process noise and a measurement model of the robotic system perturbed by stochastic measurement noise, wherein one or a combination of the kinematic model, the process noise, the measurement model, and the measurement noise is represented by a weighted set of time-varying basis functions having weights determined by a corresponding Gaussian distribution. the parameters having uncertainty modeled as a time-varying Gaussian process represented as a set of a time-varying Gaussian process, the time-varying mean of the Gaussian process being a function of the basis functions modified by the mean of the corresponding Gaussian distribution, the time-varying variance of the Gaussian process being a function of the basis functions modified by the variance of the corresponding Gaussian distribution, the probabilistic filter recursively updates both a distribution of the current state of the robotic system and the Gaussian distribution of the weights of the basis functions, each of the basis functions being selected to satisfy the structural constraints implied by the series of measurements of states of the robotic system, and further Executing a control action determined based on an estimate of a distribution of the current state of the robotic system to modify the current state of the robotic system according to a control objective.

20. A non-transitory computer-readable storage medium containing a program executable by a processor to perform a method, the method comprising: collecting a digital representation of a series of measurements of states of a robotic system at different times that indicate structural constraints on the shapes of functions of parameters affecting the behavior of said robotic system; and executing a stochastic filter configured to recursively estimate a distribution of a current state of the robotic system taking into account previous states of the robotic system and based on a kinematic model of the robotic system perturbed by stochastic process noise and a measurement model of the robotic system perturbed by stochastic measurement noise, wherein one or a combination of the kinematic model, the process noise, the measurement model, and the measurement noise is represented by a weighted combination of time-varying basis functions with weights determined by a corresponding Gaussian distribution. the parameters having uncertainty modeled as a time-varying Gaussian process represented as a combination of a time-varying Gaussian process and a time-varying variance of the Gaussian process which is a function of the basis functions modified by the mean of the corresponding Gaussian distribution, the time-varying mean of the Gaussian process being a function of the basis functions modified by the variance of the corresponding Gaussian distribution, the probabilistic filter recursively updates both a distribution of the current state of the robotic system and the Gaussian distribution of the weights of the basis functions, each of the basis functions being selected to satisfy the structural constraints implied by the series of measurements of the state of the robotic system, and executing a control action determined based on an estimate of a distribution of the current state of the robotic system to modify the current state of the robotic system in accordance with a control objective.

Images