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

article/2025/10/4 12:21:01

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

文章目录

  • 前言
  • 一、RRT的原理与步骤
  • 二、RRT算法编写的步骤
    • 1.算法步骤
    • 2.算法的实现
  • 三、RRT*算法编写的步骤
    • 1.算法的步骤
    • 2.算法的实现
  • 三、所有程序附录
    • RRT算法
    • RRT*算法

前言

RRT和RRT*的区别：

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

一、RRT的原理与步骤

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

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

  1. 在空间中随机采样
    如图中的蓝色圆，将其作为目标点
    在这里插入图片描述
  2. 确定生长树的生长方向
    以刚刚生成的随机点为目标，遍历生长树上的现存节点，计算每个节点到该随机点的距离，筛选出距离最小的节点作为最近点。此时树上仅存在起点（一颗没发芽的种子），所以直接选取起点为最近点。以最近点和随机点的连线为生长方向，如图中红色箭头所示
    在这里插入图片描述
  3. 向目标点生长
    生长步长是固定的，可由程序决定，但不宜太大也不宜太小，太小的话路径规划时间长，太大则会略过目标点。从此时的最近点也就是起点沿着生长方向生长一个步长得到一个生长点（紫色圆）
    在这里插入图片描述
  4. 循环1~2步
    随机采样（蓝色圆形）
    在这里插入图片描述
    确定生长树的生长方向，图中共有两个点，红色和紫色圆，离目标点（蓝色）最近的是红色点，以最近点和随机点的连线为生长方向，如图中红色箭头所示
    在这里插入图片描述
    从此时的最近点也就是起点沿着生长方向生长一个步长得到一个生长点（紫色圆）
    在这里插入图片描述
    随机采样（蓝色圆形）
    在这里插入图片描述
    确定生长树的生长方向，以图中离目标点（蓝色）最近的点和随机点的连线为生长方向，如图中红色箭头所示，从此时的最近点也就是起点沿着生长方向生长一个步长得到一个生长点（紫色圆）
    在这里插入图片描述
    随机采样（蓝色圆形）
    在这里插入图片描述
    确定生长树的生长方向，以图中离目标点（蓝色）最近的点和随机点的连线为生长方向，如图中红色箭头所示
    在这里插入图片描述
    从此时的最近点也就是起点沿着生长方向生长一个步长得到一个生长点（紫色圆），但是生长点都长障碍物里面去了会发生碰撞，生长失败！
    在这里插入图片描述
    剔除该生长节点，此次生长作废，不合格，树不接受。
    在这里插入图片描述
    重复以上的步骤，直到有生长节点进入终点的设定邻域
    在这里插入图片描述
    不断追溯它们的父节点，可找到一条从起点到终点的安全路径。如图中绿色线所示
    在这里插入图片描述

二、RRT算法编写的步骤

1.算法步骤

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

2.算法的实现

  1. 初始化整个空间，定义初始点、终点、采样点数、点与点之间的步长t等信息
from math import sqrt
import numpy as np
import random
import itertools
import matplotlib.pyplot as plt
import warningswarnings.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] = 1x0 = 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')

在这里插入图片描述

  1. 在空间中随机产生一个点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
  1. 在已知树的点集合中找到距离这个随机点最近的点xnear，构建一个函数
# 在已知树的点集合中找到距离这个随机点最近的点xnear
def product_near(tree_list, xrand):m = np.inffor 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
  1. 确定方向：在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_valuesin_value = (xrand[1] - xnear[1]) / z_valuexnew = [(xnear[0] + t * cos_value), (xnear[1] + t * sin_value)]return xnew
  1. 判断从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 = 0while 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')
  1. 循环，循环结束条件：有树节点在终点的设定固定邻域之内
# 循环
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 = 0while 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')

在这里插入图片描述

  1. 循环以找到父节点，将这些点保存在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] + distancecandidate_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_distancefor 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 + dreturn tree_list.tolist()

效果图：
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

三、所有程序附录

RRT算法

