This may be the most comprehensive collection of Python algorithms ever!

EEWorld

Sharp interpretation of electronic information

Technical content updated daily

This article is a collection of Python codes for some robot algorithms (especially automatic navigation algorithms). Its main features are as follows: it selects algorithms that are widely used in practice; it has minimal dependencies; it is easy to read and understand the basic ideas of each algorithm. I hope that reading this article will be helpful to you.

Friendly reminder: the article is quite long, please save it for later reading.

Table of contents

Environmental requirements

how to use

Localization

• Extended Kalman Filter Localization

• Lossless Kalman Filter Localization

• Particle filter localization

• Histogram Filter Localization

Mapping

• Gaussian Grid Mapping

• Raycasting Mesh Mapping

• k-means object clustering

• Circular fitting object shape recognition

SLAM

• Iterative Closest Point Matching

• EKF SLAM

• FastSLAM 1.0

• FastSLAM 2.0

• Graph-based SLAM

route plan

• Dynamic Window Mode

• Grid-based search

• Dijkstra's algorithm

• A* Algorithm

• Potential Field Algorithm

• Model prediction path generation

• Path Optimization Example

• Lookup Table Generation Example

• State lattice planning

• Uniform polar sampling

• Biased polar sampling

• Lane sampling

• Probabilistic Path Graph (PRM) Planning

• Voronoi Path Diagram Planning

• Rapid Randomized Trees (RRT)

• Basic RRT

• RRT*

• RRT based on Dubins pathway

• RRT based on Dubins pathway*

• RRT based on the Reeds-Shepp pathway*

• Informed RRT*

• Batch Informed RRT*

• Cubic spline programming

• B-spline planning

• Beziers route planning

• Quintic Polynomial Programming

• Dubins Path Planning

• Reeds Shepp Path Planning

• Path planning based on LQR

• Optimal Path in Frenet Frame

Path Tracing

• Pure tracking

• Stanley Control

• Rear wheel feedback control

• Linear quadratic regulator (LQR) steering control

• Linear quadratic regulator (LQR) steering and speed control

project support

Environmental requirements

• Python 3.6.x

• numpy

• scipy

• matplotlib

• pandas

• cvxpy 0.4.x

how to use

1. Install necessary libraries;

2. Clone this code repository;

3. Execute the python script in each directory;

4. If you like it, please bookmark this code repository :)

Localization

Extended Kalman Filter Localization

The algorithm uses Extended Kalman Filter (EKF) to achieve sensor hybrid localization.

The blue line is the true path, the black line is the dead reckoning trajectory, the green dot is the position observation (such as GPS), and the red line is the path estimated by EKF.

The red ellipse is the covariance estimated by EKF.

Related Reading:

Probabilistic Robotics

http://www.probabilistic-robotics.org/

Lossless Kalman Filter Localization

The algorithm uses Unscented Kalman Filter (UKF) to achieve sensor hybrid localization.

The meanings of the lines and points are the same as in the EKF simulation example.

Related Reading:

Robotic motion localization using an undifferentiated trained lossless Kalman filter

https://www.researchgate.net/publication/267963417_Discriminatively_Trained_Unscented_Kalman_Filter_for_Mobile_Robot_Localization

Particle filter localization

The algorithm uses particle filter (PF) to achieve sensor hybrid localization.

The blue line is the real path, the black line is the dead reckoning trajectory, the green dot is the position observation (such as GPS), and the red line is the path estimated by PF.

The algorithm assumes that the robot is able to measure the distance to the landmarks (RFID).

This measurement is used for PF localization.

Related Reading:

Probabilistic Robotics

http://www.probabilistic-robotics.org/

Histogram Filter Localization

This algorithm is an example of using a histogram filter to achieve two-dimensional localization.

The red cross is the actual location and the black dot is the location of the RFID.

The blue grids are the probability locations of the histogram filters.

In this simulation, x, y are unknown and yaw is known.

The filter integrates the velocity input and the distance observation data obtained from the RFID for localization.

No initial position is required.

