robot-path-planner

A Python collection of robotics algorithms for localization, mapping, SLAM, path planning, path tracking, arm navigation, aerial navigation and bipedal motion. Every algorithm is a small, readable script with an animated simulation, so you can run it, watch it work, and learn how it behaves.

Maintained by Brian Kipruto Chepchieng (@Brian44ki).

This project is built on the open-source PythonRobotics project by Atsushi Sakai and contributors, released under the MIT License. See Acknowledgements and License.

• About
• Requirements
• Getting Started
• Localization
• Mapping
• SLAM
• Path Planning
• Path Tracking
• Arm Navigation
• Aerial Navigation
• Bipedal
• Contributing
• Acknowledgements and License
• Author

robot-path-planner is a hands-on reference for how robots find out where they are, build maps, plan routes and follow them.

Goals:

1. Easy to read. Each algorithm's core idea is visible in the code.
2. Practical. Widely used, well-established algorithms.
3. Minimal dependencies. NumPy, SciPy, Matplotlib and cvxpy.

For the mathematics behind many of the algorithms, see the original textbook: PythonRobotics documentation.

To run the sample code:

• Python 3.13.x
• NumPy
• SciPy
• Matplotlib
• cvxpy

For development:

• pytest and pytest-xdist (unit tests)
• mypy (type checking)
• sphinx (documentation)
• pycodestyle (code style)

1. Clone this repository:

   git clone https://github.com/Brian44ki/robot-path-planner.git
   cd robot-path-planner

2. Install the required libraries.

   Using conda:

   conda env create -f requirements/environment.yml

   Using pip:

   pip install -r requirements/requirements.txt

3. Run the Python script inside any algorithm's folder, for example:

   python PathPlanning/AStar/a_star.py

4. If you find this useful, a ⭐ on the repo is appreciated.

Reference: documentation

An extended Kalman filter fuses body-frame accelerometer and gyroscope readings with lower-rate GPS positions, estimates sensor biases, and keeps predicting from inertial data during a temporary GPS outage. The animation compares it with an EKF without bias estimation and with IMU-only dead reckoning on the same measurements. It also plots the accelerometer and gyroscope bias estimates against their true values, with covariance-derived 3σ position ellipses and bias uncertainty bands.

• documentation
• sample code

Sensor-fusion localization with a particle filter (PF). The blue line is the true trajectory, the black line is dead reckoning, and the red line is the PF estimate. The robot measures its distance to landmarks (RFID tags), and those measurements drive the filter.

Reference: Probabilistic Robotics

2D localization with a histogram filter. The red cross is the true position, the black points are RFID positions, and the blue grid shows the position probability. In this simulation x and y are unknown while yaw is known. The filter integrates speed input and RFID range observations, and no initial position is needed.

Reference: Probabilistic Robotics

2D Gaussian grid mapping.

2D ray casting grid mapping.

Converting 2D range measurements into a grid map.

2D object clustering with the k-means algorithm.

2D rectangle fitting for vehicle detection.

Simultaneous Localization and Mapping examples.

2D ICP matching using singular value decomposition. It computes the rotation matrix and translation vector between two point sets.

Reference: Introduction to Mobile Robotics: Iterative Closest Point Algorithm

Feature-based SLAM using FastSLAM 1.0. The blue line is ground truth, the black line is dead reckoning, and the red line is the FastSLAM trajectory estimate. Red points are particles, black points are landmarks, and blue crosses are estimated landmark positions.

References:

• Probabilistic Robotics
• SLAM simulations by Tim Bailey

2D navigation with the Dynamic Window Approach.

Reference: The Dynamic Window Approach to Collision Avoidance

2D grid-based shortest path planning with Dijkstra's algorithm. In the animation, cyan points are searched nodes.

2D grid-based shortest path planning with A*. Cyan points are searched nodes, and the heuristic is the 2D Euclidean distance.

2D grid-based path planning with D*. The animation shows a robot finding its path while avoiding an obstacle.

Reference: D* algorithm (Wikipedia)

