机器人综合设计与实践 课件 第7、8章 机器人路径规划综合设计与实践、机器人轨迹规划综合设计与实践

机器人综合设计与实验07机器人路径规划综合设计与实践合作QQ：243001978合作QQ：243001978contents目录概述01机器人路径规划理论基础02机器人路径规划综合设计037.1概述第5章设计与实践了机器人视觉感知系统；第6章设计与实践了目标识别和障碍物识别，并重构了障碍物的三维空间。在此基础上，本章将设计和实践机器人的路径规划。本章重点解决两个问题：一是在二维空间中，机器人如何移动到目标物的周围；二是在目标物所处的多障碍物三维空间中，机器人末端执行器通过什么样的路径去抓取目标物。7.2机器人路径规划理论基础获取到八叉树地图，将其进行降维操作，成为二维栅格地图，方便机器人进行路径规划。根据机器人是否拥有地图，可以将路径规划概括分为两大类：基于地图的路径规划和无地图的路径规划。其中基于地图的路径规划需要先验知识，而后者是指移动机器人识别并跟踪某个目标物，从而解决各种特定目的的导航问题。两种路径规划方式又分为了若干子类，具体如图7-1所示。图7-1路径规划分类7.2机器人路径规划理论基础下面将介绍图7-1中的3种路径规划方式。（1）人工势场法。人工势场法是在二维（2D）栅格地图上实现的，采用了虚拟力场法（VirtualForceField）。假设在栅格地图中，包含障碍物的栅格上面存在着排斥移动机器人前进的虚拟力，这个力用于排斥机器人以绕开障碍物；包含目标点的栅格上面存在着吸引移动机器人前进的虚拟力，这个力用于吸引机器人朝着目标点位置移动。所有栅格中排斥力和吸引力均以向量的形式表现，合力按照向量的和计算，用于推动移动机器人的运动。将排斥力和吸引力组合构成一个虚拟力势场。然而，人工势场法存在着搜索时发生“局部最小”的情况，即机器人会因为在势场中由于合力等于零而出现不运动的情况。为了解决这个问题，需要引入更多的参数。7.2机器人路径规划理论基础（2）智能规划方法。智能规划方法包括模糊逻辑控制法、蚁群算法、粒子群算法、遗传算法、神经网络算法等新兴的智能技术。模糊逻辑控制法采用一种近似自然语言的方式，将目前已知的环境障碍物信息作为模糊推理的输入量，通过模糊推理的方式来输出移动机器人的运动方式；蚁群算法是通过模拟蚂蚁寻找食物的最短路径行为设计的一种仿生学算法；粒子群算法也称为鸟群觅食法，从随机解出发，通过迭代的方式寻求最佳解，这种算法搜索速度快，精度较高；遗传算法采用多点式搜索方法，能够极大可能地搜索到全局最优解。7.2机器人路径规划理论基础（3）启发式算法。启发式算法以A*算法为代表，其为Dijkstra算法的改进体。随后有学者也提出了D*算法，D*算法主要利用了A*算法使用启发式评价函数这个优点，能够解决动态最短路径问题。这两种算法都能够根据机器人在运动过程中获取的环境信息快速修正和重新规划出一条最优的路径，能够有效地减少局部规划时间，满足实时导航规划的要求。此外，许多研究工作者通过修改A*算法的启发式评价函数和图搜索的方向，能够有效地提高A*算法的路径规划速度，对复杂环境具有一定的适应能力。各种算法优缺点并存，结合本机器人的软件平台ROS，在ROS导航开发包中，提供了两种基于栅格地图的路径规划算法，其中包括了A*算法，可直接嵌套使用，实现较为容易。故本书选用A*算法用于机器人路径规划综合设计。7.3二维空间全局路径规划将已知的三维地图降维成二维栅格地图，在这种环境地图已经完全知道的情况下进行路径规划，也被称为全局路径规划问题。全局规划的准确性是由环境地图的精准度来决定的。本书设定二维栅格地图的分辨率为1cm，即每个栅格的宽度代表实际场景中1cm的距离，对于本书的路径规划来说，已经足够使用。下面将详细介绍A*算法的原理及相关改进。1．A*算法A*算法在1968年被Hart、Nilsson和Raphael共同提出，它是一种较为有效地求解最优路径的直接搜索算法。它通过结合BSF算法和Dijkstra算法，引入了启发式评价函数，避免了盲目搜索，提高了搜索效率，大幅度地提高了路径规划准确性。A*算法包括从当前节点到目标节点的成本估计，以及从出发节点到当前节点的实际花费代价。采用数学模型来表达某条最优路径的估算函数，如式（7-1）所示。7.3.1二维空间全局路径规划7.3二维空间全局路径规划

