一种智能清洁机器人及其路径规划方法与流程

本发明属于智能车技术领域，具体涉及一种基于先进路径规划技术的智能清洁机器人及其路径规划方法。

背景技术：

目前，公知的扫地机器的机身为无线机器，以圆盘型为主。使用充电电池为电源运作，操作方式以遥控器、机器上的操作面板为主。一般能设定时间预约打扫，自行充电。机器人前方设置有传感器，可侦测障碍物，如侦测到墙壁或其他障碍物，会自行转弯，依据不同厂商设定，按照不同判断机制行驶并规划清扫地区。目前市场上大部分扫地机器人采用是并无精准定位的随机路径清扫方式，清扫面积覆盖率低，耗时长，效率低，容易发生部分房间与角落的漏扫和重复清扫，且清洁耗时长。而采用改进算法规划的扫地机器人优化了弓形清扫模式，但因为实现方式上没有定位，本质上还是随机式，价格较高，性价比较低。

技术实现要素：

本发明所要解决的技术问题在于针对上述现有技术中的不足，提供一种智能清洁机器人及其路径规划方法，能够高效半自动或全自动的完成室内环境(地面)工作，能自动记录环境地图判断自己的位置并规划出最优清洁路径，将最新型的物联网芯片与智清洁能扫地机器人有机地融合起来，实现室内静音节能清扫，替代传统繁重的人工清洁工作。

本发明采用以下技术方案：

一种智能清洁机器人，包括控制单元、驱动模块、传感器模块、人机交互模块以及电源模块，驱动模块、传感器模块、人机交互模块、精准定位模块以及电源模块分别与控制单元连接；传感器模块用于分析并反馈实时清洁环境信息；精准定位模块用于获取当前智能清洁机器人在环境地图上所在的位置；控制单元采用esp32双模中央控制单元，用于获取外部环境信息，利用几何-拓扑混合地图技术建立环境地图，通过结合环境地图和实时位置，用先进路径规划算法规划出最优清扫路径，并将数据上传云平台实现实时分析记录与控制；驱动模块用于按照规划出的最优路径驱动智能清洁机器人运行并进行清洁工作；人机交互模块能够利用温湿度传感器结合摄像头实现对智能清洁机器人工作状态和性能的显示，并通过wifi/蓝牙技术可以完成智能清洁机器人的遥控与预约功能，通过电源模块为智能清洁机器人供电。

具体的，传感器模块包括转角传感器、速度传感器、红外传感器、碰撞传感器以及超声波传感器，碰撞传感器用于记录突发碰撞情况；转角传感器用于对角度的实时测量或监控；速度传感器用于检测机器人运动速度；红外传感器与超声波传感器用来检测智能清洁机器人车体和路况，当随机障碍物出现在红外传感器与超声波传感器的检测范围内时，将检测信号发送给esp32双模中央控制单元控制扫地机器人实现躲避。

进一步的，红外传感器3包括11组，间隔设置在智能清洁机器人前侧的圆弧形机身内，超声波传感器4包括4组，互成九十度夹角设置在机身前侧，在超声波传感中间设置有摄像头。

具体的，精准定位模块包括陀螺仪、光电编码器和摄像头，陀螺仪用于采集车身偏移的角速度、速度和加速度，判断智能清洁机器人的行走方向，配合室内导航定位系统与行走电机的运行状况；光电编码器用于采集智能清洁机器人的实际速度，摄像头设置在智能清洁机器人机身顶部，180°旋转用于监控和辅助定位。

具体的，人机交互模块包括通信模块，无线通信单元，按钮，摄像头，温湿度传感器，显示屏以及led，无线通信单元用于实现手机与esp32双模中央控制单元的信息传输；按钮用于供用户选择清扫模式及运动速度；温湿度传感器及摄像头与esp32双模中央控制单元结合，在手机或者web端远程监测家中情况。

一种智能清洁机器人路径规划方法，利用所述的智能清洁机器人，具体步骤如下：

s1、打开按钮，连接家庭wifi/手机蓝牙，对智能清洁机器人进行初始化设定；

s2、用户根据需求对清洁机器人进行功能选择；

s3、开始沿边搜索充电基站，回到基站后，若电量不足则充电，电量充足则进入步骤s4；

