PyTorch MPPI 实现

本仓库在 PyTorch 中实现了带有近似动力学的模型预测路径积分（MPPI）。MPPI 通常需要实际的轨迹样本，但此论文表明，通过重要性采样，它可以结合近似动力学（例如神经网络）来实现。

因此，它可以替代其他轨迹优化方法，如交叉熵方法（CEM）或随机射击法。

---

2024年8月新增平滑方法，包括我们自己的 KMPPI，详见下面的平滑部分

安装

pip install pytorch-mppi

若要自动调整超参数，请使用以下命令安装

pip install pytorch-mppi[tune]

为运行测试，请按以下方式安装

pip install pytorch-mppi[test]

若要进行开发，请克隆代码仓库，然后以可编辑模式安装。

pip install -e .

使用方法

有关使用神经网络逼近摆动力学的示例，请参见 tests/pendulum_approximate.py。有关更易于阅读的算法，请参见 not_batch 分支。基本使用示例如下

from pytorch_mppi import MPPI

# create controller with chosen parameters
ctrl = MPPI(dynamics, running_cost, nx, noise_sigma, num_samples=N_SAMPLES, horizon=TIMESTEPS,
            lambda_=lambda_, device=d,
            u_min=torch.tensor(ACTION_LOW, dtype=torch.double, device=d),
            u_max=torch.tensor(ACTION_HIGH, dtype=torch.double, device=d))

# assuming you have a gym-like env
obs = env.reset()
for i in range(100):
    action = ctrl.command(obs)
    obs, reward, done, _ = env.step(action.cpu().numpy())

要求

• pytorch（版本 >= 1.0）
• next state <- dynamics(state, action) 函数（不必是真实动力学模型）
  • state 为 K x nx 维度，action 为 K x nu 维度
• cost <- running_cost(state, action) 函数
  • cost 为 K x 1 维度，state 为 K x nx 维度，action 为 K x nu 维度

特性

• 采用重要性采样的近似动力学 MPPI
• 并行/批量 pytorch 实现，加速采样过程
• 通过从修正高斯分布中采样控制噪声来实现控制边界
• 通过 rollout_samples 为同一动作轨迹采样多条状态轨迹，以处理随机动力学模型（假设每次调用都是一个样本）

参数调优与提示

terminal_state_cost - 函数（输入为状态 (K x T x nx)）-> 输出为代价 (K x 1)。默认情况下没有终端代价，但如果您发现轨迹接近目标却始终无法到达，那么添加终端代价可能会有所帮助。该函数的尺度应与时间 horizon (T) 相匹配，以与运行代价的尺度保持一致。

lambda_ - 该值越高，控制噪声的代价越大，因此最终会在均值附近产生更多样本；通常较小的值效果更好（可尝试 1e-2）

num_samples - 要采样的轨迹数量；通常数量越多越好。运行时性能受 num_samples 的影响比受 horizon 的影响要小得多，尤其是在使用 GPU 设备时（记得传入设备参数！）

noise_mu - 所有控制维度的默认值为 0，如果存在控制边界且允许范围不以 0 为中心，这可能会导致效果很差。对于非对称的控制维度，请记得将此值更改为适当的数值。

平滑处理

从 0.8.0 版本开始，您可以使用具有控制信号平滑功能的 MPPI 变体。我们实现了 SMPPI 以及我们自己的核插值 MPPI（KMPPI）。在基础算法中，您可以通过增大 lambda_ 来获得相对平滑的轨迹；然而，这会以牺牲最优性为代价。显式的平滑算法可以在不牺牲最优性的前提下实现平滑效果。

我们在最近的论文中使用并描述了该方法（arxiv），在我们发布专门针对 KMPPI 的研究成果之前，您可以引用该论文。下面展示了在一个控制量为受约束的位置增量的 2D 导航玩具问题上，MPPI、SMPPI 和 KMPPI 之间的差异。您可以在 tests/smooth_mppi.py 中查看相关代码。

API 基本保持一致，但增加了一些额外的构造函数选项：

import pytorch_mppi as mppi
ctrl = mppi.KMPPI(args, 
                 kernel=mppi.RBFKernel(sigma=2), # kernel in trajectory time space (1 dimensional)
                 num_support_pts=5,              # number of control points to sample, <= horizon
                 **kwargs)

