Patent Publication Number: US-11662731-B2

Title: Systems and methods for controlling a robot

Description:
CROSS-REFERENCE TO RELATED APPLICATION 
     This application claims the benefit of U.S. Provisional Patent Application No. 63/077,971, “DR-ILEQG: Distributionally-Robust Optimal Control of Nonlinear Dynamical Systems for Safety-Critical Applications,” filed Sep. 14, 2020, which is incorporated by reference herein in its entirety. 
    
    
     STATEMENT REGARDING FEDERALLY SPONSORED RESEARCH OR DEVELOPMENT 
     This invention was made with government support under contract N00014-18-1-2830 awarded by the Office of Naval Research (ONR). The government has certain rights in the invention. 
    
    
     TECHNICAL FIELD 
     The subject matter described herein relates in general to robots and, more specifically, to systems and methods for controlling a robot. 
     BACKGROUND 
     Proper modeling of a robot (one example of a stochastic system) is an important aspect of successful control and decision making under uncertainty due to probabilistically-described disturbances (e.g., noise). In particular, accurate characterization of the underlying probability distribution associated with those disturbances is important, since it encodes how the system is expected to behave unexpectedly over time. However, such a modeling process can pose significant challenges in real-world problems. On the one hand, only limited knowledge of the underlying system may be available, resulting in the use of an erroneous model (the “model-mismatch” problem). On the other hand, even if a complicated stochastic phenomenon, such as a complex multi-modal distribution, can be perfectly modeled, it may still not be appropriate for the sake of real-time control or planning. Indeed, many model-based stochastic control methods require a Gaussian noise assumption, and many of the other methods require computationally intensive sampling. 
     SUMMARY 
     An example of a system for controlling a robot is presented herein. The system comprises one or more processors and a memory communicably coupled to the one or more processors. The memory stores an input module including instructions that when executed by the one or more processors cause the one or more processors to receive an initial state of the robot, an initial nominal control trajectory of the robot, and a Kullback-Leibler (KL) divergence bound between a modeled probability distribution for a stochastic disturbance and an unknown actual probability distribution for the stochastic disturbance. The memory also stores a computation module including instructions that when executed by the one or more processors cause the one or more processors to solve a bilevel optimization problem subject to the modeled probability distribution and the KL divergence bound using an iterative Linear-Exponential-Quadratic-Gaussian (iLEQG) algorithm and a cross-entropy process, the iLEQG algorithm outputting an updated nominal control trajectory, the cross-entropy process outputting a risk-sensitivity parameter. The memory also stores a control module including instructions that when executed by the one or more processors cause the one or more processors to control operation of the robot based, at least in part, on the updated nominal control trajectory and the risk-sensitivity parameter. 
     Another embodiment is a non-transitory computer-readable medium for controlling a robot and storing instructions that when executed by one or more processors cause the one or more processors to receive an initial state of the robot, an initial nominal control trajectory of the robot, and a Kullback-Leibler (KL) divergence bound between a modeled probability distribution for a stochastic disturbance and an unknown actual probability distribution for the stochastic disturbance. The instructions also cause the one or more processors to solve a bilevel optimization problem subject to the modeled probability distribution and the KL divergence bound using an iterative Linear-Exponential-Quadratic-Gaussian (iLEQG) algorithm and a cross-entropy process, the iLEQG algorithm outputting an updated nominal control trajectory, the cross-entropy process outputting a risk-sensitivity parameter. The instructions also cause the one or more processors to control operation of the robot based, at least in part, on the updated nominal control trajectory and the risk-sensitivity parameter. 
     Another embodiment is a method of controlling a robot, the method comprising receiving an initial state of the robot, an initial nominal control trajectory of the robot, and a Kullback-Leibler (KL) divergence bound between a modeled probability distribution for a stochastic disturbance and an unknown actual probability distribution for the stochastic disturbance. The method also includes solving a bilevel optimization problem subject to the modeled probability distribution and the KL divergence bound using an iterative Linear-Exponential-Quadratic-Gaussian (iLEQG) algorithm and a cross-entropy process, the iLEQG algorithm outputting an updated nominal control trajectory, the cross-entropy process outputting a risk-sensitivity parameter. The method also includes controlling operation of the robot based, at least in part, on the updated nominal control trajectory and the risk-sensitivity parameter. 
    
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
       The accompanying drawings, which are incorporated in and constitute a part of the specification, illustrate various systems, methods, and other embodiments of the disclosure. It will be appreciated that the illustrated element boundaries (e.g., boxes, groups of boxes, or other shapes) in the figures represent one embodiment of the boundaries. In some embodiments, one element may be designed as multiple elements or multiple elements may be designed as one element. In some embodiments, an element shown as an internal component of another element may be implemented as an external component and vice versa. Furthermore, elements may not be drawn to scale. 
         FIG.  1    illustrates a robot, in accordance with an illustrative embodiment of the invention. 
         FIG.  2    illustrates one embodiment of a robot control system. 
         FIG.  3 A  illustrates a reference probability distribution, in accordance with an illustrative embodiment of the invention. 
         FIG.  3 B  illustrates an actual probability distribution, in accordance with an illustrative embodiment of the invention. 
         FIG.  4    is a flowchart of a method of controlling a robot, in accordance with an illustrative embodiment of the invention. 
     
    
    
     To facilitate understanding, identical reference numerals have been used, wherever possible, to designate identical elements that are common to the figures. Additionally, elements of one or more embodiments may be advantageously adapted for utilization in other embodiments described herein. 
     DETAILED DESCRIPTION 
     In various embodiments disclosed herein, systems and methods for controlling a robot address the problem of model mismatch via distributionally robust control, wherein a potential distributional mismatch is considered between a baseline Gaussian process noise and the true, unknown model within a certain Kullback-Leibler (KL) divergence bound. The use of the Gaussian distribution is advantageous to retain computational tractability without the need for sampling in the state space. Some embodiments include a model predictive control (MPC) method for nonlinear, non-Gaussian systems with non-convex costs. In some embodiments, the robot is an autonomous vehicle, and the techniques disclosed herein can be used, for example, to safely navigate the autonomous vehicle among human pedestrians where the stochastic transition model for the human pedestrians is imperfect. 
     The various embodiments described herein make use of the equivalence between distributionally robust control and risk-sensitive optimal control. Unlike the conventional stochastic optimal control that is concerned with the expected cost, risk-sensitive optimal control seeks to optimize the following entropic risk measure: 
                   R     p   ,   θ       ⁡     (   J   )       ⁢     =   Δ     ⁢       1   θ     ⁢   log   ⁢           ⁢       𝔼   p     ⁡     [     exp   ⁡     (     θ   ⁢   J     )       ]           ,         
