機械臂模糊PID控制matlab仿真

何爲模糊PID:鏈接

模糊PID理論基礎：鏈接

二自由度機械臂運動建模：

    末端位置E(x,y),則兩個關節角度可以由下式求得：

    theta1=atan2(y,x); 
    % 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角度

根據上面的建模和模糊PID基礎，用模糊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(y,x); 
    % 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(PID1,realTheta(end),fuzzTab);
     deltPID2=Fuzzy2(PID2,realPhi(end),fuzzTab);
     
     PID1.Kp=Kp1+deltPID1(1)*PID1.scalKp;
     PID1.Ki=Ki1+deltPID1(2)*PID1.scalKi;
     PID1.Kd=Kd1+deltPID1(3)*PID1.scalKd;
     
     PID2.Kp=Kp2+deltPID2(1)*PID2.scalKp;
     PID2.Ki=Ki2+deltPID2(2)*PID2.scalKi;
     PID2.Kd=Kd2+deltPID2(3)*PID2.scalKd;
   
     PID2.Kp
    % PID 控制
    w1=PID1.Kp*PID1.err+PID1.derr*PID1.Kd+PID1.Ki*errThetaSum;%關節1 瞬時角速度
    w2=PID2.Kp*PID2.err+PID2.derr*PID2.Kd+PID2.Ki*errPhiSum;%關節2 瞬時角速度
    realTheta(end+1)=realTheta(end)+w1*dt;
    realPhi(end+1)=realPhi(end)+w2*dt;
    
    saveW1=[saveW1,w1];
    saveW2=[saveW2,w2];

    % 誤差

    errTehta(end+1)=PID1.err;
    errPhi(end+1)=PID2.err;
    errThetaSum=errThetaSum+errTehta(end);
    errPhiSum=errPhiSum+errPhi(end);
    PID1.derr=errTehta(end)-errTehta(end-1);
    PID2.derr=errPhi(end)-errPhi(end-1);
    % 當前時間
    Time(end+1)=Time(end)+dt;
    
    %連桿 P 位置
    realP=Rot(realTheta(end),'z')*[a;0;0];
    % 連桿末端位置（正運動學驗證）
    realE=P+Rot(realTheta(end),'z')*Rot(realPhi(end),'z')*[b;0;0];
    
    %末端繪製圓的座標
    rolx(t)=E(1);
    roly(t)=E(2);
    t=t+1;
    subplot(221);
    plotrobot(realP(1),realP(2),realE(1),realE(2),rolx,roly);  % 繪圖驗證
    axis([-50,200,-50,200]);
    hold off
    subplot(222);
    plot(Time,errTehta,'k',Time,errPhi,'r');
    subplot(223);
    plot(Time,realTheta,'k',Time,aimTheta,'r');
    subplot(224);
    plot(Time,realPhi,'k',Time,aimPhi,'r');
    pause(0.0000001)
   
end

    
    

%% 模糊PID控制文件
function [deltPID]=Fuzzy2(PID,realAng,fuzzTab)

valErr=LinearQuantization(PID,realAng);% 偏差線性化的值
[indexErr,valueErr]=CalcMemberShip(valErr(1),fuzzTab);%偏差的隸屬度計算
[indexDErr,valueDErr]=CalcMemberShip(valErr(2),fuzzTab);%偏差的變化量的隸屬度計算
FuzVal(1)=valueErr(1)*(valueDErr(1)*PID.pTab(indexErr(1),indexDErr(1))+valueDErr(2)*PID.pTab(indexErr(1),indexDErr(2)))...
          +valueErr(2)*(valueDErr(1)*PID.pTab(indexErr(2),indexDErr(1))+valueDErr(2)*PID.pTab(indexErr(2),indexDErr(2)));
FuzVal(2)=valueErr(1)*(valueDErr(1)*PID.iTab(indexErr(1),indexDErr(1))+valueDErr(2)*PID.iTab(indexErr(1),indexDErr(2)))...
          +valueErr(2)*(valueDErr(1)*PID.iTab(indexErr(2),indexDErr(1))+valueDErr(2)*PID.iTab(indexErr(2),indexDErr(2)));
