Predictive-Stanley-Control-for-Tractor-Path-Tracking/calculate_init_k.m at main · HPC2H2/Predictive-Stanley-Control-for-Tractor-Path-Tracking · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
%% calculate_init_k.m
%% 计算初始直线跟踪参数
%% 作者：HPC2H2
%% 日期：20240501

e_y = 0.1;
ts = 0.1;
% v = 1.98/3.6;
% v = 3/3.6;
% v= 4.71/3.6;
v = 1;

k_y = 1;
k_v = 1;
c = 1;
L = 1.9;
delta = atan2(k_y*e_y,k_v*v + c) - 1/180*pi;

if delta > pi/9
    delta = pi/9;
end

delta*180/pi
dx = v*cos(delta)*ts;
dy = v*sin(delta)*ts;
dyaw = tan(delta)/L *ts;
sqrt(dx^2 + dy^2)
e_y = e_y - sqrt(dx^2 + dy^2)


for i = 1:3
    dx = dx + v*cos(dyaw + delta)*ts;
    dy = dy + v*sin(dyaw + delta)*ts;
    dyaw = dyaw + tan(delta)/L*ts;
    sqrt(dx^2 + dy^2)
    e_y = e_y - sqrt(dx^2 + dy^2)
    e_h = (0- dyaw);
    delta = atan2(k_y*e_y,k_v*v + c) + e_h;
    if delta > pi/9
    delta = pi/9;
    end
    if delta < -pi/9
    delta = -pi/9;
    end
    delta_a = delta*180/pi;
    e_h_a = e_h*180/pi;
end


% x_predict(i) = x_predict(i-1) + ...
%                vd1*cos(yaw_predict(i-1) + delta_predict(i-1))*Ts;
%          y_predict(i) = y_predict(i-1) + ...
%                vd1*sin(yaw_predict(i-1) + delta_predict(i-1))*Ts;
%            yaw_predict(i) = yaw_predict(i-1) +...
%                vd1*tan(delta_predict(i-1))/L*Ts;