写在前面
在一些避障的应用场景下，一般都是先在任务空间中对多轴机械臂的末端进行
路径规划，得到的是末端的运动路径点数据。这条轨迹只包含位置关系，并没有告诉机器人应该以怎样的速度、加速度运动，这就需要进行带时间参数的
轨迹规划处理，也就是对这条空间轨迹进行速度、加速度约束，并且计算运动到每个路点的时间，高级的算法有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，并且中间插补点的速度已知，这是最简单的情况
第二种情况，如果中间点的速度我们未知，也就是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$，在简单计算整理之后能够得到
其中k=0,…,n-2。
上面的关系可以整理成矩阵的形式，$A^{prime} v^{prime}=c^{prime}$，其中
其中常数项ck仅取决于中间位置和已知的样条曲线段的持续时间Tk。由于速度v0和vn也是已知的，所以可以消除矩阵A‘的相应列并获得
也就是
例子
另外v0=2，v6=-3。
% 三次样条：指定初始速度v0和终止速度vn，但是中间点速度未知
测试：
%% 自写cubicSpline_2函数测试
周期三次样条：没有指定初始速度v0和最终速度vn（也就是v0和vn未知）
在许多应用中，要执行的运动是周期性的，即初始位置和最终位置相同。在这种情况下，利用为计算样条曲线而指定的最后两个自由度，以使曲线具有初始和最终速度和加速度的连续性。因此，计算系数的方法与先前报告的略有不同。事实上，在这种情况下，必须考虑代替任意选择的初始和最终速度v0和vn的条件。
最后一个公式可以写成：
代入系数表达式后，从（4.8）中得到
通过将该方程添加到系统（4.10）中，并考虑到在这种情况下，速度vn等于v0但未知（因此在（4.10）中，必须在左侧移动$T_{n-2}v_n$和$T_1v_0$），计算速度的线性系统变为
系统的矩阵不再是三对角的。这种情况被称为循环三对角系统，也存在有效的求解计算方法。一旦获得速度v0，…，vn−1，就可以通过（4.8）计算样条曲线的系数。
例子
% 三次样条：周期三次样条，没有指定初始速度v0和终止速度vn，也就是v0和vn未知
测试：
%% 自写cubicSpline_3函数测试
具有指定初始速度v0和最终速度vn的三次样条（v0和vn已知）：基于加速度的计算
定义三次样条还有一种方法：样条曲线的一般多项式qk（t）可以表示为在其端点处计算的二阶导数的函数，也就是加速度$ddot{q}left(t_{k}right)=omega_{k}{k=0,...,n}$,，而不是速度$v_k$。可以计算得到此时的多项式表达式为
这样的话，速度和加速度就是如下计算式
这样的话，未知数就是加速度$omega_k$，因为从上面的计算式可以发现，曲线的未知参数就是加速度了，因此它是唯一定义样条曲线的。由于中间点的速度和加速度的连续性，我们得到
结合式（4.15）、（4.17）和（4.18），可以得到
其中k=1,…,n-1。另外，初始速度和最终速度有如下条件
由此可以推导得到
结合（4.19）-（4.21），可以得到如下线性系统
其中，(n+1)阶三对角对称矩阵A（其中未知参数$omega=[omega_0,omega_1,...,omega_n]^T$）为
同样是利用追赶法解三对角线方程组，通过将解方程得到的$omega _k$带入（4.14）中，最终得到样条曲线。除了利用（4.14）来描述三次样条曲线之外，显然，还是可以根据初始定义来描述三次样条曲线，即
其中样条曲线的参数a呢就可以通过已知的点$q_k$和已经求得的加速度$omega _k$来计算，计算式如下
这个方法主要是为下面的方法做准备，因此不写例子。
具有指定初始、最终速度以及加速度的三次样条曲线
样条曲线是一个二阶连续导数的函数，但通常不可能同时指定初始速度和最终速度以及加速度。因此，样条曲线在其末端的特征是速度或加速度的不连续性，一般情况下我们会指定初始和最终速度，则此时初始和最终加速度难以保证连续，会出现突变。下面需要做的就是需要保证在指定初始速度和最终速度的前提下，还要保证初始、最终加速度从0开始连续变化。如果这些不连续代表一个问题，可以采用不同的方法：
- 一个5次多项式函数可以用于第一个和最后一个域，其缺点是允许在这些段中有更大的超调，并且稍微增加了计算负担；
- 在第一段和最后一段中添加两个自由额外点（从这个意义上说，这些点不能先验地固定），并通过施加所需的速度和加速度的初始值和最终值来计算它们的值。
后一种方法现在详细说明。让我们考虑一个要插值的n-1点向量，这两个向量中都没有第二个值以及倒数第二个值，也就是q1、t1和qn-1、tn-1（目前我还不知道为啥要这么做。。。）
对应的时间节点为
以及同时考虑速度v0，vn和加速度a0，an的边界条件。为了施加所需的加速度，增加了两个额外的点$bar{q}{1}$ 和 $bar{q}{n-1}$。时间瞬间$overline{t_{1}}$ 和 $bar{t}_{n-1}$分别位于t0和t2之间以及tn-2和tn之间。不过可以分析得到，这种处理办法虽然能够对一条轨迹施加轨迹长度、初始速度、终止速度、初始加速度、终止加速度这五种约束条件，但是前提是额外增加两个轨迹点以及时间点，这样可能破坏时间最优规划的初衷，额外增加约束可能也会导致轨迹灵活性变差。。。
增加的这两个点，可以通过已知的变量去表达这两个点，即初始/最终的位置、速度以及加速度（q0/qn,v0/vn,a0/an）同时包括在这些点上的加速度w1和wn-1（其中边界点的加速度是用a0和an来表示，而中间点的加速度是用w来表示）。这样，就可以考虑初始加速度和最终加速度的约束。
将（4.26）和（4.27）替换掉（4.24）中的有关项，通过重新排列n-1方程，我们得到一个线性系统
其中
注意，T0、T1和Tn-2、Tn-1分别是$overline{t_{1}}$ 和 $bar{t}_{n-1}$的函数，可以在间隔（t0、t2）和（tn-2、tn）中任意选择，例如
通过求解方程组（4.28）可以得到中间插补点的加速度为
与边界值a0和an一起，就可以根据（4.14）或者（4.25）计算整体的样条曲线。
例子
其中v0=2，vn=-3，a0=0，an=0。额外增加的两个时间点t1=2.5，t7=16.5，当然也可以任意选择。
% 三次样条：指定初始、终止速度以及加速度，也就是v0，vn，a0，an已知
测试：
%% 自写cubicSpline_4函数测试
三次样条部分还有平滑三次样条，后面再写，今天就到这里~
参考文献（实际上我就是翻译了一下下。。。）
Trajectory Planning for Automatic Machines and Robots