Diffeomorphic Safety Tubes for Stochastic Nonlinear Control

A robot planning a path under uncertainty needs to know where it might end up. The common approach draws a "tube" around the planned path, meaning a region that should contain the true state with high probability. The planner then keeps that tube clear of obstacles. For linear systems the tube can be a ball or an ellipsoid, and it stays that shape over time. For nonlinear systems such as a quadrotor banking hard through trees, the set of places the robot might be stretches into curved, lopsided shapes. A ball that covers such a set is either wastefully large or unsafe.

We propose to represent each cross-section of the tube as a warped ball. The warp is a learned diffeomorphism: a smooth, invertible map that cannot fold, tear, or glue space. It is built with the same velocity-field integration used in medical image registration. Because the map is invertible, checking whether a point is inside the tube, or whether the tube hits an obstacle, reduces to a test on the ball or on its mapped boundary. Because the map cannot fold, that boundary test is valid. A split-conformal calibration step then gives the tube a finite-sample coverage level. The disturbance model that generates training rollouts is written in Stratonovich form. We treat that choice as a testable modeling claim, not as decoration.

Core Claim

The claim has three parts. First, tubes whose cross-sections are images of a ball under a learned, fold-free diffeomorphism reach a target coverage (for example 95%) with smaller volume than ellipsoidal or constant-radius tubes. Second, they have zero boundary self-intersections, which pointwise learned quantile tubes cannot promise. Third, this holds for nonlinear, non-feedback-linearizable closed loops, at a per-step cost compatible with sequential convex model predictive control (MPC). MPC here means a controller that re-plans a short horizon at every step.

A secondary claim concerns the disturbance model. Real disturbances such as wind gusts and rotor thrust fluctuations are smooth in time and often scale with the state. For such disturbances the correct white-noise idealization is the Stratonovich one: the Wong–Zakai limit of smooth noise. An Itô model with the same noise fields adds a spurious drift. That drift should show up as miscalibrated tubes when noise is multiplicative, for example torque noise acting on attitude in $\mathrm{SO}(3)$, the group of 3-D rotations. If this effect turns out negligible in practice, the proposal stands on its first claim alone.

Where This Sits

Tube MPC for nonlinear systems. Dynamic Tube MPC for Nonlinear Systems lets tube thickness vary with controller bandwidth, pinching near obstacles and widening in free space. It keeps a symmetric cross-section, assumes worst-case bounded disturbances, and requires feedback-linearizable or minimum-phase dynamics. We keep its idea of an optimizable, state-dependent tube but let the cross-section take general shapes. We also drop the linearizability assumption by learning the tube from closed-loop rollouts under any ancillary controller.

Learned tubes. Deep Learning Tubes for Tube MPC learns asymmetric, non-Gaussian tube boundaries by quantile regression on a 12-D quadrotor. A negative-divergence loss keeps the tubes monotone. Its boundaries are per-dimension quantiles, not a map of the state space, so nothing prevents the implied boundary from self-intersecting under aggressive rotation. This is the method we must beat.

Linear and distributional tubes. Wasserstein Tube MPC with Exact Uncertainty Propagation propagates distributional uncertainty exactly for linear systems. It motivates our work by showing that exactness is lost once dynamics are nonlinear. It also constrains our design by showing that pure sample-average approaches are fragile under distribution shift, which is why we calibrate rather than simply fit.

Diffeomorphic maps. Our tube representation reuses the method of image registration. Unsupervised learning of probabilistic diffeomorphic registration for images and surfaces integrates a stationary velocity field by scaling-and-squaring, yielding maps that are invertible in the continuous limit. On brain MRI it reports on average 0.2 folding voxels, against thousands for unconstrained networks. Coupling-based Invertible Neural Networks Are Universal Diffeomorphism Approximators offers a competing invertible parameterization, which we test as an ablation.

