3次B样条曲线求长度

给定5个控制顶点：P0[-24 0 0]；P1[-12 6 0]；P2[1 8 0]；P3[10 2 0]；P4[12 0 0]；
节点矢量为：[0 0 0 0 0.75 1 1 1 1]。
B样条曲线如下：

在CAD中验证曲线长度是38.7648。但是CAD的样条曲线是3次NURBS曲线，与matlab实际有点区别。


使用matlab计算结果为38.6359，与AutoCad结果相差无几。
matlab代码如下：

clc;
clear all;
close all;
%-------------配置-------------
P1=[-24 0 0;-12 6 0;1 8 0;10 2 0;12 0 0];%控制顶点
u1=[0 0 0 0 0.75 1 1 1 1];%节点矢量
p1=3;%3次B样条曲线
d=3;%3维曲线
n1=length(u1)-p1-1;
hold on;
for i=0:0.001:1
    Pos = BSplinePoint(i,n1,p1,u1,P1,d);
    plot3(Pos(:,1),Pos(:,2),Pos(:,3),'r.');
end
plot3(P1(:,1),P1(:,2),P1(:,3),'bx');
grid on;
xlabel('x');ylabel('y');zlabel('z');

%% 求B样条曲线长度
% 曲线求导
% 导数曲线对应的节点矢量可通过去掉原曲线节点矢量的首末端点获得
[x1,y1]=size(u1);% x1为行，y1为列
[x2,y2]=size(P1);% x2为行，x2为列
u_new = u1(2: y1-1);
Q = zeros(x2-1, y2);
% 求导数曲线对应的控制点
for i = 1: x2-1
    Q(i, :) = p1 * (P1(i+1, :)-P1(i, :)) / (u1(i+p1+1) - u1(i+1));
end
n_Length=length(u_new)-(p1-1)-1;

%% 求弧长代码
a=0;
b=1;

qt=BSplinePoint(a,n_Length,p1-1,u_new,Q,d);
fa=7*sqrt((qt(1))^2+(qt(2))^2+(qt(3))^2);%-------------------
qt=BSplinePoint(b,n_Length,p1-1,u_new,Q,d);
fb=7*sqrt((qt(1))^2+(qt(2))^2+(qt(3))^2);%-------------------

C1n=0;
C2n=1;
n=1;
eps=0.0001;
while abs(C1n-C2n)>eps
    %---------------------------------C1n---------------------------------
    k1=(b-a)/(90*n);
    ff1=0;
    for kj=0:n-1
        xk=a+kj/n;
        xk_1=xk+1/(4*n);
        qt=BSplinePoint(xk_1,n_Length,p1-1,u_new,Q,d);
        f1=32*sqrt((qt(1))^2+(qt(2))^2+(qt(3))^2);%-------------------
        xk_1=xk+2/(4*n);
        qt=BSplinePoint(xk_1,n_Length,p1-1,u_new,Q,d);
        f2=12*sqrt((qt(1))^2+(qt(2))^2+(qt(3))^2);%-------------------
        xk_1=xk+3/(4*n);
        qt=BSplinePoint(xk_1,n_Length,p1-1,u_new,Q,d);
        f3=32*sqrt((qt(1))^2+(qt(2))^2+(qt(3))^2);%-------------------
        ff1=ff1+f1+f2+f3;
    end
    ff2=0;
    for kj=0:n-1
        xk=a+kj/n;
        qt=BSplinePoint(xk,n_Length,p1-1,u_new,Q,d);
        f4=14*sqrt((qt(1))^2+(qt(2))^2+(qt(3))^2);%-------------------
        ff2=ff2+f4;
    end
    C1n=k1*(fa+ff1+ff2+fb);
    
    %---------------------------------C2n---------------------------------
    n=n*2;
    k1=(b-a)/(90*n);
    ff1=0;
    for kj=0:n-1
        xk=a+kj/n;
        xk_1=xk+1/(4*n);
        qt=BSplinePoint(xk_1,n_Length,p1-1,u_new,Q,d);
        f1=32*sqrt((qt(1))^2+(qt(2))^2+(qt(3))^2);%-------------------
        xk_1=xk+2/(4*n);
        qt=BSplinePoint(xk_1,n_Length,p1-1,u_new,Q,d);
        f2=12*sqrt((qt(1))^2+(qt(2))^2+(qt(3))^2);%-------------------
        xk_1=xk+3/(4*n);
        qt=BSplinePoint(xk_1,n_Length,p1-1,u_new,Q,d);
        f3=32*sqrt((qt(1))^2+(qt(2))^2+(qt(3))^2);%-------------------
        ff1=ff1+f1+f2+f3;
    end
    ff2=0;
    for kj=0:n-1
        xk=a+kj/n;
        qt=BSplinePoint(xk,n_Length,p1-1,u_new,Q,d);
        f4=14*sqrt((qt(1))^2+(qt(2))^2+(qt(3))^2);%-------------------
        ff2=ff2+f4;
    end
    C2n=k1*(fa+ff1+ff2+fb);
end
rTotalLength=C2n

参考论文：《基于三次非均匀B样条曲线的机器人轨迹规划算法研究》