基于RRT算法的路径规划
快速随机搜索树算法（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算法的路径规划相关推荐
- 详细介绍用MATLAB实现基于A*算法的路径规划（附完整的代码，代码逐行进行解释）（一）--------A*算法简介和环境的创建
本系列文章主要介绍基于A*算法的路径规划的实现,并使用MATLAB进行仿真演示.本文作为本系列的第一篇文章主要介绍如何进行环境的创建,还有一定要记得读前言!!! 本系列文章链接: ------- ...
- 路径规划算法：基于樽海鞘算法的路径规划算法- 附代码
路径规划算法:基于樽海鞘优化的路径规划算法- 附代码 文章目录 路径规划算法:基于樽海鞘优化的路径规划算法- 附代码 1.算法原理 1.1 环境设定 1.2 约束条件 1.3 适应度函数 2.算法结果 ...
- 路径规划算法：基于蜻蜓算法的路径规划算法- 附代码
路径规划算法:基于蜻蜓优化的路径规划算法- 附代码 文章目录 路径规划算法:基于蜻蜓优化的路径规划算法- 附代码 1.算法原理 1.1 环境设定 1.2 约束条件 1.3 适应度函数 2.算法结果 3 ...
- 路径规划算法：基于飞蛾扑火算法的路径规划算法- 附代码
路径规划算法:基于飞蛾扑火优化的路径规划算法- 附代码 文章目录 路径规划算法:基于飞蛾扑火优化的路径规划算法- 附代码 1.算法原理 1.1 环境设定 1.2 约束条件 1.3 适应度函数 2.算法 ...
- 【三维路径规划】基于RRT实现三维路径规划matlab源码
对RRT算法感兴趣,是因为我对它不仅在二维平面上适用,在高维空间也能使用而感到心动,最近比较忙,下周或者什么时候要要用代码亲自实现一下. 路径规划都有哪些方法呢?比较流行的有:图搜索.势场法.RRT ...
- 【RRT三维路径规划】基于matlab RRT_Star算法三维路径规划【含Matlab源码 1571期】
一.获取代码方式 获取代码方式1: 完整代码已上传我的资源:[三维路径规划]基于matlab RRT_Star算法三维路径规划[含Matlab源码 1571期] 点击上面蓝色字体,直接付费下载,即可. ...
- 自动驾驶路径规划——基于概率采样的路径规划算法（RRT、RRT*）
目录 1. RRT算法背景 1.1 RRT算法核心思想 1.2 RRT算法优缺点 2. 经典RRT算法 2.1 RRT算法流程 2.2 RRT伪代码 3. 基于目标概率采样 4. RRT*算法 4.1 ...
- 基于SSD的自动路径规划算法
目录 1.场景需求 2.路径规划算法简介 2.1 .PRM算法简介 2.2.RRT算法简介 3.基于SSD的自动路径规划算法简介 4.基于SSD的自动路径规划算法详解 4.1.利用外置摄像头获取图像或 ...
- rrt算法流程图_RRT算法移动机器人路径规划(快速扩展随机树).pdf
( ) 第 34 卷 第 5期 南京理工大学学报 自然科学版 Vo l. 34 No. 5 20 10年 10 月 Journal of N anj ing Un iversity of Scienc ...
- Dijkstra、RRT两类路径规划算法原理的直观理解
在路径规划的算法里,有两大类算法是很常用的,一类是基于搜索和图的Dijkstra算法,还有一类是基于采样的RRT算法.本文对其算法原理进行简单的理解,力图生动的展示枯燥的数学公式背后精彩的思想. Di ...
最新文章
- （转）HTTP 协议之压缩
- html怎么消除打印的进纸,打印机缺纸状态怎么消除？
- JavaScript中this指向问题
- mysql单表最大数据量_你的Mysql库真需要Adaptive Hash Index
- .NET Core开发实战（第9课：命令行配置提供程序）--学习笔记
- Android画板控件,可以写字，签名，画画并生成图片
- 数学家的亿万商业王国：创建“验证码”和“多邻国”，20岁就被盖茨挖去微软...
- [译] 手摸手教你如何轻松发布私有 App
- jgit - java实现git操作
- 17个机器学习的常用算法！
- IDEA for Mac设置JVM运行参数解决运行卡顿问题
- 视频分辨率过高，导致部分手机播放失败
- C++基础继承和多态（水上飞机）
- 打卡赠书 | 养成一个思考习惯
- 谷歌浏览器如何重置？谷歌浏览器恢复默认设置？
- android 投放 电视,手机投屏到电视的5种方法 看完才知道原来这么简单！
- 使用VS2012进行单元测试
- ESP32 寻迹模块测试
- 有关AOP术语（织入、增强等）的个人理解
- QT学习日志（附：简易计算器，qq登录界面，简易绘图板，植物大战僵尸魔改版项目实践）