Baidu Apollo代码解析之Open Space Planner中的平滑优化

 

大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。

---

目录

 

1、Hybrid A*和平滑优化的效果对比

2、Optimization Smoothing的调用

3、添加障碍物

---

1、Hybrid A*和平滑优化的效果对比

如我在之前的博文所述，Baidu Apollo 先使用Hybrid A*算法规划了一条粗糙无碰的轨迹，然后使用优化方法来平滑这条轨迹。我搭建了一个非常简单的停车场景，使用Hybrid A*规划的轨迹和平滑后的轨迹分别如下所示，深绿色为粗糙轨迹，浅绿色为平滑轨迹。单纯看path的形状，有时“平滑”会不太明显，但是看平滑后的速度v、朝向phi、转向角steer、加速度a，会更明显些。（注：图1-2与图3-4并不是对应的，此处仅仅是对比优化前后的效果） 

图0 Hybrid A*输出的路径有很多转折

图1 Hybrid A* 输出的路径

图2 Optimization smoothing 输出的路径

图3 Hybrid A* 输出路径的朝向角

 

图4 优化后的路径的朝向角

 

从图2可以看到，Hybrid A*输出的路径是保障了没有碰撞的，因为在扩展路径的同时做了碰撞检测，而平滑后的路径却发生了碰撞，因为Apollo在优化的目标函数中并没有将“与障碍物的间距”考虑进去，仅仅考虑了曲线平滑。在接下来的优化介绍中，我会更详细的说明这一点。

2、Optimization Smoothing的调用

Open Space Planner的调用入口在 Apollo5.0/modules/planning/open_space/tools/distance_approach_problem_wrapper.cc 文件的DistancePlan()中。

bool DistancePlan(HybridAStar* hybridA_ptr, ObstacleContainer* obstacles_ptr,
                  ResultContainer* result_ptr, double sx, double sy,
                  double sphi, double ex, double ey, double ephi,
                  double* XYbounds) {
  //载入Open Space Planner的配置文件，设置参数值            
  //在apollo/modules/planning/conf/planner_open_space_config.pb.txt中
  ...
  //先调用Hybrid A*产生初步的轨迹作为warm start
  if (!hybridA_ptr->Plan(sx, sy, sphi, ex, ey, ephi, XYbounds_,
                         obstacles_ptr->GetObstacleVec(),
                         &hybrid_astar_result)) {
    ...
  }

  if (FLAGS_enable_parallel_trajectory_smoothing) {    
  ...
  } else {
    ...
    //然后调用DistanceSmoothing()做平滑
    if (!DistanceSmoothing(planner_open_space_config_, *obstacles_ptr, sx, sy,
                           sphi, ex, ey, ephi, XYbounds_, &hybrid_astar_result,
                           &state_result_ds, &control_result_ds,
                           &time_result_ds, &dual_l_result_ds,
                           &dual_n_result_ds)) {
      return false;
    }
    ...
  }
  return true;
}

DistanceSmoothing()设置好状态量和控制量矩阵、初始状态和结束状态矩阵、车辆尺寸矩阵等优化需要的输入信息，然后调用DistanceApproachProblem::Solve()，在该函数内根据不同的方法设置，具体执行不同的优化算法。

