Step Sizes That Survive Moving Obstacles

Robots that plan by gradient descent push a candidate trajectory downhill on a cost made of smoothness, control effort, and collision penalties. In practice someone hand-tunes the step size, usually in static or slow-moving test scenes. This proposal argues that the tuning is quietly tied to the static environment assumption. When obstacles move faster, the collision penalty becomes sharper and changes from one replanning cycle to the next. A step size that worked in the lab then either overshoots, sending the trajectory into infeasible contortions, or stalls in front of an oncoming pedestrian.

The proposed fix borrows from stochastic optimization. There, "model-based" step rules such as the stochastic Polyak step and truncated proximal-point updates stay stable across many orders of magnitude of step size, because they exploit a known lower bound on the loss. Collision penalties have exactly such a bound: a safe trajectory incurs zero penalty. Treating each sampled future obstacle position as a "data point" turns a collision-free trajectory into the interpolation regime, where a model can fit every sample perfectly. That is the regime in which these step rules are provably most stable. The planner we build should therefore need essentially no step-size retuning when the world starts moving.

Core Claim

We claim two things. First, step-size sensitivity in gradient-based planning grows with obstacle speed and density, so the static environment assumption hides a tuning cost. Second, a planner that pairs two pieces closes most of that gap. One piece is a truncated linear model of each sampled collision term, built from the per-scenario penalty, which is non-negative with known lower bound zero. The other is an exact proximal treatment of the smooth trajectory cost. With these two pieces, success and collision rates should stay flat across at least four orders of magnitude of base step size, at a fixed per-cycle iteration budget.

Let $z$ be the trajectory variables (positions and controls over the horizon), and let $g(z)$ be the smooth quadratic cost for effort, tracking, and dynamics residuals. Let $h_i(z) = \max(0,\, r - d(p_t(z), o_{s,t}))$ be the collision penalty for sampled obstacle scenario $s$ at time $t$. Here $r$ is a safety radius, $d$ is distance, $p_t(z)$ is the robot position, and $o_{s,t}$ is the sampled obstacle position. The update draws one term $i$ and solves

$$z_{k+1} = \arg\min_z \Big\{ \max\big(0,\, h_i(z_k) + \nabla h_i(z_k)^\top (z - z_k)\big) + g(z) + \tfrac{1}{2\alpha}\|z - z_k\|^2 \Big\},$$

where $\alpha$ is the base step size. If $g$ is dropped, this reduces to moving along $\nabla h_i$ by $\min(\alpha, h_i/\|\nabla h_i\|^2)$. That move is exactly a projection onto the linearized safe half-space whenever $\alpha$ is large, so the step can never overshoot the constraint no matter how large $\alpha$ is.

Two pieces of prior work supply the method. The truncation comes from the aProx family in Stochastic (Approximate) Proximal Point Methods, which converged across six orders of magnitude of step size on synthetic regression. The split, which keeps $g$ exact instead of folding it into the Polyak ratio, comes from A Stochastic Proximal Polyak Step Size. That paper showed that plain SPS on a composite objective "swirls" because the total objective's lower bound is unknown. Since $g$ is quadratic and banded in time, the proximal subproblem reduces to a one-dimensional search over the truncation multiplier plus a banded linear solve.

Prior Work and How This Differs

Three papers motivate the gap. Step-Size Stability in Stochastic Optimization defines a stability index, which measures how far the loss after a step departs from the step's own surrogate prediction. It proves that this index grows linearly in $\alpha$ for SGD but stays bounded for SPS and stochastic proximal point under interpolation. On ResNet20 it reports SGD breaking at $\alpha \approx 1$ while SPS and NGN stay stable to $\alpha \approx 10$. Its proofs assume (weak) convexity, and our disk-obstacle penalty is not convex in robot position, so we treat the theory as a hypothesis to test rather than inherit. Parallel Stochastic Gradient-Based Planning for World Models (GRASP) optimizes lifted intermediate states with plain gradient steps, Langevin noise, and a "grad-cut" that blocks gradients through world-model state inputs. It reports sensitive noise-scale and synchronization hyperparameters and evaluates only stationary walls. We change the optimizer, not the lifting, and target moving obstacles.

Scenario-Based Trajectory Optimization in Uncertain Dynamic Environments (S-MPCC) samples pedestrian scenarios and linearizes collision constraints into half-spaces, then hands the result to a deterministic QP solver. Our per-scenario truncated step uses the same linearization but processes scenarios stochastically, one or a few at a time. This avoids building the full scenario program, at the cost of giving up its certificate.

Several papers act as constraints rather than methods. Topology-Driven Parallel Trajectory Optimization in Dynamic Environments shows that local optimizers among pedestrians fall into poor homotopy classes, meaning they commit to passing on the wrong side of an obstacle. A better step rule does not fix that, so we run our optimizer inside its homotopy-guided parallel scheme rather than claiming to replace it. Evaluating Trajectory Collision Probability through Adaptive Importance Sampling uses stochastic mirror descent only on sampler weights for evaluating a fixed trajectory, in static scenes. We borrow it as an independent post-hoc collision-probability evaluator. Finally, GRASP's grad-cut might seem to conflict with obstacle-aware gradients, but the conflict is only apparent. The grad-cut blocks gradients through the learned dynamics, while our collision terms depend on decoded positions directly, so the two coexist.

Research Program