Related Reading:

Probabilistic Robotics

http://www.probabilistic-robotics.org/

Mapping

Gaussian Grid Mapping

This algorithm is an example of two-dimensional Gaussian grid mapping.

Raycasting Mesh Mapping

This algorithm is an example of a 2D ray casting grid map.

k-means object clustering

This algorithm is an example of using the k-means algorithm to cluster two-dimensional objects.

Circular fitting object shape recognition

This algorithm is an example of using circle fitting for object shape recognition.

The blue circle is the actual object shape.

The red crosses are points observed by the distance sensor.

The red circle is the object shape estimated using circle fitting.

SLAM

Example of Simultaneous Localization and Mapping (SLAM).

Iterative Closest Point Matching

This algorithm is an example of two-dimensional iterative closest point (ICP) matching using single value deconstruction.

It can calculate the rotation matrix and translation matrix from some points to other points.

Related Reading:

Introduction to Robotic Motion: Iterative Closest Point Algorithm

https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf

EKF SLAM

This is an example of SLAM based on the Extended Kalman Filter.

The blue line is the true path, the black line is the navigation guessed path, and the red line is the path estimated by EKF SLAM.

Green crosses are estimated landmarks.

Related Reading:

Probabilistic Robotics

http://www.probabilistic-robotics.org/

FastSLAM 1.0

This is an example of feature-based SLAM using FastSLAM 1.0.

The blue line is the actual path, the black line is the navigation guess, and the red line is the guessed path of FastSLAM.

The red dots are particles in FastSLAM.

The black dots are landmarks, and the blue crosses are the landmark positions estimated by FastLSAM.

Related Reading:

Probabilistic Robotics

http://www.probabilistic-robotics.org/

FastSLAM 2.0

This is an example of feature-based SLAM with FastSLAM 2.0.

The meaning of animation is the same as in the case of FastSLAM 1.0.

Related Reading:

Probabilistic Robotics

http://www.probabilistic-robotics.org/

Tim Bailey's SLAM simulation

http://www-personal.acfr.usyd.edu.au/tbailey/software/slam_simulations.htm

Graph-based SLAM

This is an example of graph-based SLAM.

The blue line is the actual path.

The black line is the navigation guess path.

The red line is the path estimated by graph-based SLAM.

The black stars are landmarks used to generate the edges of the graph.

Related Reading:

Getting Started with Graph-Based SLAM

http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf

route plan

Dynamic Window Mode

This is sample code for 2D navigation using the Dynamic Window Approach.

Related Reading:

Avoid collisions with dynamic windows

https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf

Grid-based search

Dijkstra's algorithm

This is the shortest path planning based on two-dimensional grid using Dijkstra algorithm.

The cyan dots in the animation are searched nodes.

A* Algorithm

Below is the shortest path planning based on a two-dimensional grid using the A-star algorithm.

The cyan dots in the animation are searched nodes.

The heuristic algorithm is two-dimensional Euclidean distance.

Potential Field Algorithm

Below is the path planning based on a two-dimensional grid using the potential field algorithm.

The blue heat map in the animation shows the potential energy of each grid.

Related Reading:

Robotic Motion Planning: Potential Energy Functions

https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf

Model prediction path generation

Below is an example of path optimization generated by the model's predicted path.

The algorithm is used for state lattice planning.

Path Optimization Example

Lookup Table Generation Example

Related Reading:

Optimal rough terrain path generation for wheeled robots

http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328

State lattice planning

This script uses state lattice planning to implement path planning.

This code solves the boundary problem by generating model prediction paths.

Related Reading:

Optimal rough terrain path generation for wheeled robots

http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328

State-space sampling of feasible motions for high-performance locomotion robot navigation in complex environments

http://www.frc.ri.cmu.edu/~alonzo/pubs/papers/JFR_08_SS_Sampling.pdf

Uniform polar sampling

Biased polar sampling

Lane sampling

Probabilistic Path Graph (PRM) Planning

This Probabilistic Road-Map (PRM) planning algorithm uses Dijkstra's method for graph search.