where p is a probability distribution characterizing any source of randomness in the system, θ&gt;0 is a user-defined scalar parameter called the risk-sensitivity parameter, and J is an optimal control cost. The risk-sensitivity parameter θ determines a relative weight between the expected cost and other higher-order moments such as the variance. Loosely speaking, the larger θ becomes, the more the objective cares about the variance and the more risk-sensitive it becomes.
 
     The distributionally robust control algorithms employed by various embodiments disclosed herein can alternatively be viewed as algorithms for automatic online tuning of the risk-sensitivity parameter in applying risk-sensitive control. Risk-sensitive optimal control has been shown to be effective and successful in many robotics applications. However, conventional approaches require the user to specify a fixed risk-sensitivity parameter offline. This requires an extensive trial and error process until a desired robot behavior is observed. Furthermore, a risk-sensitivity parameter that works in a certain state can be infeasible in another state. Ideally, the risk-sensitivity should be adapted online depending on the situation to obtain a specifically desired robot behavior, yet this is nontrivial because no simple general relationship is known between the risk-sensitivity parameter and the performance of the robot. The embodiments discussed herein address that challenge. Due to the fundamental equivalence between distributionally robust control and risk-sensitive control, those embodiments provide nonlinear risk-sensitive control that can dynamically adjust the risk-sensitivity parameter depending on the state of the robot as well as the surrounding environment. 
     In some embodiments, a system for controlling a robot receives an initial state of the robot, an initial nominal control trajectory of the robot, and a KL divergence bound between the modeled probability distribution for a stochastic disturbance and the unknown actual probability distribution for the stochastic disturbance. The system solves a bilevel optimization problem subject to the modeled probability distribution and the KL divergence bound using an iterative Linear-Exponential-Quadratic-Gaussian (iLEQG) algorithm and a cross-entropy process. The iLEQG algorithm outputs, among other things, an updated nominal control trajectory, and the cross-entropy process outputs a risk-sensitivity parameter. In U.S. Provisional Patent Application No. 63/077,971, the algorithm for solving the bilevel optimization problem was called the Distributionally Robust iLEQG (DR-ILEQG) algorithm. Herein, this algorithm is sometimes referred to as the Risk Auto-Tuning iterative Linear-Quadratic Regulator (RAT iLQR) algorithm. 
     The various embodiments described herein perform the bilevel optimization based on the worst-case distribution within a set of possible distributions that also includes the distribution used in the stochastic-system model. Such a set of distributions can be analyzed using a metric such as the KL divergence bound. Importantly, the system does not have to know, a priori, what that worst-case distribution is. 
     The remainder of this Detailed Description is organized as follows. First, an overview of a robot  100  and an associated robot control system  120  is provided in connection with  FIGS.  1  and  2   . A more detailed explanation of the RAT iLQR algorithm employed by robot control system  120 , including the underlying mathematical concepts, is then presented. That explanation includes reference to  FIGS.  3 A and  3 B . This explanation is then followed by a discussion of the method flowchart of  FIG.  4   . 
     Referring to  FIG.  1   , an example of a robot  100  is illustrated. Some examples of a robot  100  include, without limitation, an autonomous or semi-autonomous vehicle (e.g., an autonomous or semi-autonomous automobile), an autonomous aerial drone (e.g., a quadrotor), a security robot, a customer-service robot, and a delivery robot. The robot  100  also includes various elements. It will be understood that in various embodiments it may not be necessary for the robot  100  to have all of the elements shown in  FIG.  1   . The robot  100  can have any combination of the various elements shown in  FIG.  1   . Further, the robot  100  can have additional elements to those shown in  FIG.  1   . In some arrangements, the robot  100  may be implemented without one or more of the elements shown in  FIG.  1   . 
     Some of the possible elements of the robot  100  are shown in  FIG.  1   , and some of those elements will be described in greater detail in connection with subsequent figures. Additionally, it will be appreciated that for simplicity and clarity of illustration, where appropriate, reference numerals have been repeated among the different figures to indicate corresponding or analogous elements. In addition, the discussion outlines numerous specific details to provide a thorough understanding of the embodiments described herein. Those skilled in the art, however, will understand that the embodiments described herein may be practiced using various combinations of these elements. 
     Robot  100  includes a sensor system  110  including any of a variety of different types of sensors, depending on the particular kind of robot and application. Such sensors can include, without limitation, cameras, Light Detection and Ranging (LIDAR) sensors, infrared sensors, radar sensors, and sonar sensors. In a vehicular embodiment, sensor system  110  can also include sensors that produce Controller-Area-Network (CAN-bus) data such as position, heading, speed, acceleration, etc., of robot  100  itself. Sensor system  110  outputs various corresponding types of sensor data  135  (e.g., images, LIDAR point clouds, CAN-bus data, etc.). 
     The sensor data  135  is input to a perception system  115 , which performs tasks such as image segmentation and object detection, trajectory prediction, and tracking. These perceptual tasks can apply to robot  100  itself, other objects in the environment (e.g., other road users, in a vehicular embodiment), or both. Perception system  115  outputs an initial state  140 , an initial nominal control trajectory  145 , and a KL divergence bound  150  for robot  100 . Those inputs are processed by a robot control system  120  that executes the RAT iLQR algorithm, an algorithm for solving the bilevel optimization problem mentioned above. Robot control system  120  outputs an updated nominal control trajectory  155 , control gains  160 , and a risk-sensitivity parameter  165 . These outputs are fed to one or more actuators  125  in robot  100 , which output forces and torques  170  to control robot  100 . These forces and torques  170  impact the movement of robot  100  (robot dynamics) and, in some cases, its interactions with other objects in the environment (dynamics and environment  130 , in  FIG.  1   ), which involve motion and forces  175  that are detected via sensor system  110 . In some embodiments, robot  100  may be classified as a stochastic nonlinear system. 
     With reference to  FIG.  2   , one embodiment of the robot control system  120  of  FIG.  1    is further illustrated. The robot control system  120  is shown as including one or more processors  210 . Robot control system  120  also includes a memory  220  communicably coupled to the one or more processors  210 . The memory  220  stores an input module  230 , a computation module  240 , and a control module  250 . The memory  220  is a random-access memory (RAM), read-only memory (ROM), a hard-disk drive, a flash memory, or other suitable memory for storing the modules  230 ,  240 , and  250 . The modules  230 ,  240 , and  250  are, for example, computer-readable instructions that when executed by the one or more processors  210 , cause the one or more processors  210  to perform the various functions disclosed herein. 
     In connection with its control functions, robot control system  120  can stores various kinds of data in a database  260 . For example, in the embodiment shown in  FIG.  2   , robot control system  120  stores, in database  260 , input data  270 , model data  280  (e.g., data associated with solving the bilevel optimization problem such as intermediate calculations, model parameters, probability distributions, etc.), and output data  290  (e.g., updated nominal control trajectory  155 , control gains  160 , and risk-sensitivity parameter  165 ). 
     Input module  230  generally includes instructions that when executed by the one or more processors  210  cause the one or more processors  210  to receive an initial state  140  of the robot  100 , an initial nominal control trajectory  145  of the robot  100 , and a KL divergence bound  150  between a modeled probability distribution for a stochastic disturbance and an unknown actual probability distribution for the stochastic disturbance. Initial state  140  can include, for example, the position, velocity, and heading/pose of robot  100 , as well as object-tracking information concerning the state of objects in the environment near robot  100 . In an autonomous-vehicle embodiment, the objects in the environment could include, for example, other road users and obstacles. Other road users include, without limitation, other vehicles, motorcyclists, bicyclists, and pedestrians. As discussed above, the various embodiments described herein perform the bilevel optimization based on the worst-case distribution within a set of possible distributions that also includes the distribution used in the stochastic-system model. Such a set of distributions can be analyzed using a metric such as the KL divergence bound  150 . As also mentioned above, the robot control system  120  does not have to know, a priori, what that worst-case distribution is. This is one of the advantages of the various embodiments disclosed herein. 
     The stochastic disturbance can take on different forms, depending on the particular embodiment. For example, in an embodiment in which robot  100  is an autonomous vehicle, the stochastic disturbance could be slippery road conditions caused by rain, ice, or snow. Another example of a stochastic disturbance is that associated with the motion of an other road user such as another vehicle, a motorcyclist, a bicyclist, or a pedestrian. As discussed further below, in some embodiments the modeled probability distribution for the stochastic disturbance is a Gaussian distribution. 
     Computation module  240  generally includes instructions that when executed by the one or more processors  210  cause the one or more processors  210  to solve a bilevel optimization problem subject to the modeled probability distribution and the KL divergence bound  150  using an iLEQG algorithm and a cross-entropy process in combination. The iLEQG algorithm outputs, among other things, an updated nominal control trajectory  155 , and the cross-entropy process outputs a risk-sensitivity parameter  165 . The details of the iLEQG algorithm and the cross-entropy process included in the overall RAT iLQR algorithm, including the underlying mathematical concepts, are presented below. 
     Control module  250  generally includes instructions that when executed by the one or more processors  210  cause the one or more processors  210  to control the operation of the robot  100  based, at least in part, on the updated nominal control trajectory  155  and the risk-sensitivity parameter  165 . In an autonomous-vehicle embodiment, the instructions in the control module  250  to control operation of the robot  100  can also include, for example, instructions to avoid a collision with an other road user (e.g., another vehicle, a motorcyclist, a bicyclist, or a pedestrian). The instructions to control the operation of the robot  100  can include instructions to control the movement (e.g., speed, trajectory) of robot  100 . For example, in an autonomous-vehicle embodiment, the instructions can cause the one or more processors  210  to control the steering, acceleration, and braking of the vehicle (robot  100 ). 
     This description next turns to a more detailed explanation of the RAT iLQR algorithm employed by robot control system  120  (specifically, computation module  240 ). Consider the following stochastic nonlinear system: x k+1 =f(x k , u k )+g(x k , u k )w k , where x k  ∈    n  denotes the state, u k  ∈    m  the control, and w k  ∈    r  the noise input to the system at time k. For some finite time horizon N, let w 0:N   (w 0 , . . . , w N ) denote the joint noise vector with probability distribution p(w 0:N ). In this embodiment, this distribution is assumed to be a known Gaussian white noise process; i.e., w i  is independent of w 1  for all i≠j, and the stochastic nonlinear system defined above is considered to be the reference system. 
     Ideally, the model distribution p would perfectly characterize the noise in the dynamical system. However, in reality the noise may come from a different, more complex distribution that is not known exactly. This is illustrated in  FIGS.  3 A and  3 B .  FIG.  3 A  depicts a reference distribution  310 , and  FIG.  3 B  depicts an actual distribution  320 . Let  w   0:N   ( w   0 , . . . ,  w   N ) denote a perturbed noise vector that is distributed according to q( w   0:N ). The perturbed system that characterizes the true but unknown dynamics can be defined as follows: x k+1 =f(x k , u k )+g(x k , u k ) w   k . Note that no assumptions are made that q is Gaussian or that it is white noise. One could also attribute it to potentially unmodeled dynamics. The true, unknown probability distribution q is contained in the set   of all probability distributions on the support    r(N+1) . The unknown distribution q is assumed not to be “too different” from p. This is expressed as the following constraint (bound) on the KL divergence between q and p:    KL (q∥p)≤d, where    KL (⋅∥⋅) is the KL divergence, and d&gt;0 is a given constant. Note that    KL (q∥p)≥0 always holds, with equality if and only if p≡q. The set of all possible probability distributions q ∈   satisfying the above KL divergence constraint is denoted by Ξ which is defined as the ambiguity set. Note that Ξ is a convex subset of  for a fixed p. 
     One objective is to control the perturbed system defined above using a state feedback controller of the form u k = (k, x k ). The operator  (k,⋅) defines a mapping from    n  into    m . The class of all such controllers is denoted Λ. 
     The cost model considered in this embodiment is defined as follows: J(x 0:N+1 , u 0:N )  h(x N+1 )+Σ k=0   N c(k, x k , u k ). The foregoing objective is assumed to satisfy the following non-negativity constraints: The functions h(⋅) and c(k,⋅,⋅) satisfy h(x)≥0 and c(k, x, u)≥0 for all k ∈ {0, . . . , N}, x ∈    n , and u ∈    m . 
     Under the above dynamics model for the perturbed system, cost model, and KL divergence constraint on q, an admissible controller   ∈ Λ is sought that minimizes the worst-case expected value of the cost model. In other words, in this embodiment, computation module  240  solves the following distributionally robust optimal control problem: 
                 inf     𝒦   ∈   Λ       ⁢     sup     q   ∈   Ξ       ⁢           ⁢       𝔼   q     (     J   ⁡     (       x       0   ⁢     :     ⁢   N     +   1       ,     u     0   ⁢     :     ⁢   N         )       ]       ,         
