自动驾驶时空联合规划：EPSILON: An Efficient Planning System for Automated Vehicles in Highly Interactive Env-CSDN博客

本文链接：https://blog.csdn.net/a8598671/article/details/147554553

代码地址：https://github.com/HKUST-Aerial-Robotics/EPSILON

1. 摘要

论文提出了一种名为EPSILON的高效自动驾驶规划系统，用于在高度交互的环境中操作车辆。EPSILON是一个具备交互感知能力的高效规划系统，已经在仿真和真实世界密集城市交通中进行了广泛验证。系统采用分层结构，包括交互式行为规划层和基于优化的运动规划层。行为规划采用部分可观测马尔可夫决策过程（POMDP）进行建模，但比直接使用POMDP更高效，其关键在于对动作空间和观测空间的引导分支，将原问题分解成少量闭环策略评估问题。此外，提出了一种具有安全机制的新型驾驶员模型，以克服先验知识不完美带来的风险。运动规划方面，提出了时空语义走廊（SSC）以统一建模复杂驾驶环境的约束条件，并在此基础上优化出安全、平滑的轨迹，遵循行为规划层的决策。实验证明，EPSILON在密集交通流中能实现接近人类的自然驾驶行为，既平滑又安全，且相较于现有方法不过于保守。

2. 方法

EPSILON规划系统的整体结构如图所示，整个规划系统包括环境理解模块、行为规划和运动规划三个模块。感知模块包括了自由空间检测、语义检测（如车道线检测、交通灯检测等）以及目标检测与跟踪。感知模块的输出被同步传递给一个语义地图管理器，该模块负责组织各种数据结构，并向规划模块提供查询接口，语义地图以20Hz频率更新。

行为规划层主要进行上层语义级规划，EPSILON不需要一个单独的轨迹预测模块，轨迹预测功能已融合进了行为规划器内部。行为规划器主要包括三个过程：引导式分支（Guided Branching）：根据预定义的策略扩展自车的动作序列，同时推理其他交通参与者的可能意图。场景实现（Scenario Realization）：将一个具体的场景通过多智能体闭环仿真逐步实现。策略评估（Policy Evaluation）：评估不同场景下的各个策略，选择最优决策，行为规划层的工作频率为20Hz。

运动规划层对行为规划层的输出轨迹进行优化。将静态障碍物、动态障碍物以及环境语义元素施加的约束统一建模为一个时空语义通道（Spatio-Temporal Semantic Corridor, SSC），以行为层提供的初步轨迹为参考初值。然后，采用分段贝塞尔曲线（piecewise Bézier curves）对轨迹进行优化，运动规划层的工作频率为20Hz。

2.1 高效行为规划

人类驾驶员在决策时，并不会在心中建立一个细粒度的栅格地图。相反，人类通常根据通用驾驶知识，只会考虑少数几个长期的语义级别动作（semantic-level actions），比如：保持当前车道（lane keeping）、变道（lane change）、让行（yield）等等，这种决策方式使得人类的决策非常高效。此外，在POMDP框架下，另一个导致效率低下的问题是：其他智能体的意图（intentions）是不可观测的，需要在观测空间（observation space）上进行采样和分支。但问题在于，随着周围交通参与者数量的增加，意图组合数量呈指数级增长，计算代价极高。

2.2.1 自车行为策略树（DCP-Tree：Domain-Specific Closed-loop Policy Tree）

为了解决动作空间过大的问题，引入了语义级动作（Semantic-Level Actions）进行引导式规划。这些动作具有如下特点：直接对应人类可感知的驾驶操作（如加速、变道）；每个语义动作可以封装为一个小型闭环控制器（predefined controller）；可以在执行时逐步生成具体的原始控制量（如方向盘转角、加速度）。在引导式规划中，仅自车的语义动作是可控的，其他车辆的动作由驾驶员模型推断。为了进一步提高效率，论文设计了领域特定的闭环策略树DCP-Tree策略如下：

