七章机器人的轨迹规划教案资料

七章机器人的轨迹规划教案资料 1?第七章机器人的轨迹规划7.1机器人规划的定义和作用7.1.1概述机器人学中的一个基本问题是为解决某个预定的任务而规划机器人的动作，然后在机器人执行完成那些动作所需的命令时控制它。 这里，规划的意思就是机器人在行动前确定一系列动作作(作决策)，这种动作的确定可用问题求解系统来解决，给定初始情况后，该系统可达到某一规定的目标。 因此，规划就是指机器人为达到目标而需要的行动过程的描述。 规划内容可能没有次序，但是一般来说，规划具有某个规划目标的蕴含排序。 例如，早晨起床后的安排。 缺乏规划可能导致不是最佳的问题求解，甚至得不到问题的求解。 2许多规划所包的步骤是含糊的，而且需要进一步说明（子规划）。 大多数规划具有很大的子规划结构，规划中的每个目标可以由达到此目标的比较详细的子规划所代替。 因此，最终得到的规划是某个问题求解算符的线性或分部排序，但是由算符来实现的目标常常具有分层结构。 把某些比较复杂的问题分解为一些比较小的问题的想法使我们应用规划方法求解问题在实际上成为可能。 有两条能够实现这种分解的重要途径第一条是当从一个问题状态移动到下一个状态时，无需计算整个新的状态，而只要考虑状态中可能变化了的那些部分。 第二条是把单一的困难问题分割为几个有希望的、较为容易解决的子问题，这种分解能够使困难问题的求解变得容易些。 3机器人规划分为高层规划和低层规划。 自动规划在机器人规划中称为高层规划。 在无特别说明时，机器人规划都是指自动规划。 自动规划是一种重要的问题求解技术，它从某个特定的问题状态出发，寻求一系列行为动作，并建立一个操作序列，直到求得目标状态为止。 与一般问题求解相比，自动规划更注重于问题的求解过程，而不是求解结果。 我们在阐述机器人自动规划问题时，机器人一般配备有传感器和一组能在某个易于理解的现场中完成的基本动作。 这些动作可把该现场从一种状态或布局变换为另一种状态或布局。 例如，“积木世界”。 4机器人能得到的一个解答是由下面的算符序列组成的目标状态机器人规划是机器人学的一个重要研究领域，也是人工智能与机器人学一个令人感兴趣的结合点。 6机器人轨迹规划属于机器人低层规划，基本上不涉及人工智能问题，而是在机械手运动学和动力学的基础上，讨论机器人运动的规划及其方法。 所谓轨迹，就是指机器人在运动过程中的位移、速度和加速度。 轨迹规划问题通常是将轨迹规划器看成“黑箱”，接受表示路径约束的输入变量，输出为起点和终点之间按时间排列的操作机中间形态（位姿,速度和加速度）序列。 路径约束动力学约束路径设定轨迹规划器7.2机器人轨迹规划的一般性问题由初始点运动到终止点，所经过的由中间形态序列构成的空间曲线称为路径。 这些形态序列即是曲线上的“点”。 7规划操作机的轨迹有两种常用的方法?第一种方法要求使用者在沿轨迹选定的位置点上(称为结节或插值点)显式地给定广义坐标位置、速度和加速度的一组约束(例如，连续性和光滑程度等)。 然后，轨迹规划器从插值和满足插值点约束的函数中选定参数化轨迹。 显然，在这种方法中，约束的给定和操作机轨迹规划是在关节坐标系中进行的。 ?第二种方法使用者以解析函数显式地给定操作机必经之路径，例如，笛卡尔坐标中的直线路径。 然后，轨迹规划器在关节坐标或笛卡几坐标中确定一条与给定路径近似的轨迹。 在这种方法中，路径约束是在笛卡尔坐标中给定的。 8在第一种方法中，约束的给定和操作机轨迹规划在关节坐标系中进行。 由于对操作机手部没有约束，使用者难于跟踪操作机手部运行的路径。 因此，操作机手部可能在没有事先警告的情况下与障碍物相碰。 在第二种方法中，路径约束在笛卡尔坐标中给定，而关节驱动器是在关节坐标中受控制的。 因此，为了求得一条逼近给定路径的轨迹，必须用函数近似把笛卡尔坐标中的路径约束变换为关节坐标中的路径约束，再确定满足关节坐标路径约束的参数化轨迹。 ?轨迹规划既可在关节变量空间中进行，也可在笛卡尔空间进行。 对于关节变量空间的规划，要规划关节变量的时间函数及其前二阶时间导数，以便描述操作机的预定运动。 在笛卡尔空间规划中，要规划操作机手部位置、速度和加速度的时间函数，。 而相应的关节位置、速度和加速度可根据手部信息导出。 9?面向笛卡尔空间方法的优点是概念直观，而且沿预定直线路径可达到相当的准确性。 可是由于现代还没有可用笛卡尔坐标测量操作机手部位置的传感器，所有可用的控制算法都是建立在关节坐标基础上的。 因此，笛卡尔空间路径规划就需要在笛卡尔坐标和关节之间进行实时变换，这是一个计算量很大的任务，常常导致较长的控制间隔。 ?由笛卡尔坐标向关节坐标的变换是病态的，因而它不是一一对应的映射。 ?如果在轨迹规划阶段要考虑操作机的动力学特性，就要以笛卡尔坐标给定路径约束，同时以关节坐标给定物理约束(例如，每个关节电机的力和力矩、速度和加速度权限)。 这就会使最后的优化问题具有在两个不同坐标系中的混合约束。 在笛卡尔空间进行轨迹规划的特点10在关节变量空间的规划有三个优点 (1)直接用运动时的受控变量规划轨迹； (2)轨迹规划可接近实时地进行； (3)关节轨迹易于规划。 伴随的缺点是难于确定运动中各杆件和手的位置，但是，为了避开轨迹上的障碍常常又要求知道一些杆件和手位置。 由于面向笛卡尔空间的方法有前述钟种缺点，使得面向关节空间的方法被广泛采用。 它把笛卡尔结点变换为相应的关节坐标，并用低次多项式内插这些关节结点。 这种方法的优点是计算较快，而且易于处理操作机的动力学约束。 但当取样点落在拟合的光滑多项式曲线上时，面向关节空间的方法沿笛卡尔路径的准确性会有损失。 11一般说来，生成关节轨迹设定点的基本算法是很简单的0t t?循环等待下一个控制间隔；时刻操作机关节所处的位置；若，则退出循环；转向循环。 t t t?()h t t?ft t?从上述算法可以看出，要计算的是在每个控制间隔中必须更新的轨迹函数(或轨迹规划器)h(t)。 因此，对规划的轨迹要提出四个限制。 第一，必须便于用迭代方式计算轨迹设定点；第二，必须求出并明确给定中间位置；第三，必须保证关节变量及其前二阶时间导数的连续性，使得规划的关节轨迹是光滑的；最后，必须减少额外的运动(例如，“游移”)。 7.3关节插值轨迹12若某关节(例如关节i)的关节轨迹使用p个多项式，则要满足初始和终止条件(关节位置、速度和加速度)，并保证这些变量在多项式衔接处的连续性，因而需要确定3(p十1)个系数。 若给定附加的中间条件(例如位置)，则对每个中间条件需要增加一系数。 通常，可以给定两个中间位置，一个靠近初始位置；另一个靠近终止位置。 这样做，除了可以较好地控制运动。 外，还能保证操作机末端以适当的方向离开起点和接近终点。 因此，对于连接初始位置和终止位置的每个关节变量，一迹个七次多项式就足够了，或者用两段四次轨迹加一段三次轨迹（434），也可以用两段三次轨迹加一段五次轨迹（353），或五段三次轨迹（33333）。 p+1个点数由约束条件数所对应的多项式系数的个数确定多项式的次数13为了控制操作机，在规划运动轨迹之前，需要给定机器人在初始点和终止点的手臂形态。 在规划机器人关节插值运动轨迹时，需要注意下述几点 1、抓住一个物体时，手的运动方向应该指向离开物体支承表面的方向。 否则，手可能与支承面相碰。 2、若沿支承面的法线方向从初始点向外给定一个离开位置(提升点)，并要求手(即手部坐标系的原点)经过此位置，这种离开运动就是允许的。 如果还给定由初始点运动到离开位置的时间，我们就可以控制提起物体运动的速度。 3、对于手臂运动提升点的要求同样也适用于终止位置运动的下放点(即必须先运动到支承表面外法线方向上的某点，再慢慢下移至终止点)。 这样，可获得和控制正确的接近方向。 4、对手臂的每一次运动，都设定上述四个点初始点，提升点，下放点和终止点。 145、位置约束(a)初始点给定速度和加速度(一般为零)；(b)提升点中间点运动的连续；(c)下放点同提升点；(d)终止点给定速度和加速度(一般为零)。 156、除上述约束外，所有关节轨迹的极值不能超出每个关节变量的物理和几何极限。 7、时间的考虑(a)轨迹的初始段和终止段时间由手接近和离开支承表面的速率决定；也是由关节电机特性决定的某个常数。 (b)轨迹的中间点或中间段时间由各关节的最大速度和加速度决定，将使用这些时间中的一个最长时间(即用最低速关节确定的最长时间来归一化)。 在关节轨迹的典型约束条件之下，我们所要研究的是选择一种n次(或小于n次)的多项式函数，使得在各结点(初始点，提升点，下放点和终止点)上满足对位置、速度和加速度的要求，并使关节位置、速度和加速度在整个时间间隔t0,t f中中保持连续。 161.位置(给定)2速度(给定，通常为零)3.加速度(给定，通常为零)4.提升点位置(给定)5提升点位置（与前一段轨迹连续）6.速度（与前一段轨迹连续）7加速度（与前一段轨迹连续）8.下放点位置(给定)9下放点位置（与前一段轨迹连续）10.速度（与前一段轨迹连续）11.加速度（与前一段轨迹连续）12.位置(给定)13.速度(给定，通常为零)14.加速度(给定，通常为零)终止位置中间位置初始位置?规划关节插值轨迹的约束条件17一种方法是为每个关节规定一个七次多项式函数式中，系数a j可由已知的位置和连续条件确定。 但用这种高次多项式内插给定的结点也许不能令人满意，因为它的极值难求，而且容易产生额外的运动。 另一种办法是将整个关节空间轨迹分割成几段，在每段轨迹中用不同的低次多项式来插值。 有几种分割轨迹的方法，每种方法的特性各不相同。 ?434轨迹每个关节有下面三段轨迹第一段由初始点到提升点的轨迹用四次多项式表示。 第二段(或中间段）由提升点到下放点的轨迹用三次多项式表示。 最后一段由下放点到终止点的轨迹由四次多项式表示。 18?353轨迹与434轨迹相同，但每段所用多项式次数与前种不同。 第一段用三次多项式，第二段用五次多项式，最后一段用三次多项式。 ?(33333)轨迹对五段轨迹都使用三次多项式样条函数。 注意，上述讨论对每个关节轨迹都是有效的，即每个关节轨迹可分割成三段或五段。 7.3.1434关节轨迹的计算对于N个关节,在每段轨迹规划中就要确定N条关节轨迹，引用归一化时间变量是方便的，它使我们能用同样的方法处理每个关节每段轨迹的方程。 时间变化范围均由(各各段轨迹的初始时间)变到(各段轨迹的终止时间)。 ?0,1t?0t?1t?19定义下列变量轨迹是由多项式序列h i(t)构成的，这些多项式合起来形成关节节j的轨迹。 在每段轨迹中关节变量的多项式用归一化时间表示为:20这些关节轨迹分段多项式所应满足的边界条件为:21434关节轨迹的边界条件如图示。 22这些多项式对实际时间t的一阶和二阶导数。 可写成第一段轨迹的基本多项式是四次的23对于t=0(这段轨迹的初始位置)，要满足此位置的边界条件，则24用求出的这些量得到对于t=1(这段轨迹的终点)，在此位置，我们放宽插值多项式必须准确通过该点的要求，仅要求在此位置的速度和加速度必须和下一段轨迹起点的速度和加速度连续。 此处的速度和加速度为第二段轨迹的基本多项式是三次的25对于t=0(提升点)，此点的速度和加速度分别为由于此点的速度相加速度必须分别和前一段轨迹终点的速度和加速度连续，故可得26或或或或对于t=1(下放点)，该点的速度相加速度必须与下一段轨迹起点处的速度和加速度连续。 可分别求得此位置的速度和加速度为27末段轨迹的基本多项式是四次的如果在上式中以代替t，我们就把归一化时间t由由移至。 1t t?0,1t?1,0t?28可得对于(这段轨迹的终点)，要满足轨迹终点的边界条件，即0t?对于(这段轨迹的起点)，要满足轨迹起点的边界条件，即1t?29在此下放点的速度和加速度连续性条件是或或30可以求出相连轨迹段间关节角之差为轨迹多项式其余七个系数可由以上的速度、加速度连续约束条件的联立方程解出，用矩阵矢量符号改写这些方程，可得得其中31或或C矩阵的结构便于计算系数，若时间间隔t i为正值，C的逆矩阵总是存在的。 则关节各段关节轨迹多项式系数即可全部求得。 (1,2,)i n?32同样，我们可用此方法可计算353关节轨迹。 对于末段轨迹，把归一化时间t由-1，0重新变回0,1，可求出h n(t)334-3-4关节轨迹多项式34353-5-3关节轨迹多项式367.3.2三三次样条轨迹（五段三次多项式）用一组三次多项式插补给定的函数并保证插补点处一阶和二阶导数连续的方法，称为三次样条函数法。 所能达到的近似程度和光滑程度是相当好的。 一般来说，样条曲线是在插值点具有k-1阶导数连续性的k次多项式。 对于三次样条函数，一阶导数代表速度的连续性，二阶导数代表加速度的连续性。 三次样条函数有某些优点。 首先，它是使速度和加速度连续的最低次多项式函数。 其次，低次多项式减少了计算量和数值不稳定的可能性。 每段关节轨迹的五段三次多项式的通式为其中37在在应用五段三次多项式插值时，需要有五段轨迹和六个插值点。 但是，在前面的讨论中只有四个插值点，即初始点、提升点、下放点和终止点。 所以，必须选择另外两个插值点，以便有足够多的边界条件求解各多项式系数。 可以在提升点和下放点之间选取这两个额外的结点。 没有必要知道这两个点的确切位置，只要知道时间间隔，以及必须满足这两点速度和加速度的连续性条件。 因此，这组关节轨迹分段多项式必须满足的边界条件是 (1)在初始点、提升点、下放点和终止点的位置约束； (2)在所有插值点的速度和加速度的连续性。 38五段三次关节轨迹的边界条件示于中。 其中有下划线的变量是在计算五段三次多项式之前的已知量。 39这些多项式对实际时间的一阶和二阶导数为式中，t j是通过第j段轨迹所需的实际时间。 给定了初始点和终止点的位置、速度和加速度，第一段轨迹和末段轨迹的多项式式h1(t)和h n(t)就完全确定了。 旦算出这两个多项式，就可用位置约束和连续条件求出h2(t)、h3(t)和h4(t)。 第一段轨迹，基本多项式为当当t=0时，由该位置的边界条件，可得(给定)40由此可得当当t=1时，由此位置的边界条件，可得可得其中，i=ii-1。 第一段轨迹的多项式就完全确定了。 (给定)41由上式可求出t1时的速度和加速度。 此速度相加速度必须与下一段轨迹起点的速度和加速度连续。 末段轨迹的多项式为（仍做类似代换）(给定)(给定)1t t?当当t0和t1时（对应和），由边界条件，有1t?0t?42由上述方程解出系数，最后得到给定初始点、提升点、下放点和终止点的位置以及通过每段轨迹所需要的时间(t j)后，五段三次多项式即可唯一地确定，满足所有位置约束和连续条件。 算出以上个多项式，就可用位置约束和连续条件求出h2(t)、h3(t)和h4(t)。 43对于较复杂的机器人系统，为了控制操作机完成作业而设部计了编程语言。 在这种系统中，作业通常是用操作机手部(或或末端执行器)必须通过的笛卡尔结点序列给定的。 因此，描述操作机在作业中的运动时，我们就更关心描述操作机手部要达到的目标位置及通过的笛卡尔空间曲线(或路径)的形式。 7.4笛卡尔路径轨迹规划一般，实现笛卡尔路径规划可采用下述两个相连的步骤 (1)沿笛卡尔路径，按照某种规则以笛卡尔坐标生成或选择；一组结点或插值点； (2)规定一种函数，按某些准则连接这些结点(或逼近分段的路径)。 对于第二步，所选用的准则可以采用两种控制算法，以保证跟踪给定的路径。 44 (1)面向笛卡尔空间的方法。 在此方法中，大部分计算和优化是以笛卡尔坐标完成的，然后，在手部这一级上进行控制。 按固定的取样间隔在预定路径上选择伺服取样点，在控制操作机时实时地把它们转换为与之相应的关节变量。 所得到的轨迹是分段直线。 (2)面向关节空间的方法。 这种方法用关节变量空间中的低次多项式函数迫近直线路径上的两相邻结点间的一段路径，而控制是在关节这一级上进行的。 所得到的笛卡尔路径是不分段的直线。 Paul叙述了用一系列直线段构成操作机手部笛卡尔路径的设计方法。 把手部在这些直线段中的速度和加速度转换到关节坐标，并用二次插值进行平滑连接，从而实现了手部的规划运动控制。 我们将学习如何用该方法设计笛卡尔直线路径。 45对于笛卡尔路径控制，轨迹设定点算法为0tt?循环等待下一个控制间隔；时刻操作机末端所处的位置；相应于的关节变量解；若，则退出循环；转向循环。 ttt?()H tt?ft t?()Q H t?()H t这里，除了需要计算每一控制间隔中操作机手部轨迹函数外，还需把笛卡尔位置变换为相应的关节变量。 矩阵函数表示操作机手部在时刻的预定位置。 可用44齐次变换矩阵表示。 ()Ht?()Q Ht()H tt46在可编程机器人系统中，预定的运动可用笛卡尔结点序列给定，相应于这些笛卡尔结点的关节坐标可用运动学逆问题求解程序算出，并用二次多项式平滑关节坐标中两个相邻的关节结点，以利于控制，从而，控制操作机手部沿连接这些结点的直线运动。 这种方法的优点是，它使我们能控制操作机手部跟踪运动的物体。 虽然目标位置是用变换描述的，但它们并不规定操作机手部怎样由一个变换运动到另一个变换。 Paul用一个直线平移和两个转动来完成两相邻笛卡尔结点间的运动。 第一个转动是绕单位矢量k进行的，把工具或末端执行器的轴线调整到预定的接近方向；而第二个转动则绕工具轴调整工具的方向。 7.4.1齐次变换矩阵法47一般来说，操作机的目标位置可用如下基本矩阵方程表示060base6tool base objTT=C(t)P其中，0T6为描述操作机手部相对于机座坐标系位姿的齐次变换矩阵；6T tool为描述工具相对于手部坐标系位姿的齐次变换矩阵，它描述了受控制的工具端点；0C base(t)为描述物体位姿的工作坐标系相对于机座坐标系的齐次变换矩阵；base Pobj为描述末端执行器相对于工作坐标系预定抓取物体的位姿的齐次变换矩阵。 左边的矩阵描述操作机抓取位姿，而右边的矩阵描述了我们要让操作机工具抓握物体特征部位的位姿。 0C base(t)B060base6tool baseobjTT=C(t)P060base6tool baseobjTT=C(t)T060base6tool baseobjTT=C(t)Tobj tool48我们可以解出0T6，它描述了以正确和预定的方式抓取物体时操作机应有的形态。 1?00base66baseobjtoolT=C(t)PT如果计算0T6的速度足够高，并转换成相应的关节角，那么就可控制操作机沿预定的轨迹运动利用前式，确定一项作业的N个目标位置的序列可表示为49由由C i(t)P i确定的位置，可求得相邻点之间的距离。 若进一步给定线速度和角速度，即可求得由位置i到i+1所需的时间。 以上位置的工具和运动坐标系都是相对于机座坐标系给定的。 从一个位置移向下一个位置最好是相对于运动坐标系给定位置和工具。 这样，从运动坐标系观察，工具是静止的。 为了做到这一点，需要相对于后续的坐标系重新定义当前的位置和工具。 利用双下标符号P ij重新定义P i变换，就不难做到这一点。 P ij是在第j个运动坐标系中表示的位置P i。 则在位置1，将它在目标坐标系中表示，可得1111tool6T T=C(t)P2212tool6T T=C(t)P将它用位置2时的坐标系表示，则有50由以上两个方程即可求得1112211112()?tool toolP=C(t)C(t)PT T上述方程的目的是要在已知P11时时求得P12。 因此，在任意二相邻位置i和i+1之间的运动有111,11,11()()()tool toolii i iii iiP CtCtPT T?其中P i,i+1和和P i+1,i+1为上面所讨论过的变换。 Paul使用了一种简单的方法，以控制操作机手部从一个变换运动到另一个变换。 此法包括一个平移运动和一个绕空间某定轴的转动以及另一个绕工具轴的转动，以便形成操作机受控的线速度和角速度运动。 第一个转动使工具轴与预定的接近方向对准，第二个转动使工具轴与方向矢量对准。 51由位置i到i+1的运动可用“驱动”变换D()表示，它是归一化时间的函数161,11()()()()tooli ii iT CPDT?其中，1,1,1 (1)iiiiP PD?1,11,1 (1)()iiiiD PP?可得t?t于是在位置i时，=0，D (0)为单位矩阵52用位置i和i+1各自的齐次变换矩阵表示它们，可得利用前式，求P i,i+1的逆并乘以P i+1,i+1，得到53若驱动函数由一个平移运动和两个旋转运动构成，那么这些平移和转动量将直接同成正比。 若随时间线性地变化，则D()所代表的合成运动将相应于一个不变的线速度和两个角速度。 平移运动可由齐次变换矩阵L()表示；第一个转动可用齐次变换矩阵R A()表示，它的作用是把P i位置的接近矢量转向P i+1位置的接近矢量；第二个转动可用R B()表示，它的作用是把P i位置的方向矢量绕工具轴转向P i+1位置的方向矢量。 因此，驱动函数可表示为54其中，旋转矩阵R A()表示绕P i的接近矢量（z i轴）转动了角，又绕方向矢量（y i轴）转动了角；R B()表示绕工具的接近矢量转动角的旋转。 ?55其中，56操作机经常要沿相连的直线段轨迹运动，以满足作业运动的要求或避开障碍物。 为了避免在