RRT与RRT*算法具体步骤与程序详解（python）

问题很多de流星 2023-06-10 原文

提示：前面写了A*、Dijkstra算法

文章目录

前言

RRT和RRT*的区别：

RRT的中文名为快速随机探索树，它的原理很简单，实际上就是维护一棵路径树：从起点开始，在空间中随机采样，并找到路径树上与采样点最接近且能与它无障碍地连接的点，连接这个点与采样点，将采样点加入路径树，直至终点附近区域被探索到。这种方式无法保证得到的路径是最优的。
RRT* 在RRT基础上做了改进，主要是进行了重新选择父节点和重布线的操作。试想在RRT中，我们的采样点最终与整棵树上和它最近的点连了起来，但这未必是最好的选择，我们的最终目的是让这个点与起点的距离尽可能近。RRT* 便对此做了改进，它在采样点加入路径树以后，以其为圆心画了一个小圈，考虑是否有更好的父节点，连到那些节点上能使起点到该点的距离更短（尽管那些节点并不是离采样点最近的点）。如果选择了更加合适的父节点，那么就把它们连接起来，并去除原来的连线（重布线）。

一、RRT的原理与步骤

我的原理启蒙：RRT算法原理图解
根据这篇文章，班门弄斧自己推导一遍这个过程，加强理解：

如图所示，红色的圆是起点，黄色的圆是终点，黑色代表障碍物
RRT的原理如下：

在空间中随机采样
如图中的蓝色圆，将其作为目标点
确定生长树的生长方向
以刚刚生成的随机点为目标，遍历生长树上的现存节点，计算每个节点到该随机点的距离，筛选出距离最小的节点作为最近点。此时树上仅存在起点（一颗没发芽的种子），所以直接选取起点为最近点。以最近点和随机点的连线为生长方向，如图中红色箭头所示
向目标点生长
生长步长是固定的，可由程序决定，但不宜太大也不宜太小，太小的话路径规划时间长，太大则会略过目标点。从此时的最近点也就是起点沿着生长方向生长一个步长得到一个生长点（紫色圆）
循环1~2步
随机采样（蓝色圆形）

确定生长树的生长方向，图中共有两个点，红色和紫色圆，离目标点（蓝色）最近的是红色点，以最近点和随机点的连线为生长方向，如图中红色箭头所示

从此时的最近点也就是起点沿着生长方向生长一个步长得到一个生长点（紫色圆）

随机采样（蓝色圆形）

确定生长树的生长方向，以图中离目标点（蓝色）最近的点和随机点的连线为生长方向，如图中红色箭头所示，从此时的最近点也就是起点沿着生长方向生长一个步长得到一个生长点（紫色圆）

随机采样（蓝色圆形）

确定生长树的生长方向，以图中离目标点（蓝色）最近的点和随机点的连线为生长方向，如图中红色箭头所示

从此时的最近点也就是起点沿着生长方向生长一个步长得到一个生长点（紫色圆），但是生长点都长障碍物里面去了会发生碰撞，生长失败！

剔除该生长节点，此次生长作废，不合格，树不接受。

重复以上的步骤，直到有生长节点进入终点的设定邻域

不断追溯它们的父节点，可找到一条从起点到终点的安全路径。如图中绿色线所示

二、RRT算法编写的步骤

1.算法步骤

初始化整个空间，定义初始点、终点、采样点数、点与点之间的步长t等信息
在空间中随机产生一个点xrand
在已知树的点集合中找到距离这个随机点最近的点xnear
在xnear到xrand的直线方向上从xnear以步长t截取点xnew
判断从xnear到xnew之间是否存在障碍物，若存在则舍弃该点
将new点加入到树集合中
循环2~6，循环结束条件：有一个new点在终点的设定邻域内

2.算法的实现

初始化整个空间，定义初始点、终点、采样点数、点与点之间的步长t等信息

from math import sqrt
import numpy as np
import random
import itertools
import matplotlib.pyplot as plt
import warnings

warnings.filterwarnings('ignore')

# 初始化整个空间，定义初始点、终点、采样点数、点与点之间的步长t等信息
x_width = 25  # 空间的长度
y_width = 12  # 空间的宽度
error_list = [[0 for i in range(0, x_width)] for j in range(0, y_width)]
error_list[2][10] = 1
error_list[3][10] = 1
error_list[4][10] = 1
error_list[5][10] = 1
error_list[6][10] = 1
error_list[7][10] = 1
error_list[8][10] = 1

x0 = 6  # 定义初始点的x坐标
y0 = 4  # 定义初始点的y坐标
xn = 17  # 定义终点的x坐标
yn = 5  # 定义终点的y坐标
t = 1  # 点与点之间的步长
error_list[y0][x0] = 4
error_list[yn][xn] = 3
error_list = np.array(error_list)

