【优化控制】基于策略迭代算法求解重构机械臂容错跟踪控制优化问题含Matlab代码

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知。

更多Matlab仿真内容点击👇

智能优化算法       神经网络预测       雷达通信       无线传感器        电力系统

信号处理              图像处理               路径规划       元胞自动机        无人机 

⛄ 内容介绍

随着现代科学技术的不断推进,以及"十三五"国家科技创新规划的部署,针对像可重构机械臂一类具有模块化,灵活性等特点,以及对外界环境与操作任务具有强大适应能力的智能机械设备,将被大力发展应用于深空探测,智能工厂以及核工业等很多应用领域中.然而,当可重构机械臂长期工作在极端的恶劣环境下,其执行机构等元器件不可避免会发生故障.若不及时对其进行处理,将会带来重大的财产损失.因此,研究故障诊断与容错控制方法来解决此类问题以维持系统可靠的性能十分迫切.与此同时,在提高可重构机械臂系统的控制精度,简化控制器结构以及优化能源代价消耗这方面的研究也十分必要,不仅将为复杂机构的机器人以及大型工程机械的实际应用提供有力的技术支持,同时对机械设备向着自动化,智能化以及最优化的方向健康发展具有切实深远的意义.

⛄ 部分代码

clear all

close all

clc

global Q Q1 R l1 l2 wc0 lr P k k1 sita;

k=5;  

Q=k*eye(2); 

Q1=k*eye(4);

R=0.1*eye(2);

sita=1;

lr=0.001;

l1=2;  

l2=20*eye(4);

P=0.6*eye(2);

k1=1; 

wc0=[20 30 40 20 30 40 50 40 50 55];

xw0=[1 1 0 0 2 -2 0 0 wc0 0 0];

options = odeset('OutputFcn',@odeplot);

[t,xw]= ode15s('plant2',[0 60],xw0,options);

    

   % %  config a %%

%   y1d=0.4*sin(0.3*t)-0.1*cos(0.5*t);

%   y2d=0.3*cos(0.6*t)+0.6*sin(0.2*t);

%   y3d=(3*cos((3*t)/10))/25 + sin(t/2)/20;

%   y4d=(3*cos(t/5))/25-(9*sin((3*t)/5))/50;  

%   

  

    % %  config b %%  

  y1d=0.2*cos(0.5*t)+0.2*sin(0.4*t);

  y2d=0.3*cos(0.2*t)-0.4*sin(0.6*t);

  y3d=(2*cos((2*t)/5))/25 - sin(t/2)/10;

  y4d=-(6*cos((3*t)/5))/25-(3*sin(t/5))/50;

U=[];

% 

for i=1:size(xw,1)

% %  config a %% 

%   x1d=0.4*sin(0.3*t(i))-0.1*cos(0.5*t(i));

%   x2d=0.3*cos(0.6*t(i))+0.6*sin(0.2*t(i));

%   x3d=(3*cos((3*t(i))/10))/25 + sin(t(i)/2)/20;

%   x4d=(3*cos(t(i)/5))/25 - (9*sin((3*t(i))/5))/50;

%   

%   dx1d=(3*cos((3*t(i))/10))/25 + sin(t(i)/2)/20;

%   dx2d=(3*cos(t(i)/5))/25 - (9*sin((3*t(i))/5))/50;

%   dx3d=cos(t(i)/2)/40-(9*sin((3*t(i))/10))/250;

%   dx4d=-(27*cos((3*t(i))/5))/250-(3*sin(t(i)/5))/125;

% %  config b %%

  x1d=0.2*cos(0.5*t(i))+0.2*sin(0.4*t(i));

  x2d=0.3*cos(0.2*t(i))-0.4*sin(0.6*t(i));

  x3d=(2*cos((2*t(i))/5))/25 - sin(t(i)/2)/10;

  x4d=-(6*cos((3*t(i))/5))/25-(3*sin(t(i)/5))/50;

  dx1d=0.3*cos(0.2*t(i))-0.4*sin(0.6*t(i));

  dx2d=-(6*cos((3*t(i))/5))/25-(3*sin(t(i)/5))/50;

  dx3d=-cos(t(i)/2)/20-(4*sin((2*t(i))/5))/125;

  dx4d=(18*sin((3*t(i))/5))/125-(3*cos(t(i)/5))/250;    

  

  

  dxd=[dx1d;dx2d;dx3d;dx4d];