每次规划周期，从当前正在执行的动作（ongoing action）出发；
允许在规划周期内发生最多一次动作切换（one action change）；
每个动作序列表示一个完整的策略；
整个动作树的规模随树深度线性增长，而不是指数增长。

本部分内容开源代码如下：纵向语义包括三个：加速、减速和匀速。横向语义包括三个：车道保持、左换道和右换道。动作序列总共有5层，每层1s，总规划时长5s。横向允许一次动作改变，如从保持车道切到左车道或右车道，如果换了车道，后面动作就一直保持新车道，不再切换。所以其动作序列总共有九种。每一个分支整个时间序列保持相同的纵向语义，一共有9*3=27条轨迹。如果初始意图是正在左换道，不允许直接从左换道到右换道，轨迹一共就有5*3=15条轨迹。

保持车道 + 保持车道 + 保持车道 + 保持车道 + 保持车道
换到左车道 + 保持左车道 + 保持左车道 + 保持左车道 + 保持左车道
换到右车道 + 保持右车道 + 保持右车道 + 保持右车道 + 保持右车道
保持车道 + 换到左车道 + 保持左车道 + 保持左车道 + 保持左车道
保持车道 + 换到右车道 + 保持右车道 + 保持右车道 + 保持右车道
保持车道 + 保持车道 + 换到左车道 + 保持左车道 + 保持左车道
保持车道 + 保持车道 + 换到右车道 + 保持右车道 + 保持右车道
保持车道 + 保持车道 + 保持车道 + 换到左车道 + 保持左车道
保持车道 + 保持车道 + 保持车道 + 换到右车道 + 保持右车道

  enum class DcpLonAction {
    kMaintain = 0,
    kAccelerate,
    kDecelerate,
    MAX_COUNT = 3
  };

  enum class DcpLatAction {
    kLaneKeeping = 0,
    kLaneChangeLeft,
    kLaneChangeRight,
    MAX_COUNT = 3
  };

ErrorType DcpTree::GenerateActionScript() {
  action_script_.clear();
  std::vector<DcpAction> ongoing_action_seq;
  for (int lon = 0; lon < static_cast<int>(DcpLonAction::MAX_COUNT); lon++) {
    ongoing_action_seq.clear();
    ongoing_action_seq.push_back(
        DcpAction(DcpLonAction(lon), ongoing_action_.lat, ongoing_action_.t));

    for (int h = 1; h < tree_height_; ++h) {
      for (int lat = 0; lat < static_cast<int>(DcpLatAction::MAX_COUNT);
           lat++) {
        if (lat != static_cast<int>(ongoing_action_.lat)) {
          auto actions = AppendActionSequence(
              ongoing_action_seq,
              DcpAction(DcpLonAction(lon), DcpLatAction(lat), layer_time_),
              tree_height_ - h);
          action_script_.push_back(actions);
        }
      }
      ongoing_action_seq.push_back(
          DcpAction(DcpLonAction(lon), ongoing_action_.lat, layer_time_));
    }
    action_script_.push_back(ongoing_action_seq);
  }
  // override the last layer time
  for (auto& action_seq : action_script_) {
    action_seq.back().t = last_layer_time_;
  }
  return kSuccess;
}

2.1.2 他车目标筛选

为了在评估策略时减少观测空间的组合爆炸，论文提出了条件关注分支（Conditional Focused Branching, CFB）机制。其核心思想是：在策略评估时，只关注那些与自车动作相关联、且有潜在碰撞风险的其他车辆，在保证安全评估准确率的同时，大幅降低了场景数量，代码中这段目前没有进行筛选，可根据需求制定筛选逻辑。同时，根据规则或者数据驱动模型判断他车意图，例如车道保持还是换道，这个意图是在整个前向仿真前就确定的。代码中根据横向位置 (fs.vec_dt[0]) 和横向速度 (fs.vec_dt[1]) 简单规则判断：

