目次
- 目次
- はじめに
- RRT* アルゴリズムの概要
- RRT*の利点と欠点
- シンプルな経路生成Python サンプルプログラム
- Dubinsパスを使った車両ロボットのためのRRT*プランニングPythonサンプルプログラム
- Reeds Sheppパスを使った車両ロボットのためのRRT*プランニングPythonサンプルプログラム
- 参考資料
- MyEnigma Supporters
はじめに
以前、Rapidly exploring Random Tree (RRT)を使った、
パスプランニングのサンプルコードを紹介しましたが、
このRRTの問題として、
サンプリングするノードの数を増やしても、
コースが最適になる可能性はかなり少ないということが上げられます。
これはRRTの論文でも証明されています。
そこでRRTのアルゴリズムをベースにして、
ノードが増える度に、
コースが最適値に近づくように改良したものが、
RRT* (スター)です。
今回はこのRRT*の概要と
Pythonサンプルプログラムを紹介したいと思います。
RRT* アルゴリズムの概要
RRT*は基本的にRRTのアルゴリズムをベースにしていますが、
(RRTのアルゴリズムに関しては、下記の記事参照
サンプリングノードが増える度に、
コースのコスト関数(大抵はコース長さ)が最小になるように
2つの手法を追加しています。
1. 隣接ノード間での連結判定
RRTでは、サンプリングされた点に対して、
最も近いノードを選び、サンプリング点に近くなるように、
ノードを延長させた点をそのままノードリストに入れていましたが、
RRT*では、延長させたノードのまわりの隣接ノード群を選び、
その中から最もコストが小さくなるノードを、
新しいノードの親ノードになるように選定します。
隣接ノードを選ぶための半径は下記の式で決定されます。
ここで、Nはノード数、dは状態空間の次元数、
Rはパラメータです。
この式より、Nの数が増えると
対数的に近接半径が小さくなる形になります。
2. ノード間の再接続
先程のノードの連結判定をした後に、
残りの隣接ノードの中で、
新しいノードを親ノードにした時にコストが下がる場合は、
そのノードの親ノードを新しいノードに組み替えて、
再接続します。
そして、再接続された場合は、
そのノードの子ノードのコストを
新しいノードのコストをベースに再計算します。
上記の2つの手法により、
隣接ノード間のコストが最適化され、
結果的にすべてのコースが漸近的に最適化されます。
上記の2つの手法に関しては、
下記の記事が図で説明されているため、
わかりやすいです。
RRT*の利点と欠点
利点
漸近的に探索が完了する
漸近的にパスの最適性が向上する
状態空間が高次元でも探索可能
実装が容易
制約条件を簡単に追加できる
欠点
2つの状態間を結ぶ境界値問題(Steer関数)を解く方法が必要(Rewireのため)
うねったコースを作ってしまうことが多い
オフライン計算を使ってパス生成を高速化するのが難しい
シンプルな経路生成Python サンプルプログラム
下記がRRT*のPythonサンプルコードです。
#!/usr/bin/python # -*- coding: utf-8 -*- u""" @brief: Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT) @author: AtsushiSakai(@Atsushi_twi) @license: MIT """ import random import math import copy import numpy as np class RRT(): u""" Class for RRT Planning """ def __init__(self, start, goal, obstacleList, randArea, expandDis=0.5, goalSampleRate=20, maxIter=5000): u""" Setting Parameter start:Start Position [x,y] goal:Goal Position [x,y] obstacleList:obstacle Positions [[x,y,size],...] randArea:Ramdom Samping Area [min,max] """ self.start = Node(start[0], start[1]) self.end = Node(goal[0], goal[1]) self.minrand = randArea[0] self.maxrand = randArea[1] self.expandDis = expandDis self.goalSampleRate = goalSampleRate self.maxIter = maxIter def Planning(self, animation=True): u""" Pathplanning animation: flag for animation on or off """ animation = False self.nodeList = [self.start] for i in range(self.maxIter): rnd = self.get_random_point() nind = self.GetNearestListIndex(self.nodeList, rnd) newNode = self.steer(rnd, nind) # print(newNode.cost) if self.__CollisionCheck(newNode, obstacleList): nearinds = self.find_near_nodes(newNode) newNode = self.choose_parent(newNode, nearinds) self.nodeList.append(newNode) self.rewire(newNode, nearinds) if animation: self.DrawGraph(rnd) # generate coruse lastIndex = self.get_best_last_index() path = self.gen_final_course(lastIndex) return path def choose_parent(self, newNode, nearinds): if len(nearinds) == 0: return newNode dlist = [] for i in nearinds: dx = newNode.x - self.nodeList[i].x dy = newNode.y - self.nodeList[i].y d = math.sqrt(dx ** 2 + dy ** 2) theta = math.atan2(dy, dx) if self.check_collision_extend(self.nodeList[i], theta, d): dlist.append(self.nodeList[i].cost + d) else: dlist.append(float("inf")) mincost = min(dlist) minind = nearinds[dlist.index(mincost)] if mincost == float("inf"): print("mincost is inf") return newNode newNode.cost = mincost newNode.parent = minind return newNode def steer(self, rnd, nind): # expand tree nearestNode = self.nodeList[nind] theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) newNode = copy.deepcopy(nearestNode) newNode.x += self.expandDis * math.cos(theta) newNode.y += self.expandDis * math.sin(theta) newNode.cost += self.expandDis newNode.parent = nind return newNode def get_random_point(self): if random.randint(0, 100) > self.goalSampleRate: rnd = [random.uniform(self.minrand, self.maxrand), random.uniform(self.minrand, self.maxrand)] else: # goal point sampling rnd = [self.end.x, self.end.y] return rnd def get_best_last_index(self): disglist = [self.calc_dist_to_goal( node.x, node.y) for node in self.nodeList] goalinds = [disglist.index(i) for i in disglist if i <= self.expandDis] # print(goalinds) mincost = min([self.nodeList[i].cost for i in goalinds]) for i in goalinds: if self.nodeList[i].cost == mincost: return i return None def gen_final_course(self, goalind): path = [[self.end.x, self.end.y]] while self.nodeList[goalind].parent is not None: node = self.nodeList[goalind] path.append([node.x, node.y]) goalind = node.parent path.append([self.start.x, self.start.y]) return path def calc_dist_to_goal(self, x, y): return np.linalg.norm([x - self.end.x, y - self.end.y]) def find_near_nodes(self, newNode): nnode = len(self.nodeList) r = 50.0 * math.sqrt((math.log(nnode) / nnode)) # r = self.expandDis * 5.0 dlist = [(node.x - newNode.x) ** 2 + (node.y - newNode.y) ** 2 for node in self.nodeList] nearinds = [dlist.index(i) for i in dlist if i <= r ** 2] return nearinds def rewire(self, newNode, nearinds): nnode = len(self.nodeList) for i in nearinds: nearNode = self.nodeList[i] dx = newNode.x - nearNode.x dy = newNode.y - nearNode.y d = math.sqrt(dx ** 2 + dy ** 2) scost = newNode.cost + d if nearNode.cost > scost: theta = math.atan2(dy, dx) if self.check_collision_extend(nearNode, theta, d): nearNode.parent = nnode - 1 nearNode.cost = scost def check_collision_extend(self, nearNode, theta, d): tmpNode = copy.deepcopy(nearNode) for i in range(int(d / self.expandDis)): tmpNode.x += self.expandDis * math.cos(theta) tmpNode.y += self.expandDis * math.sin(theta) if not self.__CollisionCheck(tmpNode, obstacleList): return False return True def DrawGraph(self, rnd=None): u""" Draw Graph """ import matplotlib.pyplot as plt plt.clf() if rnd is not None: plt.plot(rnd[0], rnd[1], "^k") 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 obstacleList: plt.plot(ox, oy, "ok", ms=30 * size) plt.plot(self.start.x, self.start.y, "xr") plt.plot(self.end.x, self.end.y, "xr") plt.axis([-2, 15, -2, 15]) plt.grid(True) plt.pause(0.01) def GetNearestListIndex(self, nodeList, rnd): dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in nodeList] minind = dlist.index(min(dlist)) return minind def __CollisionCheck(self, node, obstacleList): for (ox, oy, size) in obstacleList: dx = ox - node.x dy = oy - node.y d = dx * dx + dy * dy if d <= size ** 2: return False # collision return True # safe class Node(): u""" RRT Node """ def __init__(self, x, y): self.x = x self.y = y self.cost = 0.0 self.parent = None if __name__ == '__main__': print("Start rrt start planning") import matplotlib.pyplot as plt # ====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=[5, 10], randArea=[-2, 15], obstacleList=obstacleList) path = rrt.Planning(animation=True) # Draw final path rrt.DrawGraph() plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') plt.grid(True) plt.pause(0.01) # Need for Mac plt.show()
下記のリポジトリでも公開しています。
上記のコードを実行すると、
下記のような結果パスプランニング結果が得られます。
赤線が最終的なパスなので、
かなり最適なパスができていることがわかります。
下記が通常のRRTの結果ですが、
ノードを増やしても、
コースが最短になる感じはなさそうです。
Dubinsパスを使った車両ロボットのためのRRT*プランニングPythonサンプルプログラム
下記の記事で説明した通常のRRTによる車両ロボットのための経路生成ですが、
RRTそのものが最適コースを生成できないため、
ノードの数を増やしても、非常に非効率なコースを生成してしまいます。
そこで最適な車両ロボット用コースを生成するために、
先程のシステムのRRTの部分をRRT*に改良してみました。
コードは下記で公開しています。
下記はサンプリングのノード数を1000とした場合の、
生成されたコースになります。
先程の通常のRRTのパスの結果と比較すると分かる通り、
かなり効率の良いコースが生成できていることがわかります。
下記はノードを先程の2倍の2000に増やした場合の
シミュレーション結果です。
RRT*ではノードの数を増やすと、
より効率的なコースが生成できていることがわかります。
Reeds Sheppパスを使った車両ロボットのためのRRT*プランニングPythonサンプルプログラム
前述のDubinsパスは、後進するコースを考慮していませんでしたが、
Reeds SheppというDubinsパスの拡張アルゴリズムを利用すれば、
後進コースも含めた最短コースを生成できます。
Reeds Sheppパスの詳細は下記を参照下さい。
このReeds Sheeppパスと、RRT*を組み合わせて、
最適なコースを生成するアルゴリズムのサンプルコードを作りました。
下記の結果を見ると分かる通り、
後進も含めたコースが生成できていることがわかります。
参考資料
MyEnigma Supporters
もしこの記事が参考になり、
ブログをサポートしたいと思われた方は、
こちらからよろしくお願いします。