# print(error_list)
plt.figure()
plt.xlim((-1, x_width))
plt.ylim((-1, y_width))
plt.xlabel('x')
plt.ylabel('y')
plt.xticks(np.arange(x_width))
plt.yticks(np.arange(y_width))
plt.grid()

tree_list = []
tree_list.append([x0, y0, x0, y0])  # 把起点作为树的点放入列表，避免随机点与起点重合
plt.plot(x0, y0, 'ro')
plt.plot(xn, yn, marker='o', color='yellow')
plt.plot([10, 10, 10, 10, 10, 10, 10], [2, 3, 4, 5, 6, 7, 8], 'k-', linewidth='5')

在空间中随机产生一个点xrand，这个点不能在tree_list里面，构建一个函数

# 在空间中随机产生一个点xrand ->这个点不能是起点
def product_rand(tree_list):
    x_width = 25  # 空间的长度
    y_width = 12  # 空间的宽度
    random_point = list(itertools.product(range(0, x_width), range(0, y_width)))
    xrand = random.sample(random_point, 1)
    xrand = list(xrand[0])  # 将随机点转换成list形式
    tree_list = np.array(tree_list)
    tree = tree_list[:, 0:2]
    while xrand in tree:  # 如果随机点在树的点列表里，重新生成随机点
        xrand = random.sample(random_point, 1)
        xrand = list(xrand[0])  # 将随机点转换成list形式
    return xrand

在已知树的点集合中找到距离这个随机点最近的点xnear，构建一个函数

# 在已知树的点集合中找到距离这个随机点最近的点xnear
def product_near(tree_list, xrand):
    m = np.inf
    for i in range(0, len(tree_list)):
        if abs(tree_list[i][0] - xrand[0]) + abs(tree_list[i][1] - xrand[1]) < m:
            m = abs(tree_list[i][0] - xrand[0]) + abs(tree_list[i][1] - xrand[1])
            xnear = [tree_list[i][0], tree_list[i][1]]
    return xnear

确定方向：在xnear到xrand的直线方向上从xnear以步长t截取点xnew，构建一个函数

def decide_direction(xrand, xnear, t):
    z_value = sqrt((xnear[0] - xrand[0]) ** 2 + (xnear[1] - xrand[1]) ** 2)  # 斜边长度
    cos_value = (xrand[0] - xnear[0]) / z_value
    sin_value = (xrand[1] - xnear[1]) / z_value
    xnew = [(xnear[0] + t * cos_value), (xnear[1] + t * sin_value)]
    return xnew

判断从xnear到xnew之间是否存在障碍物，若存在则舍弃该点

xrand = product_rand(tree_list)  # 随机生成点
xnear = product_near(tree_list, xrand)
xnew = decide_direction(xrand, xnear, t)
if xnear[0] != xrand[0]:
    k = (xrand[1] - xnear[1]) / (xrand[0] - xnear[0])
    y = k * (10 - xnear[0]) + xnear[1]
else:
    y = 0

while 10 <= max(xnear[0], xnew[0]) and 10 <= min(xnear[0], xnew[0]) and 2 <= y <= 8:
    xrand = product_rand(tree_list)  # 随机生成点
    xnear = product_near(tree_list, xrand)
    xnew = decide_direction(xrand, xnear, t)
    if xrand[0] - xnear[0] != 0:
        k = (xrand[1] - xnear[1]) / (xrand[0] - xnear[0])
        y = k * (10 - xnear[0]) + xnear[1]
tree_list.append([xnew[0], xnew[1], xnear[0], xnear[1]])
plt.plot(xrand[0], xrand[1], marker='o', color='cyan')
plt.plot(xnew[0], xnew[1], color='violet', marker='o')

循环，循环结束条件：有树节点在终点的设定固定邻域之内

# 循环
while ((xnew[0] - xn) ** 2 + (xnew[1] - yn) ** 2) > 1:
    xrand = product_rand(tree_list)  # 随机生成点
    xnear = product_near(tree_list, xrand)
    xnew = decide_direction(xrand, xnear, t)
    if xnear[0] != xrand[0]:
        k = (xrand[1] - xnear[1]) / (xrand[0] - xnear[0])
        y = k * (10 - xnear[0]) + xnear[1]
    else:
        y = 0

    while 10 <= max(xnear[0], xnew[0]) and 10 <= min(xnear[0], xnew[0]) and 2 <= y <= 8:
        xrand = product_rand(tree_list)  # 随机生成点
        xnear = product_near(tree_list, xrand)
        xnew = decide_direction(xrand, xnear, t)
        if xrand[0] - xnear[0] != 0:
            k = (xrand[1] - xnear[1]) / (xrand[0] - xnear[0])
            y = k * (10 - xnear[0]) + xnear[1]
    tree_list.append([xnew[0], xnew[1], xnear[0], xnear[1]])
    plt.plot(xrand[0], xrand[1], marker='o', color='cyan')
    plt.plot(xnew[0], xnew[1], color='violet', marker='o')

