Điều khiển PID mô hình con lắc ngược
Ở bài viết này sẽ hướng dẫn các bạn viết chương trình mô phỏng con lắc ngược trong Matlab. Mô hình sử dụng bộ điều khiển PID. Phần tính toán động lực học các bạn xem ở bài viết Điều khiển LQR mô hình con lắc ngược
Code Matlab
Inverted_Pendulum.m
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 |
%Single Link Inverted Pendulum Control clc; clear all; close all global A B C D; %Single Link Inverted Pendulum Parameters g=9.8; M=0.5; m=0.2; L=0.3; % Fc=0.0005; % Fp=0.000002; I=1/12*m*L^2; l=1/2*L; t1=m*(M+m)*g*l/[(M+m)*I+M*m*l^2]; t2=-m^2*g*l^2/[(m+M)*I+M*m*l^2]; t3=-m*l/[(M+m)*I+M*m*l^2]; t4=(I+m*l^2)/[(m+M)*I+M*m*l^2]; A=[0,1,0,0; t1,0,0,0; 0,0,0,1; t2,0,0,0]; B=[0;t3;0;t4]; C=[1,0,0,0; 0,0,1,0]; D=[0;0]; Kp = [10 1]; Kd = [1 1]; Ki = [1 1]; desired = [0 1];% theta theta_dot x x_dot init=[0.2,0.1,0,0]; dt=0.01; N = 2000; % pre-assign all the arrays to optimize simulation time Prop = zeros(N+1,2); Der = zeros(N+1,2); Int = zeros(N+1,2); I = zeros(N+1,2); PID = zeros(N+1,1); FeedBack = zeros(N+1,2); Output = zeros(N+1,4); Error = zeros(N+1,2); time = zeros(N+1,1); Output(1,:) = init; for k=1:N time(k+1)=k*dt; Tspan=[0 dt]; control_input=PID(k); [t,x]=ode45('pendulum',Tspan,Output(k,:),[],control_input); FeedBack(k+1,:)=x(end,[1,3]); Output(k+1,:) = x(end,:); Error(k+1,:) = desired - FeedBack(k+1,:); % error entering the PID controller Prop(k+1,:) = Error(k+1,:);% error of proportional term Der(k+1,:) = (Error(k+1,:) - Error(k,:))/dt; % derivative of the error Int(k+1,:) = (Error(k+1,:) + Error(k,:))*dt/2; % integration of the error I(k+1,:) = sum(Int); % the sum of the integration of the error PID(k+1) = -Kp*Prop(k+1,:)' - Ki*I(k+1,:)'- Kd*Der(k+1,:)'; % the three PID terms % Limit control signal if PID(k+1)>=10 PID(k+1)=10; elseif PID(k+1)<=-10 PID(k+1)=-10; end end % Reference = desired.*ones(N+1,4); Reference = zeros(N+1,2); Reference(:,1) = desired(1)*ones(N+1,1); Reference(:,2) = desired(2)*ones(N+1,1); figure(1) subplot(4,1,1) hold on plot(time,Reference(:,1),'--r'); plot(time,Output(:,1),'b'); grid on xlabel('Time (sec)'); ylabel('theta (rad)'); subplot(4,1,2) hold on plot(time,Reference(:,2),'--r'); plot(time,Output(:,3),'b'); grid on xlabel('Time (sec)'); ylabel('x (m)'); subplot(4,1,3) hold on plot(time,Output(:,2),'b'); grid on xlabel('Time (sec)'); ylabel('theta rate(rad/s)'); subplot(4,1,4) hold on plot(time,Output(:,4),'b'); grid on xlabel('Time (sec)'); ylabel('v(m/s)'); |
pendulum.m
|
1 2 3 4 5 6 7 8 |
function dx=pendulum(t,x,flag,control_input) global A B C D; u=control_input; dx=zeros(4,1); %State equation for one link inverted pendulum dx=A*x+B*u; end |
Kêt quả
PID Invert Pendulum