s4、利用机身前侧成弧线布置的红外近距传感器和布置在机器顶部超声波测距传感器，构建实时环境信息分析反馈，并利用几何-拓扑混合地图技术建立环境地图；

s5、利用陀螺仪模块与光电编码器配合获取机器人的加速度和角速度进行二重积分，依据几何-拓扑混合地图，对其自身进行定位；

s6、在已知环境地图和实时位置时，利用先进路径规划算法自动规划出最优清扫路径；

s7、智能清洁机器人按规划出的最优清扫路径运行进行清扫；

s8、在清扫过程中，进行路障扫描，遇到障碍时进入避障程序同时把障碍物所在位置信息记录进地图数据中；判断是否打扫完全部可清洁格栅，若清扫完成则结束；否则返回步骤s3。

具体的，步骤s4具体如下：

401、在已知环境地图的情况下，智能清洁机器人使用超声波传感器和红外传感器获得信息反馈，结合几何-拓扑混合定位与已有环境地图进行对比，当出现不一致之处时则更新环境地图信息，当变动量超过阀值则重新绘制环境地图；

402、当未知环境地图的情况下，智能清洁机器人利用近距红外传感器实现对环境感知，整体环境通过拓扑节点串联，保证局部精确定位，拓扑节点采用几何表述，同时维护环境地图表述的整体一致性，考虑方面包括提取常见环境特征、建立拐角垂直线段、根据经验阈值判断和定位拐点端点坐标；几何-拓扑混合地图定位推算在实际中拐角判断如下：

其中，(x1,y1,xc1,yc1)与(x2,y2,xc2,yc2)为形成拐角两条近似垂直线段；(xc1,yc1)和(xc2,yc2)为处于拐角区线段端点坐标，es和ed为阈值经验值；实际拐角断点坐标(xc,yc)可近似计算为：

具体的，步骤s5中，轨迹推算公式如下：

vn＝vn-1+0.5an-1+0.5an

sn＝sn-1+0.5vn-1+0.5vn

θn＝θn-1+0.5wn-1+0.5wn

其中，an表示加速度，vn表示速度，sn表示位移，wn表示角速度，θn表示转过角度，参数角标n表示第n个测量点得到的数据，其中an，vn，sn具有水平和竖直分量。

具体的，步骤s6中，最优清扫路径包括地图已知情况下的单元域规划和地图未知情况下的路径规划；地图已知情况下的单元域规划的输入步骤如下：

输入代表地图二值二维数组map＝(x，y)；按照二维数组特点，坐标原点在左上角，y是高，x是宽，y向下递增，x向右递增；将x和y封装成一个类，进行传参，重写equals方法比较坐标(x,y)；map(k)＝map(k-1)，当x(k)＝x(k-1)，y(k)＝y(k-1)，否则map(k)＝map(k-2)，当x(k)不等于x(k-1)或y(k)不等于y(k-1)；封装路径结点类，字段包括：坐标、g值、f值、父结点，实现comparable接口；最后数据结构为a*算法输入的所有数据封装在一起；

处理步骤如下：

算法中定义几个常量来确定：二维数组中bar＝1值表示障碍物、path＝2为二维数组中绘制路径的代表值、direct_value＝10表示计算g值需要横纵移动代价，direct_value＝14表示斜移动代价；使用priorityqueue与arraylist定义open和close辅助表分别取最小值与保存结点；定义布尔类判断方法；计算h值，这里使用“曼哈顿”法，坐标分别取差值相加；从open列表中查找结点；添加邻结点到open表；回溯法绘制路径；开循环移动结点寻找路径，设定循环结束条件，open表为空或者最终结点在close表。

具体的，步骤s7中，在清扫过程中，进行路障扫描，遇到障碍时进入避障程序同时把障碍物所在位置信息记录进地图数据中，遇到障碍物时的避障碍策略为bug2障碍策略，具体为：

智能清洁机器人先跟踪障碍物的轮廓，当能直接移动到目标点时，立即离开；bug算法中的m-line连接qstart和qgoal为一条固定不变的直线；遇到障碍物时智能清洁机器人会进入轮廓跟踪模式，当到达m-line上一个接近目标点的位置后，继续沿m-line驶向目标；如果智能清洁机器人再次在m-line遇到上之前的撞击点，则到达目标的路径不存在；在清扫同时进行红外扫描并对陀螺仪传回数据检测，检测智能清洁机器人车体是否离开地面；若检测到车体离开地面，则停止所有功能进入待机状态。