偏右且速度向右 → 向右变道 (kLaneChangeRight)

偏左且速度向左 → 向左变道 (kLaneChangeLeft)

否则 → 保持车道 (kLaneKeeping)

ErrorType BehaviorPlannerMapAdapter::GetKeyVehicles(
    common::VehicleSet *key_vehicle_set) {
  if (!is_valid_) return kWrongStatus;
  // TODO: (@denny.ding) add vehicle selection strategy here
  *key_vehicle_set = map_->surrounding_vehicles();
  return kSuccess;
}

ErrorType SemanticMapManager::NaiveRuleBasedLateralBehaviorPrediction(
    const common::Vehicle &vehicle, const int nearest_lane_id,
    common::ProbDistOfLatBehaviors *lat_probs) {
  if (nearest_lane_id == kInvalidLaneId) {
    lat_probs->is_valid = false;
    return kWrongStatus;
  }

  SemanticLane nearest_lane =
      semantic_lane_set_.semantic_lanes.at(nearest_lane_id);
  common::StateTransformer stf(nearest_lane.lane);

  common::FrenetState fs;
  if (stf.GetFrenetStateFromState(vehicle.state(), &fs) != kSuccess) {
    lat_probs->is_valid = false;
    return kWrongStatus;
  }

  decimal_t prob_lcl = 0.0;
  decimal_t prob_lcr = 0.0;
  decimal_t prob_lk = 0.0;

  const decimal_t lat_distance_threshold = 0.4;
  const decimal_t lat_vel_threshold = 0.35;
  if (use_right_hand_axis_) {
    if (fs.vec_dt[0] > lat_distance_threshold &&
        fs.vec_dt[1] > lat_vel_threshold &&
        nearest_lane.l_lane_id != kInvalidLaneId &&
        nearest_lane.l_change_avbl) {
      prob_lcl = 1.0;
      printf(
          "[NaivePrediction]vehicle %d lane id %d, lat d %lf lat dd %lf, "
          "behavior "
          "lcl.\n",
          vehicle.id(), nearest_lane_id, fs.vec_dt[0], fs.vec_dt[1]);
    } else if (fs.vec_dt[0] < -lat_distance_threshold &&
               fs.vec_dt[1] < -lat_vel_threshold &&
               nearest_lane.r_lane_id != kInvalidLaneId &&
               nearest_lane.r_change_avbl) {
      prob_lcr = 1.0;
      printf(
          "[NaivePrediction]vehicle %d lane id %d, lat d %lf lat dd %lf, "
          "behavior "
          "lcr.\n",
          vehicle.id(), nearest_lane_id, fs.vec_dt[0], fs.vec_dt[1]);
    } else {
      prob_lk = 1.0;
    }
  } else {
    if (fs.vec_dt[0] > lat_distance_threshold &&
        fs.vec_dt[1] > lat_vel_threshold &&
        nearest_lane.r_lane_id != kInvalidLaneId &&
        nearest_lane.r_change_avbl) {
      prob_lcr = 1.0;
      printf(
          "[NaivePrediction]vehicle %d lane id %d, lat d %lf lat dd %lf, "
          "behavior "
          "lcr.\n",
          vehicle.id(), nearest_lane_id, fs.vec_dt[0], fs.vec_dt[1]);
    } else if (fs.vec_dt[0] < -lat_distance_threshold &&
               fs.vec_dt[1] < -lat_vel_threshold &&
               nearest_lane.l_lane_id != kInvalidLaneId &&
               nearest_lane.l_change_avbl) {
      prob_lcl = 1.0;
      printf(
          "[NaivePrediction]vehicle %d lane id %d, lat d %lf lat dd %lf, "
          "behavior "
          "lcl.\n",
          vehicle.id(), nearest_lane_id, fs.vec_dt[0], fs.vec_dt[1]);
    } else {
      prob_lk = 1.0;
    }
  }

  lat_probs->SetEntry(common::LateralBehavior::kLaneChangeLeft, prob_lcl);
  lat_probs->SetEntry(common::LateralBehavior::kLaneChangeRight, prob_lcr);
  lat_probs->SetEntry(common::LateralBehavior::kLaneKeeping, prob_lk);
  lat_probs->is_valid = true;

  return kSuccess;
}