循环以找到父节点，将这些点保存在routine_list列表中，并可视化

tree_list = np.array(tree_list)
routine_list = [[xn,yn]]
n = len(tree_list)-1
x = tree_list[n,0]
y = tree_list[n,1]
f_x = tree_list[n,2]
f_y = tree_list[n,3]
routine_list.append([x,y])
search_list=[]
while [x0,y0] not in routine_list:
    search_list = tree_list[np.where((tree_list[:,0]==f_x) & (tree_list[:,1]==f_y))][0]
    search_list = search_list.tolist()
    routine_list.append([search_list[0],search_list[1]])
    f_x = search_list[2]
    f_y = search_list[3]

print(routine_list)
routine_list = np.array(routine_list)
plt.plot(routine_list[:,0], routine_list[:,1], '-', linewidth='2')
plt.show()

三、RRT*算法编写的步骤

RRT算法只能找到一条可行路径，并不能保证找到一条最优路径，RRT* 算法在RRT算法的基础上增加了两步：rewrite和random relink。也就是重写和随机重连。
重写就是在新节点xnew加入到树种之后，重新为它选择父节点，好让它到起始点的路径长度（代价）更小。
随机重连就是在重写完成之后，对新节点xnew附近一定范围内的节点进行重连。重连就是，检查一下如果把xnew附近的这些节点的父节点设置为xnew，这些节点的代价会不会减小。如果能够减小，就把这些节点的父节点更改为xnew；否则，就不更改。RRT* 算法考虑每一个节点到出发点的距离，为此每一个节点会增加一个属性：distance_to_start，即到出发点的距离。相应地在每一个节点选择父节点地时候，新节点的距离等于父节点的距离加上父节点到子节点的直线距离。

1.算法的步骤

在RRT的基础上增加两个功能：
①rewrite重写

遍历整个树，
获得到新节点xnew的距离小于一定阈值（比如1.5倍的步长，也就是1.5*t）的所有节点
将这些节点加入到一个名为candidate_parent_of_newpoint的列表中，
为了方便，这些节点的distance不再用来存储到出发点的距离，而是用来存储如果把该节点设置为xnew的父节点的话，xnew到出发点的距离。
找到candidate_parent_of_newpoint列表中具有最小distance的那个节点，返回他的索引index，将新节点newpoint的父节点设置为index。

②random relink

遍历整个列表，对每一个节点执行如下动作{
if(该节点到xnew的距离小于一定的阈值，比如1.6倍的步长，也就是1.6*t){
if（该节点现在的distance>把该节点的父节点更新为newpoint之后的distance){
把该节点的父节点设置为xnew，并更新该节点的distance值
更新以该节点为根节点的子树中的每个节点的distance。
}
}

2.算法的实现

rewrite（重写）：

# rewrite重写
def rewrite(tree_list, t, xnew):
    # 遍历整个树
    candidate_parent_of_xnew = []
    for i in range(0, len(tree_list)):
        distance = sqrt((xnew[0] - tree_list[i][0]) ** 2 + (xnew[1] - tree_list[i][1]) ** 2)
        # 获得新节点xnew的距离小于一定阈值（比如1.5倍步长，也就是1.5*t）所有节点
        if distance < 1.5 * t and (xnew[0] != tree_list[i][0] or xnew[1] != tree_list[i][1]):
            distance = tree_list[i][4] + distance
            candidate_parent_of_xnew.append([tree_list[i][0], tree_list[i][1], distance])
    candidate_parent_of_xnew = np.array(candidate_parent_of_xnew)
    # 将这些节点加入到candidate_parent_of_xnew列表中
    parent_point = candidate_parent_of_xnew[np.where(candidate_parent_of_xnew[:, 2] == candidate_parent_of_xnew[:, 2].min())]
    tree_list.append([xnew[0], xnew[1], parent_point[0][0], parent_point[0][1], parent_point[0][2]])
    # 找到candidate_parent_of_xnew列表中具有最小distance的那个节点，将新节点xnew的父节点设置为该节点
    return tree_list

random relink：

