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 R_1 R1​表示从节点 p 1 p_1 p1​到节点 p p p的代价。

// 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;
};

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16

// 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;
};

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27

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;
}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51
• 52
• 53
• 54
• 55
• 56
• 57
• 58
• 59
• 60
• 61
• 62
• 63
• 64
• 65
• 66
• 67
• 68
• 69
• 70
• 71
• 72
• 73
• 74
• 75
• 76
• 77
• 78
• 79
• 80
• 81
• 82
• 83
• 84
• 85
• 86
• 87
• 88
• 89
• 90
• 91
• 92
• 93
• 94
• 95
• 96
• 97
• 98
• 99
• 100
• 101
• 102
• 103
• 104
• 105
• 106
• 107

// 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);
}

(73条消息) 路径规划——动态规划在Apollo的Planner中的应用及C++代码实现_肥宅快乐谁-CSDN博客_apollo动态规划