工业机器人轨迹规划

直线长度：
ts间隔内行程d =vts；
插补总步数N为L/d+1的整数部分；
各轴增量：
各插补点坐标值
xi x0 ix

yi

y0

iy
图6.4
zi z0 iz 式中的i=l，2，…N
6.3.2 圆弧插补(MOVC) 一、平面圆弧插补
平面圆弧是指圆弧平面与基坐标系的三大平面之一重合，以 XOY平面圆弧为例。已知不在一条直线上的三点P1、P2、P3及这三 点对应的机器人手端的姿态，如图6.5及图6.6所示。
(3) 规划机器人的运动轨迹时，需明确其路径上是否存在障碍约 束的组合。
本章主要讨论连续路径的无障碍轨迹规划方法。
6.2 插补方式分类与轨迹控制
6.2.1 插补方式分类 点位控制(PTP控制)通常没有路径约束，多以关节坐标运
动表示。点位控制只要求满足起终点位姿，在轨迹中间只有 关节的几何限制、最大速度和加速度约束；为了保证运动的 连续性，要求速度连续，各轴协调。如MOVJ
建立圆弧平面插补坐标系，即把ORXRYRZR坐标系原点与圆心OR重 合，设ORXRYRZR平面为圆弧所在平面，且保持ZR为外法线方向。这样，
一个三维问题就转化成平面问题，可以应用平面圆弧插补的结论。
求解两坐标系(图6.7)的转换矩阵。令TR表示由圆弧坐标ORXRYRZR至 基础坐标系OX0Y0Z0的转换矩阵。用TR可以将圆弧坐标下的点转换成
解：
位移曲线 (t) 30 5.4t2 0.72t3
80 60 40 20 0 -20
0
位置
加速度
速度
1
2
3
4
5
图 关节位移、速度和加速度
6秒
二、过路径点的三次多项式插值
若所规划的机器人作业路径在多个点上有位姿要求，如图6.9所 示，机器人作业除在A、D点有位姿要求外，在路径点B、C也有位 姿要求。对于这种情况，假如末端执行器在路径点停留，即各路径 点上速度为0，则轨迹规划可连续直接使用前面介绍的三次多项式插 值方法；但若末端执行器只是经过，并不停留，就需要将前述方法 推广。
基础坐标系下的点。
6.3.3 定时插补与定距插补 由上述可知，机器人实现一个空间轨迹的过程即是实现轨
迹离散的过程，如果这些离散点间隔很大，则机器人运动轨迹 与要求轨迹可能有较大误差。只有这些插补得到的离散点彼此 距离很近，才有可能使机器人轨迹以足够的精确度逼近要求的 轨迹。模拟CP控制实际上是多次执行插补点的PTP控制，插补 点越密集，越能逼近要求的轨迹曲线。
机器人运动轨迹的描述一般是对其手部位姿的描述，此位姿值可与关 节变量相互转换。控制轨迹也就是按时间控制手部或工具中心走过的空间 路径。
在轨迹规划中，为叙述方便，也常用点来表示机器人的状 态，或用它来表示工具坐标系的位姿。
更详细地描述运动时不仅要规定机器人的起始点和终止点， 而且要给出介于起始点和终止点之间的中间点，也称路径点。 这时，运动轨迹除了位姿约束外，还存在着各路径点之间的时 间分配问题。
当要求以更高的精度实现运动轨迹时，可采用定距插补。
二、定距插补
v是要求的运动速度，它不能变化，如果要两插补点的距 离Pi Pi+1恒为一个足够小的值，以保证轨迹精度，ts就要变化。 也就是在此方式下，插补点距离不变，但ts要随着不同工作速 度v的变化而变化。
这两种插补方式的基本算法相同，只是前者固定ts，易于 实现，后者保证轨迹插补精度，但ts要随之变化，实现起来比 前者困难。
对于机器人作业路径上的所有 路径点可以用求解逆运动学的方法 先得到多组对应的关节空间路径点， 进行轨迹规划时，把每个关节上相 邻的两个路径点分别看做起始点和 终止点，再确定相应的三次多项式 插值函数，把路径点平滑连接起来。 一般情况下，这些起始点和终止点 的关节运动速度不再为零。
图6.9 机器人作业路径点
6.3.1 直线插补(MOVL)
直线插补和圆弧插补是机器人系统中的基本插补算法。
空间直线插补是在已知该直线始末两点的位置和姿态的条件下， 求各轨迹中间点(插补点)的位置和姿态。下面介绍位置插补原理，如图 6.4所示。已知直线始末两点的坐标值P0(X0，Y0，Z0)、Pe(Xe，Ye，Ze) 及姿态。这些已知的位置和姿态通常是通过示教方式得到的。设v为要 求的沿直线运动的速度；ts为插补时间间隔。
机器人轨迹规划
6.1 机器人轨迹规划概述
6.1.1 机器人轨迹的概念 机器人轨迹泛指工业机器人在运动过程中的运动轨迹，即运动点的位
移、速度和加速度。 机器人在作业空间要完成给定的任务，其手部运动必须按一定的轨迹
进行。轨迹的生成一般是先给定轨迹上的若干个点，将其经运动学反解映 射到关节空间，对关节空间中的相应点建立运动方程，然后按这些运动方 程对关节进行插值，从而实现作业空间的运动要求，这一过程通常称为轨 迹规划。本章仅讨论在关节空间或笛卡尔空间中工业机器人运动的轨迹规 划和轨迹生成方法。
N = /θ + 1
对Pi+1 点的坐标，有
式中：Xi=R cosθi；Yi=Rsinθi。
同理有
由θi+1=θi +θ可判断是否到插补终点。若θi+1，则继续
插补下去；当 θi+1> 时，则修正最后一步的步长θ，并以
表示，
，故平面圆弧位置插补为
二、空间圆弧插补(MOVC) 空间圆弧是指三维空间任一平面内的圆弧，此为空间一般平面的
连续轨迹控制(CP控制)有路径约束，因此要对路径进行 设计，如MOVL。
6.2.2 机器人轨迹控制过程 机器人的基本操作方式是示教-再现。对于有规律的轨迹，
仅示教几个特征点，计算机就能利用插补算法获得中间点的坐 标，如直线需要示教两点，圆弧需要示教三点，通过机器人逆 向运动学算法由这些点的坐标求出机器人各关节的位置和角度
插补点要多么密集才能保证轨迹不失真和运动连续平滑呢？ 可采用定时插补和定距插补方法来解决。
一、定时插补
从轨迹控制过程知道，每插补出一轨迹点的坐标值，就要转换 成相应的关节角度值并加到位置伺服系统以实现这个位置，这个过
程每隔一个时间间隔ts完成一次。为保证运动的平稳，显然ts不能
太长。
由于关节型机器人的机械结构大多属于开链式，刚度不高，ts 一般不超过25 ms(40 Hz)，这样就产生了ts的上限值。当然ts越小
(1, …, n)，然后由后面的角位置闭环控制系统实现要求的轨迹
上的一点。继续插补并重复上述过程，从而实现要求的轨迹。 机器人轨迹控制过程如图6.3所示。
图6.3 机器人轨迹控制过程
6.3 机器人轨迹插值计算
给出各个路径结点后，轨迹规划的任务包含解 变换方程，进行运动学反解和插值计算。在关节空 间进行规划时，需进行的大量工作是对关节变量的 插值计算。
机器人的运动应当平稳，不平稳的运动将加剧机械部件的 磨损，并导致机器人的振动和冲击。为此，要求所选择的运动 轨迹描述函数必须连续，而且它的一阶导数(速度)，有时甚至 二阶导数(加速度)也应该连续。
轨迹规划既可以在关节空间中进行，也可以在直角坐标空 间中进行。在关节空间中进行轨迹规划是指将所有关节变量表 示为时间的函数，用这些关节函数及其一阶、二阶导数描述机 器人预期的运动；在直角坐标空间中进行轨迹规划是指将手爪 位姿、速度和加速度表示为时间的函数，而相应的关节位置、 速度和加速度由手爪信息通过逆运动学导出。
图6.5 由已知的三点P1、P2、P3 决定的圆弧
图6.6 圆弧插补
设v为沿圆弧运动速度；ts为插补时时间隔。类似直线插补情况计 算出： (1) 由P1、P2、P3决定的圆弧半径R。
(2) 总的圆心角=1+2，即
(3) ts时间内角位移量θ=tsv/R，据图6.4所示的几何关系求各插
补点坐标。 (4) 总插补步数(取整数)
6.3.4 关节空间插补(MOVJ)
在关节空间中进行轨迹规划，需要给定机器人在起始点和 终止点手臂的位形。对关节进行插值时应满足一系列的约束条 件，例如抓取物体时手部的运动方向(初始点)、提升物体离开 的方向(提升点)、放下物体(下放点)和停止点等结点上的位姿、 速度和加速度的要求；与此相应的各个关节位移、速度、加速 度在整个时间间隔内的连续性要求以及其极值必须在各个关节 变量的容许范围之内等。满足所要求的约束条件之后，可以选 取不同类型的关节插值函数，生成不同的轨迹。常用的关节空 间插补有以下方法：
t