where    q  [⋅] indicates that the expectation is taken with respect to the true, unknown distribution q in the ambiguity set Ξ.
 
     Unfortunately, the foregoing distributionally robust optimal control problem is intractable because it involves maximization with respect to the unknown probability distribution q. To overcome this, it can be shown that the foregoing distributionally robust optimal control problem is equivalent to a bilevel optimization problem involving risk-sensitive optimal control with respect to the model distribution p. Before summarizing this equivalence in equation form, the following additional assumption is made: For any admissible controller   ∈ Λ, the resulting closed-loop system satisfies 
                 sup     v   ∈   ??       ⁢       𝔼   v     ⁡     [     J   ⁡     (       x       0   ⁢     :     ⁢   N     +   1       ,     u     0   ⁢     :     ⁢   N         )       ]         =     ∞   .           
This assumption means that, without the KL divergence constraint, some adversarially-chosen noise could make the expected cost objective arbitrarily large, in the worst case. This amounts to a controllability-type assumption with respect to the noise input and an observability-type assumption with respect to the cost objective. Under this assumption and the non-negativity assumption discussed above, the following equivalence holds for the distributionally robust optimal control problem defined above:
 
                   inf     𝒦   ∈   Λ       ⁢     sup     q   ∈   Ξ       ⁢       𝔼   q     (     J   ⁡     (       x       0   ⁢     :     ⁢   N     +   1       ,     u     0   ⁢     :     ⁢   N         )       ]       =         inf     τ   ∈   Γ       ⁢     inf     𝒦   ∈   Λ       ⁢       τlog𝔼   p     [     exp   (       J   ⁡     (       x       0   ⁢     :     ⁢   N     +   1       ,     u     0   ⁢     :     ⁢   N         )       τ     )     ]       +     τ   ⁢           ⁢   d         ,         
