Model-based control using 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 behavior 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 and 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 system's model. 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 control variables. State variables define the state of the system; that is, the state of a controlled system is the smallest 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, state variables may include the vehicle's position, velocity, and heading. Control variables are inputs to the system designed to change the state of the machine. For example, in chemical processes, control variables are often pressure, flow rate, temperature, valve opening, and damper stiffness. State variables in these processes are other measurements that represent either control objectives or 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 targets, 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 calculation. 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 the unknown portions of the controlled system are often subject to structural constraints. In such cases, applying control to an 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 need to estimate unknown parameters of the machine model. Traditional approaches to address such issues include adaptive or learning-based MPC, in which the MPC control problem is enhanced using 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, US 2011 / 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 motion model parameters, in some situations the system dynamics, i.e., the functional relationships between successive states of the controlled system, are changing. In such situations, adaptive or learning-based methods for estimating the model parameters may be insufficient.

[0010] Additionally, as the dynamics of the system change, 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 object of some embodiments is to provide stochastic control of a robotic system using a kinematic model that describes the dynamics of the system. Additionally or alternatively, another object of some embodiments is to provide model-based control of a system using a kinematic model with uncertainties. Additionally or alternatively, another object of some embodiments is to provide model-based control of a system using a kinematic model with uncertainties in the dynamics of the system, where the motion of the robotic system is subject to structural constraints. Additionally or alternatively, another object 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 the structural constraints are incorporated into the estimation of the kinematic model. [Means for solving the problem]

[0013] Typically, a robotic system can be modeled using 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 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 multiple kinematic models. This uncertainty is referred to herein as process noise. The second model is a measurement model that relates 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] When the process noise, measurement noise, and model are known, i.e., the shape and parameters of the distributions of the process noise and measurement noise, and the nonlinear function describing the model are known, various techniques can be used to estimate both the state and state uncertainty of the system, for example, using a Kalman filter or a particle filter. Both the state and 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 input to the system to achieve the control objective, and the state uncertainty can be used to adjust the control input to ensure control feasibility.

[0016] For example, if the distributions of process noise and measurement noise are known and the motion model and measurement model 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, and the weights are 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, 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 or even completely unknown during the startup 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 a motion model of the system. As such, some embodiments estimate the state and motion model of the system by taking into account previous states, measurement models, and 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 the task of reducing the uncertainty of the kinematic model. A kinematic model relates parameters of a robotic system, which describe the structural and operational configuration of the robotic system, to variables that represent the inputs and outputs of the robotic system's behavior with these parameters. Reducing the uncertainty of the kinematic model thus involves 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 being carried by a robotic arm. While this task is challenging in itself, 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 vehicle's speed and acceleration and / or the type of road surface. Thus, an objective of some embodiments is to learn functions of parameters that affect the behavior of the robotic system.

[0020] Some embodiments are based on the understanding that the uncertainty in the parameters of a kinematic model, or the uncertainty in a function of parameters that affects the behavior of a robotic system, can be represented as a time-varying Gaussian process. Some embodiments recognize that due to the time-varying nature of the function of parameters, updating the kinematic model requires updating a Gaussian process, which is a computationally challenging problem. However, some 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 problem of training the model is reduced to learning the weights of each basis function, greatly simplifying the training problem. In other words, to update the Gaussian process, some embodiments need only update these Gaussian distributions of weights, and to determine the kinematic model, some embodiments need only determine N scalar weights from the Gaussian distribution.

[0021] Indeed, by viewing 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 computations, this representation introduces additional problems. Specifically, in some situations, the parameter function has a structure that reflects the properties of the parameters and / or the robotic system. For example, the parameter function may be symmetric, indicating the symmetry of the effect of parameter variation on the system's performance. The structure of the parameter function may be asymmetric, as in the tire friction example, which may affect, for example, the Lyapunov stability of the robotic system's motion. Therefore, updating the Gaussian process for the parameter function uncertainty must be performed 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 a robotic system. Specifically, a series of measurements of the state of a robotic system at different times reveals structural constraints on the shape of functions of parameters that affect the behavior of the robotic system. This dependence is probabilistic, but it exists and differs between different types of robotic systems.

