Stein変分勾配降下法を用いた多峰性行動分布に対するモデル予測経路積分制御

Stein変分勾配降下法を用いた 多峰性行動分布に対するモデル予測経路積分制御 本田康平*，赤井直樹，鈴木康介，青木瑞穂，細萱広高，奥田裕之，鈴木達也 (名古屋大学) F1Tenth: 1/10 Scale Autonomous Racing @ICRA’23 1

Robotics needs gradient-free motion planner 非線形なロボットのダイナミクス Off-road exploration Racing car Manipulator 勾配ベースMPC (局所) 最適性の確保 (≒解が良質) 最適化問題の微分情報が必要 非線形性の扱いが難しい 微分不可能な (コスト) マップ Navigation 2 サンプルベースMPC vs 最適性 微分不可能・非線形でもOK 並列化計算が容易 最適性を担保可能な (≒解の良質な) サンプルベースMPCが求められる 2

「古典的な」 サンプルベースMPC Random shooting method 1. ランダムサンプリング 2. ベストスコアサンプルを採用 ▲ サンプル効率 Dynamic Window Approach (DWA) 1. 平滑化された候補解をサンプル 2. ベストスコアサンプルを採用 ▲最適性 サンプルベースMPCの最適性・サンプル効率を向上するためには？ 3

Model Predictive Path Integral control [1] (MPPI) 経路積分制御&情報論的なアプローチのサンプルベースMPC  高いサンプル効率 & 滑らかな解 (最適性)  制御入力ノイズを確率的に考慮可能 Random Shooting MPPI DWA [1] G. Williams+, Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving, 2018. 4

MPPI = 変分推論による最適行動分布近似 ➀ 制御入力系列を 多変量ガウス分布によって近似 予測ホライズン 𝑇𝑇 ③ 平均値系列を 最適入力系列として印加 ➁ 最適制御問題から 最適な行動分布を導出 𝑇𝑇 ∗ 𝑞𝑞 = 𝜂𝜂 𝑞𝑞 = � 𝒩𝒩(𝑈𝑈, Σ) −1 𝑐𝑐 𝑉𝑉 exp − 𝜆𝜆 𝑞𝑞 𝑈𝑈 ∗ KLダイバージェンス最小化 特長 : KLダイバージェンスを閉経式で最小化する (≒解析解が反復計算無しに求まる) 最適解: 𝑈𝑈 ∗ = argmin 𝔻𝔻KL (𝑞𝑞∗ ‖ 𝑞𝑞) 𝑈𝑈 = 𝔼𝔼𝑉𝑉~𝑞𝑞 コスト関数 𝑐𝑐 𝑉𝑉 softmax − 𝜆𝜆 温度パラメータ 𝑉𝑉 ←重み付き平均を1回計算 (モンテカルロ法) 5

MPPIの課題 : 多峰性な最適分布への対応 衝突軌道が最適解として求まる 確率密度 最適行動分布 最適解 推定ガウス分布 ランダムサンプル 共分散行列 制御入力ベクトル Why? KLダイバージェンス最小 = 確率分布の重なり度を最大化 → 複数のモードに覆いかぶさるようなガウス分布が推定される 6

MPPIの課題 : 多峰性な最適分布への対応 衝突軌道が最適解として求まる 最適行動分布 最適解 確率密度 推定ガウス分布 ランダムサンプル 共分散行列 制御入力ベクトル Why? KLダイバージェンス最小 = 確率分布の重なり度を最大化 → 複数のモードに覆いかぶさるようなガウス分布が推定される 研究目的: 推定ガウス分布が単一モードのみを覆うようにMPPIを改良する 7

