モデル予測経路積分制御 (Model Predictive Path Integral Control; MPPI) 入門

本記事は名古屋大学の本田康平（https://kohonda.github.io/ ）による寄稿です．

はじめに

ロボットの制御や運動計画で人気を博しているモデル予測制御 (MPC) ですが，MPCの中でもサンプルベースMPCは手頃に実装できる上に，性能もそこそこ良いため非常に使い勝手が良いです．サンプルベースMPCとは，有限時間将来までの制御入力のサンプルを複数用意して，それらを制御対象の予測モデルを用いて未来の状態を予測・評価して，制御入力を決定するというものです．これらは予測モデルやコスト関数が微分不可能であったり非線形性が強い場合でも利用できるので，とても使い勝手が良く，モデルベース強化学習などでもしばしば利用されます．

近年，サンプルベースMPCに対して確率推論的にアプローチをすることでサンプル効率の向上を図ろうという研究が盛んに行われています．Model Predictive Path Integral Control (MPPI)はその内の代表手法であり，とてもポピュラーな手法になりつつあります^[1]．
実際，MPPIで車両のステア角制御をしてみるとサンプルベースとは思えないような滑らかな制御が可能です．

MPPIは解の分布をガウス分布として仮定し，その分布と最適解の分布のKLダイバージェンスを最小化することで最適なガウス分布を求めます．すなわち，変分推論によって最適な制御入力を求めるようなアプローチを取っています．また，実際の計算では (逐次) 重点サンプリングによって最適解を計算するため，パーティクルフィルタのような雰囲気のアルゴリズムとなっています．

MPPIの特長としては以下のようなものが挙げられます．

• 実装が簡単
• 他のサンプルベースMPCよりも比較的サンプル効率が良い
• 解が閉経式で求まるので，解の反復更新が不要 = 並列化により大幅な高速化が可能

実装がとてもシンプルである一方，裏側にある理論は興味深く，多くの後発研究が生まれています．

MPPIの理論が記述されている論文はいくつかありますが，本記事ではTransaction on Robotics (T-RO) にて発表された Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving を紹介します．また，MPPIの導出には情報理論的なアプローチと経路積分制御からのアプローチがありますが，本記事では前者を紹介します．

1. MPPIのアルゴリズム

理論の前にMPPIのアルゴリズムを紹介します．

初めに変数を定義します．制御時刻$tt$における，$TT$時刻先までの未来の予測状態系列を$Xt=[x0,x1,…,xT]X_t=[x_0, x_1, \dots, x_T ]$，入力系列を$Ut=[u0,u1,…,uT−1]U_t=[u_0, u_1, \dots, u_{T-1}]$とします．ここで，$x0x_0$は現在の状態であり，$xτx_{\tau}$と$uτu_{\tau}$は予測時刻$tt$における状態と入力ベクトルです．ここで，予測状態に関するステージコスト関数$c(xτ)c(x_{\tau})$と終端コスト関数$Φ(xT)\Phi(x_T)$，および状態遷移関数$f(xτ+1∣xτ,uτ)f(x_{\tau+1} \mid x_{\tau}, u_{\tau})$がタスクに応じて設計されているとします．これらの関数は一般に微分不可能・非線形を許容します．

MPPIは以下のようなアルゴリズムで時刻$tt$における最適な入力系列$Ut∗U_{t}^*$ (最適解) を求めます．

(1) ランダムサンプリング & 予測状態の評価

初めにガウシアンノイズを付与した入力系列のランダムサンプル$Vk=[v0,v1,…,vT−1]V_k=[ v_{0}, v_{1}, \dots, v_{T-1} ]$を$KK$個，以下のようにサンプリングし，制御入力の上下限制約に収まるようにクリッピングします．

$$Vk∼∏τ=0T−1N(U^t,Σ)Vk=clamp(Vk,Umin,Umax) \begin{align} & V_k \sim \prod_{\tau=0}^{T-1}\mathcal{N}(\hat{U}_t, \Sigma) \nonumber \\ & V_k = \text{clamp}(V_k, U_{\rm{min}}, U_{\rm{max}}) \nonumber \end{align}$$