[0024] Thus, an objective of some embodiments is to update a Gaussian process that represents the uncertainty in a function of parameters that affect the behavior of the robotic system, subject to structural constraints on the shape of the function of parameters. 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 recognition 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, then an odd number of basis functions must be selected. Conversely, if the shape is symmetric, then an even number of basis functions must be selected. In this way, the constrained update is replaced by the selection of basis functions based on the structural constraints implied 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 the 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 indicating structural constraints on the shape of a function of a parameter 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 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; the stochastic filter recursively updates both a distribution of a 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 a structural constraint implied by a series of measurements of the state of the robotic system; and the instructions further cause the controller to perform a control action determined based on the 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 states of the robotic system at different times, the series of measurements being indicative of structural constraints on the shapes of functions of parameters affecting the operation of the robotic system, and running a stochastic filter configured to recursively estimate a distribution of current states 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 method further includes a step of: executing 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; and a step of executing 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. The method further includes a step of executing 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 including: collecting digital representations of a series of measurements of a state of a robotic system at different time points, the digital representations indicating structural constraints on the shape of a function of a parameter affecting 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, 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 / are used. The fitting 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, the time-varying mean of the Gaussian process being a function of the basis functions modified by the mean of the corresponding Gaussian distributions, the time-varying variance of the Gaussian process being a function of the basis functions modified by the variance of the corresponding Gaussian distributions, the stochastic filter recursively updating 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 a structural constraint implied by a series of measurements of the state of the robotic system, and further including executing 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 explanation of the drawings]

[0030] [Figure 1A] 1 shows a schematic diagram of the operation of a Kalman filter (KF) according to some embodiments. [Figure 1B] 1 shows a schematic diagram of a particle filter (PF) used by some embodiments. [Figure 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 recursive double updates in accordance with some embodiments. [Figure 3B] 1 shows a schematic diagram of a particular example of a stochastic filter, namely a particle filter (PF). [Figure 4A] 1 illustrates an iterative flowchart of a method for stochastic feedback control for controlling 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. [Figure 4B] 1 illustrates a flowchart of the 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] 1 shows a diagram of weight means and weight covariances associated with different basis functions, according to some embodiments. [Figure 5C] 5 shows a diagram of the influence 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. 1 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 affect 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] 10 illustrates fitting a shape to measurements according to some embodiments. [Figure 7C] 10 illustrates fitting a shape to measurements according to one embodiment. [Figure 7D] 10A-10C show diagrams of error reduction 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. 1 illustrates a block diagram of discrete-time or discretized propagation of state mean and covariance information, according to one embodiment. [Figure 8C] FIG. 10 shows a block diagram of discrete-time or discretized propagation of state mean and covariance information according to another embodiment. [Figure 9A] 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 realized and often computes an optimal trajectory by using some embodiments. [Figure 10D] 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] 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 INVENTION

[0031] An object of some embodiments is to provide stochastic control of a robotic system using a kinematic model that describes the dynamics of the system. Additionally or alternatively, another object of some embodiments is to provide model-based control of a robotic system using a kinematic model with uncertainties. Additionally or alternatively, another object of some embodiments is to provide model-based control of a system using a kinematic model with uncertainties in the dynamics of the system, where the motion of the robotic system is subject to structural constraints. Additionally or alternatively, another object 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 the structural constraints are incorporated into the estimation of the kinematic model.

[0032] As used herein, a robotic system 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 foregoing entities, or a combination of the foregoing 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 multiple kinematic models. This uncertainty is referred to herein as process noise. The second model is a measurement model that relates available measurements of the system to the state of the system. The measurement model also includes measurement noise and / or other uncertainties, referred to herein as measurement noise. These uncertainties are referred to herein as model uncertainties.

[0034]

number

[0035]

number

[0036] When the process noise, measurement noise, and model are known, i.e., the shape and parameters of the distributions of the process noise and measurement noise, and the nonlinear function describing the model are known, various techniques can be used to estimate both the state and state uncertainty of the system, for example, using a Kalman filter or a particle filter. Both the state and 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 input to the system to achieve the control objective, and the state uncertainty can be used to adjust the control input to ensure control feasibility.

