Questions about the PID controller of an inverted pendulum

12 views (last 30 days)
Carmen Serrano Portillo on 29 Jan 2024
I need help with this problem I am trying to solve. I have implemented two codes, one for a PD controller and another for a PID controller. The first one works correctly without any issues, but the second one gives me a dimension error. I understand that it is due to the difference in sizes of K.A, K.B, K.C, and K.D. I can't find a way to solve the problem, and I would appreciate it if you could help me. I am sharing my code:
Code for PD close-loop controller:
%% Define the process model
% Define the constants values
M = 0.5; % Mass
g = 9.81; % Acceleration due to gravity
f = 0.25; % Friction coefficient
l = 1; % Length
% Define the operation parameters, force, and torque.
pos_oper = 0; % Operating position
torque_oper = 0; % Operating torque
% Define the structure model for process part
Model.Process.StateEq = @(t,theta,T) [theta(2); g*sin(theta(1)) - f/M*theta(2)+T/M];
Model.Process.OutputEq = @(t,theta,T) theta(1);
Process_Order = 2;
%% Linear PD controller
% Hand-tuned PD controller (with derivative noise filter):
s = tf('s');
Kp = 9.71; % Proportional gain
Kd = 3.03; % Derivative gain
filter = 20; % Derivative noise filter
F_transfer = Kp+Kd*s/(s/filter + 1);
K = ss(F_transfer);
Control_Order = size(K.A,1);
%% Define the control model
% Define the model - control - stateEq
Model.Control.StateEq = @(t,theta,y) K.A*theta+K.B*(-(y-pos_oper));
T_limit_sup = 3.5; % Upper torque limit
T_limit_inf = -3.5; % Lower torque limit
saturate=@(T) max(min(T,T_limit_sup),T_limit_inf);
% Define the model-control-output equation
Model.Control.OutputEq = @(t,theta,y) saturate(torque_oper + K.C*theta + K.D*(-(y-pos_oper)));
%% Numerical simulation
Idx_process=1:Process_Order;
Idx_control = Process_Order + (1:Control_Order);
odefun = @(t,FullState) CL_StateEQ(t,FullState(Idx_process),FullState(Idx_control),Model);
Ini_state_process = [pi/30;0]; % Initial state for the process
Ini_state_control = 0; % Initial state for the control
x0 = [Ini_state_process;Ini_state_control];
t_fin = 6; % Final time for simulation
CPU_ode45 =cputime;
tic;
[T_ode45,X_ode45] = ode45(odefun, [0 t_fin], x0, odeset('RelTol',1e-6,'AbsTol',1e-6));
Unrecognized function or variable 'CL_StateEQ'.

Error in solution>@(t,FullState)CL_StateEQ(t,FullState(Idx_process),FullState(Idx_control),Model) (line 34)
odefun = @(t,FullState) CL_StateEQ(t,FullState(Idx_process),FullState(Idx_control),Model);

Error in odearguments (line 92)
f0 = ode(t0,y0,args{:}); % ODE15I sets args{1} to yp0.

Error in ode45 (line 104)
odearguments(odeIsFuncHandle,odeTreatAsMFile, solver_name, ode, tspan, y0, options, varargin);
ejecution_time_ode45 = toc;
CPU_ode45 = cputime - CPU_ode45;
Wrong code for PID controller:
%% Define the process model
% Define the constants values
M = 0.5; % Mass
g = 9.81; % Acceleration due to gravity
f = 0.25; % Friction coefficient
l = 1; % Length
% Define the operation parameters, force, and torque.
pos_oper = 0; % Operating position
torque_oper = 0; % Operating torque
% Define the structure model for process part
Model.Process.StateEq = @(t,theta,T) [theta(2); g*sin(theta(1)) - f/M*theta(2)+T/M];
Model.Process.OutputEq = @(t,theta,T) theta(1);
Process_Order = 2;
%% Linear PD controller
% Hand-tuned PD controller (with derivative noise filter):
s = tf('s');
Kp = 7; % Proportional gain
Ki = 0.01;
Kd = 1.3; % Derivative gain
filter = 20; % Derivative noise filter
F_transfer = Kp+Ki/s+Kd*s/(s/filter + 1);
K = ss(F_transfer);
Control_Order = size(K.A,1);
%% Define the control model
% Define the model - control - stateEq
Model.Control.StateEq = @(t,theta,y) K.A*theta+K.B*(-(y-pos_oper));
T_limit_sup = 3.5; % Upper torque limit
T_limit_inf = -3.5; % Lower torque limit
saturate=@(T) max(min(T,T_limit_sup),T_limit_inf);
% Define the model-control-output equation
Model.Control.OutputEq = @(t,theta,y) saturate(torque_oper + K.C*theta + K.D*(-(y-pos_oper)));
%% Numerical simulation
Idx_process=1:Process_Order;
Idx_control = Process_Order + (1:Control_Order);
odefun = @(t, FullState) [
Model.Process.StateEq(t, FullState(Idx_process), Model.Control.OutputEq(t, FullState(Idx_control), Model.Process.OutputEq(t, FullState(Idx_process), 0)));
Model.Control.StateEq(t, FullState(Idx_control), Model.Process.OutputEq(t, FullState(Idx_process), 0), Model.Process.OutputEq)
];
Ini_state_process = [pi/30;0]; % Initial state for the process
Ini_state_control = 0; % Initial state for the control
x0 = [Ini_state_process;Ini_state_control];
t_fin = 6; % Final time for simulation
CPU_ode45 =cputime;
tic;
[T_ode45,X_ode45] = ode45(odefun, [0 t_fin], x0, odeset('RelTol',1e-6,'AbsTol',1e-6));
ejecution_time_ode45 = toc;
CPU_ode45 = cputime - CPU_ode45;
%%
odefun = @(t,FullState) CL_StateEQ(t,FullState(Idx_process),FullState(Idx_control(2,1)),Model);
Ini_state_process = [pi/30;0]; % Initial state for the process
Ini_state_control = 0; % Initial state for the control
x0 = [Ini_state_process;Ini_state_control];
t_fin = 6; % Final time for simulation
CPU_ode45 =cputime;
tic;
[T_ode45,X_ode45] = ode45(odefun, [0 t_fin], x0, odeset('RelTol',1e-6,'AbsTol',1e-6));
ejecution_time_ode45 = toc;
CPU_ode45 = cputime - CPU_ode45;
Jon on 29 Jan 2024
I tried to run the code you attached to see if I could reproduce the problem and help you debug it, but I do not have the function CL_StateEQ. Can you please attach this, and any other code needed to provide a self contained example that reproduces the problem.
Carmen Serrano Portillo on 29 Jan 2024
Hi, Is true, sorry I added it here. Thanks you so much!
function dxdtbc=CL_StateEQ(t,ProcessState,ControlState, M)
% That indicates de close-loop
Measurements=M.Process.OutputEq(t,ProcessState);
dControllerStatedt=M.Control.StateEq(t,ControlState,Measurements);
u=M.Control.OutputEq(t,ControlState,Measurements);
dSProcesStatedt=M.Process.StateEq(t,ProcessState,u);
dxdtbc=[dSProcesStatedt;dControllerStatedt];
end