ただし，$U^t\hat{U}_t$は初期解であり，前回の最適な入力系列 (最適解) を用いることが一般的です (すなわち，$U^t=Ut−1\hat{U }_t = U _{t-1}$)．また，$Σ\Sigma$は共分散行列であり，一般には対角行列として予め与えられます．

生成した$KK$個の入力系列$[Vk]k=1K[V_ k ]_ {k=1}^K$に対して，状態遷移関数を用いて時刻$TT$までの予測状態を計算し，コスト関数$c(xτ)c(x_{\tau})$および$Φ(xT)\Phi(x_T)$を用いて各入力系列のコスト$S(Vk)S(V_k)$を計算します．

$$S(Vk)=Φ(xT)+∑τ=0T−1c(xτ)xτ+1=f(xτ+1∣xτ,vτ)x0=xt \begin{align} & S(V_k) = \Phi(x_T) + \sum_{\tau=0}^{T-1} c(x_{\tau}) \nonumber \\ & x_{\tau+1} = f(x_{\tau+1} \mid x_{\tau}, v_{\tau}) \nonumber \\ & x_{0} = x_{t} \nonumber \end{align}$$

※ 上記は並列計算によって実行されます.

(2) 重みの計算

次に，各ランダムサンプル$VkV_k$に対する重み$wkw_k$を以下のようにSoftmax関数を用いて計算します．

$$wk=1ηexp⁡(−1λS(Vk)−∑τ=0T−1(u^τ−u~τ)⊤Σ−1vτ) \begin{align} & w_k = \frac{1}{\eta} \exp \left( - \frac{1}{\lambda} S(V_k) - \sum_{\tau=0}^{T-1} (\hat{u}_{\tau} - \tilde{u}_{\tau})^{\top} \Sigma^{-1} v_{\tau} \right) \nonumber \\ \end{align}$$

ここで，$η\eta$は正規化項であり，$λ\lambda$は温度パラメータと呼ばれるハイパーパラメータです．また，$U^t=[u^0,u^1,…,u^T−1]\hat{U}_ t = [\hat{u}_ {0}, \hat{u}_ {1}, \dots, \hat{u}_ {T-1}]$は初期推定解であり，通常は前回の最適解を用います (i.e., $U^t=Ut−1\hat{U}_ t = U_ {t-1}$)．また，$U~t=[u~0,u~1,…,u~T−1]\tilde{U}_ t = [\tilde{u}_ {0}, \tilde{u}_ {1}, \dots, \tilde{u}_ {T-1}]$はノミナル入力系列と呼ばれ，MPPIの外部から与えられる入力系列です．通常は$U~t=0\tilde{U}_t = 0$としておきます．各パラメータの意味については後ほど解説します．

(3) 重み付け平均による解の更新

最後に重み付け平均によって最適解$Ut∗U_t^*$を計算します．

$$Ut∗=∑k=1KwkVk \begin{align} & U_t^* = \sum_{k=1}^{K} w_k V_k \nonumber \end{align}$$

以上の3ステップより，時刻$tt$における最適解$Ut∗U^*_t$が求まります．
詳しい実装はこちらをご覧ください．また，3章ではこのコードを用いて実験結果と各種パラメータの解説を行います．

2. MPPIの理論

上記のようにMPPIはとてもシンプルなアルゴリズムで解を求めることが出来ますが，その裏側の理論を理解することで，MPPIの特徴をより深く理解していきます．MPPIは下図のように大きく分けて3つのポイントがあります．以下ではそれぞれのポイントについて詳しく解説していきます．

(1) MPPIの問題設定とアプローチ

初めにMPPIで扱う問題のクラスを定義していきます．制御時刻$tt$における，$TT$時刻先までの未来の予測状態系列を$Xt=[x0,x1,…,xT]X_ t=[x_0, x_1, \dots, x_T ]$，入力系列を$Ut=[u0,u1,…,uT−1]U_ t =[u_0, u_1, \dots, u_{T-1}]$とします．ここで，$x0x_0$は現在の状態であり，$xτx_{\tau}$と$uτu_{\tau}$は予測時刻$tt$における状態と入力ベクトルです．