与现有技术相比，本发明至少具有以下有益效果：

本发明智能清洁机器人能够凭借多传感器信息融合技术，记录环境地图并实时判断现处的位置，并基于先进路径规划技术设计出最优清洁路径，可以在清洁地面时选择最优路径来节省清洁时间，同时实现对其清扫环境的最大覆盖率与清洁程度。

进一步的，能够凭借着多传感器信息融合技术，结合市场现有功能进行拓展，通过温湿度传感器或者摄像头获取室内环境状态，在出现异常时通过云平台，将异常情况发送至手机或者web端，远程监控家中的环境。

进一步的，精确定位智能扫地机器人在构建的环境地图上的位置，精确定位障碍物如桌椅等所在位置，实现能做到最大清洁度的最优路径规划，同时节约清洁时间。

进一步的，人机交互模块用于深度简化操作流程，优化人机交互中机器的易用性，通过扩展延伸其功能提高扫地机器人的智能反馈；使得用户更加方便地操控扫地机器人，随时随地能通过wifi/蓝牙进行远程无线操控。扩展智能扫地机器人的功能，提升机器反馈，了解清洁状态以及机器的使用程度耗损程度，性能状态。同时利用摄像头，传感器使扫地机器人充当智能监控系统，让人在外出途中可以通过扫地机器人实时关注家庭情况，包括水电状况、安全情况、小孩老人的身体状况，实现一机多用，功能充分扩展延伸。

本发明还公开了一种智能清洁机器人路径规划方法，按照开始，初始化，定位，构建几何-拓扑混合环境地图，规划最优路径，开始清扫，能够更改切换不同清扫模式，自行设置机器人返航电量，大容量电池电量低于预定值后机器人自动返回充电桩进行充电。并记录环境地图记录断点清扫位置；电量充足后返回中断点位置继续清扫，最大可能减少重复清扫。

进一步的，利用机身前侧成弧线布置的红外近距传感器和布置在机器顶部超声波测距传感器，构建实时环境信息分析反馈，并利用几何-拓扑混合地图技术建立环境地图。

进一步的，通过陀螺仪和光电编码器准确计算出实时速度，转过角度和位移，再根据几何-拓扑混合地图，对其自身进行定位；陀螺仪和光电编码器是常用的测量角速度和机器人运动状态的工具，能对速度、加速度和角速度进行精准测量，即可解出机器人移动的距离以及转过的角度,再根据依据几何-拓扑混合地图，对其自身进行定位。这种方法具有自参自测的优点,即无需外部信息即可自行测量其运动参数。与市场上的航迹推算不同的是，通过陀螺仪和光电编码器测得的物理量更加精确，且不会随工作时间的推移,逐渐累进积分运算误差和定系统时差造成的误差。根据依据几何-拓扑混合地图，对其自身进行定位，适合长时间的精准定位。

进一步的，智能扫地机器人在熟悉房间规划后，能够选择最佳清扫模式，制定最佳的清洁路径，做到全面清洁、节省电量、代价最小。保证扫地机器人在有限的工作时间里达到最优的清洁效率且清扫时间最短。相比于传统扫地机器人，更快更全面完成清扫工作，大幅度减少电量消耗。

进一步的，bug2障碍策略在宾馆、酒店、图书馆、办公场所与大众家庭等复杂性不强的场合可进行完美避障。

综上所述，本发明降低劳动强度、提高劳动效率，适用于宾馆、酒店、图书馆、办公场所与大众家庭。利用多传感器融合技术，采用最优环境地图绘制、路径规划与避障算法，大容量电池断点续航模式，增添侧刷，滚刷前增加扫风装置，做到省时节电的高效率卫生清扫。人机交互中机器中易用性增强，利用摄像头与传感器，使扫地机器人充当智能监控系统。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为本发明的系统总框图；

图2为本发明的硬件结构图；

图3为本发明的流程图；

图4为本发明的结构示意图；

图5为本发明的走廊几何-拓扑环境地图示意图；

图6为本发明的最优避障示意图；

图7为本发明的esp32第三方平台接入原理示意图。

其中，1.万向轮；2.滚刷；3.红外传感器；4.超声波传感器；5.拖板位；6.垃圾收纳盒；7.挡风板；8.侧刷；9.旋转边刷。