Sam Chak on 29 Jan 2024
Could you please check if the current response of the inverted pendulum under the PID controller aligns with the expected result? In the code, I have marked the lines where I made changes with '<--'.
%% Define the process model
% Define the constants values
M = 0.5; % Mass
g = 9.81; % Acceleration due to gravity
f = 0.25; % Friction coefficient
l = 1; % Length
% Define the operation parameters, force, and torque.
pos_oper = 0; % Operating position
torque_oper = 0; % Operating torque
% Define the structure model for process part
Model.Process.StateEq = @(t,theta,T) [theta(2); g*sin(theta(1)) - f/M*theta(2)+T/M];
Model.Process.OutputEq = @(t,theta,T) theta(1);
Process_Order = 2;
%% Linear PID controller <--
% Hand-tuned PID controller (with derivative noise filter): <--
s = tf('s');
Kp = 7; % Proportional gain
Ki = 0.01;
Kd = 1.3; % Derivative gain
filter = 20; % Derivative noise filter
F_transfer = Kp+Ki/s+Kd*s/(s/filter + 1);
K = ss(F_transfer);
Control_Order = size(K.A,1);
%% Define the control model
% Define the model - control - stateEq
Model.Control.StateEq = @(t,theta,y) K.A*theta+K.B*(-(y-pos_oper));
T_limit_sup = 3.5; % Upper torque limit
T_limit_inf = -3.5; % Lower torque limit
saturate=@(T) max(min(T,T_limit_sup),T_limit_inf);
% Define the model-control-output equation
Model.Control.OutputEq = @(t,theta,y) saturate(torque_oper + K.C*theta + K.D*(-(y-pos_oper)));
%% Numerical simulation
Idx_process=1:Process_Order;
Idx_control = Process_Order + (1:Control_Order);
% odefun = @(t, FullState) [
% Model.Process.StateEq(t, FullState(Idx_process), Model.Control.OutputEq(t, FullState(Idx_control), Model.Process.OutputEq(t, FullState(Idx_process), 0)));
% Model.Control.StateEq(t, FullState(Idx_control), Model.Process.OutputEq(t, FullState(Idx_process), 0), Model.Process.OutputEq)
% ];
% Ini_state_process = [pi/30;0]; % Initial state for the process
% Ini_state_control = [0; 0]; % Initial state for the control
% x0 = [Ini_state_process;Ini_state_control];
% t_fin = 6; % Final time for simulation
% CPU_ode45 =cputime;
% tic;
% [T_ode45,X_ode45] = ode45(odefun, [0 t_fin], x0, odeset('RelTol',1e-6,'AbsTol',1e-6));
% ejecution_time_ode45 = toc;
% CPU_ode45 = cputime - CPU_ode45;
%%
odefun = @(t,FullState) CL_StateEQ(t, FullState(Idx_process), FullState(Idx_control), Model); % <--
Ini_state_process = [pi/30;0]; % Initial state for the process
Ini_state_control = [0; 0]; % TWO Initial state for the control <--
x0 = [Ini_state_process;Ini_state_control];
t_fin = 6; % Final time for simulation
CPU_ode45 = cputime;
tic;
[T_ode45,X_ode45] = ode45(odefun, [0 t_fin], x0, odeset('RelTol',1e-6,'AbsTol',1e-6));
ejecution_time_ode45 = toc;
CPU_ode45 = cputime - CPU_ode45;
function dxdtbc = CL_StateEQ(t, ProcessState, ControlState, M)
% That indicates de close-loop
Measurements = M.Process.OutputEq(t, ProcessState);
dControllerStatedt = M.Control.StateEq(t, ControlState, Measurements);
u = M.Control.OutputEq(t, ControlState, Measurements);
dSProcesStatedt = M.Process.StateEq(t, ProcessState, u);
dxdtbc = [dSProcesStatedt; dControllerStatedt];
end
Carmen Serrano Portillo on 29 Jan 2024
Oh my God, yes, thank you very much! I am very grateful for the help. Best regards