1 MPPI 算法动机
在机器人控制、自动驾驶和无人机导航等领域，系统往往需要在不确定和动态变化的环境中实现高精度、鲁棒性的轨迹跟踪。传统控制方法如 PID 控制或基于模型的预测控制（MPC），虽然在许多场景中表现良好，但它们通常依赖于精确的系统模型和梯度信息。当系统模型复杂或存在显著不确定性时，这些方法的性能可能不稳定。此外，传统优化方法在实时性要求高的场景中可能面临计算瓶颈，特别是面对非凸问题难以在有限时间内找到全局最优解。
**模型预测路径积分控制（Model Predictive Path Integral, MPPI）**正是在这样的背景下应运而生的一种控制策略。它属于随机采样模型预测控制方法，通过大量采样来近似系统的随机动态，从而在不需要梯度信息的情况下处理非线性、非高斯噪声系统。MPPI 的核心优势在于其能够通过并行采样和计算高效地处理高维状态空间，并在实时控制中实现鲁棒性。因此，MPPI 为现代无人系统的智能控制提供了一种新的解决思路，弥补了传统方法依赖于梯度信息导致的灵活性不足。
2 MPPI 算法原理
MPPI 的数学基础建立在随机最优控制理论框架之上。考虑一个离散时间的动态系统，其状态转移由非线性函数 $\mathbf{F}$ 描述，即
$$ \mathbf{x}_{t+1} = \mathbf{F}(\mathbf{x}_t, \mathbf{v}_t) $$
其中 $\mathbf{x}_t \in \mathbb{R}^n$ 是系统状态，$\mathbf{v}_t \in \mathbb{R}^m$ 是控制输入。控制目标是最小化从初始状态 $\mathbf{x}_0$ 出发的期望累积成本，包括终端成本 $\phi(\mathbf{x}_T)$ 和运行成本 $c(\mathbf{x}_t)$。
MPPI 通过采样随机输入序列来近似期望成本。首先，定义均值控制序列
$$ U = (\mathbf{u}_0, \mathbf{u}1, ..., \mathbf{u}{T-1}) $$
通常使用上一时刻的最优序列作为初始值。然后，围绕 $U$ 进行随机采样，生成 $K$ 个输入序列
$$ V_k = (\mathbf{v}_0^k, \mathbf{v}1^k, ..., \mathbf{v}{T-1}^k) $$
其中 $\mathbf{v}_t^k = \mathbf{u}_t + \epsilon_t^k$，$\epsilon_t^k \sim \mathcal{N}(0, \Sigma)$ 是服从零均值高斯分布的噪声。这些采样序列通过系统模型进行前向模拟，得到对应的状态轨迹
$$ \mathcal{H}(V_k; \mathbf{x}_0) = (\mathbf{x}_0, \mathbf{x}_1^k, ..., \mathbf{x}_T^k) $$
并计算每条轨迹的成本
$$ S(V_k; \mathbf{x}_0) = \phi(\mathbf{x}T^k) + \sum{t=0}^{T-1} c(\mathbf{x}_t^k) $$
接下来，利用采样 MPC 理论将期望最优控制输入表示为加权平均。为每个采样序列分配权重
$$ w(V_k) = \frac{\exp(-\frac{1}{\lambda} S(V_k; \mathbf{x}0))}{\sum{j=1}^K \exp(-\frac{1}{\lambda} S(V_j; \mathbf{x}_0))} $$
其中 $\lambda > 0$ 是温度参数，用于调节权重对成本差异的敏感度。这种指数加权机制确保了低成本轨迹获得更高权重，从而引导控制策略向更优方向更新。最终，更新后的最优控制序列由
$$ \mathbf{u}_t^{i+1} = \mathbf{u}t^i + \sum{k=1}^K w(V_k) \epsilon_t^k $$
给出，即当前均值加上加权噪声扰动。这一更新规则无需梯度计算，而是通过采样直接估计控制输入的分布偏移，从而实现了在随机环境中的高效优化。
3 算法仿真
3.1 ROS C++ 仿真
以下是 MPPI 控制的核心代码：
bool MPPIController::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) {
if (!initialized_) {
R_ERROR << "MPPI Controller has not been initialized";
return false;
}
;
MPPISampledControlSequence sampled_control_seq;
MPPISampledStateSequence sampled_state_seq;
(curr_state, prev_control_seq_, &sampled_control_seq, &sampled_state_seq);
Eigen::ArrayXd costs;
& path_valid_flags = (prune_plan);
;
(cost_engine_ptr_->(mppi_config_.(), cost_feature, sampled_state_seq, &costs)) {
& curr_control_seq = (costs, prev_control_seq_, sampled_control_seq);
& optimized_trajectory = (curr_state, curr_control_seq);
cmd_vel.linear.x = curr_control_seq.();
cmd_vel.angular.z = curr_control_seq.();
prev_control_seq_ = curr_control_seq;
} {
cmd_vel.linear.x = ;
cmd_vel.angular.z = ;
R_WARN << ;
}
;
}
