一、概述

        机械手最常用的轨迹规划方式有两种：

        第一种方法要求用户对于选定的转变节点（插值点）上的位姿、速度和加速度给出一组显式约束（例如连续性和光滑程度等），轨迹规划器从一类函数（例如n次多项式）中选取参数化轨迹，对节点进行插值，并满足约束条件。

        第二种方法要求用户给出运动路径的解析式，如为直角坐标空间中的直线路径，规划器在关节空间或直角空间中确定一条轨迹来逼近预定的路径。

        在第一种方法中，约束的设定和轨迹规划均在关节空间进行，因此可能会发生与障碍物相碰撞。

        第二种方法的路径约束是在直角坐标中给定的，而关节驱动器是在关节空间中受控的。因此为了得到与给定路径十分接近的轨迹，首先必须采用某种函数逼近的方法将直角坐标路径约束转化为关节坐标路径约束，然后确定满足关节路径约束的参数化路径。

        轨迹规划即可以在关节空间进行也可以在直角空间中进行，但是所规划的轨迹函数都必须连续和平滑，使得操作臂的运动平稳。在关节空间进行规划时，是将关节变量表示成时间的函数，并规划他的一阶和二阶时间导数；在直角空间中进行规划是将手部位姿、速度和加速度表示为时间的函数。而相应的关节位移、速度和加速度由手部的信息导出。通常通过运动学反解得出关节位移。用逆雅可比求出关节速度，用逆雅可比及其导数求出关节加速度。

二、关节轨迹的插值计算

        为了求得在关节空间形成所求轨迹，首先用运动学反解将路径点转换成关节矢量角度值，然后对每个拟合一个光滑函数，使之从起始点开始，依次通过所有路径点，最后到达目标点。对于每一段路径，各个关节运动时间均相同，这样保证所有关节同时到达路径点和终止点，从而得到工具坐标系{T}应有的位置和姿态。尽管每个关节在同一段路径中的运动时间相同，但各个关节函数之间却是相互独立的。

        关节空间法是以关节角度的函数描述机器人轨迹的，关节空间法不必在直角坐标系中描述两个路径点之间的路径形状，计算简单、容易。此外，由于关节空间与直角坐标空间之间并不是连续的对应关系，因而不会发生机构的奇异性问题。

        在关节空间中进行轨迹规划，需要给定机器人在起始点和终止点手臂的位形。对关节进行插值时，应满足一系列的约束条件，例如抓取物体时，手部运动方向（初始点），提升物体离开的方向（提升点），放下物体（下放点）和停止点等节点上的位姿、速度和加速度的要求；与此相应的各个关节位移、速度、加速的在整时间间隔内连续性要求；其极值必须在各个关节变量的允许范围之内。

（一） 三次多项式插值

        在机械手运动的过程中，由于相应与起始点的关节角度θ0是已知的，而终止点的关节角θf可以通过运动学反解得到。因此运动轨迹的描述，可以用起始点关节角度与终止点关节角度的一个平滑插值函数θ(t)来表示，在t0=0时刻的值是其实关节角度θ0，在终端时刻tf的值是终止关节角度θf。显然，有许多平滑函数可以作为关节插值函数。

        为了实现单个关节的平稳运动，轨迹函数θ(t)至少需要满足4个约束条件，其中两个约束条件是起始点和终止点对应的关节角度：

   

        为了满足关节运动速度的连续性要求，另外还有两个约束，即在起始点和终止点的关节速度要求。在当前情况下，规定：

     

        故上述四个边界约束条件唯一确定了一个三次多项式：

      

        则关节轨迹上的运动速度和加速度可以通过求导得到：

       

     

        对于关节位置、速度、加速度，带入起始点和终止点的关节角度对应的约束条件，得到有关系数 a0 a1 a2 a3 的四个线性方程：

       

        

       

       

        求解这四个方程可得四个系数a0 a1 a2 a3

       

        但是这一组解只适用于起始速度和终止速度为0的运动情况，对于其他情况，后面另行讨论。

（二）  过路径点的三次多项式插值

        一般情况下，要求规划过路径点的轨迹。如果机械手在路径点停留，则可直接使用前面三次多项式插值的方法：如果只是路过路径点，并不停留，则需要对公式进行推广。

        实际上，可以把所有路径点也看做是“起始点”或者“终止点”，求解逆运动学，得到相应关节的矢量值。然后确定要求的三次多项式插值函数，把路径点平滑地连接起来，但是在这些起始点和终止点的速度并不为0。

        路径点上的关节速度可以根据需要设定，这样一来，确定三次多项式的方法与前面所述的完全相同，只是速度约束条件式子变为：

       

        同理确定三次多项式的四个方程为：

      

      

      

      

        可求解出a0 a1 a2 a3 :

   

        实际上，由上式确定的三次多项式描述了起始点和终止点具有任意给定位置和速度的运动轨迹，是前面三次多项式插值的推广。剩下的问题就是如何确定路径点上的关节速度，由下面三种方法可以确定：

        1) 根据工具坐标系在直角坐标空间中的瞬时线速度和角速度来确定每个路径点的关节速度

        2) 在直角坐标空间或关节空间中采用适当的启发方式，由控制系统自动地选择路径点的速度

        3) 为了保证每个路径点上地加速度连续，由控制系统按此要求自动地选择路径点的速度。

（三） 高阶多项式插值

        如果对于运动轨迹的要求更为严格。约束条件增多，那么三次多项式就不能满足需要，必须用更高阶的多项式对运动轨迹的路径段进行插值。例如：对某段路径的起始点和终止点都规定了关节的位置、速度和加速度，则要用一个五次多项式进行插值，即：

     

        项式的系数a0 a1 a2 a3 a4 a5必须满足6个约束条件：

  

        这个线性方程式组的解为：

        

补充：

        有关轨迹规划与路径规划的区别：

        轨迹规划是在路径规划的基础上，赋予路径时间戳。它决定了机械臂在每一个时间点应该处于什么位置、速度是多少、加速度是多少。

        路径规划的本质是一个几何优化与拓扑搜索问题。它不关心机器人运动的快慢，只关心能不能在不撞到障碍物的前提下，找到一条连接起点和终点的几何连线。

        ·