The A* algorithm is a discrete path planner that can work on grid maps by creating weighted graphs connecting the start and goal nodes. A* works on a discretized grid map and expands the search tree using x-y linear connections. The nodes are explored one by one based on a cost function that estimates the cost to travel from node to node.

How It Works

To find the path with the least cost, A* works to minimize the cost function f(n):

f(n) = g(n) + h(n)

where

• n is the next node on the path
• g(n) is the cost of the path from the start node to n
• h(n) is a heuristic function that estimates the cost of the cheapest path from n to goal

A* Algorithm in MATLAB

plannerAStarGrid in Navigation Toolbox provides most commonly used heuristics, such as Euclidean and Manhattan. You can use either the predefined cost functions or custom cost functions for h(n) and g(n). You can specify either a binaryOccupancyMap or occupancyMap object as an input to the planner.

See A* example

Applications

The A* algorithm is directly applicable to omnidrive wheeled robots. A* is also popular for computer-based applications such as pathfinding in video games.

Hybrid A* is an extension of A*. Like A*, Hybrid A* works in a discretized search space, but it associates each grid cell with a continuous 3D state (x, y, θ) of a vehicle. It uses a continuous state space consisting of motion primitives to generate smooth and drivable paths.

How It Works

Hybrid A* uses efficient guiding heuristics that make the search tree expand in the direction of the goal. It also uses analytic expansion of the path, which improves accuracy and reduces planning time.

Hybrid A generates a smooth and drivable path for a vehicle based on the exact goal pose provided.

Hybrid A* in MATLAB

Navigation Toolbox provides plannerHybridAStar, which uses a state validator such as validatorOccupancyMap or validatorVechicleCostmap for collision checking in the occupancy map and includes costs and parameters for tuning specific to your application.

See Hybrid A* example

Applications

Unlike the traditional A*, hybrid A* is suitable for vehicles with nonholonomic constraints, such as self-driving cars. It guarantees kinematic feasibility and takes differential constraints—the orientation and velocity of the vehicle—into account.

Search-based algorithms are not suitable for applications with high degrees of freedom or those in which the map size is very large. Storing data from a large grid map is computationally expensive. PRM is a sampling-based planning algorithm which is helpful in such cases.

How It Works

PRM is a network graph of different possible paths in a map. The graph is generated by a limited number of random points or nodes within a given area. After randomly sampling each node, the PRM algorithm creates multiple clusters of nodes by connecting all the nodes within a fixed radius.

Once the roadmap has been constructed, a path can be queried from a given start location to a given goal location on the map. Since PRM allows multiple queries in the same road map for different start and goal locations, it saves computation time if the map is static (does not change over time). PRM uses a graph search algorithm such as A* planner to search for the path inside the road map it creates.

PRM in MATLAB

Robotics System Toolbox™ provides mobileRobotPRM, which takes a binaryOccupancyMap as input and creates a road map in the free space of the map by connecting the randomly sampled nodes. You can use the findpath function to query for a path from a start to a goal location. Since PRM does not consider the dimension of the vehicle while finding an obstacle-free path, you can inflate the map by the vehicle’s radius by using the inflate function.

See PRM example

Applications

The PRM implementation in MATLAB is suitable for holonomic mobile robot applications such as a static warehouse environment with no unknown obstacles, where you want to change the start and goal locations without the robot’s having to relearn the whole map.

RRT is a sampling-based algorithm suitable for non-holonomic constraints. The RRT algorithm efficiently searches nonconvex high-dimensional spaces. It creates a search tree incrementally by using random samples in a defined state space. The search tree eventually spans the search space and connects the start state to the goal state.

How It Works

The RRT planner grows the search tree rooted at the start state X_start following these steps:

1. The planner samples a random state X_rand in the state space.
2. The planner finds a state X_near that is already in the search tree and is closest to X_rand (based on the distance definition in the state space).
3. The planner expands from X_near toward X_rand, until a state X_new is reached.
4. The new state X_new is added to the search tree.

This process is repeated until the tree reaches X_goal. Every time a new node X_new is sampled, its connection with other nodes is checked for collision. To realize a drivable path from X_start to X_goal, you can use motion primitives or motion models such as Reeds-Shepp.

