机器人技术基础 课件 第7章 工业机器人运动规划 - 2

第7章工业机器人运动规划机器人技术基础目录contants7.1机器人运动规划概述7.2关节空间的轨迹规划方法7.3操作空间的轨迹规划7.2关节空间的轨迹规划方法关节空间的轨迹规划就是关节变量的插补问题。虽然机械臂有多个关节，但各个关节的插补过程是互相独立的，所以只要理清单一关节变量（即标量）的插补方法即可。由于不平稳的运动会导致机器人关节产生震动和冲击，加剧机械零部件的磨损和破坏。因此轨迹规划的原则是加速度为有限值且尽量连续，速度与位置则必须连续。7.2关节空间的轨迹规划方法第7章工业机器人运动规划7.2关节空间的轨迹规划方法7.2.1三次多项式插补7.2.2高阶多项式插补7.2.3多项式插补用于多点间轨迹规划7.2.4抛物线过渡的线性插补7.2.5抛物线过渡的线性插补用于多点间轨迹规划7.2.1三次多项式插补在数学课中我们熟悉的三次多项式标准形式如下：我们可以赋予自变量x和因变量y任何物理意义，但人们习惯于将(x,y)看作平面两坐标，此时y(x)代表的是一个空间概念，即平面上的一条曲线。做轨迹规划时，自变量为时间t，对于转动关节而言因变量是转角θ，所以θ(t)代表转角随时间的变化规律，为了强调这一点，本节将三次多项式插补函数写为但请不要因此认为7.2节介绍的插补方法仅适用于关节空间轨迹规划，事实上这些方法可以用于所有标量的插补。7.2.1三次多项式插补7.2.1三次多项式插补7.2.1三次多项式插补插补函数表达式起点

关节角终点

关节角终点

时间从起点到终点过程中，任一时间点对应的转角7.2.1三次多项式插补例7.1解：①

将已知条件带入下式所以满足该运动的三次多项式轨迹插补函数为代入7.2.1三次多项式插补例7.1位置曲线光滑连续递增速度曲线为抛物线，也是光滑连续的，初始点速度为0，然后逐渐增大，运行到半程（t=1.5s）时速度达到极大值，然后逐渐减小，到终止点的速度降为0加速度相对时间t的变化为一条直线，初始点加速度为最大，然后逐渐减小，运行到半程（t=1.5s）时加速度为0，然后反向加速度逐渐增大，直到终止点的加速度达到负向最大。7.2.1三次多项式插补例7.1拓展练习：读者可以尝试自己编写代码求解该题。7.2.1三次多项式插补例7.27.2.1三次多项式插补例7.2解：①

关节1的插补函数在例7.1中已经求出。将关节2的已知条件代入代入所以满足关节2运动的三次多项式轨迹插补函数为7.2.1三次多项式插补例7.2据此可画出两关节的位置、速度和加速度随时间变化的函数曲线。关节2的三次多项式轨迹插补函数为已知关节角求末端位姿，需要用到机器人正向运动学。在例3.5中已经推导出该机械臂末端的坐标为7.2.1三次多项式插补例7.2机械臂末端从起始点（左图）运动到终止点（右图）的轨迹拓展练习：读者可以尝试自己编写代码求解该题7.2.1三次多项式插补例7.27.2.1三次多项式插补思考：在上例中要求起始点和终止点的关节速度均为0，如果要求起始点和终止点的关节速度为某给定值，是否仍旧可以通过三次多项式插补得到符合要求的运动曲线？由图可以看出，两关节在起始点和终止点的加速度都比较大，如果要在这两点指定期望的加速度，三次多项式插补是否还适用？第7章工业机器人运动规划7.2关节空间的轨迹规划方法7.2.1三次多项式插补7.2.2高阶多项式插补7.2.3多项式插补用于多点间轨迹规划7.2.4抛物线过渡的线性插补7.2.5抛物线过渡的线性插补用于多点间轨迹规划7.2.2高阶多项式插补用三次多项式做插补函数时，由于共有4个待定系数，要获得唯一解就只能有4个约束。如果对运动轨迹有更为严格的要求，约束条件必然增多，三次多项式就不能满足需求，此时高阶多项式就有了用武之地。7.2.2高阶多项式插补例如，对某关节运动的起始点和终止点的位置、速度和加速度均有明确要求，则有6个约束条件，这时就可以选用5次多项式做插补函数（有6个系数），即其约束方程组为求导7.2.2高阶多项式插补7.2.2高阶多项式插补例7.3解：将tf=3代入可求得线性方程组系数矩阵A根据第一组已知条件可以得常数向量b=[15750000]’用MATLAB命令a=A\b即可求得5次多项式的系数数组a。对于第2组和第3组情况，分别有b=[1575503000]’和b=[157530-3000]’，同样用a=A\b可求得对应5次多项式的系数。7.2.2高阶多项式插补例7.37.2.2高阶多项式插补例7.37.2.2高阶多项式插补例7.3思考：①第二组的速度曲线在t=1.5~1.8区间速度接近0，这意味着什么？②前两组插补函数在t=0~3秒内都是单调递增的，而第三组在t=0~2.3区间内为递增，t=2.3~3区间内为递减。这意味着什么？这个插补轨迹是否令人满意？第7章工业机器人运动规划7.2关节空间的轨迹规划方法7.2.1三次多项式插补7.2.2高阶多项式插补7.2.3多项式插补用于多点间轨迹规划7.2.4抛物线过渡的线性插补7.2.5抛物线过渡的线性插补用于多点间轨迹规划7.2.3多项式插补用于多点间轨迹规划根据作业要求，经常需要机械臂末端连续顺滑地经过操作空间的某些路径点，比如下图中，不但要求机械臂在3秒内从起始点A顺滑地运动到终止点D，而且必须在t=1和t=2秒时经过B和C两个路径点。我们应如何规划出符合要求的各条关节轨迹？7.2.3多项式插补用于多点间轨迹规划7.2.3多项式插补用于多点间轨迹规划但这种插补方法的问题在于，因为约束中包含起始点和终止点速度为0，机械臂经过B，C等路径点时必然会停下。对于很多作业，这种不断启停的运行方式不够平稳，效率也不高。如果我们希望机械臂经过路径点但不停留，就需要将前述方法加以推广，将三次多项式插补用于多点间的轨迹规划。7.2.3多项式插补用于多点间轨迹规划回顾7.2.1中三次多项式插补函数的约束条件，如果希望经过中间路径点不停留，只要将关节速度约束修改为给定的速度即可，因此四个约束方程变为：解该线性方程组可得7.2.3多项式插补用于多点间轨迹规划从上式可以看出，