内核可以是 mppi.TimeKernel 的任何子类。它是轨迹时间空间（一维）中的内核。 请注意，B样条平滑可以通过使用B样条内核来实现。支持点的数量是要采样的控制点数量。中间的任何轨迹点都使用内核进行插值。例如，如果轨迹时域为20且 num_support_pts 为5，则会在整个时域内均匀间隔采样5个控制点（第一个和最后一个对应于轨迹的实际起点和终点）。轨迹的其余部分使用内核进行插值。内核应用于控制信号，而非状态信号。

无平滑的MPPI

SMPPI 通过在动作导数空间中采样噪声进行平滑，在此问题上效果不佳

使用RBF内核的KMPPI平滑效果良好

自动调优

从0.5.0版本开始，您可以自动调整超参数。 已实现一个与流行的 ray tune 库兼容的便捷调优器。您可以从各种尖端的黑盒优化器中进行选择，例如 CMA-ES、HyperOpt、fmfn/BayesianOptimization 等。 有关示例，请参见 tests/auto_tune_parameters.py。下面是基于此的教程。

该调优器也可用于其他控制器，但您需要定义相应的 TunableParameter 子类。

首先，我们创建一个用于控制的玩具2D环境，并使用一些默认参数创建控制器。

import torch
from pytorch_mppi import MPPI

device = "cpu"
dtype = torch.double

# create toy environment to do on control on (default start and goal)
env = Toy2DEnvironment(visualize=True, terminal_scale=10)

# create MPPI with some initial parameters
mppi = MPPI(env.dynamics, env.running_cost, 2,
            terminal_state_cost=env.terminal_cost,
            noise_sigma=torch.diag(torch.tensor([5., 5.], dtype=dtype, device=device)),
            num_samples=500,
            horizon=20, device=device,
            u_max=torch.tensor([2., 2.], dtype=dtype, device=device),
            lambda_=1)

接下来，我们需要为调优器创建一个用于调优的评估函数。该函数应不接受任何参数，并返回一个至少包含成本的EvaluationResult。如果成本评估不需要轨迹推演（rollouts），则可在返回时将其设为 None。创建评估函数的相关提示已在下方注释中说明：

from pytorch_mppi import autotune
# use the same nominal trajectory to start with for all the evaluations for fairness
nominal_trajectory = mppi.U.clone()
# parameters for our sample evaluation function - lots of choices for the evaluation function
evaluate_running_cost = True
num_refinement_steps = 10
num_trajectories = 5

def evaluate():
    costs = []
    rollouts = []
    # we sample multiple trajectories for the same start to goal problem, but in your case you should consider
    # evaluating over a diverse dataset of trajectories
    for j in range(num_trajectories):
        mppi.U = nominal_trajectory.clone()
        # the nominal trajectory at the start will be different if the horizon's changed
        mppi.change_horizon(mppi.T)
        # usually MPPI will have its nominal trajectory warm-started from the previous iteration
        # for a fair test of tuning we will reset its nominal trajectory to the same random one each time
        # we manually warm it by refining it for some steps
        for k in range(num_refinement_steps):
            mppi.command(env.start, shift_nominal_trajectory=False)

        rollout = mppi.get_rollouts(env.start)

        this_cost = 0
        rollout = rollout[0]
        # here we evaluate on the rollout MPPI cost of the resulting trajectories
        # alternative costs for tuning the parameters are possible, such as just considering terminal cost
        if evaluate_running_cost:
            for t in range(len(rollout) - 1):
                this_cost = this_cost + env.running_cost(rollout[t], mppi.U[t])
        this_cost = this_cost + env.terminal_cost(rollout, mppi.U)

        rollouts.append(rollout)
        costs.append(this_cost)
    # can return None for rollouts if they do not need to be calculated
    return autotune.EvaluationResult(torch.stack(costs), torch.stack(rollouts))

有了这些，我们就足以开始调优了。例如，我们可以使用 CMA-ES 优化器进行迭代调优。

