预瞄跟踪算法

function pursuit
clc;clear;close all
%预瞄
global dt;dt = 0.1;  % [s]
global L;L = 2.9;  % [m] wheel base of vehicle
%main
%  target course
% cx = 0:0.1:50;%
% cy = sin（cx/5.0）.* cx/2.0 ;

% t=0:pi/200:2*pi;
% cx = 10*cos（t）;
% cy = 10*sin（t） ;
% plot（cxcy）
cx=0:0.005:600;
for i=1:length（cx）
if  （cx（i）>=200 && cx（i）<=321）
    cy（i）=0;
elseif （cx（i）>=363 && cx（i）<=398）
     cy（i）=3.5;
elseif （cx（i）>=433） 
    cy（i）=0;
elseif （cx（i）>321 && cx（i）<363）
    cy（i）=3.5/（363-321）*cx（i）-3.5/（363-321）*321;
elseif （cx（i）>398 && cx（i）<433）
    cy（i）=-3.5/（433-398）*cx（i）+43.3;
end
end
 plot（cx cy ‘.r‘）;hold on
target_speed = 10.0 / 3.6; % [m/s]
T = 1000.0;   % max simulation time
%initial state
state = [11.00.00.00.0];   %[xyyawv]为什么是-3
lastIndex = length（cx）;
time = 0.0;
x = state（1）;
y = state（2）;
yaw = state（3）;
v = state（4）;
t = 0.0;
target_ind = calc_target_index（state cx cy）;
%% main loop
while （T >= time） && （lastIndex > target_ind）
    ai = PIDControl（target_speed state（4））;%获得ai需要（）中的两个内容
    [di target_ind] = pure_pursuit_control（state cx cy target_ind）;
    state = update（state ai di）;
    time = time + dt;
    x=[xstate（1）];
    y=[ystate（2）];
    yaw=[yawstate（3）];
    v=[vstate（4）];
    t=[ttime];
end
% Test
% if show_animation
%     plot（cx cy “.r“ label=“course“）
%     plot（x y “-b“ label=“trajectory“）
%     plot（cx[target_ind] cy[target_ind] “xg“ label=“target“）
%     axis（“equal“）
%     grid（True）
%     plt.title（“Speed[km/h]:“ + str（state.v * 3.6）[:4]）
%     plt.pause（0.001）
% end
plot（x y ‘b‘）
legend（‘courser‘ ‘trajectory‘）
title（‘预瞄跟踪算法‘）
hold on;
end
%% sub function
%预瞄寻找距离参考轨迹中最近一点
function target_ind = calc_target_index（state cx cy）
% search nearest point index
%%[xy