Pure Pursuit纯跟踪算法的Matlab算法实现

本文的python源代码来自：
https://github.com/gameinskysky/PythonRobotics/blob/master/PathTracking/pure_pursuit/pure_pursuit.py

纯跟踪算法的原理，详见https://blog.csdn.net/gophae/article/details/100012763

原文链接：https://blog.csdn.net/gophae/article/details/102761138

matlab源码如下:

k = 0.1;  % look forward gain
Lfc = 1.0;  % 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;
endi = 1;
target_speed = 10/3.6;
T = 80;
lastIndex = length(cx);
x = 0; y = -3; yaw = 0; v = 0;
time = 0;Lf = k * v + Lfc;figure
while T > time target_ind= calc_target_index(x,y,cx,cy,Lf)ai = PIDcontrol(target_speed, v,Kp);di = 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, di,dt,L)time = time + dt;
%     pause(0.1)plot(cx,cy,'b',x,y,'r-*')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 = 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;
%     end
%     ind = ind + 10
end

工作区变量如下:

展示效果如图：

Pure Pursuit纯跟踪算法的Matlab算法实现相关推荐

Pure Pursuit纯跟踪算法Python/Matlab算法实现
本文的python源代码来自: https://github.com/gameinskysky/PythonRobotics/blob/master/PathTracking/pure_pursuit ...
pure pursuit纯跟踪
Pure Pursuit是一种几何追踪方法,速度越小,performance越好; :汽车前轮转角 L:前后轮轴距(车长) R:转弯半径将车辆模型简化为自行车模型(这里默认左轮和右轮的旋转是一致的) ...
LQR轨迹跟踪算法Python/Matlab算法实现_代码(2)
本文根据LQR轨迹跟踪算法Python/Matlab算法实现_LQRmatrix推导(2)使用代码实现,进行仿真: clc clear allKp = 1.0 ; dt =0.1 ;% [s] L = ...
LQR轨迹跟踪算法Python/Matlab算法实现2
这里对上一篇LQR轨迹跟踪算法Python/Matlab算法实现进行勘误: clc clear allKp = 1.0 ; dt = 0.1 ;% [s] L = 2.9 ;% [m] wheel b ...
LQR轨迹跟踪算法Python/Matlab算法实现_LQRmatrix推导
对于文章 LQR轨迹跟踪算法Python/Matlab算法实现中的LQR推导的问题,我简单写了一下手稿,不高兴做成公式了:
[运动控制算法]Pure Pursuit纯路径跟踪算法
纯路径跟踪 Pure Pursuit是一种用于路径跟踪的控制算法.它通过计算角速度控制机器人从当前位置移到机器人前方的某个预瞄点.假定线速度是恒定的,当然可以随意更改机器人的线速度.该算法会根据机器人 ...
pure pursuit：无人车轨迹追踪算法
对于无人车辆来说,在规划好路径以后(这个路径我们通常称为全局路径),全局路径由一系列路径点构成,这些路径点只要包含空间位置信息即可,也可以包含姿态信息,但是不需要与时间相关,这些路径点被称为全局路径点 ...
【老生谈算法】matlab算法离合器——离合器
基于Matlab算法离合器设计与试验 1.原文下载: 本算法原文如下,有需要的朋友可以点击进行下载序号原文(点击下载) 本项目原文 [老生谈算法]基于Matlab算法离合器设计与试验.doc 2. ...
Stanley轨迹跟踪算法Python/Matlab算法实现
本文针对Python/Matlab实现Stanley进行介绍: Stanley 的数学几何原理来自: https://blog.csdn.net/gophae/article/details/1000 ...
LQR轨迹跟踪算法Python/Matlab算法实现_LQRmatrix推导(2)
本文提供了另一种LQR状态空间矩阵的推导方式:

Pure Pursuit纯跟踪算法的Matlab算法实现

Pure Pursuit纯跟踪算法的Matlab算法实现相关推荐

最新文章

热门文章