[0037] For example, if the distributions of process noise and measurement noise are known and the motion model and measurement model 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, and the weights are 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 stage, 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 system's motion model. As such, some embodiments estimate states and functions of the parameters contained in the system's motion model, 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 represented as time-varying Gaussian processes. Some embodiments are based on the recognition that updating a kinematic model requires updating a Gaussian process, which is a computationally difficult problem. This is because updating a Gaussian process using a set of measurements that describe 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 must be incorporated into the Gaussian process formulation.

[0041] As such, 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. By doing so, the problem of learning a model is reduced to learning the weights of each basis function, greatly simplifying the learning problem. In other words, to update a Gaussian process, some embodiments need only update these Gaussian distributions of weights, and to determine a motion model, some embodiments need only determine N scalar weights from the Gaussian distribution. Indeed, viewing 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. Furthermore, the learned function 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 the reliable 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 stochastic 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 stochastic filter can be realized, for example, by Kalman filtering, particle filtering, or other nonlinear filtering methods, where the stochastic filter performs a dual update of the state and the Gaussian distribution of the weights of the basis functions.

[0045] 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 source is known and Gaussian, in which case the state estimate is also Gaussian-distributed. The KF estimates the mean and variance of the Gaussian distribution, since the mean and variance are two necessary and sufficient statistics to describe the Gaussian distribution.

[0046] The KF starts with initial knowledge 110a of the state to find the mean of the state and its variance 111a. The KF then uses a model of the system to predict 120a the state and variance to the next time step, thereby obtaining updated means and variances 121a of the state. The KF then finds updated means and variances 141a of the state 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 linear-regression KFs (LRKFs), such as extended KFs (EKFs), unscented KFs (UKFs), and particle filters. While 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 target probability distribution.

[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 state distribution using measurements at each time step by propagating particles forward in time. The particles include state hypotheses and weights that determine the state's consistency with the measurements using a measurement model. FIG. 1B shows a simplified schematic diagram of the results of three iterations generating state trajectories when five particles are generated per 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., 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 KFs, PFs, and their variants, typically operate under the assumption of a known motion model and a known measurement model. The motion model is used to predict a current state from a previous state based on control inputs, and the measurement model is used to compare the predicted state with the measurement and update the state by inserting the predicted state into the measurement model, taking a difference, and minimizing the difference. However, if either the motion model or the measurement model has uncertainties different from the process and / or measurement noise, the single update shown in FIGS. 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 stochastic filter, i.e., a state update and a function update of parameters with uncertainty affecting the model.

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

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

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

[0054] Some embodiments are based on the recognition that to include a kinetic model in a filter, the filter must include the state 342a, the parameters 344a included in the kinetic model 347a, measures of uncertainty in the parameters that affect the kinetic model, and corrections 346a that determine the joint fitting of the state and 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 recognition that the uncertainty in the function of parameters that influences 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 stochastic filter, or particle filter (PF). Some embodiments are based on the recognition that a PF can estimate the distribution of states by including a state 312b and a weight 314b for each particle 310b and determining the fit of the state 312b to the measurements using a measurement model 324b affected by measurement noise 126c, where the weight 314b is a correction term 314a. However, if the particle includes the state, only the state can be updated, 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 process noise 322b. In this way, nothing external to the particle motion model 320b changes.

[0057] Some embodiments are based on the understanding that the PF can 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 can include parameters 344b included in kinetic model 347b, and particle 350b can include parameters 354b included in kinetic model 357b. In fact, because the kinetic models of different particles can be different, the modified PF can update not only the state of the system but also the parameters that affect the kinetic model of the system.

[0058] Some embodiments recognize that to include a kinetic model in each particle, the particle must include states 342b and 352b, uncertain parameters 344b and 354b, kinetic models 347b and 357b, and weights 346b and 356b that determine the joint fitting of the state and kinetic model with the uncertain parameters to the measurement model 324b. This ensures that the weightings of each particle 340b and 350b reflect the quality of the parameter estimates.

[0059] Some embodiments are based on the recognition that the uncertainty in a function of parameters can be modeled as a Gaussian process over possible functions of the parameters affecting the robotic system. The parameters are often nonlinear functions that describe the dependence of the parameters as a function of the state. Each sample of the Gaussian process over 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 transformation of the Gaussian process, capture the uncertainty in 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 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 uncertainty modeled as Gaussian processes over the possible parameters affecting the system. Thus, different particles may have different parameters by having 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 viewed as stochastic uncertainties in a model of the system's motion. In addition, an embodiment recognizes that there are typically other disturbances that act on the system's motion, for example due to uncertainties in the actuators that generate 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] One embodiment is based on the recognition that a function can be represented 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 represented 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 for controlling 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 estimated information from a 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, perform a method of collecting a series of measurements of the state of the robotic system at different times that indicate structural constraints on the shape of functions of parameters that affect the behavior of the robotic system. The method searches a memory configured to store a stochastic filter and executes 420a the stochastic 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 collected measurements 411a. 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, 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, and the stochastic 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 indicated 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 flowchart of one iteration of a stochastic filter according to one embodiment. Using a kinematic model 405b of the robotic system with uncertain parameters determined in a previous iteration, control inputs 406b determined during the previous iteration, and state 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 kinematic model 405b is represented by a distribution over the state. 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 kinematic model of the robotic system to generate updated parameters 445b.

