rrt算法流程图_基于采样的路径搜索算法代码实现（RRT和PRM）

1、采样法

利用网格法可以得到对特定离散化后C-space的最优解。但网格法的缺点是计算复杂度高，适用于自由度较低的机器人。本文简要介绍另一种基于采样的规划器，它涉及以下函数，通过这些函数构建一个图(graph)或树(tree)，用于表达机器人的可行运动：

一个随机或者确定函数从C-space或state space中选择样本点

一个用于评估样本点或者运动是否在

空间的函数

一个选择之前自由空间里邻近样本点的函数

一个局部规划器，用于连接新的样本点

采样法放弃了网格法得到的分辨率最优解(resolution-optimal solutions)，由此换来了在处理高自由度机器人规划问题的满意解(satisficing solution)，且求解速度快速。所选择的样本点构成了一个路图或搜索树。相比网格法，采样法可通过更少的样本点来逼近

，因为随着搜索空间自由度的增加，网格法的样本点数目是以指数形式增长的。大部分采样法是概率完备的，也就是说，在有解的情况下，当样本点数目趋于无穷大，找到解的概率趋于100%。

目前有两类算法基于采样法：快速搜索随机树(RRT)和 概率路图(PRM)。RRT采用了树结构在C-space或state space中进行单次规划(single-query planning)，PRM主要是在C-space中创建路图，可实现多次规划(multiple-query planning)。下面分别介绍这两种算法的原理和Python代码实现。

2、RRT算法

RRT算法搜索了一条无碰撞运动，用于从初始状态

到达目标集合

。当用于运动学问题时，

代表了位型

，当用于动力学问题时，

则同时包含了速度。RRT算法从

开始生成一个树，以下是RRT算法的伪代码：

1: initialize search tree T with xstart

2: while T is less than the maximum tree size do

3: xsamp sample from X

4: xnearest nearest node in T to xsamp

5: employ a local planner to find a motion from xnearest to xnew in the direction of xsamp

6: if the motion is collision-free then

7: add xnew to T with an edge from xnearest to xnew

8: if xnew is in Xgoal then

9: return SUCCESS and the motion to xnew

10: end if

11: end if

12: end while

13: return FAILURE

以一个典型的运动学问题为例(x代表q)，第3行将从均匀分布的

中随机地(略倾向于

)选取一个

。第4行中的

是搜索树

中距离

最近的节点。第5行中的

可以选择[

,

]线段上的一点，距离

有一小段距离

。由于

很小，可以用一个简单的局部规划器返回一条直线运动，连接

到

，如果运动是无碰撞的，

将被加入到搜索树

中。

均匀分布的样本点会让生成的树快速探索

空间，下图是由RRT生成的一个树：

RRT算法生成树

从上面的伪代码可以看出在编程时我们有较多的选择，包括如何选择样本点(第3行)，如何定义最近点(第4行)，如何规划向的运动(第5行)。即便对采样法做很小的改变，都可能对规划时间产生很大的影响。下面分别对这三行做进一步描述。

2.1、第3行：采样器(Sampler)

最常见的方法是随机地从均匀分布的

空间采样。比如，对于欧几里德C-space

，以及

关节机器人的C-space

，我们可以在每个关节上均匀采样，对于平面移动机器人的C-space

，可以分别在

和

上均匀采样。

对于动态系统，则是在状态空间中均匀采样，分为两部分：在C-space中均匀采样和在有界速度集上均匀采样(即

)。

实际上，样本点的也不一定需要随机选择。比如可以采用一个确定的采样方案，在

空间生成逐渐细化的网格，最终会在状态空间产生浓密的样本点，当样本点无穷多时，样本将与

空间无限接近。

2.2、第4行：定义最近节点(Nearest Node)

寻找最近节点依赖于在

空间中如何定义距离。对于在

空间上无运动约束的机器人来说，一个很自然的选择就是采用欧几里德距离。对于某些空间，距离的定义就不是这么明显了。

比如平面上的小车机器人，它的C-space表达为

，下图中哪一个与

更近呢？

哪一个位型和灰色的最近？

由于运动约束的存在，小车无法原地转动或侧向移动，为了朝着移动，那么在它后面的那个位型是最佳的位置。因此应该被定义为去往最快速的节点，然而要计算出来是很困难的。一种简单的选择是在每个维度上设置权重，然后计算的加权和。权重描述了不同维度的重要性 。