具体实施方式

请参阅图1，本发明一种智能清洁机器人，包括控制单元、驱动模块、传感器模块、人机交互模块、精准定位模块以及电源模块，驱动模块、传感器模块、人机交互模块以及电源模块分别与控制单元连接；传感器模块用于分析并反馈实时清洁环境信息；精准定位模块用于获取当前智能清洁机器人在环境地图上所在的位置；控制单元采用esp32双模中央控制单元，用于获取外部环境信息，利用几何-拓扑混合地图技术建立环境地图，通过结合环境地图和实时位置，用先进路径规划算法规划出最优清扫路径，并将数据上传云平台实现实时分析记录与控制；驱动模块用于按照规划出的最优路径驱动智能清洁机器人运行并进行清洁工作；人机交互模块能够利用温湿度传感器结合摄像头实现对智能清洁机器人工作状态和性能的显示，并通过wifi/蓝牙技术可以完成智能清洁机器人的遥控与预约功能，通过电源模块为智能清洁机器人供电。

请参阅图2，传感器模块包括，转角传感器，速度传感器，红外传感器以及超声波传感器。

其中，转角传感器用于对角度的实时测量或监控；速度传感器用来检测机器人运动速度；红外传感器与超声波传感器用来检测车体和路况，当随机障碍物出现在红外与超声波传感器的检测范围内时，传感器模块用于将检测信号发送给控制单元控制扫地机器人实现躲避。

精准定位模块包括陀螺仪、光电编码器和摄像头，陀螺仪用于采集车身偏移的角速度、速度和加速度，判断智能清洁机器人的行走方向，配合室内导航定位系统与行走电机的运行状况；光电编码器用于采集智能清洁机器人的实际速度，摄像头设置在智能清洁机器人机身顶部，180°旋转用于监控和辅助定位。

陀螺仪模块(含三轴加速度计)用来采集车身偏移的角速度、速度和加速度，判断出机器人的行走方向，配合室内导航定位系统与行走电机的运行状况，准确定位出xoy坐标碰撞传感器用于判断突发的碰撞情况；

控制单元以esp32双模中央控制单元为核心芯片，配备外围电路进行完善。

传感器信号输入输出端与控制单元的传感器信号输入输出端相连，通过对传感器反馈信号的读取改变智能清洁器机人的运行状态，同时通过驱动模块控制电机的运行；利用多传感器反馈信息的融合实现对智能清扫机器人路况与运动状态及时判断；通过声光提示模块提示主面板的信号从而控制智能清洁机器人的模式转换。

人机交互模块包括通信模块(包括但不限于手机、电脑以及上位机通信)，无线通信单元，按钮，摄像头，温湿度传感器，显示屏以及led。

其中，无线通信单元实现手机与主控芯片进行信息传输；按钮可以供用户选择清扫模式及运动速度等；

温湿度传感器及摄像头将与esp32芯片相结合，通过esp32第三方平台，在手机或者web端程序，远程监测家中情况。

请参阅图3，开始工作后，进行功能选择，智能清洁机器人返回充电基座，进行障碍和悬空扫描，进行位置计算和路径规划，进行电量检测，如果电量不充足，返回充电基座，电量充足，进行清洁完成度检测，如果清洁完成，返回充电基座，如果清洁未完成，返回进行障碍和悬空扫描。当智能清洁机器人处于不同模式或工作状态时，点亮不同的led灯，提醒用户；将电源模块的电量，当前清扫完成度等信息显示在显示屏上。

用户可以通过http协议在web服务器上进行独立应用授权，以确保隐私安全性。在web服务器上注册用户，管理此扫地机器人设备，创建与设备的关联，再通过web服务器与数据库中心进行信息的上传下载、交互，校准，更新。

在物联网层面，用户通过mqtt协议把扫地机器人的即时更新信息传给数据库中心，然后进行保存。用户通过登录uid进行token的身份验证，也可以随时获取机器人的实时数据，控制设备自行检查、更新、操控机器人。

驱动模块包括电机驱动电路、直流无刷电机、信号检测电路、系统保护电路。

