MyEnigma

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

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

Planning Algorithms

Planning Algorithms

目次

はじめに

先日、Dubinsパスを利用した

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

myenigma.hatenablog.com

Dubinsパスは後進するコースを生成できないため、

目的の位置と姿勢に到達するパスを生成するのに、

大回りのパスを生成したり、

障害物により、コースが生成できないことがありました。

 

そこで、Dubinsパスを拡張し、

後進コースも生成できるようにした

Reeds Sheppパスというアルゴリズムの概要と、

Pythonサンプルコードを紹介したいと思います。

 

Reeds Shepp Pathとは?

Reeds Shepp Pathは、

基本的に以前紹介したDubins Pathと考え方は似ています。

myenigma.hatenablog.com

 

車両のモデルはKinematic Modelを利用しながらも、

myenigma.hatenablog.com

Dubins Pathとは違い、後進も可能なパスを生成出来る所が違います。

つまりReeds Shepp Pathは、後進も含めた、

最短コースを生成できるのです。

 

Dubinsパスの時に利用した、

パスのクラス分けをすると、

Reeds Shepp Pathは48個のパスの種類があります。

Dubins Pathで利用したワード表現をすると、

下記のように9種類に分けられます。

f:id:meison_amsl:20170523144222p:plain

上記の図において、縦棒はギア変更を意味します。

またβやπ/2はある指定された旋回角度の分だけの旋回を意味します。

 

上記のコースワードにおいて、各旋回角度や距離は、

下記の表のような範囲を持ちます。

f:id:meison_amsl:20170523145246p:plain

 

上記の範囲内ですべてのコースを生成し、

その中で、最も走行距離が短いコースを最終コースとして採用します。

詳しくは、下記の論文を参照下さい。

Reeds , Shepp : Optimal paths for a car that goes both forwards and backwards.

 

Reeds-SheppパスプランニングPythonサンプルプログラム

今回は、下記のpythonライブラリを利用するラッパーモジュールを作成しました。

github.com

 

上記のコードは、

下記のC++製のThe Open Motion Planning Libraryを利用しているようです。

The Open Motion Planning Library

 

下記がPythonサンプルプログラムです。

#! /usr/bin/python
# -*- coding: utf-8 -*-
"""
Reeds Shepp path planner sample code
author Atsushi Sakai(@Atsushi_twi)
License MIT
"""
import reeds_shepp
import math


def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
    u"""
    Plot arrow
    """
    import matplotlib.pyplot as plt

    if not isinstance(x, float):
        for (ix, iy, iyaw) in zip(x, y, yaw):
            plot_arrow(ix, iy, iyaw)
    else:
        plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
                  fc=fc, ec=ec, head_width=width, head_length=width)
        plt.plot(x, y)


def reeds_shepp_path_planning(start_x, start_y, start_yaw,
                              end_x, end_y, end_yaw, curvature):
    q0 = [start_x, start_y, start_yaw]
    q1 = [end_x, end_y, end_yaw]
    step_size = 0.1
    qs = reeds_shepp.path_sample(q0, q1, curvature, step_size)
    xs = [q[0] for q in qs]
    ys = [q[1] for q in qs]
    yaw = [q[2] for q in qs]

    xs.append(end_x)
    ys.append(end_y)
    yaw.append(end_yaw)

    clen = reeds_shepp.path_length(q0, q1, curvature)
    pathtypeTuple = reeds_shepp.path_type(q0, q1, curvature)

    ptype = ""
    for t in pathtypeTuple:
        if t == 1:
            ptype += "L"
        elif t == 2:
            ptype += "S"
        elif t == 3:
            ptype += "R"

    return xs, ys, yaw, ptype, clen


if __name__ == '__main__':
    print("Reeds Shepp path planner sample start!!")
    import matplotlib.pyplot as plt

    start_x = 10.0  # [m]
    start_y = 1.0  # [m]
    start_yaw = math.radians(180.0)  # [rad]

    end_x = -0.0  # [m]
    end_y = -3.0  # [m]
    end_yaw = math.radians(-45.0)  # [rad]

    curvature = 1.0

    px, py, pyaw, mode, clen = reeds_shepp_path_planning(
        start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)

    plt.plot(px, py, label="final course " + str(mode))

    # plotting
    plot_arrow(start_x, start_y, start_yaw)
    plot_arrow(end_x, end_y, end_yaw)

    plt.legend()
    plt.grid(True)
    plt.axis("equal")
    plt.show()

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

github.com

 

下記のように、スタート地点とゴール地点を指定すると、

後進も含めた最短コースが生成できていることがわかります。

PythonRobotics/figure_1-7.png at master · AtsushiSakai/PythonRobotics

PythonRobotics/figure_1-7.png at master · AtsushiSakai/PythonRobotics

  

参考資料

15.3.2 Reeds-Shepp Curves

Reeds-Shepp Cars | Andy G's Blog

Reeds , Shepp : Optimal paths for a car that goes both forwards and backwards.

graph algorithms - Any existing Reeds-Shepp implementations? - Theoretical Computer Science Stack Exchange

https://homepages.laas.fr/jpl/promotion/chap3.pdf

ompl::base::ReedsSheppStateSpace Class Reference

ompl/base/spaces/src/ReedsSheppStateSpace.cpp Source File

Planning Algorithms

Planning Algorithms