# random relink
def random_relink(tree_list, t, xnew):
    # 遍历整个列表，对每一个节点执行如下动作：
    tree_list = np.array(tree_list)
    for i in range(0, len(tree_list)):
        parent_distance = sqrt((xnew[0] - tree_list[i, 0]) ** 2 + (xnew[1] - tree_list[i, 1]) ** 2)
        if parent_distance < 1.6 * t:
            child_distance = parent_distance + tree_list[
                np.where((tree_list[:, 0] == xnew[0]) & (tree_list[:, 1] == xnew[1])), 4]
            if tree_list[i][4] > child_distance:
                tree_list[np.where((tree_list[:, 0] == xnew[0]) & (tree_list[:, 1] == xnew[1])), 2] = xnew[0]
                tree_list[np.where((tree_list[:, 0] == xnew[0]) & (tree_list[:, 1] == xnew[1])), 3] = xnew[1]
                tree_list[np.where((tree_list[:, 0] == xnew[0]) & (tree_list[:, 1] == xnew[1])), 4] = child_distance
                for j in range(0, len(tree_list)):
                    if tree_list[j, 2] == tree_list[i, 0] and tree_list[j, 3] == tree_list[i, 1]:
                        d = sqrt((tree_list[i, 0] - tree_list[j, 0]) ** 2 + (tree_list[i, 1] - tree_list[j, 1]) ** 2)
                        tree_list[j, 4] = child_distance + d
    return tree_list.tolist()

效果图：

三、所有程序附录

RRT算法

from math import sqrt
import numpy as np
import random
import itertools
import matplotlib.pyplot as plt
import warnings

warnings.filterwarnings('ignore')

# 初始化整个空间，定义初始点、终点、采样点数、点与点之间的步长t等信息
x_width = 25  # 空间的长度
y_width = 12  # 空间的宽度
error_list = [[0 for i in range(0, x_width)] for j in range(0, y_width)]
error_list[2][10] = 1
error_list[3][10] = 1
error_list[4][10] = 1
error_list[5][10] = 1
error_list[6][10] = 1
error_list[7][10] = 1
error_list[8][10] = 1

x0 = 6  # 定义初始点的x坐标
y0 = 4  # 定义初始点的y坐标
xn = 17  # 定义终点的x坐标
yn = 5  # 定义终点的y坐标
t = 1  # 点与点之间的步长
error_list[y0][x0] = 4
error_list[yn][xn] = 3
error_list = np.array(error_list)

# print(error_list)
plt.figure()
plt.xlim((-1, x_width))
plt.ylim((-1, y_width))
plt.xlabel('x')
plt.ylabel('y')
plt.xticks(np.arange(x_width))
plt.yticks(np.arange(y_width))
plt.grid()

tree_list = []
tree_list.append([x0, y0, x0, y0])  # 把起点作为树的点放入列表，避免随机点与起点重合
plt.plot(x0, y0, 'ro')
plt.plot(xn, yn, marker='o', color='yellow')
plt.plot([10, 10, 10, 10, 10, 10, 10], [2, 3, 4, 5, 6, 7, 8], 'k-', linewidth='5')


# 在空间中随机产生一个点xrand ->这个点不能是起点
def product_rand(tree_list):
    x_width = 25  # 空间的长度
    y_width = 12  # 空间的宽度
    random_point = list(itertools.product(range(0, x_width), range(0, y_width)))
    xrand = random.sample(random_point, 1)
    xrand = list(xrand[0])  # 将随机点转换成list形式
    tree_list = np.array(tree_list)
    tree = tree_list[:, 0:2]
    while xrand in tree:  # 如果随机点在树的点列表里，重新生成随机点
        xrand = random.sample(random_point, 1)
        xrand = list(xrand[0])  # 将随机点转换成list形式
    return xrand


# 在已知树的点集合中找到距离这个随机点最近的点xnear
def product_near(tree_list, xrand):
    m = np.inf
    for i in range(0, len(tree_list)):
        if abs(tree_list[i][0] - xrand[0]) + abs(tree_list[i][1] - xrand[1]) < m:
            m = abs(tree_list[i][0] - xrand[0]) + abs(tree_list[i][1] - xrand[1])
            xnear = [tree_list[i][0], tree_list[i][1]]
    return xnear


# 确定方向：在xnear到xrand的直线方向上从xnear以步长t截取点xnew
# tree_list.append(xrand)
def decide_direction(xrand, xnear, t):
    z_value = sqrt((xnear[0] - xrand[0]) ** 2 + (xnear[1] - xrand[1]) ** 2)  # 斜边长度
    cos_value = (xrand[0] - xnear[0]) / z_value
    sin_value = (xrand[1] - xnear[1]) / z_value
    xnew = [(xnear[0] + t * cos_value), (xnear[1] + t * sin_value)]
    return xnew


# 判断从xnear到xnew之间是否存在障碍物，若存在则舍弃该点
xrand = product_rand(tree_list)  # 随机生成点
xnear = product_near(tree_list, xrand)
xnew = decide_direction(xrand, xnear, t)
if xnear[0] != xrand[0]:
    k = (xrand[1] - xnear[1]) / (xrand[0] - xnear[0])
    y = k * (10 - xnear[0]) + xnear[1]