请参阅图4，本发明智能清洁机器人的机身呈扁圆状，采用前端两个旋转边刷9+真空吸尘+紫外灯杀菌+抗菌纤维抹布的清洁装置方案，在前侧的圆弧形机身内设置十一组红外传感器3与超声波传感器4用来检测障碍；利用布置在机器反射人内部的小型真空吸尘器和布置在机器人前端的旋转边刷9将地面杂物先吸纳进入自身的垃圾收纳盒6，从而完成地面清理功能。

超声波传感器4互成九十度夹角，利用充电基座发出的超声波信号和机器人顶部布置在四角的超声波传感器4，确定充电基座的位置并能在电量不足的时候自动回基座冲电。

摄像头位于机身顶部在四角分布的超声波传感中间，用于辅助精准定位与优化路径。

机身底部呈三角分布两个动力轮与一个万向轮1，旋转边刷9分别设置在万向轮1的两侧，位于万向轮1和两个动力轮之间，动力轮外侧分别设置有挡风板7，挡风板7的两侧机身上分别设置有侧刷8，机身底部中间设置进污口，进污口下侧设置拖板位5，进污口内设置滚刷2，进污口与机身内部的垃圾收纳盒6连通，机身上方分布按键与取放垃圾装置。

本发明以圆盘机身为载体，利用陀螺仪模块(含三轴加速度计)和光电编码器实现室内定位、通过布置在机身前侧圆弧的红外传感器和机身上部的四个超声波传感器感知外部环境、通过机身上部的超声波传感器与充电基座发射的超声波来确定其相对充电机座的位置，并在电量不足设定值时自动回冲电基座充电、利用先进算法自动规划最优清扫路径。本装置主要用于实现清洁时间最省，覆盖面积最大的室内清洁。

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中的描述和所示的本发明实施例的组件可以通过各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图3，本发明一种智能清洁机器人的路径规划方法，按照开始，初始化，定位，构建几何-拓扑混合环境地图，规划最优路径，开始清扫，具体过程如下：

s1、开始，打开按钮，连接家庭wifi/手机蓝牙，对清洁机器人进行初始化设定；

s2、用户根据需求对清洁机器人进行功能选择；

s3、开始沿边搜索充电基站(充电基站为坐标原点)，回到基站后，若电量不足则充电，电量充足则进入步骤s4；

s4、利用机身前侧成弧线布置的若干红外近距传感器，和布置在机器顶部超声波测距传感器，构建实时环境信息分析反馈(机器人周围障碍物信息和充电基座方位信息)，如图5所示，在对智能扫地机器人的环境地图进行抽样表述时，采用几何与拓扑两种描述方式。墙壁、房门、通路等静态物体与环境特征采用几何元素表示，各种对象的关系采用拓扑严格定量测量。拓扑节点与几何特征相互绑定，拓扑边给出相邻拓扑节点的可行路径与局部环境切换的可行通路。

401、在已知环境地图的情况下，机器人将使用超声波传感器和红外传感器获得信息反馈，结合几何-拓扑混合定位与已有环境地图进行对比，当出现不一致之处时则更新环境地图信息，当变动量超过阀值则重新绘制环境地图。

402、当未知环境地图的情况下，机器人将利用近距红外传感器实现对环境感知，整体环境通过拓扑节点串联，其中保证局部精确定位，拓扑节点采用几何表述。同时维护环境地图表述的整体一致性。其考虑方面如下：

提取常见环境特征

建立拐角垂直线段

根据经验阈值判断

定位拐点端点坐标

几何-拓扑混合地图定位推算公式如下：

设形成拐角两条近似垂直线段为(x1,y1,xc1,yc1)与(x2,y2,xc2,yc2)；处于拐角区线段端点坐标为(xc1,yc1)和(xc2,yc2)，阈值经验值记为es和ed；则在实际中拐角判断如下：

实际拐角断点坐标(xc,yc)可近似计算为：

s5、利用陀螺仪模块(含三轴加速度计)与光电编码器配合算法完成定位。该方法通过对陀螺仪模块(含三轴加速度计)提供的加速度和角速度进行二重积分，光电编码器亦同时记录航迹，并对陀螺仪所得到的运行轨迹进行修正，并依据轨迹推算基本算法，来对自身进行精确定位。

轨迹推算公式如下：

其中a(x)表示x时刻的加速度，w(x)表示x时刻的角加速度，s表示t时刻的位移，表示t时刻转过角度，这几个量均为矢量，s(0)＝0，θ(0)＝0。