[0066] The parameter function, i.e., Gaussian process distribution, update 440b can be performed in several ways: In one embodiment, the update is performed 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 including 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 the joint fitting of the model and 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 the 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 the state determined using uncertainty in the particles' parameters and the 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 with 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] 5A shows an illustration of the use of weighted basis functions according to one embodiment. In this illustration, there are three basis functions 510, 520a, and 530a. Also shown are parameters that map previous states to the current state, along with a motion model. 540a The true function of σ is also shown. 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 motion model.

[0070] 5A shows an illustration of the use of weighted basis functions according to one embodiment. In this illustration, there are three basis functions 510, 520a, and 530a. Also shown is the true function of parameters 340a, which maps previous states to the current state, along with the motion model. By combining the basis functions and using different weights for each basis function, they can be combined to recreate the true function of parameters, and therefore the true motion 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 and 530c very lightly and weighting 510c heavily, basis function expansion can reproduce the function of parameter 540c with a single basis function. While FIGS. 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] 5D shows a block diagram of one iteration of a method for updating 440b the probability distribution of the weights of the basis functions, according to one embodiment. This method uses the determined current and previous states 220cThe weights of the weighted combination of basis functions are updated using the combination of the weighted basis functions (510d), and the updated weights are used to update the probability distribution of the function of the parameters according to the weighted combination of the weighted basis functions (520d).

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

