七章机器人的轨迹规划.ppt

《七章机器人的轨迹规划.ppt》由会员分享，可在线阅读，更多相关《七章机器人的轨迹规划.ppt（62页珍藏版）》请在三一文库上搜索。

1、1,第七章 机器人的轨迹规划,7.1 机器人规划的定义和作用,7.1.1 概述,机器人学中的一个基本问题是为解决某个预定的任务而规划机器人的动作，然后在机器人执行完成那些动作所需的命令时控制它。这里，规划的意思就是机器人在行动前确定一系列动作(作决策)，这种动作的确定可用问题求解系统来解决，给定初始情况后，该系统可达到某一规定的目标。因此，规划就是指机器人为达到目标而需要的行动过程的描述。 规划内容可能没有次序，但是一般来说，规划具有某个规划目标的蕴含排序。例如，早晨起床后的安排。 缺乏规划可能导致不是最佳的问题求解，甚至得不到问题的求解。,2,许多规划所包的步骤是含糊的，而且需要进一步说明（

2、子规划）。大多数规划具有很大的子规划结构，规划中的每个目标可以由达到此目标的比较详细的子规划所代替。因此，最终得到的规划是某个问题求解算符的线性或分部排序，但是由算符来实现的目标常常具有分层结构。 把某些比较复杂的问题分解为一些比较小的问题的想法使我们应用规划方法求解问题在实际上成为可能。 有两条能够实现这种分解的重要途径：第一条是当从一个问题状态移动到下一个状态时，无需计算整个新的状态，而只要考虑状态中可能变化了的那些部分。第二条是把单一的困难问题分割为几个有希望的、较为容易解决的子问题，这种分解能够使困难问题的求解变得容易些。,3,机器人规划分为高层规划和低层规划。自动规划在机器人规划中称

3、为高层规划。在无特别说明时，机器人规划都是指自动规划。自动规划是一种重要的问题求解技术，它从某个特定的问题状态出发，寻求一系列行为动作，并建立一个操作序列，直到求得目标状态为止。与一般问题求解相比，自动规划更注重于问题的求解过程，而不是求解结果。 我们在阐述机器人自动规划问题时，机器人一般配备有传感器和一组能在某个易于理解的现场中完成的基本动作。这些动作可把该现场从一种状态或布局变换为另一种状态或布局。例如， “积木世界” 。,4,机器人能得到的一个解答是由下面的算符序列组成的：,机器人规划是机器人学的一个重要研究领域，也是人工智能与机器人学一个令人感兴趣的结合点。,5,机器人的规划程序只需要

4、给定任务初始状态和最终状态的描述。这些规划系统一般都不说明为实现一个算符所需的详细的机器人运动。任务规划程序则把任务级的说明变换成操作机级的说明。为了进行这种变换，任务规划程序必须包含被操作物体、任务环境、机器人执行的任务、环境的初始状态和所要求的最终(目标)状态等描述。任务规划程序的输出就是一个机器人程序，在指定的初始状态下执行该程序后，就能达到所要求的最终状态。 任务规划有三个阶段：建立模型、任务说明和操作机程序综合。任务的世界模型应含有如下的信息:(1)任务环境中的所有物体和机器人的几何描述；(2)所有物体的物理描述；(3)所有连接件的运动学描述，(4) 机器人和传感器特性的描述。在世界

5、模型中，任务状态模型还必包括全部物体和连接件的布局。,7.1.2 机器人任务规划的作用,6,机器人轨迹规划属于机器人低层规划，基本上不涉及人工智能问题，而是在机械手运动学和动力学的基础上，讨论机器人运动的规划及其方法。所谓轨迹，就是指机器人在运动过程中的位移、速度和加速度。,轨迹规划问题通常是将轨迹规划器看成“黑箱”，接受表示路径约束的输入变量，输出为起点和终点之间按时间排列的操作机中间形态（位姿, 速度和加速度）序列。,7.2 机器人轨迹规划的一般性问题,由初始点运动到终止点，所经过的由中间形态序列构成的空间曲线称为路径。这些形态序列即是曲线上的“点”。,7,规划操作机的轨迹有两种常用的方法