的取值会直接影响多项式系数，对插补函数的走向有重大影响。那么，应该如何合理确定机械臂经过路径点时各关节的过渡速度？7.2.3多项式插补用于多点间轨迹规划应该如何合理确定机械臂经过路径点时各关节的过渡速度？目前主要有以下三种方式。（1）根据机械臂末端在操作空间的瞬时线速度和角速度来确定路径点的关节速度利用机械臂末端在该路径点上的速度逆雅克比矩阵，可将其在该点在瞬时线速度和角速度映射为所要求的关节速度。该方法生成的轨迹虽然能满足用户设置速度的需求，但是逐点设置速度工作量很大，而且如果某个路径点是奇异点还无法任意设置速度值。因此这种方法并不常用。7.2.3多项式插补用于多点间轨迹规划（2）系统采用适当的启发式方法，自动选择合理的过渡速度图为一种简单而合理的过渡速度选择方式，图中短小直线段的系列代表各路径点的速度。首先用直线把路径点连接起来，如果路径点两侧的直线的斜率正负发生变化，则将该点速度设定为零，图中

的速度均属于这种情况；如果路径点两侧的直线的斜率正负没有改变，则选取两条直线的平均斜率作为该点的速度，图中路径点

的速度均属于这种情况。7.2.3多项式插补用于多点间轨迹规划（2）系统采用适当的启发式方法，自动选择合理的过渡速度如图所示，启发法规划出的轨迹是连续光滑的，不难看出路径点的速度正好是轨迹曲线在该点的切线斜率。从速度图可以发现在路径点处速度曲线出现尖点，导致加速度在该点出现突变，而且起始点和终止点的加速度也都比较大，这显然不是我们希望看到的。因此还需要进一步优化算法。7.2.3多项式插补用于多点间轨迹规划（3）以路径点处加速度连续为原则选取各点的速度θ0θ1θ2θ3θ4θ5θf7.2.3多项式插补用于多点间轨迹规划（3）以路径点处加速度连续为原则选取各点的速度另外起始点和终止点还有位置和速度约束，即有n个路径点则可以确定4n个约束。4个约束这样我们就确定了4(n+1)个线性方程，求解该线性方程组就可以获得4(n+1)个待定系数的唯一解。以上正是构造三次样条函数的基本思想。由于三次样条函数使用非常广泛，各种数学软件一般都会提供相关函数方便大家调用，比如MATLAB中的spline函数。7.2.3多项式插补用于多点间轨迹规划例7.47.2.3多项式插补用于多点间轨迹规划例7.4解：MATLAB函数spline可用于求过多点的三次样条函数。主要命令如下。表示三次样条曲线的需要经过的路径点坐标为(t,q)，且起始点和终止点的速度为0，计算出的各段三次多项式的系数等信息存储在结构pp中。7.2.3多项式插补用于多点间轨迹规划例7.4启发法加速度连续法（样条函数）对比启发法的轨迹规划图可以发现，以加速度连续为原则的插补方法，其速度曲线更加光滑，加速度曲线连续，也就是说通过各个路径点时不会出现加速度突变。7.2.3多项式插补用于多点间轨迹规划例7.4程序NCUT7_4a：加速度连续法

