b样条和三次样条_机器人轨迹规划：三次样条曲线（matlab）

写在前面

在一些避障的应用场景下，一般都是先在任务空间中对多轴机械臂的末端进行路径规划，得到的是末端的运动路径点数据。这条轨迹只包含位置关系，并没有告诉机器人应该以怎样的速度、加速度运动，这就需要进行带时间参数的轨迹规划处理，也就是对这条空间轨迹进行速度、加速度约束，并且计算运动到每个路点的时间，高级的算法有TOPP等，一般的呢就是贝塞尔、三次准/非/均匀B、五次及三次样条等。下面从最简单的三次样条开始讨论。

三次样条曲线性质

当给出n+1个点时，可以使用n个p次多项式（通常较低）代替唯一的n次插值多项式，每个多项式定义一段轨迹。以这种方式定义的总函数s（t）称为p阶的样条曲线。p的值是根据所需的样条连续度来选择的。例如，为了在两个连续段之间发生过渡的时刻tk获得速度和加速度的连续性，可以假定多项式的阶数p＝3（三次多项式）。

定义三次样条曲线的函数形式为：

这段轨迹由n个三次多项式构成，并且每个多项式需要计算四个参数。由于n个多项式是定义一条通过n+1点的轨迹所必需的，因此需要确定的系数总数为4n。为了解决这个问题，必须考虑以下条件：

• 给定点插值的2n条件，因为每一个三次函数必须在其极值处穿过点。
• n-1个条件，过渡点的速度要连续；
• n-1个条件，过渡点的加速度要连续；

这样的话，就已经限制了2n+2(n-1)个条件，还剩下2个自由度还未限制。通过前面分析，还需要两个限制条件才行，这里讨论的就是初始点和终点的速度以及加速度。下面是几种可能的选择，可以任意选择：

通常情况，样条曲线具有如下几个特性：

• 对于由给定点(tk,qk),k=0,…n得到的p阶样条曲线s(t)，[n(p+1)]个参数可以确定
• 给定n+1个点，并且给定边界条件，则p阶插值样条曲线s(t)能被唯一确定
• 用于构造样条曲线的多项式的阶数p不取决于数据点的数目
• 函数s(t)p-1阶连续可导
• 自然样条曲线是指初始加速度和最终加速度均为0的样条曲线

当指定初始速度v0和最终速度vn时的参数计算（也就是v0和vn已知）

在定义自动机械的轨迹时，速度剖面的连续性条件至关重要。因此，计算样条曲线的典型选择是指定初始和最终速度v0和vn。因此，给定点(tk,qk),k=0,…n以及速度的边界条件（初始速度和最终速度）v0,vn，就有如下几个条件成立：

可以最终确定样条曲线的函数s(t)为

系数ak,i可以由以下算法进行确定：

第一种情况，如果中间点（插补点）的速度我们已知，也就是vk,k=1,…,n-1，对于每段三次样条曲线，有

其中，$T_{k}=t_{k+1}-t_{k}$。通过解上面的方程，可以得到

这就可以把每一段曲线的系数都求出来，从而得到样条曲线。这是最简单的情况！

子函数如下：

% 三次样条：指定初始速度v0和终止速度vn，并且中间插补点的速度已知，这是最简单的情况
% Input：
%   q：给定点的位置
%   t：给定点位置对应的时间
%   v：包括给定起始、中间及终止速度的速度向量
%   tt：插补周期
% Output：
%   yy dyy ddyy：样条曲线函数值、速度、加速度值
function [yy dyy ddyy] = cubicSpline_1(q, t, v, tt)
if length(q) ~= length(t)
    error('输入的数据应成对')
end
n = length(q);
T = t(n) - t(1); % 运行总时长
nn = T / tt; % 总点数
yy = zeros(1, nn);
dyy = zeros(1, nn);
ddyy = zeros(1, nn);
j = 1;
for i = 1: n-1
    Tk = t(i+1) - t(i);
    a0 = q(i);
    a1 = v(i);
    a2 = (1/Tk) * ((3*(q(i+1)-q(i)))/Tk - 2*v(i) - v(i+1));
    a3 = (1/(Tk*Tk)) * ((2*(q(i)-q(i+1)))/Tk + v(i) + v(i+1));

    for tk = t(i): tt: t(i+1)
        if i > 1 && tk == t(i)
            continue
        end
        yy(j) = a0 + a1*(tk-t(i)) + a2*power(tk-t(i), 2) + a3*power(tk-t(i), 3);
        dyy(j) = a1 + 2*a2*(tk-t(i)) + 3*a3*power(tk-t(i), 2);
        ddyy(j) = 2*a2 + 6*a3*(tk-t(i));
        j = j + 1;
    end
end

end

第二种情况，如果中间点的速度我们未知，也就是vk,k=1,…,n-1均未知，然而这些值是必须被计算的。为此，考虑了中间加速度的连续条件：

在这些条件下，通过考虑参数$a_{k, 2}, quad a_{k, 3}, quad a_{k+1,2}$的表达式乘以$left(T_{k} T_{k+1}right) / 2$，在简单计算整理之后能够得