6、： 第一种方法：要求使用者在沿轨迹选定的位置点上(称为结节或插值点)显式地给定广义坐标位置、速度和加速度的一组约束(例如，连续性和光滑程度等)。然后，轨迹规划器从插值和满足插值点约束的函数中选定参数化轨迹。显然，在这种方法中，约束的给定和操作机轨迹规划是在关节坐标系中进行的。 第二种方法：使用者以解析函数显式地给定操作机必经之路径，例如，笛卡尔坐标中的直线路径。然后，轨迹规划器在关节坐标或笛卡几坐标中确定一条与给定路径近似的轨迹。在这种方法中，路径约束是在笛卡尔坐标中给定的。,8,在第一种方法中，约束的给定和操作机轨迹规划在关节坐标系中进行。由于对操作机手部没有约束，使用者难于跟踪操作机手部运

7、行的路径。因此，操作机手部可能在没有事先警告的情况下与障碍物相碰。 在第二种方法中，路径约束在笛卡尔坐标中给定，而关节驱动器是在关节坐标中受控制的。因此，为了求得一条逼近给定路径的轨迹，必须用函数近似把笛卡尔坐标中的路径约束变换为关节坐标中的路径约束，再确定满足关节坐标路径约束的参数化轨迹。 轨迹规划既可在关节变量空间中进行，也可在笛卡尔空间进行。对于关节变量空间的规划，要规划关节变量的时间函数及其前二阶时间导数，以便描述操作机的预定运动。在笛卡尔空间规划中，要规划操作机手部位置、速度和加速度的时间函数，而相应的关节位置、速度和加速度可根据手部信息导出。,9,面向笛卡尔空间方法的优点是概念直观

8、，而且沿预定直线路径可达到相当的准确性。可是由于现代还没有可用笛卡尔坐标测量操作机手部位置的传感器，所有可用的控制算法都是建立在关节坐标基础上的。因此，笛卡尔空间路径规划就需要在笛卡尔坐标和关节之间进行实时变换，这是一个计算量很大的任务，常常导致较长的控制间隔。 由笛卡尔坐标向关节坐标的变换是病态的，因而它不是一一对应的映射。 如果在轨迹规划阶段要考虑操作机的动力学特性，就要以笛卡尔坐标给定路径约束，同时以关节坐标给定物理约束(例如，每个关节电机的力和力矩、速度和加速度权限)。这就会使最后的优化问题具有在两个不同坐标系中的混合约束。,在笛卡尔空间进行轨迹规划的特点：,10,在关节变量空间的规划

9、有三个优点： 直接用运动时的受控变量规划轨迹； 轨迹规划可接近实时地进行； 关节轨迹易于规划。 伴随的缺点是难于确定运动中各杆件和手的位置，但是，为 了避开轨迹上的障碍常常又要求知道一些杆件和手位置。,由于面向笛卡尔空间的方法有前述钟种缺点，使得面向关节空间的方法被广泛采用。它把笛卡尔结点变换为相应的关节坐标，并用低次多项式内插这些关节结点。这种方法的优点是计算较快，而且易于处理操作机的动力学约束。但当取样点落在拟合的光滑多项式曲线上时，面向关节空间的方法沿笛卡尔路径的准确性会有损失。,11,一般说来，生成关节轨迹设定点的基本算法是很简单的：,循环：等待下一个控制间隔； 时刻操作机关节所处的位

10、置； 若 ，则退出循环； 转向循环。,从上述算法可以看出，要计算的是在每个控制间隔中必须更新的轨迹函数(或轨迹规划器)h(t)。因此，对规划的轨迹要提出四个限制。第一，必须便于用迭代方式计算轨迹设定点；第二，必须求出并明确给定中间位置；第三，必须保证关节变量及其前二阶时间导数的连续性，使得规划的关节轨迹是光滑的；最后，必须减少额外的运动 (例如，“游移” )。,7.3 关节插值轨迹,12,若某关节(例如关节 i )的关节轨迹使用 p 个多项式，则要满足初始和终止条件(关节位置、速度和加速度)，并保证这些变量在多项式衔接处的连续性，因而需要确定 3( p十1)个系数。 若给定附加的中间条件(例如

11、位置)，则对每个中间条件需要增加一系数。通常，可以给定两个中间位置，一个靠近初始位置；另一个靠近终止位置。这样做，除了可以较好地控制运动外，还能保证操作机末端以适当的方向离开起点和接近终点。 因此，对于连接初始位置和终止位置的每个关节变量，一个七次多项式就足够了，或者用两段四次轨迹加一段三次轨迹 （434），也可以用两段三次轨迹加一段五次轨迹（353），或五段三次轨迹（33333）。,由约束条件数所对应的多项式系数的个数确定多项式的次数,13,为了控制操作机，在规划运动轨迹之前，需要给定机器人在初始点和终止点的手臂形态。在规划机器人关节插值运动轨迹时，需要注意下述几点： 1、抓住一个物体时，手

