4
$\begingroup$

I'm trying to implement the tracking problem for this example using PID controller. The dynamic equation is

$$ I \ddot{\theta} + d \dot{\theta} + mgL \sin(\theta) = u $$

where

$\theta$ : joint variable.

$u$ : joint torque

$m$ : mass.

$L$ : distance between centre mass and joint.

$d$ : viscous friction coefficient

$I$ : inertia seen at the rotation axis.

$\textbf{Regulation Problem:}$

In this problem, the desired angle $\theta_{d}$ is constant and $\theta(t)$ $\rightarrow \theta_{d}$ and $\dot{\theta}(t)$ $\rightarrow 0$ as $t$ $\rightarrow \infty$. For PID controller, the input $u$ is determined as follows

$$ u = K_{p} (\theta_{d} - \theta(t)) + K_{d}( \underbrace{0}_{\dot{\theta}_{d}} - \dot{\theta}(t) ) + \int^{t}_{0} (\theta_{d} - \theta(\tau)) d\tau $$

The result is

enter image description here

and this is my code main.m

clear all
clc

global error;
error = 0;

t = 0:0.1:5;

x0 = [0; 0];

[t, x] = ode45('ODESolver', t, x0);

e = x(:,1) - (pi/2); % Error theta

plot(t, e, 'r', 'LineWidth', 2);
title('Regulation Problem','Interpreter','LaTex');
xlabel('time (sec)');
ylabel('$\theta_{d} - \theta(t)$', 'Interpreter','LaTex');
grid on

and ODESolver.m is

function dx = ODESolver(t, x)

global error; % for PID controller

dx = zeros(2,1);

%Parameters:
m = 0.5;       % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1;         % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81;      % acceleration due to gravity m/s^2


% PID tuning
Kp = 5;
Kd = 1.9;
Ki = 0.02;

% u: joint torque
u = Kp*(pi/2 - x(1)) + Kd*(-x(2)) + Ki*error;
error = error + (pi/2 - x(1));

dx(1) = x(2);
dx(2) = 1/I*(u - d*x(2) - m*g*L*sin(x(1)));

end

$\textbf{Tracking Problem:}$

Now I would like to implement the tracking problem in which the desired angle $\theta_{d}$ is not constant (i.e. $\theta_{d}(t)$); therefore, $\theta(t)$ $\rightarrow \theta_{d}(t)$ and $\dot{\theta}(t)$ $\rightarrow \dot{\theta}_{d}(t)$ as $t$ $\rightarrow \infty$. The input is

$$ u = K_{p} (\theta_{d} - \theta(t)) + K_{d}( \dot{\theta}_{d}(t) - \dot{\theta}(t) ) + \int^{t}_{0} (\theta_{d}(t) - \theta(\tau)) d\tau $$

Now I have two problems namely to compute $\dot{\theta}_{d}(t)$ sufficiently and how to read from txt file since the step size of ode45 is not fixed. For the first problem, if I use the naive approach which is

$$ \dot{f}(x) = \frac{f(x+h)-f(x)}{h} $$

the error is getting bigger if the step size is not small enough. The second problem is that the desired trajectory is stored in txt file which means I have to read the data with fixed step size but I'v read about ode45 which its step size is not fixed. Any suggestions!


Edit:

For tracking problem, this is my code

main.m

clear all
clc

global error theta_d dt;
error = 0;


theta_d = load('trajectory.txt');

i    = 1;
t(i) = 0;
dt   = 0.1;
numel(theta_d)
while ( i < numel(theta_d) )
   i = i + 1;
   t(i) = t(i-1) + dt;
end

x0 = [0; 0];
options= odeset('Reltol',dt,'Stats','on');
[t, x] = ode45(@ODESolver, t, x0, options);

 e = x(:,1) - theta_d; % Error theta


plot(t, x(:,2), 'r', 'LineWidth', 2);
title('Tracking Problem','Interpreter','LaTex');
xlabel('time (sec)');
ylabel('$\dot{\theta}(t)$', 'Interpreter','LaTex');
grid on

ODESolver.m

function dx = ODESolver(t, x)

persistent i theta_dPrev

if isempty(i)
    i = 1;
    theta_dPrev = 0;
end

global error theta_d dt ; 

dx = zeros(2,1);

%Parameters:
m = 0.5;       % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1;         % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81;      % acceleration due to gravity m/s^2


% PID tuning
Kp = 35.5;
Kd = 12.9;
Ki = 1.5;

if ( i == 49 )
    i = 48;
end
% theta_d first derivative
theta_dDot  = ( theta_d(i) - theta_dPrev ) / dt;
theta_dPrev = theta_d(i);


