fixed paths

Author: anishk85

docs/modules/5_path_planning/particleSwarmOptimization/particleSwarmOptimization_main.rst

@@ -0,0 +1,153 @@
+.. _particle_swarm_optimization:
+
+Particle Swarm Optimization Path Planning
+------------------------------------------
+
+This is a 2D path planning implementation using Particle Swarm Optimization (PSO).
+
+PSO is a metaheuristic optimization algorithm inspired by the social behavior of bird flocking or fish schooling. In path planning, particles represent potential solutions that explore the search space to find collision-free paths from start to goal.
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ParticleSwarmOptimization/animation.gif
+
+Algorithm Overview
+++++++++++++++++++
+
+The PSO algorithm maintains a swarm of particles that move through the search space according to simple mathematical rules:
+
+1. **Initialization**: Particles are randomly distributed near the start position
+2. **Evaluation**: Each particle's fitness is calculated based on distance to goal and obstacle penalties
+3. **Update**: Particles adjust their velocities based on:
+   - Personal best position (cognitive component)
+   - Global best position (social component)  
+   - Current velocity (inertia component)
+4. **Movement**: Particles move to new positions and check for collisions
+5. **Convergence**: Process repeats until maximum iterations or goal is reached
+
+Mathematical Foundation
++++++++++++++++++++++++
+
+The core PSO velocity update equation is:
+
+.. math::
+
+   v_{i}(t+1) = w \cdot v_{i}(t) + c_1 \cdot r_1 \cdot (p_{i} - x_{i}(t)) + c_2 \cdot r_2 \cdot (g - x_{i}(t))
+
+Where:
+- :math:`v_{i}(t)` = velocity of particle i at time t
+- :math:`x_{i}(t)` = position of particle i at time t  
+- :math:`w` = inertia weight (controls exploration vs exploitation)
+- :math:`c_1` = cognitive coefficient (attraction to personal best)
+- :math:`c_2` = social coefficient (attraction to global best)
+- :math:`r_1, r_2` = random numbers in [0,1]
+- :math:`p_{i}` = personal best position of particle i
+- :math:`g` = global best position
+
+Position update:
+
+.. math::
+
+   x_{i}(t+1) = x_{i}(t) + v_{i}(t+1)
+
+Fitness Function
+++++++++++++++++
+
+The fitness function combines distance to target with obstacle penalties:
+
+.. math::
+
+   f(x) = ||x - x_{goal}|| + \sum_{j} P_{obs}(x, O_j)
+
+Where:
+- :math:`||x - x_{goal}||` = Euclidean distance to goal
+- :math:`P_{obs}(x, O_j)` = penalty for obstacle j
+- :math:`O_j` = obstacle j with position and radius
+
+The obstacle penalty function is defined as:
+
+.. math::
+
+   P_{obs}(x, O_j) = \begin{cases}
+   1000 & \text{if } ||x - O_j|| < r_j \text{ (inside obstacle)} \\
+   \frac{50}{||x - O_j|| - r_j + 0.1} & \text{if } r_j \leq ||x - O_j|| < r_j + R_{influence} \text{ (near obstacle)} \\
+   0 & \text{if } ||x - O_j|| \geq r_j + R_{influence} \text{ (safe distance)}
+   \end{cases}
+
+Where:
+- :math:`r_j` = radius of obstacle j
+- :math:`R_{influence}` = influence radius (typically 5 units)
+
+Collision Detection
++++++++++++++++++++
+
+Line-circle intersection is used to detect collisions between particle paths and circular obstacles:
+
+.. math::
+
+   ||P_0 + t \cdot \vec{d} - C|| = r
+
+Where:
+- :math:`P_0` = start point of path segment
+- :math:`\vec{d}` = direction vector of path
+- :math:`C` = obstacle center
+- :math:`r` = obstacle radius
+- :math:`t \in [0,1]` = parameter along line segment
+
+Algorithm Parameters
+++++++++++++++++++++
+
+Key parameters affecting performance:
+
+- **Number of particles** (n_particles): More particles = better exploration but slower
+- **Maximum iterations** (max_iter): More iterations = better convergence but slower  
+- **Inertia weight** (w): High = exploration, Low = exploitation
+- **Cognitive coefficient** (c1): Attraction to personal best
+- **Social coefficient** (c2): Attraction to global best
+
+Typical values:
+- n_particles: 20-50
+- max_iter: 100-300
+- w: 0.9 → 0.4 (linearly decreasing)
+- c1, c2: 1.5-2.0
+
+Advantages
+++++++++++
+
+- **Global optimization**: Can escape local minima unlike gradient-based methods
+- **No derivatives needed**: Works with non-differentiable fitness landscapes
+- **Parallel exploration**: Multiple particles search simultaneously
+- **Simple implementation**: Few parameters and straightforward logic
+- **Flexible**: Easily adaptable to different environments and constraints
+
+Disadvantages
++++++++++++++
+
+- **Stochastic**: Results may vary between runs
+- **Parameter sensitive**: Performance heavily depends on parameter tuning
+- **No optimality guarantee**: Metaheuristic without convergence proof
+- **Computational cost**: Requires many fitness evaluations
+- **Prone to stagnation**: Premature convergence where the entire swarm can get trapped in a local minimum if exploration is insufficient
+
+Code Link
++++++++++
+
+.. autofunction:: PathPlanning.ParticleSwarmOptimization.particle_swarm_optimization.main
+
+Usage Example
++++++++++++++
+
+.. code-block:: python
+
+   import matplotlib.pyplot as plt
+   from PathPlanning.ParticleSwarmOptimization.particle_swarm_optimization import main
+   
+   # Run PSO path planning with visualization
+   main()
+
+References
+++++++++++
+
+- `Particle swarm optimization - Wikipedia <https://en.wikipedia.org/wiki/Particle_swarm_optimization>`__
+- Kennedy, J.; Eberhart, R. (1995). "Particle Swarm Optimization". Proceedings of IEEE International Conference on Neural Networks. IV. pp. 1942–1948.
+- Shi, Y.; Eberhart, R. (1998). "A Modified Particle Swarm Optimizer". IEEE International Conference on Evolutionary Computation.
+- `A Gentle Introduction to Particle Swarm Optimization <https://machinelearningmastery.com/a-gentle-introduction-to-particle-swarm-optimization/>`__
+- Clerc, M.; Kennedy, J. (2002). "The particle swarm - explosion, stability, and convergence in a multidimensional complex space". IEEE Transactions on Evolutionary Computation. 6 (1): 58–73.