2.1.3 多智能体闭环仿真

在每个场景下，执行多智能体闭环仿真（multi-agent closed-loop forward simulation），其特点是：所有智能体根据上一时刻的整体状态感知信息，做出控制决策并同步推进各自状态。不同于传统轨迹预测模块，论文将预测与规划深度耦合（deeply coupled），能够考虑自车未来动作对他车反应的影响，提升了交互建模精度。

闭环仿真采用多线程进行同步计算，根据自车行为策略树，针对每个分支进行仿真。推演的时候先推演自车，然后推演他车，推演步长为0.2s。仿真代码入口为SimulateActionSequence，调用SimulateScenario，进入SimulateSingleAction，自车调用EgoAgentForwardSim，最后在OnLaneForwardSimulation::PropagateOnceAdvancedLK() // 保持车道和OnLaneForwardSimulation::PropagateOnceAdvancedLC() // 换道时采用IDM和PP算法进行规划推演。他车则在进入SimulateSingleAction后调用SurroundingAgentForwardSim，之后进入OnLaneForwardSimulation::PropagateOnce，PropagateOnce调用IDM和PP算法进行他车规划推演，IDM和PP算法是控制算法，每次计算后得到控制量，然后按照运动学方程进行推演获得自车和他车轨迹。在每一个动作序列（前面策略树的动作序列，时间是1s，不是单步0.2s）执行完成后，系统会进行一次安全检查RSS check，对于不通过的分支进行舍弃。

 for (int i = 0; i < n_sequence; ++i) {
    thread_set[i] =
        std::thread(&EudmPlanner::SimulateActionSequence, this, ego_vehicle_,
                    surrounding_fsagents, action_script[i], i);
  }
  for (int i = 0; i < n_sequence; ++i) {
    thread_set[i].join();
  }

if (StrictSafetyCheck(ego_traj_multisteps, surround_trajs_multisteps,
                      &is_strictly_safe, &collided_id) != kSuccess) {
  (*sub_sim_res)[sub_seq_id] = 0;
  (*sub_sim_info)[sub_seq_id] += std::string("(Check F)");
  return kWrongStatus;
 }

代码中纵向规划采用IDM(Intelligent Driver Model, 智能驾驶模型) 模型，原始IDM模型主要用于模拟车辆在跟车时的加速度，其基本公式是通过考虑当前车辆与前方车辆之间的距离、速度差异、期望速度和舒适刹车等因素来计算加速度。原始IDM公式进入代码如下，论文中对原始IDM进行了改进，主要是车速超过目标车速时，原IDM会产生过大的减速度，另外原IDM在自车与目标车辆较近时跟车距离过远，修改的代码如下。

期望加速度公式：

$a = a_{\text{max}} \left( 1 - \left( \frac{v}{v_{\text{desired}}} \right)^\delta - \left( \frac{s^*(v)}{s_{\text{actual}}} \right)^2 \right)$

其中：

$a_{\text{max}}$ ：最大加速度
$v$ ：是当前车辆的速度
$v_{\text{desired}}$ ：期望车速
$\delta$ ：是速度对加速度的影响指数（通常大于 1，用来调节加速的曲线）。
$s_{\text{actual}}$ ：是当前车辆与前车之间的实际距离。
$s^*(v)$ 是理想的安全跟车距离，公式如下：

$s^*(v) = s_0 + v T + \frac{v (v - v_{\text{front}})}{2 \sqrt{a_{\text{max}} b_{\text{max}}}}$