12、的运动方向应该指向离开物体支承表面的方向。否则，手可能与支承面相碰。 2、若沿支承面的法线方向从初始点向外给定一个离开位置(提升点)，并要求手(即手部坐标系的原点)经过此位置，这种离开运动就是允许的。如果还给定由初始点运动到离开位置的时间，我们就可以控制提起物体运动的速度。 3、对于手臂运动提升点的要求同样也适用于终止位置运动的下放点(即必须先运动到支承表面外法线方向上的某点，再慢慢下移至终止点)。这样，可获得和控制正确的接近方向。 4、对手臂的每一次运动，都设定上述四个点：初始点，提升点，下放点和终止点。,14,5、位置约束 (a) 初始点：给定速度和加速度(一般为零)； (b) 提升点：中

13、间点运动的连续； (c) 下放点：同提升点； (d) 终止点：给定速度和加速度(一般为零)。,15,6、除上述约束外，所有关节轨迹的极值不能超出每个关节变量的物理和几何极限。 7、时间的考虑 (a) 轨迹的初始段和终止段：时间由手接近和离开支承表面的速率决定；也是由关节电机特性决定的某个常数。 (b) 轨迹的中间点或中间段：时间由各关节的最大速度和加速度决定，将使用这些时间中的一个最长时间(即用最低速关节确定的最长时间来归一化)。,在关节轨迹的典型约束条件之下，我们所要研究的是选择一种 n 次(或小于 n 次)的多项式函数，使得在各结点(初始点，提升点，下放点和终止点)上满足对位置、速度和加速

