基于Bezier匀速进给时间律U的设计计算

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');