e1=xw(i,1)-x1d;

e2=xw(i,2)-x2d;

e3=xw(i,3)-x3d;

e4=xw(i,4)-x4d;

e=[e1 e2 e3 e4];

sigma=[e1^2 e1*e2 e1*e3 e1*e4 e2^2 e2*e3 e2*e3 e3^2 e3*e4 e4^2];

d_sigma=[2*e1 0 0 0;e2 e1 0 0;e3 0 e1 0;e4 0 0 e1;0 2*e2 0 0;0 e3 e2 0;0 e4 0 e2;0 0 2*e3 0;0 0 e4 e3;0 0 0 2*e4];

% congif a %

% 

% Md=[0.36*cos(x2d)+0.6066 0.18*cos(x2d)+0.1233;

%     0.18*cos(x2d)+0.1233 0.1233];

% Cd=[-0.36*sin(x2d)*x4d -0.18*sin(x2d)*x4d;

%     0.18*sin(x2d)*(x3d-x4d) 0.18*sin(x2d)*x3d];

% Nd=[-5.88*sin(x1d+x2d)-17.64*sin(x1d);

%     -5.88*sin(x1d+x2d)];

% %config b %

Md=[0.17-0.1166*cos(x2d)*cos(x2d) -0.06*cos(x2d);

    -0.06*cos(x2d) 0.1233];

Cd=[0.1166*sin(2*x2d)*x4d 0.06*sin(x2d)*x4d;

    0.06*sin(x2d)*x4d-0.0583*sin(2*x2d)*x3d 0.06*sin(x2d)*x3d];

Nd=[0;-5.88*cos(x2d)];

f_xd=-inv(Md)*(Cd*[x3d;x4d]+Nd);

g_xd=-inv(Md);

fd=[x3d;x4d;f_xd];

gd=[0 0;0 0;g_xd];

ud=pinv(gd)*(dxd-fd);

% congif a %

% M=[0.36*cos(xw(i,2))+0.6066 0.18*cos(xw(i,2))+0.1233;

%     0.18*cos(xw(i,2))+0.1233 0.1233];

% C=[-0.36*sin(xw(i,2))*xw(i,4) -0.18*sin(xw(i,2))*xw(i,4);

%     0.18*sin(xw(i,2))*(xw(i,3)-xw(i,4)) 0.18*sin(xw(i,2))*xw(i,3)];

% N=[-5.88*sin(xw(i,1)+xw(i,2))-17.64*sin(xw(i,1));

%     -5.88*sin(xw(i,1)+xw(i,2))];

%config b %

M=[0.17-0.1166*cos(xw(i,2))^2 -0.06*cos(xw(i,2));-0.06*cos(xw(i,2)) 0.1233];

C=[0.1166*sin(2*xw(i,2))*xw(i,4) 0.06*sin(xw(i,2))*xw(i,4);

    0.06*sin(xw(i,2))*xw(i,4)-0.0583*sin(2*xw(i,2))*xw(i,3) -0.06*sin(xw(i,2))*xw(i,3)];

N=[0;-5.88*cos(xw(i,2))];

f_x=-inv(M)*(C*[xw(i,3);xw(i,4)]+N);

g_x=-inv(M);

f=[xw(i,3);xw(i,4);f_x];

g=[0 0;0 0;g_x];

ue=-0.5*inv(R)*g'*d_sigma'*wc0;

u=ud+ue;

% u=ua;

U=[U u];

end

figure (1);

subplot(4,1,1),plot(t,y1d,'r:',t,xw(:,1));

xlabel ('Time (s)');

