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.
Introduction
In robotics, uncertainty is a primary obstacle to efficiency. Most conventional path planners handle uncertainty in one of two ways: they either ignore it entirely (deterministic planning) or assume it grows monotonically over time (open-loop uncertainty). When uncertainty is left unchecked, robots must maintain large safety margins from obstacles to ensure a low probability of collision. This leads to "conservative" paths, where a robot might take a long detour simply because it cannot guarantee its position precisely enough to navigate a narrow gap.
While low-level feedback controllers are typically employed to mitigate disturbances, high-level planners rarely account for this capability during the trajectory generation phase. This paper introduces a framework that bridges 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?
Methodology: Active Uncertainty Management
The authors propose a two-fold approach to solve the stochastic path-planning problem by combining Optimal Covariance Steering (CS) with Mixed-Integer Convex Programming (MICP).
Efficient Covariance Steering
Traditional CS methods are often computationally expensive because they do not fully exploit the Markov property of the system. The authors derive a leaner formulation where the control gain matrix is block-diagonal. This reduces the number of variables from $O(N^2)$ to $O(N)$ relative to the time horizon, significantly improving memory efficiency and speed.
Handling Non-Convexity via MICP
Real-world environments are rarely convex. To handle obstacles, the authors decompose the obstacle-free space into a union of overlapping convex polytopes. They then use MICP to allow the optimization algorithm to "choose" which convex region the robot should occupy at any given time step.
Instead of hard boundaries, the framework utilizes chance constraints. This ensures that the probability of collision remains below a user-defined threshold (e.g., 0.1%), allowing for a rigorous mathematical treatment of safety.
Experimental Results
The proposed approach was validated through numerical simulations of a double-integrator vehicle model in two challenging scenarios: a "double slit" wall and a cluttered environment with narrow corridors.
The most significant finding is the robot's ability to "pay" a control cost to reduce its own variance. In the double-slit experiment, a standard planner (mean-only steering) avoids the narrow gap because its uncertainty is too high, forcing it to take a longer path through a wider opening. In contrast, the proposed planner realizes it can apply more control effort to "tighten" its state distribution, allowing it to safely navigate the shorter, narrower path.
In cluttered environments, the covariance-steering vehicle followed the shortest corridor, whereas the baseline had to detour around the entire obstacle field because it could not guarantee safety within the narrow hallway.
Conclusion and Future Work
By reformulating the steering problem to respect the Markov property, the authors have made stochastic path planning more viable for real-time applications. The core insight is the "safety margin" trade-off: the cost function balances control energy against state error, allowing the robot to be "brave" in open spaces and "cautious" near obstacles.
Current limitations include the manual decomposition of free space into convex sub-regions and the inherent NP-hardness of Mixed-Integer Programming. Future research will focus on automated space decomposition and scaling the approach to higher-dimensional systems.
Summary updated