RAT iLQR: A Risk Auto-Tuning Controller to Optimally Account for Stochastic Model Mismatch

Distributionally robust control seeks to optimize control actions against a worst-case distribution within a given set of probability distributions, often called the ambiguity set [8, 9]. There exist various formulations to account for distributional robustness in optimal control. Some works are concerned with minimizing the worst-case expectation of a cost objective [9, 10], while others enforce risk-based or chance constraint satisfaction under a worst-case distribution [11, 8]. The present work belongs to the former class. Existing methods also differ in the formulation of the ambiguity set. Moment-based ambiguity sets require knowledge of moments of the ground-truth distribution up to a finite order [8, 9], which is often overly conservative [11]. Statistical distance-based ambiguity sets are also gaining attention. The authors of [11] use a Wasserstein metric to define the ambiguity set for motion planning with collision avoidance, but their MPC formulation is not suited for nonlinear systems. $\chi^{2}$-divergence and more general $\phi$-divergences (which KL divergence belongs to) are employed in [10], similar to the present work. However, the ambiguity set considered in [10] is restricted to categorical distributions, while our work requires no assumption on the class of the ground-truth distributions. Furthermore, we make use of risk-sensitive optimal control to obtain planned robot trajectories with feedback, unlike sampling in their implementation.

Optimization of the entropic risk measure has been an active research topic in economics and controls literature since 1970s [12, 13, 3, 14]. The concept of risk-sensitive optimal control has been successfully applied to robotics in various domains, including haptic assistance [4, 5], model-based reinforcement learning (RL) [6], and safe robot navigation [7, 15], to name a few. In all these works, the risk-sensitivity parameter is introduced as a user-specified constant, and is found to significantly affect the behavior of the robot. For instance, our prior work on safe robot navigation in human crowds [7] reveals that a robot with higher risk-sensitivity tends to yield more to oncoming human pedestrians. However, how to find a desirable risk-sensitivity parameter still remains an open research question; in the robot navigation problem, the robot simply freezes if it is too risk-sensitive when the scene is crowded. As the authors of [5] point out, the robot should adapt its risk-sensitivity level depending on the situation, yet there still does not exist an effective algorithmic framework to automate it due to the issues discussed in Section I. In this work, we provide such an algorithm for nonlinear, non-Gaussian stochastic systems. As mentioned earlier, our approach is built on previously-established theoretical results that link risk-sensitive and distributionally robust control [1].

The theory of optimal control lets us derive an optimal feedback control law via dynamic programming (DP) [16]. For linear systems with additive Gaussian white noise and quadratic cost functions, the exact DP solution is tractable and is known as Linear-Quadratic-Gaussian (LQG) [17] or Linear-Exponential-Quadratic-Gaussian (LEQG) [13]. They are different in that LQG optimizes the expected cost while LEQG optimizes the entropic risk measure, although both DP recursions are quite similar.

However, solving general optimal control problems for nonlinear systems remains a challenge due to lack of analytical tractability. Hence, approximate local optimization methods have been developed, including Differential Dynamic Programming (DDP) [18], iterative Linear-Quadratic Regulator (iLQR) [19], and iterative Linear-Quadratic-Gaussian (iLQG) [20, 21]. While both DDP and iLQR are designed for deterministic systems with quadratic cost functions, iLQG can locally optimize the expected cost objective for Gaussian stochastic systems with non-convex cost functions. Similarly, the iterative Linear-Exponential-Quadratic-Gaussian (iLEQG) has been recently proposed to locally optimize the entropic risk for Gaussian systems with non-convex costs [22, 15, 23]. Note however that they are not designed to be robust to model mismatches that we consider in this paper. In fact, it is known that even LQG does not possess guaranteed robustness [24].

Consider the following stochastic nonlinear system:

