smc_invertedpendulum.m

I upload Inverted pendulum matlab code

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Example of SMC (Sliding Mode Control) for an inverted pendulum
%
% Affiliation: Ocean Engineering Lab., Seoul National University
% Name: Gyungnam Jo
%
% Problem: second order oscillation system (Mass-Spring-Damper system)
% system
% dx(1)/dt = x(2)
% dx(2)/dt = (-g/l)*sin(x(1)+a) + (-k/m)*x(2) + (1/(m*l^2))*u
%
% f(x) = (-g/l)*sin(x(1)+a) + (-k/m)*x(2)
% g(x) = 1/(m*l^2)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

close all
clear all
clc

disp('Example of SMC (Sliding Mode Control)')

tTimeStep = 5E-02;      % step size for numerical integration
tTotalTime = 12;        % total simulation time
tHorizon = round(tTotalTime/tTimeStep);

disp(sprintf('Time step is %f sec\nTotal simulation time is %f sec', tTimeStep, tTotalTime))
tic

% values for RK4 (4th order Runge-Kutta)
k1 = [0 0]';
k2 = [0 0]';
k3 = [0 0]';
k4 = [0 0]';

x   = [3 0]';   % initial value: x1(0) = 3, x2(0) = 0
ui  = 0;

g = 9.81;           % acceleration due to gravity
m = 0.125;          % mass
k = 0.025;          % coefficient of friction
l = 1;              % length
alpha = 0;          % phase angle
values = [g m k l alpha]';

a = 1;              % sliding surface factor
b1 = 0.2;
b2 = 0.1;

out = zeros(tHorizon+1, 3);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SMC
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

out(1, :) = [x' ui];    % at t = 0
for t = 2:1:(tHorizon+1)
    s = x(2) + a*x(1);

%-------------------------------------------------------------------------------
% signum input, exact SMC, state vector follows the sliding mode manifold.
    ui = g*m*l*sin(x(1)+alpha) + (k-a)*(m*l*l)*x(2) - (b1+b2*s*s)*sign(s);

%-------------------------------------------------------------------------------
% hyperbolic tangent function to reduce the chattering phenom
%     ui = g*m*l*sin(x(1)+alpha) + (k-a)*(m*l*l)*x(2) - (b1+b2*s*s)*tanh(s);

%-------------------------------------------------------------------------------
% linear saturation to reduce the chattering phenom
%     ui = g*m*l*sin(x(1)+alpha) + (k-a)*(m*l*l)*x(2) - (b1+b2*s*s)*LinSat(s);

    % RK4, ui is constant on integraion interval. (ZOH)
    k1 = InvertedPendulum(x, ui, values);
    k2 = InvertedPendulum(x + 0.5*tTimeStep*k1, ui, values);
    k3 = InvertedPendulum(x + 0.5*tTimeStep*k2, ui, values);
    k4 = InvertedPendulum(x + tTimeStep*k3, ui, values);

    x = x + tTimeStep*(k1 + 2*k2 + 2*k3 + k4)/6;

    out(t, :) = [x' ui];
end

toc

disp('End of simulation')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Display
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

disp('Now... display the results')
tt = 1:1:tTotalTime;
x1m = zeros(tTotalTime, 1);
x2m = zeros(tTotalTime, 1);

for ii=1:1:tTotalTime
    x1m(ii) = out(ii/tTimeStep, 1);
    x2m(ii) = out(ii/tTimeStep, 2);
end

t = 0:tTimeStep:tTotalTime;

x1 = out(:, 1);
x2 = out(:, 2);
ui = out(:, 3);

figure('Name','position & velocity','NumberTitle','off')
hold on
plot(tt, x1m, 'ro')
plot(tt, x2m, 'g^')
plot(t, x1, 'linewidth', 2, 'color', 'red'), grid
plot(t, x2, 'linewidth', 2, 'color', 'green'), xlabel('time (sec)')
title('Sliding mode control'), legend('x_1', 'x_2')
hold off

figure('Name','Control input','NumberTitle','off')
plot(t, ui, 'linewidth', 2, 'color', 'blue'), grid
title('Input for SMC'), legend('u'), xlabel('time (sec)')

figure('Name','Phase plane','NumberTitle','off')
hold on
plot(x1, -a*x1, 'linewidth', 2, 'color', 'cyan')
plot(x1, x2, 'linewidth', 2, 'color', 'magenta'), grid
hold off
title('Phase plane'), xlabel('x_1'), ylabel('x_2')
legend('sliding manifold', 'phase portrait')

disp('END')