用数值分析的方法，将利用梯形数值积分公式，化简运算公式为

其中，an表示加速度，vn表示速度，sn表示位移，wn表示角速度，θn表示转过角度，δt测量时间间隔，参数角标n表示第n个测量点得到的数据，其中an，vn，sn具有水平和竖直分量。

航迹推算的过程的每一次推算并不需要对之前所有的加度速度两次数值积分得到位置坐标，上一次的梯形积分得到的速度、位置结果中已经包含之前所有运动历史数据的信息，只需在上一次的速度、位置上进行递推即可。

vn＝vn-1+0.5an-1+0.5an

sn＝sn-1+0.5vn-1+0.5vn

θn＝θn-1+0.5wn-1+0.5wn

s6、在已知环境地图和实时位置时，可以利用先进路径规划算法自动规划出最优清扫路径；

利用已有的几何-拓扑混合地图规划最优清扫路径，实现清洁时间最省。其基本思想分为地图已知情况下的单元域规划和地图未知情况下的路径规划。

当未知环境地图的情况下，机器人利用近距红外传感器，构建实时环境信息分析反馈，并将整体环境通过拓扑节点串联，构建几何-拓扑环境地图后，进行地图已知情况下的单元域内路径规划。

地图已知情况下的单元域域内规划遵循的基本原则是：修扫边界，制造矩形，平行铺扫，兼顾障碍。采用离线事先规划的启发式搜索a*算法，在规划完毕之后，可以沿着最优路径移动。

在对节点按以下的方式进行排序：

f＝g+h

其中，f是计算出的权值，f值越大代表节点收益越小，g指从初始节点到达现在的节点的过程中付出的代价；h值指目前状态到目标状态预计代价；估算越准确，所需遍历节点越少。

对于h值的计算是采用启发式函数h(n)告诉a*从任意结点n到目标结点的最小代价评估值，从而控制a*的行为。

当启发式函数精确地等于实际最佳路径，此时a*扩展的结点将减少。a*算法则会实现：在每一结点它都计算f(n)＝g(n)+h(n)。当h(n)精确地和g(n)匹配时，f(n)的值在沿着该路径时将不会改变。不在最短路径上的所有结点的f值均大于正确路径上的f值。如果已经有较低f值的结点，a*将不考虑f值较高的结点，因此将不会偏离最短路径。

采用构造精确启发函数需要预先计算任意一对结点之间最短路径的长度。之后添加一个启发函数h’用于评估从任意位置到达邻近导航点(w)的代价。最终的启发式函数可以是：

h(n)＝h'(n,w1)+distance(w1,w2),h'(w2,goal)

我们参考了标准的启发式函数--曼哈顿距离。考虑代价函数并找到从一个位置移动到邻近位置的最小代价d。因此，启发式函数应该是曼哈顿距离的d倍：

h(n)＝d*(abs(n.x–goal.x)+abs(n.y–goal.y))

如果单位可以沿着任意角度移动(而不是网格方向)，那么可以使用直线距离：

h(n)＝d*sqrt((n.x-goal.x)^2+(n.y-goal.y)^2)

为了在搜索最优路径而带来的低效率问题(当某些路径具有相同的f值的时候，它们都将会被搜索，而我们只需要搜索其中的一条)将采用以下解决策略：我们将为启发函数添加一个附加值，附加值对于结点必须是确定性的，而且必须让f值体现差异。当a*对f值排序时，对于不同的f值将只有一个“需要的”的f值会被检测。

使用两个辅助表来记录结点，用于记录可被访问的结点，为open表；记录已访问过结点为close表；两个表决定了算法结束：条件是最终结点在close表中(找到路径)或open表为空(找不到了路径)；之后就来移动当前的节点，寻找路径；每次从open表中取出f值最小的结点出来(使用优先队列来处理)，作为当前结点；然后将当前结点的所有邻结点按照邻结点规则加入到open表中；最后将当前结点放入close表中，这里就是每次循环的执行内容。

邻结点规则：

(1)当邻结点不在地图中，不加入open表；

(2)当邻结点是障碍物，不加入open表；

(3)当邻结点在close表中，不加入open表；

(4)当邻结点不在open中，加入open表，设该邻结点的父节点为当前结点；

