資源簡介
建立了二自由度機械臂的運動學模型,并在不調(diào)用任何庫的情況下手寫實現(xiàn)模糊PID控制,仿真中通過控制角速度實現(xiàn)機械臂末端位置的跟蹤。

代碼片段和文件信息
clc;
clear;
fuzzTab=[-6?-4?-2??0??2??4??6]
%?????????[NB?NM?NS?ZO?PS?PM?PB]
NB=fuzzTab(1);
NM=fuzzTab(2);?
NS=fuzzTab(3);
Z0=fuzzTab(4);?
PS=fuzzTab(5);?
PM=fuzzTab(6);?
PB=fuzzTab(7);
%?模糊規(guī)則表
PID1.pTab=[NB?NB?NM?NM?NS?Z0?Z0;
??????????NB?NB?NM?NS?NS?Z0?Z0;
??????????NB?NM?NS?NS?Z0?PS?PS;
??????????NM?NM?NS?Z0?PS?PM?PM;
??????????NM?NS?Z0?PS?PS?PM?PB;
??????????Z0?Z0?PS?PS?PM?PB?PB;
??????????Z0?Z0?PS?PM?PM?PB?PB];
PID1.iTab=[NB?NB?NM?NM?NS?Z0?Z0;
??????????NB?NB?NM?NS?NS?Z0?Z0;
??????????NB?NM?NS?NS?Z0?PS?PS;
??????????NM?NM?NS?Z0?PS?PM?PM;
??????????NM?NS?Z0?PS?PS?PM?PB;
??????????Z0?Z0?PS?PS?PM?PB?PB;
??????????Z0?Z0?PS?PM?PM?PB?PB];
PID1.dTab=[PS?NS?NB?NB?NB?NM?PS;
??????????PS?NS?NB?NM?NM?NS?Z0;
??????????Z0?NS?NM?NM?NS?NS?Z0;
??????????Z0?NS?NS?NS?NS?NS?Z0;
??????????Z0?Z0?Z0?Z0?Z0?Z0?Z0
??????????PB?NS?PS?PS?PS?PS?PB;
??????????PB?PM?PM?PM?PS?PS?PB];
??????
%?模糊PID控制器?1
PID1.ref=0;?%?期望值
PID1.Kp=10;?%比例
PID1.Ki=2;?%積分
PID1.Kd=4;?%微分
PID1.err=0;%偏差
PID1.derr=0;
PID1.max=pi;%最大測量值?
PID1.min=-pi;%最小測量值?
PID1.maxDltKp=10;%Kp上限
PID1.minDltKp=-5;%Kp下限
PID1.scalKp=0.2;%Kp?權重系數(shù)
PID1.maxDltKi=1;%Ki上限
PID1.minDltKi=-2;%Ki下限
PID1.scalKi=0.5;%Ki?權重系數(shù)
PID1.maxDltKd=12;%Kd上限
PID1.minDltKd=-4;%Kd下限
PID1.scalKd=0.15;%Kd?權重系數(shù)
%?模糊PID控制器?2
PID2=PID1;
PID2.Kp=8;?%比例
PID2.Ki=1;?%積分
PID2.Kd=2;?%微分
PID2.maxDltKp=8;%Kp上限
PID2.minDltKp=-2;%Kp下限
PID2.scalKp=0.9;%Kp?權重系數(shù)
PID2.maxDltKi=1;%Ki上限
PID2.minDltKi=-1;%Ki下限
PID2.scalKi=0.5;%Ki?權重系數(shù)
PID2.maxDltKd=8;%Kd上限
PID2.minDltKd=-2;%Kd下限
PID2.scalKd=0.15;%Kd?權重系數(shù)
%圓心坐標
x0=110;
y0=110;
%半徑
R=40;
%連桿長度
a=100;
b=100;
t=1;
%?控制周期?
dt=0.05?%?秒
Time=[0];?%?當前時間
aimTheta=[0];%?關節(jié)1目標角度
aimPhi=[0];%關節(jié)2目標角度
realTheta=[0];%?關節(jié)1實際角度
realPhi=[0];%關節(jié)2實際角度
errTehta=[0];?%?關節(jié)1?角度誤差
errPhi=[0];?%?關節(jié)2?角度誤差
errThetaSum=0;%關節(jié)1累積誤差
errPhiSum=0;%關節(jié)2累積誤差
derrTheta=0;%關節(jié)1?本次角度誤差與上一次角度誤差的差值
derrPhi=0;%關節(jié)2?本次角度誤差與上一次角度誤差的差值
%?PID?參數(shù)(關節(jié)1)
%?Kp1=10;
%?Ki1=2;
%?Kd1=4;
%?%?PID?參數(shù)(關節(jié)2)
%?Kp2=8;
%?Ki2=1;
%?Kd2=2;
Kp1=8;
Ki1=2;
Kd1=4;
%?PID?參數(shù)(關節(jié)2)
Kp2=6;
Ki2=1;
Kd2=2;
saveW1=[];
saveW2=[];
for?i=0:0.1:2*pi+0.1
???
%?????theta2=i/150*2*pi;
%?????phi=i/150*pi;
????x=x0+R*cos(i);
????y=y0+R*sin(i);
????theta1=atan2(yx);?
????%?theta1=acos(x/sqrt(x*x+y*y));
????c=sqrt(x*x+y*y);?%?末端到原點的距離
????theta3=acos((c*c+a*a-b*b)/(2*a*c));
????theta2=theta1-theta3;?%?關節(jié)1?角度
????phi=pi-acos((a*a+b*b-c*c)/(2*a*b));?%關節(jié)2角度
????aimTheta(end+1)=theta2;
????aimPhi(end+1)=phi;
????
????%連桿?P?位置
????P=Rot(theta2‘z‘)*[a;0;0];
????
????%?連桿末端位置(正運動學驗證)
????E=P+Rot(theta2‘z‘)*Rot(phi‘z‘)*[b;0;0];
????
????%?PID?偏差
?????PID1.err=theta2-realTheta(end);
?????PID2.err=phi-realPhi(end);
????
?????deltPID1=Fuzzy2(PID1realTheta(end)fuzzTab);
?????deltPID2=Fuzzy2(PID2realPhi(end)fuzzTab);
?????
?????PID1.Kp=Kp1+deltPID1(1)*PID1.scalKp;
?????PID1.Ki=Ki1+deltPID1(2)*PID1.scalKi;
?????PID1.Kd=Kd1+deltPID1(
?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????4660??2020-05-11?23:37??ctrl.m
?????文件????????2965??2020-05-11?23:26??Fuzzy2.m
?????文件?????????860??2020-02-21?08:27??main.m
?????文件?????????305??2020-02-21?08:51??plotrobot.m
?????文件?????????366??2020-02-19?08:36??Rot.m
評論
共有 條評論