第十一讲 轨迹规划

1、山东大学机械工程学院机电工程研究所2010/09/02第五章第五章 轨迹生成轨迹生成Trajectory Generation第一节第一节 概概 述述 Introduction第二节第二节 关节空间的轨迹规划关节空间的轨迹规划Trajectory Planning in joint space第三节第三节 直角空间的轨迹规划直角空间的轨迹规划Trajectory Planning in Cartesian space山东大学机械工程学院机电工程研究所2010/09/025.1 5.1 概概 述述 机器人的任务：机器人的任务：与一般数控机床比较： 机器人控制方式：机器人控制方式： 焊接机器人：连

2、续轨迹控制。 搬运机器人：点位控制。项目关节机器人数控机床作用器末端执行器刀架作用器与控制轴的关系时变、非线性常线性控制轴之间的关系强耦合正交、独立如何使用数控机床进行加工？山东大学机械工程学院机电工程研究所2010/09/025.1.1 5.1.1 轨迹规划的任务和要求轨迹规划的任务和要求 机器人的任务：机器人的任务： 基本问题: 移动手部从起始位姿TA到达目标位姿TC。还可能经过某些点TB。为什么要进行轨迹规划？山东大学机械工程学院机电工程研究所2010/09/025.1.1 5.1.1 轨迹规划的任务和要求轨迹规划的任务和要求轨迹生成涉及轨迹的轨迹生成涉及轨迹的规划、表示及计算规划、表示

3、及计算。轨迹(Trajectory)：轨迹可分为：1、空间位置轨迹：如：直线、圆弧等 直线（无障碍时）最短。2、时空运动轨迹：要求运动平稳。轨迹规划可在任务空间或关节空间进行。空间时空我们主要研究后者。山东大学机械工程学院机电工程研究所2010/09/025.1.1 5.1.1 轨迹规划的任务和要求轨迹规划的任务和要求轨迹规划轨迹规划(Trajectory Planing(Trajectory Planing) )任务任务： 根据机器人的作业要求，构造出末端执行器或各关节运动的合理、优化的时间历程。 即如何确定经过起始点、目标点及中间点的轨迹上的其他点及运动参数。 为了机器人的运动平稳，需要对

4、运动速度、加速度进行合理安排。对轨迹的要求：光顺（Smooth）,拥有连续的一阶、二阶导数，计算量小。 可用于轨迹规划的时间函数很多，常用的有：线性函数、多项式及组合。山东大学机械工程学院机电工程研究所2010/09/025.1.1 5.1.1 轨迹规划的任务和要求轨迹规划的任务和要求轨迹规划的实现：轨迹规划的实现：自动的、交互的自动的、交互的 1、 编程，像数控编程那样。 2、关键点的示教(在工作空间进行，获得关节空间值)等。 3、机器人控制系统内部设定，像数控系统中的加、减速。 轨迹的描述要求：轨迹的描述要求： 使用者简单描述，机器人补充细节。路径点路径点(Path point)： 包括：

5、 起始点、目标点、一系列中间点。山东大学机械工程学院机电工程研究所2010/09/025.1.2 在两类空间的轨迹规划关节空间的轨迹规划： 确定各关节的位置、速度和加速度随时间变化的规律，多用于点位作业。末端执行器的轨迹间接实现。问题：在关节空间光顺，在工作空间就一定光顺吗？山东大学机械工程学院机电工程研究所2010/09/025.1.2 在两类空间的轨迹规划 在工作空间中的轨迹规划 以末端执行器在直角坐标系中的位姿、速度和加速度描述其空间运动，以此规划机器人运动的称为在直角坐标空间中规划，多用于连续作业方式。 山东大学机械工程学院机电工程研究所2010/09/025.2 在关节空间的轨迹规划

6、关节变量值的获取（有限个关键点）： 1、通过逆运动学方程。 2、通过示教。 目标：根据一系列给定的关节离散值拟合一个光滑的时间函数，光滑通过所有离散点；它描述了该关节的轨迹。其他约束要求：速度和加速度要求，各关节的同步运行要求。 关节空间轨迹规划常用多项式和带抛物线过度的线性函数作为拟合函数。起点、终点、中间点山东大学机械工程学院机电工程研究所2010/09/025.2.1 以三次多项式规划 关节空间的轨迹规划可分为起终型和通过中间点型两大类。 考虑机器人的某个关节，在开始运动时刻 t0=0时的关节值为0，在tf 时刻运动到目标值f ，又设在运动开始和到达目标时的速度均为0（起终型规划），我们