[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 interpolated into basis functions 507e to map the sampled current states 509e and previous states 508e interpolated 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 recognition that the satisfaction of structural constraints on the behavior of a 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 therefore on the Gaussian processes that model functions of parameters affecting the robotic system.

[0082] Various embodiments are based on the recognition that a series of measurements indicates structural constraints that affect the behavior of a robotic system. Specifically, a series 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 series of measurements of a state of a robotic system is related to values ​​of parameters that statistically preserve structural constraints on the shape of the function of the parameters.

[0083] Another embodiment is based on the concept that in order to determine basis functions that satisfy a structural constraint, the structural constraint itself must be determined. In some cases, the structural constraints are obvious from the physical configuration of the robot system, but in most cases, the structural constraints are 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 affecting 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. Next, the method compares (620a) measurement data using the predicted series 618a and a series 619a to generate compared data 625a. As an example, the compared data 625a may be the difference between the measured values ​​and predicted estimates for a series of measurements from different actuations. Finally, the method determines (630a) a shape 635a of a function of parameters that preserves structural constraints based on the data difference 625a.

[0085] FIG. 6B illustrates a flowchart of a method for determining structural constraints offline, according to one embodiment, in which basis functions are selected offline in response to determining structural constraints. .childThe method runs 610b a robotic system using inputs 609b, resulting in state motion 618b of the system. The inputs must be diverse enough to effectively stimulate the state space. The method then measures 620b the system, resulting in a digital representation of a series of measurements 625b. It compares 630b the measurements 625b with a nominal model 628b that does not have a weighted combination of basis functions inserted into the model, 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.

[0086] FIG. 6B shows a flowchart of a method for determining structural constraints offline, according to one embodiment, in which basis functions are selected offline in response to the determination of structural constraints. Using inputs 609b to the system, the method executes 610b the robotic system using the inputs 609b, resulting in system state motion 618b. The inputs must be diverse enough to effectively stimulate the state space. The method then measures 620b the system, resulting in a digital representation of a series of measurements 625b. The measurements 625b are compared 630b with a nominal model 628b that does not have a weighted combination of basis functions inserted into the model, 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 differentials 635b relative to the inputs to the system. Other embodiments classify the constraints relative to previously tabulated structural constraints.

[0088] 6C illustrates a flowchart of one iteration of a method for determining structural constraints online, according to one embodiment, in which basis functions are selected online during operation of a robotic system in response to determining the structural constraints. Initially, the method initiates (610c) a 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 initiating a 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 615c that does not have a weighted combination of basis functions inserted into the model. Using the resulting differences 635c, the method updates (640c) 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. For 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 control inputs to trigger the system and measure 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 that 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 Figure 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 basis functions is required.

[0093]

number

[0094]

number

[0095] Accordingly, one embodiment discloses a method for offline selection of basis functions according to predetermined rules and solutions of a linear system of equations. Figure 7A shows a flowchart of a method for selecting basis functions according to one embodiment. The method is performed 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. Next, using 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. To determine the shape, for longitudinal tire friction, which is tire friction in the forward direction of the tire, one embodiment starts the vehicle using different braking and acceleration inputs. Using acceleration sensors, it is possible to measure acceleration, which is friction if the accelerometer is located on the wheel, or a scaled and rotated version of tire friction if not. This results in several measurements 710b that only indicate 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. Here, measurement 710b and its noise characteristics were used to determine a distribution from which the friction value can be obtained. For example, distributions 740b and 750b have already been determined. 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 understanding that imposing structural constraints on the shape of the function of the parameters reduces the difference between the robotic system state predicted using a kinematic model with the parameters and the measured robotic system state.

[0099] 7D shows an illustration of the reduction in error due to imposing a structural constraint, according to one embodiment, where a function of parameters 720d has a symmetric structural constraint and its distribution 730d is estimated with or 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 the constraint is imposed.

[0100] 8A shows a block diagram of a stochastic feedback controller 850a, according to some embodiments, which initiates a system such that its estimated state 821a and output 803a comply with commands 801a, taking into account 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 memory 865a for storing a model 840a, an uncertainty model 841a, e.g., 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 random access memory (RAM), read-only memory (ROM), 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 explicit linearization-based propagation equations for the discrete-time or discretized nonlinear system dynamics, taking into account the initial state estimates and corresponding uncertainties 840 and Gaussian approximations of probability density functions for (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 x k+1 is the time step t k+1represents the state variables in . In 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, according to 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 unscented Kalman filtering (UKF) can be used to compute more accurate propagation of mean and covariance information for nonlinear system dynamics than explicit linearization-based propagation of mean and covariance information using, for example, extended Kalman filtering (EKF). Some embodiments of the present invention are based on the recognition that UKF is a special case of a more comprehensive class of linear regression Kalman filtering (LRKF), which 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 ADFs use statistical linearization based on approximated matching of one and / or more higher-order moment integrals instead of explicit linearization based on Taylor series approximations (e.g., in EKFs). Thus, while EKFs are first-order methods based on explicit linearization for handling nonlinearities, classes of ADFs based on statistical linearization can achieve second-order or better accuracy in the 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, statistical linearization based on matching 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 stochastic predictive controllers for controlled systems under uncertainty.

[0108]

number

[0109]

number

[0110] Finally, according to some embodiments of the present 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 present 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 calculates control signals 811a by taking into account a current state estimate 821a of a system and 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., calculating a solution vector 965 that includes a set of future optimal or near-optimal control inputs over a prediction time horizon of the system 960. The objective function, equality and inequality constraint data 945 in this optimization problem 950 depend on a dynamic model and system constraints 940, the current state estimate 821a of the system, the estimated uncertainties 822a, and the control commands 801a.

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

[0115] In some embodiments of the present invention, the solution to the inequality constrained optimization problem 950 uses exact or approximate control input, state mean, and / or covariance values ​​over the forecast time horizon from the previous control time step 910, which can be read from memory, as a solution guess to reduce the computational effort of solving the inequality constrained optimization problem 950 at the current control time step. This concept of computing a solution guess from solution information at the previous control time step 910 is referred to as 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 an exact or approximate set of control input, state mean, and / or covariance values ​​for the next control time step 960. In some embodiments of the present invention, a time-shifting procedure can be used to compute a more accurate solution guess for the inequality constrained optimization problem 950 at the current control time step, taking into account the control input, state mean, 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 for the stochastic chance constraints 955 and based on 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 discrete-time covariance propagation equations, and a discrete-time covariance propagation equation and linearization-based approximation for each of the inequality constraints and each of the strengthened stochastic chance constraints.

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

[0127] The vehicle may also include an engine 1006 that can 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 sensors 1004 include a distance rangefinder, radar, lidar, and a camera. The vehicle 1001 may also include one or more sensors 1005 that sense its current momentum and internal status. Examples of 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 include a transceiver that enables communication functions of the controller 1002 through wired or wireless communication channels. Ba It may also be provided.

[0128] The vehicle may also include an engine 1006 that can 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 sensors 1004 include a distance rangefinder, radar, lidar, and a camera. The vehicle 1001 may also include one or more sensors 1005 that sense its current momentum and internal status. Examples of 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] 10B shows 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 uses 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] 10C shows a schematic diagram of an autonomous or semi-autonomous controlled vehicle 1050 that, using some embodiments, can calculate a dynamically realizable and often optimal trajectory 1055. The generated trajectory aims to keep the vehicle within a specific road boundary 1052 and avoid other uncontrolled 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 overtaking another vehicle on the left or right, or alternatively staying behind another vehicle in the current lane of the road 1052. Embodiments of the present invention are 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, it is possible for the controlled vehicle to 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 the second time point 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 in a system and its environment may include any time-varying parameters related to the friction behavior between a vehicle's tires and the road surface, such as parameters in the Pacejka tire force model, which can be learned online while controlling the vehicle. The parameter estimation function and estimation uncertainty may 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 embodiments of the present invention. In some embodiments, the estimated 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 that it is 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 ​​for one or a combination of a steering angle of the vehicle's wheels and a rotational velocity of the wheels, and the measurements include values ​​for one or a combination of a rotational velocity of the vehicle and an acceleration of the vehicle. Each state of the vehicle includes a velocity and a heading velocity of the vehicle, and the motion model relates values ​​of the control inputs to first values ​​of the vehicle state through the dynamics of the vehicle at successive time steps, and the measurement model relates the measurements to second values ​​of the vehicle state at the same time steps.

