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*