AURA

Asymptotically Optimal Uncertainty-Robust Replanning Algorithm for Kinodynamic Systems

Seyedali Golestaneh Zhuoyun Zhong Donghyung Lee Constantinos Chamzas

Worcester Polytechnic Institute · ELPIS Lab

Accepted to IEEE Robotics and Automation Letters (RA-L), 2026

Global Replanning Module

The planner keeps searching the state space during execution and switches to lower-cost trajectories as it finds them.

Local Optimization Module

Optimized controls are precomputed for possible execution outcomes, recoverying the system back toward the nominal trajectory.

Keep the Planner Alive During Execution!

Open-loop execution of a pushing plan drifting from the planned path compared with AURA replanning and execution — With a short initial planning budget and approximate dynamics, open-loop execution follows a suboptimal plan and drifts away (blue). AURA plans globally and optimizes locally during execution, producing a more optimal trajectory with smaller execution error (orange).

Sampling-based kinodynamic planners handle systems with complex dynamics, but they are usually run offline: the robot waits for a plan, then executes it blindly. Under a limited time budget the plan is often suboptimal, and under model mismatch the system deviates. AURA wraps around any asymptotically optimal planner and puts each execution interval to work, improving the trajectory quality and precomputing corrections beforehand.

The result: Lower-cost trajectories with any initial planning budget, up to 72% less execution deviation than open-loop execution, and up to 50% shorter total task time than receding-horizon and replanning baselines, all without a steering function.

3 Threads, 1 Parallel cycle

AURA timeline with an offline planning phase followed by runtime cycles of execution, global replanning, and local optimization — After an offline planning phase, each runtime cycle runs execution, global replanning, and local optimization concurrently, then synchronizes their results before the next control is applied.

AURA is a meta-planner on top of an asymptotically optimal sampling-based planner, such as AO-RRT, AO-EST, or SST. While the system executes the first control, two other modules use the same time window. One searches for a better global plan, the other prepares local recovery controls. At the end of each cycle, AURA evaluates candidate plans from the observed state, chooses the best one, and picks the next control to execute.

Global Replanning Module

Search tree update where unreachable branches are pruned and new samples lead to a lower-cost solution — As the first segment is executed, branches that are no longer reachable to the next state are pruned. Then, planning resumes from the new updated subtree, and any lower-cost solution it finds becomes the new best trajectory.

Pruning stage

Once the first control is committed, the next planned state becomes the new root. Only branches that cannot be reached without a steering function are removed; the rest of the exploration progress is kept.

Replanning stage

As the underlying planner is asymptotically optimal, every extra nodes of search during execution has a chance to find a cheaper path, so the system can start the execution once a feasible plan is found and the rest of the planning can happen online.

Local Optimization Module

Batched optimization computing recovery controls from sampled nearby states toward child states in the tree — (a) Applying the nominal control from a perturbed state amplifies the deviation, while a recovery control steers it back toward the nominal plan. (b) AURA samples states around the next planned state and optimizes a recovery control from each sample toward each child in the tree.

Instead of optimizing from the newly observed state like MPC, AURA samples the states the system might end up in and optimizes recovery controls for all of those outcomes in parallel on the GPU while the current action is still executing. When the actual state is observed, it picks the control computed for the nearest sample. The optimization works with any differentiable dynamics, including learned models.

Backed by theory: For trajectories with enough clearance, a recovery segment from any perturbed state back to the nominal successor is guaranteed to exist, and an approximate recovery is enough to keep the robot inside the planned safety margin.

Better Runtime Quality Trajectory

Final trajectory cost for vanilla and AURA versions of AO-RRT, SST, and AO-EST across four dynamical systems — Across a double integrator, a kinematic car, learned pushing dynamics, and a 6D Dubins airplane, AURA consistently returns lower-cost trajectories than the same planner run offline with the same initial planning time.

More Robust Tracking

Mean tracking error over ten applied controls for AURA, MPPI, and open-loop execution across five systems and environments — Open-loop execution accumulates tracking error with every control, while AURA keeps tracking error flat and comparable to MPPI across analytical and learned dynamics, under Gaussian noise and in MuJoCo.

Unlike MPPI, which optimizes only after the new state is observed, AURA has its recovery controls ready before the action finishes, so it can correct course immediately at the synchronization step. Moreover, AURA plans globally all the way to the goal, which makes it well suited for long-horizon tasks.

Faster End-to-End Task Time

Distribution of total task completion time for AURA, restart replanning, MPPI, and Robust-RRT — Including offline planning computation and execution time, AURA reaches the goal faster than restart replanning, MPPI, and Robust-RRT in the more complex systems and in MuJoCo and real-world settings.

Restart replanning throws away the tree after every large deviation, and MPPI cannot find the optimal path or gets trapped in local minima on long tasks. AURA keeps global exploration and local robustness at the same time.

Lower Offline Planning is Faster!

Car environment with obstacles and a surface plot of task time over offline planning time and maximum control duration — In a car environment where the optimal path threads a narrow passage, the shortest total task time comes from a short offline planning budget and short control durations.

Extra offline planning only pays off if it saves more time than it costs. Because AURA keeps refining during execution, the system is better off once a feasible plan exists and letting replanning find the shortcut on the way.

Simulation: Car and Manipulator in MuJoCo

MuSHR Car

UR10 Non-prehensile Pushing

Uncertainty comes from the mismatch between the simplified dynamics model and the MuJoCo physics, and AURA corrects for it at every step.

Real-world pushing with learned dynamics

Real-world Trial 1

Real-world Trial 2

On a real UR10, state is only observed after each push, so AURA's precomputed recovery controls let the robot correct course at every action boundary despite hardware inaccuracies and unmodeled dynamics.

Acknowledgments

This work was supported in part by the Amazon WPI RBE Research Award 2025, NSF CRII Grant No. 2451108, and Worcester Polytechnic Institute funds.