RRT* Algorithm Research
reference
Path Planning | Random Sampling Algorithms: PRM, RRT, RRT-Connect, RRT*
Sampling-based motion planning algorithm-RRT (Rapidly-exploring Random Trees)
"Application Research of Improved RRT Algorithm in Path Planning of Mobile Robots"
theoretical basis
The RRT* (Rapidly-exploring Random Tree Star) algorithm is an improved version of the RRT algorithm, which improves the quality and efficiency of path planning by introducing 重新连接and steps. 优化The following is a detailed description of the RRT* algorithm:
- Initialization: set the starting point start and the goal point goal, and create an RRT tree T containing only start
- Repeat the steps until a path is found or the maximum number of iterations is reached:
a. Random sampling: Randomly sample a point x_rand in the environment space
b. Expanding tree: Find the nearest node x_near from the tree T, start from x_near, and extend in a certain direction c
. Collision detection: detect whether the path x_near to x_new collides with obstacles, and if there is a collision, return to step 2 to continue the next iteration
d. Find the optimal connection: in the tree T Find the node x_min closest to x_new, and calculate the cost from x_min to x_new cost(x_min, x_new)
e. Reconnect: For nodes x_near_neighbors within a certain distance from x_new in the tree T, calculate the cost of connecting to x_new through x_near cost(x_near, x_new), select the connection method f with the least cost
. Update the tree: add x_new to the tree T, and update the connection relationship and cost information between nodes
g. Optimize the path: for some nodes in the tree T, use the target point The goal is optimized for the goal, and the quality of the path is improved by adjusting the connection relationship and cost information
h. Judging the termination condition: if the newly added node x_new is close to the goal point goal, a feasible path has been found, and the algorithm ends
The RRT* algorithm continuously improves the quality and length of the path through reconnection and optimization steps. The reconnection step tries to change the existing connection relationship to find a better path. The optimization step further optimizes the path by adjusting the connection relationship and cost information between nodes
The RRT* algorithm has the advantages of high efficiency, fast search, and easy implementation, and can be used for path planning problems, especially for complex environments and high-dimensional spaces. It explores the environment through random sampling and tree expansion, and improves the quality of paths through reconnection and optimization. By adjusting the algorithm parameters reasonably, the exploration and utilization can be balanced, and efficient path planning results can be obtained.
The pseudocode of the RRT* algorithm is as follows:
Reconnect (Rewrite) process
After RRT* finds the node Node_near closest to Node_rand and passes the collision detection, it does not immediately Edge(Node_near, Node_rand)add to the extended tree
Instead, use Node_rand as the center of the circle and R as the radius (not necessarily the step size), find all potential parent nodes, and compare with the cost of reaching the node Node_rand through the current parent node Node_near to see if there is a lower cost parent node
As shown in the figure below, when the cost of the path Node_init→Node_parent→Node_child is greater than the cost of Node_init→Node_potential_parent→Node_child, the RRT* algorithm will remove Edge{Node_parent→Node_child} and addEdge{potential_parent→Node_child}
So far, a reconnection process has been completed, and the new random number is shown in the figure below
optimization process
Optimization is a circle with the newly added node Node_newas the center and R as the radius (not necessarily the step size). There will be some nodes in the circle. Whether the cost of these nodes trying to reach through the newly added node is less than the original cost, if it is less than, update the node The parent node is a newly added node, making the cost of reaching this node even lower
As shown in the figure above, X_new is the newly generated node, 4, 6, and 8 are the neighbor nodes of X_new, and 0, 4, and 5 are the parent nodes of the neighbor nodes
- The cost of the path {0->4} is: 10
- The cost of the path {0->4->6} is: 10 + 5 = 15
- The Cost of the path {0->1->5->8} is: 3 + 5 + 1 = 9
- First try to change the parent node of node 4 to X_new, the path to reach node 4 becomes {0->1->5->9->4}, the cost of the new path=3+5+3+4=15, The cost of the new path is greater than the cost of the original path, so the parent node of node 4 is not changed
- Then try to change the parent node of node 8 to X_new, the path to reach node 8 becomes {0->1->5->9->8}, the cost of the new path=3+5+3+3=14, the new The cost of the path is greater than the cost of the original path, and the parent node of node 8 is not changed at will
- Try to change the parent node of node 6 to X_new, the path to path 6 becomes {0->1->5->9->6}, the new Cost=3+5+3+1=12, the new path The Cost of is less than the cost of the original path, so update the parent node of node 6 to node 9
MATLAB implementation
https://github.com/olzhas/rrt_toolbox
function problem = rrt_star(map, max_iter, is_benchmark, rand_seed, variant)
%RRT_STAR -- RRT* is sampling-based algorithm, solves
% the problem of motion and path planning providing feasible solutions
% taking into account the optimality of a path/motion.
%
% problem = RRT_STAR(map, max_iter, is_benchmark, rand_seed, variant)
% function returns the object of the respective class with the result
%
% map -- struct with appropriate fields (developer of
% the class provides more information on this topic)
% max_iter -- number of iteration to solve the problem
% is_benchmark -- if true saves snapshots of the tree in a special directory
% boolean variable
% rand_seed -- a random seed
% variant -- what class to choose, class used defines the problem space
%
%
% for detailed information consult with the help of the _RRT*FN Toolbox_
%
% Olzhas Adiyatov
% 05/15/2013
%%% Configuration block
if nargin < 5
clear all;
close all;
clc;
% load conf
RAND_SEED = 5;
MAX_ITER = 10e3;
MAX_NODES = MAX_ITER;
% here you can specify what class to use, each class represent
% different model.
% FNSimple2D provides RRT and RRT* for 2D mobile robot represented as a dot
% FNRedundantManipulator represents redundant robotic manipulator, DOF is
% defined in configuration files.
variant = 'FNSimple2D';
MAP = struct('name', 'bench_june1.mat', 'start_point', [-12.5 -5.5], 'goal_point', [7 -3.65]);
% variant = 'FNRedundantManipulator';
% MAP = struct('name', 'bench_redundant_3.mat', 'start_point', [0 0], 'goal_point', [35 35]);
% do we have to benchmark?
is_benchmark = false;
else
MAX_NODES = max_iter;
MAX_ITER = max_iter;
RAND_SEED = rand_seed;
MAP = map;
end
addpath(genpath(pwd));
%%% loading configuration file with object model specific options
if exist(['configure_' variant '.m'], 'file')
run([pwd '/configure_' variant '.m']);
CONF = conf;
else
disp('ERROR: There is no configuration file!')
return
end
ALGORITHM = 'RRTS';
problem = eval([variant '(RAND_SEED, MAX_NODES, MAP, CONF);']);
%problem = RedundantManipulator(RAND_SEED, MAX_NODES, MAP, CONF);
if (is_benchmark)
benchmark_record_step = 250;
benchmark_states = cell(MAX_ITER / benchmark_record_step, 1);
timestamp = zeros(MAX_ITER / benchmark_record_step, 1);
iterstamp = zeros(MAX_ITER / benchmark_record_step, 1);
end
%%% Starting a timer
disp(ALGORITHM);
% starting timer
tic;
for ind = 1:MAX_ITER
new_node = problem.sample();
nearest_node_ind = problem.nearest(new_node);
new_node = problem.steer(nearest_node_ind, new_node); % if new node is very distant from the nearest node we go from the nearest node in the direction of a new node
if(~problem.obstacle_collision(new_node, nearest_node_ind))
neighbors = problem.neighbors(new_node, nearest_node_ind);
min_node_ind = problem.chooseParent(neighbors, nearest_node_ind, new_node);
new_node_ind = problem.insert_node(min_node_ind, new_node);
problem.rewire(new_node_ind, neighbors, min_node_ind);
end
if is_benchmark && (mod(ind, benchmark_record_step) == 0)
benchmark_states{
ind/benchmark_record_step} = problem.copyobj();
timestamp(ind/benchmark_record_step) = toc;
iterstamp(ind/benchmark_record_step) = ind;
end
if(mod(ind, 1000) == 0)
disp([num2str(ind) ' iterations ' num2str(problem.nodes_added-1) ' nodes in ' num2str(toc) ' rewired ' num2str(problem.num_rewired)]);
end
end
if (is_benchmark)
if strcmp(computer, 'GLNXA64');
result_dir = '/home/olzhas/june_results/';
else
result_dir = 'C:\june_results\';
end
dir_name = [result_dir datestr(now, 'yyyy-mm-dd')];
mkdir(dir_name);
save([dir_name '/' ALGORITHM '_' MAP.name '_' num2str(MAX_NODES) '_of_' num2str(MAX_ITER) '_' datestr(now, 'HH-MM-SS') '.mat'], '-v7.3');
set(gcf, 'Visible', 'off');
% free memory, sometimes there is a memory leak, or matlab decides to
% free up memory later.
clear all;
clear('rrt_star.m');
% problem.plot();
% saveas(gcf, [dir_name '\' ALGORITHM '_' MAP.name '_' num2str(MAX_NODES) '_of_' num2str(MAX_ITER) '_' datestr(now, 'HH-MM-SS') '.fig']);
else
problem.plot();
end
run output
RRTS
1000 iterations 905 nodes in 0.66896 rewired 109
2000 iterations 1844 nodes in 0.89745 rewired 648
3000 iterations 2783 nodes in 1.2249 rewired 1755
4000 iterations 3725 nodes in 1.6224 rewired 3336
5000 iterations 4669 nodes in 2.102 rewired 5707
6000 iterations 5602 nodes in 2.5499 rewired 8174
7000 iterations 6544 nodes in 2.9863 rewired 10551
8000 iterations 7482 nodes in 3.3793 rewired 12711
9000 iterations 8421 nodes in 3.7698 rewired 14758
10000 iterations 9355 nodes in 4.2213 rewired 16772
24.7687
ans =
FNSimple2D - 属性:
tree: [2×10000 double]
parent: [1×10000 double]
children: [1×10000 double]
free_nodes: [1×10000 double]
free_nodes_ind: 1
cost: [1×10000 double]
cumcost: [1×10000 double]
XY_BOUNDARY: [-20 20 -20 20]
goal_point: [7 -3.6500]
delta_goal_point: 1
delta_near: 1.5000
nodes_added: 9356
max_step: 0.5000
obstacle: [1×1 struct]
dynamic_obstacle: []
best_path_node: -1
goal_reached: 0
max_nodes: 10000
bin_size: 7
bin: [1×36 struct]
bin_x: 6
bin_y: 6
bin_offset: 22
nbins: 36
bin_ind: [10×10000 double]
compare_table: [1×10000 double]
index: [1×10000 double]
list: [1×10000 int32]
num_rewired: 16772
Python implementation
https://github.com/zhm-real/PathPlanning
#!/usr/bin/python
import os
import sys
import math
import numpy as np
import env, plotting, utils, queue
class Node:
def __init__(self, n):
self.x = n[0]
self.y = n[1]
self.parent = None
class RrtStar:
def __init__(self, x_start, x_goal, step_len,
goal_sample_rate, search_radius, iter_max):
self.s_start = Node(x_start)
self.s_goal = Node(x_goal)
self.step_len = step_len
self.goal_sample_rate = goal_sample_rate
self.search_radius = search_radius
self.iter_max = iter_max
self.vertex = [self.s_start]
self.path = []
self.env = env.Env()
self.plotting = plotting.Plotting(x_start, x_goal)
self.utils = utils.Utils()
self.x_range = self.env.x_range
self.y_range = self.env.y_range
self.obs_circle = self.env.obs_circle
self.obs_rectangle = self.env.obs_rectangle
self.obs_boundary = self.env.obs_boundary
def planning(self):
print("Begin RRT-Star Path Planning...")
for k in range(self.iter_max):
node_rand = self.generate_random_node(self.goal_sample_rate)
node_near = self.nearest_neighbor(self.vertex, node_rand)
node_new = self.new_state(node_near, node_rand)
if k % 500 == 0:
print("Iteration counts: {}".format(k))
if k == self.iter_max - 1:
print("Iteration counts: {}".format(self.iter_max))
print("End of iterative optimization!")
if node_new and not self.utils.is_collision(node_near, node_new):
neighbor_index = self.find_near_neighbor(node_new)
self.vertex.append(node_new)
if neighbor_index:
self.choose_parent(node_new, neighbor_index)
self.rewire(node_new, neighbor_index)
index = self.search_goal_parent()
self.path = self.extract_path(self.vertex[index])
self.plotting.animation(self.vertex, self.path, "rrt*, N = " + str(self.iter_max))
def new_state(self, node_start, node_goal):
dist, theta = self.get_distance_and_angle(node_start, node_goal)
dist = min(self.step_len, dist)
node_new = Node((node_start.x + dist * math.cos(theta),
node_start.y + dist * math.sin(theta)))
node_new.parent = node_start
return node_new
def choose_parent(self, node_new, neighbor_index):
cost = [self.get_new_cost(self.vertex[i], node_new) for i in neighbor_index]
cost_min_index = neighbor_index[int(np.argmin(cost))]
node_new.parent = self.vertex[cost_min_index]
def rewire(self, node_new, neighbor_index):
for i in neighbor_index:
node_neighbor = self.vertex[i]
if self.cost(node_neighbor) > self.get_new_cost(node_new, node_neighbor):
node_neighbor.parent = node_new
def search_goal_parent(self):
dist_list = [math.hypot(n.x - self.s_goal.x, n.y - self.s_goal.y) for n in self.vertex]
node_index = [i for i in range(len(dist_list)) if dist_list[i] <= self.step_len]
if len(node_index) > 0:
cost_list = [dist_list[i] + self.cost(self.vertex[i]) for i in node_index
if not self.utils.is_collision(self.vertex[i], self.s_goal)]
return node_index[int(np.argmin(cost_list))]
return len(self.vertex) - 1
def get_new_cost(self, node_start, node_end):
dist, _ = self.get_distance_and_angle(node_start, node_end)
return self.cost(node_start) + dist
def generate_random_node(self, goal_sample_rate):
delta = self.utils.delta
if np.random.random() > goal_sample_rate:
return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
return self.s_goal
def find_near_neighbor(self, node_new):
n = len(self.vertex) + 1
r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len)
dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.vertex]
dist_table_index = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r and
not self.utils.is_collision(node_new, self.vertex[ind])]
return dist_table_index
@staticmethod
def nearest_neighbor(node_list, n):
return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
for nd in node_list]))]
@staticmethod
def cost(node_p):
node = node_p
cost = 0.0
while node.parent:
cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y)
node = node.parent
return cost
def update_cost(self, parent_node):
OPEN = queue.QueueFIFO()
OPEN.put(parent_node)
while not OPEN.empty():
node = OPEN.get()
if len(node.child) == 0:
continue
for node_c in node.child:
node_c.Cost = self.get_new_cost(node, node_c)
OPEN.put(node_c)
def extract_path(self, node_end):
path = [[self.s_goal.x, self.s_goal.y]]
node = node_end
while node.parent is not None:
path.append([node.x, node.y])
node = node.parent
path.append([node.x, node.y])
return path
@staticmethod
def get_distance_and_angle(node_start, node_end):
dx = node_end.x - node_start.x
dy = node_end.y - node_start.y
return math.hypot(dx, dy), math.atan2(dy, dx)
def main():
x_start = (18, 8) # Starting node
x_goal = (37, 18) # Goal node
rrt_star = RrtStar(x_start, x_goal, 10, 0.10, 20, 10000)
rrt_star.planning()
if __name__ == '__main__':
main()
run output
Begin RRT-Star Path Planning...
Iteration counts: 0
Iteration counts: 500
Iteration counts: 1000
Iteration counts: 1500
Iteration counts: 2000
Iteration counts: 2500
Iteration counts: 3000
Iteration counts: 3500
Iteration counts: 4000
Iteration counts: 4500
Iteration counts: 5000
Iteration counts: 5500
Iteration counts: 6000
Iteration counts: 6500
Iteration counts: 7000
Iteration counts: 7500
Iteration counts: 8000
Iteration counts: 8500
Iteration counts: 9000
Iteration counts: 9500
Iteration counts: 10000
End of iterative optimization!
The main logic is still in planning()the function, the code encapsulates the function function very well, it is worth learning
A total of up to 10,000 iterations ( iter_max) are performed, and the following operations are performed in each iteration
1. Generate random points, select the nearest point, and generate new points
node_rand = self.generate_random_node(self.goal_sample_rate)
node_near = self.nearest_neighbor(self.vertex, node_rand)
node_new = self.new_state(node_near, node_rand)
2. If the new node passes the collision detection, find all adjacent nodes within the search circle and add the new node to the path
if node_new and not self.utils.is_collision(node_near, node_new):
neighbor_index = self.find_near_neighbor(node_new)
self.vertex.append(node_new)
3. If there are nodes in the search circle, perform reconnection and optimization operations, corresponding to choose_parent()and rewire()functions respectively
if neighbor_index:
self.choose_parent(node_new, neighbor_index)
self.rewire(node_new, neighbor_index)
4. Backtracking to determine the search path
index = self.search_goal_parent()
self.path = self.extract_path(self.vertex[index])
Summarize
In view of the disadvantage that the RRT algorithm is not optimal, the RRT algorithm can be realized 递进优化, but at the same time, some new problems have appeared in RRT:
- In the environment of irregular obstacles,
路径不平滑a problem arises: when the random expansion tree of the RRT algorithm generates sampling points, it uses a uniform sampling method to sample the surrounding environment, and at the same time, the random expansion tree of the RRT algorithm generates sampling points When pointing, the surrounding redundant nodes that are not asymptotically optimal will be deleted directly. Although the path can achieve asymptotically optimal, the generated path is not smooth enough, which will cause problems when the robot moves along this path振荡. - The RRT algorithm can achieve progressive optimization, but the implementation process is slow. When the random expansion tree of
耗时较长the RRT algorithm generates the sampling point, it calculates the distance from the starting point to the parent node, the parent node to the sampling point, and the sampling point to the nearest node in turn, and Compare these distances in turn, and finally delete the original lines with longer distances, and then reconnect those lines. Although an asymptotically optimal path will be found at this time, the path planning time will be longer due to too many iterations, and the efficiency will also decrease.
Comparison of RRT Algorithm and RRT* Algorithm