where $x_{k}\in\mathbb{R}^{n}$ denotes the state, $u_{k}\in\mathbb{R}^{m}$ the control, and $w_{k}\in\mathbb{R}^{r}$ the noise input to the system at time $k$. For some finite horizon $N$, let $w_{0:N}\triangleq(w_{0},\dots,w_{N})$ denote the joint noise vector with probability distribution $q(w_{0:N})$. This distribution is assumed to be a known Gaussian white noise process, i.e. $w_{i}$ is independent of $w_{j}$ for all $i\neq j$, and we call (2) the reference system. Ideally, we would like the model distribution $q$ to perfectly characterize the noise in the dynamical system. However, in reality the noise may come from a different, more complex distribution which we may not know exactly. Let $\bar{w}_{0:N}\triangleq(\bar{w}_{0},\dots,\bar{w}_{N})$ denote a perturbed noise vector that is distributed according to $p(\bar{w}_{0:N})$. We define the following perturbed system that characterizes the true but unknown dynamics:

Note that we make no assumptions on Gaussianity or whiteness of $p$. One could also attribute it to potentially unmodeled dynamics. The true, unknown probability distribution $p$ is contained in the set $\mathcal{P}$ of all probability distributions on the support $\mathbb{R}^{r(N+1)}$. We assume that $p$ is not “too different” from $q$. This is encoded as the following constraint on the KL divergence between $p$ and $q$:

where $\mathbb{D}_{\mathrm{KL}}(\cdot\|\cdot)$ is the KL divergence and $d>0$ is a given constant. Note that $\mathbb{D}_{\mathrm{KL}}(p\|q)\geq 0$ always holds, with equality if and only if $p\equiv q$. The set of all possible probability distributions $p\in\mathcal{P}$ satisfying (4) is denoted by $\Xi$, which we define as our ambiguity set. This set is a convex subset of $\mathcal{P}$ for a fixed $q$ (Lemma 1.4.3, [25]).

We are interested in controlling the perturbed system (3) with a state feedback controller of the form $u_{k}=\mathcal{K}(k,x_{k})$. The operator $\mathcal{K}(k,\cdot)$ defines a mapping from $\mathbb{R}^{n}$ into $\mathbb{R}^{m}$. The class of all such controllers is denoted $\Lambda$.

The cost function considered in this paper is given by

where $c$ is the stage cost function and $h$ is the terminal cost. We assume that the above objective satisfies the following non-negativity assumption.

Under the dynamics model (3), the cost model (5), and the KL divergence constraint (4) on $p$, we are interested in finding an admissible controller $\mathcal{K}\in\Lambda$ that minimizes the worst-case expected value of the cost objective (5). In other words, we are concerned with the following distributionally robust optimal control problem:

where $\mathbb{E}_{p}[\cdot]$ indicates that the expectation is taken with respect to the true, unknown distribution $p$. In this formulation, the robustness arises from the ability of the controller to plan against a worst-case distribution $p$ in the ambiguity set $\Xi$.

Unfortunately, the distributionally robust optimal control problem (6) is intractable as it involves maximization with respect to a probability distribution $p$. To circumvent this, [1] proves that problem (6) is equivalent to a bilevel optimization problem involving risk-sensitive optimal control with respect to the model distribution $q$. We refer the reader to [1] for the derivation and only re-state the main results in this section for self-containedness. Before doing so, we impose an additional assumption on the worst-case expected cost.

This assumption states that, without the KL divergence constraint, some adversarially-chosen noise could make the expected cost objective arbitrarily large in the worst case. It amounts to a controllability-type assumption with respect to the noise input and an observability-type assumption with respect to the cost objective [1].

Under Assumptions 1 and 2, the following theorem holds.

Let us first consider the inner minimization of (11):

where we omitted the extra term $d/\theta$ as it is constant with respect to the controller $\mathcal{K}$. This amounts to solving a risk-sensitive optimal control problem for a nonlinear Gaussian system. Recently, a computationally-efficient, local optimization method called iterative Linear-Exponential-Quadratic-Gaussian (iLEQG) has been proposed for both continuous-time systems [22] and the discrete-time counterpart [23, 15]. Both versions locally optimize the entropic risk measure with respect to a receding horizon, affine feedback control law for general nonlinear systems with non-convex costs.

