引言：
前面学习了正向运动学和反向运动学，我们知道如何求解机械臂各个关节的角度来操控机械臂到达指定的目标点，如抓取桌面上一个杯子或者一支笔。那么我们如何控制机械臂来让这支笔写字或者把杯子放到某一个位置呢？我们需要知道在这个过程中机械臂的状态（位置，状态）与时间的关系，通过轨迹规划来拟合一条连续的曲线让机械臂完成目标。
1）基础知识
轨迹：机械臂的末端/操作点的位置，速度，加速度对时间的历程。
轨迹规划：已知起始点，终点，我们自己定义若干个中间点（via point），把这些点用平滑曲线连接起来。理想的轨迹都是物体运动的位置和速度都是连续的。
轨迹规划的类型：joint space和Cartesian-space下的轨迹规划。
曲线拟合：选择适当的曲线类型来拟合数据，并用拟合的曲线来分析变量间的关系。
2）joint space下的轨迹规划
对于六自由度的机械臂，假设我们已知起始点，终点和中间点的状态（位置和姿态信息）。
1，反向运动学IK求解角度：将机械臂操作点起始端，末端和中间经过的若干点的空间状态转化为joint状态（具体就是通过反向运动学ik，求解出各个joint的角度）；
2，轨迹规划（拟合曲线）：对各个的joint的角度进行轨迹规划，拟合曲线（得到这些点平滑的函数表示，也就是找出函数关系，坐标系的映射）；
3，正向运动学验证：知道了手臂一路上需要的joint对时间的关系，由正向运动学将joint状态转换到末端点的状态，就能描绘出手臂的轨迹，验证是否经过各个已知的点。
4，检查可行性：是否能够避开障碍物等，满足我们的要求。
3）Cartesian-space下的轨迹规划同样的，我们用六个参数（自由度）来表达移动和转动，定义起始点，到达点和中间若干经过的点；
1，轨迹规划（拟合曲线）：将所有机械臂末端点的状态进行规划，拟合曲线，找出他们的函数关系。
2，反向求解验证：将规划好的手臂末端点的状态的轨迹转换成joint，检查各个joint能否满足规划要求；
3，检查可行性：是否满足避开障碍物等要求。
对于以上两种方法，Cartesian-space下的轨迹规划更加直接的符合我们的要求，直接就能规划出我们需要的轨迹，缺点是在验证阶段，我们需要通过反向运动学求解各个joint的角度，计算量大；而joint space下的轨迹规划，我们直接计算机械臂各个状态下对应的joint，然后拟合成曲线，通过正向运动学就能得到机械臂状态的轨迹（对joint角度的曲线拟合比反求各个角度的计算要简单）
总的来说，轨迹规划的核心就是求解各个状态下对应的joint的角度，然后拟合成曲线。那么如何来拟合这些曲线呢？
4）轨迹规划方法（曲线拟合）—Cubic Polynomials三次多项式
纵坐标并不是特指角度，在这里我们代表各个自由度。
图中各段的轨迹我们以不同的参数来规划：每两个相邻的点，我们用一段函数来串联，为了方便每段函数都是同一个类型，但是每段函数都是不同的参数。所以整个曲线就是多段参数不同的多项式拟合而来。
需要定义各段函数的边界条件（包含位置和速度，两个点一共四个条件）：每段头尾两端的位置和速度条件要定义清楚（确保各段相连）。
由以上的四个边界条件可知，我们可以选择Cubic Polynomials三次多项式来规划轨迹。假设两侧点的位置/姿态和速度条件已知，我们对每段进行规划求解：
可知a0和a1，带入求解
对于上述运算，我们可以用矩阵的形式来表达
对于前面的求解，我们是已知边界的位置/姿态和速度的；那么真实情况下，我们通常已知的只有位置，对于两侧点的速度我们如何进行选择？
1，直接定义（不选择）
2，以简单的法则去自动生成
3，规划速度的时候也要考虑不同段的加速度的连续性，这需要我们把每一段的三次式联立一起解（以一个例子说明中间的速度和加速度如何选择？）
八个方程式
两侧的速度不一定是0，看具体的条件；
对于中间点的速度由左边的多项式和右边的多项式来算的话，必须相等，才能保证连续的；
八个方程式，联立求解
接下来我们对更广义轨迹的一系列中间点的速度进行求解（所有的速度和加速度都是连续的，我们用第三种方法）
对于4n个未知数，我们需要n个方程去求解。
每段左右两端点都要相等，所以每段都有两个位置条件，n段就有2n个位置条件；对于所有的中间点，我们都有速度和加速度要连续的条件，n段有n-1个中间点，所以有2（n-1）个条件可以用。还差2个条件？
5）举例说明
四个点，可以规划成三段曲线（类型相同，参数不同的三次多项式），所以一共是12个参数；对于位置条件，我们已知每个点的位置，每段代入两端的位置条件，可以得到6个方程，然后代入首尾的速度条件得到2个方程，然后中间两个点代入两端的多项式方程，令速度和加速度相等，两个点一共四个方程（然后将速度和加速度相对的方程每一项都移到一边，另外一边就为0了）。这样便能算出任一个自由度的轨迹。
本题是三个自由度的轨迹求解，所以我们还有向上面步骤那样计算三次，写成矩阵形式如上图。