Stratonovich geometric noise. A Geometric Framework for Stochastic Shape Analysis is the closest prior link between Stratonovich noise and diffeomorphic flows. It injects Stratonovich transport noise into shape-deforming velocity fields. The work is open-loop and descriptive, and its moment-matching inference scales quadratically in landmark count. Neural Hamiltonian Diffusions for Modeling Structured Geometric Dynamics learns Stratonovich dynamics on manifolds but does no control. The argument that Itô corrections distort geometric structure comes from Stochastic Hamiltonian dynamical systems. We use none of these for control synthesis directly. They supply the disturbance-modeling choice we test.

Research Program

The tube map. The tube at horizon step $t$ is defined as

$$\mathcal{T}_t = \{\, x : \lVert \phi_{\theta}^{-1}(x; \bar{x}_t, t) - \bar{x}_t \rVert \le r_t \,\}.$$

Here $\bar{x}_t$ is the nominal planned state, and $\phi_\theta$ is a diffeomorphism conditioned on $\bar{x}_t$ and $t$. The map is the exponential of a small network's velocity field, computed by scaling-and-squaring. Its inverse is the same computation run on the negated field. The radius $r_t$ is set by split-conformal calibration: $r_t$ is the $\lceil (n+1)(1-\delta)\rceil$-th smallest score $\lVert \phi_\theta^{-1}(x^{(i)}_t) - \bar{x}_t\rVert$ over $n$ held-out closed-loop rollouts. This gives marginal coverage of at least $1-\delta$ at each step whenever the held-out rollouts are exchangeable with deployment. That caveat is real under closed-loop distribution shift, and we measure it rather than assume it away.

Fitting. The map is trained to minimize the log-volume of $\mathcal{T}_t$, computed from the ball volume and the log-Jacobian determinant of the flow. It is trained jointly with a soft coverage loss on training rollouts. For attitude states the velocity field lives in the Lie algebra and is exponentiated on the group, so tubes stay on $\mathrm{SO}(3)$ rather than leaking into $\mathbb{R}^9$.

Disturbance model. Rollouts come from the nonlinear model driven by disturbances of the form $\sum_i g_i(x)\circ dW^i$. We simulate them with a geometric integrator whose midpoint rule matches the Stratonovich interpretation. The Itô twin uses identical $g_i$.

Planning. The obstacle constraint samples $K$ points on the unit sphere and maps them through $\phi_\theta$. It then requires each mapped point to have nonnegative signed distance to every obstacle. Invertibility guarantees that the mapped sphere is the true boundary of $\mathcal{T}_t$. With no folds, the region inside that boundary is exactly the tube, so clearing the boundary clears the tube for convex obstacles. For non-convex obstacles we add an interior check through $\phi^{-1}$. The constraint is differentiable in $\bar{x}_t$ and drops into the sequential convex programming loop of Lopez et al.

Baselines

Do-nothing reference. The naive reference is a constant-radius ball tube: nominal MPC with a fixed safety margin tuned by grid search to reach the target coverage. It tells us how much any shape adaptation buys.

Ellipsoidal tubes. The first structured baseline is an ellipsoidal tube whose shape matrix is fitted to the same rollouts, with a radius calibrated by the same conformal step. This isolates the gain from non-convex shape over the best convex shape.

Dynamic Tube MPC. We run Dynamic Tube MPC for Nonlinear Systems on its own double-integrator-with-drag benchmark. There it reports 0 tube violations in a 1,000-run Monte Carlo simulation and convergence in 3 to 4 sequential convex iterations. We match its disturbance bounds, then add colored multiplicative noise that its worst-case bound does not model.

Deep quantile tubes. The strongest competitor is Deep Learning Tubes for Tube MPC. On its 12-D quadrotor obstacle forest it reports maintaining a 95% probability of constraint satisfaction, with safe navigation from as few as 10 training episodes. It is the only method in the file that already learns non-convex, asymmetric tubes on a nonlinear system. We reimplement it in the Flightmare simulator (Flightmare) with identical rollout budgets (10, 50, and 200 episodes), the same ancillary controller, the same MPC solver, and equal hyperparameter search trials. We also apply the same conformal calibration to its output so that both methods target identical coverage.