We adopt a variant of the discrete-time iLEQG algorithm [15] to obtain a locally optimal solution to (12). In what follows, we assume that the noise coefficient function $g(x_{k},u_{k})$ in (2) is the identity mapping for simplicity, but it is straightforward to handle nonlinear functions in a similar manner as discussed in [20]. The algorithm starts by applying a given nominal control sequence $l_{0:N}$ to the noiseless dynamics to obtain the corresponding nominal state trajectory $\bar{x}_{0:N+1}$. In each iteration, the algorithm maintains and updates a locally optimal controller $\mathcal{K}$ of the form:

where $L_{k}\in\mathbb{R}^{m\times n}$ denotes the feedback gain matrix. The $i$-th iteration of our iLEQG implementation consists of the following four steps:

1. 1)

   Local Approximation: Given the nominal trajectory $\{l^{(i)}_{0:N},\bar{x}^{(i)}_{0:N+1}\}$, we compute the following linear approximation of the dynamics as well as the quadratic approximation of the cost functions:

   	$\displaystyle A_{k}$	$\displaystyle=D_{x}f(\bar{x}^{(i)}_{k},l^{(i)}_{k}),B_{k}=D_{u}f(\bar{x}^{(i)}_{k},l^{(i)}_{k})$		(14)
   	$\displaystyle q_{k}$	$\displaystyle=c(k,\bar{x}^{(i)}_{k},l^{(i)}_{k})$		(15)
   	$\displaystyle\mathbf{q}_{k}$	$\displaystyle=D_{x}c(k,\bar{x}^{(i)}_{k},l^{(i)}_{k}),Q_{k}=D_{xx}c(k,\bar{x}^{(i)}_{k},l^{(i)}_{k})$		(16)
   	$\displaystyle\mathbf{r}_{k}$	$\displaystyle=D_{u}c(k,\bar{x}^{(i)}_{k},l^{(i)}_{k})$		(17)
   	$\displaystyle R_{k}$	$\displaystyle=D_{uu}c(k,\bar{x}^{(i)}_{k},l^{(i)}_{k}),P_{k}=D_{ux}c(k,\bar{x}^{(i)}_{k},l^{(i)}_{k})$		(18)

   for $k=0$ to $N$, where $D$ is the differentiation operator. We also let $q_{N+1}=h(\bar{x}^{(i)}_{N+1})$, $\mathbf{q}_{N+1}=D_{x}h(\bar{x}^{(i)}_{N+1})$, and $Q_{N+1}=D_{xx}h(\bar{x}^{(i)}_{N+1})$.

