路径规划——RRT算法

路径规划——RRT算法

算法原理

RRT算法的全称是快速扩展随机树算法(Rapidly Exploring Random Tree)，它的思想是选取一个初始点作为根节点，通过随机采样，增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，边可以在随机树中通过回溯的方式，找到这条从初始点到目标点的路径。

此算法的重点随机采样+步⻓限制+碰撞检测

算法流程：
1.初始化：以起点start为根节点，创建一棵树（通常用二叉树表示），树的根节点表示起始位置。
2.随机采样：在搜索空间中随机生成一个点x_rand。这个点可能在自由空间中，也可能在障碍物中。
3.寻找最近的节点：在当前的树中找到距离x_rand最近的节点x_near。
4.扩展树：从x_near沿着指向x_rand的方向移动一小步，生成一个新的节点x_new。如果x_new在自由空间中（即不与障碍物碰撞），则将x_new加入到树中，并将x_near和n_new用一条边连接。
5.检查目标：检查x_new是否在目标区域附近，这里的“附近”可以设置一个搜索距离来量化。如果是，则生成一条路径从起点到x_new，并结束算法。
6.迭代：重复步骤2~步骤5，直到找到目标点goal，或达到预设的迭代次数。

由于RRT采用随机采样的方法，RRT生成的路径通常不一定是最优路径，但可以通过多次运行RRT或结合其他优化算法来获得更优路径。

算法实现

import numpy as np
import random
import math
from itertools import combinations
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import matplotlib.patches as patchesclass RRT:def __init__(self,start,goal,obstacles,board_size,max_try,max_dist,goal_sample_rate,env) -> None:self.start = self.Node(start,None,0)self.goal = self.Node(goal,None,0)self.obstacles = obstaclesself.board_size = board_sizeself.max_try = max_try # Number of iterationsself.max_dist = max_dist # Maximum sampling distanceself.goal_sample_rate = goal_sample_rateself.env = envself.inflation = 1self.searched = []class Node:def __init__(self,position,parent,cost):self.position = positionself.parent = parentself.cost = costdef run(self):cost,path = self.plan()self.visualize(cost,path)def plan(self):self.searched.append(self.start)closed_list = {self.start.position: self.start}# plan max_try timesfor i in range(self.max_try):node_rand = self.get_random_node()# visitedif node_rand.position in closed_list:continue# Get the nearest neighbor nodenode_near = self.get_nearest_neighbor(list(closed_list.values()),node_rand)# Get the new nodenode_new = self.get_new_node(node_rand,node_near)if node_new:closed_list[node_new.position] = node_newself.searched.append(node_new)dist = self.distance(node_new,self.goal)# Found goal successfullyif dist <= self.max_dist and not self.isCollision(node_new,self.goal):self.searched.append(self.goal)self.goal.parent = node_newself.goal.cost = node_new.cost + self.distance(self.goal,node_new)closed_list[self.goal.position] = self.goalcost, path= self.extractPath(closed_list)print("Exploring {} nodes.".format(i))return cost,pathreturn 0,Nonedef get_random_node(self) :"""Return a random node."""if random.random()>self.goal_sample_rate:node = self.Node((random.uniform(0,self.env.height),random.uniform(0,self.env.width)),None,0)else:node = self.Node(self.goal.position,None,0)return nodedef get_nearest_neighbor(self,node_list,node) -> Node:"""Return node that is nearest to 'node' in node_list"""dist = [self.distance(node, n) for n in node_list]node_near = node_list[int(np.argmin(dist))]return node_neardef get_new_node(self,node_rand,node_near):"""Return node found based on node_near and node_rand."""dx = node_rand.position[0] - node_near.position[0]dy = node_rand.position[1] - node_near.position[1]dist = math.hypot(dx,dy)theta = math.atan2(dy, dx)d = min(self.max_dist,dist)position = ((node_near.position[0]+d*math.cos(theta)),node_near.position[1]+d*math.sin(theta))node_new = self.Node(position,node_near,node_near.cost+d)if self.isCollision(node_new, node_near):return Nonereturn node_newdef isCollision(self,node1,node2):"""Judge collision from node1 to node2 """if self.isInObstacles(node1) or self.isInObstacles(node2):return Truefor rect in self.env.obs_rectangle:if self.isInterRect(node1,node2,rect):return Truefor circle in self.env.obs_circle:if self.isInterCircle(node1,node2,circle):return Truereturn Falsedef distance(self,node1,node2):dx = node2.position[0] - node1.position[0]dy = node2.position[1] - node1.position[1]return math.hypot(dx,dy)def isInObstacles(self,node):"""Determine whether it is in obstacles or not."""x,y = node.position[0],node.position[1]for (ox,oy,w,h) in self.env.boundary:if ox-self.inflation<x<ox+w+self.inflation and oy-self.inflation<y<oy+h+self.inflation:return Truefor (ox,oy,w,h) in self.env.obs_rectangle:if ox-self.inflation<x<ox+w+self.inflation and oy-self.inflation<y<oy+h+self.inflation:return Truefor (ox,oy,r) in self.env.obs_circle:if math.hypot(x-ox,y-oy)<=r+self.inflation:return Truereturn Falsedef isInterRect(self,node1,node2,rect):""""Judge whether it will cross the rectangle when moving from node1 to node2"""ox,oy,w,h = rectvertex = [[ox-self.inflation,oy-self.inflation],[ox+w+self.inflation,oy-self.inflation],[ox+w+self.inflation,oy+h+self.inflation],[ox-self.inflation,oy+h+self.inflation]]x1,y1 = node1.positionx2,y2 = node2.positiondef cross(p1,p2,p3):x1 = p2[0]-p1[0]y1 = p2[1]-p1[1]x2 = p3[0]-p1[0]y2 = p3[1]-p1[0]return x1*y2 - x2*y1for v1,v2 in combinations(vertex,2):if max(x1,x2) >= min(v1[0],v2[0]) and min(x1,x2)<=max(v1[0],v2[0]) and \max(y1,y2) >= min(v1[1],v2[1]) and min(y1,y2) <= max(v1[1],v2[1]):if cross(v1,v2,node1.position) * cross(v1,v2,node2.position)<=0 and \cross(node1.position,node2.position,v1) * cross(node1.position,node2.position,v2):return Truereturn Falsedef isInterCircle(self,node1,node2,circle):"""Judge whether it will cross the circle when moving from node1 to node2"""ox,oy,r = circledx = node2.position[0] - node1.position[0]dy = node2.position[1] - node1.position[1]# Projectiont = ((ox - node1.position[0]) * dx + (oy - node1.position[1]) * dy) / (dx * dx + dy * dy)# The projection point is on line segment ABif 0 <= t <= 1:closest_x = node1.position[0] + t * dxclosest_y = node1.position[1] + t * dy# Distance from center of the circle to line segment ABdistance = math.hypot(ox-closest_x,oy-closest_y)return distance <= r+self.inflationreturn Falsedef extractPath(self,closed_list):""""Extract the path based on the closed list."""node = closed_list[self.goal.position]path = [node.position]cost = node.costwhile node != self.start:parent = node.parentnode_parent = closed_list[parent.position]node = node_parentpath.append(node.position)return cost,pathdef visualize(self, cost, path):"""Plot the map."""....

结果图：