MPPIはシステムに実際に印加する制御入力系列$VV$は$UtU_t$を中心とした多変量ガウス分布$qq$に従うと仮定します．すなわち，$xτ+1=f(xτ+1∣xτ,vτ)x_{\tau+1} = f(x_{\tau+1} \mid x_{\tau}, v_{\tau})$であり，

$$V=(v0,v1,…,vT−1)∼q(V∣Ut,Σ)=∏τ=0T−1N(Ut,Σ)=Z−1exp⁡(−12∑τ=0T−1(vτ−uτ)⊤Σ−1(vτ−uτ)) \begin{align} V &= (v_0, v_1, \dots, v_{T-1}) \nonumber \\ & \sim q(V \mid U_t, \Sigma) \nonumber \\ & = \prod_{\tau=0}^{T-1} \mathcal{N}(U_t, \Sigma) \nonumber \\ & = Z^{-1} \exp \left( - \frac{1}{2} \sum _{\tau=0}^{T-1} (v_{\tau} - u_{\tau})^{\top} \Sigma^{-1} (v_{\tau} - u_{\tau}) \right) \tag{1} \end{align}$$

です．$ff$は一般に非線形・微分不可能な状態遷移関数，$Σ\Sigma$は共分散行列^[2]であり，予め与えられているとします．また，$ZZ$は正規化項です．

さて，MPPIは最適な制御入力系列$Ut∗U_t^*$を求めるために，以下の確率最適制御問題を解くことを目的とします．

$$Ut∗=arg⁡min⁡Ut∈UEV∼q[Φ(xT)+∑τ=0T−1L(xτ,uτ)] \begin{align} & U_t^* = \arg \min_{U_t \in \mathcal{U}} \mathbb{E}_{V \sim q} \left[ \Phi(x_T) + \sum_{\tau=0}^{T-1} L(x_{\tau}, u_{\tau}) \right] \tag{2} \end{align}$$

ここで$U\mathcal{U}$は制御入力の制約集合，$Φ(xT)\Phi(x_T)$は終端コスト，$L(xτ,uτ)L(x_{\tau}, u_{\tau})$はステージコストであり，設計者が任意に設定できるものとします．ただし，ステージコストは状態に関する項と入力に関する項に分離可能であり，

$$L(xτ,uτ)=c(xτ)+λ2(uτ⊤Σ−1uτ+β⊤uτ) \begin{align} & L(x_{\tau}, u_{\tau}) = c(x_{\tau}) + \frac{\lambda}{2} \left(u_{\tau}^{\top} \Sigma^{-1} u_{\tau} + \beta^{\top} u_{\tau} \right) \nonumber \end{align}$$

として表現されることを仮定します．$c(xτ)c(x_{\tau})$は状態に関するコスト，$Σ\Sigma$は共分散行列，$λ\lambda$と$β\beta$は定数パラメータです．$λ\lambda$は後ほど登場する温度パラメータとなります^[3]．
以下では，一本の入力系列$VkV_k$の状態のみに関する軌道コスト$S(Vk)S(V_k)$を以下のように定義します．

$$S(Vk)=Φ(xT)+∑τ=0T−1c(xτ) \begin{align} & S(V_k) = \Phi(x_T) + \sum_{\tau=0}^{T-1} c(x_{\tau}) \nonumber \\ \end{align}$$

一般に，式 (2) の確率最適制御問題を直接解くことは難しいです．そこで，MPPIは代わりに，最適な入力系列の分布$q∗q^*$と入力系列の分布$qq$の分布間の距離の指標，すなわちKLダイバージェンスを最小化するような変分推論問題を解くことで最適解を求めます．すなわち，

$$Ut∗≃arg⁡min⁡Ut∈UDKL[q∗∣∣q] \begin{align} & U_t^* \simeq \arg \min_{U_t \in \mathcal{U}} \mathbb{D}_{\rm{KL}} \left[ q^* || q \right] \tag{3} \end{align}$$

を解くことを目指します．

(2) 最適行動分布の導出

