Unified Optimization for Planning and Control

This direction is split into other directions: unified planning and control. Its last version stays readable and citable here.

Version 1 of 145 papers63 conceptsfrozen 2026-10-04

In the traditional "autonomy stack" of robotics, there is a sharp divide between decision-making, planning, and control. A high-level module decides to overtake a car; a mid-level planner draws a geometric path; and a low-level controller nudges the motors to follow that path.

While intuitive, this hierarchy creates a "feasibility gap." The planner might propose a trajectory that is geometrically perfect but physically impossible for the robot to execute—like asking a heavy truck to pull a U-turn at 60 mph. As Tianqi Zheng, John W. Simpson-Porco, and Enrique Mallada argue in Closed-Loop Motion Planning for Differentially Flat Systems: A Time-Varying Optimization Framework, this separation forces robots to be overly conservative, moving slowly to ensure they stay within the "safe" margins of a decoupled system.

A growing body of research is collapsing this hierarchy into a single, unified optimization problem. By solving for high-level intent and low-level motor commands simultaneously, robots can operate at their physical limits with unprecedented fluidity.

Collapsing the Hierarchy

The core intuition behind this shift is that planning and control are actually the same mathematical task: finding a sequence of inputs that minimizes a cost function while satisfying physical constraints.

In UDMC: Unified Decision-Making and Control Framework for Urban Autonomous Driving, Haichao Liu and colleagues demonstrate how to treat behavioral decisions—like whether to enter a congested roundabout—as part of a single nonlinear programming problem. Instead of a discrete "decision" followed by a "plan," the system evaluates the cost of all possible maneuvers (safety, speed, comfort) in one go.

This approach eliminates the latency and coordination issues inherent in sequential pipelines. As Bai Li notes in Milestones in Autonomous Driving and Intelligent Vehicles, the field is transitioning from "behavior-then-trajectory" models to unified models where behavioral intent is simply another constraint in a continuous optimization.

Ensuring Dynamic Feasibility

When planning and control are unified, the robot's physical dynamics (mass, friction, motor torque) are embedded directly into the search for a path. This ensures that every proposed movement is "dynamically feasible."

For example, Guido De Croon and Javier Alonso-Mora developed a framework for MAVs with slung payloads in Online trajectory planning and control of a MAV payload system in dynamic environments. By using Non-Linear Model Predictive Control (NMPC) to unify these layers, the drone doesn't just plan a path around an obstacle; it plans a path that accounts for how the payload will swing, ensuring the motors can actually handle the resulting forces.

Similarly, in the high-stakes world of autonomous racing, Alexander Liniger and Manfred Morari showed in Optimization-Based Autonomous Racing of 1:43 Scale RC Cars that integrated optimization allows cars to find the "racing line" while simultaneously managing tire friction and drifting—maneuvers that would be impossible for a decoupled planner to coordinate.

Emergent Behavior from Unified Costs

One of the most powerful aspects of unified optimization is that complex behaviors emerge naturally from the math, rather than being hard-coded as "if-then" rules.

In Learning from Naturalistic Driving Data for Human-like Autonomous Highway Driving, Donghao Xu and Mathieu Moze integrate "lane incentives" directly into a trajectory cost function. The vehicle doesn't "decide" to change lanes in a separate module; instead, the mathematical cost of staying in a slow lane becomes higher than the cost of the steering maneuver required to move to a faster one. The lane change "emerges" as the optimal solution to the unified equation.

This concept extends to obstacle avoidance. Wenjun Liu and Alois Knoll demonstrate in Gaussian Process-Based Model Predictive Control for Overtaking that by formulating geometric relationships as linear constraints directly within the control loop, the system can perform high-speed overtaking without a separate high-level path planner.

Solving the "Stalemate" Problem

Traditional planners often struggle with "stalemates"—situations where a robot gets stuck because the repulsive force of an obstacle perfectly cancels out the attractive force of the goal.

By using Lie Algebra and improved Artificial Potential Fields (APF), Xinxing Lai and Zhong Jin show in Analysis Of Inertial Stability Dynamical For Robot Based On Lie Algebra how a unified potential field can treat obstacles, targets, and even camera field-of-view limits as a single mathematical landscape. This allows the robot to "slide" through complex constraints that would paralyze a hierarchical system.

The Computational Challenge

If unified optimization is so effective, why hasn't it always been the standard? The answer is computational cost. Solving a high-fidelity, long-horizon nonlinear optimization problem in milliseconds is incredibly difficult.

Recent breakthroughs like UNIQUE, proposed by Rudolf Reiter and Davide Scaramuzza in Unifying Quadrotor Motion Planning and Control by Chaining Different Fidelity Models, solve this by "chaining" models. They use a high-fidelity model for the immediate future (the "control" horizon) and a simpler, low-fidelity model for the distant future (the "planning" horizon), all within a single optimization. This provides the foresight of a planner with the reactivity of a controller, without the prohibitive computational overhead.

Papers in this version

History & citationVersion 1 · 2026-10-04

Split into other directions · Permanent link

View frozen version 1

Cite this direction

Latest version (a living page):

@misc{lacuna_unified_optimization_for_planning_and_co_94121fb1,
  title = {Unified Optimization for Planning and Control},
  author = {{Lacuna}},
  howpublished = {Lacuna research direction},
  year = {2026},
  url = {https://lacuna.tiptreesystems.com/direction/unified-optimization-for-planning-and-control/rdr_94121fb1804146b088bdb809a56a9386},
  note = {Latest version of a living research direction; cite a version for a fixed text. Accessed 2026-11-21},
}

This exact version (version 1, frozen text):

@misc{lacuna_unified_optimization_for_planning_and_co_94121fb1_v1,
  title = {Unified Optimization for Planning and Control},
  author = {{Lacuna}},
  howpublished = {Lacuna research direction},
  year = {2026},
  url = {https://lacuna.tiptreesystems.com/direction/unified-optimization-for-planning-and-control/rdr_94121fb1804146b088bdb809a56a9386/v/1},
  note = {Version 1, frozen 2026-10-04. Accessed 2026-11-21},
}