Topology-Driven Trajectory Optimization for Modelling Controllable Interactions Within Multi-Vehicle Scenario

TL;DR

The paper tackles the challenge of controllable interactions in multi-vehicle trajectory optimization by introducing a differentiable local homotopy invariant metric that encodes topological relations near obstacles. This metric is integrated as a penalty to transform the constrained topology problem into an unconstrained optimization, enabling multiple interaction patterns from the same initial values. A bi-level optimization framework is developed, where an inner problem computes the closest approach time between vehicles and the outer problem optimizes the full trajectories with topology penalties, aided by gradient calculations via KKT conditions. To mitigate conflicts between topology and collision avoidance, a two-stage optimization strategy is used: first enforce topology, then incorporate collision constraints for final refinement. The framework demonstrates superior optimality and efficiency in simulations and real-world experiments, and the authors provide open-source code to facilitate broader adoption and further research.

Abstract

Trajectory optimization in multi-vehicle scenarios faces challenges due to its non-linear, non-convex properties and sensitivity to initial values, making interactions between vehicles difficult to control. In this paper, inspired by topological planning, we propose a differentiable local homotopy invariant metric to model the interactions. By incorporating this topological metric as a constraint into multi-vehicle trajectory optimization, our framework is capable of generating multiple interactive trajectories from the same initial values, achieving controllable interactions as well as supporting user-designed interaction patterns. Extensive experiments demonstrate its superior optimality and efficiency over existing methods. We will release open-source code to advance relative research.

Figures (7)

• Figure 1: Snapshots of four vehicles navigating at the interaction area. The arrows point to the directions that the vehicles are moveing towards. The colored curves are trajectories of each vehicle. In order to avoid collision, the vehicles move counterclockwise relative to each other at t=6s. This illustrates an interaction pattern within multi-vehicle scenario.
• Figure 2: Illustration of trajectories under different homotopy classes with different winding angles. The green and blue trajectories navigate above the obstacle, with a common winding angle of $\theta$. The red trajectory navigates below the obstacle, with a winding angle of $2\pi-\theta$. The winding angles are represented by the dashed lines with arrows. All trajectories navigate from right to left, sharing the same start and goal.
• Figure 3: This figure illustrates that the topological structure between two moving vehicles is equivalent to that between a moving vehicle and a static obstacle. The left figure describes the absolute trajectories of two moving vehicles, whereas the right figure describes the relative trajectory by fixing the yellow vehicle.
• Figure 4: Illustration of a gradual trajectory deformation process from (a) to (d). For simplicity, the obstacle is represented by its center O. Point A is the key point, and point B is a trajectory point slightly later than point A. In (a) and (b), the key point A moves clockwise around the obstacle to point B. In (c), the key point coincides with the obstacle center. In (d), the key point A moves counterclockwise around the obstacle to point B.
• Figure 5: Illustration of penalty terms. Two surfaces in the left figure respectively represent the obstacle avoidance penalty and topological constraint penalty. Right figure is a projection of the two surfaces onto the XOZ plane. The red dotted circle in the right figure is a bad local minimum caused by the conflict between obstacle avoidance and topological constraints.