MyEnigma

とある自律移動システムエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

State Lattice Plannerの概要とPythonサンプルコード

目次

はじめに

先日、

ロボティクスの境界問題を解くパスプランニングの手法として、

Model Predictive Trajectory Generatorを紹介しました。

myenigma.hatenablog.com

 

今回はこの手法と

効率的な目標状態のサンプリング手法を組み合わせた

State Lattice Plannerという

有名なパスプランニングアルゴリズムの概要と

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

 

State Lattice Planner

State Lattice Plannerは、

下記の論文で提案されている通り、

以前紹介したModel Predictive Trajectory Plannerと、

myenigma.hatenablog.com

いくつかの効率的な最終状態のサンプリング手法を組みわせた

経路生成アルゴリズムです。

 

経路生成のフローとしては、

まずはじめに複数の最終状態をサンプリングし、

それぞれの状態に対して

Model Predictive Trajectory Plannerでパスを引きます。

そしてそれぞれのパスに対して、

衝突チェックを実施し、

myenigma.hatenablog.com

衝突しないパスの中で、

最もコストが低いパスを最終コースとして選択します。

コストとしては、単純にゴールに最も近いパスを選ぶ方法もあれば、

コースの曲率が最も小さいものを選ぶ方法もあります。

 

このアルゴリズムで一番重要なのは、

どれだけ適切な場所に最終状態をサンプリングするかです。

あまり変な場所にサンプリングすると、

最終的な経路が不安定なものになってしまいます。

またできるだけサンプリングの数も減らすことで、

計算コストを下げる必要もあります。

 

前述の論文では、

大きくわけて3つの効率的なサンプリング手法が提案されています。

それぞれの概要を下記で説明します。

詳細は前述の論文と後述のPythonコードを参照ください。

 

Uniform Polar sampling

f:id:meison_amsl:20170720145030p:plain

一つ名の手法はUniform Polar samplingです。

上記の図のように、ある距離と角度の扇形の弧上に

等間隔に複数点サンプリングし、

その点に対して等間隔の複数角度の終端状態を

サンプリングする方法です。

 

Biased Polar sampling

f:id:meison_amsl:20170720145234p:plain

2つ目のサンプリング手法はBiased Polar samplingです。

前述のUniform Polar samplingは、

弧上を等間隔でサンプリングしていましたが、

すでにゴールの方向などが分かる場合は、

等間隔ではなくゴールの方向に密にサンプリングしたほうが、

よりゴールに近づくパスが生成できます。

 

Biased Polar samplingでは、

コスト関数(ゴールの方向など)を使って、

それらの確率密度関数から

コスト関数が小さくなる場所を重点的に

サンプリングする方法です。

 

Lane sampling

f:id:meison_amsl:20170720145302p:plain

3つ目のサンプリング手法はLane samplingです。

これは自動車の自動運転のように、

すでに走行するコース(レーン)が与えられており、

終端状態の方位が一意に決まる場合に、

その方向に対して横方向にオフセットした

終端状態をサンプリングする手法です。

 

State Lattice Planningの利点と欠点

State Lattice Planningの利点と欠点を

簡単にまとめておきます。

利点

  • グリットベースの手法などと比べて、解像度の問題が発生しない。

  • オフライン計算を使って、パス探索の高速化が可能

欠点

  • 状態量が大きくなるにつれて、急激に計算量とメモリ量が増加

  • Stateの離散化度合いによっては、狭い道は走行できない

  • 離散化により、コースが不連続になり任意の場所に移動できない

 

State Lattice PlannerのPythonサンプルプログラム

下記にState Lattice PlannerのPythonサンプルプログラムを公開しています。

github.com

 

上記のサンプルコードでは、

境界問題を解く方法として

以前紹介したのModel Predictive Trajectory Generatorを利用し、

myenigma.hatenablog.com

前述のUniform polar samplingと、

Biased polar sampling、

そしてLane samplingによって生成された

ローカルゴールに対し、それぞれパスを生成します。

 

下記がそれぞれのサンプリングによる

パスの生成結果です。

 

Uniform polar samplingを使った下記の結果では、

左右の広い範囲を5等分し、

それぞれの点に3つの異なる終端方位のパスを生成しています。

PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics

下記は左側にパスを引くようにした場合です。

PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics

 

Biased polar samplingを使用した下記の結果では、

進行方向にゴールがあるとし、

進行方向に密なサンプリングがされています。

PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics

下記は、左側にゴールがある場合です。

PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics

 

Lane sampling を利用した下記の結果では、

左側のレーンチェンジするのを想定して

複数のパスを引いています。

PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics

下記の結果は、

交差点を左に曲がる場合を想定したものです。

PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics

 

それぞれロボットのアプリケーションに応じて、

サンプリング手法を切り替えると

スムーズなロボットの自律移動が実現できます。

 

参考資料

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

  

MyEnigma Supporters

もしこの記事が参考になり、

ブログをサポートしたいと思われた方は、

こちらからよろしくお願いします。

myenigma.hatenablog.com