机器人轨迹规划RRT-RRPstar-informed RRPstar-PRM算法python代码

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=10, maxIter=200): 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] # steer 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 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 = None lastPathLength = 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] # steer 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: 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) - 1 tempPath = self.get_final_course(lastIndex) tempPathLen = self.get_path_len(tempPath) if lastPathLength > tempPathLen: path = tempPath lastPathLength = tempPathLen print("current path length: {}, It costs {} s".format(tempPathLen, time.time()-start_time)) return path def 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 infinite cBest = float('inf') path = None # Computing the sampling space cMin = 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 found rnd = self.informed_sample(cBest, cMin, xCenter, C) n_ind = self.get_nearest_list_index(self.node_list, rnd) nearestNode = self.node_list[n_ind] # steer 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: 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) - 1 tempPath = self.get_final_course(lastIndex) tempPathLen = self.get_path_len(tempPath) if tempPathLen < cBest: path = tempPath cBest = tempPathLen print("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 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_li