ssk tech blog

自律移動ロボットの話を書きます

State Lattice Plannerのアルゴリズム解読 [PythonRobotics]

目次

はじめに

PythonRoboticsのstate_lattice_plannerのソースと関連論文を読んだかつState Lattice Plannerを説明してる日本語記事があまりなかったので,ソースを基にアルゴリズムの説明をします.

ちなみにState Lattice Plannerは千葉工業大学のfuroでも使われていて,その機体は公道で行われる自律ロボットの走行公開実験であるつくばチャレンジを完走しています. (私が興味を持ったきっかけ)
Cartographer と Autoware を用いた自律走行

f:id:ssk0109:20190627170858p:plain

State Lattice Plannerとは

State Lattice Plannerの概要に関しては以下を参照ください.
State Lattice Plannerの概要とPythonサンプルコード - MyEnigma

 State Lattice Plannerの説明を上の記事から引用します.

経路生成のフローとしては、まずはじめに複数の最終状態をサンプリングし、それぞれの状態に対してModel Predictive Trajectory Plannerでパスを引きます。そしてそれぞれのパスに対して、衝突チェックを実施し、衝突しないパスの中で、最もコストが低いパスを最終コースとして選択します。

今回は状態を位置姿勢\textbf{x}=(x,y,yaw)^Tとしています.
最終状態のサンプリング手法に関しては大きく3つあるのですが,本記事では簡単なUniform Polar samplingでアルゴリズムの説明をします.
また,Model Predictive Trajectory Generationに関しては,以下の記事を参照ください.
Model Predictive Trajectory Generationの概要とPythonサンプルコード - MyEnigma
上で言及されているModel Predictive Trajectory Generationは車両の運動モデルを基にパラメータを最適化し,パラメータからパスを生成する手法です.今回,最適化しているパラメータ\textbf{p}はパスの長さs,中間ステアリング角k_m,終端ステアリング角k_fの3個です.

アルゴリズムの流れ

  • 状態量:\textbf{x}=(x,y,yaw)^T

  • 最適化するパラメータ:\textbf{p}=(s,k_m,k_f)^T

1. 終端状態のサンプリングステップ(Sampling)

Uniform Polar samplingの場合,画像1のように扇状に等間隔で5(=n_{xy})個の点をサンプリングします.各点毎に異なる3(=n_h)個の向きの終端状態があります. この終端状態をどうサンプルするかでUniform Polar sampling,Biased Polar sampling,Lane samplingを使い分けます.

  • 画像1. 初期状態(左端)と終端状態
    f:id:ssk0109:20190627025629p:plain
    サンプルされる終端状態は以下の設定パラメータで決定されます.
  • 設定パラメータ
    n_{xy} = 5 (終端状態を扇状に何個とるか)
    n_h = 3 (ある1つの終端状態の位置に関して車両の向きを何個求めるか)
    d = 20 (y=0にある終端状態と初期状態間の長さ)
    -45^\circ \leqq a \leqq 45^\circ (終端状態をサンプルするための扇の角度の範囲)
    -45^\circ \leqq p \leqq 45^\circ (終端状態の車両の向きの範囲)

2. パス生成ステップ(Trajectory Generation)

各終端状態ごとに,Model Predictive Trajectory Generationでパラメータ\textbf{p}=(s,k_m,k_f)^Tを最適化し,パスを生成していきます。

2.1 初期パラメータ決定

