二自由度机械臂模糊PID控制.zip

建立了二自由度机械臂的运动学模型，并在不调用任何库的情况下手写实现模糊PID控制，仿真中通过控制角速度实现机械臂末端位置的跟踪。

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）;

% 模糊规则表
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 权重系数

PID1.maxDltKi=1;%Ki上限
PID1.minDltKi=-2;%Ki下限
PID1.scalKi=0.5;%Ki 权重系数

PID1.maxDltKd=12;%Kd上限
PID1.minDltKd=-4;%Kd下限
PID1.scalKd=0.15;%Kd 权重系数


% 模糊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 权重系数

PID2.maxDltKi=1;%Ki上限
PID2.minDltKi=-1;%Ki下限
PID2.scalKi=0.5;%Ki 权重系数

PID2.maxDltKd=8;%Kd上限
PID2.minDltKd=-2;%Kd下限
PID2.scalKd=0.15;%Kd 权重系数



%圆心坐标
x0=110;
y0=110;
%半径
R=40;
%连杆长度
a=100;
b=100;
t=1;

% 控制周期 
dt=0.05 % 秒
Time=[0]; % 当前时间
aimTheta=[0];% 关节1目标角度
aimPhi=[0];%关节2目标角度
realTheta=[0];% 关节1实际角度
realPhi=[0];%关节2实际角度
errTehta=[0]; % 关节1 角度误差
errPhi=[0]; % 关节2 角度误差
errThetaSum=0;%关节1累积误差
errPhiSum=0;%关节2累积误差
derrTheta=0;%关节1 本次角度误差与上一次角度误差的差值
derrPhi=0;%关节2 本次角度误差与上一次角度误差的差值
% PID 参数（关节1）
% Kp1=10;
% Ki1=2;
% Kd1=4;
% % PID 参数（关节2）
% Kp2=8;
% Ki2=1;
% Kd2=2;

Kp1=8;
Ki1=2;
Kd1=4;
% PID 参数（关节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; % 关节1 角度
    phi=pi-acos（（a*a+b*b-c*c）/（2*a*b））; %关节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