机器人技术第七章机器人的轨迹规划

1、1& 第七章第七章 机器人的轨迹规划机器人的轨迹规划7.1 7.1 机器人规划的定义和作用机器人规划的定义和作用7.1.1 概述概述机器人学中的一个基本问题是为解决某个预定的任务而规划机器人学中的一个基本问题是为解决某个预定的任务而规划机器人的动作，然后在机器人执行完成那些动作所需的命令时机器人的动作，然后在机器人执行完成那些动作所需的命令时控制它。这里，规划的意思就是机器人在行动前确定一系列动控制它。这里，规划的意思就是机器人在行动前确定一系列动作作(作决策作决策)，这种动作的确定可用问题求解系统来解决，给定，这种动作的确定可用问题求解系统来解决，给定初始情况后，该系统可达到某一规定

2、的目标。因此，初始情况后，该系统可达到某一规定的目标。因此，规划就是规划就是指机器人为达到目标而需要的行动过程的描述。指机器人为达到目标而需要的行动过程的描述。 规划内容可能没有次序，但是一般来说，规划具有某个规划规划内容可能没有次序，但是一般来说，规划具有某个规划目标的蕴含排序。例如，早晨起床后的安排。目标的蕴含排序。例如，早晨起床后的安排。 缺乏规划可能导致不是最佳的问题求解，甚至得不到问题的缺乏规划可能导致不是最佳的问题求解，甚至得不到问题的求解。求解。2 许多规划所包的步骤是含糊的，而且需要进一步说明（子许多规划所包的步骤是含糊的，而且需要进一步说明（子规划）。规划）。大多数规划具有很

3、大的子规划结构，规划中的每个大多数规划具有很大的子规划结构，规划中的每个目标可以由达到此目标的比较详细的子规划所代替。因此，目标可以由达到此目标的比较详细的子规划所代替。因此，最终得到的规划是某个问题求解算符的线性或分部排序，但最终得到的规划是某个问题求解算符的线性或分部排序，但是由算符来实现的目标常常具有分层结构。是由算符来实现的目标常常具有分层结构。 把某些比较复杂的问题分解为一些比较小的问题的想法使把某些比较复杂的问题分解为一些比较小的问题的想法使我们应用规划方法求解问题在实际上成为可能。我们应用规划方法求解问题在实际上成为可能。 有两条能够实现这种分解的重要途径：第一条是当从一个有两条

4、能够实现这种分解的重要途径：第一条是当从一个问题状态移动到下一个状态时，无需计算整个新的状态，而问题状态移动到下一个状态时，无需计算整个新的状态，而只要考虑状态中可能变化了的那些部分。第二条是把单一的只要考虑状态中可能变化了的那些部分。第二条是把单一的困难问题分割为几个有希望的、较为容易解决的子问题，这困难问题分割为几个有希望的、较为容易解决的子问题，这种分解能够使困难问题的求解变得容易些种分解能够使困难问题的求解变得容易些。3 机器人规划分为机器人规划分为高层规划高层规划和和低层规划低层规划。自动规划自动规划在机器人在机器人规划中称为高层规划。在无特别说明时，机器人规划都是指规划中称为高层规

5、划。在无特别说明时，机器人规划都是指自动规划。自动规划是一种重要的问题求解技术，它从某个自动规划。自动规划是一种重要的问题求解技术，它从某个特定的问题状态出发，寻求一系列行为动作，并建立一个操特定的问题状态出发，寻求一系列行为动作，并建立一个操作序列，直到求得目标状态为止。与一般问题求解相比，自作序列，直到求得目标状态为止。与一般问题求解相比，自动规划更注重于问题的求解过程，而不是求解结果。动规划更注重于问题的求解过程，而不是求解结果。 我们在阐述机器人自动规划问题时，机器人一般配备有传我们在阐述机器人自动规划问题时，机器人一般配备有传感器和一组能在某个易于理解的现场中完成的基本动作。这感器和

6、一组能在某个易于理解的现场中完成的基本动作。这些动作可把该现场从一种状态或布局变换为另一种状态或布些动作可把该现场从一种状态或布局变换为另一种状态或布局。例如，局。例如， “积木世界积木世界” 。4 机器人能得到的一个解答是由下面的算符序列组成的：机器人能得到的一个解答是由下面的算符序列组成的：目标状态目标状态 机器人规划是机器人学的一个重要研究领域，也是人工智能机器人规划是机器人学的一个重要研究领域，也是人工智能与机器人学一个令人感兴趣的结合点。与机器人学一个令人感兴趣的结合点。5 机器人的规划程序只需要给定任务机器人的规划程序只需要给定任务初初始状态和最终状态的描始状态和最终状态的描述。这

7、些规划系统一般都不说明为述。这些规划系统一般都不说明为实实现现一一个算符所需的详个算符所需的详细细的的机器人运动。任务规划程序机器人运动。任务规划程序则则把任务级的说明变换成操作机级把任务级的说明变换成操作机级的说明。为了进行这种变换，任务规划程序必须的说明。为了进行这种变换，任务规划程序必须包含包含被操作物被操作物体、任务环境、机器人执行的任务、环境的体、任务环境、机器人执行的任务、环境的初初始状态和所要求始状态和所要求的最终的最终( (目标目标) )状态等描述。任务规划程序状态等描述。任务规划程序的的输出就是一个机器输出就是一个机器人程序，在指定的人程序，在指定的初初始状态下执行该程序后，

8、就能达到所要求始状态下执行该程序后，就能达到所要求的最终状态。的最终状态。 任务规划有三个阶段：建立模型、任务说明和操作机程序综任务规划有三个阶段：建立模型、任务说明和操作机程序综合。任务的世界模型应含有如下的信息合。任务的世界模型应含有如下的信息:(1):(1)任务环境中的所有任务环境中的所有物体和机器人的几何描述；物体和机器人的几何描述；(2)(2)所有物体的物理描述；所有物体的物理描述；(3)(3)所有所有连接件的运动学描述，连接件的运动学描述，(4) (4) 机器人和传感器特性的描述。在世机器人和传感器特性的描述。在世界模型中，任务状态模型还必包括全部物体和连接件的布局。界模型中，任务

9、状态模型还必包括全部物体和连接件的布局。7.1.2 机器人任务规划的作用机器人任务规划的作用6 机器人轨迹规划机器人轨迹规划属于机器人低层规划，基本上不涉及人工属于机器人低层规划，基本上不涉及人工智能问题，而是在机械手运动学和动力学的基础上，讨论机智能问题，而是在机械手运动学和动力学的基础上，讨论机器人运动的规划及其方法。所谓器人运动的规划及其方法。所谓轨迹轨迹，就是指机器人在运动，就是指机器人在运动过程中的位移、速度和加速度。过程中的位移、速度和加速度。 轨迹规划问题通常是将轨迹规划器看成轨迹规划问题通常是将轨迹规划器看成“黑箱黑箱”，接受表示，接受表示路径约束的输入变量，输出为起点和终点之

10、间按时间排列的操路径约束的输入变量，输出为起点和终点之间按时间排列的操作机中间形态（位姿作机中间形态（位姿, 速度和加速度）序列。速度和加速度）序列。路径约束路径约束动力学约束动力学约束路径设定路径设定轨迹规划器轨迹规划器7.2 7.2 机器人轨迹规划的一般性问题机器人轨迹规划的一般性问题由初始点运动到终止由初始点运动到终止点，所经过的由中间点，所经过的由中间形态序列构成的空间形态序列构成的空间曲线称为曲线称为路径。路径。这些这些形态序列即是曲线上形态序列即是曲线上的的“点点”。7规划操作机的轨迹有两种常用的方法：规划操作机的轨迹有两种常用的方法： 第一种方法：要求使用者在沿轨迹选定的位置点上

11、第一种方法：要求使用者在沿轨迹选定的位置点上(称为结称为结节或插值点节或插值点)显式地给定广义坐标位置、速度和加速度的一组显式地给定广义坐标位置、速度和加速度的一组约束约束(例如，连续性和光滑程度等例如，连续性和光滑程度等)。然后，轨迹规划器从插值。然后，轨迹规划器从插值和满足插值点约束的函数中选定参数化轨迹。显然，在这种和满足插值点约束的函数中选定参数化轨迹。显然，在这种方法中，约束的给定和操作机轨迹规划是在关节坐标系中进方法中，约束的给定和操作机轨迹规划是在关节坐标系中进行的。行的。 第二种方法：使用者以解析函数显式地给定操作机必经之第二种方法：使用者以解析函数显式地给定操作机必经之路径，

12、例如，笛卡尔坐标中的直线路径。然后，轨迹规划器路径，例如，笛卡尔坐标中的直线路径。然后，轨迹规划器在关节坐标或笛卡几坐标中确定一条与给定路径近似的轨迹。在关节坐标或笛卡几坐标中确定一条与给定路径近似的轨迹。在这种方法中，路径约束是在笛卡尔坐标中给定的。在这种方法中，路径约束是在笛卡尔坐标中给定的。8 在第一种方法中，约束的给定和操作机轨迹规划在关节坐标在第一种方法中，约束的给定和操作机轨迹规划在关节坐标系中进行。由于对操作机手部没有约束，使用者难于跟踪操作系中进行。由于对操作机手部没有约束，使用者难于跟踪操作机手部运行的路径。因此，操作机手部可能在没有事先警告的机手部运行的路径。因此，操作机手

13、部可能在没有事先警告的情况下与障碍物相碰。情况下与障碍物相碰。 在第二种方法中，路径约束在笛卡尔坐标中给定，而关节驱在第二种方法中，路径约束在笛卡尔坐标中给定，而关节驱动器是在关节坐标中受控制的。因此，为了求得一条逼近给定动器是在关节坐标中受控制的。因此，为了求得一条逼近给定路径的轨迹，必须用函数近似把笛卡尔坐标中的路径约束变换路径的轨迹，必须用函数近似把笛卡尔坐标中的路径约束变换为关节坐标中的路径约束，再确定满足关节坐标路径约束的参为关节坐标中的路径约束，再确定满足关节坐标路径约束的参数化轨迹。数化轨迹。v 轨迹规划既可在关节变量空间中进行，也可在笛卡尔空间进轨迹规划既可在关节变量空间中进行

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

15、笛卡尔坐标径可达到相当的准确性。可是由于现代还没有可用笛卡尔坐标测量操作机手部位置的传感器，所有可用的控制算法都是建立测量操作机手部位置的传感器，所有可用的控制算法都是建立在关节坐标基础上的。因此，笛卡尔空间路径规划就需要在笛在关节坐标基础上的。因此，笛卡尔空间路径规划就需要在笛卡尔坐标和关节之间进行实时变换，这是一个计算量很大的任卡尔坐标和关节之间进行实时变换，这是一个计算量很大的任务，常常导致较长的控制间隔。务，常常导致较长的控制间隔。 由笛卡尔坐标向关节坐标的变换是病态的，因而它不是一一由笛卡尔坐标向关节坐标的变换是病态的，因而它不是一一对应的映射。对应的映射。 如果在轨迹规划阶段要考虑

16、操作机的动力学特性，就要以笛如果在轨迹规划阶段要考虑操作机的动力学特性，就要以笛卡尔坐标给定路径约束，同时以关节坐标给定物理约束卡尔坐标给定路径约束，同时以关节坐标给定物理约束(例如，例如，每个关节电机的力和力矩、速度和加速度权限每个关节电机的力和力矩、速度和加速度权限)。这就会使最。这就会使最后的优化问题具有在两个不同坐标系中的混合约束。后的优化问题具有在两个不同坐标系中的混合约束。在笛卡尔空间进行轨迹规划的特点：在笛卡尔空间进行轨迹规划的特点：10 在关节变量空间的规划有三个优点：在关节变量空间的规划有三个优点：(1) 直接用运动时的受控变量规划轨迹；直接用运动时的受控变量规划轨迹；(2)

17、 轨迹规划可接近实时地进行；轨迹规划可接近实时地进行；(3) 关节轨迹易于规划。关节轨迹易于规划。 伴随的缺点是难于确定运动中各杆件和手的位置，但是，为伴随的缺点是难于确定运动中各杆件和手的位置，但是，为了避开轨迹上的障碍常常又要求知道一些杆件和手位置。了避开轨迹上的障碍常常又要求知道一些杆件和手位置。 由于面向笛卡尔空间的方法有前述钟种缺点，使得面向关节由于面向笛卡尔空间的方法有前述钟种缺点，使得面向关节空间的方法被广泛采用。它把笛卡尔结点变换为相应的关节坐空间的方法被广泛采用。它把笛卡尔结点变换为相应的关节坐标，并用低次多项式内插这些关节结点。这种方法的优点是计标，并用低次多项式内插这些关

18、节结点。这种方法的优点是计算较快，而且易于处理操作机的动力学约束。但当取样点落在算较快，而且易于处理操作机的动力学约束。但当取样点落在拟合的光滑多项式曲线上时，面向关节空间的方法沿笛卡尔路拟合的光滑多项式曲线上时，面向关节空间的方法沿笛卡尔路径的准确性会有损失。径的准确性会有损失。11一般说来，生成关节轨迹设定点的基本算法是很简单的：一般说来，生成关节轨迹设定点的基本算法是很简单的：0tt循环：等待下一个控制间隔；循环：等待下一个控制间隔； 时刻操作机关节所处的位置时刻操作机关节所处的位置； 若若 ，则退出循环；，则退出循环； 转向循环。转向循环。ttt ( )h ttftt 从上述算法可以看

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

20、 )。7.3 7.3 关节插值轨迹关节插值轨迹12 若某关节若某关节(例如关节例如关节 i )的关节轨迹使用的关节轨迹使用 p 个多项式，则要满个多项式，则要满足初始和终止条件足初始和终止条件(关节位置、速度和加速度关节位置、速度和加速度)，并保证这些变，并保证这些变量在多项式衔接处的连续性，因而需要确定量在多项式衔接处的连续性，因而需要确定 3( p十十1)个系数。个系数。 若给定附加的中间条件若给定附加的中间条件(例如位置例如位置)，则对每个中间条件需要，则对每个中间条件需要增加一系数。通常，可以给定两个中间位置，一个靠近初始位增加一系数。通常，可以给定两个中间位置，一个靠近初始位置；另一

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

22、所对应的多项式系数的个数确定多项式的次数由约束条件数所对应的多项式系数的个数确定多项式的次数13 为了控制操作机，在规划运动轨迹之前，需要给定机器人为了控制操作机，在规划运动轨迹之前，需要给定机器人在初始点和终止点的手臂形态。在规划机器人关节插值运动在初始点和终止点的手臂形态。在规划机器人关节插值运动轨迹时，需要注意下述几点：轨迹时，需要注意下述几点：1、抓住一个物体时，手的运动方向应该指向离开物体支承表、抓住一个物体时，手的运动方向应该指向离开物体支承表面的方向。否则，手可能与支承面相碰。面的方向。否则，手可能与支承面相碰。2、若沿支承面的法线方向从初始点向外给定一个离开位置、若沿支承面的法

23、线方向从初始点向外给定一个离开位置(提升点提升点)，并要求手，并要求手(即手部坐标系的原点即手部坐标系的原点)经过此位置，这种经过此位置，这种离开运动就是允许的。如果还给定由初始点运动到离开位置离开运动就是允许的。如果还给定由初始点运动到离开位置的时间，我们就可以控制提起物体运动的速度。的时间，我们就可以控制提起物体运动的速度。3、对于手臂运动提升点的要求同样也适用于终止位置运动的、对于手臂运动提升点的要求同样也适用于终止位置运动的下放点下放点(即必须先运动到支承表面外法线方向上的某点，再慢即必须先运动到支承表面外法线方向上的某点，再慢慢下移至终止点慢下移至终止点)。这样，可获得和控制正确的接

24、近方向。这样，可获得和控制正确的接近方向。4、对手臂的每一次运动，都设定上述四个点：初始点，提升、对手臂的每一次运动，都设定上述四个点：初始点，提升点，下放点和终止点。点，下放点和终止点。145、位置约束、位置约束 (a) 初始点：给定速度和加速度初始点：给定速度和加速度(一般为零一般为零)； (b) 提升点：中间点运动的连续；提升点：中间点运动的连续； (c) 下放点：同提升点；下放点：同提升点； (d) 终止点：给定速度和加速度终止点：给定速度和加速度(一般为零一般为零)。156、除上述约束外，所有关节轨迹的极值不能超出每个关节、除上述约束外，所有关节轨迹的极值不能超出每个关节变量的物理和

25、几何极限。变量的物理和几何极限。7、时间的考虑、时间的考虑 (a) 轨迹的初始段和终止段：时间由手接近和离开支承表面轨迹的初始段和终止段：时间由手接近和离开支承表面的速率决定；也是由关节电机特性决定的某个常数。的速率决定；也是由关节电机特性决定的某个常数。 (b) 轨迹的中间点或中间段：时间由各关节的最大速度和加轨迹的中间点或中间段：时间由各关节的最大速度和加速度决定，将使用这些时间中的一个最长时间速度决定，将使用这些时间中的一个最长时间(即用最低速即用最低速关节确定的最长时间来归一化关节确定的最长时间来归一化)。 在关节轨迹的典型约束条件之下，我们所要研究的是选择在关节轨迹的典型约束条件之下

26、，我们所要研究的是选择一种一种 n 次次(或小于或小于 n 次次)的多项式函数，使得在各结点的多项式函数，使得在各结点(初始点，初始点，提升点，下放点和终止点提升点，下放点和终止点)上满足对位置、速度和加速度的要上满足对位置、速度和加速度的要求，并使关节位置、速度和加速度在整个时间间隔求，并使关节位置、速度和加速度在整个时间间隔 t0, tf 中中保持连续。保持连续。16 1. 位置位置(给定给定) 2速度速度(给定，通常为零给定，通常为零) 3. 加速度加速度(给定，通常为零给定，通常为零) 4. 提升点位置提升点位置(给定给定) 5提升点位置（与前一段轨迹连续）提升点位置（与前一段轨迹连续

27、） 6. 速度（与前一段轨迹连续）速度（与前一段轨迹连续） 7加速度（与前一段轨迹连续）加速度（与前一段轨迹连续） 8. 下放点位置下放点位置(给定给定) 9下放点位置（与前一段轨迹连续）下放点位置（与前一段轨迹连续） 10. 速度（与前一段轨迹连续）速度（与前一段轨迹连续） 11. 加速度（与前一段轨迹连续）加速度（与前一段轨迹连续） 12. 位置位置(给定给定) 13. 速度速度(给定，通常为零给定，通常为零) 14. 加速度加速度(给定，通常为零给定，通常为零)终止位置终止位置中间位置中间位置初始位置初始位置 规划关节插值轨迹的约束条件：规划关节插值轨迹的约束条件：17一种方法是为每个关

28、节规定一个七次多项式函数一种方法是为每个关节规定一个七次多项式函数式中，未知系数式中，未知系数 aj 可由已知的位置和连续条件确定。但用这种可由已知的位置和连续条件确定。但用这种高次多项式内插给定的结点也许不能令人满意，因为它的极值高次多项式内插给定的结点也许不能令人满意，因为它的极值难求，而且容易产生额外的运动。难求，而且容易产生额外的运动。 另一种办法是将整个关节空间轨迹分割成几段，在每段轨迹另一种办法是将整个关节空间轨迹分割成几段，在每段轨迹中用不同的低次多项式来插值。有几种分割轨迹的方法，每种中用不同的低次多项式来插值。有几种分割轨迹的方法，每种方法的特性各不相同。方法的特性各不相同。