provided that the set
 
               Γ   ~     ⁢     =   Δ     ⁢     {       τ   &gt;   0     :       inf     𝒦   ∈   Λ       ⁢       τlog𝔼   p     ⁡     [     exp   ⁡     (     J   /   τ     )       ]       ⁢           ⁢   is   ⁢           ⁢   finite       }           
is non-empty. Observe that the first term in the right-hand side of the foregoing equivalence relationship is the entropic risk measure
 
                 R     p   ,     1   τ         ⁡     (   J   )       ,         
where the risk is computed with respect to the model distribution p and τ&gt;0 serves as the inverse of the risk-sensitivity parameter. Rewriting the above equivalence relationship in terms of the risk-sensitivity parameter
 
               θ   =       1   τ     &gt;   0       ,         
the right-hand side of the equation is equivalent to
 
                 inf     θ   ∈   Γ       ⁡     (         inf     𝒦   ∈   Λ       ⁢       R     p   ,   θ       ⁡     (     J   ⁡     (       x       0   ⁢     :     ⁢   N     +   1       ,     u     0   ⁢     :     ⁢   N         )       )         +     d   θ       )       ,           ⁢     
     ⁢       where   ⁢           ⁢   Γ     ⁢     =   Δ     ⁢       {       θ   &gt;   0     :       inf     𝒦   ∈   Λ       ⁢       R     p   ,   θ       ⁡     (   J   )       ⁢           ⁢   is   ⁢           ⁢   finite       }     .             
