双路PD倒立摆控制.zip

%% 双回路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')