Consider a discrete-time stochastic control model for a robot, e.g., a manipulator moving in configuration space under uncertainty: state space \( \mathcal{X} \), control space \( \mathcal{U} \), transition kernel \( P(x' \mid x,u) \), per-stage cost \( c(x,u) \), and discount factor \( \gamma \in (0,1) \). For a stationary policy \( \pi: \mathcal{X} \to \mathcal{U} \), the standard (risk-neutral) discounted cost is

\[ J^{\pi}(x_0;P) = \mathbb{E}\!\left[ \sum_{t=0}^{\infty} \gamma^t c(x_t,u_t) \,\middle|\, x_0,\; u_t = \pi(x_t),\; x_{t+1} \sim P(\cdot \mid x_t,u_t) \right]. \]

In real robots, \( P \) and \( c \) are not known exactly: contact models are approximate, friction coefficients drift, and perception yields uncertain obstacle locations. Two broad strategies are:

• Robust planning: optimize performance under the worst model in an uncertainty set \( \mathcal{U} \).
• Risk-sensitive planning: keep the nominal model but replace expectation by a risk measure that penalizes variability and tail events (e.g., collision with small but non-negligible probability).

Denote by \( \mathcal{U} \) a set of admissible transition kernels around a nominal model \( P_0 \). A robust objective is

\[ J^{\star}_{\text{rob}}(x_0) = \inf_{\pi} \sup_{P \in \mathcal{U}} J^{\pi}(x_0;P), \]

whereas a risk-sensitive objective replaces the expectation by a risk functional \( \rho \):

\[ J^{\star}_{\rho}(x_0) = \inf_{\pi} \rho\!\left( \sum_{t=0}^{\infty} \gamma^t c(x_t,u_t) \right). \]

Robust planning is conservative (safe but possibly overly cautious) and often used when we know bounded sets of model errors. Risk-sensitive planning allows tuning the trade-off between expected performance and exposure to rare but catastrophic events.

flowchart TD
  S["Start: uncertain robot model"] --> M["Specify uncertainty or risk attitude"]
  M --> R1["Model set U for transitions/costs (robust)"]
  M --> R2["Risk measure rho on cost (risk-sensitive)"]
  R1 --> B1["Bellman: min over pi, max over P in U"]
  R2 --> B2["Bellman: min over pi with rho in stage cost"]
  B1 --> P1["Robust policy: safe in worst case"]
  B2 --> P2["Risk-sensitive policy: tuned to tails"]
        

A common robust formulation considers an uncertainty set for each state-control pair: \( \mathcal{U}(x,u) \subset \Delta(\mathcal{X}) \), where \( \Delta(\mathcal{X}) \) is the simplex of probability distributions on \( \mathcal{X} \). For example, for contact-rich motion planning, we might know that transition probabilities are within an interval around a nominal model.

The robust value function for a fixed policy \( \pi \) is

\[ V^{\pi}_{\text{rob}}(x) = \sup_{P \in \mathcal{U}} \mathbb{E}^{P,\pi}\!\left[ \sum_{t=0}^{\infty} \gamma^t c(x_t,u_t) \,\middle|\, x_0 = x \right]. \]

We focus on rectangular uncertainty sets (also called s,a-rectangular): the uncertainty in each \( P(\cdot \mid x,u) \) can be chosen independently across time and state-control pairs. Under this assumption, the robust Bellman operator is

\[ (T_{\text{rob}} V)(x) = \min_{u \in \mathcal{U}(x)} \max_{p \in \mathcal{U}(x,u)} \left\{ c(x,u) + \gamma \sum_{x' \in \mathcal{X}} p(x') V(x') \right\}. \]

The optimal robust value function \( V^{\star}_{\text{rob}} \) is the unique fixed point of \( T_{\text{rob}} \), i.e., \( V^{\star}_{\text{rob}} = T_{\text{rob}} V^{\star}_{\text{rob}} \).

2.1 Contraction Property (Sketch)

Assume that costs are bounded: \( |c(x,u)| \le C_{\max} \), and that \( \gamma \in (0,1) \). We work with the sup-norm \( \|V\|_{\infty} = \sup_{x} |V(x)| \).

Claim: \(T_{\text{rob}}\) is a \(\gamma\)-contraction in \(\|\cdot\|_{\infty}\).

Proof sketch:

\[ \begin{aligned} (T_{\text{rob}} V_1)(x) &= \min_u \max_{p \in \mathcal{U}(x,u)} \left\{ c(x,u) + \gamma \sum_{x'} p(x') V_1(x') \right\}, \\ (T_{\text{rob}} V_2)(x) &= \min_u \max_{p \in \mathcal{U}(x,u)} \left\{ c(x,u) + \gamma \sum_{x'} p(x') V_2(x') \right\}. \end{aligned} \]

For any fixed \(u,p\),

\[ \left| \gamma \sum_{x'} p(x') V_1(x') - \gamma \sum_{x'} p(x') V_2(x') \right| \le \gamma \|V_1 - V_2\|_{\infty}. \]

Taking max over \( p \) and min over \( u \) cannot increase the difference, so

\[ \big|(T_{\text{rob}} V_1)(x) - (T_{\text{rob}} V_2)(x)\big| \le \gamma \|V_1 - V_2\|_{\infty}. \]

Taking supremum over \( x \) yields \( \|T_{\text{rob}}V_1 - T_{\text{rob}}V_2\|_{\infty} \le \gamma \|V_1 - V_2\|_{\infty} \), hence the contraction property. Banach's fixed point theorem implies existence and uniqueness of \( V^{\star}_{\text{rob}} \) and global convergence of value iteration:

\[ V_{k+1} = T_{\text{rob}} V_k,\quad V_k \xrightarrow[k \to \infty]{} V^{\star}_{\text{rob}}. \]

In a robotic planning context, robust value iteration corresponds to repeatedly computing worst-case cost-to-go for each configuration given an uncertainty set over motion outcomes (e.g., bounded slippage or localization error).

Risk-sensitive planning replaces the expectation by a functional \( \rho \) that encodes an attitude to risk. For a random cumulative cost \( Z \), we require \( \rho(Z) \in \mathbb{R} \). Important classes:

• Variance-based risk: \( \mathbb{E}[Z] + \lambda \operatorname{Var}(Z) \).
• Entropic (exponential utility) risk.
• Coherent risk measures such as CVaR.

3.1 Entropic Risk

For parameter \( \lambda > 0 \) (risk aversion), define the entropic risk of a scalar random variable \( Z \) by

\[ \rho_{\lambda}(Z) = \frac{1}{\lambda} \log \mathbb{E}\left[ \exp\big(\lambda Z\big) \right]. \]

For small \( \lambda \), a Taylor expansion gives

\[ \rho_{\lambda}(Z) = \mathbb{E}[Z] + \frac{\lambda}{2} \operatorname{Var}(Z) + O(\lambda^2), \]

which shows that entropic risk penalizes variance in addition to mean cost.

In a Markov setting, define the risk-sensitive value function

\[ J^{\pi}_{\lambda}(x) = \frac{1}{\lambda} \log \mathbb{E}^{\pi}\!\left[ \exp\!\left( \lambda \sum_{t=0}^{\infty} \gamma^t c(x_t,u_t) \right) \,\middle|\, x_0 = x \right]. \]

Under standard assumptions (bounded costs, \(\gamma < 1\)), the optimal risk-sensitive value function satisfies the nonlinear Bellman equation

\[ V_{\lambda}(x) = \min_{u \in \mathcal{U}(x)} \frac{1}{\lambda} \log \mathbb{E}_{x' \sim P(\cdot \mid x,u)} \left[ \exp\!\big( \lambda\big( c(x,u) + \gamma V_{\lambda}(x') \big) \big) \right]. \]

This is more expensive to compute than the linear risk-neutral Bellman update, but is still amenable to value or policy iteration in moderate-size belief/state spaces.

3.2 CVaR (Conditional Value-at-Risk)

For a random cost \( Z \) and confidence \( \alpha \in (0,1) \), define the Value-at-Risk (quantile) and CVaR as

\[ \operatorname{VaR}_{\alpha}(Z) = \inf\{z \in \mathbb{R} : \mathbb{P}(Z \le z) \ge \alpha\}, \]

\[ \operatorname{CVaR}_{\alpha}(Z) = \inf_{t \in \mathbb{R}} \left\{ t + \frac{1}{1-\alpha} \mathbb{E}\big[(Z - t)_+\big] \right\}, \]

where \( (x)_+ = \max(x,0) \). CVaR is coherent (it satisfies properties like monotonicity and subadditivity) and focuses on the tail of the distribution of \( Z \). In a robotic context, \( Z \) might be a measure of accumulated collision cost; CVaR penalizes high-cost (dangerous) trajectories even if their probability is small.

Robust and risk-sensitive planning are tightly related. Under suitable conditions, risk-sensitive objectives can be interpreted as robust optimization with respect to a set of plausible models.

4.1 KL-Divergence Uncertainty and Entropic Risk

Consider a nominal distribution \( P_0 \) and an uncertainty set defined by a Kullback–Leibler (KL) ball:

\[ \mathcal{P}_{\delta} = \left\{ P : D_{\mathrm{KL}}(P \,\|\, P_0) \le \delta \right\}, \]

where \( D_{\mathrm{KL}}(P \,\|\, P_0) = \sum_{\omega} P(\omega) \log \frac{P(\omega)}{P_0(\omega)} \).

A classical duality result shows that for suitable \( \lambda > 0 \) and \( \delta \):

\[ \sup_{P \in \mathcal{P}_{\delta}} \mathbb{E}_P[Z] = \inf_{\lambda > 0} \left\{ \frac{1}{\lambda} \log \mathbb{E}_{P_0}\left[\exp\big(\lambda Z\big)\right] + \frac{\delta}{\lambda} \right\}. \]

Thus maximizing worst-case expected cost over a KL ball is equivalent (up to constants) to minimizing an entropic risk-sensitive objective under the nominal model \( P_0 \). In robust MDPs with KL uncertainty, the robust Bellman operator becomes equivalent to a risk-sensitive Bellman operator with entropic risk.

4.2 Interpretation for Robotics

For a high-DOF manipulator planning in belief-space, one can:

• Use robust planning when bounds on model errors (e.g., worst-case pose error, bounded slip) are available and a conservative safety guarantee is desired.
• Use risk-sensitive planning when a probabilistic model is available but the designer wants to tune aversion to tail events without fully switching to worst-case reasoning.

In both cases, the policy computation can be performed via modified dynamic programming on discretized state/belief spaces, then embedded into higher-level planners (e.g., sampling-based planners where cost-to-go is evaluated via robust or risk-sensitive value functions).

flowchart TD
  N["Nominal model P0"] --> U1["Define bounded set U (L_inf, intervals)"]
  N --> U2["Define divergence ball {P: KL(P||P0)<=delta}"]
  U1 --> RB["Robust min-max Bellman"]
  U2 --> RS["Entropic risk-sensitive Bellman"]
  RB --> POL1["Conservative policy (safety-critical tasks)"]
  RS --> POL2["Tunable risk policy (tradeoff performance/safety)"]
        

For small or moderately sized discretizations (e.g., low-dimensional belief grids or coarse configuration lattices), we can directly apply value iteration or policy iteration with modified Bellman updates.

5.1 Robust Value Iteration with Interval Uncertainty

Suppose for each \( (x,u) \) we know an interval uncertainty for each successor state: \( p_{\min}(x' \mid x,u) \le p(x' \mid x,u) \le p_{\max}(x' \mid x,u) \), and the set of admissible \( p \) is

\[ \mathcal{U}(x,u) = \left\{ p \in \Delta(\mathcal{X}) : p_{\min}(x') \le p(x') \le p_{\max}(x')\;\;\forall x' \right\}. \]

For a fixed value function \( V \), the inner max over \( p \) in the robust Bellman operator reduces to a linear program:

\[ \max_{p} \sum_{x'} p(x') V(x') \quad \text{s.t.}\quad \sum_{x'} p(x') = 1,\; p_{\min}(x') \le p(x') \le p_{\max}(x'). \]

Since the objective is linear, the maximizer pushes probability mass toward states with larger \( V(x') \) within the allowed bounds. In finite state spaces, this can be computed by sorting successors by \( V(x') \) and saturating upper bounds.

5.2 Risk-Sensitive Value Iteration (Entropic)

For entropic risk, the Bellman update at \( x \) becomes

\[ (T_{\lambda} V)(x) = \min_{u} \frac{1}{\lambda} \log \sum_{x'} P(x' \mid x,u) \exp\!\Big( \lambda\big(c(x,u) + \gamma V(x')\big) \Big). \]

This preserves the contraction property in the sup-norm under suitable boundedness conditions. The resulting policies prefer actions whose successor cost distributions have lighter tails, reducing, for example, collision probability in cluttered environments at the cost of potentially longer paths.

For large-scale robotic planners (e.g., sampling-based planners over high-dimensional configuration spaces), these dynamic programming operators can be applied on local abstractions (e.g. motion primitive lattices) or used to compute cost-to-go heuristics guiding search.

We illustrate robust and risk-sensitive planning on a small grid world representing a planar robot in a discretized workspace. Each cell is a state, controls are four motion primitives, and there is stochastic slippage that we model with interval uncertainty.

import numpy as np

# Small grid world: states 0,...,N-1
# For robotics, think of states as coarse cells in C-space or x-y grid.
N_STATES = 5
N_ACTIONS = 2  # e.g. "left" and "right" primitives

gamma = 0.95
lambda_risk = 0.5  # entropic risk aversion

# Nominal transitions P0[s, a, s']
P0 = np.zeros((N_STATES, N_ACTIONS, N_STATES))
# Simple line chain dynamics:
for s in range(N_STATES):
    for a in range(N_ACTIONS):
        # a==0: left, a==1: right
        if a == 0:
            s_next = max(0, s - 1)
        else:
            s_next = min(N_STATES - 1, s + 1)
        P0[s, a, s_next] = 1.0

# Interval uncertainty around P0: allow some slip probability to neighbors
eps = 0.2
P_min = np.maximum(0.0, P0 - eps)
P_max = np.minimum(1.0, P0 + eps)

# Stage cost: penalize being away from goal (state N_STATES-1)
goal = N_STATES - 1
c = np.zeros((N_STATES, N_ACTIONS))
for s in range(N_STATES):
    dist = abs(goal - s)
    c[s, :] = dist  # same cost for both actions here

def robust_inner_max(V, s, a):
    """
    Compute max_p sum_s' p(s') V(s')
    subject to sum p = 1, P_min <= p <= P_max.
    Here we implement a greedy saturating algorithm since the LP is small.
    """
    v = V.copy()
    # Sort states by value descending
    order = np.argsort(-v)
    p = np.zeros_like(v)
    remaining = 1.0
    for s_prime in order:
        # allocate as much probability as possible up to P_max
        max_allow = min(P_max[s, a, s_prime], remaining)
        min_allow = P_min[s, a, s_prime]
        # ensure at least min_allow
        alloc = max(min_allow, max_allow)
        alloc = min(alloc, remaining)
        p[s_prime] = alloc
        remaining -= alloc
        if remaining <= 1e-12:
            break
    # If remaining > 0, distribute uniformly over free states within bounds
    if remaining > 1e-12:
        free = [idx for idx in range(N_STATES) if p[idx] < P_max[s, a, idx] - 1e-12]
        if free:
            extra = remaining / len(free)
            for idx in free:
                p[idx] = min(P_max[s, a, idx], p[idx] + extra)
    return float(np.dot(p, V))

def value_iteration_robust(max_iter=200, tol=1e-6):
    V = np.zeros(N_STATES)
    for it in range(max_iter):
        V_new = np.zeros_like(V)
        for s in range(N_STATES):
            q_vals = []
            for a in range(N_ACTIONS):
                worst = robust_inner_max(V, s, a)
                q_vals.append(c[s, a] + gamma * worst)
            V_new[s] = min(q_vals)
        if np.max(np.abs(V_new - V)) < tol:
            break
        V = V_new
    # greedy policy
    pi = np.zeros(N_STATES, dtype=int)
    for s in range(N_STATES):
        q_vals = []
        for a in range(N_ACTIONS):
            worst = robust_inner_max(V, s, a)
            q_vals.append(c[s, a] + gamma * worst)
        pi[s] = int(np.argmin(q_vals))
    return V, pi

def value_iteration_entropic(max_iter=200, tol=1e-6):
    V = np.zeros(N_STATES)
    for it in range(max_iter):
        V_new = np.zeros_like(V)
        for s in range(N_STATES):
            q_vals = []
            for a in range(N_ACTIONS):
                # Entropic risk-sensitive Bellman update under nominal P0
                exp_term = 0.0
                for s_prime in range(N_STATES):
                    if P0[s, a, s_prime] > 0.0:
                        exp_term += P0[s, a, s_prime] * np.exp(
                            lambda_risk * (c[s, a] + gamma * V[s_prime])
                        )
                q_vals.append((1.0 / lambda_risk) * np.log(exp_term))
            V_new[s] = min(q_vals)
        if np.max(np.abs(V_new - V)) < tol:
            break
        V = V_new
    pi = np.zeros(N_STATES, dtype=int)
    for s in range(N_STATES):
        q_vals = []
        for a in range(N_ACTIONS):
            exp_term = 0.0
            for s_prime in range(N_STATES):
                if P0[s, a, s_prime] > 0.0:
                    exp_term += P0[s, a, s_prime] * np.exp(
                        lambda_risk * (c[s, a] + gamma * V[s_prime])
                    )
            q_vals.append((1.0 / lambda_risk) * np.log(exp_term))
        pi[s] = int(np.argmin(q_vals))
    return V, pi

if __name__ == "__main__":
    V_rob, pi_rob = value_iteration_robust()
    V_risk, pi_risk = value_iteration_entropic()
    print("Robust value:", V_rob)
    print("Robust policy:", pi_rob)
    print("Risk-sensitive value:", V_risk)
    print("Risk-sensitive policy:", pi_risk)
      

In a robotics framework such as OMPL (C++ with Python bindings), these dynamic programming computations can be used to assign robust/risk-sensitive costs to motion primitives in a state lattice, guiding the planner away from states or actions with poorly modeled dynamics or high tail risk.

The following C++-style sketch shows how one could integrate robust value iteration into a motion-primitive-based planner (e.g., an OMPL-based lattice planner). For brevity, we use simple arrays.

#include <iostream>
#include <vector>
#include <cmath>

struct RobustMDP {
    int nStates;
    int nActions;
    double gamma;
    // Costs: c[s][a]
    std::vector<std::vector<double> > cost;
    // Interval uncertainty: pMin[s][a][s'], pMax[s][a][s']
    std::vector<std::vector<std::vector<double> > > pMin, pMax;

    RobustMDP(int nS, int nA, double g)
        : nStates(nS), nActions(nA), gamma(g) {
        cost.assign(nStates, std::vector<double>(nActions, 0.0));
        pMin.assign(nStates,
                    std::vector<std::vector<double> >(
                        nActions, std::vector<double>(nStates, 0.0)));
        pMax = pMin;
    }

    double innerWorst(const std::vector<double> &V, int s, int a) const {
        // Greedy saturating allocation
        std::vector<double> v = V;
        std::vector<int> order(nStates);
        for (int i = 0; i < nStates; ++i) order[i] = i;
        std::sort(order.begin(), order.end(),
                  [&](int i, int j) { return v[i] > v[j]; });

        std::vector<double> p(nStates, 0.0);
        double remaining = 1.0;
        for (int idx = 0; idx < nStates; ++idx) {
            int sp = order[idx];
            double maxAllow = std::min(pMax[s][a][sp], remaining);
            double minAllow = pMin[s][a][sp];
            double alloc = std::max(minAllow, maxAllow);
            alloc = std::min(alloc, remaining);
            p[sp] = alloc;
            remaining -= alloc;
            if (remaining <= 1e-12) break;
        }
        if (remaining > 1e-12) {
            // distribute leftover uniformly among non-saturated states
            std::vector<int> freeIdx;
            for (int sp = 0; sp < nStates; ++sp) {
                if (p[sp] < pMax[s][a][sp] - 1e-12) {
                    freeIdx.push_back(sp);
                }
            }
            if (!freeIdx.empty()) {
                double extra = remaining / double(freeIdx.size());
                for (int sp : freeIdx) {
                    p[sp] = std::min(pMax[s][a][sp], p[sp] + extra);
                }
            }
        }
        double val = 0.0;
        for (int sp = 0; sp < nStates; ++sp) {
            val += p[sp] * V[sp];
        }
        return val;
    }

    void valueIteration(std::vector<double> &V, std::vector<int> &policy,
                        int maxIter = 200, double tol = 1e-6) const {
        V.assign(nStates, 0.0);
        std::vector<double> Vnew(nStates);
        for (int it = 0; it < maxIter; ++it) {
            double diff = 0.0;
            for (int s = 0; s < nStates; ++s) {
                double best = 1e300;
                for (int a = 0; a < nActions; ++a) {
                    double worst = innerWorst(V, s, a);
                    double q = cost[s][a] + gamma * worst;
                    if (q < best) best = q;
                }
                Vnew[s] = best;
                diff = std::max(diff, std::fabs(Vnew[s] - V[s]));
            }
            V = Vnew;
            if (diff < tol) break;
        }
        policy.assign(nStates, 0);
        for (int s = 0; s < nStates; ++s) {
            double best = 1e300;
            int bestA = 0;
            for (int a = 0; a < nActions; ++a) {
                double worst = innerWorst(V, s, a);
                double q = cost[s][a] + gamma * worst;
                if (q < best) { best = q; bestA = a; }
            }
            policy[s] = bestA;
        }
    }
};

int main() {
    RobustMDP mdp(5, 2, 0.95);
    // Fill cost, pMin, pMax from robot motion primitive model...
    std::vector<double> V;
    std::vector<int> policy;
    mdp.valueIteration(V, policy);
    std::cout << "Robust value function:" << std::endl;
    for (double v : V) std::cout << v << " ";
    std::cout << std::endl;
    return 0;
}
      

This core can be embedded in an OMPL-based pipeline by treating each lattice node as a state and motion primitive as an action, with interval bounds derived from scenario-based simulation (e.g., varying friction or payload).

Java implementations are often used in higher-level robotics middleware or simulation environments. Below is a minimal entropic risk-sensitive value iteration routine using arrays.

public class RiskSensitiveMDP {
    private int nStates;
    private int nActions;
    private double gamma;
    private double lambdaRisk;
    // P[s][a][s']
    private double[][][] P;
    private double[][] cost;

    public RiskSensitiveMDP(int nStates, int nActions, double gamma, double lambdaRisk) {
        this.nStates = nStates;
        this.nActions = nActions;
        this.gamma = gamma;
        this.lambdaRisk = lambdaRisk;
        this.P = new double[nStates][nActions][nStates];
        this.cost = new double[nStates][nActions];
    }

    public void setTransition(int s, int a, int sp, double prob) {
        P[s][a][sp] = prob;
    }

    public void setCost(int s, int a, double c) {
        cost[s][a] = c;
    }

    public void valueIteration(double[] V, int[] policy, int maxIter, double tol) {
        double[] Vnew = new double[nStates];
        for (int s = 0; s < nStates; ++s) V[s] = 0.0;

        for (int it = 0; it < maxIter; ++it) {
            double diff = 0.0;
            for (int s = 0; s < nStates; ++s) {
                double best = Double.POSITIVE_INFINITY;
                for (int a = 0; a < nActions; ++a) {
                    double expTerm = 0.0;
                    for (int sp = 0; sp < nStates; ++sp) {
                        double p = P[s][a][sp];
                        if (p > 0.0) {
                            expTerm += p * Math.exp(
                                lambdaRisk * (cost[s][a] + gamma * V[sp])
                            );
                        }
                    }
                    double q = (1.0 / lambdaRisk) * Math.log(expTerm);
                    if (q < best) best = q;
                }
                Vnew[s] = best;
                diff = Math.max(diff, Math.abs(Vnew[s] - V[s]));
            }
            System.arraycopy(Vnew, 0, V, 0, nStates);
            if (diff < tol) break;
        }

        for (int s = 0; s < nStates; ++s) {
            double best = Double.POSITIVE_INFINITY;
            int bestA = 0;
            for (int a = 0; a < nActions; ++a) {
                double expTerm = 0.0;
                for (int sp = 0; sp < nStates; ++sp) {
                    double p = P[s][a][sp];
                    if (p > 0.0) {
                        expTerm += p * Math.exp(
                            lambdaRisk * (cost[s][a] + gamma * V[sp])
                        );
                    }
                }
                double q = (1.0 / lambdaRisk) * Math.log(expTerm);
                if (q < best) { best = q; bestA = a; }
            }
            policy[s] = bestA;
        }
    }
}
      

This core routine can be used as a component in Java-based robot frameworks to compute risk-sensitive policies over abstract task graphs or state lattices.

MATLAB is widely used for prototyping robust and risk-sensitive controllers. One common pattern is to compute a robust (or risk-sensitive) policy offline in MATLAB and then deploy it as a lookup table or function block in Simulink.

function [V, policy] = robust_value_iteration(Pmin, Pmax, cost, gamma, tol, maxIter)
% Pmin, Pmax: S x A x S arrays (interval transitions)
% cost: S x A matrix
% gamma: discount factor

if nargin < 5, tol = 1e-6; end
if nargin < 6, maxIter = 200; end

[S, A, ~] = size(Pmin);
V = zeros(S, 1);
Vnew = zeros(S, 1);

for it = 1:maxIter
    diff = 0;
    for s = 1:S
        best = inf;
        for a = 1:A
            % inner worst-case expectation
            v = V;
            [~, order] = sort(-v);  % descending
            p = zeros(S, 1);
            remaining = 1.0;
            for idx = 1:S
                sp = order(idx);
                maxAllow = min(Pmax(s, a, sp), remaining);
                minAllow = Pmin(s, a, sp);
                alloc = max(minAllow, maxAllow);
                alloc = min(alloc, remaining);
                p(sp) = alloc;
                remaining = remaining - alloc;
                if remaining <= 1e-12
                    break;
                end
            end
            if remaining > 1e-12
                free = find(p < squeeze(Pmax(s, a, :)) - 1e-12);
                if ~isempty(free)
                    extra = remaining / numel(free);
                    for k = 1:numel(free)
                        sp = free(k);
                        p(sp) = min(Pmax(s, a, sp), p(sp) + extra);
                    end
                end
            end
            worst = p' * V;
            q = cost(s, a) + gamma * worst;
            if q < best
                best = q;
            end
        end
        Vnew(s) = best;
        diff = max(diff, abs(Vnew(s) - V(s)));
    end
    V = Vnew;
    if diff < tol
        break;
    end
end

policy = zeros(S, 1);
for s = 1:S
    best = inf;
    bestA = 1;
    for a = 1:A
        v = V;
        [~, order] = sort(-v);
        p = zeros(S, 1);
        remaining = 1.0;
        for idx = 1:S
            sp = order(idx);
            maxAllow = min(Pmax(s, a, sp), remaining);
            minAllow = Pmin(s, a, sp);
            alloc = max(minAllow, maxAllow);
            alloc = min(alloc, remaining);
            p(sp) = alloc;
            remaining = remaining - alloc;
            if remaining <= 1e-12
                break;
            end
        end
        if remaining > 1e-12
            free = find(p < squeeze(Pmax(s, a, :)) - 1e-12);
            if ~isempty(free)
                extra = remaining / numel(free);
                for k = 1:numel(free)
                    sp = free(k);
                    p(sp) = min(Pmax(s, a, sp), p(sp) + extra);
                end
            end
        end
        worst = p' * V;
        q = cost(s, a) + gamma * worst;
        if q < best
            best = q;
            bestA = a;
        end
    end
    policy(s) = bestA;
end
end
      

In Simulink, one can use a MATLAB Function block that reads the current discretized state (or belief), looks up the precomputed robust action from policy, and drives a lower-level trajectory-tracking controller.

Mathematica is convenient for symbolic checks and prototyping of dynamic programming operators. Below is a simple entropic risk-sensitive value iteration for a finite MDP with given transition tensor P and cost matrix C.

gamma = 0.95;
lambdaRisk = 0.5;
nStates = 5;
nActions = 2;

(* P[[s, a, sp]] and C[[s, a]] must be defined externally *)

riskBellmanStep[V_] := Module[{Vnew, s, a, sp, expTerm, qvals},
  Vnew = Table[0., {nStates}];
  For[s = 1, s <= nStates, s++,
    qvals = Table[
      expTerm = Sum[
        P[[s, a, sp]] *
          Exp[lambdaRisk * (C[[s, a]] + gamma * V[[sp]])],
        {sp, 1, nStates}
      ];
      (1.0/lambdaRisk) * Log[expTerm],
      {a, 1, nActions}
    ];
    Vnew[[s]] = Min[qvals];
  ];
  Vnew
];

valueIterationRisk[imax_Integer : 200, tol_ : 10.^-6] :=
 Module[{V = ConstantArray[0., nStates], Vnew, k = 0, diff = 0., policy},
  While[k < imax,
    Vnew = riskBellmanStep[V];
    diff = Max[Abs[Vnew - V]];
    V = Vnew;
    k++;
    If[diff < tol, Break[]];
  ];
  policy = Table[
    First@
      Ordering[
        Table[
          Module[{expTerm},
            expTerm = Sum[
              P[[s, a, sp]] *
                Exp[lambdaRisk * (C[[s, a]] + gamma * V[[sp]])],
              {sp, 1, nStates}
            ];
            (1.0/lambdaRisk) * Log[expTerm]
          ],
          {a, 1, nActions}
        ]
      ],
    {s, 1, nStates}
  ];
  <<{"Value" -> V, "Policy" -> policy}
];

res = valueIterationRisk[];
res["Value"]
      

Symbolic tools can also be used to verify contraction properties and to explore limiting behaviors as \( \lambda \to 0 \) (risk-neutral limit) or \( \lambda \to \infty \) (increasingly conservative behavior).

Problem 1 (Contraction of Robust Bellman Operator): Let \( T_{\text{rob}} \) be the robust Bellman operator from Section 2 with bounded costs and \( \gamma \in (0,1) \). Prove rigorously that \( T_{\text{rob}} \) is a \(\gamma\)-contraction in the sup-norm.

Solution:

For any two functions \( V_1,V_2 : \mathcal{X} \to \mathbb{R} \), and a fixed state \( x \),

\[ \begin{aligned} (T_{\text{rob}} V_1)(x) &= \min_{u} \max_{p \in \mathcal{U}(x,u)} \left\{ c(x,u) + \gamma \sum_{x'} p(x') V_1(x') \right\}, \\ (T_{\text{rob}} V_2)(x) &= \min_{u} \max_{p \in \mathcal{U}(x,u)} \left\{ c(x,u) + \gamma \sum_{x'} p(x') V_2(x') \right\}. \end{aligned} \]

Fix an action \( u \) and distribution \( p \). Then

\[ \gamma \left| \sum_{x'} p(x') V_1(x') - \sum_{x'} p(x') V_2(x') \right| \le \gamma \|V_1 - V_2\|_{\infty}. \]

Taking max over \( p \) on both sides and then min over \( u \) cannot increase the left-hand side (as each operation corresponds to taking extrema over subsets of possible values), hence

\[ \big|(T_{\text{rob}}V_1)(x) - (T_{\text{rob}}V_2)(x)\big| \le \gamma \|V_1 - V_2\|_{\infty}. \]

Taking supremum over \( x \) yields \( \|T_{\text{rob}}V_1 - T_{\text{rob}}V_2\|_{\infty} \le \gamma \|V_1 - V_2\|_{\infty} \). Thus \( T_{\text{rob}} \) is a contraction, implying a unique fixed point and global convergence of value iteration.

Problem 2 (Risk-Neutral Limit of Entropic Risk): Let \( Z \) be a square-integrable random variable. Show that \( \lim_{\lambda \to 0} \rho_{\lambda}(Z) = \mathbb{E}[Z] \), where \( \rho_{\lambda}(Z) = \frac{1}{\lambda} \log \mathbb{E}[e^{\lambda Z}] \).

Solution:

Use the expansion \( e^{\lambda Z} = 1 + \lambda Z + \frac{\lambda^2 Z^2}{2} + o(\lambda^2) \). Then

\[ \mathbb{E}[e^{\lambda Z}] = 1 + \lambda \mathbb{E}[Z] + \frac{\lambda^2}{2} \mathbb{E}[Z^2] + o(\lambda^2). \]

Since \( \log(1 + x) = x - \frac{x^2}{2} + o(x^2) \) as \( x \to 0 \), take \( x = \lambda \mathbb{E}[Z] + O(\lambda^2) \) to obtain

\[ \log \mathbb{E}[e^{\lambda Z}] = \lambda \mathbb{E}[Z] + O(\lambda^2). \]

Dividing by \( \lambda \) and letting \( \lambda \to 0 \) gives \( \rho_{\lambda}(Z) \to \mathbb{E}[Z] \). Hence entropic risk converges to the risk-neutral expectation as risk aversion vanishes.

Problem 3 (CVaR of a Two-Point Cost): Let \( Z \) take value \( z_1 \) with probability \( p \) and \( z_2 > z_1 \) with probability \( 1-p \). Compute \( \operatorname{CVaR}_{\alpha}(Z) \) for \( \alpha \in (0,1) \).

Solution:

• If \( \alpha \le p \), then the \( \alpha \)-quantile lies at \( z_1 \), but the upper tail events beyond this quantile are \( \{Z = z_2\} \), so the tail above \( \alpha \) has cost \( z_2 \) only. Hence \( \operatorname{CVaR}_{\alpha}(Z) = z_2 \).
• If \( \alpha > p \), then the quantile lies at \( z_2 \), and the upper tail conditional on \( Z \ge \operatorname{VaR}_{\alpha} \) is again \( Z = z_2 \). Thus \( \operatorname{CVaR}_{\alpha}(Z) = z_2 \).

Hence for a two-point distribution with \( z_2 > z_1 \), CVaR equals the worse cost \( z_2 \) for all \( \alpha \in (0,1) \), which highlights its conservativeness when there is a single worst outcome.

Problem 4 (Robust vs Risk-Sensitive Policy Monotonicity): Suppose a robot has two actions at a state \(x\): \(u_1\) and \(u_2\). Under the nominal model \(P_0\), \(u_1\) has lower expected cost but heavier tail than \(u_2\). Argue qualitatively how the optimal policy changes as the risk parameter \( \lambda \) in entropic risk increases.

Solution:

For small \( \lambda \), \( \rho_{\lambda}(Z) \approx \mathbb{E}[Z] + \frac{\lambda}{2} \operatorname{Var}(Z) \), so the action with lower expected cost \( u_1 \) is preferred unless its variance is significantly larger. As \( \lambda \) increases, higher moments (tails) of the distribution become more important. If \( u_1 \) has much heavier tail risk (e.g., small probability of catastrophic collision), then for sufficiently large \( \lambda \), \( u_2 \) becomes optimal despite higher expected cost, effectively emulating a robust worst-case preference.

In this lesson we formulated robust and risk-sensitive planning for robots operating under model and sensing uncertainty. Robust planning optimizes worst-case cost over a set of models and leads to min-max Bellman equations whose operators remain contractive under rectangular uncertainty. Risk-sensitive planning introduces risk measures such as entropic risk and CVaR, resulting in nonlinear Bellman equations that trade off expected performance and tail behavior. Duality results show deep connections between robustness and risk, especially with KL-divergence-based uncertainty sets. We also sketched implementations in Python, C++, Java, MATLAB/Simulink, and Mathematica suitable for embedding into higher-level robotic motion planners.