Using problem based optimization to control a system
2 Ansichten (letzte 30 Tage)
Ältere Kommentare anzeigen
As a learning excercise I'm trying to get the optimal control force for a spring mass system using the optimization toolbox. The solutions that I'm getting seems very wrong. The validaton script that runs the open control loop seems to verify there is an error but I'm not sure what my error is, though I'm pretty sure it has to do with some constraints somewhere.
Here is the code:
clc
clear
close all
p0 = 2;
pf = 1;
v0 = 0;
k = 1;
m = 1;
T = 5; % Total time
N = 50; % Number of discretization points
init.F = ones(N,1);
[massModel, p, t] = springMassSystem(N, T,p0,pf,v0);
sol = solve(massModel, init);
plotMotionAndForce(sol,p,p0,pf,t,T)
springMassVerify(m,k,p0,v0,sol.F,t,T)
function [mass, p, t] = springMassSystem(N, T, p0, pf, v0)
t = T/N; % discretized time step size
Fmax = 10; % Max force
a_max = 3;
s_max = 5;
k = 1;
m = 1;
F = optimvar('F',N,1,'LowerBound',-Fmax, 'UpperBound',Fmax); % Our controlling force
p = optimexpr(N,1); % position variable - controlled
accel_x = (-k*p + F)/m; % ma = -kx + F % accelleration of spring mass damper system
a_cons = optimconstr(N);
for i = 1:N-1
a_cons(i) = accel_x(i) <= a_max; % accel constraint - magnitude of a should be less than a_max
end
v = cumsum([v0; t*accel_x(1:N-1)]); % v equals the cumulative sum of v0 + accel*time - vel at each time instance
p = cumsum([p0; t*v(1:N-1) + 1/2*t*t*(accel_x(1:N-1))]); % use pos equation with pos and accel instead of avg velocity
accel_xCon = accel_x(N,:) == 0;
vEndCon = v(N,:) == 0; % set end point velocity as a constraint
pEndCon = p(N,:) == pf; % set end point position as a constraint
mass = optimproblem('ObjectiveSense','min'); % model
s = norm(p);
mass.Objective = sum(s); % we want to minimize the total distance traveled
% mass.Constraints.accel_xCon = accel_xCon;
mass.Constraints.aCon = a_cons;
mass.Constraints.vEndCon = vEndCon;
mass.Constraints.pEndCon = pEndCon;
end
%verification for the spring mass system
function springMassVerify(m,k,p0,v0,F,t,T)
positions = [];
positions(1) = p0;
velocities = [];
velocities(1) = v0;
N = length(F);
for i = 1:N
[positions(i+1), velocities(i+1)] = springMassUpdate(m,k,positions(i), velocities(i),F(i),t);
end
plotPos(positions,t,T)
end
% updated the positin and velocity of the spring mass system
function [posNew, velNew]= springMassUpdate(m,k,x,v,F,t)
accel = (-k*x + F)/m;
velNew = v + t*accel;
posNew = x + t*v + 1/2*t*t*accel;
end
% plotting for validation
function plotPos(positions,t,T)
figure
plot(t:t:T, positions(1:end-1),'rx')
xlabel("Time step")
ylabel("position/acceleration")
end
% main plotting
function plotMotionAndForce(sol,p,p0,pF,t,T)
figure
t_vec = t:t:T;
psol = evaluate(p, sol);
plot(t_vec,psol(1:end,1),'rx')
hold on
plot(t,p0(1),'ks')
plot(T,pF(1),'bo')
fsol = sol.F;
plot(t_vec(:), fsol,"mx")
xlabel("Time step")
ylabel("position/acceleration")
legend("Steps","Initial Point","Final Point","accel")
end
0 Kommentare
Antworten (2)
Hiro Yoshino
am 6 Jun. 2023
It seems that it simply found a local optimum. If this is not the one that you expected, you should change the intial values. Try something similar to the solution to see if it converges the solution.
By the way, why is "init" structure?
Sam Chak
am 6 Jun. 2023
I'm unsure of what your expectation is. However, if the final position is changed to the origin, then the convergence is observed. Thus, you should try modifying the objective function for a non-zero final state. Can also try optimizing according to the Integral Time Absolute Error (ITAE) criterion.
p0 = 1; % initial position <--
pf = 0; % final position <--
v0 = 0; % initial velocity
k = 1; % stiffness
m = 1; % mass
T = 5; % Total time
N = 50; % Number of discretization points
init.F = ones(N,1);
[massModel, p, t] = springMassSystem(N, T, p0, pf, v0);
sol = solve(massModel, init);
plotMotionAndForce(sol, p, p0, pf, t, T)
springMassVerify(m, k, p0, v0, sol.F, t, T)
function [mass, p, t] = springMassSystem(N, T, p0, pf, v0)
t = T/N; % discretized time step size
Fmax = 10; % Max force
a_max = 3;
s_max = 5;
k = 1;
m = 1;
F = optimvar('F', N, 1, 'LowerBound', -Fmax, 'UpperBound', Fmax); % Our controlling force
p = optimexpr(N, 1); % position variable - controlled
accel_x = (F - k*p)/m; % ma = -kx + F % accelleration of spring mass damper system
a_cons = optimconstr(N);
for i = 1:N-1
a_cons(i) = accel_x(i) <= a_max; % accel constraint - magnitude of a should be less than a_max
end
v = cumsum([v0; t*accel_x(1:N-1)]); % v equals the cumulative sum of v0 + accel*time - vel at each time instance
p = cumsum([p0; t*v(1:N-1) + 1/2*t*t*(accel_x(1:N-1))]); % use pos equation with pos and accel instead of avg velocity
accel_xCon = accel_x(N,:) == 0;
vEndCon = v(N,:) == 0; % set end point velocity as a constraint
pEndCon = p(N,:) == pf; % set end point position as a constraint
mass = optimproblem('ObjectiveSense','min'); % model
s = norm(p);
mass.Objective = sum(s); % we want to minimize the total distance traveled
% mass.Constraints.accel_xCon = accel_xCon;
mass.Constraints.aCon = a_cons;
mass.Constraints.vEndCon = vEndCon;
mass.Constraints.pEndCon = pEndCon;
end
%verification for the spring mass system
function springMassVerify(m, k, p0, v0, F, t, T)
positions = [];
positions(1) = p0;
velocities = [];
velocities(1) = v0;
N = length(F);
for i = 1:N
[positions(i+1), velocities(i+1)] = springMassUpdate(m, k, positions(i), velocities(i), F(i), t);
end
plotPos(positions, t, T)
end
% updated the positin and velocity of the spring mass system
function [posNew, velNew]= springMassUpdate(m, k, x, v, F, t)
accel = (- k*x + F)/m;
velNew = v + t*accel;
posNew = x + t*v + 1/2*t*t*accel;
end
% plotting for validation
function plotPos(positions, t, T)
figure
plot(t:t:T, positions(1:end-1), 'rx')
xlabel("Time step")
ylabel("position/acceleration")
end
% main plotting
function plotMotionAndForce(sol, p, p0, pF, t, T)
figure
t_vec = t:t:T;
psol = evaluate(p, sol);
plot(t_vec, psol(1:end,1), 'rx'), grid on
xlabel("Time step")
ylabel("Position")
% hold on
% plot(t, p0(1), 'ks')
% plot(T, pF(1), 'bo')
%
% fsol = sol.F;
% plot(t_vec(:), fsol, "mx")
% xlabel("Time step")
% ylabel("position/acceleration")
% legend("Steps", "Initial Point", "Final Point", "accel", 'location', 'best')
end
0 Kommentare
Siehe auch
Kategorien
Mehr zu Linear Least Squares finden Sie in Help Center und File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!