2.3、第5行：局部规划器(Local Planner)

局部规划器的作用是寻找到一条路径，从

到某个与

更近的节点

。规划器应该简单、运行快速，以下是三种规划器：

直线规划器

获得的规划是一条通往

的直线，

可以选择为

或者选择为距离

一个固定距离

处。这种规划器适用于运动上无约束的系统。

离散控制规划器

对于有运动约束的系统，比如轮式移动机器人或动态系统，控制量可以被离散化至一个离散集合{

}。每一个控制量通过

从

积分

时间，对应的获得一个新的状态，在所有的无碰撞状态中，选择与

最近的节点作为

。

轮式机器人规划器

对于轮式机器人，规划器可以采用Reeds-Shepp曲线，此处不做展开。

3、Python代码(RRT)

了解了RRT算法后，可以开始解决一个实际问题：为一个point机器人在有障碍物环境中从起点到终点规划一条无碰撞路径。项目要求见此处，这里简要描述：

程序的输入是obstacles.csv文件，该文件描述了障碍物的位置和直径。程序的输出包括nodes.csv, edges.csv, 和 path.csv。这三个文件的含义见上一篇文章。C-space可表达为 [-0.5, 0.5] x [-0.5, 0.5]，两个位型的距离用欧几里德距离来计算，而路径是否碰撞用直线和障碍物的交点情况来判断。据此，笔者编写了下面的Python代码：

import numpy as np

import csv

import random

import math

class RRTPathPlanner(object):

def __init__(self, obstacle_path):

self.x_start = [-0.5, -0.5]

self.goal = [0.5, 0.5]

distance = self.distance_between_two_point(self.x_start, self.goal)

self.nodes = [[1, self.x_start[0], self.x_start[1], distance]]

self.parent = {1: 0}

self.tree = []

self.tree.append(self.x_start)

self.max_tree_size = 1000

self.d = 0.1

self.goal_set = [0.4, 0.4, 0.5, 0.5]

self.epslon = 0.03

self.obstacles = []

with open(obstacle_path, "rt") as f_obj:

contents = csv.reader(f_obj)

for row in contents:

if row[0][0] != '#':

self.obstacles.append([float(row[0]), float(row[1]), float(row[2])])

def distance_between_two_point(self, point_1, point_2):

x1, y1 = point_1

x2, y2 = point_2

return np.sqrt((x1-x2)**2 + (y1-y2)**2)

def calculate_vector_angle(self, a, b):

a = np.array(a)

b = np.array(b)

a_norm = np.linalg.norm(a)

b_norm = np.linalg.norm(b)

a_dot_b = a.dot(b)

value = a_dot_b / (a_norm*b_norm)

if value > 1:

value = 1

if value < -1:

value = -1

# print(value)

theta = np.arccos(value)

return theta * 180 / np.pi

def point_in_circle(self, point, circle):

x, y, d = circle

r = d / 2

# self.epslon is accounted for the robot radius itself

if self.distance_between_two_point(point, [x, y]) < r + self.epslon:

return True

else:

return False

# find foot of perpendicular of the point to the line

def find_vertical_point(self, point, line):

[x0, y0] = point

[x1, y1, x2, y2] = line

k = -1 * ((x1 - x0) * (x2 - x1) + (y1 - y0) * (y2 - y1)) / ((x2 - x1) ** 2 + (y2 - y1) ** 2)

x = k * (x2 - x1) + x1

y = k * (y2 - y1) + y1

return [x, y]

def point_within_line_segment(self, point, point1, point2):

a = [point1[i]-point[i] for i in range(len(point))]

b = [point2[i] - point[i] for i in range(len(point))]

if self.calculate_vector_angle(a, b) > 90:

return True

else:

return False

# the distance from x_new to x_nearest is chosen to be self.d

def solve_for_x_new(self, x_nearest, x_sample):

total_distance = self.distance_between_two_point(x_nearest, x_sample)

if total_distance <= self.d:

return x_sample

ratio = self.d/total_distance

[x1, y1] = x_nearest

[x2, y2] = x_sample

x = x1 + (x2-x1)*ratio

y = y1 + (y2-y1)*ratio

return [x, y]

# check if a line with point1 and point2 as its end

# is in collision with a circle

def in_collision(self, point1, point2, circle):

[x1, y1] = point1

[x2, y2] = point2

