rrt算法的仿真实现和代码
时间: 2024-05-09 20:15:25 浏览: 6
以下是RRT算法的Python代码实现,用于路径规划:
```python
import random
import math
import matplotlib.pyplot as plt
class Node:
def __init__(self, x, y):
self.x = x
self.y = y
self.parent = None
class RRT:
def __init__(self, start, goal, obstacle_list, rand_area, expand_dis=0.5, goal_sample_rate=5, max_iter=1000):
self.start = Node(start[0], start[1])
self.goal = Node(goal[0], goal[1])
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.expand_dis = expand_dis
self.goal_sample_rate = goal_sample_rate
self.max_iter = max_iter
self.obstacle_list = obstacle_list
self.node_list = [self.start]
def planning(self):
for i in range(self.max_iter):
rnd_node = self.get_random_node()
nearest_ind = self.get_nearest_node_index(self.node_list, rnd_node)
nearest_node = self.node_list[nearest_ind]
new_node = self.steer(nearest_node, rnd_node, self.expand_dis)
if self.check_collision(new_node, self.obstacle_list):
self.node_list.append(new_node)
if self.calc_dist_to_goal(self.node_list[-1].x, self.node_list[-1].y) <= self.expand_dis:
final_node = self.steer(self.node_list[-1], self.goal, self.expand_dis)
if self.check_collision(final_node, self.obstacle_list):
return self.generate_final_course(len(self.node_list) - 1)
if i % 5 == 0:
self.draw_graph(rnd_node)
return None
def steer(self, from_node, to_node, extend_length=float("inf")):
new_node = Node(from_node.x, from_node.y)
d, theta = self.calc_distance_and_angle(new_node, to_node)
dx = math.cos(theta) * min(extend_length, d)
dy = math.sin(theta) * min(extend_length, d)
new_node.x += dx
new_node.y += dy
new_node.parent = from_node
return new_node
def generate_final_course(self, goal_ind):
path = [[self.goal.x, self.goal.y]]
node = self.node_list[goal_ind]
while node.parent is not None:
path.append([node.x, node.y])
node = node.parent
path.append([node.x, node.y])
return path
def calc_dist_to_goal(self, x, y):
return math.sqrt((x - self.goal.x) ** 2 + (y - self.goal.y) ** 2)
def get_random_node(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:
rnd = [self.goal.x, self.goal.y]
return Node(rnd[0], rnd[1])
def get_nearest_node_index(self, node_list, rnd_node):
dlist = [(node.x - rnd_node.x) ** 2 + (node.y - rnd_node.y) ** 2 for node in node_list]
minind = dlist.index(min(dlist))
return minind
def check_collision(self, node, obstacleList):
for (ox, oy, size) in obstacleList:
dx = ox - node.x
dy = oy - node.y
d = math.sqrt(dx * dx + dy * dy)
if d <= size:
return False # collision
return True # safe
def draw_graph(self, rnd=None):
plt.clf()
plt.plot(self.start.x, self.start.y, "xr")
plt.plot(self.goal.x, self.goal.y, "xb")
plt.grid(True)
for node in self.node_list:
if node.parent:
plt.plot([node.x, node.parent.x], [node.y, node.parent.y], "-g")
for (ox, oy, size) in self.obstacle_list:
plt.plot(ox, oy, "ok", ms=20 * size)
if rnd is not None:
plt.plot(rnd[0], rnd[1], "^k")
plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand])
plt.pause(0.01)
def calc_distance_and_angle(self, from_node, to_node):
dx = to_node.x - from_node.x
dy = to_node.y - from_node.y
d = math.sqrt(dx ** 2 + dy ** 2)
theta = math.atan2(dy, dx)
return d, theta
```
下面是一个简单例子的演示:
```python
def main():
print("start RRT path planning")
# ====Search Path with RRT====
obstacleList = [
(5, 5, 1),
(3, 6, 2),
(3, 8, 2),
(3, 10, 2),
(7, 5, 2),
(9, 5, 2)
] # [x,y,size(radius)]
# Set Initial parameters
rrt = RRT(start=[0, 0], goal=[6, 10], rand_area=[-2, 15], obstacle_list=obstacleList)
path = rrt.planning()
# Draw final path
if path is not None:
print("Path found!")
rrt.draw_graph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.grid(True)
plt.pause(0.01)
plt.show()
else:
print("Path not found!")
if __name__ == '__main__':
main()
```
这个例子中,我们设置了起点、终点和障碍物。然后我们创建了RRT对象并调用planning()方法来计算路径。最后,我们绘制了路径并将其显示出来。