Apollo 6.0的EM Planner (3)：速度規劃的動態規劃DP過程
前言
與路徑DP過程的區別
多了剪枝操作
不用五次多項式連線兩點計算EdgeCost
回溯終點的選擇，遍歷每一列找到best_end_point，因為可能
在最大采樣時刻前就已經到達終點
這一部分涉及到的程式碼路徑為
modules\planning\tasks\optimizers\path_time_heuristic\
path_time_heuristic_optimizer
。h
modules\planning\tasks\optimizers\path_time_heuristic\
http：//
path_time_heuristic_optimizer。cc
相關的輔助類為
modules\planning\tasks\optimizers\path_time_heuristic\
st_graph_point
。h
modules\planning\tasks\optimizers\path_time_heuristic\
http：//
st_graph_point。cc
modules\planning\tasks\optimizers\path_time_heuristic\
dp_st_cost
。h
modules\planning\tasks\optimizers\path_time_heuristic\
http：//
dp_st_cost。cc
modules\planning\tasks\optimizers\path_time_heuristic\
gridded_path_time_graph
。h
modules\planning\tasks\optimizers\path_time_heuristic\
http：//
gridded_path_time_graph。cc
詳細註釋連結見
1、主要流程
離散化連續的S-T域，這裡和路徑規劃的S-L域有點小區別，S軸方法包含
稠密區dense_dimension_s
_和
稀疏區sparse_dimension_s_
，cost_table_這裡用的是
vector<vector<>>
，路徑規劃用的是list
forward過程
，遞推出每個節點的cost，這裡有一個剪枝操作，依據max_acceleration_和max_deceleration_找到下一列可能到達的範圍
[next_lowest_row, next_highest_row]
cost
和
父節點更新
，這裡同樣有個剪枝操作，依據
FLAGS_planning_upper_speed_limit
得到pre_lowest_s，最後得到追溯範圍
[r_low, r]
backward過程
，順藤摸瓜，得到最優的一串ST point
2、主要框架
PathTimeHeuristicOptimizer
是速度DP規劃的入口
GriddedPathTimeGraph
完成具體的動態規劃DP過程
StGraphPoint
定義了每個取樣點資料形式，
DpStCost
進行每個點的cost計算
3、PathTimeHeuristicOptimizer 方法入口
這個Class比較簡潔or空洞，Process（ ）作為方法入口，呼叫SearchPathTimeGraph（ ）完成規劃任務，最終的DP過程借用
第4.1節
中的
Search( )完成
4、GriddedPathTimeGraph 核心動態規劃DP過程
資料結構和方法
GriddedPathTimeGraph
這個Class進行具體的DP過程，具體的資料結構和方法見上圖，成員函式見的依賴關係見下面的樹狀圖
Search（ ）為入口
依次呼叫InitCostTable（）、InitSpeedLimitLookUp（ ）、CalculateTotalCost（ ）、RetrieveSpeedProfile（ ）完成任務
其中的CalculateTotalCost（ ）完成S-T graph的遍歷，過程中使用CalculateCostAt（ ）進行
cost的計算
和
父節點的更新
成員函式依賴關係
4。1 Search（ ）方法入口
Search（ ）依次呼叫InitCostTable（）、InitSpeedLimitLookUp（ ）、CalculateTotalCost（ ）、RetrieveSpeedProfile（ ）完成任務
/****************************************************************************
*
* 速度的主流程：
* 1、初始化CostTable， InitCostTable（）
* 2、初始化限速查詢表， InitSpeedLimitLookUp（）
* 3、計算更新CostTable， CalculateTotalCost（）
* 4、回溯，得到SpeedProfile
* **************************************************************************/
Status
GriddedPathTimeGraph
：：
Search
（
SpeedData
*
const
speed_data
）
{
。。。
// 1 初始化CostTable
if
（
！
InitCostTable
（）。
ok
（））
{。。。
}
// 2 初始化限速查詢表
if
（
！
InitSpeedLimitLookUp
（）。
ok
（））
{。。。
}
// 3 計算所有的cost，並更新CostTable
if
（
！
CalculateTotalCost
（）。
ok
（））
{。。。
}
// 4 回溯得到SpeedProfile
if
（
！
RetrieveSpeedProfile
（
speed_data
）。
ok
（））
{。。。
}
return
Status
：：
OK
（）；
}
4。2 InitCostTable（ ） 初始化cost_table_
這一環節主要是把連續的S-T域離散化，
，
兩層for迴圈完成任務，
外層為T，解析度為
unit_t_
內層為S，解析度為
dense_unit_s_
和
sparse_unit_s_ ，
距離近的一段離散的
稠密些（見橙色點）
，距離遠的一段
稀疏些（見藍色點）
/*************************************************************************
*
* cost_table_ 初始化
* 1、t方向 和 s方向的離散化
* 2、兩層for迴圈完成初始化
**************************************************************************/
Status
GriddedPathTimeGraph
：：
InitCostTable
（）
{
。。。
// t方向的離散化，dimension_t_ = total_length_t_ 除以 unit_t_（時間解析度）。
dimension_t_
=
static_cast
<
uint32_t
>
（
std
：：
ceil
（
total_length_t_
/
static_cast
<
double
>
（
unit_t_
）））
+
1
；
// s方向的離散化
double
sparse_length_s
=
total_length_s_
-
static_cast
<
double
>
（
dense_dimension_s_
-
1
）
*
dense_unit_s_
；
// 如果sparse_length_s>0，sparse_dimension_s_ = sparse_length_s / sparse_unit_s_
sparse_dimension_s_
=
// 稀疏取樣點個數
sparse_length_s
>
std
：：
numeric_limits
<
double
>：：
epsilon
（）
？
static_cast
<
uint32_t
>
（
std
：：
ceil
（
sparse_length_s
/
sparse_unit_s_
））
：
0
；
// 否則為0，即全部為密集取樣點。
dense_dimension_s_
=
// 密集取樣點個數
sparse_length_s
>
std
：：
numeric_limits
<
double
>：：
epsilon
（）
？
dense_dimension_s_
：
static_cast
<
uint32_t
>
（
std
：：
ceil
（
total_length_s_
/
dense_unit_s_
））
+
1
；
dimension_s_
=
dense_dimension_s_
+
sparse_dimension_s_
；
。。。
// cost_table_為雙層vector，外層是t，內層是s，
cost_table_
=
std
：：
vector
<
std
：：
vector
<
StGraphPoint
>>
（
dimension_t_
，
std
：：
vector
<
StGraphPoint
>
（
dimension_s_
，
StGraphPoint
（）））；
double
curr_t
=
0。0
；
// 起點 t = 0
for
（
uint32_t
i
=
0
；
i
<
cost_table_
。
size
（）；
++
i
，
curr_t
+=
unit_t_
）
{
auto
&
cost_table_i
=
cost_table_
［
i
］；
double
curr_s
=
0。0
；
//先對dense_s 初始化
for
（
uint32_t
j
=
0
；
j
<
dense_dimension_s_
；
++
j
，
curr_s
+=
dense_unit_s_
）
{
cost_table_i
［
j
］。
Init
（
i
，
j
，
STPoint
（
curr_s
，
curr_t
））；
}
curr_s
=
static_cast
<
double
>
（
dense_dimension_s_
-
1
）
*
dense_unit_s_
+
sparse_unit_s_
；
//再對sparse_s 初始化
for
（
uint32_t
j
=
dense_dimension_s_
；
j
<
cost_table_i
。
size
（）；
++
j
，
curr_s
+=
sparse_unit_s_
）
{
cost_table_i
［
j
］。
Init
（
i
，
j
，
STPoint
（
curr_s
，
curr_t
））；
}
}
。。。
}
4。3 CalculateTotalCost（ ） 更新cost_table_
這裡與S-L域的路徑路徑規劃有點區別，多了些
剪枝操作
，
實現細節見 GetRowRange( )
，依據
max_acceleration_
和
max_deceleration_
算出下一時刻可以到達的S，進而得到範圍
[next_lowest_row, next_lowest_row]
/*******************************************************************************
*
* 動態規劃的主體，兩層for迴圈遍歷cost_table_，t外迴圈，s內迴圈
* 呼叫CalculateCostAt（）計算total_cost
* ****************************************************************************/
Status
GriddedPathTimeGraph
：：
CalculateTotalCost
（）
{
// 需要透過函式GetRowRange（）計算出s的範圍［next_lowest_row，next_highest_row］
// 剪枝操作，避免不必要的計算
size_t
next_highest_row
=
0
；
size_t
next_lowest_row
=
0
；
// 外迴圈，每一列的index，即每一個t
for
（
size_t
c
=
0
；
c
<
cost_table_
。
size
（）；
++
c
）
{
size_t
highest_row
=
0
；
size_t
lowest_row
=
cost_table_
。
back
（）。
size
（）
-
1
；
// count為每一列的點數
int
count
=
static_cast
<
int
>
（
next_highest_row
）
-
static_cast
<
int
>
（
next_lowest_row
）
+
1
；
if
（
count
>
0
）
{
std
：：
vector
<
std
：：
future
<
void
>>
results
；
// 內迴圈，每行的index，即每一個s
for
（
size_t
r
=
next_lowest_row
；
r
<=
next_highest_row
；
++
r
）
{
auto
msg
=
std
：：
make_shared
<
StGraphMessage
>
（
c
，
r
）；
if
（
FLAGS_enable_multi_thread_in_dp_st_graph
）
{
results
。
push_back
（
cyber
：：
Async
（
&
GriddedPathTimeGraph
：：
CalculateCostAt
，
this
，
msg
））；
}
else
{
CalculateCostAt
（
msg
）；
}
}
if
（
FLAGS_enable_multi_thread_in_dp_st_graph
）
{
for
（
auto
&
result
：
results
）
{
result
。
get
（）；
}
}
}
// 下一輪迴圈的準備工作，更新範圍 ［next_lowest_row， next_highest_row］
for
（
size_t
r
=
next_lowest_row
；
r
<=
next_highest_row
；
++
r
）
{
const
auto
&
cost_cr
=
cost_table_
［
c
］［
r
］；
if
（
cost_cr
。
total_cost
（）
<
std
：：
numeric_limits
<
double
>：：
infinity
（））
{
size_t
h_r
=
0
；
size_t
l_r
=
0
；
GetRowRange
（
cost_cr
，
&
h_r
，
&
l_r
）；
highest_row
=
std
：：
max
（
highest_row
，
h_r
）；
lowest_row
=
std
：：
min
（
lowest_row
，
l_r
）；
}
}
next_highest_row
=
highest_row
；
next_lowest_row
=
lowest_row
；
}
return
Status
：：
OK
（）；
}
4。4 CalculateCostAt（ ） 計算每一個點的totalCost，並更新父節點
一個點的total_cost由四部分構成：
obstacle_cost：障礙物cost
spatial_potential_cost：空間位置cost，於終點s的差值
前一個點的total_cost
EdgeCost：由三部分構成Speedcost、AccelCost、JerkCost
在
更新父節點
時同樣有一個
剪枝操作
，使用限速資訊FLAGS_planning_upper_speed_limit得到
pre_lowest_s
，進而將尋找範圍限制在
[r_low, r]
，其中r為當前行號，因為
EM Planner主要是前進場景
，不會考慮
倒車情況
，那麼
S值是遞增或不變，不會下降
，所以r最大也就當前行號
/****************************************************************************
*
* 計算每一個點的totalCost，並更新父節點
* 輸入 msg，即（c，r），cost_table_中t、s方向離散後的index
* *************************************************************************/
void
GriddedPathTimeGraph
：：
CalculateCostAt
（
const
std
：：
shared_ptr
<
StGraphMessage
>&
msg
）
{
const
uint32_t
c
=
msg
->
c
；
const
uint32_t
r
=
msg
->
r
；
auto
&
cost_cr
=
cost_table_
［
c
］［
r
］；
// 1、計算 obstacle_cost，如果為無窮大，則停止，
cost_cr
。
SetObstacleCost
（
dp_st_cost_
。
GetObstacleCost
（
cost_cr
））；
if
（
cost_cr
。
obstacle_cost
（）
>
std
：：
numeric_limits
<
double
>：：
max
（））
{
return
；
}
// 2、計算SpatialPotentialCost
cost_cr
。
SetSpatialPotentialCost
（
dp_st_cost_
。
GetSpatialPotentialCost
（
cost_cr
））；
const
auto
&
cost_init
=
cost_table_
［
0
］［
0
］；
// 第0列的特殊處理，設定起始點的TotalCost 為0。
if
（
c
==
0
）
{
DCHECK_EQ
（
r
，
0U
）
<<
“Incorrect。 Row should be 0 with col = 0。 row： ”
<<
r
；
cost_cr
。
SetTotalCost
（
0。0
）；
// 起點的cost設定為0
cost_cr
。
SetOptimalSpeed
（
init_point_
。
v
（））；
return
；
}
const
double
speed_limit
=
speed_limit_by_index_
［
r
］；
const
double
cruise_speed
=
st_graph_data_
。
cruise_speed
（）；
// The mininal s to model as constant acceleration formula
// default： 0。25 * 7 = 1。75 m
const
double
min_s_consider_speed
=
dense_unit_s_
*
dimension_t_
；
//第一列的特殊處理
if
（
c
==
1
）
{
const
double
acc
=
//當前點的加速度
2
*
（
cost_cr
。
point
（）。
s
（）
/
unit_t_
-
init_point_
。
v
（））
/
unit_t_
；
//加速度、減速度超出範圍，返回
if
（
acc
<
max_deceleration_
||
acc
>
max_acceleration_
）
{
return
；
}
if
（
init_point_
。
v
（）
+
acc
*
unit_t_
<
-
kDoubleEpsilon
&&
cost_cr
。
point
（）。
s
（）
>
min_s_consider_speed
）
{
return
；
}
// 當前點與起始點的連線與stboundary有重疊，返回
if
（
CheckOverlapOnDpStGraph
（
st_graph_data_
。
st_boundaries
（），
cost_cr
，
cost_init
））
{
return
；
}
// 計算當前點的total_cost
cost_cr
。
SetTotalCost
（
cost_cr
。
obstacle_cost
（）
+
cost_cr
。
spatial_potential_cost
（）
+
cost_init
。
total_cost
（）
+
CalculateEdgeCostForSecondCol
（
r
，
speed_limit
，
cruise_speed
））；
cost_cr
。
SetPrePoint
（
cost_init
）；
//前一個點為初始點
cost_cr
。
SetOptimalSpeed
（
init_point_
。
v
（）
+
acc
*
unit_t_
）；
return
；
}
// 剪枝操作
static
constexpr
double
kSpeedRangeBuffer
=
0。20
；
// 由當前點推出能到達該點的前一列最小的s
// 將當前點的pre_col縮小至 ［r_low， r］
const
double
pre_lowest_s
=
cost_cr
。
point
（）。
s
（）
-
FLAGS_planning_upper_speed_limit
*
（
1
+
kSpeedRangeBuffer
）
*
unit_t_
；
const
auto
pre_lowest_itr
=
std
：：
lower_bound
（
spatial_distance_by_index_
。
begin
（），
spatial_distance_by_index_
。
end
（），
pre_lowest_s
）；
uint32_t
r_low
=
0
；
// 計算 pre_lowest_s 的 index
if
（
pre_lowest_itr
==
spatial_distance_by_index_
。
end
（））
{
r_low
=
dimension_s_
-
1
；
}
else
{
r_low
=
static_cast
<
uint32_t
>
（
std
：：
distance
（
spatial_distance_by_index_
。
begin
（），
pre_lowest_itr
））；
}
const
uint32_t
r_pre_size
=
r
-
r_low
+
1
；
const
auto
&
pre_col
=
cost_table_
［
c
-
1
］；
double
curr_speed_limit
=
speed_limit
；
// 第二列的特殊處理
if
（
c
==
2
）
{
// 對於前一列，遍歷從r->r_low的點，
// 依據重新算得的cost，當前點的pre_point，也就是DP過程的狀態轉移方程
for
（
uint32_t
i
=
0
；
i
<
r_pre_size
；
++
i
）
{
uint32_t
r_pre
=
r
-
i
；
if
（
std
：：
isinf
（
pre_col
［
r_pre
］。
total_cost
（））
||
pre_col
［
r_pre
］。
pre_point
（）
==
nullptr
）
{
continue
；
}
const
double
curr_a
=
//當前點的加速度
2
*
（（
cost_cr
。
point
（）。
s
（）
-
pre_col
［
r_pre
］。
point
（）。
s
（））
/
unit_t_
-
pre_col
［
r_pre
］。
GetOptimalSpeed
（））
/
unit_t_
；
if
（
curr_a
<
max_deceleration_
||
curr_a
>
max_acceleration_
）
{
continue
；
}
if
（
pre_col
［
r_pre
］。
GetOptimalSpeed
（）
+
curr_a
*
unit_t_
<
-
kDoubleEpsilon
&&
cost_cr
。
point
（）。
s
（）
>
min_s_consider_speed
）
{
continue
；
}
// 當前點與前一個點的連線是不是在boundary裡
if
（
CheckOverlapOnDpStGraph
（
st_graph_data_
。
st_boundaries
（），
cost_cr
，
pre_col
［
r_pre
］））
{
continue
；
}
curr_speed_limit
=
std
：：
fmin
（
curr_speed_limit
，
speed_limit_by_index_
［
r_pre
］）；
const
double
cost
=
cost_cr
。
obstacle_cost
（）
+
cost_cr
。
spatial_potential_cost
（）
+
pre_col
［
r_pre
］。
total_cost
（）
+
CalculateEdgeCostForThirdCol
（
r
，
r_pre
，
curr_speed_limit
，
cruise_speed
）；
if
（
cost
<
cost_cr
。
total_cost
（））
{
cost_cr
。
SetTotalCost
（
cost
）；
cost_cr
。
SetPrePoint
（
pre_col
［
r_pre
］）；
//找到每一個當前點的pre_point
cost_cr
。
SetOptimalSpeed
（
pre_col
［
r_pre
］。
GetOptimalSpeed
（）
+
curr_a
*
unit_t_
）；
}
}
return
；
}
// 第3列，及以後列的處理
// 依據重新算得的cost，當前點的pre_point，也就是DP過程的狀態轉移方程
for
（
uint32_t
i
=
0
；
i
<
r_pre_size
；
++
i
）
{
uint32_t
r_pre
=
r
-
i
；
if
（
std
：：
isinf
（
pre_col
［
r_pre
］。
total_cost
（））
||
pre_col
［
r_pre
］。
pre_point
（）
==
nullptr
）
{
continue
；
}
const
double
curr_a
=
2
*
（（
cost_cr
。
point
（）。
s
（）
-
pre_col
［
r_pre
］。
point
（）。
s
（））
/
unit_t_
-
pre_col
［
r_pre
］。
GetOptimalSpeed
（））
/
unit_t_
；
if
（
curr_a
>
max_acceleration_
||
curr_a
<
max_deceleration_
）
{
continue
；
}
if
（
pre_col
［
r_pre
］。
GetOptimalSpeed
（）
+
curr_a
*
unit_t_
<
-
kDoubleEpsilon
&&
cost_cr
。
point
（）。
s
（）
>
min_s_consider_speed
）
{
continue
；
}
if
（
CheckOverlapOnDpStGraph
（
st_graph_data_
。
st_boundaries
（），
cost_cr
，
pre_col
［
r_pre
］））
{
continue
；
}
uint32_t
r_prepre
=
pre_col
［
r_pre
］。
pre_point
（）
->
index_s
（）；
const
StGraphPoint
&
prepre_graph_point
=
cost_table_
［
c
-
2
］［
r_prepre
］；
if
（
std
：：
isinf
（
prepre_graph_point
。
total_cost
（）））
{
continue
；
}
if
（
！
prepre_graph_point
。
pre_point
（））
{
continue
；
}
const
STPoint
&
triple_pre_point
=
prepre_graph_point
。
pre_point
（）
->
point
（）；
//上上上個點
const
STPoint
&
prepre_point
=
prepre_graph_point
。
point
（）；
//上上個點
const
STPoint
&
pre_point
=
pre_col
［
r_pre
］。
point
（）；
//上個點
const
STPoint
&
curr_point
=
cost_cr
。
point
（）；
//當前點
curr_speed_limit
=
std
：：
fmin
（
curr_speed_limit
，
speed_limit_by_index_
［
r_pre
］）；
double
cost
=
cost_cr
。
obstacle_cost
（）
+
cost_cr
。
spatial_potential_cost
（）
+
pre_col
［
r_pre
］。
total_cost
（）
+
CalculateEdgeCost
（
triple_pre_point
，
prepre_point
，
pre_point
，
curr_point
，
curr_speed_limit
，
cruise_speed
）；
if
（
cost
<
cost_cr
。
total_cost
（））
{
cost_cr
。
SetTotalCost
（
cost
）；
cost_cr
。
SetPrePoint
（
pre_col
［
r_pre
］）；
cost_cr
。
SetOptimalSpeed
（
pre_col
［
r_pre
］。
GetOptimalSpeed
（）
+
curr_a
*
unit_t_
）；
}
}
}
4。5 CalculateEdgeCost（ ）、CalculateEdgeCostForSecondCol（ ）、CalculateEdgeCostForThirdCol（）
不同型別的EdgeCost的計算
4。6 RetrieveSpeedProfile（ ） 順藤摸瓜，找到最優S-T曲線
需要注意
回溯起點的選擇
遍歷每一列的最後一個點，
找正在的best_end_point
，並更新min_cost
這裡不直接使用最後一列的min_cost點作為終點
因為取樣時間是一個預估時間視窗，在這之前可能就到達終點了
/*************************************************************************************
*
* 回溯，得到最優 speed_data
*
* **********************************************************************************/
Status
GriddedPathTimeGraph
：：
RetrieveSpeedProfile
（
SpeedData
*
const
speed_data
）
{
double
min_cost
=
std
：：
numeric_limits
<
double
>：：
infinity
（）；
const
StGraphPoint
*
best_end_point
=
nullptr
；
// 從cost_table_最後一列找 min_cost
for
（
const
StGraphPoint
&
cur_point
：
cost_table_
。
back
（））
{
。。。
}
// 遍歷每一列的最後一個點，找正在的best_end_point，並更新min_cost
// 這裡不直接使用最後一列的min_cost點作為終點
// 因為取樣時間是一個預估時間視窗，在這之前可能就到達終點了
for
（
const
auto
&
row
：
cost_table_
）
{
const
StGraphPoint
&
cur_point
=
row
。
back
（）；
if
（
！
std
：：
isinf
（
cur_point
。
total_cost
（））
&&
cur_point
。
total_cost
（）
<
min_cost
）
{
best_end_point
=
&
cur_point
；
min_cost
=
cur_point
。
total_cost
（）；
}
}
。。。
// 回溯，得到最優的 speed_profile
std
：：
vector
<
SpeedPoint
>
speed_profile
；
const
StGraphPoint
*
cur_point
=
best_end_point
；
while
（
cur_point
！=
nullptr
）
{
。。。
}
std
：：
reverse
（
speed_profile
。
begin
（），
speed_profile
。
end
（））；
。。。
// 計算每個點的速度 v
for
（
size_t
i
=
0
；
i
+
1
<
speed_profile
。
size
（）；
++
i
）
{
const
double
v
=
（
speed_profile
［
i
+
1
］。
s
（）
-
speed_profile
［
i
］。
s
（））
/
（
speed_profile
［
i
+
1
］。
t
（）
-
speed_profile
［
i
］。
t
（）
+
1e-3
）；
speed_profile
［
i
］。
set_v
（
v
）；
}
*
speed_data
=
SpeedData
（
speed_profile
）；
return
Status
：：
OK
（）；
}
5、StGraphPoint、DpStCost 輔助的資料結構
StGraphPoint
這個Class定義了取樣點的資料結構，以及一些Set（ ）、Get（ ）的方法
DpStCost
實現了取樣點Cost的具體計算過程，具體提供下面幾類cost
ObstacleCost
SpatialPotentialCost
Speedcost、AccelCost、JerkCost，三者構成EdgeCost
