Online Trajectory Optimization for Arbitrary-Shaped Mobile Robots via Polynomial Separating Hypersurfaces

TL;DR

Many trajectory planners rely on convex approximations, which lead to conservatism for nonconvex robots in cluttered spaces. This paper generalizes the separating hyperplane concept to polynomial separating hypersurfaces, proving that any two disjoint bounded closed sets can be strictly separated by a polynomial; it then formulates an NLP that jointly optimizes the robot trajectory and the separating polynomial coefficients, with obstacles represented as clustered point clouds. The approach enables geometry-aware collision avoidance for arbitrary-shaped robots in real time, demonstrated through simulations and real-world experiments against baseline methods, showing smooth, collision-free, agile maneuvers in narrow passages and dense forests. The results suggest a practical online planning tool capable of handling nonconvex morphologies without conservative convex hull simplifications, enabling safer navigation in unknown environments.

Abstract

An emerging class of trajectory optimization methods enforces collision avoidance by jointly optimizing the robot's configuration and a separating hyperplane. However, as linear separators only apply to convex sets, these methods require convex approximations of both the robot and obstacles, which becomes an overly conservative assumption in cluttered and narrow environments. In this work, we unequivocally remove this limitation by introducing nonlinear separating hypersurfaces parameterized by polynomial functions. We first generalize the classical separating hyperplane theorem and prove that any two disjoint bounded closed sets in Euclidean space can be separated by a polynomial hypersurface, serving as the theoretical foundation for nonlinear separation of arbitrary geometries. Building on this result, we formulate a nonlinear programming (NLP) problem that jointly optimizes the robot's trajectory and the coefficients of the separating polynomials, enabling geometry-aware collision avoidance without conservative convex simplifications. The optimization remains efficiently solvable using standard NLP solvers. Simulation and real-world experiments with nonconvex robots demonstrate that our method achieves smooth, collision-free, and agile maneuvers in environments where convex-approximation baselines fail.

Figures (7)

• Figure 1: Comparison of collision avoidance constraints for a C-shaped robot in a tight cornering scenario. (a) Proposed method. Separating hypersurfaces (yellow curves) are generated to partition the robot and obstacles within the workspace of interest (dark gray). (b) Corridor-based methods. Relying on convex decompositions of the collision-free space, these methods fail to find a convex free region (yellow region) large enough to contain the robot fully. (c) Separating hyperplane-based methods. These methods enforce a separating hyperplane (yellow dotted line) between each obstacle and the robot's convex hull, resulting in an overly conservative solution space.
• Figure 2: Visualization of a polynomial separating hypersurface for two bounded sets with different shapes.
• Figure 3: Robot models used in simulations and experiments. (a) L-shaped robot model adopted in simulations. (b) Quadruped robot used in the real-world experiments. Its geometric profile (white region) is a non-convex shape composed of an L-shaped frame and a central rectangular body.
• Figure 4: The performance of our proposed method in the narrow passage scenario. (a) Visualization of the L-shaped robot navigating through a passage narrower than its own dimensions. (b)-(c) Illustration of how separating hypersurfaces (yellow curves) work when traversing the narrow passage.
• Figure 5: Illustration of the L-shaped robot navigating through the dense forest scenario via our proposed planner. The RViz snapshots at four representative time instants (A, B, C, D) during execution illustrates how the separating hypersurfaces (yellow curves) function during trajectory optimization. When the robot’s non-convexity is not necessary for collision avoidance, the hypersurfaces naturally degenerates into simple separating hyperplanes.

Theorems & Definitions (3)

• Lemma 1
• Theorem 1
• proof