机器人的轨迹规划ppt课件

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

2、许多规划所包的步骤是模糊的，而且需求进一步阐明子许多规划所包的步骤是模糊的，而且需求进一步阐明子规划。大多数规划具有很大的子规划构造，规划中的每个规划。大多数规划具有很大的子规划构造，规划中的每个目的可以由到达此目的的比较详细的子规划所替代。因此，目的可以由到达此目的的比较详细的子规划所替代。因此，最终得到的规划是某个问题求解算符的线性或分部排序，但最终得到的规划是某个问题求解算符的线性或分部排序，但是由算符来实现的目的经常具有分层构造。是由算符来实现的目的经常具有分层构造。 把某些比较复杂的问题分解为一些比较小的问题的想法使把某些比较复杂的问题分解为一些比较小的问题的想法使我们运用规划方法求

3、解问题在实践上成为能够。我们运用规划方法求解问题在实践上成为能够。 有两条可以实现这种分解的重要途径：第一条是当从一个有两条可以实现这种分解的重要途径：第一条是当从一个问题形状挪动到下一个形状时，无需计算整个新的形状，而问题形状挪动到下一个形状时，无需计算整个新的形状，而只需思索形状中能够变化了的那些部分。第二条是把单一的只需思索形状中能够变化了的那些部分。第二条是把单一的困难问题分割为几个有希望的、较为容易处理的子问题，这困难问题分割为几个有希望的、较为容易处理的子问题，这种分解可以使困难问题的求解变得容易些。种分解可以使困难问题的求解变得容易些。 机器人规划分为高层规划和低层规划。自动规划

4、在机器人机器人规划分为高层规划和低层规划。自动规划在机器人规划中称为高层规划。在无特别阐明时，机器人规划都是指规划中称为高层规划。在无特别阐明时，机器人规划都是指自动规划。自动规划是一种重要的问题求解技术，它从某个自动规划。自动规划是一种重要的问题求解技术，它从某个特定的问题形状出发，寻求一系列行为动作，并建立一个操特定的问题形状出发，寻求一系列行为动作，并建立一个操作序列，直到求得目的形状为止。与普通问题求解相比，自作序列，直到求得目的形状为止。与普通问题求解相比，自动规划更注重于问题的求解过程，而不是求解结果。动规划更注重于问题的求解过程，而不是求解结果。 我们在论述机器人自动规划问题时，

5、机器人普通配备有传我们在论述机器人自动规划问题时，机器人普通配备有传感器和一组能在某个易于了解的现场中完成的根本动作。这感器和一组能在某个易于了解的现场中完成的根本动作。这些动作可把该现场从一种形状或规划变换为另一种形状或规些动作可把该现场从一种形状或规划变换为另一种形状或规划。例如，划。例如， “积木世界积木世界 。 机器人能得到的一个解答是由下面的算符序列组成的：机器人能得到的一个解答是由下面的算符序列组成的：目的形状目的形状 机器人规划是机器人学的一个重要研讨领域，也是人工智能机器人规划是机器人学的一个重要研讨领域，也是人工智能与机器人学一个令人感兴趣的结合点。与机器人学一个令人感兴趣的

6、结合点。 机器人的规划程序只需求给定义务初始形状和最终形状的描机器人的规划程序只需求给定义务初始形状和最终形状的描画。这些规划系统普通都不阐明为实现一个算符所需的详细的画。这些规划系统普通都不阐明为实现一个算符所需的详细的机器人运动。义务规划程序那么把义务级的阐明变换成操作机机器人运动。义务规划程序那么把义务级的阐明变换成操作机级的阐明。为了进展这种变换，义务规划程序必需包含被操作级的阐明。为了进展这种变换，义务规划程序必需包含被操作物体、义务环境、机器人执行的义务、环境的初始形状和所要物体、义务环境、机器人执行的义务、环境的初始形状和所要求的最终求的最终( (目的目的) )形状等描画。义务规

7、划程序的输出就是一个机形状等描画。义务规划程序的输出就是一个机器人程序，在指定的初始形状下执行该程序后，就能到达所要器人程序，在指定的初始形状下执行该程序后，就能到达所要求的最终形状。求的最终形状。 义务规划有三个阶段：建立模型、义务阐明和操作机程序综义务规划有三个阶段：建立模型、义务阐明和操作机程序综合。义务的世界模型应含有如下的信息合。义务的世界模型应含有如下的信息:(1):(1)义务环境中的一切义务环境中的一切物体和机器人的几何描画；物体和机器人的几何描画；(2)(2)一切物体的物理描画；一切物体的物理描画；(3)(3)一切一切衔接件的运动学描画，衔接件的运动学描画，(4) (4) 机器

8、人和传感器特性的描画。在世机器人和传感器特性的描画。在世界模型中，义务形状模型还必包括全部物体和衔接件的规划。界模型中，义务形状模型还必包括全部物体和衔接件的规划。7.1.2 机器人义务规划的作用机器人义务规划的作用 机器人轨迹规划属于机器人低层规划，根本上不涉及人工智能问题，而是在机械手运动学和动力学的根底上，讨论机器人运动的规划及其方法。所谓轨迹，就是指机器人在运动过程中的位移、速度和加速度。 轨迹规划问题通常是将轨迹规划器看成轨迹规划问题通常是将轨迹规划器看成“黑箱，接受表示黑箱，接受表示途径约束的输入变量，输出为起点和终点之间按时间陈列的操途径约束的输入变量，输出为起点和终点之间按时间

