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

1、采样法

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

一个随机或者确定函数从C-space或state space中选择样本点
一个用于评估样本点或者运动是否在 $\chi_{free}$ 空间的函数
一个选择之前自由空间里邻近样本点的函数
一个局部规划器，用于连接新的样本点

采样法放弃了网格法得到的分辨率最优解（resolution-optimal solutions），由此换来了在处理高自由度机器人规划问题的满意解（satisficing solution），且求解速度快速。所选择的样本点构成了一个路图或搜索树。相比网格法，采样法可通过更少的样本点来逼近 $\chi_{free}$ ，因为随着搜索空间自由度的增加，网格法的样本点数目是以指数形式增长的。大部分采样法是概率完备的，也就是说，在有解的情况下，当样本点数目趋于无穷大，找到解的概率趋于100%。

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

2、RRT算法

RRT算法搜索了一条无碰撞运动，用于从初始状态 $x_{start}$ 到达目标集合 $\chi_{goal}$ 。当用于运动学问题时， $x$ 代表了位型 $q$ ，当用于动力学问题时， $x$ 则同时包含了速度。RRT算法从 $x_{start}$ 开始生成一个树，以下是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行将从均匀分布的 $\chi$ 中随机地（略倾向于 $\chi_{goal}$ ）选取一个 $x_{sample}$ 。第4行中的 $x_{nearest}$ 是搜索树 $T$ 中距离 $x_{sample}$ 最近的节点。第5行中的 $x_{new}$ 可以选择[ $x_{nearest}$ , $x_{sample}$ ]线段上的一点，距离 $x_{nearest}$ 有一小段距离 $d$ 。由于 $d$ 很小，可以用一个简单的局部规划器返回一条直线运动，连接 $x_{new}$ 到 $x_{nearest}$ ，如果运动是无碰撞的， $x_{new}$ 将被加入到搜索树 $T$ 中。

均匀分布的样本点会让生成的树快速探索 $\chi_{free}$ 空间，下图是由RRT生成的一个树：

RRT算法生成树

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

2.1、第3行：采样器（Sampler）

最常见的方法是随机地从均匀分布的 $\chi$ 空间采样。比如，对于欧几里德C-space $\mathbb{R}^{n}$ ，以及 $n$ 关节机器人的C-space $T^{n} = S^{1}\times \cdot \cdot \cdot \times S^{1}$ ，我们可以在每个关节上均匀采样，对于平面移动机器人的C-space $\mathbb{R}^{n}\times S^{1}$ ，可以分别在 $\mathbb{R}^{n}$ 和 $S^{1}$ 上均匀采样。

对于动态系统，则是在状态空间中均匀采样，分为两部分：在C-space中均匀采样和在有界速度集上均匀采样（即 $(q,\dot{q})$ ）。

实际上，样本点的也不一定需要随机选择。比如可以采用一个确定的采样方案，在 $\chi$ 空间生成逐渐细化的网格，最终会在状态空间产生浓密的样本点，当样本点无穷多时，样本将与 $\chi$ 空间无限接近。

2.2、第4行：定义最近节点（Nearest Node）

寻找最近节点依赖于在 $\chi$ 空间中如何定义距离。对于在 $\mathbb{R}^{n}$ 空间上无运动约束的机器人来说，一个很自然的选择就是采用欧几里德距离。对于某些空间，距离的定义就不是这么明显了。

比如平面上的小车机器人，它的C-space表达为 $\mathbb{R}^{n}\times S^{1}$ ，下图中哪一个与 $x_{sample}$ 更近呢？

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

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

2.3、第5行：局部规划器（Local Planner）

局部规划器的作用是寻找到一条路径，从 $x_{nearest}$ 到某个与 $x_{sample}$ 更近的节点 $x_{new}$ 。规划器应该简单、运行快速，以下是三种规划器：

直线规划器
获得的规划是一条通往 $x_{new}$ 的直线， $x_{new}$ 可以选择为 $x_{sample}$ 或者选择为距离 $x_{sample}$ 一个固定距离 $d$ 处。这种规划器适用于运动上无约束的系统。
离散控制规划器
对于有运动约束的系统，比如轮式移动机器人或动态系统，控制量可以被离散化至一个离散集合{ $u_{1},u_{2},...$ }。每一个控制量通过 $\dot{x} = f(x,u)$ 从 $x_{nearest}$ 积分 $\Delta t$ 时间，对应的获得一个新的状态，在所有的无碰撞状态中，选择与 $x_{sample}$ 最近的节点作为 $x_{new}$ 。
轮式机器人规划器
对于轮式机器人，规划器可以采用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次计算，就将 $x_{sample}$ 设置为目标点，并检查是否可以产生连接至目标的路径。这种策略体现了在均匀采样时略倾向于 $\chi_{goal}$ 。路径找到后生成文件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算法核心思想是建立路图来表达 $C_{free}$ 空间，而后再进行路径搜索。路图是一个无向图，即机器人可在任意边上双向运动。因此，PRM主要适用于运动学问题。路图建立好后，可尝试将起点 $q_{start}$ 和 $q_{goal}$ 连接至路图中最近点。最后，用路径搜索算法，比如 $A^{*}$ 来完成路径规划。以下是用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路图建立算法中一个关键在于如何从 $C_{free}$ 中采样（对应于伪代码1～4行）。从均匀分布的C-space中随机采样（同时避免碰撞）是一种可行的方案。而经验表明，在障碍物附近进行密集采样（sampling more densely）能够提高找到狭窄道路的可能性。第6～10行对每个采样点取k最近邻节点，尝试添加边最终构建成路图。实际编写代码时还需将成本计算出来以用于最短路径搜索。

下图是一个在 $\mathbb{R}^{2}$ 空间中创建路图的示例。图中采用的k值为3，而看到一个节点的边数大于3是由于该节点也能够是其它节点的邻近节点。

PRM路图创建

5、Python代码（PRM）

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

1、采样
从均匀分布的[-0.5, 0.5] x [-0.5, 0.5]空间随机采样，并将起点和终点加入采样点；
2、创建边
用k最近邻节点连接两个节点，完整路图构建；
3、基于图搜索
用上一篇文章中编写的 $A^{*}$ 算法搜索路径。

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后，在 $C_{free}$ 中构建路图，并将采样点和边信息分别写入文件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*