Rrt

ランダムツリーパスプランニング(RRT)pythonの急速な拡張



Rapid Expansion Random Tree Path Planning Python



ランダムツリーパスプランニング(RRT)pythonの急速な拡張

ランダムツリーパスプランニング(RRT)の基本的な方法をすばやく拡張します。
1.ノードクラスを作成します。属性にはxy座標値と親ノードがあります
2.開始点をノードリストに追加します
3.マップ上にランダムにポイントを生成し、
ノードリスト内の各ノードをトラバースし、ランダムポイントまでの距離を計算します。
ランダムノードaからの距離が最も短いノードbを見つけます。
2つのノード間の方向角を計算します
ノードb(リスト内のノード)を開始点とし、角度に沿って単位長さを増やし、終了点に新しいノードを生成します
ノードをノードリストに追加します
新しいノードと終点の間の距離が指定された値よりも小さい場合は、終点をノードリストに追加して、パスを見つけます。
そうしないと
上記のループトラバーサルを繰り返します

他の人を引用しているPythonコードは次のとおりです。



import numpy as np import matplotlib.pyplot as plt import random import math import copy show_animation = True class Node(object): ''' RRT Node ''' def __init__(self, x, y): self.x = x self.y = y self.parent = None class RRT(object): ''' Class for RRT Planning ''' def __init__(self, start, goal, obstacle_list, rand_area): ''' Setting Parameter start:Start Position [x,y] goal:Goal Position [x,y] obstacleList:obstacle Positions [[x,y,size],...] randArea:random sampling Area [min,max] ''' self.start = Node(start[0], start[1]) self.end = Node(goal[0], goal[1]) self.min_rand = rand_area[0] self.max_rand = rand_area[1] self.expandDis = 1.0 self.goalSampleRate = 0.05 # The probability of choosing the end point is 0.05 self.maxIter = 500 self.obstacleList = obstacle_list self.nodeList = [self.start] def random_node(self): node_x = random.uniform(self.min_rand, self.max_rand) node_y = random.uniform(self.min_rand, self.max_rand) node = [node_x, node_y] return node def get_nearest_list_index(self,node_list, rnd): ''' :param node_list: :param rnd: :return: ''' d_list = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in node_list] min_index = d_list.index(min(d_list)) return min_index def collision_check(self,new_node, obstacle_list): a = 1 for (ox, oy, size) in obstacle_list: dx = ox - new_node.x dy = oy - new_node.y d = math.sqrt(dx * dx + dy * dy) if d <= size: a = 0 # collision return a # safe def planning(self): ''' Path planning animation: flag for animation on or off ''' while True: # Random Sampling if random.random() > self.goalSampleRate: rnd = self.random_node() else: rnd = [self.end.x, self.end.y] # Find nearest node min_index = self.get_nearest_list_index(self.nodeList, rnd) # print(min_index) # expand tree nearest_node = self.nodeList[min_index] # Return to radians theta = math.atan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x) new_node = copy.deepcopy(nearest_node) new_node.x += self.expandDis * math.cos(theta) new_node.y += self.expandDis * math.sin(theta) new_node.parent = min_index if not self.collision_check(new_node, self.obstacleList): continue self.nodeList.append(new_node) # check goal dx = new_node.x - self.end.x dy = new_node.y - self.end.y d = math.sqrt(dx * dx + dy * dy) if d <= self.expandDis: print('Goal!!') break if True: self.draw_graph(rnd) path = [[self.end.x, self.end.y]] last_index = len(self.nodeList) - 1 while self.nodeList[last_index].parent is not None: node = self.nodeList[last_index] path.append([node.x, node.y]) last_index = node.parent path.append([self.start.x, self.start.y]) return path def draw_graph(self, rnd=None): ''' Draw Graph ''' print('aaa') plt.clf() if rnd is not None: plt.plot(rnd[0], rnd[1], '^g') for node in self.nodeList: if node.parent is not None: plt.plot([node.x, self.nodeList[node.parent].x], [ node.y, self.nodeList[node.parent].y], '-g') for (ox, oy, size) in self.obstacleList: plt.plot(ox, oy, 'sk', ms=10*size) plt.plot(self.start.x, self.start.y, '^r') plt.plot(self.end.x, self.end.y, '^b') plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand]) plt.grid(True) plt.pause(0.01) def draw_static(self, path): plt.clf() for node in self.nodeList: if node.parent is not None: plt.plot([node.x, self.nodeList[node.parent].x], [ node.y, self.nodeList[node.parent].y], '-g') for (ox, oy, size) in self.obstacleList: plt.plot(ox, oy, 'sk', ms=10*size) plt.plot(self.start.x, self.start.y, '^r') plt.plot(self.end.x, self.end.y, '^b') plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand]) plt.plot([data[0] for data in path], [data[1] for data in path], '-r') plt.grid(True) plt.show() def main(): print('start RRT path planning') obstacle_list = [ (5, 1, 2), (3, 6, 3), (3, 8, 2), (3, 5, 2), (6, 5, 2)] # Set Initial parameters rrt = RRT(start=[0, 0], goal=[8, 9], rand_area=[-2, 10], obstacle_list=obstacle_list) path = rrt.planning() print(path) # Draw final path if show_animation: plt.close() rrt.draw_static(path) if __name__ == '__main__': main()

手順のステップバイステップの説明:

