机器人轨迹规划：求解样条曲线长度——自适应辛普森公式求积分

写在前面

挖个坑~
在样条学习过程中遇到了积分求解样条长度的问题，一般曲线长度的求解可以采用对速度积分的方式，但如果直接由速度解析表达式计算路径长度非常困难，尤其当表达式复杂的时候，对于计算机编程而言非常不友好。为此，可通过数值积分的方式对表达式进行近似积分，常见方法有复合梯形积分、复合辛普森积分。下面尝试采用自适应辛普森公式，先简单用matlab测试一下~

自适应辛普森公式求积分

matlab简单测试代码如下

% 自适应辛普森求积分
% https://blog.csdn.net/frosero/article/details/45799135
function y = adaptive_simpson(a, b, eps)

    y = main_adaptive_simpson(a, b, eps, simpson(a, b));

end

function y = main_adaptive_simpson(a, b, eps, A)
    c = a + (b - a) / 2.0; % A为区间[a,b]的三点辛普森值
    L = simpson(a,c);
    R = simpson(c,b);
    if (abs(L + R - A) <= 15 * eps) 
        y =  L + R + (L + R - A) / 15.0;
    else
        y = main_adaptive_simpson(a,c,eps/2.0,L) + main_adaptive_simpson(c,b,eps/2.0,R);
    end
end

% 自定义函数
function f = func(x)
    f = x * x * x + x;
end

function y = simpson(a, b)
    c = a + (b - a) / 2.0;
    y = (func(a) + 4 * func(c) + func(b)) * (b - a) / 6.0;
end

求样条曲线长度

核心函数如下：

...上接B样条基本曲线程序
% 求B样条曲线长度
% 曲线求导
% 导数曲线对应的节点矢量可通过去掉原曲线节点矢量的首末端点获得
u_new = u(2: end-1);
Q = zeros(n+1, 3);
line = zeros(1, 3);
% 求导数曲线对应的控制点
for i = 1: n+1
    Q(i, :) = k * (P(i+1, :)-P(i, :)) / (u(i+k+1) - u(i+1));
end
for j = 1: n-1
    La = u_new(j+2); % u_new比之前u少两个
    Lb = u_new(j+3);
    line(j) = adaptive_simpson(La, Lb, 1e-5, Q, u_new, k, j);
end
length_arc_B = line(1) + line(2) + line(3);
...

按照上述辛普森积分程序，适当进行修改

function y = adaptive_simpson(a, b, eps, Q, u, k, j)

    y = main_adaptive_simpson(a, b, eps, simpson(a, b, Q, u, k, j), Q, u, k, j);

end

function y = main_adaptive_simpson(a, b, eps, A, Q, u, k, j)
    c = a + (b - a) / 2.0; % A为区间[a,b]的三点辛普森值
    L = simpson(a, c, Q, u, k, j);
    R = simpson(c, b, Q, u, k, j);
    if (abs(L + R - A) <= 15 * eps) 
        y =  L + R + (L + R - A) / 15.0;
    else
        y = main_adaptive_simpson(a, c, eps/2.0, L, Q, u, k, j) + ...
            main_adaptive_simpson(c, b, eps/2.0, R, Q, u, k, j);
    end
end

% 自定义函数
%{
function f = func(x)
    f = x * x * x + x;
end
%}

function y = simpson(a, b, Q, u, k, j)
    c = a + (b - a) / 2.0;
    % y = (func(a) + 4 * func(c) + func(b)) * (b - a) / 6.0;
    Fa = Bspline_diff(Q, u, k, j, a);
    Fb = Bspline_diff(Q, u, k, j, b);
    Fc = Bspline_diff(Q, u, k, j, c);
    y = (Fa + 4*Fc + Fb) * (b-a) / 6.0;
end

以三次分均匀B样条为例，其导数函数为

%% 三次非均匀B样条导数
% Input:
%   Q: 导数曲线对应的控制点
%   u: 新节点
%   k: 样条曲线次数，此时k=3
%   j: 第j段曲线
%   uu: 细分后的当前u值
function y = Bspline_diff(Q, u, k, j, uu)

x = Q(j, 1) * Base(j, k-1, u, uu) + Q(j+1, 1) * Base(j+1, k-1, u, uu) + Q(j+2, 1) * Base(j+2, k-1, u, uu);
y = Q(j, 2) * Base(j, k-1, u, uu) + Q(j+1, 2) * Base(j+1, k-1, u, uu) + Q(j+2, 2) * Base(j+2, k-1, u, uu);
z = Q(j, 3) * Base(j, k-1, u, uu) + Q(j+1, 3) * Base(j+1, k-1, u, uu) + Q(j+2, 3) * Base(j+2, k-1, u, uu);

y = sqrt(x*x + y*y + z*z);

end

参考

[学习笔记]自适应辛普森（Simpson）积分
自适应辛普森公式求积分
赖永林,林茂松,梁艳阳.基于三次非均匀B样条曲线的机器人轨迹规划算法研究[J].科学技术与工程,2013,13(35):10511-10517.