6
t
3 f
f 0
t2

t


6
t
2 f
f 0

12
t
3 f
f 0
t
三次多项式插值的关节运动轨迹曲线如图6.8所示。由图可知， 其速度曲线为抛物线，相应的加速度曲线为直线。
图6.8 三次多项式插值的关节运动轨迹
【例6.1】 要求一个六轴机器人的第一关节在5秒钟内从初始角300运 动到终端角750，且起始点和终止点速度均为零。用三次多项式规划该 关节的运动，并计算在第1、2、3秒和第4秒时关节的角度。
1、三次多项式插值
2、过路径点的三次多项式插值
3、高阶多项式插值
4、用抛物线过渡的线性插值
一、三次多项式插值
在机器人运动过程中，若末端执行器的起始和终止位姿已知，由 逆向运动学即可求出对应于两位姿的各个关节角度。末端执行器实现 两位姿的运动轨迹描述可在关节空间中用通过起始点和终止点关节角
的一个平滑轨迹函数 (t)来表示。
设机器人需要的运动轨迹为直线，运动速度为v(mm/s)， 时间间隔为ts (ms)，则每个ts间隔内机器人应走过的距离为
Pi Pi+1= v ts 可见两个插补点之间的距离正比于要求的运动速度，两点之间 的轨迹不受控制，只有插补点之间的距离足够小，才能满足一 定的轨迹精度要求。