本文参考大神gophae的文章进行适当代码修改。实现四象限轨迹跟踪。
Pure Pursuit纯跟踪算法Python/Matlab算法实现_gophae的博客-CSDN博客_纯跟踪算法
横向自动控制方法：Purepursuit, Stanley, MPC对比_肉bot的博客-CSDN博客_mpc横向控制
这种方法最大的特点是有lookahead distance: ld, 这个距离与前进速度成比例kdd，速度越快需要往前看的更远。这种方法的好处是因为看的远，汽车行驶时非常robust，即使参考曲线时不连续的，而是waypoints形式的，也不会造成很大影响，而且不会对速度，弯道弧度的瞬间变化过度敏感。然而缺点就是cutting corner: 在转弯处，因为转向角度一直是根据前面的位置状态决定的，所以它通常超前拐弯，不能很好的贴合设计的弯道线路，而且可以从公式上看到，这种方式并没有考虑cross track error: e，因此在转弯时偏离预设道路的问题没法很好解决。
适合：直线多、弯道少、精度不太高的场合。
曲率或曲率半径对pure pursuit工作效果影响很大，某个曲率下，给定的pure模型可能工作的很好，在另一个曲率下，工作效果会突然变差，这是因为pure仅仅考虑了curvature，因为对curvature的依赖性就很强。并且这种效果会随着车速的增加越发增强。
clear all;close all;clc;k = 0.1; % look forward gain前视距离系数Lfc = 1.0; % look-ahead distance前视距离Kp = 1.0 ; % speed proportional gain速度P控制器系数dt = 0.1 ;% 时间间隔，单位：sL = 0.6 ;% [m] wheel base of vehicle车辆轴距，单位：m% cx = 0:2:50; %x坐标点 1×501% cx = cx'; % 501×1 % for i = 1:length(cx)% cy(i) = sin(cx(i)/2)*cx(i)/2;% endcx = [0.76, 1.52, 2.28, 3.04, 3.81, 4.57, 5.33, 5.44, 5.74, 6.66, 7.4, 7.6, 7.1, 6.9,6.5, 6, 5.6, 5.1];cy = [0.1406933,0.5239,1.01,1.518,1.7990,1.726582,1.2278608,1.113062589,0.769004,-0.62, -1.8565, -3.5, -3.936, -4,-4.4, -4.7, -4.6, -4.2];% i = 1;target_speed = 2/3.6; %目标车速T = 30; %最大模拟时间lastIndex = length(cx); %最后索引% dx = zeros(1,length(cx));% dy = zeros(1,length(cx));% x(1) = 0; y(1) = 0;%初始坐标x = 0; y = 0; yaw = 0; v = 1; %初始偏航角 初始车速time = 0;Lf = k * v + Lfc;%Ld预瞄距离figuretarget_ind = calc_target_index(x,y,cx,cy,Lf);while T > time && lastIndex > target_indtarget_ind = calc_target_index(x,y,cx,cy,Lf);ai = PIDcontrol(target_speed, v,Kp);[delta, target_ind] = pure_pursuit_control(x,y,yaw,v,cx,cy,target_ind,k,Lfc,L,Lf); [x,y,yaw,v] = update(x,y,yaw,v, ai, delta,dt,L);time = time + dt;% pause(0.1) %pause(n) 暂停执行 n 秒，然后继续执行。必须启用暂停，此调用才能生效。plot(cx,cy,'bo',x,y,'r-*')drawnow %drawnow 更新图窗并处理任何挂起的回调。如果您修改图形对象并且需要在屏幕上立即查看这次更新，请使用该命令。hold onend% plot(cx,cy,x,y,'*')% hold on%更新x y yaw vfunction [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 / L * tan(delta) * dt = v / R * dt =w*dt v = v + a * dt;end%PID控制function [a] = PIDcontrol(target_v, current_v, Kp) %由当前车速调控目标车速a = Kp * (target_v - current_v);%加速度end%pure pursuit算法function [delta, ind] = pure_pursuit_control(x,y,yaw,v,cx,cy,ind,k,Lfc,L,Lf)% ind = calc_target_index(x,y,cx,cy,Lf);if ind < length(cx) %若目标点没有超过范围，去具体坐标赋予 tx，ty用作目标tx = cx(ind);ty = cy(ind);else%若超过了，把最后一个点赋给目标tx = cx(ind); ty = cy(ind);ind = length(cx) - 1;endalpha = atan2((ty-y),(tx-x))-yaw; %求航向角if v < 0alpha = pi - alpha;elsealpha = alpha;endLf = k * v + Lfc; %前视距离的选取与速度有关，也与单位时间距离有关delta = atan2(2*L * sin(alpha)/Lf,1) ; %必须用atan2()：四象限 atan()：二象限end%计算目标索引function ind = calc_target_index(x,y, cx,cy,Lf)N = length(cx);Distance = zeros(N,1); %14x1dx = zeros(N,1);dy = zeros(N,1);for i = 1:Ndx(i) = x - cx(i);endfor i = 1:Ndy(i) = y - cy(i);endfor i = 1:NDistance(i) = abs( sqrt(dx(i)^2 + dy(i)^2)); %离上一个点的距离end[~, location]= min(Distance); %出最小的Distance所在的位置%[M,I] = min(___) 在上述语法基础上返回 A 中最小值在运算维度上的索引。ind = location;%首先从目标点中找到一个离当前点最近的点%然后计算离这个点距离满足前视距离的下一个点%当两点之间的距离小于前视距离，需要累加几个点直至距离超过前视距离% %# search look ahead target point index%# 解读：从path point 接下来中找到 离当前点最接近于 前视距离的一个点%# 当路径中的下一个点离当前很远时，这里保证了目标点至少下移一个点，不会停留在原地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;endend
到达最后一个目标点后，小车原地转圈，不知道怎么将小车自动停下来。有知道的大佬欢迎指点我这个小白。
如果觉得《Pure Pursuit轨迹跟踪matlab程序》对你有帮助，请点赞、收藏，并留下你的观点哦！
