State Lattice Plannerのアルゴリズム解読 [PythonRobotics]
目次
はじめに
PythonRoboticsのstate_lattice_plannerのソースと関連論文を読んだかつState Lattice Plannerを説明してる日本語記事があまりなかったので,ソースを基にアルゴリズムの説明をします.
PythonRoboticsにおけるState Lattice Plannerのソースコード
PythonRobotics/PathPlanning/StateLatticePlanner at master · AtsushiSakai/PythonRobotics · GitHub論文
State Lattice Planner
State space sampling of feasible motions for high-performance mobile robot navigation in complex environments
Model Predictive Trajectory Generation(State Lattice Planner内で軌道生成のために用いられている手法)
Optimal Rough Terrain Trajectory Generation for Wheeled Mobile Robots
ちなみにState Lattice Plannerは千葉工業大学のfuroでも使われていて,その機体は公道で行われる自律ロボットの走行公開実験であるつくばチャレンジを完走しています.
(私が興味を持ったきっかけ)
Cartographer と Autoware を用いた自律走行
State Lattice Plannerとは
State Lattice Plannerの概要に関しては以下を参照ください.
State Lattice Plannerの概要とPythonサンプルコード - MyEnigma
State Lattice Plannerの説明を上の記事から引用します.
経路生成のフローとしては、まずはじめに複数の最終状態をサンプリングし、それぞれの状態に対してModel Predictive Trajectory Plannerでパスを引きます。そしてそれぞれのパスに対して、衝突チェックを実施し、衝突しないパスの中で、最もコストが低いパスを最終コースとして選択します。
今回は状態を位置姿勢としています.
最終状態のサンプリング手法に関しては大きく3つあるのですが,本記事では簡単なUniform Polar samplingでアルゴリズムの説明をします.
また,Model Predictive Trajectory Generationに関しては,以下の記事を参照ください.
Model Predictive Trajectory Generationの概要とPythonサンプルコード - MyEnigma
上で言及されているModel Predictive Trajectory Generationは車両の運動モデルを基にパラメータを最適化し,パラメータからパスを生成する手法です.今回,最適化しているパラメータはパスの長さ,中間ステアリング角,終端ステアリング角の3個です.
アルゴリズムの流れ
状態量:
最適化するパラメータ:
1. 終端状態のサンプリングステップ(Sampling)
Uniform Polar samplingの場合,画像1のように扇状に等間隔で個の点をサンプリングします.各点毎に異なる個の向きの終端状態があります. この終端状態をどうサンプルするかでUniform Polar sampling,Biased Polar sampling,Lane samplingを使い分けます.
- 画像1. 初期状態(左端)と終端状態
サンプルされる終端状態は以下の設定パラメータで決定されます. - 設定パラメータ
(終端状態を扇状に何個とるか)
(ある1つの終端状態の位置に関して車両の向きを何個求めるか)
(にある終端状態と初期状態間の長さ)
(終端状態をサンプルするための扇の角度の範囲)
(終端状態の車両の向きの範囲)
2. パス生成ステップ(Trajectory Generation)
各終端状態ごとに,Model Predictive Trajectory Generationでパラメータを最適化し,パスを生成していきます。
2.1 初期パラメータ決定
「あらかじめある程度の数の終端状態に対して、パラメータが既に計算されたLookup Table」内におけるパスのゴールが目標とする終端状態に最も近いパスのパラメータを初期値とします.こうすることで最適化の収束を早める効果があります.
- lookup table(あるに対してをModel Predictive Trajectory Generationであらかじめ計算したもの)の一部
x | y | yaw | s | km | kf |
---|---|---|---|---|---|
1.0 | 0.0 | 0.0 | 1.0 | 0.0 | 0.0 |
0.9734888894493215 | -0.009758406565994977 | 0.5358080146312756 | 0.9922329557399788 | -0.10222538550473198 | 3.0262632253145982 |
10.980728996433243 | -0.0003093605787364978 | 0.522622783944529 | 11.000391678142623 | 0.00010296091030877934 | 0.2731556687244648 |
16.020309241920156 | 0.0001292339008200291 | 0.5243399938698222 | 16.100019813021202 | 0.00013263212395994706 | 0.18999138959173634 |
20.963495745193626 | -0.00033031017429944326 | 0.5226120033275024 | 21.10082901143343 | 0.00011687467551566884 | 0.14550546012583987 |
6.032553475650599 | 2.008504211720188 | 0.5050517859971599 | 6.400329805864408 | 0.1520002249689879 | -0.13105940607691127 |
10.977487445230075 | 2.0078696810700034 | 0.5263634407901872 | 11.201040572298973 | 0.04895863722280565 | 0.08356555007223682 |
15.994057699325753 | 2.025659106131227 | 0.5303858891065698 | 16.200300421483128 | 0.0235708657178127 | 0.10002225103921249 |
20.977228843605943 | 2.0281289825388513 | 0.5300376140865567 | 21.20043308669372 | 0.013795675421657671 | 0.09331700188063087 |
25.95453914157977 | 1.9926432818499131 | 0.5226203078411618 | 26.200880299840527 | 0.00888830054451281 | 0.0830622000626594 |
0.9999999999999999 | 0.0 | 0.0 | 1.0 | 0.0 | 0.0 |
画像2:lookup tableを基にパスを生成したもの
2.2 パラメータ最適化
2.2.1 パス生成
パラメータ,初期ステアリング角,時間刻みを基にステアリング角の時系列をquandratic interpolationで生成し,その後,制御モデルで位置姿勢の時系列であるパスを生成します.
- 制御モデル
ここで,
:ホイールベース
:速度(一定と仮定)
:時間刻み
:距離刻み
2.2.2 閾値チェック
生成したパスに関するコストを計算し,閾値以下なら対象の終端状態でのパス生成を終了し,残りの終端状態のパス生成に移ります.PythonRoboticsでは,コストは生成したパスのゴールの位置姿勢 と目標となる終端状態の位置姿勢 間のノルムとなっています.
2.2.3 パラメータ調整
コストが閾値以上なら,ニュートン法を基にパラメータを変化させ,パスを再計算します.(2.2.2に戻る)
ここで,
:学習率
また,ヤコビアンは解析的に計算できないので、以下のように中心差分で数値的に求めます.
ここで,
:パラメータ摂動
3. パス選択ステップ
画像3のように生成されたパス群の中から各パスに対して衝突チェックをし,なんらかの指標を基に最適なパスを選択します.PythonRobotisでは,衝突チェック・パス選択まではしていません.
- 画像3.パス生成結果
参考にしたもの
lattice_planner · master · Autoware Foundation / Autoware.AI / core_planning · GitLab
https://blog.csdn.net/yuxuan20062007/article/details/82330165