RRT* 算法研究（附 MATLAB 和 Python 实现）

RRT* 算法研究

参考

机器人路径规划、轨迹优化课程-第六讲-RRT*算法原理和代码讲解

路径规划 | 随机采样算法：PRM、RRT、RRT-Connect、RRT*

基于采样的运动规划算法-RRT(Rapidly-exploring Random Trees)

《改进RRT算法在移动机器人路径规划中的应用研究》

理论基础

RRT*（Rapidly-exploring Random Tree Star）算法是RRT算法的改进版本，它通过引入重新连接和优化步骤，提高了路径规划的质量和效率。下面是对RRT*算法的详细描述：

初始化：设定起始点start和目标点goal，并创建一个只包含start的RRT树T
重复步骤直到找到路径或达到最大迭代次数：
a. 随机采样：在环境空间中随机采样一个点x_rand
b. 扩展树：从树T中找到最近的节点x_near，以x_near为起点，在方向上延伸一定的距离，得到新的节点x_new
c. 碰撞检测：检测路径x_near到x_new之间是否与障碍物发生碰撞，如果发生碰撞，则返回步骤2继续下一次迭代
d. 寻找最优连接：在树T中找到与x_new最近的节点x_min，并计算从x_min到x_new的代价cost(x_min, x_new)
e. 重新连接：对于树T中与x_new距离在一定范围内的节点x_near_neighbors，计算通过x_near连接到x_new的代价cost(x_near, x_new)，选择代价最小的连接方式
f. 更新树：将x_new加入树T，并更新节点间的连接关系和代价信息
g. 优化路径：对树T中的部分节点，以目标点goal为目标进行优化，通过调整连接关系和代价信息，改善路径的质量
h. 判断终止条件：如果新加入的节点x_new接近目标点goal，则找到了一条可行路径，算法结束

RRT*算法通过重新连接和优化步骤，不断改善路径的质量和长度。重新连接步骤尝试改变已有的连接关系，以找到更优的路径。优化步骤则通过调整节点间的连接关系和代价信息，进一步优化路径

RRT*算法具有高效、快速搜索、易于实现等优点，可以用于路径规划问题，特别适用于复杂环境和高维空间。它通过随机采样和树的扩展来探索环境，并通过重新连接和优化来提高路径的质量。通过合理的调整算法参数，可以平衡探索和利用，获得高效的路径规划结果

RRT* 算法伪代码如下：

在这里插入图片描述

重新连接（Rewrite）过程

RRT* 在找到距离 Node_rand 最近的节点 Node_near 并通过碰撞检测后，并不立即将 Edge(Node_near, Node_rand) 加入扩展树中

在这里插入图片描述

而是以 Node_rand 为圆心，R 为半径(不一定是步长) 的圆内，寻找所有潜在的父节点，并与通过当前父节点 Node_near 到达该节点 Node_rand 的代价对比，看是否存在代价更小的父节点

在这里插入图片描述

如下图所示，当路径 Node_init→Node_parent→Node_child 的代价大于 Node_init→Node_potential_parent→Node_child 的代价时，RRT* 算法会将 Edge{Node_parent→Node_child}剔除，并新增Edge{potential_parent→Node_child}

在这里插入图片描述

至此完成了一次重新连接的过程，新的随机数如下图所示

在这里插入图片描述

优化过程

优化就是在以新添加节点 Node_new 为圆心，R 为半径(不一定是步长)的圆，圆内会包含一些节点，这些节点尝试通过新增节点到达的代价是否小于原始代价，小于则更新该节点父节点为新增的节点，使得到达该节点的代价更小

在这里插入图片描述

如上图所示，X_new 为新生成节点，4、6、8 是 X_new 的近邻节点，0、4、5 为近邻节点的父节点

路径{0->4}的Cost为: 10
路径{0->4->6}的Cost为： 10 + 5 = 15
路径{0->1->5->8}的Cost为: 3 + 5 + 1 = 9
先尝试将节点4的父节点改为 X_new，到达节点4的路径变为{0->1->5->9->4}，新路径的Cost=3+5+3+4=15，新路径的Cost大于原路径Cost，所以不改变节点4的父节点
再尝试改变节点8的父节点为 X_new，到达节点8的路径变为{0->1->5->9->8},新路径的Cost=3+5+3+3=14，新路径的Cost大于原路径Cost，随意不改变节点8的父节点
再尝试改变节点6的父节点为 X_new，到达路径6的路径变为{0->1->5->9->6},新的Cost=3+5+3+1=12,新路径的Cost小于原路径Cost，因此将节点6的父节点更新为节点9

MATLAB 实现

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 < 5clear all;close all;clc;% load confRAND_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;
elseMAX_NODES   = max_iter;MAX_ITER    = max_iter;RAND_SEED   = rand_seed;MAP = map;
endaddpath(genpath(pwd));%%% loading configuration file with object model specific options 
if exist(['configure_' variant '.m'], 'file')run([pwd '/configure_' variant '.m']);CONF = conf;
elsedisp('ERROR: There is no configuration file!')return
endALGORITHM = '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 timerdisp(ALGORITHM);
% starting timer
tic;
for ind = 1:MAX_ITERnew_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 nodeif(~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);endif 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;endif(mod(ind, 1000) == 0)disp([num2str(ind) ' iterations ' num2str(problem.nodes_added-1) ' nodes in ' num2str(toc) ' rewired ' num2str(problem.num_rewired)]);end
endif (is_benchmark)if strcmp(computer, 'GLNXA64');result_dir = '/home/olzhas/june_results/';elseresult_dir = 'C:\june_results\';enddir_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']);
elseproblem.plot();
end

