MyEnigma

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

Model Predictive Trajectory Generationの概要とPythonサンプルコード

Numerical Optimization (Springer Series in Operations Research and Financial Engineering)

Numerical Optimization (Springer Series in Operations Research and Financial Engineering)

目次

はじめに

ロボットのパスプランニングにおいて、

有名な問題の一つに境界問題(Boundary Problem)という問題があります。

 

これは、2つの状態量

(二次元平面では、ロボットの位置x,y,そして方位yaw)

が与えられた時に、障害物は考慮せず、

その2つの状態量を結ぶパスを生成するという問題です。

 

この2つの問題を解くための手法として、

以前紹介したDubins-pathや、

myenigma.hatenablog.com

Reeds-Shepp pathがありますが、

myenigma.hatenablog.com

両方とも、非常にシンプルなモデルを使用していたり、

パスが最大曲率の円と直線で構成されているなど、

いくつか技術的な課題がありました。

 

そこで今回は

非線形運動モデルを利用した状態で、

パスの形状を規定すること無く、

境界問題を解くことができる、

Model Predictive Trajectory Generation

という技術を紹介しようと思います。

 

Model Predictive Trajectory Generationの概要

Model Predictive Trajectory Generation(MPTG)は、

ロボットのパスプランニング問題の一つである

境界問題を解くアルゴリズムです。

 

複雑な車両モデルや、

複雑な形状をしている環境においても、

パスを生成することができるので、

主に宇宙ロボットのパスプランニングのアルゴリズムとして使用されています。

Optimal Rough Terrain Trajectory Generation for Wheeled Mobile Robots - Google 検索

 

また、DARPA Urban challengeの

優勝チームであるCMUの自動運転車両Bossも

このMPTGをパスプランニングアルゴリズムとして使っていました。

 

Model Predictive Trajectory Genrationは、

考え方としてはモデル予測制御(MPC)に近く、

myenigma.hatenablog.com

車両の運動モデルや、周辺環境のモデル、サスペンションモデルを元に、

車両の時系列入力プロファイルを最適化によってもとめます。

 

MPCと異なる所としては、

まず一つ目として、

入力のプロファイルをそれぞれの時刻の値として計算するのではなく、

入力プロファイルをスプライン曲線などで近似し、

モデルパラメータを最適化によって計算します。

myenigma.hatenablog.com

これにより、最適化のパラメータの次元を減らすことができ、

最適化計算の高速化と安定性向上を実現できます。

 

二つ目の特徴としては、

MPCとは異なり、車両のモデルの線形化などはせずに、

非線形のモデルをそのまま最適化します。

つまり非線形最適化により、

前述の入力モデルパラメータを最適化するのです。

 

最適化のコスト関数としては、

目標状態と、

入力プロファイルと運動モデルから計算された

パスの最終位置の誤差が

最小になるように入力モデルパラメータを最適化します。

 

非線形最適化のアルゴリズムとしては、

下記の式のようにニュートン法を利用し、

f:id:meison_amsl:20170716092728p:plain

myenigma.hatenablog.com

ニュートン法のヘッセ行列は、

誤差ベクトルのサンプリングにより、

逐次線形化し推定する手法が使われます。

f:id:meison_amsl:20170716093048p:plain

 

MPTGでは、前述の通り、

勾配法であるニュートン法を使って最適化を実施するため、

最適化の結果が、局所解に陥ることがあります。

この問題に対応するためにMPTGでは、

目標ゴールの位置から、

精度の良い初期入力モデルパラメータを推定するために、

事前に準備したLookup Tableや、

ニューラルネットの回帰モデルを使用し、

目標ゴールに近いパスを生成できる

初期入力パラメータを推定するデータベースを

事前に準備しておく手法が一般的なようです。

 

詳細は下記の論文を参照ください。

 

Model Predictive Trajectory Generation による2状態空間のパス生成Pythonプログラム

下記のコードは、

MPTGを利用した2状態空間の間のパス生成Pythonプログラムです。

github.com

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

ロボットのモデルとして、

以前紹介したステアリング型ロボットの

Kinematic Modelを使用しています。

myenigma.hatenablog.com

 

またMPTGの制御モデルとしては、

速度は一定速度とし、

ステアリング入力は2次スプラインを使用しています。

したがって、最適化パラメータは

パスの長さs, 中間ステアリング角km, 終端ステアリング角kf

の3つになります。

 

初期ステアリング角度は現在のセンサ値から観測できるものとし、

非線形最適化の境界条件として利用します。

 

下記は、上記のコードを使った

パスプランニングのシミュレーション動画です。

矢印が目標状態で、赤線が各最適化ループにおけるパスです。

 

目標ゴールが異なっていても

最適化によってパスが生成できていることがわかります。

PythonRobotics/animation2.gif at master · AtsushiSakai/PythonRobotics

PythonRobotics/animation2.gif at master · AtsushiSakai/PythonRobotics

 

下記の動画は、

二つ目の動画と同じ目標位置ですが、

初期ステアリング角度が右に切れている場合のパス最適化の結果です。

下記の結果の通り、初期ステアリング角度が異なっていても

パスが生成できていることがわかります。

PythonRobotics/animation2.gif at master · AtsushiSakai/PythonRobotics

 

Model Predictive Trajectory Generation による初期制御パラメータ推定用Lookup Table生成Pythonプログラム

下記のプログラムは、

MPTG用の初期制御パラメータ推定用Lookup Tableを生成する

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

github.com

 

2次元空間上のx,y,yawを等間隔にサンプリングし、

それぞれのゴールに対して、上記のパス最適化を利用し、

パスを生成しています。

加えて、パス最適化の初期値として

作成中のLookup tableの中から、最も目標ゴールに近いものを選択し、

そのゴールに対応した、制御パラメータを最適化の際の初期値としています。

 

下記のグラフは上記のプログラムで生成された

Lookup Tableのデータを元に

各Tableの制御パラメータを元にパスを生成したものです。

PythonRobotics/lookuptable.png at master · AtsushiSakai/PythonRobotics

実際にリアルタイムでロボットのパスを生成する場合は、

毎回パスを生成する際に

上記のLookup Tableから最も近いゴールのパスの

制御パラメータを初期値とすることで、

パスの最適化を高速化&安定化することができます。

 

参考資料

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

 

MyEnigma Supporters

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

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

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

myenigma.hatenablog.com