MPPI算法实战：运动规划新利器

这是一个关于 基于采样的模型预测控制（Model Predictive Path Integral Control, MPPI） 算法在运动规划中的实战案例介绍。MPPI 是模型预测控制（MPC）的一种变体，特别适合处理高维、非线性、存在不确定性的系统控制问题，例如机器人导航、自动驾驶等。

1. MPPI 算法核心思想

MPPI 的核心在于 利用蒙特卡洛采样和路径积分思想来近似求解最优控制序列。它不像传统 MPC 那样依赖复杂的优化求解器，而是通过并行采样大量随机控制扰动序列，评估它们在预测时域内的代价，并加权平均得到最优控制输出。

关键公式： 对于一个离散时间系统，在时间步 $k$ ，MPPI 计算最优控制输入 $u_k\^*$ 的近似为： $$u_k^* \approx \frac{\sum_{m=1}^{M} \exp(-\frac{1}{\lambda} S(\tau^{(m)})) u_k^{(m)}}{\sum_{m=1}^{M} \exp(-\frac{1}{\lambda} S(\tau^{(m)}))}$$ 其中：
$M$ ：采样的轨迹数量。
$\\tau\^{(m)} = {u_k\^{(m)}, u_{k+1}\^{(m)}, \\dots, u_{k+T-1}\^{(m)}}$ ：第 $m$ 条随机扰动后的控制序列（长度为预测时域 $T$ ）。
$S(\\tau\^{(m)})$ ：第 $m$ 条轨迹 $\\tau\^{(m)}$ 对应的总代价（状态代价 + 控制代价）。
$\\lambda$ ：温度参数，控制权重分布的"尖锐"程度。

2. 算法步骤（伪代码描述）

python 复制代码

# MPPI 主循环 (每个控制周期执行一次)
def mppi_control(current_state, reference_trajectory, params):
    # 初始化
    total_weight = 0.0
    optimal_control = np.zeros(control_dim)
    
    # 1. 生成 M 条随机扰动控制序列 (基于上一个最优序列的均值)
    for m in range(M):
        # 生成噪声序列: epsilon ~ N(0, Sigma)
        noise_sequence = generate_noise_sequence(T, params['Sigma'])
        # 构建扰动控制序列: U_m = U_prev_mean + K * epsilon (K 通常为对角阵)
        perturbed_sequence = previous_optimal_sequence_mean + params['K'] * noise_sequence
        
        # 2. 前向模拟: 使用系统动力学模型预测状态轨迹 X_m
        state_trajectory = simulate_system(current_state, perturbed_sequence, system_model)
        
        # 3. 计算轨迹代价 S(τ_m)
        cost = calculate_trajectory_cost(state_trajectory, reference_trajectory, perturbed_sequence)
        # 计算权重: w_m = exp(-(S(τ_m) - min_cost) / lambda) (数值稳定技巧)
        weight = np.exp(-(cost - min_cost_observed) / params['lambda'])
        
        # 4. 累积加权控制
        optimal_control += weight * perturbed_sequence[0]  # 只取第一个控制用于输出
        total_weight += weight
    
    # 5. 计算加权平均控制输出
    optimal_control /= total_weight
    
    # 6. 更新: 将本次最优序列作为下次的均值序列 (滚动优化)
    previous_optimal_sequence_mean = roll_sequence(previous_optimal_sequence_mean, optimal_control)
    
    return optimal_control

3. ROS C++/Python 仿真实现要点

3.1 系统模型

需要定义系统动力学模型 $x_{k+1} = f(x_k, u_k)$ 。
C++ Example (Differential Drive Robot):

cpp 复制代码

void dynamics(const State& x, const Control& u, State& x_next, double dt) {
    // x = [x, y, theta], u = [v, w]
    x_next[0] = x[0] + u[0] * std::cos(x[2]) * dt; // x
    x_next[1] = x[1] + u[0] * std::sin(x[2]) * dt; // y
    x_next[2] = x[2] + u[1] * dt;                 // theta
}

3.2 代价函数

包含状态跟踪误差（如与参考轨迹的距离、角度差）和控制代价（如加速度、加加速度惩罚）。
Python Example:

python 复制代码

def trajectory_cost(states, ref_traj, controls):
    state_cost = 0.0
    for i, (x, ref) in enumerate(zip(states, ref_traj)):
        state_cost += (x[0] - ref[0])**2 + (x[1] - ref[1])**2 + 0.1 * (x[2] - ref[2])**2
    control_cost = 0.001 * np.sum(np.square(controls))  # 控制平滑性
    return state_cost + control_cost

3.3 噪声生成

使用高斯分布生成控制扰动 $\\epsilon_k \\sim \\mathcal{N}(0, \\Sigma)$ 。
C++ Example (Eigen):

cpp 复制代码

#include <random>
#include <Eigen/Dense>
Eigen::VectorXd generate_noise(int dim, const Eigen::MatrixXd& cov) {
    static std::default_random_engine generator;
    static std::normal_distribution<double> dist(0.0, 1.0);
    Eigen::VectorXd noise(dim);
    Eigen::MatrixXd L = cov.llt().matrixL(); // Cholesky 分解
    for (int i = 0; i < dim; ++i) noise(i) = dist(generator);
    return L * noise;
}

3.4 并行化加速

MPPI 天然适合并行计算（每个采样轨迹相互独立）。
ROS 实现建议:
- C++：使用 std::thread 或 OpenMP。
- Python：使用 multiprocessing 或 concurrent.futures。

4. 仿真效果与调参

效果: MPPI 能处理动态障碍物避让、非完整约束（如差速机器人）、路面附着系数变化等复杂场景。
关键参数:
- $M$ (采样数)：平衡计算效率与精度（通常 1000~5000）。
- $\\lambda$ (温度参数)：影响探索与利用的权衡。
- $\\Sigma$ (噪声协方差)：控制扰动幅度。
- $T$ (预测时域)：影响计算负载与前瞻性。

5. 话题延伸

计算优化: 重要性采样、GPU 加速。
结合学习: 用神经网络学习动力学模型或代价函数。
硬件部署: 在嵌入式系统（如 Jetson）上的实时性挑战。

希望这个实战案例能帮助你理解 MPPI 并动手实现！需要具体代码框架可以进一步讨论。