对《Pure Pursuit纯跟踪算法Python/Matlab算法实现》该博客matlab算法提升：

该博客链接：https://blog.csdn.net/gophae/article/details/102761138

该博客算法跑的效果：

路径没有完整跟下来，可以看出尾部少一块

我在该博客提供算法的基础上做了改进：

下面展示一些 内联代码片。

// An highlighted block
k = 0.1;  % look forward gain
Lfc = 1;  % look-ahead distance
Kp = 1.0 ; % speed propotional gain
dt = 0.1  ;% [s]
L = 2.9  ;% [m] wheel base of vehicle
cx = 0:0.1:50;
cx = cx';
for i = 1:length(cx)
cy(i) = sin(cx(i)/5)*cx(i)/2;                                               %曲线函数
% cy(i)=(3.75/(3.^5))*(6*i.^5-15*3*i.^4+10*3^2*i.^3);
% cy(i) = i;
endi = 1;
target_speed = 3;
T = 80;
lastIndex = length(cx);                                                     %lastIndex=501
x = 0; y = -3; yaw = 0; v = 2;                                               %初始状态
time = 0;
Lf = k * v + Lfc;                                                           %相当于预瞄距离
figurewhile T > time   [target_ind,~]= calc_target_index(x,y,cx,cy,Lf)                         %找到预瞄点ai = PIDcontrol(target_speed, v,Kp);                                    %pid控速度[~,After_Lf]= calc_target_index(x,y,cx,cy,Lf)  di = pure_pursuit_control(x,y,yaw,v,cx,cy,target_ind,k,Lfc,L,After_Lf);[x,y,yaw,v] = update(x,y,yaw,v, ai, di,dt,L)time = time + dt;pause(0.05)plot(cx,cy,'b',x,y,'r-*')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)
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 = atan((ty-y)/(tx-x))-yaw;                                           %实际点的夹角-航向角alpha=atan2((ty-y),(tx-x))-yaw;%预瞄距离delta = atan(2*L * sin(alpha)/Lf)  ;
endfunction [ind,After_Lf] = calc_target_index(x,y, cx,cy,Lf)                     %找到最近的那个
N =  length(cx); %N=501
Distance = zeros(N,1);
for i = 1:N
Distance(i) =  sqrt((cx(i)-x)^2 + (cy(i)-y)^2);
end
[distance, location]= min(Distance);
ind = location;                                                                %第几个数
Relative_distance=distance;                                                    %实际的数
% 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;
%     end
%
h=sqrt(Lf^2-Relative_distance^2);
sum=0;
z=1;while z<(length(cx)-ind-1)sum=sum+sqrt((cx(ind+z+1)-cx(ind+z))^2+(cy(ind+z+1)-cy(ind+z))^2);z=z+1;if (sum>=h)break;endend After_Lf=sqrt(sum^2+Relative_distance^2);
%  if After_Lf>=3
%      After_Lf=3
%  else
%      After_Lf=After_Lf
%  endind=z+ind;
end

实现效果：

matlab实现纯跟踪算法相关推荐

Pure Pursuit纯跟踪算法Python/Matlab算法实现
本文的python源代码来自: https://github.com/gameinskysky/PythonRobotics/blob/master/PathTracking/pure_pursuit ...
自动驾驶笔记-轨迹跟踪之①纯跟踪算法（Pure Pursuit）
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档文章目录前言一.阿克曼转向模型 1.1 模型理解 1.2 模型表达二.纯跟踪算法(Pure Pursuit) 2.1 算法理解 ...
无人车采用纯跟踪算法跟随离线路径（ROS，C++实现）第一部分
一前言最近是由于实验室需求,做了一个简单的无人车轨迹跟踪算法,最主要要完成的功能还是希望无人车能够跟随离线轨迹动起来,因为自己也是在学习阶段,第一次接触这一块的知识,所以就写了这个博客,希望自己能 ...
自动驾驶纯跟踪算法仿真基于Carsim-ros-simulink联合仿真平台的pp算法仿真
自动驾驶纯跟踪算法仿真基于Carsim-ros-simulink联合仿真平台的pp算法仿真 pure pursuit纯跟踪算法轨迹跟踪仿真可用于两个PC端或者虚拟机支持Carsim2018和m ...
纯跟踪算法用于无人车自动泊车
目的使用简单的"纯跟踪算法"实现无人车自动泊车或者位姿调整.在泊车或者工业场景,如果空间不够,那么车辆经常需要做一些大角度的转向或者倒车,例如叉车.这些场景与一般的道路行驶场景可 ...
dst matlab,DSTcode DST跟踪算法MATLAB代码,复杂环境中仿多目标实现的单 Other systems 其他 272万源代码下载- www.pudn.com...
文件名称: DSTcode下载收藏√ [ 5 4 3 2 1 ] 开发工具: matlab 文件大小: 82 KB 上传时间: 2017-03-17 下载次数: 0 提供者: Mar ...
matlab运行LCT跟踪算法代码
可以参考的链接是https://blog.csdn.net/qq_32734095/article/details/86652360 至于opencv的版本,博主说opencv3.0.0没问题,我试的 ...
计算机视觉中，目标跟踪算法的综述
作者:YaqiLYU 链接:https://www.zhihu.com/question/26493945/answer/156025576 来源:知乎著作权归作者所有.商业转载请联系作者获得授权, ...
路径跟踪算法之PID路径跟踪与PP跟踪
路径跟踪算法之PID路径跟踪与PP跟踪 1路径跟踪原理 2 常见的路径跟踪算法 2.1 Pure puresuit(pp)纯跟踪 2.2 PID 跟踪 1路径跟踪原理在运用好的路径规划算法,规划好一 ...

matlab实现纯跟踪算法

对《Pure Pursuit纯跟踪算法Python/Matlab算法实现》该博客matlab算法提升：

我在该博客提供算法的基础上做了改进：

matlab实现纯跟踪算法相关推荐

最新文章

热门文章