Note that the new problem does not involve any optimization with respect to the true distribution q.
 
     The above background leads to formulation of the RAT iLQR algorithm. Even though the mathematical equivalence discussed above is general, it does not immediately lead to a tractable method to efficiently compute a solution for general nonlinear systems. Two remaining challenges need to be addressed. First, exact optimization of the entropic risk with a state feedback control law is intractable, except for linear systems with quadratic costs. Second, the optimal risk-sensitivity parameter needs to be searched efficiently over the feasible space F, which not only is unknown but also varies depending on the initial state x 0 . The RAT iLQR algorithm overcomes these challenges for general nonlinear systems. An explanation of how the algorithm solves both the inner and outer loop of 
               inf     θ   ∈   Γ       ⁡     (         inf     𝒦   ∈   Λ       ⁢       R     p   ,   θ       ⁡     (     J   ⁡     (       x       0   ⁢     :     ⁢   N     +   1       ,     u     0   ⁢     :     ⁢   N         )       )         +     d   θ       )           
to develop a distributionally-robust, risk-sensitive MPC follows.
 
     First, consider the inner minimization: 
                 inf     𝒦   ∈   Λ       ⁢       R     p   ,   θ       ⁡     (     J   ⁡     (       x       0   ⁢     :     ⁢   N     +   1       ,     u     0   ⁢     :     ⁢   N         )       )         ,         
where the term d/θ has been omitted, since it is constant with respect to the controller  . This amounts to solving a risk-sensitive optimal control problem for a nonlinear Gaussian system. In this embodiment, a variant of the discrete-time iLEQG algorithm is employed to obtain a locally optimal solution to the above inner minimization. In what follows, it is assumed that the noise coefficient function g(x k , u k ) discussed above is the identity mapping, for simplicity. The algorithm begins by applying a given nominal control sequence l 0:N  to the noiseless dynamics to obtain the corresponding nominal state trajectory  x   0:N+1 . During each iteration, the algorithm maintains and updates a locally optimal controller   of the form  (k, x k )=L k (x k − x   k )+l k , where L k  ∈    m×n  denotes the feedback gain matrix. The i-th iteration of the iLEQG implementation includes four steps that are described in detail below.
 
     Step 1 is local approximation. Given the nominal trajectory {l 0:N   i , x   0:N+1   i }, the following linear approximation of the dynamics, as well as the quadratic approximation of the cost functions, are computed by computation module  240 :
 
 A   k   =D   x   f (   x     k   (i)   ,l   k   (i) )
 
 B   k   =D   u   f (   x     k   (i)   ,l   k   (i) )
 
 q   k   =c ( k, x     k   (i)   ,l   k   (i) )
 
 q   k   =D   x   c ( k, x     k   (i)   ,l   k   (i) )
 
 Q   k   =D   xx   c ( k, x     k   (i)   ,l   k   (i) )
 
 r   k   =D   u   c ( k, x     k   (i)   ,l   k   (i) )
 
 R   k   =D   uu   c ( k, x     k   (i)   ,l   k   (i) )
 
 P   k   =D   ux   c ( k, x     k   (i)   ,l   k   (i) )
 
for k=0 to N, where D is the differentiation operator. Also, let q N+1 =h( x   N+1   (i) ), q N+1 =D x h( x   N+1   (i) ), and Q N+1 =D xx h( x   N+1   (i) ).
 
     Step 2 is the backward pass. In this step, computation module  240  performs approximate dynamic programming (DP) using the current feedback gain matrices L 0:N   (i)  as well as the approximate model obtained in Step 1. In this embodiment, it is assumed that the noise vector w k  is Gaussian-distributed according to  (0, W k ) with W k   0. Also, let s N+1   q N+1 , s N+1    q N+1 , and S N+1   Q N+1 . Given these terminal conditions, computation module  240  recursively computes the following quantities:
 
 M   k   =W   k   −1   −θS   k+1  
 
 g   k   =r   k   +B   k   T ( I+θS   k+1   M   k   −1 ) S   k+1  
 
 G   k   =P   k   +B   k   T ( I+θS   k+1   M   k   −1 ) S   k+1   A   k  
 
 H   k   =R   k   +B   k   T ( I+θS   k+1   M   k   −1 ) S   k+1   B   k  
 
 s   k   =q   k   +s   k+1 −1/2θ log det( I−θW   k   S   k+1 )+η/2 s   k+1   T   M   k   −1   s   k+1 +½ l   k   (i)T   H   k   l   k   (i)   +l   k   (i)T   g   k  
 
 s   k   =q   k   +A   k   T ( I+θS   k+1   M   k   −1 ) s   k+1   +L   k   (i)T   H   k   L   k   (i)   +L   k   (i)T   g   k   +G   k   T   l   k   (i)  
 
 S   k   =Q   k   +A   k   T ( I+θS   k+1   M   k   −1 ) S   k+1   A   k   +L   k   (i)T   H   k   L   k   (i)   +L   k   (i)T   G   k   +G   k   T   L   k   (i)  
 