（7-1）

7.3二维空间全局路径规划

图7-2A*算法的搜索过程A*算法的搜索过程如图7-2所示。在整个搜索过程中，A*算法主要利用了两个集合：close_set和open_set。open_set中主要存放没有检测过的点，而close_set中主要存放已经检测过的点。open_set中的元素按照f值升序排列，每次循环搜索时，都先从open_set中选取f值最小的节点进行检测，将检测出的点放入close_set中，同时计算与该点相邻的节点的f、g、h。当open_set中节点为空时或者搜索到目标节点时，停止搜索。7.3二维空间全局路径规划以一个5*5大小的网格为例，假设其出发点为S，终点为E，水平和竖直方向上的移动代价为10，且只能沿着水平或者竖直方向移动，那么整个搜索过程如图7-3所示。图7-3中，黑色栅格代表障碍物；红色箭头代表最终获得的最优路径；每个小格子上面的数值表示如下：右上角数值代表g值，左下角数值代表h值，右下角数值代表f值。图7-3A*算法搜索示意图7.3二维空间全局路径规划2．A*算法的改进传统的A*算法是一个单向搜索递进的过程，当场景较为简单时，A*算法能够在较短的时间内寻找到一条最优路径。但是，由于实际场景构建出来的地图往往空间较大，地图的状况也较为复杂，这样就会增加路径搜索的时间。针对这一问题，本书对A*算法进行改进，采用双向搜索的方式来提高搜索效率（只将时间最优作为约束条件）。A*算法是从出发点到目标点的单向搜索。而所谓的双向搜索，就是从出发点和目标点同时开始向着对方搜索，构建两个单向搜索。将出发点到目标点的单向搜索看作正向搜索，目标点到出发点的单向搜索看作反向搜索。正向搜索时，将反向搜索到的当前最优节点看作目标点进行搜索；反向搜索时，将正向搜索到的当前最优节点看作目标点进行搜索，两种搜索交叉进行，直到两种搜索的目标点为同一个目标点，搜索结束。7.3二维空间全局路径规划

7.3二维空间全局路径规划②将节点n存放到open_set1中。（3）判断open_set1是否为空集合，若是，则搜索失败，同时终止搜索；若不是，则将open_set1中的节点按照f1值升序排列，从open_set1中选取f1值最小的节点进行检测，将检测出的点放入close_set1中。如果该点也在close_set2中，那么终止搜索，跳到第5步，否则进行下一步。（4）按照第2、3步进行反向搜索，将对应集合和代价函数进行相应的变换。（5）根据估计函数，分别计算两种搜索方式的代价，同时判断两个close集合中是否有相交节点N（出发点和目标点不计算在内），若有，则以节点N为中间连接点，分别回溯正向搜索和反向搜索，计算从出发点到节点N和目标点到节点N的两个代价，将二者相加得到总的代价值；若没有，则搜索失败，采用单项A*算法进行搜索。7.3二维空间全局路径规划在不同大小的栅格地图中，对传统的A*算法和双向A*算法进行多组仿真实验，其中一组仿真结果如图7-4所示。图7-4用不同的颜色代表搜索的各个阶段。其中，蓝色格子代表出发点；青色格子代表目标点；白色格子代表空白区域，即在栅格中没有任何障碍物，同时是未搜索的区域；黑色格子代表存在障碍物；玫红色格子代表在下一次搜索时的节点；红色格子代表正向搜索过程中，已经检索过的节点，但不是最优节点；黄色格子代表反向搜索过程中，已经检索过的节点，但不是最优节点；绿色格子代表出发点到目标点的搜索路径。对5组不同实验进行数据记录，得到的结果如表7-1所示。图7-4两种搜索方法仿真图7.3二维空间全局路径规划图7-4两种搜索方法仿真图7.3二维空间全局路径规划表7-1