2. 2)

   Backward Pass: We perform approximate DP using the current feedback gain matrices $L^{(i)}_{0:N}$ as well as the approximated model obtained in the previous step. Suppose that the noise vector $w_{k}$ is Gaussian-distributed according to $\mathcal{N}(0,W_{k})$ with $W_{k}\succ 0$. Let $s_{N+1}\triangleq q_{N+1}$, $\mathbf{s}_{N+1}\triangleq\mathbf{q}_{N+1}$, and $S_{N+1}\triangleq Q_{N+1}$. Given these terminal conditions, we recursively compute the following quantities:

   	$\displaystyle M_{k}$	$\displaystyle=W_{k}^{-1}-\theta S_{k+1}$		(19)
   	$\displaystyle\mathbf{g}_{k}$	$\displaystyle=\mathbf{r}_{k}+B_{k}^{\mathrm{T}}(I+\theta S_{k+1}M_{k}^{-1})\mathbf{s}_{k+1}$		(20)
   	$\displaystyle G_{k}$	$\displaystyle=P_{k}+B_{k}^{\mathrm{T}}(I+\theta S_{k+1}M_{k}^{-1})S_{k+1}A_{k}$		(21)
   	$\displaystyle H_{k}$	$\displaystyle=R_{k}+B_{k}^{\mathrm{T}}(I+\theta S_{k+1}M_{k}^{-1})S_{k+1}B_{k},$		(22)

   and

   $$s_{k}=q_{k}+s_{k+1}-\frac{1}{2\theta}\log\det(I-\theta W_{k}S_{k+1})\\ +\frac{\theta}{2}\mathbf{s}_{k+1}^{\mathrm{T}}M_{k}^{-1}\mathbf{s}_{k+1}+\frac{1}{2}l^{(i)\mathrm{T}}_{k}H_{k}l^{(i)}_{k}+l^{(i)\mathrm{T}}_{k}\mathbf{g}_{k}$$

   (23)

   $$\mathbf{s}_{k}=\mathbf{q}_{k}+A_{k}^{\mathrm{T}}(I+\theta S_{k+1}M_{k}^{-1})\mathbf{s}_{k+1}\\ +L^{(i)\mathrm{T}}_{k}H_{k}l^{(i)}_{k}+L_{k}^{(i)\mathrm{T}}\mathbf{g}_{k}+G_{k}^{\mathrm{T}}l^{(i)}_{k}$$

   (24)

   $$S_{k}=Q_{k}+A_{k}^{\mathrm{T}}(I+\theta S_{k+1}M_{k}^{-1})S_{k+1}A_{k}\\ +L_{k}^{(i)\mathrm{T}}H_{k}L_{k}^{(i)}+L_{k}^{(i)\mathrm{T}}G_{k}+G_{k}^{\mathrm{T}}L_{k}^{(i)},$$

   (25)

   from $k=N$ down to $0$. Note that $M_{k}\succ 0$ is necessary so it is invertible, which may not hold if $\theta$ is too large. This is called “neurotic breakdown,” when the optimizer is so pessimistic that the cost-to-go approximation becomes infinity [3]. Otherwise, the approximated cost-to-go for this optimal control (under the controller $\{L^{(i)}_{0:N},l^{(i)}_{0:N}\}$) is given by $s_{0}$.

3. 3)

   Regularization and Control Computation: Having derived the DP solution, we compute new control gains $L^{(i+1)}_{0:N}$ and offset updates $dl_{0:N}$ as follows:

   	$\displaystyle L^{(i+1)}_{k}$	$\displaystyle=-(H_{k}+\mu I)^{-1}G_{k}$		(26)
   	$\displaystyle dl_{k}$	$\displaystyle=-(H_{k}+\mu I)^{-1}\mathbf{g}_{k},$		(27)

   where $\mu\geq 0$ is a regularization parameter to prevent $(H_{k}+\mu I)$ from having negative eigenvalues. We adaptively change $\mu$ across multiple iterations as suggested in [21], so the algorithm enjoys fast convergence near a local minimum while ensuring the positive-definiteness of $(H_{k}+\mu I)$ at all times.

