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

MyEnigma

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

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

Robot Python

f:id:meison_amsl:20160320175657p:plain

目次

はじめに

以前、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によるパスプランニング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()

 

#!/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):
        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 (x,y,size) in obstacleList:
            self.PlotCircle(x,y,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 PlotCircle(self,x,y,size):
        deg=range(0,360,5)
        deg.append(0)
        xl=[x+size*math.cos(math.radians(d)) for d in deg]
        yl=[y+size*math.sin(math.radians(d)) for d in deg]
        plt.plot(xl, yl, "-k")

    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


def GetPathLength(path):
    l = 0
    for i in range(len(path) - 1):
        dx = path[i + 1][0] - path[i][0]
        dy = path[i + 1][1] - path[i][1]
        d = math.sqrt(dx * dx + dy * dy)
        l += d

    return l


def GetTargetPoint(path, targetL):
    l = 0
    ti = 0
    lastPairLen = 0
    for i in range(len(path) - 1):
        dx = path[i + 1][0] - path[i][0]
        dy = path[i + 1][1] - path[i][1]
        d = math.sqrt(dx * dx + dy * dy)
        l += d
        if l >= targetL:
            ti = i-1
            lastPairLen = d
            break

    partRatio = (l - targetL) / lastPairLen
    #  print(partRatio)
    #  print((ti,len(path),path[ti],path[ti+1]))

    x = path[ti][0] + (path[ti + 1][0] - path[ti][0]) * partRatio
    y = path[ti][1] + (path[ti + 1][1] - path[ti][1]) * partRatio
    #  print((x,y))

    return [x, y, ti]


def LineCollisionCheck(first, second, obstacleList):
    # Line Equation

    x1=first[0]
    y1=first[1]
    x2=second[0]
    y2=second[1]

    try:
        a=y2-y1
        b=-(x2-x1)
        c=y2*(x2-x1)-x2*(y2-y1)
    except ZeroDivisionError:
        return False

    #  print(first)
    #  print(second)

    for (ox,oy,size) in obstacleList:
        d=abs(a*ox+b*oy+c)/(math.sqrt(a*a+b*b))
        #  print((ox,oy,size,d))
        if d<=(size):
            #  print("NG")
            return False

    #  print("OK")

    return True  # OK


def PathSmoothing(path, maxIter, obstacleList):
    #  print("PathSmoothing")

    l = GetPathLength(path)

    for i in range(maxIter):
        # Sample two points
        pickPoints = [random.uniform(0, l), random.uniform(0, l)]
        pickPoints.sort()
        #  print(pickPoints)
        first = GetTargetPoint(path, pickPoints[0])
        #  print(first)
        second = GetTargetPoint(path, pickPoints[1])
        #  print(second)

        if first[2]<=0 or second[2]<=0:
            continue

        if (second[2]+1) > len(path):
            continue

        if second[2]==first[2]:
            continue

        # collision check
        if not LineCollisionCheck(first, second, obstacleList):
            continue

        #Create New path
        newPath=[]
        newPath.extend(path[:first[2]+1])
        newPath.append([first[0],first[1]])
        newPath.append([second[0],second[1]])
        newPath.extend(path[second[2]+1:])
        path=newPath
        l = GetPathLength(path)

    return path


if __name__ == '__main__':
    import matplotlib.pyplot as plt
    #====Search Path with RRT====
    # Parameter
    obstacleList = [
        (5, 5, 1),
        (3, 6, 2),
        (3, 8, 2),
        (3, 10, 2),
        (7, 5, 2),
        (9, 5, 2)
    ]  # [x,y,size]
    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')

    #Path smoothing
    maxIter=1000
    smoothedPath = PathSmoothing(path, maxIter, obstacleList)
    plt.plot([x for (x,y) in smoothedPath], [y for (x,y) in smoothedPath],'-b')

    plt.grid(True)
    plt.pause(0.01)  # Need for Mac
    plt.show()

参考資料

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