Abstract
This paper presents a novel two-layer motion planning framework designed to solve complex, non-convex trajectory optimization problems efficiently. By decomposing the global planning task into iterative "planning cycles," the proposed method integrates fast heuristic searches—specifically a customized vortex potential field—with rigorous convex optimization. This approach significantly reduces computational overhead compared to traditional Sequential Convex Programming (SCP) while maintaining near-optimal travel times in both static and dynamic environments.
Introduction
Autonomous agents, such as unmanned aerial vehicles (UAVs) and self-driving cars, require motion planning algorithms that can navigate complex environments in real-time. A primary challenge in this domain is "non-convexity," where obstacles create disjointed feasible regions that lead to local minima in optimization landscapes. While Model Predictive Control (MPC) and Sequential Convex Programming (SCP) are standard tools, they often suffer from high computational costs or failure to converge in dense, fast-changing scenarios. This research aims to bridge the gap between high-speed heuristic methods and high-fidelity mathematical programming.
Methodology: The Two-Layer Planning Framework
The core of the proposed strategy is a hierarchical model that operates iteratively to generate time-optimal trajectories.
Outer Layer: Planning Cycles
Rather than attempting to solve the entire trajectory from start to goal—a process that is computationally expensive and sensitive to environmental changes—the algorithm utilizes a receding horizon approach. It solves for a short "planning cycle" with a fixed time horizon. These segments are concatenated to form the complete path, allowing for real-time adjustments.
Inner Layer: Three-Step Subroutine
Within each cycle, a three-step process ensures both speed and safety:
- Reformulation: The vehicle's dynamics are linearized to simplify the underlying optimization problem.
- Trajectory Search: A heuristic search identifies a "nominal" collision-free path. The authors introduce the Customized Vortex Artificial Potential Field (CVAPF), which incorporates a rotational component to guide the agent around obstacles, avoiding the local minima common in standard potential fields.
- Convex Optimization: The nominal path defines a "safe tube"—a convex region of space. The final trajectory is then refined within this tube using strict optimization to ensure time-optimality.
This "Safe Tube" strategy is pivotal; it allows the heavy-duty optimizer to operate within a region where convergence is guaranteed and efficient, bypassing the complexities of the global non-convex space.
Experimental Evaluation
The framework was evaluated in custom-built simulation environments using MATLAB and the CVX toolbox. Testing involved random static maps with 20 circular obstacles and dynamic maps where obstacles moved according to a double-integrator model. The proposed method (Alg1 + CVAPF) was benchmarked against a standard SCP solver.
Key Results
- Efficiency: In static environments, the two-layer model reduced average computation time by approximately 50% (13.90s vs. 27.80s for SCP).
- Reliability: The model achieved a 90.1% success rate in large-scale static tests, surpassing SCP's 87.4%. In dynamic scenarios, it maintained high performance (up to 91.7% success rate) with predictable computation times suitable for real-time hardware.
- Optimality: Despite the iterative nature of the planning cycles, the final travel times remained nearly identical to global SCP solutions, demonstrating minimal loss in optimality for significant gains in speed.
Conclusion and Future Work
The two-layer planning model effectively balances computational speed with trajectory optimality. By leveraging heuristics to "convexify" the search space, it provides a robust solution for real-time autonomous navigation. Future research will focus on addressing discrete feasibility issues to ensure safety between time steps and extending the framework to more complex, non-linear vehicle models.
Summary updated