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

首先规划出reference line, 探测到障碍物后，进行贝塞尔曲线轨迹规划，绕开障碍物，给出新的reference line, 然后根据pure pursuit模型进行软件在环测试，pure pursuit部分参考之前的博文。

clc
clear allk = 0.1;  % look forward gain
Lfc = 0.1;  % look-ahead distance
global Kp ;
Kp = 1.0 ; % speed propotional gain
dt = 0.1  ;% [s]
L = 2.9  ;% [m] wheel base of vehicle
cx = 0:0.1:100; % sampling interception from 0 to 100, with step 0.1
startp = [60,-24]; % indicate the location where we leave waypoints reference line
endp = [80,-24]; % indicate  the location where we go back to our reference line
p = BezierAvoid (startp, endp) ; %%% call Beizier Curve function during the starting
% point (startp) and the end point (endp)for i = 1:500 % here we create a original reference line, which the vehicle should always follow when there is no obstacles;cy(i) = sin(cx(i)/10)*cx(i)/2;
end
for i = 501: length(cx)cy(i) = cy(500);
endfigureplot(cx,cy,70,-24,'-o')  %plot the original reference line, and the obstacle we set, at the point(70,-24)hold on waypoints = [cx;cy]'; START = Findlocation(waypoints, startp);
END =  Findlocation(waypoints, endp);  %find the index NO. where the start and end points locate;waypoints = [waypoints(1: (START-1),:); p ; waypoints((END+1):end,:)];
% the rest of the reference line remains the same, only the part between
% start point and end points are deleted and replace the new waypoints that
% we generated in the Beizier Function;
cx = waypoints(:,1);
cy = waypoints(:,2);for i  = 1:length(cx) % draw the new reference line;plot(cx(i), cy(i),'.-')drawnowhold onendi = 1;
target_speed = 10/3.6;
T = 80;
lastIndex = length(cx);
x = 0; y = -5; yaw = 0; v = 0;
time = 0;Lf = k * v + Lfc; %some constant parameters we set for the vehicle while T > time target_ind= calc_target_index(x,y,cx,cy,Lf) %find the current location using reference line index to indicate.ai = PIDcontrol(target_speed, v,Kp); % calculate the PID controller output;di = pure_pursuit_control(x,y,yaw,v,cx,cy,target_ind,k,Lfc,L,Lf); % pure pursuit controller will give desired steering angle;[x,y,yaw,v] = update(x,y,yaw,v, ai, di,dt,L) % update the vehicle states;time = time + dt;
%     pause(0.1)plot(cx,cy,'b',x,y,'r-*')%,cx((target_ind-10):target_ind),cy((target_ind-10):target_ind),'g-o'drawnowhold on
end
% plot(cx,cy,x,y,'*')
% hold onfunction [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)
a = Kp * (target_v - current_v);
endfunction [delta] = pure_pursuit_control(x,y,yaw,v,cx,cy,ind,k,Lfc,L,Lf)tx = cx(ind);ty = cy(ind);alpha =pipi( atan((ty-y)/(tx-x))-yaw);Lf = k * v + Lfc;delta = atan(2*L * sin(alpha)/Lf)  ;
endfunction [ind] = calc_target_index(x,y, cx,cy,Lf)
N =  length(cx);
Distance = zeros(N,1);
for i = 1:N
Distance(i) =  sqrt((cx(i)-x)^2 + (cy(i)-y)^2);
end
[~, location]= min(Distance);
ind = location;
LL = 0;
%     while Lf > LL && (ind + 1) < length(cx)
%         dx = cx(ind + 1 )- cx(ind);
%         dy = cx(ind + 1) - cx(ind);
%         LL = LL + sqrt(dx * 2 + dy * 2);
%         ind  = ind + 1;
%     endind = ind + 10;
endfunction [angle] = pipi(angle)if (angle > pi)angle =  angle - 2*pi;
elseif (angle < -pi)angle = angle + 2*pi;
elseangle = angle;
end
endfunction [location] = Findlocation(trace, point)
N = length(trace);
Distance = zeros(N,1);for  i = 1:N
Distance(i) =  sqrt((trace(i,1)-point(1))^2 + (trace(i,2)-point(2))^2);
end[~, index]= min(Distance);
location=index;endfunction p= BezierAvoid (startp, endp) P0 =startp;P1 = [startp(1)+ (endp(1) -startp(1))/8, startp(2)];P2= [startp(1)+ (endp(1) -startp(1))/8*2, startp(2)];P3 = [startp(1)+ (endp(1) -startp(1))/8*2, startp(2)+4];P4 = [startp(1)+ (endp(1) -startp(1))/8*3,startp(2)+4];P5 = [startp(1)+ (endp(1) -startp(1))/8*4,startp(2)+4];P6 = [startp(1)+ (endp(1) -startp(1))/8*5,startp(2)+4];P7 = [startp(1)+ (endp(1) -startp(1))/8*6,startp(2)+4];P8 = [startp(1)+ (endp(1) -startp(1))/8*6,startp(2)];P9 = [startp(1)+ (endp(1) -startp(1))/8*7,startp(2)]; P10 = endp;i = 1;for u =startp(1):0.1:startp(1)+10p(i,:)= (1- (u-startp(1))/10)^5*P0 + 5*(1- (u-startp(1))/10)^4* (u-startp(1))/10*P1 +...10*(1- (u-startp(1))/10)^3*((u-startp(1))/10)^2*P2 ...+10*(1- (u-startp(1))/10)^2*( (u-startp(1))/10)^3*P3...+5*(1- (u-startp(1))/10)*( (u-startp(1))/10)^4*P4 + ( (u-startp(1))/10)^5*P5;
i = i+ 1;end%   figure
%   for i = 1: length(p)
%  plot (p(i,1),p(i,2),'+r')
%  hold on
%   end
%  hold onfor u =(startp(1)+10) :0.1:endp(1)p(i,:)= (1-(u-startp(1)-10) /10)^5*P5 + 5*(1- (u-startp(1)-10)  /10)^4* (u-startp(1)-10)  /10*P6 +...10*(1- (u-startp(1)-10)  /10)^3* ((u-startp(1)-10)  /10)^2*P7+...10*(1- (u-startp(1)-10)  /10)^2* ((u-startp(1)-10)  /10)^3*P8 ...+5*(1- (u-startp(1)-10)  /10)* ((u-startp(1)-10)  /10)^4*P9 +((u-startp(1)-10)  /10)^5*P10;
i = i+ 1;end
end

