I have to evaluate the error for each state changing Q and R and i have to show that a response is faster or slower changing Q and R
Evaluate Settling Time,Constant Time of my System
36 views (last 30 days)
Show older comments
Hello everyone,i am studying my system,it'about an LQR control of a double pendulum around 0°angle equilibrium.I have to discuss about how poles of my system moves,changing Q and R values and i have to discuss about how Q and R high values could make my system faster but potentially create oscillations and in opposite case,slower...so i wanted to analize the Settling Time of my LQR linear system states plots to show that..How could i evaluate from my plots the settling time.I saw something around stepinfo but that was the response of system from step input in my case there is no input..i tried something to see when values where higher than 63% of final state value but it didnt work..can someone please help me.thanks
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);
%%info = stepinfo(newsys);
%%settling_time = info.SettlingTime; % Tempo di assestamento
% 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(20); -deg2rad(20); 0; 0; 0], 10);
%[y,t,x]=initial(sysnew,[0;deg2rad(20);deg2rad(20);0;0;0],10);
%%I NEED HERE TO EVALUATE TIME TO SHOW IF SYSTEM IS FASTER OR SLOWER
%%CHANGING Q AND R
% 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(20); -deg2rad(20); 0; 0; 0]; %initial condition
t_a = linspace(0, 10,101); %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')
and the function is :
%% 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
also i want to know why if i change angles and i try higher angles my code became very slow and i cant compute,thanks
7 Comments
Accepted Answer
Sam Chak
on 13 Mar 2024
Edited: Sam Chak
on 13 Mar 2024
To observe the state responses, you can manually adjust the design values for the state and input-weighted factors Q and R. Once you have extracted the response characteristics, such as Settling Time and Percent Overshoot, from the response data using the 'stepinfo()' function, you can utilize the 'struct2table()' command to create a table for displaying and comparing these characteristics.
In the example below, I have demonstrated the comparison for and . However, I'm unsure about what you meant by the term "time constant" since it is typically used in reference to first-order systems, which is not the case for your double-inverted pendulum system.
Update: Demonstrated the computation of steady-steady errors and added them into the stepinfo Table.
%% 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 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
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
Q = diag([50 50 50 700 700 700]); % <-- design value input by the User
R = 1; % <-- design value input by the User
sys = ss(Ass, Bss, eye(6), 0);
[K,S,P] = lqr(sys, Q, R); % p are closed-loop poles system
newsys = ss(Ass-Bss*K, Bss, eye(6), 0);
%% Pole Representation
figure;
myLocus = ss(Ass-Bss*K, Bss, [1 0 0 0 0 0], 0);
rlocus(myLocus), grid on, ylim([-15, 15]) % maybe making the graph more meaningful
%% Simulate the linear system for initial conditions
[y, t, ~] = initial(newsys, [0; deg2rad(10); -deg2rad(10); 0; 0; 0], 5);
y(:,2:3) = rad2deg(y(:,2:3)); % Convert from rad to deg
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')
%% Computation of Steady-state Errors (theoretical results)
setpoint= 0;
theta1 = ss(Ass-Bss*K, Bss, [0 1 0 0 0 0], 0); % 2nd state as output
ssOut1 = dcgain(theta1) % steady-state response of theta 1
ssErr1 = setpoint - ssOut1 % steady-state error for theta 1
theta2 = ss(Ass-Bss*K, Bss, [0 0 1 0 0 0], 0); % 3rd state as output
ssOut2 = dcgain(theta2) % steady-state response of theta 2
ssErr2 = setpoint - ssOut2 % steady-state error for theta 2
%% Evaluate Response Characteristics from Response Data
y1 = y(:,2); % theta 1
y1final = y1(end); % Beware!: not exactly relative to 0°
y1init = 10;
S1 = stepinfo(y1, t, y1final, y1init);
S1.SteadyStateError = ssErr1; % add Steady-state Error into S1 structure
y2 = y(:,3); % theta 2
y2final = y2(end); % Beware!: not exactly relative to 0°
y2init = 10;
S2 = stepinfo(y2, t, y2final, y2init);
S2.SteadyStateError = ssErr2; % add Steady-state Error into S2 structure
%% Create a stepInfoTable to tabulate the Response Characteristics
stepInfoTable = struct2table([S1, S2]);
stepInfoTable = removevars(stepInfoTable, {'SettlingMin', 'SettlingMax', 'Undershoot', 'Peak', 'PeakTime'});
stepInfoTable.Properties.RowNames = {'theta 1', 'theta 2'};
stepInfoTable
5 Comments
Sam Chak
on 13 Mar 2024
However, the steady-state error of the Cart Position in this case can be easily addressed by incorporating a sort of "Pre-amp" at the Reference Input port. This will amplify the commanded signal to a level sufficient for achieving zero steady-state error.
%% Compute steady-state error for state 1 (x1)
Proxy1 = ss(Ass-Bss*K, Bss, [1 0 0 0 0 0], 0); % Proxy Cart Position model
Preamp = 1/dcgain(Proxy1)
CartPos = ss(Ass-Bss*K, Bss*Preamp, [1 0 0 0 0 0], 0); % Cart Position as output
ssOut = dcgain(CartPos) % steady-state response of Cart Position
setpoint= 1; % to inject a unit step input signal
step(CartPos), grid on
ssErr = setpoint - ssOut % steady-state error for Cart Position
More Answers (0)
See Also
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!