14、度的要求，并使关节位置、速度和加速度在整个时间间隔 t0, tf 中保持连续。,16,规划关节插值轨迹的约束条件：,17,一种方法是为每个关节规定一个七次多项式函数,式中，未知系数 aj 可由已知的位置和连续条件确定。但用这种高次多项式内插给定的结点也许不能令人满意，因为它的极值难求，而且容易产生额外的运动。 另一种办法是将整个关节空间轨迹分割成几段，在每段轨迹中用不同的低次多项式来插值。有几种分割轨迹的方法，每种方法的特性各不相同。,434 轨迹 每个关节有下面三段轨迹：第一段由初始点到提升点的轨迹用四次多项式表示。第二段(或中间段）由提升点到下放点的轨迹用三次多项式表示。最后一段由下放点到

15、终止点的轨迹由四次多项式表示。,18,353 轨迹 与434轨迹相同，但每段所用多项式次数与前种不同。第一段用三次多项式，第二段用五次多项式，最后一段用三次多项式。 (33333) 轨迹 对五段轨迹都使用三次多项式样条函数。 注意，上述讨论对每个关节轨迹都是有效的，即每个关节轨迹可分割成三段或五段。,7.3.1 434 关节轨迹的计算,对于N个关节, 在每段轨迹规划中就要确定 N 条关节轨迹，引用归一化时间变量 是方便的，它使我们能用同样的方法处理每个关节每段轨迹的方程。时间变化范围均由 (各段轨迹的初始时间)变到 (各段轨迹的终止时间)。,19,定义下列变量：,轨迹是由多项式序列hi(t)构

16、成的，这些多项式合起来形成关节 j 的轨迹。在每段轨迹中关节变量的多项式用归一化时间表示为:,20,这些关节轨迹分段多项式所应满足的边界条件为:,21,434关节轨迹的边界条件如图示。,22,这些多项式对实际时间 t 的一阶和二阶导数。可写成,第一段轨迹的基本多项式是四次的,23,对于t =0(这段轨迹的初始位置)，要满足此位置的边界条件，则,24,用求出的这些未知量得到,对于t=1(这段轨迹的终点)，在此位置，我们放宽插值多项式必须准确通过该点的要求，仅要求在此位置的速度和加速度必须和下一段轨迹起点的速度和加速度连续。此处的速度和加速度为,第二段轨迹的基本多项式是三次的,25,对于t=0(提

17、升点)，此点的速度和加速度分别为,由于此点的速度相加速度必须分别和前一段轨迹终点的速度和加速度连续，故可得,26,或,或,对于t=1(下放点)，该点的速度相加速度必须与下一段轨迹起点处的速度和加速度连续。可分别求得此位置的速度和加速度为,27,末段轨迹的基本多项式是四次的,如果在上式中以 代替 t ，我们就把归一化时间 t 由 移至 。,28,可得,对于 (这段轨迹的终点)，要满足轨迹终点的边界条件，即,对于 (这段轨迹的起点)，要满足轨迹起点的边界条件，即,29,在此下放点的速度和加速度连续性条件是：,或,30,可以求出相连轨迹段间关节角之差为,轨迹多项式其余七个未知系数可由以上的速度、加速

18、度连续约束条件的联立方程解出，用矩阵矢量符号改写这些方程，可得,其中,31,或,C 矩阵的结构便于计算未知系数，若时间间隔 ti 为正值， C 的逆矩阵总是存在的。则关节各段关节轨迹多项式系数即可全部求得。,32,同样，我们可用此方法可计算353 关节轨迹。,对于末段轨迹，把归一化时间 t 由-1，0重新变回0, 1，可求出 hn(t),33,4-3-4 关节轨迹多项式,34,35,3-5-3 关节轨迹多项式,36,7.3.2 三次样条轨迹（五段三次多项式）,用一组三次多项式插补给定的函数并保证插补点处一阶和二阶导数连续的方法，称为三次样条函数法。所能达到的近似程度和光滑程度是相当好的。 一般

19、来说，样条曲线是在插值点具有 k-1 阶导数连续性的 k次多项式。对于三次样条函数，一阶导数代表速度的连续性，二阶导数代表加速度的连续性。 三次样条函数有某些优点。首先，它是使速度和加速度连续的最低次多项式函数。其次，低次多项式减少了计算量和数值不稳定的可能性。,每段关节轨迹的五段三次多项式的通式为,其中,37,在应用五段三次多项式插值时，需要有五段轨迹和六个插值点。但是，在前面的讨论中只有四个插值点，即初始点、提升点、下放点和终止点。所以，必须选择另外两个插值点，以便有足够多的边界条件求解各多项式系数。可以在提升点和下放点之间选取这两个额外的结点。 没有必要知道这两个点的确切位置，只要知道时

20、间间隔，以及必须满足这两点速度和加速度的连续性条件。因此，这组关节轨迹分段多项式必须满足的边界条件是： (1) 在初始点、提升点、下放点和终止点的位置约束； (2) 在所有插值点的速度和加速度的连续性。,38,五段三次关节轨迹的边界条件示于中。其中有下划线的变量是在计算五段三次多项式之前的已知量。,39,这些多项式对实际时间的一阶和二阶导数为：,式中，tj 是通过第 j 段轨迹所需的实际时间。给定了初始点和终止点的位置、速度和加速度，第一段轨迹和末段轨迹的多项式 h1 (t) 和hn (t) 就完全确定了。旦算出这两个多项式，就可用位置约束和连续条件求出h2 (t)、 h3 (t) 和 h4

21、(t) 。,第一段轨迹，基本多项式为,当 t=0 时，由该位置的边界条件，可得,(给定),40,由此可得：,当 t=1 时，由此位置的边界条件，可得：,可得,其中，i= i i-1 。第一段轨迹的多项式就完全确定了。,(给定),41,由上式可求出 t1时的速度和加速度：,此速度相加速度必须与下一段轨迹起点的速度和加速度连续。,末段轨迹的多项式为（仍做类似代换 ）,当t0 和t1时（对应 和 ），由边界条件，有,42,由上述方程解出未知系数，最后得到：,给定初始点、提升点、下放点和终止点的位置以及通过每段轨迹所需要的时间 (tj ) 后，五段三次多项式即可唯一地确定，满足所有位置约束和连续条件。

22、,算出以上个多项式，就可用位置约束和连续条件求出h2 (t)、 h3 (t) 和 h4 (t) 。,43,对于较复杂的机器人系统，为了控制操作机完成作业而设计了编程语言。在这种系统中，作业通常是用操作机手部(或末端执行器)必须通过的笛卡尔结点序列给定的。因此，描述操作机在作业中的运动时，我们就更关心描述操作机手部要达到的目标位置及通过的笛卡尔空间曲线(或路径)的形式。,7.4 笛卡尔路径轨迹规划,一般，实现笛卡尔路径规划可采用下述两个相连的步骤：(1)沿笛卡尔路径，按照某种规则以笛卡尔坐标生成或选择一组结点或插值点；(2)规定一种函数，按某些准则连接这些结点(或逼近分段的路径)。 对于第二步，

23、所选用的准则可以采用两种控制算法，以保证跟踪给定的路径。,44,(1) 面向笛卡尔空间的方法。在此方法中，大部分计算和优化是以笛卡尔坐标完成的，然后，在手部这一级上进行控制。按固定的取样间隔在预定路径上选择伺服取样点，在控制操作机时实时地把它们转换为与之相应的关节变量。所得到的轨迹是分段直线。 (2) 面向关节空间的方法。这种方法用关节变量空间中的低次多项式函数迫近直线路径上的两相邻结点间的一段路径，而控制是在关节这一级上进行的。所得到的笛卡尔路径是不分段的直线。,Paul 叙述了用一系列直线段构成操作机手部笛卡尔路径的设计方法。把手部在这些直线段中的速度和加速度转换到关节坐标，并用二次插值进

24、行平滑连接，从而实现了手部的规划运动控制。我们将学习如何用该方法设计笛卡尔直线路径。,45,对于笛卡尔路径控制，轨迹设定点算法为：,循环：等待下一个控制间隔； 时刻操作机末端所处的位置； 相应于 的关节变量解； 若 ，则退出循环； 转向循环。,这里，除了需要计算每一控制间隔中操作机手部轨迹函数 外，还需把笛卡尔位置变换为相应的关节变量 。矩阵函数 表示操作机手部在 时刻的预定位置。可用44齐次变换矩阵表示。,46,在可编程机器人系统中，预定的运动可用笛卡尔结点序列给定，相应于这些笛卡尔结点的关节坐标可用运动学逆问题求解程序算出，并用二次多项式平滑关节坐标中两个相邻的关节结点，以利于控制，从而，

25、控制操作机手部沿连接这些结点的直线运动。这种方法的优点是，它使我们能控制操作机手部跟踪运动的物体。虽然目标位置是用变换描述的，但它们并不规定操作机手部怎样由一个变换运动到另一个变换。 Paul 用一个直线平移和两个转动来完成两相邻笛卡尔结点间的运动。第一个转动是绕单位矢量 k 进行的，把工具或末端执行器的轴线调整到预定的接近方向；而第二个转动则绕工具轴调整工具的方向。,7.4.1 齐次变换矩阵法,47,一般来说，操作机的目标位置可用如下基本矩阵方程表示：,其中，0T6 为描述操作机手部相对于机座坐标系位姿的齐次变换矩阵； 6Ttool 为描述工具相对于手部坐标系位姿的齐次变换矩阵，它描述了受控

26、制的工具端点； 0Cbase(t) 为描述物体位姿的工作坐标系相对于机座坐标系的齐次变换矩阵； basePobj 为描述末端执行器相对于工作坐标系预定抓取物体的位姿的齐次变换矩阵。,左边的矩阵描述操作机抓取位姿，而右边的矩阵描述了我们要让操作机工具抓握物体特征部位的位姿。,48,我们可以解出0T6 ，它描述了以正确和预定的方式抓取物体时操作机应有的形态。,如果计算0T6 的速度足够高，并转换成相应的关节角，那么就可控制操作机沿预定的轨迹运动 利用前式，确定一项作业的 N个 目标位置的序列可表示为,49,由Ci(t) Pi确定的位置，可求得相邻点之间的距离。若进一步给定线速度和角速度，即可求得由

27、位置i 到 i+1所需的时间。,以上位置的工具和运动坐标系都是相对于机座坐标系给定的。从一个位置移向下一个位置最好是相对于运动坐标系给定位置和工具。这样，从运动坐标系观察，工具是静止的。为了做到这一点，需要相对于后续的坐标系重新定义当前的位置和工具。利用双下标符号 Pij 重新定义Pi变换，就不难做到这一点。 Pij是在第 j 个运动坐标系中表示的位置Pi。则在位置1，将它在目标坐标系中表示，可得,将它用位置2时的坐标系表示，则有,50,由以上两个方程即可求得,上述方程的目的是要在已知 P11时 求得P12 。因此，在任意二相邻位置 i 和 i+1 之间的运动有,其中Pi,i+1 和Pi+1,

28、i+1 为上面所讨论过的变换。 Paul 使用了一种简单的方法，以控制操作机手部从一个变换运动到另一个变换。此法包括一个平移运动和一个绕空间某定轴的转动以及另一个绕工具轴的转动，以便形成操作机受控的线速度和角速度运动。第一个转动使工具轴与预定的接近方向对准，第二个转动使工具轴与方向矢量对准。,51,由位置 i 到 i+1的运动可用“驱动”变换D()表示，它是归一化时间 的函数,其中，,可得,于是在位置 i 时， =0，D(0)为单位矩阵,52,用位置 i 和 i+1各自的齐次变换矩阵表示它们，可得,利用前式，求 Pi,i+1的逆并乘以Pi+1,i+1 ，得到,53,若驱动函数由一个平移运动和两

29、个旋转运动构成，那么这些平移和转动量将直接同成正比。若随时间线性地变化，则 D() 所代表的合成运动将相应于一个不变的线速度和两个角速度。 平移运动可由齐次变换矩阵L()表示；第一个转动可用齐次变换矩阵RA()表示，它的作用是把Pi位置的接近矢量转向Pi+1位置的接近矢量；第二个转动可用RB()表示，它的作用是把Pi 位置的方向矢量绕工具轴转向Pi+1位置的方向矢量。因此，驱动函数可表示为,54,其中，,旋转矩阵RA() 表示绕 Pi 的接近矢量（zi 轴）转动了角，又绕方向矢量（yi 轴）转动了角； RB() 表示绕工具的接近矢量转动 角的旋转。,55,其中，,56,操作机经常要沿相连的直线

30、段轨迹运动，以满足作业运动的要求或避开障碍物。为了避免在每段端点处速度不连续，当由一段轨迹过渡到下一段轨迹时，需要加速或减速运动。 在操作机到达端点前的个单位时间，可以开始改变速度，然后保持加速度不变，直到下段轨迹开始后的个单位时间为止。若在时间 - 和 之间，每一变量的加速度保持为常数。,7.4.2 两段路径间的过渡,57,其中C 和B为矢量，其元素分别是点 D 到 C 和点 B 到 A的笛卡尔距离和角度。,式中,则为改变位置和速度所必需的加速度值为,58,时的速度和位置为,其中，,时，运动用下面的方程描述：,其中，,59,注意，与前面一样，代表归一化时间，变化范围是0，1，不过，对于不同的

31、时间间隔，归一化因子通常是不同的。,对于由 A 到 B 再到 C 的运动，我们把 定义为在 区间的运动中的线性插值，并表示为,式中 和 分别是由A 到B 和由B 到C 的运动确定的，由此可确定 。,总之，为了从位置 Pi 运动到Pi +1，可计算出驱动函数D()，然后计算 T6() ，再用运动学逆问题程序算出相应的关节变量值。如有必要，在用运动学逆问题解出的点之间，可用二次多项式函数进行插值。,60,例 要求一个机器人按直线运动，把螺栓放入托架的一个孔中，写出所有必要的矩阵方程，使机器人能沿虚线运动并完成作业。可用符号表示沿直线运动的中间位置。,解 令Pi 为操作机手部必须经过的笛卡尔结点i

32、=0, 1, 2, 3, 4, 5)。则基本矩阵方程为,61,其中，WORLD是绝对坐标系（也称世界坐标系）; BASE、INIT、B0、BR 分别是基座坐标系、手部位姿、工件库坐标系、托架坐标系等相对于绝对坐标系的变换矩阵； T6 是手部位姿相对基座坐标系的变换矩阵; E 是工具坐标系相对手部坐标系的变换矩阵; P0 是在 INIT 中描述的轨迹点；P1 P3 是在B0中描述的轨迹点；P4、P5 是在 BR 中描述的轨迹点。以上的变换矩阵均为44 坐标矩阵。,62,由P0 位置运动到P1，我们在P0 坐标系中用双下标描述P0有,在 P1 坐标系中描述P0 ，可得,由以上两式相等，得到,因此，由P0位置沿直线运动到P1时，操作机手部位姿必须由,变化到,由Pi 位置到Pi+1 位置 ( i=1, 2, 3, 4, 5) 时，操作机运动的手部位姿可用同样方法求解。,