-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmain.m
190 lines (161 loc) · 6.25 KB
/
main.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
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
clc; clear; close all;
import casadi.*
% Optimal Time to Park the car in a slot
% ----------------------
% An optimal control problem (OCP),
% solved with direct multiple-shooting.
N = 40; % number of control intervals
opti = casadi.Opti(); % Optimization problem
% Read model parameters
params = get_params();
% ---- decision variables ---------
X = opti.variable(5,N+1); % state trajectory
posx = X(1,:); % x position
posy = X(2,:); % y position
yaw = X(3,:); % orientation angle
velocity = X(4,:); % car velocity
phi = X(5,:); % Steering Angle
U = opti.variable(2,N); % control trajectory (throttle)
acc = U(1,:); % Acceleration
omega = U(2,:); % Angular Velocity
Tf = opti.variable(); % final time
% ---- objective ---------
opti.minimize(Tf); % parking in minimal time
% ---- dynamic constraints --------
dt = Tf/N; % length of a control interval
for k=1:N % loop over control intervals
% Runge-Kutta 4 integration
x_next = rk4_step(@ode, X(:,k), U(:,k), dt);
opti.subject_to(X(:,k+1) == x_next); % close the gaps
% ---- path constraints over the whole trajectory-----------
[Ax_k, Ay_k, Bx_k, By_k, Cx_k, Cy_k, Dx_k, Dy_k]= get_car_coordinates(X(:,k), params);
opti.subject_to(Ay_k <= params.ceiling_l);
opti.subject_to(By_k <= params.ceiling_l);
opti.subject_to(Cy_k <= params.ceiling_l);
opti.subject_to(Dy_k <= params.ceiling_l);
opti.subject_to(Ay_k >= slot_function(Ax_k, params));
opti.subject_to(By_k >= slot_function(Bx_k, params));
opti.subject_to(Cy_k >= slot_function(Cx_k, params));
opti.subject_to(Dy_k >= slot_function(Dx_k, params));
% O and E slot function points constraints to avoid collison of car at
% corner points
[Area_k, AreaO_k, AreaE_k] = OE_boundary(Ax_k, Ay_k, Bx_k, By_k, Cx_k, Cy_k, Dx_k, Dy_k, params);
opti.subject_to(Area_k < AreaO_k);
opti.subject_to(Area_k < AreaE_k);
end
% ---- controls box constraints -----------
opti.subject_to(-0.75 <= acc <= 0.75); % control is limited
% ---- states box constraints -----------
opti.subject_to(-Inf <= yaw <= Inf);
opti.subject_to(-2 <= velocity <= 2);
opti.subject_to(deg2rad(-33) <= phi <= deg2rad(33));
% ---- Start boundary conditions --------
% case_num = 1;
posx_start = (params.slot_l + params.rear_l);
posy_start = 1.5;
yaw_start = deg2rad(5);
velocity_start = 0;
% case_num = 2;
% posx_start = -5.14;
% posy_start = 1.41;
% yaw_start = deg2rad(13.18);
% velocity_start = 0;
% case_num = 3;
% posx_start = 8.97;
% posy_start = 2.17;
% yaw_start = deg2rad(-16.06);
% velocity_start = 0.36;
% case_num = 4;
% posx_start = 6.15;
% posy_start = 1.55;
% yaw_start = deg2rad(5);
% velocity_start = 0;
opti.subject_to(posx(1) == posx_start);
opti.subject_to(posy(1) == posy_start);
opti.subject_to(yaw(1) == yaw_start);
opti.subject_to(velocity(1) == velocity_start); % ... from stand-still
opti.subject_to(phi(1) == deg2rad(0));
% ---- Terminal boundary conditions --------
opti.subject_to(velocity(N+1) == 0); % finish line at position 1
% ---- Terminal constraints --------
[Ax_Tf, Ay_Tf, Bx_Tf, By_Tf, Cx_Tf, Cy_Tf, Dx_Tf, Dy_Tf] = get_car_coordinates(X(:,N+1), params);
opti.subject_to(-params.slot_w <= Ay_Tf <= 0);
opti.subject_to(-params.slot_w <= By_Tf <= 0);
opti.subject_to(-params.slot_w <= Cy_Tf <= 0);
opti.subject_to(-params.slot_w <= Dy_Tf <= 0);
opti.subject_to(0 <= Ax_Tf <= params.slot_l);
opti.subject_to(0 <= Bx_Tf <= params.slot_l);
opti.subject_to(0 <= Cx_Tf <= params.slot_l);
opti.subject_to(0 <= Cx_Tf <= params.slot_l);
% ---- misc. constraints ----------
opti.subject_to(Tf>=0); % Time must be positive
% ---- initial control values for solver ---
opti.set_initial(acc, 0);
opti.set_initial(omega, 0);
% ---- solve NLP ------
opti.solver('ipopt'); % set numerical backend
sol = opti.solve(); % actual solve
% ---- post-processing ------
figure(1)
hold on
disp(sol.value(Tf))
% Plot starting coordinates of the vehicle
plot(sol.value(posx(1)),sol.value(posy(1)), 'bo', 'MarkerFaceColor','green')
% Plot slot function and ceiling line
x = linspace( -10, 15, 1000);
plot(x, slot_function(x, params), 'Color', 'k', 'LineWidth', 1)
plot(x, params.ceiling_l * ones(size(x)), 'k', 'LineWidth', 1)
% Plot optimal state trajectory
plot(sol.value(posx), sol.value(posy), 'r--')
% Plot polygons for the car at the solutions
colour_intensity = linspace(0.1, 0.2, N);
for k=1:N+1 % loop over control intervals
[Ax, Ay, Bx, By, Cx, Cy, Dx, Dy] = get_car_coordinates(sol.value(X(:,k)), params);
pgon = polyshape([Ax,Bx,Cx,Dx],[Ay,By,Cy,Dy]);
if k==N+1
plot(pgon, 'FaceAlpha', 0,'EdgeColor' ,'b' ,'EdgeAlpha',1 ,'LineWidth', 1)
else
plot(pgon, 'FaceAlpha', 0, 'EdgeColor','b', 'EdgeAlpha', colour_intensity(k))
end
end
% Add text to the plot with control intervals and finish time
str = {['Control Intervals: ', num2str(N)],['Final Time: ',num2str(sol.value(Tf)), ' sec']};
annotation("textbox", 'String', str, 'FitBoxToText','on');
legend('Car Start Position','Parking Slot Function', 'Opposite Road Boundary', ...
'Car Positions', 'Car Shape', 'Location','southwest')
xlabel('X position (m)')
ylabel('Y position (m)')
title('2D Parking Area')
% Plot the optimal controls and states
time = linspace(0, sol.value(Tf), N+1);
figure(2)
subplot(2,3,1)
plot(time, sol.value(velocity), 'r', 'LineWidth', 1);
xlabel('Time (sec)')
ylabel('v(t)(m/s)')
title('linear velocity')
subplot(2,3,2)
plot(time, sol.value(yaw), 'r', 'LineWidth', 1);
xlabel('Time (sec)')
ylabel('θ(t)(m/s)')
title('orientation angle')
subplot(2,3,3)
plot(time, sol.value(phi), 'r', 'LineWidth', 1);
xlabel('Time (sec)')
ylabel('ϕ(t)(m/s)')
title('steering angle')
subplot(2,3,4)
plot(time(1:N), sol.value(acc), 'b', 'LineWidth', 1);
xlabel('Time (sec)')
ylabel('a(t)(m/s^{2})')
title('acceleration')
subplot(2,3,5)
plot(time(1:N), sol.value(omega), 'b', 'LineWidth', 1);
xlabel('Time (sec)')
ylabel('w(t)(rad/s)')
title('angular velocity')
% Plot the Jacobian and Hessian matrices
figure(3)
spy(sol.value(jacobian(opti.g,opti.x)))
figure(4)
spy(sol.value(hessian(opti.f+opti.lam_g'*opti.g,opti.x)))