else:
    y = 0

while 10 <= max(xnear[0], xnew[0]) and 10 <= min(xnear[0], xnew[0]) and 2 <= y <= 8:
    xrand = product_rand(tree_list)  # 随机生成点
    xnear = product_near(tree_list, xrand)
    xnew = decide_direction(xrand, xnear, t)
    if xrand[0] - xnear[0] != 0:
        k = (xrand[1] - xnear[1]) / (xrand[0] - xnear[0])
        y = k * (10 - xnear[0]) + xnear[1]
tree_list.append([xnew[0], xnew[1], xnear[0], xnear[1]])
plt.plot(xrand[0], xrand[1], marker='o', color='cyan')
plt.plot(xnew[0], xnew[1], color='violet', marker='o')

# 循环
while ((xnew[0] - xn) ** 2 + (xnew[1] - yn) ** 2) > 1:
    xrand = product_rand(tree_list)  # 随机生成点
    xnear = product_near(tree_list, xrand)
    xnew = decide_direction(xrand, xnear, t)
    if xnear[0] != xrand[0]:
        k = (xrand[1] - xnear[1]) / (xrand[0] - xnear[0])
        y = k * (10 - xnear[0]) + xnear[1]
    else:
        y = 0

    while 10 <= max(xnear[0], xnew[0]) and 10 <= min(xnear[0], xnew[0]) and 2 <= y <= 8:
        xrand = product_rand(tree_list)  # 随机生成点
        xnear = product_near(tree_list, xrand)
        xnew = decide_direction(xrand, xnear, t)
        if xrand[0] - xnear[0] != 0:
            k = (xrand[1] - xnear[1]) / (xrand[0] - xnear[0])
            y = k * (10 - xnear[0]) + xnear[1]
    tree_list.append([xnew[0], xnew[1], xnear[0], xnear[1]])
    plt.plot(xrand[0], xrand[1], marker='o', color='cyan')
    plt.plot(xnew[0], xnew[1], color='violet', marker='o')

tree_list = np.array(tree_list)
routine_list = [[xn,yn]]
n = len(tree_list)-1
x = tree_list[n,0]
y = tree_list[n,1]
f_x = tree_list[n,2]
f_y = tree_list[n,3]
routine_list.append([x,y])
search_list=[]
while [x0,y0] not in routine_list:
    search_list = tree_list[np.where((tree_list[:,0]==f_x) & (tree_list[:,1]==f_y))][0]
    search_list = search_list.tolist()
    routine_list.append([search_list[0],search_list[1]])
    f_x = search_list[2]
    f_y = search_list[3]

print(routine_list)
routine_list = np.array(routine_list)
plt.plot(routine_list[:,0], routine_list[:,1], '-', linewidth='2')
plt.show()

RRT*算法

from math import sqrt
import numpy as np
import random
import itertools
import matplotlib.pyplot as plt
import warnings

warnings.filterwarnings('ignore')

# 初始化整个空间，定义初始点、终点、采样点数、点与点之间的步长t等信息
x_width = 25  # 空间的长度
y_width = 12  # 空间的宽度
error_list = [[0 for i in range(0, x_width)] for j in range(0, y_width)]
error_list[2][10] = 1
error_list[3][10] = 1
error_list[4][10] = 1
error_list[5][10] = 1
error_list[6][10] = 1
error_list[7][10] = 1
error_list[8][10] = 1

x0 = 6  # 定义初始点的x坐标
y0 = 4  # 定义初始点的y坐标
xn = 17  # 定义终点的x坐标
yn = 5  # 定义终点的y坐标
t = 1  # 点与点之间的步长
error_list[y0][x0] = 4
error_list[yn][xn] = 3
error_list = np.array(error_list)

# print(error_list)
plt.figure()
plt.xlim((-1, x_width))
plt.ylim((-1, y_width))
plt.xlabel('x')
plt.ylabel('y')
plt.xticks(np.arange(x_width))
plt.yticks(np.arange(y_width))
plt.grid()

tree_list = []
tree_list.append([x0, y0, x0, y0, 0])  # 把起点作为树的点放入列表，避免随机点与起点重合
plt.plot(x0, y0, 'ro')
plt.plot(xn, yn, marker='o', color='yellow')
plt.plot([10, 10, 10, 10, 10, 10, 10], [2, 3, 4, 5, 6, 7, 8], 'k-', linewidth='5')