9、陈列的操作机中间形状位姿作机中间形状位姿, 速度和加速度序列。速度和加速度序列。途径约束途径约束动力学约束动力学约束途径设定途径设定轨迹规划器轨迹规划器7.2 7.2 机器人轨迹规划的普通性问题机器人轨迹规划的普通性问题由初始点运动到终止由初始点运动到终止点，所经过的由中间点，所经过的由中间形状序列构成的空间形状序列构成的空间曲线称为途径。这些曲线称为途径。这些形状序列即是曲线上形状序列即是曲线上的的“点。点。规划操作机的轨迹有两种常用的方法：规划操作机的轨迹有两种常用的方法： 第一种方法：要求运用者在沿轨迹选定的位置点上第一种方法：要求运用者在沿轨迹选定的位置点上( (称为结称为结节或插值点

10、节或插值点) )显式地给定广义坐标位置、速度和加速度的一组显式地给定广义坐标位置、速度和加速度的一组约束约束( (例如，延续性和光滑程度等例如，延续性和光滑程度等) )。然后，轨迹规划器从插。然后，轨迹规划器从插值和满足插值点约束的函数中选定参数化轨迹。显然，在这值和满足插值点约束的函数中选定参数化轨迹。显然，在这种方法中，约束的给定和操作机轨迹规划是在关节坐标系中种方法中，约束的给定和操作机轨迹规划是在关节坐标系中进展的。进展的。 第二种方法：运用者以解析函数显式地给定操作机必经之途第二种方法：运用者以解析函数显式地给定操作机必经之途径，例如，笛卡尔坐标中的直线途径。然后，轨迹规划器在径，例

11、如，笛卡尔坐标中的直线途径。然后，轨迹规划器在关节坐标或笛卡几坐标中确定一条与给定途径近似的轨迹。关节坐标或笛卡几坐标中确定一条与给定途径近似的轨迹。在这种方法中，途径约束是在笛卡尔坐标中给定的。在这种方法中，途径约束是在笛卡尔坐标中给定的。 在第一种方法中，约束的给定和操作机轨迹规划在关节坐标在第一种方法中，约束的给定和操作机轨迹规划在关节坐标系中进展。由于对操作机手部没有约束，运用者难于跟踪操作系中进展。由于对操作机手部没有约束，运用者难于跟踪操作机手部运转的途径。因此，操作机手部能够在没有事先警告的机手部运转的途径。因此，操作机手部能够在没有事先警告的情况下与妨碍物相碰。情况下与妨碍物相

12、碰。 在第二种方法中，途径约束在笛卡尔坐标中给定，而关节驱在第二种方法中，途径约束在笛卡尔坐标中给定，而关节驱动器是在关节坐标中受控制的。因此，为了求得一条逼近给定动器是在关节坐标中受控制的。因此，为了求得一条逼近给定途径的轨迹，必需用函数近似把笛卡尔坐标中的途径约束变换途径的轨迹，必需用函数近似把笛卡尔坐标中的途径约束变换为关节坐标中的途径约束，再确定满足关节坐标途径约束的参为关节坐标中的途径约束，再确定满足关节坐标途径约束的参数化轨迹。数化轨迹。 轨迹规划既可在关节变量空间中进展，也可在笛卡尔空间进展。轨迹规划既可在关节变量空间中进展，也可在笛卡尔空间进展。对于关节变量空间的规划，要规划关

13、节变量的时间函数及其前对于关节变量空间的规划，要规划关节变量的时间函数及其前二阶时间导数，以便描画操作机的预定运动。在笛卡尔空间规二阶时间导数，以便描画操作机的预定运动。在笛卡尔空间规划中，要规划操作机手部位置、速度和加速度的时间函数，而划中，要规划操作机手部位置、速度和加速度的时间函数，而相应的关节位置、速度和加速度可根据手部信息导出。相应的关节位置、速度和加速度可根据手部信息导出。 面向笛卡尔空间方法的优点是概念直观，而且沿预定直线途面向笛卡尔空间方法的优点是概念直观，而且沿预定直线途径可到达相当的准确性。可是由于现代还没有可用笛卡尔坐标径可到达相当的准确性。可是由于现代还没有可用笛卡尔坐

14、标丈量操作机手部位置的传感器，一切可用的控制算法都是建立丈量操作机手部位置的传感器，一切可用的控制算法都是建立在关节坐标根底上的。因此，笛卡尔空间途径规划就需求在笛在关节坐标根底上的。因此，笛卡尔空间途径规划就需求在笛卡尔坐标和关节之间进展实时变换，这是一个计算量很大的义卡尔坐标和关节之间进展实时变换，这是一个计算量很大的义务，经常导致较长的控制间隔。务，经常导致较长的控制间隔。 由笛卡尔坐标向关节坐标的变换是病态的，因此它不是一一由笛卡尔坐标向关节坐标的变换是病态的，因此它不是一一对应的映射。对应的映射。 假设在轨迹规划阶段要思索操作机的动力学特性，就要以笛假设在轨迹规划阶段要思索操作机的动