from k=N down to 0. Note that M k   0 is assumed so that it is invertible, which might not hold if θ is too large. This is sometimes referred to as “neurotic breakdown,” when the optimizer is so pessimistic that the cost-to-go approximation becomes infinity. Otherwise, the approximated cost-to-go for this optimal control (under the controller {L 0:N   (i) , l 0:N   (i) }) is given by s 0 .
 
     Step 3 is regularization and control computation. Having derived the DP solution, computation module  240  computes new control gains L 0:N   (i+1)  and offset updates dl 0:N  as follows:
 
 L   k   (i+1) =−( H   k   +μl ) −1   G   k  
 
 dl   k =−( H   k   +μl ) −1   g   k ,
 
where μ≥0 is a regularization parameter to prevent (H k +μI) from having negative eigenvalues. Computation module  240  adaptively changes this regularization parameter μ across multiple iterations so the algorithm enjoys fast convergence near a local minimum while ensuring the positive-definiteness of (H k +μI) at all times.
 
     Step 4 is a line search for ensuring convergence. It is known that the update could lead to increased cost or even divergence if a new trajectory strays too far from the region where the local approximation is valid. Thus, computation module  240  computes the new nominal control trajectory L 0:N   (i+1)  by backtracking line search with line search parameter ϵ. Initially, ϵ=1 and computation module  240  derives a new candidate nominal trajectory as follows:
 
 {circumflex over (l)}   k   =L   k   (i+1) ( {circumflex over (x)}   k   − x     k   (i) )+ l   k   (i)   +ϵdl   k  
 
 {circumflex over (x)}   k+1   =f ( {circumflex over (x)}   k   ,{circumflex over (l)}   k ).
 
If this candidate trajectory {{circumflex over (l)} 0:N , {circumflex over (x)} 0:N+1 } results in a lower cost-to-go than the current nominal trajectory, then the candidate trajectory is accepted and returned as {{circumflex over (l)} 0:N   (i+1) , {circumflex over (x)} 0:N+1   (i+1) }. Otherwise, the trajectory is rejected and re-derived with ϵ←ϵ/2 until it is accepted.
 
     The above inner-loop procedure (Steps 1-4) is iterated until the nominal control l k  does not change beyond some threshold in a norm. Once converged, the algorithm returns the updated nominal trajectory {l 0:N ,  x   0:N+1 } as well as the feedback gains L 0:N  and the approximate cost-to-go s 0 . 
     Having implemented the iLEQG algorithm for the inner-loop optimization of 
                 inf     θ   ∈   Γ       ⁡     (         inf     𝒦   ∈   Λ       ⁢       R     p   ,   θ       ⁡     (     J   ⁡     (       x       0   ⁢     :     ⁢   N     +   1       ,     u     0   ⁢     :     ⁢   N         )       )         +     d   θ       )       ,         
