3次Bezier匀速进给的设计计算——Matlab

3次Bezier匀速进给的设计计算——Matlab

用Bezier曲线规划轨迹，控制顶点为：P0（0,0,1），P1（0,2,7），P2（1,1.5,2）和P3（2,3,5）。由于控制顶点由4个组成，所以组合为一起为3次Bezier曲线。曲线方程为：
b(t)=(1-t)³P0+3t(1-t)²P1+3t²(1-t)P2+t³P3-----------------0<=t<=1

m次Bezier曲线的n次导数为m-n次贝塞尔曲线。
规划采样时间是0.001ms，速度为15。代码如下：

%3次Bezier匀速进给的设计计算——Matlab
clc;clear all;close all;

m=3;
P=[0 0 1 2;0 2 1.5 3;1 7 2 5];

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

图形如下：