Topology-Driven Parallel Trajectory Optimization in Dynamic Environments

TL;DR

This work proposes a novel topology-driven trajectory optimization strategy for dynamic environments that plans multiple distinct evasive trajectories to enhance the robot's behavior and efficiency and demonstrates on a mobile robot navigating among pedestrians that this approach leads to faster trajectories than existing planners.

Abstract

Ground robots navigating in complex, dynamic environments must compute collision-free trajectories to avoid obstacles safely and efficiently. Nonconvex optimization is a popular method to compute a trajectory in real-time. However, these methods often converge to locally optimal solutions and frequently switch between different local minima, leading to inefficient and unsafe robot motion. In this work, We propose a novel topology-driven trajectory optimization strategy for dynamic environments that plans multiple distinct evasive trajectories to enhance the robot's behavior and efficiency. A global planner iteratively generates trajectories in distinct homotopy classes. These trajectories are then optimized by local planners working in parallel. While each planner shares the same navigation objectives, they are locally constrained to a specific homotopy class, meaning each local planner attempts a different evasive maneuver. The robot then executes the feasible trajectory with the lowest cost in a receding horizon manner. We demonstrate, on a mobile robot navigating among pedestrians, that our approach leads to faster and safer trajectories than existing planners.

Figures (15)

• Figure 1: T-MPC first computes distinct guidance trajectories in the state space (time is visualized in the upwards direction). Each guidance trajectory initializes a local planner, resulting in several distinct locally optimized trajectories. The locally optimized trajectories each pass the obstacles (predicted future motion visualized as cylinders) in a distinct way.
• Figure 2: (a) Depiction of the planning problem and (b) equivalent in the state-space. Trajectory $1$ and $2$ are in the same homotopy class while trajectory $1$ and $3$ are in distinct homotopy classes.
• Figure 3: Schematic of T-MPC. An environment with several obstacles and a robot is visualized in $x, y, t$ (time in the upwards axis). Obstacle motion predictions are denoted with cylinders. (1) A guidance planner (Sec. \ref{['sec:global_planner']}) finds $P=4$ trajectories (visualized with colored lines) from the robot initial position to one of the goals. Each of these trajectories is in a distinct homotopy class in the state space. (2) Each trajectory guides a local planner (Sec. \ref{['sec:local_planner']}) as initial guess and through a set of homotopy constraints. Four guidance trajectories and optimized trajectories (as occupied regions for each step) are visualized. (3) The optimized trajectories are compared through their objective value (Sec. \ref{['sec:decision_making']}) and a single trajectory (in red) is excuted by the robot.
• Figure 4: Illustration of the guidance planner in the state-space (time in the upwards axis). Visualization follows Fig. \ref{['fig:method-schematic']}. (a) The visibility-PRM graph (black lines and dots) explores the free space toward the goals placed at $t=T$ around the reference path (orange dots). The homotopy distinct guidance paths (colored lines) are obtained by searching the graph. (b) The final trajectories are smoothened.
• Figure 5: Two distinct locally planned trajectories for a robot (black dot) evading an obstacle (black region and dot) that is static ($\boldsymbol{o}_k = \boldsymbol{o}_0, \ k = 1, \hdots, N$). For both planners, we depict the topology constraints for each time step in their respective colors showing the constraint boundaries (broken lines) and their feasible region (colored regions with increasing transparency over time).

Theorems & Definitions (9)

• Definition 1
• Definition 2
• Definition 3
• Theorem 1
• proof
• Theorem 2
• proof
• Definition 4
• Definition 5