Using problem based optimization to control a system

3 views (last 30 days)
2Lindsay2 on 6 Jun 2023
Answered: Sam Chak on 6 Jun 2023
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);
Solving problem using fmincon. Local minimum possible. Constraints satisfied. fmincon stopped because the size of the current step is less than the value of the step size tolerance and constraints are satisfied to within the value of the constraint tolerance.
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

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?
2Lindsay2 on 6 Jun 2023
The init is a structure because I was getting an error message saying the initialization needed to be a structure.
I'm a little skeptical that this is a valid solution though. The verification function shows that the system is doing something else.

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);
Solving problem using fmincon. Local minimum possible. Constraints satisfied. fmincon stopped because the size of the current step is less than the value of the step size tolerance and constraints are satisfied to within the value of the constraint tolerance.
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

Categories

Find more on Linear Least Squares in Help Center and File Exchange

R2022b

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!