数学建模扫地机器人最佳路线设计.pdf 26页

扫地机器人最佳路线设计摘要将扫地机在房间内扫垃圾的路径策略问题抽象为格栅模型。在不预知障碍物位置和数量的情况下，使用内螺旋算法规划扫地机器人清扫路线。在遇到障碍物时给出：前进方向、前进方向右侧、前进方向左侧的监听顺序（优先级）。扫地机器人已清扫的面积在代码中更新为障碍物，当扫地机器人遇到死区（前进方向及前进方向左右均为障碍物或者已清扫的格栅）时，程序检查当前地图中最近未清扫格栅。根据A*算法给出最优路径，使将机器人运行到最近的未清扫点重新开始上述内螺旋算法，直到清扫完整个给定区域。模型建立过程中，根据扫地机需要的行走路径进行程序嵌套，并用线性规划的方法来进行最优解的求取，然后根据建立的模型，用Matlab进行仿真演示。针对图1模型验证将图形1转换为地图矩阵输入程序进行验证发现当出现“死区”时程序能够正常跳出死区继续内螺旋算法，在跳出但由于机器人尺寸为20cm*20cm在清扫图一中宽为25cm区域时会出现宽为5cm的清扫盲区。这一清扫盲区我们通过修改监听步长（步长设置小于5cm即可清扫该区域）的方式对该区域区域进行全面清扫。在遇到3cm*3cm桌腿时同理可以将步长设置为3cm以内。具体验证结果见图：针对图2模型验证将图形2转换为地图矩阵输入程序进行验证发现当出现“死区”时程序也能够正常跳出死区继续内螺旋算法，在处理清扫进度时与图一验证方法相同。具体验证结果见图：清扫所用时间与清扫覆盖率均衡比较监听精度的提高可以最大化提升清扫覆盖率，能绕开较小障碍物。但监听频率的升高使得清扫时间变得冗长。为均衡清扫精度与清扫时间我们以测试环境为例给出了折中方案即监听步长=(机器人尺寸+最小障碍物尺寸)/2。关键词：线性规划内螺旋A*算法清扫效率1 目录1.问题重述.........................................................................................................................................................41.1问题背景..............................................................................................................................................41.2目标任务..............................................................................................................................................41.3具体条件及数据..................................................................................................................................42.模型假设.......................................................................................................................................................53.符号说明.......................................................................................................................................................54.模型建立与求解...........................................................................................................................................64.1内螺旋算法模型..................................................................................................................................64.2格栅地图模型.....................................................................................................................................74.3环境建模方法......................................................................................................................................74.4A*算法模型.........................................................................................................................................84.5避障方案模型....................................................................................................................................105.模型验证.......................................................................................................................................................115.1无障碍模型验证................................................................................................................................115.2障碍模型验证....................................................................................................................................125.2.1图一障碍模型验证................................................................................................................125.2.2图二障碍模型验证........................................................................................................................136.模型评价.....................................................................................................................................................136.1清扫效率分析....................................................................................................................................136.2清扫时间分析....................................................................................................................................146.3清扫时间与覆盖率均衡算法...........................................................................................................146.4A*算法的不足...................................................................................................................................14参考文献..........................................................................................................................................................15附录一：..........................................................................................................................................................16附录二..............................................................................................................................................................202 3 1.问题重述1.1问题背景随着科学技术的不断发展，扫地机逐步走入平常百姓家，并被越来越多的人所接受，扫地机（也称扫地机器人）将在不久的将来像白色家电一样成为每个家庭必不可少的清洁帮手。产品也会由现在的初级智能向着更高程度的智能化程度发展，逐步取代人工清洁。扫地机是通过电动机的高速旋转，在主机内形成真空，利用由此产生的高速气流，从吸入口吸进垃圾。扫地机一般为半径0.2米圆盘，、运行速度一般在每秒0.25米左右，只走直线，且碰到墙壁等障碍才可转弯。与传统的扫地机不同，智能扫地机可以通过微处理器进行现场环境分析，自动选择运行路线。遇到障碍发生碰撞后将重新随机地选择路线，逐步进行清扫。智能扫地机具有记忆、存储功能。利用传感器扫描现场环境，设计运行路径并存储。一般不能100%的清扫指定区域（如墙角部分）。清扫后的垃圾装进机子尾部的集尘盒，再通过人工清倒垃圾。机器在工作电压不足时会自动回到充电站充电。1.2目标任务在未知障碍物环境（地图）中，设计扫地机器人算法，机器人的清洁行进速度每4秒一米.碰到障碍时会停顿1秒后适当转向（基于你所给的算法）再次行进。设计合理的清扫路线，达到良好的擦地效果。在模型建立时分别建立：“要求清洁最全面（即除了机器人达不到的死角，都能擦到在保证清洁较为全面的前提下，要求耗时最少综合考虑“清洁最全面”与“耗时少”两方面的要求，达到一个平衡”等三个模型。1.3具体条件及数据机器人的机身长、宽为20cm×20cm，高度为5cm。它可以前进、后退以及360度左右转向。无障碍时，机器人的清洁行进速度每4秒一米.碰到障碍时会停顿1秒后适当转向（基于你所给的算法）再次行进。4 2.模型假设1、假设扫地机有充足的电量行驶完全部的轨迹，并且不会发生任何障碍；2、假设路面平坦，不会产生较大程度的偏移或者是翻转；4、假设扫地机扫过单元格一半及以上的面积时，单元格内的垃圾指标减少1；5、假设给定地图中左下角为坐标原点即起始清扫位置；6、假设由于种种环境因素不会对扫地机速率产生比较大的影响；7、假设扫地机工作时没有人为的影响改变扫地机的轨迹；8、假设当每个点的垃圾指标不超过1时扫地机的清扫任务结束；9、假设扫地机监听过程没有额外的占用移动时间，即扫描时间不会影响最终的运动时间。3.符号说明1、L机器人最外圈行走长度；2、H1机器人最外圈行走宽度；3、α长方向上的清扫次数；4、Β宽方向上清扫的次数；5、F路径评分；6、G当前标记离开始标记的路径耗费；7、H当前标记离目标方格的路径估值耗费；8、㔠房间1（图片1房间）的总面积；9、t房间1（图片1房间）障碍物总面积；10、房间1（图片1房间）待清扫区域总面积；11、t房间1（图片1房间）擦地机器人实际清扫区域总面积；12、房间1（图片1房间）清扫率；13、t㔠房间2（图片2房间）的总面积；14、tt房间2（图片2房间）障碍物总面积；15、t房间2（图片2房间）待清扫区域总面积；16、tt房间2（图片2房间）擦地机器人实际清扫区域总面积；5 17、t房间2（图片2房间）清扫率；4.模型建立与求解4.1内螺旋算法模型本文采用改进的内螺旋线路覆盖法对区域进行整体清扫。内螺旋示意图，如图4.1所示。无障碍物时，机器人按照逆时针的方向单向行驶至转弯处为一次清扫过程，则清扫一圈需要4次清扫过程。图4.1：内螺旋模型示意图设机器人机身长度为a,（a=20cm）初次清扫模式取清扫房间一角（选取没有障碍物的一角）作为清扫起点，机器人外圈行走的长度为L宽度为H1，则机器人从外圈往内圈螺旋的转数可设为α。判断清扫是否完成的标准为：用L、H1与行走轨迹宽度α相除，商即为长、宽方向上各自所需清扫次数，有余数则说明还有一块宽小于α的矩形区域需要清扫。其数学描述如下：设f={α,β|α,β分别为某时刻机器人在长宽方向上的清扫次数}，定义函数g(α,β)表示是否还存在需要清扫的区域，则其表达式如下式（1）㔠㔠g（α,β）=（）㔠㔠g(α,β)=0表示清扫任务完成；而g(α,β)=1则表示还有区域需要清扫。6 螺旋转弯值的设定及清扫是否完成的判断有效地避免了随机覆盖法低覆盖率及高重复率的缺点。4.2格栅地图模型栅格地图是20世纪80年代有Elfes和Moravec等人提出的，是指将环境分为大小相等的一些方格，每个方格表示环境中的一个区域。栅格地图是应用最为广范的地图构建方法，广泛用于机器人的路径规划、导航等领域。栅格地图属于近似描述环境的一种方法，描述同一个环境，栅格的数目越多对环境的描述越精确。栅格地图创建更新方便，地图中的每一个栅格对应环境中的部分区域，当传感器检测到环境发生变化时，栅格地图及时更新。栅格的大小决定了栅格地图的精度，栅格选择过大，地图的精度不够，进行区域覆盖时会有一些可达区域被标记为障碍物区域，造成覆盖率的下降；栅格选择过小，会导致地图存储空间增大，计算复杂程度增大，因此运用栅格法创建地图需要考虑覆盖率和计算复杂程度选择合适的栅格大小。4.3环境建模方法利用生物激励神经网络进行全区域覆盖，理论上不需要获得环境的先验信息，但是由于算法不能识别环境边界，因此需要在进行区域覆盖之前获得环境边界的信息。机器人通过自身的传感器感知环境信息，并将环境信息用一定的形式表示就是环境建模。环境地图的表示方法主要有三种，分别为拓扑地图、栅格地图以及特征地图。每种方法都有各自的优劣点和适用范围，下面对三种方法进行比较。(1)拓扑地图1978年，Kuipers提出了一种紧凑的环境表示方法——拓扑地图，用结点和线来表示环境的一种方法，结点表示环境中的一些比较重要的位置点，例如障碍物、拐角等，线则表示结点之间的相互连接关系。拓扑地图的复杂程度与环境的结构有关，当环境结构复杂时，拓扑地图内的结点增加，拓扑图也越复杂。将环境表示为拓扑图，忽略了环境内部具体几何特征，不同结点之间的位置关系不需要给出精确的描述。机器人由一个结点到另一个结点，机器人只需要知道从哪条线走就可以了，对机器人的位姿精度要求不高。拓扑地图表示的方法简单，存储空间要求小，处理速度快，但是拓扑地图要求环境中的结点之间有明确的可区分标志，否则机器人很难区分环境中存在的相似结点。拓7 扑地图的方法适用于点到点的路径规划，机器人只需从环境中的一个结点到另一个结点，但是对于清扫机器人的全区域覆盖，拓扑地图的方法有一定的局限性。拓扑地图无法具体的表述环境中的每一点的信息，这样在进行区域覆盖时就无法确定区域覆盖与否。(2)特征地图特征地图是指根据机器人的传感器采集到环境中的特征，例如拐角点、目标边缘等环境特征，从中获得这些特征的几何信息数据，将这些数据保存起来存储在地图中，机器人识别环境中的这些特征，实现机器人的定位。在一些室内的环境中，可以用一些线、面来表征环境，例如环境里的障碍物，门等环境特征。创建特征地图对传感器的要求很高，需要将传感器采集到的信息进行预处理，一般使用于高度结构化的环境。(3)栅格地图栅格地图是应用最为广范的地图构建方法，广泛用于机器人的路径规划、导航等领域。栅格地图属于近似描述环境的一种方法，描述同一个环境，栅格的数目越多对环境的描述越精确。栅格地图创建更新方便，地图中的每一个栅格对应环境中的部分区域，当传感器检测到环境发生变化时，栅格地图及时更新。栅格的大小决定了栅格地图的精度，栅格选择过大，地图的精度不够，进行区域覆盖时会有一些可达区域被标记为障碍物区域，造成覆盖率的下降；栅格选择过小，会导致地图存储空间增大，计算复杂程度增大，因此运用栅格法创建地图需要考虑覆盖率和计算复杂程度选择合适的栅格大小。4.4A*算法模型A*搜寻算法俗称A星算法。A*算法是比较流行的启发式搜索算法之一，被广泛应用于路径优化领域。它的独特之处是检查最短路径中每个可能的节点时引入了全局信息，对当前节点距终点的距离做出估计，并作为评价该节点处于最短路线上的可能性的量度。A*寻路算法，只是保证在低消耗的情况在最短的时间找出路径，但A*寻路算法寻出来的路不一定是最近，但也绝对不会远的离谱，可也不排除你对路径评分算法的优化可以做到最快最短最低消耗，或者对最终路径的优化来达到目的，下面就来讲讲通用的路径评分计算公式：8 F=G+H（2）F值表示路径评分，G值表示当前所判断的标记离开始标记的路径耗费，H值表示当前所判断的标记离目标方格的路径估值耗费。G值的计算方式是，如果为斜走判断则用父标记的G值加上14表示当前标记的G值，如果为直走判断则用父标记的G值加上10表示当前标记G值。H值通常的计算方式是一种称作为曼哈顿方法的方式，当前标记离目标方格横着的方格数加上竖着的方格数，然后乘以10，最后得值就是H值。当然若你想通过A*寻出最好的路径，那么改善算法的主要地方就是这个H值的算法。根据上面讲的A*算法的做法来讲，则表示前面判断哪个标记离开始标记更近一些只需判断一下G值即可；前面所说的取出一个路径评分最低标记，也就是将F值进行升序排序取出第一个，或降序排序取出最后一个。图4.2：A*算法实现最短路径示意图在此基础上为了更加显性的感知A*算法我们将本题中的最短路径问题单独列出，实现了两种不同地形下的最短路径规划，其路径规划结果如下图4.3所示：9 图4.3：A*算法实现最短路径实例4.5避障方案模型在遇到复杂清扫地形时机器人依靠单纯的内螺旋算法很难顺利遍历待清扫区域。在清扫障碍物分布不规整的空间时需要内螺旋算法与A*算法合理的配合。本文中扫地机器人避障逻辑流程图如下图4.3所示。如流程图所示，开始时输入给定障碍物地图参数矩阵，选取地图没有障碍物的一角做为机器人起始清扫地点。最开始清扫方向为逆时针方向，随即监听右侧障碍物情况若右侧没有障碍物则进入右侧方向清扫，并返回右向障碍判断，若右侧有障碍物则进行前向障碍物判断。若前向无障碍物则保持当前清扫方向，并返回右向障碍判断，如前向有障碍物则进入左向障碍物判断。若左向无障碍则进入左向清扫，并返回右向障碍判断，若左向任然有障碍物则进入剩余未清扫区域判断。若全图中已无未清扫区域则结束程序完成清扫，若图中仍有未清扫区域则通过A*算法寻找最短路径将机器人移动到最近的未清扫区域重新开始内螺旋算法。直到遍历全图中所有待清扫区域为止。10 图4.3:机器人避障逻辑流程图5.模型验证5.1无障碍模型验证为验证模型算法的可靠性我们首先对无障碍模型进行了验证，该无障碍模型16×23个格栅组成，清扫起始点设置于右下角，待清扫区域示意图如图5.1（a）所示，清扫结果如如图5.1（b）所示：绿色为起始方向点，在无障碍时机器人按照内螺旋遍历了待清扫区域，清扫覆盖率达到100%。11 终点起点（a）模型地图（b）清扫结果图图5.1：无障碍模型验证5.2障碍模型验证5.2.1图一障碍模型验证将图一模型尺寸按照20cm×20cm一个格栅的方式进行划分，多余部分（以cm为单位的边长除以20的余数部分）与其中一条边界融合为障碍物（墙体）。得到的待清扫区域如下如5.2（a）所示，其中左下角为起始点，运行程序清扫完成的结果如下图5.2（b）所示，该算法成功避开了所有障碍物及墙体完成了清扫。（a）模型地图（b）清扫结果图图5.2：图一障碍模型验证12 5.2.2图二障碍模型验证同理将图二模型尺寸按照20cm×20cm一个格栅的方式进行划分，多余部分（以cm为单位的边长除以20的余数部分）与其中一条边界融合为障碍物（墙体）。得到的待清扫区域如下如5.3（a）所示，其中左下角为起始点，运行程序清扫完成的结果如下图5.3（b）所示，该算法成功避开了所有障碍物及墙体完成了清扫。由于图二长宽比不协调，matlab在运行时将正方形格栅转化为了矩形格栅，但这并不影响程序的运行结果。（a）模型地图（b）清扫结果图图5.2：图二障碍模型验证6.模型评价6.1清扫效率分析算法在不同尺寸地图上进行清扫时由于倍数关系的不确定性，会得有不同的清扫效率，下面针对图一和图二障碍模型进行了效率的计算分析。本算法房间1清扫率计算：St=（3）S根据图片1结合本算法，可求得S=11.2t，S=5.4036t，S=5.46t，又由于㔠ttS=S㔠-St（4）13 故S=5.7964t，所以由上述公式可得=94.2%本算法房间2清扫率计算：Sttt=St根据图片1结合本算法，可求得S=28t，S=1.943t，S=24.8t，又由于t㔠ttttSt=St㔠-Stt故S=26.057t，所以由上述公式可得=95.18%。tt从清扫结果来看，本文给出的模型算法的清扫覆盖率均超过94%。6.2清扫时间分析题干中规定每撞击一次扫地机器人停止工作1秒钟，因此高效避障算法将减小撞击次数，文中我们选取机器人尺寸（20cm×20cm）为一个格栅的尺寸，并将监听周期定义为每移动20cm一个周期，题干中指出监听完全通过撞击来判断，因此降低监听频率将有效缩短清扫时间，配合最优化路径A*算法故本文所给出的算法是清扫时间最短算法。6.3清扫时间与覆盖率均衡算法本文所给出的算法可以实现短时间较高覆盖率清扫效果，要实现清扫时间与清扫覆盖率的双均衡需要精确控制监听精度。因为监听精度却小，避开障碍物时显得越精准，能极大程度上减少甚至消除清扫盲区。本文中给出这种方案，监听步长计算公式如下:监听步长=（机器人的边长（题目中为20cm）+最小障碍物边长（直径））/2通过该方案确定的监听步长可以实现较快速度高精度清扫。6.4A*算法的不足A*算法进行下一步将要走的节点的搜索的时候，每次都是选择F值最小的节点，因此找到的是最优路径。但是正因为如此A*算法每次都要扩展当前节点的全部后继节点，运用启发函数计算它们的F值，然后选择F值最小的节点作为下一步走的节点。在这个过程中，OPEN表需要保存大量的节点信息，不仅存储量大是一个问题，而且在查找F值最小的节点时，需要查询的节点也非常多，当然就非常耗时，这个问题就非常严重了。14 再加上如果游戏地图庞大，路径比较复杂，路径搜索过程则可能要计算成千上万的节点，计算量非常巨大。因此，搜索一条路径需要一定的时间，这就意味着游戏运行速度降低。参考文献[1]肖雄军,蔡自兴.服务机器人的发展[J].自动化博览,2004(6):10-13.[2]BrianR.Robotsreachthehomefloor[J].IndustrialRobot,2001,28(1):27-28.[3]邢敏.清洁机器人系统开发及路径规划研究[D].北京交通大学硕士论文,2007.[4]赵金星.割草机器人总体设计与关键技术研究[D].南京理工大学硕士学位论文,2007.[5]范路桥,姚锡凡,祁亨年,蒋梁中.排爆机器人的研究现状及其关键技术[J].机床与液压,2008,36(6):139-143.[6]朱世强,刘瑜,庞作伟等.自主吸尘机器人研究现状[J].机器人,2004,(6):569-574.[7]MarroneFabrizio,RaimondiFrancescoM.Kinematicsandcontrolofahousekeepingrobotassistant[J].WSEASTransactionsonCircuitsandSystems,2005,4(4):224-233.[8]FioriniP,PrasslerE.Cleaningandhouseholdrobots:Atechnologysurvey[J].AutonomousRobots,2000,9(3):227-235.[9]李金山,李琳,谭定忠.清洁机器人概述[J].中国科技信息,2005,5:18.[10]林红,翁桂荣.地面清扫机器人的研究[J].基础自动化,2000,7(4):29-31.[11]罗胜.吸尘机器人的现状及其智能系统的若干关键技术[J].传感器与微系统,2007,26(11):5-9.[12]MarroneFabrizio,RaimondiFrancescoM.Kinematicsandcontrolofahousekeep-ingrobotassistant[J].WSEASTransactionsonCircuitsandSystems,2005,4(4):224-233.[13]马翔,朱世强,吴海彬.智能吸尘器的开发及设计[J].电子技术应用,2000,26(8):6-8.[14]AdorniG,CagnoniS,EnderleS,etal.Vision-basedlocalizationformobilerobots[J].RoboticsandAutonomousSystems,2001,36(2-3):103-119.[15]董晓杰,陈启军.卡尔曼滤波在足球机器人定位中的应用[J].装备制造技术,2008.2:45-48.[16]陈小宁,黄玉清,杨佳.多传感器信息融合在移动机器人定位中的应用[J].传感器与微系统,2008,27(6):110-113.[17]李瑞峰,李伟招.基于多传感器信息融合的移动机器人路径规划[J].机电一体化,2002(4):20~23.15 附录一：A*算法实例functionastardemon=20;wallpercent=0.45;[field,startposind,goalposind,costchart,fieldpointers]=...initializeField(n,wallpercent);setOpen=[startposind];setOpenCosts=[0];setOpenHeuristics=[Inf];setClosed=[];setClosedCosts=[];movementdirections={"R","L","D","U"};counterIterations=1;axishandle=createFigure(field,costchart,startposind,goalposind);while~max(ismember(setOpen,goalposind))&&~isempty(setOpen)[temp,ii]=min(setOpenCosts+setOpenHeuristics);[costs,heuristics,posinds]=findFValue(setOpen(ii),setOpenCosts(ii),...field,goalposind,"euclidean");%putnodeinCLOSEDandrecorditscostsetClosed=[setClosed;setOpen(ii)];setClosedCosts=[setClosedCosts;setOpenCosts(ii)];%updateOPENandtheirassociatedcostsif(ii>1&&iicosts(jj)costchart(setOpen(I))=costs(jj);setOpenCosts(I)=costs(jj);setOpenHeuristics(I)=heuristics(jj);fieldpointers(setOpen(I))=movementdirections(jj);endelseI=find(setClosed==posinds(jj));ifsetClosedCosts(I)>costs(jj)costchart(setClosed(I))=costs(jj);setClosedCosts(I)=costs(jj);fieldpointers(setClosed(I))=movementdirections(jj);endendendendifisempty(setOpen)break;endset(axishandle,"CData",[costchartcostchart(:,end);costchart(end,:)costchart(end,end)]);set(gca,"CLim",[01.1*max(costchart(find(costchart0pos(1,:)=[newynewx];switchlower(heuristicmethod)case"euclidean"heuristic(1)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);case"taxicab"heuristic(1)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);endcost(1)=costsofar+field(newy,newx);endnewx=currentpos(2)+1;newy=currentpos(1);ifnewx<=npos(2,:)=[newynewx];switchlower(heuristicmethod)case"euclidean"heuristic(2)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);case"taxicab"heuristic(2)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);endcost(2)=costsofar+field(newy,newx);endnewx=currentpos(2);newy=currentpos(1)-1;ifnewy>0pos(3,:)=[newynewx];18 switchlower(heuristicmethod)case"euclidean"heuristic(3)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);case"taxicab"heuristic(3)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);endcost(3)=costsofar+field(newy,newx);endnewx=currentpos(2);newy=currentpos(1)+1;ifnewy<=npos(4,:)=[newynewx];switchlower(heuristicmethod)case"euclidean"heuristic(4)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);case"taxicab"heuristic(4)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);endcost(4)=costsofar+field(newy,newx);endposinds=sub2ind([nn],pos(:,1),pos(:,2));%%%%%%%%%%%%%%%%%%%%%%%%%%%%function[field,startposind,goalposind,costchart,fieldpointers]=...initializeField(n,wallpercent)field=ones(n,n)+10*rand(n,n);field(ind2sub([nn],ceil(n^2.*rand(floor(n*n*wallpercent),1))))=Inf;startposind=sub2ind([n,n],ceil(n.*rand),ceil(n.*rand));goalposind=sub2ind([n,n],ceil(n.*rand),ceil(n.*rand));field(startposind)=0;field(goalposind)=0;costchart=NaN*ones(n,n);costchart(startposind)=0;fieldpointers=cell(n,n);fieldpointers{startposind}="S";fieldpointers{goalposind}="G";fieldpointers(field==Inf)={0};%%%%%%%%%%%%%%%%%%%%functionaxishandle=createFigure(field,costchart,startposind,goalposind)ifisempty(gcbf)f1=figure("Position",[600300500500],"Units","Normalized",..."MenuBar","none");Caxes2=axes("position",[0.010.010.980.98],"FontSize",12,..."FontName","Helvetica");elsegcf;cla;endn=length(field);19 field(fieldend!");break;endifm-h>=1r1=m-h;elser1=1;isend1=1;end23 ifm+h<=roomwidthr2=m+h;elser2=roomwidth;isend2=1;endifn-h>=1c1=n-h;elsec1=1;isend3=1;endifn+h<=roomlengthc2=n+h;elsec2=roomlength;isend4=1;endfprintf("r1r2=%d,%dc1c2=%d,%dn",r1,r2,c1,c2);forr=r1:1:r2iffinishforc=n:c2ifmap(r,c)==1;x1=c;y1=r;flag=1;break;endendendiffinishforc=n:-1:c1ifmap(r,c)==1;x1=c;y1=r;flag=1;break;endendendend24 forc=c1:1:c2iffinishforr=m:r1ifmap(r,c)==1;x1=c;y1=r;flag=1;break;endendendiffinishforr=m:-1:r2ifmap(r,c)==1;x1=c;y1=r;flag=1;break;endendendendh=h+1;ifisend1==1&&isend2==1&&isend3==1&&isend4==1finish=0;fprintf("---->endwithH=%d!",h);break;endifflag==1flag=0;m=y1;n=x1;isend1=0;isend2=0;isend3=0;isend4=0;h=1;map(m,n)=2;plot(x1,y1,"go-");fprintf("x=%dy=%d,",n,m);robmove=1;break;25 endfprintf("...n");endendtt=int2str(x);switch(robmove)case1s="右";case2s="左";case3s="上";case4s="下";endtitle(["向",s,"运动","当前位置(",num2str(n),",",num2str(m),")"])suqm=[-0.5+0.5+0.5-0.5]+m;suqn=[-0.5-0.5+0.5+0.5]+n;patch("xData",suqn,"yData",suqm,"FaceColor","b");ylabel(["",""]);pause(0.5);suqm=[-0.5+0.5+0.5-0.5]+m;suqn=[-0.5-0.5+0.5+0.5]+n;patch("xData",suqn,"yData",suqm,"FaceColor","y");endmapsuqm=[-0.5+0.5+0.5-0.5]+m;suqn=[-0.5-0.5+0.5+0.5]+n;patch("xData",suqn,"yData",suqm,"FaceColor","r");map26