前節では式 (2) の確率最適制御問題を解く代わりに，式 (3)の変分推論問題を解くことを目指すと述べました．しかしながらそのためには，式 (3) で用いられている「最適な」分布$q∗q^*$が式 (2) のコストを最小化する意味で「最適」でなければ一貫性がありません．そこで，本節では最適な分布 $q∗q^*$ を導出し，$q∗q^*$ からサンプルされる制御入力が式 (2) のコストを最小化することを示します^[4]．

まず，天下り的ではありますがシステムの自由エネルギー$FF$を以下のように定義します．

$$F(V)=log⁡(EV∼p(V)[exp⁡(−1λS(V))]) \begin{align} & F(V) = \log \left( \mathbb{E}_{V \sim p(V)} \left[ \exp \left( - \frac{1}{\lambda} S(V) \right) \right] \right) \nonumber \\ \end{align}$$

ここで，$p(V)p(V)$は基底分布と呼ばれ，ベイズ推定における事前分布に類似した役割を果たします．また，$S(V)S(V)$は前節で定義した軌道の状態コスト，$λ\lambda$は温度パラメータです．後ほど分かるとおり，MPPIの文脈における自由エネルギーは制御の過程で最小化されるある種のメトリックであり，熱力学的な意味での自由エネルギーとは異なります^[5]．
自由エネルギーは重点サンプリングの式変形トリックとJensenの不等式を用いることで以下のように変形できます．

$$F(V)=log⁡(EV∼q[p(V)q(V)exp⁡(−1λS(V))])≥EV∼q[log⁡(p(V)q(V)exp⁡(−1λS(V)))]=EV∼q[log⁡(p(V)q(V))]−1λEV∼q(V)[S(V)]⇔−λF(V)≤EV∼q[S(V)]⏟状態軌道コストの期待値+λDKL[q(V)∣∣p(V)]⏟基底分布との乖離に対する正則化項 \begin{align} F(V) &= \log \left( \mathbb{E}_{V \sim q } \left[ \frac{p(V)}{q(V)} \exp \left( - \frac{1}{\lambda} S(V) \right) \right] \right) \nonumber \\ &\geq \mathbb{E}_{V \sim q} \left[ \log \left( \frac{p(V)}{q(V)} \exp \left( - \frac{1}{\lambda} S(V) \right) \right) \right] \nonumber \\ &= \mathbb{E}_{V \sim q} \left[ \log \left( \frac{p(V)}{q(V)} \right) \right] - \frac{1}{\lambda} \mathbb{E}_{V \sim q(V)} \left[ S(V) \right] \nonumber \\ \Leftrightarrow -\lambda F(V) &\leq \underbrace{\mathbb{E}_{V \sim q} \left[ S(V) \right]}_{状態軌道コストの期待値} + \underbrace{\lambda \mathbb{D}_{\rm{KL}} \left[ q(V) || p(V) \right]}_{基底分布との乖離に対する正則化項} \tag{4} \\ \end{align}$$

つまり，$−λF(V)-\lambda F(V)$は強化学習でもよく見かけるような，KLペナルティ付きコスト最小化問題の下界となっていることが分かります．

さて，ここからは制御入力系列の分布$qq$と$pp$が多変量ガウス分布であること，すなわち，

$$q(V∣Ut,Σ)=∏τ=0T−1N(Ut,Σ)p(V∣U~t,Σ)=q(V∣U~t,Σ)=∏τ=0T−1N(U~t,Σ) \begin{align} & q(V \mid U_t, \Sigma) = \prod_{\tau=0}^{T-1} \mathcal{N}(U_t, \Sigma) \nonumber \\ & p(V \mid \tilde{U}_t, \Sigma) = q(V \mid \tilde{U}_t, \Sigma) = \prod_{\tau=0}^{T-1} \mathcal{N}(\tilde{U}_t, \Sigma) \nonumber \\ \end{align}$$

を仮定します．ここで，$U~t\tilde{U}_t$はノミナル制御入力系列とよばれ，MPPIの外部から与えることができる (つまり任意の) 制御入力系列です．
式変形の詳細は省略しますが，このガウス分布仮定の下で，式 (4) を変形すると以下の不等式が得られます．