We will build the optimizer as a drop-in replacement for the inner solver in two hosts. The first is a homotopy-guided parallel MPC in the style of T-MPC. Each guidance class runs a fixed budget of our updates per replanning cycle, warm-started from the previous cycle, with scenarios drawn from a constant-velocity-plus-noise pedestrian predictor. The second is GRASP's lifted-state planner on PointMaze and Wall-Single. Here we add moving obstacles with known sampled futures and apply the truncated step to the obstacle terms while keeping GRASP's structure.

Alongside the planners we will measure the empirical stability index per iteration, as a function of $\alpha$ and of obstacle speed. This tests whether the bounded-index behaviour proven for convex interpolation carries over to the linearize-then-truncate update on non-convex penalties. A modest theoretical piece covers the case of half-space-linearized constraints with quadratic $g$, where the problem is convex. There we will state the bounded-index result as an adaptation of the existing analysis and be explicit that the disk case is empirical only.

Baselines

The naive reference point is plain gradient descent on the same penalized objective with one step size tuned on static scenes and then frozen. This mirrors GRASP's standard-GD arm, which reached 6.4% success on Push-T at an 80-step horizon versus GRASP's 10.4% and CEM's 2.8%, as reported in Parallel Stochastic Gradient-Based Planning for World Models. Push-T is our sanity check that the host implementation reproduces those numbers before any obstacles move.

The strongest step-size competitor is Adam with gradient-norm clipping and a cosine annealing schedule. We include annealing because Learning Rate Annealing Improves Tuning Robustness in Stochastic Optimization gives it theoretical robustness advantages. Clipping is the most direct cheap competitor to truncation. Every optimizer gets the same scenario samples, the same per-cycle iteration and wall-clock budget, and the same tuning budget: a log-spaced grid over $\alpha$ plus a matched grid over clip threshold. The grid is tuned only on static and slow scenes and then evaluated unchanged on fast ones.

The strongest existing planner on the pedestrian task is T-MPC++ with its nonlinear-program inner solver, run at the same replanning rate. The paper reports zero collisions in physical trials and a task-duration standard deviation less than half that of the TEB local planner. For its simulated collision or success rate, no reported result was found on Lacuna, so we will reproduce it ourselves. On the S-MPCC corridor with up to six pedestrians, the paper reports only that collisions were prevented and risk stayed below a 3σ threshold; no reported numeric collision rate was found on Lacuna.

Beating the baselines means two things. Against the optimizer baselines, we need a statistically flat success curve over a step-size range at least ten times wider than theirs at fast obstacle speeds. Against T-MPC++, we need equal or lower collision rate at equal or lower solve time.

Two ablations isolate the key ingredient. The first removes truncation but keeps the proximal split: the same proximal step without the $\max(0,\cdot)$ floor. The second keeps truncation but drops the split: SPS applied to $h_i + g$ jointly. If the first ablation matches the full method, the zero lower bound is not doing the work and our claim fails.

Why not just…?

Why not just use Adam with gradient clipping? Clipping caps the step length but does not know that the penalty bottoms out at zero. So it can still overshoot past the safe boundary into a different collision, or under-step when one large violation dominates. Adam also remains sensitive to its base rate, per Step-Size Stability in Stochastic Optimization and Theoretical analysis of Adam using hyperparameters close to one without Lipschitz smoothness. This is a genuine threat, so it is our primary baseline with a matched tuning budget. If tuned Adam plus clipping shows the same flat curve, we will report that the problem is solved by clipping.

Why not just hand the linearized scenarios to a QP or NLP solver, as S-MPCC and T-MPC do? For a few pedestrians with convexified constraints, an exact solver has no step size at all and may well win. We therefore compare at equal wall clock, and we accept that our contribution narrows to regimes the solvers handle poorly: many scenarios, costs defined through learned world models as in GRASP, and GPU-parallel candidate trajectories. S-MPCC itself notes compute grows substantially in crowds. The experiment that settles this is the scenario-count sweep described below.

Why not just backtrack with a line search? Each trial needs a fresh evaluation of a sampled, noisy objective. Hybrid Deterministic-Stochastic Methods for Data Fitting explains why this breaks down under sampling noise, and the extra evaluations eat into a sub-second replanning budget. We include Armijo backtracking on the full scenario batch as an extra arm so this is measured rather than asserted.

Why not just use sampling-based planning like CEM? GRASP's reported 2.8% CEM success on long-horizon Push-T suggests derivative-free search scales poorly with horizon. We still include CEM in the dynamic PointMaze variant at matched compute.

Proposed Experiments

The central experiment is a step-size-by-speed sweep. We span $\alpha$ over six orders of magnitude and obstacle speed from zero to roughly twice nominal pedestrian speed. This runs in the social-forces pedestrian simulation with up to 12 agents used by T-MPC, and in the S-MPCC-style corridor. We report success rate, collision rate (cross-checked with the adaptive importance sampling evaluator), time to goal and its variance, and iterations until the first feasible plan within each cycle.

The decisive check is the transfer gap. This is the drop in success when the $\alpha$ chosen on static scenes is applied at the fastest speed. We predict a large gap for SGD and Adam, a small one for our method, and a large one again for the no-truncation ablation.

A second experiment sweeps scenario count from tens to thousands at a fixed replan deadline of about 0.3 seconds, matching the CATNIPS replan rate in CATNIPS. Here we compare against the exact-solver hosts. A third experiment adds moving obstacles to GRASP's PointMaze and Wall-Single and checks that the truncated step reduces GRASP's dependence on its noise scale.

Throughout, we log the empirical stability index. If it grows linearly in $\alpha$ for our method on non-convex disk penalties, the theoretical motivation does not transfer and we will say so.