class Node(object): def __init__(self, x, y): self.x = x self.y = y self.parent = None

ノードクラスを作成します。属性にはxy座標値と親ノードがあります



class RRT(object): ''' Class for RRT Planning ''' def __init__(self, start, goal, obstacle_list, rand_area): ''' Setting Parameter start:Start Position [x,y] goal:Goal Position [x,y] obstacleList:obstacle Positions [[x,y,size],...] randArea:random sampling Area [min,max] ''' self.start = Node(start[0], start[1]) self.end = Node(goal[0], goal[1]) self.min_rand = rand_area[0] self.max_rand = rand_area[1] self.expandDis = 1.0 self.goalSampleRate = 0.05 # The probability of choosing the end point is 0.05 self.maxIter = 500 self.obstacleList = obstacle_list self.nodeList = [self.start]

RRTクラスの初期プロパティを定義します。

def random_node(self): node_x = random.uniform(self.min_rand, self.max_rand) node_y = random.uniform(self.min_rand, self.max_rand) node = [node_x, node_y] return node

0-1以内のランダムノード座標を生成します

def get_nearest_list_index(self,node_list, rnd): ''' :param node_list: :param rnd: :return: ''' d_list = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in node_list] min_index = d_list.index(min(d_list)) return min_index

ノードリストの各ポイントをトラバースし、ランダムポイントに最も近いノードを見つけて、リスト内のノードのポインターを返します。



def collision_check(self,new_node, obstacle_list): a = 1 for (ox, oy, size) in obstacle_list: dx = ox - new_node.x dy = oy - new_node.y d = math.sqrt(dx * dx + dy * dy) if d <= size: a = 0 # collision return a # safe

新しいノードがバリアエリアにあるかどうかを確認します

def planning(self): ''' Path planning animation: flag for animation on or off ''' while True: # Random Sampling if random.random() > self.goalSampleRate: rnd = self.random_node() else: rnd = [self.end.x, self.end.y]

エンドポイントを選択するには、goalSampleRate = 0.05の確率でランダムポイントを設定します

min_index = self.get_nearest_list_index(self.nodeList, rnd) # print(min_index) # expand tree nearest_node = self.nodeList[min_index] # Return to radians theta = math.atan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x) new_node = copy.deepcopy(nearest_node) new_node.x += self.expandDis * math.cos(theta) new_node.y += self.expandDis * math.sin(theta) new_node.parent = min_index if not self.collision_check(new_node, self.obstacleList): continue self.nodeList.append(new_node) # check goal dx = new_node.x - self.end.x dy = new_node.y - self.end.y d = math.sqrt(dx * dx + dy * dy) if d <= self.expandDis: print('Goal!!') break if True: self.draw_graph(rnd)

ランダムノードaからの距離が最も短いノードbを見つけます。
2つのノード間の方向角を計算します
ノードb(リスト内のノード)を開始点とし、角度に沿って単位長さを増やし、終了点に新しいノードを生成します
ノードをノードリストに追加します
新しいノードと終点の間の距離が指定された値よりも小さく、終点がノードリストに追加され、パスが検出されます

path = [[self.end.x, self.end.y]] last_index = len(self.nodeList) - 1 while self.nodeList[last_index].parent is not None: node = self.nodeList[last_index] path.append([node.x, node.y]) last_index = node.parent path.append([self.start.x, self.start.y]) return path

last_indexは、ノードリストのエンドポイントを除く最後のポイントのポインタです。
終点を除く最後の点の親ノードがNoneでない場合:
パスリストがこのノードに追加され、ポインタがその親ノードを指します。親ノードの親ノードがNoneでない場合、ループは続行されます。
最後に、返されるパスリストは最初から最後までのパスです。

def draw_graph(self, rnd=None): ''' Draw Graph ''' print('aaa') plt.clf() if rnd is not None: plt.plot(rnd[0], rnd[1], '^g') for node in self.nodeList: if node.parent is not None: plt.plot([node.x, self.nodeList[node.parent].x], [ node.y, self.nodeList[node.parent].y], '-g') for (ox, oy, size) in self.obstacleList: plt.plot(ox, oy, 'sk', ms=10*size) plt.plot(self.start.x, self.start.y, '^r') plt.plot(self.end.x, self.end.y, '^b') plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand]) plt.grid(True) plt.pause(0.01) def draw_static(self, path): plt.clf() for node in self.nodeList: if node.parent is not None: plt.plot([node.x, self.nodeList[node.parent].x], [ node.y, self.nodeList[node.parent].y], '-g') for (ox, oy, size) in self.obstacleList: plt.plot(ox, oy, 'sk', ms=10*size) plt.plot(self.start.x, self.start.y, '^r') plt.plot(self.end.x, self.end.y, '^b') plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand]) plt.plot([data[0] for data in path], [data[1] for data in path], '-r') plt.grid(True) plt.show()

説明せずに絵を描く

def main(): print('start RRT path planning') obstacle_list = [ (5, 1, 2), (3, 6, 3), (3, 8, 2), (3, 5, 2), (6, 5, 2)] # Set Initial parameters rrt = RRT(start=[0, 0], goal=[8, 9], rand_area=[-2, 10], obstacle_list=obstacle_list) path = rrt.planning() print(path) # Draw final path if show_animation: plt.close() rrt.draw_static(path) if __name__ == '__main__': main()

主な機能

プログラムの効果は次のとおりです。
画像