$$−λF(V)≤EV∼q[Φ(xT)+∑τ=0T−1(c(xτ)+λ2(uτ⊤Σ−1uτ+β⊤uτ))] \begin{align} & -\lambda F(V) \leq \mathbb{E}_{V \sim q} \left[ \Phi(x_T) + \sum_{\tau=0}^{T-1} \left( c(x_{\tau}) + \frac{\lambda}{2} \left( u_{\tau}^{\top} \Sigma^{-1} u_{\tau} + \beta^{\top} u_{\tau} \right) \right) \right] \nonumber \end{align}$$

上式の右辺はまさに，本来解きたかった確率最適制御問題 (2) のコスト関数と等価となります．つまり，ガウス分布仮定の下では，$−λF(V)-\lambda F(V)$は本来解きたかった確率最適制御問題 (2) のコスト最小値であることが分かります．
以上より，式 (2) の確率最適制御問題のコストを最小化する意味での最適な確率分布$q∗q^*$は，式 (4) の等号成立条件を満たすような$qq$となります．よって，

$$q∗(V∣U~t,Σ)=η−1exp⁡(−1λS(V))q(V∣U~t,Σ) \begin{align} & q^*(V \mid \tilde{U}_t, \Sigma) = \eta^{-1} \exp \left( - \frac{1}{\lambda} S(V) \right) q(V \mid \tilde{U}_t, \Sigma) \tag{5} \end{align}$$

が導出されます．ここで，$η\eta$は正規化項です．$q∗(V∣U~t,Σ)q^*(V \mid \tilde{U}_t, \Sigma)$からサンプルされる制御入力系列$VV$は，式 (2) のコストを最小化することができます．

(3) 重点サンプリングによるKLダイバージェンスの最小化

ここまでの議論をまとめますと，

• MPPIは式 (2) の確率最適制御問題を解く代わりに，最適制御分布 $q∗q^*$ とのKLダイバージェンスを最小化するようなガウス分布$qq$を求める変分推論問題を解きます．
• また，式 (2) の意味で最適な分布$q∗q^*$は式 (5) で与えられることが導出されました．

本節では導出した式 (5) を用いて，式 (3) の変分推論問題を解いていきます．

まず，$q(V∣U~t,Σ)q(V \mid \tilde{U}_t, \Sigma)$がガウス分布である時，その定義から式 (3) は以下のように変形できます．

$$Ut∗=arg⁡min⁡Ut∈UDKL[q∗∣∣q]=arg⁡min⁡Ut∈UEV∼q∗[log⁡(q∗(V∣U~t,Σ)q(V∣Ut,Σ))]=arg⁡max⁡Ut∈UEV∼q∗[log⁡(q(V∣Ut,Σ))]=arg⁡min⁡Ut∈UEV∼q∗[∑τ=0T−1(vτ−uτ)⊤Σ−1(vτ−uτ)]≃EV∼q∗[V] \begin{align} U_t^* &= \arg \min_{U_t \in \mathcal{U}} \mathbb{D}_{\rm{KL}} \left[ q^* || q \right] \nonumber \\ &= \arg \min_{U_t \in \mathcal{U}} \mathbb{E}_{V \sim q^*} \left[ \log \left( \frac{q^*(V \mid \tilde{U}_t, \Sigma)}{q(V \mid U_t, \Sigma)} \right) \right] \nonumber \\ &= \arg \max_{U_t \in \mathcal{U}} \mathbb{E}_{V \sim q^*} \left[ \log \left( q(V \mid U_t, \Sigma) \right) \right] \nonumber \\ &= \arg \min_{U_t \in \mathcal{U}} \mathbb{E}_{V \sim q^*} \left[ \sum_{\tau=0}^{T-1} (v_{\tau} - u_{\tau})^{\top} \Sigma^{-1} (v_{\tau} - u_{\tau}) \right] \nonumber \\ &\simeq \mathbb{E}_{V \sim q^*} \left[ V \right] \tag{6} \end{align}$$

式 (6) の最後の近似は$UtU_t$に関する制約が無い場合，つまり$U=Rm\mathcal{U} = \mathbb{R}^{m}$の場合にのみ，無制約凸最適化となり，完全に等しくなりますが，実用上は1章の (1)で述べたようにサンプリング時にクリッピングを行うことで近似的に無制約最適化問題として扱うことができます．つまり，状態遷移関数が制約条件を満たすように必ず遷移する，という設定に持ち込んでいます．