The blue dots in the animation are sampling points.

The cyan crosses are the points that have been searched by the Dijkstra method.

The red line is the final path of the PRM.

Related Reading:

Random Path Graph

https://en.wikipedia.org/wiki/Probabilistic_roadmap

Voronoi Path Diagram Planning

This Voronoi Probabilistic Road-Map (PRM) planning algorithm uses Dijkstra's method for graph search.

The blue points in the animation are Voronoi points.

The cyan crosses are the points that have been searched by the Dijkstra method.

The red line is the final path of the Voronoi path diagram.

Related Reading:

Robot motion planning

https://www.cs.cmu.edu/~motionplanning/lecture/Chap5-RoadMap-Methods_howie.pdf

Rapid Randomized Trees (RRT)

Basic RRT

This is a simple path planning code using Rapidly-Exploring Random Trees (RRT).

The black circle is the obstacle, the green line is the search tree, and the red cross is the starting position and the target position.

RRT*

Here is the path planning code using RRT*.

The black circle is the obstacle, the green line is the search tree, and the red cross is the starting position and the target position.

Related Reading:

Incremental sampling based algorithm for optimal motion planning

https://arxiv.org/abs/1005.0416

Sampling-based algorithm for optimal motion planning

http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.419.5503&rep=rep1&type=pdf

RRT based on Dubins path

A path planning algorithm using RRT and Dubins path planning is provided for a car-shaped robot.

RRT based on Dubins pathway*

Path planning algorithm using RRT* and Dubins path planning for a car-shaped robot.

RRT based on the Reeds-Shepp pathway*

Path planning algorithm for car-shaped robots using RRT* and Reeds Shepp path planning.

Informed RRT*

This is the path planning code using Informed RRT*.

The cyan ellipse is the inspiration sampling domain of Informed RRT*.

Related Reading:

Informed RRT*: Optimal sampling-based path planning via direct sampling of admissible ellipsoids

https://arxiv.org/pdf/1404.2334.pdf

Batch Informed RRT*

This is the path planning code using batch Informed RRT*.

Related Reading:

Batch Informed Trees (BIT*): Sampling-based optimal planning via heuristic search of implicit random geometries

https://arxiv.org/abs/1405.5848

Closed loop RRT*

Vehicle model-based path planning using Closed loop RRT*.

In this code, the steering control uses a pure-pursuit algorithm.

The speed control adopts PID.

Related Reading:

Using closed-loop prediction to enable motion planning in complex environments

http://acl.mit.edu/papers/KuwataGNC08.pdf)

Real-time motion planning for autonomous urban driving

http://acl.mit.edu/papers/KuwataTCST09.pdf

[1601.06326] A sampling-based algorithm for optimal motion planning using closed-loop prediction

https://arxiv.org/abs/1601.06326

LQR-RRT*

This is a path planning simulation using LQR-RRT*.

LQR local planning adopts a double integral motion model.

Related Reading:

LQR-RRT*: Optimal sampling-based motion planning using automatically derived extended heuristics

http://lis.csail.mit.edu/pubs/perez-icra12.pdf

MahanFathi/LQR-RRTstar: LQR-RRT* method for random motion planning in pendulum phases

https://github.com/MahanFathi/LQR-RRTstar

Cubic spline programming

This is a sample code for cubic path planning.

This code uses cubic splines to generate a curvature-continuous path based on xy waypoints.

The pointing angle of each point can also be calculated analytically.

B-spline planning

This is an example of using B-spline curves for planning.

Enter the waypoints and it will generate a smooth path using B-splines.

The first and last waypoints are located on the final path.

Related Reading:

B-spline

https://en.wikipedia.org/wiki/B-spline

Et a^ 3 spline path planning

This is path planning using Eta^3 splines.

Related Reading:

eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots

https://ieeexplore.ieee.org/document/4339545/

Beziers route planning

Example code for Beziers path planning.

Generate a Béziers path based on four control points.

By changing the offset distance of the start and end points, different Beziers paths can be generated:

