Pure Pursuit轨迹跟踪matlab程序

本文参考大神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  ;% 时间间隔，单位：s
L = 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;
% end
cx = [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 on
end
% plot(cx,cy,x,y,'*')
% hold on%更新x  y  yaw  v
function [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);  %14x1
dx = 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程序相关推荐

自动驾驶（七十一）---------Pure Pursuit轨迹追踪
前面介绍过很多轨迹规划,也有很多控制相关的知识,例如pid调节.kalman 滤波.MPC等等.今天再学习一种轨迹追踪的方法,轨迹追踪是在有轨迹规划的基础上,如何实现车辆按照规划的轨迹行驶的问题,属于 ...
Pure Pursuit纯跟踪算法Python/Matlab算法实现
本文的python源代码来自: https://github.com/gameinskysky/PythonRobotics/blob/master/PathTracking/pure_pursuit ...
pure pursuit纯跟踪
Pure Pursuit是一种几何追踪方法,速度越小,performance越好; :汽车前轮转角 L:前后轮轴距(车长) R:转弯半径将车辆模型简化为自行车模型(这里默认左轮和右轮的旋转是一致的) ...
车辆跟踪 matlab,汽车路径跟踪matlab程序
[实例简介] This entry contains the Simulink model for the "Path Planning and Navigation for Autonom ...
无人车系统（五）：轨迹跟踪Pure Pursuit方法
今天介绍一种基于几何追踪的无人车轨迹跟踪方法--Pure Pursuit(纯跟踪)方法. 1. 阿克曼转向几何模型在无人车系统(一):运动学模型及其线性化一文中,里面介绍无人车的运动学模型为阿克曼转 ...
自动驾驶笔记-轨迹跟踪之①纯跟踪算法（Pure Pursuit）
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档文章目录前言一.阿克曼转向模型 1.1 模型理解 1.2 模型表达二.纯跟踪算法(Pure Pursuit) 2.1 算法理解 ...
终端滑模matlab程序,机器人轨迹跟踪控制方法研究(含MATLAB程序)
机器人轨迹跟踪控制方法研究(含MATLAB程序)(课题申报表,任务书,开题报告,中期检查表,外文翻译,论文15300字,程序,答辩PPT) 摘要机器人是一类复杂的.具有不确定性的.多输入多输出非线 ...
差速机器人的纯轨迹跟踪仿真（Matlab）
差速机器人的纯轨迹跟踪仿真(Matlab) 刚入门,有的地方不对,烦请大家指正. 目录差速机器人的纯轨迹跟踪仿真(Matlab) 1 差速机器人运动模型 1.1 运动学分析建模 1.2 差速机器人的 ...
无人驾驶汽车系统入门（十八）——使用pure pursuit实现无人车轨迹追踪
无人驾驶汽车系统入门(十八)--使用pure pursuit实现无人车轨迹追踪对于无人车辆来说,在规划好路径以后(这个路径我们通常称为全局路径),全局路径由一系列路径点构成,这些路径点只要包含空间位 ...
pure pursuit：无人车轨迹追踪算法
对于无人车辆来说,在规划好路径以后(这个路径我们通常称为全局路径),全局路径由一系列路径点构成,这些路径点只要包含空间位置信息即可,也可以包含姿态信息,但是不需要与时间相关,这些路径点被称为全局路径点 ...

Pure Pursuit轨迹跟踪matlab程序

Pure Pursuit轨迹跟踪matlab程序相关推荐

最新文章

热门文章