式 (6) より，$qq$がガウス分布である場合，KLダイバージェンスの最小化は閉経式で解くことができます^[6]．すなわち，勾配法のような解の反復更新を行う必要がなく，最適分布 $q∗q^*$ から十分な入力系列をサンプルし，それらの平均を求めることで最適解を得ることができます．
しかしながら，現実的には $q∗q^*$ はサンプルを評価しなければ計算できないため，直接サンプリングをする方法がありません．そこで，MPPIでは重点サンプリングを用いて近似的に $q∗q^*$からサンプリングをします．すなわち，

$$Ut∗=EV∼q∗[V]=∫q∗(V∣U~t,Σ)VdV=∫q∗(V∣U~t,Σ)q(V∣U~t,Σ)q(V∣U^t,Σ)VdV=EV∼q[q∗(V∣U~t,Σ)q(V∣U^t,Σ)V]≃1K∑k=1Kw(Vk)Vk \begin{align} U_t^* = \mathbb{E}_{V \sim q^*} \left[ V \right] &= \int q^*(V \mid \tilde{U}_t, \Sigma) V dV \nonumber \\ &= \int q^*(V \mid \tilde{U}_t, \Sigma) \frac{q(V \mid \tilde{U}_t, \Sigma)}{q(V \mid \hat{U}_t, \Sigma)} V dV \nonumber \\ &= \mathbb{E}_{V \sim q} \left[ \frac{q^*(V \mid \tilde{U}_t, \Sigma)}{q(V \mid \hat{U}_t, \Sigma)} V \right] \nonumber \\ &\simeq \frac{1}{K} \sum_{k=1}^{K} w(V_k) V_k \tag{7} \end{align}$$

として最適解が求まります．ただし，式 (5)と$qq$の定義より，

$$w(Vk)=q∗(Vk∣U~t,Σ)q(Vk∣U^t,Σ)=1ηexp⁡(−1λS(Vk)−∑τ=0T−1(u^τ−u~τ)⊤Σ−1vτ) \begin{align} w(V_k) &= \frac{q^*(V_k \mid \tilde{U}_t, \Sigma)}{q(V_k \mid \hat{U}_t, \Sigma)} \nonumber \\ &= \frac{1}{\eta} \exp \left( - \frac{1}{\lambda} S(V_k) - \sum_{\tau=0}^{T-1} (\hat{u}_{\tau} - \tilde{u}_{\tau})^{\top} \Sigma^{-1} v_{\tau} \right) \tag{8} \end{align}$$

となります．$η\eta$は正規化項，$U^t=[u^0,u^1,…,u^T−1]\hat{U}_ t = [\hat{u} _{0}, \hat{u} _{1}, \dots, \hat{u} _{T-1} ]$は初期推定解であり，通常は前回の最適解を用います ($U^t=Ut−1\hat{U} _t = U _{t-1}$)．以上より，最適解$Ut∗U_t^*$はガウス分布から$VkV_k$をサンプリングし，重み付け平均を1度だけ取ることで求まることが分かります．すなわち，解の反復更新は不要となります．その結果，並列化の恩恵を存分に受けることができるため，サンプル数$KK$を大きくしやすくなります．
また，式 (6) より，最適解は閉経式で求まることから，大数の法則より，サンプル数$KK$が十分に大きければ，式 (3) のKLダイバージェンスを最小化するという意味での大域最適解が保証されます．

MPPIの各種パラメータについて

これまでにMPPIの基本的な理論・実装については述べましたが，本節では，これまでに登場したMPPIのアルゴリズムに関する重要なパラメータの意味を考察し，調整のポイントをまとめます．

A. 共分散行列$Σ\Sigma$

式 (1) のランダムサンプルを生成するガウス分布の共分散行列です．また，式 (2) のMPPIの解く最適化問題のステージコストでは，入力に関する項に$Σ−1\Sigma^{-1}$が現れます．式 (8) の重みについても同様に入力に関する項に関わっています．この共分散行列は以下の2点に関して重要な役割を果たします．

