mirror of
https://github.com/ohmyjesus/RBF_NeuralNetwork.git
synced 2026-02-05 11:09:47 +08:00
53 lines
3.5 KiB
Matlab
53 lines
3.5 KiB
Matlab
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;
|