bool DistanceSmoothing(...) {
  // load Warm Start result(horizon is the "N", not the size of step points)
  size_t horizon_ = hybrid_a_star_result->x.size() - 1;
  ...
  //若初步的轨迹点较少，则插值
  if (horizon_ <= 10 && horizon_ > 2 &&
      planner_open_space_config.enable_linear_interpolation()) {
    ...
  } else {
    //用Hybrid A*的输出来初始化状态分量和控制分量
    ...
  }
  //状态量和控制量，注意不同的行所对应的不同的含义，顺序很重要
  Eigen::MatrixXd xWS = Eigen::MatrixXd::Zero(4, horizon_ + 1);
  Eigen::MatrixXd uWS = Eigen::MatrixXd::Zero(2, horizon_);
  xWS.row(0) = x;
  xWS.row(1) = y;
  xWS.row(2) = phi;
  xWS.row(3) = v;
  uWS.row(0) = steer;
  uWS.row(1) = a;

  //初始状态
  Eigen::MatrixXd x0(4, 1);
  x0 << sx, sy, sphi, 0.0;
  //结束状态
  Eigen::MatrixXd xF(4, 1);
  xF << ex, ey, ephi, 0.0;
  ...
  //车辆尺寸
  ego_ << front_to_center, right_to_center, back_to_center, left_to_center;

  // result for distance approach problem
  Eigen::MatrixXd l_warm_up;
  Eigen::MatrixXd n_warm_up;

  DualVariableWarmStartProblem* dual_variable_warm_start_ptr =
      new DualVariableWarmStartProblem(planner_open_space_config);

  if (FLAGS_use_dual_variable_warm_start) {
    bool dual_variable_warm_start_status = dual_variable_warm_start_ptr->Solve(
        ...);
  } else {
    l_warm_up = 0.5 * Eigen::MatrixXd::Ones(
                          obstacles.GetObstaclesEdgesNum().sum(), horizon_ + 1);
    n_warm_up = 0.5 * Eigen::MatrixXd::Ones(4 * obstacles.GetObstaclesNum(),
                                            horizon_ + 1);
  }

  DistanceApproachProblem* distance_approach_ptr =
      new DistanceApproachProblem(planner_open_space_config);

  bool status = distance_approach_ptr->Solve(...);
  ...
  }
  return true;
}

DistanceApproachProblem类是个中间层，负责根据conf配置文件，为优化问题匹配不同的优化算法，该类只有一个成员函数Solve()。类似于《Effective C++》系列中提到的“指针与impl”的用法。

bool DistanceApproachProblem::Solve(...) {
  //DistanceApproachInterface类是所有distance approach的基类
  DistanceApproachInterface* ptop = nullptr;

  if (distance_approach_mode() == DISTANCE_APPROACH_IPOPT) {
    ptop = new DistanceApproachIPOPTInterface(...);
  } else if (distance_approach_mode() == DISTANCE_APPROACH_IPOPT_FIXED_TS) {
    ptop = new DistanceApproachIPOPTFixedTsInterface(...);
  } else if (distance_approach_mode() == DISTANCE_APPROACH_IPOPT_CUDA) {
    ptop = new DistanceApproachIPOPTCUDAInterface(...);
  } else if (distance_approach_mode() == DISTANCE_APPROACH_IPOPT_FIXED_DUAL) {
    ptop = new DistanceApproachIPOPTFixedDualInterface(...);
  } else if (distance_approach_mode() == DISTANCE_APPROACH_IPOPT_RELAX_END) {
    ptop = new DistanceApproachIPOPTRelaxEndInterface(...);
  }
  //构造IPOPT求解器要求解的目标问题
  Ipopt::SmartPtr problem = ptop;

  // Create an instance of the IpoptApplication
  Ipopt::SmartPtr app = IpoptApplicationFactory();
  //按照IPOPT的使用方法设定参数
  ...
  //初始化IPOPT求解器
  Ipopt::ApplicationReturnStatus status = app->Initialize();
  //
  status = app->OptimizeTNLP(problem);
  ...
  ptop->get_optimization_results(state_result, control_result, time_result,
                                 dual_l_result, dual_n_result);
  ...
}

在DistanceApproachProblem::Solve()中定义了一个DistanceApproachInterface类型的指针。DistanceApproachInterface类继承自Ipopt::TNLP虚基类，实现其中的几个虚函数（图5），便可以利用IPOPT来求解优化问题。关于IPOPT的使用，可以参考官方文档，我还没有搞懂，后续可能会补上一篇。

图5 需继承实现的Ipopt::TNLP中的虚函数

继承自DistanceApproachInterface类的几个distance approach，便对上述函数进行了重写实现。如图6、7所示，虽然这个文件夹内的文件和内容很多，但是不用吓到，其实逻辑结构非常简单和清晰。 

图6 不同的优化方法

图7 各类继承关系图

 