15、力学特性，就要以笛卡尔坐标给定途径约束，同时以关节坐标给定物理约束卡尔坐标给定途径约束，同时以关节坐标给定物理约束(例如，例如，每个关节电机的力和力矩、速度和加速度权限每个关节电机的力和力矩、速度和加速度权限)。这就会使最。这就会使最后的优化问题具有在两个不同坐标系中的混合约束。后的优化问题具有在两个不同坐标系中的混合约束。在笛卡尔空间进展轨迹规划的特点：在笛卡尔空间进展轨迹规划的特点： 在关节变量空间的规划有三个优点：在关节变量空间的规划有三个优点： 直接用运动时的受控变量规划轨迹；直接用运动时的受控变量规划轨迹； 轨迹规划可接近实时地进展；轨迹规划可接近实时地进展； 关节轨迹易于规划。关节

16、轨迹易于规划。 伴随的缺陷是难于确定运动中各杆件和手的位置，但是，为伴随的缺陷是难于确定运动中各杆件和手的位置，但是，为了避开轨迹上的妨碍经常又要求知道一些杆件和手位置。了避开轨迹上的妨碍经常又要求知道一些杆件和手位置。 由于面向笛卡尔空间的方法有前述钟种缺陷，使得面向关节由于面向笛卡尔空间的方法有前述钟种缺陷，使得面向关节空间的方法被广泛采用。它把笛卡尔结点变换为相应的关节坐空间的方法被广泛采用。它把笛卡尔结点变换为相应的关节坐标，并用低次多项式内插这些关节结点。这种方法的优点是计标，并用低次多项式内插这些关节结点。这种方法的优点是计算较快，而且易于处置操作机的动力学约束。但当取样点落在算较

17、快，而且易于处置操作机的动力学约束。但当取样点落在拟合的光滑多项式曲线上时，面向关节空间的方法沿笛卡尔途拟合的光滑多项式曲线上时，面向关节空间的方法沿笛卡尔途径的准确性会有损失。径的准确性会有损失。普通说来，生成关节轨迹设定点的根本算法是很简单的：普通说来，生成关节轨迹设定点的根本算法是很简单的：0tt循环：等待下一个控制间隔；循环：等待下一个控制间隔； 时辰操作机关节所处的位置；时辰操作机关节所处的位置； 假设假设 ，那么退出循环；，那么退出循环； 转向循环。转向循环。ttt ( )h ttftt 从上述算法可以看出，要计算的是在每个控制间隔中必需更从上述算法可以看出，要计算的是在每个控制间

18、隔中必需更新的轨迹函数新的轨迹函数(或轨迹规划器或轨迹规划器)h(t)。因此，对规划的轨迹要提出。因此，对规划的轨迹要提出四个限制。第一，必需便于用迭代方式计算轨迹设定点；第二，四个限制。第一，必需便于用迭代方式计算轨迹设定点；第二，必需求出并明确给定中间位置；第三，必需保证关节变量及其必需求出并明确给定中间位置；第三，必需保证关节变量及其前二阶时间导数的延续性，使得规划的关节轨迹是光滑的；最前二阶时间导数的延续性，使得规划的关节轨迹是光滑的；最后，必需减少额外的运动后，必需减少额外的运动 (例如，例如，“游移游移 )。7.3 7.3 关节插值轨迹关节插值轨迹 假设某关节假设某关节(例如关节例

19、如关节 i )的关节轨迹运用的关节轨迹运用 p 个多项式，那么个多项式，那么要满足初始和终止条件要满足初始和终止条件(关节位置、速度和加速度关节位置、速度和加速度)，并保证这，并保证这些变量在多项式衔接处的延续性，因此需求确定些变量在多项式衔接处的延续性，因此需求确定 3( p十十1)个系个系数。数。 假设给定附加的中间条件假设给定附加的中间条件(例如位置例如位置)，那么对每个中间条件，那么对每个中间条件需求添加一系数。通常，可以给定两个中间位置，一个接近初需求添加一系数。通常，可以给定两个中间位置，一个接近初始位置；另一个接近终止位置。这样做，除了可以较好地控制始位置；另一个接近终止位置。这

20、样做，除了可以较好地控制运动外，还能保证操作机末端以适当的方向分开起点和接近终运动外，还能保证操作机末端以适当的方向分开起点和接近终点。点。 因此，对于衔接初始位置和终止位置的每个关节变量，一因此，对于衔接初始位置和终止位置的每个关节变量，一个七次多项式就足够了，或者用两段四次轨迹加一段三次轨迹个七次多项式就足够了，或者用两段四次轨迹加一段三次轨迹 434，也可以用两段三次轨迹加一段五次轨迹，也可以用两段三次轨迹加一段五次轨迹353，或五段三次轨迹，或五段三次轨迹33333。p +1个点个点由约束条件数所对应的多项式系数的个数确定多项式的次数由约束条件数所对应的多项式系数的个数确定多项式的次数