RRT in MATLAB

Navigation Toolbox provides plannerRRT, which follows the customizable sampling-based planning interface described in Chapter 3.

Bidirectional RRT (biRRT) is a variant of RRT that creates two trees, starting from both the start and goal states simultaneously. biRRT is useful for robot manipulators as it improves the search speed in high-dimensional spaces. It is available as manipulatorRRT in Robotics System Toolbox.

See RRT example

Applications

RRTs are particularly suited to path planning problems that involve obstacles, differential constraints for robots with high degrees of freedom, such as robot manipulators, and mobile robot path planning. Note that RRT planning can result in paths with sudden turns. You can use a path-smoothing algorithm to compensate for these irregularities.

The RRT algorithm gives a valid path, but not necessarily the shortest path. RRT* is an optimized version of the RRT algorithm. While realistically unfeasible, in theory the RRT* algorithm can deliver the shortest possible path to the goal when the number of nodes approaches infinity.

How It Works

The basic principle of RRT* is the same as RRT, but it has two key additions that enable it to produce significantly different results:

• RRT* includes the cost of each node as defined by the distance relative to its parent node. It always looks for the node with the cheapest cost within a fixed radius in the neighborhood of X_new.
• RRT* examines the decrease in the cost of the nodes and rewires the search tree to obtain a shorter and smoother path.

RRT* in MATLAB

plannerRRTStar in Navigation Toolbox is suitable for solving geometric planning problems. 

See RRT* example

Applications

RRT* gives an asymptotically optimal solution, which makes it particularly useful for high-dimensional problems such as robot manipulators. It is also useful in dense environments containing many obstacles. While RRT* finds the shortest path with the fewest nodes, it is not suitable for nonholonomic vehicles. RRT, by comparison, can be used for nonholonomic vehicles and is capable of handling differential constraints

Trajectory optimal Frenet is a local planner that plans trajectories based on a reference global path. A trajectory is a set of states where the state includes variables that are functions of time. Trajectory planning is useful for situations in which velocity is a consideration.

How It Works

As a local planner, trajectory optimal Frenet requires a global reference path provided as a set of \([𝑥\,𝑦\,𝜃] \) waypoints. For planning along curved and continuous reference paths, such as highways, it uses Frenet coordinates consisting of run length and lateral distance from the reference path.

Trajectory optimal Frenet samples alternate trajectories from the initial state, deviating from the reference path by some lateral distance. It uses a Frenet frame of reference, and two kinds of states:

• Cartesian state: \([𝑥\,𝑦\, 𝜃\, 𝜅\, 𝑥̇ \,𝑥̈] \)
• Frenet state: \([𝑠\,𝑠̇ \,𝑠̈\, 𝑙 \,𝑙̇ \,𝑙̈] 𝑠=𝑙𝑜𝑛𝑔𝑖𝑡𝑢𝑑𝑒𝑎𝑙𝑜𝑛𝑔𝑅𝑒𝑓𝑃𝑎𝑡ℎ, 𝑙=𝑙𝑎𝑡𝑒𝑟𝑎𝑙𝐷𝑒𝑣𝑖𝑎𝑡𝑖𝑜𝑛𝑤𝑟𝑡𝑅𝑒𝑓𝑃𝑎𝑡ℎ\)

The initial state is connected to sampled terminal states via a fifth-order polynomial, which attempts to minimize jerk and guarantees continuity along states. Sampled trajectories are evaluated for kinematic feasibility, collision, and cost.

Trajectory Optimal Frenet in MATLAB

trajectoryOptimalFrenet in Navigation Toolbox finds optimal trajectories along reference paths where reference waypoints are generated by a global planner such as Hybrid A* or RRT. trajectoryOptimalFrenet generates multiple alternate paths and evaluates them for cost based on the final state’s deviation from the reference path, path smoothness, time, and distance. It uses a state validator such as validatorOccupancyMap to check the validity of the states.

Applications

Trajectory optimal Frenet can be used as a local planner between the global planner and the vehicle or robot controller. It is applicable to highway driving applications such as lane-changing maneuvers and adaptive cruise control (including velocity keeping). It can also be used with mobile robots and other Ackermann type vehicles for dynamic replanning.