読者です 読者をやめる 読者になる 読者になる

MyEnigma

とあるエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

PythonによるRRT*パスプランニングサンプルコード

Rapidly-Exploring Random Tree

Rapidly-Exploring Random Tree

  • 作者: Lambert M. Surhone,Mariam T. Tennoe,Susan F. Henssonow
  • 出版社/メーカー: Betascript Publishing
  • 発売日: 2011/02/23
  • メディア: ペーパーバック
  • この商品を含むブログを見る

目次

はじめに

以前、Rapidly exploring Random Tree (RRT)を使った、

パスプランニングのサンプルコードを紹介しましたが、

myenigma.hatenablog.com

このRRTの問題として、

サンプリングするノードの数を増やしても、

コースが最適になる可能性はかなり少ないということが上げられます。

これはRRTの論文でも証明されています。

 

そこでRRTのアルゴリズムをベースにして、

ノードが増える度に、

コースが最適値に近づくように改良したものが、

RRT* (スター)です。

 

今回はこのRRT*の概要と

Pythonサンプルプログラムを紹介したいと思います。

 

RRT* アルゴリズムの概要

RRT*は基本的にRRTのアルゴリズムをベースにしていますが、

(RRTのアルゴリズムに関しては、下記の記事参照

myenigma.hatenablog.com)

サンプリングノードが増える度に、

コースのコスト関数(大抵はコース長さ)が最小になるように

2つの手法を追加しています。

1. 隣接ノード間での連結判定

RRTでは、サンプリングされた点に対して、

最も近いノードを選び、サンプリング点に近くなるように、

ノードを延長させた点をそのままノードリストに入れていましたが、

RRT*では、延長させたノードのまわりの隣接ノード群を選び、

その中から最もコストが小さくなるノードを、

新しいノードの親ノードになるように選定します。

 

隣接ノードを選ぶための半径は下記の式で決定されます。

f:id:meison_amsl:20170507021043p:plain

ここで、Nはノード数、dは状態空間の次元数、

Rはパラメータです。

この式より、Nの数が増えると

対数的に近接半径が小さくなる形になります。

2. ノード間の再接続

先程のノードの連結判定をした後に、

残りの隣接ノードの中で、

新しいノードを親ノードにした時にコストが下がる場合は、

そのノードの親ノードを新しいノードに組み替えて、

再接続します。

 

上記の2つの手法により、

隣接ノード間のコストが最適化され、

結果的にすべてのコースが漸近的に最適化されます。

 

上記の2つの手法に関しては、

下記の記事が図で説明されているため、

わかりやすいです。

 

シンプルな経路生成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()

下記のリポジトリでも公開しています。

github.com

 

上記のコードを実行すると、

下記のような結果パスプランニング結果が得られます。

f:id:meison_amsl:20170507013628p:plain

赤線が最終的なパスなので、

かなり最適なパスができていることがわかります。

 

下記が通常のRRTの結果ですが、

ノードを増やしても、

コースが最短になる感じはなさそうです。

f:id:meison_amsl:20170507014310p:plain

 

車両ロボットにおけるRRT*プランニングPythonサンプルプログラム

下記の記事で説明した通常のRRTによる車両ロボットのための経路生成ですが、

myenigma.hatenablog.com

myenigma.hatenablog.com

 

RRTそのものが最適コースを生成できないため、

ノードの数を増やしても、非常に非効率なコースを生成してしまいます。

f:id:meison_amsl:20170523075224p:plain

 

そこで最適な車両ロボット用コースを生成するために、

先程のシステムのRRTの部分をRRT*に改良してみました。

コードは下記で公開しています。

github.com

下記はサンプリングのノード数を1000とした場合の、

生成されたコースになります。

f:id:meison_amsl:20170523080017p:plain

先程の通常のRRTのパスの結果と比較すると分かる通り、

かなり効率の良いコースが生成できていることがわかります。

 

下記はノードを先程の2倍の2000に増やした場合の

シミュレーション結果です。

f:id:meison_amsl:20170523080902p:plain

 

RRT*ではノードの数を増やすと、

より効率的なコースが生成できていることがわかります。

 

参考資料

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

Rapidly-Exploring Random Tree

Rapidly-Exploring Random Tree

  • 作者: Lambert M. Surhone,Mariam T. Tennoe,Susan F. Henssonow
  • 出版社/メーカー: Betascript Publishing
  • 発売日: 2011/02/23
  • メディア: ペーパーバック
  • この商品を含むブログを見る