「あらかじめある程度の数の終端状態に対して、パラメータ\textbf{p}が既に計算されたLookup Table」内におけるパスのゴールが目標とする終端状態に最も近いパスのパラメータを初期値とします.こうすることで最適化の収束を早める効果があります.

  • lookup table(あるx,y,yawに対してs,k_m,k_fを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を基にパスを生成したもの f:id:ssk0109:20190627112821p:plain

2.2 パラメータ最適化

2.2.1 パス生成

パラメータ\textbf{p},初期ステアリング角k_0,時間刻みdtを基にステアリング角の時系列k_{p}=(k_{p1},k_{p2},...,k_{pi},…,k_{pf})をquandratic interpolationで生成し,その後,制御モデルで位置姿勢\textbf{x}=(x,y,yaw)^Tの時系列であるパス\textbf{x(p)}=(\textbf{x}_\textbf{1} \textbf{(p)},\textbf{x}_\textbf{2} \textbf{(p)},...,\textbf{x}_\textbf{i} \textbf{(p)},…,\textbf{x}_\textbf{f} \textbf{(p)})を生成します.

  • 制御モデル
    x_{i+1} = x_{i} + v \cdot cos(yaw_{i}) \cdot dt
    y_{i+1} = y_{i} + v \cdot sin(yaw_{i}) \cdot dt
    yaw_{i+1} = yaw_{i} + v/L \cdot tan(k_{pi}) \cdot dt
    ここで,
    L=1.0(m)ホイールベース
    v=10.0/3.6(m/s):速度(一定と仮定)
    dt= ds/v:時間刻み
    ds=0.1(m):距離刻み

2.2.2 閾値チェック

生成したパスに関するコストを計算し,閾値以下なら対象の終端状態でのパス生成を終了し,残りの終端状態のパス生成に移ります.PythonRoboticsでは,コストは生成したパスのゴールの位置姿勢x(p)_{f} と目標となる終端状態の位置姿勢x_f 間のノルムとなっています.

2.2.3 パラメータ調整

コストが閾値以上なら,ニュートン法を基にパラメータを変化させ,パスを再計算します.(2.2.2に戻る)

ここで,
\Delta x_f(p)=(x_f - x(p)_{f})
\alpha:学習率
また,ヤコビアン \frac{\partial \Delta x_f(p)}{\partial p}は解析的に計算できないので、以下のように中心差分で数値的に求めます.


\frac{\partial \Delta x_f(p)}{\partial p}= \begin{pmatrix}
\frac{x_f(s+h_s,k_m,k_f)-x_f(s- h_s,k_m,k_f)}{2h_s} &\frac{x_f(s,k_m+h_{km},k_f)-x_f(s,k_m-h_{km},k_f)}{2h_{km}} &\frac{x_f(s,k_m,k_f+h_{kf})-x_f(s,k_m,k_f-h_{kf})}{2h_{kf}} \\\ 
\frac{y_f(s+h_s,k_m,k_f)-y_f(s- h_s,k_m,k_f)}{2h_s} &\frac{y_f(s,k_m+h_{km},k_f)-y_f(s,k_m-h_{km},k_f)}{2h_{km}} &\frac{y_f(s,k_m,k_f+h_{kf})-y_f(s,k_m,k_f-h_{kf})}{2h_{kf}} \\\ 
\frac{yaw_f(s+h_s,k_m,k_f)-yaw_f(s- h_s,k_m,k_f)}{2h_s} &\frac{yaw_f(s,k_m+h_{km},k_f)-yaw_f(s,k_m-h_{km},k_f)}{2h_{km}} &\frac{yaw_f(s,k_m,k_f+h_{kf})-yaw_f(s,k_m,k_f-h_{kf})}{2h_{kf}} 
\end{pmatrix}

ここで,
x_f(s,k_m,k_f)=x_f(p)
h=(h_{s},h_{km},h_{kf})^{T}=(0.5,0.02,0.02)^{T}:パラメータ摂動

3. パス選択ステップ

画像3のように生成されたパス群の中から各パスに対して衝突チェックをし,なんらかの指標を基に最適なパスを選択します.PythonRobotisでは,衝突チェック・パス選択まではしていません.

  • 画像3.パス生成結果
    f:id:ssk0109:20190619030948p:plain

参考にしたもの

lattice_planner · master · Autoware Foundation / Autoware.AI / core_planning · GitLab

https://blog.csdn.net/yuxuan20062007/article/details/82330165