%% 双回路PID控制倒立摆
% 作者：陈东阳
% 时间：2019/6/15
% 最大外力10N
% 模型里角度顺时针为正，但是控制里，需要逆时针为正，故需要取反。
clc;
clear;
close all
%% PID控制,双回路，位置角度PD求和
tStep = 0.025; %采样时间0.025s
tFinal = 30; %控制时长
Kpa = 45;
Kda = 0.2;
Kpx = 1.8;
Kdx = 1.2;
temp=[0,0,15*pi/180,0,0];
Ea=temp(3);
Ea1=0;
Ex=temp(1);
Ex1=0;
tt = (0:tStep:tFinal);
wx = (0:tStep:tFinal);
wc = (0:tStep:tFinal);
out= (0:tStep:tFinal);
wc(1)=temp(3)*180/pi;
wx(1)=temp(1);
am = 0;
an = 0;
xm = 0;
x = 0;
fitness = 0;
for tp = tStep:tStep:tFinal
[t,y]=ode45(@DaoliBai,[tp-tStep,tp],temp);
xm = y(end,1);
if(y(end,3)>pi) %角度转化到[-pi,pi],wm为模型角度
am = y(end,3) - 2*pi;
elseif(y(end,3) < -pi)
am = y(end,3) + 2*pi;
else
am = y(end,3);
end
an = -am; %控制的角度
x = -xm;
Ea = 0 - an; %计算当前误差
Ex = 0 - x;
outw = Kpa*(Ea+Kda*(Ea-Ea1)/tStep) ;
outx = Kpx*(Ex+Kdx*(Ex-Ex1)/tStep) ;
temp(5)=outw + outx; %PID计算输出
if(temp(5)>20) %最大外力10N
temp(5) = 20;
elseif(temp(5)<-20)
temp(5) = -20;
end
temp(1) = y(end,1);
temp(2) = y(end,2);
temp(3) = am;
temp(4) = y(end,4);
Ea1=Ea; %更新误差
Ex1=Ex;
wc(int32(tp/tStep)+1) = am*180/pi; %画图数据
wx(int32(tp/tStep)+1) = xm;
out(int32(tp/tStep)+1) = temp(5);
fitness = fitness + 0.5*abs(Ea) + 0.5*abs(Ex);
end
fitness
figure(1)
plot(tt,wc,'r-');
title('angle');
figure(2)
plot(tt,wx,'b-');
title('position');
figure(3)
plot(tt,out,'r');
title('output')
