RRT的python
时间: 2023-11-09 19:02:39 浏览: 54
RRT是一种基于树形结构的路径规划算法,常用于机器人运动规划等领域。以下是一个简单的Python实现:
```python
import random
import math
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, expand_dis=0.5, goal_sample_rate=20, max_iter=500):
self.start = Node(start[0], start[1])
self.goal = Node(goal[0], goal[1])
self.min_x, self.min_y, self.max_x, self.max_y = self.get_boundary(obstacle_list)
self.obstacle_list = obstacle_list
self.expand_dis = expand_dis
self.goal_sample_rate = goal_sample_rate
self.max_iter = max_iter
self.node_list = [self.start]
def get_boundary(self, obstacle_list):
min_x = min([obs[0] for obs in obstacle_list])
min_y = min([obs[1] for obs in obstacle_list])
max_x = max([obs[2] for obs in obstacle_list])
max_y = max([obs[3] for obs in obstacle_list])
return min_x, min_y, max_x, max_y
def planning(self):
for i in range(self.max_iter):
if random.randint(0, 100) > self.goal_sample_rate:
rnd = Node(random.uniform(self.min_x, self.max_x), random.uniform(self.min_y, self.max_y))
else:
rnd = self.goal
nearest_node = self.get_nearest_node(rnd)
new_node = self.steer(nearest_node, rnd)
if self.check_collision(new_node):
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)
if self.check_collision(final_node):
return self.generate_path(len(self.node_list) - 1)
return None
def steer(self, from_node, to_node):
new_node = Node(from_node.x, from_node.y)
dist, theta = self.calc_distance_and_angle(new_node, to_node)
if dist > self.expand_dis:
dist = self.expand_dis
new_node.x += dist * math.cos(theta)
new_node.y += dist * math.sin(theta)
new_node.parent = from_node
return new_node
def get_nearest_node(self, node):
dlist = [(n.x - node.x) ** 2 + (n.y - node.y) ** 2 for n in self.node_list]
min_index = dlist.index(min(dlist))
return self.node_list[min_index]
def check_collision(self, node):
if node.x < self.min_x or node.x > self.max_x or node.y < self.min_y or node.y > self.max_y:
return False
for obs in self.obstacle_list:
if obs[0] <= node.x <= obs[2] and obs[1] <= node.y <= obs[3]:
return False
return True
def calc_distance_and_angle(self, from_node, to_node):
dx = to_node.x - from_node.x
dy = to_node.y - from_node.y
dist = math.hypot(dx, dy)
theta = math.atan2(dy, dx)
return dist, theta
def calc_dist_to_goal(self, x, y):
dx = x - self.goal.x
dy = y - self.goal.y
return math.hypot(dx, dy)
def generate_path(self, goal_index):
path = [(self.goal.x, self.goal.y)]
node = self.node_list[goal_index]
while node.parent is not None:
path.append((node.x, node.y))
node = node.parent
path.append((node.x, node.y))
return path[::-1]
# example usage
start = (0, 0)
goal = (5, 5)
obstacle_list = [(1, 1, 2, 2), (3, 3, 4, 4)]
rrt = RRT(start, goal, obstacle_list)
path = rrt.planning()
print(path)
```