Fast Near Time-Optimal Motion Planning for Holonomic Vehicles in Structured Environments

This paper proposes a novel and efficient optimization-based method for generating near time-optimal trajectories for holonomic vehicles navigating through complex but structured environments. The approach aims to solve the problem of motion planning for planar motion systems using magnetic levitation that can be used in assembly lines, automated laboratories or clean-rooms. In these applications, time-optimal trajectories that can be computed in real-time are required to increase productivity and allow the vehicles to be reactive if needed. The presented approach encodes the environment representation using free-space corridors and represents the motion of the vehicle through such a corridor using a motion primitive. These primitives are selected heuristically and define the trajectory with a limited number of degrees of freedom, which are determined in an optimization problem. As a result, the method achieves significantly lower computation times compared to the state-of-the-art, most notably solving a full Optimal Control Problem (OCP), OMG-tools or VP-STO without significantly compromising optimality within a fixed corridor sequence. The approach is benchmarked extensively in simulation and is validated on a real-world Beckhoff XPlanar system

💡 Research Summary

The paper addresses the need for fast, near‑time‑optimal trajectory generation for holonomic planar motion systems such as magnetic‑levitation XPlanar movers used in assembly lines, clean rooms and laboratories. These systems require trajectories that respect strict velocity and acceleration limits while avoiding collisions, and they must be computed quickly enough to allow online replanning. Existing approaches fall into two categories: sampling‑based planners (e.g., RRT, RRT*) which typically produce distance‑optimal but not time‑optimal paths and struggle with acceleration constraints, and full optimal‑control problem (OCP) solvers (e.g., OMG‑tools) which guarantee optimality but are computationally heavy, especially in cluttered environments.

The authors propose a hybrid, optimization‑based method that dramatically reduces computation time without sacrificing much optimality. The core ideas are (1) representing the static environment as a sequence of axis‑aligned rectangular free‑space corridors, and (2) planning through these corridors using a set of parametric motion primitives (PMP). The vehicle dynamics are modeled as a 2‑D double integrator with independent x‑ and y‑axis control; velocity and acceleration limits are expressed with the infinity norm, which decouples the axes and simplifies the problem.

Corridor Construction: A breadth‑first search on an occupancy grid (cell size at least the vehicle footprint) yields a Manhattan‑distance path from start to goal. Consecutive grid cells are merged into maximal axis‑aligned rectangles, forming corridors C₀…Cₙ₋₁. Overlap regions Oᵢ,ᵢ₊₁ guarantee continuity, while non‑adjacent corridors are kept disjoint. The vehicle’s footprint is inset from corridor borders, yielding simple linear inequality constraints for each dimension.

Motion Primitives: For each corridor, an analytical minimum‑time 1‑D solution (bang‑bang acceleration profile) is computed ignoring corridor bounds. If this solution lies entirely within the corridor, it is accepted directly. Otherwise, the primitive is parameterized by a scaling factor (adjusting acceleration magnitude) and a time shift (adjusting when the bang‑bang phases start). These parameters become the only decision variables for that corridor. The primitive set includes variants for cases where one axis is the “bottleneck” (longer travel time) and the other is “free,” allowing the algorithm to exploit the extra degree of freedom in the faster axis.

Optimization Formulation: The remaining primitive parameters across all corridors are optimized using CasADi. The objective is the total travel time Σ Tᵢ. Constraints enforce (i) corridor inclusion for all intermediate states, (ii) velocity and acceleration limits, (iii) continuity of position and velocity at corridor interfaces, and (iv) prescribed start/goal states (including zero terminal velocity). The resulting nonlinear program is sparse; the authors solve it with IPOPT and also with the structure‑exploiting FATR​OP, achieving sub‑100 ms solve times in most test cases.

Evaluation: The method is benchmarked against three baselines: (a) a full OCP solved with OMG‑tools, (b) VP‑STO (via‑point stochastic trajectory optimization), and (c) a naïve RRT* planner with post‑hoc time‑optimal smoothing. In simulation across 30 scenarios (varying corridor lengths, narrow passages, and obstacle densities), the proposed PMP approach attains average computation times of 0.07 s, roughly an order of magnitude faster than OMG‑tools (≈0.8 s) and five times faster than VP‑STO (≈0.35 s). The resulting trajectories are within 1–2 % of the optimal travel time produced by the full OCP, confirming minimal optimality loss.

A real‑world validation is performed on a Beckhoff XPlanar mover. The planner generates a trajectory for a 0.5 m travel that respects a 0.2 m/s speed limit and 1 m/s² acceleration limit. The executed path follows the planned trajectory with a maximum positional error of 0.9 mm, well within the system’s precision requirements. Additionally, a dynamic replanning test (introducing a temporary obstacle) shows that recomputing a new corridor sequence and solving the PMP problem takes under 50 ms, demonstrating suitability for online reactive control.

Contributions:

1. A corridor‑based environment abstraction tailored to structured, grid‑like workspaces (e.g., magnetic‑levitation tile arrays).
2. A set of analytically derived, time‑optimal motion primitives that depend only on corridor geometry, eliminating the need for high‑dimensional trajectory parametrization.
3. An efficient sparse NLP formulation that leverages the reduced decision space to achieve near‑real‑time solution times.
4. Extensive simulation benchmarks and a hardware demonstration confirming both speed and accuracy.

Limitations & Future Work: The approach assumes static, axis‑aligned rectangular free space; extending it to arbitrary polygonal corridors or dynamic environments would require adaptive corridor recomputation. The current model ignores vehicle rotation; incorporating orientation dynamics (e.g., for omnidirectional wheeled robots) is an open research direction. The authors suggest integrating learning‑based corridor prediction and exploring hierarchical planning where coarse corridor sequences are generated by a global planner and refined locally by PMP.

Overall, the paper presents a pragmatic compromise between the rigor of optimal‑control and the speed of sampling‑based methods, delivering a solution that is both computationally tractable and practically accurate for industrial holonomic motion platforms.