mirror of
https://github.com/ohmyjesus/RBF_NeuralNetwork.git
synced 2026-10-02 18:23:03 +08:00
CRP六关节机械臂神经网络控制仿真
This commit is contained in:
iHoney725
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,88 @@
|
||||
function [sys,x0,str,ts] = CRP_S(t,x,u,flag)
|
||||
switch flag
|
||||
case 0
|
||||
[sys,x0,str,ts]=mdlInitializeSizes;
|
||||
case {1,2,4,9}
|
||||
sys=[];
|
||||
case 3
|
||||
sys=mdlOutputs(t,x,u);
|
||||
otherwise
|
||||
DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));
|
||||
|
||||
end
|
||||
function [sys,x0,str,ts]=mdlInitializeSizes
|
||||
sizes = simsizes;
|
||||
sizes.NumContStates = 0;
|
||||
sizes.NumDiscStates = 0;
|
||||
sizes.NumOutputs = 6;
|
||||
sizes.NumInputs = 30;
|
||||
sizes.DirFeedthrough = 1;
|
||||
sizes.NumSampleTimes = 0;
|
||||
sys = simsizes(sizes);
|
||||
x0 = [];
|
||||
str = [];
|
||||
ts = [];
|
||||
|
||||
function sys=mdlOutputs(t,x,u)
|
||||
global xite
|
||||
% ½Ç¶È¸ú×ÙÖ¸Áî
|
||||
qd1 = u(1);
|
||||
qd2 = u(2);
|
||||
qd3 = u(3);
|
||||
qd4 = u(4);
|
||||
qd5 = u(5);
|
||||
qd6 = u(6);
|
||||
|
||||
% ½ÇËٶȸú×ÙÖ¸Áî
|
||||
dqd1 = u(7);
|
||||
dqd2 = u(8);
|
||||
dqd3 = u(9);
|
||||
dqd4 = u(10);
|
||||
dqd5 = u(11);
|
||||
dqd6 = u(12);
|
||||
|
||||
% ½Ç¼ÓËٶȸú×ÙÖ¸Áî
|
||||
ddqd1 = u(13);
|
||||
ddqd2 = u(14);
|
||||
ddqd3 = u(15);
|
||||
ddqd4 = u(16);
|
||||
ddqd5 = u(17);
|
||||
ddqd6 = u(18);
|
||||
|
||||
q1 = u(19);
|
||||
q2 = u(20);
|
||||
q3 = u(21);
|
||||
q4 = u(22);
|
||||
q5 = u(23);
|
||||
q6 = u(24);
|
||||
|
||||
dq1 = u(25);
|
||||
dq2 = u(26);
|
||||
dq3 = u(27);
|
||||
dq4 = u(28);
|
||||
dq5 = u(29);
|
||||
dq6 = u(30);
|
||||
|
||||
e1 = qd1 - q1; % e = qd - q
|
||||
e2 = qd2 - q2;
|
||||
e3 = qd3 - q3; % e = qd - q
|
||||
e4 = qd4 - q4;
|
||||
e5 = qd5 - q5; % e = qd - q
|
||||
e6 = qd6 - q6;
|
||||
|
||||
de1 = dqd1 - dq1;
|
||||
de2 = dqd2 - dq2;
|
||||
de3 = dqd3 - dq3;
|
||||
de4 = dqd4 - dq4;
|
||||
de5 = dqd5 - dq5;
|
||||
de6 = dqd6 - dq6;
|
||||
|
||||
e = [e1; e2; e3; e4; e5; e6];
|
||||
de = [de1; de2; de3; de4; de5; de6];
|
||||
|
||||
s = xite * e + de;
|
||||
|
||||
sys(1:6) = s;
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,90 @@
|
||||
function [sys,x0,str,ts] = CRP_i(t,x,u,flag)
|
||||
switch flag
|
||||
case 0
|
||||
[sys,x0,str,ts]=mdlInitializeSizes;
|
||||
case 3
|
||||
sys=mdlOutputs(t,x,u);
|
||||
case {2,4,9}
|
||||
sys=[];
|
||||
otherwise
|
||||
error(['Unhandled flag = ',num2str(flag)]);
|
||||
end
|
||||
|
||||
function [sys,x0,str,ts]=mdlInitializeSizes
|
||||
sizes = simsizes;
|
||||
sizes.NumContStates = 0;
|
||||
sizes.NumDiscStates = 0;
|
||||
sizes.NumOutputs = 1;
|
||||
sizes.NumInputs = 24;
|
||||
sizes.DirFeedthrough = 1;
|
||||
sizes.NumSampleTimes = 0;
|
||||
sys = simsizes(sizes);
|
||||
x0 = [];
|
||||
str = [];
|
||||
ts = [];
|
||||
|
||||
function sys=mdlOutputs(t,x,u)
|
||||
% 角速度跟踪指令
|
||||
dqd1 = 0.1*pi*cos(pi*t);
|
||||
dqd2 = 0.1*pi*cos(pi*t);
|
||||
dqd3 = 0.1*pi*cos(pi*t);
|
||||
dqd4 = 0.1*pi*cos(pi*t);
|
||||
dqd5 = 0.1*pi*cos(pi*t);
|
||||
dqd6 = 0.1*pi*cos(pi*t);
|
||||
|
||||
% 角加速度跟踪指令
|
||||
ddqd1 = -0.1*pi*pi*sin(pi*t);
|
||||
ddqd2 = -0.1*pi*pi*sin(pi*t);
|
||||
ddqd3 = -0.1*pi*pi*sin(pi*t);
|
||||
ddqd4 = -0.1*pi*pi*sin(pi*t);
|
||||
ddqd5 = -0.1*pi*pi*sin(pi*t);
|
||||
ddqd6 = -0.1*pi*pi*sin(pi*t);
|
||||
qd1 = u(1);
|
||||
qd2 = u(2);
|
||||
qd3 = u(3);
|
||||
qd4 = u(4);
|
||||
qd5 = u(5);
|
||||
qd6 = u(6);
|
||||
|
||||
q1 = u(7);
|
||||
q2 = u(8);
|
||||
q3 = u(9);
|
||||
q4 = u(10);
|
||||
q5 = u(11);
|
||||
q6 = u(12);
|
||||
|
||||
dq1 = u(13);
|
||||
dq2 = u(14);
|
||||
dq3 = u(15);
|
||||
dq4 = u(16);
|
||||
dq5 = u(17);
|
||||
dq6 = u(18);
|
||||
|
||||
e1 = qd1 - q1; % e = qd - q
|
||||
e2 = qd2 - q2;
|
||||
e3 = qd3 - q3; % e = qd - q
|
||||
e4 = qd4 - q4;
|
||||
e5 = qd5 - q5; % e = qd - q
|
||||
e6 = qd6 - q6;
|
||||
|
||||
de1 = dqd1 - dq1;
|
||||
de2 = dqd2 - dq2;
|
||||
de3 = dqd3 - dq3;
|
||||
de4 = dqd4 - dq4;
|
||||
de5 = dqd5 - dq5;
|
||||
de6 = dqd6 - dq6;
|
||||
|
||||
e = [e1; e2; e3; e4; e5; e6];
|
||||
de = [de1; de2; de3; de4; de5; de6];
|
||||
|
||||
dq = [dq1; dq2; dq3; dq4; dq5; dq6];
|
||||
q = [q1; q2; q3; q4; q5; q6];
|
||||
dqd = [dqd1; dqd2; dqd3; dqd4; dqd5; dqd6;];
|
||||
ddqd = [ddqd1; ddqd2; ddqd3; ddqd4; ddqd5; ddqd6];
|
||||
|
||||
% 参数的定义
|
||||
xite = 5.0*eye(6); % xite>0
|
||||
|
||||
s = xite * e + de;
|
||||
|
||||
sys(1) = s(1);
|
||||
+72
@@ -0,0 +1,72 @@
|
||||
function [sys,x0,str,ts] = CRP_input(t,x,u,flag)
|
||||
switch flag
|
||||
case 0
|
||||
[sys,x0,str,ts]=mdlInitializeSizes;
|
||||
case 1
|
||||
sys=mdlDerivatives(t,x,u);
|
||||
case 3
|
||||
sys=mdlOutputs(t,x,u);
|
||||
case {2,4,9}
|
||||
sys=[];
|
||||
otherwise
|
||||
error(['Unhandled flag = ',num2str(flag)]);
|
||||
end
|
||||
|
||||
function [sys,x0,str,ts]=mdlInitializeSizes
|
||||
sizes = simsizes;
|
||||
sizes.NumContStates = 0;
|
||||
sizes.NumDiscStates = 0;
|
||||
sizes.NumOutputs = 18;
|
||||
sizes.NumInputs = 0;
|
||||
sizes.DirFeedthrough = 0;
|
||||
sizes.NumSampleTimes = 0;
|
||||
sys = simsizes(sizes);
|
||||
x0 = [];
|
||||
str = [];
|
||||
ts = [];
|
||||
|
||||
function sys=mdlOutputs(t,x,u)
|
||||
global xite
|
||||
% ½Ç¶È¸ú×ÙÖ¸Áî
|
||||
qd1 = 0.1*cos(2*pi*t);
|
||||
qd2 = 0.1*cos(pi*t);
|
||||
qd3 = 0.1*cos(pi*t);
|
||||
qd4 = 0.1*cos(pi*t);
|
||||
qd5 = 0.1*cos(pi*t);
|
||||
qd6 = 0.1*cos(pi*t);
|
||||
|
||||
% ½ÇËٶȸú×ÙÖ¸Áî
|
||||
dqd1 = -0.1*2*pi*sin(2*pi*t);
|
||||
dqd2 = -0.1*pi*sin(pi*t);
|
||||
dqd3 = -0.1*pi*sin(pi*t);
|
||||
dqd4 = -0.1*pi*sin(pi*t);
|
||||
dqd5 = -0.1*pi*sin(pi*t);
|
||||
dqd6 = -0.1*pi*sin(pi*t);
|
||||
|
||||
% ½Ç¼ÓËٶȸú×ÙÖ¸Áî
|
||||
ddqd1 = -0.1*2*2*pi*pi*cos(2*pi*t);
|
||||
ddqd2 = -0.1*pi*pi*cos(pi*t);
|
||||
ddqd3 = -0.1*pi*pi*cos(pi*t);
|
||||
ddqd4 = -0.1*pi*pi*cos(pi*t);
|
||||
ddqd5 = -0.1*pi*pi*cos(pi*t);
|
||||
ddqd6 = -0.1*pi*pi*cos(pi*t);
|
||||
|
||||
sys(1) = qd1;
|
||||
sys(2) = qd2;
|
||||
sys(3) = qd3;
|
||||
sys(4) = qd4;
|
||||
sys(5) = qd5;
|
||||
sys(6) = qd6;
|
||||
sys(7) = dqd1;
|
||||
sys(8) = dqd2;
|
||||
sys(9) = dqd3;
|
||||
sys(10) = dqd4;
|
||||
sys(11) = dqd5;
|
||||
sys(12) = dqd6;
|
||||
sys(13) = ddqd1;
|
||||
sys(14) = ddqd2;
|
||||
sys(15) = ddqd3;
|
||||
sys(16) = ddqd4;
|
||||
sys(17) = ddqd5;
|
||||
sys(18) = ddqd6;
|
||||
|
||||
@@ -0,0 +1,52 @@
|
||||
function [sys,x0,str,ts] = CRProbotJoint(t,x,u,flag)
|
||||
switch flag
|
||||
case 0
|
||||
[sys,x0,str,ts]=mdlInitializeSizes;
|
||||
case 1
|
||||
sys=mdlDerivatives(t,x,u);
|
||||
case {2,4,9}
|
||||
sys=[];
|
||||
case 3
|
||||
sys=mdlOutputs(t,x,u);
|
||||
otherwise
|
||||
DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));
|
||||
end
|
||||
|
||||
function [sys,x0,str,ts]=mdlInitializeSizes %系统的初始化
|
||||
sizes = simsizes;
|
||||
sizes.NumContStates = 0; %设置系统连续状态的变量
|
||||
sizes.NumDiscStates = 0; %设置系统离散状态的变量
|
||||
sizes.NumOutputs = 6; %设置系统输出的变量
|
||||
sizes.NumInputs = 0; %设置系统输入的变量
|
||||
sizes.DirFeedthrough = 0; %如果在输出方程中显含输入变量u，则应该将本参数设置为1
|
||||
sizes.NumSampleTimes = 0; % 模块采样周期的个数
|
||||
% 需要的样本时间，一般为1.
|
||||
% 猜测为如果为n，则下一时刻的状态需要知道前n个状态的系统状态
|
||||
sys = simsizes(sizes);
|
||||
x0 = []; % 系统初始状态变量
|
||||
str = []; % 保留变量，保持为空
|
||||
ts = []; % 采样时间[t1 t2] t1为采样周期，如果取t1=-1则将继承输入信号的采样周期；参数t2为偏移量，一般取为0
|
||||
|
||||
|
||||
function sys=mdlOutputs(t,x,u) %产生（传递）系统输出
|
||||
% R1 = (0.2693)*sin((0.2519)*t + (-0.2811)) + (0.08634)*sin((0.5401)*t + (2.424)) + (0.008846)*sin((1.175)*t + (1.882)) + (0.004509)*sin((1.749)*t + (1.906)) + (0.001863)*sin((2.389)*t + (2.173)) + (0.001096)*sin((3.001)*t + (2.104)) + (0.0006325)*sin((3.686)*t + (1.908));
|
||||
% R2 = (0.3132)*sin((0.2725)*t + (2.859)) + (0.0973)*sin((0.5103)*t + (-0.6751)) + (0.01545)*sin((1.175)*t + (-1.24)) + (0.006022)*sin((1.72)*t + (-1.014)) + (0.002018)*sin((2.423)*t + (-1.143)) + (0.001371)*sin((3.004)*t + (-1.004)) + (0.0007587)*sin((3.689)*t + (-1.204));
|
||||
% R3 = (0.5944)*sin((0.25)*t + (-0.0491)) + (0.2337)*sin((0.4144)*t + (3.144)) + (0.01604)*sin((1.171)*t + (1.843)) + (0.00728)*sin((1.754)*t + (1.81)) + (0.002588)*sin((2.357)*t + (2.304)) + (0.001568)*sin((2.978)*t + (2.203)) + (0.0008342)*sin((3.678)*t + (1.939));
|
||||
% R4 = (1.095)*sin((0.284)*t + (-0.299)) + (0.3042)*sin((0.5665)*t + (1.304)) + (0.06447)*sin((1.358)*t + (0.999)) + (0.05748)*sin((1.452)*t + (3.85)) + (0.005996)*sin((2.295)*t + (3.25)) + (0.00376)*sin((3.028)*t + (2.512)) + (0.002784)*sin((3.687)*t + (2.177));
|
||||
% R5 = (0.3831)*sin((0.2474)*t + (2.992)) + (0.1344)*sin((0.4627)*t + (-0.2944)) + (0.00835)*sin((1.175)*t + (-1.363)) + (0.004525)*sin((1.748)*t + (-1.374)) + (0.001669)*sin((2.401)*t + (-1.041)) + (0.001098)*sin((3.006)*t + (-1.08)) + (0.0006398)*sin((3.691)*t + (-1.248));
|
||||
% R6 = (1.056)*sin((0.2844)*t + (2.851)) + (0.2907)*sin((0.5716)*t + (-1.896)) + (0.1078)*sin((1.35)*t + (-2.024)) + (0.09987)*sin((1.404)*t + (0.9588)) + (0.005609)*sin((2.314)*t + (0.03832)) + (0.003715)*sin((3.008)*t + (-0.5013)) + (0.002569)*sin((3.694)*t + (-0.9631));
|
||||
|
||||
%路径规划 x-0.2 y0.2 z-0.2
|
||||
R1 = 1.306e-05 * t^5 - 0.0003628 * t^4 + 0.002629 * t^3 -0.0002011 * t^2 + 0.001278 *t - 0.0004736;
|
||||
R2 = 1.04e-05 * t^5 - 0.0001797 *t^4 - 0.0002723 *t^3 + 0.01482 *t^2 - 0.01434 *t + 0.003022;
|
||||
R3 = -1.682e-05*t^5 + 0.0003065 *t^4 + 0.0003115 *t^3 - 0.02495 *t^2 + 0.02353 *t - 0.004826;
|
||||
R4 = 1.264e-19 * t^5 - 3.305e-18 *t^4 + 3.235e-17 *t^3 - 1.414e-16 *t^2 + 2.391e-16 *t - 6.501e-17;
|
||||
R5 = -6.422e-06 *t^5 + 0.0001268 *t^4 + 3.923e-05 *t^3 - 0.01013 *t^2 + 0.009188 *t - 0.001804;
|
||||
R6 = 1.306e-05 *t^5 - 0.0003628 *t^4 + 0.002629 *t^3 - 0.0002011 *t^2 + 0.001278 *t - 0.0004736;
|
||||
|
||||
sys(1) = R1;
|
||||
sys(2) = R2;
|
||||
sys(3) = R3;
|
||||
sys(4) = R4;
|
||||
sys(5) = R5;
|
||||
sys(6) = R6;
|
||||
Reference in New Issue
Block a user