数学建模扫地机器人最佳路线设计

1 扫地机器人最佳路线设计扫地机器人最佳路线设计 摘要摘要 将扫地机在房间内扫垃圾的路径策略问题抽象为格栅模型。 在不预知障碍物位置和 数量的情况下，使用内螺旋算法规划扫地机器人清扫路线。在遇到障碍物时给出：前进 方向、前进方向右侧、前进方向左侧的监听顺序（优先级）。扫地机器人已清扫的面积 在代码中更新为障碍物，当扫地机器人遇到死区（前进方向及前进方向左右均为障碍物 或者已清扫的格栅）时，程序检查当前地图中最近未清扫格栅。根据 A*A*算法算法给出最优路 径，使将机器人运行到最近的未清扫点重新开始上述内螺旋算法，直到清扫完整个给定 区域。模型建立过程中，根据扫地机需要的行走路径进行程序嵌套，并用线性规划的方 法来进行最优解的求取，然后根据建立的模型，用 Matlab 进行仿真演示。 针对图针对图 1 1 模型验证模型验证 将图形 1 转换为地图矩阵输入程序进行验证发现当出现“死区”时程序能够正常跳 出死区继续内螺旋算法， 在跳出但由于机器人尺寸为 20cm*20cm 在清扫图一中宽为 25cm 区域时会出现宽为 5cm 的清扫盲区。这一清扫盲区我们通过修改监听步长（步长设置小 于 5cm 即可清扫该区域）的方式对该区域区域进行全面清扫。在遇到 3cm*3cm 桌腿时同 理可以将步长设置为 3cm 以内。具体验证结果见图： 针对图针对图 2 2 模型验证模型验证 将图形 2 转换为地图矩阵输入程序进行验证发现当出现“死区”时程序也能够正常 跳出死区继续内螺旋算法， 在处理清扫进度时与图一验证方法相同。 具体验证结果见图： 清扫所用时间与清扫覆盖率均衡比较清扫所用时间与清扫覆盖率均衡比较 监听精度的提高可以最大化提升清扫覆盖率，能绕开较小障碍物。但监听频率的升 高使得清扫时间变得冗长。 为均衡清扫精度与清扫时间我们以测试环境为例给出了折中 方案即监听步长=(机器人尺寸+最小障碍物尺寸)/2。 关键词：关键词：线性规划内螺旋A*算法清扫效率 2 目目 录录 1.问题重述.4 1.1 问题背景.4 1.2 目标任务.4 1.3 具体条件及数据.4 2. 模型假设.5 3. 符号说明.5 4. 模型建立与求解.6 4.1 内螺旋算法模型.6 4.2 格栅地图模型.7 4.3 环境建模方法.7 4.4A*算法模型.8 4.5 避障方案模型.10 5.模型验证.11 5.1 无障碍模型验证.11 5.2 障碍模型验证.12 5.2.1 图一障碍模型验证. 12 5.2.2 图二障碍模型验证. 13 6. 模型评价.13 6.1 清扫效率分析.13 6.2 清扫时间分析.14 6.3 清扫时间与覆盖率均衡算法.14 6.4A*算法的不足.14 参考文献.15 附录一：.16 附录二.20 3 4 1.问题重述问题重述 1.1 问题背景问题背景 随着科学技术的不断发展， 扫地机逐步走入平常百姓家， 并被越来越多的人所接受， 扫地机（也称扫地机器人）将在不久的将来像白色家电一样成为每个家庭必不可少的清 洁帮手。产品也会由现在的初级智能向着更高程度的智能化程度发展，逐步取代人工清 洁。 扫地机是通过电动机的高速旋转，在主机内形成真空，利用由此产生的高速气流， 从吸入口吸进垃圾。扫地机一般为半径 0.2 米圆盘，、运行速度一般在每秒 0.25 米左 右，只走直线，且碰到墙壁等障碍才可转弯。与传统的扫地机不同，智能扫地机可以通 过微处理器进行现场环境分析，自动选择运行路线。遇到障碍发生碰撞后将重新随机地 选择路线，逐步进行清扫。智能扫地机具有记忆、存储功能。利用传感器扫描现场环境， 设计运行路径并存储。一般不能 100%的清扫指定区域（如墙角部分）。清扫后的垃圾装 进机子尾部的集尘盒，再通过人工清倒垃圾。机器在工作电压不足时会自动回到充电站 充电。 1.2 目标任务目标任务 在未知障碍物环境（地图）中，设计扫地机器人算法，机器人的清洁行进速度每 4 秒一米.碰到障碍时会停顿 1 秒后适当转向（基于你所给的算法）再次行进。设计合理 的清扫路线，达到良好的擦地效果。在模型建立时分别建立：“要求清洁最全面（即 除了机器人达不到的死角，都能擦到在保证清洁较为全面的前提下，要求耗时最少 综合考虑“清洁最全面”与 “耗时少”两方面的要求，达到一个平衡”等三个模型。 1.3 具体条件及数据具体条件及数据 机器人的机身长、宽为 20cm20cm，高度为 5cm。它可以前进、后退以及 360 度左 右转向。无障碍时，机器人的清洁行进速度每 4 秒一米.碰到障碍时会停顿 1 秒后适当 转向（基于你所给的算法）再次行进。 5 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 房间）擦地机器人实际清扫区域总面积； 6 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 则表示还有区域需要清扫。 7 螺旋转弯值的设定及清扫是否完成的判断有效地避免了随机覆盖法低覆盖率及高 重复率的缺点。 4.2 格栅地图模型格栅地图模型 栅格地图是 20 世纪 80 年代有 Elfes 和 Moravec 等人提出的，是指将环境分为大小 相等的一些方格，每个方格表示环境中的一个区域。栅格地图是应用最为广范的地图构 建方法，广泛用于机器人的路径规划、导航等领域。栅格地图属于近似描述环境的一种 方法， 描述同一个环境， 栅格的数目越多对环境的描述越精确。 栅格地图创建更新方便， 地图中的每一个栅格对应环境中的部分区域，当传感器检测到环境发生变化时，栅格地 图及时更新。 栅格的大小决定了栅格地图的精度，栅格选择过大，地图的精度不够， 进行区域覆盖时会有一些可达区域被标记为障碍物区域，造成覆盖率的下降；栅格选择 过小，会导致地图存储空间增大，计算复杂程度增大，因此运用栅格法创建地图需要考 虑覆盖率和计算复杂程度选择合适的栅格大小。 4.3 环境建模方法环境建模方法 利用生物激励神经网络进行全区域覆盖，理论上不需要获得环境的先验信息，但是 由于算法不能识别环境边界，因此需要在进行区域覆盖之前获得环境边界的信息。机器 人通过自身的传感器感知环境信息，并将环境信息用一定的形式表示就是环境建模。 环境地图的表示方法主要有三种，分别为拓扑地图、栅格地图以及特征地图。每种 方法都有各自的优劣点和适用范围，下面对三种方法进行比较。 (1)(1) 拓扑地图拓扑地图 1978 年，Kuipers 提出了一种紧凑的环境表示方法拓扑地图，用结点和线来表 示环境的一种方法，结点表示环境中的一些比较重要的位置点，例如障碍物、拐角等， 线则表示结点之间的相互连接关系。拓扑地图的复杂程度与环境的结构有关，当环境结 构复杂时，拓扑地图内的结点增加，拓扑图也越复杂。将环境表示为拓扑图，忽略了环 境内部具体几何特征，不同结点之间的位置关系不需要给出精确的描述。机器人由一个 结点到另一个结点，机器人只需要知道从哪条线走就可以了，对机器人的位姿精度要求 不高。拓扑地图表示的方法简单，存储空间要求小，处理速度快，但是拓扑地图要求环 境中的结点之间有明确的可区分标志，否则机器人很难区分环境中存在的相似结点。拓 8 扑地图的方法适用于点到点的路径规划，机器人只需从环境中的一个结点到另一个结 点，但是对于清扫机器人的全区域覆盖，拓扑地图的方法有一定的局限性。拓扑地图无 法具体的表述环境中的每一点的信息，这样在进行区域覆盖时就无法确定区域覆盖与 否。 (2)(2) 特征地图特征地图 特征地图是指根据机器人的传感器采集到环境中的特征，例如拐角点、目标边缘等 环境特征，从中获得这些特征的几何信息数据，将这些数据保存起来存储在地图中，机 器人识别环境中的这些特征， 实现机器人的定位。 在一些室内的环境中， 可以用一些线、 面来表征环境，例如环境里的障碍物，门等环境特征。 创建特征地图对传感器的要求 很高，需要将传感器采集到的信息进行预处理，一般使用于高度结构化的环境。 (3)(3) 栅格地图栅格地图 栅格地图是应用最为广范的地图构建方法，广泛用于机器人的路径规划、导航等领 域。栅格地图属于近似描述环境的一种方法，描述同一个环境，栅格的数目越多对环境 的描述越精确。栅格地图创建更新方便，地图中的每一个栅格对应环境中的部分区域， 当传感器检测到环境发生变化时，栅格地图及时更新。 栅格的大小决定了栅格地图的 精度，栅格选择过大，地图的精度不够，进行区域覆盖时会有一些可达区域被标记为障 碍物区域，造成覆盖率的下降；栅格选择过小，会导致地图存储空间增大，计算复杂程 度增大，因此运用栅格法创建地图需要考虑覆盖率和计算复杂程度选择合适的栅格大 小。 4.4 A*算法模型算法模型 A*搜寻算法俗称 A 星算法。A*算法是比较流行的启发式搜索算法之一，被广泛应 用于路径优化领域。它的独特之处是检查最短路径中每个可能的节点时引入了全局信 息，对当前节点距终点的距离做出估计，并作为评价该节点处于最短路线上的可能性的 量度。 A*寻路算法，只是保证在低消耗的情况在最短的时间找出路径，但 A*寻路算法寻 出来的路不一定是最近，但也绝对不会远的离谱，可也不排除你对路径评分算法的优化 可以做到最快最短最低消耗，或者对最终路径的优化来达到目的，下面就来讲讲通用的 路径评分计算公式： 9 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 所示： 10 图 4.3：A*算法实现最短路径实例 4.5 避障方案模型避障方案模型 在遇到复杂清扫地形时机器人依靠单纯的内螺旋算法很难顺利遍历待清扫区域。 在 清扫障碍物分布不规整的空间时需要内螺旋算法与 A*算法合理的配合。本文中扫地机 器人避障逻辑流程图如下图 4.3 所示。如流程图所示，开始时输入给定障碍物地图参数 矩阵，选取地图没有障碍物的一角做为机器人起始清扫地点。最开始清扫方向为逆时针 方向，随即监听右侧障碍物情况若右侧没有障碍物则进入右侧方向清扫，并返回右向障 碍判断， 若右侧有障碍物则进行前向障碍物判断。 若前向无障碍物则保持当前清扫方向， 并返回右向障碍判断，如前向有障碍物则进入左向障碍物判断。若左向无障碍则进入左 向清扫，并返回右向障碍判断，若左向任然有障碍物则进入剩余未清扫区域判断。若全 图中已无未清扫区域则结束程序完成清扫，若图中仍有未清扫区域则通过 A*算法寻找 最短路径将机器人移动到最近的未清扫区域重新开始内螺旋算法。 直到遍历全图中所有 待清扫区域为止。 11 图 4.3:机器人避障逻辑流程图 5.模型验证模型验证 5.1 无障碍模型验证无障碍模型验证 为验证模型算法的可靠性我们首先对无障碍模型进行了验证，该无障碍模型 1623 个格栅组成，清扫起始点设置于右下角，待清扫区域示意图如图 5.1（a）所示，清扫结 果如如图 5.1（b）所示：绿色为起始方向点，在无障碍时机器人按照内螺旋遍历了待清 扫区域，清扫覆盖率达到 100%。 12 （a）模型地图（b）清扫结果图 图 5.1：无障碍模型验证 5.2 障碍模型验证障碍模型验证 5.2.1 图一障碍模型验证图一障碍模型验证 将图一模型尺寸按照 20cm20cm 一个格栅的方式进行划分，多余部分（以 cm 为单位的边长除 以 20 的余数部分）与其中一条边界融合为障碍物（墙体）。得到的待清扫区域如下如 5.2（a）所示， 其中左下角为起始点，运行程序清扫完成的结果如下图 5.2（b）所示，该算法成功避开了所有障碍 物及墙体完成了清扫。 （a）模型地图（b）清扫结果图 图 5.2：图一障碍模型验证 终点终点 起点起点 13 5.2.2 图二障碍模型验证图二障碍模型验证 同理将图二模型尺寸按照 20cm20cm 一个格栅的方式进行划分，多余部分（以 cm 为单位的边 长除以 20 的余数部分）与其中一条边界融合为障碍物（墙体）。得到的待清扫区域如下如 5.3（a） 所示，其中左下角为起始点，运行程序清扫完成的结果如下图 5.3（b）所示，该算法成功避开了所 有障碍物及墙体完成了清扫。由于图二长宽比不协调，matlab 在运行时将正方形格栅转化为了矩形 格栅，但这并不影响程序的运行结果。 （a）模型地图（b）清扫结果图 图 5.2：图二障碍模型验证 6.模型评价模型评价 6.1 清扫效率分析清扫效率分析 算法在不同尺寸地图上进行清扫时由于倍数关系的不确定性， 会得有不同的清扫效 率，下面针对图一和图二障碍模型进行了效率的计算分析。 本算法房间 1 清扫率计算： ?=S?t S? （3） 根据图片 1 结合本算法，可求得S?=11.2 ?t，S?t=5.4036 ?t，S?t=5.46?t，又由于 S?=S?-S?t（4） 14 故S?=5.7964?t，所以由上述公式可得?=94.2% 本算法房间 2 清扫率计算： ?t=Stt St? 根据图片 1 结合本算法，可求得St=28 ?t，Stt=1.943 ?t，Stt=24.8?t，又由于 St?=St-Stt 故St?=26.057?t，所以由上述公式可得?t=95.18%。 从清扫结果来看，本文给出的模型算法的清扫覆盖率均超过 94%。 6.2 清扫时间分析清扫时间分析 题干中规定每撞击一次扫地机器人停止工作 1 秒钟， 因此高效避障算法将减小撞击 次数，文中我们选取机器人尺寸（20cm20cm）为一个格栅的尺寸，并将监听周期定义 为每移动 20cm 一个周期，题干中指出监听完全通过撞击来判断，因此降低监听频率将 有效缩短清扫时间，配合最优化路径 A*算法故本文所给出的算法是清扫时间最短算法。 6.3 清扫时间与覆盖率均衡算法清扫时间与覆盖率均衡算法 本文所给出的算法可以实现短时间较高覆盖率清扫效果， 要实现清扫时间与清扫覆 盖率的双均衡需要精确控制监听精度。因为监听精度却小，避开障碍物时显得越精准， 能极大程度上减少甚至消除清扫盲区。本文中给出这种方案，监听步长计算公式如下: 监听步长=（机器人的边长（题目中为 20cm）+最小障碍物边长（直径）/2 通过该方案确定的监听步长可以实现较快速度高精度清扫。 6.4 A*算法的不足算法的不足 A*算法进行下一步将要走的节点的搜索的时候， 每次都是选择 F 值最小的节点， 因 此找到的是最优路径。但是正因为如此 A*算法每次都要扩展当前节点的全部后继节点， 运用启发函数计算它们的 F 值，然后选择 F 值最小的节点作为下一步走的节点。在这个 过程中，OPEN 表需要保存大量的节点信息，不仅存储量大是一个问题，而且在查找 F 值最小的节点时， 需要查询的节点也非常多， 当然就非常耗时， 这个问题就非常严重了。 15 再加上如果游戏地图庞大， 路径比较复杂， 路径搜索过程则可能要计算成千上万的节点， 计算量非常巨大。 因此， 搜索一条路径需要一定的时间， 这就意味着游戏运行速度降低。 参考文献参考文献 1 肖雄军, 蔡自兴. 服务机器人的发展J. 自动化博览, 2004 (6): 10-13. 2 Brian R. Robots reach the home floor J. Industrial Robot, 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 Marrone Fabrizio, Raimondi Francesco M. Kinematics and control of a house keeping robot assistant J. WSEAS Transactions on Circuits and Systems, 2005, 4(4): 224-233. 8 Fiorini P, Prassler E. Cleaning and household robots: A technology survey J. Autonomous Robots, 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 Marrone Fabrizio, Raimondi Francesco M. Kinematics and control of a house keep -ing robot assistant J. WSEAS Transactions on Circuits and Systems, 2005, 4(4): 224-233. 13 马翔, 朱世强, 吴海彬. 智能吸尘器的开发及设计J. 电子技术应用, 2000, 26(8): 6-8. 14 Adorni G, Cagnoni S, Enderle S, et al. Vision-based localization for mobile robots J. Robotics and Autonomous Systems, 2001, 36(2-3): 103-119. 15 董晓杰, 陈启军.卡尔曼滤波在足球机器人定位中的应用J. 装备制造技术, 2008. 2: 45-48. 16 陈小宁, 黄玉清, 杨佳. 多传感器信息融合在移动机器人定位中的应用J. 传 感器与微系统, 2008, 27(6): 110-113. 17 李瑞峰, 李伟招. 基于多传感器信息融合的移动机器人路径规划J. 机电一体 化, 2002(4) : 2023. 16 附录一：附录一： A*算法实例 function astardemo n = 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) costs,heuristics,posinds = findFValue(setOpen(ii),setOpenCosts(ii), . field,goalposind,euclidean); % put node in CLOSED and record its cost setClosed = setClosed; setOpen(ii); setClosedCosts = setClosedCosts; setOpenCosts(ii); % update OPEN and their associated costs if (ii 1 setOpenCosts(I) = costs(jj); setOpenHeuristics(I) = heuristics(jj); fieldpointers(setOpen(I) = movementdirections(jj); end else I = find(setClosed = posinds(jj); if setClosedCosts(I) costs(jj) costchart(setClosed(I) = costs(jj); setClosedCosts(I) = costs(jj); fieldpointers(setClosed(I) = movementdirections(jj); end end end end if isempty(setOpen) break; end set(axishandle,CData,costchartcostchart(:,end);costchart(end,:) costchart(end,end); set(gca,CLim,0 1.1*max(costchart(find(costchart 0 pos(1,:) = newy newx; switch lower(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); end cost(1) = costsofar +