# 在空间中随机产生一个点xrand ->这个点不能是起点
def product_rand(tree_list):
    x_width = 25  # 空间的长度
    y_width = 12  # 空间的宽度
    random_point = list(itertools.product(range(0, x_width), range(0, y_width)))
    xrand = random.sample(random_point, 1)
    xrand = list(xrand[0])  # 将随机点转换成list形式
    tree_list = np.array(tree_list)
    tree = tree_list[:, 0:2]
    while xrand in tree:  # 如果随机点在树的点列表里，重新生成随机点
        xrand = random.sample(random_point, 1)
        xrand = list(xrand[0])  # 将随机点转换成list形式
    return xrand


# 在已知树的点集合中找到距离这个随机点最近的点xnear
def product_near(tree_list, xrand):
    m = np.inf
    for i in range(0, len(tree_list)):
        if abs(tree_list[i][0] - xrand[0]) + abs(tree_list[i][1] - xrand[1]) < m:
            m = abs(tree_list[i][0] - xrand[0]) + abs(tree_list[i][1] - xrand[1])
            xnear = [tree_list[i][0], tree_list[i][1]]
    return xnear


# 确定方向：在xnear到xrand的直线方向上从xnear以步长t截取点xnew
# tree_list.append(xrand)
def decide_direction(xrand, xnear, t):
    z_value = sqrt((xnear[0] - xrand[0]) ** 2 + (xnear[1] - xrand[1]) ** 2)  # 斜边长度
    cos_value = (xrand[0] - xnear[0]) / z_value
    sin_value = (xrand[1] - xnear[1]) / z_value
    xnew = [(xnear[0] + t * cos_value), (xnear[1] + t * sin_value)]
    return xnew


# 判断从xnear到xnew之间是否存在障碍物，若存在则舍弃该点
xrand = product_rand(tree_list)  # 随机生成点
xnear = product_near(tree_list, xrand)
xnew = decide_direction(xrand, xnear, t)
if xnear[0] != xrand[0]:
    k = (xrand[1] - xnear[1]) / (xrand[0] - xnear[0])
    y = k * (10 - xnear[0]) + xnear[1]
else:
    y = 0

while 10 <= max(xnear[0], xnew[0]) and 10 <= min(xnear[0], xnew[0]) and 2 <= y <= 8:
    xrand = product_rand(tree_list)  # 随机生成点
    xnear = product_near(tree_list, xrand)
    xnew = decide_direction(xrand, xnear, t)
    if xrand[0] - xnear[0] != 0:
        k = (xrand[1] - xnear[1]) / (xrand[0] - xnear[0])
        y = k * (10 - xnear[0]) + xnear[1]
    else:
        y = 0

tree_list.append([xnew[0], xnew[1], xnear[0], xnear[1], t])
plt.plot(xrand[0], xrand[1], marker='o', color='cyan')
plt.plot(xnew[0], xnew[1], color='violet', marker='o')


# rewrite重写
def rewrite(tree_list, t, xnew):
    # 遍历整个树
    candidate_parent_of_xnew = []
    for i in range(0, len(tree_list)):
        distance = sqrt((xnew[0] - tree_list[i][0]) ** 2 + (xnew[1] - tree_list[i][1]) ** 2)
        # 获得新节点xnew的距离小于一定阈值（比如1.5倍步长，也就是1.5*t）所有节点
        if distance < 1.5 * t and (xnew[0] != tree_list[i][0] or xnew[1] != tree_list[i][1]):
            distance = tree_list[i][4] + distance
            candidate_parent_of_xnew.append([tree_list[i][0], tree_list[i][1], distance])
    candidate_parent_of_xnew = np.array(candidate_parent_of_xnew)
    # 将这些节点加入到candidate_parent_of_xnew列表中
    parent_point = candidate_parent_of_xnew[np.where(candidate_parent_of_xnew[:, 2] == candidate_parent_of_xnew[:, 2].min())]
    tree_list.append([xnew[0], xnew[1], parent_point[0][0], parent_point[0][1], parent_point[0][2]])
    # 找到candidate_parent_of_xnew列表中具有最小distance的那个节点，将新节点xnew的父节点设置为该节点
    return tree_list


# random relink
def random_relink(tree_list, t, xnew):
    # 遍历整个列表，对每一个节点执行如下动作：
    tree_list = np.array(tree_list)
    for i in range(0, len(tree_list)):
        parent_distance = sqrt((xnew[0] - tree_list[i, 0]) ** 2 + (xnew[1] - tree_list[i, 1]) ** 2)
        if parent_distance < 1.6 * t:
            child_distance = parent_distance + tree_list[
                np.where((tree_list[:, 0] == xnew[0]) & (tree_list[:, 1] == xnew[1])), 4]
            if tree_list[i][4] > child_distance:
                tree_list[np.where((tree_list[:, 0] == xnew[0]) & (tree_list[:, 1] == xnew[1])), 2] = xnew[0]
                tree_list[np.where((tree_list[:, 0] == xnew[0]) & (tree_list[:, 1] == xnew[1])), 3] = xnew[1]
                tree_list[np.where((tree_list[:, 0] == xnew[0]) & (tree_list[:, 1] == xnew[1])), 4] = child_distance
                for j in range(0, len(tree_list)):
                    if tree_list[j, 2] == tree_list[i, 0] and tree_list[j, 3] == tree_list[i, 1]:
                        d = sqrt((tree_list[i, 0] - tree_list[j, 0]) ** 2 + (tree_list[i, 1] - tree_list[j, 1]) ** 2)
                        tree_list[j, 4] = child_distance + d
    return tree_list.tolist()