29、 434 轨迹轨迹 每个关节有下面三段轨迹：第一段由初始点到提升点的轨每个关节有下面三段轨迹：第一段由初始点到提升点的轨迹用四次多项式表示。第二段迹用四次多项式表示。第二段(或中间段）由提升点到下放或中间段）由提升点到下放点的轨迹用三次多项式表示。最后一段由下放点到终止点的点的轨迹用三次多项式表示。最后一段由下放点到终止点的轨迹由四次多项式表示。轨迹由四次多项式表示。18 353 轨迹轨迹 与与434轨迹相同，但每段所用多项式次数与前种不同。轨迹相同，但每段所用多项式次数与前种不同。第一段用三次多项式，第二段用五次多项式，最后一段用三第一段用三次多项式，第二段用五次多项式，最后一段用三次多项式

30、。次多项式。 (33333) 轨迹轨迹 对五段轨迹都使用三次多项式样条函数。对五段轨迹都使用三次多项式样条函数。 注意，上述讨论对每个关节轨迹都是有效的，即每个关节注意，上述讨论对每个关节轨迹都是有效的，即每个关节轨迹可分割成三段或五段。轨迹可分割成三段或五段。7.3.1 434 关节轨迹的计算关节轨迹的计算 对于对于N个关节个关节, 在每段轨迹规划中就要确定在每段轨迹规划中就要确定 N 条关节轨迹，条关节轨迹，引用归一化时间变量引用归一化时间变量 是方便的，它使我们能用同样的方是方便的，它使我们能用同样的方法处理每个关节每段轨迹的方程。时间变化范围均由法处理每个关节每段轨迹的方程。时间变化范

31、围均由 (各各段轨迹的初始时间段轨迹的初始时间)变到变到 (各段轨迹的终止时间各段轨迹的终止时间)。0,1t0t 1t 19定义下列变量：定义下列变量： 轨迹是由多项式序列轨迹是由多项式序列hi(t)构成的，这些多项式合起来形成关构成的，这些多项式合起来形成关节节 j 的轨迹。在每段轨迹中的轨迹。在每段轨迹中关节变量关节变量的多项式用归一化时间表的多项式用归一化时间表示为示为:20这些关节轨迹分段多项式所应满足的边界条件为这些关节轨迹分段多项式所应满足的边界条件为:21434关节轨迹的边界条件如图示关节轨迹的边界条件如图示。22这些多项式对实际时间这些多项式对实际时间 t 的一阶和二阶导数。可

32、写成的一阶和二阶导数。可写成第一段轨迹的基本多项式是四次的第一段轨迹的基本多项式是四次的23对于对于t =0(这段轨迹的初始位置这段轨迹的初始位置)，要满足此位置的边界条件，则，要满足此位置的边界条件，则24用求出的这些未知量得到用求出的这些未知量得到对于对于t=1(这段轨迹的终点这段轨迹的终点)，在此位置，我们放宽插值多项式，在此位置，我们放宽插值多项式必须准确通过该点的要求，仅要求在此位置的速度和加速度必必须准确通过该点的要求，仅要求在此位置的速度和加速度必须和下一段轨迹起点的速度和加速度连续。此处的速度和加速须和下一段轨迹起点的速度和加速度连续。此处的速度和加速度为度为第二段轨迹的基本多

33、项式是三次的第二段轨迹的基本多项式是三次的25对于对于t=0(提升点提升点)，此点的速度和加速度分别为，此点的速度和加速度分别为由于此点的速度相加速度必须分别和前一段轨迹终点的速度和由于此点的速度相加速度必须分别和前一段轨迹终点的速度和加速度连续，故可得加速度连续，故可得26或或或或 对于对于t=1(下放点下放点)，该点的速度相加速度必须与下一段轨迹，该点的速度相加速度必须与下一段轨迹起点处的速度和加速度连续。可分别求得此位置的速度和加速起点处的速度和加速度连续。可分别求得此位置的速度和加速度为度为27末段轨迹的基本多项式是四次的末段轨迹的基本多项式是四次的如果在上式中以如果在上式中以 代替代

34、替 t ，我们就把归一化时间，我们就把归一化时间 t 由由 移至移至 。1tt 0,1t1,0t 28可得可得对于对于 (这段轨迹的终点这段轨迹的终点)，要满足轨迹终点的边界条件，即，要满足轨迹终点的边界条件，即0t 对于对于 (这段轨迹的起点这段轨迹的起点)，要满足轨迹起点的边界条件，即，要满足轨迹起点的边界条件，即1t 29在此下放点的速度和加速度连续性条件是：在此下放点的速度和加速度连续性条件是：或或30可以求出相连轨迹段间关节角之差为可以求出相连轨迹段间关节角之差为 轨迹多项式其余七个未知系数可由以上的速度、加速度连续轨迹多项式其余七个未知系数可由以上的速度、加速度连续约束条件的联立方

35、程解出，用矩阵矢量符号改写这些方程，可约束条件的联立方程解出，用矩阵矢量符号改写这些方程，可得得其中其中31或或C 矩阵的结构便于计算未知系数，若时间间隔矩阵的结构便于计算未知系数，若时间间隔 ti 为正值，为正值， C 的逆矩阵总是存在的。则关节各段关节轨迹多项的逆矩阵总是存在的。则关节各段关节轨迹多项式系数即可全部求得。式系数即可全部求得。(1,2, )in32同样，我们可用此方法可计算同样，我们可用此方法可计算353 关节轨迹。关节轨迹。 对于末段轨迹，把归一化时间对于末段轨迹，把归一化时间 t 由由-1，0重新变回重新变回0, 1，可求出可求出 hn(t)334-3-4 关节轨迹多项式

36、关节轨迹多项式34353-5-3 关节轨迹多项式关节轨迹多项式367.3.2 三三次样条轨迹（五段三次多项式）次样条轨迹（五段三次多项式） 用一组三次多项式插补给定的函数并保证插补点处一阶和二用一组三次多项式插补给定的函数并保证插补点处一阶和二阶导数连续的方法，称为三次样条函数法。所能达到的近似程阶导数连续的方法，称为三次样条函数法。所能达到的近似程度和光滑程度是相当好的。度和光滑程度是相当好的。 一般来说，样条曲线是在插值点具有一般来说，样条曲线是在插值点具有 k-1 阶导数连续性的阶导数连续性的 k次多项式。对于三次样条函数，一阶导数代表速度的连续性，次多项式。对于三次样条函数，一阶导数代

37、表速度的连续性，二阶导数代表加速度的连续性。二阶导数代表加速度的连续性。 三次样条函数有某些优点。首先，它是使速度和加速度连续三次样条函数有某些优点。首先，它是使速度和加速度连续的最低次多项式函数。其次，低次多项式减少了计算量和数值的最低次多项式函数。其次，低次多项式减少了计算量和数值不稳定的可能性。不稳定的可能性。每段关节轨迹的五段三次多项式的通式为每段关节轨迹的五段三次多项式的通式为其中其中37 在在应用五段三次多项式插值时，需要有五段轨迹和六个插值应用五段三次多项式插值时，需要有五段轨迹和六个插值点。点。但是，在前面的讨论中只有四个插值点，即初始点、提升但是，在前面的讨论中只有四个插值点

38、，即初始点、提升点、下放点和终止点。所以，必须选择另外两个插值点，以便点、下放点和终止点。所以，必须选择另外两个插值点，以便有足够多的边界条件求解各多项式系数。可以在提升点和下放有足够多的边界条件求解各多项式系数。可以在提升点和下放点之间选取这两个额外的结点。点之间选取这两个额外的结点。 没有必要知道这两个点的确切位置，只要知道时间间隔，以没有必要知道这两个点的确切位置，只要知道时间间隔，以及必须满足这两点速度和加速度的连续性条件。因此，这组关及必须满足这两点速度和加速度的连续性条件。因此，这组关节轨迹分段多项式必须满足的边界条件是：节轨迹分段多项式必须满足的边界条件是： (1) 在初始点、提

39、升点、下放点和终止点的位置约束；在初始点、提升点、下放点和终止点的位置约束； (2) 在所有插值点的速度和加速度的连续性。在所有插值点的速度和加速度的连续性。38 五段三次关节轨迹的边界条件示于中。其中有下划线的变量五段三次关节轨迹的边界条件示于中。其中有下划线的变量是在计算五段三次多项式之前的已知量。是在计算五段三次多项式之前的已知量。39这些多项式对实际时间的一阶和二阶导数为：这些多项式对实际时间的一阶和二阶导数为：式中，式中，tj 是通过第是通过第 j 段轨迹所需的实际时间。给定了初始点和段轨迹所需的实际时间。给定了初始点和终止点的位置、速度和加速度，第一段轨迹和末段轨迹的多项终止点的位

40、置、速度和加速度，第一段轨迹和末段轨迹的多项式式 h1 (t) 和和hn (t) 就完全确定了。就完全确定了。旦算出这两个多项式，就可旦算出这两个多项式，就可用位置约束和连续条件求出用位置约束和连续条件求出h2 (t)、 h3 (t) 和和 h4 (t) 。第一段轨迹，基本多项式为第一段轨迹，基本多项式为当当 t=0 时，由该位置的边界条件，可得时，由该位置的边界条件，可得(给定给定)40由此可得：由此可得：当当 t=1 时，由此位置的边界条件，可得：时，由此位置的边界条件，可得：可得可得其中，其中，i= i i-1 。第一段轨迹的多项式就完全确定了。第一段轨迹的多项式就完全确定了。(给定给定

41、)41由上式可求出由上式可求出 t1时的速度和加速度：时的速度和加速度：此速度相加速度必须与下一段轨迹起点的速度和加速度连续。此速度相加速度必须与下一段轨迹起点的速度和加速度连续。末段轨迹的多项式为（仍做类似代换末段轨迹的多项式为（仍做类似代换 ）(给定给定)(给定给定)1tt 当当t0 和和t1时（对应时（对应 和和 ），由边界条件，有），由边界条件，有1t 0t 42由上述方程解出未知系数，最后得到：由上述方程解出未知系数，最后得到： 给定初始点、提升点、下放点和终止点的位置以及通过每段给定初始点、提升点、下放点和终止点的位置以及通过每段轨迹所需要的时间轨迹所需要的时间 (tj ) 后，五

42、段三次多项式即可后，五段三次多项式即可唯一地唯一地确定，确定，满足所有位置约束和连续条件。满足所有位置约束和连续条件。算出以上个多项式，就可用位置约束和连续条件求出算出以上个多项式，就可用位置约束和连续条件求出h2 (t)、 h3 (t) 和和 h4 (t) 。43 对于较复杂的机器人系统，为了控制操作机完成作业而设对于较复杂的机器人系统，为了控制操作机完成作业而设计了编程语言。在这种系统中，作业通常是用操作机手部计了编程语言。在这种系统中，作业通常是用操作机手部(或或末端执行器末端执行器)必须通过的笛卡尔结点序列给定的。因此，描述必须通过的笛卡尔结点序列给定的。因此，描述操作机在作业中的运动

43、时，我们就更关心描述操作机手部要达操作机在作业中的运动时，我们就更关心描述操作机手部要达到的目标位置及通过的笛卡尔空间曲线到的目标位置及通过的笛卡尔空间曲线(或路径或路径)的形式。的形式。7.4 7.4 笛卡尔路径轨迹规划笛卡尔路径轨迹规划 一般，实现笛卡尔路径规划可采用下述两个相连的步骤：一般，实现笛卡尔路径规划可采用下述两个相连的步骤：(1)沿笛卡尔路径，按照某种规则以笛卡尔坐标生成或选择沿笛卡尔路径，按照某种规则以笛卡尔坐标生成或选择一组结点或插值点；一组结点或插值点；(2)规定一种函数，按某些准则连接这规定一种函数，按某些准则连接这些结点些结点(或逼近分段的路径或逼近分段的路径)。 对

44、于第二步，所选用的准则可以采用两种控制算法，以保对于第二步，所选用的准则可以采用两种控制算法，以保证跟踪给定的路径。证跟踪给定的路径。44 (1) 面向笛卡尔空间的方法。在此方法中，大部分计算和面向笛卡尔空间的方法。在此方法中，大部分计算和优化是以笛卡尔坐标完成的，然后，在手部这一级上进行控优化是以笛卡尔坐标完成的，然后，在手部这一级上进行控制。按固定的取样间隔在预定路径上选择伺服取样点，在控制。按固定的取样间隔在预定路径上选择伺服取样点，在控制操作机时实时地把它们转换为与之相应的关节变量。所得制操作机时实时地把它们转换为与之相应的关节变量。所得到的轨迹是分段直线。到的轨迹是分段直线。 (2)

45、 面向关节空间的方法。这种方法用关节变量空间中的面向关节空间的方法。这种方法用关节变量空间中的低次多项式函数迫近直线路径上的两相邻结点间的一段路径，低次多项式函数迫近直线路径上的两相邻结点间的一段路径，而控制是在关节这一级上进行的。所得到的笛卡尔路径是不而控制是在关节这一级上进行的。所得到的笛卡尔路径是不分段的直线。分段的直线。 Paul 叙述了用一系列直线段构成操作机手部笛卡尔路径叙述了用一系列直线段构成操作机手部笛卡尔路径的设计方法。把手部在这些直线段中的速度和加速度转换的设计方法。把手部在这些直线段中的速度和加速度转换到关节坐标，并用二次插值进行平滑连接，从而实现了手到关节坐标，并用二次

46、插值进行平滑连接，从而实现了手部的规划运动控制。我们将学习如何用该方法设计笛卡尔部的规划运动控制。我们将学习如何用该方法设计笛卡尔直线路径。直线路径。45对于笛卡尔路径控制，轨迹设定点算法为：对于笛卡尔路径控制，轨迹设定点算法为：0tt循环：等待下一个控制间隔；循环：等待下一个控制间隔； 时刻操作机末端所处的位置时刻操作机末端所处的位置； 相应于相应于 的关节变量解的关节变量解； 若若 ，则退出循环；，则退出循环； 转向循环。转向循环。ttt ( )H ttftt( )Q H t( )H t 这里，除了需要计算每一控制间隔中操作机手部轨迹函数这里，除了需要计算每一控制间隔中操作机手部轨迹函数

47、外，还需把笛卡尔位置变换为相应的关节变量外，还需把笛卡尔位置变换为相应的关节变量 。矩阵函数矩阵函数 表示操作机手部在表示操作机手部在 时刻的预定位置。可用时刻的预定位置。可用44齐次变换矩阵表示。齐次变换矩阵表示。( )H t( )Q H t( )H tt46 在可编程机器人系统中，预定的运动可用笛卡尔结点序列给在可编程机器人系统中，预定的运动可用笛卡尔结点序列给定，每个结点可用联系操作机手部坐标系和工作空间坐标系的定，每个结点可用联系操作机手部坐标系和工作空间坐标系的齐次变换来描述。相应于这些笛卡尔结点的关节坐标可用运动齐次变换来描述。相应于这些笛卡尔结点的关节坐标可用运动学逆问题求解程序

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

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

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

51、姿。们要让操作机工具抓握物体特征部位的位姿。0Cbase(t)B060base6toolbaseobjTT= C(t)P060base6toolbaseobjTT= C(t)T060base6toolbaseobjTT= C(t)Tobjtool48 我们可以解出我们可以解出0T6 ，它描述了以正确和预定的方式抓取物体，它描述了以正确和预定的方式抓取物体时操作机应有的形态。时操作机应有的形态。100base66baseobjtoolT = C(t)PT 如果计算如果计算0T6 的速度足够高，并转换成相应的关节角，那么的速度足够高，并转换成相应的关节角，那么就可控制操作机沿预定的轨迹运动就可控制

52、操作机沿预定的轨迹运动 利用前式，确定一项作业的利用前式，确定一项作业的 N个个 目标位置的序列可表示为目标位置的序列可表示为49由由Ci(t) Pi确定的位置，可求得相邻点之间的距离。若进一步确定的位置，可求得相邻点之间的距离。若进一步给定线速度和角速度，即可求得由位置给定线速度和角速度，即可求得由位置i 到到 i+1所需的时间。所需的时间。 以上位置的工具和运动坐标系都是相对于机座坐标系给定以上位置的工具和运动坐标系都是相对于机座坐标系给定的。从一个位置移向下一个位置最好是相对于目标坐标系给的。从一个位置移向下一个位置最好是相对于目标坐标系给定位置和工具。这样，从运动定位置和工具。这样，从

53、运动坐标系观察坐标系观察，工具是静止的。，工具是静止的。为了做到这一点，需要相对于后续的坐标系重新定义当前的为了做到这一点，需要相对于后续的坐标系重新定义当前的位置位置和工具。利用双下标符号和工具。利用双下标符号 Pij 重新定义重新定义Pi变换，就不难做变换，就不难做到这一点。到这一点。 Pij是在第是在第 j 个工作坐标系个工作坐标系中表示的位置中表示的位置Pi。则在位置。则在位置1，将它在目标坐标系将它在目标坐标系中表示，可中表示，可得得1111tool6TT = C (t)P2212tool6TT = C (t)P将它用位置将它用位置2时的坐标系表示，则有时的坐标系表示，则有50由以上

54、两个方程即可求得由以上两个方程即可求得1112211112()tooltoolP= C(t)C (t)PTT 上述方程的目的是耍在已知上述方程的目的是耍在已知 P11时时 求得求得P12 。因此，在任。因此，在任意二相邻位置意二相邻位置 I 和和 i+1 之间的运动有之间的运动有111,11,11( )( )()tooltooliiiii iiiPCt C t PTT其中其中Pi,i+11 和和Pi+1,i+1 为上面所讨论过的变换。为上面所讨论过的变换。 Paul 使用了一种简单的方法，以控制操作机手部从一个使用了一种简单的方法，以控制操作机手部从一个变换运动到另一个变换。此法包括一个平移运

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

56、ii iPPD1,11,1(1)()i iiiDPP 可得可得 于是有于是有52用位置用位置 i 和和 i+1各自的齐次变换矩阵表示它们，可得各自的齐次变换矩阵表示它们，可得利用前式，求利用前式，求 Pi,i+1的逆并乘以的逆并乘以Pi+1,i+1 ，得到，得到53 若驱动函数由一个平移运动和两个旋转运动构成，那么这些若驱动函数由一个平移运动和两个旋转运动构成，那么这些平移和转动量将直接同平移和转动量将直接同成正比。若成正比。若随时间线性地变化，则随时间线性地变化，则 D() 所代表的合成运动将相应于一个不变的线速度和两个角速所代表的合成运动将相应于一个不变的线速度和两个角速度。度。 平移运动可由齐次变换矩阵平移运动可由齐次变换矩阵L()表示；第一个转动可用齐次表示；第一个转动可用齐次变换矩阵变换矩阵RA()表示，它的作用是把表示，它的作用是把Pi位置的接近矢量转向位置的接近矢量转向Pi+1位置的接近矢量；第二个转动可用位置的接近矢量；第二个转动可用RB()表示，它的作用是把表示，它的作用是把Pi 位置的方向矢量绕工具轴转向位置的方向矢量绕工具轴转向Pi+1位置的方向矢量。因此，驱位置的方向矢量。因此，驱动函数可表示为动函数可表示为 54其中，其中， 旋转矩阵旋转矩阵RA() 表示绕表示绕 Pi 的接近矢量（的接近矢量（zi 轴轴）转动了）转