MPPI各评价器解析

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

---

前言

提示：这里可以添加本文要记录的大概内容：

评价器（critics）是MPPI 算法的核心部分，各个评价器对最终生成路径起到了不同的作用，本文以此介绍各个评价器的作用及其实现原理。

---

提示：以下是本篇文章正文内容，下面案例可供参考

一、Constraint Critic

1.1 简介

根据限速进行约束计算。

1.2 实现原理

比较计算得到的速度跟设置的机器人各个维度的限速，进行约束计算。不超过限速则惩罚得分为0，否则按照一定的权重进行加权计算。

1.3 核心代码介绍

void ConstraintCritic::score(CriticData & data)
{
  using xt::evaluation_strategy::immediate;

  if (!enabled_) {
    return;
  }

  auto sgn = xt::where(data.state.vx > 0.0, 1.0, -1.0);
  //计算预测速度
  auto vel_total = sgn * xt::sqrt(data.state.vx * data.state.vx + data.state.vy * data.state.vy);
  //计算与设置极致差值
  auto out_of_max_bounds_motion = xt::maximum(vel_total - max_vel_, 0);
  auto out_of_min_bounds_motion = xt::maximum(min_vel_ - vel_total, 0);
  
  ...
  //惩罚加权
  data.costs += xt::pow(
    xt::sum(
      (std::move(out_of_max_bounds_motion) +
      std::move(out_of_min_bounds_motion)) *
      data.model_dt, {1}, immediate) * weight_, power_);
}

---

二、Goal Angle Critic

2.1 简介

根据与目标朝向的夹角进行约束计算。

2.2 实现原理

靠近目标点时将启用该模块，比较计算轨迹方向夹角与目标点夹角，进行约束计算。按照一定的权重进行加权计算。

2.3 核心代码介绍

void GoalAngleCritic::score(CriticData & data)
{
  if (!enabled_ || !utils::withinPositionGoalTolerance(
      threshold_to_consider_, data.state.pose.pose, data.path))
  {//未启用该模块或者离目标较远则直接返回
    return;
  }
  //目标朝向计算
  const auto goal_idx = data.path.x.shape(0) - 1;
  const float goal_yaw = data.path.yaws(goal_idx);
  //惩罚加权
  data.costs += xt::pow(
    xt::mean(xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) *
    weight_, power_);
}

---

三、Goal Critic

3.1 简介

根据与目标点的距离进行约束计算。

3.2 实现原理

靠近目标点时将启用该模块，比较计算轨迹预测点与目标点距离，进行约束计算。按照一定的权重进行加权计算。

3.3 核心代码介绍

void GoalCritic::score(CriticData & data)
{
  if (!enabled_ || !utils::withinPositionGoalTolerance(
      threshold_to_consider_, data.state.pose.pose, data.path))
  {//未启用该模块或者离目标较远则直接返回
    return;
  }
  const auto goal_idx = data.path.x.shape(0) - 1;
  const auto goal_x = data.path.x(goal_idx);
  const auto goal_y = data.path.y(goal_idx);
  const auto traj_x = xt::view(data.trajectories.x, xt::all(), xt::all());
  const auto traj_y = xt::view(data.trajectories.y, xt::all(), xt::all());
  //目标距离计算
  auto dists = xt::sqrt(
    xt::pow(traj_x - goal_x, 2) +
    xt::pow(traj_y - goal_y, 2));
  //惩罚加权
  data.costs += xt::pow(xt::mean(dists, {1}, immediate) * weight_, power_);
}

---

四、Obstacles Critic

4.1 简介

根据与障碍物的距离进行约束计算。

4.2 实现原理

该模块计算两种权重的代价，一种是用于计算距离障碍物很近的时候，其代价权重比例为critical_weight，值一般设置较大；另一种是计算膨胀层内的代价以允许通过动态场景的路径对齐对障碍物行为进行微调，其权重是repulsion_weight，值一般设置较小。最终计算预测轨迹点与障碍物距离，从而根据两种权重代价叠加，进行约束计算。注意靠近目标点时不叠加repulsive_cost，避免难以到达目标点。

4.3 核心代码介绍

void ObstaclesCritic::score(CriticData & data)
{
  using xt::evaluation_strategy::immediate;
  if (!enabled_) {
    return;
  }

  if (consider_footprint_) {
    // footprint may have changed since initialization if user has dynamic footprints
    possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
  }

  // 靠近目标时，不使用第二种代价
  bool near_goal = false;
  if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) {
    near_goal = true;
  }
	//初始化代价值
  auto && raw_cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
  raw_cost.fill(0.0f);
  auto && repulsive_cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
  repulsive_cost.fill(0.0f);
  
  const size_t traj_len = data.trajectories.x.shape(1);
  bool all_trajectories_collide = true;
  //遍历所有轨迹点
  for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) {
    bool trajectory_collide = false;
    float traj_cost = 0.0f;
    const auto & traj = data.trajectories;
    CollisionCost pose_cost;

    for (size_t j = 0; j < traj_len; j++) {
      pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j));
      if (pose_cost.cost < 1.0f) {continue;}  // In free space

      if (inCollision(pose_cost.cost)) {
        trajectory_collide = true;
        break;
      }

      // Cannot process repulsion if inflation layer does not exist
      if (inflation_radius_ == 0.0f || inflation_scale_factor_ == 0.0f) {
        continue;
      }

      const float dist_to_obj = distanceToObstacle(pose_cost);

      // 严惩近距离碰撞轨迹点
      if (dist_to_obj < collision_margin_distance_) {
        traj_cost += (collision_margin_distance_ - dist_to_obj);
      } else if (!near_goal) {  // 远离障碍物的轨迹代价更小
        repulsive_cost[i] += (inflation_radius_ - dist_to_obj);
      }
    }

    if (!trajectory_collide) {all_trajectories_collide = false;}
    raw_cost[i] = trajectory_collide ? collision_cost_ : traj_cost;
  }
  //综合计算两种代价
  data.costs += xt::pow(
    (critical_weight_ * raw_cost) +
    (repulsion_weight_ * repulsive_cost / traj_len),
    power_);
  data.fail_flag = all_trajectories_collide;
}

---

总结

以上是官方目前提供的一些评价函数，可根据实际情况自己添加评价模块已达到更好效果。