Using problem based optimization to control a system
2 views (last 30 days)
Show older comments
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 Comments
Answers (2)
Hiro Yoshino
on 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
on 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 Comments
See Also
Categories
Find more on Linear Least Squares in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!