搜索时间对比由表7-1可以明显看到，利用双向搜索法进行路径规划，可以降低约40%的消耗时间，对于场景较大的环境，会极大地缩减机器人整体的路径规划时间。7.3二维空间全局路径规划7.3.2三维空间内路径规划在高维空间中，针对机器人的路径规划问题，目前常用的解决方案是RRT（Rapidly-exploringRandomTree，快速扩展随机树）算法。该算法是由Lavalle于1998年提出的。它利用树形结构来代替具有方向的图结构；对状态空间的随机采样进行碰撞检测；通过采样和扩展树节点的方式最终获取到一条从起点到终点的有效路径。同时RRT算法是一种概率完备的搜索算法，即在自由空间中，只要存在可以指向起点到终点的路径，同时给足采样点，RRT算法就一定能够寻找到一条可通过的路径。该算法不需要对空间结构进行精确建模，计算量小。截至目前，经过众多学者改进的RRT算法已经被广泛应用到机器人的路径规划问题中。在本节中，只针对仿人形移动机器人的作业臂末端执行器进行三维路径规划。7.3二维空间全局路径规划

（7-2）图7-5RRT算法示意图

（7-3）7.3二维空间全局路径规划

图7-5RRT算法示意图（7-3）7.3二维空间全局路径规划RRT算法的伪代码如图7−6所示。图7-6RRT算法的伪代码7.3二维空间全局路径规划上述伪代码中引入了若干函数，说明如下。（1）RandomSample函数，它是随机采样函数，即所有的采样点均服从均匀分布。

（7-4）（2）SearchNearest函数，它是寻找最近点的函数，具体如式（7-5）所示。（3）ExtendTree函数，它是扩展随机树的函数，扩展公式如式（7-6）所示。（7-5）（7-6）7.3二维空间全局路径规划（4）CollisionCheck函数，它是碰撞检测函数，是一个布尔类型的函数。对于节点Xnearest到Xnew之间的所有点，都表示为（7-7）对RRT算法进行仿真，结果如图7-7所示。图7-7RRT二维仿真图7.3二维空间全局路径规划图7-7中，红色点代表搜索的起始点；绿色点代表目标点；黑色点代表搜索过程中新增加的节点；蓝色线代表树节点之间的联系；绿色折线代表最终搜索得到的路径。在仿真过程中，发现RRT算法存在一些缺陷。（1）不同的采样策略会影响整体的收敛速度。（2）邻近空间的范围选择会对算法的精确度有明显的影响。（3）如果空间中障碍物信息过多，会导致求解速度过慢。（4）随机算法导致规划出来的路径较为曲折。图7-7RRT二维仿真图7.3二维空间全局路径规划2．RRT*算法由于RRT算法在复杂环境中，路径搜索效率较低，故Lavalle于2001年提出了基于P概率的采样模式，通过概率值来决定采样空间内随机点的选取，提高了算法搜索的效率。同时，他与Kuff教授提出了双向搜索的策略来提高算法的效率，其基本思想类似于上文中提到的双向A*搜索法，即同时从起始点和目标点产生两棵树，双向依次搜索，交替进行，这样使得搜索的效率得到了极大的提升。而本书中，主要针对作业臂手爪进行三维空间路径规划，在保证安全操作的情况下，更多要求是需要一条最优的路径（本书主要考虑消耗最小和路径最短）。RRT*算法是Karaman在2011年提出的，将代价函数引入RRT算法中，通过多次迭代来优化之前的路径。RRT*算法在保留RRT算法优点的同时，还能够通过代价函数的引入使得RRT*算法既能快速有效地实现路径规划，又能得到一条满足条件的最优路径。7.3二维空间全局路径规划

7.3二维空间全局路径规划

7.3二维空间全局路径规划

图7-8RRT*算法示意图7.3二维空间全局路径规划与RRT算法相比，RRT*算法实际上就是增加了对代价函数的判断，以及重构树的过程，RRT*算法的伪代码如图7-9所示。图7-9RRT*算法的伪代码7.3二维空间全局路径规划3．RRT*算法改进RRT算法和RRT*算法能够初步寻找到一条路径，但是还不能满足本书的使用。结合本书中作业臂运动时的状态，对路径有如下需求：①路径应尽可能短，通常越短的路径，抓取时对作业臂的规划要求越少；②路径转折点越少越好，能够使得作业臂的运动变得简单化。故对RRT*算法提出了改进算法。以二维空间为例，说明角度代价的来源，如图7-10所示。图7-10RRT*代价函数示意图7.3二维空间全局路径规划（1）重新构造代价函数Cost_function，该代价函数由两部分构成，计算公式如式（7-8）所示。（7-8）