% u: joint torque
u = Kp*(theta_d(i) - x(1)) + Kd*( theta_dDot - x(2)) + Ki*error;
error = error + (theta_dDot - x(1));

dx(1) = x(2);
dx(2) = 1/I*(u - d*x(2) - m*g*L*sin(x(1)));

i = i + 1;
end

trajectory's code is

clear all 
clc

a = 0:0.1:(3*pi)/2;

file = fopen('trajectory.txt','w');


for i = 1:length(a)
    fprintf(file,'%4f \n',a(i));
end

fclose(file);

The result of the velocity is

enter image description here

Is this correct approach to solve the tracking problem?

$\endgroup$

4 Answers 4

2
$\begingroup$

I've made few changes to your ODESolver:

  1. The time step requested by ODE45 is not going to match what you have in your file. In your ODESolver() desired thetas are read one after the other and then the last one is repeated. As a result the desired theta was not a function of $t$. I used interp1() to fix that.

  2. The time difference between two iteration of ODE45 is not constant so you can't use dt to calculate the $\Delta{\theta}$. This is fixed by storing the t in a variable for future reference.

  3. Same goes for calculating the integral part of PID.

Here is the new ODESolver():

function dx = ODESolver(t, x)
persistent theta_dPrev time_prev

if isempty(theta_dPrev)
    theta_dPrev = 0;
    time_prev = 0;
end

global error theta_d dt time_array; 

dx = zeros(2,1);

%Parameters:
m = 0.5;       % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1;         % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81;      % acceleration due to gravity m/s^2


% PID tuning
Kp = 40;%35.5;
Kd = 10;%12.9;
Ki = 20;%1.5;

% theta_d first derivative
theta_desired_current = interp1(time_array,theta_d,t);
if t==time_prev
    theta_dDot = 0;
else
    theta_dDot  = ( theta_desired_current - theta_dPrev ) / (t-time_prev);
end
theta_dPrev = theta_desired_current;


% u: joint torque
u = Kp*(theta_desired_current - x(1)) + Kd*( theta_dDot - x(2)) + Ki*error;
error = error + (theta_desired_current - x(1))*(t-time_prev);

dx(1) = x(2);
dx(2) = 1/I*(u - d*x(2) - m*g*L*sin(x(1)));



time_prev = t;
end

I also changed the main.m to the following:

clear all; close all; clc;

global error theta_d dt time_array;
error = 0;

theta_d = load('trajectory.txt');
% theta_d = sin([1:.1:20]/3);

time_array=0:dt:dt*(numel(theta_d)-1);


x0 = [0; 0];
options= odeset('Reltol',dt,'Stats','on');
[t_ode, x] = ode45(@ODESolver, [time_array(1),time_array(end)], x0, options);

theta_desired_ode = interp1(time_array,theta_d,t_ode);
 e = x(:,1) - theta_desired_ode; % Error theta

figure(1)
plot(t_ode, x(:,2), 'r', 'LineWidth', 2);
title('Velocity','Interpreter','LaTex');
xlabel('time (sec)');
ylabel('$\dot{\theta}(t)$', 'Interpreter','LaTex');
grid on

figure(2)
plot(t_ode,x(:,1))
hold on 
plot(t_ode,theta_desired_ode)
title('Theta','Interpreter','LaTex');
xlabel('time (sec)');
ylabel('$\dot{\theta}(t)$', 'Interpreter','LaTex');
legend('Actual Theta', 'Desired Theta')
grid on
$\endgroup$
6
  • $\begingroup$ wow this is really nice work. Thank you so much. The results are perfect. $\endgroup$
    – CroCo
    Mar 24, 2015 at 19:31
  • 1
    $\begingroup$ I'm glad you found it useful. If you plan to implement it on hardware or want to take your control system one step further, try using anti-windup PID instead of the regular one. $\endgroup$
    – BarzinM
    Mar 24, 2015 at 19:36
  • $\begingroup$ Any good reference for anti-windup PID? $\endgroup$
    – CroCo
    Mar 24, 2015 at 19:41
  • 1
    $\begingroup$ page 11: cds.caltech.edu/~murray/courses/cds101/fa02/caltech/… page 10: cse.lab.imtlucca.it/~bemporad/teaching/ac/pdf/… $\endgroup$
    – BarzinM
    Mar 24, 2015 at 19:44
  • 1
    $\begingroup$ I'm not a big fan of global variables but since you are already using them, you can also set u as global. A better way would be using object-oriented programming. I also want to suggest not using ODE45. Make a loop with fixed time step specially since you seem to be interested in hardware in future. A loop would simulate how your controller and sensors work much much better. Also, it is going to be easier to implement your already developed codes on hardware. $\endgroup$
    – BarzinM
    Mar 25, 2015 at 16:29
