什么是动态规划(Dynamic Programming, DP)
- 贝尔曼最优原理
多阶段决策过程的特点是每个阶段都要进行决策,具有n个阶段的决策过程的策略是由n个相继进行的阶段决策构成的决策序列。由于前阶段的终止状态又是后一阶段的初始状态,因此确定阶段最优决策不能只从本阶段的效应出发,必须通盘考虑,整体规划。就是说,阶段k的最优决策不应只是本阶段的最优,而必须是本阶段及其所有后续阶段的总体最优,即关于整个后部子过程的最优决策。 对此,贝尔曼在深入研究的基础上,针对具有无后效性的多阶段决策过程的特点,提出了著名的多阶段决策的最优性原理:
整个过程的最优策略具有这样的性质:即无论过程过去的状态和决策如何,对前面的决策所形成的状态而言,余下的诸决策必须构成最优策略。 简而言之,最优性原理的含意就是:最优策略的任何一部分子策略也必须是最优的。
- 动态规划算法
基于动态规划的路径选择算法
EM planner中,路径的选择是基于S-L坐标系进行的,主要分为以下几个步骤:
-
道路撒点
撒点规则主要由以下几个方面确定:车辆的宽度,车辆的位置,车道宽度,车辆速度,撒点的最大步长(S和L方向),撒点的最小步长(S和L方向),撒点的最小长度,撒点的最大长度等; -
利用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的代价。
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;
};
- 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动态规划