看了一篇老哥的文章自己又做了一些注释,原地址为https://blog.csdn.net/gophae/article/details/102761138
以下是为了便于自己理解加的注释,由于初学pure pursuit算法,难免有理解错误之处,请各位大佬指正
%% 纯跟踪算法跟随效果与预瞄距离关系很大。就像人开车时眼睛往前看多远,看的太近了,到了弯道才慌忙打方向盘;看的太远了,又容易导致最近一段的轨迹跟随效果不好。
%% initialization
k = 0.1; % look forward gain 前向预测距离所用增益
Kp = 1.0 ; % speed propotional gain
Lfc = 1; % 基础预瞄距离
dt = 0.1; % [s]
L = 2.9 ; % [m] wheel base of vehicle
cx = 0:0.1:50; %每0.1长度取一次预瞄点的x值
for i = 1:length(cx) %全局路径c(y)生成 路径初始化
cy(i) = cos(cx(i)/5)*cx(i)/4;
end
i = 1;
target_speed = 10/3.6; %设定速度 如果实际自动驾驶,这个就是设定要到达的速度,
%附加:目前这里没有规划,如果有规划,就在过弯的时候给出横向加速度,所以车身方向速度就会减小。
T = 500;
lastIndex = length(cx); %返回cx的向量长度 有501个预瞄点x值 有501个序号
x = 0; y = 4; yaw = 0; v = 0; %车辆初始位置
time = 0;
Ld = k * v + Lfc; %初始速度为0时的预瞄距离Ld=Lfc
figure(1);
while T > time
target_index = calc_target_index(x,y,cx,cy); %输出距离车辆当前最近的点的位置
ai = PIDcontrol(target_speed,v,Kp); %
Ld = k * v + Lfc ;
delta = pure_pursuit_control(x,y,yaw,v,cx,cy,target_index,k,Lfc,L,Ld); %前轮要转的角度
[x,y,yaw,v] = update(x,y,yaw,v, ai, delta,dt,L); %更新车辆状态
time = time + dt; %时间过完一周期
% pause(0.1)
plot(cx,cy,'b',x,y,'r-*') %将预瞄点位置用蓝色表示,当前位置用红色表示
drawnow
hold on
end
function [x, y, yaw, v] = update(x, y, yaw, v, ai, delta,dt,L) %更新车辆状态的函数
x = x + v * cos(yaw) * dt;
y = y + v * sin(yaw) * dt;
yaw = yaw + v / L * tan(delta) * dt;
v = v + ai * dt;
end
function [a] = PIDcontrol(target_v, current_v, Kp) %计算加速度的函数
a = Kp * (target_v - current_v);
end
function [delta] = pure_pursuit_control(x,y,yaw,v,cx,cy,ind,k,Lfc,L,Ld) %纯追踪控制器前轮转角方程的函数
tx = cx(ind); %距离车辆当前最近的点的位置对应的cx值 ind是距离车辆当前最近的点的位置
ty = cy(ind); %距离车辆当前最近的点的位置对应的cy值
alpha = atan((ty-y)/(tx-x))-yaw; %该处定义向左转为alpha=beta-Fai,所以向右转就输出-alpha
Ld = k * v + Lfc ; % 预瞄距离Ld与车速成线性关系
delta = atan(2*L * sin(alpha)/Ld) ; %前轮转角
end
function [ind] = calc_target_index(x,y, cx,cy) %计算找到距车辆当前位置最近点的序号 输出最近点的函数
N = length(cx); %N = 501
Distance = zeros(N,1); % 501行 1列的矩阵
for i = 1:N
Distance(i) = sqrt((cx(i)-x)^2 + (cy(i)-y)^2); %每一个预瞄点到初始位置的距离
end
[~, location]= min(Distance); %找出最近的距离对应的位置
ind = location;
ind = ind + 10
end