Lattice Planner从学习到放弃（四）：问题与处理

前情提要

AEB仿真效果

  前期把apollo中的lattice规划器的大体实现了，收获颇丰，docker搭了，protobuf也取代了老旧的数据结构，自己的码能也肉眼提升，另外为某厂开发的AEB也到了验收阶段，CNACP全满通过。再接再厉。
  感知的小伙伴们目标级障碍物输出了，控制的小伙伴们优化了车辆控制，大哥搞定了框架，我得以继续完善，目标不变： 让车在封闭的园区内像人一样自由稳定的行驶 。

1.单车道nudge的实现

  车辆的避障行为分为制动避障与转向避障，即下图中的A和B、C、D：
  上图中的B、C、D是理想的nudge表现。此阶段并未考虑换道，因为目前仅使用的单车道地图，对于增大偏移量实现换道的效果并不建议，因为这种做法直接抹杀了Lane的概念，一方面无法扩展交通规则，另一方面跳过了HD map中的车道信息，在建立OSQP的bounds时存在大量风险。

1. 目标 ：
     规划结果与实车表现同B、C、D吻合，实现车道内的平滑绕行
2. 现象 ：
     当车辆行驶到车道某位置附近时，突然往右避让幅度超大，基本要撞上右侧车了。但是nudge的值为常值并无改动。
   
3. 问题排查 ：
     通过调整FLAGS_nudge_buffer值来改变偏移程度，无法起作用，然后注意到该车是倾斜的，正常停的车是没有这个现象的，基本锁定是碰撞检测环节出问题，要么没算对，要么障碍物尺寸不对！碰撞检测的源码：

bool CollisionChecker::InCollision(
    const DiscretizedTrajectory& discretized_trajectory) {
  CHECK_LE(discretized_trajectory.NumOfPoints(),
           predicted_bounding_rectangles_.size());
  const auto& vehicle_config =
      common::VehicleConfigHelper::Instance()->GetConfig();
  double ego_length = vehicle_config.vehicle_param().length();
  double ego_width = vehicle_config.vehicle_param().width();

  for (size_t i = 0; i < discretized_trajectory.NumOfPoints(); ++i) {
    const auto& trajectory_point =
        discretized_trajectory.TrajectoryPointAt(static_cast<std::uint32_t>(i));
    double ego_theta = trajectory_point.path_point().theta();
    Box2d ego_box(
        {trajectory_point.path_point().x(), trajectory_point.path_point().y()},
        ego_theta, ego_length, ego_width);
    // 车辆center和车辆的几何中心不重合，所以box需要校正一下
    double shift_distance =
        ego_length / 2.0 - vehicle_config.vehicle_param().back_edge_to_center();
    Vec2d shift_vec{shift_distance * std::cos(ego_theta),
                    shift_distance * std::sin(ego_theta)};
    ego_box.Shift(shift_vec);

    for (const auto& obstacle_box : predicted_bounding_rectangles_[i]) {
      if (ego_box.HasOverlap(obstacle_box)) {
        return true;
      }
    }
  }
  return false;
}

  方法很简单，核对一下自车box数据没问题后，断点查看ego_box.HasOverlap()入参障碍物的Box尺寸参数，发现果然不对，尺寸比真实尺寸大了很多…变长了。
  让我们进入它！在函数CollisionChecker::BuildPredictedEnvironment()中发现对障碍物做了膨胀处理，如下：相当于把障碍物增长2×FLAGS_lon_collision_buffer米，增宽2×FLAGS_lat_collision_buffer米。

while (relative_time < FLAGS_trajectory_time_length) {
    std::vector<Box2d> predicted_env;
    for (const Obstacle* obstacle : obstacles_considered) {
      TrajectoryPoint point = obstacle->GetPointAtTime(relative_time);
      Box2d box = obstacle->GetBoundingBox(point);
      box.LongitudinalExtend(2.0 * FLAGS_lon_collision_buffer);
      box.LateralExtend(2.0 * FLAGS_lat_collision_buffer);
      predicted_env.push_back(std::move(box));
    }
    predicted_bounding_rectangles_.push_back(std::move(predicted_env));
    relative_time += FLAGS_trajectory_time_resolution;
  }

  把经过膨胀后的障碍物对车道的占据情况显示出来，在碰撞检测阶段，真实的障碍物如右图所示，很明显车当然过不去了，即使强行通过也会再更近的时候由于突破bounds导致osqp无解进而规划失败…
  障碍物膨胀不难理解，为了安全，人在开车的过程中也会适当的远离附近车辆，但是这个膨胀方法显然太过死板了，或许百度内部早就优化了，只是开源没释放？所以暂时做了些许改动： 静止物体膨胀程度缩小、动态障碍物适当膨胀 。后期必然是需要继续完善的，进一步可借鉴物体危险程度势场方法。

