How to simulate a non linear system
266 views (last 30 days)
Show older comments
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 Comments
Walter Roberson
on 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
Accepted Answer
Sam Chak
on 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 Comments
More Answers (1)
Sam Chak
on 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 Comments
See Also
Categories
Find more on Stability Analysis 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!