関連研究 1. 非ガウス分布を提案分布に用いる (e.g. 混合ガウス分布 (GMM) [2-5]) 変分推論によって最適分布を近似 Mirror descent algorithm [2, 3] SVGD algorithm[4, 5] 多峰性を捉えることが可能 閉形式解は求まらない (収束性悪化) 単一解をどのように絞り込む？ 2. KLダイバージェンスの非対称性を利用してモード探索解を推定 [5, 6] Reverse KLダイバージェンスを最小化 単一モードを推定可能 (mode-seek) 閉形式解は求まらない 𝑞𝑞Forward = argmin 𝔻𝔻KL (𝒒𝒒∗ ‖ 𝒒𝒒) Forward (MPPI) 𝑞𝑞 𝑞𝑞 ∗ 𝑞𝑞Reverse = argmin 𝔻𝔻KL (𝒒𝒒‖ 𝒒𝒒∗ ) 𝑞𝑞 Reverse 課題: 閉形式性を維持しつつReverse KLダイバージェンスを最小化するには？ [2] M. Okada+, Variational inference MPC for Bayesian model-based reinforcement learning, 2020. [3] Z. Wang+, Variational inference MPC using tsallis divergence, 2021. [4] L. Barcelos+, Dual online Stein variational inference for control and dynamics, 2021. [5] A. Lambert+, Stein variational model predictive control, 2021. [6] T. Kobayashi+, Real-time Sampling-based Model Predictive Control based on Reverse Kullback-Leibler Divergence and Its Adaptive Acceleration, 2022. 8

Stein Variational Guided MPPI (SVG-MPPI): Reverse KL-Dを利用してMPPIの解を誘導 ターゲットモード推定 Reverse KL-Dの最小化によって ターゲットモードを探索 ターゲットモード Forward KL-Dの最小化によって 最適解を探索 Probability density 最適行動分布 最適解 推定ガウス分布 ノミナル系列 輸送 Gaussian fitting ガイド粒子 Control input Stein Variational Gradient Descent (SVGD) 適応的 共分散行列  Reverse KLダイバージェンスによって解をmode-seekに誘導  Forward KLダイバージェンスによって最適解は高速に収束 MPPI アルゴリズム Reverse KL-Dの最小化 には反復計算が必要 9

(Step 1) SVGDによるRKL-Dの最小化 少量のガイド粒子を散布し，SVGD法[7]により輸送する ※ サンプル全体を輸送しないため計算量が抑えられる SVGD method ガイド粒子 輸送式: Probability density 最適行動分布 Reverse KLD 最小化 = 単一モードへ集まる 輸送 Reverse KLダイバージェンス 𝑔𝑔 𝑔𝑔 𝑉𝑉�𝑘𝑘 ← 𝑉𝑉𝑘𝑘 − 𝜖𝜖 𝛁𝛁𝑽𝑽𝒈𝒈 𝔻𝔻𝐊𝐊𝐊𝐊 𝒒𝒒 ‖𝒒𝒒∗ 輸送軌跡 𝒌𝒌 イエンセンの不等式 & 方策勾配定理 ≈− ガイド粒子 Control input (iterative update) 𝔼𝔼𝑉𝑉~𝑞𝑞 Softmax −𝑐𝑐(𝑉𝑉)/𝜆𝜆 ∇𝑉𝑉 log 𝑞𝑞 𝔼𝔼𝑉𝑉~𝑞𝑞 Softmax −𝑐𝑐(𝑉𝑉)/𝜆𝜆 輸送軌跡はピークへと続く軌跡 = 輸送軌跡を用いて最適分布のモードの位置と形状の推定をする アイデア [7] Q. Liu+, Stein Variational Gradient Descent: A General Purpose Bayesian Inference Algorithm, 2016. 10