# these are subclass of TunableParameter (specifically MPPIParameter) that we want to tune
params_to_tune = [autotune.SigmaParameter(mppi), autotune.HorizonParameter(mppi), autotune.LambdaParameter(mppi)]
# create a tuner with a CMA-ES optimizer
tuner = autotune.Autotune(params_to_tune, evaluate_fn=evaluate, optimizer=autotune.CMAESOpt(sigma=1.0))
# tune parameters for a number of iterations
iterations = 30
for i in range(iterations):
  # results of this optimization step are returned
  res = tuner.optimize_step()
  # we can render the rollouts in the environment
  env.draw_rollouts(res.rollouts)
# get best results and apply it to the controller
# (by default the controller will take on the latest tuned parameter, which may not be best)
res = tuner.get_best_result()
tuner.apply_parameters(res.param_values)

这是一种从初始定义的参数开始进行优化的局部搜索方法。对于全局搜索，我们使用与 ray tune 兼容的搜索算法。请注意，您可以修改每个参数的搜索空间，但已提供默认的合理搜索空间。

# can also use a Ray Tune optimizer, see
# https://docs.ray.io/en/latest/tune/api_docs/suggestion.html#search-algorithms-tune-search
# rather than adapting the current parameters, these optimizers allow you to define a search space for each
# and will search on that space
from pytorch_mppi import autotune_global
from ray.tune.search.hyperopt import HyperOptSearch
from ray.tune.search.bayesopt import BayesOptSearch

# the global version of the parameters define a reasonable search space for each parameter
params_to_tune = [autotune_global.SigmaGlobalParameter(mppi),
                  autotune_global.HorizonGlobalParameter(mppi),
                  autotune_global.LambdaGlobalParameter(mppi)]

# be sure to close any figures before ray tune optimization or they will be duplicated
env.visualize = False
plt.close('all')
tuner = autotune_global.AutotuneGlobal(params_to_tune, evaluate_fn=evaluate,
                                       optimizer=autotune_global.RayOptimizer(HyperOptSearch))
# ray tuners cannot be tuned iteratively, but you can specify how many iterations to tune for
res = tuner.optimize_all(100)
res = tuner.get_best_result()
tuner.apply_parameters(res.params)

例如，仅在玩具问题上（每次重置标称轨迹，因此它们是从噪声中采样）使用CMA-ES调整超参数：

如果您想要的不仅仅是找到的最佳解决方案，例如您希望超参数值具有多样性，或者您的评估函数具有较大的不确定性，那么您可以通过以下方式直接查询过去的结果

for res in tuner.optim.all_res:
    # the cost
    print(res.metrics['cost'])
    # extract the parameters
    params = tuner.config_to_params(res.config)
    print(params)
    # apply the parameters to the controller
    tuner.apply_parameters(params)

或者，您可以尝试使用 CMA-ME 优化器 进行质量多样性优化。该优化器会尝试优化参数以获得高质量，同时确保参数之间具有多样性。不过，它的速度非常慢，您可能更适合使用 RayOptimizer 并在检查多样性的同时选择最佳结果。 要使用它，您需要安装

pip install ribs

然后您可以将其用作

import pytorch_mppi.autotune_qd

optim = pytorch_mppi.autotune_qd.CMAMEOpt()
tuner = autotune_global.AutotuneGlobal(params_to_tune, evaluate_fn=evaluate,
                                       optimizer=optim)

iterations = 10
for i in range(iterations):
  # results of this optimization step are returned
  res = tuner.optimize_step()
  # we can render the rollouts in the environment
  best_params = optim.get_diverse_top_parameters(5)
  for res in best_params:
    print(res)

测试

在 tests 目录下，你可以找到将 MPPI 方法应用于已知摆动力学和近似摆动力学（使用两层前馈网络估计状态残差）的示例。使用连续角度表示（输入 cos(\theta)、sin(\theta) 而非直接输入 \theta）会带来巨大差异。尽管两种表示方法都能工作，但连续表示对控制器参数和随机种子的鲁棒性要强得多。此外，过摆后持续旋转的问题也不会出现。

使用 100 步随机策略数据初始化动力学后，在近似动力学上的示例结果：

相关项目

• pytorch CEM - 另一种 MPC 打靶法，与本项目具有相似的 API
• pytorch iCEM - 另一种基于采样的 MPC