MPPI算法实战：机器人避障与仿真

运动规划实战案例 | 基于采样的MPC控制(MPPI)算法(附ROS C++/Python仿真)

在运动规划领域，基于采样的模型预测控制（MPC）方法如MPPI（Model Predictive Path Integral）算法，能高效处理不确定环境下的机器人导航问题。MPPI通过采样多条轨迹、评估成本并加权优化控制输入，实现实时规划。下面我将逐步解释算法原理、提供一个实战案例，并附上ROS C++和Python的仿真代码示例。

1. MPPI算法原理

MPPI算法是一种基于采样的随机控制方法，适用于连续状态空间。核心思想是：在预测时域内，生成多条随机轨迹，计算每条轨迹的成本函数，然后通过加权平均更新控制输入。

• 轨迹采样：假设系统状态为x_t，控制输入为u_t。在时间步k时，采样N条轨迹\\tau_i（i=1,2,\\dots,N），每条轨迹包含T步预测。采样基于当前控制序列u_k添加高斯噪声： $$u_k^{(i)} = u_k + \delta u_k^{(i)}$$ 其中\\delta u_k\^{(i)} \\sim \\mathcal{N}(0, \\Sigma)，\\Sigma是协方差矩阵。

• 成本函数：每条轨迹\\tau_i的成本S(\\tau_i)包括状态误差和控制惩罚。例如，目标跟踪问题中： $$S(\tau_i) = \sum_{t=k}^{k+T-1} \left( |x_t - x_{\text{goal}}|^2 + \lambda |u_t|^2 \right)$$ 这里，x_{\\text{goal}}是目标状态，\\lambda是正则化参数。

• 控制更新：基于成本权重w_i，计算加权平均控制： $$w_i = \exp\left(-\frac{1}{\eta} S(\tau_i)\right)$$ 其中\\eta是温度参数。最终控制输入更新为： $$u_k^* = \frac{\sum_{i=1}^N w_i u_k^{(i)}}{\sum_{i=1}^N w_i}$$ 这确保低成本轨迹有更高权重。

MPPI的优势在于能处理非线性系统和噪声，计算效率高，适合实时应用如机器人导航。

2. 实战案例：机器人障碍物避障

考虑一个实战场景：移动机器人在室内环境中从起点导航到目标点，同时避开动态障碍物。环境包括静态墙壁和移动障碍物（如行人）。

• 问题设置：

  • 状态x_t = \[p_x, p_y, \\theta\]：位置(p_x, p_y)和朝向\\theta。
  • 控制输入u_t = \[v, \\omega\]：线速度v和角速度\\omega。
  • 成本函数S(\\tau_i)：包括目标距离、障碍物距离惩罚和控制平滑度： $$S(\tau_i) = \sum_{t=0}^{T-1} \left( \alpha |p_t - p_{\text{goal}}|^2 + \beta \exp\left(-\gamma d_{\text{obs}}(p_t)\right) + \rho |u_t|^2 \right)$$ 其中d_{\\text{obs}}(p_t)是到最近障碍物的距离，\\alpha, \\beta, \\gamma, \\rho是权重参数。

• 仿真结果：在ROS Gazebo仿真中，机器人成功避开障碍物并到达目标。MPPI的采样策略（如N=1000）能快速响应环境变化。

3. 代码仿真示例

以下提供MPPI算法的核心实现代码，支持ROS（C++）和Python仿真。代码包括轨迹采样、成本计算和控制更新。

Python仿真代码

这是一个简化的Python实现，用于2D机器人导航。依赖NumPy。

import numpy as np

class MPPIController:
    def __init__(self, T=10, N=500, lambda_=0.1, eta=1.0, noise_std=0.1):
        self.T = T  # 预测时域
        self.N = N  # 采样数
        self.lambda_ = lambda_  # 控制正则化参数
        self.eta = eta  # 温度参数
        self.noise_std = noise_std  # 噪声标准差
        self.u_prev = np.zeros((T, 2))  # 初始化控制序列 (v, omega)

    def cost_function(self, trajectory, goal, obstacles):
        """计算轨迹成本 S(tau)"""
        cost = 0
        for t in range(self.T):
            pos = trajectory[t, :2]  # 位置 (px, py)
            # 目标距离成本
            cost += np.linalg.norm(pos - goal) ** 2
            # 障碍物惩罚 (指数形式)
            min_obs_dist = min(np.linalg.norm(pos - obs) for obs in obstacles)
            cost += 10 * np.exp(-0.5 * min_obs_dist)
            # 控制正则化
            if t > 0:
                cost += self.lambda_ * np.linalg.norm(trajectory[t, 2:]) ** 2  # 控制输入 (v, omega)
        return cost

    def update_control(self, current_state, goal, obstacles):
        """MPPI控制更新"""
        # 采样N条轨迹
        trajectories = []
        costs = np.zeros(self.N)
        for i in range(self.N):
            # 添加噪声到控制序列
            noise = np.random.normal(0, self.noise_std, (self.T, 2))
            u_seq = self.u_prev + noise
            # 模拟轨迹 (简化动力学: 差速驱动机器人)
            traj = [current_state.copy()]
            state = current_state.copy()
            for t in range(self.T):
                v, omega = u_seq[t]
                # 状态更新: x_{t+1} = x_t + v * dt * cos(theta), 类似
                state[0] += v * np.cos(state[2]) * 0.1  # dt=0.1s
                state[1] += v * np.sin(state[2]) * 0.1
                state[2] += omega * 0.1
                traj.append(state.copy())
            trajectories.append(np.array(traj))
            costs[i] = self.cost_function(np.array(traj)[1:], goal, obstacles)  # 计算成本
        
        # 计算权重并更新控制
        weights = np.exp(-costs / self.eta)
        sum_weights = np.sum(weights)
        if sum_weights > 0:
            u_new = np.zeros((self.T, 2))
            for i in range(self.N):
                u_new += weights[i] * (self.u_prev + noise)  # 加权平均
            u_new /= sum_weights
            self.u_prev = u_new
        
        return self.u_prev[0]  # 返回当前控制输入

