clc;clear;close
m=3;
P=[0 0 1 2;0 1 2.5 3;1 7 2 5];
%{
jj=1;
for u=0:0.01:1
b=BezierP(P,m,u);
db=BezierPDerivatives(P,m,u);
ddb=BezierPDerivatives2(P,m,u);
bx(jj)=b(1,1);
by(jj)=b(2,1);
bz(jj)=b(3,1);
dbx(jj)=db(1,1);
dby(jj)=db(2,1);
dbz(jj)=db(3,1);
ddbx(jj)=ddb(1,1);
ddby(jj)=ddb(2,1);
ddbz(jj)=ddb(3,1);
jj=jj+1;
end
plot3(bx,by,bz,'r');
hold on;
plot3(P(1,:),P(2,:),P(3,:));
xlabel('X轴');
ylabel('Y轴');
zlabel('Z轴');
figure('name','dbx');
plot(1:jj-1,dbx,'b');
figure('name','dby');
plot(1:jj-1,dby,'b');
figure('name','dbz');
plot(1:jj-1,dbz,'b');
figure('name','ddbx');
plot(1:jj-1,ddbx,'b');
figure('name','ddby');
plot(1:jj-1,ddby,'b');
figure('name','ddbz');
plot(1:jj-1,ddbz,'b');
%}
Ts=0.001;
uk=0;
vc=15;
for k=0:1/Ts;
u=k*Ts;
b=BezierP(P,m,u);
db=BezierPDerivatives(P,m,u);
ddb=BezierPDerivatives2(P,m,u);
uk=uk+(vc*Ts)/norm(db)-((vc*Ts)^2/2)*((db'*ddb)/norm(db)^4);
ukk(k+1)=uk;
end
ukt(:,2:size(ukk,2)+1)=ukk;
jj=1;
while jj~=size(ukt,2)&&u<=1
u=(jj-1)*Ts;
b=BezierP(P,m,u);
db=BezierPDerivatives(P,m,u);
ddb=BezierPDerivatives2(P,m,u);
bx(jj)=b(1,1);
by(jj)=b(2,1);
bz(jj)=b(3,1);
dbx(jj)=db(1,1);
dby(jj)=db(2,1);
dbz(jj)=db(3,1);
normdb1(jj)=norm(db);
normdb(jj)=norm(db)*((ukt(jj+1)-ukt(jj))/Ts);
ddbx(jj)=ddb(1,1);
ddby(jj)=ddb(2,1);
ddbz(jj)=ddb(3,1);
jj=jj+1;
end
plot3(bx,by,bz,'r');
hold on;
plot3(P(1,:),P(2,:),P(3,:));
xlabel('X轴');
ylabel('Y轴');
zlabel('Z轴');
figure('name','db');
plot(1:jj-1,normdb,'b');
hold on
plot(1:jj-1,normdb1,'r');
figure('name','u');
plot(1:jj,ukt,'b');