路径规划-PRM算法（1）

probabilistic roadmap（PRM）算法是一类用于机器人路径规划的算法，最早在^[1]中被提出，属于随机规划器的一种，其数据的主要形式为无向图（另一种 RRT 基于树）。^[2]将 PRM 算法分成了两个阶段：learning 阶段和 query 阶段。其中 learning 阶段主要在 [[Configuration 空间]]中进行一定次数的采样（一般而言是均匀采样，后面为了解决一些狭小空间的问题进行了一些特殊的采样手段），并对采样点进行碰撞检测，再通过距离判断和碰撞检测将采样点进行连线，最终得到由有效采样点作为 vertex 而采样点之间的连线作为 edge（两点之间的距离很自然的作为 edge 的值）所形成的无向图 \(G =(V, E)\)。
PRM 算法的使用基于 3 个前提：

我们已经具备了碰撞检测的方法，无论是对 vertex 的检测还是对 edge 的检测。对于 vertex 的检测可能比较直观，而对于 edge，最简单的方式是在 edge 上插值得到很多个点，然后再像 vertex 一样进行碰撞检测。
无论是在 \(C\) 空间还是欧几里德空间，我们有比较合理的方式计算采样点之间的距离
已经具备一个可以使用的无向图路径规划器，例如 [[A_star算法]]。
PRM 算法的实现即是将上述 3 个前提工具进行组合使用，得到最终的结果。

learning 阶段

learning 阶段的目标是在 \(C_{free}\) 空间（即 \(C\) 空间中不会发生碰撞的空间）中得到一个无向图 G 。基本的实现方式如下：

将图的点和边置为空集
从 \(C_{free}\) 空间采样一个点（从 \(C\) 空间采样，直到找到一个不碰撞的点）
将点添加到图的点集合中，并尝试对新的点和图中原有的点进行连线。是否连线的标准为：1）不发生碰撞；2）在一定距离内。成功连线后，将连线添加到图的边集合中。
重复 2 与 3，直到采集到一定数量的点

这里在 \(2D\) 的 \(C\) 空间进行了仿真。
首先，将障碍物在 \(C\) 空间简化为多边形，多变形可能是 convex 的，也可以是 concave 的，甚至多边形中间有孔洞。
这里给出相应的多边形生成代码，大致思想为：在空间中某个 center 的位置向周边随机方向取 edge_num 个随机距离的点，这些点就是多边形的顶点。^[3]
使用 random_polygon 函数生成 3 个多变形障碍物。

import math
import numpy as np

def random_polygon(edge_num, center, radius_range):
    """
    generate points to construct a random polygon

    Parameters:
    -----------
    edge_num: a number
        edge numbers of polygon
    center: a list/tuple contain two numbers
        center of polygon
    radius_range: a list/tuple containing two numbers
        range of distances from center to polygon vertices

    Returns:
    --------
    points: ndarray
        points that can construct a random polygon
    """
    angles = uniform_random(0, 2 * np.pi, edge_num)
    angles = np.sort(angles)
    random_radius = uniform_random(radius_range[0], radius_range[1], edge_num)
    x = np.cos(angles) * random_radius
    y = np.sin(angles) * random_radius
    x = np.expand_dims(x, 1)
    y = np.expand_dims(y, 1)
    points = np.concatenate([x, y], axis=1)
    points += np.array(center)
    points = np.round(points).astype(np.int32)
    return points

polygon1 = random_polygon(8, [80, 80], [60, 70])
polygon2 = random_polygon(6, [250, 80], [40, 50])
polygon3 = random_polygon(5, [100, 280], [40, 80])
polygons = [polygon1,polygon2,polygon3]

然后，随机采样了 500 个点，并选取了不与障碍物干涉的前 100 个点。

sample_x = uniform_random(0, 350, 500)
sample_y = uniform_random(0, 350, 500)

sample_num = 100
samples = []
for point in zip(sample_x,sample_y):
    is_collided = False
    for polygon in polygons:
        if is_in_poly(point, polygon):
            is_collided = True
            break
    if not is_collided:
        samples.append(point)
    if len(samples)>=sample_num:
        break

这里面通过 is_in_poly 这个函数判断点是否与障碍物干涉，其中思想是从点往外射出一条射线，如果射线与多边形的交点是奇数，则说明点在多边形内部。^[4]

def is_in_poly(p, poly):
    """
    :param p: [x, y]
    :param poly: [[], [], [], [], ...]
    :return:
    """
    px, py = p
    is_in = False
    for i, corner in enumerate(poly):
        next_i = i + 1 if i + 1 < len(poly) else 0
        x1, y1 = corner
        x2, y2 = poly[next_i]
        if (x1 == px and y1 == py) or (x2 == px and y2 == py):  # if point is on vertex
            is_in = True
            break
        if min(y1, y2) < py <= max(y1, y2):  # find horizontal edges of polygon
            x = x1 + (py - y1) * (x2 - x1) / (y2 - y1)
            if x == px:  # if point is on edge
                is_in = True
                break
            elif x > px:  # if point is on left-side of line
                is_in = not is_in
    return is_in

