第05章-机器人轨迹规划

1,第五章机器人的轨迹规划,5.1机器人规划的定义和作用,5.1.1概述,所谓机器人的规划(P1anning)，指的是机器人根据自身的任务，求得完成这一任务的解决方案的过程。这里所说的任务，具有广义的概念，既可以指机器人要完成的某一具体任务，也可以是机器人的某个动作，比如手部或关节的某个规定的运动等。,2,为说明机器人规划的概念，我们举下面的例子：在一些老龄化比较严重的国家，开发了各种各样的机器人专门用于伺候老人，这些机器人有不少是采用声控的方式比如主人用声音命令机器人“给我倒一杯开水”，我们先不考虑机器人是如何识别人的自然语言，而是着重分析一下机器人在得到这样一个命今后，如何来完成主人交给的任务。,3,首先，机器人应该把任务进行分解，把主人交代的任务分解成为“取一个杯子”、“找到水壶”、“打开瓶塞”、“把水倒人杯中”、“把水送给主人”等一系列子任务。这一层次的规划称为任务规划(Taskplanning)，它完成总体任务的分解。然后再针对每一个子任务进行进一步的规划。以“把水倒入杯中”这一子任务为例，可以进一步分解成为“把水壶提到杯口上方”、“把水壶倾斜倒水入杯”、“把水壶竖直“、“把水壶放回原处”等一系列动作，这一层次的规划称为动作规划(MotionP1anning)，它把实现每一个子任务的过程分解为一系列具体的动作。为了实现每一个动作，需要对手部的运动轨迹进行必要的规定，这是手部轨迹规划(Handtrajectoryplanning)。为了使手部实现预定的运动，就要知道各关节的运动规律，这是关节轨迹规划(Jointtrajectoryplanning)。最后才是关节的运动控制(Motioncontrol)。,4,上述例子可以看出，机器人的规划是分层次的，从高层的任务规划，动作规划到手部轨迹规划和关节轨迹规划，最后才是底层的控制(见图)。在上述例子中，我们没有讨论力的问题，实际上，对有些机器人来说，力的大小也是要控制的，这时，除了手部或关节的轨迹规划，还要进行手部和关节输出力的规划。智能化程度越高，规划的层次越多，操作就越简单。对工业机器人来说，高层的任务规划和动作规划一般是依赖人来完成的。而且一般的工业机器人也不具备力的反馈，所以，工业机器人通常只具有轨迹规划的和底层的控制功能。,5,取一个杯子,找到水壶,打开水壶,把水倒入杯中,把水送给主人,给主人倒一杯水,提起水壶到杯口上方,把水壶倾斜,把水壶竖直,把水壶放回原处,手部从A点移到B点,关节从A点移到B点,图智能机器人的规划层次,6,许多规划所包的步骤是含糊的，而且需要进一步说明（子规划）。大多数规划具有很大的子规划结构，规划中的每个目标可以由达到此目标的比较详细的子规划所代替。因此，最终得到的规划是某个问题求解算符的线性或分部排序，但是由算符来实现的目标常常具有分层结构。把某些比较复杂的问题分解为一些比较小的问题的想法使我们应用规划方法求解问题在实际上成为可能。有两条能够实现这种分解的重要途径：第一条是当从一个问题状态移动到下一个状态时，无需计算整个新的状态，而只要考虑状态中可能变化了的那些部分。第二条是把单一的困难问题分割为几个有希望的、较为容易解决的子问题，这种分解能够使困难问题的求解变得容易些。,7,机器人规划分为高层规划和低层规划。自动规划在机器人规划中称为高层规划。在无特别说明时，机器人规划都是指自动规划。自动规划是一种重要的问题求解技术，它从某个特定的问题状态出发，寻求一系列行为动作，并建立一个操作序列，直到求得目标状态为止。与一般问题求解相比，自动规划更注重于问题的求解过程，而不是求解结果。我们在阐述机器人自动规划问题时，机器人一般配备有传感器和一组能在某个易于理解的现场中完成的基本动作。这些动作可把该现场从一种状态或布局变换为另一种状态或布局。,8,机器人规划是机器人学的一个重要研究领域，也是人工智能与机器人学一个令人感兴趣的结合点。,机器人学中的一个基本问题是为解决某个预定的任务而规划机器人的动作，然后在机器人执行完成那些动作所需的命令时控制它。这里，规划的意思就是机器人在行动前确定一系列动作(作决策)，这种动作的确定可用问题求解系统来解决，给定初始情况后，该系统可达到某一规定的目标。因此，规划就是指机器人为达到目标而需要的行动过程的描述。规划内容可能没有次序，但是一般来说，规划具有某个规划目标的蕴含排序。例如，早晨起床后的安排。缺乏规划可能导致不是最佳的问题求解，甚至得不到问题的求解。,9,机器人的规划程序只需要给定任务初始状态和最终状态的描述。这些规划系统一般都不说明为实现一个算符所需的详细的机器人运动。任务规划程序则把任务级的说明变换成操作机级的说明。为了进行这种变换，任务规划程序必须包含被操作物体、任务环境、机器人执行的任务、环境的初始状态和所要求的最终(目标)状态等描述。任务规划程序的输出就是一个机器人程序，在指定的初始状态下执行该程序后，就能达到所要求的最终状态。任务规划有三个阶段：建立模型、任务说明和操作机程序综合。任务的世界模型应含有如下的信息:(1)任务环境中的所有物体和机器人的几何描述；(2)所有物体的物理描述；(3)所有连接件的运动学描述，(4)机器人和传感器特性的描述。在世界模型中，任务状态模型还必包括全部物体和连接件的布局。,5.1.2机器人任务规划的作用,10,轨迹规划的目的是将操作人员输入的简单的任务描述变为详细的运动轨迹描述。例如，对一般的工业机器人来说，操作员可能只输入机械手末端的目标位置和方位，而规划的任务便是要确定出达到目标的关节轨迹的形状、运动的时间和速度等。这里所说的轨迹是指随时间变化的位置、速度和加速度。,11,简言之，机器人的工作过程，就是通过规划，将要求的任务变为期望的运动和力，由控制环节根据期望的运动和力的信号，产生相应的控制作用，以使机器人输出实际的运动和力，从而完成期望的任务。这一过程表述如下图所示。这里，机器人实际运动的情况通常还要反馈给规划级和控制级，以便对规划和控制的结果做出适当的修正。,图机器人的工作原理示意图,12,上图中，要求的任务由操作人员输入给机器人，为了使机器人操作方便、使用简单，必须允许操作人员给出尽量简单的描述。上图中，期望的运动和力是进行机器人控制所必需的输入量，它们是机械手末端在每一个时刻的位姿和速度，对于绝大多数情况，还要求给出每一时刻期望的关节位移和速度，有些控制方法还要求给出期望的加速度等。,13,对于PTP控制:通常只给出机械手末端的起点和终点，有时也给出一些中间经过点，所有这些点统称为路径点。应注意这里所说的“点”不仅包括机械手末端的位置，而且包括方位，因此描述一个点通常需要6个量。通常希望机械手末端的运动是光滑的，即它具有连续的一阶导数，有时甚至要求具有连续的二阶导数。不平滑的运动容易造成机构的磨损和破坏，甚至可能激发机械手的振动。因此规划的任务便是要根据给定的路径点规划出通过这些点的光滑的运动轨迹。,14,对于CP控制:机械手末端的运动轨迹是根据任务的需要给定的，但是它也必须按照一定的采样间隔，通过逆运动学计算，将其变换到关节空间，然后在关节空间中寻找光滑函数来拟合这些离散点最后，还有在机器人的计算机内部如何表示轨迹，以及如何实时地生成轨迹的问题。,15,机器人轨迹规划属于机器人低层规划，基本上不涉及人工智能问题，而是在机械手运动学和动力学的基础上，讨论机器人运动的规划及其方法。所谓轨迹，就是指机器人在运动过程中的位移、速度和加速度。,轨迹规划问题通常是将轨迹规划器看成“黑箱”，接受表示路径约束的输入变量，输出为起点和终点之间按时间排列的操作机中间形态（位姿,速度和加速度）序列。,5.2机器人轨迹规划的一般性问题,由初始点运动到终止点，所经过的由中间形态序列构成的空间曲线称为路径。这些形态序列即是曲线上的“点”。,16,规划操作机的轨迹有两种常用的方法：第一种方法：要求使用者在沿轨迹选定的位置点上(称为结节或插值点)显式地给定广义坐标位置、速度和加速度的一组约束(例如，连续性和光滑程度等)。然后，轨迹规划器从插值和满足插值点约束的函数中选定参数化轨迹。显然，在这种方法中，约束的给定和操作机轨迹规划是在关节坐标系中进行的。第二种方法：使用者以解析函数显式地给定操作机必经之路径，例如，笛卡尔坐标中的直线路径。然后，轨迹规划器在关节坐标或笛卡几坐标中确定一条与给定路径近似的轨迹。在这种方法中，路径约束是在笛卡尔坐标中给定的。,17,在第一种方法中，约束的给定和操作机轨迹规划在关节坐标系中进行。由于对操作机手部没有约束，使用者难于跟踪操作机手部运行的路径。因此，操作机手部可能在没有事先警告的情况下与障碍物相碰。在第二种方法中，路径约束在笛卡尔坐标中给定，而关节驱动器是在关节坐标中受控制的。因此，为了求得一条逼近给定路径的轨迹，必须用函数近似把笛卡尔坐标中的路径约束变换为关节坐标中的路径约束，再确定满足关节坐标路径约束的参数化轨迹。轨迹规划既可在关节变量空间中进行，也可在笛卡尔空间进行。对于关节变量空间的规划，要规划关节变量的时间函数及其前二阶时间导数，以便描述操作机的预定运动。在笛卡尔空间规划中，要规划操作机手部位置、速度和加速度的时间函数，而相应的关节位置、速度和加速度可根据手部信息导出。,18,面向笛卡尔空间方法的优点是概念直观，而且沿预定直线路径可达到相当的准确性。可是由于现代还没有可用笛卡尔坐标测量操作机手部位置的传感器，所有可用的控制算法都是建立在关节坐标基础上的。因此，笛卡尔空间路径规划就需要在笛卡尔坐标和关节之间进行实时变换，这是一个计算量很大的任务，常常导致较长的控制间隔。由笛卡尔坐标向关节坐标的变换是病态的，因而它不是一一对应的映射。如果在轨迹规划阶段要考虑操作机的动力学特性，就要以笛卡尔坐标给定路径约束，同时以关节坐标给定物理约束(例如，每个关节电机的力和力矩、速度和加速度权限)。这就会使最后的优化问题具有在两个不同坐标系中的混合约束。,在笛卡尔空间进行轨迹规划的特点：,19,在关节变量空间的规划有三个优点：直接用运动时的受控变量规划轨迹；轨迹规划可接近实时地进行；关节轨迹易于规划。伴随的缺点是难于确定运动中各杆件和手的位置，但是，为了避开轨迹上的障碍常常又要求知道一些杆件和手位置。,20,由于面向笛卡尔空间的方法有前述钟种缺点，使得面向关节空间的方法被广泛采用。它把笛卡尔结点变换为相应的关节坐标，并用低次多项式内插这些关节结点。这种方法的优点是计算较快，而且易于处理操作机的动力学约束。但当取样点落在拟合的光滑多项式曲线上时，面向关节空间的方法沿笛卡尔路径的准确性会有损失。,21,5.3关节空间法轨迹规划问题又可以分为关节空间的轨迹规划和直角空间的轨迹规划。关节空间法首先将在工具工作空间中期望的路径点，通过逆运动学计算，得到期望的关节位置，然后在关节空间内，给每个关节找到一个经过中间点到达目的终点的光滑函数，同时使得每个关节到达中间点和终点的时间相同，这样便可保证机械手工具能够到达期望的直角坐标位置。这里只要求各个关节在路径点之间的时间相同，而各个关节的光滑函数的确定则是互相独立的。,22,一般说来，生成关节轨迹设定点的基本算法是很简单的：,循环：等待下一个控制间隔；时刻操作机关节所处的位置；若，则退出循环；转向循环。,从上述算法可以看出，要计算的是在每个控制间隔中必须更新的轨迹函数(或轨迹规划器)h(t)。因此，对规划的轨迹要提出四个限制。第一，必须便于用迭代方式计算轨迹设定点；第二，必须求出并明确给定中间位置；第三，必须保证关节变量及其前二阶时间导数的连续性，使得规划的关节轨迹是光滑的；最后，必须减少额外的运动(例如，“游移”)。,23,若某关节(例如关节i)的关节轨迹使用p个多项式，则要满足初始和终止条件(关节位置、速度和加速度)，并保证这些变量在多项式衔接处的连续性，因而需要确定3(p十1)个系数。若给定附加的中间条件(例如位置)，则对每个中间条件需要增加一系数。通常，可以给定两个中间位置，一个靠近初始位置；另一个靠近终止位置。这样做，除了可以较好地控制运动外，还能保证操作机末端以适当的方向离开起点和接近终点。因此，对于连接初始位置和终止位置的每个关节变量，一个七次多项式就足够了，或者用两段四次轨迹加一段三次轨迹（434），也可以用两段三次轨迹加一段五次轨迹（353），或五段三次轨迹（33333）。,由约束条件数所对应的多项式系数的个数确定多项式的次数,24,为了控制操作机，在规划运动轨迹之前，需要给定机器人在初始点和终止点的手臂形态。在规划机器人关节插值运动轨迹时，需要注意下述几点：1、抓住一个物体时，手的运动方向应该指向离开物体支承表面的方向。否则，手可能与支承面相碰。2、若沿支承面的法线方向从初始点向外给定一个离开位置(提升点)，并要求手(即手部坐标系的原点)经过此位置，这种离开运动就是允许的。如果还给定由初始点运动到离开位置的时间，我们就可以控制提起物体运动的速度。3、对于手臂运动提升点的要求同样也适用于终止位置运动的下放点(即必须先运动到支承表面外法线方向上的某点，再慢慢下移至终止点)。这样，可获得和控制正确的接近方向。4、对手臂的每一次运动，都设定上述四个点：初始点，提升点，下放点和终止点。,25,5、位置约束(a)初始点：给定速度和加速度(一般为零)；(b)提升点：中间点运动的连续；(c)下放点：同提升点；(d)终止点：给定速度和加速度(一般为零)。,26,6、除上述约束外，所有关节轨迹的极值不能超出每个关节变量的物理和几何极限。7、时间的考虑(a)轨迹的初始段和终止段：时间由手接近和离开支承表面的速率决定；也是由关节电机特性决定的某个常数。(b)轨迹的中间点或中间段：时间由各关节的最大速度和加速度决定，将使用这些时间中的一个最长时间(即用最低速关节确定的最长时间来归一化)。,在关节轨迹的典型约束条件之下，我们所要研究的是选择一种n次(或小于n次)的多项式函数，使得在各结点(初始点，提升点，下放点和终止点)上满足对位置、速度和加速度的要求，并使关节位置、速度和加速度在整个时间间隔t0,tf中保持连续。,27,规划关节插值轨迹的约束条件：,28,一种方法是为每个关节规定一个七次多项式函数,式中，未知系数aj可由已知的位置和连续条件确定。但用这种高次多项式内插给定的结点也许不能令人满意，因为它的极值难求，而且容易产生额外的运动。另一种办法是将整个关节空间轨迹分割成几段，在每段轨迹中用不同的低次多项式来插值。有几种分割轨迹的方法，每种方法的特性各不相同。,434轨迹每个关节有下面三段轨迹：第一段由初始点到提升点的轨迹用四次多项式表示。第二段(或中间段）由提升点到下放点的轨迹用三次多项式表示。最后一段由下放点到终止点的轨迹由四次多项式表示。,29,353轨迹与434轨迹相同，但每段所用多项式次数与前种不同。第一段用三次多项式，第二段用五次多项式，最后一段用三次多项式。(33333)轨迹对五段轨迹都使用三次多项式样条函数。注意，上述讨论对每个关节轨迹都是有效的，即每个关节轨迹可分割成三段或五段。,30,下面具体介绍在关节空间内常用的规划方法5.3.1三次多项式函数插值考虑机械手末端在一定时间内从初始位置和方位移动到目标位置和方位的问题。利用逆运动学计算，可以首先求出一组起始和终了的关节位置现在的问题是求出一组通过起点和终点的光滑函数。满足这个条件的光滑函数可以有许多条，如下图所示：,31,显然，这些光滑函数必须满足以下条件：,同时若要求在起点和终点的速度为零，即：,那么可以选择如下的三次多项式：,满足起点和终点的关节角度约束,满足起点和终点的关节速度约束（满足关节速度的连续性要求）,32,作为所要求的光滑函数。式4-3中有4个待定系数，而该式需满足式4-1和4-2的4个约束条件，因此可以唯一地解出这些系数：,注意：这组解只适用于关节起点、终点速度为零的运动情况。,33,例：设机械手的某个关节的起始关节角0150，并且机械手原来是静止的。要求在3秒钟内平滑地运动到f=750时停下来(即要求在终端时速度为零)。规划出满足上述条件的平滑运动的轨迹，并画出关节角位置、角速度及角加速度随时间变化的曲线。解：根据所给约束条件，直接代入式(4-4)，可得：a0=15，a1=0，a2=20，a3=-4.44则关节角的位置函数可求，对位置求导，可以得到角速度和角加速度,34,画出它们随时间的变化曲线如下图所示。由图看出，速度曲线为一抛物线，加速度则为一直线。,利用三次多项式规划出的关节角的运动轨迹,35,5.3.2过路径点的三次多项式插值,方法是：把所有路径点都看成是“起点”或“终点”，求解逆运动学，得到相应的关节矢量值。然后确定所要求的三次多项式插值函数，把路径点平滑的连接起来。不同的是，这些“起点”和“终点”的关节速度不再是零（即不在路径点停留）。,36,由上式确定的三次多项式描述了起始点和终止点具有任意给定位置和速度的运动轨迹。剩下的问题就是如何确定路径点上的关节速度。,同理可以求得此时的三次多项式系数：,37,（1）根据工具坐标系在直角坐标空间中的瞬时线速度和角速度来确定每个路径点的关节速度；该方法工作量大。,（2）为了保证每个路径点上的加速度连续，由控制系统按照此要求自动地选择路径点的速度。,（3）在直角坐标空间或关节空间中采用某种适当的启发式方法，由控制系统自动地选择路径点的速度；,38,对于方法（2），为了保证路径点处的加速度连续，可以设法用两条三次曲线在路径点处按照一定的规则联系起来，拼凑成所要求的轨迹。其约束条件是：联接处不仅速度连续，而且加速度也要连续。,39,对于方法（3），这里所说的启发式方法很简单，即假设用直线段把这些路径点依次连接起来，如果相邻线段的斜率在路径点处改变符号，则把速度选定为零；如果相邻线段不改变符号，则选择路径点两侧的线段斜率的平均值作为该点的速度。,40,如果对于运动轨迹的要求更为严格，约束条件增多，那么三次多项式就不能满足需要，必须用更高阶的多项式对运动轨迹的路径段进行插值。例如，对某段路径的起点和终点都规定了关节的位置、速度和加速度（有六个未知的系数），则要用一个五次多项式进行插值。,41,5.3.3抛物线连接的线性函数插值前面介绍了利用三次多项式函数插值的规划方法。另外一种常用方法是线性函数插值法，即用一条直线将起点与终点连接起来。但是，简单的线性函数插值将使得关节的运动速度在起点和终点处不连续，它也意味着需要产生无穷大的加速度，这显然是不希望的。因此可以考虑在起点和终点处，用抛物线与直线连接起来，在抛物线段内，使用恒定的加速度来平滑地改变速度，从而使得整个运动轨迹的位置和速度是连续的。,42,线性函数插值图,利用抛物线过渡的线性函数插值图,43,单纯线性插值将导致在结点处关节运动速度不连续，加速度无限大。,44,解决办法：在使用线性插值时，把每个结点的邻域内增加一段抛物线的“缓冲区段”，从而使整个轨迹上的位移和速度都连续。,45,对于多解情况，如下图所示。加速度的值越大，过渡长度越短。,46,5.3.4过路径点的用抛物线过渡的线性插值,如图所示，某个关节在运动中设有n个路径点，其中三个相邻的路径点表示为j，k和l，每两个相邻的路径点之间都以线性函数相连，而所有的路径点附近则有抛物线过渡。（同样存在多解）,47,5.4.1直角坐标空间法,前面介绍的在关节空间内的规划，可以保证运动轨迹经过给定的路径点。但是在直角坐标空间，路径点之间的轨迹形状往往是十分复杂的，它取决于机械手的运动学机构特性。在有些情况下，对机械手末端的轨迹形状也有一定要求，如要求它在两点之间走一条直线，或者沿着一个圆弧运动以绕过障碍物等。这时便需要在直角坐标空间内规划机械手的运动轨迹直角坐标空间的路径点，指的是机械手末端的工具坐标相对于基坐标的位置和姿态每一个点由6个量组成，其中3个量描述位置，另外3个量描述姿态。在直角坐标空间内规划的方法主要有：线性函数插值法和圆弧插值法。,48,5.5路