How to simulate a non linear system

267 views (last 30 days)
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
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
Gianluca Di pietro
Gianluca Di pietro on 7 Mar 2024
Thank you so much!!i changed the vector x0 and i put the same initial condition of the linear one and looks like it works…even if matlab code requires a lot of time to solve the Ode45 when i choose a medium number of points with the Time linspace…just last question: In general is possible to apply the Lqr U control to the non linear model?and what the difference should be ?i Think they are similar for small angles and different for big ones ?I tried also to increase Q and R to study poles and they moved to the right while i increase Q but poles always stays negative even if they became closer to 0 for very high Q values…so in this case looks like that even if i increase Q it doesnt affect my system stability…but in general this is not true right?Q & R should modify the dynamic of the system maKing the sistem faster but for example potentially instable right?

Sign in to comment.

Accepted Answer

Sam Chak
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))
Ass = 6×6
0 0 0 1.0000 0 0 0 0 0 0 1.0000 0 0 0 0 0 0 1.0000 0 -7.3500 0.7875 0 0 0 0 73.5000 -33.0750 0 0 0 0 -58.8000 51.1000 0 0 0
Bb(x_) = jacobian(nonLeqn, sym('u'));
Bss = double(Bb(0, 0, 0, 0, 0, 0))
Bss = 6×1
0 0 0 0.6071 -1.5000 0.2857
%% 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
Gianluca Di pietro
Gianluca Di pietro on 7 Mar 2024
Oh understood i believed that cause i studied that the raggiungibility of (F,D) where Q= D* D^transpose from Chokesky and F came from Xdot = Fx+Gu was necessary for stability of closed loop system…in this case in matlab yes looks like Q and R just moves the poles and act on speed system but dont change the stability.Thanks so much everyone for the help!
Gianluca Di pietro
Gianluca Di pietro on 13 Mar 2024
And what happened in general if Q and R are both high or both low?

Sign in to comment.

More Answers (1)

Sam Chak
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
Q = 2x2
1 0 0 1
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)
K = 1x2
0.5662 0.7775
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)
S = struct with fields:
RiseTime: 1.8504 TransientTime: 3.3568 SettlingTime: 3.3568 SettlingMin: 0.9013 SettlingMax: 0.9994 Overshoot: 0 Undershoot: 0 Peak: 0.9994 PeakTime: 6.1504
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
Q = 2x2
10000 0 0 10000
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)
K = 1x2
0.5662 0.7775
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)
S = struct with fields:
RiseTime: 1.8504 TransientTime: 3.3568 SettlingTime: 3.3568 SettlingMin: 0.9013 SettlingMax: 0.9994 Overshoot: 0 Undershoot: 0 Peak: 0.9994 PeakTime: 6.1504
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
Q = 2x2
1.0e-04 * 1.0000 0 0 1.0000
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)
K = 1x2
0.5662 0.7775
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)
S = struct with fields:
RiseTime: 1.8504 TransientTime: 3.3568 SettlingTime: 3.3568 SettlingMin: 0.9013 SettlingMax: 0.9994 Overshoot: 0 Undershoot: 0 Peak: 0.9994 PeakTime: 6.1504
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')

Community Treasure Hunt

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

Start Hunting!