(Step 2) ターゲットモードの位置・形状推定 輸送軌跡を用いてターゲットモードの位置と形状を推定する ターゲットモード 頂点: ベストスコアのガイド粒子 輸送軌跡 Gaussian fitting 共分散行列: Gaussian fitting [7]で高速&ラフに推定 𝜎𝜎𝜏𝜏𝑖𝑖 = by SVGD method −1/2𝑧𝑧2 where 共分散行列 推定 [7] H. Guo, A simple algorithm for fitting a gaussian function, 2011. ∑𝐛𝐛2 ∑𝐚𝐚𝐛𝐛2 ∑𝐚𝐚2 𝐛𝐛2 ∑𝐚𝐚𝐛𝐛2 ∑𝐚𝐚2 𝐛𝐛2 ∑𝐚𝐚3 𝐛𝐛2 ∑𝐚𝐚2 𝐛𝐛2 ∑𝐚𝐚3 𝐛𝐛2 ∑𝐚𝐚4 𝐛𝐛2 ∑𝐛𝐛2 log𝐛𝐛 𝑧𝑧0 𝑧𝑧1 = ∑𝐚𝐚𝐛𝐛2 log𝐛𝐛 𝑧𝑧2 ∑𝐚𝐚2 𝐛𝐛2 log𝐛𝐛 𝐚𝐚: 輸送軌跡 𝐛𝐛: 𝑞𝑞 ∗ 値 11

(Step 3) MPPIアルゴリズムへの推定モードの組み込み MPPIアルゴリズム モード推定 ターゲットモード 最適解 推定ガウス分布 頂点 ノミナル系列 Gaussian fitting 分散 ∗ 最適解: 𝑈𝑈 = 𝔼𝔼𝑉𝑉~𝑞𝑞(𝑈𝑈, � Σ) 事前分布の 分散 = 𝔼𝔼𝑉𝑉~𝑞𝑞(𝑈𝑈, � Σ) 𝑐𝑐 𝑉𝑉 softmax − 𝜆𝜆 State cost 𝑉𝑉 正則化項 𝑇𝑇−1 1 1 � 𝑡𝑡−1 − 𝒖𝒖 � 𝑡𝑡 exp − 𝑆𝑆 𝑉𝑉 + 𝜆𝜆 � 𝒖𝒖 𝜂𝜂 𝜆𝜆 事前分布の分散 𝑡𝑡=0 T Σ −1 𝒗𝒗𝑡𝑡 𝑉𝑉 ノミナル系列 最適分布の単一のモードのみを正規分布が覆うようにMPPIの解を誘導 12

実験設定 : 経路追従 & 障害物回避 シミュレーション環境 ノミナル系列 最適制御問題 ※ 速度系列は オフライン最適化 Find: 1秒先 (約5 m先) までのタイヤ舵角系列 コストマップより Min.: (状態コスト) = (参照経路追従項) + (障害物回避項) 共分散行列 評価方法 シナリオ PT (経路追従)  未知障害物無し  評価メトリック : 平均状態コスト シナリオ OA (障害物回避)  各周ランダムに未知障害物を5個配置  評価メトリック: 平均状態コスト，衝突確率 S.t.: 車両ダイナミクス, 舵角入力上下限 比較手法  MPPI[1] Forward KL-Dを閉形式で最小化  SV-MPC[5] SVGD法によってReverse KL-Dを最小化  Reverse MPPI[6] 鏡像勾配降下によってReverse KL-Dを最小化  SVG-MPPI (提案手法) Reverse / Forward KL-Dのハイブリッド最小化 13

従来手法との比較：障害物回避&追従性能向上 14

シミュレーションにおける定量評価 各シナリオにおける平均状態コスト ↓ 経路追従 シナリオ 障害物回避 シナリオ 従来手法 提案手法 15

シミュレーションにおける定量評価 MPPI 各シナリオにおける平均状態コスト ↓ 共分散 : 大 経路追従 障害物回避 共分散 : 小 小 共分散 大 通常のMPPI (Forward KLD最小化): 事前分布の共分散値に応じて得意なタスクが変化 共分散 = 解の探索範囲 → 大きいほど大きな解の変化に対応可能 16

シミュレーションにおける定量評価 MPPI 経路追従 小 各シナリオにおける平均状態コスト ↓ 障害物回避 共分散 大 通常のMPPI (Forward KLD最小化): 事前分布の共分散値に応じて得意なタスクが変化 共分散 = 解の探索範囲 → 大きいほど多峰性分布に覆いかぶさる 17

