How to simulate a non linear system
269 visualizzazioni (ultimi 30 giorni)
Mostra commenti meno recenti
Gianluca Di pietro
il 6 Mar 2024
Hello guys,i have a question…I m simulating an LQR of a double inverted pendulum on a cart using matlab.I did the linearization,i tried different values for Q and R and i did the plot of states.Now the problem is that i want to compare the Linear Lqr Model in the form:
Dx/dt = (Ax+Bu) with u=-Kx i want to compare this with the LQR applied to the original non linear system.I know that LQR is for linear system but it was Asked to simulate the LQR u=-Kx in the non linear model The non linear model is in the form: Dx/dt= A(x)*x +B(x)*u + H(x) so using U=-Kx my sistem became dx/dt = (A(x)-B(x)K)x + H(x) .Now how can i simulate this system?for the linear one i used the space state form,how can i Now use the non linear Matrix to simulate and represent my sistem?Thanks
8 Commenti
Walter Roberson
il 6 Mar 2024
%%Tesina Doppio Pendolo Invertito su un Carrellino
%Definiamo le matrici e i parametri del sistema
m0 = 1.5;
m1= 0.5;
m2=0.75;
L1=0.5;
L2=0.75;
g =9.8;
d1 = m0 + m1 + m2;
d2 = (0.5*m1 + m2)*L1;
d3 = 0.5*m2*L2;
d4 = (1/3 * m1 + m2)*L1^2;
d5= 0.5*m2*L1*L2;
d6= 1/3*m2*L2^2;
f1 = (0.5*m1 + m2)*L1*g;
f2 = 0.5*m2*L2*g;
syms Theta0 Theta1 Theta2;
syms DTheta0 DTheta1 DTheta2;
x_ = transpose([Theta0 Theta1 Theta2 DTheta0 DTheta1 DTheta2]);
D(x_)= [d1 d2*cos(x_(2)) d3*cos(x_(3));
d2*cos(x_(2)) d4 d5*cos(x_(2)-x_(3));
d3*cos(x_(3)) d5*cos(x_(2)-x_(3)),d6];
G(x_)= transpose([0 -f1*sin(x_(2)) -f2*sin(x_(3))]);
C(x_)=[0 -d2*sin(x_(2))*x_(5) -d3*sin(x_(3))*x_(6);
0 0 d5*sin(x_(2)-x_(3))*x_(6);0 -d5*sin(x_(2)-x_(3))*x_(5) 0];
H = transpose([1 0 0]);
%matrici A e B sistema linearizzato
deriveG(x_) = transpose(jacobian(G(x_(1),x_(2),x_(3),x_(4),x_(5),x_(6)),[x_(1),x_(2),x_(3)]));
A = [zeros(3) eye(3);-inv(D(0,0,0,0,0,0))*deriveG(0,0,0,0,0,0) zeros(3)];
Anumeric = double(A);
B = [zeros(3,1);inv(D(0,0,0,0,0,0))*H];
Bnumeric = double(B);
%%matrici sistema non lineare
Anl(x_)= [zeros(3) eye(3);zeros(3) -inv(D(x_(1),x_(2),x_(3),x_(4),x_(5),x_(6)))*C];
Bnl(x_) = [zeros(3,1);inv(D(x_(1),x_(2),x_(3),x_(4),x_(5),x_(6)))*H];
Hnl(x_) = [zeros(3,1);-inv(D(x_(1),x_(2),x_(3),x_(4),x_(5),x_(6)))*G];
%Progettazione controllore LQR
%%Bryson rule: Bryson's method [17] was introduced to adjust Q and R weighting
% matricesas an attempt to overcome the drawbacks of using the trial and error method.
% According to this rule, Q and R can be calculated by finding the reciprocals
% of squares of the maximum acceptable values of the state and the input control variables.
Q = diag([50 50 50 700 700 700]);
R = 1;
sys = ss(Anumeric,Bnumeric,eye(6),0);
Co = ctrb(sys);
res = rank(Co); %rank is equal to number of state 6,system is controllable
Ob=obsv(sys);
res2= rank(Ob); %%rank is equal to number of state 6,system is observable
[K,S,P]=lqr(sys,Q,R); %p are closed-loop poles system
newsys = ss((Anumeric-Bnumeric*K),Bnumeric,eye(6),0);
%Pole Representation
reale = real(P);
immaginario = imag(P);
% Plot dei poli nel piano complesso (cartesiano)
figure;
plot(reale, immaginario, 'rx', 'MarkerSize', 10); % 'rx' indica crocette rosse
xlabel('Parte Reale');
ylabel('Parte Immaginaria');
title('Diagramma dei Poli (Cartesiano)');
grid on;
axis equal;
%plot states
% Simulate the system for initial conditions
[y,t,~]=initial(newsys,[0;deg2rad(10);-deg2rad(10);0;0;0],20);
%[y,t,x]=initial(sysnew,[0;deg2rad(20);deg2rad(20);0;0;0],10);
% Convert from rad to deg
y(:,2:3)=rad2deg(y(:,2:3));
y(:,5:6)=rad2deg(y(:,5:6));
figure
for i=1:1:6
subplot(2,3,i)
plot(t,y(:,i))
grid
end
%%ode45 for non linear model
x0 = [0,0,0,0,0,0]; %initial condition
t_a = linspace(0,1,20); %time vector
[t,x] = ode45(@(t,x)myfunction(t,x,K,Anl,Bnl,Hnl),t_a,x0);
figure
plot(t_a,x.','LineWidth',1.5);
function dxdt = myfunction(t,x,K,Anl,Bnl,Hnl)
u = -K*x;
dxdt_ = Anl(x(1),x(2),x(3),x(4),x(5),x(6))*x + Bnl(x(1),x(2),x(3),x(4),x(5),x(6))*u + Hnl(x(1),x(2),x(3),x(4),x(5),x(6));
dxdt = double(dxdt_(:));
end
Risposta accettata
Sam Chak
il 7 Mar 2024
You should be able to compare the linear and nonlinear systems in the code provided by @Walter Roberson by using the same initial state values. I have streamlined the linearization process to derive the linear state-space system directly from the original nonlinear system (according to @Aquatris). However, please note that I only ran the simulation for 2 seconds to allow for a clear observation of the differences in transient responses.
%% Parameters
m0 = 1.5;
m1 = 0.5;
m2 = 0.75;
L1 = 0.5;
L2 = 0.75;
g = 9.8;
d1 = m0 + m1 + m2;
d2 = (0.5*m1 + m2)*L1;
d3 = 0.5*m2*L2;
d4 = (1/3*m1 + m2)*L1^2;
d5 = 0.5*m2*L1*L2;
d6 = 1/3*m2*L2^2;
f1 = (0.5*m1 + m2)*L1*g;
f2 = 0.5*m2*L2*g;
%% Symbolic matrices in the nonlinear dynamics
syms Theta0 Theta1 Theta2 DTheta0 DTheta1 DTheta2;
x_ = transpose([Theta0 Theta1 Theta2 DTheta0 DTheta1 DTheta2]);
D(x_)= [d1 d2*cos(x_(2)) d3*cos(x_(3));
d2*cos(x_(2)) d4 d5*cos(x_(2)-x_(3));
d3*cos(x_(3)) d5*cos(x_(2)-x_(3)) d6];
G(x_)= [0;
-f1*sin(x_(2));
-f2*sin(x_(3))];
C(x_)=[0 -d2*sin(x_(2))*x_(5) -d3*sin(x_(3))*x_(6);
0 0 d5*sin(x_(2)-x_(3))*x_(6);
0 -d5*sin(x_(2)-x_(3))*x_(5) 0];
H = [1;
0;
0];
%% matrici A e B sistema linearizzato
% deriveG(x_) = transpose(jacobian(G(x_(1),x_(2),x_(3),x_(4),x_(5),x_(6)), [x_(1),x_(2),x_(3)]));
% A = [zeros(3) eye(3);-inv(D(0,0,0,0,0,0))*deriveG(0,0,0,0,0,0) zeros(3)];
% Anumeric = double(A)
% B = [zeros(3,1);
% inv(D(0,0,0,0,0,0))*H]
% Bnumeric = double(B)
%% matrici sistema non lineare
Anl(x_) = [zeros(3) eye(3);
zeros(3) -inv(D(x_(1), x_(2), x_(3), x_(4), x_(5), x_(6)))*C(x_(1), x_(2), x_(3), x_(4), x_(5), x_(6))];
Bnl(x_) = [zeros(3,1);
inv(D(x_(1),x_(2),x_(3),x_(4),x_(5),x_(6)))*H];
Hnl(x_) = [zeros(3,1);
-inv(D(x_(1), x_(2), x_(3), x_(4), x_(5), x_(6)))*G(x_(1), x_(2), x_(3), x_(4), x_(5), x_(6))];
%% Full nonlinear dynamics for Double Inverted Pendulum system (I guess!)
nonLeqn = Anl(x_(1), x_(2), x_(3), x_(4), x_(5), x_(6))*[x_(1); x_(2); x_(3); x_(4); x_(5); x_(6)] + Hnl(x_(1), x_(2), x_(3), x_(4), x_(5), x_(6)) + Bnl(x_(1), x_(2), x_(3), x_(4), x_(5), x_(6))*sym('u');
%% Linearization of nonlinear system using Jacobian method
Aa(x_) = jacobian(nonLeqn, [x_(1), x_(2), x_(3), x_(4), x_(5), x_(6)]);
Ass = double(Aa(0, 0, 0, 0, 0, 0))
Bb(x_) = jacobian(nonLeqn, sym('u'));
Bss = double(Bb(0, 0, 0, 0, 0, 0))
%% Progettazione controllore LQR
% Bryson rule: Bryson's method [17] was introduced to adjust Q and R weighting
% matricesas an attempt to overcome the drawbacks of using the trial and error method.
% According to this rule, Q and R can be calculated by finding the reciprocals
% of squares of the maximum acceptable values of the state and the input control variables.
Q = diag([50 50 50 700 700 700]);
R = 1;
sys = ss(Ass, Bss, eye(6), 0);
% Co = ctrb(sys);
% res = rank(Co); %rank is equal to number of state 6,system is controllable
% Ob = obsv(sys);
% res2 = rank(Ob); %%rank is equal to number of state 6,system is observable
[K, S, P] = lqr(sys,Q,R); %p are closed-loop poles system
newsys = ss((Ass-Bss*K), Bss, eye(6), 0);
% Pole Representation
reale = real(P);
immaginario = imag(P);
% Plot dei poli nel piano complesso (cartesiano)
figure;
plot(reale, immaginario, 'rx', 'MarkerSize', 10); % 'rx' indica crocette rosse
xlabel('Parte Reale');
ylabel('Parte Immaginaria');
title('Diagramma dei Poli (Cartesiano)');
grid on;
axis equal;
%% Simulate the linear system for initial conditions
[y,t,~] = initial(newsys, [0; deg2rad(10); -deg2rad(10); 0; 0; 0], 2);
%[y,t,x]=initial(sysnew,[0;deg2rad(20);deg2rad(20);0;0;0],10);
% Convert from rad to deg
y(:,2:3)=rad2deg(y(:,2:3));
y(:,5:6)=rad2deg(y(:,5:6));
figure
tl1 = tiledlayout(2,3, 'TileSpacing', 'Compact');
for i=1:1:6
nexttile
plot(t,y(:,i)), title('x'+string(i), 'FontSize', 10),
grid
end
title(tl1, 'Linear Double-Pendulum System under LQR control')
xlabel(tl1, 'Time')
%% ode45 for nonlinear model
x0 = [0; deg2rad(10); -deg2rad(10); 0; 0; 0]; %initial condition
t_a = linspace(0, 2, 201); %time vector
[t, x] = ode45(@(t, x) nlode(t, x, K, Anl, Bnl, Hnl), t_a, x0);
% Convert from rad to deg
x(:,2:3) = rad2deg(x(:,2:3));
x(:,5:6) = rad2deg(x(:,5:6));
figure
tl2 = tiledlayout(2,3, 'TileSpacing', 'Compact');
for i=1:1:6
nexttile
plot(t, x(:,i), 'color', "#D95319"), title('x'+string(i), 'FontSize', 10)
grid
end
title(tl2, 'Nonlinear Double-Pendulum System under LQR control')
xlabel(tl2, 'Time')
%% Nonlinear dynamics of Double Inverted Pendulum on a Cart system
function dxdt = nlode(t, x, K, Anl, Bnl, Hnl)
% Matrices
A = Anl(x(1), x(2), x(3), x(4), x(5), x(6));
B = Bnl(x(1), x(2), x(3), x(4), x(5), x(6));
H = Hnl(x(1), x(2), x(3), x(4), x(5), x(6));
% LQR controller
u = - K*x; % originally designed based on the Linearized pendulum system
% Nonlinear dynamics
dxdt_ = A*x + H + B*u;
dxdt = double(dxdt_(:));
end
4 Commenti
Più risposte (1)
Sam Chak
il 13 Mar 2024
You have the option to explore and experiment with it on your own. In this case, I used a straightforward 2nd-order system as an example and calculated the LQR gains for three scenarios: Baseline, High QR, and Low QR.
Case 1: Baseline
%% Case 1: Baseline
P = 1;
Q = P*eye(2) % state-weighted factor
R = P; % input-weighted factor
A = [0, 1; -3, -2];
B = [0; 5];
C = [1, 0];
D = 0;
sys = ss(A, B, C, D); % state-space system
K = lqr(A, B, Q, R)
pxy = ss(A-B*K, B, C, D); % proxy model
N = 1/dcgain(pxy);
cls = ss(A-B*K, B*N, C, D); % closed-loop system
S = stepinfo(cls)
step(sys), hold on
step(cls), grid on, hold off
legend('open-loop sys', 'closed-loop sys')
title('Case 1: Baseline with norm(Q) = norm(R) = 1')
Case 2: When both Q and R are high
%% Case 2: When both Q and R are high
P = 10000;
Q = P*eye(2) % state-weighted factor
R = P; % input-weighted factor
A = [0, 1; -3, -2];
B = [0; 5];
C = [1, 0];
D = 0;
sys = ss(A, B, C, D); % state-space system
K = lqr(A, B, Q, R)
pxy = ss(A-B*K, B, C, D); % proxy model
N = 1/dcgain(pxy);
cls = ss(A-B*K, B*N, C, D); % closed-loop system
S = stepinfo(cls)
step(sys), hold on
step(cls), grid on, hold off
legend('open-loop sys', 'closed-loop sys')
title('Case 2: When both norm(Q) and norm(R) are 10000')
Case 3: When both Q and R are low
%% Case 3: When both Q and R are low
P = 0.0001;
Q = P*eye(2) % state-weighted factor
R = P; % input-weighted factor
A = [0, 1; -3, -2];
B = [0; 5];
C = [1, 0];
D = 0;
sys = ss(A, B, C, D); % state-space system
K = lqr(A, B, Q, R)
pxy = ss(A-B*K, B, C, D); % proxy model
N = 1/dcgain(pxy);
cls = ss(A-B*K, B*N, C, D); % closed-loop system
S = stepinfo(cls)
step(sys), hold on
step(cls), grid on, hold off
legend('open-loop sys', 'closed-loop sys')
title('Case 3: When both norm(Q) and norm(R) are 0.0001')
0 Commenti
Vedere anche
Categorie
Scopri di più su Stability Analysis in Help Center e File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!