$s_0$ 是最小车距；
$T$ 是时距（通常为 1.5s 到 2s）；
$v_{\text{front}}$ 是前车的速度；
$v$ 是当前车辆速度；
$a_{\text{max}}$ 和 $b_{\text{max}}$ 分别是最大加速度和最大舒适刹车加速度。

ErrorType IntelligentDriverModel::GetIdmDesiredAcceleration(
    const IntelligentDriverModel::Param &param,
    const IntelligentDriverModel::State &cur_state, decimal_t *acc) {
  decimal_t s_star =
      param.kMinimumSpacing +
      std::max(0.0,
               cur_state.v * param.kDesiredHeadwayTime +
                   cur_state.v * (cur_state.v - cur_state.v_front) /
                       (2.0 * sqrt(param.kAcceleration *
                                   param.kComfortableBrakingDeceleration)));
  decimal_t s_alpha =
      std::max(0.0, cur_state.s_front - cur_state.s - param.kVehicleLength);
  *acc = param.kAcceleration *
         (1.0 - pow(cur_state.v / param.kDesiredVelocity, param.kExponent) -
          pow(s_star / s_alpha, 2));
  return kSuccess;
}

ErrorType IntelligentDriverModel::GetIIdmDesiredAcceleration(
    const IntelligentDriverModel::Param &param,
    const IntelligentDriverModel::State &cur_state, decimal_t *acc) {
  // The Improved IntelligentDriverModel (IIDM) tries to address two
  // deficiencies of the original IDM model:
  // 1) If the actual speed exceeds the desired speed (e.g., after entering a
  // zone with a reduced speed limit), the deceleration is unrealistically
  // large, particularly for large values of the acceleration exponent δ.
  // 2) Near the desired speed v0, the steady-state gap becomes much
  // greater than s∗(v, 0) = s0 + vT so that the model parameter T loses its
  // meaning as the desired time gap. This means that a platoon of identical
  // drivers and vehicles disperses much more than observed. Moreover, not all
  // cars will reach the desired speed
  decimal_t a_free =
      cur_state.v <= param.kDesiredVelocity
          ? param.kAcceleration *
                (1 - pow(cur_state.v / param.kDesiredVelocity, param.kExponent))
          : -param.kComfortableBrakingDeceleration *
                (1 - pow(param.kDesiredVelocity / cur_state.v,
                         param.kAcceleration * param.kExponent /
                             param.kComfortableBrakingDeceleration));
  decimal_t s_alpha =
      std::max(0.0, cur_state.s_front - cur_state.s - param.kVehicleLength);
  decimal_t z =
      (param.kMinimumSpacing +
       std::max(0.0,
                cur_state.v * param.kDesiredHeadwayTime +
                    cur_state.v * (cur_state.v - cur_state.v_front) /
                        (2.0 * sqrt(param.kAcceleration *
                                    param.kComfortableBrakingDeceleration)))) /
      s_alpha;
  decimal_t a_out =
      cur_state.v <= param.kDesiredVelocity
          ? (z >= 1.0
                 ? param.kAcceleration * (1 - pow(z, 2))
                 : a_free * (1 - pow(z, 2.0 * param.kAcceleration / a_free)))
          : (z >= 1.0 ? a_free + param.kAcceleration * (1 - pow(z, 2))
                      : a_free);
  a_out = std::max(std::min(param.kAcceleration, a_out),
                   -param.kHardBrakingDeceleration);
  *acc = a_out;
  return kSuccess;
}

横向规划采用Pure Pursuit Controller（纯跟踪控制器，简称 PP 算法），PP算法计算理想转向角（steering angle）：

$\delta = \arctan\left( \frac{2 L \sin(\alpha)}{L_d} \right)$

$\delta$ ：输出的转向角（steer）
$L$ ：车辆轴距（wheelbase_len）
$\alpha$ ：车辆朝向与目标点连线的夹角（angle_diff）
$L_d$ ：预瞄距离（lookahead distance）

#include "vehicle_model/controllers/pure_pursuit_controller.h"