from math import sqrt
import numpy as np
import random
import itertools
import matplotlib.pyplot as plt
import warningswarnings.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] = 1x0 = 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.inffor 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_valuesin_value = (xrand[1] - xnear[1]) / z_valuexnew = [(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 = 0while 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 = 0while 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 warningswarnings.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] = 1x0 = 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.inffor 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_valuesin_value = (xrand[1] - xnear[1]) / z_valuexnew = [(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 = 0while 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 = 0tree_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] + distancecandidate_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_distancefor 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 + dreturn 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 = 0while 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()

http://chatgpt.dhexx.cn/article/rbheQPnW.shtml

相关文章

RRT算法原理图解

RRT算法原理图解

RRT算法原理图解 开始 本人很懒&#xff0c;习惯了只看不写。废话少说&#xff0c;直奔主题&#xff1a;原始RRT算法原理图文简介&#xff08;图都是我自己按照步骤一幅幅画的——闲的蛋疼&#xff0c;但应该比较直观易懂&#xff0c;能被借鉴参考也算我的功德&#xff09;。 R…
阅读更多...
linux中要怎么创建文件夹

linux中要怎么创建文件夹

我是一个linux初学者,由于工作上面需要,我需要在linux中创建一个文件夹,然后自学了一点点,其实创建文件夹很简单,下面分享给大家,越努力越幸运,共勉! 创建文件夹 mkdir 后面加文件夹名字 例如: mkdir aa 然后第一个文件夹就创好了 假如要在文件夹里面再创一个文件夹就是子目…
阅读更多...
Ubuntu系统下如何创建.txt文件

Ubuntu系统下如何创建.txt文件

问题 在Ubutnu系统下&#xff0c;右键桌面会发现并没有创建文本文件的选项。 解决 首先进入模板 会发现里面是空的 然后右键在终端打开 输入如下指令 sudo gedit 文本文件保存即可 这个时候在模板文件夹下就有 现在右键的时候就会有一个创建文本文件的选项了。
阅读更多...
Linux中创建文件与文件夹

Linux中创建文件与文件夹

一、创建文件夹 命令&#xff1a;mkdir 文件夹名 例&#xff1a; 一开始home目录下没有test文件夹&#xff0c;命令创建后生成 二、创建文件 命令&#xff1a;touch 文件名 例&#xff1a; 一开始test文件夹下没有boot.properties&#xff0c;命令创建后生成 三、注意事项…
阅读更多...
Ubuntu零基础教学-Ubuntu下如何创建.txt记事本文件

Ubuntu零基础教学-Ubuntu下如何创建.txt记事本文件

环境：Ubuntu20.04 前言： 安装好ubuntu20.04后，发现右键菜单中没有新建空白文件，这样工作的时候需要创建文本文件就不是很方便；那么，基于这里，我们可以通过以下的方式把新建空白文件添加到右键哦！ 在此，针对小白系列教学，bug菌专门开放了一个Ubunt…
阅读更多...
linux中创建目录

linux中创建目录

在根下创建一个目录ceshi 1、用mkdir创建目录 2、用ls查看当前目录下的所有文件 3、拷贝需要复制的两个文件 4、将user移动至ceshi下&#xff0c;用move 5、用mv命令来为目录改名 linux中在root用户下创建目录 1、进入root用户目录&#xff0c;输入su后回车 2、查看当前路径…
阅读更多...
linux下创建文件和文件夹

linux下创建文件和文件夹

使用linux系统会有一些常见的命令&#xff0c;譬如说&#xff0c;创建文件夹&#xff0c;创建文件&#xff0c;这些命令都是比较常见的。 方法/步骤 首先说一下touch 创建二进制文件&#xff0c;用法就非常的简单&#xff0c;touch文件名 之间一定要空格。先查看一下有什么文…
阅读更多...
linux创建文件夹命令

linux创建文件夹命令

我们可以使用mkdir命令在 Linux 或类似 Unix 的操作系统中创建新目录或文件夹。本文将介绍如何在 Linux 或 Unix 系统中创建文件夹&#xff08;也称为“目录”&#xff09;。 操作步骤如下&#xff1a;1.在 Linux 中打开终端应用程序。2.输入mkdir命令。3.输入文件夹名称。 具…
阅读更多...
Linux：创建文件夹

Linux：创建文件夹

&#xff08;1&#xff09;输入命令&#xff1a;mkdir music&#xff0c;创建文件夹 music&#xff0c;再次查看列表&#xff0c;可以看到多了一个文件夹 music&#xff1b; &#xff08;2&#xff09;输入命令&#xff1a;mkdir a1 a2 a3&#xff0c;批量创建文件夹 a1、文件夹…
阅读更多...
linux创建文件

linux创建文件

linux创建文件 1、在 Linux 上使用重定向符&#xff08;>&#xff09;创建一个文件 标准重定向符允许我们创建一个 0KB 的空文件。它通常用于重定向一个命令的输出到一个新文件中。在没有命令的情况下使用重定向符号时&#xff0c;它会创建一个文件。 但是它不允许你在创建…
阅读更多...
Linux 创建目录和文件

Linux 创建目录和文件

mkdir 创建目录 在linux中&#xff0c;mkdir是创建目录的意思&#xff0c;是“make directories”的缩写&#xff1b;该命令用于创建新的目录&#xff0c;语法为“mkdir [-mp] 目录名”&#xff1b;设置参数“-m”用于手动配置创建目录的权限&#xff0c;设置参数“-p”用于递…
阅读更多...
Linux 创建文件

Linux 创建文件

目录 1. 使用重定向符&#xff08;>&#xff09;创建文件 2. 使用 touch 命令创建文件 3. 使用 echo 命令创建文件 4. 使用 printf 命令创建文件 5. 使用 cat 命令创建文件 6. 使用 vi / vim 创建文件 7. 使用 nano 创建文件 8. 使用 head 命令创建文件 9. 使用 tail 命令创…
阅读更多...
Python中colorbar全色表

Python中colorbar全色表

如图&#xff0c;所有cmap可选参数
阅读更多...
python 画图colorbar 颜色大全 plt.cm.get_cmap

python 画图colorbar 颜色大全 plt.cm.get_cmap

名字后_r取反
阅读更多...
matplotlib调节colorbar的大小

matplotlib调节colorbar的大小

调节plt.colorbar的fraction系数即可调节colorbar的大小 weight np.random.random([8, 8]) plt.imshow(weight) plt.colorbar(fraction0.05, pad0.05) plt.savefig(tjn.png, bbox_inchestight) plt.show()
阅读更多...
python可视化 matplotlib画图使用colorbar工具自定义颜色

python可视化 matplotlib画图使用colorbar工具自定义颜色

python matplotlib画图使用colorbar工具自定义颜色 colorbar&#xff08;draw colorbar without any mapple/plot&#xff09; 自定义colorbar可以画出任何自己想要的colorbar&#xff0c;自由自在、不受约束&#xff0c;不依赖于任何已有的图(plot/mappable)。这里使用的是m…
阅读更多...
MATLAB自定义colorbar

MATLAB自定义colorbar

matlab画平面分布图时colorbar的设置是非常重要的&#xff0c;好的colorbar不仅使图像更美观&#xff0c;而且能够使人更容易捕捉图上传递的信息。用过matlab的同学都知道matlab默认的colormap是jet, 也就是你画完图后输入“colorbar” 它所显示出来的颜色。此外&#xff0c;ma…
阅读更多...
MATLAB | 如何按照任意比例调整颜色条(colorbar)

MATLAB | 如何按照任意比例调整颜色条(colorbar)

之前写过的setPivot函数只能把颜色条的中点放到0处或者其他数值处&#xff1a; https://slandarer.blog.csdn.net/article/details/129341645 这次提供的函数可以将任意百分比的点位放置在任意数值处&#xff0c;这个函数大概长这样&#xff1a; 百分比点位设置 function s…
阅读更多...
matplotlib中【colormap】和【colorbar】的使用，以及用自己的颜色创建colormap

matplotlib中【colormap】和【colorbar】的使用，以及用自己的颜色创建colormap

目录 官方自带的colormap其他的colormap结果单独绘制一个colorbar 用自己的颜色创建colormap获取图片颜色给定一个基本颜色&#xff0c;通过改变饱和度来创建colorbar 官方自带的colormap import numpy as np import matplotlib.pyplot as pltnum_curves 100 #100条曲线 cm …
阅读更多...
解决python画图中colorbar设置刻度和标签字体大小

解决python画图中colorbar设置刻度和标签字体大小

介绍 python很火&#xff0c;因为有各种库的支持&#xff0c;所以功能格外强大。在可视化方面&#xff0c;目前用得较多的是matplotlib. 在基于matplotlib.pyplot画带色标(colorbar)的图时候&#xff0c;往往为了美观和科研用途&#xff0c;需要对colorbar的Ticks(刻度) &#…
阅读更多...
推荐文章

Copyright @ 2022~2023