Path Planning: Particle Swarm Optimization

[guilyx]

Summary

Rework PSO path planner from a global path-optimizer to a swarm-navigation approach matching the classic Kennedy & Eberhart formulation:

• Each particle is a 2D agent that physically navigates from start toward the goal (previously each particle encoded an entire candidate path as a flat waypoint vector)
• Fitness = Euclidean distance to goal + soft obstacle proximity penalty (previously path length + collision penalty per segment)
• Collision handling via velocity deflection (bounce-back at −0.3×) instead of abstract segment checks
• The global-best particle's trail of positions forms the planned path (previously decoded from optimised waypoint vectors)
• Early termination when global best reaches goal within tolerance
• Tuned for convergence: max_vel=3.0, c1=1.8, c2=2.0, spawn_radius=8.0, inertia decay 0.9→0.4
• Search GIF now shows particles swarming through the map with fading trails (blue=free, red=blocked), global-best trail in green, at 8 fps for clear step-by-step visibility

Test plan

• test_pso_path_planning.py passes (planner finds path, simulation completes)