while (relative_time < FLAGS_trajectory_time_length) {
    std::vector<Box2d> predicted_env;
    for (const Obstacle* obstacle : obstacles_considered) {
      TrajectoryPoint point = obstacle->GetPointAtTime(relative_time);
      Box2d box = obstacle->GetBoundingBox(point);
      if(obstacle->IsStatic()){
        box.LongitudinalExtend(2.0 * 0.1);
        box.LateralExtend(2.0 * 0.05);
      } else {
        box.LongitudinalExtend(2.0 * FLAGS_lon_collision_buffer);
        box.LateralExtend(2.0 * FLAGS_lat_collision_buffer);
      }      
      predicted_env.push_back(std::move(box));
    }
    predicted_bounding_rectangles_.push_back(std::move(predicted_env));
    relative_time += FLAGS_trajectory_time_resolution;
  }

2.SL图建立-bug描述与解决

  我一直在告诫自己，是我错了是我错了，应该是我错了…但是，把障碍物尺寸更新到bounds里，怎么也不是光start_s吧，源码：

  auto start_iter = std::lower_bound(
      discretized_path.begin(), discretized_path.end(), sl_boundary.start_s());
  // 这里应该是sl_boundary.end_s()吧？
  auto end_iter = std::upper_bound(
      discretized_path.begin(), discretized_path.end(), sl_boundary.start_s());

  按照源码逻辑，SL图实际如下左图：
  仅采用障碍物SL中的start_s进行采样的话，必然导致车辆轨迹过早的回归，如果理解没出错的话，是apollo这里出错了吧，issue已提交，希望是自己被打脸，为了实现右图的效果，没错，我改了代码，在查询上边界时sl_boundary.start_s()改为sl_boundary.end_s()。
  然后，然后就搞定了…然后让目标车横停、竖停、斜着停，丝滑～

nudge实车测试

3.狭窄路段通行过程中规划失败

  在通过狭窄路段时，车辆正常行驶均正常，但在即将驶出时，出现无轨迹现象，排查后发现由OSQP求解失败导致。
  OSQP求解失败最常见原因之前记录过，大致：边界以及轨迹已经规划好了，但是控制误差太大导致车辆行驶过程中驶出了边界，导致求解器失败。这个问题控制的小伙伴优化后已经解决。
  此次问题，bounds为[-1.072, -0.635]，自车横向d=-0.8并未出现越界行现象：
  为了确定约束问题，重新对二次规划建模进行了整理：
  A为Cloum矩阵进行CSC压缩后，需要满足等式与不等式约束：
            
认为车辆在驶离狭窄路段时，车辆驶出角度过大导致运动约束无法满足。
解决方案

  通过调整目标函数即P与Q，来调整目标轨迹的形状，使得车辆能够平滑的通过狭窄路段，实现nudge，同时可扩大对control精度的宽容度。P与Q的作用效果如下图所示：
  通过适当的调节P和Q来调控a、b，以达到nudge的范围容忍度调节，当然，如果control足够精确，完全可以让车辆尽量近的贴近障碍物进行nudge。代码实现：

P_data->at(i) = 2.0 * FLAGS_weight_lateral_offset +
                    2.0 * FLAGS_weight_lateral_obstacle_distance;

和

q[i] = -2.0 * FLAGS_weight_lateral_obstacle_distance *
             (d_bounds[i].first + d_bounds[i].second);

最终实现效果：车辆更贴近外边界进行nudge，不会出现较大的转向控制。

下一个议题

未完待续

---

后续：
丰富障碍物的处理，包括：bounding box的膨胀优化，逐步完成lattice planner的全功能。
一直在获得很多大佬的指导，希望自己继续深入，每天进步一点点