首页 > 其他分享 >机械臂模糊PID控制matlab仿真

机械臂模糊PID控制matlab仿真

时间：2023-05-21 23:05:26浏览次数：34  
标签：仿真 PS PID2 PID matlab PID1 NS Z0


何为模糊PID:链接

模糊PID理论基础：链接

二自由度机械臂运动建模：

机械臂模糊PID控制matlab仿真_模糊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


标签：仿真,PS,PID2,PID,matlab,PID1,NS,Z0
From： https://blog.51cto.com/u_16063698/6320525

相关文章

  • Matlab二维绘图
    %%1.基本绘图指令plot()%%2.矩阵绘图%%3.绘制三条曲线%%4.绘制双纵坐标图%%5.极坐标绘图%%6.极坐标和直角坐标的相互转化%%7.对数坐标系绘图%%8.图形窗分割subplot()%%9.坐标轴设置%%10.图形标识%%11.ezplot()简易绘图指令(......
  • Matlab符号计算（实例）
    %%1.数值常量转换为符号变量%%2.符号表达式的创建%%3.符号表达式中符号自变量的确定%%4.符号对象和数值对象的转换%%5.符号数值的精度控制%%6.合并同类项%%7.因式分解%%8.分子多项式和分母多项式的提取%%9.符号表达式的展开%%10.......
  • matlab默认工作路径的修改方法，永久的
    说起来也简单，就是到安装路径文件夹下C:\ProgramFiles\R2011a\toolbox\local找文件mathrc.m文件，在最后一行添加cd'你想要的默认路径下文件夹',andifyoufinishthisstep,thenyoumakeit.......
  • MATLAB 能做什么?Matlab下载_Matlab中文版下载_Matlab使用教程
    MATLAB软件是一款非常强大的科学计算和数据分析工具，广泛应用于工程、物理、金融等领域。对于初次使用MATLAB软件的用户来说，想要运用它进行科学计算和数据分析可能并不是一件容易的事情。因此，本文将通过实际案例的方式，给大家介绍关于MATLAB软件功能使用技巧。首先，让我们来了解一下MA......
  • Matlab字符串相关操作-拼接、格式化
    ✅作者简介：热爱科研的算法开发者，Python、Matlab项目可交流、沟通、学习。......
  • ★教程3:Simulink学习教程入门50例目录2.MATLAB/Simulink安装
    1.订阅本教程用户可以免费获得本博任意1个(包括所有免费专栏和付费专栏)博文对应代码；2.本Simulink课程的所有案例(部分理论知识点除外)均由博主编写而成，供有兴趣的朋友们自己订阅学习使用。未经本人允许，禁止任何形式的商业用途；3.本课程除了介绍常见的Simulink模块介绍之外，我们更侧......
  • matlab练习程序（二维图形的傅里叶级数）
    如何用傅里叶级数表示二维图形，首先要找到数学表达式，然后做傅里叶拟合即可。我最初想的是$R=f(theta)$这样的式子，$R$是极径，$theta$是极角。不过这样似乎处理不了$theta$一样的情况，比如图形有凹陷的情况。后来看了一些文章说可以把$x$和$y$分开表示，即$x=f(t)$,$y=f(t)$，这样可以......
  • matlab如何通过命令查看主机相关信息
    ✅作者简介：热爱科研的算法开发者，Python、Matlab项目可交流、沟通、学习。......
  • 基于Graph-Cut算法的彩色图像深度信息提取matlab仿真
    1.算法仿真效果matlab2022a仿真结果如下：2.算法涉及理论知识概要Graphcuts是一种十分有用和流行的能量优化算法，在图像处理领域普遍应用于前后背景分割（Imagesegmentation）、立体视觉（stereovision）、抠图（Imagematting）等，目前在医学图像领域应用较多。GraphCut（图形切割）应用于......
  • 基于GA遗传优化的CDVRP,CVRP,DVRP,TSP以及VRPTW常见路径优化问题求解matlab仿真
    1.算法仿真效果matlab2022a仿真结果如下：        TSP最优路径TSP最优路径TSP最优路径BestRoute:0->2->10->5->3->6->9->1->4->7->8->0TotalDistance=95.275km  DVRP最优路径DVRP最优路径DVRP最优路径总路程=19......

赞助商

阅读排行

网站主页关于我们联系我们网站地图

本网站内容转载自其他媒体，侵权联系[admin##ips99.com]。

Copyright © 2020-2023 IPS99 版权所有 IPS99