Rotary Inverted Pendulum State Space Model PID Cascade Control

52 次查看(过去 30 天)
Hello, i'm currently working on a rotary inverted pendulum, main objective being stabilizing it in its upright position and possibly following a reference on the pendulum arm. For reference : α will be the pendulum angle, θ the arm position. I'm trying to set up a cascade controller, exploiting the transfer functions and therefore finding .
I'll put an image of the simulink scheme below, i'm not sure if the problem is the tuning or the overall idea.
The model is linearized around the unstable equilibrium, and it should be precise enough since with Pole Placement and LQR it works, here's the code to have State Space.
% Motor
Rm = 8.4; % Resistance
kt = 0.042; % Current-torque (N-m/A)
km = 0.042; % Back-emf constant (V-s/rad)
% Rotary Arm
mr = 0.095; % Mass (kg)
r = 0.085; % Total length (m)
Jr = mr*r^2/3; % Moment of inertia about pivot (kg-m^2)
% Pendulum Link
mp = 0.024; % Mass (kg)
Lp = 0.129; % Total length (m)
l = Lp/2; % Pendulum center of mass (m)
Jp = mp*Lp^2/3; % Moment of inertia about pivot (kg-m^2)
Jt = 1.314528060e-08;
br = 4.7078e-05; % From identification
bp = 4e-4;
g = 9.81; % Gravity constant
K = 0.0061; % Stiffness coefficient as for least squares
a_31 = -Jp*K/Jt;
a_32 = mp^2*l^2*r*g/Jt;
a_33 = -(Jp*(km^2/Rm + br))/Jt;
a_34 = -(mp*l*r*bp)/Jt;
a_41 = -K*l*mp*r/Jt;
a_42 = (mp*g*l*Jr)/Jt;
a_43 = -(mp*l*r*(km^2/Rm +br))/Jt;
a_44 = -Jr*bp/Jt;
% State space description
A = [0 0 1 0;
0 0 0 1;
a_31 a_32 a_33 a_34;
a_41 a_42 a_43 a_44];
B = [0; 0; Jp*km/Rm; mp*r*l*km/Rm]/Jt;
C = [1 0 0 0;
0 1 0 0];
D = [0; 0];
sys = ss(A,B,C,D);
G = tf(sys); % G(1) for theta G(2) for alpha
G_alpha_theta = G(2)/G(1);
G_alpha_theta_simpl = minreal(G_alpha_theta,1e-3); % needed to close the cascaded loop
I'm aware I could use the gains found through pole placement to then set-up a PD like control, but i'd like to do it directly by shaping the PIDs on the transfer functions. For the tuning I've tuned C_theta on and C_alpha on .
The order of the outputs is but i'm not using velocities for the PID control since on the real system they're not measurable directly.

采纳的回答

Sam Chak
Sam Chak 2025-5-16
If you examine the transfer function from control input to θ, you will observe that it is a 4th-order function, with the coefficients of , , and all being negative. Although the proportional action of the PID controller can manipulate the term and the derivative action can influence the term, the sign of the term remains negative. According to the Routh–Hurwitz stability theorem, the PID-controlled system will still be unstable.
A 4th-order θ compensator can stabilize the transfer function, resulting in the inner closed-loop becoming an 8th-order system. However, placing an 8th-order system in series with a 2nd-order unstable system ​ (from θ to α) results in a 10th-order transfer function. In other words, you will need to design a 10th-order α compensator. Consequently, the outer closed-loop will become a substantial 20th-order system. The stabilization gains in a 20th-order system will be ridiculously high, as you need to stabilize the inverted pendulum at a very fast rate. If your computation can handle such precision in calculations, theoretically, it will work.
A practical approach is to use a parallel output-feedback PD controller configuration, where the PD gains are obtained from the LQR design method. Please see the block diagram below.
% Motor
Rm = 8.4; % Resistance
kt = 0.042; % Current-torque (N-m/A)
km = 0.042; % Back-emf constant (V-s/rad)
% Rotary Arm
mr = 0.095; % Mass (kg)
r = 0.085; % Total length (m)
Jr = mr*r^2/3; % Moment of inertia about pivot (kg-m^2)
% Pendulum Link
mp = 0.024; % Mass (kg)
Lp = 0.129; % Total length (m)
l = Lp/2; % Pendulum center of mass (m)
Jp = mp*Lp^2/3; % Moment of inertia about pivot (kg-m^2)
Jt = 1.314528060e-08;
br = 4.7078e-05; % From identification
bp = 4e-4;
g = 9.81; % Gravity constant
K = 0.0061; % Stiffness coefficient as for least squares
a_31= -Jp*K/Jt;
a_32= mp^2*l^2*r*g/Jt;
a_33= -(Jp*(km^2/Rm + br))/Jt;
a_34= -(mp*l*r*bp)/Jt;
a_41= -K*l*mp*r/Jt;
a_42= (mp*g*l*Jr)/Jt;
a_43= -(mp*l*r*(km^2/Rm +br))/Jt;
a_44= -Jr*bp/Jt;
% State space description
A = [0 0 1 0;
0 0 0 1;
a_31 a_32 a_33 a_34;
a_41 a_42 a_43 a_44];
B = [0;
0;
Jp*km/Rm;
mp*r*l*km/Rm]/Jt;
C = [1 0 0 0;
0 1 0 0];
D = [0;
0];
sys = ss(A, B, C, D);
G = tf(sys); % G(1) for input-to-theta G(2) for input-to-alpha
G01 = G(1) % input-to-theta TF
G01 = 50.64 s^2 + 152.1 s - 5776 -------------------------------------------- s^4 + 9.565 s^3 - 194.7 s^2 - 111.4 s - 7047 Continuous-time transfer function.
G12 = minreal(G(2)/G(1)) % theta-to-alpha TF
G12 = 0.9884 s^2 + 2.018e-17 s - 1.153e-14 ------------------------------------ s^2 + 3.005 s - 114.1 Continuous-time transfer function.
%% LQR design method
K = lqr(A, B, eye(4), 1)
K = 1×4
-2.7975 39.2611 -1.4004 3.0550
<mw-icon class=""></mw-icon>
<mw-icon class=""></mw-icon>
% check closed-loop eigenvalues
ecl = eig(A - B*K)
ecl =
-73.7109 + 0.0000i -8.0792 + 2.8970i -8.0792 - 2.8970i -1.6780 + 0.0000i
  1 个评论
Sam Chak
Sam Chak 2025-5-16
You also can see the Hurwitz-stable characteristic polynomial of degree 20 below.
m = 20;
HurwitzMatrix = sym(zeros(m + 1, m + 1));
for n = 0:m
for k = 0:n
HurwitzMatrix(n + 1, k + 1) = nchoosek(n, k);
end
end
disp('Hurwitz-stable Matrix:'); disp(HurwitzMatrix);
Hurwitz-stable Matrix:

请先登录,再进行评论。

更多回答(0 个)

Community Treasure Hunt

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

Start Hunting!

Translated by