(5)当邻结点在open表中，需要做个比较：如果邻结点的g值>当前结点的g值+当前结点到这个邻结点的代价，那么修改该邻结点的父节点为当前的结点(在open表中的结点除了起点，都会有父节点)，修改g值＝当前结点的g值+当前结点到这个邻结点的代价。

具体步骤如下：

1.输入

(1)输入代表地图二值二维数组map＝(x，y)；

(2)按照二维数组特点，坐标原点在左上角，y是高，x是宽，y向下递增，x向右递增；将x和y封装成一个类，进行传参，重写equals方法比较坐标(x,y)；map(k)＝map(k-1)，当x(k)＝x(k-1)，y(k)＝y(k-1)，否则map(k)＝map(k-2)，当x(k)不等于x(k-1)或y(k)不等于y(k-1)；

(3)封装路径结点类，字段包括：坐标、g值、f值、父结点，实现comparable接口，方便优先队列排序。

(4)最后数据结构为a*算法输入的所有数据，封装在一起，方便传参。

2.处理

(1)算法中定义几个常量来确定：二维数组中bar＝1值表示障碍物、path＝2为二维数组中绘制路径的代表值、direct_value＝10表示计算g值需要横纵移动代价，direct_value＝14表示斜移动代价；

(2)使用priorityqueue与arraylist定义open和close辅助表分别取最小值与保存结点。

queue<node>openlist＝newpriorityqueue<node>()；

list<node>closelist＝newarraylist<node>()；

(3)定义布尔类判断方法：

最终结点的判断：returncoord！＝null&&end.equals(coord)；

结点能否加入open表的判断：

如果(x<0||x>＝mapinfo.width||y<0||y>＝mapinfo.hight)，则终止，否则判断(mapinfo.maps[y][x]＝＝bar)与(iscoordinclose(x,y)；

结点是否在close表中的判断：node.coord.x＝＝x&&node.coord.y＝＝y；

(4)计算h值，这里使用“曼哈顿”法，坐标分别取差值相加

(5)从open列表中查找结点

addneighbornodeinopen(mapinfo,current,x-1,y,direct_value)；//左

addneighbornodeinopen(mapinfo,current,x,y-1,direct_value)；//上

addneighbornodeinopen(mapinfo,current,x+1,y,direct_value)；//右

addneighbornodeinopen(mapinfo,current,x,y+1,direct_value)；//下

addneighbornodeinopen(mapinfo,current,x-1,y-1,oblique_value)；//左上

addneighbornodeinopen(mapinfo,current,x+1,y-1,oblique_value)；//右上

addneighbornodeinopen(mapinfo,current,x+1,y+1,oblique_value)；//右下

addneighbornodeinopen(mapinfo,current,x-1,y+1,oblique_value)；//左下

(6)添加邻结点到open表；

(7)回溯法绘制路径；

(8)开循环移动结点寻找路径，设定循环结束条件，open表为空或者最终结点在close表。

s7、按规划出的最优清扫路径运行，进行清扫；

在清扫过程中，进行路障扫描，遇到障碍时进入避障程序同时把障碍物所在位置信息记录进地图数据中。其中遇到障碍物时的避障碍策略为bug2障碍策略；说明如下：

避障算法bug2避障算法

机器人先跟踪障碍物的轮廓，当它能直接移动到目标点时，就立即离开。如图6所示，bug算法中的m-line连接qstart和qgoal，为一条固定不变的直线。遇到障碍物时机器人会进入轮廓跟踪模式，当其到达m-line上一个接近目标点的位置后(而非初次遇到障碍物的撞击点)，继续沿m-line驶向目标。如果机器人再次在m-line遇到上之前的撞击点，则到达目标的路径不存在。

在清扫同时进行红外扫描并对陀螺仪传回数据检测，检测车体是否离开地面。若检测到车体离开地面，则停止所有功能进入待机状态。

s8、判断是否打扫完全部可清洁格栅，若清扫完成则进入步骤s9；若没有结束，返回步骤s3；

s9、结束。

本发明机器人能够凭借着多传感器信息融合技术，自动记录环境地图并实时判断现处的位置，并基于先进路径规划技术，利用格栅地图与基于航迹的室内定位算法，将人机交互和传感器的扫描检测部分放在主函数的循环里，而将用于判断是否能运行放在中断里。设计出最优清洁路径。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。