Accelerated Spline-Based Time-Optimal Motion Planning with Continuous Safety Guarantees for Non-Differentially Flat Systems

1. Problem Statement

The paper addresses the challenge of generating time-optimal, collision-free trajectories for Autonomous Mobile Robots (AMRs) operating in dynamic, obstacle-rich environments.

• The Core Trade-off: There is a fundamental conflict between ensuring continuous safety (guaranteeing no collision at any instant, not just at discrete time steps) and computational efficiency.
• The Bottleneck: State-of-the-art spline-based motion planners formulate the problem as a single Optimal Control Problem (OCP). To enforce continuous collision avoidance, they treat the parameters of separating hyperplanes (which separate the robot from obstacles) as decision variables within the OCP.
  • This introduces non-convex, bi-linear constraints and significantly increases the number of optimization variables.
  • Consequently, the computational cost becomes prohibitive for real-time applications, especially as the number of obstacles increases.
• Target System: The method specifically targets non-differentially flat systems (e.g., unicycle models), which are harder to plan for than differentially flat systems because their states cannot be directly parameterized by flat outputs.

2. Methodology

The authors propose a decoupled approach that separates the determination of separating hyperplanes from the main trajectory optimization loop.

A. Spline-Based Parameterization

• Bernstein Polynomials: The robot's state trajectory is parameterized using Bernstein polynomials over discrete control intervals.
• Convex Hull Property: This parameterization leverages the convex hull property of splines. If the coefficients (control points) satisfy safety bounds, the entire continuous trajectory between time steps is guaranteed to satisfy those bounds. This eliminates the risk of constraint violations between discretization points.
• Non-Flat Systems: The method uses a Runge-Kutta integration scheme linked to Bernstein coefficients to handle non-differentially flat dynamics.

B. Decoupling the Hyperplane Computation

Instead of optimizing the hyperplane normal vectors ($w$) and offsets ($b$) within the OCP, the authors treat the separation of the robot and obstacles as an independent classification problem.

• Classification Formulation: The problem of finding a hyperplane that separates the robot (class +1) from obstacle vertices (class -1) is solved using:
  1. Least-Squares Support Vector Machines (LS-SVM): Solved via a linear system.
  2. Quadratic Programming (QP): Solved via an inequality-constrained QP.
• Integration: Once $w$ and $b$ are computed in this separate step, they are treated as fixed parameters in the main OCP.
  • This transforms the original non-convex, bi-linear constraints into linear constraints.
  • It removes $M(n_y + 1)$ variables and $M \times N_{obstacles}$ constraints from the OCP (where $M$ is the number of obstacles).

C. Two-Step Filter Procedure

To ensure the decoupled approach remains robust and allows the hyperplanes to adapt as the trajectory changes:

1. Collision Check: The algorithm checks for collisions at interval boundaries and within intervals.
2. Adaptive Update: If a collision is detected, the hyperplane parameters are re-computed using the LS or QP solver.
3. Trust Region: A trust-region mechanism limits how much the hyperplane can change between iterations to ensure stability.

3. Key Contributions

1. Decoupled Optimization: The primary contribution is the separation of the hyperplane determination (classification) from the trajectory optimization (OCP). This eliminates hyperplane parameters from the decision variables.
2. Convexification: By fixing the hyperplanes, the method transforms non-convex, bi-linear constraints into linear constraints, significantly simplifying the OCP structure.
3. Applicability to Non-Flat Systems: The method extends spline-based planning to non-differentially flat systems (like unicycle robots) while maintaining rigorous continuous safety guarantees via the Bernstein basis.
4. Computational Efficiency: The approach drastically reduces the dimensionality of the optimization problem, leading to faster convergence times without sacrificing the continuous safety guarantees provided by the spline formulation.

4. Experimental Results

The method was validated on a 2D unicycle robot navigating environments with up to 10 rectangular obstacles. It was benchmarked against a state-of-the-art coupled method (LSR - Local Spline Relaxation).

• Computation Time:
  • The decoupled approach reduced total trajectory computation time by up to 59.06% (a factor of ~2.44) in environments with 8 obstacles.
  • The reduction scales with the number of obstacles; the more obstacles present, the greater the time savings.
• Optimality:
  • The time-optimality of the resulting trajectories remained nearly identical to the coupled method (median error in motion time was negligible, e.g., <0.5% for low obstacle counts).
• Success Rate:
  • The method maintained a high success rate (97-100%) for up to 6 obstacles.
  • Limitation: In highly cluttered environments (7-8 obstacles), the success rate dropped slightly (to ~87%) compared to the coupled method (99%). This is attributed to the conservative nature of the linear approximation and the lack of first-order sensitivity information regarding hyperplane evolution.
• Safety: The method successfully maintained continuous collision avoidance guarantees, ensuring no collisions occurred between discretization points.

5. Significance and Future Work

• Significance: This work bridges the gap between high-fidelity safety guarantees and real-time computational feasibility. It demonstrates that complex, non-convex safety constraints can be handled efficiently by decoupling them into classification problems, making time-optimal planning viable for complex, non-flat robots in cluttered industrial settings.
• Future Directions:
  • Advanced Solvers: Implementing structure-exploiting solvers (e.g., BLASFEO for LS, HPIPM for QP) to further reduce computation time.
  • Gradient Information: Incorporating first-order derivative information from the classification step into the trajectory optimization to improve the success rate in dense environments and close the optimality gap.
  • Scalability: Extending the approach to robots with more complex geometries (more vertices).

In conclusion, the paper presents a pragmatic and highly effective acceleration technique for spline-based motion planning, achieving near-optimal performance with significantly reduced computational load while rigorously guaranteeing continuous safety.