シミュレーションにおける定量評価 各シナリオにおける平均状態コスト ↓ MPPI Reverse SV-MPC SVG-MPPI サンプル数 10,000 200 500 8,000 計算時間[ms] 12.1 26.0 29.1 11.0 Reverse KLDの直接最小化:  PT・OA性能にトレードオフ有り  反復計算のため1サンプルごとの計算量が増加 → モンテカルロ法の推定精度低下 18

シミュレーションにおける定量評価 各シナリオにおける平均状態コスト ↓ シナリオ OAにおける衝突確率 ↓ Reverse & Forward KLDの ハイブリッド最小化 提案手法 (SVG-MPPI) が経路追従&衝突回避シナリオで共に最高性能 19

実機実験 : 経路追従シナリオ 平均状態コスト ↓: ※10 laps MPPI 5.83 SVG-MPPI (Ours) 2.03 20

実機実験：障害物回避シナリオ 平均状態コスト ↓ 衝突確率 [%] ↓ MPPI SVG-MPPI (Ours) 55.6 15.4 62.7 27.8 ※ 3 laps x 6 different layouts 21

まとめ : Stein Variational Guided MPPI  課題 : MPPIは多峰性分布や急な分布の変化に弱い  手法 : KLダイバージェンスの非対称性を利用して解を単一モードに誘導  結果 : 経路追従・障害物回避性能が大きく向上 Take Home Message:  Forward & Reverse KLダイバージェンスの併用が変分推論MPCには有効  MPPIはSVGDのような (疑似) 勾配ベースの手法と組み合わせると効果的 22

解決の鍵：KLダイバージェンスの非対称性 最適行動分布 𝑞𝑞∗ ガウス分布 𝑞𝑞 確率密度 Forward KLダイバージェンス最小化 (MPPI) 𝐦𝐦𝐦𝐦𝐦𝐦 𝔻𝔻𝐊𝐊𝐊𝐊 𝒒𝒒∗ ‖ 𝒒𝒒 = min 𝔼𝔼𝑞𝑞∗ 𝑞𝑞 ∗ log 𝑞𝑞 Large penalty when 𝑞𝑞 ∗ > 0 and 𝑞𝑞 ≪ 0 全モードを覆う確率分布が推定 例 : 拡散モデル，VAE Reverse KLダイバージェンス最小化 𝐦𝐦𝐦𝐦𝐦𝐦 𝔻𝔻𝐊𝐊𝐊𝐊 𝒒𝒒‖ 𝒒𝒒∗ = min 𝔼𝔼𝑞𝑞 log 𝑞𝑞 𝑞𝑞∗ Large penalty when 𝑞𝑞 > 0 and 𝑞𝑞∗ ≪ 0 単一モードを覆う確率分布が推定 例 : GAN 23

