MPPI控制速度生成过程解析

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

前面文章对MPPI算法原理做了简要介绍，本文对其核心功能控制速度生成过程代码做一个介绍。

---

一、主要变量介绍

1.1 models::State

• 作用：用于记录优化过程中状态，包括velocities, controls, poses, speed

结构体定义如下：

struct State
{
  xt::xtensor<float, 2> vx;
  xt::xtensor<float, 2> vy;
  xt::xtensor<float, 2> wz;

  xt::xtensor<float, 2> cvx;
  xt::xtensor<float, 2> cvy;
  xt::xtensor<float, 2> cwz;

  geometry_msgs::msg::PoseStamped pose;
  geometry_msgs::msg::Twist speed;

  /**
    * @brief Reset state data
    */
  void reset(unsigned int batch_size, unsigned int time_steps)
  {
    vx = xt::zeros<float>({batch_size, time_steps});
    vy = xt::zeros<float>({batch_size, time_steps});
    wz = xt::zeros<float>({batch_size, time_steps});

    cvx = xt::zeros<float>({batch_size, time_steps});
    cvy = xt::zeros<float>({batch_size, time_steps});
    cwz = xt::zeros<float>({batch_size, time_steps});
  }
};

1.2 models::ControlSequence

• 作用：用于保持一段时间内的预测控制速度序列

结构体定义如下：

struct ControlSequence
{
  xt::xtensor<float, 1> vx;
  xt::xtensor<float, 1> vy;
  xt::xtensor<float, 1> wz;

  void reset(unsigned int time_steps)
  {
    vx = xt::zeros<float>({time_steps});
    vy = xt::zeros<float>({time_steps});
    wz = xt::zeros<float>({time_steps});
  }
};

1.3 models::Trajectories

• 作用：用于保持一段时间内的候选轨迹

结构体定义如下：

struct Trajectories
{
  xt::xtensor<float, 2> x;
  xt::xtensor<float, 2> y;
  xt::xtensor<float, 2> yaws;

  /**
    * @brief Reset state data
    */
  void reset(unsigned int batch_size, unsigned int time_steps)
  {
    x = xt::zeros<float>({batch_size, time_steps});
    y = xt::zeros<float>({batch_size, time_steps});
    yaws = xt::zeros<float>({batch_size, time_steps});
  }
};

二、主函数介绍

• 作用：主函数，更新状态及生成当前控制速度

  /*
   * param in:  robot_pose -- 机器人当前位姿
   * param in:  robot_speed -- 机器人当前速度
   * param in:  plan -- 全局路径
   * param in:  goal_checker -- 目标是否到达判断器
   * return：control -- 控制速度
   */
geometry_msgs::msg::TwistStamped Optimizer::evalControl(
  const geometry_msgs::msg::PoseStamped & robot_pose,
  const geometry_msgs::msg::Twist & robot_speed,
  const nav_msgs::msg::Path & plan, nav2_core::GoalChecker * goal_checker)
{
	//数据准备相关过程：
  //1 将机器人位置及速度信息赋值给模型状态变量（state_）
  //2 为评价器准备相关需要的数据
  prepare(robot_pose, robot_speed, plan, goal_checker);

  do {
    //进行优化
    optimize();
  } while (fallback(critics_data_.fail_flag));
  //使用Savisky-Golay滤波器对控制序列进行平滑滤波
  utils::savitskyGolayFilter(control_sequence_, control_history_, settings_);
  //从序列中获取控制输出
  auto control = getControlFromSequenceAsTwist(plan.header.stamp);

  if (settings_.shift_control_sequence) {
    //前移控制序列一个step
    shiftControlSequence();
  }

  return control;
}

三、optimize分析

• 作用：生成轨迹优化轨迹生成控制序列等

3.1 主过程分析

//跟进设置迭代次数，循环迭代优化
void Optimizer::optimize()
{
  for (size_t i = 0; i < settings_.iteration_count; ++i) {
    //生成随机轨迹
    generateNoisedTrajectories();
    //轨迹评价得分
    critic_manager_.evalTrajectoriesScores(critics_data_);
    //更新控制序列
    updateControlSequence();
  }
}

3.2 轨迹生成

• 作用：根据当前状态以及噪声生成轨迹

void Optimizer::generateNoisedTrajectories()
{
  //根据轨迹序列及随机噪声生成新的控制序列state.cvx(state.cvy state.cvz同理)
  noise_generator_.setNoisedControls(state_, control_sequence_);
  //再随机生成噪声供下次使用（regenerate_noises设置为false时该步骤不起效）
  noise_generator_.generateNextNoises();
  //更新当前速度至state.vx第0列，更新state.cvx第0列至倒数第2列至state.vx
  updateStateVelocities(state_);
  //根据状态state_更新轨迹generated_trajectories_
  integrateStateVelocities(generated_trajectories_, state_);
}

3.3 轨迹评分

• 作用：使用各个评价器对生成轨迹进行评分

void CriticManager::evalTrajectoriesScores(
  CriticData & data) const
{
  for (size_t q = 0; q < critics_.size(); q++) {
    if (data.fail_flag) {
      break;
    }
    critics_[q]->score(data);
  }
}

3.4 更新控制序列

• 作用：根据评分进行控制序列更新

void Optimizer::updateControlSequence()
{
  ...
  //从state_.cvx中选取最大评分速度控制值赋值给control_sequence_.vx
  xt::noalias(control_sequence_.vx) = xt::sum(state_.cvx * softmaxes_extened, 0, immediate);
  xt::noalias(control_sequence_.wz) = xt::sum(state_.cwz * softmaxes_extened, 0, immediate);
  if (isHolonomic()) {
    xt::noalias(control_sequence_.vy) = xt::sum(state_.cvy * softmaxes_extened, 0, immediate);
  }
  //进行容忍度限制
  applyControlSequenceConstraints();
}

---

总结

以上就是今天要讲的内容，本文就各人理解对MPPI代码做了一次简要解析，由于能力有限可能存在不足之处望见谅，更希望能指出问题便于改进。