自动驾驶算法（四）：RRT*算法讲解与代码实现（基于采样的路径规划）

算法（四）：RRT*算法讲解与代码实现（基于采样的路径规划）"/>

自动驾驶算法（四）：RRT*算法讲解与代码实现（基于采样的路径规划）

目录

1 RRT算法和RRT*算法

2 RRT*代码相比于RRT的改进

3 RRT*完整代码

1 RRT算法和RRT*算法

        从上篇博客我们可以看出，RRT算法找到最短路径特别快。因为它是一段一段的过去的，但同时它产生的路径也是非常糟糕、随机的只要找到了终点就会结束。

        因此我们提出RRT*算法的动机是能否找到一条最优的路径。

        看看我们的想法：

        红色圈画的是我们生长出来的结果，我们再新生长树的时候还是和RRT算法相同的方法，但是我们再生长完这个节点的时候，我们为它重新寻找父节点，我们需要先定一个范围，即红色的光晕（这个范围需要我们去指定的），我们把圆圈里面的点假设为它的父节点去判断一下看看这条路径是否会更加的优秀。

        现在它可以选择的父亲节点有两个，我们看第一个：

        最右面的节点：现在的路径就是画出的红色圆圈部分，右面再连上新节点

        还是原本的路径更加好～因此不需要改变

        如果选择最左面的点作为父节点，那么就有了这么一条路径。这条路径比先前的更加好，那么我们选择这个节点作为父节点。

        同时，我们将范围内的节点做重新连接：

        我们找到了Xnew是重新连接后最好的一个节点。对x1、x2重新连接，他们都会以Xnew作为父节点重新判断一下：若以X1为例会产生 Xnear--Xnew--X1路径，前面的都是一样的。但是Xnear-X1 < Xnear--Xnew--X1不满足条件。若X2重新连接如右图，X2原本的路径是Xnear--X1--X2，但Xnear--Xnew--X2 < Xnear--X1--X2，因此我们选择新路径。

        我们比对下RRT和RRT*的结果：

        RRT：

        RRT*：

2 RRT*代码相比于RRT的改进

    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 = NonelastPathLength = float('inf')

        我们比RRT算法会初始化。

        前面的流程和RRT一样：

        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]# steertheta = 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)

        在这里我们增加两部分代码：

        1.选择红框范围内的节点：find_near_nodes

    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) ** 2for node in self.node_list]# 比半径小near_inds = [d_list.index(i) for i in d_list if i <= r ** 2]return near_inds

        我们选择动态规划这个选择的半径大小，随着节点数量越来越大log(n_node)的增长越来越缓慢，因此math.log(n_node) / n_node的值越来越小。我们返回所有红圈内节点的索引。

        然后我们看看choose_parent：

    def choose_parent(self, newNode, nearInds):# 圈里是否有候选节点if len(nearInds) == 0:return newNodedList = []for i in nearInds:dx = newNode.x - self.node_list[i].xdy = newNode.y - self.node_list[i].yd = 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 newNodenewNode.cost = minCostnewNode.parent = minIndreturn newNode

        函数传来的参数为新生成的节点newode以及和它在一个圈内的节点，如果圈内没有候选节点则退出。

        我们遍历每一个圈内的节点：

        math.hypot(dx, dy) 用于计算以 (dx, dy) 为坐标的点到原点 (0, 0) 的直线距离（欧几里得距离）。

        具体来说，dx 和 dy 分别是点的横纵坐标的差值。math.hypot(dx, dy) 将返回这两个坐标差值构成的直角三角形的斜边长度，也就是点到原点的距离。

        例如，如果 dx 为 3，dy 为 4，那么math.hypot(dx, dy) 将返回 5，因为这是一个以 (3, 4) 为直角三角形的斜边长度，也就是点 (3, 4) 到原点 (0, 0) 的距离。

        d也就是两个点（nednode和一个圆的点）的直线距离，判断这两个点之间是否会有障碍物，若没有的话更新代价self.node_list[i].cost + d，也就是通过邻接点到这个节点的距离。        

        如果存在路径最小的点则更新我们最新节点的父节点和代价值。

        看下节点重新连接函数：

    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 + dif 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 - 1nearNode.cost = s_cost

        还是遍历新加入的节点和它相邻圆圈里的点，以newnode作为父节点 计算两条节点之间的距离。如果比原先小则替换。

3 RRT*完整代码

import copy
import math
import random
import timeimport matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation as Rot
import numpy as npshow_animation = Trueclass 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 = Noneself.goal = Noneself.min_rand = randArea[0]self.max_rand = randArea[1]self.expand_dis = expandDisself.goal_sample_rate = goalSampleRateself.max_iter = maxIterself.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 = Nonefor 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 pathdef 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 = NonelastPathLength = 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]# steertheta = 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) - 1tempPath = self.get_final_course(lastIndex)tempPathLen = self.get_path_len(tempPath)if lastPathLength > tempPathLen:path = tempPathlastPathLength = tempPathLenprint("current path length: {}, It costs {} s".format(tempPathLen, time.time()-start_time))return pathdef 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 infinitecBest = float('inf')path = None# Computing the sampling spacecMin = 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 foundrnd = self.informed_sample(cBest, cMin, xCenter, C)n_ind = self.get_nearest_list_index(self.node_list, rnd)nearestNode = self.node_list[n_ind]# steertheta = 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) - 1tempPath = self.get_final_course(lastIndex)tempPathLen = self.get_path_len(tempPath)if tempPathLen < cBest:path = tempPathcBest = tempPathLenprint("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 pathdef 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 rnddef choose_parent(self, newNode, nearInds):# 圈里是否有候选节点if len(nearInds) == 0:return newNodedList = []for i in nearInds:dx = newNode.x - self.node_list[i].xdy = newNode.y - self.node_list[i].yd = 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 newNodenewNode.cost = minCostnewNode.parent = minIndreturn newNodedef 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) ** 2for node in self.node_list]# 比半径小near_inds = [d_list.index(i) for i in d_list if i <= r ** 2]return near_indsdef 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) + xCenterrnd = [rnd[(0, 0)], rnd[(1, 0)]]else:rnd = self.sample()return rnd@staticmethoddef sample_unit_ball():a = random.random()b = random.random()if b < a:a, b = b, asample = (b * math.cos(2 * math.pi * a / b),b * math.sin(2 * math.pi * a / b))return np.array([[sample[0]], [sample[1]], [0]])@staticmethoddef get_path_len(path):pathLen = 0for 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@staticmethoddef line_cost(node1, node2):return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)@staticmethoddef 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 minIndexdef 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_indreturn newNodedef is_near_goal(self, node):# 计算距离d = self.line_cost(node, self.goal)if d < self.expand_dis:return Truereturn Falsedef 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 + dif 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 - 1nearNode.cost = s_cost@staticmethoddef distance_squared_point_to_segment(v, w, p):# Return minimum distance between line segment vw and point pif np.array_equal(v, w):return (p - v).dot(p - v)  # v == w casel2 = (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 segmentreturn (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  # collisionreturn Truedef check_collision(self, nearNode, theta, d):tmpNode = copy.deepcopy(nearNode)end_x = tmpNode.x + math.cos(theta) * dend_y = tmpNode.y + math.sin(theta) * dreturn 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.parentpath.append([self.start.x, self.start.y])return pathdef 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)@staticmethoddef plot_ellipse(xCenter, cBest, cMin, e_theta):  # pragma: no covera = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0b = cBest / 2.0angle = math.pi / 2.0 - e_thetacx = 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 = xself.y = yself.cost = 0.0self.parent = Nonedef 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()