路径规划——动态规划在Apollo的Planner中的应用及C++代码实现

什么是动态规划（Dynamic Programming, DP）

1. 贝尔曼最优原理

  多阶段决策过程的特点是每个阶段都要进行决策，具有n个阶段的决策过程的策略是由n个相继进行的阶段决策构成的决策序列。由于前阶段的终止状态又是后一阶段的初始状态，因此确定阶段最优决策不能只从本阶段的效应出发，必须通盘考虑，整体规划。就是说，阶段k的最优决策不应只是本阶段的最优，而必须是本阶段及其所有后续阶段的总体最优，即关于整个后部子过程的最优决策。 对此，贝尔曼在深入研究的基础上，针对具有无后效性的多阶段决策过程的特点，提出了著名的多阶段决策的最优性原理：
   整个过程的最优策略具有这样的性质：即无论过程过去的状态和决策如何，对前面的决策所形成的状态而言，余下的诸决策必须构成最优策略。 简而言之，最优性原理的含意就是：最优策略的任何一部分子策略也必须是最优的。

2. 动态规划算法

基于动态规划的路径选择算法

EM planner中，路径的选择是基于S-L坐标系进行的，主要分为以下几个步骤：

1. 道路撒点
   撒点规则主要由以下几个方面确定：车辆的宽度，车辆的位置，车道宽度，车辆速度，撒点的最大步长（S和L方向），撒点的最小步长（S和L方向），撒点的最小长度，撒点的最大长度等；

2. 利用DP生成Cost最小的路径
   
   如上图， p点的cost计算方式为：
   p . c o s t = s t d : : m i n { p 1 . c o s t + R 1 , . . . , p 7 . c o s t + R 7 } p.{cost} = std::min\{p_1.cost + R_1, ..., p_7.cost +R_7\} p.cost=std::min{p1​.cost+R1​,...,p7​.cost+R7​}.
   其中，$R_1$表示从节点$p_1$到节点$p$的代价。

C++代码实现

// node.h file
struct SlPoint {
  SlPoint(const double _s, const double _l) : s(_s), l(_l) {
  }
  double s;
  double l;
};

struct Node {
  Node(const SlPoint& _sl_point)
      : sl_point(_sl_point), pre_node(nullptr), cost(std::numeric_limits<double>::max()) {
  }
  SlPoint sl_point;
  Node* pre_node;
  double cost;
};

// class DpPath file
class DpPath final {
 public:
  explicit DpPath(const std::vector<std::vector<SlPoint>>& sample_points,
                  const std::vector<Box2d>& obstacles);

  bool Search(std::vector<SlPoint>* const path);

 private:
  void Init(const std::vector<std::vector<SlPoint>>& sample_points);

  void CalculateCostTable();

  void CalculateCostAt(const int32_t l, const int32_t s);

  double CalculateAllObstacleCost(const SlPoint& pre_point, const SlPoint& cur_point) const;

  double CalculateObstacleCost(const Box2d& obs, const Box2d& veh) const;

  double CalculateReferenceLineCost(const SlPoint& pre_point, const SlPoint& cur_point) const;

 private:
  std::vector<std::vector<Node>> cost_table_;
  std::vector<Box2d> obstacles_;
  const double vehicle_length_ = 2.0;
  const double vehicle_width_ = 1.0;
};

DpPath::DpPath(const std::vector<std::vector<SlPoint>>& sample_points,
               const std::vector<Box2d>& obstacles)
    : obstacles_(obstacles) {
  CHECK_GT(sample_points.size(), 1);
  CHECK_EQ(sample_points.front().size(), 1);
  Init(sample_points);
}

bool DpPath::Search(std::vector<SlPoint>* const path) {
  CHECK(path!=nullptr);

  CalculateCostTable();
  auto& last_level = cost_table_.back();
  Node* min_cost_node = nullptr;
  double min_cost = std::numeric_limits<double>::max();
  for (auto& p : last_level) {
    if (p.cost < min_cost) {
      min_cost = p.cost;
      min_cost_node = &p;
    }
  }
  if (min_cost_node == nullptr || min_cost == std::numeric_limits<double>::max()) {
    return false;
  }

  while (min_cost_node != nullptr) {
    path->emplace_back(min_cost_node->sl_point);
    min_cost_node = min_cost_node->pre_node;
  }
  std::reverse(path->begin(), path->end());
  return true;
}

void DpPath::Init(const std::vector<std::vector<SlPoint>>& sample_points) {
  for (const auto& level : sample_points) {
    std::vector<Node> level_points;
    for (const auto& p : level) {
      level_points.emplace_back(p);
    }
    cost_table_.emplace_back(level_points);
  }
}

