关节空间轨迹规划

机械臂轨迹规划是根据机械臂末端执行器的操作任务，在其初始位置、中间路径点和终止位置之间，采用多项式函数来逼近给定路径，它是机器人学的一个重要的研究内容。关于机械臂的轨迹规划可以分为关节空间的轨迹规划和操作空间轨迹规划。在操作空间的轨迹规划概念直观，但是需要进行大量的矩阵计算，并且操作空间的参数很难通过传感器直接获得，很难用于实时控制。在关节空间的轨迹规划能够根据设计要求适时调整机械臂各关节位置、角速度和角加速度，能够有效避免机构奇异性和机械臂冗余问题。因此，面向关节空间的轨迹规划得到广泛的应用。

关节空间轨迹规划是把关节角度值描述成关于时间的函数，不用描述操作空间中两点之间的路径，计算量较小。关节空间轨迹规划主要有多项式插值法和样条插值法，其中多项式插值法简单实用，应用较多的主要是三次和五次多项式插值法。其中，三次多项式函数有4个系数，最多可以指定机械臂关节的起始位置、终点位置、起始速度和终点速度四个约束，但是不能设定关节的起点加速度和终点加速度为零，这会导致机械臂在起点和终点的加速度过大而产生振动。在这里介绍五次多项式插值法进行关节空间轨迹规划。

关节角位移的五次多项式函数为：

对上式求一阶导数得到关节角速度函数：

对上式求二阶导数得到关节角加速度函数：

五次多项式的未知系数共有6个，需要6个方程才能实现对它的求解。假设已知起始时刻和终点时刻关节的位置、速度和加速度，可求出多项式各系数。由关节空间点到点运动要求起点位置和终点位置的速度和加速度均等于零，可进而求出关节角度位置、角速度和角加速度的函数式。

三次多项式的方法与五次多项式法相同。

样条插值是一种工业设计中常用到的、达到平滑曲线的一种插值方法，三次样条插值是其中应用较为广泛的一种。

三次样条插值具有以下性质：

• 三次样条曲线在衔接点处是连续光滑的；
• 三次样条的以及倒数以及二阶导数是连续的；
• 自由边界三次样条的边界的二阶导数也是连续的；
• 单个点并不会影响整个函数曲线。

样条插值与多项式的方法有异曲同工之妙。根据机械臂的操作任务，将任务空间的轨迹离散化，得到一系列的路径点，通过逆运算求解，将各个路径点转化成各个关节对应的角度或位移值，为使机械臂一次达到各个路径点，各关节需要同时运动到相应的关节角度，因此，对于每个关节而言，个路径所需要的运动时间是相同的。设各关节的关节角—时间序列为：

其中， ti为关节运动到各个路径点对应的时间

为了使机械臂运动平稳，需要保证每个关节的运动函数二阶连续，生成的路径函数经过每一个路径点。

算法总结：

• 假设有n+1个数据节点

• 计算步长

• 将数据节点以及指定的首末端点条件带入矩阵方程

• 求解矩阵方程，求得二次微分值Mi
• 计算样条曲线系数

• 在每个子区间中创建方程