4. 4)

   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 [21]. Thus, the new nominal control trajectory $l^{(i+1)}_{0:N}$ is computed by backtracking line search with line search parameter $\epsilon$. Initially, $\epsilon=1$ and we derive a new candidate nominal trajectory as follows:

   	$\displaystyle\hat{l}_{k}$	$\displaystyle=L_{k}^{(i+1)}(\hat{x}_{k}-\bar{x}^{(i)}_{k})+l^{(i)}_{k}+\epsilon dl_{k}$		(28)
   	$\displaystyle\hat{x}_{k+1}$	$\displaystyle=f(\hat{x}_{k},\hat{l}_{k}).$		(29)

   If this candidate trajectory $\{\hat{l}_{0:N},\hat{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 $\{l^{(i+1)}_{0:N},\bar{x}^{(i+1)}_{0:N+1}\}$. Otherwise, the trajectory is rejected and re-derived with $\epsilon\leftarrow\epsilon/2$ until it is accepted. More details on this line search can be found in [26].

The above procedure is iterated until the nominal control $l_{k}$ does not change beyond some threshold in a norm. Once converged, the algorithm returns the nominal trajectory $\{l_{0:N},\bar{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 as a local approximation method¹¹1Although the solutions obtained by iLEQG are local optima and different for varied risk-sensitivity $\theta$, we have empirically observed that they are not changing drastically as a function of $\theta$. This indicates that iLEQG is a sensible choice for the inner-loop optimization problem, since the solutions do not hop around local optima that are too different as we vary $\theta$. for the inner-loop optimization of (11), it remains to solve the outer-loop optimization for the optimal risk-sensitivity parameter $\theta^{*}$. This is a one-dimensional optimization problem in which the function evaluation is done by solving the corresponding risk-sensitive optimal control (12). In this work we choose to adapt the cross entropy method [27, 28] to derive the approximately optimal value for $\theta^{*}$. This method is favorable for online optimization due to its any-time nature and high parallelizability of the Monte Carlo sampling. As a byproduct, it is also effective in approximately finding the maximum feasible $\theta\in\Gamma$ within a few iterations, which is detailed later in this section. We note however that it is possible to use other methods for the outer-loop optimization as well. The cross entropy method is a stochastic method that maintains an explicit probability distribution over the design space. At each step, a set of $m_{\text{s}}$ Monte Carlo samples is drawn from the distribution, out of which a subset of $m_{\text{e}}$ “elite samples” that achieve the best performance is retained. The parameters of the distribution is then updated according to the maximum likelihood estimate on the elite samples. The algorithm stops after a desired number of steps $M$.

In our implementation we model the distribution²²2Note that this distribution is defined in the space of $\theta$ and has nothing to do with $p$ or $q$ that define stochastic noise in the dynamical system. as univariate Gaussian $\mathcal{N}(\mu,\sigma^{2})$. A remaining issue is that the iLEQG may return the cost-to-go of infinity if a sampled $\theta$ is too large, due to neurotic breakdown. Since our search space is limited to $\Gamma$ where $\theta$ yields a finite cost-to-go, we have to ensure that each iteration has enough samples in $\Gamma$.

To address this problem, we augment the cross entropy method with rejection and re-sampling. Out of the $m_{\text{s}}$ samples drawn from the univariate Gaussian, we first discard all non-positive samples. For each of the remaining samples, we evaluate the objective (11) by a call to iLEQG, and then count the number of samples that obtained a finite cost-to-go. Let $m_{\text{v}}$ be the number of such valid samples. If $m_{\text{v}}\geq\max(m_{\text{e}},m_{\text{s}}/2)$, we proceed and fit the distribution. Otherwise, we redo the sampling procedure as there are not sufficiently many valid samples to choose the elites from.

In practice, re-sampling is not likely to occur after the first iteration of the cross entropy method. At the same time, we empirically found that the first iteration has a risk of re-sampling multiple times, hence degrading the efficiency. We therefore also perform an adaptive initialization of the Gaussian parameters $\mu_{\text{init}}$ and $\sigma_{\text{init}}$ in the first iteration as follows. If the first iteration with $\mathcal{N}(\mu_{\text{init}},\sigma^{2}_{\text{init}})$ results in re-sampling, we not only re-sample but also divide $\mu_{\text{init}}$ and $\sigma_{\text{init}}$ by half. If all of the $m_{\text{s}}$ samples are valid, on the other hand, we accept them but double $\mu_{\text{init}}$ and $\sigma_{\text{init}}$, since it implies that the initial set of samples is not wide-spread enough to cover the whole feasible set $\Gamma$. The parameters $\mu_{\text{init}}$ and $\sigma_{\text{init}}$ are stored internally in the cross entropy solver and carried over to the next call to the algorithm. We have empirically found that this adaptive initialization is also useful for approximately finding the maximum feasible $\theta$, which we exploited in a comparative study in Section V-C.

We name the proposed bilevel optimization algorithm RAT iLQR. The pseudo-code is given in Algorithm 1. At run time, it 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}$ is reused for the initial nominal control trajectory at the next time step to warm-start the computation.

We consider a dynamic collision avoidance problem where a unicycle robot has to avoid a pedestrian in a collision course as illustrated in Figure 2. Collision avoidance problems are often modeled by stochastic optimal control in the autonomous systems literature [29] and the human-robot interaction literature [30, 31], including our prior work [7].

The state of the robot is defined by $(r_{x},r_{y},v,\theta)\in\mathbb{R}^{4}$, where $(r_{x},r_{y})$ [m] denotes the position, $v$ [m/s] the velocity, and $\theta$ [rad] the heading angle. The robot’s control input is $u=(a,b)\in\mathbb{R}^{2}$, where $a$ [m/s²] is the acceleration and $b$ [rad/s] is the angular velocity. The pedestrian is modeled as a single integrator, whose position is given by $(p_{x},p_{y})$ [m]. We assume that the position of the pedestrian is known to the robot through onboard sensing. We also employ a constant nominal velocity model $(u_{x},u_{y})=(0.0,1.0)$ [m/s] for the pedestrian. The joint system $x\in\mathbb{R}^{6}$ consists of the state of the robot and the position of the pedestrian. The dynamics are propagated by Euler integration with time interval $dt=0.1$ [s] and additive noise $w_{k}$ to the joint state. The model distribution for $w_{k}$ is a zero-mean Gaussian $\mathcal{N}(0,W)$ with $W=\text{diag}([1e^{-10},1e^{-10},1e^{-3},1e^{-4},0.02,0.02])\times dt$. This covariance matrix $W$ is chosen to encode our modeling assumption that the pedestrian motion is the main source of uncertainty in this joint system, and that the magnitude of the slip is almost negligible for the robot. The ground-truth distribution for the robot is the same Gaussian as in the model, but the pedestrian’s distribution is a mixture of Gaussians that is independent of the robot’s noise. Both the model and the true distributions for the pedestrian are illustrated in Figure 1. Gaussian mixtures are favored by many recent papers in machine learning to account for multi-modality in human’s decision making [32, 33, 34].

RAT iLQR requires an upper-bound on the KL divergence between the model and the true distribution. For the sake of this paper we assume that there is a separate module that provides an estimate. In this specific simulation study, we performed Monte Carlo integration with samples drawn from the true distribution offline. During the simulation, however, we did not reveal any information on the true distribution to RAT iLQR but the estimated KL value³³3One could test RAT iLQR under a distribution that has stronger multi-modality with a much larger KL bound than the one used in this simulation study, but it could introduce over-conservatism and lead to poor mean performance as the ambiguity set becomes too large. This is a common property of distributionally robust optimization [35]. of 32.02. This offline computation was possible due to our time-invariant assumption on the Gaussian mixture. If one is to use more realistic data-driven prediction instead, it is necessary to estimate the KL divergence online since the predictive distribution may change over time as the human-robot interaction evolves. Even though RAT iLQR works with time-varying KL bounds owing to its MPC formulation, we limit our attention to a static KL bound in this work as real-time computation of KL divergence can be challenging. Note that efficient and accurate estimation of information measures (including KL divergence) is still an active area of research in information theory and machine learning [36, 37], which is one of our future research directions.

The cost functions for this problem are given by

where $c_{\text{track}}$ denotes a quadratic cost that penalizes the deviation from a given target robot trajectory, $c_{\text{coll}}$ a collision penalty that incurs high cost when the robot is too close to the pedestrian, and $c_{\text{ctrl}}$ a small quadratic cost on the control input. Mirroring the formulation in [15], we used the following collision cost:

RAT iLQR was implemented in Julia and the Monte Carlo sampling of the cross entropy method was distributed across multiple CPU cores. Our implementation with $N=19$, $M=5$, $m_{\text{s}}=10$, and $m_{\text{e}}=3$ yielded the average computation time of 0.27 [s]. This is 2.7 times slower than real time, with $dt=0.1$ [s] used to measure the real-time property. We expect to achieve improved efficiency by further parameter tuning as well as more careful parallelization.

We compared the performance of RAT iLQR against two baseline MPC algorithms, iLQG [20] and PETS [38]. iLQG corresponds to RAT iLQR with the KL bound of $d=0$, i.e. no distributional robustness is considered. Instead it is more computationally efficient than RAT iLQR, taking only 0.01 [s]. PETS is a state-of-the-art, model-based stochastic MPC algorithm with sampling and is originally proposed in a model-based reinforcement learning (RL) context. We chose PETS as our baseline since it also relies on the cross entropy method for online control optimization and is not limited to Gaussian distributions, similar to RAT iLQR. However, there are three major differences between PETS and RAT iLQR. First, PETS performs the cross entropy optimization directly in the high-dimensional control sequence space, which is far less sample efficient than RAT iLQR which uses the cross entropy method to only optimize the scalar risk-sensitivity parameter. Second, PETS does not consider feedback during planning as opposed to RAT iLQR. Third, PETS requires access to the exact ground-truth Gaussian mixture distribution to perform sampling, while RAT iLQR only relies on the KL divergence bound and the Gaussian distribution that we have modeled. We let PETS perform $M=5$ iterations of the cross entropy optimization, each with $25$ samples for the control sequence coupled with $50$ samples for the joint state trajectory prediction, which resulted in the average computation time of 0.67 [s].

We performed 30 runs of the simulation for each algorithm, with randomized pedestrian start positions and stochastic transitions. To measure the performance, we computed the minimum separation distance between the robot and the pedestrian in each run, assuming that the both agents are circular with the same diameter. The histogram plots presented in Figure 3 clearly indicates the failure of iLQG and PETS as well as RAT iLQR’s capability to maintain a sufficient safety margin for collision avoidance despite the distributional model mismatch. As summarized in Table I, RAT iLQR achieved the largest minimum separation distance on average with the smallest standard deviation, which contributed to safe robot navigation. Note that even iLQG had one collision under this large model mismatch. Figure 2 provides a qualitative explanation of this failure; the planned trajectories by iLQG tend to be much closer to the passing pedestrian than those by the risk-sensitive RAT iLQR. This difference is congruous with our earlier observations in prior work [7] where risk-sensitivity is shown to affect the global behavior of the robot.

We also performed two additional sets of 30 simulations for RAT iLQR, with two different ground-truth distributions that are closer to the model distribution having the estimated KL divergence of 7.78 and 1.34, respectively. This is to study how different KL bounds affect the optimal choice of the risk-sensitivity parameter $\theta^{*}$. The results are shown in Figure 4. As the KL bound increases from 1.34 to 32.02, the ratio between the optimal $\theta^{*}$ found by RAT iLQR to the maximum feasible $\theta$ also increases. This matches our intuition that the larger the model mismatch, the more risk-sensitive the robot becomes. However, we also note that the robot does not saturate $\theta$ all the time even under the largest KL bound of 32.02. This raises a fundamental question on the benefits of RAT iLQR as a risk-sensitive optimal control algorithm: how favorable is RAT iLQR with optimal $\theta^{*}$ compared to iLEQG with the highest risk-sensitivity?

To answer this question, we performed a comparative study between RAT iLQR with $\theta^{*}$ and iLEQG with $\theta_{\max}$ (i.e. maximum feasible $\theta$ found during the cross entropy sampling of RAT iLQR) under the same simulation setup as before. The results are reported in Table II. In terms of collision avoidance, both algorithms were equally safe with collision count of $0$. However, RAT iLQR achieved significantly more efficient robot navigation compared to iLEQG with $\theta_{\max}$, reducing the average tracking error by $39\%$, $34\%$, and $16\%$ for the KL values of $1.34$, $7.78$, and $32.02$, respectively. The efficiency and the safety of robot navigation are often in conflict in dynamic collision avoidance, and prior work [7] struggles to find the right balance by manually tuning a fixed $\theta$. With RAT iLQR, such need for manual tuning is eliminated since the algorithm dynamically adjusts $\theta$ so it is the most desirable to handle the potential model mismatch specified by the KL bound.