# 示例使用
if __name__ == "__main__":
    controller = MPPIController()
    current_state = np.array([0, 0, 0])  # [px, py, theta]
    goal = np.array([5, 5])  # 目标位置
    obstacles = [np.array([2, 2]), np.array([3, 4])]  # 障碍物位置列表
    
    # 仿真循环
    for _ in range(100):
        u = controller.update_control(current_state, goal, obstacles)
        # 应用控制并更新状态 (这里简化)
        print(f"控制输入: v={u[0]:.2f}, omega={u[1]:.2f}")

ROS C++仿真代码

在ROS中，MPPI可集成到导航包中。以下是基于ROS Noetic的C++示例，使用ros_control和nav_msgs。

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/Twist.h>
#include <random>

class MPPIController {
public:
    MPPIController(ros::NodeHandle& nh, int T=10, int N=500, double lambda=0.1, double eta=1.0, double noise_std=0.1)
        : T_(T), N_(N), lambda_(lambda), eta_(eta), noise_std_(noise_std) {
        u_prev_ = Eigen::MatrixXd::Zero(T_, 2); // 控制序列初始化
        cmd_vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 10);
    }

    double costFunction(const Eigen::MatrixXd& trajectory, const Eigen::Vector2d& goal, const std::vector<Eigen::Vector2d>& obstacles) {
        double cost = 0;
        for (int t = 0; t < T_; ++t) {
            Eigen::Vector2d pos = trajectory.block(t, 0, 1, 2).transpose();
            // 目标成本
            cost += (pos - goal).squaredNorm();
            // 障碍物成本
            double min_obs_dist = 1e6;
            for (const auto& obs : obstacles) {
                double dist = (pos - obs).norm();
                if (dist < min_obs_dist) min_obs_dist = dist;
            }
            cost += 10 * exp(-0.5 * min_obs_dist);
            // 控制正则化
            if (t > 0) {
                Eigen::Vector2d u(trajectory(t, 2), trajectory(t, 3)); // 假设轨迹包括控制
                cost += lambda_ * u.squaredNorm();
            }
        }
        return cost;
    }

    void updateControl(const Eigen::Vector3d& current_state, const Eigen::Vector2d& goal, const std::vector<Eigen::Vector2d>& obstacles) {
        std::vector<Eigen::MatrixXd> trajectories;
        Eigen::VectorXd costs = Eigen::VectorXd::Zero(N_);
        std::default_random_engine generator;
        std::normal_distribution<double> noise_dist(0.0, noise_std_);

        for (int i = 0; i < N_; ++i) {
            Eigen::MatrixXd u_seq = u_prev_;
            // 添加噪声
            for (int t = 0; t < T_; ++t) {
                u_seq(t, 0) += noise_dist(generator); // v
                u_seq(t, 1) += noise_dist(generator); // omega
            }
            // 模拟轨迹 (简化模型)
            Eigen::MatrixXd traj(T_, 5); // [px, py, theta, v, omega]
            traj.row(0) << current_state[0], current_state[1], current_state[2], u_seq(0,0), u_seq(0,1);
            for (int t = 1; t < T_; ++t) {
                double v = u_seq(t-1, 0);
                double omega = u_seq(t-1, 1);
                double theta = traj(t-1, 2);
                traj(t, 0) = traj(t-1, 0) + v * cos(theta) * 0.1; // px
                traj(t, 1) = traj(t-1, 1) + v * sin(theta) * 0.1; // py
                traj(t, 2) = theta + omega * 0.1; // theta
                traj(t, 3) = u_seq(t, 0); // v
                traj(t, 4) = u_seq(t, 1); // omega
            }
            trajectories.push_back(traj);
            costs[i] = costFunction(traj.block(0, 0, T_, 3), goal, obstacles); // 只取状态部分
        }

        // 计算权重
        Eigen::VectorXd weights = (-costs / eta_).array().exp();
        double sum_weights = weights.sum();
        if (sum_weights > 0) {
            Eigen::MatrixXd u_new = Eigen::MatrixXd::Zero(T_, 2);
            for (int i = 0; i < N_; ++i) {
                u_new += weights[i] * trajectories[i].block(0, 3, T_, 2); // 加权控制部分
            }
            u_new /= sum_weights;
            u_prev_ = u_new;
        }

        // 发布控制命令
        geometry_msgs::Twist cmd_vel;
        cmd_vel.linear.x = u_prev_(0, 0);
        cmd_vel.angular.z = u_prev_(0, 1);
        cmd_vel_pub_.publish(cmd_vel);
    }

private:
    int T_, N_;
    double lambda_, eta_, noise_std_;
    Eigen::MatrixXd u_prev_;
    ros::Publisher cmd_vel_pub_;
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "mppi_controller");
    ros::NodeHandle nh;
    MPPIController controller(nh);
    // 在回调或循环中调用updateControl，例如基于传感器更新状态
    ros::spin();
    return 0;
}

4. 算法优势与挑战

• 优势：MPPI处理非线性和噪声能力强，计算并行化（适合GPU加速），实时性好。
• 挑战：参数调优（如\\eta和\\lambda）敏感；高维状态空间采样效率低；需要精确动力学模型。
• 改进方向 ：结合深度学习方法减少采样数；集成到ROS导航栈如move_base。

通过以上案例和代码，您可以在ROS或Python环境中实现MPPI算法，用于机器人运动规划仿真。实践中，建议从简单场景开始，逐步增加复杂度。