FuzVal(3)=valueErr(1)*(valueDErr(1)*PID.dTab(indexErr(1),indexDErr(1))+valueDErr(2)*PID.dTab(indexErr(1),indexDErr(2)))...
          +valueErr(2)*(valueDErr(1)*PID.dTab(indexErr(2),indexDErr(1))+valueDErr(2)*PID.dTab(indexErr(2),indexDErr(2)));

deltPID=LinearDeb(PID,FuzVal);

function retW=LinearQuantization(PID,realAng)
%=======   線性化函數 【-6，6】
% retW(1)：偏差的線性化
% retW(1)：偏差導數的線性化
error=PID.ref-realAng; 
derror=error-PID.err; 
retW(1)=6*error/(PID.max-PID.min);
retW(2)=3*derror/(PID.max-PID.min);


function [indexMem,valueMem]=CalcMemberShip(err,fuzzTab)
% 隸屬度計算函數
% fuzzTab 模糊規則表
% 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);
indexMem=[];
valueMem=[];
if err>=NB && err<NM
    indexMem(1)=0;
    indexMem(2)=1;
    valueMem(1)=-0.5*err-2.0;  % y=0.5x-2
    valueMem(2)=0.5*err+3.0;   % y=0.5x+3
elseif err>=NM && err<NS
    indexMem(1)=1;
    indexMem(2)=2;
    valueMem(1)=-0.5*err-1.0;  % y=-0.5x-1
    valueMem(2)=0.5*err+2.0;   % y=0.5x+2
elseif err>=NS && err<Z0
    indexMem(1)=2;
    indexMem(2)=3;
    valueMem(1)=-0.5*err;  % y=-0.5x
    valueMem(2)=0.5*err+1.0; % y=0.5x+1
elseif err>=Z0 && err<PS
    indexMem(1)=3;
    indexMem(2)=4;
    valueMem(1)=-0.5*err+1.0;  % y=-0.5x+1
    valueMem(2)=0.5*err; % y=0.5x
elseif err>=PS && err<PM
    indexMem(1)=4;
    indexMem(2)=5;
    valueMem(1)=-0.5*err+2;  % y=-0.5x+2
    valueMem(2)=0.5*err-1.0; % y=0.5x-1
elseif err>=PM && err<=PB
    indexMem(1)=5;
    indexMem(2)=6;
    valueMem(1)=-0.5*err+3.0;  % y=-0.5x+3
    valueMem(2)=0.5*err-2.0; % y=0.5x+1-2
end
indexMem=indexMem+1;


function FuzVal=LinearDeb(PID,FuzVal)
% 限幅處理
if FuzVal(1)>PID.maxDltKp 
   FuzVal(1)=PID.maxDltKp ;
elseif FuzVal(1)<PID.minDltKp 
   FuzVal(1)=PID.minDltKp ;
end
if FuzVal(2)>PID.maxDltKi 
   FuzVal(2)=PID.maxDltKi ;
elseif FuzVal(2)<PID.minDltKi 
   FuzVal(2)=PID.minDltKi;
end
if FuzVal(3)>PID.maxDltKd 
   FuzVal(3)=PID.maxDltKd ;
elseif FuzVal(3)<PID.minDltKd 
   FuzVal(3)=PID.minDltKd;
end

%% 旋轉矩陣
function R=Rot(theta,ch)
% @brief: 繞某個軸的旋轉矩陣的求法
% @param: theta,繞ch軸旋轉的角度；ch,x、y、z中的某個軸
% @ret: 繞 ch 軸的旋轉矩陣
% @birth: created by MY on 20200218
c=cos(theta);
s=sin(theta);
switch(ch)
    case'x'
        R=[1,0,0;0,c,-s;0,s,c];
    case'y'
        R=[c,0,s;0,1,0;-s,0,c];
    case'z'
        R=[c,-s,0;s,c,0;0,0,1];
end

長期從事機器人學相關研究，涉及機械臂、輪式機器人、四足機器人的建模及仿真，可共同探討機器人相關問題，可指導課程設計及畢業設計，詳詢：QQ:2963325158

代碼鏈接