line = [x1, y1, x2, y2]

[x0, y0, diameter] = circle

radius = diameter / 2

center = [x0, y0]

vertical_point = self.find_vertical_point(center, line)

# only when both point1 and point2 are outside the obstacle, the path might be collision free

# otherwise the path must be in collision

if not self.point_in_circle(point1, circle) and \

not self.point_in_circle(point2, circle):

distance_to_line = self.distance_between_two_point(vertical_point, center)

if distance_to_line > radius:

return False

if self.point_within_line_segment(vertical_point, point1, point2):

return True

else:

return False

else:

return True

def node_in_goal_set(self, node):

[x, y] = node

[x1, y1, x2, y2] = self.goal_set

if x1 < x < x2 and y1 < y < y2:

return True

else:

return False

def search_for_path(self):

index = 1

count = 0

while len(self.tree) < self.max_tree_size:

count = count + 1

# sample uniformly from C-space

x = random.uniform(-0.5, 0.5)

y = random.uniform(-0.5, 0.5)

x_sample = [x, y]

if count % 10 == 0:

x_sample = self.goal

# search for x_nearest in current tree

x_nearest = self.tree[0]

min_distance = 100

min_index = 0

for i in range(0, len(self.tree)):

distance = self.distance_between_two_point(self.tree[i], x_sample)

if distance < min_distance:

x_nearest = self.tree[i]

min_distance = distance

min_index = i + 1

# employ a local planner to find a path to x_new

x_new = self.solve_for_x_new(x_nearest, x_sample)

in_collision = False

for each_circle in self.obstacles:

if self.in_collision(x_nearest, x_new, each_circle):

# print('xnearest:', x_nearest, 'x_new:', x_new, 'each_circle:', each_circle)

in_collision = True

break

if not in_collision:

# print(x_new)

self.tree.append(x_new)

index = index + 1

self.parent[index] = min_index

cost_to_go = self.distance_between_two_point(x_new, self.goal)

self.nodes.append([index, x_new[0], x_new[1], cost_to_go])

if self.node_in_goal_set(x_new):

self.tree.append(self.goal)

self.nodes.append([index + 1, self.goal[0], self.goal[1], 0])

self.parent[index + 1] = index

return True

return False

def get_tree_length(self):

return len(self.tree)

def get_tree_parent_info(self):

return self.parent

def write_to_nodes_file(self):

with open('nodes.csv', 'wt') as f_obj:

writer = csv.writer(f_obj, delimiter=',')

for each_row in self.nodes:

writer.writerow(each_row)

def get_path_to_goal(self):

current = len(self.tree)

self.path = [current]

# reconstruct path by parent node

while True:

current = self.parent[current]

self.path.append(current)

if current == 1:

break

self.path = self.path[::-1]

print(self.path)

with open('path.csv', 'wt') as f_obj:

writer = csv.writer(f_obj, delimiter=',')

writer.writerow(self.path)

if __name__ == '__main__':

rrt = RRTPathPlanner('obstacles.csv')

ret = rrt.search_for_path()

length = rrt.get_tree_length()

print(length)

if ret:

rrt.write_to_nodes_file()

rrt.get_path_to_goal()

else:

print("no solution found")

在规划过程中，每经过10次计算，就将

设置为目标点，并检查是否可以产生连接至目标的路径。这种策略体现了在均匀采样时略倾向于

。路径找到后生成文件path.csv和nodes.csv，其中path.csv包含起点到终点所经过的节点号(见下面的代码输出)，nodes.csv文件包含了程序运行过程中探索到的所有节点号和位置信息。在V-rep场景Scene5_motion_planning中导入文件所在目录，通过GUI仿真可检验程序的正确性。以下是某次代码输出和对应的V-rep仿真结果：

代码输出：

[1, 2, 3, 4, 5, 6, 10, 12, 14, 15, 16, 17, 47, 71, 74, 79, 80, 81]

路径仿真：

path planning

由于均匀随机采样的特性，程序在每次运行时得到的规划结果将是不一致的，下面是另一次代码执行后得到的结果，执行期间探索了144个节点，仍然可保证生成无碰撞路径。

代码输出：

[1, 2, 3, 4, 5, 6, 7, 9, 10, 11, 15, 17, 21, 33, 99, 122, 133, 135, 138, 140, 143, 144]

路径仿真：

path planning

4、PRM算法

