-
Notifications
You must be signed in to change notification settings - Fork 0
/
DBM_skidpad.m
155 lines (135 loc) · 4.08 KB
/
DBM_skidpad.m
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
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
%%車両諸元
m = 280.0; %質量
Iz = 160.0;%yaw inertia
wd = 0.45;%weight distrubution
WB = 1.75;%Wheel Base
Tr = 1.25;%Track
lf = WB*(1.0-wd);
lr = WB*wd;
Cf = 39000;%コーナリング係数
Cr = 42000;%コーナリング係数
%%
%simulation parameter
dt = 0.01;
Lfc = 3;%前方注視距離
eR = 1;%許容旋回誤差
Ka = 0.5;%加速速度
Kb = 0.6;%減速速度
timeout = 6;%終了時間
n = timeout/dt;%要素数
%%
%記憶領域
a = zeros(n,1);
delta = zeros(n,1);
beta = zeros(n,1);
Fy = zeros(n,2);% Fyf Fyr
pose = zeros(n,3);%x y theta
v = zeros(n,3);%vx,vy,omega
rear_posi = zeros(n,2);% x y
%%
%Initial condition
pose(1,1) = 0.0; pose(1,2) = 0.0; pose(1,3) = pi/2;
v(1,1) = 10; v(1,2) = 1.0; v(1,3) = 1.25;
delta(1,1) = 80*pi/180;
a(1,1) = 0.0;
Fy(1,1)=-455.0; Fy(1,2)=-455.0;
%%
%コースを導入
skidpad;
% figure
% plot(path(:,1),path(:,2))
% hold on
%%
%frameの条件
% Determine vehicle frame size to most closely represent vehicle with plotTransforms
vizRate = rateControl(1/dt);
frameSize = WB/0.8;
%%
goal = path(end,:);
goalr = 1;
%%
%パスに沿った前方注視点座標を取得
controller = controllerPurePursuit('LookaheadDistance',Lfc,'Waypoints',path);
%%
%Dynamic Bicycle Modelの更新式
t=zeros(n,1);
t(1,1)=0.0;
for i= 1:n-1
if(v(i+1,1) == 0)
v(i+1,1) = 0.1;
end
pose(i+1,1) = pose(i,1) + v(i,1)*cos(pose(i,3))*dt - v(i,2)*sin(pose(i,3))*dt;
pose(i+1,2) = pose(i,2) + v(i,1)*sin(pose(i,3))*dt + v(i,2)*cos(pose(i,3))*dt;
pose(i+1,3) = pose(i,3) + v(i,3)*dt;
v(i+1,1) = v(i,1) + (a(i,1) - Fy(i,1)*sin(delta(i,1))/m + v(i,2)*v(i,3))*dt;
v(i+1,2) = v(i,2) + (Fy(i,2)/m + Fy(i,1)*cos(delta(i,1))/m - v(i,1)*v(i,3))*dt;
v(i+1,3) = v(i,3) + dt/Iz*(Fy(i,1)*lf - Fy(i,2)*lr);
rear_posi(i+1,1) = pose(i,1) -(lr*cos(pose(i,3)));
rear_posi(i+1,2) = pose(i,2) -(lr*sin(pose(i,3)));
%pure pursuit controller
[vel,angvel,Lp] = controller(pose(i+1,:));
%軌跡と車の角度
X = Lp(1,1)-rear_posi(i+1,1);
Y = Lp(1,2)-rear_posi(i+1,2);
alp = atan2(Y,X) - pose(i+1,3);
%舵角計算
delta(i+1,1) = atan2(2*Tr*sin(alp),Lfc);
if(delta(i+1) < -pi/2)
delta(i+1) = -pi/2;
end
if(delta(i+1) >= pi/2)
delta(i+1) = pi/2;
end
%加減速制御
rho = (1-m/(2*WB^2)*(lf*Cf-lr*Cr)/(Cf*Cr)*(v(i+1,1)^2+v(i+1,2)^2))*WB/abs(delta(i+1,1)*180/pi);
if(rho <R)%eRは許容膨れ
v(i+1,1) = v(i+1,1) + Ka*dt;%Kは加速速度
a(i+1,1) = Ka;
elseif(rho>R+eR)%eRは許容膨れ
v(i+1,1) = v(i+1,1) + Kb*dt;%Kは減速速度
a(i+1,1) = -Kb;
else
a(i+1) = 0;
end
% スリップアングル計算
beta(i) = atan2(v(i+1,2), v(i+1,1));
betaf(i) = beta(i) + lf*v(i+1,3)/sqrt(v(i+1,1)^2+v(i+1,2)^2) - delta(i+1,1);
betar(i) = beta(i) - lr*v(i+1,3)/sqrt(v(i+1,1)^2+v(i+1,2)^2);
betaf(i) = betaf(i)*180/pi;
betar(i) = betar(i)*180/pi;
%タイヤ力
Fy(i+1,1) = -Cf*(atan2(v(i+1,2),v(i+1,1)) + lf*v(i+1,3)/v(i+1,1) - delta(i+1,1));
Fy(i+1,2) = -Cr*(atan2(v(i+1,2),v(i+1,1)) - lr*v(i+1,3)/v(i+1,1));
%逐次プロット
% plotTrVec = [pose(i+1,1:2)'; 0];
% plotRot = axang2quat([0 0 1 pose(i+1,3)]);
% plotTransforms(plotTrVec', plotRot, "MeshFilePath", "groundvehicle.stl", "Parent", gca, "View","2D", "FrameSize", frameSize);
%
% xlim([-15 35])
% ylim([-20 20])
% axis equal
%
%
%
% waitfor(vizRate);
t(i+1,1)= t(i,1)+dt;
txt = [ "t=" num2str( t(i+1,1) ) "s "];
text(.5,.5,txt,'Position',[25,25,0]);
%終了条件
distanceToGoal = norm(pose(i+1,1:2) - goal);
if(distanceToGoal < goalr && t(i+1,1)>3)
t_end = t(i+1,1);
break
end
% if(distanceToGoal < goalr/10 && t(i+1,1)<4)
% t_str = t;
% end
if( t(i+1,1) >8 )
t_end = t(i+1,1);
break
end
end
%%
t_end
plot(betar(:))
hold on