# 循环
while ((xnew[0] - xn) ** 2 + (xnew[1] - yn) ** 2) > 1:
    xrand = product_rand(tree_list)  # 随机生成点
    xnear = product_near(tree_list, xrand)
    xnew = decide_direction(xrand, xnear, t)
    if xnear[0] != xrand[0]:
        k = (xrand[1] - xnear[1]) / (xrand[0] - xnear[0])
        y = k * (10 - xnear[0]) + xnear[1]
    else:
        y = 0

    while 10 <= max(xnear[0], xnew[0]) and 10 <= min(xnear[0], xnew[0]) and 2 <= y <= 8:
        xrand = product_rand(tree_list)  # 随机生成点
        xnear = product_near(tree_list, xrand)
        xnew = decide_direction(xrand, xnear, t)
        if xrand[0] - xnear[0] != 0:
            k = (xrand[1] - xnear[1]) / (xrand[0] - xnear[0])
            y = k * (10 - xnear[0]) + xnear[1]

    tree_list = rewrite(tree_list, t, xnew)
    tree_list = random_relink(tree_list, t, xnew)
    plt.plot(xrand[0], xrand[1], marker='o', color='cyan')
    plt.plot(xnew[0], xnew[1], color='violet', marker='o')

tree_list = np.array(tree_list)
routine_list = [[xn, yn]]
n = len(tree_list) - 1
x = tree_list[n, 0]
y = tree_list[n, 1]
f_x = tree_list[n, 2]
f_y = tree_list[n, 3]
routine_list.append([x, y])
search_list = []
while [x0, y0] not in routine_list:
    search_list = tree_list[np.where((tree_list[:, 0] == f_x) & (tree_list[:, 1] == f_y))][0]
    search_list = search_list.tolist()
    routine_list.append([search_list[0], search_list[1]])
    f_x = search_list[2]
    f_y = search_list[3]

print(routine_list)
routine_list = np.array(routine_list)
plt.plot(routine_list[:, 0], routine_list[:, 1], '-', linewidth='2')
plt.show()

详解 RRT span class token 算法 python 机器学习

有关RRT与RRT*算法具体步骤与程序详解（python）的更多相关文章

