目次
はじめに
先日、Dubinsパスを利用した
パスプランニングの概要とPythonサンプルコードを紹介しましたが、
Dubinsパスは後進するコースを生成できないため、
目的の位置と姿勢に到達するパスを生成するのに、
大回りのパスを生成したり、
障害物により、コースが生成できないことがありました。
そこで、Dubinsパスを拡張し、
後進コースも生成できるようにした
Reeds Sheppパスというアルゴリズムの概要と、
Pythonサンプルコードを紹介したいと思います。
Reeds Shepp Pathとは?
Reeds Shepp Pathは、
基本的に以前紹介したDubins Pathと考え方は似ています。
車両のモデルはKinematic Modelを利用しながらも、
Dubins Pathとは違い、後進も可能なパスを生成出来る所が違います。
つまりReeds Shepp Pathは、後進も含めた、
最短コースを生成できるのです。
Dubinsパスの時に利用した、
パスのクラス分けをすると、
Reeds Shepp Pathは48個のパスの種類があります。
Dubins Pathで利用したワード表現をすると、
下記のように9種類に分けられます。
上記の図において、縦棒はギア変更を意味します。
またβやπ/2はある指定された旋回角度の分だけの旋回を意味します。
上記のコースワードにおいて、各旋回角度や距離は、
下記の表のような範囲を持ちます。
上記の範囲内ですべてのコースを生成し、
その中で、最も走行距離が短いコースを最終コースとして採用します。
詳しくは、下記の論文を参照下さい。
Reeds , Shepp : Optimal paths for a car that goes both forwards and backwards.
Reeds-SheppパスプランニングPythonサンプルプログラム
今回は、下記のpythonライブラリを利用するラッパーモジュールを作成しました。
上記のコードは、
下記の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()
下記のリポジトリでも公開しています。
下記のように、スタート地点とゴール地点を指定すると、
後進も含めた最短コースが生成できていることがわかります。
参考資料
Reeds-Shepp Cars | Andy G's Blog
Reeds , Shepp : Optimal paths for a car that goes both forwards and backwards.
https://homepages.laas.fr/jpl/promotion/chap3.pdf
ompl::base::ReedsSheppStateSpace Class Reference
ompl/base/spaces/src/ReedsSheppStateSpace.cpp Source File
- 作者: Steven M. LaValle
- 出版社/メーカー: Cambridge University Press
- 発売日: 2006/05/29
- メディア: ハードカバー
- この商品を含むブログを見る
MyEnigma Supporters
もしこの記事が参考になり、
ブログをサポートしたいと思われた方は、
こちらからよろしくお願いします。