效果如图：

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

自动驾驶使用贝塞尔曲线进行动态障碍物避障测试
本文介绍如果使用以state lattice planner为基础的曲线生成和动态障碍物规避的方法. 我们的曲线将position profile 和 velocity profile进行了分离.位置 ...
已知环境静态障碍物避障_我女儿如何教我无障碍环境
已知环境静态障碍物避障 by Drew 通过德鲁我女儿如何教我无障碍环境 (How my daughter taught me about accessibility) 在过去的几个月里,花了很多时 ...
自动驾驶轨迹生成-贝塞尔(Bézier)曲线
引言最近刚看完贝塞尔曲线,工作就遇到了相应的需求,所以写一下过程.主要讲的是自动驾驶中,车换道时用到贝塞尔曲线,当然其他的很多领域也会有,例如图形学等. 在车遇到障碍物或者是前车速度较慢的时候,就会 ...
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 ...
机器人局部避障的动态窗口法DWA (dynamic window approach)-matlab代码修改及动态障碍物避障效果
具体效果视频:[DWA动态障碍物-哔哩哔哩] https://b23.tv/pQp6ne 一.源码及问题最初的源码链接https://blog.csdn.net/heyijia0327/articl ...
自动驾驶路径规划论文解析(2)
对论文 Focused Trajectory Planning for Autonomous On-Road Driving的解析本文对Focused Trajectory Planning for ...
百度Apollo自动驾驶专题讲座笔记之运动规划模块
在百度技术学院有Apollo的技术专题课程,对各个模块都有一个入门级的课程,对于了解各个模块间的相互作用关系有很大的作用,很适合对自动驾驶领域感兴趣的人的入门课程.感谢百度Apollo开放了这么好的课 ...

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

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

最新文章

热门文章