PRM算法核心思想是建立路图来表达

空间，而后再进行路径搜索。路图是一个无向图，即机器人可在任意边上双向运动。因此，PRM主要适用于运动学问题。路图建立好后，可尝试将起点

和

连接至路图中最近点。最后，用路径搜索算法，比如

来完成路径规划。以下是用PRM算法建立一个N节点路图R的伪代码：

1: for i = 1, . . . , N do

2: q i ← sample from C free

3: add q i to R

4: end for

5: for i = 1, . . . , N do

6: N (qi) ← k closest neighbors of qi

7: for each q ∈ N (qi) do

8: if there is a collision-free local path from q to q i and

there is not already an edge from q to qi then

9: add an edge from q to q i to the roadmap R

10: end if

11: end for

12: end for

13: return R

PRM路图建立算法中一个关键在于如何从

中采样(对应于伪代码1～4行)。从均匀分布的C-space中随机采样(同时避免碰撞)是一种可行的方案。而经验表明，在障碍物附近进行密集采样(sampling more densely)能够提高找到狭窄道路的可能性。第6～10行对每个采样点取k最近邻节点，尝试添加边最终构建成路图。实际编写代码时还需将成本计算出来以用于最短路径搜索。

下图是一个在

空间中创建路图的示例。图中采用的k值为3，而看到一个节点的边数大于3是由于该节点也能够是其它节点的邻近节点。

PRM路图创建

5、Python代码(PRM)

基于PRM算法，笔者编写了Python代码实现，同样用于求解上面的问题。算法分三个阶段：

1、采样

从均匀分布的[-0.5, 0.5] x [-0.5, 0.5]空间随机采样，并将起点和终点加入采样点；

2、创建边

用k最近邻节点连接两个节点，完整路图构建；

3、基于图搜索

用上一篇文章中编写的

算法搜索路径。

import numpy as np

import csv

import random

import math

from a_star import AStarPathPlanner

class PRMPathPlanner(object):

def __init__(self, obstacle_path, N, k):

self.N = N

self.k = k

self.x_start = [-0.5, -0.5]

self.goal = [0.5, 0.5]

distance = self.distance_between_two_point(self.x_start, self.goal)

self.nodes = [[1, self.x_start[0], self.x_start[1], distance]]

self.epslon = 0.03

self.edges_and_cost = []

self.edges = []

self.obstacles = []

with open(obstacle_path, "rt") as f_obj:

contents = csv.reader(f_obj)

for row in contents:

if row[0][0] != '#':

self.obstacles.append([float(row[0]), float(row[1]), float(row[2])])

def distance_between_two_point(self, point_1, point_2):

x1, y1 = point_1

x2, y2 = point_2

return np.sqrt((x1-x2)**2 + (y1-y2)**2)

def calculate_vector_angle(self, a, b):

a = np.array(a)

b = np.array(b)

a_norm = np.linalg.norm(a)

b_norm = np.linalg.norm(b)

a_dot_b = a.dot(b)

value = a_dot_b / (a_norm*b_norm)

if value > 1:

value = 1

if value < -1:

value = -1

# print(value)

theta = np.arccos(value)

return theta * 180 / np.pi

def point_in_circle(self, point, circle):

x, y, d = circle

r = d / 2

# self.epslon is accounted for the robot radius itself

if self.distance_between_two_point(point, [x, y]) < r + self.epslon:

return True

else:

return False

# find foot of perpendicular of the point to the line

def find_vertical_point(self, point, line):

[x0, y0] = point

[x1, y1, x2, y2] = line

k = -1 * ((x1 - x0) * (x2 - x1) + (y1 - y0) * (y2 - y1)) / ((x2 - x1) ** 2 + (y2 - y1) ** 2)

x = k * (x2 - x1) + x1

y = k * (y2 - y1) + y1

return [x, y]

def point_within_line_segment(self, point, point1, point2):

a = [point1[i]-point[i] for i in range(len(point))]

b = [point2[i] - point[i] for i in range(len(point))]

if self.calculate_vector_angle(a, b) > 90:

return True

else:

return False

# check if a line with point1 and point2 as its end

# is in collision with a circle

def in_collision_with_circle(self, point1, point2, circle):

[x1, y1] = point1

[x2, y2] = point2

line = [x1, y1, x2, y2]

[x0, y0, diameter] = circle

radius = diameter / 2

center = [x0, y0]

