动力学约束
本章主要讨论连续路径的无障碍的轨迹规划方法。
轨迹规划器可形象地看成为一个黑
箱（图4—2），其输入包括路径的“设定”和“约束”，输出的是操作臂末端手部的“位姿序
列”，表示手部在各离散时刻的中间形位。
操作臂最常用的轨迹规划方法有两种：
第一种方法要求用户对于选定的轨迹结点（插值点）上的位姿、速度和加速度给出一组显
式约束（例如连续性和光滑程度等），轨迹规划器从一类函数（例如n次多项式）中选取参数化轨迹，对结点进行插值，并满足约束条件。
第二种方法要求用户给出运动路径的解析式；如直角坐标空间中的直线路径，轨迹规划器在关节空间或直角坐标空间中确定一条轨迹来逼近预定的路径。
在第一种方法中，约束的设定和轨迹规划均在关节空间进行。
由于对操作臂手部（直角
坐标形位）没有施加任何约束，用户很难弄清手部的实际路径，因此可能会发生与障碍物相碰。
第二种方法的路径约束是在直角坐标空间中给定的、而关节驱动器是在关节空间中受控的。
因此，为了得到与给定路径十分接近的轨迹，首先必须采用某种函数逼近的方法将直角坐标路径约束转化为关节坐标路径约束，然后确定满足关节路径约束的参数化路径。
轨迹规划既可在关节空间也可在直角空间中进行．但是所规划的轨迹函数都必须连续和平滑，使得操作臂的运动平稳。
在关节空间进行规划时、是将关节变量表示成时间的函数，并规划它的一阶和二阶时间导数；在直角空间进行规划是指将手部位姿、速度和加速度表示为时间的函数。
而相应的关节位移、速度和加速度由手部的信息导出。
通常通过运动学反解得出关节位移、用逆稚可比求出关节速度，用逆雅可比及其导数求解关节加速度。
用户根据作业给出各个路径结点后．规划器的任务包含：
解变换方程、进行运动学反解和插值运算等；在关节空间进行规划时，大量工作是对关节变量的插值运算。
下面讨论关节轨迹的插值计算。
三、轨迹的生成方式
运动轨迹的描述或生成有以下几种方式：
（1）示教-再现运动。
这种运动由人手把手示教机器人，定时记录各关节变量，得到沿路径运动时各关节的位移时间函数q（t）;再现时，按内存中记录的各点的值产生序列动作。
（2）关节空间运动。
这种运动直接在关节空间里进行。
由于动力学参数及其极限值直接在关节空间里描述，所以用这种方式求最短时间运动很方便。
（3）空间直线运动。
这是一种直角空间里的运动，它便于描述空间操作，计算量小，适宜简单的作业。
（4）空间曲线运动。
这是一种在描述空间中用明确的函数表达的运动，如圆周运动、螺旋运动等。
四、轨迹规划涉及的主要问题为了描述一个完整的作业，往往需要将上述运动进行组合。
通常这种规划涉及到以下几方面的问题：
（1）对工作对象及作业进行描述，用示教方法给出轨迹上的若干个结点（knot）。
（2）用一条轨迹通过或逼近结点，此轨迹可按一定的原则优化，如加速度平滑得到直角空间的位移时间函数X（t）或关节空间的位移时间函数q（t）;在结点之间如何进行插补，即根据轨迹表达式在每一个采样周期实时计算轨迹上点的位姿和各关节变量值。
（3）以上生成的轨迹是机器人位置控制的给定值，可以据此并根据机器人的动态参数设计一定的控制规律。
（4）规划机器人的运动轨迹时，尚需明确其路径上是否存在障碍约束的组合。
一般将机器人的规划与控制方式分为四种情况，如表4.1所示。
4.2插补方式分类与轨迹控制
一、插补方式分类
点位控制（PTP控制）通常没有路径约束，多以关节坐标运动表示。
点位控制只要求满足起终点位姿，在轨迹中间只有关节的几何限制、最大速度和加速度约束；为了保证运动的连续性，要求速度连续，各轴协调。
连续轨迹控制（CP控制）有路径约束，因此要对路径进行设
计。
路径控制与插补方式分类如表4.2所示。
表4.2路径控制与插补方式分类
路径控制
不插补
关节插补（平滑）
空间插补
点位控制
PTP
（1）各轴独立快速到达。
（2）各关节最大加速度限制
（1）各轴协调运动定时插补。
（2）各关节最大加速度限制
连续路径
控制CP
（1）在空间插补点间进行关节定时插补。
（2）用关节的低阶多项式拟合空间直线使各轴协调运动。
（3）各关节最大加速度限制
（1）直线、圆弧、曲线等距插补。
（2）起停线速度、线加速度给定，各关节速度、加速度限制
机器人轨迹控制过程
机器人的基本操作方式是示教-再现，即首先教机器人如何做，机器人记住了这个过程,于是它可以根据需要重复这个动作。
操作过程中，不可能把空间轨迹的所有点都示教一遍使机器人记住，这样太繁琐，也浪费很多计算机内存。
实际上，对于有规律的轨迹，仅示教几个特征点，计算机就能利用插补算法获得中间点的坐标，如直线需要示教两点，圆弧需要示教三点，通过机器人逆向运动学算法由这些点的坐标求出机器人各关节的位置和角度（也…,4），然后由后面的角位置闭环控制系统实现要求的轨迹上的一点。
继续插补并重复上述过程，从而实现要求的轨迹。
机器人轨迹控制过程如图4.3所示。
图4.3机器人轨迹控制过程
4.3机器人轨迹插值计算
给出各个路径结点后，轨迹规划的任务包含解变换方程，进行运动学反解和插值计算。
在关节空间进行规划时，需进行的大量工作是对关节变量的插值计算。
一、直线插补
直线插补和圆弧插补是机器人系统中的基本插补算法。
对于非直线和圆弧轨迹，可以采用直线或圆弧逼近，以实现这些轨迹。
空间直线插补是在已知该直线始末两点的位置和姿态的条件下，求各轨迹中间点（插补
点）的位置和姿态。
由于在大多数情况下，机器人沿直线运动时其姿态不变，所以无姿态插补，即保持第一个示教点时的姿态。
当然在有些情况下要求变化姿态，这就需要姿态插补，可仿照下面介绍的位置插补原理处理，也可参照圆弧的姿
态插补方法解决，如图4.4所示。
已知直线始末两点的坐标值Po（Xo,Yo,Zo）、Pe（Xe,Ye,乙）及姿态，其中Po、Pe是相对于基坐标系的位置。
这些已知的位置和姿态通常是
通过示教方式得到的。
设v为要求的沿直线运动的速度；ts为插补时间间隔。
为减少实时计算量，示教完成后，可求出：
直线长度L
222Xe-XoYe-丫0Ze-Zo；
ts间隔内行程d=vts；
插补总步数N为L/d+1
各轴增量
的整数部分;
X=[Xe-Xo/N
Y=亿-Y0/N
辽二Ze-Zo/N
各插补点坐标值
式中：
i=0,1,2，…
圆弧插补
1•平面圆弧插补
平面圆弧是指圆弧平面与基坐标系的三大平面之一重合，以XOY平面圆弧为例。
已知
不在一条直线上的三点P2、P3及这三点对应的机器人手端的姿态，如图4.5及图4.6所
示。
设V为沿圆弧运动速度；ts为插补时时间隔。
类似直线插补情况计算出：
（1）由Pi、P2、P3决定的圆弧半径R。
（2）总的圆心角=1+2，即
q=arccos{（X2_Xi）2+（丫2-Yi）-2R2I/2R2}
2=arccos||（X3—X2）2Y3—Y2彳_2R2/2R2』
（3）ts时间内角位移量A（==tsv/R,据图4.4所示的几何关系求各插补点坐标。
（4）总插补步数（取整数）
N=/•：
0+1
对Pi+1点的坐标，有
Xj彳=Rcos（〒：
巧二Rcos^cos门-Rsin耳sin：
v-XjCOS门-Ysin-'：
1
式中：
Xj=Rcos0;Yi=Rsin0。
同理有
Y1二Rsin（q：
v）=Rsin弓cosvRcos^sin：
v-YcosX,sin
由0+1=0+=0可判断是否到插补终点。
若0+1一；则继续插补下去；当0+1>'时，则
修正最后一步的步长A0,并以如’表示，△日77，故平面圆弧位置插补为
Xj1=Xjcos：
v-Ysin二口
Y1=丫cosvXjsin二口
2.空间圆弧插补
问题。
空间圆弧是指三维空间任一平面内的圆弧，此为空间一般平面的圆弧空间圆弧插补可分三步来处理：
（1）把三维问题转化成二维，找出圆弧所在平面。
（2）利用二维平面插补算法求出插补点坐标（Xi+i,Yi+i）。
⑶把该点的坐标值转变为基础坐标系下的值，如图4.7所示。
通过不在同一直线上的三点Pi、P2、P3可确定一个圆及三点间的圆弧，其圆心为Or,
半径为R,圆弧所在平面与基础坐标系平面的交线分别为AB、BC、CA。
建立圆弧平面插补坐标系，即把OrXrYrZr坐标系原点与圆心Or重合，设OrXrYrZr平
面为圆弧所在平面，且保持Zr为外法线方向。
这样，一个三维问题就转化成平面问题，可以应用平面圆弧插补的结论。
求解两坐标系（图4.7）的转换矩阵。
令TR表示由圆弧坐标OrXrYrZr至基础坐标系
OX0Y0Z0的转换矩阵。
若Zr轴与基础坐标系Zo轴的夹角为G,Xr轴与基础坐标系的夹角为则可完成下述
步骤：
①将XrYrZr的原点Or放到基础原点0上；②绕Zr轴转使Xo与Xr平行；③再绕Xr轴转：
•角，使Zo与Zr平行。
这三步完成了XrYrZr向XoYoZo的转换，故总转换矩阵应为
TR二T（X°r,Yor,Z°r）R（Zc）R（X,：
J
式中:
-
cos0
sinO
0
—（XorCOS0+Ygsind）〕
T丄一
■r—
-sinOcos日
cosTcosa
sina—（Xor
sinGcosa+YdRco的cosasina）
lx
sinTsinot
-cosBsina
cosa—（XoR
sinTsina+YoRcosBsina+ZoRcosa）
0
0
0
1一
三、定时插补与定距插补
由上述可知，机器人实现一个空间轨迹的过程即是实现轨迹离散的过程，如果这些离散点间隔很大，则机器人运动轨迹与要求轨迹可能有较大误差。
只有这些插补得到的离散点彼此距离很近，才有可能使机器人轨迹以足够的精确度逼近要求的轨迹。
模拟CP控制实
际上是多次执行插补点的PTP控制，插补点越密集，越能逼近要求的轨迹曲线。
插补点要多么密集才能保证轨迹不失真和运动连续平滑呢？
可采用定时插补和定距插补方法来解决。
1.定时插补
从图4.3所示的轨迹控制过程知道，每插补出一轨迹点的坐标值，就要转换成相应的关节角度值并加到位置伺服系统以实现这个位置，这个过程每隔一个时间间隔ts完成一次。
为保证运动的平稳，显然ts不能太长。
由于关节型机器人的机械结构大多属于开链式，刚度不高，ts一般不超过25ms（40Hz），
这样就产生了ts的上限值。
当然ts越小越好，但它的下限值受到计算量限制，即对于机器人
的控制，计算机要在ts时间里完成一次插补运算和一次逆向运动学计算。
对于目前的大多
数机器人控制器，完成这样一次计算约需几毫秒。
这样产生了ts的下限值。
当然，应当选
择ts接近或等于它的下限值，这样可保证较高的轨迹精度和平滑的运动过程。
以一个XOY平面里的直线轨迹为例说明定时插补的方法。
设机器人需要的运动轨迹为直线，运动速度为v（mm/s），时间间隔为ts（ms），则每个ts
间隔内机器人应走过的距离为
可见两个插补点之间的距离正比于要求的运动速度，两点之间的轨迹不受控制，只有
插补点之间的距离足够小，才能满足一定的轨迹精度要求。
机器人控制系统易于实现定时插补，例如采用定时中断方式每隔ts中断一次进行一次
插补，计算一次逆向运动学，输出一次给定值。
由于ts仅为几毫秒，机器人沿着要求轨迹
的速度一般不会很高，且机器人总的运动精度不如数控机床、加工中心高，故大多数工业机器人采用定时插补方式。
当要求以更高的精度实现运动轨迹时，可采用定距插补。
2.定距插补
由式（4.2）可知v是要求的运动速度，它不能变化，如果要两插补点的距离PiPi+1恒为一个足够小的值，以保证轨迹精度，ts就要变化。
也就是在此方式下，插补点距离不变，但t
要随着不同工作速度v的变化而变化。
这两种插补方式的基本算法相同，只是前者固定ts,易于实现，后者保证轨迹插补精度，
但ts要随之变化，实现起来比前者困难。
四、关节空间插补
如上所述，路径点（结点）通常用工具坐标系以相对于工作坐标系位姿来表示。
为了求得
在关节空间形成所要求的轨迹，首先用运动学反解将路径点转换成关节矢量角度值，然后对每个关节拟合一个光滑函数，使之从起始点开始，依次通过所有路径点，最后到达目标点。
对于每一段路径，各个关节运动时间均相同，这样保证所有关节同时到达路径点和终
止点，从而得到工具坐标系应有的位置和姿态。
但是，尽管每个关节在同一段路径中的运动时间相同，各个关节函数之间却是相互独立的。
总之，关节空间法是以关节角度的函数来描述机器人的轨迹的，关节空间法不必在直
角坐标系中描述两个路径点之间的路径形状，计算简单、容易。
再者，由于关节空间与直
角坐标空间之间并不是连续的对应关系，因而不会发生机构的奇异性问题。
在关节空间中进行轨迹规划，需要给定机器人在起始点、终止点手臂的形位。
对关节
进行插值时，应满足一系列约束条件，例如抓取物体时，手部运动方向（初始点），提升物体
离开的方向（提升点），放下物体（下放点）和停止点等结点上的位姿、速度和加速度的要求；与此相应的各个关节位移、速度、加速度在整个时间间隔内连续性要求；其极值必须在各个关节变量的容许范围之内等。
在满足所要求的约束条件下，可以选取不同类型的关节插值函数生成不同的轨迹。
本节着重讨论关节轨迹的插值方法“
1.三次多项式插值
在操作臂运动的过程中，由于相应于起始点的关节角度6是已知的•而终止点的关节
角f可以通过运动学反解得到，因此，运动轨迹的描述，可用起始点关节角与终止点关节角度的一个平滑插值函数二（t）来表示。
v（t）在to=O时刻的值是起始关节角度如终端时刻tf的
值是终止关节角度-f。
显然，有许多平滑函数可作为关节插值函数，如图4—3所示。
为实现单个关节的平稳运动，轨迹函数r（t）至少需要满足四个约束条件，即两端点位
置约束和两端点速度约束。
端点位置约束是指起始位姿和终止位姿分别所对应的关节角度。
V（t）在时刻to=0时的值
是起始关节角度飞，在终端时刻tf时的值是终止关节角度f即
二Of
二tfT
为满足关节运动速度的连续性要求，两外还有两个约束条件，即在起始点和终止点的关节速度要求。
在但前的情况下，可简单地设定为零，即
0=0
tf=0
上面给出的四个约束条件可以惟一地确定一个三次多项式
丁t二a0a1ta2t2a3t3
运动过程中的关节速度和加速度则为
4t=a*i+2a?
t+3a?
tt
再t=2a26a3t
为求得三次多项式的系数ao,a^a2和a3，代以给定的约束条件，有方程组
二0-a0
4na。
©tf-a?
#-ast；
0=q
（4.7）
0—ai2a2tf'3a3tf
求解该方程组，可得
a22（・f-二0
tf
（4.9）
由式（4.9）可得关节角速度和角加速度的表达式为
舟（t）2（半-V0）t3（吕-二0）『
tftf
O4Q
F（t）2（VfiJ。
）-'3（诂16八
tftf
（4.10）
三次多项式插值的关节运动轨迹曲线如图4.8所示。
由图可知，其速度曲线为抛物线,
相应的加速度曲线为直线。
这里再次指出：
这组解只适用于关节起始、终止速度为零的运动情况。
对于其他情况，后面另行讨论。
例4.1设有一台具有转动关节的机器人，其在执行一项作业时关节运动历时2s。
根
据需要，其上某一关节必须运动平稳，并具有如下作业状态：
初始时，关节静止不动，位置0）=0°运动结束时6=90。
此时关节速度为0。
试根据上述要求规划该关节的运动。
解根据要求，可以对该关节采用三次多项式插值函数来规划其运动。
已知00=0°6f
=90°,tf=2s，代入式（4.8）可得三次多项式的系数
a°=0.0,ai=0.0,a2=22.5,a3=_67.5
由式（4.5）和式（4.6）可确定该关节的运动轨迹，即
rti=22.5t267.5t3
4t；=45.0t202.5t2
月t[=45405.0t
2.过路径点的三次多项式插值
一般情况下•要求规划过路径点的轨迹。
如图4.9所示，机器人作业除在A、B点有
位姿要求外，在路径点C、D也有位姿要求。
对于这种情况，假如末端执行器在路径点停留，即各路径点上速度为0,则轨迹规划可连续直接使用前面介绍的三次多项式插值方法；但若
末端执行器只是经过，并不停留，就需要将前述方法推广。
实际上，可以把所有路径点也看作是“起始点”或“终止点”，求解逆运动学，得到相
应的关节矢量值。
然后确定所要求的三次多项式插值函数，把路径点平滑地连接起来。
但是，在这些“起始点”和“终止点“的关节运动速度不再是零。
设路径点上的关节速度已知，在某段路径上，起始点为和-0，终止点为Q和勺f，这
时，确定三次多项式系数的方法与前所述完全一致，只是速度约束条件变为
‘00勺tf#
利用约束条件确定三次多项式系数，有下列方程组
=逐
0=80+3^+a?
tf+a3tf—1>
求解方程组，得ai（i^0,1,2,3）为
a。
=二0
31—^0
a22Wf…00f
tftftf
a3壬-f
tf
df=q+2a2tf+3a3t：
—日。
）*他弋）
tf（4.13）
实际上，由上式确定的三次多项式描述了起始点和终止点具有任意给定位置和速度的
运动轨迹，是式的推广。
当路径点上的关节速度为0，即勺0=岂=0时，式（4.13）与式（4.8）完
全相同，这就说明了由式（4.13）确定的三次多项式描述了起始点和终止点具有任意给定位置
和速度约束条件的运动轨迹。
剩下的问题就是如何来确定路径点上的关节速度，可由以下三种方法规定:
（1）根据工具坐标系在直角坐标空间中的瞬时线速度和角速度来确定每个路径点的关节速度。
对于方法
（1），利用操作臂在此路径点上的逆雅可比，把该点的直角坐标速度"映射”为所要求的关节速度。
当然，如果操作臂的某个路径点是奇异点，这时就不能任意设置速度值。
按照方法
（1）生成的轨迹虽然能满足用户设置速度的需要，但是逐点设置速度毕竟要耗费很大的工作量。
因此。
机器人的控制系统最好具有方法
（2）或（3）的功能，或者二者兼而
有之。
（2）在直角坐标空间或关节空间中采用适当的启发式方法，由控制系统自动地选择路径点的速度。
对于方法
（2）系统采用某种启发式方法自动选取合适的路径点速度。
图4—5表示一种启
发式选择路径点速度的方式。
图中（0为起始点；0D为终止点，0a,0B和6C是路径点，用细
实线表示过路径点时的关节运动速度。
这里所用的启发式信息从概念到计算方法都很简单，即，假设用宣线段把这些路径
升级会员 