第4章 轨迹规划

（3）计算机求出 (1, …, n)，
（4）控制系统实现预期轨迹。
4.3 机器人轨迹插值计算
给出结点（位置姿态）； 进行运动学反解； 关节变量的插值计算； 位置伺服系统实现。
每隔一个时间间隔ts完成一次
4.3.1 直线插补 直线插补和圆弧插补是机器人系统中的基本插补
算法。对于其他轨迹，可以采用直线或圆弧逼近，以 实现这些轨迹。
0T6 6TT= 0TB Bpi
0T6=0TB Bpi 6T-1T
0T6=0TB Bpi+1 6T-1T 式中：6TT 为工具坐标系{T}相对末端连杆系{6}的变换；
BPi和BPi+1分别为两结点Pi和Pi+1相对坐标系{B}的齐次变换。 可将气动手爪从结点Pi到结点Pi+1的运动看成是与气动手爪 固接的坐标系的运动，按前述运动学知识可求其解。
对工业机器人来说，高层的任务规划和动作规划一 般是依赖人来完成的。
一般的工业机器人不具备力的反馈，所以，工业机 器人通常只具有轨迹规划的和底层的控制功能。
轨迹规划的目的：将操作人员输入的任务描述变为 详细的运动轨迹描述。
对一般的工业机器人来说，操作员可能只输入机械 手末端的目标位置和方位，而规划的任务便是要确 定出达到目标的关节轨迹、运动的时间和速度等。
后两项插值依据
一、三次多项式插值
0 0
tf
f
&0 0
&tf
0
t a0 a1t a2t2 a3t3
满足连续平稳运动要求的三次多项式插值函数为 关节角速度和角加速度的表达式为
二、过路径点的三次多项式插值
机器人作业路径在多个点上有位姿要求
把每个关节上相邻的两个路径点分别看做起始点和 终止点，确定相应的三次多项式插值函数。
各坐标轴增量
各插补点坐标值
X Xe X0 / N Y Ye Y0 / N Z Ze Z0 / N
X i1 X i iX Yi1 Yi iY Zi1 Zi iZ
4.3.2 圆弧插补
一、平面圆弧插补（圆弧在坐标平面内） 已知（示教给出）不在一条直线上的三点P1、
P2、P3及对应姿态。 求各轨迹中间点(插补点)的位置和姿态。 设v为沿圆弧运动速度；ts为插补时时间隔。
cos
sin
0
0
sin cos cos cos
sin
0
sin cos cos sin
cos
0
X OR
YOR
ZOR
1
4.3.3 定时插补与定距插补
机器人实现空间轨迹的过程即是轨迹离散的过 程。模拟CP控制实际上是多次执行插补点的PTP 控制，插补点越密集，越能逼近要求的轨迹曲线。
插补点如何保证轨迹不失真和运动连续平滑呢？
一、定时插补
ts选取：保证较高的轨迹精度和平滑的运动过程 Pi Pi+1= v ts
ts上限值：受刚度限制，ts一般不超过25 ms ts下限值：受计算量限制，在ts时间里完成一次 插补运算和一次逆向运动学计算，约需几毫秒。
二、定距插补 两插补点的距离Pi Pi+1恒为一个足够小的值，
上述例子可以看出，机器人的规划是分层次的， 从高层的任务规划，动作规划到手部轨迹规划 和关节轨迹规划，最后才是底层的控制。
喷涂、搬运、点焊等操作机器人，只有简单的 轨迹控制。
对执行擦玻璃、拧螺丝、研磨力等作业的机器 人，末端与环境存在力的作用。除了手部或关 节的轨迹规划，还要进行力的控制与规划。
智能化程度越高，规划的层次越多，操作就越 简单。
t a0 a1t a2t2 a3t3 a4t 4 a5t5
四、用抛物线过渡的线性插值
直线是路径规划的最优方案之一。 带有抛物线过渡域的线性轨迹
采用带抛物线过渡域的线性路径方案：
要经过一组路径点（如绕过障碍物），经过路径 点不停歇。
以线性函数将每两个相邻路径点之间相连，而所 有路径点附近都采用抛物线过渡，保证加速度连 续。
轨迹：点的轨迹、位移、速度和加速度。
轨迹规划：关节空间的轨迹规划和直角空间的轨迹 规划。
关节空间的轨迹规划：首先将在工具空间中期望的 路径点，通过运动学逆计算，得到期望的关节位置。
然后在关节空间内，给每个关节找到一个经过中间 点到达目的终点的光滑函数，同时使得每个关节到 达中间点和终点的时间相同，这样便可保证机械手 工具能够到达期望的直角坐标位置。
4.2 插补方式分类与轨迹控制 4.2.1 插补方式分类
点位控制(PTP控制)：只要求起终点位姿，没有路 径约束。
插补的依据是（1）关节最大速度和加速度；（2） 速度连续，各轴协调。
连续轨迹控制(CP控制)：有路径约束，要对路径进 行设计。
路径控制与插补方式分类
不插补
关节插补(平滑)
空间插补
(1) 各轴独立 快速到达。 PTP (2) 关节最大 加速度限制
Xi1 R cos(i ) R cosi cos Rsini sin Xi cos Yi sin Yi1 Rsin(i ) Rsini cos R cosi sin Yi cos Xi sin
式中：Xi=R
cosi；Yi=Rsin
。
i
Xi1 Xi cos Yi sin
二、轨迹规划一般过程 (1) 作业描述：给出轨迹上的若干个结点（用示
教方法）。 (2) 插值：在结点之间进行插补，得到直角空间
的X(t)或关节空间的位移q(t)； (3) 以X(t)或q(t) 为依据设计控制规律。 (4) 考虑路径上是否存在障碍。
MOTOMAN-UP20型
程序点1——开始位置 程序点2——作业开始位置附近 程序点3——作业开始位置 程序点4——作业结束位置 程序点5——不碰触工件、夹具的位置 程序点6——开始位置附近
(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
第4章 机器人轨迹规划
4.1 机器人轨迹规划概述 一、 机器人轨迹的概念
机器人规划(P1anning): 高层任务规划 轨迹规划
老龄化比较严重，开发了各种服务老人的机器人。 比如：主人用声音命令机器人“给我倒一杯开水”， 分析机器人得到命今后，如何来完成任务。 首先，进行任务分解。
任务分解成子任务: 取一个杯子、找到水壶、 打开瓶塞、把水倒人杯中、 把水送给主人等一系列子任务。
直角坐标空间轨迹规划
对机器人末端有严格的位姿要求，则必须在直 角坐标空间进行轨迹规划。
4.4 机器人手部路径的轨迹规划 作业的描述
4.4 机器人手部路径的轨迹规划 作业的描述
机器人的作业过程可 用手部位姿结点序列 来规定，每个结点可 用工具坐标系相对于 作业坐标系的齐次变 换来描述。相应的关 节变量可用运动学反 解程序计算。
----任务规划，完成总体任务的分解。
对每个子任务做规划---动作规划 把水倒入杯中子任务进一步分解成为： 把水壶提到杯口上方、把水壶倾斜倒水入杯、 把水壶竖直、把水壶放回原处等一系列动作。
把子任务分解为一系列具体的动作。 为了实现每一个动作，需要规划手部的运动轨迹 ---手部轨迹规划 为了实现手部预定的运动，要规划各关节的运动规 律----关节轨迹规划。 最后才是关节的运动控制。
表3.3 螺栓的抓紧和插入过程
结点 P0P1P2 NhomakorabeaP2
P3
P4
P5
P5
P6
运动 INIT MOVE MOVE GRASP MOVE MOVE MOVE RELEASE MOVE
目标 原始 接近螺栓 到达 抓住 提升 接近托架 插入孔中 松夹
移开
要生成从任一结点Pi到下一结 点Pi+1的运动可表示为从：
CP
(1) 各轴协调运动定时 插补。 (2) 各关节最大加速度 限制
(1) 在空间插补点间进 行关节定时插补。 (2) 用低阶多项式拟合 空间直线使各轴协调运 动。 (3) 最大加速度限制
(1) 直线、圆弧、 曲线等距插补。 (2)给定起停线速 度、线加速度； 关节速度、加速 度限制
4.2.2 机器人轨迹控制过程（示教-再现过程） （1）对于有规律的轨迹，仅示教几个特征点，如 直线需要示教两点，圆弧需要示教三点； （2）计算机利用插补算法获得中间点的坐标；
以保证轨迹精度。 实现更高的精度运动轨迹， ts要变化。
4.3.4 关节空间插补
给定：机器人在起始点和终止点手臂的位形。 对关节进行插值时应满足一系列的约束条件：
例如： 抓取物体时手部的运动方向(初始点)、 提升物体离开的方向(提升点)、 放下物体(下放点)和停止点等结点上的位姿、速
度和加速度的要求； 各关节位移、速度、加速度的连续性要求； 各个关节变量在容许范围之内等。
一般情况下，关节运动速度不再为零。
0 0
tf
f
&0 &0 &tf &f
t a0 a1t a2t2 a3t3
三、高阶多项式插值 若对于运动轨迹的要求更为严格，约束条件增多，
三次多项式就不能满足需要，须用更高阶的多项式对运 动轨迹的路径段进行插值。
例如:起始点和终止点都规定了关节的位置、速度 和加速度，则要用一个五次多项式进行插值。
空间直线插补：直线始末两点的坐标值P0(X0，Y0，Z0)、 Pe(Xe，Ye，Ze)及姿态，求各轨迹中间点(插补点)的位