前言
决策规划的路径，除了能够满足车辆动力学的限制，最基本的一点是需要规划出合理的路线避开障碍物，然后在道路可行域内建立目标函数进行求解，这是一件很难做到的事情，因为我们大多数的优化方法都建立在一个基本的条件上，那就是可行域是一个凸空间，而我们目前面临的避障问题是一个典型的非凸问题。如下图，在可行域(道路路面除开障碍物的区域)内任取两个点，这两个点之间的连线却不一定在可行域内。
为了解决这个问题，一个常见的方法就是将非凸优化转化为凸优化，以一个简单的二维非凸函数为例：
我们在可行的轴区间上随机的取点（图中红色的点），然后比较这些点的目标函数值，取最小的那个点，以他为中心开辟一个凸空间，再在这个凸空间上去做凸优化，进而找到真正的全局最小值，但是不能保证找到，我们随机抽取的点数越多，找到真正的全局最小值的概率就越大。
我们现在将这个“离散化”、“随机抽样”的思想应用到路径规划中来：
我们在自然空间下，沿着参考线的方向(道路纵向)将可行域划分为层，道路横向划分为行，例子中是4行*6层，我们需要求出一条路径，这条路径从规划起点出发，依次经过每一层，到达第6层，其路径开销最小。
动态规划问题描述
主体框架
前面描述的问题其实就是一个最短路问题，最短路的求解算法很多，诸如迪杰斯特拉算法、弗洛伊德算法、A*算法等，但是这里的求解不需要用到这些复杂的方法，该问题有一个显著的特点：不可能出现横向移动的路段：假设我们已经知道了从规划起点到第5层各个点的最短路径，那么我要计算规划起点到第6层各个点的最短路径，我只需要考虑第5层各个点到第6层各个点的路段开销即可，依次类推，我们只要用到前一层的最短路信息以及前一层与当前层的路段权重信息即可，是一个动态规划问题。
对于上述问题，对于一个行*层的问题，设：
矩阵：表示规划起点到第行第层点的最小路径开销
矩阵：表示规划起点到第行第层点的最短路径的前序点的行号，即前序点的位置为
表示规划起点，表示第行第列(层)的节点，表示两个相邻层的节点之间的路径开销
伪代码逻辑如下：
i.初始化，计算规划起点到第一层各点的路段开销，求得:
ii.遍历每一层(),如图：
图示举例：
从规划起点到层第1行的最短路径的前序层行号为3
从规划起点到层第2行的最短路径的前序层行号为2
所以矩阵标记如图：
我们需要计算相邻两层之间的前序层的任一个节点到后序层的最短路径，假设我们所处的是层，我们已知从起点出发到层任一节点的最短路径开销，那么从起点出发到达层任一节点的最短路径如何计算？仅需遍历后序层任一节点，对每一个后序层节点k，计算所有前序层节点到该点的路径开销，找到最小值，然后记录该前序层节点的行号即可。
相邻层间节点路径拟合
相邻节点之间的路径使用五次多项式拟合（为什么要使用5次多项式呢？）。
待求的系数项，在方程两边同时乘矩蓝色矩阵的逆即可求得。
路段代价计算
两个节点之间的路段代价，考虑由以下三部分组成：平滑代价项、参考线代价项、障碍物距离代价项。在拟合的五次曲线中采样个点来代表该曲线。
平滑代价
对于一个五次多项式，考虑其平滑代价为一阶导数项、二阶导数项、三阶导数项构成，某路段的某采样点的平滑代价、该路段的平滑代价：
参考线代价
路段上的点的坐标就代表了其距离参考线的远近，所以某路段的参考线代价：
障碍物代价
首先定义某点到障碍物的距离代价函数,简单考虑,设车辆中心点到障碍物中心点在空间下的距离为
若: 3 < dis < 4: f(dis) = 1000 / (dis * dis)
若: dis >= 4: f(dis) = 0
若: dis =< 3: f(dis) = 1e8
设共有个障碍物，某路段上的某点到第个障碍物的代价为，某路段到所有障碍物的距离代价为：
dp代码实现
import numpy as np
import sympy as sp
import matplotlib.pyplot as plt
from icecream import ic
import time
def plan(row_num=3, col_num=3, sample_s=1, sample_l=1, obs_s_set=None, obs_l_set=None, sample_num=15,
plan_start_s=None, plan_start_l=None, plan_start_dl=None,
plan_start_ddl=None, w_smooth=1.0, w_obstacle=9999,
w_reference=1.0,
w_dl=1.0, w_ddl=0.8, w_dddl=0.75):
# node_cost_mat矩阵, 第i行第j列表示起点到该点的最短cost
node_cost_mat = np.array([[np.inf for i in range(0, col_num)] for j in range(0, row_num)])
# pre_index_mat, 第i行第j列的值为: 到达该单元格的最短路径的前序节点的行号
pre_index_mat = np.array([[0 for i in range(0, col_num)] for j in range(0, row_num)])
# 生成格点, 得到格点的坐标字典
grid_loc_dict, node_x, node_y = generate_grid(row_num=row_num, col_num=col_num, sample_l=sample_l, sample_s=sample_s)
# 或者起点到第一列格点的权重
first_cost = {row: cost for row, cost in zip(range(0, row_num), [calc_path_cost(sample_num=sample_num,
start_sl=(0, 0),
end_sl=grid_loc_dict[(_row, 0)],
dl_s0=0.12, ddl_s0=0,
dl_s1=0, ddl_s1=0,
w_dl=w_dl, w_ddl=w_ddl,
w_dddl=w_dddl,
w_obstacle=w_obstacle,
obs_s_set=obs_s_set,
obs_l_set=obs_l_set)
for _row in range(0, row_num)])}
ic(first_cost)
# 当前层到下一层的cost, 当前层第i行到下一层第j行的cost
# 计算起点到第一层的最短路径
for row in range(0, row_num):
node_cost_mat[row, 0] = first_cost[row]
# 分列分层计算
for col in range(0, col_num - 1):
other_cost_mat = {((row_now, col), (row_post, col + 1)): calc_path_cost(sample_num=sample_num,
start_sl=grid_loc_dict[(row_now, col)],
start_loc=(row_now, col),
end_loc=(row_post, col + 1),
end_sl=grid_loc_dict[
(row_post, col + 1)],
dl_s0=0, ddl_s0=0, dl_s1=0, ddl_s1=0,
w_dl=w_dl, w_ddl=w_ddl,
w_dddl=w_dddl, obs_s_set=obs_s_set,
obs_l_set=obs_l_set,
w_smooth=w_smooth,
w_obstacle=w_obstacle,
w_reference=w_reference)
for row_post in range(0, row_num) for row_now in range(0, row_num)}
ic(f'第{col}列到{col + 1}的权重矩阵:')
# ic(other_cost_mat)
# 计算到达第row行第 col + 1列的最小cost
# 该cost需要遍历当前列的所有行
# 对于下一列的每一行
for row_post in range(0, row_num):
_min_cost = np.inf
# 都要遍历本列的每一行
for row_now in range(0, row_num):
# 起始点到当前节点的最短cost
_min_start_to_now_node = node_cost_mat[row_now, col]
print(f'从第{row_now}行第{col}列到第{row_post}行第{col + 1}列的最小cost:{other_cost_mat[(row_now, col), (row_post, col + 1)]}')
print(f'从起始节点到第{row_now}行第{col}列的cost:{_min_start_to_now_node}')
# 如从当前位置到下一层的第row_post行的cost + 起点到当前位置的cost < 最小值
if other_cost_mat[(row_now, col), (row_post, col + 1)] + _min_start_to_now_node < _min_cost:
_min_cost = other_cost_mat[(row_now, col), (row_post, col + 1)] + _min_start_to_now_node
pre_index_mat[row_post, col + 1] = row_now
node_cost_mat[row_post, col + 1] = _min_cost
else:
pass
print(f'从起始节点到第{row_post}行第{col + 1}列的最小cost:{_min_start_to_now_node}')
print('********')
ic(pre_index_mat)
print(node_cost_mat)
# 最短路径
min_val_row_index = np.where(node_cost_mat[:, col_num - 1] == node_cost_mat[:, col_num - 1].min())[0][0]
path_list_reverse = [(min_val_row_index, col_num - 1)]
start_row_num = path_list_reverse[0][0]
start_col_num = path_list_reverse[0][1]
while start_col_num > 0:
pre_row_index = pre_index_mat[start_row_num, start_col_num]
path_list_reverse.append((pre_row_index, start_col_num - 1))
start_col_num -= 1
start_row_num = pre_row_index
path_list_reverse = path_list_reverse[::-1]
# print(path_list_reverse)
# 生成路径的坐标序列
dp_path_init = [(plan_start_s + (row_col[1] + 1) * sample_s,
plan_start_l + sample_l * (row_num - 1) / 2 - row_col[0] * sample_l) for row_col in path_list_reverse]
dp_path_s_init = [dp_path[0] for dp_path in dp_path_init]
dp_path_l_init = [dp_path[1] for dp_path in dp_path_init]
ic(path_list_reverse)
# 增密采样60个
dp_path_final_s, dp_path_final_l, dp_path_final_dl, dp_path_final_ddl = dp_path_densification(
dp_path_s_init=dp_path_s_init,
dp_path_l_init=dp_path_l_init,
plan_start_s=plan_start_s,
plan_start_l=plan_start_l,
plan_start_dl=plan_start_dl,
plan_start_ddl=plan_start_ddl)
return dp_path_final_s, dp_path_final_l, dp_path_final_dl, dp_path_final_ddl, node_x, node_y
def calc_curve_coeff(s0=None, l0=None, dl_s0=None, ddl_s0=None,
s1=None, l1=None, dl_s1=None, ddl_s1=None):
dl_s1 = 0 if dl_s1 is None else dl_s1
ddl_s1 = 0 if ddl_s1 is None else ddl_s1
ddl_s0 = 0 if ddl_s0 is None else ddl_s0
A = np.array(([1, s0, s0 ** 2, s0 ** 3, s0 ** 4, s0 ** 5],
[1, s1, s1 ** 2, s1 ** 3, s1 ** 4, s1 ** 5],
[0, 1, 2 * s0, 3 * s0 ** 2, 4 * s0 ** 3, 5 * s0 ** 4],
[0, 1, 2 * s1, 3 * s1 ** 2, 4 * s1 ** 3, 5 * s1 ** 4],
[0, 0, 2, 6 * s0, 12 * s0 ** 2, 20 * s0 ** 3],
[0, 0, 2, 6 * s1, 12 * s1 ** 2, 20 * s1 ** 3]))
b = np.array([l0, l1, dl_s0, dl_s1, ddl_s0, ddl_s1])
try:
A_inv = np.linalg.inv(A)
except:
A_inv = np.linalg.pinv(A)
coefficient = np.matmul(A_inv, b)
return coefficient
def calc_path_cost(sample_num=50, start_sl=None, end_sl=None, dl_s0=None, ddl_s0=None,
dl_s1=None, ddl_s1=None, w_dl=None, w_ddl=None, w_dddl=None,
obs_s_set=None, obs_l_set=None, w_smooth=1.0, w_obstacle=1.0,
w_reference=1.0, start_loc=None, end_loc=None):
"""
在自然坐标系下计算两点之间曲线路径的cost
:param sample_num: int, 曲线采样点数目
:param dl_s0: float, 起点dl/ds值, l对s的一阶导
:param ddl_s0: float, 起点ddl/ds值, l对s的二阶导
:param dl_s1: float, 终点ddl/ds值, l对s的二阶导
:param ddl_s1: float, 终点ddl/ds值, l对s的二阶导
:param w_dl: float, 平滑代价中一阶导部分的权重系数
:param w_ddl: float, 平滑代价中二阶导部分的权重系数
:param w_dddl: float, 平滑代价中三阶导部分的权重系数
:param obs_s_set: list, 障碍物坐标s集合
:param obs_l_set: list, 障碍物坐标l集合
:param w_smooth: float, 曲线平滑代价权重系数
:param w_obstacle: float, 障碍物距离代价权重系数
:param w_reference: float, 参考线距离代价权重系数
:param start_sl: tuple, 起点sl坐标
:param end_sl: tuple, 终点sl坐标
:return: float, path_cost
"""
# s坐标采样
sample_s = np.linspace(start_sl[0], end_sl[0], num=sample_num)
# 曲线起点s坐标, l坐标, 曲线终点s坐标, l坐标
s0, l0, s1, l1 = start_sl[0], start_sl[1], end_sl[0], end_sl[1]
# 计算曲线平滑代价
curve_smooth_cost, sample_l = calc_curve_smooth_cost(s0=s0, l0=l0, dl_s0=dl_s0, ddl_s0=ddl_s0, s1=s1, l1=l1,
dl_s1=dl_s1, ddl_s1=ddl_s1, w_dl=w_dl, w_ddl=w_ddl,
w_dddl=w_dddl,
sample_s=sample_s)
# 计算曲线的障碍物代价
# print(f'计算路径与障碍物的代价')
# print(f'路径在{start_sl}->{end_sl}')
# print(f'{start_loc, end_loc}')
obstacle_dis_cost = calc_obstacle_cost(obs_s_set=obs_s_set, obs_l_set=obs_l_set,
sample_s=sample_s, sample_l=sample_l)
# print(obstacle_dis_cost)
# 计算
reference_cost = calc_reference_cost(sample_l=sample_l)
# ic(f'{(s0, l0)} --->> {(s1, l1)}: cost: {w_smooth * curve_smooth_cost + w_obstacle * obstacle_dis_cost + w_reference * reference_cost}')
# print(f'总代价{w_smooth * curve_smooth_cost + w_obstacle * obstacle_dis_cost + w_reference * reference_cost}')
return w_smooth * curve_smooth_cost + w_obstacle * obstacle_dis_cost + w_reference * reference_cost
def calc_curve_smooth_cost(sample_s=None, s0=None, l0=None,
dl_s0=None, ddl_s0=None, s1=None, l1=None,
dl_s1=None, ddl_s1=None, w_dl=None, w_ddl=None, w_dddl=None):
# 先生成曲线表达式
[a0, a1, a2, a3, a4, a5] = calc_curve_coeff(s0=s0, l0=l0, dl_s0=dl_s0, ddl_s0=ddl_s0,
s1=s1, l1=l1, dl_s1=dl_s1, ddl_s1=ddl_s1)
s = sp.symbols('s')
l = a0 + a1 * s + a2 * s ** 2 + a3 * s ** 3 + a4 * s ** 4 + a5 * s ** 5
# 一二三阶导数
dl = sp.diff(l, s)
ddl = sp.diff(dl, s)
dddl = sp.diff(ddl, s)
smooth_cost_dl = w_dl * np.array([(dl.evalf(subs={s: val}))**2 for val in sample_s]).sum()
smooth_cost_ddl = w_ddl * np.array([(ddl.evalf(subs={s: val}))**2 for val in sample_s]).sum()
smooth_cost_dddl = w_dddl * np.array([(dddl.evalf(subs={s: val}))**2 for val in sample_s]).sum()
return smooth_cost_dl + smooth_cost_ddl + smooth_cost_dddl, np.array([l.evalf(subs={s: val}) for val in sample_s])
def calc_obstacle_cost(obs_s_set=None, obs_l_set=None, sample_s=None, sample_l=None):
if obs_s_set is None:
return 0
else:
cost = sum([obstacle_cost_function_a(dis_square_to_obstacle=(_s - _ob_s) ** 2 + (_l - _ob_l) ** 2)
for _s, _l in zip(sample_s, sample_l) for _ob_s, _ob_l in zip(obs_s_set, obs_l_set)])
return cost
def obstacle_cost_function(dis_square_to_obstacle=None):
remote_dis = 36
close_dis = 1
inf_cost = 9999
k = inf_cost / (close_dis - remote_dis)
b = -k * remote_dis
if dis_square_to_obstacle >= remote_dis:
return 0
elif dis_square_to_obstacle < close_dis:
return inf_cost
else:
return k * dis_square_to_obstacle + b
def obstacle_cost_function_a(dis_square_to_obstacle=None):
if 9 < dis_square_to_obstacle < 16:
return 1000 / dis_square_to_obstacle
elif dis_square_to_obstacle >= 16:
return 0
else:
return 1e8
def calc_reference_cost(sample_l=None):
return (sample_l ** 2).sum()
def check_first_curve(a0=None, a1=None, a2=None, a3=None, a4=None, a5=None,
s0=None, l0=None, dl_s0=None, ddl_s0=None,
s1=None, l1=None, dl_s1=None, ddl_s1=None):
x = sp.symbols('x')
f = a0 + a1 * x + a2 * x ** 2 + a3 * x ** 3 + a4 * x ** 4 + a5 * x ** 5
df = sp.diff(f, x)
ddf = sp.diff(df, x)
x_val = np.linspace(s0, s1, num=int(np.abs(s1 - s0) * 60))
y = np.array([f.evalf(subs={x: val}) for val in x_val])
plt.scatter(x_val, y)
plt.show()
pass
def generate_grid(row_num=3, col_num=3, sample_s=None, sample_l=None):
x = [0]
y = [0]
loc_dict = dict()
for row in range(0, row_num):
for col in range(0, col_num):
_y = sample_l * (row_num - 1) / 2 - row * sample_l
_x = (col + 1) * sample_s
loc_dict[(row, col)] = (_x, _y)
x.append(_x)
y.append(_y)
return loc_dict, x, y
def dp_path_densification(dp_path_s_init=None,
dp_path_l_init=None,
plan_start_s=None,
plan_start_l=None,
plan_start_dl=None,
plan_start_ddl=None):
# 每隔1m取一个轨迹点
ds = 1
# 只会规划规划起点往前60m的轨迹
dp_path_final_s = [-1] * 60
dp_path_final_l = [-1] * 60
dp_path_final_dl = [-1] * 60
dp_path_final_ddl = [-1] * 60
# 用于存储第一次采样的结果
temp_s, temp_l, temp_dl, temp_ddl = [], [], [], []
s_cur = []
start_s, start_l, start_dl, start_ddl = plan_start_s, plan_start_l, plan_start_dl, plan_start_ddl
for seq in range(0, len(dp_path_s_init)):
# 在start点和end点之间采样
end_s, end_l, end_dl, end_ddl = dp_path_s_init[seq], dp_path_l_init[seq], 0, 0
for j in range(0, 10000000):
_s = start_s + j * ds
if _s < dp_path_s_init[seq]:
s_cur.append(_s)
temp_s.append(_s)
continue
else:
break
# 计算五次多项式
# 先生成曲线表达式
[a0, a1, a2, a3, a4, a5] = calc_curve_coeff(s0=start_s, l0=start_l, dl_s0=start_dl, ddl_s0=start_ddl,
s1=end_s, l1=end_l, dl_s1=end_dl, ddl_s1=end_ddl)
s = sp.symbols('s')
l = a0 + a1 * s + a2 * s ** 2 + a3 * s ** 3 + a4 * s ** 4 + a5 * s ** 5
# 一二三阶导数
dl = sp.diff(l, s)
ddl = sp.diff(dl, s)
temp_l += [l.evalf(subs={s: val}) for val in s_cur]
temp_dl += [dl.evalf(subs={s: val}) for val in s_cur]
temp_ddl += [ddl.evalf(subs={s: val}) for val in s_cur]
# 此次的end节点作为下一次的start起点
start_s, start_l, start_dl, start_ddl = end_s, end_l, end_dl, end_ddl
s_cur = []
# 从temp_s, temp_l, temp_dl, temp_ddl中取60个点
if len(temp_s) < 60:
pass
else:
dp_path_final_s = temp_s[0:61]
dp_path_final_l = temp_l[0:61]
dp_path_final_dl = temp_dl[0:61]
dp_path_final_ddl = temp_ddl[0:61]
return dp_path_final_s, dp_path_final_l, dp_path_final_dl, dp_path_final_ddl