21、 为了控制操作机，在规划运动轨迹之前，需求给定机器人为了控制操作机，在规划运动轨迹之前，需求给定机器人在初始点和终止点的手臂形状。在规划机器人关节插值运动在初始点和终止点的手臂形状。在规划机器人关节插值运动轨迹时，需求留意下述几点：轨迹时，需求留意下述几点：1、抓住一个物体时，手的运动方向应该指向分开物体支承外、抓住一个物体时，手的运动方向应该指向分开物体支承外表的方向。否那么，手能够与支承面相碰。表的方向。否那么，手能够与支承面相碰。2、假设沿支承面的法线方向从初始点向外给定一个分开位置、假设沿支承面的法线方向从初始点向外给定一个分开位置(提升点提升点)，并要求手，并要求手(即手部坐标系的原

22、点即手部坐标系的原点)经过此位置，这种经过此位置，这种分开运动就是允许的。假设还给定由初始点运动到分开位置分开运动就是允许的。假设还给定由初始点运动到分开位置的时间，我们就可以控制提起物体运动的速度。的时间，我们就可以控制提起物体运动的速度。3、对于手臂运动提升点的要求同样也适用于终止位置运动的、对于手臂运动提升点的要求同样也适用于终止位置运动的下放点下放点(即必需先运动到支承外表外法线方向上的某点，再渐即必需先运动到支承外表外法线方向上的某点，再渐渐下移至终止点渐下移至终止点)。这样，可获得和控制正确的接近方向。这样，可获得和控制正确的接近方向。4、对手臂的每一次运动，都设定上述四个点：初始

23、点，提升、对手臂的每一次运动，都设定上述四个点：初始点，提升点，下放点和终止点。点，下放点和终止点。5、位置约束、位置约束 (a) 初始点：给定速度和加速度初始点：给定速度和加速度(普通为零普通为零)； (b) 提升点：中间点运动的延续；提升点：中间点运动的延续； (c) 下放点：同提升点；下放点：同提升点； (d) 终止点：给定速度和加速度终止点：给定速度和加速度(普通为零普通为零)。6、除上述约束外，一切关节轨迹的极值不能超出每个关节、除上述约束外，一切关节轨迹的极值不能超出每个关节变量的物理和几何极限。变量的物理和几何极限。7、时间的思索、时间的思索 (a) 轨迹的初始段和终止段：时间由

24、手接近和分开支承外表轨迹的初始段和终止段：时间由手接近和分开支承外表的速率决议；也是由关节电机特性决议的某个常数。的速率决议；也是由关节电机特性决议的某个常数。 (b) 轨迹的中间点或中间段：时间由各关节的最大速度和加轨迹的中间点或中间段：时间由各关节的最大速度和加速度决议，将运用这些时间中的一个最长时间速度决议，将运用这些时间中的一个最长时间(即用最低速即用最低速关节确定的最长时间来归一化关节确定的最长时间来归一化)。 在关节轨迹的典型约束条件之下，我们所要研讨的是选择在关节轨迹的典型约束条件之下，我们所要研讨的是选择一种一种 n 次次(或小于或小于 n 次次)的多项式函数，使得在各结点的多

25、项式函数，使得在各结点(初始点，初始点，提升点，下放点和终止点提升点，下放点和终止点)上满足对位置、速度和加速度的要上满足对位置、速度和加速度的要求，并使关节位置、速度和加速度在整个时间间隔求，并使关节位置、速度和加速度在整个时间间隔 t0, tf 中坚持延续。中坚持延续。 1. 位置位置(给定给定) 2速度速度(给定，通常为零给定，通常为零) 3. 加速度加速度(给定，通常为零给定，通常为零) 4. 提升点位置提升点位置(给定给定) 5提升点位置与前一段轨迹延续提升点位置与前一段轨迹延续 6. 速度与前一段轨迹延续速度与前一段轨迹延续 7加速度与前一段轨迹延续加速度与前一段轨迹延续 8. 下

26、放点位置下放点位置(给定给定) 9下放点位置与前一段轨迹延续下放点位置与前一段轨迹延续 10. 速度与前一段轨迹延续速度与前一段轨迹延续 11. 加速度与前一段轨迹延续加速度与前一段轨迹延续 12. 位置位置(给定给定) 13. 速度速度(给定，通常为零给定，通常为零) 14. 加速度加速度(给定，通常为零给定，通常为零)终止位置终止位置中间位置中间位置初始位置初始位置 规划关节插值轨迹的约束条件：规划关节插值轨迹的约束条件：一种方法是为每个关节规定一个七次多项式函数一种方法是为每个关节规定一个七次多项式函数式中，未知系数式中，未知系数 aj 可由知的位置和延续条件确定。但用这种可由知的位置和

27、延续条件确定。但用这种高次多项式内插给定的结点也许不能令人称心，由于它的极值高次多项式内插给定的结点也许不能令人称心，由于它的极值难求，而且容易产生额外的运动。难求，而且容易产生额外的运动。 另一种方法是将整个关节空间轨迹分割成几段，在每段轨迹另一种方法是将整个关节空间轨迹分割成几段，在每段轨迹中用不同的低次多项式来插值。有几种分割轨迹的方法，每种中用不同的低次多项式来插值。有几种分割轨迹的方法，每种方法的特性各不一样。方法的特性各不一样。 434 轨迹轨迹 每个关节有下面三段轨迹：第一段由初始点到提升点的每个关节有下面三段轨迹：第一段由初始点到提升点的轨迹用四次多项式表示。第二段轨迹用四次多