[0134] 10E shows 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 routes for particular vehicles 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 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. This type of communication network can transmit information across most of road 1101. In addition, traffic information such as red lights can be sent and received.

[0137] Some embodiments are based on the recognition that for certain roads, it is equally likely that the number of vehicles traveling in a particular direction on the road will be 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 a 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 drone 1200 to move and sensors for perceiving the environment and the location of the device 1200. The rotor 1201 may be an actuator, and the sensors for perceiving the environment may include a light detection and ranging (LIDAR) 1202 and a camera 1203. Furthermore, the sensors for determining the location may include a GPS 1204. Such sensors may be integrated with an inertial measurement unit (IMU) capable of measuring 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 transceiver 1205, calculating commands to the actuators 1401, and calculating 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 movement 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 will travel. In some embodiments, there are one or more devices (drones, such as drone 2400) whose movement is coordinated and controlled by controller 1211. Controlling and coordinating the movement of the one or more devices corresponds to solving a mixed integer optimization problem.

[0140] FIG. 12B illustrates a multi-device motion planning problem according to some embodiments of the present disclosure. FIG. 12B illustrates multiple devices (e.g., drone 1201b, drone 1201a, drone 1201c, and drone 1201d) required to reach assigned final positions 1202c, 1202b, 1202b, and 1202d. Additionally, obstacles 1203a, 1203b, 1203c, 1203d, 1203e, and 1203f in the surrounding environment of drones 1201a-1201d are illustrated. Drones 1201a-1201d are required to reach their assigned final positions 1202a-1202d while avoiding obstacles 1203a-1203f in the surrounding environment. A simple trajectory ( Figure 12B 1201a-1201d) can cause collisions. Accordingly, 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 overlapping trajectories or, if multiple trajectories overlap 1206, by ensuring that corresponding drones reach the overlapping point at times within their future planning time horizons that are sufficiently separated.

