第3章轨迹规划
合集下载
机器人运动轨迹规划
二自由度机器人关节空间的非归一化运动设机器人手臂两个关节的运动用有关公共因子做归一化处理使手臂运动范围较小的关节运动成比例的减慢这样两个关节就能够同步开始和结束运动即两个关节以不同速度一起连续运动速率分别为4s和10s
第2章 工业机器人运动学和动力学
第5讲 讲 机器人运动轨迹规划 机器人运动轨迹规划
第2章 工业机器人运动学和动力学 在规划中,不仅要规定机器人的起始点和终止点, 而且要 给出中间点(路径点)的位姿及路径点之间的时间分配, 即给出 两个路径点之间的运动时间。 轨迹规划既可在关节空间中进行, 即将所有的关节变量表 示为时间的函数,用其一阶、二阶导数描述机器人的预期动作, 也可在直角坐标空间中进行,即将手部位姿参数表示为时间的 函数, 而相应的关节位置、 速度和加速度由手部信息导出。
θ (t ) = c0 + c1t + c2t 2 + c3t 3
(3.67)
第2章 工业机器人运动学和动力学 这里初始和末端条件是:
θ (ti ) = θ i θ (t ) = θ f f & θ (ti ) = 0 θ (t ) = 0 & f
对式(3.67)求一阶导数得到:
第2章 工业机器人运动学和动力学
二自由度机器人关节空间的归一化运动
第2章 工业机器人运动学和动力学 如果希望机器人的手部可以沿AB这条直线运动, 最简单的 方法是将该直线等分为几部分(图3.21中分成5份), 然后计算出各 个点所需的形位角α和β的值, 这一过程称为两点间的插值。 可 以看出,这时路径是一条直线, 而形位角变化并不均匀。很显然, 如果路径点过少, 将不能保证机器人在每一小段内的严格直线轨 迹, 因此,为获得良好的沿循精度, 应对路径进行更加细致的分割。 由于对机器人轨迹的所有运动段的计算均基于直角坐标系, 因此 该法属直角坐标空间的轨迹规划。
第2章 工业机器人运动学和动力学
第5讲 讲 机器人运动轨迹规划 机器人运动轨迹规划
第2章 工业机器人运动学和动力学 在规划中,不仅要规定机器人的起始点和终止点, 而且要 给出中间点(路径点)的位姿及路径点之间的时间分配, 即给出 两个路径点之间的运动时间。 轨迹规划既可在关节空间中进行, 即将所有的关节变量表 示为时间的函数,用其一阶、二阶导数描述机器人的预期动作, 也可在直角坐标空间中进行,即将手部位姿参数表示为时间的 函数, 而相应的关节位置、 速度和加速度由手部信息导出。
θ (t ) = c0 + c1t + c2t 2 + c3t 3
(3.67)
第2章 工业机器人运动学和动力学 这里初始和末端条件是:
θ (ti ) = θ i θ (t ) = θ f f & θ (ti ) = 0 θ (t ) = 0 & f
对式(3.67)求一阶导数得到:
第2章 工业机器人运动学和动力学
二自由度机器人关节空间的归一化运动
第2章 工业机器人运动学和动力学 如果希望机器人的手部可以沿AB这条直线运动, 最简单的 方法是将该直线等分为几部分(图3.21中分成5份), 然后计算出各 个点所需的形位角α和β的值, 这一过程称为两点间的插值。 可 以看出,这时路径是一条直线, 而形位角变化并不均匀。很显然, 如果路径点过少, 将不能保证机器人在每一小段内的严格直线轨 迹, 因此,为获得良好的沿循精度, 应对路径进行更加细致的分割。 由于对机器人轨迹的所有运动段的计算均基于直角坐标系, 因此 该法属直角坐标空间的轨迹规划。
工业机器人位姿描述资料
齐次变换法 矢量法
位姿描述
旋量法 四元数法
5
上海电机学院
位姿描述——点的位置描述
1.点的位置描述{位置矢量}
对于直角坐标系{A}，空间任一点P的位置可用3×1的列矢量 表示。
px
A
P
p
y
pz
AP的上标A代表参考坐标系｛A｝。
6
上海电机学院
位姿描述——姿态的描述(旋转矩阵)
姿态可由某个固连于此物体的坐标系描述。 BAR [AxB ,AyB, A zB ]
nx ox ax
A B
R
ny
oy
a
y
nz oz az
旋转矩阵
7
上海电机学院
位姿描述——姿态的描述(旋转矩阵)
nx ox ax cos(n, x) cos(o, x) cos(a, x)
A B
R
ny
oy
a
y
cos(n,
y)
cos(o, y)
cos(a, y)
nz oz az cos(n, z) cos(o, z) cos(a, z)
(1)点的齐次坐标
px
AP
py
pz
齐次坐标
px
p
y
pz
1
注意： 齐次坐标的表示不是惟一的。
P px py pz 1 T px py pz T a b c T
11
上海电机学院
位姿描述——齐次坐标
规定：
(1) (4×1)列阵[a b c ω]T中第四个元素不为零，则表示空间某点的 位置；
14
上海电机学院
位姿描述——动坐标系位姿的描述
静系
在机器人坐标系中，运动时相对 于连杆不动的坐标系称为静坐标
轨迹规划
弧法，圆弧前一点为 第一点，两个MOVC 分别为中间点和目标
圆弧插补方式移动至目标 位置P，P点是提前示教好的位置。
点。
P=<位置点> 说明：P的取值范围为1至1019， 其中1至 999用于标定位置点，1000 至 1019 用于码垛运动，自动获取码垛位置点。例1中如 果没有此参数，表示目标位置使用运动过程中 标定的位置点，例2中如果有P点参数，表示位 置点是在位置型变量内标定好的点。
(2)如图9-2，点击{程序 }-{程序管理}。
创建程序
(3)如图9-3，在{目标程 序}栏输入“4.9”，点击 {新建}。
图
图9.1
图9.2
图9.3
操作要点
建立工具坐 标系及示教
如图9-4，参考 4.6“工具坐标系 标定”建立工具 坐标系“TCS-3”。
如图9-5，参考 4.7“工件坐标系 的标定”建立工 件坐标系 “PCS1-5”。
使用举例
参数说明
V=<运行速度百分比> 说明：运行速度百分比 ，取值为1 至 100，默认值为 25。运动指令的 实际速度=设置中MOVJ 最大速度*V 运动指令 设置运行速度百分比*SPEED 指令速度设置百 分比。
圆弧插补方
式移动至目 标位置。 采用三点圆
MOVL V= 25 BL=0 VBL=0 MOVC V=25 BL=0 VBL=0 MOVC P=1 V= 25 BL=0 VBL=0
圆周程序 编写
(2)如图9-17，移 动机器人夹具末端 至圆周上P4点， 点击【插入】【确认】。
BL=<过渡段长度> 说明：过渡段长度，单位毫 米 ，此长度不能超出运行总长度一 半，如果 BL=0 则表示，不使用过渡段。
机器人轨迹规划
2016/9/25
-9-
99
工业机器人技术基础
二、轨迹规划的一般性问题
工业机器人的作业可以描述成工具坐标系{T}相 对于工件坐标系{S}的一系列运动。
图4.1所示的将销插入 工件孔中的作业，可以 借助工具坐标系的一系 列位姿Pi (i=1，2，…， n)来描述。
图4.1 机器人将销插入工件孔中的作业描述
2016/9/25
-4-
44
工业机器人技术基础
首先，机器人应该把任务进行分解，把主人交代的任务分解成为 “取一个杯子”、“找到水壶”、“打开瓶塞”、“把水倒人杯中”、 “把水送给主人”等一系列子任务。这一层次的规划称为任务规划 (Task planning)，它完成总体任务的分解。
给主人倒一杯水
取一个杯子 找到水壶
图4.2 机器人的初始状态和终止状态
2016/9/25
-11-
11 11
工业机器人技术基础
对点位作业(pick and place operation)的机器 人，需要描述它的起始状态和目标状态，即工 具坐标系的起始值{T0}，目标值{Tf}。在此， 用“点”这个词表示工具坐标系的位置和姿态 (简称位姿) 。
打开水壶
把水倒入杯中 把水送给主人
然后再针对每一个子任务进行进一步的规划。以“把水倒入杯中”这
一子任务为例，可以进一步分解成为一系列动作，这一层次的规划称为
动作规划，它把实现每一个子任务的过程分解为一系列具体的动作。
把水倒入杯中
提起水壶到杯口上方 把水壶倾斜 把水壶竖直 把水壶放回原处
2016/9/25
-5-
55
工业机器人技术基础
为了实现每一个动作，需要对手部的运动轨迹进行必要的规定， 这是手部轨迹规划(Hand trajectory planning )。
第三章 机器人轨迹规划
第三章轨迹规划第三章轨迹规划在领域，轨迹规划是一项重要的任务，它指的是确定在给定环境中的移动路径。
轨迹规划可以帮助在复杂的环境中安全且高效地移动，并且避免碰撞和与障碍物的冲突。
本章将详细介绍轨迹规划的相关概念和方法。
3.1 环境建模轨迹规划过程的第一步是进行环境建模。
环境建模包括将所在的环境转化为计算机能够理解和处理的数据形式。
常见的环境建模方法包括栅格地图、几何模型和点云等。
栅格地图将环境划分为网格，每个网格可以表示为空间中的一个点或者一个障碍物。
几何模型使用几何形状（如多边形、圆等）来表示环境。
点云是三维空间中离散点的集合，可以用于描述物体的形状和位置。
3.2 运动约束在进行轨迹规划时，需要考虑的运动约束。
运动约束可以包括的速度限制、加速度限制和转弯半径等。
通过对运动约束进行建模，可以帮助遵守规定的运动限制，确保其在运动过程中不会超出规定范围或造成损坏。
3.3 路径规划方法路径规划是轨迹规划的核心任务，通过路径规划可以确定从起始位置到目标位置的最佳路径。
常见的路径规划方法包括无碰撞路径规划方法、局部路径规划方法和全局路径规划方法等。
3.3.1 无碰撞路径规划方法无碰撞路径规划方法旨在寻找一条可以安全通过的路径。
其中，最著名的方法是A算法，它可以在有向图中找到最短路径。
此外，还有Dijkstra算法、RRT算法和PRM算法等。
3.3.2 局部路径规划方法局部路径规划方法主要用于在行进的过程中避开障碍物。
常见的局部路径规划方法包括VFH算法、LaserScan算法和Potential Field算法等。
3.3.3 全局路径规划方法全局路径规划方法用于寻找从起始位置到目标位置的最佳路径。
常见的全局路径规划方法包括D算法、D Lite算法和PRM算法等。
3.4 仿真与实验验证在进行轨迹规划研究时，常常需要进行仿真与实验验证。
通过仿真可以模拟在各种不同场景下的行为，并评估轨迹规划算法的性能。
实验验证则通过在真实的平台上进行验证，检验轨迹规划算法的可行性和有效性。
第3章 机器人轨迹规划
二 机器人轨迹控制过程
机器人的基本操作方式是示教-再现，即首先教机 器人如何做，机器人记住了这个过程，于是它可以根 据需要重复这个动作。操作过程中，不可能把空间轨 迹的所有点都示教一遍使机器人记住，这样太繁琐， 也浪费很多计算机内存。实际上，对于有规律的轨 迹，仅示教几个特征点，计算机就能利用插补算法获 得中间点的坐标，如直线需要示教两点，圆弧需要示 教三点，通过机器人逆向运动学算法由这些点的坐标 求出机器人各关节的位置和角度(1, …, n)，然后由
图3.4 空间直线插补
第3章 机器人轨迹规划
3.3 机器人轨迹插值计算
为减少实时计算量，示教完成后，可求出：
直线长度 L X e X 0 2 Ye Y0 2 Z e Z 0 2 ts间隔内行程d = vts； 插补总步数N为L/d+1的整数部分 各轴增量 X X e X 0 / N
无
离线路径规划+在线路径 跟踪 位置控制
本章主要讨论连续路径的无障碍轨迹规划方法。
第3章 机器人轨迹规划
3.2 插补方式分类与轨迹控制
一 插补方式分类
路径控制 点位控制 PTP 不插补 (1) 各轴独立 快速到达。 (2) 各关节最 大加速度限制
表3.2 路径控制与插补方式分类
关节插补(平滑) (1)各轴协调运动定时插补。 (2) 各关节最大加速度限制 空间插补
第3章 机器人轨迹规划
3.1 机器人轨迹规划概述
图3.1 机器人将销插入工件孔中的作业描述
第3章 机器人轨迹规划
3.1 机器人轨迹规划概述
三 轨迹的生成方式
运动轨迹的描述或生成有以下几种方式： (1) 示教-再现运动。这种运动由人手把手示教机器人， 定时记录各关节变量，得到沿路径运动时各关节的位移时间 函数q(t)；再现时，按内存中记录的各点的值产生序列动作. (2) 关节空间运动。这种运动直接在关节空间里进行。由 于动力学参数及其极限值直接在关节空间里描述，所以用这 种方式求最短时间运动很方便。 (3) 空间直线运动。这是一种直角空间里的运动，它便于 描述空间操作，计算量小，适宜简单的作业。 (4) 空间曲线运动。这是一种在描述空间中用明确的函数 表达的运动，如圆周运动、螺旋运动等。
机器人的基本操作方式是示教-再现，即首先教机 器人如何做，机器人记住了这个过程，于是它可以根 据需要重复这个动作。操作过程中，不可能把空间轨 迹的所有点都示教一遍使机器人记住，这样太繁琐， 也浪费很多计算机内存。实际上，对于有规律的轨 迹，仅示教几个特征点，计算机就能利用插补算法获 得中间点的坐标，如直线需要示教两点，圆弧需要示 教三点，通过机器人逆向运动学算法由这些点的坐标 求出机器人各关节的位置和角度(1, …, n)，然后由
图3.4 空间直线插补
第3章 机器人轨迹规划
3.3 机器人轨迹插值计算
为减少实时计算量，示教完成后，可求出：
直线长度 L X e X 0 2 Ye Y0 2 Z e Z 0 2 ts间隔内行程d = vts； 插补总步数N为L/d+1的整数部分 各轴增量 X X e X 0 / N
无
离线路径规划+在线路径 跟踪 位置控制
本章主要讨论连续路径的无障碍轨迹规划方法。
第3章 机器人轨迹规划
3.2 插补方式分类与轨迹控制
一 插补方式分类
路径控制 点位控制 PTP 不插补 (1) 各轴独立 快速到达。 (2) 各关节最 大加速度限制
表3.2 路径控制与插补方式分类
关节插补(平滑) (1)各轴协调运动定时插补。 (2) 各关节最大加速度限制 空间插补
第3章 机器人轨迹规划
3.1 机器人轨迹规划概述
图3.1 机器人将销插入工件孔中的作业描述
第3章 机器人轨迹规划
3.1 机器人轨迹规划概述
三 轨迹的生成方式
运动轨迹的描述或生成有以下几种方式： (1) 示教-再现运动。这种运动由人手把手示教机器人， 定时记录各关节变量，得到沿路径运动时各关节的位移时间 函数q(t)；再现时，按内存中记录的各点的值产生序列动作. (2) 关节空间运动。这种运动直接在关节空间里进行。由 于动力学参数及其极限值直接在关节空间里描述，所以用这 种方式求最短时间运动很方便。 (3) 空间直线运动。这是一种直角空间里的运动，它便于 描述空间操作，计算量小，适宜简单的作业。 (4) 空间曲线运动。这是一种在描述空间中用明确的函数 表达的运动，如圆周运动、螺旋运动等。
轨迹规划
轨迹规划分为在任务空间和关节空间两种。
根据并联机器人完成工作任务所经过的空间轨迹，编制相应的轨迹规划软件，通过计算机来事先离线计算出各驱动关节在运动中的轨迹，亦即完成轨迹规划的任务。
Paul[16]提出一种机器人手臂沿空间直线段运动的关节轨迹规划方法，Kim和shin[18]又提出一种时间最短轨迹规划方法，这种方法也是基于关节空间的。
运动轨迹是指在运动过程中的位移、速度和加速度。
轨迹规划，是根据作业任务的要求，计算出预期的运动轨迹，然后，在机器人初始位置和目标位置之间用多项式函数来“内插"或者“逼近”给定的路径，并且求出一系列“控制设定点’’，并将其提供给控制单元处理。
根据上述方法求出各轴的移动位移最后，即可规划运动曲线。
在各轴位移求出的情况下，根据所规划速度曲线的形状，可求出各个时间点对应的速度值来确定速度曲线，从而完成运动规划常规的PID控制对于大多数点位控制应用是相当有效的，而对于轨迹跟踪控制问题则效果不理想。
由于并联机器人的绝大多数应用是要求轨迹控制的，因此很少使用常规的PID控制。
并联机器人轨迹规划首先要根据系统运动要求由并联机器人机构的位置逆解方程求解出机器人的始末位姿；然后运用三次多项式插值的方法，分别对并联们器人的三条支路轨迹规划。
Matlab仿真。
并联机器人控制系统模型的建立机器人控制系统的结构如图。
在输入期望轨迹以后，机器人控制系统首先通过轨迹规划，把期望的运动轨迹转换为驱动关节的广义位置坐标。
在机器人控制系统的三个相对独立的回路中分别形成闭环控制回路，通过检测编码器的反馈信号，并与实际的给定位置相比较，根据两者间的误差不断产生控制作用，使机器人关节的实际位置运动到给定值。
系统中轨迹规划和控制在上位机由软件实现，控制输出由运动控制卡和驱动器完成，最终由电机执行。
（哈尔滨工程大学. 6-PRRS并联机器人运动控制方法的研究，2006）建立了6-PRRS并联机器人的运动学模型，并对位置逆解的选取进行了简化，方便了计算。
第5章-轨迹规划
(t 0 ) i c0 c0 i (t 0 ) 0 c1 c1 0 c (t ) c 2 2 1 (t ) i c 2 t 2 2 (t ) c 2 t (t ) c 2
将初始和末端条件代入 (t ) c0 c1t c2t 2 c3t 3 c4 t 4 c5t 5 (t ) c 2c t 3c t 2 4c t 3 5c t 4
1 2 3 4 5
(t ) 2c2 6c3t 12 c4t 2 20 c5t 3
§5.3 轨迹规划的基本原理
一 关节空间的轨迹规划
1. 非归一化运动 计算起点和终点的关节变量，各关节都以最大角 速度运动
A
B 特点：轨迹不规则，末端走过的距离不均匀，且各 关节不是同时到达。
2.归一化运动
在1的基础上对关节速率做归一化处理，使各关 节同时到达终点。 A
B
特点：各关节同时到达终点，轨迹各部分比较均 衡，但所得路径仍然是不规则的。
(t ) 5 9 .6t 6 .96 t 2 0 .928 t 3
关节位置、速度和加速度图形
三、抛物线过渡的线性运动轨迹
如果机器人关节以恒定速度运动，那么轨迹方程就相当于 一次多项式，其速度是常数，加速度为0，这说明在起点和终 点，加速度为无穷大，只有这样才可以瞬间达到匀速状态。但 很显然这是不可能的，因此在起点和终点处，可以用抛物线来 进行过渡。如图所示
§第5章 轨迹规划（4学时）
学习目的： 1 理解轨迹规划原理 2 学会用轨迹规划处理实际问题 学习内容： 1 轨迹规划原理 2 关节空间的轨迹规划 3 直角坐标空间的轨迹规划
重点：轨迹规划的基本原理 难点：关节空间的轨迹规划
将初始和末端条件代入 (t ) c0 c1t c2t 2 c3t 3 c4 t 4 c5t 5 (t ) c 2c t 3c t 2 4c t 3 5c t 4
1 2 3 4 5
(t ) 2c2 6c3t 12 c4t 2 20 c5t 3
§5.3 轨迹规划的基本原理
一 关节空间的轨迹规划
1. 非归一化运动 计算起点和终点的关节变量，各关节都以最大角 速度运动
A
B 特点：轨迹不规则，末端走过的距离不均匀，且各 关节不是同时到达。
2.归一化运动
在1的基础上对关节速率做归一化处理，使各关 节同时到达终点。 A
B
特点：各关节同时到达终点，轨迹各部分比较均 衡，但所得路径仍然是不规则的。
(t ) 5 9 .6t 6 .96 t 2 0 .928 t 3
关节位置、速度和加速度图形
三、抛物线过渡的线性运动轨迹
如果机器人关节以恒定速度运动，那么轨迹方程就相当于 一次多项式，其速度是常数，加速度为0，这说明在起点和终 点，加速度为无穷大，只有这样才可以瞬间达到匀速状态。但 很显然这是不可能的，因此在起点和终点处，可以用抛物线来 进行过渡。如图所示
§第5章 轨迹规划（4学时）
学习目的： 1 理解轨迹规划原理 2 学会用轨迹规划处理实际问题 学习内容： 1 轨迹规划原理 2 关节空间的轨迹规划 3 直角坐标空间的轨迹规划
重点：轨迹规划的基本原理 难点：关节空间的轨迹规划
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
算法。对于其他轨迹，可以采用直线或圆弧逼近，以 实现这些轨迹。
已知（示教给出）直线始末两点的坐标值P0(X0，Y0， Z0)、Pe(Xe，Ye，Ze)及姿态，要求走空间直线： 求各轨迹中间点(插补点)的位置和姿态。 设v为要求的沿直线运动的速度；ts为插补时间间隔。
直线长度： L Xe X0 2 Ye Y0 2 Ze Z0 2
第3章 机器人轨迹规划 3.1 机器人轨迹规划概述
3.1.1 机器人轨迹的概念
轨迹：点的轨迹、位移、速度和加速度。 轨迹规划：（1）或对直角空间插值：生成手部 轨迹，再将手部运动轨迹换算成关节空间运动规 律（控制依据）；（2）或对关节空间进行插值， 生成关节空间运动规律（控制依据）。
机器人手部路径的轨迹规划 作业的描述
作业的描述
结点 运动 目标
P0 INIT 原始
P1 MOVE 接近螺栓
P2 MOVE 到达
P2 GRASP
抓住
作业的描述
P3 MOVE 提升
P4 MOVE 接近托架
P5 MOVE 插入孔中
P5 RELEASE
松夹
P6 MOVE 移开
3.1.2 轨迹规划的一般性问题 （1）描述成工具坐标系{T}相对于工件坐标系
（1）对于有规律的轨迹，仅示教几个特征点，如 直线需要示教两点，圆弧需要示教三点；
（2）计算机利用插补算法获得中间点的坐标；
（3）计算机求出 (1, …, n)，
（4）半闭环控制系统实现预期轨迹。
3.3 机器人轨迹插值计算
给出结点（位置姿态）；进行运动学反解；关节 变量的插值计算。
3.3.1 直线插补 直线插补和圆弧0 0
tf
f
&0 0
&tf
0
t a0 a1t a2t2 a3t3
二、过路径点的三次多项式插值
i1 i
二、空间圆弧插补
空间圆弧插补可分三步来处理： (1) 把三维问题转化成二维，找出圆弧所在平面。 (2) 利用二维平面插补算法求出插补点坐标(Xi+1, Yi+1)。 (3) 把该点的坐标值转变为基础坐标系下的值。
TR T ( XOR ,YOR , ZOR )R(Z , )R( X , )
{S}的一系列运动。 （2）轨迹的点：包含位置和姿态； （3）插值的原则：保证运动平稳。即位移、
速度函数必须连续，有时甚至加速度也要求连续。
（4）关节空间插值：将所有关节变量表示为 时间的函数，用这些关节函数及其一阶、二阶导 数描述机器人预期的运动；
（5）直角坐标空间插值：将手部位姿、速度 和加速度表示为时间的函数，再算出所有关节位 置、速度和加速度函数值。
3.3.2 圆弧插补
一、平面圆弧插补（圆弧在坐标平面内） 已知（示教给出）不在一条直线上的三点P1、
P2、P3及对应姿态。 求各轨迹中间点(插补点)的位置和姿态。 设v为沿圆弧运动速度；ts为插补时时间隔。
(1) 计算P1、P2、P3决定的圆弧半径R。
(2)计算总的圆心角=1+2:
1 arccos
( X2
X1)2
Y2
Y1 2
2R2
/
2R2
2 arccos
( X3
X2 )2
Y3
Y2
2
2R2
/
2R2
(3) ts时间内角位移量：=tsv/R， (4) 总插补步数(取整数)：N = / + 1
Xi1 R cos(i ) R cosi cos Rsini sin Xi cos Yi sin
3.1.4 轨迹规划涉及的主要问题 轨迹规划一般过程：
(1) 作业描述：（用示教方法）给出轨迹上的若 干个结点。
(2) 插值：在结点之间进行插补，得到直角空间 的X(t)或关节空间的位移q(t)；
(3) 以X(t)或q(t) 为依据设计控制规律。 (4) 考虑路径上是否存在障碍。
4.2 插补方式分类与轨迹控制 3.2.1 插补方式分类
点位控制(PTP控制)：只要求起终点位姿，没有路 径约束。
插补的依据是（1）关节最大速度和加速度；（2） 速度连续，各轴协调。
连续轨迹控制(CP控制)：有路径约束，要对路径进 行设计。
路径控制与插补方式分类
不插补
关节插补(平滑)
空间插补
(1) 各轴独立 快速到达。 PTP (2) 关节最大 加速度限制
Yi1 Rsin(i ) Rsini cos R cosi sin Yi cos Xi sin
式中：Xi=R
cosi；Yi=Rsin
。
i
Xi1 Xi cos Yi sin
Yi1 Yi cos Xi sin
ts间隔内行程：d = vts； 插补总步数N：L/d+1，取整；
各坐标轴增量
各插补点坐标值
X Xe X0 / N Y Ye Y0 / N Z Ze Z0 / N
X i1 X i iX Yi1 Yi iY Zi1 Zi iZ
CP
(1) 各轴协调运动定时 插补。 (2) 各关节最大加速度 限制
(1) 在空间插补点间进 行关节定时插补。 (2) 用关节的低阶多项 式拟合空间直线使各轴 协调运动。 (3) 关节最大加速度限 制
(1) 直线、圆弧、 曲线等距插补。 (2)给定起停线速 度、线加速度； 关节速度、加速 度限制
3.2.2 机器人轨迹控制过程（示教-再现过程）： （属于直角空间插补过程）
cos
sin
0
0
sin cos cos cos
sin
0
sin cos cos sin
cos
0
X OR
YOR
ZOR
1
3.3.3 关节空间插补
给定：机器人在起始点和终止点手臂的位形。 可以给出：首末两点的力、速度和加速度的要 求； 插值依据：关节位移、速度、加速度连续性； 关节变量的容许范围等。
已知（示教给出）直线始末两点的坐标值P0(X0，Y0， Z0)、Pe(Xe，Ye，Ze)及姿态，要求走空间直线： 求各轨迹中间点(插补点)的位置和姿态。 设v为要求的沿直线运动的速度；ts为插补时间间隔。
直线长度： L Xe X0 2 Ye Y0 2 Ze Z0 2
第3章 机器人轨迹规划 3.1 机器人轨迹规划概述
3.1.1 机器人轨迹的概念
轨迹：点的轨迹、位移、速度和加速度。 轨迹规划：（1）或对直角空间插值：生成手部 轨迹，再将手部运动轨迹换算成关节空间运动规 律（控制依据）；（2）或对关节空间进行插值， 生成关节空间运动规律（控制依据）。
机器人手部路径的轨迹规划 作业的描述
作业的描述
结点 运动 目标
P0 INIT 原始
P1 MOVE 接近螺栓
P2 MOVE 到达
P2 GRASP
抓住
作业的描述
P3 MOVE 提升
P4 MOVE 接近托架
P5 MOVE 插入孔中
P5 RELEASE
松夹
P6 MOVE 移开
3.1.2 轨迹规划的一般性问题 （1）描述成工具坐标系{T}相对于工件坐标系
（1）对于有规律的轨迹，仅示教几个特征点，如 直线需要示教两点，圆弧需要示教三点；
（2）计算机利用插补算法获得中间点的坐标；
（3）计算机求出 (1, …, n)，
（4）半闭环控制系统实现预期轨迹。
3.3 机器人轨迹插值计算
给出结点（位置姿态）；进行运动学反解；关节 变量的插值计算。
3.3.1 直线插补 直线插补和圆弧0 0
tf
f
&0 0
&tf
0
t a0 a1t a2t2 a3t3
二、过路径点的三次多项式插值
i1 i
二、空间圆弧插补
空间圆弧插补可分三步来处理： (1) 把三维问题转化成二维，找出圆弧所在平面。 (2) 利用二维平面插补算法求出插补点坐标(Xi+1, Yi+1)。 (3) 把该点的坐标值转变为基础坐标系下的值。
TR T ( XOR ,YOR , ZOR )R(Z , )R( X , )
{S}的一系列运动。 （2）轨迹的点：包含位置和姿态； （3）插值的原则：保证运动平稳。即位移、
速度函数必须连续，有时甚至加速度也要求连续。
（4）关节空间插值：将所有关节变量表示为 时间的函数，用这些关节函数及其一阶、二阶导 数描述机器人预期的运动；
（5）直角坐标空间插值：将手部位姿、速度 和加速度表示为时间的函数，再算出所有关节位 置、速度和加速度函数值。
3.3.2 圆弧插补
一、平面圆弧插补（圆弧在坐标平面内） 已知（示教给出）不在一条直线上的三点P1、
P2、P3及对应姿态。 求各轨迹中间点(插补点)的位置和姿态。 设v为沿圆弧运动速度；ts为插补时时间隔。
(1) 计算P1、P2、P3决定的圆弧半径R。
(2)计算总的圆心角=1+2:
1 arccos
( X2
X1)2
Y2
Y1 2
2R2
/
2R2
2 arccos
( X3
X2 )2
Y3
Y2
2
2R2
/
2R2
(3) ts时间内角位移量：=tsv/R， (4) 总插补步数(取整数)：N = / + 1
Xi1 R cos(i ) R cosi cos Rsini sin Xi cos Yi sin
3.1.4 轨迹规划涉及的主要问题 轨迹规划一般过程：
(1) 作业描述：（用示教方法）给出轨迹上的若 干个结点。
(2) 插值：在结点之间进行插补，得到直角空间 的X(t)或关节空间的位移q(t)；
(3) 以X(t)或q(t) 为依据设计控制规律。 (4) 考虑路径上是否存在障碍。
4.2 插补方式分类与轨迹控制 3.2.1 插补方式分类
点位控制(PTP控制)：只要求起终点位姿，没有路 径约束。
插补的依据是（1）关节最大速度和加速度；（2） 速度连续，各轴协调。
连续轨迹控制(CP控制)：有路径约束，要对路径进 行设计。
路径控制与插补方式分类
不插补
关节插补(平滑)
空间插补
(1) 各轴独立 快速到达。 PTP (2) 关节最大 加速度限制
Yi1 Rsin(i ) Rsini cos R cosi sin Yi cos Xi sin
式中：Xi=R
cosi；Yi=Rsin
。
i
Xi1 Xi cos Yi sin
Yi1 Yi cos Xi sin
ts间隔内行程：d = vts； 插补总步数N：L/d+1，取整；
各坐标轴增量
各插补点坐标值
X Xe X0 / N Y Ye Y0 / N Z Ze Z0 / N
X i1 X i iX Yi1 Yi iY Zi1 Zi iZ
CP
(1) 各轴协调运动定时 插补。 (2) 各关节最大加速度 限制
(1) 在空间插补点间进 行关节定时插补。 (2) 用关节的低阶多项 式拟合空间直线使各轴 协调运动。 (3) 关节最大加速度限 制
(1) 直线、圆弧、 曲线等距插补。 (2)给定起停线速 度、线加速度； 关节速度、加速 度限制
3.2.2 机器人轨迹控制过程（示教-再现过程）： （属于直角空间插补过程）
cos
sin
0
0
sin cos cos cos
sin
0
sin cos cos sin
cos
0
X OR
YOR
ZOR
1
3.3.3 关节空间插补
给定：机器人在起始点和终止点手臂的位形。 可以给出：首末两点的力、速度和加速度的要 求； 插值依据：关节位移、速度、加速度连续性； 关节变量的容许范围等。