7、可以唯一地确定一个描述该关节运动轨迹的三次多项式函数：332210)(tatataat其中，约束条件为：0)(, 0)()(,)(000ffftttt（5.1）（5.2）山东大学机械工程学院机电工程研究所2010/09/025.2.1 以三次多项式规划 下面根据约束条件，确定三次多项式函数的系数。 对式(5.1)求一阶导数，得到该轨迹上的关节速度：232132)(tataat带入约束条件（5.2式），得：0320,2321133221000ffffffftatataatatataaa(5.3)(5.4)山东大学机械工程学院机电工程研究所2010/09/025.2.1 以三次多项式规划解得：)(

8、2(30,033022100fffftataaa），代入（5.1）式有：30320200)(2)(3)(tttttvtffff(5.5) 此函数以时间t为变量，给出了该关节从t0 到tf 任意时刻的轨迹（位置）。山东大学机械工程学院机电工程研究所2010/09/025.2.1 以三次多项式规划 该轨迹上，关节速度和加速度分别为：ttttffff)(12)(6)(0302 20302)(6)(6)(tttttffff山东大学机械工程学院机电工程研究所2010/09/025.2.1 以三次多项式规划相应的图像： 关节位置关节速度关节加速度山东大学机械工程学院机电工程研究所2010/09/025.2

9、.1 三次多项式规划三次多项式规划的特点： 1、位置曲线为三次曲线，速度为抛物线，加速度为直线。 2、能够保证关节位置和速度的平滑过渡，但在两端加速度不平稳、连续。山东大学机械工程学院机电工程研究所2010/09/025.2.1 三次多项式规划多段连接的情况： 为保证角度和角速度的连续，每段（下标1）结束时的关节位置和速度就是下一段（下标2）的起始条件，即。)()(),()(021021ttttff山东大学机械工程学院机电工程研究所2010/09/025.2.1 三次多项式规划例例5.1 设机器人执行一次作业任务时，某关节历时3秒从起始位置09运动到终了位置f60，该关节在起、终点的关节速度均

10、为0，试用三次多项式规划其运动。解：已知条件：09、 f60、 tf3s，代入公式(5.5)得：78. 3, 0 .17, 0, 0 . 93210aaaa所以，该关节的运动轨迹为：tttttttt68.220.34)(34.110.34)(78.30.170.9)(232 山东大学机械工程学院机电工程研究所2010/09/025.2.1 三次多项式规划 该关节位置、速度和加速度随时间变化的曲线：山东大学机械工程学院机电工程研究所2010/09/025.2.2 五次多项式规划 设给定关节在运动起点和终点的位置、速度和加速度六个参数，即已知：,)(,)(,)(,)(,)(,)(000000fff

11、ffftttttt 可以唯一地确定下列五次多项式的六个系数：相应的关节速度和加速度为：5544332210)(tatatatataat35243245342321201262)(5432)(tatataattatatataat (5.6)(5.7)(5.8)山东大学机械工程学院机电工程研究所2010/09/025.2.2 五次多项式规划 将已知式(5.6)代入式(5.7)和(5.8)，可得到系数： 5200054200043200030201002)()66()(122)23()1614()(302)3()1218()(202fffffffffffffffffftttatttatttaaaa

12、山东大学机械工程学院机电工程研究所2010/09/025.2.2 五次多项式规划五次多项式规划的特点： 1、位置曲线为五次曲线，速度为四次曲线，加速度为三次曲线。 2、能够保证关节位置、速度和加速度的平滑过渡，有利于机构平稳运行。 3、阶次高，计算复杂。山东大学机械工程学院机电工程研究所2010/09/025.2.3 带抛物线过渡的线性规划 很多应用要求运动速度恒速。 在两端不可能实现。 以抛物线作为平滑过渡，在过渡段，速度线性变化，加速度等于常数。即数控插补中的加减速处理。 特点：先加速、再匀速、 后减速运行山东大学机械工程学院机电工程研究所2010/09/025.2.3 带抛物线过渡的线性

13、规划加速段AB（0tb） ： 设加速度为 ，则：0 设运动开始(t0=0)时的已知条件为：0)( t0 对时间积分可得：01201021)()(ctcttctt . 0)(,)(000tt(5.12)(5.13)(5.11)代入式（5.12-13）得：0,100cc山东大学机械工程学院机电工程研究所2010/09/025.2.3 带抛物线过渡的线性规划得到AB段的关节轨迹：0 设加速段结束时，期望的关节运动速度为，则：bbtt0)( , 0)()(21)(00200bttttttt 山东大学机械工程学院机电工程研究所2010/09/025.2.3 带抛物线过渡的线性规划从而，加速阶段的加速度为

14、：bt0 B点的关节位移量为：bbbbttt2121)(0200 在线性段BC部分，关节以均速运动，其位移为：,)()(bfbbbttttttt)2()()(bfbbbfbbfcttttttt至C点处的总位移为：山东大学机械工程学院机电工程研究所2010/09/025.2.3 带抛物线过渡的线性规划 在减速段CD，关节做均减速运动，其加速度度为：btt0)( 关节的减速运动轨迹为：,)()()()(21)(02fbfbfbfbftttttttttttttt 山东大学机械工程学院机电工程研究所2010/09/025.2.3 带抛物线过渡的线性规划过渡段持续时间tb的确定： 由加速段与减速段位移量

15、相等，知：)(0bcf代入，可得：ffbtt0 如右图，为防止不出现线性段，甚至发生干涉，对最大关节速度应进行限制：fft)(20max（5.24）tb山东大学机械工程学院机电工程研究所2010/09/025.2.3 带抛物线过渡的线性规划例例5.25.2 设某关节在5秒钟的时间内要从起始位置015运动到终了位置f75，试用带抛物线过渡的线性轨迹规划其运动。解：要在5s内完成上述位移，关节的最小等速运动速度为： stff/12515750min 存在线性段的最大关节速度为：max=24,因此取=15进行计算。由已知条件可得：s11551575150ffbtt山东大学机械工程学院机电工程研究所2

16、010/09/025.2.3 带抛物线过渡的线性规划加速度：20/15115stb 加速段轨迹：1 , 015)(15)(5 .715)(2tttttt 等速段轨迹：4, 1)1(155.22)(ttt5 .225 . 715b减速段轨迹：5 , 415)(515)(55 .775)(2tttttt ）（）（山东大学机械工程学院机电工程研究所2010/09/025.2.3 带抛物线过渡的线性规划山东大学机械工程学院机电工程研究所2010/09/025.2.4 过中间点的轨迹规划何谓过中间点的轨迹规划？ 关节的轨迹除了经过起始点、目标点外，还必须经过指定的中间一系列点。一、过中间点的三次多项式规

17、划一、过中间点的三次多项式规划 中间点的特点：一般速度不等于零，即在中间点不停留。方法：把相邻的两个点分别看成“起点”和“终点”，两点之间构成一个运动段。山东大学机械工程学院机电工程研究所2010/09/025.2.4 过中间点的轨迹规划 路径上起始和终止点的位移和速度已知，为：332210)(tatataat由给定条件可得：fffftttt)(,)()(,)(0000设关节的三次多项式轨迹为：2321103322100032,fffffffftatataatatataaa山东大学机械工程学院机电工程研究所2010/09/025.2.4 过中间点的轨迹规划解得：)(1)(2)2(1(3,020

18、3300220100ffffffffttattaaa，）即：30203200200)(1)(2)2(1)(3)(ttttttttffffffff 此轨迹确定了该关节从t0 到tf 任意时刻的关节位置。山东大学机械工程学院机电工程研究所2010/09/021、由末端执行器在笛卡尔空间的瞬时速度和角速度确定每个中间点上的关节速度。 使用雅克比逆矩阵：5.2.4 过中间点的轨迹规划关节速度 的获取： ）、（即）、（即ffvv00vJ12、采用启发式和近似算法： 如：速度反向的点，取速度为零值，速度同向的点，速度为两段速度的平均。反向同向山东大学机械工程学院机电工程研究所2010/09/025.2.4

19、 过中间点的轨迹规划 3、以过中间点时加速度连续为原则，决定中间点速度。山东大学机械工程学院机电工程研究所2010/09/025.2.4 过中间点的轨迹规划二、过中间点的带抛物线过渡的规划二、过中间点的带抛物线过渡的规划 如图所示，关节经过一组中间点，线性函数连接各中间点，为了运动平稳，在每个中间点的两侧增加抛物线过度。山东大学机械工程学院机电工程研究所2010/09/02 设给定所有的路径点 、所需时 间 及加速度大小 。5.2.4 过中间点的轨迹规划 考虑三个相邻的轨迹点i,j和k，设k点的过度期间为tk,中间点j与k之间线性部分的区间为tjk,连接j和k点的总时间段为tdjk,线性部分的

20、速度为 ，过度区间的加速度为 。jkdjktj k k山东大学机械工程学院机电工程研究所2010/09/025.2.4 过中间点的轨迹规划 先考虑中间的三个点，根据速度、加速度定义，有：)(21kjdjkjkkkljkkttttt kjkklkdjkkjjkt )sgn( 从图中看出，增加过渡段后，实际轨迹将不通过给定的中间点。山东大学机械工程学院机电工程研究所2010/09/025.2.4 过中间点的轨迹规划 对第一段，为求t1，列出线性段的速度方程：11121121221tttd 可解得：1122121211121)(2)sgn( ddttt21121211212122121ttttttd

21、d山东大学机械工程学院机电工程研究所2010/09/025.2.4 过中间点的轨迹规划 同样，对最后一段，过渡段的持续时间tn为：nnnnndnnttt 21)1(1可解得：nnnnndnndnnnnnttt )(2)sgn(12) 1() 1(11)1()1()1(1)1(2121nnnndnnnnndnnnntttttt山东大学机械工程学院机电工程研究所2010/09/025.2.4 过中间点的轨迹规划小结： 1、时间或速度分为：点处和点间两类。 2、计算从第一点开始，按顺序计算段加速度、点时间、段间速度及持续时间。 上述推导中，为了使轨迹通过轨迹的起始和终止位置，在两端点处我们重复放置了

22、坐标值。 上面我们介绍了多段轨迹的过渡时间以及关节速度的计算；通常，用户仅提供关节位置和每段的时间，而机器人控制系统自动地采用默认的加速度值。加速度值越大，过渡到线性段的时间就越短。山东大学机械工程学院机电工程研究所2010/09/025.2.4 过中间点的轨迹规划例：某个关节的轨迹如下：关节位置（度）：10、35、25 、10每段时间间隔(秒)：2、1、3过渡段默认加速度的大小为50度/秒2，要求计算各段的速度、过渡时间和线性时间。山东大学机械工程学院机电工程研究所2010/09/025.2.4 过中间点的轨迹规划解：根据题意，第一点的加速度，0 .501 起始点处的过渡时间：27. 050

23、)1035(2421t速度：5 .1327. 05 . 021035120 .101352523第二点为减速度点，0 .502 过渡时间：47. 00 .5050.130 .102t第一段的线性部分时间：50. 147. 05 . 027. 0212t山东大学机械工程学院机电工程研究所2010/09/025.2.4 过中间点的轨迹规划接下来计算末尾点，其加速度为：0 .504 末尾的过渡时间：102. 050)2510( 2934t速度：10. 5050. 03251034第三点为减速度点，0 .503 过渡时间：098. 00 .500 .101 . 53）（t最后：716. 0098. 0

24、5 . 047. 05 . 0123t849. 2012. 0098. 05 . 0334t 运行时，轨迹发生器计算关节在采样时刻的位置、速度和加速度。山东大学机械工程学院机电工程研究所2010/09/025.2.4 过中间点的轨迹规划 在以上规划中，关节位置并不精确地通过各个中间点。加速度取的越大，轨迹离中间点越近。 为了让轨迹通过中间点，我们在中间点的两端增设两个“伪节点”，使中间点位于两伪节点间的直线段上，如图所示。山东大学机械工程学院机电工程研究所2010/09/025.3 5.3 在直角空间的轨迹规划在直角空间的轨迹规划 由多个关节的轨迹间接地精确获得的直角空间特定轨迹是相当困难的。

25、 许多作业任务要求末端执行器沿着特定的轨迹（如：直线、圆弧）精确运动，这时应当在直角空间进行轨迹规划，即将描述位姿的直角坐标变量表示成时间的函数。 末端执行器的位姿有两部分构成：位置和姿态；表示位置的三个分量可以分别用三个时间的线性函数表示；通常姿态由旋转矩阵表示，它的各列之间必须相互正交，这为姿态的规划带来不便。山东大学机械工程学院机电工程研究所2010/09/025.3 在直角空间的轨迹规划在直角空间的轨迹规划 姿态也可以用轴角法表示，轴角的三个分量相互独立，为轨迹规划带来方便。 位置的三个分量与轴角的三个分量组成表示末端执行器位姿的6X1的向量阵，如果对每个分量选择适当的线性函数，末端执

26、行器就能够从一个位姿平滑地变化到下一个位姿。具体的实现方法与关节空间的相识，不再介绍。 在运行时，直角空间轨迹上的点还必须由逆运动学方程计算出相应的关节角，这是一项非常费时的任务。 山东大学机械工程学院机电工程研究所2010/09/025.4 运行时的轨迹生成 得到轨迹函数后，当机器人运行时，内部的轨迹发生器子程序以一定的时间周期计算某时刻的轨迹状态：位置、速度及加速度，并将结果传送给机器人控制系统。 如果轨迹由多段函数组成，则计算完一段后，t归零，在计算下一段。 一、关节轨迹的生成 考虑带有抛物线过度的线性规划的情况，在每个采样点，需检查时间t处于线性段还是过度段？山东大学机械工程学院机电工程研究所2010/09/025.4 运行时的轨迹生成1、线性段：每个关节的轨迹值由前面得到的轨迹规划公式递推计算：设t为插补周期。有： 0, ttttjjk2、过度段：221)()21(