vertical_point = self.find_vertical_point(center, line)

# only when both point1 and point2 are outside the obstacle, the path might be collision free

# otherwise the path must be in collision

if not self.point_in_circle(point1, circle) and \

not self.point_in_circle(point2, circle):

distance_to_line = self.distance_between_two_point(vertical_point, center)

if distance_to_line > radius:

return False

if self.point_within_line_segment(vertical_point, point1, point2):

return True

else:

return False

else:

return True

# check if specified line segment is in collision with any obstacles

def in_collision(self, point1, point2):

collision = False

for each_circle in self.obstacles:

if self.in_collision_with_circle(point1, point2, each_circle):

collision = True

break

return collision

def construct_roadmap(self):

index = 1

while len(self.nodes) < self.N:

x = random.uniform(-0.5, 0.5)

y = random.uniform(-0.5, 0.5)

x_sample = [x, y]

is_free = True

for each_circle in self.obstacles:

if self.point_in_circle(x_sample, each_circle):

is_free = False

if is_free:

index = index + 1

distance = self.distance_between_two_point(x_sample, self.goal)

self.nodes.append([index, x_sample[0], x_sample[1], distance])

# add goal to nodes

self.nodes.append([index + 1, self.goal[0], self.goal[1], 0])

self.write_to_nodes_file()

# print(self.nodes)

# find k closest neighbors of each sample

for each_node in self.nodes:

current_index, x1, y1, cost1 = each_node

current_pos = [x1, y1]

neighbor_distance = []

for other_node in self.nodes:

other_index , x2, y2 ,cost2 = other_node

if(current_index != other_index):

d = self.distance_between_two_point(current_pos, [x2, y2])

neighbor_distance.append([other_index, d])

neighbor_distance = sorted(neighbor_distance, key=lambda d:d[1])

# find k closest collision-free neighbors for current node

del neighbor_distance[3:]

for neighbor in neighbor_distance:

neighbor_index, neighbor_cost = neighbor

# extract neighbor information

node, x, y, cost = self.nodes[neighbor_index - 1]

if not self.in_collision(current_pos, [x, y]) \

and [current_index, neighbor_index] not in self.edges \

and [neighbor_index, current_index] not in self.edges:

self.edges.append([current_index, neighbor_index])

self.edges_and_cost.append([current_index, neighbor_index, neighbor_cost])

# print(self.edges_and_cost)

self.write_to_edges_file()

def write_to_edges_file(self):

with open('edges.csv', 'wt') as f_obj:

writer = csv.writer(f_obj, delimiter=',')

for each_row in self.edges_and_cost:

writer.writerow(each_row)

def write_to_nodes_file(self):

with open('nodes.csv', 'wt') as f_obj:

writer = csv.writer(f_obj, delimiter=',')

for each_row in self.nodes:

writer.writerow(each_row)

if __name__ == "__main__":

prm = PRMPathPlanner('obstacles.csv', 200, 3)

prm.construct_roadmap()

# now we have got a roadmap representing C-free

# call A star algorithm to find the shortest path

planner = AStarPathPlanner("nodes.csv", "edges.csv")

success, path = planner.search_for_path()

if success:

print(path[::-1])

planner.save_path_to_file("path.csv")

else:

print("no solution found")

使用时，构造函数PRMPathPlanner中传入3个参数，依次是：描述障碍物的文件名，采样点总数，最近邻数目。采样点数目应当足够多，以使得起点和终点在路图中能够得到连接。调用construct_roadmap后，在

中构建路图，并将采样点和边信息分别写入文件nodes.csv和edges.csv。最后将生成的文件传入AStarPathPlanner，调用search_for_path搜索与当前路图对应的最短路径。以下是代码运行后的2次路径输出和V-rep仿真结果。

代码输出：

[1, 64, 40, 114, 22, 191, 135, 10, 136, 134, 177, 68, 155, 41, 167, 129, 8, 149, 178, 190, 39, 169, 26, 53, 107, 86, 154, 48, 199, 179, 23, 70, 201]

路径仿真：

PRM + A*

代码输出：

[1, 54, 78, 76, 36, 101, 50, 32, 117, 38, 22, 88, 26, 49, 186, 37, 40, 92, 28, 164, 73, 141, 56, 132, 39, 6, 27, 143, 176, 89, 84, 61, 46, 201]

路径仿真：

PRM + A*