第十一讲 轨迹规划

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

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

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

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

5、述要求： 使用者简单描述，机器人补充细节。 路径点路径点(Path point)： 包括： 起始点、目标点、一系列中间点。 山东大学机械工程学院机电工程研究所2010/09/02 5.1.2 在两类空间的轨迹规划 关节空间的轨迹规划： 确定各关节的位置、速度和加速度随 时间变化的规律，多用于点位作业。 末端执行器的轨迹间接实现。 问题：在关节空间光顺，在工作空间就一 定光顺吗？ 山东大学机械工程学院机电工程研究所2010/09/02 5.1.2 在两类空间的轨迹规划 在工作空间中的轨迹规划 以末端执行器在直角坐标系中的位姿、 速度和加速度描述其空间运动，以此规划 机器人运动的称为在直角坐标空间

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

7、某个关节，在开始运动时 刻 t0=0时的关节值为0，在tf 时刻运动到目 标值f ，又设在运动开始和到达目标时的 速度均为0（起终型规划），我们可以唯一 地确定一个描述该关节运动轨迹的三次多项 式函数： 3 3 2 210 )(tatataat 其中，约束条件为： 0)(, 0)( )(,)( 0 00 f ff tt tt （5.1） （5.2） 山东大学机械工程学院机电工程研究所2010/09/02 5.2.1 以三次多项式规划 下面根据约束条件，确定三次多项式 函数的系数。 对式(5.1)求一阶导数，得到该轨迹 上的关节速度： 2 321 32)(tataat 带入约束条件（5.2式），

8、得： 032 0 , 2 321 1 3 3 2 210 00 fff ffff tatata a tatataa a (5.3) (5.4) 山东大学机械工程学院机电工程研究所2010/09/02 5.2.1 以三次多项式规划 解得： )( 2 ( 3 0, 0 3 30 2 2 100 f f f f t a t a aa ）， 代入（5.1）式有： 3 0 3 2 0 2 00 )( 2 )( 3 )(t t t t tvt f f f f (5.5) 此函数以时间t为变量，给出了该关节 从t0 到tf 任意时刻的轨迹（位置）。 山东大学机械工程学院机电工程研究所2010/09/02 5

9、.2.1 以三次多项式规划 该轨迹上，关节速度和加速度分别为： t tt t f f f f )( 12 )( 6 )( 0 3 0 2 2 0 3 0 2 )( 6 )( 6 )(t t t t t f f f f 山东大学机械工程学院机电工程研究所2010/09/02 5.2.1 以三次多项式规划 相应的图像： 关节位置 关节速度 关节加速度 山东大学机械工程学院机电工程研究所2010/09/02 5.2.1 三次多项式规划 三次多项式规划的特点： 1、位置曲线为三次曲线，速度为抛 物线，加速度为直线。 2、能够保证关节位置和速度的平滑 过渡，但在两端加速度不平稳、连续。 山东大学机械工程

10、学院机电工程研究所2010/09/02 5.2.1 三次多项式规划 多段连接的情况： 为保证角度和角速度的连续，每段 （下标1）结束时的关节位置和速度就是下 一段（下标2）的起始条件，即。 )()(),()( 021021 tttt ff 山东大学机械工程学院机电工程研究所2010/09/02 5.2.1 三次多项式规划 例例5.1 设机器人执行一次作业任务时，某 关节历时3秒从起始位置09运动到终了 位置f60，该关节在起、终点的关节速度 均为0，试用三次多项式规划其运动。 解：已知条件：09、 f60、 tf3s， 代入公式(5.5)得： 78. 3, 0 .17, 0, 0 . 9 32

11、10 aaaa 所以，该关节的运动轨迹为： tt ttt ttt 68.220.34)( 34.110.34)( 78.30.170.9)( 2 32 山东大学机械工程学院机电工程研究所2010/09/02 5.2.1 三次多项式规划 该关节位置、速度和加速度随时间变 化的曲线： 山东大学机械工程学院机电工程研究所2010/09/02 5.2.2 五次多项式规划 设给定关节在运动起点和终点的位置、 速度和加速度六个参数，即已知： ,)(,)( ,)(,)( ,)(,)( 00 00 00 ff ff ff tt tt tt 可以唯一地确定下列五次多项式的六 个系数： 相应的关节速度和加速度为：

12、 5 5 4 4 3 3 2 210 )(tatatatataat 3 5 2 432 4 5 3 4 2 321 201262)( 5432)( tatataat tatatataat (5.6) (5.7) (5.8) 山东大学机械工程学院机电工程研究所2010/09/02 5.2.2 五次多项式规划 将已知式(5.6)代入式(5.7)和(5.8)， 可得到系数： 5 2 000 5 4 2 000 4 3 2 000 3 0 2 01 00 2 )()66()(12 2 )23()1614()(30 2 )3()1218()(20 2 f fffff f fffff f fffff t

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

14、程学院机电工程研究所2010/09/02 5.2.3 带抛物线过渡的线性规划 加速段AB（0tb） ： 设加速度为 ，则： 0 设运动开始(t0=0)时的 已知条件为： 0 )( t 0 对时间积分可得： 01 2 0 10 2 1 )( )( ctctt ctt . 0)(,)( 000 tt (5.12) (5.13) (5.11) 代入式（5.12-13）得： 0, 100 cc 山东大学机械工程学院机电工程研究所2010/09/02 5.2.3 带抛物线过渡的线性规划 得到AB段的关节轨迹： 0 设加速段结束时，期望 的关节运动速度为，则： bb tt 0 )( , 0 )( )( 2

15、 1 )( 0 0 2 00 b tt t tt tt 山东大学机械工程学院机电工程研究所2010/09/02 5.2.3 带抛物线过渡的线性规划 从而，加速阶段的加速度为： b t 0 B点的关节位移量为： bbbb ttt 2 1 2 1 )( 0 2 00 在线性段BC部分，关节以均速运动， 其位移为： ,)()( bfbbb ttttttt )2( )()( bfb bbfbbfc tt ttttt 至C点处的总位移为： 山东大学机械工程学院机电工程研究所2010/09/02 5.2.3 带抛物线过渡的线性规划 在减速段CD，关节做均减速运动，其 加速度度为： b t t 0 )( 关

16、节的减速运动轨迹为： , )( )()( )( 2 1 )( 0 2 fbf b f b f b f tttt t t tt t t tt t t 山东大学机械工程学院机电工程研究所2010/09/02 5.2.3 带抛物线过渡的线性规划 过渡段持续时间tb的确定： 由加速段与减速段位移量相等，知： )( 0 bcf 代入，可得： ff b t t 0 如右图，为防止不出现 线性段，甚至发生干涉，对 最大关节速度应进行限制： f f t )(2 0 max （5.24） tb 山东大学机械工程学院机电工程研究所2010/09/02 5.2.3 带抛物线过渡的线性规划 例例5.25.2 设某关节

17、在5秒钟的时间内要从起始 位置015运动到终了位置f75，试用 带抛物线过渡的线性轨迹规划其运动。 解：要在5s内完成上述位移，关节的最小等 速运动速度为： s t f f /12 5 1575 0 min 存在线性段的最大关节速度为：max=24, 因此取=15进行计算。由已知条件可得： s1 15 5157515 0 ff b t t 山东大学机械工程学院机电工程研究所2010/09/02 5.2.3 带抛物线过渡的线性规划 加速度： 2 0 /15 1 15 s tb 加速段轨迹： 1 , 0 15)( 15)( 5 .715)( 2 t t tt tt 等速段轨迹： 4, 1)1(15

18、5.22)(ttt 5 .225 . 715 b 减速段轨迹： 5 , 4 15)( 515)( 55 .775)( 2 t t tt tt ）（ ）（ 山东大学机械工程学院机电工程研究所2010/09/02 5.2.3 带抛物线过渡的线性规划 山东大学机械工程学院机电工程研究所2010/09/02 5.2.4 过中间点的轨迹规划 何谓过中间点的轨迹规划？ 关节的轨迹除了经过起始点、目标点外， 还必须经过指定的中间一系列点。 一、过中间点的三次多项式规划一、过中间点的三次多项式规划 中间点的特点：一般速度不等于零，即 在中间点不停留。 方法：把相邻的两个点分别 看成“起点”和“终点”， 两点之

19、间构成一个运动段。 山东大学机械工程学院机电工程研究所2010/09/02 5.2.4 过中间点的轨迹规划 路径上起始和终止点的位移和速度已知， 为： 3 3 2 210 )(tatataat 由给定条件可得： ff ff tt tt )(,)( )(,)( 00 00 设关节的三次多项式轨迹为： 2 321 10 3 3 2 210 00 32 , ffff ffff tatata a tatataa a 山东大学机械工程学院机电工程研究所2010/09/02 5.2.4 过中间点的轨迹规划 解得： )( 1 )( 2 )2( 1 ( 3 , 0 2 0 3 300 2 2 0100 f f

20、 f f f f f f tt a tt a aa ，） 即： 3 0 2 0 3 2 00 2 00 )( 1 )( 2 )2( 1 )( 3 )( t tt t tt tt f f f f f f f f 此轨迹确定了该关节从t0 到tf 任意时 刻的关节位置。 山东大学机械工程学院机电工程研究所2010/09/02 1、由末端执行器在笛卡尔空间的瞬时速度 和角速度确定每个中间点上的关节速度。 使用雅克比逆矩阵： 5.2.4 过中间点的轨迹规划 关节速度 的获取： ）、（即）、（即 ff vv0 0 v J 1 2、采用启发式和近似算法： 如：速度反向的点，取 速度为零值，速度同向的点，

21、速度为两段速度的平均。 反向 同向 山东大学机械工程学院机电工程研究所2010/09/02 5.2.4 过中间点的轨迹规划 3、以过中间点时加速度连续为原则， 决定中间点速度。 山东大学机械工程学院机电工程研究所2010/09/02 5.2.4 过中间点的轨迹规划 二、过中间点的带抛物线过渡的规划二、过中间点的带抛物线过渡的规划 如图所示，关节经过一组中间点，线性函 数连接各中间点，为了运动平稳，在每个中 间点的两侧增加抛物线过度。 山东大学机械工程学院机电工程研究所2010/09/02 设给定所有的 路径点 、所需时 间 及加速度大 小 。 5.2.4 过中间点的轨迹规划 考虑三个相邻的轨迹

22、点i,j和k，设k点 的过度期间为tk,中间点j与k之间线性部分 的区间为tjk,连接j和k点的总时间段为tdjk, 线性部分的速度为 ，过度区间的加速度 为 。 jk djk t j k k 山东大学机械工程学院机电工程研究所2010/09/02 5.2.4 过中间点的轨迹规划 先考虑中间的三个点，根据速度、加速度定 义，有： )( 2 1 kjdjkjk k kljk k tttt t kjkklk djk kj jk t )sgn( 从图中看出，增加过渡段后，实际轨 迹将不通过给定的中间点。 山东大学机械工程学院机电工程研究所2010/09/02 5.2.4 过中间点的轨迹规划 对第一段

23、，为求t1，列出线性段的速度 方程： 1112 112 12 2 1 t ttd 可解得： 1 12 2 12121 1121 )(2 )sgn( dd ttt 211212 112 12 12 2 1 2 1 tttt tt d d 山东大学机械工程学院机电工程研究所2010/09/02 5.2.4 过中间点的轨迹规划 同样，对最后一段，过渡段的持续时间 tn为： nn nnnd nn t tt 2 1 )1( 1 可解得： n nn nndnndn nnnn ttt )(2 )sgn( 1 2 ) 1() 1( 1 1)1()1( )1( 1 )1( 2 1 2 1 nnnndnn nnn

24、d nn nn tttt tt 山东大学机械工程学院机电工程研究所2010/09/02 5.2.4 过中间点的轨迹规划 小结： 1、时间或速度分为：点处和点间两类。 2、计算从第一点开始，按顺序计算段加 速度、点时间、段间速度及持续时间。 上述推导中，为了使轨迹通过轨迹的起 始和终止位置，在两端点处我们重复放置了 坐标值。 上面我们介绍了多段轨迹的过渡时间以 及关节速度的计算；通常，用户仅提供关节 位置和每段的时间，而机器人控制系统自动 地采用默认的加速度值。加速度值越大，过 渡到线性段的时间就越短。 山东大学机械工程学院机电工程研究所2010/09/02 5.2.4 过中间点的轨迹规划 例：

25、某个关节的轨迹如下： 关节位置（度）：10、35、25 、10 每段时间间隔(秒)：2、1、3 过渡段默认加速度的大小为50度/秒2，要 求计算各段的速度、过渡时间和线性时间。 山东大学机械工程学院机电工程研究所2010/09/02 5.2.4 过中间点的轨迹规划 解：根据题意，第一点的加速度，0 .50 1 起始点处的过渡时间： 27. 0 50 )1035(2 42 1 t 速度： 5 .13 27. 05 . 02 1035 12 0 .10 1 3525 23 第二点为减速度点，0 .50 2 过渡时间：47. 0 0 .50 50.130 .10 2 t 第一段的线性部分时间： 50

26、. 147. 05 . 027. 02 12 t 山东大学机械工程学院机电工程研究所2010/09/02 5.2.4 过中间点的轨迹规划 接下来计算末尾点，其加速度为：0 .50 4 末尾的过渡时间：102. 0 50 )2510( 2 93 4 t 速度： 10. 5 050. 03 2510 34 第三点为减速度点，0 .50 3 过渡时间： 098. 0 0 .50 0 .101 . 5 3 ）（ t 最后： 716. 0098. 05 . 047. 05 . 01 23 t 849. 2012. 0098. 05 . 03 34 t 运行时，轨迹发生器计算关节在采样 时刻的位置、速度和

27、加速度。 山东大学机械工程学院机电工程研究所2010/09/02 5.2.4 过中间点的轨迹规划 在以上规划中，关节位置并不精确地 通过各个中间点。加速度取的越大，轨迹 离中间点越近。 为了让轨迹通过中 间点，我们在中间点 的两端增设两个“伪 节点”，使中间点位 于两伪节点间的直线 段上，如图所示。 山东大学机械工程学院机电工程研究所2010/09/02 5.3 5.3 在直角空间的轨迹规划在直角空间的轨迹规划 由多个关节的轨迹间接地精确获得的直 角空间特定轨迹是相当困难的。 许多作业任务要求末端执行器沿着特定 的轨迹（如：直线、圆弧）精确运动，这时 应当在直角空间进行轨迹规划，即将描述位 姿

28、的直角坐标变量表示成时间的函数。 末端执行器的位姿有两部分构成：位置 和姿态；表示位置的三个分量可以分别用三 个时间的线性函数表示；通常姿态由旋转矩 阵表示，它的各列之间必须相互正交，这为 姿态的规划带来不便。 山东大学机械工程学院机电工程研究所2010/09/02 5.3 在直角空间的轨迹规划在直角空间的轨迹规划 姿态也可以用轴角法表示，轴角的三 个分量相互独立，为轨迹规划带来方便。 位置的三个分量与轴角的三个分量组 成表示末端执行器位姿的6X1的向量阵， 如果对每个分量选择适当的线性函数，末 端执行器就能够从一个位姿平滑地变化到 下一个位姿。具体的实现方法与关节空间 的相识，不再介绍。 在

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