My code of Spline Cubic gives an error for velocity profil

Question

nanou 2016-3-2

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/270997-my-code-of-spline-cubic-gives-an-error-for-velocity-profil

关闭： John D'Errico 2016-3-2

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;