基于采样的路径搜索算法代码实现(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和PRM)_第1张图片
RRT算法生成树

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

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

最常见的方法是随机地从均匀分布的空间采样。比如,对于欧几里德C-space ,以及关节机器人的C-space ,我们可以在每个关节上均匀采样,对于平面移动机器人的C-space ,可以分别在和上均匀采样。

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

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

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

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

比如平面上的小车机器人,它的C-space表达为,下图中哪一个与更近呢?

基于采样的路径搜索算法代码实现(RRT和PRM)_第2张图片
哪一个位型和灰色的最近?

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

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]

路径仿真

基于采样的路径搜索算法代码实现(RRT和PRM)_第3张图片
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]

路径仿真

基于采样的路径搜索算法代码实现(RRT和PRM)_第4张图片
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是由于该节点也能够是其它节点的邻近节点。

基于采样的路径搜索算法代码实现(RRT和PRM)_第5张图片
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]

路径仿真:

基于采样的路径搜索算法代码实现(RRT和PRM)_第6张图片
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]

路径仿真:

基于采样的路径搜索算法代码实现(RRT和PRM)_第7张图片
PRM + A*

你可能感兴趣的:(基于采样的路径搜索算法代码实现(RRT和PRM))