Beating the strongest baseline. Beating it means, at matched empirical coverage of at least 95%, some combination of the following: statistically lower mean tube volume, higher success in the narrowest passages, zero boundary self-intersections where it shows some, and per-step solve time within a factor of two.

Ablations. Three ablations isolate the key ingredients. The first replaces the velocity-field map with an unconstrained displacement network $x + u(x)$, to measure how often folding breaks the boundary test. The second replaces it with a coupling invertible network, testing whether any invertible map suffices. The third swaps the Stratonovich disturbance simulator for its Itô twin.

Why not just…?

Why not just take the convex hull, or a union of ellipsoids, of sampled rollouts and calibrate it? This scenario approach is simple and non-parametric, and it is a real threat when states are low-dimensional. It is poor at runtime, because the hull must be recomputed for every candidate nominal trajectory inside the optimizer and it is not differentiable. ABC-LMPC already concedes that sample-based optimization strains high-frequency control. It is also poor at shape, because a convex hull cannot represent the curved, banana-shaped sets that rotation produces. We include a calibrated hull tube, evaluated offline at the same coverage. If its volume matches ours on the quadrotor, the case for learned warps reduces to speed and differentiability.

Why not just use deep quantile tubes, which already work? Fan et al. do reach 95% satisfaction. Their boundaries, however, are independent per-output quantiles, so inside-versus-outside is ill-defined wherever the implied surface crosses itself. We measure the self-intersection rate of their boundaries during high-angular-rate maneuvers. If that rate is essentially zero and their volumes match ours, our contribution shrinks to a guarantee that is not needed in practice, and we will report that.

Why not just use an off-the-shelf normalizing flow instead of velocity-field integration? Coupling networks are provably universal diffeomorphism approximators, so they may do just as well. The coupling ablation settles this. We expect the velocity-field version to win on smoothness of the boundary and on cheap exact inversion. If it does not, we adopt coupling layers, since the core idea is the warped-ball tube and not the particular map.

Why not just model disturbances with Itô calculus, the standard in stochastic control? With additive noise the two conventions coincide, and the choice is irrelevant. The difference appears only with state-dependent noise. There, an Itô model fitted with the same fields adds a drift that smooth physical noise does not produce. The Itô-twin ablation under colored multiplicative wind and torque noise tests this directly. A coverage gap that grows with noise state-dependence supports the claim; no gap means the Stratonovich component is dropped from the headline.

Proposed Experiments

We use three testbeds. The first is the double integrator with nonlinear drag from Lopez et al., which checks that the method reproduces the zero-violation regime and shows where the fixed symmetric cross-section wastes volume. The second is the 12-D quadrotor obstacle forest from Fan et al., rebuilt in Flightmare, with Ornstein–Uhlenbeck-colored wind whose magnitude scales with airspeed. The third is an attitude-only $\mathrm{SO}(3)$ reorientation task with multiplicative torque noise, where manifold-respecting tubes and the Itô/Stratonovich contrast matter most.

Metrics are:

  • empirical per-step and trajectory-level coverage on 10,000 held-out rollouts;
  • mean tube log-volume;
  • boundary self-intersection count and the fraction of negative-Jacobian points on a dense grid;
  • task success and closed-loop cost;
  • MPC solve time per step.

Finally, we compare coverage under deliberate distribution shift, in the form of stronger wind than in calibration, to quantify how far the exchangeability assumption bends.

The decisive checks are four. The first: at matched 95% coverage on the quadrotor, diffeomorphic tubes are smaller than ellipsoidal tubes and no larger than calibrated quantile tubes, with zero folds. The second: success in the narrowest passages improves over quantile tubes. The third: the unconstrained-displacement ablation shows boundary folds that cause missed collisions, demonstrating that topology preservation matters operationally. The fourth: on $\mathrm{SO}(3)$ with multiplicative noise, Itô-simulated tubes under-cover relative to their nominal level while Stratonovich-simulated tubes do not.

Failure of the first check kills the proposal. Failure of the fourth only removes the Stratonovich claim.