（7-9）此时通过判断total_angle_cost1和total_angle_cost2，可以得到一条较为“笔直”的路径。7.3二维空间全局路径规划（2）对障碍物进行膨胀处理。为了降低机器人碰撞的风险，对障碍物进行膨胀处理，这样虽然牺牲一部分作业臂的工作空间，却降低了机器人的操作危险程度。对RRT*算法进行二维仿真，结果如图7-11所示。图7-11RRT*二维仿真图图7-11中，红色点代表搜索的起始点；绿色点代表目标点；黑色点代表树中的节点；蓝色线段代表Xnew与Xnearest之间的连线；红色线段代表在通过代价函数判断后，Xnearest与Xmin之间的连线；绿色折线代表最终搜索得到的路径。可以看到，由RRT*算法搜索出来的路径明显比RRT算法搜索出来的路径更加“笔直”。第七章机器人路径规划综合设计与实践习题7.1针对一个四轮车，采用避障技术，对每种选择，阐述它的优点和缺点。7.2考虑一个机器人，设计并实践其路径规划算法和避障算法。7.3论述二维空间全局路径规划与局部路径规划的异同点。7.4论述三维空间全局路径规划与局部路径规划的异同点。7.5简述二维路径规划和三维路径规划的异同点。7.6设计并实践一个6自由度串联型机器人的三维路径规划算法。机器人综合设计与实验08机器人轨迹规划综合设计与实践合作QQ：243001978合作QQ：243001978contents目录概述01机器人轨迹规划理论基础02机器人轨迹规划综合设计031.概述第7章介绍了机器人的路径规划，就是规划机器人末端执行器从其初始位姿避开障碍物运动到指定的目标位姿，与时间无关，即机器人末端执行器的运动路径。机器人的轨迹规划，是在机器人末端执行器沿着其规划的路径运动过程中，对作业臂各个关节的位置、速度、加速度等运动参数相对于时间的变化关系进行规划，确保机器人末端执行器及其他关节能够平稳地运动到目标点。本书中已有的末端执行器空间路径，只考虑了末端执行器的无碰撞条件，并没有考虑作业臂各个连杆在末端执行器运动过程中能否避开障碍物。在此基础上，本书根据末端执行器的无碰撞路径对机器人进行避障规划并确定出最终的运动路径。同时，为了控制末端执行器从其对应的初始位姿运动到目标位姿，对机器人平台双臂进行轨迹规划，期望机器人能够稳、准、快地完成其空间运动。2机器人轨迹规划理论基础传统意义的关节式避障，是基于W空间（WorkSpace）假设-修正的避障方法，是早期提出的机器人避障方法。其基本思想是，先假设一条从初始点到目标点的路径，再进行作业臂与障碍物的碰撞检测；如果发生了碰撞，就根据障碍物的信息对路径进行修改；在已经修改过的路径上继续重复前述步骤，直到找到一条完整的无碰撞路径再停止。20世纪80年代，有学者提出了基于C空间（ConfigurationSpace）的避障方法，也是目前比较受欢迎的一种方法。该方法中，建立的C空间是将作业臂各个关节中心轴作为坐标系，并把周围环境中的障碍物映射到空间中，形成障碍空间。所以，C空间是由障碍空间和自由空间组成的，除去障碍空间，余下的就是自由空间。在自由空间中的各个点代表作业臂不与障碍物发生碰撞，此时的避障问题就转变为在C空间中避开障碍空间寻找一条无碰撞自由路径。2机器人轨迹规划理论基础传统的W空间避障法虽然简单，但是当环境信息比较复杂时，该方法就需要不断去进行碰撞检测，实时性较差。而基于C空间的避障规划的缺点是，C空间的维数为机器人关节数，机器人的关节越多，计算过程越复杂。根据上述所讲内容，本书作业臂的避障规划实际上是对作业臂各个关节在W空间中的避障规划，却又与传统意义上W空间中关节式作业臂的避障规划不同。在本书中，由于得到的路径只是关于主作业臂末端执行器的无碰撞路径，并没有对主作业臂各个杆件进行避障规划。因此，本书根据已知的无碰撞路径，在W空间中首先确定出末端执行器到达目标点时主作业臂各个杆件的最终构型，然后规划主作业臂沿着最终构型移动到目标点，确保作业臂在运动的整个过程中不与障碍物产生碰撞。2机器人轨迹规划理论基础在作业臂的作业过程中，依据在工作空间中已规划好的无碰撞路径，期望作业臂能够跟踪路径完成抓取任务，即在作业臂跟踪无碰撞路径的运动过程中，当末端执行器最终到达目标点抓取到目标物时，期望作业臂的构型最大限度上可以与规划路径的轨迹是重合的。但是，规划出的已知末端执行器避障路径是由空间中的众多离散点拟合成的一条曲线，故末端执行器抓取到目标物后作业臂的构型是不可能完全与已知路径重合的。因此，确定出了作业臂末端执行器到达目标点的最终构型，即确定出了作业臂最终的跟踪路径。3.机器人轨迹规划综合设计8.3.1关节空间下的轨迹规划由于关节空间下的轨迹规划没有限定机器人末端执行器的路径，所以其一般使用在点到点的任务中。关节空间下的轨迹规划：首先，需要确定出机器人末端执行器的工作空间下的初始位姿及期望达到的目标位姿；其次，经过逆运动学的分析，求解出机器人末端执行器达到最终目标位姿时各个关节所需转动的角度，再对各个关节的转角随时间的变化来进行规划。关节空间下的轨迹规划一般采用三次多项式插值法、高阶多项式插值法等多项式函数，来描述作业臂关节随时间运动而产生的轨迹。3.机器人轨迹规划综合设计