运行输出

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.7687ans = FNSimple2D - 属性:tree: [2×10000 double]parent: [1×10000 double]children: [1×10000 double]free_nodes: [1×10000 double]free_nodes_ind: 1cost: [1×10000 double]cumcost: [1×10000 double]XY_BOUNDARY: [-20 20 -20 20]goal_point: [7 -3.6500]delta_goal_point: 1delta_near: 1.5000nodes_added: 9356max_step: 0.5000obstacle: [1×1 struct]dynamic_obstacle: []best_path_node: -1goal_reached: 0max_nodes: 10000bin_size: 7bin: [1×36 struct]bin_x: 6bin_y: 6bin_offset: 22nbins: 36bin_ind: [10×10000 double]compare_table: [1×10000 double]index: [1×10000 double]list: [1×10000 int32]num_rewired: 16772

在这里插入图片描述

Python 实现

https://github.com/zhm-real/PathPlanning

#!/usr/bin/python
import os
import sys
import math
import numpy as npimport env, plotting, utils, queueclass Node:def __init__(self, n):self.x = n[0]self.y = n[1]self.parent = Noneclass 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_lenself.goal_sample_rate = goal_sample_rateself.search_radius = search_radiusself.iter_max = iter_maxself.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_rangeself.y_range = self.env.y_rangeself.obs_circle = self.env.obs_circleself.obs_rectangle = self.env.obs_rectangleself.obs_boundary = self.env.obs_boundarydef 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_startreturn node_newdef 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_newdef 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_indexif not self.utils.is_collision(self.vertex[i], self.s_goal)]return node_index[int(np.argmin(cost_list))]return len(self.vertex) - 1def get_new_cost(self, node_start, node_end):dist, _ = self.get_distance_and_angle(node_start, node_end)return self.cost(node_start) + distdef generate_random_node(self, goal_sample_rate):delta = self.utils.deltaif 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_goaldef find_near_neighbor(self, node_new):n = len(self.vertex) + 1r = 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 andnot self.utils.is_collision(node_new, self.vertex[ind])]return dist_table_index@staticmethoddef 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]))]@staticmethoddef cost(node_p):node = node_pcost = 0.0while node.parent:cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y)node = node.parentreturn costdef update_cost(self, parent_node):OPEN = queue.QueueFIFO()OPEN.put(parent_node)while not OPEN.empty():node = OPEN.get()if len(node.child) == 0:continuefor 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_endwhile node.parent is not None:path.append([node.x, node.y])node = node.parentpath.append([node.x, node.y])return path@staticmethoddef get_distance_and_angle(node_start, node_end):dx = node_end.x - node_start.xdy = node_end.y - node_start.yreturn math.hypot(dx, dy), math.atan2(dy, dx)def main():x_start = (18, 8)  # Starting nodex_goal = (37, 18)  # Goal noderrt_star = RrtStar(x_start, x_goal, 10, 0.10, 20, 10000)rrt_star.planning()if __name__ == '__main__':main()

运行输出

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!

在这里插入图片描述

主要的逻辑还是在 planning() 函数中，代码对于功能函数做了很好的封装，很值得学习

一共进行最大 10000 次迭代（iter_max），每次迭代中进行如下操作

1、生成随机点，选取最近点，生成新点

			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 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、若搜索圆内存在节点，则进行重连接和优化操作，分别对应 choose_parent() 和 rewire() 函数

				if neighbor_index:self.choose_parent(node_new, neighbor_index)self.rewire(node_new, neighbor_index)

4、回溯确定搜索路径

		index = self.search_goal_parent()self.path = self.extract_path(self.vertex[index])

总结

鉴于RRT算法不是最优的缺点，RRT算法却能够实现了递进优化，但与此同时，在RRT中又出现了一些新的问题：

在不规则障碍物环境中，产生了路径不平滑的问题：由于RRT算法的随机扩展树在产生采样点时，是对周围的环境采用均匀的采样方法来进行采样，同时RRT算法的随机扩展树在生成采样点时会将周围不是渐近最优的冗余节点直接删除，虽然路径可以实现渐近最优，但是生成的路径不够光滑，从而使得机器人按照这种路径运动时会发生振荡问题
RRT算法能够实现递进优化，但实现过程缓慢，耗时较长：RRT算法的随机扩展树在生成采样点时，依次计算起始点到父节点、父节点到采样点、采样点到最近的节点的距离，并将这些距离依次进行比较，最终删除原先那些距离较长的连线，然后再对那些线进行重新连接。此时虽然会找到一条渐近最优的路径，但是由于迭代次数过多导致路径规划的时间较长，效率也会降低

RRT 算法与 RRT* 算法对比

在这里插入图片描述