ruby - 在 Ruby 程序执行时阻止 Windows 7 PC 进入休眠状态 - 2
我需要在客户计算机上运行Ruby应用程序。通常需要几天才能完成(复制大备份文件)。问题是如果启用sleep，它会中断应用程序。否则，计算机将持续运行数周，直到我下次访问为止。有什么方法可以防止执行期间休眠并让Windows在执行后休眠吗？欢迎任何疯狂的想法;-) 最佳答案 Here建议使用SetThreadExecutionStateWinAPI函数，使应用程序能够通知系统它正在使用中，从而防止系统在应用程序运行时进入休眠状态或关闭显示。像这样的东西:require'Win32API'ES_AWAYMODE_REQUIRED=0x0
python - 如何使用 Ruby 或 Python 创建一系列高音调和低音调的蜂鸣声？ - 2
关闭。这个问题是opinion-based.它目前不接受答案。想要改进这个问题？更新问题，以便editingthispost可以用事实和引用来回答它.关闭4年前。Improvethisquestion我想在固定时间创建一系列低音和高音调的哔哔声。例如:在150毫秒时发出高音调的蜂鸣声在151毫秒时发出低音调的蜂鸣声200毫秒时发出低音调的蜂鸣声250毫秒的高音调蜂鸣声有没有办法在Ruby或Python中做到这一点？我真的不在乎输出编码是什么(.wav、.mp3、.ogg等等)，但我确实想创建一个输出文件。
ruby - 如何指定 Rack 处理程序 - 2
Rackup通过Rack的默认处理程序成功运行任何Rack应用程序。例如:classRackAppdefcall(environment)['200',{'Content-Type'=>'text/html'},["Helloworld"]]endendrunRackApp.new但是当最后一行更改为使用Rack的内置CGI处理程序时，rackup给出“NoMethodErrorat/undefinedmethod`call'fornil:NilClass”:Rack::Handler::CGI.runRackApp.newRack的其他内置处理程序也提出了同样的反对意见。例如Rack
ruby - 在 Ruby 中编写命令行实用程序 - 2
我想用ruby编写一个小的命令行实用程序并将其作为gem分发。我知道安装后，Guard、Sass和Thor等某些gem可以从命令行自行运行。为了让gem像二进制文件一样可用，我需要在我的gemspec中指定什么。最佳答案 Gem::Specification.newdo|s|...s.executable='name_of_executable'...endhttp://docs.rubygems.org/read/chapter/20 关于ruby-在Ruby中编写命令行实用程序
ruby-on-rails - Rails 应用程序之间的通信 - 2
我构建了两个需要相互通信和发送文件的Rails应用程序。例如，一个Rails应用程序会发送请求以查看其他应用程序数据库中的表。然后另一个应用程序将呈现该表的json并将其发回。我还希望一个应用程序将存储在其公共(public)目录中的文本文件发送到另一个应用程序的公共(public)目录。我从来没有做过这样的事情，所以我什至不知道从哪里开始。任何帮助，将不胜感激。谢谢! 最佳答案无论Rails是什么，几乎所有Web应用程序都有您的要求，大多数现代Web应用程序都需要相互通信。但是有一个小小的理解需要你坚持下去，网站不应直接访问彼此
ruby - 无法运行 Rails 2.x 应用程序 - 2
我尝试运行2.x应用程序。我使用rvm并为此应用程序设置其他版本的ruby:$rvmuseree-1.8.7-head我尝试运行服务器，然后出现很多错误:$script/serverNOTE:Gem.source_indexisdeprecated,useSpecification.Itwillberemovedonorafter2011-11-01.Gem.source_indexcalledfrom/Users/serg/rails_projects_terminal/work_proj/spohelp/config/../vendor/rails/railties/lib/r
ruby-on-rails - Rails 应用程序中的 Rails : How are you using application_controller. rb 是新手吗？ - 2
刚入门rails，开始慢慢理解。有人可以解释或给我一些关于在application_controller中编码的好处或时间和原因的想法吗？有哪些用例。您如何为Rails应用程序使用应用程序Controller？我不想在那里放太多代码，因为据我了解，每个请求都会调用此Controller。这是真的？最佳答案 ApplicationController实际上是您应用程序中的每个其他Controller都将从中继承的类(尽管这不是强制性的)。我同意不要用太多代码弄乱它并保持干净整洁的态度，尽管在某些情况下ApplicationContr
ruby-on-rails - 如何在我的 Rails 应用程序 View 中打印 ruby 变量的内容？ - 2
我是一个Rails初学者，但我想从我的RailsView(html.haml文件)中查看Ruby变量的内容。我试图在ruby中打印出变量(认为它会在终端中出现)，但没有得到任何结果。有什么建议吗？我知道Rails调试器，但更喜欢使用inspect来打印我的变量。最佳答案您可以在View中使用puts方法将信息输出到服务器控制台。您应该能够在View中的任何位置使用Haml执行以下操作:-puts@my_variable.inspect 关于ruby-on-rails-如何在我的R
Python 相当于 Perl/Ruby ||= - 2
这个问题在这里已经有了答案:关闭10年前。PossibleDuplicate:Pythonconditionalassignmentoperator对于这样一个简单的问题表示歉意，但是谷歌搜索||=并不是很有帮助；)Python中是否有与Ruby和Perl中的||=语句等效的语句？例如:foo="hey"foo||="what"#assignfooifit'sundefined#fooisstill"hey"bar||="yeah"#baris"yeah"另外，类似这样的东西的通用术语是什么？条件分配是我的第一个猜测，但Wikipediapage跟我想的不太一样。
ruby - 检查是否通过 require 执行或导入了 Ruby 程序 - 2
如何检查Ruby文件是否是通过“require”或“load”导入的，而不是简单地从命令行执行的？例如:foo.rb的内容:puts"Hello"bar.rb的内容require'foo'输出:$./foo.rbHello$./bar.rbHello基本上，我想调用bar.rb以不执行puts调用。最佳答案将foo.rb改为:if__FILE__==$0puts"Hello"end检查__FILE__-当前ruby文件的名称-与$0-正在运行的脚本的名称。关于ruby-检查是否

RRT与RRT*算法具体步骤与程序详解（python）

文章目录

前言

一、RRT的原理与步骤

二、RRT算法编写的步骤

1.算法步骤

2.算法的实现

三、RRT*算法编写的步骤

1.算法的步骤

2.算法的实现

三、所有程序附录

RRT算法

RRT*算法

有关RRT与RRT*算法具体步骤与程序详解（python）的更多相关文章

随机推荐