[0141] FIG. 12B illustrates a multi-device motion planning problem according to some embodiments of the present disclosure. FIG. 12B illustrates multiple devices (e.g., drone 1201b, drone 1201a, drone 1201c, and drone 1201d) required to reach assigned final positions 1202c, 1202b, 1202b, and 1202d. Additionally, obstacles 1203a, 1203b, 1203c, 1203d, 1203e, and 1203f in the surrounding environment of drones 1201a-1201d are illustrated. 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 result in 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 within 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 beyond 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, 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, a processor may be implemented using circuitry 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. Additionally, 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] Additionally, 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, even though they are shown as a sequence in the illustrated embodiment.

[0147] While the disclosure has been described with examples of preferred embodiments, it is to 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 as 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 structural constraints on the shape of functions of parameters that affect the behavior 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, 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 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, wherein 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; 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, 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 the instructions to the controller further include: a stochastic feedback controller that executes 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.

2. 2. The stochastic feedback controller of claim 1, wherein each measurement in the series of measurements of the 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 wherein the series of measurements of the state of the robotic system is related to a series of values ​​of the parameter that statistically preserves the structural constraint on the shape of the function of the parameter.

3. a smoothed representation of the data fitted to a range of values ​​of the parameters comprises a function having a shape that preserves the structural constraints; or The stochastic feedback controller of claim 2 , wherein statistical properties of the sequence of values ​​of the parameter depend on the structural constraints.

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

5. The structural constraints are determined offline based on experimental or simulation data of the operation of the robot system, and the basis functions are selected offline in response to the determination of the structural constraints; or 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 the determination of the structural constraints.

6. The structural constraints are determined online, and to select the basis functions, the processor: actuating the robotic system using a range of control inputs to collect training measurements of the state of the robotic system; estimating states of the robotic system resulting from actuation by the range of control inputs using different stochastic filters with different structural constraints on the shapes of functions of the parameters affecting the behavior of the robotic system; selecting the structural constraints from the probabilistic filter that best estimate 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 5 , configured as follows:

7. The structural constraints are determined online, and to select the basis functions, the processor: actuating the robotic system using a range of control inputs to collect training measurements of the state of the robotic system; estimating states of the robotic system resulting from actuation with control inputs in the range using a stochastic 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 the state of the robotic system; Transforming the pattern into the structural constraints The stochastic feedback controller of claim 5 , configured as follows:

8. The stochastic feedback controller of claim 7 , 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.

9. 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 robot system and the time-varying Gaussian process; or 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.

10. In order to satisfy the structural constraints, each of the basis functions is selected to be an odd function; or The stochastic feedback controller of claim 1 , wherein each of the basis functions is selected to be an even function.

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

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

13. 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; or 2. The stochastic feedback controller of claim 1, wherein the robotic system includes a set of drones, the set of drones being controlled by a magnetic field represented by a nonlinear function with a no-rotation structural constraint, and the basis functions are selected as a combination of sine and cosine basis functions.

14. 1. 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, the steps of the method comprising: collecting a digital representation of a series of measurements of the state of the robotic system at different times, the measurements indicating structural constraints on the shapes of functions of parameters affecting the behavior 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, 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 with 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 stochastic filter recursively updating both the 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 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.

15. 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 the state of the robotic system at different times, the measurements indicating structural constraints on the shape 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 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, taking into account previous states of the robotic system, wherein one or a combination of the kinematic model, the process noise, the measurement model, and the measurement noise is 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 represented as a combination of a time-varying Gaussian process and a Gaussian process having a time-varying mean that is a function of the basis functions modified by the mean of the corresponding Gaussian distribution, and a time-varying variance of the Gaussian process that is a function of the basis functions modified by the variance of the corresponding Gaussian distribution, 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, 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 performing 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