computation module  240  employs a cross-entropy process to solve the outer-loop optimization for the optimal risk-sensitivity parameter θ*. This is a one-dimensional optimization problem in which the function evaluation is done by solving the corresponding risk-sensitive optimal control discussed above. In some embodiments, the cross-entropy method is adapted somewhat to derive the approximately optimal value for θ*. This approach is favorable for online optimization due to the any-time and highly-parallel nature of the Monte Carlo sampling. However, in other embodiments, a different approach can be used. The cross-entropy process is a stochastic method that maintains an explicit probability distribution over the design space. At each step, a set of m s  Monte Carlo samples are drawn from the distribution, from which a subset of m e  “elite samples” that achieve the best performance are selected and retained. The parameters of the distribution are then updated according to the maximum likelihood estimate based on the elite samples. The algorithm terminates after a desired number of steps M.
 
     In one embodiment, computation module  240  models the distribution as univariate Gaussian  (μ, σ 2 ). Another issue mentioned above is that the iLEQG algorithm may return a cost-to-go of infinity if a sampled θ is too large due to neurotic breakdown. Since the search space is limited to Γ where θ yields a finite cost-to-go, computation module  240  ensures that each iteration has sufficient samples in Γ. 
     To address this problem, the cross-entropy process is augmented, in some embodiments, with rejection and re-sampling. Out of the m s  samples drawn from the univariate Gaussian distribution, all non-positive samples are first discarded. For each of the remaining samples, computation module  240  evaluates the objective function discussed above by a call to iLEQG and counts the number of samples that obtained a finite cost-to-go. Let m v  be the number of such valid samples. If m v ≥max(m e , m s /2), computation module  240  proceeds to fit the distribution. Otherwise, computation module  240  repeats the sampling procedure, since there are insufficient valid samples from which to choose the elite samples. 
     In practice, re-sampling is unlikely to occur after the first iteration of the cross-entropy process. However, to avoid the risk that the first iteration might result in re-sampling multiple times, degrading efficiency, computation module  240  also performs an adaptive initialization of the Gaussian parameters μ init  and G init  in the first iteration as follows. If the first iteration with   (μ init , σ init ) results in re-sampling, computation module  240  not only re-samples but also divides μ ink  and σ init  by half. On the other hand, if all of the m s  samples are valid, computation module  240  accepts them but doubles μ ink  and σ init  because it implies that the initial set of samples is not wide-spread enough to cover the whole feasible set Γ. The parameters μ ink  and σ init  can be stored internally in the cross-entropy solver of computation module  240  and carried over to the next call to the algorithm. 
     At runtime, the RAT iLQR algorithm, in some embodiments, is executed as an MPC in a receding-horizon fashion. The control is re-computed after executing the first control input u 0 =l 0  and transitioning to a new state. A previously computed control trajectory l 0:N  can be reused for the initial nominal control trajectory at the next time step to warm-start the computation. The RAT iLQR algorithm (“Algorithm 1”) can be summarized by the following pseudocode: 
     
       
         
           
               
             
               
                   
               
             
            
               
                 Input: Initial state x 0 , initial nominal control trajectory l 0:N , KL divergence bound d 
               
               
                 Output: New nominal trajectory {l 0:N ,  x   0:N+1 }, control gains L 0:N , risk-sensitivity 
               
               
                     parameter θ* 
               
            
           
           
               
               
            
               
                 1: 
                 Compute initial nominal state trajectory  x   0:N+1  using l 0:N   
               
               
                 2: 
                 i ← 1 
               
               
                 3: 
                  while i ≤ M do 
               
               
                 4: 
                   while True do 
               
               
                 5:  
                    if i = 1 then 
               
               
                 6:  
                     θ sampled  ← drawSamples(m s , μ init , σ init ) 
               
               
                 7:  
                    else 
               
               
                 8:  
                     θ sampled  ← drawSamples(m s , μ, σ) 
               
               
                 9:  
                    end if 
               
               
                 10:  
                    array r      Empty array of size m s   
               
               
                 11:  
                    for j ← 1 : m s  do 
               
               
                 12:  
                     Solve iLEQG with {l 0:N ,  x   0:N+1 , θ sampled [j]} 
               
               
                 13:  
                     Obtain approximate cost-to-go s 0   
               
               
                 14:  
                     r[j] ← s 0  + d/θ sampled [j] 
               
               
                 15:  
                    end for 
               
               
                 16:  
                    m v  ← countValidSamples(θ sampled , r) 
               
               
                 17:  
                    if i = 1 and m v  &lt; max(m e , m s /2) then 
               
               
                 18:  
                     μ init  ← μ init /2, σ init  ← σ init /2 
               
               
                 19:  
                    else if i = 1 and m v  = m s  then 
               
               
                 20:  
                     μ init  ← 2μ init , σ init  ← 2σ init   
               
               
                 21:  
                     break 
               
               
                 22:  
                    else if m v  ≥ max(m e , m s /2) then 
               
               
                 23:  
                     break 
               
               
                 24:  
                    end if 
               
               
                 25: 
                   end while 
               
               
                 26:  
                   θ elite  ← selectElite(m e , θ sampled , r) 
               
               
                 27:  
                   {μ, σ} ← fitGaussian(θ elite ) 
               
               
                 28:  
                   i ← i +1 
               
               
                 29: 
                  end while 
               
               
                 30: 
                  θ* ← μ 
               
               
                 31: 
                  Solve iLEQG with {l 0:N ,  x   0:N+1 , θ*} 
               
               
                 32:  
                  Obtain new nominal trajectory {l 0:N ,  x   0:N+1 } and control gains L 0:N   
               
               
                 33:  
                  return l 0:N ,  x   0:N+1 , L 0:N , θ* 
               
               
                   
               
            
           
         
       
     
       FIG.  4    is a flowchart of a method of controlling a robot, in accordance with an illustrative embodiment of the invention. Method  400  will be discussed from the perspective of robot control system  120  in  FIGS.  1  and  2   . While method  400  is discussed in combination with robot control system  120 , it should be appreciated that method  400  is not limited to being implemented within robot control system  120 , but robot control system  120  is instead one example of a system that may implement method  400 . 
     At block  410 , input module  230  receives an initial state  140  of the robot  100 , an initial nominal control trajectory  145  of the robot  100 , and a KL divergence bound  150  between a modeled probability distribution for a stochastic disturbance and an unknown actual probability distribution for the stochastic disturbance. As discussed above, initial state  140  can include, for example, the position, velocity, and heading/pose of robot  100 , as well as object-tracking information concerning the state of objects in the environment near robot  100 . In an autonomous-vehicle embodiment, the objects in the environment could include, for example, other road users and obstacles. Other road users include, without limitation, other vehicles, motorcyclists, bicyclists, and pedestrians. As also discussed above, the various embodiments described herein perform the bilevel optimization based on the worst-case distribution within a set of possible distributions that also includes the distribution used in the stochastic-system model. Such a set of distributions can be analyzed using a metric such as the KL divergence bound  150 . 
     As discussed above, the stochastic disturbance can take on different forms, depending on the particular embodiment. For example, in an embodiment in which robot  100  is an autonomous vehicle, the stochastic disturbance could be slippery road conditions caused by rain, ice, or snow. Another example of a stochastic disturbance is that associated with the motion of an other road user such as another vehicle, a motorcyclist, a bicyclist, or a pedestrian. As discussed further below, in some embodiments the modeled probability distribution for the stochastic disturbance is a Gaussian distribution. 
     At block  420 , computation module  240  solves a bilevel optimization problem subject to the modeled probability distribution and the KL divergence bound using an iterative Linear-Exponential-Quadratic-Gaussian (iLEQG) algorithm and a cross-entropy process. The iLEQG algorithm outputs an updated nominal control trajectory, and the cross-entropy process outputs a risk-sensitivity parameter. The details of the iLEQG algorithm and the cross-entropy process included in the overall RAT iLQR algorithm (Algorithm 1), including the underlying mathematical concepts, are discussed above. 
     At block  430 , control module  250  controls operation of the robot  100  based, at least in part, on the updated nominal control trajectory and the risk-sensitivity parameter. As discussed above, in an autonomous-vehicle embodiment, the instructions in the control module  250  to control operation of the robot  100  can also include, for example, instructions to avoid a collision with an other road user (e.g., another vehicle, a motorcyclist, a bicyclist, or a pedestrian). The instructions to control the operation of the robot  100  can include instructions to control the movement (e.g., speed, trajectory) of robot  100 . For example, in an autonomous-vehicle embodiment, the instructions can control the steering, acceleration, and braking of the vehicle. 
     Detailed embodiments are disclosed herein. However, it is to be understood that the disclosed embodiments are intended only as examples. Therefore, specific structural and functional details disclosed herein are not to be interpreted as limiting, but merely as a basis for the claims and as a representative basis for teaching one skilled in the art to variously employ the aspects herein in virtually any appropriately detailed structure. Further, the terms and phrases used herein are not intended to be limiting but rather to provide an understandable description of possible implementations. Various embodiments are shown in  FIGS.  1 - 4   , but the embodiments are not limited to the illustrated structure or application. 
     The flowcharts and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of systems, methods and computer program products according to various embodiments. In this regard, each block in the flowcharts or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that, in some alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. 
     The systems, components and/or processes described above can be realized in hardware or a combination of hardware and software and can be realized in a centralized fashion in one processing system or in a distributed fashion where different elements are spread across several interconnected processing systems. Any kind of processing system or another apparatus adapted for carrying out the methods described herein is suited. A typical combination of hardware and software can be a processing system with computer-usable program code that, when being loaded and executed, controls the processing system such that it carries out the methods described herein. The systems, components and/or processes also can be embedded in a computer-readable storage, such as a computer program product or other data programs storage device, readable by a machine, tangibly embodying a program of instructions executable by the machine to perform methods and processes described herein. These elements also can be embedded in an application product which comprises all the features enabling the implementation of the methods described herein and, which when loaded in a processing system, is able to carry out these methods. 
     Furthermore, arrangements described herein may take the form of a computer program product embodied in one or more computer-readable media having computer-readable program code embodied, e.g., stored, thereon. Any combination of one or more computer-readable media may be utilized. The computer-readable medium may be a computer-readable signal medium or a computer-readable storage medium. The phrase “computer-readable storage medium” means a non-transitory storage medium. A computer-readable storage medium may be, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any suitable combination of the foregoing. More specific examples (a non-exhaustive list) of the computer-readable storage medium would include the following: a portable computer diskette, a hard disk drive (HDD), a solid-state drive (SSD), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or Flash memory), a portable compact disc read-only memory (CD-ROM), a digital versatile disc (DVD), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing. In the context of this document, a computer-readable storage medium may be any tangible medium that can contain or store a program for use by or in connection with an instruction execution system, apparatus, or device. 
     Program code embodied on a computer-readable medium may be transmitted using any appropriate medium, including but not limited to wireless, wireline, optical fiber, cable, RF, etc., or any suitable combination of the foregoing. Computer program code for carrying out operations for aspects of the present arrangements may be written in any combination of one or more programming languages, including an object-oriented programming language such as Java™ Smalltalk, C++ or the like and conventional procedural programming languages, such as the “C” programming language or similar programming languages. The program code may execute entirely on the user&#39;s computer, partly on the user&#39;s computer, as a stand-alone software package, partly on the user&#39;s computer and partly on a remote computer, or entirely on the remote computer or server. In the latter scenario, the remote computer may be connected to the user&#39;s computer through any type of network, including a local area network (LAN) or a wide area network (WAN), or the connection may be made to an external computer (for example, through the Internet using an Internet Service Provider). 
     Generally, “module,” as used herein, includes routines, programs, objects, components, data structures, and so on that perform particular tasks or implement particular data types. In further aspects, a memory generally stores the noted modules. The memory associated with a module may be a buffer or cache embedded within a processor, a RAM, a ROM, a flash memory, or another suitable electronic storage medium. In still further aspects, a module as envisioned by the present disclosure is implemented as an application-specific integrated circuit (ASIC), a hardware component of a system on a chip (SoC), as a programmable logic array (PLA), or as another suitable hardware component that is embedded with a defined configuration set (e.g., instructions) for performing the disclosed functions. 
     The terms “a” and “an,” as used herein, are defined as one or more than one. The term “plurality,” as used herein, is defined as two or more than two. The term “another,” as used herein, is defined as at least a second or more. The terms “including” and/or “having,” as used herein, are defined as comprising (i.e. open language). The phrase “at least one of . . . and . . . ” as used herein refers to and encompasses any and all possible combinations of one or more of the associated listed items. As an example, the phrase “at least one of A, B, and C” includes A only, B only, C only, or any combination thereof (e.g. AB, AC, BC or ABC). 
     Aspects herein can be embodied in other forms without departing from the spirit or essential attributes thereof. Accordingly, reference should be made to the following claims rather than to the foregoing specification, as indicating the scope hereof.