Info

Cette question est clôturée. Rouvrir pour modifier ou répondre.

My code of Spline Cubic gives an error for velocity profil

2 vues (au cours des 30 derniers jours)
nanou
nanou le 2 Mar 2016
Clôturé : John D'Errico le 2 Mar 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

Réponses (0)

Cette question est clôturée.

Community Treasure Hunt

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

Start Hunting!

Translated by