第5章机器人规划与人机交互
本章主要介绍任务规划、运动规划、路径规划和人机交互等内容。
机器人得到的任务有时是复杂的，无法一步解决，而任务规划的目的就是将某些比较复杂的问题分解为一些比较小的问题，并进一步分解为机器人可执行的动作。本章介绍了任务规划的作用、问题分解途径、规划域的预测、规划的修正以及机器人规划系统的任务与方法。
在实际应用中，机器人工作空间大多存在障碍物，因此必须考虑机器人执行任务和操作作业时的避碰问题，这称为运动规划。对于运动规划问题，机器人机械手和移动机器人都可采用位形空间的概念，采用有效方式进行公式化表示； 其求解技术本质上是算数技术，包括确定性方法、随机性方法和启发式方法。本章第5.2节介绍了典型的机器人运动规划方法。
机器人轨迹规划属于机器人底层规划，将介绍在关节空间和工作空间中机器人运动轨迹和轨迹生成方法。所谓轨迹，是指机械手在运动过程中的位移、速度和加速度等项目。例如，在机器人搬运材料的任务中，操作人员只需要指定抓取和放下目标的位置(点对点运动)就可以了，而在加工任务中，末端执行器必须遵循一条期望轨迹(路径运动)。轨迹规划的目的是从对期望运动的简明描述出发，生成相关变量(关节或末端执行器)的时间律，作为运动控制系统的参考输入，以确保机械手完成规划的轨迹。
在机器人规划过程中，任务大多是由人进行下达的。人对机器人下达任务则涉及人与机器人交互的问题。人与机器人的交互(HRI)是和人与计算机的交互(HCI)的发展分不开的。本章5.2节将对现阶段人与机器人的交互方式进行介绍，包括基于人机界面交互的示教盒示教系统以及快速发展的语音交互、视觉交互、脑机接口等新型交互方式。
5.1机器人任务规划
任务规划是一种重要的问题求解技术，它从某个特定的问题状态出发，寻求一系列的机器人行为动作，并建立一个操作序列，直到求得目标状态为止。机器人规划(Robot Planning)是机器人学的一个重要研究领域； 智能化程度越高，规划的层数越多，用户操作越简单。一般的工业机器人，以轨迹规划为主，高层的规划由人工示教或者智能控制算法完成。服务机器人轨迹规划和高层规划都需要自己完成。
5.1.1任务规划的作用与问题分解
1. 任务规划的概念及作用
在日常生活中，机器人任务规划决定了需要在行动之前决定行动的进程，规划指的是机器人在执行一个任务指令或问题求解程序中的任何一步之前，设计该程序相关步骤的过程。一般一个任务规划是一个行动过程的描述，规划意味着在行动之前决定行动的过程，可用来监控问题的求解过程，并能够在造成较大的危害之前发现错误。
2. 问题分解途径及方法
把某些复杂的问题分解为一些较小的子问题的思想，使应用规划方法求解问题在实际上成为可能。有以下两种实现分解的途径：
(1) 当从一个问题状态移动到下一个状态时，无须计算整个新的状态，而只要考虑状态中可能变化了的部分。例如，一个家庭服务机器人从卧室走动到客厅或厨房，这并不改变两个房屋内门窗的位置。当环境或路径等状态的复杂程度提高时，研究如何决定哪些事物是变化的以及哪些是不变的问题，就显得越来越重要。
(2) 把单一的困难问题分割为若干个较为容易解决的子问题，这种分解能够使困难问题的求解变得容易。但有时候，这种分解是不可行的。替代的方法是，把许多问题看成是待分解的问题，即被分割为只有少量互作用的子问题。
3. 域的预测和规划的修正
任务规划的成功取决于问题的另一个方面，即问题论域的可预测性。如果通过在实际上执行某个操作序列以寻找问题的解答，那么在这个过程的任何一步都能够确信该步的结果。但对于不可预测的论域，如果只是通过模拟的方法求解过程，那么就无法知道求解步骤的结果。
最好能考虑可能结果的集合，这些结果很可能按照它们出现的可能性以某个次序排列，然后产生一个规划，并试图去执行这个规划。但对真实世界的任何方面进行完全的预测几乎是不可能的。因此必须随时准备面对规划的失败，并对其进行修改。
5.1.2机器人任务规划基本要素
机器人任务规划的基本要素包括状态空间(State)、时间(Time)、操作状态的动作序列(Actions)、初始和目标状态(Initial and goal states)、准据(A criterion)和运动计划(A plan)。
1. 机器人状态空间
(1) 设计问题包括所有可能发生的机器人状态空间。例如，机器人的位置和方向、飞行机器人的位置和速度等。
(2) 离散的和连续的机器人状态空间都是被允许的； 应该可以被简洁地用一个计划算法描述。在大多数应用里，状态空间的大小(数目和复杂度)应该尽可能地被简洁描述。
(3) 机器人状态空间是设计问题中最基本的，也是最重要的，应该仔细设计及分析。
2. 时间
(1) 所有的设计问题都包括在时间范围内的一系列决策。
(2) 时间模型一定要设计的简洁，能简单反映事实，以便动作执行。
(3) 特殊的时间设计是不必要的，但简洁的时间序列也要保证正确的动作序列。
(4) 一个简洁的时间设计的例子是Piano Mover’s Problem： 解决方案是移动钢琴使其到另一个模拟状态，但是特殊的速度在方案中不被专属对待。
3. 机器人操作状态的动作序列
(1) 一个机器人计划产生一系列可以改变机器人状态的动作。机器人动作这个术语在这里可以理解为人工智能中通用的operators，在控制理论和机器人理论中的对应术语为inputs和controls。
(2) 在设计规范中，当机器人动作序列执行时，状态如何改变是需要进行细致描述的。这就需要一个状态返回函数来处理离散的时间变化或者可微分的连续时间上的变化。
(3) 对应机器人绝大多数动作设计问题，关于时间的函数设计，需要避免直接在状态空间相邻位置连续变换。
4. 机器人初始和目标状态
(1) 一个计划问题通常包含机器人初始化的状态和目标状态，过程通过一系列设计的机器人中间状态及动作序列组成。
(2) 初始状态是状态空间的一个特殊点，也是动作序列未发生时的全局状态。
(3) 目标状态是设计的一系列动作执行后，决策者期待经过一系列状态变化后的最终状态。
5. 准据
准据是将一个关于机器人状态和动作的计划的输出结果进行编码，形成可执行格式。根据准据的类型，大致有两类方法。
① 可行性： 不考虑其效率，只考虑可行性。
② 最佳性： 可行的且最优效能，在一些特定条件下，可达到目标状态。
6. 机器人运动计划
(1) 总体来说，一个计划利用一个特殊的策略或者行为施加于决策者。
(2) 机器人的运动计划应该使得其动作序列容易被执行。
(3) 预测未来的机器人状态是困难的，因此更多地关注状态转移的实现。
(4) 若不考虑未来的状态，当前机器人状态的最优方案是可以被设计的。
(5) 当前的方法中，利用反馈和活动计划是有效且广泛被采纳的； 但是这样的情况下，有些状态不被精确测量。这将被定义成为信息状态，在此之上计划的动作有条件被执行。
5.1.3机器人规划系统的任务与方法
在规划系统中，必须执行下列各项任务：
(1) 根据最有效的启发信息，选择应用于机器人下一步的最优规则。
(2) 应用所选取的规则，计算机器人应用于该规则而生成的新状态。
(3) 对所求得的解答进行检验。
(4) 检验空端，以便舍弃它们，使系统的求解工作向着更有效的方向进行。
(5) 检验正确的解答，并运用具体技术使之完全正确。
规划系统必须具有执行以上步骤任务的方法，方法包含如下所述：
1. 选择和应用规则
选择合适的机器人应用规则，最广泛采用的技术是： 首先查出机器人的期望目标状态与现有状态之间的差别集合，然后辨识出与减少这些差别有关的规则。如果同时有几种可以使用，则可运用各种启发信息对这些规则加以挑选。
2. 检验解答与空端
当规划系统找到一个能够把机器人的初始问题状态变换为目标状态的操作序列时，此系统就成功地求得问题的一个解答。但如何知道求得了一个解答？对于简单问题的处理比较容易，但是对于一些复杂的问题，例如，如果整个状态不是显示地由一个相关特征集合进行描述，则确定解答要困难得多。
解决上述问题的关键是，问题的表示和描述方法。原则上，问题的任何表示方法及它们的组合都可用来描述系统的状态。
3. 修正殆正确解
一个求解殆可分解问题的办法是： 当执行与所提出的解答相对应的操作序列时，检查求得的状态，并把它们与期望目标比较。
修正一个殆正确的解答的较好办法是，注意有关的出错信息，然后直接加以修正。修正一个殆正确的解答的更好办法是，不对解答进行全面修正，而是不完全确定地让它们保留到最后的可能时刻； 然后，当由尽可能多的信息可供利用时，再用一种不产生矛盾的方法完成对解答的详细说明。
5.1.4机器人任务规划的发展现状
机器人任务规划问题(Task Planning)是机器人学科一个重要的研究领域，也是机器人学与人工智能学的一个闪亮结合点。机器人任务规划问题与生活中的规划有类似之处，通常指对机器人任务执行或其他行动的过程进行设计，其目的为制定机器人未来整套行动的一个方案，以达到某种程度的优化。
机器人任务规划是机器人技术研究的重点方向之一，对于提高机器人的智能化水平具有重要意义。机器人领域内的任务规划有两大主要的挑战，第一，机器人工作在非结构化的环境中，任务复杂而多变，难以灵活且有效地对所处的动态环境进行表示和处理。第二，用于描述机器人动作执行序列的构建是任务自主规划的另一个难题。为解决机器人任务规划这两个主要的问题，国内外学者对此展开了广泛而深入的研究。
针对第一个问题，有如下研究。机器人自身具备一定的知识储备，才能更好地提升其任务决策能力。一方面，机器人需要具备语义本体知识。早期，有学者提出基于反应行为的机器人，通过全面预先定义各种反应机制，使没有全局环境知识的机器人做出动作规划； 另一方面，机器人需要具备所处空间的环境知识。环境表征的一个主要应用是在本体中通过人工智能推断，处理未知或隐含信息的情况。
针对第二个问题，有如下研究。机器人执行动作序列的构建是决定机器人任务规划是否完整的关键。分层任务规划算法将动作从高层到底层逐层划分，分别使用STRIPS结构和ABSTRIPS问题解析结构遍历动作序列树生成有效的任务规划。分层任务规划算法因其在构建动作序列方面的高效性和整洁性，成为一直以来高层次任务规划的流行方法。
目前机器人任务规划动作序列的生成大多是通过硬件编码或者预先定义好环境场景，这种方法在灵活性能上具有局限性，因为它需要手动修改源代码，以便对每个任务的动作顺序进行重新编程。总体而言，机器人任务规划目前还基本处于研究阶段，目前机器人规划还没有其他的实用方法，如STRIPS是19世纪80年代的研究成果，但并没有得到实际应用。
5.1.5典型的机器人任务规划系统
典型的机器人任务规划系统有STRIPS规划系统、具有学习能力的规划系统和基于专家系统的机器人规划等。
1. STRIPS规划系统
STRIPS是由Fikes、Hart和Nilsson三人在1981—1982年研究成功的，它是夏凯(Shakey)机器人程序控制系统的一个组成部分。
1) STRIPS系统组成
（1） 世界模型： 为一阶谓词演算公式；
（2） 操作符(F规则)： 包括先决条件、删除表和添加表；
（3） 操作方法： 应用状态空间表示和中间—结局分析。
对于一个复杂问题，首先做出规划，即制订求解计划步骤，然后再执行。在执行过程中如出现问题，需要对没有执行的部分计划进行修改，再继续执行。
2) STRIPS系统的规划过程
每个STRIPS问题的解答，就是为某个实现目标的操作符序列，也即达到目标的规划。
2. 具有学习能力的规划系统
PULPⅠ机器人规划系统： PULPⅠ机器人规划系统是一种具有学习能力的系统，它采用管理式学习，其作用原理是建立在类比的基础上。
STRIPS的弱点： 需要极其大量的控制器内存和时间等。应用具有学习能力的规划系统能够克服这一缺点。PULPⅠ系统的总体结构如图5.1所示。
图5.1PULPⅠ系统的总体结构
“字典”是英语词汇的集合。
“模型”部分包括模型世界和物体现有状态的事实。
“过程”集中了预先准备好的过程知识。
“方块”集中了LISP程序，它配合“规划”对“模型”进行搜索和修正。
PULPⅠ系统的操作方式
PULPⅠ系统具有两种操作方式： 学习方式和规则方式。
在学习方式下，输入至系统的知识是由操作人员或者所谓“教师”提供的，如图5.2所示。
图5.2学习方式下PULPⅠ系统的结构
当某个命令句子送入系统时，PULPⅠ就进入规划方式，如图5.3所示。
图5.3规划方式下PULPⅠ系统的结构
3. 基于专家系统的机器人规划
机器人规划专家系统就是用专家系统的结构和技术建立起来的机器人规划系统。
管理式学习能力的机器人规划系统的不足：
(1) 表达子句的语义网络结构过于复杂。
(2) 与复杂的系统内部数据结构有关的是，PULPⅠ系统具有许多子系统，而且需要花费大量时间编写程序。
(3) 尽管PULPⅠ系统的执行速度要比 STRIPS系统快得多，然而它仍然不够快。
系统结构及规划机理如图5.4所示。
图5.4机器人规划专家系统的结构
基于规划的机器人规划专家系统由5个部分组成： 知识库、控制策略、推理机、知识获取、解释与说明。
基于规划的专家系统的目标就是要通过逐条执行规划及其有关操作，以逐步改变总数据库的状况，直到得到一个可接受的数据库(称为目标数据库)为止。
5.1.6机器人规划系统的任务举例
为说明机器人规划的概念，我们举下面的两个例子：
例5.1在一些老龄化比较严重的国家，开发了各种各样的机器人专门用于伺候老人，这些机器人有不少是采用声控的方式。例如，主人用声音命令机器人“给我倒一杯开水”，我们先不考虑机器人是如何识别人的自然语言，而是着重分析一下机器人在得到这样一个命令后，如何来完成主人交给的任务。
首先，机器人应该把任务进行分解，把主人交代的任务分解成为“取一个杯子”“找到水壶”“打开瓶塞”“把水倒入杯中”“把水送给主人”等一系列子任务（见图5.5）。这一层次的规划称为任务规划，它完成总体任务的分解。
图5.5智能机器人的任务分解
然后再针对每一个子任务进行进一步的规划。以“把水倒入杯中”这一子任务为例，可以进一步分解成为一系列动作，这一层次的规划称为动作规划，它把实现每一个子任务的过程分解为一系列具体的动作（见图5.6）。
图5.6智能机器人的进一步规划
为了实现每一个动作，需要对手部的运动轨迹进行必要的规定，这是手部轨迹规划。
为了使手部实现预定的运动，就要知道各关节的运动规律，这是关节轨迹规划，最后才是关节的运动控制（见图5.7）。
图5.7智能机器人的规划层次
机器人的工作过程，就是通过规划将要求的任务变为期望的运动和力，由控制环节根据期望的运动和力的信号产生相应的控制作用，以使机器人输出实际的运动和力，从而完成期望的任务，如图5.8所示。机器人实际运动的情况通常还要反馈给规划级和控制级，以便对规划和控制的结果做出适当的修正。
图5.8机器人的工作原理
要求的任务由操作人员输入给机器人，为了使机器人操作方便、使用简单，操作人员应该给出尽量简单的描述。
期望的运动和力是进行机器人控制所必需的输入量，它们是机械手末端在每一个时刻的位姿和速度，对于绝大多数情况，还要求给出每一时刻期望的关节位移和速度，有些控制方法还要求给出期望的加速度等。
例5.2考虑STRIPS系统一个比较简单的情况，即要求机器人到邻室去取回一个箱子。机器人的初始状态和目标状态的世界模型如图5.9所示。
图5.9机器人的初始状态和目标状态的世界模型
设有两个操作符，即gothru和pushthru(“走过”和“推过”)。
OP1： gothru(d，r1，r2)
机器人通过房间r1和房间r2之间的d，即机器人从房间r1走过门d而进入房间r2。
先决条件： INROOM(ROBOT，r1)∧CONNECTS(d，r1，r2)； 即机器人在房间r1内，而且门d连接r1和r2两个房间。
删除表： INROOM(ROBOT，S)； 对于任何S值。
添加表： INROOM(ROBOT，r2)。
OP2： pushthru(b，d，r1，r2)
机器人把物体b从房间r1经过门d推到房间r2。
先决条件： INROOM(b，r1)∧INROOM(ROBOT，r1)∧CONNECTS(d，r1，r2)。
删除表： INROOM(ROBOT，S)，INROOM(b，S)； 对于任何S值。
添加表： INROOM(ROBOT，r2)，INROOM(b，r2)。
这个问题的差别如表5.1所示。
表5.1针对差别，须采用标有“×”的操作
差别
操作符
gothrupushthru
机器人和物体不在同一房间内×
物体不在目标房间内×
机器人不在目标房间内×
机器人和物体在一房间内，但不是目标房间×
假定这个问题的初始状态M0和目标G0如下：
M0： INROOM(ROBOT，R1)∧INROOM(BOX1，R2)∧CONNECTS(D1，R1，R2)
G0： INROOM(ROBOT，R1)∧INROOM(BOX1，R1)∧CONNECTS(D1，R1，R2)
采用中间—结局分析方法，逐步求解这个机器人规划：
(1) do GPS的主循环迭代，until M0与G0匹配为止。
(2) begin。
(3) G0不能满足M0，找出M0与G0的差别。尽管这个问题不能马上得到解决，但是如果初始数据库含有语句INROOM(BOX1，R1)，那么这个问题的求解过程就可以得到继续。GPS找到它们的差别d1： INROOM(BOX1，R1)，即要把箱子(物体)放到目标房间R1内。
(4) 选取操作符： 一个与减少差别d1有关的操作符。根据差别表，STRIPS选取操作为：
OP2： pushthru(BOX1，d，r1，R1)
(5) 消去差别d1，为OP2设置先决条件G1为：
INROOM(BOX1，r1)∧INROOM(ROBOT，r1)∧CONNECTS(d，r1，R1)
这个先决条件被设定为子目标。STRIP发现：
若r1=R2，d=D1，当前数据库含有INROOM(ROBOT，R1)
那么此过程能够继续进行。现在新的子目标G1为：
G1： INROOM(BOX1，R2)∧INROOM(ROBOT，R2)∧CONNECTS(D1，R2，R1)
(6) GPS(p)； 重复第3~第5步骤，迭代调用，以求解此问题。
步骤3： G1和M0的差别d2为INROOM(ROBOT，R2)，即要求机器人移到房间R2。
步骤4： 根据差别表，对应于d2的相关操作符为OP1： gothru(d，r1，R2)。
步骤5： OP1的先决条件为G2： INROOM(ROBOT，R1)∧CONNECTS(d，r1，R2)。
步骤6： 应用置换式r1=R1和d=D1，STRIPS系统能够达到G2。
(7) 把操作符gothru(D1，R1，R2)作用于M0
删除表： INROOM(ROBOT，R1)
添加表： INROOM(ROBOT，R2)
求出中间状态M1： INROOM(ROBOT，R2)
INROOM(BOX1，R2)
CONNECTS(D1，R1，R2)
把操作符pushthru应用中间状态M1
删除表： INROOM(ROBOT，R2)，INROOM(BOX1，R2)
添加表： INROOM(ROBOT，R1)，INROOM(BOX1，R1)
得到另一中间状态M2为INROOM(ROBOT，R1)
INROOM(BOX1，R1)
CONNECTS(D1，R1，R2)
M2=G0
(8) end。
求解过程中，用到的STRIPS规则为操作符OP1和OP2，即gothru(D1，R1，R2)和pushthru(BOX1，D1，R2，R1)。
中间—结局分析法逐步求解机器人规划如图5.10所示。
图5.10中间—结局分析法逐步求解机器人规划
5.2机器人运动规划方法
在实际应用中，机器人的工作环境大多是分布着障碍物的。安装在工厂生产线上的机器人系统的工作区域经常会有其他的物体，这些物体的存在对于机器人的运动构成了阻碍。例如，在生产线上一个工位中工作的机械手必须避免与可能靠近的运动物体发生碰撞，包含其他机械手、物流机器人等，同时它也必须避免与其自身结构发生碰撞。在服务机器人领域，例如餐厅中的送餐机器人必须在动态的环境中进行自主导航，避免与固定障碍物(建筑构件、桌椅、各种摆设等)或移动障碍物(工作人员、食客等)发生碰撞。
因此，机器人在完成任务规划后，需要在其工作区域内规划出一条无碰撞的运动路径，以顺利完成其被分配的任务。本节将介绍机器人运动规划的相关内容，包含位形空间、障碍等，并着重介绍多种机器人运动规划方法的原理。
5.2.1机器人运动规划的概念与发展现状
1. 机器人运动规划的概念
运动规划是决定一条从初始位姿到最终位姿的路径，使机器人能沿这条路径无碰撞地完成作业任务。这需要为机器人赋予自主规划能力，需要从用户提供的任务级高层描述和工作空间的几何特征出发进行自主规划。
根据工作空间是否已知，运动规划分为离线规划和在线规划。离线规划是在工作空间为完全预先已知的条件下进行的运动规划，在线规划则要求机器人在运动过程中借助自身搭载的传感器对工作空间进行实时感知，并以此进行运动规划。
在空间中寻找一条无碰撞的路径对人类来说是一件很简单的事情，但对机器人来说是一项十分艰巨的任务。其主要原因在于，很难将人们本能地安全穿行于障碍中所依赖的空间感进行复制并转化为机器人可以执行的算法。时至今日，机器人运动规划仍然是一个非常活跃的研究方向，并得益于如相关的算法和理论、计算几何和自动控制等不同领域的研究成果。
无论机器人运动规划属于哪种类别，采用何种规划算法，基本上都遵循以下步骤：
(1) 建立环境模型，即对机器人根据所在的现实环境进行抽象后建立相关模型；
(2) 搜索无碰路径，是在某个模型的空间中找到符合条件路径的搜索算法。
移动机器人的运动规划问题，可以形式化地描述为以下的推理机：
〈X，xinit，Xgoal，U，f，Xobst〉
其中，X是搜索空间； xinit∈X，表示初始位置 (包括姿态、状态等)； xgoal∈X，表示目标区域，即目标位置的集合； 对于每一个状态x∈X，U(x)是在状态X下所有备选控制的输入集合； 状态对控制输入响应由状态转换方程f确定。一般来说，当时间是离散的，状态转换方程可以用函数f：X×U→X表示； 当时间是连续的，状态转换方程可以用一个偏微分方程dx/dt=f(x，u)表示； XobstX定义，由一些不允许通行的非法状态组成的集合，也就是C空间中的障碍集合。
为定义规划问题的解决方案，考虑一组行为控制序列u1，u2，…，uk，由此序列导出一个状态序列：
x1=xinit，
xi=f(xi－1，ui－1)(i=1，2，…，k)。
如果xk+1∈Xgoal 且{x1，x2，…，xk+1}∩Xobst=，那么称序列u1，u2，…，uk为规划问题的一个解决方案。机器人运动规划的目的就是要找到一组满足一定准则的行为控制序列。
2. 运动规划的发展及现状
运动规划的研究最早集中在移动机器人领域，移动机器人的运动规划一般称为路径规划(Path Planning)。目前对于移动机器人路径规划技术的研究已经取得了大量的成果，一系列的方法被提出，例如可视图法、自由空间法、栅格法、最优控制法、拓扑法、人工势场法、蚁群算法、遗传算法、模糊逻辑算法和神经网络法等。许多问题获得了比较满意的答案。
根据掌握环境信息的完整程度可以分为环境信息完全已知的全局路径规划、环境信息完全未知或部分未知的局部路径规划。全局路径规划的目的是尽量使规划的效果达到最优。对于全局路径规划已经有了许多成熟的方法，包括可视图法、切线图法、Voronoi图法、拓扑法、惩罚函数法、栅格法等。局部路径规划由于对环境信息未知，因此以提高机器人的避障能力为主，代表性算法有人工势场法、模糊逻辑算法、遗传算法、人工神经网络、模拟退火算法、蚁群优化算法、粒子群算法和启发式搜索方法等。
机器人运动规划方法经历了几十年的发展，20世纪70年代，科学界开始了对移动机器人路径规划技术的广泛应用与深入研究，怎样为机器人提供一种高效的工作路径是当时的研究核心。早期的机器人路径规划技术主要面向全局静态已知的环境，路径规划技术的最早研究方法是基于直角坐标空间的假设和检验法，该方法由Pieper在1968年提出，可以起到躲避碰撞的作用； 同年，Nilsson提出了用可视图方法为移动机器人寻找一条无碰撞路径的方法，并描述了具有运动规划能力的机器人。20世纪80年代初，基于C空间的自由空间法被麻省理工学院人工智能实验室的LozanoPerez提出，得到了众多研究学者的认可，之后由其拓展的各种几何法和拓扑法也得到了广泛应用。到20世纪80年代末，Fujimura等提出相对动态的人工势场法，其将时间参量引入到路径规划模型中，在新的模型中动态障碍物是静态表示的，这样动态路径规划就可以用静态的路径规划算法实现。进入21世纪后，机器人运动规划算法取得了快速的发展，在2000年，Ge等人利用优化的人工势场法解决移动机器人的路径规划问题，明确了吸引势场和排斥势场函数及虚拟合力的方向，该方法可用于足球移动机器人； 2009年，Perez等使用了基于速度场的模糊路径算法解决路径规划问题，该方法处理环境信息不完全已知的情况具有很大优越性。近年来，有许多学者采取模糊逻辑、人工神经网络以及遗传算法等创新的技术解决机器人路径规划的难题，并得到显著的效果。
但对于机械臂的运动规划而言，其规划维度更高。规划维度的增加导致了计算量增加，障碍物更是无法在构型空间(Configuration Space)中进行描述。基于随机采样的规划算法目前是机械臂运动规划领域中最为主流的算法之一，其中以RRTs算法最具代表性，RRTs算法由美国爱荷华州立大学的Steven M. LaValle教授在1998年提出，RRTs算法不考虑障碍物在构型空间中的分布情况，它仅仅通过对位形空间中的采样点进行碰撞检测以获取障碍物信息，并在此基础上进行运动规划； RRTs对同一个规划问题的表现可能时好时坏，连续出现完全相同的规划结果的概率很低。要判断算法对于某一规划问题的效果，往往需要多次反复的试验，因此对RRTs的改进以及与其他算法的融合，仍是当前的研究热点方向。很多用于移动机器人路径规划的算法无法应用于机械臂，目前还没有能够兼顾实时性和最优性的算法，因此，运动规划目前是机器人领域的研究热点之一。
机器人运动规划算法主要应用到移动机器人领域，随着移动机器人应用范围的扩大，移动机器人路径规划对规划技术的要求也越来越高，单个运动规划方法有时不能很好地解决某些规划问题，所以新的发展趋势为将多种方法相结合，如机器人组协同工作的路径搜索算法、静态全局路径搜索算法与动态局部搜索算法相结合、传统规划方法与新的智能方法之间的结合等新方向。
5.2.2位形空间
解决运动规划问题一个非常有效的策略是在适当的空间中将机器人表示成移动质点，并在其中标明工作区和障碍。机器人上各个位置的一个完整规范被称为位形(Configuration)，而由所有可能位形组成的集合被称为位形空间，用C表示位形空间。
以下给出几个位形空间的例子。
(1) 对于一个多边形移动机器人，用在固定参考系下本体上一个代表点(如顶点)的位置和多边形朝向描述其位形空间，由此，C=IR2×SO(2)，其维数为3。
(2) 对于一个多面体移动机器人，C=IR3×SO(3)，其维数为6。
(3) 对于一个单连杆回转机械臂，它的位形空间只是连杆的姿态角，因此，其位形空间C=S1，S1表示单位圆，也可以表示为C=SO(2)，实际上S1和SO(2)的选择不是特别重要，因为这两种是等价的表示方法。不管哪种情况下，我们都可以用参数对C进行参数化。对于双连杆平面机械臂，有C=S1×S1=T2，T2表示环面(torus)，这样可以用关节变量向量q=(θ1，θ2)表示一个位形。直角坐标型机械臂有C=R3，用q=(d1，d2，d3)表示其中一个位形。
(4) 对于一个有固定基座的n个旋转关节平面机械手，位形空间C是(IR2×SO(2))n的一个子集，维数等于(IR2×SO(2))n的维数减去因关节带来的约束个数，即3n-2n=n。实际上，一个平面运动链中，每个关节对其后的部分施加了两个非完整约束。
(5) 对于一个有固定基座的n个旋转关节空间机械手，位形空间C是(IR3×SO(3))n的一个子集。维数等于(IR3×SO(3))n的维数减去因关节带来的约束个数，即6n-5n=n。此时每个关节对其后的部分施加了5个约束。
如果位形空间C的维数为n，则其中的一个位形可用向量q∈IRn表示。但这样的表示只是局部有效的，这是因为位形空间C的几何结构通常比欧几里得空间更加复杂。
5.2.3障碍
位形空间C用来描述无碰撞路径规划的代表性方法，障碍是运动物体在位形空间相应的运动禁区。假设操作空间障碍O={Oi，i=1，2，…，p}是封闭的(包含边界)，但一定有界，每一个障碍物Oi在位形空间C中定义为：
COi={q∈C∶B(q)∩Oi≠}(5.1)
即COi表示一个在工作空间中会导致机器人B和障碍Oi发生碰撞(包括接触)的一个位形空间子集。位形空间中障碍物Oi膨胀为COi。
CO为上述集合的并集：
CO=∪pi=1COi(5.2)
空间Cfree为CO的补集：
Cfree=C－CO=q∈C∶B(q)∩∪pi－1Oi=(5.3)
亦即使机器人不会与障碍碰撞的机器人位形空间子集。如果一条位形空间中的路径完全包含在Cfree中，则称为自由路径。
虽然C空间本身是连通的(即给定两形位q1与q2，存在一条路径将它们连接起来)，但因障碍引起的影响，自由位形空间Cfree并不是总是连通的。如果一条线路完全在Cfree中，则这是一条自由(可行)线路。
障碍举例
下面将介绍一些典型情况的“C障碍”产生过程。为简便起见，假设中的障碍具有多边形或多边体外形。
例5.3如图5.11所示，平面中运动的圆形机器人，其位置用中心原点表示，机器人对障碍物的检测不会因为机器人自身的转动而受到影响，因此位形空间C就是此平面，位形空间C则由障碍物以机器人的半径做等距生长新生成的包络线构成。
图5.11平面中运动的圆形机器人及障碍物
例5.4对一个在IRN中既可以旋转和平移的多面体机器人，因为需对其方向自由度进行描述，所以其位形空间的维数相对少一些。考虑一个在IR2中平移和旋转的多边形，其位形可以用一个在参考系中描述机器人朝向的方向角θ和一个代表点(如一个顶点)的笛卡儿坐标表示。相应的位形空间是IR2×SO(2)，可以用IR3表示局部。确定一个障碍Oi在位形空间C中的像，原则上需要对机器人每个可能的朝向角度θ重复如图5.12中的过程。“C障碍”COi是由所有恒值方向时得到的阴影切片“堆垒”(在θ轴方向上)而成的空间体。
图5.12IR2中一个平移运动多边形机器人的“C障碍”
（左图： 机器人B，障碍Oi，以及建立“C障碍”的生长过程； 右图： 位形空间C和“C障碍”COi）
5.2.4图形搜索类规划算法
图形搜索类规划算法的基本思想是按照一些原则将机器人所在的空间用图形的形式加以表示，在给定起始点与目标点后在图形中进行搜索得到连接起始点与目标点的无碰撞路径。根据搜索出的路径是否是随机的，可将图形搜索类算法分为确定性图形搜索和随机图形搜索。图形搜索类规划算法主要有两个步骤：
第一步是图形构建，节点放在何处以及用边将其连接。图形的构建有多种方法。典型的有可视性图法、沃罗诺伊(Voronoi)图法、单元分解法。在可视性图的情况下，道路尽可能地靠近障碍物，且最终最优的路径是极小长度解。在沃罗诺伊(Voronoi)图情况下，路径尽可能地远离障碍物。而单元分解方法的概念是区分自由和被占几何面积的差别，精确的单元分解是无损失的分解，而近似的单元分解代表对原始地图的一个近似。然后，通过单元间的特殊连接关系，形成了图形。第二步是图形搜索，是在以构建的图形中进行(最优)解计算。
而对于随机图形搜索算法，图形的构建与搜索往往是同时进行的，典型的代表有PRM算法与RRT算法。下面将分别介绍沃罗诺伊图法和单元分解法等算法。
1. 可视性图
可视图法通常以障碍物多边形为环境表示，将起始点S、目标点G和多边形障碍物的各顶点(设VO是所有障碍物的顶点构成的集合)进行组合连接，要求起始点和障碍物各顶点之间、目标点和障碍物各顶点之间以及各障碍物顶点与顶点之间的连线，均不能穿越障碍物，即直线是“可视的”。给图中的边赋权值，构造可见图G(V，E)，点集V=VO∪{S，G}，E为所有弧段即可见边的集合。然后釆用某种算法搜索从起始点S到目标点G的最优路径，则规划最优路径的问题转化为从起始点至目标点经过这些可视直线的最短距离问题。
障碍物环境示意图如图5.13所示，O1、O2表示的封闭多边形分别代表两个障碍物，S、G分别表示起始点和目标点。其对应的可视图如图5.14所示，由起始点、目标点与各障碍物顶点之间的可视直线构成。
图5.13障碍物环境示意图
图5.14可视图
由此可见，利用可视图法规划避障路径主要在于构建可视图，而构建可视图的关键在于障碍物各顶点之间可见性的判断。判断时主要分为两种情况，同一障碍物各顶点之间可见性的判断以及不同障碍物之间顶点可见性的判断。
图5.15可见性判断示意图
(1) 同一障碍物中，相邻顶点可见(通常不考虑凹多边形障碍物中不相邻顶点也有可能可见的情况)，不相邻顶点不可见，权值赋为∞。
(2) 不同障碍物之间顶点可见性的判断则转化为判断顶点连线是否会与其他顶点连线相交的几何问题。如图5.15虚线所示，V1、V2分别是障碍物O1、O2的顶点，但V1与V2连线与障碍物其他顶点连线相交，故V1、V2之间不可见； 而实线所示的V3与V4连线不与障碍物其他顶点连线相交，故V3、V4之间可见。
可视图的构建是使用搜索策略的基础，其建立的成功与否影响着最终避障路径的规划结果。但是，可视图中的大部分可视直线在生成避障路径时是用不到的，却又占据着较大的存储空间，对搜索算法的运算效率也会产生极大的考验，且如果障碍物发生变化，可视图还需重新生成。可视性图在移动机器人学的路径规划中比较普遍，部分是因为实现比较简单。特别在连续的或离散的空间中，当环境的表示把环境中的物体描述成多边形时，可视性图可容易地使用障碍物的多边形描述。但可视图法忽略了机器人尺寸，容易造成机器人通过障碍物时与障碍物顶点距离非常近，从而产生摩擦，且该方法搜索时间较长。该方法灵活性较差，一旦机器人的起点和终点发生变化，可视图就需要重新构建，所需的工作量较大。
2. 沃罗诺伊图
与可视性图相比，沃罗诺伊图是一种全道路图的方法，它倾向于使图中机器人与障碍物之间的距离最大化。对自由空间中的各点，计算它到最近障碍物的距离。当你离开障碍物而移动时，高度值增加。在离两个或多个障碍物等距离的点上，这种距离图就有陡的山脊，沃罗诺伊图就是由这些陡的山脊点所形成的边缘组成。当方位空间障碍物都是多边形时，沃罗诺伊图仅由直线和抛物线段组成。在沃罗诺伊道路图上寻找路径的算法，像可视性图方法一样，是完备的，因为自由空间中路径的存在意味着在沃罗诺伊图上也存在一条路径(即两种方法都确保完备性)。但是，在总长度的意义上，沃罗诺伊图常常远非最优。
假设在一片林区内设置n个火情观察塔P1，P2，…，Pn，每个观察塔Pi(i=1，2，…，n)负责其附近V(Pi)的火情发现及灭火任务。其中V(Pi)由距其他Pj(j=1，2，…，n，j≠i)更近的树组成，则V(Pi)就是关联于Pi的Voronoi多边形，由所有的Pi(i=1，2，…，n)组成的图就是关于n个生成元的沃罗诺伊图。
沃罗诺伊图构建的基本原理为： 在一个已确定了尺度的量度平面上，对该平面上分布的离散点集进行区域划分，使划分区域中的点到点集中某一点的距离比其到点集中所有其他点的距离小。具体的数学定义为：
设平面B是一个已确定了尺度的量度平面，设P为平面上的离散点集，p1，p2，…，pn∈p且p1，p2，…，pn对应的坐标为(xi，yi)(i=1，2，…，n)，取B平面上任一点(x，y)称：
(x－xi)2+(y－yi)2<(x－xj)2+(y－yj)2(5.4)
图5.16Voronoi图
使式(5.4)对于选定点pi，任取点pi∈p且pi≠pj成立的(x，y)点形成的轨迹称为离散点pi的沃罗诺伊区域，如图5.16所示。
经过Voronoi区域划分之后，最终把平面B划分成n个相互邻接的多边形，并且每个多边形中有且只有包含一个离散点。如果点(x，y)位于相邻多边形的公共边上，则有任取Pk∈P且Pi≠pj≠pk式(5.5)成立：
(x－xi)2+(y－yi)2=(x－xj)2+(y－yj)2
(x－xj)2+(y－yj)2<(x－xk)2+(y－yk)2(5.5)
其中 Pi，Pj为相邻两多边形包含的离散点。我们称由式(5.5)确定出的点(x，y)形成的轨迹为离散点集 P 的沃罗诺伊边，p1，p2，…，pn称为沃罗诺伊图的生成元。
点集沃罗诺伊图的一些性质：
在叙述沃罗诺伊图性质之前，假设在沃罗诺伊图的离散生成元中，没有四个点是共圆的。
性质1： 沃罗诺伊图的每一个顶点恰好是图的三条边的公共交点；
性质2： 在生成元点集{p1，p2，…，pn}中，Pi的每一个最邻近点，确定沃罗诺伊多边形V(i)的一条边；
性质3： 沃罗诺伊边是由相邻的两个生成元间的垂直平分线，或是该直线上的线段或射线构成。
3. 单元分解法
单元分解法的思想是将机器人所在环境空间切分为多个简单相连的区域，每个区域单元分为自由的和被物体占用的两种。然后找出起点和目标位置所在单元，并在连接图中用搜索算法找到一条连接起点和目标单元的路径。单元分解法又分为精确单元分解(Exact Cell Decomposition)和近似单元分解(Approximate Cell Decomposition)。单元分解法的一个重要评价标准是对环境划分的完备程度，如果环境分解后是无损的，那么这种划分法就是精确单元分解。如果分解形成实际地图的近似，则称为近似单元分解。
1) 精确单元分解
这里，单元的边界建立在几何临界性的基础上。最后得到的单元或各自是完全自由的，或各自被完全占用的。支持这种分解的基本抽象概念是： 在自由空间的各单元内，机器人的特殊位置无关紧要，重要的是机器人从各自由空间单元走向其相邻自由单元的能力。精确单元分解法包括梯形图法(见图5.17)、三角剖分法(见图5.18)等。
图5.17梯形图法
图5.18三角剖分法
精确单元分解法的计算效率取决于环境中物体的密度和复杂性，如果在复杂和高密度的环境中，该算法的计算复杂性会增加，而且基于梯形图和三角剖分等算法，很难找到长度最短的路径。
2) 近似单元分解
近似单元分解是移动机器人路径规划中普遍应用的技术之一，这种方法最流行的形式是栅格法。栅格法是一种最直接的对环境描述的方法。如图5.16所示，在用栅格法描述环境时，把一系列的格子“镶嵌”到要描述的环境中去，通过格子的状态描述环境信息。假设在环境中，用黑色表示障碍物，用白色表示可行路径，则落在黑色区域的格子不可行，落在白色区域的格子可行。然后，可以利用搜索算法找到一条由起点到终点的只穿越白色可行格子的路径。
栅格法的最大优点是其把整个环境作为一个整体进行描述，而不是对每一个障碍物进行描述。这样一来，就极大地减少了在环境中区分不同障碍物并获取其几何特性的麻烦，从而使其具有特别强的通用性。但是，此种描述环境的方法也有一个明显的不足之处，即对环境描述的精确程度完全取决于格子的大小。当格子取得越小越密时，对环境的描述就越精确。当格子取得越大越稀疏时，对环境的描述也就越粗糙。但同时，格子的大小、多少又直接决定了存储量的大小和搜索空间的大小，即格子越多越密，存储量越大，搜索空间就越大； 格子越少越稀疏，存储量越小，搜索空间就越小； 而这些又影响了计算的复杂度和实时性。
为了解决这个矛盾，于是有学者提出了分级递归的思想，也称为“四叉树”法。如图5.19所示，即在用栅格法描述环境的时候，对环境进行分级递归的划分。包围自由空间的矩形格子被分解成四个相同的矩形，当矩形格子完全落在“黑”色区域，或者完全落在“白”色区域时就停止对其做进一步的划分； 否则，当格子中既包含“黑”色，又包含“白”色时，则继续对其进行划分，直至得到某种预定的解。与精确单元分解法相比，近似的方法可能牺牲完备性，但它很少涉及数学几何运算，所以比较容易实现。
图5.19栅格法
4. 随机图形搜索
随机图形搜索代表着一类非常高效的规划方法，尤其对高维位形空间中的规划问题更是如此。它们属于基于抽样(Samplingbased)的方法族，其基本思路是： 确定一个能充分表示Cfree连通性的有限避碰位形集合，并利用该位形集合建立用于解决运动规划问题的路径图。
实现该思路的途径是在每步迭代过程中抽取一个位形样本并检查是否会使机器人与工作空间内的障碍发生碰撞。如果结论是肯定的，就丢弃该样本。而对一个不会导致碰撞的位形，则将其加入当前路径图中并与其他已经记录的位形建立可行的连接即可。
1) PRM方法
PRM算法将路径规划问题求解分为两个阶段来完成： 学习阶段(learning phase)和查询阶段(query phase)。在学习阶段，PRM算法主要是利用局部规划器，通过对整个位姿空间进行某种方式的采样，
图5.20基本PRM扩展示意图
构建一张自由空间内的无向路径图G(V，E)，其中V是顶点的集合，E是Cfree中路径的集合。在查询阶段通过图形搜索算法，在构建的路径图中找到一条从起点到终点的连通路径，该路径包含了一系列通过边连接在一起的采样点。PRM算法的扩展示意图如图5.20所示，其具体执行步骤如下：
（1） 初始化路径图G(V，E)、终止循环数K、最大连接边数i、步长λ等。在终止循环数K范围内循环执行(2)~(4)，对G进行扩展；
（2） 通过某种采样机制得到一个随机采样点α(i)；
（3） 对α(i)进行碰撞检测，若发生碰撞，则进入下一循环，未碰撞则继续当前循环；
（4） 将α(i)加入V中，找到V中与α(i)间距离小于设定步长λ的点集，对于集合中每个点q与α(i)之间进行碰撞，若发生碰撞，则检查下一个点，未发生碰撞则新建边α(i)并加入E中；
（5） 在路径图中导入起始点和终止点；
（6） 利用图搜索算法在G中搜索连接起点和终点的可行路径，直到找到最短路径或者搜索失败。
基本PRM算法构建路径图的伪代码如下：
BUILD_ROADMAP
1G.init()； i←0；
2while i<N do；
3if α(i)∈Cfree
4G.add_vertex()； i←i+1；
5for each q∈neighborhood(α(i)，G)
6if (not G.same_component(α(i).q)) and CONNECT(α(i)，q)
7G.add_edge(α(i)，q)；
PRM算法通过碰撞检测判断采样点和边是否处于自由空间Cfree，而不需要精确计算空间，避免了描述位姿空间时进行大量的计算； 此外算法具有概率完备性，即对于任意存在可行解的复杂求解空间，当PRM算法的采样数量足够多时，一定能够找到一条可行的路径。由于算法在构建路径图的过程中更倾向于探索未知空间，并且把有效的采样点加入路径图中，因此对于具有多个起点或终点的分支路径问题求解来说，PRM算法可以将尽可能多的路径解包含在路径图中。
PRM算法进行路径规划的两个阶段中，学习阶段构建的路径图的质量直接关系到查询阶段获得的路径质量的好坏，前者在计算过程中耗费的时间也远比后者多，因此国内外学者对算法学习阶段的改进进行了大量的研究，主要集中在采样点的生成方面，同时在其他方面也有相关的改进。
2) RRT方法
快速扩展随机树(Rapidlyexploring Random Tree)最初是由美国科学家S.M.LaVall在1998年提出，此算法可以解决在高维空间内的非完整性约束的问题。他是基于以下设想提出： 将机器人的起始点设置为树的根节点，在书中根据某种原则选择一个已有节点，然后在这个选择的节点上根据机器人的约束条件做扩展，产生一个新的节点，然后将此新节点添加到树中，如此反复直到找到目标点为止，算法要保证随机采样可使机器人向未被探索过的空间探索，并在探索过程中逐步推进搜索树。
以下为随机数扩张的伪代码：
Build RRT(xinit，xgoal)
1 T.init(xinit)
2 for k=1 to K do
3xrand←Random_state()
4Extend(Xrand，T)
5if Reach； break；
6 return T
Extend(T，xrand)
1 xnear←Nearest_Neighbor(Xrand，t)；
2 u←Control_Select(U)
3 xnew=Newstate(u，xnear)
4 T.add_vertex(xnew)
5 T.add_edge(xnear，xnew，unew)
基本的RRT算法的伪代码如上所示，首先我们要建立一棵树T，起始点xinit为其根节点。在状态空间中随机选取某个状态点xrand，并执行Extend函数向此状态点对数进行扩展。首先我们要找到树T中离xrand最近的点xnear。然后选择一个控制输入量U，将其作用在xnear上得到新的状态点，其扩展过程如图5.21所示。不考虑运动约束的情况下，状态空间和位姿空间相等，一般情况下，对控制变量的选择为对扩张方向的选择，一般在xnear和xrand的连线上，前进一个步长得到xnew。在非完整型约束下，控制变量可以随机选取，也可以将控制集合中的所以变量都进行尝试(若U是一个连续集，则可以先将其进行离散化)。得到xnew点后将xnew和xnear做链接，RRT算法流程如图5.22所示。Extend有三种可能的结果：
(1) 若‖xnew－xgoal‖<ε，ε为事先设定好的比较小的常数，则可以将xnew和xgoal看作是同一点，搜索成功。
(2) 将xnew添加到树上，但是xnew≠xgoal。
(3) 通过碰撞检测，发现并不位于自由空间中，或者从点到点的路径与障碍物发生碰撞。
图5.21静态环境下树的扩展过程
图5.22RRT算法流程
RRT算法的特点分析：
RRT作为路径规划的经典算法，主要有以下的特点：
(1) RRT算法的流程比较简单，只需要进行重复迭代就可以了；
(2) RRT算法是一种概率完备的算法，即只要时间充足，迭代的次数足够，一定能找到一条合适的路径；
(3) RRT算法会倾向于像没有被探索过的空间做扩展。
5.2.5基于人工势场的规划方法
人工势场法在机器人导航和路径规划方面有广泛的应用，最初由Khatib在1985年提出。人工势场是对机器人运行环境的一种抽象描述，它将物理学中场的概念引入到规划环境的表达中。这种方法的基本思想是引入一个称为人工势场的数值函数描述空间结构，通过势场中的力引导机器人到达目标。这种势场分两种： 目标产生的吸引势和障碍产生的排斥势。吸引势使机器人接近目标，排斥势使机器人避开障碍，二者的叠加构成机器人运动的虚拟势场。
人工势场法是一种拟物方法，按势函数的不同选取方法，可以分为牛顿型势场(Newton Potential Field)、圆形对称势场(Circular Symmetry Potential Field)、虚拟力场(Virtual Force Field)、超四次方势场和调和势场(Harmonic Potential Field)。这种方法的实质是使障碍物分布情况及其形状等信息反映在环境每一点的势场值中，机器人依次决定行进方向。
人工势场法的优点是机器人的运行环境可以直接对系统路径的生成起到闭环控制的作用，因此可以加强导航系统的动态避障能力，从而提高导航系统对环境的适用性。人工势场法具有一个不可避免的缺陷，也即局部极小点问题。所谓局部极小点，就是在状态空间中的某些区域内，机器人受到了多个势函数的作用，造成了其所受到的斥力与引力的相等的点，也即在该点处，机器人受到的合力为零，进而产生“死锁现象”，使机器人滞留在该局部极小点处，无法成功地到达目标点。通常情况下，障碍物越密集，机器人所受到的势能函数也就越多，产生局部极小点问题的概率也就越大。
1. 人工势场法的基本原理
假设在位姿空间中，任意一个位姿用q表示，势场用U(q)表示，目标状态位姿用qg表示，与目标位姿相关联的吸引势Uatt(q)及和障碍物相关联的排斥势Urep(q)。那么位姿空间中某一位姿的势场可用下面的公式表示：
Uq=Uatt(q)+Urep(q)(5.6)
假定每一个位姿U(q)都是可微的，那么机器人所受的合力为：
F→(q)=－U(q)=－Uatt(q)－Urep(q)(5.7)
U(q)表示势场在q点的梯度，该方向是q点势场变化率最大的方向。吸引势场和排斥势场采用静电力势场模型进行定义：
Uatt(q)=12ξρ2g(q)
Urep(q)=12η1ρ(q)－1ρ0，ρ(q)≤ρ0
0，ρ(q)>ρ0(5.8)
由式(5.7)和式(5.8)可以得到机器人所受吸引力、排斥力为：
Fatt(q)=－ξ(q－qg)
Frep(q)=ηρ2(q)1ρ(q)－1ρ0ρ(q)(5.9)
继而可以得到控制机器人运动的加速度：
ak=F(qk)‖F(qk)‖a0(5.10)
设系统对环境的采样周期为T0，系统实际位姿为q(k)=(xk，yk，θk)，经过一个采样周期后，系统的位姿变成q(k+1)=(xk+1，yk+1，θk+1)，则有：
图5.23人工势场法路径规划流程图
xk+1=xk+axkT20/2
yk+1=yk+aykT20/2(5.11)
利用上述公式计算环境中每一点的势场，机器人作为一个质点，在势场力的引导下从起点开始移动，直到终点结束，其移动轨迹即为规划路径。
梯度势场算法的描述如下：
(1) 输入： 受控机器人初始位姿qi、目标位姿qg和障碍物信息；
(2) 过程： 从qi开始每次计算当前位姿qk的势场力F(qk)并沿其方向前进一个小步长δk，δk根据当前位姿设置不同的值，δk必须足够小，以免在从qk到qk+1的过程中碰到障碍物；
(3) 重复步骤(2)，一直到找到qg或无路可走时结束；
(4) 输出： 一条连接qi和qg的位姿序列或指出该序列不存在。
人工势场算法的流程图如图5.23所示。
势场的形式根据实际需要而定，把机器人等效成质点，使用位置相关的势函数解决机器人的避碰问题，以便在实际系统中得以实现。
2. 引力势函数选取
机器人无限接近目标点时，其所受势能为零，表示机器人已经到达了目标点，引力势函数可如下表示：
Ua=Kgd(5.12)
d为机器人和目标点之间的距离； Kg为动态参数； 引力函数表示为：
Fa=Kg(5.13)
障碍物斥力分别为Fi(i,…,n)，n表示障碍物个数，合力Fr_totle为：
Fr_totle=∑ni=1Fi(5.14)
机器人R受到合力为：
F=Fa_totle+Fr_totle
(5.15)
经上述分析可知，在机器人的路径规划过程中，可以根据F的方向决定机器人的运动方向，进一步调用机器人的底层控制模块，实现机器人避障功能。
3. 斥力势函数选取
由于在人工势场中障碍物ORi产生的势场对机器人R将会产生排斥作用，距离越小，排斥作用越大； 反之依然。这种势场与电势场比较类似，与距离成反比，一般可以将斥力函数表达为以下形式：
Ur=K0/ρ，ρ≤ρm
K0/ρm，ρ>ρm(5.16)
其中，ρm定义为势场最大范围作用域； ρ表示为障碍物ORi到机器人R的距离； k0为加权系数。
机器人所受斥力函数可用下式表示：
Fr=－(U0)=－dU0/dρ=k0/ρ2，ρ≤ρm
0，ρ>ρm(5.17)
此时，斥力方向为背离障碍物。
(1) 当ρ→ρ0时，Fr→∞，同时为使Fr连续，并且Fr可修改为：
Fi=K0(ρ－ρ0)2－K0(ρm－ρ0)2,ρ≤ρm
0,ρ>ρm(5.18)
(2) 当ρ→0时，Fr→∞，也即当机器人与障碍物相碰时，机器人受到的斥力将会变成无穷大； 要避免机器人与障碍物相碰，则可以设置一个最小安全距离ρ0。
ρ0和ρm的取值是由机器人速度、尺寸和实际环境中障碍物的稀疏程度等因素有关。
4. 局部极点的问题处理
实际过程中，采用上述的路径规划算法，通常会存在局部极点的问题，即在机器人尚未到达目标点之前的某个运动位置上，所受合力为零，导致机器人停止不前。局部极点问题表现为以下缺陷：
(1) 当目标附近有障碍物时无法到达目标点；
(2) 在狭窄通道中不能发现路径，也即碰撞问题；
(3) 在障碍物前发生振荡；
(4) 存在陷阱区域。
针对局部极值点问题，许多研究者进行了深入的研究和改进。
目前主要有采用随机势场法，采用随机运动逃脱局部的最小点的方法； 引入模拟退火算法和一些启发性知识避免局部极点问题的方法； 把人工势场和蚁群算法或神经网络相结合克服局部极点的方法。从本质上讲，上述的克服局部极点的方法的基本思想都是尽量避免机器人在运动途中，产生势场合力为零的情况，或者是在运动到局部极点之后，设计某种规则，使机器人改变运动方向，最终脱离局部极点的位置。
针对局部极点问题提出了如下的解决方案，设某一时刻机器人所处点为R(xi，yi)，势力场对机器人的作用点为V(xv，yv)，在产生局部极小值情况下，让机器人垂直于受力方向，按逆时针方向移动a步长，其算法描述为：
(1) 当机器人处于局部极小值时，机器人的受力方向为： k=(yi－yv)/(xi－xv)；
(2) 机器人的垂直受力方向为： k′=－(xi－xv)/(yi－yv)；
(3) 按逆时针移动后，机器人所在位置： (xi+a，k′xi+k′a+yi)。
5.3机器人轨迹规划
机器人轨迹规划意味着在行动之前决定行动的进程； 机器人轨迹规划是一个对机器人行动过程的描述。
机器人自动轨迹规划是一种重要的问题求解技术，它从某个特定的问题状态出发，寻求一系列行为动作，并建立一个操作序列，直到求得目标状态为止。与一般问题求解相比，自动轨迹规划更注重于求解过程。此外轨迹规划要解决的问题往往是真实问题，而不是抽象的数学模型问题。机器人轨迹规划是机器人学的一个重要研究领域，研究机器人各种控制求解问题。
5.3.1机器人轨迹规划概述与发展现状
1. 机器人轨迹的概念
机器人轨迹泛指机器人在运动过程中的位移、速度和加速度，也可定义为机器人运动构件的位姿和位姿变化情况。多数是指机器人末端执行器的位姿和位姿变化情况。
机器人运动轨迹的描述一般是对其末端执行器位姿变化的描述。控制轨迹也就是按时间控制手部走过的空间路径。在轨迹规划中，也常用点表示机器人在某一时刻的状态，或某一时刻的轨迹，或用它表示末端执行器的位姿，例如起始点、终止点就分别表示末端执行器的起始位姿及终止位姿。
2. 轨迹规划的一般性问题
机器人在作业空间要完成给定的任务，其手部运动必须按一定的轨迹进行。轨迹规划是根据作业任务的要求，计算出预期的运动轨迹。
机器人轨迹的生成一般是先给定轨迹上的若干个点，将其经运动学反解映射到关节空间，对关节空间中的相应点建立运动方程，然后按这些运动方程对机器人关节进行插值，用于机器人关节运动的控制，从而实现作业空间的运动要求，这一过程通常称为机器人轨迹规划。
(1) 机器人的作业可看作是工具坐标系{T}相对于工件坐标系{S}的一系列运动。如图5.24所示，将销插入工件孔中作业，可以借助工具坐标系的一系列位姿Pi (i=1，2，…，n)描述。
图5.24机器人将销插入工件孔中的作业描述
(2) 用工具坐标系相对于工件坐标系的运动描述作业路径是一种通用的作业描述方法。它把作业路径描述与具体的机器人、手爪或工具分离开来，形成了模型化的作业描述方法，从而使这种描述既适用于不同的机器人，也适用于在同一机器人上装夹不同规格的工具。把图5.25所示的机器人从初始状态运动到终止状态的作业，看作是工具坐标系从初始位置{T0}变化到终止位置{Tf}的坐标变换。
图5.25机器人的初始状态和终止状态
(3) 更详细地描述运动时不仅要规定机器人的起始点和终止点，而且要给出介于起始点和终止点之间的中间点，也称路径点。这时，机器人运动轨迹除了位姿约束外，还存在着各路径点之间的时间分配问题。
(4) 机器人的运动应当平稳，不平稳的运动将加剧机械部件的磨损，并导致机器人的振动和冲击。为此，要求所选择的运动轨迹描述函数必须连续，且它的一阶导数(速度)，有时二阶导数(加速度)也应该连续。
(5) 轨迹规划既可以在关节空间中进行，也可以在直角坐标空间中进行。在关节空间中进行轨迹规划是指将所有关节变量表示为时间的函数，用这些关节函数及其一阶、二阶导数描述机器人预期的运动； 在直角坐标空间中进行轨迹规划是指将手爪位姿、速度和加速度表示为时间的函数，而相应的关节位置、速度和加速度由手爪信息导出。
3. 机器人轨迹的生成方式
(1) 机器人示教—再现运动。这种运动由人手把手示教机器人，定时记录各关节变量，得到沿路径运动时各关节的位移时间函数q(t)； 再现时，按内存中记录的各点的值产生序列动作。
(2) 机器人关节空间运动。这种运动直接在关节空间中进行。由于动力学参数及其极限值直接在关节空间里描述，所以用这种方式求最短时间运动很方便。
(3) 机器人空间直线运动。这是一种直角空间里的运动，它便于描述空间操作，计算量小，适宜简单的作业。
(4) 机器人空间曲线运动。这是一种在描述空间中用明确的函数表达的运动，如圆周运动、螺旋运动等。
4. 机器人轨迹规划涉及的主要问题
为了描述一个完整的作业，往往需要将上述运动进行组合。这种规划涉及以下问题：
(1) 用示教方法给出轨迹上的若干个节点。
(2) 用一条轨迹通过或逼近节点，此轨迹可按一定的原则优化，如加速度平滑得到直角空间的位移时间函数X(t)或关节空间的位移时间函数q(t)； 在节点之间如何进行插补，即根据轨迹表达式在每一个采样周期实时计算轨迹上点的位姿和各关节变量值。
(3) 以上生成的轨迹是机器人位置控制的给定值，可以据此或根据机器人的动态参数设计一定的控制规律。
(4) 规划机器人的运动轨迹时，尚需明确其路径上是否存在障碍约束的组合。一般将机器人的规划与控制方式分为四种情况，如表5.2所示。
表5.2机器人的规划与控制方式
状态
障 碍 约 束
有无
路径
约束
有离线无碰撞路径规划+在线路径跟踪离线路径规划+在线路径跟踪
无位置控制+在线障碍探测和避障位置控制
5. 机器人轨迹规划的发展及现状
随着 21 世纪工业自动化的不断发展，工业生产领域的很多人工作业都被工业机器人所取代，在工业应用中，关节型机器人(Articulated Robot，也称作机械手或机械臂)最为常见，其主要特点是模仿人类身体从腰部到手部的构造，形式上从二自由度到冗余自由度不等。轨迹规划，是机器人设计中一个非常关键的技术模块，轨迹规划的重点是研究满足用户多样化需求的各种轨迹规划算法。工业机器人的轨迹规划在机器人的运动控制中占据着重要的位置，其不但直接控制着机器人末端执行器的工作方式，还对机器人的运动效率、能量消耗、平稳运行和使用寿命有着较大的影响，所以轨迹规划成了机器人学最重要的研究领域之一。
工业机器人的轨迹规划问题经过半个世纪的研究，在基本轨迹规划方面已有了一大批成熟的技术，在最优轨迹规划方面也不乏可以指导工程实践的成果。轨迹规划主要分为笛卡儿空间中的轨迹规划和关节空间中轨迹规划。在众多学者对轨迹规划的研究过程中，关节空间的轨迹规划是大多数学者的研究方向，而笛卡儿空间的轨迹规划并不多见。机器人的轨迹规划大多在关节空间中进行，这种规划方式十分便捷； 但是针对某些对空间轨迹要求严格的工况下，则采用笛卡儿空间规划，这是因为虽然笛卡儿空间轨迹规划有着众多的优点，但是之前并没有用于笛卡儿空间坐标测量机器人末端执行器位置的传感器。
近年来，随着图像处理技术的发展以及多轴传感器定位技术的快速研发，使得笛卡儿空间坐标定位的问题得以解决。与此同时，工业加工作业中对机器人执行精度的要求越来越高，如高精度焊接等，所以对笛卡儿空间轨迹规划进行深入研究具有重要意义，例如目前离线编程技术可直接运用计算机仿真，提取工件的模型，生成作业需求的运动轨迹，这时笛卡儿空间轨迹规划就显得更加直观，易于运用。任务级机器人语言(如发出“抓住螺钉”指令)的发展趋势，也需要机器人拥有自动执行并规划任务的能力，能够从空间任务中直接提取出笛卡儿空间的运动路径，并规划运动状态。随着机器人逆解算法的进步和计算芯片性能的提高，笛卡儿空间规划也会越来越多地发挥其显示直观、实时性高的优点。建立在基本轨迹规划上的最优轨迹规划，则是以后轨迹规划的重要发展方向，找到各种工况下的最优轨迹也是学者们研究的热点，但是目前还没有一种通用性的综合优化方法适用于工业机器人。
目前智能轨迹规划算法是机器人研究的热点之一，基于最优化理论的发展，即捜寻最优轨迹的一种算法，如遗传算法、模糊算法、粒子群算法、模拟退火算法和人工神经网络等，并且很多实际应用与实验仿真也表明这些优化算法应用到机器人轨迹规划中，确实能够达到优化的目的，优化的结果包括降低能耗或者缩短时间，实现最优时间或者最优能量的轨迹优化设计，在保证能稳定准确的运行情况下，提高机器人的工作效率。
工业自动化应用中越来越高的精度要求和越来越复杂的工况，也给机器人轨迹规划提出了新的挑战，轨迹规划必须朝着高精度、高效率、模块化、自动化、智能化的方向进化。综上所述，展望工业机器人轨迹规划的最新发展趋势，包含基于机器视觉(Machine Vision)的实时自动轨迹规划、考虑实际工作模式的高可靠性轨迹规划、基于虚拟现实的机器人轨迹规划系统及轨迹规划算法的集成化、可视化、智能化等方面。
5.3.2机器人关节轨迹的插值
机器人关节空间路径规划，是指给定关节角的约束条件，包括机器人的起点、终点或中间点的位置、速度、加速度等，生成机器人各关节变量的变化曲线的过程。当只给定起点、终点时刻的约束条件时，相应的机器人路径规划称为点到点路径规划； 若要求机器人关节变量严格按照指定曲线运动时的规划，称为连续路径规划； 当要求经过多个中间点，而对节点间的轨迹没有严格限制时的路径规划，称为多节点路径规划。
实际中，往往是采用介于点到点和连续路径规划之间的机器人多节点路径规划。可以采用三次多项式、
图5.26单个关节的不同轨迹曲线
过路径点的三次多项式插值、高阶多项式插值、用抛物线过渡的线性插值等插值函数进行关节空间的路径规划。
1. 三次多项式插值
考虑机器人末端在一定时间内从初始位置和方位移动到目标位置和方位的问题。利用逆运动学计算，可首先求出一组起始和终点的关节位置。现在的问题是求出一组通过起点和终点的光滑函数，满足这个条件的光滑函数可以有许多条，如图5.26所示。
为满足机器人关节运动速度的连续性要求，还有两个约束条件，即在起始点和终止点的关节速度要求。为了满足关节运动速度连续性的要求，起始点和终止点的关节速度可简单地设定为零。
θ(0)=θ0
θ(tf)=θf(5.19)
θ·(0)=0
θ·(tf)=0(5.20)
上述四个边界约束条件式(5.19)和式(5.20)唯一地确定了一个三次多项式：
θ(t)=a0+a1t+a2t2+a3t3(5.21)
机器人运动轨迹上的关节速度和加速度则为：
θ·(t)=a1+2a2t+3a3t2
θ¨(t)=2a2+6a3t(5.22)
为求得三次多项式的系数a0，a1，a2和a3，代以给定的约束条件，有方程组：
θ0=a0
θf=a0+a1tf+a2t2f+a3t3f
0=a1
0=a1+2a2tf+3a3t2f(5.23)
求解上述线性方程组可得：
a0=θ0
a1=0
a2=－3t3f(θf－θ0)
a3=－2t3f=(θf－θ0)(5.24)
对于起始速度及终止速度为零的关节运动，满足连续平稳运动要求的三次多项式插值函数为：
θ(t)=θ0+3t2f(θf－θ0)t2－2t3f(θf－θ0)t3(5.25)
机器人关节角速度和角加速度的表达式为：
θ·(t)=6t2f(θf－θ0)t－6t3f(θf－θ0)t2
θ¨(t)=6t2f(θf－θ0)t－12t3f(θf－θ0)t(5.26)
这里再次指出： 这组解只适用于机器人关节起始、终止速度为零的运动情况。
三次多项式插值的机器人关节运动轨迹曲线如图5.27所示。由图可知，其速度曲线为抛物线，相应的加速度曲线为直线。
图5.27三次多项式插值的关节运动轨迹
可以把所有路径点也看作是“起始点”或“终止点”，求解逆运动学，得到相应的关节矢量值。然后确定所要求的三次多项式插值函数，把路径点平滑地连接起来。但是，这些“起始点”和“终止点”的关节运动速度不再是零。
路径点上的关节速度可以根据需要设定，确定三次多项式的方法与前面所述的完全相同，只是速度约束条件变为：
θ·(0)=θ·0
θ·(tf)=θ·f(5.27)
求得三次多项式的系数：
a0=θ0
a1=θ·0
a2=3t2f(θf－θ0)－2tfθ·0－1tfθ·f
a3=－2t3f(θf－θ0)+1tf(θ·0+θ·f)(5.28)
路径点上的关节速度可由以下三种方法规定：
(1) 在直角坐标空间或关节根据工具坐标系在直角坐标空间中的瞬时线速度和角速度来确定每个路径点的关节速度。
(2) 空间中采用适当的启发式方法，由控制系统自动地选择路径点的速度。
(3) 为了保证每个路径点上加速度的连续，由控制系统按此要求自动地选择路径点的速度。
2. 过路径点的三次多项式插值
对于这种情况，假如末端执行器在路径点停留，即各路径点上速度为0，则轨迹规划可连续直接使用前面介绍的三次多项式插值方法； 但若末端执行器只是经过，并不停留，就需要将前述方法推广。
对于机器人作业路径上的所有路径点可以用求解逆运动学的方法，先得到多组对应的关节空间路径点，进行轨迹规划时，把每个关节上相邻的两个路径点分别看作起始点和终止点，再确定相应的三次多项式插值函数，把路径点平滑连接起来。一般情况下，这些起始点和终止点的关节运动速度不再为零。
设路径点上的关节速度已知，在某段路径上，起始点为θ0和θ·0，终止点为θf和θ·f，这时，确定三次多项式的方法与前面所述的全相同，只是速度约束条件变为：
θ·(0)=θ·0
θ·(tf)=θ·f(5.29)
利用约束条件确定三次多项式系数，有下列方程组：
θ0=a0
θf=a0+a1tf+a2t2f+a3t3f
θ·0=a1
θ·f=a1+2a2tf+3a3t2f(5.30)
求解方程组可得：
a0=θ0
a1=θ·0
a2=3t2f(θf－θ0)－2tfθ·0－1tfθ·f
a3=－2t3f(θf－θ0)+1t2f(θ·0+θ·f)(5.31)
实际上，由上式确定的三次多项式描述了起始点和终止点具有任意给定位置和速度的运动轨迹。剩下的问题就是如何确定路径点上的关节速度，可由以下3种方法规定：
(1) 根据工具坐标系在直角坐标空间中的瞬时线速度和角速度确定每个路径点的关节速度。
该方法利用操作臂在此路径点上的逆雅可比，把该点的直角坐标速度“映射”为所要求的关节速度。当然，如果操作臂的某个路径点是奇异点，这时就不能任意设置速度值。按照该方法生成的轨迹虽然能满足用户设置速度的需要，但是逐点设置速度毕竟要耗费很大的工作量。
(2) 在直角坐标空间或关节空间中采用适当的启发式方法，由控制系统自动地选择路径点的速度。
图5.28表示一种启发式选择路径点速度的方式。图中θ0为起始点； θD为终止点，θA，θB和θC是路径点，
图5.28路径点上的速度自动生成
用细实线表示过路径点时的关节运动速度。这里所用的启发式信息从概念到计算方法都很简单，即假设用直线段把这些路径点依次连接起来，如果相邻线段的斜率在路径点处改变符号，则把速度选定为零； 如果相邻线段不改变符号，则选取路径点两侧的线段斜率的平均值作为该点的速度。因此，根据规定的路径点，系统就能够按此规则自动生成相应的路径点速度。
(3) 为了保证每个路径点上的加速度连续，由控制系统按此要求自动地选择路径点的速度。
为了保证路径点处的加速度连续，可设法用两条三次曲线在路径点处按一定规则连接起来，拼凑成所要求的轨迹。其约束条件是： 连接处不仅速度连续，而且加速度也连续。
设所经过的路径点处的关节角度为θv，与该点相邻的前后两点的关节角分别为θ0和θg。设其路径点处的关节加速度连续。如果路径点用三次多项式连接，试确定多项式的所有系数。该机器人路径可分为θ0~θv段及θv~θg段两段，可通过由两个三次多项式组成的样条函数连接。设θ0~θv的三次多项式插值函数为：
θ(t)=a10+a11t+a12t2+a13t3(5.32)
θv~θg的插值三次多项式为：
θ(t)=a20+a21t+a22t2+a23t3(5.33)
上述两个三次多项式的时间区间分别为［0，tf1］和［0，tf2］。对这两个多项式的约束是：
θ0=a10
θυ=a10+a11tf1+a12t2f1+a13f3f1
θv=a20
θk=a20+a21tf2+a22t2f2+a23t3f2
0=a11
0=a21+2a22tf2+3a23t2f2
a11+2a12tf1+3a13t2f1=a21
2a12+6a13tf1=2a22(5.34)
以上约束组成了含有8个未知数的8个线性方程。对于tf1=tf2=tf的情况，这个方程组的解为：
a10=θ0
a11=0
a12=12θv－3θg－9θ04t2f
a13=－8θv+3θg+5θ04t2f
a20=θυ
a21=3θg－3θ04tf
a22=－12θv+6θg+6θ04t2f
a23=8θv－5θg－3θ04t3f(5.35)
一般情况下，一个完整的机器人轨迹由多个三次多项式表示，约束条件(包括路径点处的关节加速度连续)构成的方程组。
3. 高阶多项式插值
如果对于运动轨迹的要求更为严格，约束条件增多，那么三次多项式就不能满足需要，必须用更高阶的多项式对运动轨迹的路径段进行插值。例如，对某段路径的起始点和终止点都规定了关节的位置、速度和加速度，则要用一个五次多项式进行插值，即
θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5(5.36)
多项式的系数a0，a1，…，a5必须满足6个约束条件：
θ0=a0
θf=a0+a1tf+a2t2f+a3t3f+a4t4f+a5t5f
θ·0=a1
θ·f=a1+2a2tf+3a3t2f+4a4t3f+5a5t4f
θ¨0=2a2
θ¨f=2a2+6a3tf+12a4t2f+20a5t3f(5.37)
这个线性方程组含有6个未知数和6个方程，其解为：
a0=θ0
a1=θ·0
a2=θ¨02
a3=20θf－20θ0－(8θ·f+12θ·0)tf－(3θ¨0－θ¨f)t2f2t3f
a4=30θ0－30θf+(14θ·f+16θ·0)tf+(3θ¨0－2θ¨f)t2t2t4f
a5=12θf－12θ0－(6θ·f+6θ·0)tf+(θ¨0－θ¨f)t2f2t5f(5.38)
4. 用抛物线过渡的线性插值
对于给定了起始点和终止点的机器人关节空间轨迹规划，似乎选择线性函数最为简单，但是最纯线性插值往往会导致在起始点和终止点关节运动速度的不连续，并且加速度无限大，在两端点会造成刚性冲击。
图5.29两点间的线性
插值轨迹
为了改进，在线性插值两端点的邻域内设置一段抛物线形缓冲区段，如图5.29所示。由于抛物线函数对于时间的二阶导数为常数，因此在端点的速度平滑过渡，从而使整个轨迹上的位置和速度连续,如图5.30所示。
对于这种路径规划存在有多个解，其轨迹不唯一，如图5.31所示。但是，每条路径都对称于时间中点th和位置中点θh。
为了保证机器人路径轨迹的连续、光滑，即要求抛物线轨迹的终点速度必须等于线性段的速度，故有下列关系：
θ·tb=θh－θbth－tb(5.39)
图5.30带有抛物线过渡域的线性轨迹
图5.31轨迹的多解性与对称性
式中，θb为过渡域终点tb处的关节角度。用θ¨表示过渡域内的加速度，θb的值可按下式解得：
θb=θ0+12θ¨t2b(5.40)
令t=2th，可得：
θ¨t2b－θ¨ttb+(θf－θ0)=0(5.41)
式中，t为所要求的运动持续时间。
一般情况下，θ0、θf、tf是已知条件，这样，根据式（5.41）可以选择相应的θ¨和tb，得到相应的轨迹。通常的做法是先选定加速度θ¨的值，然后按式(5.42)求出相应的tb：
tb=t2－θ¨2t2－4θ¨(θf－θ0)2θ¨(5.42)
由式（5.42）可知，为保证tb有解，过渡域加速度值θ¨必须选得足够大，即
θ¨≥4(θf－θ0)t2(5.43)
当式(5.37)中的等号成立时，机器人轨迹线性段的长度缩减为零，整个轨迹由两个过渡域组成，这两个过渡域在衔接处的斜率(关节速度)相等； 加速度θ¨的取值越大，过渡域的长度就越短，若加速度趋于无穷大，轨迹又复归到简单的线性插值情况。
5. 过路径点的用抛物线过渡的线性插值
简单的线性插值会使得机器人在某一路径点处的关节运行速度产生跳变，导致加速度急剧增加，甚至接近于无穷大，如图5.32所示。为了避免这种情况的发生，使用线性插值法时，需要在每个路径点附近加入一截使用抛物线拟合的过渡段，使生成的轨迹具有连续的角度值变化和均匀的速度。至于抛物线对时间的二阶导数，因为它是常数，所以对应的路径段具有恒定的加速度，使得该点处的轨迹运动速度平稳而不引起阶跃变化。通过抛物线的拟合，即可得到位移和速度连续的整个轨迹。
图5.32机器人某一关节的轨迹
机器人某一个关节的一组关节路径点之间使用线性样条相连，而各线性样条与路径点相连的两端都使用了一段抛物线连接，具体如图5.33所示。线性样条和抛物线拟合形成的轨迹称为带有抛物线拟合的线性
图5.33多段带有抛物线过渡的
线性插值轨迹
轨迹。在图中，关节路径点多余两个，所以有多条路径段，机器人移动到第一个路径段的终点，还会向下一点移动，下一点可以是目标点或者另一个中间点。如前所述，采用带有抛物线拟合的分段线性运动轨迹生成方法，以避免运动速度的不连续。
在各路径段进行抛物线拟合时，可利用各个路径点的边界条件得到相应的抛物线拟合函数的系数。例如，如果已知机器人关节某一起始路径点的运动速度，因为下一个中间点处于该点的路径段中，该中间点要有连续的速度和位移，根据这一边界条件，可以计算下一条路径段的系数，以此类推，直到计算出全部路径段并到达目标点。当然，我们还需要根据给定的关节运动速度对各个路径段计算新的时间t，并且还需检查加速度的值是否大于极限值。
在这里将使用以下符号： 用j，k和l表示三个临近路径点，它们都表示关节在某一时刻的转动角度值。位于路径点k处的拟合区段的运行时间间隔为tk，位于点j和k之间的直线段的时间间隔为tjk，此两点间包括拟合段的总时间间隔为tdjk。此外，j、k间直线段的速度为θ·jk，而在点j处拟合区段的加速度为θ·j。
与只包含首尾两个路径点的单一路径段的情形相似，通过多个中间路径点的带有抛物线拟合的线性插值轨迹可能存在许多不同的结果，而不同结果的产生主要由各个拟合区段的加速度值决定。已知任意一个路径点的位置θk、期望的时间间隔tdjk以及此路径点处的加速度值为|θ¨k|，那么可以计算出该点相应拟合区段的时间间隔。对于那些不是处于轨迹首或尾的路径段(j，k≠1，2； k≠n，n－1)，可直接使用下列公式计算：
θ·jk=θk-θjtdjk
θ¨k=sgn(θ·kl-θ·jk)|θ¨k|
tk=θ·kl-θ·jkθ¨k
tjk=tdjk-12tj-12tk
(5.44)
因为一条机器人的路径段包括轨迹首尾端部拟合区段的时间，而首尾端部路径段为拟合区段的一部分，其处理情况与中间路径段有所差异。为求取首部路径段的持续时间，可使此处直线区段的速度相等，即
θ2－θ1td12－12t1=θ¨1t1(5.45)
用式(5.45)算出起始点拟合区段的持续时间t1之后，进而求出θ·12和t12 ：
θ¨1=sgn(θ·2－θ·1)|θ¨1|
t1=td12－t2d12－2(θ2－θ1)θ¨1
θ·12=θ2－θ1td12－t1－12t2
t12=td12－t1－12t2(5.46)
对于最后一个路径段，路径点n-1与终止点n之间的参数与第一个路径段相似，即
θm－1－θmtd(n－1)n－12tn=θ¨ntn(5.47)
根据式(5.47)便可求出：
θ¨n=sgn(θ·n－1－θ·n)|θ¨n|
tn=td(n－1)n－t2d(n－1)n+2(θm－θn－1)θ¨n
θ·(n－1)n=θn－θn－1td(n－1)n－12tn
t(n－1)n=td(n－1)n－tn－12tn－1(5.48)
可使用式(5.44)~式(5.48)的计算轨迹中各拟合段的运行时间以及相应的速度。通常情况下，需要指定的是各个路径点和每个路径段的持续时间，此时，关节轨迹规划将使用机器人的隐式加速度值。有些时候，为了简化计算，可直接使用隐式加速度值求得路径段的持续时间。对于轨迹的各拟合段部分，为了使路径段有较长的直线区段，应选取较大的加速度值以压缩拟合区段。需要注意的是，带有抛物线拟合的线性插值轨迹通常不通过各个中间路径点，除了机器人需要在某个路径点处暂停。如果选定了足够大的加速度值，那么实际生成的轨迹将非常接近提供的已知路径点。如果机器人需要通过一个路径点，可行的方法是将轨迹划分成两个段，该点可以用来作为上一段轨迹的目标点以及下一段轨迹的出发点，而运行的机器人会在此处停止并运行下一段轨迹。
5.3.3笛卡儿空间规划方法
在笛卡儿空间中，应用机器人轨迹规划的插补算法可求得中间点(插补点)的坐标，把这些插补点的位置和姿态转换成对应的机器人关节角，通过机器人运动学逆解运算，然后沿着规划的轨迹运动使得这些关节角控制机器人末端执行器。一个机器人在笛卡儿空间中的轨迹规划位姿控制如图5.34所示。
图5.34机器人在笛卡儿空间中轨迹规划的控制过程
笛卡儿空间机器人轨迹规划系统中两个最基本的轨迹规划方法，即空间直线和圆弧的轨迹规划，因为空间很多曲线都可以分割为多段直线或圆弧。而在很多情况下会出现空间多段直线连接或者直线与圆弧连接，这就不可避免地会碰到连接处尖角的问题。为能使运动轨迹平滑，本章采用圆弧过渡进行平滑尖角，接下来将介绍空间连续直线和直线—圆弧轨迹规划。
此外，还有一些自由型曲线，只给出一系列路径点，利用一条平滑曲线将这些路径点光滑地连接起来。如果由直线或圆弧分割这些自由型曲线将难以保证精度，而B样条曲线能够很好地逼近这些自由型曲线。
1. 直线轨迹规划
直线的轨迹规划是已知直线始末两点的位置和姿态，求直线轨迹上的插补点的位置和姿态。各个插补点的位置和姿态可用以下的公式求出：
x=x1+λΔx
y=y1+λΔy
z=z1+λΔz
α=α1+λΔα
β=β1+λΔβ
γ=γ1+λΔγ(5.49)
式（5.49）中，(x1，y1，z1)、(α1，β1，γ1)分别为开始点的位置RPY变换姿态角，(x1，y1，z1)、(α1，β1，γ1)分别为插补点的位置和RPY变换姿态角，λ为归一化因子，(Δx，Δy，Δz)、(Δα，Δβ，Δγ)为位置和姿态角的增量，其求解如下：
Δx=x2－x1
Δy=y2－y1
Δz=z2－z1
Δα=α2－α1
Δβ=β2－β1
Δγ=γ2－γ1(5.50)
式（5.50）中(x2，y2，z2)、(α2，β2，γ2)分别为结束点的位置和姿态角。
采用抛物线过渡的λ归一化因子线性函数，这种方式不仅简单易解，同时也能保证整条轨迹上的位移和速度的连续性要求。抛物线过渡的线性函数对两点的位姿使用线性插值时，两点的领域内增加一段抛物线的缓冲区段。由于相应区段内有恒定不变的加速度且抛物线时间的二阶导数为常数，轨迹的过渡平滑，于是整条轨迹上的位移和速度都具有连续性。为了能够构造出这段轨迹的运动曲线，假设有相等两端的抛物线运动时间，在这两个过渡区域内采用相同的恒加速度值，于是λ归一化因子可按如下的方式求得。
设抛物线过渡的线性函数的直线段速度v，抛物线段的加速度为a。那么抛物线段的运动时间和位移分别为：
Tb=va(5.51)
Lb=12aT2b(5.52)
直线运动总的位移和时间分别表示为：
L=(x2－x1)2+(y2－y1)2+(z2－z1)2
T=2Tb+L－2Lbv(5.53)
抛物线段位移、时间、加速度分别归一化处理：
Lbλ=LbL
Tbλ=TbT
αλ=2LbλT2bλ(5.54)
则，可得到λ的计算公式：
λ=12aλt2(0≤t≤Tbλ)
12aλT2bλ+aλTbλ(t－Tbλ)(Tbλ<t<1－Tbλ)
12aλT2bλ+aλTbλ(t－Tbλ)－12aλ(t+Tbλ－1)2(1－Tbλ<t<1)(5.55)
式(5.55)中，t=(i/N)，i=0，1，2，…，N，0≤λ≤1，当λ=0时，对应于起点，当λ=1时，对应于终点。λ是分段离散函数关于时间t来说，λ和t均为无量纲，对称的抛物线线段0≤t≤Tbλ和1-Tbλ≤t≤1。图5.35为λ和λ′随离散时间t变化的图形。
设抛物线过渡的线性函数的直线段速度为v，抛物线段的加速度为a，而不是给出运动的总时间，主要是考虑机器人的运动速度和加速度都有约束，并且不同的机器人约束也不一样，因此这里把速度和加速度作为输入量。
2. 平面圆弧插补
平面圆弧为与基坐标系三个平面中的任一个重合的圆弧运动轨迹的平面，如XOY内的圆弧： 平面内有三个不共线的点P1，P2，P3与它们相应的机器人末端的位姿，如图5.36所示。假设v为沿圆弧运动的速度； Ts为插补周期。由P1，P2，P3的坐标，可求出圆弧的半径R，以及总的圆心角θ，根据圆的公式可得：
图5.35为λ和λ′随离散时间t变化的图形
图5.36平面圆弧插补
θ1=arccos［(x3－x2)+(y3－y2)－2R2］2R2
θ2=arccos［(x2－x1)+(y2－y1)－2R2］2R2(5.56)
根据圆弧上的数学关系可计算各个插补点的坐标值。同直线插补一样，判断插补过程是匀速、匀加速和匀减速阶段，求出总时间t=t1+t2+t3。求解插补过程当中的中间点数： N=t/Ts。假设插补过程都是匀速阶段，那么插补周期Ts内的角位移量Δθ=vTs/R能计算出圆弧上任意插补点Pi+1的坐标：
xi+1=Rcos(θi+Δθ)=xicosθ－yisinΔ
yi+1=Rsin(θi+Δθ)=yicosθ+xisinΔ
θi+1=θi+Δθ(5.57)
3. 空间圆弧插补
给定空间中不共线的三个点p1(x1，y1，z1)，p2(x2，y2，z2)和p3(x3，y3，z3)，如图5.37所示。
图5.37空间圆弧插补算法的原理图
为了求得圆弧上各点的坐标值，必须先求出其圆心的坐标及半径R。由三个点能够决定唯一的平面，那么P1，P2，P3所描述的平面M可表示为：
x－x3y－y3z－z3
x1－x3y1－y3z1－z3
x2－x3y2－y3z2－z3(5.58)
T是过直线P1P2的中点并且还垂直P1P2的平面，那么T上的任意直线都垂直P1P2，于是平面T的方程可表示为：
x－x1+x22(x2－x1)+y－y1+y22(y2－y1)+z－z1+z22(z2－z1)=0(5.59)
同理，过P2P3中点并垂直于P2P3的平面S的方程为：
x－x2+x32(x3－x2)+y－y2+y32(y3－y2)+z－z2+z32(z3－z2)=0(5.60)
所以，平面M，T和S的交点便是要求的圆心O，确定O的坐标后可求圆弧半径：
R=(x1－x0)2+(y1－y0)2+(z1－z0)2(5.61)
一个圆弧的走向可以根据设定的起点、终点与一个中间点来确定，可取n=AB×BC，则从n的正向看去，圆弧的走向为逆时针。
设n=AB×BC=ui+vj+wk，则：
u=(y2－y1)(z3－z2)－(z2－z1)(y3－y2)
v=(z2－z1)(x3－x2)－(x2－x1)(z3－z2)
w=(x2－x1)(y3－y2)－(y2－y1)(x3－x2)(5.62)
如图5.33所示，圆弧上的任一点pi(xi，yi，zi)处在沿前进方向的切向量为：
mii+nij+lik=n×OPi=ijk
uvw
xi－x0yi－y0zi－z0(5.63)
可得：
mi=v(zi－z0)－w(yi－y0)
ni=w(xi－x0)－u(zi－z0)
li=u(yi－y0)－v(xi－x0)(5.64)
经过一个插补周期之后，机器人的末端从pi(xi，yi，zi)沿圆弧切向运动距离Δs到pi+1(xi+1，yi+1，zi+1)，则有：
x′i+1=xi+Δx′i=xi+Emi
y′i+1=yi+Δy′i=yi+Emi
z′i+1=zi+Δz′i=zi+Emi(5.65)
其中，E=Δsm2i+n2i+l2i，并能证明E=ΔsRu2+v2+w2是常量。
从图5.34可以看出，圆弧上并没有点p′i+1(x′i+1，y′i+1，z′i+1)，为了让其能落在此空间的圆弧上，要改进式(5.65)。连接OP′i+1交圆弧于点并用Pi+1代替P′i+1，这样就可以实现插补点落在圆弧上。在直角三角形OPiP′i+1中，有|OP′i+1|2=|OPi|2+|PiP′i+1|2，即(R+ΔR)2=R2+Δs2，则有：
xi+1=x0+R(x′i+1－x0)R2+Δs2
yi+1=y0+R(y′i+1－y0)R2+Δs2
zi+1=z0+R(z′i+1－z0)R2+Δs2(5.66)
令G=RR2+Δs2，并将式（5.65）代入式（5.66）中，可求出插补地推公式为：
xi+1=x0+G(xi+Emi－x0)
yi+1=y0+G(yi+Emi－y0)，(0≤i≤N－1)
zi+1=z0+G(zi+Emi－z0)(5.67)
5.3.4机器人轨迹的实时生成
上面所述的计算结果即构成了机器人的轨迹规划。运行中的轨迹实时生成是指由这些数据，以轨迹更新的速率不断产生θ，θ·和θ¨所表示的轨迹，并将此信息送至操作臂的控制系统。
1. 机器人关节空间轨迹的生成
在使用带抛物线过渡的线性轨迹生成方法时，需要判断当前轨迹位置是处于拟合区段还是直线区段，在直线区段，对每个关节的轨迹计算如下：
θ=θj+θ·jkt
θ·=θ·jk
θ¨=0(5.68)
式中，t是自第j个路径点算起的时间，θ·jk的值在路径规划时按式(5.68)计算。
在拟合区段： 令tinb=t－12tj+tjk，则对各关节的轨迹计算如下：
θ=θj+θ·jk(t－tinb)+12θ¨kt2inb
θ·=θ·jk+θ¨ktinb
θ¨=θ¨k(5.69)
其中θ·jk，θ¨k，tj和tjk在轨迹规划时，已由上式当进入新的直线区段时，重新把t置成1/2tk，利用该路径段的数据，继续生成轨迹。
综上所述，机器人的关节轨迹联合表达式为：
θ=λ(θj+θ·jkt)+(1－λ)［θj+θ·jk(t－tinb)+12θ¨kt2inb］
θ·=λθ·jk+(1－λ)(θ·jk+θ¨ktinb)
θ¨=(1－λ)θ¨k(5.70)
其中，当轨迹曲线处于直线区段时，λ取1； 当轨迹曲线处于直线区段时，λ取0。
2. 笛卡儿空间轨迹的生成
在笛卡儿空间中实时地产生运动轨迹，则需要通过规划的方式实现，从而得到轨迹参数。由于是机器人控制器完成轨迹生成，因此实际上第一步是先实时计算出离散的轨迹点，然后再将这些轨迹点按照一定的采样速率经逆运动学计算转换到关节空间中。
在笛卡儿空间中，由3个量(x，y，z)表示路径点位置，另外3个量(kx，ky，kz)通过矢量表示姿态。当线性函数插值采用抛物线连接时，则分别可对以上的6个变量，通过如式(5.71)~式(5.73)所示的计算公式计算。以其中的一个分量x为例，计算公式如下：
x(t)=x1+12x¨1t2
x·=x¨1t
x¨(t)=x¨1(0≤t≤t1)(5.71)
x(t)=x1+x·i(i+1)t+ti2
x·(t)=x·i(i+1)
x·(t)=0(0≤t≤ti(i+1)，i=1，2，…，n－1)(5.72)
x(t)=xi－1+x·i(i－1)t+ti－12+ti(i－1)+12x¨it2
x·(t)=x·i(i－1)
x¨(t)=x¨i(0≤t≤ti，i=2，3，…，n)(5.73)
通过上面相同的公式将其他各分量也进行计算，应该注意到机器人末端的速度或加速度并不与姿态3个量的一阶和二阶导数相等。当求得了直角坐标空间的轨迹s-，s·，s¨后，再利用求解逆运动学的方法，求得关节空间中的轨迹q，q·和q¨。由于计算q·和q¨的逆运动学需要较大工作量的计算，因此当进行实际计算的时候，只需要根据s-的逆解计算出q值，然后再用数值微分的方法计算出q·和q¨的值即可。
q·(t)=q(t)－q(t－t)t
q¨(t)=q·(t)－q·(t－t)t(5.74)
采样计算的周期表示为 t。当使用这种方法在直角坐标空间中计算q·和q¨时，可不必计算出各个分量的一阶以及二阶导数，如此可缩短一部分计算的工作量。
当采用圆弧插值时，直接先计算出 x(t)、y(t)和 z(t)，并通过类似的公式计算出3个姿态量。然后由s-(t)逆解计算出q(t)，再利用式(5.74)的数值微分公式算出q·(t)和q¨(t)的值。
5.4人 机 交 互
英文中的人机交互有3个对应的概念： 其一是人与计算机的交互(HumanComputer Interaction，HCI)； 其二是人与机器的交互(HumanMachine Interaction，HMI)； 其三是人与机器人的交互(HumanRobot Interaction，HRI)。
本节中的人机交互是指人与机器人的交互。这种人与机器人交互也离不开人与计算机之间的交互，其之间互为支撑、互相交叉和融合发展。
5.4.1人与机器人交互(HRI)概念和发展现状
1. 人与机器人交互的概念
人—机器人交互是指人与机器人之间通过某种特定的传感器和接口，在一定的交互技术支撑下，实现相互理解的信息通信。HRI的研究目的是使机器人与人类和谐共处、自然高效地完成用户交代的任务，并为用户提供及时有效的反馈。HRI技术的发展不仅有益于提高人类的工作效率，而且有助于满足人类的生活需求。目前HRI技术已经成为机器人应用领域的一个重要研究热点。
一般根据人与机器人交流的方式不同，人与机器人的交互可分为近距离交互和远程交互两类。
(1) 近距离交互： 人与机器人在同一个地方，交互在近距离发生，常见的工业机器人与服务机器人多为此种情况。
(2) 远程交互： 人与机器人不在同一地方，并且在时间或空间上隔开。远程交互移动机器人通常称为遥控或遥操作，远程交互的物理机械手通常称为遥操作系统。
一般情况下，不同的应用需求驱动不同的人—机器人交互方法。下面介绍用来评价HRI方法性能的主要指标：
(1) 鲁棒性。指人—机器人交互系统面对干扰因素影响时的稳定性。
(2) 效率。在人—机器人交互过程中，用户控制和机器人运动在时间上最大限度地实现无缝衔接，减少机器人和用户之间因互相等待出现饥饿现象，导致人机交互系统冗余时间累积的问题。
(3) 交互体验。指人和机器人之间的交互方式最大限度地自然化、直观化、简单化、舒适化，提高人—机器人协作系统的友好性。
(4) 智能性。指在人—机器人交互过程中，机器人对用户语言、情绪、手势等的理解程度，以及是否能做出相应的符合用户习惯的回应。
人—机器人交互技术主要经历了四个发展阶段：
(1) 人—机器人交互发展的早期阶段是在结构化环境中，人—机器人基于命令交互和基于图形化界面交互，即用户通过专业的命令行语言或图形化界面对机器人任务和任务场景进行描述，机器人运动状态也是以命令行语言或图形化界面数据方式反馈给用户。
(2) 人—机器人交互发展的第二个阶段是基于接触式手持传感装置和可佩戴传感装置的交互方式，如数据手套、加速计、陀螺仪、肌电传感器、力反馈器、惯性测量仪、特制抓握工具等。
(3) 人—机器人交互发展的第三个阶段是基于手势的人机交互方式。
(4) 人—机器人交互发展的第四个阶段是智能化人机交互方式，与语音识别等人工智能技术融合发展。
2. 人与机器人交互的发展现状
目前，不论是工业机器人还是服务机器人领域均广泛出现人机共融环境： 一方面，近年来服务机器人成为机器人学的炙手可热的研究领域，服务机器人的工作特点决定了其必然要与服务对象(人)进行交互； 另一方面，工业机器人也不可避免要与人/环境进行直接接触和交互，尤其是新一代人机协作工业机器人，它定位于辅助工业生产，与人共享工作空间，甚至需要与人协作完成工业生产，人机协作机器人已经成为工业机器人产品中的热点。
人机共融的环境中，机器人需要与人交互甚至协作完成复杂任务，称为人机交互，共融机器人、协作机器人已经成为各国机器人领域的研究重点之一。而不论是共融机器人还是人机协作机器人，均需要工作在人的活动空间内，与人进行人机交互、协作，完成不同的任务。因此，人机共融技术、人机交互技术、人机协作技术成为机器人领域的研究热点之一。
机器人人机交互技术的发展历程可分为两个阶段： 以机器人为中心的受限方式、以人为中心的非受限方式。
以机器人为中心的受限方式，要求人在交互过程中将自己的意图按照机器人特有的输入方式进行精确分解，因此人需要耗费大量时间学习和记忆交互系统的使用方法，且操作受到较大限制。传统的人机交互方式如命令语言交互、图形交互界面和直接操纵方式等大都属于此类。传统的人机交互方式在交互设备和交互效率上存在着很多局限，例如使用机器人命令语言对操作者的专业知识要求较高； 在图形化的人机交互系统中，操作员使用鼠标和键盘作为交互工具，每次只能通过单击和输入数据来控制机器人，交互效率低下，而且交互方式不够自然和方便。
随着传感器技术的兴起和发展，传统的以机器人为中心的受限式人机交互方式正逐渐退出研究的中心，而基于传感器技术和智能感知技术实现的以人为中心的非受限自然人机交互方式，则成了研究的热点和重点。
近年来，在人机交互领域中，许多公司设计开发了很多新型的交互设备，这些设备改变和促进了新型人机交互方式的产生。基于智能感知的机器人交互方式，通过多种模态感知人类的自然行为，增强机器人的自主决策能力，使得人类可以充分使用诸如手势、表情、语音等多种自然的方式实现人机通信，减少人的学习认知压力，提高人机交互的效率和自然性。
在当前的研究中，机器人主要以触觉、视觉和听觉的方式感知人和周边环境，新型交互方式的提出，促进了很多学者对于新型交互方式的探索，因此近年来在人机交互领域出现了很多创新实用的设计，如新型的交互方式语音交互、脑机交互、穿戴式交互、体感交互等。
随着机器人技术和应用领域的迅猛发展，人们对机器人人机交互方式的要求也越来越高。作为人类体能、智能和感知的延伸，人们希望机器人不再局限于一些简单的流水线任务或完全被动地受人控制，而是能够在一些复杂的、非结构化的甚至危险的环境中发挥主导的作用。因此，研究自然和智能的机器人人机交互技术，显得格外重要和迫切。
智能感知技术是实现机器人人机交互的核心技术之一，重点研究基于生物特征、以自然语言和动态图像的理解为基础、“以人为中心”的智能信息处理和控制技术。利用智能感知技术，机器人能够拥有类似人眼、人耳、鼻子、皮肤等感官的功能，使得机器人和人之间、机器人和环境之间的沟通能够像人和人以及人和环境一样自然。基于智能感知技术的机器人人机交互，将智能感知技术和机器人技术相结合，机器人通过自身携带的或外界辅助的各类传感器感知自身和外界环境信息，机器人基于上述感知的信息，做出相应的运动决策，从而完成特定工作任务。
未来的人工智能技术、人机交互方式，如意念控制、虚拟现实技术等交互方式，将会在人—机器人的交互中大放异彩。
5.4.2人机交互技术应用
1. 虚拟示教
工业机器人示教是指操作者在实际工作环境中，通过下述方法实现： 人手引导机器人末端执行器或引导一个机械模拟装置，或用示教盒操作机器人完成作业所需位姿，并记录下各个示教点的位姿数据，利用机器人语言进行在线编程，程序回放时，机器人便执行程序要求的轨迹运动。
目前，随着机器人应用技术的推广以及为了进一步提高生产效率和操作安全性，迫切需要开发新的示教编程技术； 而且随着机器人离线编程技术的出现，虚拟现实技术的飞速发展已经为机器人虚拟示教的实现提供了技术支持。
虚拟现实技术是一种对事件的现实性从时间和空间上进行分解后重新组合的技术。这一技术包括三维计算机图形学技术、多功能传感器的交互接口技术以及高清晰度的显示技术。虚拟现实技术应用于遥控机器人和临场感通信，既可以将人们从危险和恶劣的环境中解脱出来，同时还可以解决远程通信时延等问题。它是一个能使人沉浸其中、超越其上、进出自如、交互作用的环境，与以往的传统仿真环境相比，在这种多维信息空间中所进行的仿真和建模，具有更高的逼真度。例如，北京航空航天大学等高校研制成功的CRASBHI型机器人与计算机辅助脑外科手术系统，已成功用于临床。
目前虚拟现实技术在机器人学主要应用于以下三个方面：
(1) 作为遥操作界面，可应用于半自主式操作；
(2) 作为机器视觉中自动目标识别和三维场景表示的直观表达；
(3) 建立具有真实感的多传感器融合系统仿真平台。
虚拟现实技术在机器人系统编程与仿真系统中的应用研究，主要集中在以下几方面：
(1) 应用虚拟现实技术提供的新型人机交互设备，寻求更好的机器人编程方式。编程方式主要有两种： 一种是在虚拟环境中通过机器人示教生成机器人程序； 另一种是采用人的操作示范方式，跟踪人的动作序列，自动生成机器人的动作序列。
(2) 研究虚拟环境内目标对象的建模方式，更加注重对物体物理特性的建模，以实现更趋真实的拟实操作。
(3) 研究虚拟环境与真实环境的建模与映射问题，期望实现虚拟环境下生成的控制程序，可以直接在真实环境中运行。
因而，随着虚拟现实技术研究的深入将为机器人操作、维护、教学和培训等应用提供较好的平台。
例5.5仿人神经体系机器人控制体系及人机交互方式。
根据以上多方面的分析和探讨，结合人机交互系统技术，目前世界主流的机器人示教器尚处于图形用户界面的时代，其特点是： 桌面隐喻、WIMP技术、直接操纵和“所见即所得”，很大程度上依赖于菜单选择和交互。同时，也存在着极大的弊端： 图形用户界面需要占用较多的屏幕空间，并且难以表达和支持非空间性的抽象信息的交互。
工业机器人示教器尚处于人机交互技术的早期模式，因此可以断定，随着工业机器人产业和技术的发展，虚拟现实技术必将很快地被引入机器人的人机交互系统之中。仿人神经体系机器人控制体系架构如图5.38所示，将感觉器官影射为传感器，而把运动器官影射为机器人的驱动电动机，基于脑、脊髓、神经传导通路映射的机器人控制体系，融合人机交互技术的软硬件技术设备，经过映射和优化，建立新型的人机交互体系。从中可以看到，机器人示教器硬件体系更人性化的一个发展方向，更多的虚拟现实技术将会应用在未来的机器人人机交互系统中。
图5.38仿人神经体系机器人控制体系
2. 机器人智能语音交互
随着界面设计理论和各种人机交互支撑技术的发展，人机交互技术和方式有了很大的改变，图形对话、形式对话和自然语言对话都从自身的简单形式发展到了复杂形式。图形对话不再局限于简单的单窗口界面，还能够使用多窗口和图标等从各个角度模拟真实环境。基于形式对话的人机交互系统设计越发完美，其应用领域也越来越广。从人机交互的角度来讲，系统使用语音交互技术有两个直接的优点： 一是语音输入比键盘输入快速方便； 二是语音交互可以拓宽用户的输入通道，方便用户更自然地与计算机进行交互。
目前，语音识别技术已十分成熟，有的语音识别系统甚至已能够识别自然语言，这对于智能机器人产业的发展、人机交互的研究都有重大的意义。语音行业不再局限于嵌入式和固定语法结构识别模式，而是向云端推动，向自然语言识别发展。语音交互的蓬勃发展，使其与智能人机交互机器人的深度整合成为必然。而随着语音识别技术的加入，智能人机交互机器人的想象空间越来越大，应用场景也逐渐增多。
语音交互技术在服务机器人领域获得越来越普遍的应用。
语音识别技术和智能人机交互机器人技术都属于人工智能科学领域，且拥有很高的技术含量，其发展和研究依赖于大量的语料统计和规则累计。对于这两种技术而言，高效的数据积累模型和数据规模尤为重要。
例5.6日立 Emiew2机器人。
2014年，日本日立公司(Hitachi)推出了名为Emiew2的机器人，如图5.39所示，不仅会讲笑话，还能读懂听众的表情，判断听众对笑话的反应。
图5.39Emiew2机器人
Emiew2机身红白相间，依靠底部的滑轮移动。其特点是能与人类进行简短的即兴对话。首先，Emiew2从听到的句子中提取关键词，据此来判断问题在问什么，例如“多少”； 然后，它还能在给出答案之前验证自己的判断——通过读取人类的表情。向EMIEW2说话时，它能通过麦克风和声音识别系统分辨是命令还是谈话。
3. 机器人视觉交互
与人类用眼睛看世界不同，机器人多是通过传感器看到电磁频谱。如今机器人开始大量使用摄像头进行图像捕捉，进而通过算法转化为计算机信号。在识别过程中，系统应先将待识别物体正确地从图像的背景中分割出来，再设法将图像中物体的属性图与假定模型库的属性图之间进行匹配，也就是对比相似的特征。在这种情况下，如果要识别准确，则算法要准确，同时也需要大量的图片库作为支撑。谷歌公司依托每天上传的海量图片，建立了人工神经网络。尽管如此，机器人眼里的世界与人类眼中的世界还是有很大的不同。人机交互机器视觉识别过程如图5.40所示。
图5.40人机交互机器视觉识别过程
基于视觉的人机交互方式主要是通过彩色图像或者深度图像实时识别人手手势。根据交互方式的不同，基于视觉的手势交互可以分为两类，一类是通过事先定义好的手势姿态，每种手势姿态对应一种不同的运动控制指令，例如向上、向下、向左、向右等。在交互过程中，机器人通过彩色图像识别到操作者的手势姿态，通过相关算法对该手势姿态与系统手势姿态库进行匹配，进而解析为相关的机器人运动控制指令，最后发送指令远程控制机器人的运动。但这种交互方式只能定性交互，无法进行定量交互，因而较大地限制了机器人的工作效率。另一类是直接操控型，直接通过Leap Motion、Kinect等深度传感器，获取操作者手的位置和姿态变化数据，将其转化为机器人的位置和姿态的变化指令，进而控制机器人的运动。这一类交互方式直观便捷，操作者无须记忆复杂烦琐的手势数据库，也无须对操作者进行事前培训，既能定性又能定量地对机器人的运动状态进行控制，因此具有很大的发展前景。
例5.7人机协作的智能无人机系统。
2017年5月20日，中科院在沈阳科研机构公众科学日，中国科学院沈阳自动化所研制“人机协作的智能无人机系统”首次对外亮相。根据已经公开的资料，该系统是国内第一套基于视觉的智能手势控制无人机系统，如图5.41所示。
图5.41中国科学院沈阳自动化所人机协作的智能无人机系统
基于视觉的智能手势识别技术，使无人机可以根据人的意图完成不同的飞行任务，从而实现了飞行平台上的人机协同作业功能。操控的过程中，操作人员只需要对着摄像头改变相应手势即可。这套系统视觉识别模块，创新性地采用了在线实时特征提取与编程技术，在保证实时性的同时使手势正确识别率达到了99%以上，同时还具备个性化手势定制功能，用户可以根据自己的习惯与需求，实时采集用户的手势并训练识别系统，实现不同手势功能的灵活定义与切换。
4. 机器人脑机交互
脑机接口(BrainComputer Interface，BCI)能够提供一种非神经肌肉传导的通道，直接把从人头皮上采集到的脑电信号通过预处理、特征提取、选择和分类，最终转换成机器人或其他外部设备的控制指令，从而为有运动功能障碍的残疾人提供一种与外界交流或控制外部设备的方式，脑机接口的提出为实现人类梦寐以求的人脑直接控制外部设备提供了可能。
国内外学者对于脑机接口的研究已有40多年的历史，研究的方向和重点各有不同。按照不同的分类标准，可将BCI系统按以下三种方式进行分类：
(1) 按照脑电信号采集方式。
(2) 按照脑电信号控制方式。
(3) 按照脑电信号产生方式。
例5.8机器人意念控制技术。
日本国际电气通信基础技术研究所(ATR)对外展示了一款可用意念驱使的穿戴式机器人，如图5.42所示。该机构希望这款机器人未来与家电、轮椅等组合起来，在老年人或残疾人的日常生活中发挥辅助作用。
图5.42可用意念驱使穿戴式机器人
这种机器人运用了连接大脑与机器的“脑机接口(BMI)”技术，由 ATR、日本 NTT 公司、岛津制作所等共同研发。
当天一名使用者展示了坐在电动轮椅里喝水的情景。他头戴读取脑波的设备，不出声默想约6s后，电动轮椅便自动移动到水龙头面前。之后，穿在使用者上半身的机器人转动它的手臂，最终用杯子接水并送到他的嘴边。据研究小组介绍，“想喝水”的脑波不够明确，设备较难解析，因此他们让使用者默念“想活动手”，使之成为一连串动作的开关。实验房间内安装了约3000个传感器，因此成功把握了水龙头和使用者的位置。该机器人有望帮助有脑梗塞后遗症的患者等人群更好地生活。
5. 机器人触觉交互
触觉是机器人获取环境信息的一种仅次于视觉的重要知觉形式，可直接测量对象和环境的多种性质特征。触觉交互主要任务是为获取对象与环境信息和为完成某种作业任务而对机器人与对象、环境相互作用时的一系列物理特征量进行检测或感知。广义地说，它包括接触觉、压觉、力觉、滑觉、冷热觉等，狭义地说，它是机器人与对象接触面上的力感觉。
触觉传感器技术是机器人触觉功能实现的关键，触觉传感器是机器人与环境直接作用的必要媒介，是模仿人手使之具有接触觉、滑动觉、热觉等感知功能。在机器人领域使用触觉传感器的目的在于获取机械手与工作空间中物体接触的有关信息。
类皮肤型触觉传感器具有以下功能和特性：
(1) 触觉敏感能力，包括接触觉、分布压觉、接触力觉和滑觉；
(2) 柔性接触表面，以避免硬性碰撞和适应不同形状的表面；
(3) 小巧的片状外形，以利于安装在机器人手爪上。
例5.9自带触觉反馈机器人微创手术。
澳大利亚迪肯大学和哈佛大学联手打造了一款HeroSurg机器人，HeroSurg机器人是为了腹腔镜手术专门打造的，所以它尤其适用于那些需要缝合微小组织的手术中。虽然在患者看来，它的外观可能会有点惊悚，但是研发团队希望它可以让手术更加安全、精准度更高。更重要的是，它可以让外科医生们获得一个重要的感觉——触觉。它可以通过触觉反馈机制，将触觉传递给主刀医生以及3D图像处理器，进而，医生可以看到他们的手术刀到底割到了哪里。
HeroSurg机器人是个主从式的手术系统，如图5.43所示，当它处于从属地位时，配置了多重机械臂，上面带有各种手术工具和腹腔镜； 而当它处于主导地位的时候，就成了为外科医生提供触觉的操控手柄。
图5.43HeroSurg腔镜手术机器人
有了HeroSurg机器人，医生们可以感受到他们在使用器具的过程中，到底给患者施加了多少的力。即当医生通过手术器具抓到了一些东西，或是在切一些身体组织时，他们可以感受到自己对这个组织施加的力气，他们还可以感受到组织的软硬程度，并对组织的性质做出判断。HeroSurg机器人因为具有触觉反馈能力、避免碰撞的功能以及自动适应患者和床的能力，外科医生根本不用担心在手术过程中，机器人是否会触碰到患者的体内或体外。
6. 触屏交互
触屏技术是一种新型的人机交互输入方式，与传统的键盘和鼠标输入方式相比，触摸屏输入更直观。配合识别软件，触摸屏还可以实现手写输入。触摸屏作为一种最新的计算机、手机等电子类产品输入设备，它是目前最简单、方便、自然的人机交互方式之一。触摸屏主要应用于公共信息的查询、办公、工业控制、军事指挥、电子游戏、点歌点菜、多媒体教学等。
触屏技术在机器人领域主要用于和机器人的信息交互，通过触屏对机器人下达操作指令，机器人收到指令完成相应的工作，并在触屏上反馈工作执行情况。
例5.10远程呈现技术。
派宝机器人是映博智能科技公司研发的一款远程呈现机器人，如图5.44所示。人们可以利用派宝机器人在不同的地方实时呈现出自己的行为，包括话语、表情和动作。人们可以远程控制派宝机器人前进、后退、左转、右转、抬头和低头。派宝机器人创造了一种崭新的通信方式，通过触屏交互的方式，使人可以不受空间限制进行面对面的沟通。
图5.44远程呈现机器人
派宝机器人使用平板电脑作为它的大脑，例如iPad或安卓平板电脑。平板电脑通过蓝牙4.0技术和机器人的底座进行连接。人们只要将派宝App安装到手中的手机或平板上，并连入WiFi或4G网络，即可远程控制派宝机器人。
本 章 小 结
本章主要介绍了机器人任务规划、运动规划、轨迹规划和人机交互等内容。第1节介绍了机器人任务规划的基础知识，主要包含任务规划问题分解途径、规划域的预测与规划修正。第2节介绍了运动规划所要解决的问题和位形空间的概念，重点介绍了一些典型的机器人规划方法，包括确定性图形搜索方法、PRM和RRT为代表的随机图形搜索方法和人工势场法。第3节阐述了机器人轨迹规划的内容，着重讨论了机器人关节空间轨迹的差值计算方法，包括三次多项式插值、过路径点的三次多项式插值、高阶多项式插值、用抛物线过渡的线性插值和过路径点的用抛物线过渡的线性插值等方法； 随后介绍了笛卡儿空间路径轨迹规划方法。第4节介绍了人与机器人交互的基本概念，当前主要的交互方式和发展趋势。
参 考 文 献
［1］布鲁诺·西西里安诺，洛伦索·夏维科，路易吉·维拉尼，等.机器人学 建模、规划与控制［M］.张国良，曾静，陈励华，等译.西安： 西安交通大学出版社，2015.
［2］熊有伦.机器人技术基础［M］.武汉： 华中科技大学出版社，1996.
［3］Saeed B.Biku等.机器人学导论——分析、控制及应用［M］.孙富春，朱纪洪，刘国栋，等译.2版.北京： 电子工业出版社，2013.
［4］布鲁诺·西西里安诺，欧沙玛·哈提卜.机器人手册第2卷 机器人基础［M］.《机器人手册》翻译委员会,译.北京： 机械工业出版社，2016.
［5］杜广龙，张平.机器人自然交互理论与方法［M］.广州： 华南理工大学出版社，2017.
［6］J.F.Canny. The Complexity of Robot Motion Planning. Cambridge，MA： MIT Press，1998.
［7］J.Barraquand，J.C.Latombe. Robot motion planning： A distributed representation approach. International Journal of Robotics Research.vol.10，pp.628649，1991.
［8］H.Choset，K.M.Lynch，S.Hutchinson，et al.Principles of Robot Motion： Theory，Algorithms，and Implementations.Cambridge，MA： MIT Press，2005.
［9］L.E. Kavraki，P. Svestka，J.C. Latombe，et al.Probabilistic roadmaps for path planning in highdimensional configuration spaces.IEEE Transactions on Robotics and Automation，vol. 12，pp.566580，1996.
［10］O.Khatib. Realtime obstacle avoidance for manipulators and mobile robots. International Journal of Robotics Research，vol.5，no.1，pp.9098，1986.
［11］J.C.Latombe，Robot Motion Planning，Boston，MA： Kluwer，1991.
［12］J.P. Laumond. Robot Motion Planning and Control，Berlin： SpringerVerlag，1998.
［13］S.M. LaValle. Planning Algorithms，New York： Cambridge University Press，2006.
［14］S.M. LaValle，J.J. Kuffner. Rapidlyexploring random trees： Progress and prospects. New Directions in Algorithmic and Computational Robotics. pp. 293308，2001.
［15］蔡自兴，谢斌.机器人学［M］.3版.北京： 清华大学出版社，2015.
［16］TianMiao Wang，Yong Tao，Hui Liu. Current Researches and Future Development Trend of Intelligent Robot： A Review.International Journal of Automation and Computing，Vol.15， Iss5，pp.525546，2018.
［17］袁媛. 基于自然语言的服务机器人任务规划与执行研究［D］.山东大学，2018.
［18］梁献霞. 室内环境下移动机器人的路径规划研究［D］.河北科技大学.2017.
［19］李达. 工业机器人轨迹规划控制系统的研究［D］. 哈尔滨工业大学，2011.
［20］刘翔宇. 基于仿人智能的人机交互技术研究［D］.华南理工大学，2017.
［21］安徽大学高级人工智能课程http：//www.docin.com/p974289323.htm.
［22］机器人的工程及应用轨迹规划、生成与控制技术http：//www.doc88.com/p9045496433173.html.
［23］闫宏阳. 基于限定Delaunay三角剖分的移动机器人路径规划［D］. 2008.
［24］黎波. 机器人作业任务规划研究［D］. 2012.
［25］机器人的轨迹规划https：//wenku.baidu.com/view/6c2bf8172f60ddccda38a0aa.html.
［26］机器人轨迹规划http：//www.docin.com/p534767855.html.
［27］机器人的轨迹规划http：//www.doc88.com/p5425418773324.html.
［28］从人体解剖学分析机器人的人机交互及控制系统
https：//wenku.baidu.com/view/2df7d160ddccda38376bafd0.html.
［29］中国首套视觉智能手势控制无人机系统沈阳造
http：//www.sia.cn/xwzx/mtjj/201705/t20170522_4794804.html.
［30］自带触觉反馈的机器人隔空微创手术so easy
http：//tech.sina.com.cn/q/tech/20160930/docifxwkzyh3922996.shtml.
［31］2015仿人服务机器人调研报告
https：//wenku.baidu.com/view/db7b6eccb52acfc788ebc98c.html.
［32］基于限定Delaunay三角剖分的移动机器人路径规划［D］.北京工业大学，2008.
［33］刘娅. 基于可视图法的避障路径生成及优化［D］. 昆明理工大学，2012.
［34］徐联杰. 机电产品中的管路自动布局设计技术研究［D］.北京理工大学，2016.
［35］孔迎盈. 机器人路径规划算法的研究［D］. 复旦大学，2012.
［36］杨柳. 移动机器人动态路径规划方法研究［D］. 江南大学，2011.
［37］陈伟华. 工业机器人笛卡儿空间轨迹规划的研究［D］. 华南理工大学,2015.
［38］周旋. 工业机器人最优轨迹规划问题研究［D］.沈阳建筑大学，2016.
思考题与练习题
思考题
1. 机器人任务规划的要素是什么？
2. 主要有哪几种典型的机器人任务规划系统？
3. 简述机器人轨迹规划与运动规划的区别。
4. 精确单元分解与近似单元分解的异同点是什么？
5. 机器人的人机交互主要应用于哪些方面？
6. 简述你想象中的未来人机交互方式。
练习题
1. 设要求某个转动关节路径点为10、35、25。每一段的持续时间分别为2s、1s和3s。如图5.45和图5.46所示，并设在抛物线段的默认加速度均为50°/s2。要求计算各轨迹段的速度，抛物线段的持续时间以及线性段的时间。
图5.45平面机器人动作示意
图5.46关节角度随时间变化图
2. 要求一个六轴机器人的关节1在5s内从初始30°，运动到75°，且起始点和终止点速度均为零，起始点角加速度和终止点角减速度均为5°/s2，用五次多项式规划该关节的运动。
3. 设一机器人具有6个转动关节，其关节运动均按三次多项式规划，要求经过两个中间路径点后停在一个目标位置。试问要描述该机器人关节的运动，共需要多少个独立的三次多项式？要确定这些三次多项式，需要多少个系数？
4. 单连杆机器人的转动关节，从θ=－5°静止开始运动，要想在4s内使该关节平滑地运动到θ=+80°的位置停止。试按下述要求确定运动轨迹：
(1) 关节运动依三次多项式插值方式规划。
(2) 关节运动按抛物线过渡的线性插值方式规划。
5. 在从t=0到t=2的时间区间使用一条三次多项式： θ(t)=10+5t+70t2－45t3，求其起始点和终点的位置、速度和加速度。
思考题与练习题参考答案
思考题
1.
答： 机器人任务规划的基本要素包括状态空间(State)、时间(Time)、操作状态的动作序列(Actions)、初始和目标状态(Initial and goal states)、标准(A criterion)和运动计划(A plan)。
2.
答： 典型的机器人任务规划系统有STRIPS规划系统、具有学习能力的规划系统和基于专家系统的机器人规划。
3.
答：
运动规划是决定一条从初始位姿到最终位姿的路径，使机器人能沿这条路无碰撞地完成作业任务。这需要为机器人赋予自主规划能力，自主规划需要从用户提供的任务级高层描述和工作空间的几何特征出发。
机器人运动轨迹一般是对其末端执行器位姿变化的描述。控制轨迹也就是按时间控制手部走过的空间路径。在轨迹规划中，也常用点表示机器人在某一时刻的状态，或某一时刻的轨迹，或用它表示末端执行器的位姿，例如起始点、终止点就分别表示末端执行器的起始位姿及终止位姿。
4.
答： 相同点：
单元分解法的思想是将机器人所在环境空间切分为多个简单相连的区域，每个区域单元分为自由的和被物体占用的两种。然后找出起点和目标位置所在单元，并在连接图中用搜索算法找到一条连接起点和目标单元的路径。
不同点：
单元分解法又分为精确单元分解(Exact Cell Decomposition)和近似单元分解(Approximate Cell Decomposition)。单元分解法的一个重要评价标准是对环境划分的完备程度，如果环境分解后是无损的，那么这种划分法就是精确单元分解。如果分解形成实际地图的近似，则称为近似单元分解。
5.
答： 主要应用于虚拟示教、语音交互、视觉交互、脑机交互机、触觉交互、触屏交互等方面，未来还有更多的应用。
6.
答： (本题属于开放题)
伴随着信息物理融合系统、物联网等传感网络技术的不断发展，人—机器人交互将在不久的将来形成集视觉、嗅觉、触觉、推理与情感计算于一体的全方位感知智能时代，而物联网、云计算、大数据处理等新一代信息技术的研究将在不远的未来继续带动信息、生物、医疗等交叉学科的建设，并将人—机器人交互推向心理学、脑科学、认知科学、神经科学与计算技术相融合的灵魂智能时代。
练习题
1.
解： θ¨1=sgn(θ2－θ1)θ¨1=50°/s2
t1=td12－t2d12－2(θ2－θ1)θ¨1=2－4－2(35－10)50=0.27s
θ·12=θ2－θ1td12－12t1=35－102－0.5×0.27=13.5°/s2
θ·23=θ3－θ2td23=25－351=－10°/s2
θ¨2=sgn(θ·23－θ·2)θ¨2=－50°/s2
t2=θ·23－θ·12θ¨2=－10－13.5－50=0.47s
t12=td12－t1－12t2=2－0.27－12×0.47=1.5s
θ¨4=sgn(θ3－θ4)θ¨4=50°/s2
t4=td34－t2d34－2(θ4－θ3)θ¨4=3－9+2(10－25)50=0.102s
θ·34=θ4－θ3td34－12t4=10－253－0.5×0.102=－5.1°/s2
θ¨3=sgn(θ34－θ23)θ¨3=50°/s2
t3=θ·34－θ·23θ¨3=－5.1－(－10)50=0.098s
t23=td23－12t2－12t3=1－12×0.47－12×0.098=0.716s
t34=td34－t4－12t3=3－0.102－12×0.098=2.849s
2.
解： 把θ0和 θf的值代入式(5.38)中
a0=θ0
a1=θ·0
a2=θ¨02
a3=20θf－20θ0－(8θ·f+12θ·0)tf－(3θ¨0－θ¨f)t2f2t3f
a4=30θ0－30θf+(14θ·f+16θ·0)tf+(3θ¨0－2θ¨f)t2t2t4f
a5=12θf－12θ0－(6θ·f+6θ·0)tf+(θ¨0－θ¨f)t2f2t5f
可得五次多项式的系数：
a0=30.0，a1=0.0，a2=2.5，a3=1.6，a4=－0.58，a5=0.0464
确定操作臂为：
θ(t)=30+2.5t2+1.6t3－0.58t4+0.0464t5
θ·(t)=5+4.8t2－2.32t3+0.232t4
θ¨(t)=5+9.6t－6.96t2+0.928t3
3.
答： 该机器人路径可分为q0~qr段和qr~qs段及qs~q5段三段。可通过由三个三次多项式组成的样条函数连接。
设从q0~qr的三次多项式插值函数为： θ1(t)=a10+a11t+a12t2+a13t3
从qr~qs的三项多项式插值函数为： θ2(t)=a20+a21t+a22t2+a23t3
从qs~q5的三项多项式插值函数为： θ3(t)=a30+a31t+a32t2+a33t3
上述三个三次多项式的时间区间分别为［0，tf1］和［0，tf2］及［0，tf3］，若要保证路径点处的速度及加速度均连续，即存在下列约束条件：
θ·1(tf1)=θ·2(0)
θ·2(tf2)=θ·3(0)
θ¨1(tf1)=θ·2(0)
θ¨2(tf2)=θ¨3(0)
根据约束条件建立方程组，为：
θ0=a10
θr=a10+a11tf1+a12t2f2+a13t3f3
θr=a20
θs=a20+a21tf2+a22t2f2+a23t3f2
θs=a30
θ5=a30+a31tf3+a32t2f3+a33t3f3
a11=0
a21+2a22tf2+3a23t2f2=0
a11+2a12tf1+3a13t2f1=a21
a21+2a22tf2+3a23t2f2=a31
2a12+6a13tf1=2a22
2a22+6a23tf2=2a32
显然上述约束条件含有12个未知数的12个线性方程。
综上所求解知： 需要三个独立方程，12个系数。
4.
解： 过渡域： ［t0，tb］。
由于过渡域［t0，tb］终点的速度必须等于线性域的速度，所以
θ·tb=θh－θbth－tb(1)
其中，θb为过渡域终点tb处的关节角度。用θ¨表示过渡域内的加速度，θb的值可按下式求解，θb=θ0+12θ¨t2b(2)
且： t=2th
θ¨t2b－θ¨ttb+(θf－θ0)=0(3)
其中t为运动持续时间，此处为4s。
对于给定的θf=+80°，θ0=－5°，t=4s可根据(3)选择相应的θ¨和tb，得到路径曲线。通常的做法是先选择加速度θ¨的值，然后按(3)算出相应的tb，
tb=t2－θ¨2t2－4θ¨(θf－θ0)2θ¨(4)
为了保证tb有解，过渡域加速度值θ¨必须选得足够大，即
θ¨≥4(θf－θ0)t2(5)
此处不妨取： θ¨=4(θf－θ0)t2=4×［80°－(－5°)］42=21.25。代入(4)可求得：
tb=t2－θ¨2t2－4θ¨(θf－θ0)2θ¨=42－21.252×42－4×21.25×［80－(－5)］2×21.25=2
按照(2)式可求得θb得：
θb=θ0+12θ¨t2b=(－5)+12×21.25×22=37.5°
综上求解可得(1)式：
θ·tb=θh－θbth－tb=θh－37.5°th－2
5.
解： 由已知有下述条件：
时间区域： ［0，2］。
a0=10°，a1=5°，a2=70°，a3=－45°
由条件可得：
a0=θ0=10°
θf=10+5×2+70×22－45×23=－60°
θ·(t)|t=0=5+140t－135t2=5
θ¨(t)|t=0=140－270t=140
θ·(t)|t=2=5+140t－135t2=－255
θ¨(t)|t=2=140－270t=－400
问题得解。