MyEnigma

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

PythonによるRapidly-Exploring Random Trees (RRT)パスプランニングサンプルプログラム

animation.gif (640×480)

 

目次

はじめに

以前、A*やダイクストラ法、Dynamic Window Approachによる

パスプランニングシステムのサンプルプログラムを公開しましたが、

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

ロボティクスの分野で、

よく使用されるパスプランニングアルゴリズムとして、

Rapidly exploring random tree (RRT)

というアルゴリズムがあります。

今回は、このRRTのアルゴリズムの概要と、

PythonによるRRTパスプランニングの

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

 

Rapidly exploring random tree:RRTとは?

RRTは1999年に提案された、

パスプランニング&モーションプランニングアルゴリズムです。

 

RRTは、下記の図のように

空間(二次元の場合はx-y空間)に

ランダムにサンプリングを実施し、

そのサンプリング点に一番近いノードを

ある一定距離づつ伸ばしていくことで、

パスを探索するアルゴリズムです。

f:id:meison_amsl:20160323081526p:plain

 

パスを伸ばす際に、

運動モデルを考慮してパスを生成することで、

RRTをモーションプランニングのアルゴリズムとして

利用することもできます。

 

また、パスの生成時に障害物との干渉をチェックすることで、

障害物に衝突しないようなパスを引くこともできます。

 

RRTの特徴としては、

  • 高次元の状態空間の探索においても高速に探索可能

  • アルゴリズムがシンプル

ということが上げられます。

一方、

A*などのように、パスの最適性(最短性)などは

RRT単体では担保されません。

 

RRTの利点と欠点

利点

  • 漸近的に探索が完了する

  • 状態空間が高次元でも探索可能

  • 通常のRRTは境界問題を解く必要が無い

  • 実装が容易

  • 制約条件を簡単に追加できる

欠点

  • 通常のRRTはパスの最適性が保障されない

  • うねったコースを作ってしまうことが多い

  • オフライン計算を使ってパス生成を高速化するのが難しい

 

RRTによるパスプランニングPythonサンプルプログラム

下記はRRTによるパスプランニングPythonプログラムです。

すべてのコードは下記のGitHubリポジトリにおいても

管理されています。

github.com

シンプルなRRTパスプランニング (Goal biased sampling)

f:id:meison_amsl:20160320175657p:plain

下記のPythonコードは、

最もシンプルなRRTによるパスプランニング

サンプルコードです。

スタート地点とゴール地点に二次元座標、

障害物の位置と大きさ、

サンプリング範囲を指定することで、

運動モデルを考慮しないパスを、

上記のグラフのように探索することができます。

matplotlibにより、

パスの探索の様子をアニメーションで確認できるようになっています。

 

パスのスムージング後処理付きRRTプランニング

f:id:meison_amsl:20160327170359p:plain

 

先ほどのシンプルなRRTのプランニングによるパスは、

ランダムに空間を探索するので、

スタートとゴールを繋いではくれますが、

正直、最適パスとは言いがたいです。

 

そこで、よく使用される方法として、

RRTで探索されたパスを元に、

パスをスムージングすることで、

より最適なパスを生成することができます。

 

具体的には、

作成されたパスの任意の二点を結び、

その間の線が障害物と接触していなかった場合、

その線を新しいパスの一部とするような

パスを新しく生成します。

上記を繰り返すことにより、

徐々にコースを短くすることができます。

 

以上のようなパスの後処理を入れることにより、

上記の図のように、赤色の元のパスを

青色のパスのように最適化することができます。

 

サンプルコードは下記の通りです。

 

#!/usr/bin/python
# -*- coding: utf-8 -*-
u"""
@brief: Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT) 

@author: AtsushiSakai

@license: MIT

"""

import random
import math
import copy

class RRT():
    u"""
    Class for RRT Planning
    """

    def __init__(self, start, goal, obstacleList,randArea,expandDis=1.0,goalSampleRate=5,maxIter=500):
        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
        """

        self.nodeList = [self.start]
        while True:
            # Random Sampling
            if random.randint(0, 100) > self.goalSampleRate:
                rnd = [random.uniform(self.minrand, self.maxrand), random.uniform(self.minrand, self.maxrand)]
            else:
                rnd = [self.end.x, self.end.y]

            # Find nearest node
            nind = self.GetNearestListIndex(self.nodeList, rnd)
            # print(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.parent = nind

            if not self.__CollisionCheck(newNode, obstacleList):
                continue

            self.nodeList.append(newNode)

            # check goal
            dx = newNode.x - self.end.x
            dy = newNode.y - self.end.y
            d = math.sqrt(dx * dx + dy * dy)
            if d <= self.expandDis:
                print("Goal!!")
                break

            if animation:
                self.DrawGraph(rnd)

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

        return path

    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")
        plt.plot([ox for (ox,oy,size) in obstacleList],[oy for (ox,oy,size) in obstacleList], "ok", ms=size * 20)
        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 = math.sqrt(dx * dx + dy * dy)
            if d <= size:
                return False  # collision

        return True  # safe

class Node():
    u"""
    RRT Node
    """

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

if __name__ == '__main__':
    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]
    #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プランニング

animation.gif (640×480)

 

上記のパスプランニングは、

全方向移動できるモデルでしたので、

車両型のロボットなどに適したコースを生成することはできません。

そこで、上記のRRTと下記の記事で紹介しているDubins Pathを

組み合わせて、車両用RRTパスプランニングのサンプルコードを作成しました。

コードは下記のリポジトリを参照下さい。

github.com

 

基本的にはRRTのサンプリング点に対して、

Dubins Pathを生成して、障害物に衝突しない場合は、

そのパスをノードに追加しています。

 

上記のプランニング結果を見ると分かる通り、

標準のRRTなので、最適性は担保されませんが、

とりあえず衝突せずに目標値までのパスは引けました。

 

RRTのパス収束の高速化の手法

前述の通り、

RRTは非常にシンプルなアルゴリズムですが、

そのままだとコースを生成するのに時間がかかってしまいます。

そこで下記のような手法を使うことが多いようです。

 

Goal Bias Sampling

よりパスの探索を高速化するために、

ランダムなサンプリングだけでなく、

ある一定確率で、ゴールの位置をサンプリング点とすることで、

ゴール方向にパスの探索が進むようにし、

コースの生成を高速化することができます。

 

Goal Zone Sampling

Goal Zone Samplingは、

先程のGoal Bias Samplingにかなり似ていますが、

ある一定の確率で、ゴールと最も近いノードの間にサンプリングをします。

これにより、徐々にゴールに近い位置がサンプリングされるようになり、

パスの収束が高速化されます。

 

RRTの問題点

通常のRRTはランダムにサンプリングするため、

どんなにノードの数を増やしても、

最適なパス(最短経路など)を引くことはできません。

RRTで最適なパスを引きたい場合は、

RRT Starというノードの最適化を含んだアルゴリズムを使う必要があります。

RRT Starに関しては、下記の記事を参照下さい。

myenigma.hatenablog.com

 

参考資料

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

Mobile Robot Navigation and Localization: Roadmap-based Path Planning and Visibility Sector-based Localization

Mobile Robot Navigation and Localization: Roadmap-based Path Planning and Visibility Sector-based Localization