namespace control {
ErrorType PurePursuitControl::CalculateDesiredSteer(
    const decimal_t wheelbase_len, const decimal_t angle_diff,
    const decimal_t look_ahead_dist, decimal_t *steer) {
  *steer = atan2(2.0 * wheelbase_len * sin(angle_diff), look_ahead_dist);
  return kSuccess;
}
}  // namespace control

2.1.4 安全机制

为了应对现实中交通参与者不按理性模型行动带来的潜在风险，论文设计了两层安全机制：

前向仿真控制器内部的安全模块：在每步仿真中，使用责任敏感安全模型（RSS）进行检测，如果当前控制命令可能导致危险，控制器会自动调整为符合安全响应的动作。

策略评估中的备选计划检查：在行为规划时，为每个激进策略寻找一个安全的备选策略（例如取消变道），确保即使最初的计划失败，也能及时切换到安全方案，如果当前策略没有可行的备选计划，系统会触发低级保护机制，如自动紧急制动（AEB）。

2.2 基于时空语义通道的轨迹生成

EPSILON的行为规划层在每个规划周期输出一个决策序列，即在未来规划时间范围内，自车与其他交通参与者的状态序列。行为规划层已经确定了自车在战术层面（如换道、让行等）的动作，但这个决策仍然是离散且粗粒度的。运动规划层的任务是，在遵循行为决策的基础上，生成一条：平滑的、安全的、动态可行的、可执行的连续轨迹。

首先，围绕行为层提供的初步轨迹，根据环境中的动静态信息以及语义元素，建立一个统一的时空语义通道（SSC）。SSC定义了自车在未来时空内的安全可行区域，确保轨迹不会穿越不可达或非法区域，这个通道在空间和时间上共同约束了自车的轨迹生成范围。

然后，在SSC内部，运动规划层通过优化求解最终轨迹，目标是同时最小化两类代价项：

轨迹光滑性代价（Smoothness Cost）：惩罚轨迹曲率、加速度、加加速度（jerk）的剧烈变化，提升舒适性和可控性。

轨迹一致性代价（Consistency Cost）：惩罚轨迹与行为决策的偏离，确保运动规划结果紧密跟随行为层意图，避免两层之间出现不一致。

轨迹被参数化为一条分段贝塞尔曲线，在轨迹的每个分段𝑗上，沿着每个自由度（比如纵向、横向），优化代价函数：

$J_{\sigma j} = w_{\sigma s} \int_{t_{j-1}}^{t_j} \left( \frac{d^3 f_{\sigma j}(t)}{dt^3} \right)^2 dt + w_{\sigma f} \cdot \frac{1}{n_j} \sum_{k=0}^{n_j} \left( f_{\sigma j}(t_k) - r_{\sigma jk} \right)^2$

$w^\sigma_s$ 是平滑性权重；
$w^\sigma_f$ 是一致性权重；
$f^\sigma_j(t)$ 是第 $j$ 段轨迹在自由度 $\sigma$ 上的表达；
$r^\sigma_{jk}$ 是第 $j$ 段第 $k$ 个时间步的参考状态（来自行为层）。

3. 总结

传统方法在面向开放场景下容易出现轨迹分布覆盖不足、语义一致性差以及规划优化约束过强等问题。为此，本文提出基于时空语义走廊的方法，在充分建模空间语义结构和行为意图的基础上，通过语义分支和约束一致性实现高效轨迹生成。该方法在结构建模与行为预测之间建立了显式桥梁，为交互式决策规划任务提供了有效支持。实验结果从规划成功率、规划时间、路径平滑性、安全性等多个维度进行评估，结果显示论文方法在保持实时性的前提下，取得了更高的成功率和更强的鲁棒性，特别在存在遮挡、突发行为或结构不确定的场景中表现出更优的适应能力。未来工作将拓展至多主体协同场景，并考虑将语义走廊机制与深度学习模型结合，以进一步提升泛化性和规划智能水平。