（8-1）

3.机器人轨迹规划综合设计

（8-2）（8-3）

3.机器人轨迹规划综合设计

（8-4）（8-5）3.机器人轨迹规划综合设计

（8-6）3.机器人轨迹规划综合设计

（8-7）3.机器人轨迹规划综合设计则可求解得到：将通过式（8-8）得到的系数代入式（8-6）中，就可得到五次多项式插值函数的关系式。3.机器人轨迹规划综合设计8.3.2工作空间下的轨迹规划对于关节空间下的轨迹规划，并不能将作业臂末端执行器的路径轨迹描述出来。当要求末端执行器沿着给定的路径运动时，就需要在工作空间下对末端执行器的路径轨迹进行规划。对于工作空间下的轨迹规划，要求作业臂的末端执行器稳、准、快地沿着已规划路径运动。根据末端执行器在工作空间下的起始位置和目标位置，可以利用直线段和圆弧对位置路径进行拟合。所以，工作空间中的轨迹规划为对该空间中的直线段和圆弧进行轨迹规划。而对于空间中直线段和圆弧规划的主要思想，即对拟合好的路径进行离散化得到末端执行器的位置状态，再结合每个位置状态规划完成的姿态信息，即组合为末端执行器的一系列位姿信息。通过逆运动学的分析求解出每个位姿状态对应的作业臂的各个关节角度值。本节中，对末端执行器的位姿分为位置和姿态进行规划，即在工作空间中对直线段和圆弧插补时对末端执行器的位置路径进行规划；将末端执行器的姿态转换为四元数再对其进行规划。3.机器人轨迹规划综合设计

图8-1空间中末端执行器的起始点和目标点3.机器人轨迹规划综合设计根据空间中的两点可以确定出该直线段的函数方程，如式（8-9）所示。（8-9）

（8-10）

（8-11）3.机器人轨迹规划综合设计

，

（8-12）（8-13）3.机器人轨迹规划综合设计

3.机器人轨迹规划综合设计

（8-14）故可得到平面M的方程，如式（8-15）所示。3.机器人轨迹规划综合设计平面M为由A、B、C三个点确定的平面。为了确定由这三个点确定的圆弧的圆心，分别过线段AB、BC的中点作该直线段的垂直平分面T、S，如图8-2所示。（8-15）图8-2空间平面关系图3.机器人轨迹规划综合设计

（8-16）（8-17）

（8-18）3.机器人轨迹规划综合设计2）坐标变换根据上述确定出的三个平面，建立新的空间坐标系O-UVW，如图8-3所