legend('x_{1d}','x_{1}');

 axis([0 60 -1 2]);

% axes('Position',[0.18,0.62,0.28,0.25]);

% plot(t,y1d,'r:',t,xw(:,1));

% xlim([30,31]);

subplot(4,1,2),plot(t,y2d,'r:',t,xw(:,2));

xlabel ('Time (s)');

legend('x_{2d}','x_{2}'); 

 axis([0 60 -1 2]);

subplot(4,1,3),plot(t,y3d,'r:',t,xw(:,3));

xlabel ('Time (s)');

legend('x_{3d}','x_{3}'); 

 axis([0 60 -1 0.5]);

subplot(4,1,4),plot(t,y4d,'r:',t,xw(:,4));

xlabel ('Time (s)');

legend('x_{4d}','x_{4}'); 

 axis([0 60 -1 0.5]);

figure (2);

plot(t,xw(:,1)-y1d,'r:');

% legend('joint 1','joint 2'); 

 axis([0 60 -1 1]);

 xlabel ('Time (s)');

% ylabel(' Position error');

% legend('e1'); 

hold on;

plot(t,xw(:,2)-y2d);

% legend('joint 1','joint 2'); 

 axis([0 60 -1 1]);

 xlabel ('Time (s)');

% ylabel(' Position error');

% legend('e2'); 

hold on;

plot(t,xw(:,3)-y3d,'y-.');

% legend('joint 1','joint 2'); 

%  axis([0 60 -1 1]);

 xlabel ('Time (s)');

% ylabel(' Position error');

% legend('e3'); 

hold on;

plot(t,xw(:,4)-y4d,'g');

% legend('joint 1','joint 2'); 

 axis([0 60 -1 1]);

 xlabel ('Time (s)');

% ylabel(' Position error');

legend('e_1','e_2','e_3','e_4'); 

figure (5);

% plot(t,0.*(t<30)+(5).*(t>=30),t,xw(:,19),'r:');

plot(t,0.*(t<30)+(3*sin(0.2*t)+cos(1*t)).*(t>=30),t,xw(:,19),'r:');

% grid on;

% title ('Fault Estimation');

xlabel ('Time (s)');

% ylabel('Actuator Fault(Nm)');

legend ('Actual Fault','Estimated Fault');

 axis([0 60 -10 10]);

%  axis([0 10 -1.5 1.5]);

figure (6);

plot(t,xw(:,9:18));

% plot(t,xw(:,9),':',t,xw(:,10),'-.o',t,xw(:,11),'-.+',t,xw(:,12),'-.*',t,xw(:,13),'-.s',t,xw(:,14),'-.d',t,xw(:,15),'-.h',t,xw(:,16),'-.p',t,xw(:,17),'-.^',t,xw(:,18),'-.>');

% xw(:,9:18);

% grid on;

% title ('Weight of the Critic NN');

xlabel ('Time (s)');

axis([0 10 -15 60]);

legend('w_{c1}','w_{c2}', 'w_{c3}','w_{c4}','w_{c5}', 'w_{c6}','w_{c7}','w_{c8}', 'w_{c9}','w_{c10}');

% legend(['Model 1:',sprintf('\n'),'sin(x)'],['Model 2:',sprintf('\n'),'cos(x)']);

% axes('Position',[0.18,0.62,0.28,0.25]);

% plot(t,xw(:,9:18),'r');

% xlim([0,1]);

⛄ 运行结果

⛄ 参考文献

[1] 夏宏兵.基于自适应动态规划的可重构机械臂容错控制方法研究[D].长春工业大学[2023-06-15].DOI:CNKI:CDMD:2.1017.819175.

[2] 叶涵,沈陆娟,赵沈杰,等.基于全局优化模型高压油管压力控制研究[J].科学技术创新, 2021, 000(016):P.57-59.​

⛳️ 代码获取关注我

❤️部分理论引用网络文献，若有侵权联系博主删除

❤️ 关注我领取海量matlab电子书和数学建模资料