自动驾驶使用贝塞尔曲线进行动态障碍物避障测试

本文介绍如果使用以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 shortk = 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 vehicleMAX_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<= Tip(i,:)= Bezierfrenet(D0, Ti, Di, s);
%     steer(i) = YAW(S);location_ind(i) = S;s = s + sqrt((cx(S+1)-cx(S))^2 +  (cy(S+1)-cy(S))^2);S = S+1;i = i+1;endfor i = 1: length(location_ind)trans = BackTransfer(cx(location_ind(i)), cy((location_ind(i))), YAW(location_ind(i)));waypoint(:,i) = trans* [0; p(i,2);1];endref=  waypoint(1:2,:)';path_x = ref(:,1);path_y = ref(:,2);ai = PIDcontrol(target_speed, v,Kp,MAX_ACCEL); % calculate the PID controller output;delta = pure_pursuit_control(x,y,yaw,v,path_x,path_y,k,Lfc,L,predict) ;% pure pursuit controller will give desired steering angle;[x,y,yaw,v] = update(x,y,yaw,v, ai, delta,dt,L); % update the vehicle states;c_d= lat;D0= lat;
%     c_d_d =  v* sin(yaw);
%     c_d_dd = ai;time = time + dt;
%     pause(0.1)plot(x,y,'rs')hold onplot(path_x,path_y,'bs','Markersize',1)hold on%,cx((target_ind-10):target_ind),cy((target_ind-10):target_ind),'g-o'drawnowhold on
endfunction [x, y, yaw, v] = update(x, y, yaw, v, a, delta,dt,L)x = x + v * cos(yaw) * dt;y = y + v * sin(yaw) * dt;yaw = yaw + v / L * tan(delta) * dt;v = v + a * dt;
endfunction [a] = PIDcontrol(target_v, current_v, Kp,MAX_ACCEL)
a = Kp * (target_v - current_v);
a = min(max(a, -MAX_ACCEL), MAX_ACCEL);
endfunction [ trans] =  BackTransfer(x,y,heading_current)theta =  heading_current;
R = [cos(theta),-sin(theta); sin(theta), cos(theta)];
xt = x;
yt = y;
T = [xt; yt ] ;
trans = [[R,T];[0,0,1]];
endfunction [delta] = pure_pursuit_control(x,y,yaw,v,path_x,path_y,k,Lfc,L,predict)
if predict >length(path_x)predict = length(path_x);
endtx =path_x(predict);ty =path_y(predict);denom = tx-x;if denom == 0denom = 0.0001endalpha =atan(ty-y)/((denom))-yaw;Lf = k * v + Lfc;delta = atan(2*L * sin(alpha)/Lf)  ;
endfunction [lat,current_ind]= calc_current_index(x,y, cx,cy)
N =  length(cx);
Distance = zeros(N,1);
for i = 1:N
Distance(i) =  sqrt((cx(i)-x)^2 + (cy(i)-y)^2);
end
[value, location]= min(Distance);
current_ind = location;
lat = value;if cy(current_ind)>ylat = -lat;elselat = lat;end
endfunction [angle] = pipi(angle)if (angle > pi)angle =  angle - 2*pi;
elseif (angle < -pi)angle = angle + 2*pi;
elseangle = angle;
end
end% function [a0,a1,a2,a3] = quadratic_poly(xs,xe,axs,axe,T)
% A = [1 0 0 0; 1 T T^2 T^3; 0 0 2 0; 0 0 2 6*T];
% b = [xs,xe,axs, axe]';
% x = A\b;
% a0 = x(1);
% a1 = x(2);
% a2 = x(3);
% a3 = x(4);
% endfunction [a0, a1, a2, a3, a4,a5] = quintic_polynomial(xs, vxs, axs, xe, vxe, axe,T)
% A = [0,0,0,0,0,1; T^5,T^4,T^3,T^2,T,1;...
%     0,0,0,0,1,0 ; 5*T^4  4*T^3 3*T^2 2*T 1 0 ; ...
%     0 0 0 2 0 0; 20*T^3 12*T^2 6*T 2 0 0];
% b = [xs, xe, vxs, vxe, axs, 0]';
% x = A\b;
% a5 = x(1);
% a4 = x(2);
% a3 = x(3);
% a2 = x(4);
% a1 = x(5);
% a0 = x(6);
A = [T^3 T^4 T^5; 3*T^2 4*T^3 5*T^4; 6*T 12*T^2 20*T^3];
b = [(xe - xs  - vxs*T - 0.5*axs*T^2); (vxe- vxs - axs*T ); (axe - axs)];
x = A\b;
a0 = xs;
a1 = vxs;
a2 = axs/2;a3 = x(1);
a4 = x(2);
a5 = x(3);
endfunction [xt] = calc_point(a0,a1,a2,a3,a4,a5,t)
xt = a0 + a1 * t + a2 * t ^2 + a3 * t^3 + a4 * t^4+a5 * t^5;
endfunction [xt] =calc_first_derivative(a1,a2,a3,a4,a5,t)
xt = a1 + 2 * a2 * t + 3 * a3 * t^2 + 4 * a4 * t^3  +  5 * a5 * t^4;
endfunction [xt]  = calc_second_derivative(a2,a3,a4,a5,t)
xt = 2* a2 + 6* a3 * t + 12 * a4 * t^2 + 20* a5 *t^3;
endfunction [xt] = calc_third_derivative(a3,a4,a5,t)
xt =6 * a3 + 24 * a4 * t + a5 * t^2;
end% function [xt] = calc_point(a0,a1,a2,a3,t)
% xt = a0 + a1 * t + a2 * t ^2 + a3 * t^3 ;
% end% function [xt] =calc_first_derivative(a1,a2,a3,t)
% xt = a1 + 2 * a2 * t + 3 * a3 * t^2 ;
% end
%
% function [xt]  = calc_second_derivative(a2,a3,t)
% xt = 2* a2 + 6* a3 * t ;
% end
%
% function [xt] = calc_third_derivative(a3)
% xt =6 * a3;
% endfunction [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)
%    xs = c_d;
%    axs = c_d_dd;
%    axe = 0;
%    vxe = 0;
%    vxs = c_d_d;
% frenet_paths = [];
% tfp = [];
% T = [];
% D = [];
j = 1;
for di = -MAX_ROAD_WIDTH: D_ROAD_W: MAX_ROAD_WIDTHfor Ti =  MINT: DT: MAXTt = 0: DT: Ti; t = t';
%         for i = 1: length(t)
%         p(i,:)= Bezierfrenet(Ti, Di,t(i));
%         end%         for i = 1: length(t)
%             d_d(i) = calc_first_derivative(a1,a2,a3,a4,a5,t(i));
%         end
%
%         for i = 1: length(t)
%             d_dd(i) = calc_second_derivative(a2,a3,a4,a5,t(i));
%         end
%
%         for i = 1: length(t)
%             d_ddd(i) = calc_third_derivative(a3,a4,a5,t(i));
%         end%         JP= sum(d_ddd.^2);tfp(j) = KT / Ti + KD * abs(di);T(j) =  Ti;D(j) =abs(di);j = j+1; end
endfrenet_paths = [tfp',T',D'];
frenet_paths = sortrows(frenet_paths);
endfunction [val,dist] = collision_checking(frenet_paths,c_d, c_d_d, c_d_dd,DT,ROBOT_RADIUS, ob,cx,cy,YAW,current_ind )
Flag =0;
val= 1;xs = c_d;axs = c_d_dd;axe = 0;vxs = c_d_d;vxe = 0;D0 = c_d;
while Flag == 0Ti = frenet_paths(val,2);Di = frenet_paths(val,3);i = 1;S= current_ind;d = [];steer = [];location_ind = [];s = 0;while  s<= Tip(i,:)= Bezierfrenet(D0, Ti, Di,s);
%     steer(i) = YAW(S);location_ind(i) = S;s = s + sqrt((cx(S+1)-cx(S))^2 +  (cy(S+1)-cy(S))^2);S = S+1;i = i+1;endn= 1;for i = 1: length(location_ind)trans = BackTransfer(cx(location_ind(i)), cy((location_ind(i))), YAW(location_ind(i)));checkline(1:3,i) = trans* [0; p(i,2);1];endcheckline =  checkline(1:2,:)';for j =1:size(ob,1)for k = 1: length(checkline)dist(n) = sqrt((ob(j,1)-checkline(k,1))^2 + ((ob(j,2)-checkline(k,2))^2));n = n+1;endendif min(dist)<ROBOT_RADIUS val = val +1;elseFlag = 1;endif val == length(frenet_paths)Flag = 1;end
end
endfunction [p] = Bezierfrenet(D0, Ti, Di,t)p0 = [ 0 , D0];p1 = [Ti/2, D0];p2= [Ti/2, Di];p3 = [Ti, Di];%设置控制点p= (1-(t)/Ti)^3*p0 + 3*(1-(t)/Ti)^2*(t)/Ti*p1 + 3*(1-(t)/Ti)*((t)/Ti)^2*p2 + ((t)/Ti)^3*p3;endfunction [Px,Py,YAW] = cubic_spline(x,y)figure
%     plot(x,y,'ro');hold onN = length(x);A = zeros(N,N);B = zeros(N,1);for i = 1:N-1h(i) = x(i+1) - x(i);endA(1,1) = 1; A(N,N) = 1;for i = 2:N-1A(i,i) = 2*(h(i-1) + h(i));endfor i  =2 : N-1A(i, i+1) = h(i);endfor i  = 2: N-1A(i,i-1) = h(i-1);endfor i = 2:N-1B(i) = 6* (y(i+1)-y(i))/h(i) - 6* (y(i)-y(i-1))/h(i-1);endm= A\Bfor i = 1:Na(i) = y(i);endfor i = 1:Nc(i) = m(i)/2;endfor i = 1:N-1d(i) =( c(i+1)-c(i) )/(3*h(i));endfor i = 1:N-1b(i)  = (a(i+1)-a(i))/h(i)- h(i)/3*(c(i+1)+ 2*c(i));endPx= [];Py = [];for  i= 1:N-1X = x(i):0.1:x(i+1);Y = a(i)+ b(i)*(X-x(i)) + c(i) * (X- x(i)).^2 + d(i) * (X - x(i)).^3;Px = [Px,X];Py = [Py,Y];plot(X, Y,'g-','LineWidth',3)   endfor i = 1: length(Px)-1yaw(i) = atan((Py(i+1)-Py(i))/(Px(i+1)- Px(i)));endyaw(end+1) = yaw(end);YAW =   yaw;
%        s = zeros(length(Px),1);
%        s(1) = 0;
%        for i = 2: length(Px)
%            s(i) = sqrt((Px(i)-Px(i-1)^2+ Py(i)-Py(i-1)^2);
%            s(i) =s(i-1) +
%        end
%
end

python robotics也有五阶曲线的仿真，但是实际情况测试效果不佳，有兴趣的可以看看：

# coding=utf-8
import numpy as np
import matplotlib.pyplot as plt
import copy
import math
import cubic_spline
import seaborn
import sys
sys.path.append("H:\Project\TrajectoryPlanningModelDesign\Codes\frenet_optimal\frenet_optimal")
import cubic_spline# Parameter
MAX_SPEED = 50.0 / 3.6  # 最大速度 [m/s]
MAX_ACCEL = 2.0  # 最大加速度[m/ss]
MAX_CURVATURE = 1.0  # 最大曲率 [1/m]
MAX_ROAD_WIDTH = 7.0  # 最大道路宽度 [m]
D_ROAD_W = 1.0  # 道路宽度采样间隔 [m]
DT = 0.2  # Delta T [s]
MAXT = 5.0  # 最大预测时间 [s]
MINT = 4.0  # 最小预测时间 [s]
TARGET_SPEED = 30.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 = 2.0  # robot radius [m]# 损失函数权重
KJ = 0.1
KT = 0.1
KD = 1.0
KLAT = 1.0
KLON = 1.0class quintic_polynomial:def __init__(self, xs, vxs, axs, xe, vxe, axe, T):# 计算五次多项式系数self.xs = xsself.vxs = vxsself.axs = axsself.xe = xeself.vxe = vxeself.axe = axeself.a0 = xsself.a1 = vxsself.a2 = axs / 2.0A = np.array([[T ** 3, T ** 4, T ** 5],[3 * T ** 2, 4 * T ** 3, 5 * T ** 4],[6 * T, 12 * T ** 2, 20 * T ** 3]])b = np.array([xe - self.a0 - self.a1 * T - self.a2 * T ** 2,vxe - self.a1 - 2 * self.a2 * T,axe - 2 * self.a2])x = np.linalg.solve(A, b)self.a3 = x[0]self.a4 = x[1]self.a5 = x[2]def calc_point(self, t):xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5return xtdef calc_first_derivative(self, t):xt = self.a1 + 2 * self.a2 * t + \3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4return xtdef calc_second_derivative(self, t):xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3return xtdef calc_third_derivative(self, t):xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2return xtclass quartic_polynomial:def __init__(self, xs, vxs, axs, vxe, axe, T):# 计算四次多项式系数self.xs = xsself.vxs = vxsself.axs = axsself.vxe = vxeself.axe = axeself.a0 = xsself.a1 = vxsself.a2 = axs / 2.0A = np.array([[3 * T ** 2, 4 * T ** 3],[6 * T, 12 * T ** 2]])b = np.array([vxe - self.a1 - 2 * self.a2 * T,axe - 2 * self.a2])x = np.linalg.solve(A, b)self.a3 = x[0]self.a4 = x[1]def calc_point(self, t):xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \self.a3 * t ** 3 + self.a4 * t ** 4return xtdef calc_first_derivative(self, t):xt = self.a1 + 2 * self.a2 * t + \3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3return xtdef calc_second_derivative(self, t):xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2return xtdef calc_third_derivative(self, t):xt = 6 * self.a3 + 24 * self.a4 * treturn xtclass Frenet_path:def __init__(self):self.t = []self.d = []self.d_d = []self.d_dd = []self.d_ddd = []self.s = []self.s_d = []self.s_dd = []self.s_ddd = []self.cd = 0.0self.cv = 0.0self.cf = 0.0self.x = []self.y = []self.yaw = []self.ds = []self.c = []def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):frenet_paths = []# 采样，并对每一个目标配置生成轨迹for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):# 横向动作规划for Ti in np.arange(MINT, MAXT, DT):fp = Frenet_path()# 计算出关于目标配置di，Ti的横向多项式lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)fp.t = [t for t in np.arange(0.0, Ti, DT)]fp.d = [lat_qp.calc_point(t) for t in fp.t]fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]# 纵向速度规划 (速度保持)for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE, TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):tfp = copy.deepcopy(fp)lon_qp = quartic_polynomial(s0, c_speed, 0.0, tv, 0.0, Ti)tfp.s = [lon_qp.calc_point(t) for t in fp.t]tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]Jp = sum(np.power(tfp.d_ddd, 2))  # square of jerkJs = sum(np.power(tfp.s_ddd, 2))  # square of jerk# square of diff from target speedds = (TARGET_SPEED - tfp.s_d[-1]) ** 2# 横向的损失函数tfp.cd = KJ * Jp + KT * Ti + KD * tfp.d[-1] ** 2# 纵向的损失函数tfp.cv = KJ * Js + KT * Ti + KD * ds# 总的损失函数为d 和 s方向的损失函数乘对应的系数相加tfp.cf = KLAT * tfp.cd + KLON * tfp.cvfrenet_paths.append(tfp)return frenet_pathsdef calc_global_paths(fplist, csp):for fp in fplist:# 计算全局位置for i in range(len(fp.s)):ix, iy = csp.calc_position(fp.s[i])if ix is None:breakiyaw = csp.calc_yaw(fp.s[i])di = fp.d[i]fx = ix + di * math.cos(iyaw + math.pi / 2.0)fy = iy + di * math.sin(iyaw + math.pi / 2.0)fp.x.append(fx)fp.y.append(fy)# calc yaw and dsfor i in range(len(fp.x) - 1):dx = fp.x[i + 1] - fp.x[i]dy = fp.y[i + 1] - fp.y[i]fp.yaw.append(math.atan2(dy, dx))fp.ds.append(math.sqrt(dx ** 2 + dy ** 2))fp.yaw.append(fp.yaw[-1])fp.ds.append(fp.ds[-1])# calc curvaturefor i in range(len(fp.yaw) - 1):fp.c.append((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i])return fplistdef check_collision(fp, ob):for i in range(len(ob[:, 0])):d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)for (ix, iy) in zip(fp.x, fp.y)]collision = any([di <= ROBOT_RADIUS ** 2 for di in d])if collision:return Falsereturn Truedef check_paths(fplist, ob):okind = []for i in range(len(fplist)):if any([v > MAX_SPEED for v in fplist[i].s_d]):  # 最大速度检查continueelif any([abs(a) > MAX_ACCEL for a in fplist[i].s_dd]):  # 最大加速度检查continueelif any([abs(c) > MAX_CURVATURE for c in fplist[i].c]):  # 最大曲率检查continueelif not check_collision(fplist[i], ob):continueokind.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 = Nonefor fp in fplist:if mincost >= fp.cf:mincost = fp.cfbestpath = fpreturn bestpathdef 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, cspdef 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]) <= 1.0:print("到达目标")breakplt.cla()plt.plot(tx, ty, "r")plt.plot(ob[:, 0], ob[:, 1], "ob")plt.plot(path.x[1:], path.y[1:], "-og")plt.plot(path.x[1], path.y[1], "vc")plt.xlim(path.x[1] - area, path.x[1] + area)plt.ylim(path.y[1] - area, path.y[1] + area)plt.title("speed[km/h]:" + str(c_speed * 3.6)[0:4])plt.grid(True)plt.pause(0.0001)plt.grid(True)plt.pause(0.0001)plt.show()if __name__ == '__main__':main()

其中的cubic function :

import math
import numpy as np
import bisectclass Spline:u"""Cubic Spline class"""def __init__(self, x, y):self.b, self.c, self.d, self.w = [], [], [], []self.x = xself.y = yself.nx = len(x)  # dimension of xh = np.diff(x)# calc coefficient cself.a = [iy for iy in y]# calc coefficient cA = self.__calc_A(h)B = self.__calc_B(h)self.c = np.linalg.solve(A, B)#  print(self.c1)# calc spline coefficient b and dfor i in range(self.nx - 1):self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i]))tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \(self.c[i + 1] + 2.0 * self.c[i]) / 3.0self.b.append(tb)def calc(self, t):u"""Calc positionif t is outside of the input x, return None"""if t < self.x[0]:return Noneelif t > self.x[-1]:return Nonei = 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.0return resultdef calcd(self, t):u"""Calc first derivativeif t is outside of the input x, return None"""if t < self.x[0]:return Noneelif t > self.x[-1]:return Nonei = 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.0return resultdef calcdd(self, t):u"""Calc second derivative"""if t < self.x[0]:return Noneelif t > self.x[-1]:return Nonei = self.__search_index(t)dx = t - self.x[i]result = 2.0 * self.c[i] + 6.0 * self.d[i] * dxreturn resultdef __search_index(self, x):u"""search data segment index"""return bisect.bisect(self.x, x) - 1def __calc_A(self, h):u"""calc matrix A for spline coefficient c"""A = np.zeros((self.nx, self.nx))A[0, 0] = 1.0for 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.0A[self.nx - 1, self.nx - 2] = 0.0A[self.nx - 1, self.nx - 1] = 1.0#  print(A)return Adef __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 Bclass 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 sdef calc_position(self, s):u"""calc position"""x = self.sx.calc(s)y = self.sy.calc(s)return x, ydef 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 kdef calc_yaw(self, s):u"""calc yaw"""dx = self.sx.calcd(s)dy = self.sy.calcd(s)yaw = math.atan2(dy, dx)return yawdef 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的库都下载了

自动驾驶使用贝塞尔曲线进行动态障碍物避障测试相关推荐

自动驾驶使用贝塞尔曲线进行静态障碍物避障测试
首先规划出reference line, 探测到障碍物后,进行贝塞尔曲线轨迹规划,绕开障碍物,给出新的reference line, 然后根据pure pursuit模型进行软件在环测试,pure p ...
机器人局部避障的动态窗口法DWA (dynamic window approach)-matlab代码修改及动态障碍物避障效果
具体效果视频:[DWA动态障碍物-哔哩哔哩] https://b23.tv/pQp6ne 一.源码及问题最初的源码链接https://blog.csdn.net/heyijia0327/articl ...
已知环境静态障碍物避障_我女儿如何教我无障碍环境
已知环境静态障碍物避障 by Drew 通过德鲁我女儿如何教我无障碍环境 (How my daughter taught me about accessibility) 在过去的几个月里,花了很多时 ...
自动驾驶轨迹生成-贝塞尔(Bézier)曲线
引言最近刚看完贝塞尔曲线,工作就遇到了相应的需求,所以写一下过程.主要讲的是自动驾驶中,车换道时用到贝塞尔曲线,当然其他的很多领域也会有,例如图形学等. 在车遇到障碍物或者是前车速度较慢的时候,就会 ...
自动驾驶路径规划——DWA（动态窗口法）
文章目录 1. DWA(Dynamic window approach) 1.1 机器人运动模型 1.2 速度采样 1.3 评价函数 2. 实践案例--基于ROS实现Astar与DWA算法参考文献 ...
Ardupilot多旋翼自动规划路径实现绕开障碍物避障
Ardupilot官方最近升级了绕开障碍物的算法,并开始应用到多旋翼上面. 路径规划使用BendyRuler 和 Dijkstra's 算法,可以根据Fence的设置和接近光等测距数据,自动生成航线, ...
【自动驾驶】缓和曲线---clothoid回旋曲线
转载自:https://blog.csdn.net/u010241908/article/details/123046783 仅作学习记录缓和曲线由于直线与圆曲线间存在曲率半径的突变,圆曲线半径越 ...
自动驾驶：蛇形曲线跟踪(Stanley Model)
检测对生成出的期望轨迹的跟踪效果,期望轨迹为连续变道的蛇形轨迹: clc clear allload('D:\ccs\DaischTest\CurveCurve.mat') % load('D:\cc ...
RVO动态避障测试示例
简单的测试效果如下: 参考资料:https://forum.cocos.org/t/topic/114590 使用的RVO算法:https://github.com/snape/RVO2 测试demo ...

自动驾驶使用贝塞尔曲线进行动态障碍物避障测试

自动驾驶使用贝塞尔曲线进行动态障碍物避障测试相关推荐

最新文章

热门文章