28、项式表示。第二段(或中间段由提升点到下或中间段由提升点到下放点的轨迹用三次多项式表示。最后一段由下放点到终止点放点的轨迹用三次多项式表示。最后一段由下放点到终止点的轨迹由四次多项式表示。的轨迹由四次多项式表示。 353 轨迹 与434轨迹一样，但每段所用多项式次数与前种不同。第一段用三次多项式，第二段用五次多项式，最后一段用三次多项式。 (33333) 轨迹 对五段轨迹都运用三次多项式样条函数。 留意，上述讨论对每个关节轨迹都是有效的，即每个关节轨迹可分割成三段或五段。7.3.1 434 关节轨迹的计算关节轨迹的计算 对于对于N个关节个关节, 在每段轨迹规划中就要确定在每段轨迹规划中就要确定

29、N 条关节轨迹，条关节轨迹，援用归一化时间变量援用归一化时间变量 是方便的，它使我们能用同样的方是方便的，它使我们能用同样的方法处置每个关节每段轨迹的方程。时间变化范围均由法处置每个关节每段轨迹的方程。时间变化范围均由 (各各段轨迹的初始时间段轨迹的初始时间)变到变到 (各段轨迹的终止时间各段轨迹的终止时间)。0,1t0t 1t 定义以下变量：定义以下变量： 轨迹是由多项式序列轨迹是由多项式序列hi(t)构成的，这些多项式合起来构成关构成的，这些多项式合起来构成关节节 j 的轨迹。在每段轨迹中关节变量的多项式用归一化时间表的轨迹。在每段轨迹中关节变量的多项式用归一化时间表示为示为:这些关节轨迹

30、分段多项式所应满足的边境条件为这些关节轨迹分段多项式所应满足的边境条件为:434关节轨迹的边境条件如图示。关节轨迹的边境条件如图示。这些多项式对实践时间这些多项式对实践时间 t 的一阶和二阶导数。可写成的一阶和二阶导数。可写成第一段轨迹的根本多项式是四次的第一段轨迹的根本多项式是四次的对于对于t =0(这段轨迹的初始位置这段轨迹的初始位置)，要满足此位置的边境条件，那么，要满足此位置的边境条件，那么用求出的这些未知量得到用求出的这些未知量得到对于对于t=1(这段轨迹的终点这段轨迹的终点)，在此位置，我们放宽插值多项式，在此位置，我们放宽插值多项式必需准确经过该点的要求，仅要求在此位置的速度和加

31、速度必必需准确经过该点的要求，仅要求在此位置的速度和加速度必需和下一段轨迹起点的速度和加速度延续。此处的速度和加速需和下一段轨迹起点的速度和加速度延续。此处的速度和加速度为度为第二段轨迹的根本多项式是三次的第二段轨迹的根本多项式是三次的对于对于t=0(提升点提升点)，此点的速度和加速度分别为，此点的速度和加速度分别为由于此点的速度相加速度必需分别和前一段轨迹终点的速度和由于此点的速度相加速度必需分别和前一段轨迹终点的速度和加速度延续，故可得加速度延续，故可得或或或或 对于对于t=1(下放点下放点)，该点的速度相加速度必需与下一段轨迹，该点的速度相加速度必需与下一段轨迹起点处的速度和加速度延续。

32、可分别求得此位置的速度和加速起点处的速度和加速度延续。可分别求得此位置的速度和加速度为度为末段轨迹的根本多项式是四次的末段轨迹的根本多项式是四次的假设在上式中以假设在上式中以 替代替代 t ，我们就把归一化时间，我们就把归一化时间 t 由由 移至移至 。1tt 0,1t1,0t 可得可得对于对于 (这段轨迹的终点这段轨迹的终点)，要满足轨迹终点的边境条件，即，要满足轨迹终点的边境条件，即0t 对于对于 (这段轨迹的起点这段轨迹的起点)，要满足轨迹起点的边境条件，即，要满足轨迹起点的边境条件，即1t 在此下放点的速度和加速度延续性条件是：在此下放点的速度和加速度延续性条件是：或或可以求出相连轨迹

33、段间关节角之差为可以求出相连轨迹段间关节角之差为 轨迹多项式其他七个未知系数可由以上的速度、加速度延续轨迹多项式其他七个未知系数可由以上的速度、加速度延续约束条件的联立方程解出，用矩阵矢量符号改写这些方程，可约束条件的联立方程解出，用矩阵矢量符号改写这些方程，可得得其中其中或或C 矩阵的构造便于计算未知系数，假设时间间隔矩阵的构造便于计算未知系数，假设时间间隔 ti 为正值，为正值， C 的逆矩阵总是存在的。那么关节各段关节轨迹多的逆矩阵总是存在的。那么关节各段关节轨迹多项式系数即可全部求得。项式系数即可全部求得。(1,2, )in同样，我们可用此方法可计算同样，我们可用此方法可计算353 关

34、节轨迹。关节轨迹。 对于末段轨迹，把归一化时间对于末段轨迹，把归一化时间 t 由由-1，0重新变回重新变回0, 1，可求出可求出 hn(t)4-3-4 关节轨迹多项式关节轨迹多项式3-5-3 关节轨迹多项式关节轨迹多项式7.3.2 三次样条轨迹五段三次多项式三次样条轨迹五段三次多项式 用一组三次多项式插补给定的函数并保证插补点处一阶和二用一组三次多项式插补给定的函数并保证插补点处一阶和二阶导数延续的方法，称为三次样条函数法。所能到达的近似程阶导数延续的方法，称为三次样条函数法。所能到达的近似程度和光滑程度是相当好的。度和光滑程度是相当好的。 普通来说，样条曲线是在插值点具有普通来说，样条曲线是

35、在插值点具有 k-1 阶导数延续性的阶导数延续性的 k次多项式。对于三次样条函数，一阶导数代表速度的延续性，次多项式。对于三次样条函数，一阶导数代表速度的延续性，二阶导数代表加速度的延续性。二阶导数代表加速度的延续性。 三次样条函数有某些优点。首先，它是使速度和加速度延续三次样条函数有某些优点。首先，它是使速度和加速度延续的最低次多项式函数。其次，低次多项式减少了计算量和数值的最低次多项式函数。其次，低次多项式减少了计算量和数值不稳定的能够性。不稳定的能够性。每段关节轨迹的五段三次多项式的通式为每段关节轨迹的五段三次多项式的通式为其中其中 在运用五段三次多项式插值时，需求有五段轨迹和六个插值在

36、运用五段三次多项式插值时，需求有五段轨迹和六个插值点。但是，在前面的讨论中只需四个插值点，即初始点、提升点。但是，在前面的讨论中只需四个插值点，即初始点、提升点、下放点和终止点。所以，必需选择另外两个插值点，以便点、下放点和终止点。所以，必需选择另外两个插值点，以便有足够多的边境条件求解各多项式系数。可以在提升点和下放有足够多的边境条件求解各多项式系数。可以在提升点和下放点之间选取这两个额外的结点。点之间选取这两个额外的结点。 没有必要知道这两个点确实切位置，只需知道时间间隔，以没有必要知道这两个点确实切位置，只需知道时间间隔，以及必需满足这两点速度和加速度的延续性条件。因此，这组关及必需满足

37、这两点速度和加速度的延续性条件。因此，这组关节轨迹分段多项式必需满足的边境条件是：节轨迹分段多项式必需满足的边境条件是： (1) 在初始点、提升点、下放点和终止点的位置约束；在初始点、提升点、下放点和终止点的位置约束； (2) 在一切插值点的速度和加速度的延续性。在一切插值点的速度和加速度的延续性。 五段三次关节轨迹的边境条件示于中。其中有下划线的变量五段三次关节轨迹的边境条件示于中。其中有下划线的变量是在计算五段三次多项式之前的知量。是在计算五段三次多项式之前的知量。这些多项式对实践时间的一阶和二阶导数为：这些多项式对实践时间的一阶和二阶导数为：式中，式中，tj 是经过第是经过第 j 段轨迹

38、所需的实践时间。给定了初始点和段轨迹所需的实践时间。给定了初始点和终止点的位置、速度和加速度，第一段轨迹和末段轨迹的多项终止点的位置、速度和加速度，第一段轨迹和末段轨迹的多项式式 h1 (t) 和和hn (t) 就完全确定了。就完全确定了。旦算出这两个多项式，就旦算出这两个多项式，就可用位置约束和延续条件求出可用位置约束和延续条件求出h2 (t)、 h3 (t) 和和 h4 (t) 。第一段轨迹，根本多项式为第一段轨迹，根本多项式为当当 t=0 时，由该位置的边境条件，可得时，由该位置的边境条件，可得(给定给定)由此可得：由此可得：当当 t=1 时，由此位置的边境条件，可得：时，由此位置的边境

39、条件，可得：可得可得其中，其中，i= i i-1 。第一段轨迹的多项式就完全确定了。第一段轨迹的多项式就完全确定了。(给定给定)由上式可求出由上式可求出 t1时的速度和加速度：时的速度和加速度：此速度相加速度必需与下一段轨迹起点的速度和加速度延续。此速度相加速度必需与下一段轨迹起点的速度和加速度延续。末段轨迹的多项式为仍做类似代换末段轨迹的多项式为仍做类似代换 (给定给定)(给定给定)1tt 当当t0 和和t1时对应时对应 和和 ，由边境条件，有，由边境条件，有1t 0t 由上述方程解出未知系数，最后得到：由上述方程解出未知系数，最后得到： 给定初始点、提升点、下放点和终止点的位置以及经过每段

40、给定初始点、提升点、下放点和终止点的位置以及经过每段轨迹所需求的时间轨迹所需求的时间 (tj ) 后，五段三次多项式即可独一地确定，后，五段三次多项式即可独一地确定，满足一切位置约束和延续条件。满足一切位置约束和延续条件。算出以上个多项式，就可用位置约束和延续条件求出算出以上个多项式，就可用位置约束和延续条件求出h2 (t)、 h3 (t) 和和 h4 (t) 。 对于较复杂的机器人系统，为了控制操作机完成作业而设对于较复杂的机器人系统，为了控制操作机完成作业而设计了编程言语。在这种系统中，作业通常是用操作机手部计了编程言语。在这种系统中，作业通常是用操作机手部(或或末端执行器末端执行器)必需

41、经过的笛卡尔结点序列给定的。因此，描画必需经过的笛卡尔结点序列给定的。因此，描画操作机在作业中的运动时，我们就更关怀描画操作机手部要到操作机在作业中的运动时，我们就更关怀描画操作机手部要到达的目的位置及经过的笛卡尔空间曲线达的目的位置及经过的笛卡尔空间曲线(或途径或途径)的方式。的方式。7.4 7.4 笛卡尔途径轨迹规划笛卡尔途径轨迹规划 普通，实现笛卡尔途径规划可采用下述两个相连的步骤：普通，实现笛卡尔途径规划可采用下述两个相连的步骤：(1)沿笛卡尔途径，按照某种规那么以笛卡尔坐标生成或选沿笛卡尔途径，按照某种规那么以笛卡尔坐标生成或选择一组结点或插值点；择一组结点或插值点；(2)规定一种函

42、数，按某些准那么衔规定一种函数，按某些准那么衔接这些结点接这些结点(或逼近分段的途径或逼近分段的途径)。 对于第二步，所选用的准那么可以采用两种控制算法，以对于第二步，所选用的准那么可以采用两种控制算法，以保证跟踪给定的途径。保证跟踪给定的途径。 (1) 面向笛卡尔空间的方法。在此方法中，大部分计算和面向笛卡尔空间的方法。在此方法中，大部分计算和优化是以笛卡尔坐标完成的，然后，在手部这一级上进展控优化是以笛卡尔坐标完成的，然后，在手部这一级上进展控制。按固定的取样间隔在预定途径上选择伺服取样点，在控制。按固定的取样间隔在预定途径上选择伺服取样点，在控制操作机时实时地把它们转换为与之相应的关节变

43、量。所得制操作机时实时地把它们转换为与之相应的关节变量。所得到的轨迹是分段直线。到的轨迹是分段直线。 (2) 面向关节空间的方法。这种方法用关节变量空间中的面向关节空间的方法。这种方法用关节变量空间中的低次多项式函数逼近直线途径上的两相邻结点间的一段途径，低次多项式函数逼近直线途径上的两相邻结点间的一段途径，而控制是在关节这一级上进展的。所得到的笛卡尔途径是不而控制是在关节这一级上进展的。所得到的笛卡尔途径是不分段的直线。分段的直线。 Paul 表达了用一系列直线段构成操作机手部笛卡尔途表达了用一系列直线段构成操作机手部笛卡尔途径的设计方法。把手部在这些直线段中的速度和加速度转径的设计方法。把

44、手部在这些直线段中的速度和加速度转换到关节坐标，并用二次插值进展平滑衔接，从而实现了换到关节坐标，并用二次插值进展平滑衔接，从而实现了手部的规划运动控制。我们将学习如何用该方法设计笛卡手部的规划运动控制。我们将学习如何用该方法设计笛卡尔直线途径。尔直线途径。对于笛卡尔途径控制，轨迹设定点算法为：对于笛卡尔途径控制，轨迹设定点算法为：0tt循环：等待下一个控制间隔；循环：等待下一个控制间隔； 时辰操作机末端所处的位置；时辰操作机末端所处的位置； 相应于相应于 的关节变量解；的关节变量解； 假设假设 ，那么退出循环；，那么退出循环； 转向循环。转向循环。ttt ( )H ttftt( )Q H t

45、( )H t 这里，除了需求计算每一控制间隔中操作机手部轨迹函数这里，除了需求计算每一控制间隔中操作机手部轨迹函数 外，还需把笛卡尔位置变换为相应的关节变量外，还需把笛卡尔位置变换为相应的关节变量 。矩阵函数矩阵函数 表示操作机手部在表示操作机手部在 时辰的预定位置。可用时辰的预定位置。可用44齐次变换矩阵表示。齐次变换矩阵表示。( )H t( )Q H t( )H tt 在可编程机器人系统中，预定的运动可用笛卡尔结点序列给在可编程机器人系统中，预定的运动可用笛卡尔结点序列给定，相应于这些笛卡尔结点的关节坐标可用运动学逆问题求解定，相应于这些笛卡尔结点的关节坐标可用运动学逆问题求解程序算出，并

46、用二次多项式平滑关节坐标中两个相邻的关节结程序算出，并用二次多项式平滑关节坐标中两个相邻的关节结点，以利于控制，从而，控制操作机手部沿衔接这些结点的直点，以利于控制，从而，控制操作机手部沿衔接这些结点的直线运动。这种方法的优点是，它使我们能控制操作机手部跟踪线运动。这种方法的优点是，它使我们能控制操作机手部跟踪运动的物体。虽然目的位置是用变换描画的，但它们并不规定运动的物体。虽然目的位置是用变换描画的，但它们并不规定操作机手部怎样由一个变换运动到另一个变换。操作机手部怎样由一个变换运动到另一个变换。 Paul 用一个直线平移和两个转动来完成两相邻笛卡尔结点用一个直线平移和两个转动来完成两相邻笛

47、卡尔结点间的运动。第一个转动是绕单位矢量间的运动。第一个转动是绕单位矢量 k 进展的，把工具或末端进展的，把工具或末端执行器的轴线调整到预定的接近方向；而第二个转动那么绕工执行器的轴线调整到预定的接近方向；而第二个转动那么绕工具轴调整工具的方向。具轴调整工具的方向。7.4.1 齐次变换矩阵法齐次变换矩阵法 普通来说，操作机的目的位置可用如下根本矩阵方程表示：普通来说，操作机的目的位置可用如下根本矩阵方程表示：060base6toolbaseobjTT= C(t)P其中，其中，0T6 为描画操作机手部相对于机为描画操作机手部相对于机座坐标系位姿的齐次变换矩阵；座坐标系位姿的齐次变换矩阵； 6Tt

48、ool 为描画工具相对于手部坐标系位姿的齐为描画工具相对于手部坐标系位姿的齐次变换矩阵，它描画了受控制的工具端次变换矩阵，它描画了受控制的工具端点；点； 0Cbase(t) 为描画物体位姿的任务为描画物体位姿的任务坐标系相对于机座坐标系的齐次变换矩坐标系相对于机座坐标系的齐次变换矩阵；阵； basePobj 为描画末端执行器相对为描画末端执行器相对于任务坐标系预定抓取物体的位姿的齐于任务坐标系预定抓取物体的位姿的齐次变换矩阵。次变换矩阵。 左边的矩阵描画操作机抓取位姿，而右边的矩阵描画了我左边的矩阵描画操作机抓取位姿，而右边的矩阵描画了我们要让操作机工具抓握物体特征部位的位姿。们要让操作机工具

49、抓握物体特征部位的位姿。0Cbase(t)B06 0b a s e6t o o l b a s eo b jT T= C( t )P06 0 b a s e6t o o l b a s e o b jT T= C( t )T06 0 b a s e6t o o l b a s e o b jT T = C ( t )Tobjtool 我们可以解出我们可以解出0T6 ，它描画了以正确和预定的方式抓取物，它描画了以正确和预定的方式抓取物体时操作机应有的形状。体时操作机应有的形状。100base66baseobjtoolT = C(t)PT 假设计算假设计算0T6 的速度足够高，并转换成相应的关节角

50、，那么的速度足够高，并转换成相应的关节角，那么就可控制操作机沿预定的轨迹运动就可控制操作机沿预定的轨迹运动 利用前式，确定一项作业的利用前式，确定一项作业的 N个个 目的位置的序列可表示为目的位置的序列可表示为由由Ci(t) Pi确定的位置，可求得相邻点之间的间隔。假设进一确定的位置，可求得相邻点之间的间隔。假设进一步给定线速度和角速度，即可求得由位置步给定线速度和角速度，即可求得由位置i 到到 i+1所需的时间。所需的时间。 以上位置的工具和运动坐标系都是相对于机座坐标系给定以上位置的工具和运动坐标系都是相对于机座坐标系给定的。从一个位置移向下一个位置最好是相对于运动坐标系给的。从一个位置移

51、向下一个位置最好是相对于运动坐标系给定位置和工具。这样，从运动坐标系察看，工具是静止的。定位置和工具。这样，从运动坐标系察看，工具是静止的。为了做到这一点，需求相对于后续的坐标系重新定义当前的为了做到这一点，需求相对于后续的坐标系重新定义当前的位置和工具。利用双下标符号位置和工具。利用双下标符号 Pij 重新定义重新定义Pi变换，就不难变换，就不难做到这一点。做到这一点。 Pij是在第是在第 j 个运动坐标系中表示的位置个运动坐标系中表示的位置Pi。那么在位置。那么在位置1，将它在目的坐标系中表示，可得将它在目的坐标系中表示，可得1111tool6TT = C (t)P2212tool6TT

52、= C (t)P将它用位置将它用位置2时的坐标系表示，那么时的坐标系表示，那么有有由以上两个方程即可求得由以上两个方程即可求得1112211112()tooltoolP= C(t)C (t)PTT 上述方程的目的是要在知上述方程的目的是要在知 P11时时 求得求得P12 。因此，在恣。因此，在恣意二相邻位置意二相邻位置 i 和和 i+1 之间的运动有之间的运动有111,11,11( )( )()tooltooliiiii iiiPCt C t PTT其中其中Pi,i+1 和和Pi+1,i+1 为上面所讨论过的变换。为上面所讨论过的变换。 Paul 运用了一种简单的方法，以控制操作机手部从一个运

53、用了一种简单的方法，以控制操作机手部从一个变换运动到另一个变换。此法包括一个平移运动和一个绕空变换运动到另一个变换。此法包括一个平移运动和一个绕空间某定轴的转动以及另一个绕工具轴的转动，以便构成操作间某定轴的转动以及另一个绕工具轴的转动，以便构成操作机受控的线速度和角速度运动。第一个转动使工具轴与预定机受控的线速度和角速度运动。第一个转动使工具轴与预定的接近方向对准，第二个转动使工具轴与方向矢量对准。的接近方向对准，第二个转动使工具轴与方向矢量对准。 由位置由位置 i 到到 i+1的运动可用的运动可用“驱动变换驱动变换D()表示，它是归表示，它是归一化时间一化时间 的函数的函数161,11(

54、)( )( )()toolii iiTCPDT其中，其中，1,1,1(1)iii iPPD1,11,1(1)()i iiiDPP 可得tt于是在位置于是在位置 i 时，时， =0，D(0)为单位矩阵为单位矩阵用位置用位置 i 和和 i+1各自的齐次变换矩阵表示它们，可得各自的齐次变换矩阵表示它们，可得利用前式，求利用前式，求 Pi,i+1的逆并乘以的逆并乘以Pi+1,i+1 ，得到，得到 假设驱动函数由一个平移运动和两个旋转运动构成，那么这假设驱动函数由一个平移运动和两个旋转运动构成，那么这些平移和转动量将直接同些平移和转动量将直接同成正比。假设成正比。假设随时间线性地变化，随时间线性地变化，

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