Optimal Stochastic Vehicle Path Planning Using Covariance Steering

Kazuhide Okamoto, Panagiotis Tsiotras

TL;DR

This paper introduces a path-planning framework that doesn't just plan a route, but actively manages the robot's uncertainty (covariance) to squeeze through tight spaces. By combining Optimal Covariance Steering with Mixed-Integer Convex Programming, the authors allow a vehicle to "shrink" its distribution of possible states to safely navigate non-convex obstacles that traditional planners would find too risky to attempt.

What Problem Does This Paper Address?

In robotics, uncertainty is the enemy of efficiency. Most path planners treat uncertainty in one of two ways: they ignore it (deterministic) or they assume it grows over time as the robot moves (open-loop). When uncertainty grows unchecked, the robot must stay very far away from obstacles to ensure a low probability of collision. This leads to "conservative" paths—the robot takes a long detour because it isn't "sure" enough to fit through a narrow gap.

While low-level feedback controllers are usually added later to reduce this jitter, the high-level planner rarely accounts for this capability. The authors address this gap by asking: What if the planner explicitly decides how much to reduce uncertainty at each step to find the most efficient, collision-free path?

Methods

The authors propose a two-fold contribution to solve the stochastic path-planning problem:

  1. Efficient Covariance Steering (CS): Traditional CS methods often require massive memory because they don't utilize the Markov property (the idea that the future depends only on the present state). The authors derive a new, computationally leaner formulation where the control gain matrix $K$ is block-diagonal, reducing the number of variables to compute from $O(N^2)$ to $O(N)$ relative to the time horizon.
  2. Handling Non-Convex Obstacles via MICP: Real-world environments are full of "non-convex" shapes (like L-shaped rooms or narrow slits). The authors decompose the obstacle-free space into a union of overlapping convex polytopes. They then use Mixed-Integer Convex Programming (MICP)—specifically using Boolean variables—to let the optimization algorithm "choose" which convex region the robot should occupy at any given time.
  3. Chance Constraints: Instead of "hard" boundaries, they use chance constraints, ensuring the probability of hitting an obstacle stays below a user-defined threshold (e.g., 0.1%).

Datasets

The paper does not use external real-world datasets. Instead, it validates the theory using numerical simulations of a double-integrator vehicle model (representing a basic mobile robot or UAV) in two scenarios:

  • The Double Slit: A wall with two openings of different widths.
  • Cluttered Environment: A field with multiple rectangular obstacles creating narrow corridors.

Key Insights and Ideas

  • Active Uncertainty Reduction: The most striking insight is that the robot can "pay" a control cost to reduce its own variance. In the "Double Slit" experiment, a standard planner avoids a narrow gap because its uncertainty is too high. The proposed planner, however, realizes it can apply more control effort to "tighten" its state distribution, allowing it to safely navigate the shorter, narrower path.
  • Markovian Efficiency: By reformulating the steering problem to respect the Markov property, the authors made the optimization significantly faster and more memory-efficient than their own previous work, making it more viable for real-time applications.
  • The "Safety Margin" Trade-off: The cost function balances the energy spent on control versus the state error. This allows the robot to be "brave" (low control effort, high uncertainty) in open spaces and "cautious" (high control effort, low uncertainty) near walls.

Key Results

  • Feasibility in Tight Gaps: In the "Double Slit" test, the proposed approach successfully navigated a narrow slit by shrinking the 3$\sigma$ error ellipses. A "mean-only" steering approach (which doesn't control covariance) failed to find a path through the narrow gap and was forced to take a much longer route through a wider opening.
  • Optimization Performance: By restricting the search space and using the YALMIP/MOSEK solver suite, the authors demonstrated that these complex mixed-integer problems can be solved for reasonable time horizons ($N=20$) on modern hardware.
  • Shortest Path Discovery: In cluttered environments, the covariance-steering vehicle followed the shortest corridor, whereas the baseline had to detour around the entire obstacle field because it couldn't guarantee safety in the narrow "hallway."

Limitations and Future Work

  • Manual Decomposition: Currently, the "convex sub-regions" of the environment must be defined manually or by a separate algorithm. The authors note that the way you decompose the space affects how conservative the path is.
  • Computational Complexity: While more efficient than previous iterations, Mixed-Integer Programming is still NP-hard. Scaling this to very high-dimensional systems or extremely long time horizons remains a challenge.
  • Future Directions: The authors suggest investigating automated ways to find the "best" decomposition of free space into convex sets to further reduce the conservativeness of the paths.

Summary updated

Abstract

This work addresses the problem of vehicle path planning in the presence of obstacles and uncertainties, which is a fundamental problem in robotics. While many path planning algorithms have been proposed for decades, many of them have dealt with only deterministic environments or only open-loop uncertainty, i.e., the uncertainty of the system state is not controlled and, typically, increases with time due to exogenous disturbances, which leads to the design of potentially conservative nominal paths. In order to deal with disturbances and reduce uncertainty, generally, a lower-level feedback controller is used. We conjecture that, if a path planner can consider the closed-loop evolution of the system uncertainty, it can compute less conservative but still feasible paths. To this end, in this work we develop a new approach that is based on optimal covariance steering, which explicitly steers the state covariance for stochastic linear systems with additive noise under non-convex state chance constraints. The proposed framework is verified using simple numerical simulations.
BibTeX
@article{okamoto2019optimal,
  author = {Kazuhide Okamoto and Panagiotis Tsiotras},
  title = {{Optimal Stochastic Vehicle Path Planning Using Covariance Steering}},
  journal = {IEEE RA-L},
  year = {2019},
  doi = {10.1109/lra.2019.2901546},
}

Loading related content...

Figure