七章机器人的轨迹规划

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

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

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

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

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

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

6、一一种种状状态态或或布布局局变变换换为为另另一一种种状状态态或或布布局。例如，局。例如， “积木世界积木世界” 。3 机器人能得到的一个解答是由下面的算符序列组成的：机器人能得到的一个解答是由下面的算符序列组成的：目标状态目标状态 机器人规划是机器人学的一个重要研究领域，也是人工智能机器人规划是机器人学的一个重要研究领域，也是人工智能与机器人学一个令人感兴趣的结合点。与机器人学一个令人感兴趣的结合点。4 机器人的机器人的规规划程序只需要划程序只需要给给定任定任务务初初始状始状态态和最和最终终状状态态的描的描述。述。这这些些规规划系划系统统一般都不一般都不说说明明为为实实现现一一个算符所需的个算

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

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

9、的作用5 机机器器人人轨轨迹迹规规划划属属于于机机器器人人低低层层规规划划，基基本本上上不不涉涉及及人人工工智智能能问问题题，而而是是在在机机械械手手运运动动学学和和动动力力学学的的基基础础上上，讨讨论论机机器器人人运运动动的的规规划划及及其其方方法法。所所谓谓轨轨迹迹，就就是是指指机机器器人人在在运运动动过程中的位移过程中的位移、速度和加速度。速度和加速度。 轨迹规划问题通常是将轨迹规划器看成轨迹规划问题通常是将轨迹规划器看成“黑箱黑箱”，接受表示，接受表示路径约束的输入变量，输出为起点和终点之间按时间排列的操路径约束的输入变量，输出为起点和终点之间按时间排列的操作机中间形态（位姿作机中间形

10、态（位姿, 速度和加速度）序列。速度和加速度）序列。路径约束路径约束动力学约束动力学约束路径设定路径设定轨迹规划器轨迹规划器7.2 7.2 机器人轨迹规划的一般性问题机器人轨迹规划的一般性问题由初始点运动到终止由初始点运动到终止点，所经过的由中间点，所经过的由中间形态序列构成的空间形态序列构成的空间曲线称为曲线称为路径。路径。这些这些形态序列即是曲线上形态序列即是曲线上的的“点点”。6规划操作机的轨迹有两种常用的方法：规划操作机的轨迹有两种常用的方法： 第一种方法：要求使用者在沿轨迹选定的位置点上第一种方法：要求使用者在沿轨迹选定的位置点上(称为结称为结节或插值点节或插值点)显式地给定广义坐标

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

12、在关节坐标或笛卡几坐标中确定一条与给定路径近似的轨迹。在关节坐标或笛卡几坐标中确定一条与给定路径近似的轨迹。在这种方法中，路径约束是在笛卡尔坐标中给定的。在这种方法中，路径约束是在笛卡尔坐标中给定的。7 在第一种方法中，约束的给定和操作机轨迹规划在关节坐标在第一种方法中，约束的给定和操作机轨迹规划在关节坐标系中进行。由于对操作机手部没有约束，使用者难于跟踪操作系中进行。由于对操作机手部没有约束，使用者难于跟踪操作机手部运行的路径。因此，操作机手部可能在没有事先警告的机手部运行的路径。因此，操作机手部可能在没有事先警告的情况下与障碍物相碰。情况下与障碍物相碰。 在第二种方法中，路径约束在笛卡尔坐

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

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

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

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

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