void DpPath::CalculateCostTable() {
  cost_table_[0][0].cost = 0.0;
  for (uint32_t s = 1; s < cost_table_.size(); ++s) {
    for (uint32_t l = 0; l < cost_table_[s].size(); ++l) {
      CalculateCostAt(s, l);
    }
  }
}

void DpPath::CalculateCostAt(const int32_t s, const int32_t l) {
  auto& pre_level = cost_table_[s - 1];
  double min_cost = std::numeric_limits<double>::max();
  for (auto& pre_point : pre_level) {
    double cost = CalculateAllObstacleCost(pre_point.sl_point, cost_table_[s][l].sl_point) +
                  CalculateReferenceLineCost(pre_point.sl_point, cost_table_[s][l].sl_point);
    cost += pre_point.cost;
    if (cost < min_cost) {
      min_cost = cost;
      cost_table_[s][l].pre_node = &pre_point;
      cost_table_[s][l].cost = min_cost;
    }
  }
}

double DpPath::CalculateAllObstacleCost(const SlPoint& pre_point, const SlPoint& cur_point) const {
  const double curve_length = cur_point.s - pre_point.s;
  QuinticPolynomialCurve1d curve(pre_point.l, 0.0, 0.0, cur_point.l, 0.0, 0.0, curve_length);
  double cost = 0.0;
  constexpr double kStep = 0.1;
  for (double s = 0.0; s < curve_length; s += kStep) {
    double vehicle_l = curve.Evaluate(0, s);
    double vehicle_s = pre_point.s + s;
    double vehicle_heading = std::atan(curve.Evaluate(1, s));
    Box2d veh({vehicle_s, vehicle_l}, vehicle_heading, vehicle_length_, vehicle_width_);
    for (const auto& obs : obstacles_) {
      cost += CalculateObstacleCost(obs, veh);
    }
  }
  return cost;
}

double DpPath::CalculateObstacleCost(const Box2d& obs, const Box2d& veh) const {
  if (obs.HasOverlap(veh)) {
    return std::numeric_limits<double>::max();
  }
  const double dis = obs.DistanceTo(veh);
  if (dis > 2 * vehicle_width_) {
    return 0.0;
  }
  return 1.0 / (dis + std::numeric_limits<double>::epsilon());
}

double DpPath::CalculateReferenceLineCost(const SlPoint& pre_point,
                                          const SlPoint& cur_point) const {
  const double curve_length = cur_point.s - pre_point.s;
  QuinticPolynomialCurve1d curve(pre_point.l, 0.0, 0.0, cur_point.l, 0.0, 0.0, curve_length);
  double cost = 0.0;
  constexpr double kStep = 0.1;
  for (double s = 0.0; s < curve_length; s += kStep) {
    double l = curve.Evaluate(0, s);
    cost += std::fabs(l * l);
  }
  return cost;
}

// test file
class DpPathTest : public ::testing::Test {
 public:
  void SetUp() {
    SlPoint vehicle_position(0.0, 0.0);
    sample_points_.emplace_back(std::vector<SlPoint>{vehicle_position});
    for (double s = 3; s < 20; s += 3) {
      std::vector<SlPoint> level_points;
      for (double l = -1.5; l < 2.0; l += 0.5) {
        level_points.emplace_back(s, l);
      }
      sample_points_.emplace_back(level_points);
    }
  }

 protected:
  std::shared_ptr<DpPath> dp_path_ = nullptr;
  std::vector<std::vector<SlPoint>> sample_points_;
  std::vector<Box2d> obstacles_;
};

TEST_F(DpPathTest, Search1) {
  dp_path_ = std::make_shared<DpPath>(sample_points_, obstacles_);
  std::vector<SlPoint> path;
  bool ret = dp_path_->Search(&path);
  EXPECT_TRUE(ret);
  EXPECT_GT(path.size(), 2);
}

TEST_F(DpPathTest, Search2) {
  Box2d obs1({3, -0.5}, 0.0, 0.8, 1.5);
  Box2d obs2({12, 0.5}, 0.0, 0.8, 1.5);
  obstacles_.emplace_back(obs1);
  obstacles_.emplace_back(obs2);
  dp_path_ = std::make_shared<DpPath>(sample_points_, obstacles_);
  std::vector<SlPoint> path;
  bool ret = dp_path_->Search(&path);
  EXPECT_TRUE(ret);
  EXPECT_GT(path.size(), 2);
}

测试结果

说明

代码中用到的Box2d, QuinticPolynomialCurve1d等库函数，见Apollo开源代码，不做详细叙述，在计算cost的时候，点的过滤，车辆的运动学模型等限制条件，不做讨论，本文假设车辆在道路中心线上，即$l=0$。