matlab做三次拉格朗日插值多项式_机器人轨迹规划：三次样条曲线（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，对于每段三次样条曲线，有