第四章轨迹规划.docx

1、第四章轨迹规划第4章机器人轨迹规划本章在操作臂运动学和动力学的基础上，讨论在关节空间和笛卡尔空间中机器人运动 的轨迹规划和轨迹生成方法。所谓轨迹，是指操作臂在运动过程中的位移、速度和加速度。 而轨迹规划是根据作业任务的要求，计算出预期的运动轨迹。首先对机器人的任务，运动 路径和轨迹进行描述，轨迹规划器可使编程手续简化，只要求用户输入有关路径和轨迹的 若干约束和简单描述，而复杂的细节问题则由规划器解决。例如，用户只需给出手部的目 标位姿，让规划器确定到达该目标的路径点、持续时间、运动速度等轨迹参数。并且，在 计算机内部描述所要求的轨迹，即选择习惯规定及合理的软件数据结构。最后，对内部描 述的轨迹

2、、实时计算机器人运动的位移、速度和加速度，生成运动轨迹。4.1机器人轨迹规划概述一、 机器人轨迹的概念机器人轨迹泛指工业机器人在运动过程中的运动轨迹，即运动点的位移、速度和加速 度。机器人在作业空间要完成给定的任务，其手部运动必须按一定的轨迹 (trajectory)进行。轨迹的生成一般是先给定轨迹上的若干个点，将其经运动学反解映射到关节空间，对关节 空间中的相应点建立运动方程，然后按这些运动方程对关节进行插值，从而实现作业空间 的运动要求，这一过程通常称为轨迹规划。工业机器人轨迹规划属于机器人低层规划，基 本上不涉及人工智能的问题，本章仅讨论在关节空间或笛卡尔空间中工业机器人运动的轨 迹规划

3、和轨迹生成方法。机器人运动轨迹的描述一般是对其手部位姿的描述，此位姿值可与关节变量相互转换。 控制轨迹也就是按时间控制手部或工具中心走过的空间路径。二、 轨迹规划的一般性问题通常将操作臂的运动看作是工具坐标系 T相对于工件坐标系S的一系列运动。这种描述方法既适用于各种操作臂，也适用于同一操作臂上装夹的各种工具。对于移动工作台 (例如传送带)，这种方法同样适用。这时，工作坐标 S 位姿随时间而变化。例如，图4.1所示将销插入工件孔中的作业可以借助工具坐标系的一系图4.1机器人将销插入工件孔中的作业描述列位姿Pi（i=i, 2，n）来描述。这种描述方法不仅符合机器人用户考虑问题的思路，而 且有利于

4、描述和生成机器人的运动轨迹。用工具坐标系相对于工件坐标系的运动来描述作业路径是一种通用的作业描述方法。 它把作业路径描述与具体的机器人、手爪或工具分离开来，形成了模型化的作业描述方法， 从而使这种描述既适用于不同的机器人，也适用于在同一机器人上装夹不同规格的工具。 在轨迹规划中，为叙述方便，也常用点来表示机器人的状态，或用它来表示工具坐标系的 位姿，例如起始点、终止点就分别表示工具坐标系的起始位姿及终止位姿。对点位作业（pick and place operation）的机器人（如用于上、下料），需要描述它的起始状 态和目标状态，即工具坐标系的起始值 TO。目标值Tf。在此，用“点”这个词表示

5、工具 坐标系的位置和姿态（简称位姿），例如起始点和目标点等。对于另外一些作业，如弧焊和曲面加工等，不仅要规定操作臂的起始点和终止点，而且要指明两点之间的若干中间点 （称路径点），必须沿特定的路径运动（路径约束）。这类称为连续路径运动（continuous Path motion）或轮廓运动（contour motion），而前者称点到点运动 （PTP = pointtopoint motion）。在规划机器人的运动时. 还需要弄清楚在其路径上是否存在障碍物 （障碍约束）。路径约束和障碍约束的组合将机器人的规划与控制方式划分为四类、如表 4-1所示。表4.1机器人的规划与控制方式障碍约束有无路径

6、约束有离线无碰撞路径规则+在线路径跟踪离线路径规划+在线路径跟踪无位置控制+在线障碍探测和避障1位置控制越径约束F 1 4门 T fto路径设定 轨迹規划器I 时，则修正最后一步的步长 A0,并以如表示，日7 7，故平面圆弧位置插补为Xj 1 =Xj cos ：v -Y sin 二口Y 1 =丫 cos v Xj sin 二口2.空间圆弧插补问题。空间圆弧是指三维空间任一平面内的圆弧，此为空间一般平面的圆弧 空间圆弧插补可分三步来处理：(1)把三维问题转化成二维，找出圆弧所在平面。(2)利用二维平面插补算法求出插补点坐标 (Xi+i, Yi+i)。 把该点的坐标值转变为基础坐标系下的值，如图

7、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放到

