MPPI（Model Predictive Path Integral）の数理：クロスエントロピー法との統一的理解

入力: 初期制御列 μ = (μ_0, ..., μ_{T-1}), サンプル数 N, 温度 λ, ノイズ分散 Σ
繰り返し:
  1. ノイズ付き制御列をN個サンプリング:
     ε_t^(i) ~ N(0, Σ),  V^(i) = μ + ε^(i)
  2. 各サンプルのコストを計算:
     J(V^(i)) = φ(x_T^(i)) + Σ_t q(x_t^(i), v_t^(i))
  3. 重みを計算:
     w^(i) = exp(-J(V^(i))/λ) / Σ_j exp(-J(V^(j))/λ)
  4. 制御入力を更新:
     μ_t ← μ_t + Σ_i w^(i) ε_t^(i)
  5. 最初の制御入力 μ_0 を適用し、制御列をシフト

import numpy as np
import matplotlib.pyplot as plt

# ---- 問題設定 ----
n_state = 4     # 状態次元 [x, y, vx, vy]
n_ctrl = 2      # 制御次元 [ax, ay]
dt = 0.1        # 時間刻み
T = 20          # 予測ホライズン
N = 500         # サンプル数
lam = 1.0       # 温度パラメータ
sigma = 0.5     # 制御ノイズの標準偏差
goal = np.array([5.0, 5.0])  # ゴール位置

def dynamics(x, u):
    """線形力学モデル（質点の2次元運動）"""
    x_next = np.zeros(n_state)
    x_next[0] = x[0] + x[2] * dt  # px
    x_next[1] = x[1] + x[3] * dt  # py
    x_next[2] = x[2] + u[0] * dt  # vx
    x_next[3] = x[3] + u[1] * dt  # vy
    return x_next

def stage_cost(x, u):
    """ステージコスト"""
    pos = x[:2]
    dist = np.sum((pos - goal)**2)
    ctrl = 0.01 * np.sum(u**2)
    return dist + ctrl

def terminal_cost(x):
    """終端コスト"""
    pos = x[:2]
    return 10.0 * np.sum((pos - goal)**2)

class MPPIController:
    def __init__(self):
        # 制御列の初期化（ゼロ）
        self.mu = np.zeros((T, n_ctrl))

    def compute_control(self, x0):
        """MPPIによる最適制御入力の計算"""
        # ステップ1: ノイズ付き制御列のサンプリング
        noise = np.random.randn(N, T, n_ctrl) * sigma
        V = self.mu[np.newaxis, :, :] + noise  # (N, T, n_ctrl)

        # ステップ2: 各サンプルのコスト計算
        costs = np.zeros(N)
        for i in range(N):
            x = x0.copy()
            for t in range(T):
                costs[i] += stage_cost(x, V[i, t])
                x = dynamics(x, V[i, t])
            costs[i] += terminal_cost(x)

        # ステップ3: 重みの計算（式4）
        # 数値安定性のためにコストの最小値を引く
        costs_shifted = costs - np.min(costs)
        weights = np.exp(-costs_shifted / lam)
        weights /= np.sum(weights)  # 正規化

        # ステップ4: 制御入力の更新（式5）
        weighted_noise = np.sum(
            weights[:, np.newaxis, np.newaxis] * noise, axis=0
        )
        self.mu += weighted_noise

        # 最適制御入力（最初のタイムステップ）
        u_opt = self.mu[0].copy()

        # 制御列のシフト（次のタイムステップの準備）
        self.mu = np.roll(self.mu, -1, axis=0)
        self.mu[-1] = 0.0  # 最後の要素をゼロに

        return u_opt

np.random.seed(42)

# 初期状態
x = np.array([0.0, 0.0, 0.0, 0.0])
controller = MPPIController()

# シミュレーション
n_steps = 100
trajectory = [x.copy()]
controls = []

for step in range(n_steps):
    u = controller.compute_control(x)
    controls.append(u.copy())
    x = dynamics(x, u)
    trajectory.append(x.copy())

    # ゴール到達判定
    if np.linalg.norm(x[:2] - goal) < 0.1:
        print(f"Goal reached at step {step + 1}")
        break

trajectory = np.array(trajectory)
controls = np.array(controls)

# ---- 結果のプロット ----
fig, axes = plt.subplots(1, 2, figsize=(14, 6))

# 軌道
axes[0].plot(trajectory[:, 0], trajectory[:, 1], "b-o",
             markersize=3, label="MPPI trajectory")
axes[0].plot(*goal, "r*", markersize=15, label="Goal")
axes[0].plot(0, 0, "gs", markersize=10, label="Start")
axes[0].set_xlabel("x")
axes[0].set_ylabel("y")
axes[0].set_title("MPPI - Goal Reaching")
axes[0].legend()
axes[0].grid(True)
axes[0].set_aspect("equal")

# 制御入力
axes[1].plot(controls[:, 0], label="$a_x$")
axes[1].plot(controls[:, 1], label="$a_y$")
axes[1].set_xlabel("Step")
axes[1].set_ylabel("Control input")
axes[1].set_title("Control inputs over time")
axes[1].legend()
axes[1].grid(True)

plt.tight_layout()
plt.savefig("mppi_result.png", dpi=150)
plt.show()

# 温度パラメータの影響を可視化
costs_example = np.linspace(0, 10, 100)
fig, ax = plt.subplots(figsize=(8, 5))

for lam_val in [0.1, 0.5, 1.0, 5.0]:
    w = np.exp(-costs_example / lam_val)
    w /= np.sum(w)
    ax.plot(costs_example, w, label=f"$\\lambda={lam_val}$")

ax.set_xlabel("Cost $J$")
ax.set_ylabel("Weight $w$")
ax.set_title("Temperature parameter effect on MPPI weights")
ax.legend()
ax.grid(True)
plt.tight_layout()
plt.savefig("mppi_temperature.png", dpi=150)
plt.show()