NCUT7_4b：启发法7.2.3多项式插补用于多点间轨迹规划不论三次还是五次多项式插补都有一个缺点，即其速度一直在变化中。为了提高效率，我们希望机器人各关节的允许速度尽量稳定在允许范围之内的高位上，这就是下一种的插补方法闪亮登场的原因。第7章工业机器人运动规划7.2关节空间的轨迹规划方法7.2.1三次多项式插补7.2.2高阶多项式插补7.2.3多项式插补用于多点间轨迹规划7.2.4抛物线过渡的线性插补7.2.5抛物线过渡的线性插补用于多点间轨迹规划7.2.4抛物线过渡的线性插补采用多项式函数插补的优点是，在区间内速度和加速度永远是连续的，五次多项式甚至可以指定区间端点处加速度的值，但是区间内的速度是变化的。显然，这对于机器人的工作效率是一个很大的限制，机器人必须持续地高速运动，才能获得更高的工作效率。所以可以尝试线性插补。比如让关节在3秒内以恒定的速度从运转到，则其轨迹是一条直线，如图所示。7.2.4抛物线过渡的线性插补但这里有个明显的问题。由于起始点要求速度从0瞬间上升到20，理论上加速度无穷大，势必造成很大的冲击，在终止点也有类似的问题，这就需要对线性插补函数做一些改进。7.2.4抛物线过渡的线性插补为了生成一条位置和速度都连续光滑的运动轨迹，在使用线性函数进行插补时，需要在启动和停止时各增加一段抛物线（二次多项式）作为过渡，从而平滑地改变速度。直线函数和两个抛物线函数组合成一条光滑轨迹在起始点和终止点，速度是渐变的，加速度也稳定在可接受范围之内7.2.4抛物线过渡的线性插补构造抛物线过渡的直线插补函数的思路与多项式的类似，首先写出抛物线和直线的标准表达式，然后再根据约束条件确定其中的系数。为构造这样的插补函数需要增加一个约束条件，即两段抛物线关于中点（th,θh）中心对称，这意味两段抛物线具有相同的持续时间且加速度大小相等符号相反。7.2.4抛物线过渡的线性插补首先写出首段抛物线方程，即二次多项式的标准表达式求导可得其速度方程由于抛物线在起始点的位置为θ0，速度为0，所以有其速度方程为根据两段抛物线的对称性可知，第二段抛物线方程为其速度方程为7.2.4抛物线过渡的线性插补中间部分轨迹为直线，其速度方程及位置方程分别为7.2.4抛物线过渡的线性插补7.2.4抛物线过渡的线性插补7.2.4抛物线过渡的线性插补7.2.4抛物线过渡的线性插补例7.5解：①根据式有7.2.4抛物线过渡的线性插补7.2.4抛物线过渡的线性插补例7.5第二段抛物线方程第一段抛物线方程直线方程代入右式可以得到三组插补函数，每组插补函数分为三段7.2.4抛物线过渡的线性插补7.2.4抛物线过渡的线性插补思考：当过渡抛物线加速度变大时，轨迹中的直线段如何变化？过渡时间如何变化？平台速度如何变化？如果加速度无穷大，轨迹会有什么变化？7.2.4抛物线过渡的线性插补7.2.4抛物线过渡的线性插补运行NCUT7_5b看加速度变大时，线性插补轨迹的变化情况动画7.2.4抛物线过渡的线性插补7.2.4抛物线过渡的线性插补例7.5拓展练习：读者可以尝试自己编写代码求解该题。第7章工业机器人运动规划7.2关节空间的轨迹规划方法7.2.1三次多项式插补7.2.2高阶多项式插补7.2.3多项式插补用于多点间轨迹规划7.2.4抛物线过渡的线性插补7.2.5抛物线过渡的线性插补用于多点间轨迹规划7.2.5抛物线过渡的线性插补用于多点间轨迹规划在7.2.3中，我们将多项式插补用于多点间轨迹规划，本节研究如何用抛物线过渡的线性插补解决多路径点的轨迹规划问题。求解多路径点的抛物线过渡的线性插补曲线的核心思想是，首先用直线连接各路径点，相邻两条直线之间用抛物线过渡。7.2.5抛物线过渡的线性插补用于多点间轨迹规划这种轨迹规划的关键在于如何确定过渡抛物线函数的各个系数。抛物线函数的标准形式为其中有三个待定系数。如果轨迹需要经过n个路径点，则需要n个路径点过渡区，加上起始点和终止点，轨迹中共需要n+2段抛物线，待定系数则有3(n+2)个。7.2.5抛物线过渡的线性插补用于多点间轨迹规划下面再看看有多少约束。首先，为保证速度连续，每个路径点都有2个约束，即起点和终点各有两个约束2n个4个以上共有约束2(n+2)个，少于待定系数3(n+2)个，因此轨迹有无穷多解。与上一节的思路类似，我们必须指定各段抛物线的加速度或者过渡时间，轨迹才会被唯一确定下来。7.2.5抛物线过渡的线性插补用于多点间轨迹规划例7.6①取过渡时间为0.8秒，请用抛物线过渡的线性函数规划出符合要求的轨迹，并绘制出轨迹的位置、速度、加速度曲线，请借助软件完成。②上一问绘制出的轨迹是否通过各路径点？如果希望轨迹尽量接近路径点，应该加大还是减小过渡时间？此时对应的加速度是增大还是减小了？%调用mstraj生成过多路径点的抛物线过渡的直线插补轨迹，插补点存在traj中