第5章 CHAPTER 5 机器人轨迹规划 当指定机器人执行某项操作时，往往会附带一些约束条件，如要求机器人从空间位置A沿指定路径平稳地到达位置B。这类问题称为对机器人轨迹进行规划和协调的问题。前面章节完成了对机器人的运动学建模和动力学建模，在此基础上，本章将对机器人的轨迹规划进行研究，主要包括关节空间和直角坐标空间(笛卡儿空间)机器人的轨迹规划。 机器人在作业空间(即操作空间)要完成给定的任务，其末端执行器的运动必须按一定的轨迹进行。机器人的轨迹是指机械臂在空间中的期望运动，在本书中，轨迹指的是机器人中每个关节的位置、速度和加速度在一段时间内的变化，机器人的轨迹规划是机器人控制的第一步。在具体设计机器人的轨迹时，理想情况是允许用户只用相对简单的描述就可以控制机器人的运动轨迹，然后由系统确定到达目标位置的最优路径、所用时间、速度和加速度等。而不是让用户自身必须写出复杂的时间和空间的函数才能指定机器人的期望运动。 轨迹规划要解决的问题是将机械臂从初始位置运动到一个期望终点位置，运动过程包括机器人中各关节所在坐标系相对于基座坐标系的位姿变换，即同时包括位置和姿态变化。对于这种变化过程，不是只包括两个位置(即初始位置和期望终点位置)，而应该在整个路径中设置一系列期望中间点，这些中间点位于初始位置和终点位置之间，通过这些中间点组成一个针对具体运动的期望路径。实际上，这些中间点都是期望运动过程中各关节所在坐标系相对于基座参考坐标系的位姿，而非通常意义中的“点”。 5.1机器人轨迹规划概述 5.1.1运动、路径和轨迹规划 为避免大家混淆运动规划、路径规划和轨迹规划的概念，下面将分别进行解释。 运动规划由路径规划(空间)和轨迹规划(时间)组成，连接起点位置和终点位置的序列点或曲线称为路径，构成路径的策略称为路径规划。 路径规划是运动规划的主要研究内容之一。路径是机器人位姿的序列，而不考虑机器人位姿参数随时间变化的因素； 路径点是空间中的位置或关节角度。路径规划(一般指位置规划)是找到一系列要经过的路径点，而轨迹规划是赋予路径时间信息，路径规划是轨迹规划的基础。 运动规划，又称运动插补，是在给定的路径端点之间插入用于控制的中间点序列，从而实现沿给定路线的平稳运动。运动控制则主要解决如何控制目标系统准确跟踪指令轨迹的问题，即对于给定的指令轨迹，选择适合的控制算法和参数，产生输出，控制目标以实时、准确地跟踪给定的指令轨迹。 轨迹规划在路径规划的基础上加入时间序列信息，对机器人执行任务时的速度与加速度进行规划，以满足光滑性和速度可控性等要求。 路径规划的目标是使路径与障碍物的距离尽量远，同时路径的长度尽量短。而轨迹规划的主要目的是在机器人关节空间移动时使得机器人的运行时间尽可能短，或者所消耗的能量尽可能小。 5.1.2轨迹规划的一般性问题 通常将机械臂的运动看作是工具坐标系{T}相对于工件坐标系{S}的一系列运动。这种描述方法既适用于各种操作臂，也适用于同一操作臂上装夹的各种工具。在轨迹规划中，为叙述方便，也常用点来表示机器人的状态，或用它来表示工具坐标系的位姿，例如起始点、终止点就分别表示工具坐标系的起始位姿及终止位姿。 对于点位作业的机器人(如用于上、下料)，需要描述它的起始状态和目标状态，这类运动称为点到点运动。而对于曲面加工类作业，不仅要规定操作臂的起始点和终止点，而且要指明两点之间的若干中间点(即路径点)必须沿特定的路径运动(路径约束)。这类运动称为连续路径运动或轮廓运动。 在对机器人的运动轨迹进行规划时，往往还需要知道在机器人运动的路径上是否存在障碍物(障碍约束)。路径约束和障碍约束的组合将机器人的规划与控制方式划分为4类，如表5.1所示。存在障碍物的情况不在本章的讨论范围内，本章主要讨论连续运动且无障碍的轨迹规划。 表5.1机器人的规划与控制方式 障 碍 约 束 有无 路径约束 有离线无碰撞路径规则+在线路径跟踪离线路径规划+在线路径跟踪 无位置控制+在线障碍探测和避障位置控制 在各种约束条件下，期望机械臂的轨迹在整个路径的运动过程都是平稳而光滑的。因此，应该为路径规划一个连续的且具有一阶导数的平滑函数。同时，由于机械臂特定的结构等各种因素，机械臂不能发生急速运动，否则将加剧机械臂各结构之间的磨损，由此，应该在各中间点之间，对路径的空间和时间设置一些限制条件。 在满足上面各种条件的前提下，轨迹规划有两种方法，分别是关节空间规划方法和笛卡儿空间规划方法。也就是说，轨迹规划既可在关节空间中描述，也可在直角坐标空间中指定，从而形成了关节空间和直角坐标空间机器人轨迹的规划方法。不管是在关节空间还是直角坐标空间，都要保证所规划的轨迹函数必须连续和平滑，使机器人的运动平稳。因为不平稳的运动将加剧机械部件的磨损，并导致机器人的振动和冲击。为此，要求所选择的运动轨迹描述函数必须连续，而且它的一阶导数(速度)，有时甚至是二阶导数(加速度)也应该连续。 对于机器人的轨迹规划，需要运用中间点(即路径点)来描述期望的路径。其中，对每个中间点求出每个关节坐标系的位姿，然后将每个关节作为单独的函数设置期望路径的方法称为关节空间规划法； 而根据每个中间点的位姿直接设置期望路径的方法称为笛卡儿空间规划方法。 在关节空间进行规划时，是将关节变量表示成为时间的函数，并规划它的一阶和二阶时间导数； 在直角空间进行规划是指将末端执行器位姿、速度和加速度表示为时间的函数。而相应的关节位移、速度和加速度由末端执行器的信息导出。通常通过运动学反解得出关节位移，用逆雅可比求出关节速度，用逆雅可比及其导数求解关节加速度。 5.1.3轨迹的生成方式 运动轨迹的描述或生成有以下几种方式。 (1) 示教再现运动。这种运动由人手把手示教机器人，定时记录各关节变量，得到沿路径运动时各关节的位移时间函数q(t)； 再现时，按内存中记录的各点的值产生序列动作。 (2) 关节空间运动。这种运动直接在关节空间里进行。由于动力学参数及其极限值直接在关节空间里描述，所以用这种方式求最短时间运动很方便。 (3) 空间直线运动。这是一种直角空间里的运动，它便于描述空间操作，计算量小，适宜简单的作业。 (4) 空间曲线运动。这是一种在描述空间中用明确的函数表达的运动，如圆周运动、螺旋运动等。 5.2关节空间的轨迹规划 随着机器人使用得越来越广，对机器人的轨迹规划的要求也越来越高，机器人轨迹规划是机器人执行作业任务的基础。使用的轨迹规划算法，直接决定了机器人的运动轨迹、平滑程度，使点位运动有足够高的精度，并且满足规划路径的约束条件。本节将讨论用关节角的函数描述轨迹的生成方法。 机械臂运动路径点通常是用工具坐标系{T}相对于工作台坐标系{S}的期望位姿来确定。对于关节空间规划法，首先确定期望路径的各路径点上——即运动过程中各关节的坐标系上的位姿，然后运用逆运动学理论，将路径点变换成一组期望的关节角。通过这样变换得到每个路径点处各关节上的光滑函数。由于上述过程是建立在一个机械臂期望位姿的基础上，所以各关节到达各期望路径点位置所用的时间都是相同的。需要特别说明的是，虽然每个关节在同一段路径中的运动时间相同，但各个关节的轨迹函数是相对独立的，其期望的关节角函数和其他关节函数无关。 因此，用关节空间规划的方法对机器人进行轨迹规划可以得到各路径点的期望位姿。总之，关节空间法是以关节角度的函数来描述机器人的轨迹，关节空间法不必在直角坐标系中描述两个路径点之间的路径形状，计算比较简单。并且由于关节空间与直角坐标空间之间不是连续的对应关系，因而不会发生机构的奇异性问题。 给出各个路径点后，轨迹规划的任务包括解变换方程、进行运动学反解和插值计算。在关节空间进行规划时，需进行的大量工作是对关节变量的插值计算。对关节进行插值时，应满足一系列约束条件。在满足所有约束条件下，可以选取不同类型的关节插值函数，生成不同的轨迹。关节轨迹的插值方法较多，如三次多项式插值、高阶多项式插值、用抛物线过渡的线性插值等。本节将着重讨论这些关节轨迹的插值方法。 5.2.1三次多项式插值 接下来考虑在一定时间内将工具从初始位置移动到目标位置的问题。由于机械臂的初始位置(即机械臂对应于起始点的关节角度θ0)已知， 图5.1某一关节不同的轨迹曲线 可以应用逆运动学解出对应于目标位置的关节角θf。因此，可用初始位置关节角度与目标位置关节角度的一个平滑插值函数θ(t)来描述运动轨迹。θ(t)在t0时刻的值为该关节的初始位置θ0，在tf时刻的值为该关节的目标位置θf。有多种光滑函数可作为关节插值函数，如图5.1所示。 为实现单个关节的平稳运动，显然轨迹函数θ(t)需要满足4个约束条件，即两端点位置约束和两端点速度约束。通过选择初始位置和目标位置可得到对轨迹函数θ(t)的两个约束条件： θ(0)=θ0 θ(tf)=θf(5.1) 另外,两个约束条件需要保证关节运动速度函数的连续性，即在初始位置和目标位置的关节速度要求。一般情况下设为零，即 θ·(0)=0 θ·(tf)=0(5.2) 式(5.1)和式(5.2)唯一确定了一个三次多项式，形式如下： θ(t)=a0+a1t+a2t2+a3t3(5.3) 则对应于该运动轨迹的关节速度和加速度分别为 θ·(t)=a1+2a2t+3a3t2 θ¨(t)=2a2+6a3t(5.4) 把上述约束条件式(5.1)和式(5.2)代入式(5.3)和式(5.4)，可得含有4个系数a0、a1、a2和a3的线性方程： θ0=a0 θf=a0+a1tf+a2t2f+a3t3f 0=a1 0=a1+2a2tf+3a3t2f(5.5) 解式(5.5)可得 a0=θ0 a1=0 a2=3t2f(θf-θ0) a3=-2t3f(θf-θ0)(5.6) 应用式(5.6)可以求出从任意初始关节角位置到目标(终止)位置的三次多项式，但该组解只适用于起始关节速度和终止关节速度为零的情况。 例5.1设机械臂某个关节的起始关节角θ0=15°，并且机械手原来是静止的，要求它在3s内平滑地运动到 θf=75° 时停下来(即要求在终端时的速度为零)。请规划出满足上述条件的平滑运动的轨迹，并画出关节角位置、角速度及角加速度随时间变化的曲线。 解： 根据要求，可以对该关节采用三次多项式插值函数来规划其运动。已知θ0=15°，θf=75°，tf=3s，代入式(5.6)可得三次多项式的系数a0=15，a1=0，a2=20，a3=-4.44。 由式(5.3)和式(5.4)可确定该关节的运动轨迹，即 θ(t)=15+20t2-4.44t3 θ·(t)=40t-13.33t2 θ¨(t)=40-26.66t(5.7) 根据式(5.7)画出它们随时间的变化曲线如图5.2所示。由图可以看出，速度曲线为一抛物线，加速度则为一直线。 图5.2机械臂的某个关节的运动轨迹曲线 5.2.2过路径点的三次多项式插值 一般而言，希望规划过路径点的轨迹。如图5.3所示，机器人作业除在A、B点有位姿要求外，在路径点C、D也有位姿要求。 图5.3机器人作业路径点 对于这种情况，假如末端执行器在路径点停留，即各路径点上速度为0，则轨迹规划可连续直接使用前面介绍的三次多项式插值方法。通常情况下，末端执行器只是连续经过每个路径点，并不停留，就需要将前述方法推广。 实际上，可以把所有路径点也看作是“起始点”或“终止点”，通过逆运动学求解每个路径点对应的期望关节角。然后，对每个关节求出能平滑地经过每个路径点的三次多项式插值函数。但是，这些“起始点”和“终止点”处的关节运动速度不再是零。 如果已知各关节在路径点的期望速度，则可用前面所述方法确定三次多项式。不同的是，每个终止点处的速度约束条件不再为零，式(5.2)的约束条件变为 θ·(0)=θ·0 θ·(tf)=θ·f(5.8) 描述该三次多项式的4个方程为 θ0=a0 θf=a0+a1tf+a2t2f+a3t3f θ·0=a1 θ·f=a1+2a2tf+3a3t2f(5.9) 求解方程组(5.9)，可得三次多项式的系数为 a0=θ0 a1=θ·0 a2=3t2f(θf-θ0)-2tfθ·0-1tfθ·f a3=-2t3f(θf-θ0)+1t2f (θ·f+θ·0)(5.10) 式(5.10)可以求出具有任意给定位置和速度的起始点和终止点的三次多项式。若每个路径点处都有期望的关节速度，可以使用以下几种方法来确定路径点处的关节速度。 方法(1)： 根据工具坐标系的直角坐标空间中的瞬时线速度和角速度确定每个路径点的瞬时期望关节速度，该方法的工作量较大。 对于方法(1)，利用操作臂在此路径点上的逆雅可比，把该点的直角坐标速度“映射”为所要求的关节速度。当然，如果操作臂的某个路径点是奇异点，这时就不能任意设置速度值。按照方法(1)生成的轨迹虽然能满足用户设置速度的需要，但是逐点设置速度毕竟要耗费很大的工作量。因此，机器人的控制系统最好具有方法(2)或方法(3)的功能，或者二者兼而有之。 方法(2)： 在直角坐标空间或关节空间使用适当的启发式方法，由控制系统自动选取路径点的速度。 对于方法(2)系统采用某种启发式方法自动选取合适的路径点速度。启发式选择路径点速度的方式如图5.4所示。图5.4中θ0为起始点， 图5.4路径点上速度的自动生成 θD为终止点，θA、θB和θC是路径点，用细实线表示过路径点时的关节运动速度。这里所用的启发式信息从概念到计算方法都很简单——假设用虚线段把这些路径点依次连接起来，如果相邻线段的斜率在路径点处改变符号，则把速度选定为零； 如果相邻线段不改变符号，则选取路径点两侧的线段斜率的平均值作为该点的速度。因此，根据规定的路径点，系统就能够按此规则自动生成相应的路径点速度。 方法(3)： 采用使路径点处的加速度连续的方法，由控制系统按要求自动选取路径点的速度。 对于方法(3)，为了保证路径点处的加速度连续，可以设法用两条三次曲线在路径点处按一定规则连接起来，拼凑成所要求的轨迹。其约束条件是： 连接处不仅速度连续，而且加速度也连续（本节将不对此作具体介绍）。 5.2.3高阶多项式插值 在前述轨迹插值的基础上，若对运动轨迹的要求增加，如确定路径段的起始点和终止点的位置、速度和加速度，则三次多项式就无法满足需要。此时，需要用更高阶的多项式——五次多项式对运动轨迹的路径进行插值，即 θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5(5.11) 相应地，需要在前述基础上增加2个对加速度的约束条件，多项式需要满足以下6个约束条件： θ0=a0 θf=a0+a1tf+a2t2f+a3t3f+a4t4f+a5t5f θ·0=a1 θ·f=a1+2a2tf+3a3t2f+4a4t3f+5a5t4f θ¨0=2a2 θ¨f=2a2+6a3tf+12a4t2f+20a5t3f(5.12) 方程组(5.12)有6个未知数和6个方程，求解可得 a0=θ0 a1=θ·0 a2=θ¨02 a3=20θf-20θ0-(8θ·f+12θ·0)tf-(3θ¨0-θ¨f)t2f2t3f a4=30θ0-30θf-(14θ·f+16θ·0)tf-(3θ¨0-2θ¨f)t2f2t4f a5=12θf-12θ0-(6θ·f+6θ·0)tf-(θ¨0-θ¨f)t2f2t5f(5.13) 5.2.4用抛物线过渡的线性插值 在关节空间轨迹规划中，对于给定起始点和终止点的情况,选择线性函数插值较为简单，如图5.5所示。值得注意的是， 图5.5线性函数插值 该方法中虽然各关节的运动是线性的，但末端执行器的运动轨迹一般不是线性的。 此外，线性插值将会导致起始点和终止点处关节运动的速度不连续，加速度无限大，即在两个端点会造成刚性冲击。为此，需对线性函数插值方案进行修正——在线性插值两端点的邻域内设置一段抛物线形缓冲区段，从而使整个轨迹上的位移和速度都连续。由于抛物线函数对于时间的二阶导数为常数，即相应区段内的加速度恒定，以保证起始点和终止点的速度平滑过渡，从而使整个轨迹上的位置和速度连续。线性函数与两段抛物线函数平滑地衔接在一起形成的轨迹称为带有抛物线过渡域的线性轨迹。 在运动轨迹的拟合区段内，使用恒定加速度平滑地改变速度。使用这种方法构造的简单路径如图5.6所示。为了构造这样的路径段，假设两端的抛物线拟合区段具有相同的持续时间ta，具有大小相同而方向相反的恒加速度θ¨。对于这种路径规划存在有多个解，其轨迹不唯一，如图5.7所示。但是，每条路径都对称于时间中点th和位置中点θh。要保证路径轨迹的连续、光滑，即要求抛物线轨迹的终点速度必须等于线性段的速度，故有下列关系 θ¨ta=θh-θath-ta(5.14) 式中， θa为对应于抛物线持续时间ta的关节角度，θ¨表示拟合区段的加速度。θa的值可以由下式求出： θa=θ0+12θ¨t2a(5.15) 设关节从起始点到终止点的总运动时间为tf，则tf=2th，并注意到 θh=12(θ0+θf)(5.16) 则由式(5.14)~式(5.16)得 θ¨t2a-θ¨tfta+(θf-θ0)=0(5.17) 一般情况下，θ0、θf、tf是已知条件，这样，据式(5.14)可以选择相应的θ¨和ta，得到相应的轨迹。通常的做法是先选定加速度θ¨的值，然后按式(5.17)求出相应的ta： ta=tf2-θ¨2t2f-4θ¨(θf-θ0)2θ¨(5.18) 由式(5.18)可知，为保证ta有解，加速度值θ¨必须选得足够大，即 θ¨≥4(θf-θ0)t2f(5.19) 当式(5.19)中的等号成立时，轨迹线性段的长度缩减为零，整个轨迹由两个过渡域组成，这两个过渡域在衔接处的斜率(关节速度)相等。加速度θ¨的取值越大，过渡域的长度会变得越短； 若加速度趋于无穷大，轨迹又恢复到简单的线性插值情况。 图5.6带有抛物线过渡域的线性轨迹 图5.7轨迹的多解性与对称性 例5.2同例5.1，已知θ0=15°，θf=75°，tf=3s，试设计两条带有抛物线过渡的线性轨迹。 解： 根据题意，按式(5.19)定出加速度的取值范围，为此，将已知条件代入式(5.19)中，有θ¨≥26.67°/s2。 (1) 设计第一条轨迹。 对于第一条轨迹，如果选θ¨1=42°/s2，由式(5.15)算出过渡时间ta1，则 ta1=32-422×32-4×42(75-15)2×42=0.59s 用式(5.15)和式(5.14)计算过渡域结束时的关节位置θa1和关节速度θ·1，得 θa1=15+12×42×0.592=22.3° θ·1=θ¨1ta1=(42×0.59)°/s=24.78°/s 根据上面计算得出的数值可以绘出如图5.8(a)所示的轨迹曲线。 (2) 设计第二条轨迹。 对于第二条轨迹，若选择θ¨2=27°/s2，可求出 ta2=32-272×32-4×27(75-15)2×27=1.33s θa2=15+12×27×1.332=38.88° θ·2=θ¨2ta2=(27×1.33)°/s=35.91°/s 相应的轨迹曲线如图5.8(b)所示。 图5.8带有抛物线过渡的线性插值 用抛物线过渡的线性函数插值进行轨迹规划的物理概念非常清楚，即如果机器人每一关节电动机采用等加速、等速和等减速运动规律，则关节的位置、速度、加速度随时间变化的曲线如图5.8所示。 若某个关节的运动要经过一个路径点，则可采用带抛物线过渡域的线性路径方案。关节的运动要经过一组路径点，如图5.9所示，用关节角度θj、θk和θl表示其中3个相邻的路径点， 图5.9多段带有抛物线过渡域的线性轨迹 以线性函数将每两个相邻路径点之间相连，而所有路径点附近都采用抛物线过渡。 应该注意到： 各路径段采用抛物线过渡域线性函数所进行的规划，机器人的运动关节并不能真正到达那些路径点。即使选取的加速度足够大，实际路径也只是十分接近理想路径点，如图5.9所示。 关节空间规划法表明，从关节空间得出的各中间点对应的各关节的关节角函数可以保证机械臂到达目标的位置。不过，由于关注的是关节的变化，所以对于机械臂的末端执行器，其在空间中的路径并不确定，而且因为要用到逆运动学公式，所以路径的复杂性取决于各机器人特定的运动学特性。相比之下，笛卡儿空间规划方法是用每个中间点的位姿（注： 它是关于时间的函数）来规划路径，这种方法可以确定各中间点之间的具体形状，如最常见的直线或是正弦、抛物线甚至是圆。不过这种运算量会很大，因为运行时在空间中生成相关路径的时候，必须通过逆运动学实时地解出对应的关节角。 5.3直角坐标空间的轨迹规划 给出各个路径结点后，轨迹规划的任务包括解变换方程、进行运动学反解和插值计算。在关节空间进行规划时，可以保证运动轨迹经过给定的路径点，需进行的大量工作是对关节变量的插值计算。关节空间规划法表明，从关节空间得出的各中间点对应的各关节的关节角函数可以保证机械臂到达目标的位置。不过，由于关注的是关节的变化，所以对于机械臂的末端执行器，其在空间中的路径并不确定，而且因为要用到逆运动学公式，所以路径的复杂性取决于各机器人特定的运动学特性。 相比之下，笛卡儿空间规划方法(直角坐标空间)是用每个中间点的位姿关于时间的函数来规划路径，这种方法可以确定各中间点之间的具体形状，如最常见的直线、正弦曲线、抛物线甚至是圆。但是在直角坐标空间中，路径点之间的轨迹形状往往是十分复杂的，它取决于机械臂末端执行器的运动学机构特性。这将导致该方法的运算量变得很大，因为运行时在空间中生成相关路径的时候，必须实时通过逆运动学解出对应的关节角。在有些情况下，对机械臂末端的轨迹形状也有一定要求，如要求它在两点之间走一条直线，或沿着一个圆弧运动以绕过障碍物等。这时便需要在直角坐标空间内规划机械臂的运动轨迹。 直角坐标空间的路径点，指的是机械臂末端的工具坐标相对于基坐标的位置和姿态。每一个点由6个量组成，其中3个量用来描述位置，另外3个量用来描述姿态。在直角坐标空间内，规划的方法主要有线性函数插补(值)法和圆弧插补(值)法。 直线插补(线性函数插补)和圆弧插补是机器人系统中的基本插补算法。对于非直线和圆弧轨迹，可用直线或圆弧逼近，以实现这些轨迹。 5.3.1直线插补 空间直线插补是在已知该直线始末两点的位置和姿态的条件下，求各轨迹中间点(插补点)的位置和姿态。由于在大多数情况下，机器人沿直线运动时， 图5.10空间直线插补 其姿态不变，所以无姿态插补，即保持第一个示教点时的姿态。当然，在有些情况下要求变化姿态，这就需要姿态插补，可仿照下面介绍的位置插补原理来处理，也可参照圆弧的姿态插补方法解决，如图5.10所示。已知直线始末两点的坐标值P0(X0，Y0，Z0)和Pe(Xe，Ye，Ze)以及其姿态，其中P0、Pe是相对于基坐标系的位置。这些已知的位置和姿态通常是通过示教方式得到的。设v为沿直线运动的期望速度，ts为插补时间间隔。 为减少实时计算量，示教完成后，可求出直线长度为： L=(Xe-X0)2+(Ye-Y0)2+(Ze-Z0)2(5.20) ts间隔内行程为d=vts； 插补总步数N为L/d+1的整数部分； 各轴增量为 ΔX=Xe-X0N ΔY=(Ye-Y0)N ΔZ=(Ze-Z0)N(5.21) 各插补点坐标值为 Xi+1=Xi+iΔX Yi+1=Yi+iΔY Zi+1=Zi+iΔZ(5.22) 式中，i=0，1，2，…，N。 5.3.2圆弧插补 1. 平面圆弧插补 平面圆弧是指圆弧平面与基坐标系的三大平面之一重合，以XOY平面圆弧为例。已知不在一条直线上的三点P1、P2、P3及这三点对应的机器人手端的姿态，如图5.11和图5.12所示。 图5.11由已知点P1、P2、P3决定的圆弧 图5.12圆弧插补 设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}(5.23) (3) ts时间内角位移量为Δθ=tsv/R，据图5.10所示的几何关系求各插补点坐标。 (4) 总插补步数(取整数)为 N=/Δθ+1 对Pi+1点的坐标，有 Xi+1=Rcos(θi+Δθ)=RcosθicosΔθ-RsinθisinΔθ=XicosΔθ-YisinΔθ 式中： Xi=Rcosθi； Yi=Rsinθi。 同理有 Yi+1=Rsin(θi+Δθ)=RsinθicosΔθ-RcosθisinΔθ=YicosΔθ-XisinΔθ 由θi+1=θi+Δθ可判断是否到插补终点。若θi+1≤，则继续插补下去； 当 θi+1>时，则修正最后一步的步长Δθ，并以Δθ′表示，Δθ′=-θi，故平面圆弧位置插补为 Xi+1=XicosΔθ-YisinΔθ Yi+1=YicosΔθ-XisinΔθ θi+1=θi+Δθ(5.24) 2. 空间圆弧插补 空间圆弧是指三维空间任一平面内的圆弧，空间圆弧插补可分为以下三步。 图5.13基础坐标与空间圆弧平面的关系 ① 把三维问题转化成二维，找出圆弧所在平面。 ② 利用二维平面插补算法求出插补点坐标(Xi+1,Yi+1)。 ③ 把该点的坐标值转变为基础坐标系下的值，如图5.13所示。 通过不在同一直线上的三个点P1、P2、P3可确定一个圆及三点间的圆弧，其圆心为OR，半径为R，圆弧所在平面与基础坐标系平面的交线分别为AB、BC、CA。建立圆弧平面插补坐标系，即把ORXRYRZR坐标系原点与圆心OR重合，设ORXRYRZR平面为圆弧所在平面，且保持ZR为外法线方向。这样，一个三维问题就转化成平面问题，可以应用平面圆弧插补的结论。 求解两坐标系(见图5.13)的转换矩阵。令TR表示由圆弧坐标ORXRYRZR至基础坐标系OX0Y0Z0的转换矩阵。若ZR轴与基础坐标系Z0轴的夹角为α，XR轴与基础坐标系的夹角为θ，则可完成下述步骤： ① 将XRYRZR的原点OR放到基础原点O上； ② 绕ZR轴转θ，使X0与XR平行； ③ 再绕XR轴转α角，使Z0与ZR平行。 这三步完成了XRYRZR向X0Y0Z0的转换，故总转换矩阵应为 TR=T(XOR,YOR,ZOR)R(Z,θ)R(X,α) =cosθ-sinθcosθsinθcosθXOR sinθcosθcosα-cosθsinαYOR 0sinαcosαZOR 0001(5.25) 式中,XOR、YOR、ZOR为圆心OR在基础坐标系下的坐标值。 要将基础坐标系的坐标值表示在ORXRYRZR坐标系，则需要用到TR的逆矩阵 T-1R= cosθsinθ0-(XORcosθ+YORsinθ) -sinθcosθcosθcosαsinα-(XORsinθcosα+YORcosθcosα+ZORsinα) sinθsinα-cosθsinαcosα-(XORsinθsinα+YORcosθsinα+ZORcosα) 0001(5.26) 5.4本章小结 本章基于机械臂运动学和动力学基础，讨论了关节空间和直角坐标空间中机器人的运动轨迹规划方法。先对轨迹规划的基本概念和一般问题进行了简单阐述； 然后重点讨论了关节空间轨迹的插值方法，包括三次多项式插值、高阶多项式插值、用抛物线过渡的线性插值等方法，并针对个别插值方法进行举例说明； 最后讨论了两种直角坐标空间轨迹规划的方法——直线插补和圆弧插补方法，其中圆弧插补方法又包括平面圆弧插补和空间圆弧插补。