/matlab_motion_planning

Motion planning and Navigation of AGV/AMR：matlab implementation of Dijkstra, A*, Theta*, JPS, D*, LPA*, D* Lite, RRT, RRT*, RRT-Connect, Informed RRT*, ACO, Voronoi, PID, LQR, MPC, APF, RPP, DWA, DDPG, Bezier, B-spline, Dubins, Reeds-Shepp etc.

Primary LanguageMATLABGNU General Public License v3.0GPL-3.0

Introduction

Motion planning plans the state sequence of the robot without conflict between the start and goal.

Motion planning mainly includes Path planning and Trajectory planning.

  • Path Planning: It's based on path constraints (such as obstacles), planning the optimal path sequence for the robot to travel without conflict between the start and goal.
  • Trajectory planning: It plans the motion state to approach the global path based on kinematics, dynamics constraints and path sequence.

This repository provides the implement of common Motion planning algorithm, welcome your star & fork & PR.

This repository provides the implementation of common Motion Planning algorithms. The theory analysis can be found at motion-planning. Furthermore, we provide ROS C++ and Python version.

Quick Start

The file structure is shown below

├─gif
├─examples
│   ├─simulation_global.mlx
│   ├─simulation_local.mlx
│   ├─simulation_total.mlx
├─global_planner
│   ├─graph_search
│   ├─sample_search
│   └─evolutionary_search
├─local_planner
└─utils

The global planning algorithm implementation is in the folder global_planner with graph_search, sample_search and evolutionary search; The local planning algorithm implementation is in the folder local_planner.

To start simulation, open ./simulation_global.mlx or ./simulation_local.mlx and select the algorithm, for example

clear all;
clc;

% load environment
load("gridmap_20x20_scene1.mat");
map_size = size(grid_map);
G = 1;

% start and goal
start = [3, 2];
goal = [18, 29];

% planner
planner_name = "rrt";

planner = str2func(planner_name);
[path, flag, cost, expand] = planner(grid_map, start, goal);

% visualization
clf;
hold on

% plot grid map
plot_grid(grid_map);
% plot expand zone
plot_expand(expand, map_size, G, planner_name);
% plot path
plot_path(path, G);
% plot start and goal
plot_square(start, map_size, G, "#f00");
plot_square(goal, map_size, G, "#15c");
% title
title([planner_name, "cost:" + num2str(cost)]);

hold off

Version

Global Planner

Planner Version Animation
GBFS Status gbfs_matlab.png
Dijkstra Status dijkstra_matlab.png
A* Status a_star.png
JPS Status jps_matlab.png
Theta* Status theta_star_matlab.png
Lazy Theta* Status lazy_theta_star_matlab.png
D* Status d_star_matlab.gif
LPA* Status Status
D* Lite Status Status
Voronoi Status voronoi_matlab.png
RRT Status rrt_matlab.png
RRT* Status rrt_star_matlab.png
Informed RRT Status informed_rrt_matlab.png
RRT-Connect Status rrt_connect_matlab.png

Local Planner

Planner Version Animation
PID Status pid_matlab.gif
LQR Status lqr_matlab.gif
MPC Status mpc_matlab.gif
APF Status apf_matlab.gif
DWA Status dwa_matlab.gif
RPP Status rpp_matlab.gif
TEB Status Status
MPC Status Status
Lattice Status Status

Intelligent Algorithm

Planner Version Animation
ACO Status aco_matlab.png
GA Status Status
PSO Status Status
ABC Status Status

Curve Generation

Planner Version Animation
Polynomia Status polynomial_curve_matlab.png
Bezier Status bezier_curve_matlab.png
Cubic Spline Status cubic_spline_curve_matlab.png
BSpline Status bspline_curve_matlab.png
Dubins Status dubins_curve_matlab.png
Reeds-Shepp Status reeds_shepp_curve_matlab.png

Papers

Search-based Planning

  • A*: A Formal Basis for the heuristic Determination of Minimum Cost Paths
  • JPS: Online Graph Pruning for Pathfinding On Grid Maps
  • Lifelong Planning A*: Lifelong Planning A*
  • D*: Optimal and Efficient Path Planning for Partially-Known Environments
  • D* Lite: D* Lite
  • Theta*: Theta*: Any-Angle Path Planning on Grids
  • Lazy Theta*: Lazy Theta*: Any-Angle Path Planning and Path Length Analysis in 3D

Sample-based Planning

  • RRT: Rapidly-Exploring Random Trees: A New Tool for Path Planning
  • RRT-Connect: RRT-Connect: An Efficient Approach to Single-Query Path Planning
  • RRT*: Sampling-based algorithms for optimal motion planning
  • Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal heuristic

Evolutionary-based Planning

  • ACO: Ant Colony Optimization: A New Meta-Heuristic

Local Planning

  • DWA: The Dynamic Window Approach to Collision Avoidance
  • APF: Real-time obstacle avoidance for manipulators and mobile robots
  • RPP: Regulated Pure Pursuit for Robot Path Tracking

Curve Generation

  • Dubins: On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents