LQR controller for Rotary inverted pendulum tuning problems

I have to implement a controller for a rotary inverted pendulum. I managed to get it to a point where it stays upright for a while but then then simulation "breaks down". I think the encoder doesn't rotate fast enough and slowly loses controll over the bar.

I have attached the slx and the workspace where I calculate the LQR gains. I can't upload the parasolid files for the simscape model. Here is the relevant figures

PLANT

CONTROLLER

Tau OUTPUT

Bar angled relative to Top dead center = 0 and The bottom = 3.14..

I have tried chaning both the Q and R values of the LQR controller quite a bit but there is basically no change in the ouput.

Finally this is the bar angle graph when I constrain the encoder to have hard stops at 180 and -180 degrees

The scope is wrapped so those peaks just means the bar has spun past the lowest point

L_r = 0.12;        % Encoder length (m)
L_p = 0.26;       % Pendulum length (m)
G   = 9.81;          % gravity (m/s^2)
M_r = 0.0;           % Encoder mass not used in these linear entries
J_r = 9.98e-4;       % Encoder inertia (kg*m^2)
M_p = 0.127;         % pendulum mass (kg)
J_p = 0.001198;      % pendulum inertia (kg*m^2)
B_r = 0.0024;        % Encoder viscous damping (N*m/(rad/s))
B_p = 0.0024;        % pendulum viscous damping


J_T = (J_p + 0.25*M_p*L_p^2) * (J_r + M_p*L_r^2) - (0.5*M_p*L_p*L_r)^2;

% State space entries

A11 = 0;
A12 = 0;
A13 = 1;
A14 = 0;
A21 = 0;
A22 = 0;
A23 = 0;
A24 = 1;
A31 = 0;
A32 = 0.25*(M_p^2)*(L_p)*(L_r)*(G);
A33 = -(J_p+(0.25)*(M_p)*(L_p^2))*(B_r);
A34 = -((0.5)*(M_p)*(L_p)*(L_r)*(B_p));
A41 = 0;
A42 = (0.5)*(M_p)*(L_p)*(G)*(J_r+(M_p)*(L_r^2));
A43 = -((0.5)*(M_p)*(L_p)*(L_r)*(B_p));
A44 = -(J_r + (M_p)*(L_r^2))*(B_p);

B11 = 0;
B21 = 0;
B31 = J_p+0.25*(M_p)*(L_p^2);
B41 = 0.5*(M_p)*(L_r)*(L_r);

C11 = 1;
C12 = 0;
C13 = 0;
C14 = 0;
C21 = 0;
C22 = 1;
C23 = 0;
C24 = 0;

D11 = 0;
D21 = 0;


% State space matrices
A = [A11 A12 A13 A14; A21 A22 A23 A24; A31 A32 A33 A34; A41 A42 A43 A44];
A = (1/J_T)*A;

B = [B11; B21; B31; B41];
B = (1/J_T)*B;

C = [C11 C12 C13 C14; C21 C22 C23 C24 ];

D = [D11; D21];

% LQR design
Q = diag([10, 1, 100, 1]);   % Tune these weights
R = 0.1;                     % Torque penalty
K = lqr(A, B, Q, R);

[Wynand]

After messing around the the Q and R values more I achieved this

So the control is much better now but the encoder rotates too much and runs into the stops at 180 / -180 degrees and losses it. I have tried a few different Q and R values but this seems to just aggravate the encoder rotation

[Sam Chak]

Hi @Wynand

Is the rotary inverted pendulum linearized around the unstable equilibrium at 180° (where the pendulum is balanced in an upright position) to obtain the state-space representation ss(A, B, C, D)? Subsequently, are the state and input matrices (A and B) used to compute the LQR gains for the controller to swing the pendulum up from the rest position at 0°?

[Wynand]

Hi @Sam Chak

You have helped me on so many previous questions I always get more hopeful when I see your username pop up :)

So after I posted this I realised I made a lot of mistakes. An offset angle greater than 1 degree would make the system react violently. I have since gone over to a PD controller with inner and outer loop. I must admit I am extremely out of my depth and I have very little time to complete this project.

I have abandoned using the state space representation and have used to model in the post to just hand tune the controller. I have also changed the control input from torque to directly adjusting the rotor angle. I have still not had much success though. This is my controller function.

function theta_cmd = rotary_inverted_pendulum_controller(alpha, alpha_dot, theta_measured, dt)
    % ------------------------
    % Outer loop: pendulum PD with look-ahead
    % ------------------------
    Kp_outer = 5.0;       % proportional gain
    Kd_outer = 0.5;       % derivative gain
    T_lookahead = 0.05;   % seconds

    % Desired rotor angle from outer loop
    theta_des = Kp_outer * (alpha + T_lookahead * alpha_dot) + Kd_outer * alpha_dot;

    % ------------------------
    % Inner loop: velocity-limited rotor
    % ------------------------
    Kp_inner = 50;        % inner loop gain
    theta_error = theta_des - theta_measured;

    % Compute desired rotor velocity
    theta_dot_cmd = Kp_inner * theta_error;

    % Limit rotor velocity to motor maximum
    max_rotor_speed = 10;  
    theta_dot_cmd = min(max(theta_dot_cmd, -max_rotor_speed), max_rotor_speed);

    % Integrate rotor velocity to get commanded rotor position
    theta_cmd = theta_measured + theta_dot_cmd * dt;
end

And this is as good as I have managed to get it so far.

With this current setup.

I would greatly appreciate the help. I am underwater with assignments and this is destroying my time.

[Sam Chak]

Hi @Wynand

If you linearized the nonlinear inverted pendulum system around the unstable equilibrium at 180° and designed a linear feedback controller based on that linear system, then the pendulum should be able to maintain balance in an upright position when the initial angle is within the range of .

The rotary inverted pendulum system has been thoroughly studied by scholars. If you have the complete nonlinear mathematical model, you should be able to design a nonlinear controller to swing the pendulum up from the rest position at .

Please do not consider the encoder rate and actuator saturation during the preliminary non-hardware mathematical design stage.

Answer 1

[Satyam]

Hi,

According to my understanding it looks like you are experiencing instability or loss of control in your rotary inverted pendulum simulation after some time, possibly due to encoder limitations or model/controller mismatch. You can try out troubleshooting your issue and see if it works:

• Encoder Rate and Quantization: Ensure high sampling rate and resolution to avoid outdated or steppy feedback.
• Simulation Step Size: Use a small or variable solver step size to accurately capture fast dynamics.
• Model-Controller Mismatch: Verify the LQR controller is designed for the plant's operating region and initial conditions.
• Actuator Saturation: Check if actuator limits are reached, which can cause control loss.
• LQR Gain Sensitivity: Confirm Q/R tuning and correct gain application in the controller.
• Angle Wrapping: Handle discontinuities at ±π or ±180° to prevent sudden control jumps.
• Simscape Model Details: Match friction, damping, and remove hard stops to isolate instability sources

You can also try testing with a linear plant and logging all states to pinpoint instability onset. Use the Linear Analysis Tool in Simulink for model linearization: https://www.mathworks.com/help/slcontrol/ug/linear-analysis-using-simulink-control-design.html

I hope this would solve your query.