1. 解の探索空間調整:
   MPPIにおいてサンプリングするガウス分布はある種解の探索範囲とみなすことができます．つまり，付与するノイズが解の更新の原動力となっています．そのため，共分散行列が大きいほど，解の探索範囲が広く，解の更新幅が大きくなります．ただし，共分散が大きいということは，入力に関するペナルティが小さくなることを意味するため，解の滑らかさが低下します (サンプルの"密度"が不足しやすくなる，という解釈もできます)．
   また，MPPIが最小化するKLダイバージェンスは多峰性の最適分布の複数のモードをカバーしやすいという性質があるため，共分散行列が大きい時には意図しないような挙動を引き起こす場合があります．参考

2. 入力に対するペナルティの調整:
   式 (2) や式 (8) を見ても分かるとおり，共分散行列が小さいほど，コスト関数的に入力に対するペナルティが大きくなります．つまり，共分散行列が小さいほど，入力ペナルティが大きくなり，解がゆっくりと変化します．

※ 今回紹介するMPPIの原著論文では共分散行列は固定されていますが，平均$UtU_t$と同じように最適化問題の一部として共分散行列を最適化することも可能です．ただしその場合は解が閉経式で求まらなくなるため，解の反復更新が必要となります．参考

B. 温度パラメータ$λ\lambda$

温度パラメータは式 (4) と式 (8) を見るとその意味を捉えやすいです．式 (4) では温度パラメータが大きいほど，正規化項の影響が大きくなります．また，式 (8) はSoftmax関数となっていますが，温度パラメータが大きいほど，Softmax関数の出力が均一になり，重みが一様になります．逆に，温度パラメータが小さいほど，Softmax関数の出力がスパイク状になり，ランダムサンプルの中からベストなものを一つ選ぶような挙動に近くなります．

C. ノミナル入力系列$U~t\tilde{U}_t$

ノミナル入力系列はMPPIの外部から与えることができる制御入力系列です．ノミナル軌道は元を辿ると式 (4) の基底分布の平均値から由来しています．MPPIの解は基底分布との乖離に対するペナルティが加わるため，MPPIが算出する解はノミナル軌道との乖離が小さいものとなります．これは式 (8) 重み関数を見ると歴然であり，ノミナル軌道との差にペナルティが掛かっています．

3. 実験

MPPIをPyTorchを用いて実装したものがこちらになります．

2次元空間におけるロボットのナビゲーションタスクを例に，MPPIの挙動を確認します．行動空間は$uτ=[v,ω]u_{\tau} = [v, \omega]$，状態空間は$xτ=[x,y,θ]x_{\tau} = [x, y, \theta]$とします．$vv$はロボットの直進速度，$ω\omega$はロボットの角速度，$xx$と$yy$はロボットの位置，$θ\theta$はロボットの向きを表します．車両の状態遷移モデルは差動2輪モデルを用い，コスト関数は以下のように設計しました．