3
$\begingroup$

Since both $\theta_d\left(t\right)$ and $\dot\theta_d\left(t\right)$ are references at your disposal, i.e. you have to provide them in some way, why don't you simply play with $\dot\theta_d\left(t\right)$ and then compute $\theta_d\left(t\right)$ accordingly by means of integration? As you might know, integration is a well posed operation compared with the derivative.

On the other hand, $\dot\theta\left(t\right)$ is a different "beast". In physical systems you barely have access to it (you'd need ad-hoc sensors for measuring it). Valid alternatives are: Kalman estimation, Savitzky-Golay filtering, use of proper D term in the controller equipped with a high-frequency pole.

$\endgroup$
4
  • $\begingroup$ position reference is more intuitive than velocity reference even though what you are saying is perfect and a good alternative. Since I'm doing simulation, I am able to access to the velocity, therefore data is available every instant. Having said that, your answer is alternative but not a solution to my problem. Thank you so much though. $\endgroup$
    – CroCo
    Mar 24, 2015 at 19:17
  • $\begingroup$ Are you sure that you have a too generic $\theta_d\left(t\right)$ so that you cannot come up with a analytical derivative of it? How do you produce your position reference profile? I'm insisting because these details are very important in practice. What is the purpose of your simulation then? Not to go to a real implementation? Does it have only a didactic purpose? I understand you can solve the specific problem differently, but this peculiarity is too narrow and there's the risk that it hides other issues, as I said. $\endgroup$ Mar 24, 2015 at 19:54
  • $\begingroup$ I'm focusing now on simulation to enhance my skills. The problem with hardware is costly first and it needs some space and tools. A lot of academic papers are based on pure simulation. I will stick with it until I got some money and space. :) $\endgroup$
    – CroCo
    Mar 24, 2015 at 20:10
  • 1
    $\begingroup$ I see, you've started tackling the problem. "A lot of academic papers are based on pure simulation", that's right why our field (I'm in academic, tough I'll never forget I'm an engineer too) is being populated with questionable works and everything's becoming messy. Remember: exercise always your critics and never trust them :-) Good luck for the future. $\endgroup$ Mar 24, 2015 at 20:21
2
$\begingroup$

You can treat your differential equation as a third order ODE, because of the integral in the PID controller. For this I will denote the integral of $\theta$ as $\Theta$, such that the ODE becomes,

$$ I\ddot{\theta}+d\dot{\theta}+mgL\sin(\theta)+K_p\left(\theta-\theta_d(t)\right)+K_i\left(\Theta-\int_0^t\theta_d(\tau)d\tau\right)+K_d\left(\dot{\theta}-\dot{\theta}_d(t)\right)=0 $$

One way you can write this into MATLAB code would be:

clear all
clc

% Parameters:
m = 0.5;       % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1;         % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81;      % acceleration due to gravity m/s^2

% PID tuning
Kp = 5000;
Ki = 166667;
Kd = 50;

% Define reference signal
syms t
a = 3 * pi / 10;
temp = a * t;
% temp = pi/2 + 0*t;
 r = matlabFunction(temp, 'Vars', t);
 R = matlabFunction(int(temp,t), 'Vars', t);
dr = matlabFunction(diff(temp), 'Vars', t);
clear temp t

fun = @(t,x) [x(2); x(3); -m*g*L/I*sin(x(2))-d/I*x(3)] - [0; 0; ...
    Kp / I * (x(2) -  r(t)) + ...
    Ki / I * (x(1) -  R(t)) + ...
    Kd / I * (x(3) - dr(t))];

T = linspace(0, 5, 1e3);
x0 = [0; 0; 0];
[t, Y] = ode45(@(t,x) fun(t,x), T, x0);

y = zeros(size(t));
for i = 1 : length(t)
    y(i) = r(t(i));
end

figure
    subplot(2,1,1)
    plot(t, y, t, Y(:,2))
    xlabel('Time [s]')
    ylabel('Position [rad]')
    subplot(2,1,2)
    plot(t, y-Y(:,2))
    xlabel('Time [s]')
    ylabel('Position error [rad]')
$\endgroup$
-3
$\begingroup$

I think that just PID is not a good approach to solve a tracking problem.

It is better to use nonlinear control method that are suitable for that task like "Exact feedforward linearization based on differential flatness". You can get an overview in the open access paper: https://hal.archives-ouvertes.fr/hal-00431712 and references therein.

$\endgroup$
1
  • 1
    $\begingroup$ "PID is not a good approach to solve a tracking problem" sounds pretty much like an heresy :-) $\endgroup$ Apr 2, 2015 at 14:18

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.