Finds the shortest path between two points and reroutes when new obstacles are discovered, implemented for a 2D grid.

References:

• D* Lite
• Improved Fast Replanning for Robot Navigation in Unknown Terrain

2D grid-based path planning with potential fields. The blue heat map shows the potential value of each grid cell.

Reference: Robotic Motion Planning: Potential Functions

2D grid-based coverage path planning simulation.

2D path planning with Particle Swarm Optimization, a metaheuristic inspired by bird flocking. Particles explore the search space to find collision-free paths. In the animation, particles (blue dots) converge on the optimal path (yellow line) from the start (green area) to the goal (red star).

References:

• Particle swarm optimization (Wikipedia)
• Kennedy and Eberhart (1995), "Particle Swarm Optimization"

Path planning with state lattices, using a model predictive trajectory generator to solve the boundary problem.

References:

• Optimal rough terrain trajectory generation for wheeled mobile robots
• State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments

This PRM planner uses Dijkstra's method for graph search. Blue points are sampled points, cyan crosses are points searched by Dijkstra, and the red line is the final path.

Reference: Probabilistic roadmap (Wikipedia)

Path planning with RRT*. Black circles are obstacles, the green line is the search tree, and red crosses are the start and goal.

References:

• Incremental Sampling-based Algorithms for Optimal Motion Planning
• Sampling-based Algorithms for Optimal Motion Planning

Path planning for a car-like robot with RRT* and the Reeds-Shepp path planner.

Path planning with LQR-RRT*, using a double integrator motion model for the LQR local planner.

References:

• LQR-RRT*: Optimal Sampling-Based Motion Planning with Automatically Derived Extension Heuristics
• MahanFathi/LQR-RRTstar

Motion planning with quintic polynomials. It computes a 2D path along with velocity and acceleration profiles.

Reference: Local Path Planning And Motion Control For AGV In Positioning

References:

• 15.3.2 Reeds-Shepp Curves
• Optimal paths for a car that goes both forwards and backwards
• ghliu/pyReedsShepp

LQR-based path planning for a double integrator model.

Optimal trajectory generation in a Frenet frame. The cyan line is the target course, black crosses are obstacles, and the red line is the predicted path.

References:

• Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame
• Video: Optimal trajectory generation for dynamic street scenarios in a Frenet Frame

Reference: P. I. Corke, Robotics, Vision and Control, p. 102

Path tracking with Stanley steering control and PID speed control.

References:

• Stanley: The robot that won the DARPA Grand Challenge
• Automatic Steering Methods for Autonomous Automobile Path Tracking

Path tracking with rear wheel feedback steering control and PID speed control.

Reference: A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles

Path tracking with LQR speed and steering control.

Reference: Towards fully autonomous driving: Systems and algorithms

Path tracking with iterative linear model predictive speed and steering control.

References:

• documentation
• Real-time Model Predictive Control (MPC), ACADO, Python

Motion planning and path tracking with nonlinear MPC using C-GMRES.

Reference: documentation

Interactive N-joint arm control. Left-click on the plot to set the goal position of the end effector. Here N = 10, but you can change it.

3D trajectory following for a quadrotor.

3D trajectory generation for a rocket powered landing.

Reference: documentation

A bipedal planner that modifies footsteps for an inverted pendulum. Set the footsteps and the planner adjusts them automatically.

Issues and pull requests are welcome. If you spot a bug, have an improvement, or want to add an algorithm:

1. Fork the repository
2. Create a feature branch (git checkout -b feature/my-change)
3. Commit your changes and open a pull request

This project is built on PythonRobotics by Atsushi Sakai and its contributors. Their work provides the algorithm implementations and animations used here, and the animation GIFs are hosted in PythonRoboticsGifs.

If you use this code in academic work, please cite the original paper:

Atsushi Sakai et al., PythonRobotics: a Python code collection of robotics algorithms, arXiv:1808.10703

License: MIT. The original copyright notice and license text are retained in the LICENSE file, as the license requires.

Brian Kipruto Chepchieng GitHub: @Brian44ki