毕业论文-救援机器人自动路径规划设计.docx

毕业论文救援机器人自动路径规划设计学生姓名： 学号： 系 部： 自动化 专 业： 自动化 指导教师： 二一六年六月诚信声明 本人郑重声明：本论文及其研究工作是本人在指导教师的指导下独立完成的，在完成论文时所利用的一切资料均已在参考文献中列出。 本人签名： 年 月 日 太原工业学院毕业设计救援机器人自动路径规划设计摘 要 随着科学技术的发展进步，机器人在人类生活中扮演的角色也越来越重要，救援机器人也逐渐出现在这个舞台上，对于救援机器人来讲，路径规划成为其主要组成研究方向之一。本设计分析当前主流路径规划算法，如Dijkstra算法、BFS算法、A*算法，比较他们的优劣以及应用场合，并着重分析了A*算法的原理，运算流程。基于A*算法的原理，利用MATLAB的GUI技术进行界面设计及代码编写，使用户可以自定义地图信息、机器人起点终点信息以及障碍物信息，最终实现了路径规划可视化及路径坐标点的输出。 此外，基于平面全向移动平台的硬件和机器人自身运动避障算法的支持，将路径规划所得坐标点下载到机器人控制芯片中，实现了机器人的真实行走轨迹贴合规划所得到的轨迹，并能够自动避开障碍物。机器人的运动采用闭环控制的方式，控制更精确。关键词：路径规划，A*算法，MATLAB仿真，全向移动平台Design of Rescue-Robots Automatic Path PlanAbstract With the development of scientific and technological progress, the robot has become increasingly important rolein human life, rescue robots are also emerging in this arena, as for the rescue robot, path planning research has become one of its main components. The design and analysis of the current mainstream path planning algorithm, such as Dijkstra algorithm, BFS algorithm, A * algorithm, compare their advantages and disadvantages and applications, and analyzes the A * algorithm principle, the operational flow. Based on the principle of the A * algorithm, using MATLABs GUI interface design and technical writing code that allows users to customize the map information, the robot starts and ends information and obstacle information, and ultimately the visualization and path planning path coordinate point output. In addition, based on planar omnidirectional mobile platform hardware and robot itself sports avoidance algorithm support, the path resulting planning coordinate point is downloaded to the robot controller chip, to achieve a true walking robot trajectory bonding plan obtained by the track and be able to automatically avoid obstacles. Movement of the robot is using closed-loop control mode, control is more precise.Keywords: Path-Plan, A* algorithms, MATLAB Simulation, mobile platform 目 录第1章 前言11.1 课题背景和研究意义11.2 国内外研究现状11.2.1 国内研究现状11.2.2 国外研究现状2第2章 常用路径规划算法42.1 Dijkstra算法42.2 BFS算法62.3 A*算法9第3章 救援机器人应用需求分析123.1 救援机器人布防位置及环境分析123.2 算法比较选择13第4章 A*算法详解及仿真144.1 A*算法原理详析144.2 A*算法仿真194.2.1 可视化用户界面思想194.2.2 回调函数及调用逻辑分析214.2.2 仿真分析24第5章 路径规划算法应用分析265.1 救援机器人硬件平台结构265.2 救援机器人运动学和动力学分析265.2.1 运动学分析265.2.2动力学分析285.2.3 结论305.3 机器人全场定位算法分析305.3.1 确定码盘交点坐标315.3.2 码盘交点不在车中心的情况335.3.3 码盘夹角不是90的情况345.4 机器人闭环控制355.4.1 PID原理365.4.2 比例控制器365.4.3 积分控制器375.4.4 微分控制器375.4.5 PID离散化之后的计算形式37第6章 救援机器人引导算法分析386.1 引导算法需求分析386.2 引导算法处理流程386.3 基于变化地图的引导算法改进41致 谢45附 录4648第1章 前言1.1 课题背景和研究意义随着时代的发展，各种灾害的发生，例如：地震，火灾，水灾等无时无刻不在影响着人们生活，人们也开始着手于有效的避免灾害的发生和在灾害发生时的有效的救援，而一套高效的救援路径，即高效的路径规划算法可以快速、稳定的解决这一难题。在1967年提出了遗传算法，它是针对可移动的机器人研究的算法，它采用的是概率化的研究的算法，它可以自动调整路径，其不足之处是在于未知的环境下，规划路径的速度比较慢，不能满足高效的救援的目标；而后提出的C空间自由法，其是先设定一条规划的路径，然后在这条路径的点上寻找修改，重复以上的步骤，直到找到一条合适的路径，完成规划，但是它运行状态不稳定，实时性差，无法按预定的指标完成设计需求；在1986年提出的人工势场法，机器人的运动主要是设计成一种抽象的人造的引力场的运动，利用的是斥力和引力的关系实现的算法研究，它是光滑和安全可靠的算法的研究，但是其存在局部最优的问题；1991年提出了蚁群的算法，它是利用大量的环境模型，利用的是大量的蚂蚁，寻找无用栅格，将其删除，实现最优的算法规划；随之提出的栅格法，它是将机器人的运行的环境分割成无数的小的栅格，并对小栅格进行障碍区的分析，并对其进行不同的标示，使得机器人更快速的行走。因此为了解决在复杂的情况下不同地理位置的障碍物，救援现场的动态变化的不稳定性等的一系列的不利救援的因素。一个可以立即展开搜索，加大空间搜索，在复杂的环境中能根据环境的动态变化的算法的提出迫不及待。机器人的路径规划的研究是从20世纪的60年代开始的，最近几年，有关机器人的路径规划，在研究的范围和层次上都得到了丰硕的成果，已经初步形成了理论、应用和算法的多方面的研究，目前已经涌现了大量的路径规划的方法。1.2 国内外研究现状1.2.1 国内研究现状中国的机器人的路径规划起步比较晚，但是中国关于机器人的路径规划在世界的研究领域也处于比较先进的地步。20世纪80年代初，中国的学者提出用状态空间的连通性来解决路径规划的问题，它是根据环境和运动物体的几何特点，将其运动的空间划分成若干个拓扑特征一致的子区域，而后根据其连通性建立一张网，在该图中搜索一条拓扑路径，缩小了搜索的空间；李彩虹等人研究出了一种在环境未知下应用于移动的机器人的模糊处理的算法，庄晓东等人研究出了一种在动态的环境中基于模糊概念的机器人路径搜索算法，由于其是在不确定的动态的环境中，所以实现起来比较复杂1；禹建丽等提出了基于神经网络的机器人的路径规划，研究了障碍物形状和位置已知情况下的机器人路径规划算法，其能量函数的定义利用了神经网络结构，规划出的路径达到了折线形的最短无碰路径，该方法计算简单、收敛速度快2；在对栅格法的改进中，中国的学者提出了四叉树分割的方法，该算法递归地把环境分解为大小不一的矩形区域，这些矩形区域或者完全被障碍物占据，或者是完全自由可行的。2005年王力虎等研究出了一种拓扑算法，适用于清扫机器人，它是要机器人完成在区域内的充满或是覆盖，用机器人身上携带的传感器来感知环境信息以此来建立环境的拓扑地图，这样机器人利用搜索图的方法搜索环境，以达到环境的有效覆盖，这种算法没有具体的体现覆盖率的问题和节点之间的距离的问题，在搜索的效率不是很高，不能满足在实际上的应用。在2006年提出了一种基于扫描法的构造环境拓扑的方法，实现了对所构建的拓扑图的扩展，其采用的是逐步构建环境拓扑图的方法，可以做到在线构建，适用于任何的环境，但是其计算的发杂程度比较低，不能保证找到的路径是最优路径；在混合的路径规划中，中国的研究人员也提出了一种比较先进的算法，它是对在移动的机器人进行静态的路径规划中提出的神经网络的算法，它结合了遗传算法和模拟退火的算法，采用神经网络的模型表示方法，在编码中把物碰撞作为约束的条件，适应度函数的选择条件是最短的路径，提高了路径规划的质量3。1.2.2 国外研究现状1967年美国的两位学者首先提出了遗传算法，其是直接对可移动的机器人路径进行操作的，采用了概率化的寻优方法，它可以自适应地调整搜索的方向，易于并行化，但在为未知的复杂的情况下，其路径规划的速度忙，运行时占据了很大的储存的空间。1968年Pieper研究出的C空间自由空间法，这种算法是最早的机器人的避障的方法，它是由三个步骤完成：1.现在初始点和目标点之间假设一条路径，2.为了躲避有可能发生的碰撞，检验路径上的一组选定的点，3.如果发生碰撞，就修改这条路径，知道修改到无碰撞的路径规划，这种方法比较简单，实时性差，无法按指标来进行规划。在后来的十几年中，C空间的自由空间法有了很好的发展，麻省理工智能实验室提出的基于C空间自由空间法得到了国外的很多的青睐，它是根据运动对象的的大小和姿态，将已知的障碍扩展，变换为扩展障碍，运动对象被缩成一个点，使得原来空间中的运动对象的无碰撞的路径规划变成构型空间中的求质点的轨迹运动问题，简化了路径规划的复杂性，而后与之相关的拓扑法也应运而生了。拓扑法主要包括三部分：基本上可以划分状态空间，构建特征网，在特征网上搜索路径。拓扑法的最主要是节点和边，其节点表示某个特定的位置，用边表示这些位置之间的联系，利用该方法可缩小搜索空间，使得存储需求小，适合于大规模环境的路径规划，但是构建特征网的过程比较复杂，而且当障碍物增加时如何将增加的节点与已有节点进行节点匹配是一个难点。在1986年Khatib提出了人工势场法，将机器人的运动主要是设计成一种抽象的人造的引力场的运动，目标点对移动的机器人产生了较大的引力，而障碍物对移动的机器人产生了较大的斥力，通过引力和斥力来控制机器人的运动，其规划处的路径一般是安全且光滑的，但是有局部的最优的问题。主要是由于在靠近目标时，其斥力就会很小，而引力会很大，如果在障碍物和目标物很近时，就会造成机器人不能靠近目标物；在1991年Dorigo M提出了蚁群算法，它的设计的原理是：1.建立环境模型，2.建立巢穴临近区和实物气味区，3.在巢穴临近区放置数量足够大的蚂蚁，4.蚂蚁选择其行走的栅格，5.将产生的无效的栅格删除，6.调整路径，找到最优的路径。栅格法是当前应用范围最广的路径规划方法之一。它采用的方法是把移动机器人在复杂的运动地域分解为多个简单的方格，通过判断方格上是否存在障碍物进行状态分析，采取的是障碍物方格和非障碍物方格在同一地图中具有不同的标识值，使得人机器人在行走中能更好的识别自己的行走的路径。第2章 常用路径规划算法2.1 Dijkstra算法 1959年，荷兰计算机科学方面专家Dijkstra研究出Dijkstra算法，该算法是用于计算从一个起始节点到其余节点之间最优（代价最小）路径的算法，解决的是有向图中最优（代价最小）路径问题。 Dijkstra算法用于查找最优（代价最小）路径，该算法被用来计算起始节点到所有目标节点之间的最优路径。主要特点是：从起点开始向周围层层辐射，直到辐射到终点为止，如图2.1所示，起点为中央粉色点，终点为右下角紫色点，从中间想周围辐射，颜色由深到浅表示遍历节点的先后。其算法模拟图如2.1所示：图2.1 简单情况下的Dijkstra算法运行结果该算法能够得出最短路径的最优解，但是由于需要遍历计算的节点数量大，算法需要遍历几乎从起点辐射到终点之间的所有点，一旦地图面积较大或者分辨率较高的时候，会直接导致算法需要处理的节点数急剧增长，计算量随之增长，所以效率非常低。遇到诸如凹型障碍物时，Dijkstra算法能表现出其优良性，能够找到一条令人满意的路径来，如图2.2所示。图2.2 复杂情况下的Dijkstra算法运行结果Dijkstra算法详细运算步骤如下： (1) 令图G=V,E (2) 初始时令S=V0，T=V-S=其余节点，T中顶点对应的距离值，若存在，Di为弧上的权值，如果不存在弧，则Di为无穷大。 (3) 在T中进行搜索，寻找出与S中节点有相关弧的节点集合，并在这个节点集合中找到权值为最小的节点M，放置到S中。 (4) 操作T中剩余的节点，对其距离值进行如下操作：如果添加节点W作为连接节点后，从节点V0到节点Vi的距离变小，那么修改这个距离的大小，重复以上操作(3)、(4)，如果S中包含所有节点，也就是W=Vi停止。 基于以上步骤可绘制如图2.3所示算法流程图。图2.3 Dijkstra算法流程2.2 BFS算法 BFS算法全称为Best-First-Search，该算法运行流程和Dijkstra算法类似，不同的是它能够评估当前节点到目标点的代价。算法处理过程中，总是寻找距离目标节点最近的节点进行处理。他的运行速度要比Dijkstra算法快得多，因为他调用了一个启发函数，这个函数快速的引导他找到目标点。例如，目标点位于起始点的下方，则BFS将优先搜索趋向下方的节点。如图2.4所示，起点为左上红色点，终点为右下灰色点，随着颜色变深表示到终点的代价越来越小。同Dijkstra算法相比，BFS算法极大的减少了搜索点的数量，在简单情况下，能用最快的速度找到通往终点的路径。简单情况下的BFS算法图如图2.4所示：图2.4 简单情况下的BFS算法运行结果但是，在出现诸如凹型障碍物这一类情况时，虽然搜索速度快，但是此时则能表现出算法的劣处，即不能找到一条最优路径，如图2.5所示。图2.5 复杂情况下的BFS算法运行结果BFS算法详细运算步骤如下： (1) 把起始节点S放入开启列表中，计算从当前点到目标点F的距离H (2) 如果开启列表为空表，则无解，退出。 (3) 重复以下步骤： 从开启列表中进行搜索，比较各个节点的H值，提取该值最小的节点进行处理，如果处理结果中存在H值相同的节点,当其中有终点时,选择那个点,否则任意选择结果中的一个节点进行处理。 处理点N周围点，若当前被处理的节点不被包含在开启列表和关闭列表中,则把该节点添加到开启列表,并记录其父节点为节点N，以便求解最优路径。若被处理节点已经被包含于开启列表或关闭列表。则比较刚计算的H值和旧的H值,若新值小，则以新值取代旧值，并维护节点父节点的指向，若被包含于关闭列表，则把其从关闭列表中移动到开启列表。把节点N从开启列表中移去,加入关闭列表中当目标点F加入关闭列表中时停止 (4)保存路径，从目标点出发，按照父节点指针遍历，直到找到起点。基于以上步骤可绘制算法流程图如图2.6所示：图2.6 BFS算法流程2.3 A*算法 本设计需要解决的问题是将救援机器人从布防点运动到需要被救援的地点，路径规划的目的就是在这两点之间找到一个相对更优的路径，这里的“更优”既要做到绕开地上摆放的物品，还要保证路径尽可能短。一种极端的情况是：当机器人开始移动时，一个复杂而完善的路径搜索器外加一个简单的运动算法可以找到一条路径，机器人将会完全沿着该路径进行移动而忽略其他一切。另一种极端的情况是：一个单纯的运动系统不会规划路径，取而代之的是在每个节点处仅仅采取一个步骤，同时考虑周围的环境。同时使用路径搜索和运动算法将会得到最好的效果。 同其他的图搜索算法一样，A*潜在地搜索地图中很大一块区域，同Dijkstra算法一样，A*可以用于搜索最短路径，同BFS算法一样，A*可以使用启发式函数引导自己。在简单的情况中，A*算法和BFS算法一样快，如图2.7所示。图2.7 简单情况下的A*算法运行结果 在出现凹型障碍物时，A*算法能够找到一条同Dijkstra算法同样优的路径，如图2.8。原因是A*算法将Dijkstra算法和BFS算法的信息结合起来。图2.8 复杂情况下的A*算法运行结果第3章 救援机器人应用需求分析3.1 救援机器人布防位置及环境分析 本设计救援机器人面向的对象主要为大型商场，而现如今，商场构造较为复杂，如图所示3.1所示。图3.1 商场平面布局 救援机器人的布防需要慎重考虑，由于在发生火灾等灾难时，靠近出口的人们往往能够实现自救，因此救援机器人没必要被安放在靠近出口或安全通道的地方，除此之外，为了安放方便以及数量的需要，需要机器人既要能够迅速到达指定地点又要保证安放数量尽可能少。因此，就上述地图，救援机器人应该被安放在两安全通道之间，共安放四个机器人，如图3.2所示。图3.2 商场平面安防布局 安放方法可以采用立柜式，即将机器人放置于立柜中，一旦发生报警信号，自动打开立柜锁，机器人自动开始行动。报警信号来自人工，发生危险时，需要救援的人员按动自己附近的报警按钮，这样机器人就能够知道目标点的位置，就可以去目标点了。报警按钮安放数量可以尽量多一些。如上述，假设在如图3.3所示的地方发生警情，离该点最近的机器人A随即开始动作，自动规划路径。图3.3 商场平面路径规划预期3.2 算法比较选择 本设计所研究的救援机器人用于大型商场，商场内情况较为复杂，存在着许多机器人不能通过的地点，记为障碍物。算法需要做的就是绕过这些障碍物找到一条最短路径。 通过对BFS算法、Dijkstra算法和A*的研究，清楚了三种算法的特点。BFS算法虽然搜索速度快，但是在障碍物形状较为复杂时，不能找出一条令人满意的路径来，因此常用于障碍物较为简单的情况。并不适用于救援机器人的路径规划。Dijkstra算法相对BFS算法运行速度要慢很多，虽然搜索出来的路径还是可以让人满意的。对于大型商场来说，面积较大，会导致算法需要处理的点急剧增长，同时路径规划的时间就会非常久，不能满足救援机器人及时性的要求，因此也不适合作为救援机器人路径规划的算法。A*算法融合了Dijkstra算法和BFS算法的长处，既能满足及时性的要求，又能寻找到一条最优路径，因此适合做救援机器人的路径规划算法。第4章 A*算法详解及仿真4.1 A*算法原理详析 在A*算法的表示方法中，g(n)表示从初始点到任意节点N的距离，h(n)表示从节点N到目标点的评估代价，启发式函数f(n)则表示从当前节点移动到目标点所花费的代价，在这里，通常使用Manhattan算法作为评估函数。 开始的时候，为了便于讨论，将整个地图的平面分割为网络，进行完这一项操作，整个搜索的地图即被转变为一个平面直角坐标系，坐标系中的每一个节点为被划分后的地图中的一个方格。称每个方格的中心点为“节点”，每个节点都有自己独一无二的位置坐标，这样，一旦路径搜索完毕，沿着节点坐标即可获得最终路径。之所以去讨论节点而不是方格，是因为搜索区域不仅仅可以划分为方格，划分为五边形，六边形甚至是任意不规则形状在这里都是允许的。节点可以放置在形状的任意位置，中心、边界都是允许的。这时，整个地图已经被简化为容易进行处理的节点。 如图4.1所示，出发点为节点点S，用绿色标示，需要救援的地点为点F，用红色标示，蓝色区域为障碍物图4.1 A*算法简单地图 接下来开始搜索。(1) 首先从出发点S进行处理，将出发点点S放置于开启列表。该列表中存放的为待处理的节点。到目前为止，该列表中只存在一个节点，但随着路径搜索的进行，开启列表中的元素会慢慢丰富起来。(2) 寻找起点S周围可以通过或可以到达的点，忽略被障碍物占据的点，将这些点加入开启列表，并记录起点S为他们的父节点。在搜索点A周围点时将点B加入了开启列表，则点A称为点B的父节点。(3) 在开启列表中去除掉出发点S，并将该点放置到关闭列表。关闭列表中的节点为不需要第二次处理的点。到目前为止，可以得到如图4.2所示结构。图中，被标示为绿色节点为出发点S，红色边框表示该点已经被放置于关闭列表中不需要第二次处理。周围点的黄色边框说梦该点被放置于开启列表，等待处理。现在周围所有8个点都被放置于开启列表，每个节点中的黑色箭头指向自己的父节点。图4.2 A*算法起点搜索(4) 选择开启列表中临近起始点S的方格，重复上述过程，此处开启列表中存在8个点，按顺序处理其中的每一个点，处理方法如下： 计算从出发点沿着搜索到的路径移动到当前点的代价值，记为G。计算从当前点到终点F的估计代价，记为H。由于无法真正确定当前按照最优路径移动到终点的真正距离，因为在搜索过程中可能遇到障碍物等，所以此处称其为估计距离。这种方法被叫做启发式操作。在这里，得到H值得方法采用Manhattan算法。对G值和H值进行加法操作，记为F，就是通过F值得大小来搜索路径的。 假设每个方格的边长为10，则对角线方向距离为14.14。则刚刚被加入开启列表的8个点G、H、F如下表4.1所示：表4.1 开启列表节点位置G值H值F值S点左上146074S点正上105060S点右上144054S点正左105060S点正右103040S点左下146074S点正下105060S点右下144054由于寻找的是最短路径，所以选择上述点中F值最小的点A进行接下来的搜索。检查A点周围的点，如果在关闭列表中则忽略掉。如果可以到达且不再开启列表中，则将其加入开启列表，并将其父节点记录为点A。如果点A周围的点处于开启列表中，则计算从起点S通过点A到当前点这种路径的G值是不是比当前点本身的G值小，如果是，则重新记录当前点的G、F值为小的，相反不做任何操作。将点A从开启列表中去除掉并放置于关闭列表。 接下来重复上述操作步骤，从开启列表中找到F值最小的，重复操作。但此时存在两个F值最小的点，处理其中一个点即可。在这里，为了更快的搜索速度，选择后面被添加到开启列表的点进行处理。如果对F值相同的点做不同的处理，则可能得到路径长度相同的不同路径。于是，对点B进行处理。 此时，在检测点B周围的点时，发现该点右侧为障碍物，于是略过，该点上面的点已经被添加到关闭列表，同样略过。该点右下角的点可以自定义选择处理与否，在这里选择否，不对他进行处理，这样就需要绕到障碍物的下面然后通过障碍物。此时，点B周围只有两个点不在开启列表中，将他们添加进去，并指定他们的父节点为点B。点B左侧的点已经存在于开启列表中，处理这个点时，要比较经过该点的G值是否要比该点本身的G值小，如果是，则更新当前点的G、F值为小的，相反不做任何操作。将点A从开启列表中删除并加入到关闭列表中。处理结果如下图4.3所示。图4.3 A*算法第二个点搜索处理完B点后，开启列表中各点G、H、F值如下表4.2所示。表4.2 起始点周围点三值节点位置G值H值F值S点左上146074S点正上105060S点右上144054续表4.2起始点周围点三值S点正左105060S点左下146074S点正下105060B点左下286088B点正下245074 重复上述步骤，直到终点F被添加到关闭列表中，如图4.4所示：图4.4 A*算法搜索至目标点 到这里，算法运算完毕，从终点F开始，对他们的父节点进行遍历，就得到了一条最短轨迹，如图4.5所示：图4.6 A*算法搜索完毕A*算法详细运算步骤如下：(1) 将起点添加到开启列表中(2) 重复以下步骤 从开启列表中找出F值最小的节点，并把它当做当前的操作节点 检查当前点周围的点，如果已经在开启列表中，看是否能通过当前点得到更小的G，如果能就更新那个点的G、F的值，如果在关闭列表中或者是障碍物（不可达）则忽略他们 把当前点从开启列表中移除 ，加入关闭列表中 当目标点加入关闭列表中时停止(3) 保存路径，从目标点出发，按照父节点指针遍历，直到找到起点。基于上述步骤可绘制流程图如下4.7所示：图4.7 A*算法示意图4.2 A*算法仿真 进行算法仿真选择MATLAB软件，仿真时除了需要运用MATLAB的编程语言将A*算法的整个流程进行描述外，还需要提供用户操作界面，这样，可以方便地更改障碍物以及起点终点参数，更容易的对算法进行测试检验。4.2.1 可视化用户界面思想 在熟悉了A*算法的理论之后，需要进行MATLAB仿真，用途有两点：一是可以非常直观的看出所规划出的路径效果，二是可以将路径规划结果转化为机器人能够识别的数据。 仿真时，除了A*算法本身的运算部分，还需要其他模块来为算法进行参数设定，需要输入地图信息，起点终点信息，障碍物信息，路径规划完毕后还要输出路径和坐标，如图4.8所示。图4.8 软件功能模块分析 为满足以上需求，需要再MATLAB中建立用户界面，为用户输入提供接口。在MATLAB中新建空白图形用户界面，将控件拖动到窗口适当位置，获得如图4.9所示的主窗口，同样的操作分别获得欢迎窗口，创建地图窗口，创建障碍物窗口，输入起点终点窗口。图4.9 MATLAB GUI界面4.2.2 回调函数及调用逻辑分析 创建完成后，需要添加窗口加载函数和控件的回调函数。所谓窗口加载函数，即窗口生成的时候执行的函数，在这里创建全局变量并初始化，初始化控件属性。所谓callback函数，即控件发生动作的时候调用的函数。例如按钮控件，编写其callback函数，当按钮被按下的时候就会调用这个函数。上述步骤操作完毕，修正语法错误和逻辑错误后开始运行。 运行splash_window.m文件，出现如图4.10所示的欢迎界面，点击“确定”按钮进入地图编辑界面，图4.10 欢迎界面在此输入地图信息为矩形，长100宽100的地图，点击确定，如图4.11所示：图4.11 创建地图界面设计进入到MATLAB仿真主界面，如图4.12所示。图4.12 仿真主界面图4.13 界面大小设计首先创建起点终点，点击“创建起点终点”，键入起点终点平面坐标，如图4.13。此时，主界面上出现起点终点。接下来创建障碍物，点击“创建障碍物”，输入障碍物形状以及尺寸信息，障碍物信息如下表4.3所示。表4.3 障碍物信息障碍物形状障碍物尺寸信息矩形左上角坐标长宽595101557510155551015535101551510101898101818771018185010361810108409020640781510405030134020301040351013圆形圆心坐标半径5060570305507370703添加完成后点击“添加完毕”。绘制出如图4.13所示的障碍物：图4.13 主界面绘制障碍物信息图点击“规划路径”，即可得到如图4.14路径：图4.14 主界面绘制路径此时，路径规划完成，点击输出到文件，即可将得到的点输出到文本文件中，供机器人使用。4.2.2 仿真分析 在如图4.14得到的路径中存在不理想点，如图4.15的局部放大图所示不理想点图4.15不理想点 图中的不理想点既不处于障碍物内也不处于障碍物边界，因此该点是可到达的点，相对该点左侧点，该点的F值更小，因此，优先处理该点，这样，在添加周围点的时候，会将该点左上方的点添加到开启列表中，也就会记录该点为左上方点的父节点，因此，最终路径输出时，会输出如图所示的点，这是A*算法的一个弊端。第5章 路径规划算法应用分析5.1 救援机器人硬件平台结构 机器人硬件结构如图所示，包含四个全向轮，每个全向轮利用一个电机来控制转速。电机尾部为编码器，能够将电机转速以脉冲波的形式返回。主控芯片采用STM32F407。机器人中间结构为落地正交编码器，用于同陀螺仪合作来计算机器人当前位置。5.2 救援机器人运动学和动力学分析控制全向轮结构的机器人，若想让机器人运动起来，除了合理的机械结构外，还需要控制机器人的轮子转起来，四个轮子互相配合才能让机器人合理的动作，下面从两个角度对四个全向轮之间的运动关系进行讨论，分别是运动学和动力学，推导出四轮全向移动平台的运动方程，以实现全向动作。采用4个全向轮，四个全向轮两两平行，安放于底盘的四个顶点上，且均与底盘的对角线垂直，为了使底盘能够适应不同的环境，即使运动场地不平，依然能使四个轮子同时落地，需要相邻的两个轮子的电机座并非固定在底盘上，而是做成悬挂，以满足全向运动四轮必须同时抓地的要求。对这四个轮子进行运动学和动力学分析，推导出一定的公式，应用即可实现平面内移动及旋转的全向运动。5.2.1 运动学分析 救援机器人被用于大型商场，具有平整的地面。建立与地面相连的绝对平面直角坐标系Xw-Yw，建立与机器人相连的局部平面直角坐标系Xm-Ym，并使该平面直角坐标系的原点处在机器人的中点上。如图5.1所示。图5.1机器人的运动分析可求出运动学方程如下： （5.1）式5.1中各值含义如下表5.1所示：表5.1 式5.1中各值含义变量名称表示含义L机器人中心到轮子中心的距离Vi轮子i提供的沿驱动方向的速度Xm与Xw的夹角轮子与Ym的夹角下一步根据机器人自身尺寸信息，将上表中各变量替换为具体的数值，即可得到如式5.2救援机器人的运动学模型： （5.2）简写为V=Ps，其中 v=v1 v2 v3 v4T为轮子的速度， （5.3）为转换矩阵， （5.4）为机器人整体的期望速度。5.2.2动力学分析和运动学分析相似，建立如图所示的平面直角坐标系。各变量含义如下表5.2所示：表5.2 变量含义表变量名称含义P0i由机器人中心指向轮i中心的矢径Di与电机i提供驱动力方向重合的矢量引入如下旋转矩阵R= (cos-sinsincos) （5.5）图5.3 机器人的动力学分析可得： （5.6）同理可得： （5.7） （5.8） （5.9）同理可得： （5.10）设电机i沿Di方向的驱动力大小为fi，建立如下方程： （5.11） J为绕车体中心的转动惯量。电机为机器人提供的驱动力大小可计算如下：f=U-v （5.12）公式里面U表示驱动电机的电压，v为轮子的转速,为常数。代入上式可得： （5.13）整理可得系统的动力学方程为： （5.14）其中： （5.15） （5.16）5.2.3 结论至此，全向轮运动方程分析完毕，尝试运用这个方程命令机器人走一条直线出来，走直线能够实现后，尝试让机器人走一个曲线出来，从最简单的开始，让机器人按照圆轨迹进行运动，这里走圆有两种方式，一是机器人方向不变，机器人方向不变走圆依然有好多种方式，例如，可以仅仅给定一个Vy方向的速度，然后令胃值按照一定的速度自增或自减，观察现象，机器人运动轨迹为圆。当然也可以利用Vx Vy的变化关系实现走圆。第二种就是机器人方向改变，在第一种的基础上再给定一个自转的速度，就可以轻松实现了机器人在自转的同时行走轨迹为圆。虽然机器人貌似实现了直线运动和圆运动，但是如果经过测量就会发现，机器人走的并非和理论路径相同，需要说明的是：机器人可能会走出一条类似于直线的曲线出来，这可能并非是你的方程出现了错误，可能导致这样错误的原因有好多，例如：电机的减速比不同，驱动盒的码盘线数配置错误等等，当然，即使这些都没问题，机器人走不出直线仍然属于正常现象，因为还存在着装配误差等一系列误差，这就涉及到机器人的纠偏问题，这个问题将在后面做详细介绍。5.3 机器人全场定位算法分析 机器人有了轮子仅仅能够实现运动，若想实现闭环控制，则需要对其进行全场定位分析，将整个比赛场地作为一个平面直角坐标系，可以将机器人的起始点作为整个坐标系的原点，这样，机器人就做到了有路可寻，就可以完全抛开以往的巡线，“撞边”等落后的定位方法。目前使用的全场定位是基于陀螺仪和码盘实现的，可以实时得到机器人当前的坐标，误差小，速率快，可以定位到任意位置。在机器人的行走过程中，将机器人的行动曲线微分为多段短直线（此处划分的段数同检测时间息息相关，检测时间就是多久去读一次码盘和陀螺仪的值，并将其转化为全场坐标），通过一组正交码盘返回的脉冲值和陀螺仪实时采集的角度值，利用微元累加的思想，得到机器人在这个比赛场地上的二维坐标。5.3.1 确定码盘交点坐标如图5.4，假设机器人在一个采样周期（一般为3ms）内通过的路径为Ox0,y0,1P(x0+x,y0+y,1+)弧长，但由于两次采样时间间隔很短，因此近似的认为弧长等于直线长。行走过程中码盘状态如图5.4所示，在机器人从O点走到P点的这段路程中，所以OP段之间码盘返回的脉冲数就可以认为是两个码盘分别沿直线走x和y距离时返回的脉冲数。图5.4 两点运动示例图5.5中的讨论的情况是最特殊的一种：车头朝向角为0，但是在车运动过程中更多的是不为0的情况，这种情况下，就需要对其做一般性分析。图5.5 正交方向移动距离如图5.6，机器人从A点走到B点,l1 l2是与坐标轴平行的直线，车头朝向角his_angle=CAx。两码盘记录的距离是AC和BC的长度，图5.6 码盘数值计算用temp_x，和temp_y记录码盘在时间内返回的脉冲数，要换算出实际场地中对应的距离，还会用到两个常量：COUNT_X,COUNT_Y。分别为X,Y方向的码盘一个脉冲对应的实际距离。这两个常量需要通过大量的实验测得。测量时，在行走方向上用一个简单的PID算法去纠正角度偏差，让机器人沿X（或Y）方向跑一段较长的直线，用一个变量记录码盘测得的总的脉冲，并量出机器人走过的实际距离，经换算得出每个码盘所具有的常量COUNT_X，COUNT_Y（单位：mm/脉冲）。则temp_x= x;temp_y= y;上次采样时朝向角为his_angle=1；根据这三个变量计算出横纵坐标的变化量：dir为BAC，dir=arctanyx+his_angle （5.17） temp_x，和temp_y只是对应的码盘脉冲值，则换算成车整体走过的长度： （5.18）再换算成横纵坐标的变化量，分别用coordinate_x和coordinate_y表示：；coordinate_x= S*cosdir coordinate_y= S*sin(dir) （5.19）最后在当前坐标的基础上进行累加，coordinate_X+=coordinate_x coordinate_X+=coordinate_x （5.20）这样，在初始坐标(x0,y0)的基础上，每经过一个相同的采样周期（大约3ms）进行一次加法运算，得到新的的坐标(x1,y1)，此坐标(x1,y1)就用于下一次处理时的初始坐标(x0,y0)，如此循环处理，就可以时刻知道机器人当前在场地中的坐标。5.3.2 码盘交点不在车中心的情况 不难看出以上算法得出的坐标其实是码盘交点的坐标，若码盘交点处于车中心，并且两码盘夹角为90，则可以将这样计算出的坐标当作车的实际坐标使用。但是当码盘的交点不在车中心时，或者两码盘夹角不是标准的90时，就需要在算法中加入一些补偿。如图5.7，码盘采用这种方式装时，交点为P点，车中心是O点，要将计算出来的P点坐标移至O点，就需要求出两个码盘到车中心的距离，分别用宏HM_X_D和HM_Y_D表示。图5.7 码盘布局如图5.8所示，xm和Ym分别为安装在车上的两个码盘，xw和yw分别为平行于坐标轴的两条直线，O点为车中心，P点为码盘交点，其他量均在前文介绍中给出。图5.8 码盘示意根据一系列计算得出P点坐标（用result_X和result_Y表示）之后，以这一时刻状态为例，解释怎样将码盘交点移至车中心：根据HM_X_D和HM_Y_D可计算出码盘交点与车中心的距离：r_X_Y=HM_X_D2+HM_Y_D2 （5.21）根据angle_regular和his_angle计算出OPx： （5.22）可以得出O点的横纵坐标为：Coordinate_X=result_X+r_X_Y+r_X_Y*cos(angle_regular+his_angle)Coordinate_Y=result_Y+r_X_Y*sin(angle_regular+his_angle) （5.23）5.3.3 码盘夹角不是90的情况 在测码盘常量的时候，可以看出两码盘是否垂直。若与运动方向垂直的码盘返回的脉冲值很大（由于理论上在码盘夹角标准的时候不应该有脉冲，所以在这里大约大于30个脉冲就可以认为是“很大”了），就可以确定两码盘不垂直。以图5.9情况为例，若码盘装成了这种情况，则当车沿Y轴走直线的时候，x方向码盘会记录回一定的负值。图5.9 码盘夹角Y方向码盘走过的距离为y时，对应的X方向码盘应返回的距离，此时，可求得=arcsin(xy)，这个角就是X方向码盘与标准方向的夹角。(1) 若机器人沿Y方向走，那么记录回来的temp_X应为0，但由于存在偏差角度，所以加入转化：temp_X=temp_X+temp_Y*sin （5.24）此时temp_X=0(2) 若机器人不沿Y方向走，那么经过上式转化之后temp_X并不会为零，要换算到标准的X方向还需要一次转化：temp_X=temp_X/cos （5.25）综合上述两个式子：temp_X=(temp_X+temp_Y*sin)/cos （5.26）这一部分计算对时间要求比较高，建议放在定时器的中断服务函数中完成。 测量技巧：在地上贴两条间隔较长的白线（假设间隔8米），让机器人底盘的一根铝管对齐起点的白线，向终点的白线走直线，车停之后测