式中：
X亍Rcos0*Y^Rsin〃严同理有
耳+i=Asin（^+A6>）=J?
sin^cosA^+J?
cos^sin=YicqsAO+X^sinAd
由e.+1=e.+a0可判断是否到插补终点。
若5+3,则继续插补下去；当”“1>0时，则修正最后一步的步长△〃，并以表示十兔，故平面圆弧位置插补为
^+1=ijCOsA^+sinA^卜
2+i=0+AQ
二、空间圆弧插补（MOVC）
空间圆弧是指三维空间任一平面内的圆弧，此为空间一般平面的圆弧问题。
空间圆弧插补可分三步来处理:
（1）把三维问题转化成二维，找出圆弧所在平面。
（2）利用二维平面插补算法求出插补点坐标（&+],E+J。
（3）把该点的坐标值转变为基础坐标系下的值，如图6.7所示。
图6.7基础坐标与空间圆弧平面的关系
机器人轨迹规划
Zq
4（耳齐,ZJ
通过不在同一直线上的三点人、卩2、&可确定一个圆及三点间的圆弧，其圆心为0,半径为斤，圆弧所在平面与基础坐标系平面的交线分别为力从BC、CAO
建立圆弧平面插补坐标系，即把家坐标系原点与
合，设羁滋平面为圆弧所在平面，且保持初外法线方向。
这样,一个三维问题就转化成平面问题，可以应用平面圆弧插补的结论。
求解两坐标系（图6.7）的转换矩阵。
令/表示由圆弧坐标羁必至
基础坐标系下的点。
6.3.3定时插补与定距插补
由上述可知，机器人实现一个空间轨迹的过程即是实现轨迹离散的过程，如果这些离散点间隔很大，则机器人运动轨迹与要求轨迹可能有较大误差。
只有这些插补得到的离散点彼此距离很近，才有可能使机器人轨迹以足够的精确度逼近要求的轨迹。
模拟CP控制实际上是多次执行插补点的PTP控制，插补点越密集，越能逼近要求的轨迹曲线。
插补点要多么密集才能保证轨迹不失真和运动连续平滑呢?
可采用定时插补和定距插补方法来解决。
一、定时插补
从轨迹控制过程知道，每插补出一轨迹点的坐标值，就要转换成相应的关节角度值并加到位置伺服系统以实现这个位置，这个过程每隔一个时间间隔©完成一次。
为保证运动的平稳，显然方s不能太长。
由于关节型机器人的机械结构大多属于开链式，刚度不高，4一般不超过25ms（40Hz）,这样就产生了4的上限值。
当然〈越小越好，但它的下限值受到计算量限制，即对于机器人的控制，计算机要在©时间里完成一次插补运算和一次逆向运动学计算。
对于目前的大多数机器人控制器，完成这样一次计算约需几毫秒。
这样产生了©的下限值。
当然，应当选择力$接近或等于它的下限值，这样可保证较高的轨迹精度和平滑的运动过程。
以一个XOY平面里的直线轨迹为例说明定时插补的方法。
设机器人需要的运动轨迹为直线，运动速度为v（mm/s）,时间间隔为心（ms）,则每个心间隔内机器人应走过的距离为
可见两个插补点之间的距离正比于要求的运动速度，两点之间的轨迹不受控制，只有插补点之间的距离足够小，才能满足一定的轨迹精度要求。
机器人控制系统易于实现定时插补，例如采用定时中断方式每隔4中断一次进行一次插补，计算一次逆向运动学，输出一次给定值。
由于4仅为几毫秒，机器人沿着要求轨迹的速度一般不会很高，且机器人总的运动精度不如数控机床、加工中心高，故大多数工业机器人采用定时插补方式。
当要求以更高的精度实现运动轨迹时，可采用定距插补。
二、定距插补
V是要求的运动速度，它不能变化，如果要两插补点的距离沖恒为一个足够小的值，以保证轨迹精度，4就要变化。
也就是在此方式下，插补点距离不变，但4要随着不同工作速度卩的变化而变化。
这两种插补方式的基本算法相同，只是前者固定心，易于实现，后者保证轨迹插补精度，但心要随之变化，实现起来比前者困难。
6・3・4关节空间插补（MOVJ）
在关节空间中进行轨迹规划，需要给定机器人在起始点和终止点手臂的位形。
对关节进行插值时应满足一系列的约束条件，例如抓取物体时手部的运动方向（初始点）、提升物体离开的方向（提升点）、放下物体（下放点）和停止点等结点上的位姿、速度和加速度的要求；与此相应的各个关节位移、速度、加速度在整个时间间隔内的连续性要求以及其极值必须在各个关节变量的容许范围之内等。
满足所要求的约束条件之后，可以选取不同类型的关节插值函数，生成不同的轨迹。
常用的关节空间插补有以下方法：
1、三次多项式插值
2、过路径点的三次多项式插值
3、高阶多项式插值
4、用抛物线过渡的线性插值
~、三次多项式插值
在机器人运动过程中，若末端执行器的起始和终止位姿已知，由逆向运动学即可求出对应于两位姿的各个关节角度。
末端执行器实现两位姿的运动轨迹描述可在关节空间中用通过起始点和终止点关节角的一个平滑轨迹函数0⑴来表示。
为实现系统的平稳运动，每个关节的轨迹函数&⑴至少需要满足四个约束条件，即两端点位置约束和两端点速度约束。
端点位置约束是指起始位姿和终止位姿分别所对应的关节角度。
夕⑴在时刻％二0时的值是起始关节角度％，在终端时刻4时的值是终止关节角度侏，即他呵
雉）=
为满足关节运动速度的连续性要求，起始点和终止点的关节速度可简单地设定为零，即
0（0）=0、
论）=0
上面给岀的四个约束条件可以惟一地确定一个三次多项式：
&#）=两十绚扌十说尸+研
运动过程中的关节速度和加速度则为
9（f）=a{+2与+3幻厂［
^（?
）=2a2+6a3tJ
将其代以给定的约束条件，可得三次多项式的系数呦，如，和血，则对于起始速度及终止速度为零的关节运动，满足连续平稳运动要求的三次多项式插值函数为：
&r）=给+活（$-&）广-奇⑥-&旷
关节角速度和角加速度的表达式为：
諾（0-久）尸
Vif
%）=£（&『-久）-等（0-仇”
三次多项式插值的关节运动轨迹曲线如图6.8所示。
由图可知,其速度曲线为抛物线，相应的加速度曲线为直线。
图6.8三次多项式插值的关节运动轨迹
【例6・1]要求一个六轴机器人的第一关节在5秒钟内从初始角30。
运动到终端角75。
，且起始点和终止点速度均为零。
用三次多项式规划该关节的运动，并计算在第1、2、3秒和第4秒时关节的角度。
解：
位移曲线。
⑴=30+5.4尸-0.72尸
图关节位移、速度和加速度
二、过路径点的三次多项式插值
若所规划的机器人作业路径在多个点上有位姿要求，如图6.9所示，机器人作业除在A、D点有位姿要求外，在路径点B、C也有位姿要求。
对于这种情况，假如末端执行器在路径点停留，即各路径点上速度为0,则轨迹规划可连续直接使用前面介绍的三次多项式插值方法；但若末端执行器只是经过，并不停留，就需要将前述方法推广。
图6.9机器人作业路径点
对于机器人作业路径上的所有路径点可以用求解逆运动学的方法先得到多组对应的关节空间路径点,进行轨迹规划时，把每个关节上相邻的两个路径点分别看做起始点和终止点，再确定相应的三次多项式插值函数，把路径点平滑连接起来。
一般情况下，这些起始点和终止点的关节运动速度不再为零。
设路径点上的关节速度已知，这时，确定三次多项式系数的方法与前所述完全一致，只是速度约束条件变为
0（0）=0°
0（。
）胡
利用约束条件可确定三次多项式系数日°,旳，日2和包，和前面有所不同。
但当路径点上的关节速度为0时，确定的三次多项式与上一种方法完全相同，这就说明了这里所确定的三次多项式描述了起始点和终止点具有任意给定位置和速度约束条件的运动轨迹。
三、高阶多项式插值
若对于运动轨迹的要求更为严格，约束条件增多，三次多项式就不能满足需要，须用更高阶的多项式对运动轨迹的路径段进行插值。
例如，对某段路径的起始点和终止点都规定了关节的位置、速度和加速度，则要用一个五次多项式进行插值，即
=勺+2注+勾：
2+Q#+码广+碍卢
多项式的系数勺，…，牛必须满足6个约束条件
%=兔
Of=a。
+ci、t彳++Clgt；+ci^t+ci、t：
%=a、
•ca
%=%+2^2。
+3ci#彳+4。
4》/.+5。
5—.
••
&0=2。
2
0f=2a2+6a3tf+12°詁「+20a5tf3
【例6-2】同例6.1,且已知起始加速度和终止减速度均为5°/s2o
解：
e⑴=30+2.5t2+1.6?
-0.58?
+0.0464”
图关节的位置、速度和加速度曲线
四、用抛物线过渡的线性插值
对于给定起始点和终止点的情况选择线性函数插值最为简单。
然而，单纯线性插值会导致起始点和终止点的关节运动速度不连续，且加速度无穷大，显然，在两端点会造成刚性冲击。
为此在线性插值两端点的邻域内设置一段抛物线形缓冲区段。
由于抛物线函数对于时间的二阶导数为常数，即相应区段内的加速度恒定，这样保证起始点和终止点的速度平滑过渡，从而使整个轨迹上的位置和速度连续。
线性函数+两段抛物线函数平滑地衔接在一起，形成带有抛物线过渡域的线性轨迹。
T形速度曲线
Trap模式（点位运动）
61*
经验：
一般取
设两端的抛物线轨迹具有相同的持续时间乙，具有大小相同而符号相反的恒加速度。
对于这种路径规划存在有多个解，其轨迹不惟一。
如图6.12所示。
但是，每条路径都对称于时间中点4和位置中点木。
要保证路径轨迹的连续、光滑，即要求
抛物线轨迹的终点速度必须等于线性段的速图&12轨迹的多解性与对称性度，故有下列关系：
设关节从起始点到终止点的总运动时间为4,贝hf=2^h,并注意到
则2
-Otfta+（兮_&o）=O
一般情况下，4、体、耳是已知条件，这样，据前式可以选择相应的加速度和t"得到相应的轨迹。
通常的做法是先选定加速度的值，然后）求出相应的"
_tf勺;_4©（0_&o）
由上式知，为保证有解，加速度值必须选得足够大，即
当上式中的等号成立时，轨迹线性段的长度缩减为零，整个轨迹
由两个过渡域组成，这两个过渡域在衔接处的斜率（关节速度）相等;
加速度的取值愈大，过渡域的长度会变得愈短，若加速度趋于无穷大，轨迹又复归到简单的线性插值情况。
用抛物线过渡的线性函数插值进行轨迹规划的物理概念非常清楚,即如果机器人每一关节电动机采用等加速、等速和等减速运动规律，则关节的位置、速度、加速度随时间变化的曲线如图所示。
若某个关节的运动要经过一个路径点，则可采用带抛物线过渡域的线性路径方案。
如图6.14所示，关节的运动要经过一组路径点，用关节角度Or&k和6表示其中三个相邻的路径点，以线性函数将每两个相邻路径点之间相连，而所有路径点附近都采用抛物线过渡。
图6.14多段带有抛物线过渡域
的线性轨迹
应该注意到：
各路径段采用抛物线过渡域线性函数所进行的规划，机器人的运动关节并不能真正到达那些路径点。
即使选取的加速度充分大，实际路径也F是十分接近理想路径点，如图6.14所示。
【例6-3】在例6-1中，假设六轴机器人的关节1以角速度100/s在5秒内从初始角30。
运动到目的角700。
求解所需的过渡时间并绘制关节位置、速度和加速度曲线。
解：
分段求出参数，然后绘制位置、速度和加速度曲线
图关节的位置、速度和加速度曲线
T形速度
升级会员 