Info
이 질문은 마감되었습니다. 편집하거나 답변을 올리려면 질문을 다시 여십시오.
My code of Spline Cubic gives an error for velocity profil
조회 수: 3 (최근 30일)
이전 댓글 표시
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
댓글 수: 0
답변 (0개)
이 질문은 마감되었습니다.
참고 항목
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!