18、操作机的动力学约束。但当取样点落在拟合的光滑多项式曲线上时，面向关节空间的方法沿笛卡尔路拟合的光滑多项式曲线上时，面向关节空间的方法沿笛卡尔路径的准确性会有损失。径的准确性会有损失。10一般说来，生成关节轨迹设定点的基本算法是很简单的：一般说来，生成关节轨迹设定点的基本算法是很简单的：循环：等待下一个控制间隔；循环：等待下一个控制间隔； 时刻操作机关节所处的位置时刻操作机关节所处的位置； 若若 ，则退出循环；则退出循环； 转向循环。转向循环。 从上述算法可以看出，要计算的是在每个控制间隔中必须更从上述算法可以看出，要计算的是在每个控制间隔中必须更新的轨迹函数新的轨迹函数(或轨迹规划器或轨迹规划

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

20、则要满个多项式，则要满足初始和终止条件足初始和终止条件(关节位置、速度和加速度关节位置、速度和加速度)，并保证这些变，并保证这些变量在多项式衔接处的连续性，因而需要确定量在多项式衔接处的连续性，因而需要确定 3( p十十1)个系数。个系数。 若给定附加的中间条件若给定附加的中间条件(例如位置例如位置)，则对每个中间条件需要，则对每个中间条件需要增加一系数。通常，可以给定两个中间位置，一个靠近初始位增加一系数。通常，可以给定两个中间位置，一个靠近初始位置；另一个靠近终止位置。这样做，除了可以较好地控制运动置；另一个靠近终止位置。这样做，除了可以较好地控制运动外，还能保证操作机末端以适当的方向离开

21、起点和接近终点。外，还能保证操作机末端以适当的方向离开起点和接近终点。 因此，对于连接初始位置和终止位置的每个关节变量，一个因此，对于连接初始位置和终止位置的每个关节变量，一个七次多项式就足够了，或者用两段四次轨迹加一段三次轨迹七次多项式就足够了，或者用两段四次轨迹加一段三次轨迹 （434），也可以用两段三次轨迹加一段五次轨迹（），也可以用两段三次轨迹加一段五次轨迹（353），或五段三次轨迹（），或五段三次轨迹（33333）。）。p +1个点个点由约束条件数所对应的多项式系数的个数确定多项式的次数由约束条件数所对应的多项式系数的个数确定多项式的次数12 为了控制操作机，在规划运动轨迹之前，需要

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

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

24、点，提升点，下放点和终止点。点，下放点和终止点。135、位置约束、位置约束 (a) 初始点：给定速度和加速度初始点：给定速度和加速度(一般为零一般为零)； (b) 提升点：中间点运动的连续；提升点：中间点运动的连续； (c) 下放点：同提升点；下放点：同提升点； (d) 终止点：给定速度和加速度终止点：给定速度和加速度(一般为零一般为零)。146、除除上上述述约约束束外外，所所有有关关节节轨轨迹迹的的极极值值不不能能超超出出每每个个关关节节变量的物理和几何极限。变量的物理和几何极限。7、时间的考虑、时间的考虑 (a) 轨轨迹迹的的初初始始段段和和终终止止段段：时时间间由由手手接接近近和和离离开

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

26、止点点，提升点，下放点和终止点)上满足对位置、速度和加速度上满足对位置、速度和加速度的要求，并使关节位置、速度和加速度在整个时间间隔的要求，并使关节位置、速度和加速度在整个时间间隔 t0, tf 中保持连续。中保持连续。15 1. 位置位置(给定给定) 2速度速度(给定，通常为零给定，通常为零) 3. 加速度加速度(给定，通常为零给定，通常为零) 4. 提升点位置提升点位置(给定给定) 5提升点位置（与前一段轨迹连续）提升点位置（与前一段轨迹连续） 6. 速度（与前一段轨迹连续）速度（与前一段轨迹连续） 7加速度（与前一段轨迹连续）加速度（与前一段轨迹连续） 8. 下放点位置下放点位置(给定给

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

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

29、四次多项式表示。第二段(或中间段）由提升点到下放或中间段）由提升点到下放点的轨迹用三次多项式表示。最后一段由下放点到终止点的点的轨迹用三次多项式表示。最后一段由下放点到终止点的轨迹由四次多项式表示。轨迹由四次多项式表示。17 353 轨迹轨迹 与与434轨迹相同，但每段所用多项式次数与前种不同。轨迹相同，但每段所用多项式次数与前种不同。第一段用三次多项式，第二段用五次多项式，最后一段用三第一段用三次多项式，第二段用五次多项式，最后一段用三次多项式。次多项式。 (33333) 轨迹轨迹 对五段轨迹都使用三次多项式样条函数。对五段轨迹都使用三次多项式样条函数。 注意，上述讨论对每个关节轨迹都是有效

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

31、列轨迹是由多项式序列hi(t)构成的，这些多项式合起来形成关构成的，这些多项式合起来形成关节节 j 的轨迹。在每段轨迹中的轨迹。在每段轨迹中关节变量关节变量的多项式用归一化时间表的多项式用归一化时间表示为示为:19这些关节轨迹分段多项式所应满足的边界条件为这些关节轨迹分段多项式所应满足的边界条件为:20434关节轨迹的边界条件如图示关节轨迹的边界条件如图示。21这些多项式对实际时间这些多项式对实际时间 t 的一阶和二阶导数。可写成的一阶和二阶导数。可写成第一段轨迹的基本多项式是四次的第一段轨迹的基本多项式是四次的22对于对于t =0(这段轨迹的初始位置这段轨迹的初始位置)，要满足此位置的边界条

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

33、终点的速度和由于此点的速度相加速度必须分别和前一段轨迹终点的速度和加速度连续，故可得加速度连续，故可得25或或或或 对于对于t=1(下放点下放点)，该点的速度相加速度必须与下一段轨迹，该点的速度相加速度必须与下一段轨迹起点处的速度和加速度连续。可分别求得此位置的速度和加速起点处的速度和加速度连续。可分别求得此位置的速度和加速度为度为26末段轨迹的基本多项式是四次的末段轨迹的基本多项式是四次的如果在上式中以如果在上式中以 代替代替 t ，我们就把归一化时间我们就把归一化时间 t 由由 移至移至 。27可得可得对于对于 (这段轨迹的终点这段轨迹的终点)，要满足轨迹终点的边界条件，即，要满足轨迹终点

34、的边界条件，即对于对于 (这段轨迹的起点这段轨迹的起点)，要满足轨迹起点的边界条件，即，要满足轨迹起点的边界条件，即28在此下放点的速度和加速度连续性条件是：在此下放点的速度和加速度连续性条件是：或或29可以求出相连轨迹段间关节角之差为可以求出相连轨迹段间关节角之差为 轨迹多项式其余七个未知系数可由以上的速度、加速度连续轨迹多项式其余七个未知系数可由以上的速度、加速度连续约束条件的联立方程解出，用矩阵矢量符号改写这些方程，可约束条件的联立方程解出，用矩阵矢量符号改写这些方程，可得得其中其中30或或C 矩阵的结构便于计算未知系数，若时间间隔矩阵的结构便于计算未知系数，若时间间隔 ti 为为正值，

35、正值， C 的逆矩阵总是存在的。则关节各段关节轨迹多项式的逆矩阵总是存在的。则关节各段关节轨迹多项式系数即可全部求得。系数即可全部求得。31同样，我们可用此方法可计算同样，我们可用此方法可计算353 关节轨迹。关节轨迹。 对于末段轨迹，把归一化时间对于末段轨迹，把归一化时间 t 由由-1，0重新变回重新变回0, 1，可求出可求出 hn(t)324-3-4 关节轨迹多项式关节轨迹多项式33343-5-3 关节轨迹多项式关节轨迹多项式357.3.2 三三次样条轨迹（五段三次多项式）次样条轨迹（五段三次多项式） 用一组三次多项式插补给定的函数并保证插补点处一阶和二用一组三次多项式插补给定的函数并保证

36、插补点处一阶和二阶导数连续的方法，称为三次样条函数法。所能达到的近似程阶导数连续的方法，称为三次样条函数法。所能达到的近似程度和光滑程度是相当好的。度和光滑程度是相当好的。 一般来说，样条曲线是在插值点具有一般来说，样条曲线是在插值点具有 k-1 阶导数连续性的阶导数连续性的 k次多项式。对于三次样条函数，一阶导数代表速度的连续性，次多项式。对于三次样条函数，一阶导数代表速度的连续性，二阶导数代表加速度的连续性。二阶导数代表加速度的连续性。 三次样条函数有某些优点。首先，它是使速度和加速度连续三次样条函数有某些优点。首先，它是使速度和加速度连续的最低次多项式函数。其次，低次多项式减少了计算量和

37、数值的最低次多项式函数。其次，低次多项式减少了计算量和数值不稳定的可能性。不稳定的可能性。每段关节轨迹的五段三次多项式的通式为每段关节轨迹的五段三次多项式的通式为其中其中36 在在应用五段三次多项式插值时，需要有五段轨迹和六个插值应用五段三次多项式插值时，需要有五段轨迹和六个插值点。点。但是，在前面的讨论中只有四个插值点，即初始点、提升但是，在前面的讨论中只有四个插值点，即初始点、提升点、下放点和终止点。所以，必须选择另外两个插值点，以便点、下放点和终止点。所以，必须选择另外两个插值点，以便有足够多的边界条件求解各多项式系数。可以在提升点和下放有足够多的边界条件求解各多项式系数。可以在提升点和

38、下放点之间选取这两个额外的结点。点之间选取这两个额外的结点。 没有必要知道这两个点的确切位置，只要知道时间间隔，以没有必要知道这两个点的确切位置，只要知道时间间隔，以及必须满足这两点速度和加速度的连续性条件。因此，这组关及必须满足这两点速度和加速度的连续性条件。因此，这组关节轨迹分段多项式必须满足的边界条件是：节轨迹分段多项式必须满足的边界条件是： (1) 在初始点、提升点、下放点和终止点的位置约束；在初始点、提升点、下放点和终止点的位置约束； (2) 在所有插值点的速度和加速度的连续性。在所有插值点的速度和加速度的连续性。37 五段三次关节轨迹的边界条件示于中。其中有下划线的变量五段三次关节

39、轨迹的边界条件示于中。其中有下划线的变量是在计算五段三次多项式之前的已知量。是在计算五段三次多项式之前的已知量。38这些多项式对实际时间的一阶和二阶导数为：这些多项式对实际时间的一阶和二阶导数为：式中，式中，tj 是通过第是通过第 j 段轨迹所需的实际时间。给定了初始点和段轨迹所需的实际时间。给定了初始点和终止点的位置、速度和加速度，第一段轨迹和末段轨迹的多项终止点的位置、速度和加速度，第一段轨迹和末段轨迹的多项式式 h1 (t) 和和hn (t) 就完全确定了。就完全确定了。旦算出这两个多项式，就旦算出这两个多项式，就可用位置约束和连续条件求出可用位置约束和连续条件求出h2 (t)、 h3

40、(t) 和和 h4 (t) 。第一段轨迹，基本多项式为第一段轨迹，基本多项式为当当 t=0 时，由该位置的边界条件，可得时，由该位置的边界条件，可得(给定给定)39由此可得：由此可得：当当 t=1 时，由此位置的边界条件，可得：时，由此位置的边界条件，可得：可得可得其中，其中，i= i i-1 。第一段轨迹的多项式就完全确定了。第一段轨迹的多项式就完全确定了。(给定给定)40由上式可求出由上式可求出 t1时的速度和加速度：时的速度和加速度：此速度相加速度必须与下一段轨迹起点的速度和加速度连续。此速度相加速度必须与下一段轨迹起点的速度和加速度连续。末段轨迹的多项式为（仍做类似代换末段轨迹的多项式

41、为（仍做类似代换 ）(给定给定)(给定给定)当当t0 和和t1时（对应时（对应 和和 ），由边界条件，有），由边界条件，有41由上述方程解出未知系数，最后得到：由上述方程解出未知系数，最后得到： 给定初始点、提升点、下放点和终止点的位置以及通过每段给定初始点、提升点、下放点和终止点的位置以及通过每段轨迹所需要的时间轨迹所需要的时间 (tj ) 后，五段三次多项式即可后，五段三次多项式即可唯一地唯一地确定，确定，满足所有位置约束和连续条件。满足所有位置约束和连续条件。算出以上个多项式，就可用位置约束和连续条件求出算出以上个多项式，就可用位置约束和连续条件求出h2 (t)、 h3 (t) 和和 h

42、4 (t) 。42 对于较复杂的机器人系统，为了控制操作机完成作业而设对于较复杂的机器人系统，为了控制操作机完成作业而设计了编程语言。在这种系统中，作业通常是用操作机手部计了编程语言。在这种系统中，作业通常是用操作机手部(或或末端执行器末端执行器)必须通过的笛卡尔结点序列给定的。因此，描述必须通过的笛卡尔结点序列给定的。因此，描述操作机在作业中的运动时，我们就更关心描述操作机手部要达操作机在作业中的运动时，我们就更关心描述操作机手部要达到的目标位置及通过的笛卡尔空间曲线到的目标位置及通过的笛卡尔空间曲线(或路径或路径)的形式。的形式。7.4 7.4 笛卡尔路径轨迹规划笛卡尔路径轨迹规划 一一般

43、般，实实现现笛笛卡卡尔尔路路径径规规划划可可采采用用下下述述两两个个相相连连的的步步骤骤：(1)沿沿笛笛卡卡尔尔路路径径，按按照照某某种种规规则则以以笛笛卡卡尔尔坐坐标标生生成成或或选选择择一一组组结结点点或或插插值值点点；(2)规规定定一一种种函函数数，按按某某些些准准则则连连接接这这些结点些结点(或逼近分段的路径或逼近分段的路径)。 对对于于第第二二步步，所所选选用用的的准准则则可可以以采采用用两两种种控控制制算算法法，以以保保证跟踪给定的路径。证跟踪给定的路径。43 (1) 面面向向笛笛卡卡尔尔空空间间的的方方法法。在在此此方方法法中中，大大部部分分计计算算和和优优化化是是以以笛笛卡卡尔

44、尔坐坐标标完完成成的的，然然后后，在在手手部部这这一一级级上上进进行行控控制制。按按固固定定的的取取样样间间隔隔在在预预定定路路径径上上选选择择伺伺服服取取样样点点，在在控控制制操操作作机机时时实实时时地地把把它它们们转转换换为为与与之之相相应应的的关关节节变变量量。所所得得到的轨迹是分段直线。到的轨迹是分段直线。 (2) 面面向向关关节节空空间间的的方方法法。这这种种方方法法用用关关节节变变量量空空间间中中的的低低次次多多项项式式函函数数迫迫近近直直线线路路径径上上的的两两相相邻邻结结点点间间的的一一段段路路径径，而而控控制制是是在在关关节节这这一一级级上上进进行行的的。所所得得到到的的笛笛

45、卡卡尔尔路路径径是是不不分段的直线。分段的直线。 Paul 叙述了用一系列直线段构成操作机手部笛卡尔路径叙述了用一系列直线段构成操作机手部笛卡尔路径的设计方法。把手部在这些直线段中的速度和加速度转换到的设计方法。把手部在这些直线段中的速度和加速度转换到关节坐标，并用二次插值进行平滑连接，从而实现了手部的关节坐标，并用二次插值进行平滑连接，从而实现了手部的规划运动控制。我们将学习如何用该方法设计笛卡尔直线路规划运动控制。我们将学习如何用该方法设计笛卡尔直线路径。径。44对于笛卡尔路径控制，轨迹设定点算法为：对于笛卡尔路径控制，轨迹设定点算法为：循环：等待下一个控制间隔；循环：等待下一个控制间隔；

46、 时刻操作机末端所处的位置时刻操作机末端所处的位置； 相应于相应于 的关节变量解的关节变量解； 若若 ，则退出循环；则退出循环； 转向循环。转向循环。 这里，除了需要计算每一控制间隔中操作机手部轨迹函数这里，除了需要计算每一控制间隔中操作机手部轨迹函数 外，还需把笛卡尔位置变换为相应的关节变量外，还需把笛卡尔位置变换为相应的关节变量 。矩阵函数矩阵函数 表示操作机手部在表示操作机手部在 时刻的预定位置。可用时刻的预定位置。可用44齐次变换矩阵表示。齐次变换矩阵表示。45 在可编程机器人系统中，预定的运动可用笛卡尔结点序列给在可编程机器人系统中，预定的运动可用笛卡尔结点序列给定，相应于这些笛卡尔

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

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

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

50、工具抓握物体特征部位的位姿。们要让操作机工具抓握物体特征部位的位姿。0Cbase(t)Bobjtool47 我们可以解出我们可以解出0T6 ，它描述了以正确和预定的方式抓取物体，它描述了以正确和预定的方式抓取物体时操作机应有的形态。时操作机应有的形态。 如果计算如果计算0T6 的速度足够高，并转换成相应的关节角，那么的速度足够高，并转换成相应的关节角，那么就可控制操作机沿预定的轨迹运动就可控制操作机沿预定的轨迹运动 利用前式，确定一项作业的利用前式，确定一项作业的 N个个 目标位置的序列可表示为目标位置的序列可表示为48由由Ci(t) Pi确定的位置，可求得相邻点之间的距离。若进一步确定的位置

51、，可求得相邻点之间的距离。若进一步给定线速度和角速度，即可求得由位置给定线速度和角速度，即可求得由位置i 到到 i+1所需的时间。所需的时间。 以上位置的工具和运动坐标系都是相对于机座坐标系给定以上位置的工具和运动坐标系都是相对于机座坐标系给定的。从一个位置移向下一个位置最好是相对于运动坐标系给的。从一个位置移向下一个位置最好是相对于运动坐标系给定位置和工具。这样，从运动定位置和工具。这样，从运动坐标系观察坐标系观察，工具是静止的。，工具是静止的。为了做到这一点，需要相对于后续的坐标系重新定义当前的为了做到这一点，需要相对于后续的坐标系重新定义当前的位置位置和工具。利用双下标符号和工具。利用双

52、下标符号 Pij 重新定义重新定义Pi变换，就不难做变换，就不难做到这一点。到这一点。 Pij是在第是在第 j 个运动坐标系个运动坐标系中表示的位置中表示的位置Pi。则在位置则在位置1，将它在将它在目标坐标系目标坐标系中表示，可中表示，可得得将它用位置将它用位置2时的坐标系表示，则有时的坐标系表示，则有49由以上两个方程即可求得由以上两个方程即可求得 上述方程的目的是要在已知上述方程的目的是要在已知 P11时时 求得求得P12 。因此，在任。因此，在任意二相邻位置意二相邻位置 i 和和 i+1 之间的运动有之间的运动有其中其中Pi,i+1 和和Pi+1,i+1 为上面所讨论过的变换。为上面所讨

53、论过的变换。 Paul 使用了一种简单的方法，以控制操作机手部从一个变使用了一种简单的方法，以控制操作机手部从一个变换运动到另一个变换。此法包括一个平移运动和一个绕空间换运动到另一个变换。此法包括一个平移运动和一个绕空间某定轴的转动以及另一个绕工具轴的转动，以便形成操作机某定轴的转动以及另一个绕工具轴的转动，以便形成操作机受控的线速度和角速度运动。第一个转动使工具轴与预定的受控的线速度和角速度运动。第一个转动使工具轴与预定的接近方向对准，第二个转动使工具轴与方向矢量对准。接近方向对准，第二个转动使工具轴与方向矢量对准。50 由位置由位置 i 到到 i+1的运动可用的运动可用“驱动驱动”变换变换

54、D()表示，它是归表示，它是归一化时间一化时间 的函数的函数其中，其中， 可得可得于是在位置于是在位置 i 时，时， =0，D(0)为单位矩阵为单位矩阵51用位置用位置 i 和和 i+1各自的齐次变换矩阵表示它们，可得各自的齐次变换矩阵表示它们，可得利用前式，求利用前式，求 Pi,i+1的逆并乘以的逆并乘以Pi+1,i+1 ，得到，得到52 若驱动函数由一个平移运动和两个旋转运动构成，那么这些若驱动函数由一个平移运动和两个旋转运动构成，那么这些平移和转动量将直接同平移和转动量将直接同成正比。若成正比。若随时间线性地变化，则随时间线性地变化，则 D() 所代表的合成运动将相应于一个不变的线速度和

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

56、（的接近矢量（zi 轴轴）转动了）转动了角，角，又绕方向矢量（又绕方向矢量（yi 轴轴）转动了转动了角；角； RB() 表示绕工具的接近表示绕工具的接近矢量转动矢量转动 角的旋转。角的旋转。54其中，其中，55 操作机经常要沿相连的直线段轨迹运动，以满足作业运动的操作机经常要沿相连的直线段轨迹运动，以满足作业运动的要求或避开障碍物。为了避免在每段端点处速度不连续，当由要求或避开障碍物。为了避免在每段端点处速度不连续，当由一段轨迹过渡到下一段轨迹时，需要加速或减速运动。一段轨迹过渡到下一段轨迹时，需要加速或减速运动。 在操作机到达端点前的在操作机到达端点前的个单位时间，可以开始改变速度，个单位时

57、间，可以开始改变速度，然后保持加速度不变，直到下段轨迹开始后的然后保持加速度不变，直到下段轨迹开始后的个单位时间为个单位时间为止。若在时间止。若在时间 - 和和 之间，每一变量的加速度保持为常数。之间，每一变量的加速度保持为常数。7.4.2 两段路径间的过渡两段路径间的过渡路径路径56其中其中C 和和B为矢量，其元素分别是点为矢量，其元素分别是点 D 到到 C 和点和点 B 到到 A的的笛卡尔距离和角度。笛卡尔距离和角度。路径路径式中式中则为改变位置和速度所必需的加速度值为则为改变位置和速度所必需的加速度值为57时的速度和位置为时的速度和位置为其中，其中，时，运动用下面的方程描述：时，运动用下

58、面的方程描述：其中，其中，58注意，与前面一样，注意，与前面一样，代表归一化时间，变化范围是代表归一化时间，变化范围是0，1，不，不过，对于不同的时间间隔，归一化因子通常是不同的。过，对于不同的时间间隔，归一化因子通常是不同的。对于由对于由 A 到到 B 再到再到 C 的运动，我们把的运动，我们把 定义为在定义为在 区间的运动中的线性插值，并表示为区间的运动中的线性插值，并表示为式中式中 和和 分别是由分别是由A 到到B 和由和由B 到到C 的运动确定的，的运动确定的，由此可确定由此可确定 。 总之，为了从位置总之，为了从位置 Pi 运动到运动到Pi +1，可计算出驱动函数，可计算出驱动函数D

59、()，然后计算然后计算 T6() ，再用运动学逆问题程序算出相应的关节变量，再用运动学逆问题程序算出相应的关节变量值。如有必要，在用运动学逆问题解出的点之间，可用二次多值。如有必要，在用运动学逆问题解出的点之间，可用二次多项式函数进行插值。项式函数进行插值。59例例 要求一个机器人按直线运动，把螺栓放入托架的一个孔要求一个机器人按直线运动，把螺栓放入托架的一个孔中，写出所有必要的矩阵方程，使机器人能沿虚线运动并完成中，写出所有必要的矩阵方程，使机器人能沿虚线运动并完成作业。可用符号表示沿直线运动的中间位置。作业。可用符号表示沿直线运动的中间位置。解解 令令Pi 为操作机手部必须经过的笛卡尔结点

60、为操作机手部必须经过的笛卡尔结点i =0, 1, 2, 3, 4, 5)。则基本矩阵方程为则基本矩阵方程为60其中，其中，WORLD是绝对坐标系（也称世界坐标系）是绝对坐标系（也称世界坐标系）; BASE、INIT、B0、BR 分别是基座坐标系、手部位姿、工件库坐分别是基座坐标系、手部位姿、工件库坐标系、托架坐标系等相对于绝对坐标系的变换矩阵；标系、托架坐标系等相对于绝对坐标系的变换矩阵； T6 是手是手部位姿相对基座坐标系的变换矩阵部位姿相对基座坐标系的变换矩阵; E 是工具坐标系相对手部是工具坐标系相对手部坐标系的变换矩阵坐标系的变换矩阵; P0 是在是在 INIT 中描述的轨迹点；中描述

61、的轨迹点；P1 P3 是在是在B0中描述的轨迹点；中描述的轨迹点；P4、P5 是在是在 BR 中描述的中描述的轨迹点。以上的变换矩阵均为轨迹点。以上的变换矩阵均为44 坐标矩阵。坐标矩阵。61由由P0 位置运动到位置运动到P1，我们在，我们在P0 坐标系中用双下标描述坐标系中用双下标描述P0有有在在 P1 坐标系中坐标系中描述描述P0 ，可得，可得由以上两式相等，得到由以上两式相等，得到因此，由因此，由P0位置沿直线运动到位置沿直线运动到P1时，操作机手部位姿必须由时，操作机手部位姿必须由变化到变化到 由由Pi 位置到位置到Pi+1 位置位置 ( i=1, 2, 3, 4, 5) 时，操作机运动的手部时，操作机运动的手部位姿可用同样方法求解。位姿可用同样方法求解。62