Related Reading:

Generate curvature-continuous paths for autonomous vehicles based on Bezier curves

http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.294.6438&rep=rep1&type=pdf

Quintic Polynomial Programming

Path planning is performed using quintic polynomials.

It can calculate two-dimensional paths, velocities, and accelerations based on quintic polynomials.

Related Reading:

Local Path Planning and Motion Control for Agv In Positioning

http://ieeexplore.ieee.org/document/637936/

Dubins Path Planning

Example code for Dubins path planning.

Related Reading:

Dubins Path

https://en.wikipedia.org/wiki/Dubins_path

Reeds Shepp Path Planning

Example code for Reeds Shepp path planning.

Related Reading:

15.3.2 Reeds-Shepp Curve

http://planning.cs.uiuc.edu/node822.html

Optimal path for a car that can move forward and backward

https://pdfs.semanticscholar.org/932e/c495b1d0018fd59dee12a0bf74434fac7af4.pdf

ghliu/pyReedsShepp: Implements the Reeds Shepp curve

https://github.com/ghliu/pyReedsShepp

Path planning based on LQR

Example code using LQR-based path planning for a double-integration model.

Optimal Path in Frenet Frame

This code generates the optimal path in Frenet Frame.

The cyan line is the target path and the black crosses are obstacles.

The red line is the predicted path.

Related Reading:

Optimal path generation in the dynamic scene in Frenet Frame

https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf

Optimal path generation in the dynamic scene in Frenet Frame

https://www.youtube.com/watch?v=Cj6tAQe7UCY

Path Tracing

Posture control tracking

This is an analogue of gesture control tracking.

Related Reading:

Robotics, Vision and Control - Fundamental Algorithms In MATLAB® Second, Completely Revised, Extended And Updated Edition | Peter Corke | Springer

https://www.springer.com/us/book/9783319544120

Pure tracking

Path following simulation using pure pursuit steering control and PID speed control.

The red line is the target route, the green cross is the target point of pure tracking control, and the blue line is the tracking route.

Related Reading:

A survey of motion planning and control techniques for autonomous vehicles in cities

https://arxiv.org/abs/1604.07446

Stanley Control

Path following simulation using Stanley steering control and PID speed control.

Related Reading:

Stanley: The robot that won the DARPA Grand Prix

http://robots.stanford.edu/papers/thrun.stanley05.pdf

Automatic steering method for path tracking of autonomous vehicles

https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf

Rear wheel feedback control

Path following simulation using rear wheel feedback steering control and PID speed control.

Related Reading:

A survey of motion planning and control techniques for autonomous vehicles in cities

https://arxiv.org/abs/1604.07446

Linear quadratic regulator (LQR) steering control

Path following simulation using LQR steering control and PID speed control.

Related Reading:

ApolloAuto/apollo: open source autonomous driving platform

https://github.com/ApolloAuto/apollo

Linear quadratic regulator (LQR) steering and speed control

Path following simulation using LQR steering and speed control.

Related Reading:

Fully Automated Driving: Systems and Algorithms - IEEE Conference Publication

http://ieeexplore.ieee.org/document/5940562/

Model Predicts Speed ​​and Steering Control

Path-following simulations using iterative linear models to predict steering and speed control.

This code uses cxvxpy as the optimal modeling tool.

Related Reading:

Vehicle Dynamics and Control | Rajesh Rajamani | Springer

http://www.springer.com/us/book/9781461414322

MPC Course Materials - MPC Lab @ UC-Berkeley

http://www.mpc.berkeley.edu/mpc-course-material

Source: csdn

Focus on industry hot spots and understand the latest frontiers

Please pay attention to EEWorld electronic headlines

https://www.eeworld.com.cn/mp/wap

Copy this link to your browser or long press the QR code below to browse

The following WeChat public accounts belong to

EEWorld (www.eeworld.com.cn)

Welcome to long press the QR code to follow us!

EEWorld Subscription Account: Electronic Engineering World

EEWorld Service Account: Electronic Engineering World Welfare Club