快速隨機搜索樹算法（RRT）非常適合高緯度空間的路徑規劃，基于該算法隨機搜索性，對于未知空間具有很強的探索能力，但該算法 的缺點也比較突出，得到的路徑幾乎不可能是最優解，這也區別于Dijkstra、A*算法。那么RRT算法的基本原理是以起始點為根節點，在空間中隨機采樣獲得一個節點，連接根節點與當前節點判斷與周圍障礙物是否有碰撞（物體到直線的距離，方便計算的話可以用圓形作為障礙物，當然最好是判斷碰撞的距離大于計算距離），若有碰撞則放棄此次生長，若無則保留，一直迭代循環直到到達終點，那么這個生長過程形成的形狀類似樹狀型，正確路徑為枝干，四周部分為枝葉，

如上圖所示，是RRT算法在平面圖上完成的路徑規劃，缺點顯而易見，到目前為止，針對RRT算法的缺點很多研究者都提出了相關改進措施，從我所學以及所理解的方面做出以下改進優化，所以征求改進的話可以有針對性的朝著目標點方向生長優化路徑（做角度限制，柵格化地圖有效采樣），也可以改變生長長度（自適應生長最好）?，然后消除尖點做平滑處理（拉格朗日插值法，為了避免出現龍格現象所以最后采用了貝塞爾曲線進行優化，貝塞爾曲線，B樣曲線）改進后

?將改進后的算法運用于機械臂空間路徑規劃，使用MATLAB進行仿真

代碼實現是使用python語言實現，以RRT原始開源代碼為基礎進行的算法改進。

以上為個人在學習過程中的理解若有錯誤還請大佬指教 。

以下代碼（初始）可做學習參考

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:

? ? def __init__(self, obstacleList, randArea,
? ? ? ? ? ? ? ? ?expandDis=2.0, goalSampleRate=0, maxIter=100):

? ? ? ? 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
? ? ? ? self.node_list = None

? ? 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])
? ? ? ? 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]

? ? ? ? ??
? ? ? ? ? ? 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:
? ? ? ? ? ? ? ? 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 sample(self):
? ? ? ? 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]

? ? ? ? ? ? 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
? ? ? ?
? ? ? ? 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 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")
? ? ? ? plt.xlabel(u'x',fontsize=20)
? ? ? ? plt.ylabel(u'y',fontsize=20)
? ? ? ? plt.title(u"path planning",fontsize=16)

? ? ? ? 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, 16])
? ? ? ? #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, ?1),
? ? ? ? (3, ?9, ?2),
? ? ? ? (9, ?11, 1),
? ? ? ?(15, ?6, ?1),
? ? ? ?(0, ?0, ?0.8),
? ? ? ?(16, ?13, ?1),
? ? ? ?(0, ?12, ?1),
? ? ? ?(7.5 ?,4, ?1),
? ? ? ?(15.5, ?1.5, 1),
? ? ? ?(5.5, 0, ?1),
? ? ? ?(9, 7, 1),
? ? ? ?(6, ?14, 1)]
? ??
? ? rrt = RRT(randArea=[-2, 18], obstacleList=obstacleList, maxIter=200)
? ? path = rrt.rrt_planning(start=[0, 2], goal=[14, 12.5], animation=show_animation)
? ? on=show_animation
? ? print("Done!!")

? ? if show_animation and path:
? ? ? ? plt.show()

if __name__ == '__main__':
? ? main()
?

總結

以上是生活随笔為你收集整理的基于RRT算法的路径规划的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。