$$c(xτ)=Φ(xT)=∥pτ−pgoal∥2+1000⋅1collision(pτ)1collision={1if collision0otherwise \begin{align} & c(x_{\tau}) = \Phi(x_T) = \| p_{\tau} - p_{\rm{goal}} \|_2 + 1000 \cdot \mathbb{1}_{\rm{collision}} (p_{\tau}) \nonumber \\ & \mathbb{1}_{\rm{collision}} = \begin{cases} 1 & \text{if collision} \\ 0 & \text{otherwise} \end{cases} \nonumber \end{align}$$

ただし，$pτp_{\tau}$は時刻$τ\tau$におけるロボットの位置ベクトル，$pgoalp_{\rm{goal}}$はゴール位置です．障害物に対する衝突判定はOccupancy Grid Mapを用いて行います．
予測ホライズンは$T=0.1 s×50 step=5T=0.1 \text{ s} \times 50 \text{ step} = 5$ s，サンプル数は$K=10000K=10000$としました．(経験的には500~1000くらいでも十分です．) ノミナル入力系列は$U~t=0\tilde{U}_t = 0$としました．

実際にMPPIを実行した結果 ($λ=1.0\lambda=1.0$，$Σ=diag(0.25,0.25)\Sigma=\text{diag}(0.25, 0.25)$) を以下に示します．濃い青の軌道が最適解，薄い青の軌道が重みの大きな100個のサンプル軌道を表しています．滑らかに障害物を避けながらゴールに到達することが確認出来ます．

(1) 共分散行列$Σ\Sigma$の調整

共分散行列$Σ\Sigma$を変えた場合の挙動を確認します．

A. 共分散を小さくした場合 ($Σ=diag(0.1,0.1)\Sigma = \text{diag}(0.1, 0.1)$)

入力の変化量が小さくなり，解が良く言えば滑らかに，悪く言えば非効率的にゴールに向かっていることが分かります．

B. 共分散を大きくした場合 ($Σ=diag(1.0,1.0)\Sigma = \text{diag}(1.0, 1.0)$)

共分散行列を大きくした場合には逆に入力の変化量が大きくなり，解が良く言えば大きな出力で効率的に，悪く言えばjerkyな (ギクシャクした) 軌道でゴールに向かっていることが分かります．算出される軌道の分布も広がっていることが分かります．

(2) 温度パラメータ$λ\lambda$の調整

次に温度パラメータ$λ\lambda$を変えた場合の挙動を確認します．

A. 温度パラメータを小さくした場合 ($λ=0.1\lambda = 0.1$)

温度パラメータを小さくすると，Softmax関数の出力がスパイク状になり，ランダムサンプルの中からベストなものを一つ選ぶような挙動に近くなります．よって，解が激しく変化することが分かります．

B. 温度パラメータを大きくした場合 ($λ=2.0\lambda = 2.0$)

温度パラメータを大きくすると，Softmax関数の出力が均一になり，重みが一様になります．よって，解が滑らかに変化することが分かります．

(3) サンプルの質の重要性

MPPIのようなサンプリングベースの手法では，サンプルの質が重要です．ここで言う"良質な"サンプルとは，コスト関数の値が小さいサンプルのことを指します．良質なサンプルが多ければ多いほどサンプル効率は向上し，より良い解を得ることができます．逆に，良質なサンプルが少ないと，サンプル効率が悪くなり，解の質が低下します．例えばこの記事では良質なサンプルを得るために機械学習を組み合わせる方法を紹介しました．

サンプルの質を劣化させる簡単な方法として，予測ホライズンを不必要に長くする方法があります．予測ホライズンを長くしすぎると，このような障害物の多い環境ではほとんどのサンプルが障害物に衝突するため，サンプルの質が顕著に劣化します．
以下では予測ホライズンのステップ数を50，100，300と変えた場合の挙動を確認します．

A. 50ステップ: (再掲)

B. 100ステップ:

C. 300ステップ:

多くのサンプルが棄却されることによって，ランダムサンプルの中でベストなものが選ばれているような挙動に近づくことが確認されます．その結果，解の滑らかさが低下し，jerkyな動きとなっています．ちなみに，予測ホライズンを増やすとシーケンシャルな処理の計算量が増えるため，計算時間が増加していきます．

脚注

1. Navigation 2にも実装されています．また，筆者がICRA/IROS 2023で観測した限りでは，DWAやCross Entropy Methodのようなスタンダードな手法をMPPIが置き換えている印象です．

2. 共分散行列を可変とする場合もありますが，その場合は解が閉経式で求まりません．共分散行列の設計は後述する通り重要な調整要素となります．

3. 理論的に帳尻を合わせるためにステージコストがこのような関数形状となっています．

4. 変分推論的にMPCを解く方法は他にもありますが，元の確率最適制御問題とリンクできることを示してる点がMPPIのユニークな点だと筆者は考えています．

5. MPPI以前の論文でも登場しており，例えばこちらの論文などがあります．

6. KLダイバージェンスは引数を逆にすると異なる性質を示すことが知られています．つまり，$DKL[q∣∣q∗]\mathbb{D}_{\rm{KL}} \left[ q || q^* \right]$を解くと結果が変わります．ただし，こちらに限っては非凸最適化問題へと帰着するため，解を閉経式で求めることができなくなります 参考．