我们以DistanceApproachIPOPTInterface类为例，看一下其目标函数的设置。我们的目标是在Hybrid A*的轨迹基础上，计算一条平滑、无碰的轨迹。很自然的，目标函数应该至少涵盖这2方面。根据图5，eval_f()负责目标函数的设置，而其中，并没有考虑无碰，那么就出现了图2的情况，说明Apollo并没有开源停车入库轨迹规划的全部细节，这里可以参考Stanford的论文，利用Voronoi Diagram计算到障碍物的距离。

bool DistanceApproachIPOPTInterface::eval_f(int n, const double* x, bool new_x,
                                            double& obj_value) {
  eval_obj(n, x, &obj_value);
  return true;
}

template 
void DistanceApproachIPOPTInterface::eval_obj(int n, const T* x, T* obj_value) {
  // Objective is :
  // min control inputs
  // min input rate
  // min time (if the time step is not fixed)
  // regularization wrt warm start trajectory
  //此处注释已表明，没有考虑远离障碍物，即无碰
  ...

  //obj_value即总的cost值，目标就是 min(obj_value)
  *obj_value = 0.0;
  // 1. objective to minimize state diff to warm up
  ...
  // 2. objective to minimize u square
  ...
  // 3. objective to minimize input change rate for first horizon
  ...
  // 4. objective to minimize input change rates, [0- horizon_ -2]
  ...
  // 5. objective to minimize total time [0, horizon_]
  ...
}

3、添加障碍物

Open Space Planner的实际调用发生在 Apollo/modules/tools/open_space_visualization/distance_approach_visualizer.py 文件中，该文件定义了停车入库的起点和终点，并设置了几个“线条”障碍物，形状和图1非常相似。这里添加障碍物的代码非常离奇，难以复用，难以理解，估计是偷懒了吧。。。

# 以double数组的形式定义障碍物的顶点，对应Python list
# 这里只能看出来有10个点，看不出有几条线段
ROI_distance_approach_parking_boundary = (
            c_double * 20)(*[-13.6407054776, ... 5.61797800844])
# 在Python文件中使用编译好的Open Space Planner C++库
OpenSpacePlanner.AddObstacle(ROI_distance_approach_parking_boundary)

ObstacleContainer::AddObstacle()在 apollo/modules/planning/open_space/tools/distance_approach_problem_wrapper.cc中，输入参数是所有的障碍物顶点的坐标，单个障碍物的顶点按照顺时针排列，先x后y。

  void AddObstacle(const double* ROI_distance_approach_parking_boundary) {
    // the obstacles are hard coded into vertice sets of 3, 2, 3, 2
    if (!(VPresentationObstacle(ROI_distance_approach_parking_boundary) &&
          HPresentationObstacle())) {
      AINFO << "obstacle presentation fails";
    }
  }

AddObstacle()调用VPresentationObstacle()将传入的double数组转换成线段-点的形式，即std::vector> obstacles_vertices_vec_，Vec2d表示顶点， std::vector表示连续的一组线段，即一个障碍物。这里硬编码为4个障碍物，分别具有2、1、2、1条边，如图8（就是图1的线条拆解）。

bool VPresentationObstacle(
      const double* ROI_distance_approach_parking_boundary) {
    //定义了4个障碍物
    obstacles_num_ = 4;
    obstacles_edges_num_.resize(4, 1);
    //第1个障碍物有2条边，第2个障碍物有1条边，以此类推
    obstacles_edges_num_ << 2, 1, 2, 1;
    size_t index = 0;
    for (size_t i = 0; i < obstacles_num_; i++) {
      std::vector vertices_cw;
      //获取单个障碍物的所有顶点
      for (int j = 0; j < obstacles_edges_num_(i, 0) + 1; j++) {
        //获取单个顶点的x y坐标
        Vec2d vertice =
            Vec2d(ROI_distance_approach_parking_boundary[index],
                  ROI_distance_approach_parking_boundary[index + 1]);
        index += 2;
        vertices_cw.emplace_back(vertice);
      }
      obstacles_vertices_vec_.emplace_back(vertices_cw);
    }
    return true;
  }

HPresentationObstacle()负责将障碍物表示成矩阵的形式，这个函数我没有看懂。

图8 障碍物的表示