三自由度机械臂的轨迹规划实例

一、轨迹规划简介

        机械臂的轨迹规划有两种形式，一种是笛卡尔空间轨迹规划，一种是关节空间轨迹规划。笛卡尔空间轨迹规划相比较关节空间轨迹规划而言，更加直观。两种方法本质上没有差别，仅仅是变量选取稍微有些不同。本文主要介绍笛卡尔空间的轨迹规划。

        轨迹规划主要有以下几个要点：

        1.确定需要到达的各点的坐标位置，与到达的时间点。每条轨迹会有一个起点、一个终点、以及n-1个中间点。

        2.每个中间点的左右速度连续。也就是说速度不发生突变。

二、三次多项式规划

        三次多项式规划，相当于将位置与时间的关系式设置成一个如下图所示三次多项式：

        

         则可以看出位置对时间求导（即速度）为二次多项式，位置对时间求二阶导（即加速度）为一次式。

        对于如图所示的实例，要求手臂在t=0，2，4，7分别经过对应的坐标点。则可以分别设定相应的（0，2）段，（2，4）段，（4，7）段的三次多项式。注意，以下的表示的是用时间t减去本段的起始时间，以便于简化运算。

        

        

        多项式设定完毕后，可以从式中看出，一共有4*3=12个位置数。则至少需要12个方程才可以对其进行求解。

        首先看位置方程：起终点分别有1个方程，而中间点既可以放在左边的曲线上，又可以放在右边的曲线上，所以有2个方程。本实例中一共有两个中间点，则此处一共有6个方程。

        

         接着看速度方程：起点和终点分别有1个方程，一般初始速度和结束速度均为0。中间点由于速度连续性条件，则每个中间点上位置方程对时间求导后便可以得到速度方程，速度方程在中间点的t时刻计算出的速度左右段连续相等，便可以得到一个方程。于是，本例中可以得到4个速度方程。

        

        最后，利用加速度连续条件，在中间点上可以得到两个方程，具体如下。

         

         写成矩阵形式便可以进行求解

        

三、直线——抛物线轨迹规划

        直线——抛物线轨迹规划与三次多项式规划不同，它期望机械臂在运动过程中可以走直线，但如果走直线，意味着他在中间点处的速度左右不连续，因此为了实现速度的连续性，采用抛物线进行过渡。

        从下图可以看出，左图只有直线段的，速度存在突变，突变处的加速度为无穷大。中间图为存在抛物线过渡的，其加速度为一个定值。最右边的图为加速度的最小值，此时不存在直线段。加速度为最小值，可以看出它的最大速度为之有直线段的速度的两倍。

 ​​​​​​​        

       从中选取一段区域，使得中间点左右两段抛物线的时间相等。如下图所示。

         

 对于直线段有:

 对于抛物线段有：

        注意头尾采用向后、前移所需加速时间的一半的处理。同时，当我们规划好轨迹后，会发现规划的轨迹不通过中间的via point。如果我们需要机械臂通过该中间点，则我们需要在其附近找到伪中间点，使得对应轨迹可以通过中间点。

        具体计算步骤如下：

        1.计算各直线段的速度

        2.计算各抛物线段的加速度，注意头尾的速度计算需要减去加速时长的1/2.

        3.写出各段方程

        直线段

        

        抛物线段