Leveraging Swarm Intelligence to Drive Autonomously: A Particle Swarm Optimization based Approach to Motion Planning

TL;DR

The paper tackles real-time motion planning for autonomous vehicles in dynamic, multi-modal environments. It proposes a PSO-based trajectory optimization framework operating on time-discrete SE2 trajectories, using a polar control space to enable smooth, kinematically consistent interpolation and straightforward integration with perception and localization. A multi-faceted cost function balances safety and comfort across terms like obstacle clearance, driving-area adherence, and smoothness, while a set of adaptations improves initialization, sampling, and continuity to handle hard constraints in complex traffic. The system is implemented in a modular, top-down architecture with lanelet-based routing and costmap-derived obstacles, and is validated in two real-world suburban scenarios, achieving planning frequencies above $20$ Hz on commodity hardware and accumulating over $3{,}500$ km of autonomous shuttle operation, demonstrating both effectiveness and practicality.

Abstract

Motion planning is an essential part of autonomous mobile platforms. A good pipeline should be modular enough to handle different vehicles, environments, and perception modules. The planning process has to cope with all the different modalities and has to have a modular and flexible design. But most importantly, it has to be safe and robust. In this paper, we want to present our motion planning pipeline with particle swarm optimization (PSO) at its core. This solution is independent of the vehicle type and has a clear and simple-to-implement interface for perception modules. Moreover, the approach stands out for being easily adaptable to new scenarios. Parallel calculation allows for fast planning cycles. Following the principles of PSO, the trajectory planer first generates a swarm of initial trajectories that are optimized afterward. We present the underlying control space and inner workings. Finally, the application to real-world automated driving is shown in the evaluation with a deeper look at the modeling of the cost function. The approach is used in our automated shuttles that have already driven more than 3.500 km safely and entirely autonomously in sub-urban everyday traffic.

Figures (4)

• Figure 1: Our PSO planer implementation in action in a sub-urban area. The orange line indicates the particles used for the initialization and the green represent the trajectories after optimization.
• Figure 2: Interpolation between two trajectories (blue and grey). The green one is interpolated using curvature controls, the orange one with Cartesian interpolation.
• Figure 3: The inputs of the PSO. The environment is modeled by static and dynamic obstacles represented as polygon features. Maneuver decisions are passed via the driving area and the driving mode.
• Figure 4: Depiction and statistics of scenarios I (images \ref{['fig:neckar_rviz']},\ref{['fig:neckar_cost']},\ref{['fig:neckar_timing']}) and II (images \ref{['fig:freiburger_rviz']},\ref{['fig:freiburger_cost']},\ref{['fig:freiburger_timing']}). The scene with current trajectory as vehicle contours (green), the particles (orange) and underlying occupancy grid is shown in (\ref{['fig:neckar_rviz']}) and (\ref{['fig:freiburger_rviz']}). The normalized individual cost terms over time are depicted in (\ref{['fig:neckar_cost']}) and (\ref{['fig:freiburger_cost']}). Finally, the valid particles over time are given in (\ref{['fig:neckar_timing']}) and (\ref{['fig:freiburger_timing']}). The number of valid particles for the initialization phase is presented in blue and after the optimization in orange.