|
本文介绍如果使用以state lattice planner为基础的曲线生成和动态障碍物规避的方法。
我们的曲线将position profile 和 velocity profile进行了分离。位置的优化用曲线生成和cost function minimization的方法，纵向的速度规划采用ACC控制器。 主要采用我的论文：Optimization of Adaptive Cruise Control system Controller: using Linear Quadratic Gaussian based on Genetic Algorithm 横向位置的规划原理上采用state lattice planner, 对起始点和终末点的状态进行采样。 生成大量曲线，生成的曲线可以是五阶曲线，三阶曲线，贝塞尔曲线，最后我决定暂时使用贝塞尔三阶曲线，原因等等会分析。具体的方法看我的论文A Dynamic Motion Planning Framework for Autonomous Driving in Urban Environments其中部分方法并不是下文中的规划方法，主要考虑的问题是实际测试中的缺陷。改的比较多的是曲线由五阶曲线变成了贝塞尔三阶。
第二步是根据车辆动力学运动学，驾驶员舒适度等因素生成一个合理的cost function, 这个cost function 可以很复杂，考虑到尽量多的因素。
最后一步就是collision checking，检查生成出来的曲线与障碍物是否有碰撞。如果有碰撞就找cost function第二低的次优曲线进行碰撞检测。如此类推。
整体上的思路还是很简单的，实现过程中需要注意的问题有这么几个：1，曲线到底怎么选。2，cost function weight如何设置很有讲究。3，不同车速下的设置也要有所变化。
我们先来看看曲线怎么设计： 有这么几个主流选项，三次多项式，五次多项式，贝塞尔三阶，贝塞尔五阶 宝马那个叫weling的小胸弟用了五阶多项式，涉嫌操纵数据，说实话五阶多项式的效果并不是那么好。 他的论文里举的例子  这张图还是精心挑选出来的，我尝试了一下，车子开十公里的速度，画出位置曲线如下：  看上去还不错是吧，向前瞄了7m,但是理论上，我们规划是不允许只预留这么短距离的，至少要往前看15m以上，百度也是这么建议的，虽然他们水平一般，但这个建议还算中肯。于是我把采样时间的采样区间从2:2.5放大到了5:10，从5秒看到了10秒，在画图：  很明显，看的稍微远一点，到15m以上的时候，曲线初始段的横向偏离就极大了，再往外一点直接超出车道线，还玩妹啊。操纵数据的胸弟不得好死。
再看看车速稍微在快点，跑到20KPH,如图：  还是一样，初始端的偏离非常大，只有看很短的采样时间才能解决这个问题，实际操作中根本不允许。
没办法，我们再看看三阶曲线，至少二阶可导，加速度还是可以连续的： 这里使用三阶曲线，只能有四个变量，起末点的位置是一个，；另外两个可以选择起末点的速度或者加速度 我们先选加速度 复现刚刚的条件如图（把起始偏移量修改成2m,别和路中心线偏移太多）：  还是最大偏移量的问题，曲线根本不能用。
再试试用速度做变量：假设速度最后跑到了2.7m/s，也就是10kph左右：  继续爆炸，看来三阶多项式也不好使了。
最后还是选用贝塞尔试试把，观察了一下Bezier(贝塞尔)曲线的轨迹规划在自动驾驶中的应用(一)  说实话实际应用中三阶的贝塞尔曲线效果更好，原因自己去想。
于是我们最后觉得使用三阶曲线。
对于cost function的选取，主要参考到了Gu Dolan那帮人的论文，具体的也可以去看看我的论文上面链接，至于怎么选取各项的weight自己去试吧，不透露。
最后仿真效果还行，先有预定轨迹和障碍物：  我们设定了一个三阶多项式组成的曲线，黑点表示路上的障碍物，看看车子从起始点出发后的效果：  蓝色部分就是撒出去的曲线。 代码如下，其中weight随意选了一个，具体设置不予透露：
clc
clear all
format short
k = 0.1; % look forward gain
Lfc = 0.1; % look-ahead distance
Kp = 1.0 ; % speed propotional gain
dt = 0.1 ;% [s]
L = 2.8 ;% [m] wheel base of vehicle
MAX_SPEED = 50.0 / 3.6; % 最大速度 [m/s]
MAX_ACCEL = 1.0 ; % 最大加速度[m/ss]
MAX_CURVATURE = 1.0 ; %最大曲率 [1/m]
MAX_ROAD_WIDTH =1.2 ; % 最大道路宽度 [m]
D_ROAD_W = 0.2 ; % 道路宽度采样间隔 [m]
DT = 0.1 ; % Delta T [s]
MAXT = 10;% 最大预测 [s]
MINT = 6; % 最小预测 [s]
TARGET_SPEED = 12.0 / 3.6 ; % 目标速度（即纵向的速度保持） [m/s]
D_T_S = 5.0 / 3.6 ; % 目标速度采样间隔 [m/s]
N_S_SAMPLE = 1 ; % sampling number of target speed
ROBOT_RADIUS = 0.4 ; % robot radius [m]
predict =15;
% 损失函数权重
KJ = 0.1;
KT = 0.1;
KD = 10;
KLAT = 1.0;
KLON = 1.0;
wx = [0.0, 10.0, 20.5, 30.0, 40.5, 50.0, 60.0];
wy = [0.0, -4.0, 1.0, 6.5, 8.0, 10.0, 6.0];
ob = [ 4.2,-2.4;22,2.4;20.0, 10.0;30.0, 6.0;35.0, 7.0;30.0, 5.0;22,2;36,7.5;38,7.6;50.0, 12.0;14,-3];
[xt, yt,YAW] = cubic_spline(wx,wy);
plot(ob(:,1),ob(:,2),'ko')
plot(ob(:,1),ob(:,2),'ks')
plot(ob(:,1),ob(:,2),'k*')
xt = xt';
yt = yt';
i = 1;
target_speed = 10/3.6;
T = 25;
x = 0; y = 0; yaw = 0; v = 0;
time = 0;
% Lf = k * v + Lfc; %some constant parameters we set for the vehicle
waypoints = [xt,yt];
cx = waypoints(:,1);
cy = waypoints(:,2);
plot(cx,cy,'b')
c_d = y;% 当前的位置 [m]
c_d_d = 0.0; %当前速度 [m/s]
c_d_dd = 0.0 ; % 当前加速度 [m/s2]
while T > time
[frenet_paths] = calc_frenet_paths(c_d, c_d_d, c_d_dd,MAX_ROAD_WIDTH,...
D_ROAD_W, MINT,DT,MAXT,KJ,KD,KT);
D0= c_d;
[lat, current_ind]= calc_current_index(x,y,cx,cy); %find the current location using reference line index to indicate.
current_ind
[val,dist] = collision_checking(frenet_paths,c_d, c_d_d, c_d_dd,DT,ROBOT_RADIUS, ob,cx,cy,YAW,current_ind );
% val= 1;
% val=1;
Di = frenet_paths(val,3);
Ti = frenet_paths(val,2);
i = 1;
S= current_ind;
d = [];
steer = [];
location_ind = [];
s = 0;
while s MAX_CURVATURE for c in fplist[i].c]): # 最大曲率检查
continue
elif not check_collision(fplist[i], ob):
continue
okind.append(i)
return [fplist[i] for i in okind]
def frenet_optimal_planning(csp, s0, c_speed, c_d, c_d_d, c_d_dd, ob):
fplist = calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0)
fplist = calc_global_paths(fplist, csp)
fplist = check_paths(fplist, ob)
# 找到损失最小的轨迹
mincost = float("inf")
bestpath = None
for fp in fplist:
if mincost >= fp.cf:
mincost = fp.cf
bestpath = fp
return bestpath
def generate_target_course(x, y):
csp = cubic_spline.Spline2D(x, y)
s = np.arange(0, csp.s[-1], 0.1)
rx, ry, ryaw, rk = [], [], [], []
for i_s in s:
ix, iy = csp.calc_position(i_s)
rx.append(ix)
ry.append(iy)
ryaw.append(csp.calc_yaw(i_s))
rk.append(csp.calc_curvature(i_s))
return rx, ry, ryaw, rk, csp
def main():
# 路线
wx = [0.0, 10.0, 20.5, 30.0, 40.5, 50.0, 60.0]
wy = [0.0, -4.0, 1.0, 6.5, 8.0, 10.0, 6.0]
# 障碍物列表
ob = np.array([[20.0, 10.0],
[30.0, 6.0],
[30.0, 5.0],
[35.0, 7.0],
[50.0, 12.0]
])
tx, ty, tyaw, tc, csp = generate_target_course(wx, wy)
# 初始状态
c_speed = 10.0 / 3.6 # 当前车速 [m/s]
c_d = 4.0 # 当前的d方向位置 [m]
c_d_d = 0.0 # 当前横向速度 [m/s]
c_d_dd = 0.0 # 当前横向加速度 [m/s2]
s0 = 0.0 # 当前所在的位置
area = 20.0 # 动画显示区间 [m]
for i in range(500):
path = frenet_optimal_planning(
csp, s0, c_speed, c_d, c_d_d, c_d_dd, ob)
s0 = path.s[1]
c_d = path.d[1]
c_d_d = path.d_d[1]
c_d_dd = path.d_dd[1]
c_speed = path.s_d[1]
if np.hypot(path.x[1] - tx[-1], path.y[1] - ty[-1]) self.x[-1]:
return None
i = self.__search_index(t)
dx = t - self.x[i]
result = self.a[i] + self.b[i] * dx + \
self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0
return result
def calcd(self, t):
u"""
Calc first derivative
if t is outside of the input x, return None
"""
if t < self.x[0]:
return None
elif t > self.x[-1]:
return None
i = self.__search_index(t)
dx = t - self.x[i]
result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0
return result
def calcdd(self, t):
u"""
Calc second derivative
"""
if t < self.x[0]:
return None
elif t > self.x[-1]:
return None
i = self.__search_index(t)
dx = t - self.x[i]
result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx
return result
def __search_index(self, x):
u"""
search data segment index
"""
return bisect.bisect(self.x, x) - 1
def __calc_A(self, h):
u"""
calc matrix A for spline coefficient c
"""
A = np.zeros((self.nx, self.nx))
A[0, 0] = 1.0
for i in range(self.nx - 1):
if i != (self.nx - 2):
A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1])
A[i + 1, i] = h[i]
A[i, i + 1] = h[i]
A[0, 1] = 0.0
A[self.nx - 1, self.nx - 2] = 0.0
A[self.nx - 1, self.nx - 1] = 1.0
# print(A)
return A
def __calc_B(self, h):
u"""
calc matrix B for spline coefficient c
"""
B = np.zeros(self.nx)
for i in range(self.nx - 2):
B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \
h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i]
# print(B)
return B
class Spline2D:
u"""
2D Cubic Spline class
"""
def __init__(self, x, y):
self.s = self.__calc_s(x, y)
self.sx = Spline(self.s, x)
self.sy = Spline(self.s, y)
def __calc_s(self, x, y):
dx = np.diff(x)
dy = np.diff(y)
self.ds = [math.sqrt(idx ** 2 + idy ** 2)
for (idx, idy) in zip(dx, dy)]
s = [0]
s.extend(np.cumsum(self.ds))
return s
def calc_position(self, s):
u"""
calc position
"""
x = self.sx.calc(s)
y = self.sy.calc(s)
return x, y
def calc_curvature(self, s):
u"""
calc curvature
"""
dx = self.sx.calcd(s)
ddx = self.sx.calcdd(s)
dy = self.sy.calcd(s)
ddy = self.sy.calcdd(s)
k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2)
return k
def calc_yaw(self, s):
u"""
calc yaw
"""
dx = self.sx.calcd(s)
dy = self.sy.calcd(s)
yaw = math.atan2(dy, dx)
return yaw
def calc_spline_course(x, y, ds=0.1):
sp = Spline2D(x, y)
s = list(np.arange(0, sp.s[-1], ds))
rx, ry, ryaw, rk = [], [], [], []
for i_s in s:
ix, iy = sp.calc_position(i_s)
rx.append(ix)
ry.append(iy)
ryaw.append(sp.calc_yaw(i_s))
rk.append(sp.calc_curvature(i_s))
return rx, ry, ryaw, rk, s
注意把需要import的库都下载了
|