Info

La pregunta está cerrada. Vuélvala a abrir para editarla o responderla.

My code of Spline Cubic gives an error for velocity profil

1 visualización (últimos 30 días)
nanou
nanou el 2 de Mzo. de 2016
Cerrada: John D'Errico el 2 de Mzo. de 2016
I have an error when i plot the curve of velocity with 05 knots, this is my code:
clear all ;close all; clc;
%%--------------- Data---------------------------------:
h=[2.5, 2.5, 2, 1];
% q=[ 3, -2, -5, 0, 6, 12, 8];
q(1)=3; q(3)=-2; q(5)=8;
dqm=-3; ddqm=0;
dq1=2; ddq1=0;
m=5; % Number of knots
%%Calcul of matrix A and a vector B:
for i=1:m-2
for j=1:m-2
A(i,j)=0;
end
end
A(1,1)=3*h(1)+2*h(2)+(h(1)^2/h(2));
A(1,2)=h(2);
A(2,1)=h(2)-((h(1)^2)/h(2));
A(2,2)=2*(h(2)+h(3));
% m=10;
if m>4
A(2,3)=h(3);
A(m-3,m-4)=h(m-3);
A(m-3,m-3)=2*(h(m-2)+h(m-3));
A(m-3,m-2)=h(m-2)-((h(m-1))^2/h(m-2));
A(m-2,m-2)=3*h(m-1)+2*h(m-2)+(((h(m-1))^2)/(h(m-2)));
A(m-2,m-3)=h(m-2);
else
end
for i=4:m-3
A(i-1,i-2)=h(i-1);
A(i-1,i-1)=2*(h(i-1)+h(i));
A(i-1,i)=h(i)
end
% Calcul du vecteur B:
B(1)=6*((q(3)/h(2))+(q(1)/h(1)))-6*((1/h(1))+(1/h(2)))*(q(1)+(h(1)*dq1)+(((h(1)^2)/3)*ddq1))-(h(1)*ddq1);
B(2)=(6/h(2))*(q(1)+(h(1)*dq1)+((((h(1))^2)/3)*ddq1))+((6*q(4))/h(3))-6*((1/h(2))+(1/h(3)))*q(3);
% m=10;
if m>4
B(m-3)=(6/h(m-2))*(q(m)-(h(m-1)*dqm)+((((h(m-1))^2)/3)*ddqm))-6*((1/h(m-2))+(1/h(m-3)))*q(m-2)+((6*q(m-3))/h(m-3));
B(m-2)=-6*((1/h(m-1))+(1/h(m-2)))*(q(m)-(h(m-1)*dqm)+((((h(m-1))^2)/3)*ddqm))+6*((q(m)/h(m-1))+(q(m-2)/h(m-2)))-(h(m-1)*ddqm);
% B(7)
% B(8)
else
end
for i=4:m-3
B(i-1)=6*(((q(i+1)-q(i))/h(i))-((q(i)-q(i-1))/h(i-1)));
% disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%')
end
B=B';
A;
ddQi=inv(A)*B;
ddq=[0; ddQi; 0];
% disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%')
q2=q(1)+(h(1)*dq1)+((((h(1))^2)/3)*ddq(1))+(((h(1))^2/6)*ddq(2));
q4=q(5)-(h(4)*dqm)+((((h(4))^2)/3)*ddqm)+(((h(4)^2)/6)*ddq(4));
q_i=[q(1) q2 q(3) q4 q(5)];
ddQ_ti=ddq';
t_i1=0; i=1; pas=0; t=0;
for j=1:length(h)
pas=pas+h(j);
end
for z=1:length(h)
% display('*********new trajectory (z)*********')
z;
t_i1=t_i1+h(z);
% pas=h(z)/2
pas=h(z)/10;
ti=t;
for t=t:pas:t_i1
% display('*********new pices of trajectory (i)*********')
i;
t_i1;
t;
ti;
h(z);
ddQ_ti(z);
ddQ_ti(z+1);
q_i(z);
q_i(z+1);
interval=h(z);
temps(i)=t;
qt(i)=((ddQ_ti(z)/(6.*h(z))).*(t_i1-t).^3)...
+((ddQ_ti(z+1)/(6.*h(z))).*(t-ti).^3)...
+(((q_i(z+1)/h(z))-((h(z).*ddQ_ti(z+1))/6)).*(t-ti))...
+(((q_i(z)/h(z))-((h(z).*ddQ_ti(z))/6)).*(t_i1-t));
dqt(i)=(((-ddQ_ti(z))/(2*h(z))).*((t_i1-t).^2))...
+((ddQ_ti(z+1)/(2.*h(z))).*((t-ti).^2))...
+((q_i(z+1)/h(z))-((h(z).*ddQ_ti(z+1))/6))...
-((q_i(z)/h(z))-((h(z).*ddQ_ti(z))/6));
ddqt(i)=((((t_i1-t)/h(z)).*ddQ_ti(z))...
+(((t-ti)/h(z)).*ddQ_ti(z+1)));
i=i+1;
end
% display('***********************')
end
figure
[temps',qt',dqt',ddqt',dddqt']
plot(temps,qt,'b','LineWidth',2); grid on; hold on;
plot(temps,dqt,'r','LineWidth',2); grid on; hold on;
plot(temps,ddqt,'g','LineWidth',2); grid on; hold on;
Can you help me to correct the profil of velocity Thank tou

Respuestas (0)

La pregunta está cerrada.

Etiquetas

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by