然后我们将所得到的 100 个不与障碍物干涉的点构建成图。这里为了方便，直接使用的是 python 的库 NetworkX，NetworkX 中提供基本的无向图数据结构[^5]。

import networkx as nx
G = nx.Graph()
G.add_nodes_from(range(0,sample_num))

dist_threshold = 50
for i in range(samples.shape[0]):
    for j in range(i+1, samples.shape[0]):
        weight = np.linalg.norm(samples[i,:] - samples[j,:])
        if weight>dist_threshold:
            continue
        if is_line_cross_polys(samples[i,:], samples[j,:] ,polygons):
            continue
        G.add_weighted_edges_from([(i,j,weight)])

这里，我们再添加 edge 的时候进行了两个判断，一个是判断点的距离是否超过了设定的距离阈值 dist_threshold，另一个判断是通过函数 is_line_cross_polys 判断连线是否与多边形相交。is_line_cross_polys 主要实现是通过两点之间插值，检查插值点是否在多边形内。

def is_line_cross_polys(p1, p2, polys):
    t = np.linspace(0, 1, 100)
    interps = p1.reshape(-1,1)*t + p2.reshape(-1,1)*(1-t)
    for i in range(interps.shape[1]):
        for poly in polys:
            if is_in_poly(interps[:,i].tolist(),poly):
                return True
    return False

最终，得到一个无向图 \(G\)，并通过无向图 \(G\) 得到一个随机采样的 roadmap。

query 阶段

learning 阶段得到无向图 G 后，query 阶段要利用 G 来找到一条路径连接起始点和目标点。首先要将起始点和目标点与 G 连接起来其方式与 learning 阶段添加 vertex 和 edge 的过程类似，之后，利用图规划器寻找一条路径：

尝试将起始点作为新的 vertex 连接到 G 中，如果无法连接则报错
尝试将目标点作为新的 vertex 连接到 G 中，如果无法连接则报错
使用图规划器在新的无向图中找到一条路径连接起始点与目标点

start = [150,300]
target = [270, 50]
extra_node = np.array([start, target])
G.add_nodes_from([samples.shape[0], samples.shape[0]+1])
for i in range(extra_node.shape[0]):
    for j in range(0, samples.shape[0]):
        weight = np.linalg.norm(extra_node[i,:] - samples[j,:])
        if weight>dist_threshold:
            continue
        if is_line_cross_polys(extra_node[i,:], samples[j,:] ,polygons):
            continue
        
        G.add_weighted_edges_from([(samples.shape[0]+i,j,weight)])

通过上述代码将起始点和目标点都加入了 \(G\) 中，并根据同样的规则添加 edge。
构建完成新的图之后，通过 NetworkX 自带的函数寻找最短路径。

path = nx.shortest_path(G, source=samples.shape[0], target=samples.shape[0]+1)

最终画出相应的结果。（图形和前面有一点差别，因为前后不是一次运行的结果，使用了不同的随机种子）

讨论

显然，PRM 算法最耗时的阶段在 learning 阶段，因为这个过程中要进行大量的碰撞检测的运算。所以，我们期望一旦得到了 learning 阶段的结果，应尽可能多的利用这个图。那么，对于动态环境，比如运动的障碍物，PRM 将很难发挥应有的价值，因为 learning 阶段的碰撞检测在不同时刻的结果将变得不一样。

Reference

Kavraki, L.E., P. Svestka, J.-C. Latombe, and M.H. Overmars. “Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces.” IEEE Transactions on Robotics and Automation 12, no. 4 (August 1996): 566–80. https://doi.org/10.1109/70.508439. ↩︎
一种简单的随机多边形生成方法_生成多边形算法-CSDN博客 ↩︎
python判断平面内一个点是否在多边形内_yolov5 python射线法判断点是否在多边形内-CSDN博客 ↩︎
Aric A. Hagberg, Daniel A. Schult and Pieter J. Swart, “Exploring network structure, dynamics, and function using NetworkX”, in Proceedings of the 7th Python in Science Conference (SciPy2008), Gäel Varoquaux, Travis Vaught, and Jarrod Millman (Eds), (Pasadena, CA USA), pp. 11–15, Aug 2008 ↩︎

posted @ 2024-05-08 23:07 pomolnc 阅读(143) 评论(0) 编辑收藏举报

会员力量，点亮园子希望

刷新页面返回顶部

pomolnc

路径规划-PRM算法（1）

learning 阶段

query 阶段

讨论

More Reading

Reference

公告