8、基础原点 0上；绕Zr轴转使Xo与Xr平行；再绕 Xr轴转：角，使Zo与Zr平行。这三步完成了 XrYrZr向XoYoZo的转换，故总转换矩阵应为T R 二 T (Xr,Yor,Zr) R(Zc) R(X,：J式中:-cos0si nO0(Xor COS0 +Yg sind ) T丄一 r -sin Ocos 日cosT cosasi na (XorsinGcosa +YdR co的cosa sina )lxsi nTsi not-cosBsin acosa ( XoRsinTsin a+YoR cosBsin a+ZoRcosa )0001 一三、定时插补与定距插补由上述可知，机器人实现一个

9、空间轨迹的过程即是实现轨迹离散的过程，如果这些离 散点间隔很大，则机器人运动轨迹与要求轨迹可能有较大误差。只有这些插补得到的离散 点彼此距离很近，才有可能使机器人轨迹以足够的精确度逼近要求的轨迹。 模拟CP控制实际上是多次执行插补点的 PTP控制，插补点越密集，越能逼近要求的轨迹曲线。插补点要多么密集才能保证轨迹不失真和运动连续平滑呢？可采用定时插补和定距插 补方法来解决。1.定时插补从图4.3所示的轨迹控制过程知道，每插补出一轨迹点的坐标值，就要转换成相应的关 节角度值并加到位置伺服系统以实现这个位置，这个过程每隔一个时间间隔 ts完成一次。为保证运动的平稳，显然 ts不能太长。由于关节型机

10、器人的机械结构大多属于开链式， 刚度不高，ts一般不超过25 ms(40 Hz)，这样就产生了 ts的上限值。当然ts越小越好，但它的下限值受到计算量限制， 即对于机器人的控制，计算机要在 ts时间里完成一次插补运算和一次逆向运动学计算。对于目前的大多数机器人控制器，完成这样一次计算约需几毫秒。这样产生了 ts的下限值。当然，应当选择ts接近或等于它的下限值，这样可保证较高的轨迹精度和平滑的运动过程。以一个XOY平面里的直线轨迹为例说明定时插补的方法。设机器人需要的运动轨迹为直线，运动速度为 v(mm/s)，时间间隔为ts (ms)，则每个ts间隔内机器人应走过的距离为可见两个插补点之间的距离

11、正比于要求的运动速度，两点之间的轨迹不受控制，只有插补点之间的距离足够小，才能满足一定的轨迹精度要求。机器人控制系统易于实现定时插补，例如采用定时中断方式每隔 ts中断一次进行一次插补，计算一次逆向运动学，输出一次给定值。由于 ts仅为几毫秒，机器人沿着要求轨迹的速度一般不会很高，且机器人总的运动精度不如数控机床、加工中心高，故大多数工业 机器人采用定时插补方式。当要求以更高的精度实现运动轨迹时，可采用定距插补。2.定距插补由式（4.2）可知v是要求的运动速度，它不能变化，如果要两插补点的距离 Pi Pi+1恒为一 个足够小的值，以保证轨迹精度， ts就要变化。也就是在此方式下，插补点距离不变

12、，但 t要随着不同工作速度 v的变化而变化。这两种插补方式的基本算法相同， 只是前者固定ts,易于实现，后者保证轨迹插补精度，但ts要随之变化，实现起来比前者困难。四、关节空间插补如上所述，路径点（结点）通常用工具坐标系以相对于工作坐标系位姿来表示。 为了求得在关节空间形成所要求的轨迹，首先用运动学反解将路径点转换成关节矢量角度值，然后 对每个关节拟合一个光滑函数，使之从起始点开始，依次通过所有路径点，最后到达目标 点。对于每一段路径，各个关节运动时间均相同，这样保证所有关节同时到达路径点和终止点，从而得到工具坐标系应有的位置和姿态。但是，尽管每个关节在同一段路径中的运 动时间相同，各个关节函

13、数之间却是相互独立的。总之，关节空间法是以关节角度的函数来描述机器人的轨迹的，关节空间法不必在直角坐标系中描述两个路径点之间的路径形状，计算简单、容易。再者，由于关节空间与直角坐标空间之间并不是连续的对应关系，因而不会发生机构的奇异性问题。在关节空间中进行轨迹规划，需要给定机器人在起始点、终止点手臂的形位。对关节进行插值时，应满足一系列约束条件，例如抓取物体时，手部运动方向 （初始点），提升物体离开的方向（提升点），放下物体（下放点）和停止点等结点上的位姿、速度和加速度的要求； 与此相应的各个关节位移、速度、加速度在整个时间间隔内连续性要求；其极值必须在各 个关节变量的容许范围之内等。在满足所

14、要求的约束条件下，可以选取不同类型的关节插 值函数生成不同的轨迹。本节着重讨论关节轨迹的插值方法“1.三次多项式插值在操作臂运动的过程中，由于相应于起始点的关节角度 6是已知的而终止点的关节角f可以通过运动学反解得到，因此，运动轨迹的描述，可用起始点关节角与终止点关节角 度的一个平滑插值函数 二(t)来表示。v(t)在to=O时刻的值是起始关节角度 如 终端时刻tf的值是终止关节角度-f。显然，有许多平滑函数可作为关节插值函数，如图 43所示。为实现单个关节的平稳运动，轨迹函数 r(t)至少需要满足四个约束条件，即两端点位置约束和两端点速度约束。端点位置约束是指起始位姿和终止位姿分别所对应的关

15、节角度。 V(t)在时刻to=0时的值是起始关节角度 飞，在终端时刻tf时的值是终止关节角度 f 即二 Of二 tf T为满足关节运动速度的连续性要求，两外还有两个约束条件，即在起始点和终止点的 关节速度要求。在但前的情况下，可简单地设定为零，即0 =0tf =0上面给出的四个约束条件可以惟一地确定一个三次多项式丁 t 二a0 a1t a2t2 a3t3运动过程中的关节速度和加速度则为4t =a*i +2a?t +3a?t t再 t = 2a2 6a3t为求得三次多项式的系数 ao, a a2和a3，代以给定的约束条件，有方程组二0 - a04 na。tf - a?# - ast；0 = q(

16、4.7)0 ai 2a2tf 3a3tf求解该方程组，可得a2 2(f -二0tf(4.9)由式(4.9)可得关节角速度和角加速度的表达式为舟(t) 2(半- V0)t 3(吕-二0)tf tfO 4 QF(t) 2(Vf iJ。)- 3(诂 16八tf tf(4.10)三次多项式插值的关节运动轨迹曲线如图 4.8所示。由图可知，其速度曲线为抛物线,相应的加速度曲线为直线。这里再次指出：这组解只适用于关节起始、终止速度为零的运动情况。对于其他情况， 后面另行讨论。例4.1设有一台具有转动关节的机器人，其在执行一项作业时关节运动历时 2 s。根据需要，其上某一关节必须运动平稳，并具有如下作业状态

17、：初始时，关节静止不动，位 置0)=0 运动结束时 6=90。,此时关节速度为 0。试根据上述要求规划该关节的运动。解 根据要求，可以对该关节采用三次多项式插值函数来规划其运动。已知 00=0 6f=90 , tf =2 s，代入式(4.8)可得三次多项式的系数a=0.0, ai =0.0, a2=22.5, a3 = _67.5由式(4.5)和式(4.6)可确定该关节的运动轨迹，即r t i=22.5t2 67.5t34 t ；=45.0 t 202. 5 t2月 t =45 405.0 t2.过路径点的三次多项式插值一般情况下要求规划过路径点的轨迹。如图 4.9所示，机器人作业除在 A、B

18、点有位姿要求外，在路径点C、D也有位姿要求。对于这种情况，假如末端执行器在路径点停留， 即各路径点上速度为 0,则轨迹规划可连续直接使用前面介绍的三次多项式插值方法； 但若末端执行器只是经过，并不停留，就需要将前述方法推广。实际上，可以把所有路径点也看作是“起始点”或“终止点” ，求解逆运动学，得到相应的关节矢量值。然后确定所要求的三次多项式插值函数，把路径点平滑地连接起来。但 是，在这些“起始点”和“终止点“的关节运动速度不再是零。设路径点上的关节速度已知，在某段路径上，起始点为 和-0，终止点为Q和勺f，这时，确定三次多项式系数的方法与前所述完全一致，只是速度约束条件变为0 0 勺tf #

19、利用约束条件确定三次多项式系数，有下列方程组=逐0 =80+3 +a?tf +a3tf 1 求解方程组，得ai(i 0,1,2,3)为a。=二 031 0a2 2Wf 0 0 ftf tf tfa3 壬-ftfdf =q +2a2tf +3a3t：日。)*他弋)tf (4.13)实际上，由上式确定的三次多项式描述了起始点和终止点具有任意给定位置和速度的运动轨迹，是式的推广。当路径点上的关节速度为 0，即勺0 =岂=0时，式(4.13)与式(4.8)完全相同，这就说明了由式 (4.13)确定的三次多项式描述了起始点和终止点具有任意给定位置和速度约束条件的运动轨迹。剩下的问题就是 如何来确定路径点

20、上的关节速度， 可由以下三种方法规定:(1)根据工具坐标系在直角坐标空间中的瞬时线速度和角速度来确定每个路径点的关 节速度。对于方法（1），利用操作臂在此路径点上的逆雅可比，把该点的直角坐标速度映射” 为所要求的关节速度。当然，如果操作臂的某个路径点是奇异点，这时就不能任意设置速 度值。按照方法（1）生成的轨迹虽然能满足用户设置速度的需要，但是逐点设置速度毕竟要 耗费很大的工作量。因此。机器人的控制系统最好具有方法 （2）或（3）的功能，或者二者兼而有之。（2）在直角坐标空间或关节空间中采用适当的启发式方法，由控制系统自动地选择路 径点的速度。对于方法（2）系统采用某种启发式方法自动选取合适的路径点速度。 图45表示一种启发式选择路径点速度的方式。图中 （0为起始点；0D为终止点，0a, 0B和6C是路径点，用细实线表示过路径点时的关节运动速度。这里所用的启发式信息从概念到计算方法都很简单， 即，假设用宣线段把这些路径