目录
1 RRT*与Informed RRT*
上篇博客我们介绍了RRT*算法：我们在找到一个路径以后我们还会反复的搜索。
Informed RRT*算法提出的动机(motivation)是能否增加渐近最优的速度呢？
根据这篇论文的思想：Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic。
我们通过限制采样范围来增加渐进最优的速度。也就是我们在找到路径后限制采样的范围（椭圆），它的长轴就是我们找到路径的cbest，短轴就是如上图，cmin是起点和终点连起来的距离。感兴趣的同学可以去看一下论文。通过cbest越来越小采样的范围也就越来越小。最后我们就会找到路径。我们看一下椭圆是怎么找的，和RRT*相比仅仅改变了采样的Sample函数：
2 Informed RRT*代码解析
start_time = time.time() self.start = Node(start[0], start[1]) self.goal = Node(goal[0], goal[1]) self.node_list = [self.start] # max length we expect to find in our 'informed' sample space, # starts as infinite cBest = float('inf') path = None # Computing the sampling space cMin = math.sqrt(pow(self.start.x - self.goal.x, 2) + pow(self.start.y - self.goal.y, 2)) xCenter = np.array([[(self.start.x + self.goal.x) / 2.0], [(self.start.y + self.goal.y) / 2.0], [0]]) a1 = np.array([[(self.goal.x - self.start.x) / cMin], [(self.goal.y - self.start.y) / cMin], [0]]) e_theta = math.atan2(a1[1], a1[0])这里先初始化cbest为无穷大，将path置为空路径。求出起始点和终点的距离cMin
Xcenter创建了一个 numpy 数组
xCenter，其中包含了self.start（导航起始点） 和self.goal（导航终止点）之间的中心点的 x 和 y 坐标，以及一个零。
a1 = np.array([[(self.goal.x - self.start.x) / cMin], [(self.goal.y - self.start.y) / cMin], [0]]): 创建了一个 numpy 数组a1，它表示了从self.start指向self.goal的单位向量，除以距离cMin。
e_theta = math.atan2(a1[1], a1[0]): 使用math.atan2计算了单位向量a1与 x 轴之间的角度e_theta（以弧度表示）。用数学公式表示如下：
C = np.array([[math.cos(e_theta), -math.sin(e_theta), 0], [math.sin(e_theta), math.cos(e_theta), 0], [0, 0, 1]])
和RRT与RRT*相比，只改变了采样函数，我们看采样函数：
def informed_sample(self, cMax, cMin, xCenter, C): if cMax < float('inf'): r = [cMax / 2.0, math.sqrt(cMax ** 2 - cMin ** 2) / 2.0, math.sqrt(cMax ** 2 - cMin ** 2) / 2.0] L = np.diag(r) xBall = self.sample_unit_ball() rnd = np.dot(np.dot(C, L), xBall) + xCenter rnd = [rnd[(0, 0)], rnd[(1, 0)]] else: rnd = self.sample() return rnd
def sample_unit_ball(): a = random.random() b = random.random() if b < a: a, b = b, a sample = (b * math.cos(2 * math.pi * a / b), b * math.sin(2 * math.pi * a / b)) return np.array([[sample[0]], [sample[1]], [0]])这段代码定义了一个名为 `sample_unit_ball` 的函数，它用于生成一个单位球内的随机点。
具体来说，代码中的操作如下：
1. `a = random.random()`: 生成一个在 [0, 1) 范围内的随机浮点数并将其赋给变量 `a`。
2. `b = random.random()`: 同样地，生成一个在 [0, 1) 范围内的随机浮点数并将其赋给变量 `b`。
3. `if b < a: a, b = b, a`: 如果 `b` 小于 `a`，则交换它们的值，确保 `a` 始终大于等于 `b`。
4. `sample = (b * math.cos(2 * math.pi * a / b), b * math.sin(2 * math.pi * a / b))`: 这里使用生成的随机数 `a` 和 `b` 计算了一个点 `sample`，它位于极坐标系中的角度 `2πa/b` 处，并且距离原点的距离为 `b`。
5. `return np.array([[sample[0]], [sample[1]], [0]])`: 将 `sample` 转换为一个列向量，并添加一个零作为第三个元素，最终返回一个二维的列向量。
总的来说，这个函数用于在单位球内生成一个随机点，它的坐标通过随机生成的参数 `a` 和 `b` 以及一些三角函数计算得到。
rnd = np.dot(np.dot(C, L), xBall) + xCenter这里是先把单位圆压缩成椭圆，得到新采样的点。
3 完整代码
import copy import math import random import time import matplotlib.pyplot as plt from scipy.spatial.transform import Rotation as Rot import numpy as np show_animation = True class RRT: # randArea采样范围[-2--18] obstacleList设置的障碍物 maxIter最大迭代次数 expandDis采样步长为2.0 goalSampleRate 以10%的概率将终点作为采样点 def __init__(self, obstacleList, randArea, expandDis=2.0, goalSampleRate=10, maxIter=200): self.start = None self.goal = None self.min_rand = randArea[0] self.max_rand = randArea[1] self.expand_dis = expandDis self.goal_sample_rate = goalSampleRate self.max_iter = maxIter self.obstacle_list = obstacleList # 存储RRT树 self.node_list = None # start、goal 起点终点坐标 def rrt_planning(self, start, goal, animation=True): start_time = time.time() self.start = Node(start[0], start[1]) self.goal = Node(goal[0], goal[1]) # 将起点加入node_list作为树的根结点 self.node_list = [self.start] path = None for i in range(self.max_iter): # 进行采样 rnd = self.sample() # 取的距离采样点最近的节点下标 n_ind = self.get_nearest_list_index(self.node_list, rnd) # 得到最近节点 nearestNode = self.node_list[n_ind] # 将Xrandom和Xnear连线方向作为生长方向 # math.atan2() 函数接受两个参数，分别是 y 坐标差值和 x 坐标差值。它返回的值是以弧度表示的角度，范围在 -π 到 π 之间。这个角度表示了从 nearestNode 指向 rnd 的方向。 theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) # 生长 ： 输入参数为角度、下标、nodelist中最近的节点 得到生长过后的节点 newNode = self.get_new_node(theta, n_ind, nearestNode) # 检查是否有障碍物 传入参数为新生城路径的两个节点 noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y) if noCollision: # 没有碰撞把新节点加入到树里面 self.node_list.append(newNode) if animation: self.draw_graph(newNode, path) # 是否到终点附近 if self.is_near_goal(newNode): # 是否这条路径与障碍物发生碰撞 if self.check_segment_collision(newNode.x, newNode.y, self.goal.x, self.goal.y): lastIndex = len(self.node_list) - 1 # 找路径 path = self.get_final_course(lastIndex) pathLen = self.get_path_len(path) print("current path length: {}, It costs {} s".format(pathLen, time.time()-start_time)) if animation: self.draw_graph(newNode, path) return path def rrt_star_planning(self, start, goal, animation=True): start_time = time.time() self.start = Node(start[0], start[1]) self.goal = Node(goal[0], goal[1]) self.node_list = [self.start] path = None lastPathLength = float('inf') for i in range(self.max_iter): rnd = self.sample() n_ind = self.get_nearest_list_index(self.node_list, rnd) nearestNode = self.node_list[n_ind] # steer theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) newNode = self.get_new_node(theta, n_ind, nearestNode) noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y) if noCollision: nearInds = self.find_near_nodes(newNode) newNode = self.choose_parent(newNode, nearInds) self.node_list.append(newNode) # 重联 self.rewire(newNode, nearInds) if animation: self.draw_graph(newNode, path) if self.is_near_goal(newNode): if self.check_segment_collision(newNode.x, newNode.y, self.goal.x, self.goal.y): lastIndex = len(self.node_list) - 1 tempPath = self.get_final_course(lastIndex) tempPathLen = self.get_path_len(tempPath) if lastPathLength > tempPathLen: path = tempPath lastPathLength = tempPathLen print("current path length: {}, It costs {} s".format(tempPathLen, time.time()-start_time)) return path def informed_rrt_star_planning(self, start, goal, animation=True): start_time = time.time() self.start = Node(start[0], start[1]) self.goal = Node(goal[0], goal[1]) self.node_list = [self.start] # max length we expect to find in our 'informed' sample space, # starts as infinite cBest = float('inf') path = None # Computing the sampling space cMin = math.sqrt(pow(self.start.x - self.goal.x, 2) + pow(self.start.y - self.goal.y, 2)) xCenter = np.array([[(self.start.x + self.goal.x) / 2.0], [(self.start.y + self.goal.y) / 2.0], [0]]) a1 = np.array([[(self.goal.x - self.start.x) / cMin], [(self.goal.y - self.start.y) / cMin], [0]]) e_theta = math.atan2(a1[1], a1[0]) # 论文方法求旋转矩阵（2选1） # first column of identity matrix transposed # id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3) # M = a1 @ id1_t # U, S, Vh = np.linalg.svd(M, True, True) # C = np.dot(np.dot(U, np.diag( # [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), # Vh) # 直接用二维平面上的公式（2选1） C = np.array([[math.cos(e_theta), -math.sin(e_theta), 0], [math.sin(e_theta), math.cos(e_theta), 0], [0, 0, 1]]) for i in range(self.max_iter): # Sample space is defined by cBest # cMin is the minimum distance between the start point and the goal # xCenter is the midpoint between the start and the goal # cBest changes when a new path is found rnd = self.informed_sample(cBest, cMin, xCenter, C) n_ind = self.get_nearest_list_index(self.node_list, rnd) nearestNode = self.node_list[n_ind] # steer theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) newNode = self.get_new_node(theta, n_ind, nearestNode) noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y) if noCollision: nearInds = self.find_near_nodes(newNode) newNode = self.choose_parent(newNode, nearInds) self.node_list.append(newNode) self.rewire(newNode, nearInds) if self.is_near_goal(newNode): if self.check_segment_collision(newNode.x, newNode.y, self.goal.x, self.goal.y): lastIndex = len(self.node_list) - 1 tempPath = self.get_final_course(lastIndex) tempPathLen = self.get_path_len(tempPath) if tempPathLen < cBest: path = tempPath cBest = tempPathLen print("current path length: {}, It costs {} s".format(tempPathLen, time.time()-start_time)) if animation: self.draw_graph_informed_RRTStar(xCenter=xCenter, cBest=cBest, cMin=cMin, e_theta=e_theta, rnd=rnd, path=path) return path def sample(self): # 取得1-100的随机数,如果比10大的话（以10%的概率取到终点） if random.randint(0, 100) > self.goal_sample_rate: # 在空间里随机采样一个点 rnd = [random.uniform(self.min_rand, self.max_rand), random.uniform(self.min_rand, self.max_rand)] else: # goal point sampling # 终点作为采样点 rnd = [self.goal.x, self.goal.y] return rnd def choose_parent(self, newNode, nearInds): # 圈里是否有候选节点 if len(nearInds) == 0: return newNode dList = [] for i in nearInds: dx = newNode.x - self.node_list[i].x dy = newNode.y - self.node_list[i].y d = math.hypot(dx, dy) theta = math.atan2(dy, dx) # 检测是否碰到障碍物 if self.check_collision(self.node_list[i], theta, d): # 计算距离 dList.append(self.node_list[i].cost + d) else: # 无穷大 dList.append(float('inf')) # 找到路径最小的的点 父节点 minCost = min(dList) minInd = nearInds[dList.index(minCost)] if minCost == float('inf'): print("min cost is inf") return newNode newNode.cost = minCost newNode.parent = minInd return newNode def find_near_nodes(self, newNode): n_node = len(self.node_list) # 动态变化 搜索到越后面的话半径越小 r = 50.0 * math.sqrt((math.log(n_node) / n_node)) # 找到节点 d_list = [(node.x - newNode.x) ** 2 + (node.y - newNode.y) ** 2 for node in self.node_list] # 比半径小 near_inds = [d_list.index(i) for i in d_list if i <= r ** 2] return near_inds def informed_sample(self, cMax, cMin, xCenter, C): if cMax < float('inf'): r = [cMax / 2.0, math.sqrt(cMax ** 2 - cMin ** 2) / 2.0, math.sqrt(cMax ** 2 - cMin ** 2) / 2.0] L = np.diag(r) xBall = self.sample_unit_ball() rnd = np.dot(np.dot(C, L), xBall) + xCenter rnd = [rnd[(0, 0)], rnd[(1, 0)]] else: rnd = self.sample() return rnd @staticmethod def sample_unit_ball(): a = random.random() b = random.random() if b < a: a, b = b, a sample = (b * math.cos(2 * math.pi * a / b), b * math.sin(2 * math.pi * a / b)) return np.array([[sample[0]], [sample[1]], [0]]) @staticmethod def get_path_len(path): pathLen = 0 for i in range(1, len(path)): node1_x = path[i][0] node1_y = path[i][1] node2_x = path[i - 1][0] node2_y = path[i - 1][1] pathLen += math.sqrt((node1_x - node2_x) ** 2 + (node1_y - node2_y) ** 2) return pathLen @staticmethod def line_cost(node1, node2): return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2) @staticmethod def get_nearest_list_index(nodes, rnd): # 遍历所有节点 计算采样点和节点的距离 dList = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in nodes] # 获得最近的距离所对应的索引 minIndex = dList.index(min(dList)) return minIndex def get_new_node(self, theta, n_ind, nearestNode): newNode = copy.deepcopy(nearestNode) # 坐标 newNode.x += self.expand_dis * math.cos(theta) newNode.y += self.expand_dis * math.sin(theta) # 代价 newNode.cost += self.expand_dis # 父亲节点 newNode.parent = n_ind return newNode def is_near_goal(self, node): # 计算距离 d = self.line_cost(node, self.goal) if d < self.expand_dis: return True return False def rewire(self, newNode, nearInds): n_node = len(self.node_list) # 新节点 和圆圈的候选点 for i in nearInds: nearNode = self.node_list[i] # 以newnode作为父节点 计算两条节点之间的距离 d = math.sqrt((nearNode.x - newNode.x) ** 2 + (nearNode.y - newNode.y) ** 2) s_cost = newNode.cost + d if nearNode.cost > s_cost: theta = math.atan2(newNode.y - nearNode.y, newNode.x - nearNode.x) if self.check_collision(nearNode, theta, d): nearNode.parent = n_node - 1 nearNode.cost = s_cost @staticmethod def distance_squared_point_to_segment(v, w, p): # Return minimum distance between line segment vw and point p if np.array_equal(v, w): return (p - v).dot(p - v) # v == w case l2 = (w - v).dot(w - v) # i.e. |w-v|^2 - avoid a sqrt # Consider the line extending the segment, # parameterized as v + t (w - v). # We find projection of point p onto the line. # It falls where t = [(p-v) . (w-v)] / |w-v|^2 # We clamp t from [0,1] to handle points outside the segment vw. t = max(0, min(1, (p - v).dot(w - v) / l2)) projection = v + t * (w - v) # Projection falls on the segment return (p - projection).dot(p - projection) def check_segment_collision(self, x1, y1, x2, y2): # 遍历所有的障碍物 for (ox, oy, size) in self.obstacle_list: dd = self.distance_squared_point_to_segment( np.array([x1, y1]), np.array([x2, y2]), np.array([ox, oy])) if dd <= size ** 2: return False # collision return True def check_collision(self, nearNode, theta, d): tmpNode = copy.deepcopy(nearNode) end_x = tmpNode.x + math.cos(theta) * d end_y = tmpNode.y + math.sin(theta) * d return self.check_segment_collision(tmpNode.x, tmpNode.y, end_x, end_y) def get_final_course(self, lastIndex): path = [[self.goal.x, self.goal.y]] while self.node_list[lastIndex].parent is not None: node = self.node_list[lastIndex] path.append([node.x, node.y]) lastIndex = node.parent path.append([self.start.x, self.start.y]) return path def draw_graph_informed_RRTStar(self, xCenter=None, cBest=None, cMin=None, e_theta=None, rnd=None, path=None): plt.clf() # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect( 'key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if rnd is not None: plt.plot(rnd[0], rnd[1], "^k") if cBest != float('inf'): self.plot_ellipse(xCenter, cBest, cMin, e_theta) for node in self.node_list: if node.parent is not None: if node.x or node.y is not None: plt.plot([node.x, self.node_list[node.parent].x], [ node.y, self.node_list[node.parent].y], "-g") for (ox, oy, size) in self.obstacle_list: plt.plot(ox, oy, "ok", ms=30 * size) if path is not None: plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') plt.plot(self.start.x, self.start.y, "xr") plt.plot(self.goal.x, self.goal.y, "xr") plt.axis([-2, 18, -2, 15]) plt.grid(True) plt.pause(0.01) @staticmethod def plot_ellipse(xCenter, cBest, cMin, e_theta): # pragma: no cover a = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0 b = cBest / 2.0 angle = math.pi / 2.0 - e_theta cx = xCenter[0] cy = xCenter[1] t = np.arange(0, 2 * math.pi + 0.1, 0.1) x = [a * math.cos(it) for it in t] y = [b * math.sin(it) for it in t] rot = Rot.from_euler('z', -angle).as_matrix()[0:2, 0:2] fx = rot @ np.array([x, y]) px = np.array(fx[0, :] + cx).flatten() py = np.array(fx[1, :] + cy).flatten() plt.plot(cx, cy, "xc") plt.plot(px, py, "--c") def draw_graph(self, rnd=None, path=None): plt.clf() # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect( 'key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if rnd is not None: plt.plot(rnd.x, rnd.y, "^k") for node in self.node_list: if node.parent is not None: if node.x or node.y is not None: plt.plot([node.x, self.node_list[node.parent].x], [ node.y, self.node_list[node.parent].y], "-g") for (ox, oy, size) in self.obstacle_list: # self.plot_circle(ox, oy, size) plt.plot(ox, oy, "ok", ms=30 * size) plt.plot(self.start.x, self.start.y, "xr") plt.plot(self.goal.x, self.goal.y, "xr") if path is not None: plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') plt.axis([-2, 18, -2, 15]) plt.grid(True) plt.pause(0.01) class Node: def __init__(self, x, y): self.x = x self.y = y self.cost = 0.0 self.parent = None def main(): print("Start rrt planning") # create obstacles # obstacleList = [ # (3, 3, 1.5), # (12, 2, 3), # (3, 9, 2), # (9, 11, 2), # ] # 设置障碍物 (圆点、半径) obstacleList = [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2), (9, 5, 2), (8, 10, 1)] # Set params # 采样范围 设置的障碍物 最大迭代次数 rrt = RRT(randArea=[-2, 18], obstacleList=obstacleList, maxIter=200) # 传入的是起点和终点 #path = rrt.rrt_planning(start=[0, 0], goal=[15, 12], animation=show_animation) #path = rrt.rrt_star_planning(start=[0, 0], goal=[15, 12], animation=show_animation) path = rrt.informed_rrt_star_planning(start=[0, 0], goal=[15, 12], animation=show_animation) print("Done!!") if show_animation and path: plt.show() if __name__ == '__main__': main()
4 算法比较
RRT：路径较差，非最优，在本例程中为6ms。
RRT*：路径渐进最优，在本例程中为34ms。
Informed RRT*：路径渐进最优，在本例程中为109ms。
看官姥爷看自己需求用奥。
5015
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