MPPI実装 確率最適制御・情報論の観点から理論的にサンプルベース手法を改良できないか？ → Model Predictive Path Integral control (MPPI) [Williams+, 2018] から急速に発展中 実装は簡単！: 初期解 (前回の解𝑼𝑼𝒕𝒕−𝟏𝟏 ) 最適解𝑼𝑼𝒕𝒕 1. 初期解を平均としてランダムサンプリング ランダムサンプル 𝑽𝑽𝒌𝒌 = 𝑼𝑼𝒕𝒕−𝟏𝟏 + 𝝐𝝐𝒌𝒌 𝝐𝝐𝒌𝒌 ~ 𝓝𝓝(0, Σ) 2. コスト関数を用いて重みを計算 重み 初期解𝑼𝑼𝒕𝒕−𝟏𝟏 を平均とした 𝒩𝒩(0, Σ)のノイズを含む サンプル系列 𝑽𝑽 = 𝑽𝑽𝟏𝟏 , … , 𝑽𝑽𝑲𝑲 𝑤𝑤 𝑽𝑽𝒌𝒌 状態コスト 𝑇𝑇−1 𝑡𝑡=0 3. 重み付け和によって解を更新 𝐾𝐾 𝑼𝑼𝒕𝒕 = 𝑼𝑼𝒕𝒕−𝟏𝟏 + � 𝑤𝑤 𝑽𝑽𝑘𝑘 𝑽𝑽𝒌𝒌 更新解 入力コスト 1 1 � 𝑡𝑡−1 − 𝒖𝒖 � 𝑡𝑡 T Σ𝒗𝒗𝑡𝑡 ) = exp(− 𝑆𝑆 𝑽𝑽𝒌𝒌 + 𝜆𝜆 � 𝒖𝒖 𝜂𝜂 𝜆𝜆 初期解 ノミナル解 𝑘𝑘=1 • G. Williams+, Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving, IEEE T-RO, 2018 (通常は0) Softmax関数 24

(MPPI理論) 0. 準備 定義: 観測状態 𝑥𝑥0 行動分布 ℚ 実制御入力系列 Σ 𝑈𝑈𝑡𝑡 = {𝑢𝑢0 , … , 𝑢𝑢 𝑇𝑇−1 } 共分散行列 ガウスノイズ付与制御入力: 状態方程式: 行動分布ℚの確率密度関数: 𝑣𝑣𝜏𝜏 ~ 𝒩𝒩(𝑢𝑢𝜏𝜏 , Σ) 𝑥𝑥𝜏𝜏+1 = 𝐹𝐹 𝑥𝑥𝜏𝜏 , 𝑣𝑣𝜏𝜏 𝑇𝑇−1 1 𝑞𝑞 {𝑣𝑣𝜏𝜏 }0…𝑇𝑇−1 = 𝑍𝑍 −1 exp − � 𝑣𝑣𝜏𝜏 − 𝑢𝑢𝜏𝜏 T Σ 𝑣𝑣𝜏𝜏 − 𝑢𝑢𝜏𝜏 2 確率最適制御問題: 𝜏𝜏=0 状態コスト 入力ペナルティ 𝑇𝑇−1 𝜆𝜆 𝑈𝑈𝑡𝑡∗ = argmin Εℚ 𝜙𝜙 𝑥𝑥𝑇𝑇 + � 𝑐𝑐 𝑥𝑥𝜏𝜏 + 𝑢𝑢𝜏𝜏 Σ 𝑢𝑢𝜏𝜏 + 𝛽𝛽𝑢𝑢𝜏𝜏 2 𝑈𝑈𝑡𝑡 ∈𝒰𝒰 入力制約条件を満たす集合 𝜏𝜏=0 𝑆𝑆 𝑥𝑥0 , {𝑣𝑣0 , … , 𝑣𝑣𝑇𝑇−1 } → 期待値最小化は直接は解けない！ アプローチ：最適行動分布ℚ∗ を正規化行動分布ℚで近似 行動分布 ℚ (正規分布) min D𝐾𝐾𝐾𝐾 (ℚ∗ ||ℚ) 分布間距離 KLD 行動分布 ℚ∗ (未知) (Kullback-Leibler divergence) を最小化 手法の概要 1. 最適行動分布ℚ∗ を推定 (分布形状: 未知) 2. 重点サンプリングによってKLDを最小化する 𝑈𝑈𝑡𝑡∗ ≅ argmin D𝐾𝐾𝐾𝐾 (ℚ∗ ||ℚ) 𝑈𝑈𝑡𝑡 ∈𝒰𝒰 Kullback-Leibler divergence (KLD) の最小化 25

(MPPI理論) 1. 最適行動分布ℚ の導出 初期解 𝑻𝑻−𝟏𝟏 𝑺𝑺 𝑽𝑽𝒌𝒌 = 𝝓𝝓 𝒙𝒙𝑻𝑻 + � 𝒄𝒄 𝒙𝒙𝝉𝝉 + ランダムサンプリング: 𝑉𝑉1 , … , 𝑉𝑉𝐾𝐾 & 尤度 (コスト) 計算: 𝑆𝑆 𝑉𝑉1 , … , 𝑆𝑆 𝑉𝑉𝐾𝐾 ↓ 最適確率密度: 𝑞𝑞 ∗ 𝑉𝑉1 , … , 𝑞𝑞∗ (𝑉𝑉𝐾𝐾 )を推定 𝝉𝝉=𝟎𝟎 𝝀𝝀 𝒖𝒖 𝜮𝜮 𝒗𝒗𝝉𝝉 + 𝜷𝜷𝒖𝒖𝝉𝝉 𝟐𝟐 𝝉𝝉 ※ 最適行動分布ℚ∗ 形状自体は求まらない 導出: ・経路積分/ベイズ理論で良く出る式 1. 自由エネルギーを定義: ℱ 𝑉𝑉𝑘𝑘 = log Eℙ exp − 𝑆𝑆 𝑉𝑉𝑘𝑘 1 𝜆𝜆 状態コストの期待値 𝑉𝑉2 𝑆𝑆(𝑉𝑉2 ) 𝑉𝑉𝐾𝐾 𝑆𝑆(𝑉𝑉𝐾𝐾 ) ℙ: ノミナル入力印加時の行動分布 + 𝜆𝜆D𝐾𝐾𝐾𝐾 (ℚ||ℙ) 3. 正規分布の確率密度関数を代入: −𝜆𝜆𝜆 𝑉𝑉𝑘𝑘 ≤ Eℚ 𝜙𝜙 𝑥𝑥𝑇𝑇 + ∑𝑇𝑇−1 𝜏𝜏=0 𝑐𝑐 𝑥𝑥𝜏𝜏 + 𝑢𝑢𝜏𝜏 Σ 𝑢𝑢𝜏𝜏 + 𝛽𝛽𝑢𝑢𝜏𝜏 𝟏𝟏 𝝀𝝀 𝑆𝑆(𝑉𝑉1 ) ・𝜆𝜆: 温度パラメータ 2. イエンセンの不等式で下界を計算 & 式展開: −𝜆𝜆𝜆 𝑉𝑉𝑘𝑘 ≤ Eℚ 𝑆𝑆 𝑉𝑉𝑘𝑘 4. 最適確率密度関数: 𝒒𝒒∗ 𝑽𝑽𝒌𝒌 = 𝜼𝜼−𝟏𝟏 𝐞𝐞𝐞𝐞𝐞𝐞 − 𝑺𝑺 𝑽𝑽𝒌𝒌 𝑉𝑉1 ・・・ 【未知】 最適行動分布 ℚ∗ ∗ 𝒑𝒑(𝑽𝑽𝒌𝒌 ) 𝜆𝜆 2 ℙ, ℚは正規分布を仮定 = 解きたかった確率最適制御問題 → 下限値が自由エネルギーで抑えられている → 等号条件を満たす𝑞𝑞が最適確率密度関数 26

(MPPI理論) 2. 重点サンプリングによるKLD最小化 (A) 最適行動分布ℚ∗ と行動分布ℚのKLDを最小化 𝑈𝑈𝑡𝑡∗ ≅ argmin D𝐾𝐾𝐾𝐾 (ℚ∗ ||ℚ) 𝑈𝑈𝑡𝑡 ∈𝒰𝒰 = argmax Eℚ∗ log 𝑞𝑞 − Eℚ∗ log 𝑞𝑞 ∗ 𝑈𝑈𝑡𝑡 ∈𝒰𝒰 = argmax Eℚ∗ log 𝑞𝑞(𝑉𝑉𝑘𝑘 , 𝑈𝑈𝑡𝑡 ) 𝑈𝑈𝑡𝑡 ∈𝒰𝒰 = argmin Eℚ∗ 𝑈𝑈𝑡𝑡 ∈𝒰𝒰 = Eℚ∗ 𝑉𝑉𝑘𝑘 (KLDの定義) (𝑞𝑞∗ は𝑈𝑈𝑡𝑡 に非依存) (B) 重点サンプリングによってℚ∗ からのサンプルを近似 𝑈𝑈𝑡𝑡∗ = Eℚ∗ 𝑉𝑉𝑘𝑘 最適確率密度関数: = � 𝑞𝑞 ∗ 𝑉𝑉𝑘𝑘 𝑉𝑉𝑘𝑘 d 𝑉𝑉 1 𝑞𝑞∗ 𝑉𝑉𝑘𝑘 = 𝜂𝜂 −1 exp − 𝑆𝑆 𝑉𝑉𝑘𝑘 𝜆𝜆 𝑇𝑇−1 𝑞𝑞 ∗ 𝑉𝑉𝑘𝑘 𝑞𝑞 𝑉𝑉𝑘𝑘 , 𝑈𝑈𝑡𝑡 𝑉𝑉𝑘𝑘 d 𝑉𝑉 =� 𝑞𝑞 𝑉𝑉𝑘𝑘 , 𝑈𝑈𝑡𝑡 𝜏𝜏=0 𝑤𝑤 𝑉𝑉𝑘𝑘 1 � 𝑣𝑣𝜏𝜏 − 𝑢𝑢𝜏𝜏 T Σ 𝑣𝑣𝜏𝜏 − 𝑢𝑢𝜏𝜏 2 (𝑞𝑞は正規分布密度関数) 行動分布ℚが正規分布であることを仮定する → KLD最小化問題は閉形式で求まる！ → ただし，最適行動分布ℚ∗ から直接サンプルはできない ※ 𝑄𝑄∗ を正規分布に近似し，サンプリングした期待値が平均となるのはもちろん自明 重み: = Eℚ 𝑤𝑤(𝑉𝑉𝑘𝑘 )𝑉𝑉𝑘𝑘 𝑝𝑝(𝑉𝑉𝑘𝑘 ) (重点サンプリング) 𝑇𝑇−1 1 1 = exp(− 𝑆𝑆 𝑉𝑉𝑘𝑘 + 𝜆𝜆 � 𝑢𝑢� 𝑡𝑡 − 𝑢𝑢� 𝑡𝑡 𝜂𝜂 𝜆𝜆 𝑡𝑡=0 T Σ𝑣𝑣 ) 𝑡𝑡 初期解 ノミナル解 𝒩𝒩(𝑈𝑈𝑡𝑡init , Σ) (制御入力分布ℚ) から𝑉𝑉𝑘𝑘 をサンプリング → 重み付け平均によってKLD最小化の大域最適解が求まる ※ 大数の法則より，十分なサンプル数で最適性が保証 ※ 𝑈𝑈𝑡𝑡init : 前回の最適解𝑈𝑈𝑡𝑡−1 を用いることが多い (継承サンプリング) 27

ソフトウエア構成 オフライン処理 地図作成 リアルタイム処理 参照軌道計画 低速 自己位置推定 計画・制御 高速 28

参照軌道 (経路・速度) 計画 地図作成 参照軌道計画 自己位置推定 計画・制御 地図  経路・速度を同時に最適化: アウト・イン・アウト走法 • 最適化ソルバー : IPOPT [1] (主双対内点法) • 目的 : ラップタイムの最小化 中央線  制約条件 • 車両速度, 前後方向加速度, 左右方向加速度の制限 (例) カーブで滑る場合 → 左右方向加速度を小さくする • 壁面から一定距離を空けて走行 [1] IPOPT : https://coin-or.github.io/Ipopt/ 最適経路 29

高速化のための工夫 Online処理 Offline処理 地図作成 参照軌道計画 自己位置推定 計画・制御 ★ MPPIの用いる外部情報はすべてグリッドマップ化 → 近傍探索高速化 /参照軌道 /自己位置 参照情報生成 Reference SDF Generator 参照速度マップ 参照位置マップ 参照角度マップ 周辺認識 /スキャン Local Costmap Generator MPPI ★ CPU並列処理 障害物コストマップ 30