CN113031616B - 一种清洁机器人返回路径规划方法、系统和清洁机器人 - Google Patents
一种清洁机器人返回路径规划方法、系统和清洁机器人 Download PDFInfo
- Publication number
- CN113031616B CN113031616B CN202110270459.3A CN202110270459A CN113031616B CN 113031616 B CN113031616 B CN 113031616B CN 202110270459 A CN202110270459 A CN 202110270459A CN 113031616 B CN113031616 B CN 113031616B
- Authority
- CN
- China
- Prior art keywords
- point
- cleaning robot
- key
- cleaning
- target
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000004140 cleaning Methods 0.000 title claims abstract description 275
- 238000000034 method Methods 0.000 title claims abstract description 51
- 238000001514 detection method Methods 0.000 claims abstract description 30
- 230000006870 function Effects 0.000 claims description 7
- 230000001960 triggered effect Effects 0.000 claims 2
- 238000013500 data storage Methods 0.000 abstract description 5
- 238000010586 diagram Methods 0.000 description 6
- 238000003860 storage Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000005299 abrasion Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000011065 in-situ storage Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- A—HUMAN NECESSITIES
- A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
- A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
- A47L11/00—Machines for cleaning floors, carpets, furniture, walls, or wall coverings
- A47L11/40—Parts or details of machines not provided for in groups A47L11/02 - A47L11/38, or not restricted to one of these groups, e.g. handles, arrangements of switches, skirts, buffers, levers
- A47L11/4011—Regulation of the cleaning machine by electric means; Control systems and remote control systems therefor
-
- A—HUMAN NECESSITIES
- A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
- A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
- A47L11/00—Machines for cleaning floors, carpets, furniture, walls, or wall coverings
- A47L11/40—Parts or details of machines not provided for in groups A47L11/02 - A47L11/38, or not restricted to one of these groups, e.g. handles, arrangements of switches, skirts, buffers, levers
- A47L11/4061—Steering means; Means for avoiding obstacles; Details related to the place where the driver is accommodated
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
-
- A—HUMAN NECESSITIES
- A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
- A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
- A47L2201/00—Robotic cleaning machines, i.e. with automatic control of the travelling movement or the cleaning operation
- A47L2201/04—Automatic control of the travelling movement; Automatic obstacle detection
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种清洁机器人返回路径规划方法、系统和清洁机器人,该方法包括:将清洁机器人清扫开始时的起点标记为关键点存入关键路径队列的头部,并将关键点的索引设置为初始索引;将清洁机器人清扫过程中的中间点标记为关键点存入关键路径队列中,并将关键点的索引进行累加;确定清洁机器人完成清扫后的终点,从关键路径队列开始遍历,通过检测终点与关键点之间是否存在障碍查询目标点;根据终点与目标点的位置获取目标角度,控制清洁机器人沿目标角度运动至目标点;通过检测目标点是否为关键路径队列中的第一个关键点,控制清洁机器人执行与检测结果对应的操作。本发明能够减少数据存储量,降低内存开销,以及提高返回路径的规划速度。
Description
技术领域
本发明属于清洁机器人技术领域,尤其涉及到一种清洁机器人返回路径规划方法、系统和清洁机器人。
背景技术
当前市面上的清洁机器人,通常使用基于占栅格地图的宽度优先搜索或者A星搜索进行返回起点的路径规划,主要存在以下缺陷:一方面针对较大的室内空间,会导致数据存储量增大,增加了内存开销;另一方面搜索路径时,会使得机器人原地停留1~5s,降低了用户体验。
发明内容
本发明的目的在于提供一种清洁机器人返回路径规划方法、系统和清洁机器人,以解决清洁机器人使用基于占栅格地图的宽度优先搜索或者A星搜索进行返回起点的路径规划所存在的上述问题。
基于上述目的,第一方面,本发明提供一种清洁机器人返回路径规划方法,包括:
确定清洁机器人清扫开始时的起点,将所述起点标记为关键点存入关键路径队列的头部,并将所述关键点的索引设置为初始索引;
确定所述清洁机器人清扫过程中的中间点,将所述中间点标记为所述关键点存入所述关键路径队列中,并将所述关键点的索引进行累加;
确定所述清洁机器人完成清扫后的终点,从所述关键路径队列开始遍历,通过检测所述终点与所述关键点之间是否存在障碍查询目标点;
根据所述终点与所述目标点的位置获取目标角度,控制所述清洁机器人沿所述目标角度运动至所述目标点;
通过检测所述目标点是否为所述关键路径队列中的第一个所述关键点,控制所述清洁机器人执行与检测结果对应的操作。
优选地,所述确定所述清洁机器人清扫过程中的中间点,将所述中间点标记为所述关键点存入所述关键路径队列中,并将所述关键点的索引进行累加,包括:
在所述清洁机器人清扫过程中,进行占据栅格地图标记;
控制所述清洁机器人在所在的待清扫区域进行弓形清扫;
在所述待清扫区域清扫完成后,控制所述清洁机器人进行右侧沿墙运动,并在右侧沿墙运动时,将根据预设中间点标记策略获得的中间点标记为所述关键点存入所述关键路径队列中;
控制清洁机器人移动下一个的待清扫区域,返回步骤:控制所述清洁机器人在所在的待清扫区域进行弓形清扫,直至包含多个所述待清扫区域的预设空间清扫完成。
优选地,所述检测所述终点与所述关键点之间是否存在障碍,包括:
获取两个点的位置;
根据反三角函数计算第二个点相对于第一个点的移动角度;
根据单位移动距离和移动次数获取移动距离,并根据所述移动角度和所述移动距离确定途径点;
检测所述移动距离是否大于两个点之间的距离;
若所述移动距离大于两个点之间的距离,则确定两个点之间没有障碍;
若所述移动距离小于或等于两个点之间的距离,则在占据栅格地图中查询所述途径点是否为障碍;
若所述途径点为障碍,则确定两个点之间有障碍;
若所述途径点不为障碍,则对移动次数进行累加,并返回步骤:根据单位移动距离和移动次数获取移动距离,并根据所述移动角度和所述移动距离确定途径点。
优选地,所述目标角度为:
其中,θ为所述第二个点p2相对于所述第一个点p1的目标角度,且p1=(x1,y1),p2=(x2,y2)。
优选地,所述根据所述终点与所述目标点的位置获取目标角度,控制所述清洁机器人沿所述目标角度运动至所述目标点,包括:
通过计算所述清洁机器人的当前朝向角度与目标角度之间的差值获得偏差角度,并获取所述清洁机器人的当前角速度和最大轮速;
检测所述偏差角度的绝对值是否大于预设调整阈值;
若是,将所述偏差角度、所述当前角速度和所述最大轮速输入至预设控制器,通过所述预设控制器中的调整算法获取左右轮速度差,根据所述左右轮速度差和所述最大轮速调整左轮轮速和右轮轮速;
若否,根据所述偏差角度的数值符号设置左轮轮速和右轮轮速;
根据所述左轮轮速和所述右轮轮速控制所述清洁机器人沿所述目标角度运动至所述目标点。
优选地,所述预设控制器中的调整算法为:
其中,dv为所述左右轮速度差;eθ为所述偏差角度;A为所述清洁机器人的当前角速度;S为所述清洁机器人的最大轮速;δ为常数;k1、k2分别为第一调整参数和第二调整参数,且k1、k2均根据所述预设控制器的类型进行设置。
优选地,所述通过检测所述目标点是否为所述关键路径队列中的第一个所述关键点,控制所述清洁机器人执行与检测结果对应的操作,包括:
若检测结果为所述目标点不是所述关键路径队列中的第一个所述关键点,则控制所述清洁机器人执行目标点查询操作;
若检测结果为所述目标点是所述关键路径队列中的第一个所述关键点,则控制所述清洁机器人执行成功返回操作。
第二方面,本发明提供一种清洁机器人返回路径规划系统,包括:
起点确定模块,用于确定清洁机器人清扫开始时的起点,将所述起点标记为关键点存入关键路径队列的头部,并将所述关键点的索引设置为初始索引;
关键点标记模块,用于确定所述清洁机器人清扫过程中的中间点,将所述中间点标记为所述关键点存入所述关键路径队列中,并将所述关键点的索引进行累加;
目标点查询模块,用于确定所述清洁机器人完成清扫后的终点,从所述关键路径队列开始遍历,通过检测所述终点与所述关键点之间是否存在障碍查询目标点;
角度控制模块,用于根据所述终点与所述目标点的位置获取目标角度,控制所述清洁机器人沿所述目标角度运动至所述目标点;
返回确定模块,用于通过检测所述目标点是否为所述关键路径队列中的第一个所述关键点,控制所述清洁机器人执行与检测结果对应的操作。
优选地,所述角度控制模块包括:
参数获取单元,用于通过计算所述清洁机器人的当前朝向角度与所述目标角度之间的差值获得偏差角度,并获取所述清洁机器人的当前角速度和最大轮速;
角度检测单元,用于检测所述偏差角度的绝对值是否大于预设调整阈值;
角度调整单元,用于若是,将所述偏差角度、所述当前角速度和所述最大轮速输入至预设控制器,通过所述预设控制器中的调整算法获取左右轮速度差,根据所述左右轮速度差和所述最大轮速调整左轮轮速和右轮轮速;若否,根据所述偏差角度的数值符号设置左轮轮速和右轮轮速;
角度控制单元,用于根据所述左轮轮速和所述右轮轮速控制所述清洁机器人沿所述目标角度运动至所述目标点。
第三方面,本发明提供一种清洁机器人,包括处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的返回路径规划程序,所述处理器执行所述返回路径规划程序时实现如权上述的清洁机器人返回路径规划方法。
上述清洁机器人返回路径规划方法、系统和清洁机器人,将清洁机器人清扫开始所在位置的起点,以及清扫过程中顺次获取的中间点作为关键点存入关键路径队列中,在清扫完成后,从关键路径队列的头部遍历,通过检测清洁机器人当前所在位置与关键路径队列中每一个点是否存在障碍查询目标点,并根据当前所在位置和目标点获取目标角度,进而控制清洁机器人沿目标角度方向运行至目标点,当检测到目标点为关键路径队列中的第一个关键点时,确定成功返回起点。上述清洁机器人返回路径规划方法,通过关键路径队存储用于规划返回路径的关键点,能够减少数据存储量,降低内存开销;同时通过检测两个点是否存在障碍查询目标点,能够提高目标点查询效率,进而提高返回路径的规划速度,以及用户体验度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本发明一实施例中清洁机器人返回路径规划方法的一流程图;
图2为本发明一实施例中的键路径队列的顺序存储结构图;
图3为本发明一实施例中的关键点标记图
图4为本发明一实施例中清洁机器人返回路径规划方法的步骤S30的一流程图;
图5为本发明一实施例中清洁机器人返回路径规划方法的步骤S40的一流程图;
图6为本发明一实施例中的清洁机器人的预期返回路径图;
图7为本发明一实施例中清洁机器人返回路径规划系统的一原理框图;
图8为本发明一实施例中清洁机器人返回路径规划系统的角度控制模块的一原理框图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分的实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都应当属于本发明保护的范围。
如图1所示,本发明一实施例提供一种清洁机器人返回路径规划方法,包括如下步骤:
步骤S10,确定清洁机器人清扫开始时的起点,将起点标记为关键点存入关键路径队列的头部,并将关键点的索引设置为初始索引。
在本实施例中,如图2所示的关键路径队列的顺序存储结构,关键路径队列中包含N个关键点,且N个关键点的索引从初始索引开始依次递增。可选地,初始索引为0。
具体的,在清洁机器人清扫开始时,将清扫开始时所在位置确定为清洁机器人的起点P1,并将起点P1标记为关键点存入关键路径队列中,同时将起点P1所对应的关键点在关键路径队列中的索引设置为0。
步骤S20,确定清洁机器人清扫过程中的中间点,将中间点标记为关键点存入关键路径队列中,并将关键点的索引进行累加。
具体的,在清洁机器人清扫过程中,根据预设中间点标记策略顺次获取中间点Pi,并将中间点Pi标记为关键点存入关键路径队列中,同时将中间点Pi所对应的关键点在关键路径队列中的索引进行累加,如图2所示的关键路径队列的顺序存储结构图,由图2可知,关键路径队列中的第i个关键点Pi的索引为i+1,i=0,1,…,N,N为关键路径队列的关键点总数。
在本实施例中,预设中间点标记策略是指,清洁机器人右侧沿墙运动时,若触发墙角右转,将清洁机器人完成右转后、继续沿墙行走前的所在位置确定为第一个中间点,以及将清洁机器人沿当前朝向角度的右侧90°移动预设距离值后的所在位置确定为第二个中间点,以将这两个中间点标记为关键点存入关键路径队列中。其中,预设距离值根据墙厚和清洁机器人的机身尺寸进行设置。
作为优选,步骤S20包括以下步骤:
步骤一,在清洁机器人清扫过程中,进行占据栅格地图标记。
步骤二,控制清洁机器人在所在的待清扫区域进行弓形清扫。
步骤三,在待清扫区域清扫完成后,控制清洁机器人进行右侧沿墙运动,并在右侧沿墙运动时,将根据预设中间点标记策略获得的中间点标记为关键点存入关键路径队列中。
步骤四,控制清洁机器人移动下一个的待清扫区域,返回步骤二,循环继续执行弓形清扫和中间点标记,直至包含多个待清扫区域的预设空间清扫完成。其中,多个待清扫区域为两个或两个以上的待清扫区域,每一个待清扫区域对应一个房间。
可理解的,清洁机器人在所在的待清扫区域进行清扫时,进行占据栅格地图标记,确定待清扫区域中包含的障碍物信息,控制清洁机器人在待清扫区域开始弓形清扫,并实时检测到前方存在标记过的障碍物时,控制清洁机器人进行转向之后,继续开始弓形清扫,直至完成待清扫区域的清扫后,通过机身与墙体保持预设安全距离进行右侧沿墙运动,到达下一个待清扫区域后继续进行全覆盖的清扫,使得清洁机器人不断运行,直到完成整个预设空间的清扫后,开始返回起点,即进入步骤S30。其中,预设安全距离设置为1~3厘米,可以避免在清洁机器人右侧沿墙运动时,机身与墙体接触造成机身磨损的问题。
在清洁机器人右侧沿墙运动时,根据预设中间点标记策略获取中间点,并将中间点标记为关键点存入关键路径队列,同时对中间点对应的关键点在关键路径队列中的索引进行加一操作。如图3所示的关键点标记图,图3中预设空间中包含4个待清扫区域,索引为0的关键点对应清洁机器人清扫开始时的起点,索引为1-8的关键点对应清洁机器人清扫过程中顺次获取的中间点。
上述实施例,在清洁机器人清扫过程中,采用弓形清扫和右侧沿墙的运行模式对多个待清扫区域进行清扫,同时通过预设中间点标记策略顺次获取中间点,以将中间点作为关键点存入关键路径队列中,能够高效率地完成多个房间的清扫作业,且只需存储较少的关键点,减少了数据存储量,有利于提高目标点查询效率。
步骤S30,确定清洁机器人完成清扫后的终点,从关键路径队列的头部开始遍历,通过检测终点与关键点之间是否存在障碍查询目标点。
在清洁机器人完成预设空间的清扫后,将清扫完成后所在位置确定为清洁机器人的终点,从关键路径队列的头部开始遍历,检测关键路径队列的每一个关键点与终点之间是否存在故障,若存在,则继续遍历下一个关键点,否则,将所遍历的关键点确定为清洁机器人下一个需要达到的目标点。
作为优选,如图4所示,步骤S30中检测终点与关键点之间是否存在障碍,具体包括包括以下步骤:
步骤S301,获取两个点的位置,其中,第一个点可以表示p1=(x1,y1),第二个点可以表示p2=(x2,y2)。
步骤S302,根据反三角函数计算第二个点p2相对于第一个点p1的目标角度θ。其中,反三角函数具体表示为:
步骤S303,根据单位移动距离b和移动次数n获取移动距离h,并根据目标角度θ和移动距离h确定途径点C=(x,y)。其中,单位移动距离b根据栅格单元尺寸进行设置。
步骤S304,检测移动距离h是否大于两个点之间的距离d(p1,p2)。
步骤S305,若移动距离h大于两个点之间的距离d(p1,p2),则确定两个点之间没有障碍。
步骤S306,若移动距离h小于或等于两个点之间的距离d(p1,p2),则在占据栅格地图中查询途径点C是否为障碍。
步骤S307,若途径点C为障碍,则确定两个点之间有障碍。
步骤S308,若途径点C不为障碍,则对移动次数n进行累加,并返回步骤S303。
在本实施例中,首先将清洁机器人的终点作为第一个点p1,将关键路径队列中当前遍历的关键点作为第二个点p2,根据反正切函数计算第二个点p2相对于第一个点p1的目标角度θ,并设置移动次数n的初始值;然后沿目标角度θ方向上将第一个点p1移动n×b的距离h获得途径点C=(x,y),并检测距离h是否大于p1、p2之间的距离d(p1,p2);若h>d(p1,p2),则结束检测过程,确定p1、p2之间没有障碍,以将当前遍历的点作为下个目标点,若h≤d(p1,p2),则根据标记有障碍物信息的占据栅格地图进一步检测途径点C是否为障碍;若途径点C是障碍,则结束检测过程,确定p1、p2之间有障碍,以获取下一个遍历点进行两点间障碍检测,否则,对移动次数n进行累加,累加后的移动次数n=n+1,并返回步骤S303,以获取下一个途径点继续进行两点间障碍检测,直至h>d(p1,p2)。
上述实施例中,在清洁机器人返回过程中,从关键路径队列的头部进行遍历,通过检测两点之间是否存在障碍查询下个目标点,目标点查询过程中产生的数据量少,且查询时间大大缩短,能够明显提升查询效率,进而提高返回路径的规划速度,使得清洁机器人无需在原地停留即可直接返回起点,有利于提高用户体验。
步骤S40,根据终点与目标点的位置获取目标角度,控制清洁机器人沿目标角度运动至目标点。
在本实施例中,根据清洁机器人当前所在点(即终点)的位置与根据步骤S30获得的下个目标点的位置采用反三角函数计算目标点相对于终点的目标角度,通过计算机器人当前朝向角度与目标角度的差值得到偏差角度,根据偏差角度采用预设控制器获得左右轮速,根据左右轮速控制清洁机器人沿目标角度的运动。
作为优选,如图5所示,步骤S40具体包括以下步骤:
步骤S401,通过计算清洁机器人的当前朝向角度与目标角度之间的差值获得偏差角度eθ,并获取清洁机器人的当前角速度A和最大轮速S。
步骤S402,检测偏差角度eθ的绝对值是否大于预设调整阈值Tθ。
步骤S403,若是,将偏差角度eθ、当前角速度A和最大轮速S输入至预设控制器,通过预设控制器中的调整算法获取左右轮速度差dv,根据左右轮速度差dv和最大轮速S调整左轮轮速和右轮轮速。其中,预设控制器的调整算法为:
在公式(2)中,S为清洁机器人的最大轮速,δ为常数,k1、k2分别为第一调整参数和第二调整参数,且k1、k2均根据控制器类型进行设置。可选的,若预设控制器为比例-微分控制器,则k1为比例系数,k2为微分系数。
具体的,在采用公式(2)计算得到左右轮速度差dv之后,若左右轮速度差dv大于零,则左轮轮速为最大轮速S,且右轮轮速为S-dv;若左右轮速度差dv小于零,则左轮轮速为S-dv,右轮轮速为最大轮速S。
步骤S404,若否,根据偏差角度eθ的数值符号设置左轮轮速和右轮轮速。
具体的,在偏差角度eθ的绝对值小于等于预设调整阈值Tθ,也即|eθ|≤Tθ,进一步判断偏差角度eθ的数值符号,若偏差角度eθ的数值符号为正号,即偏差角度eθ大于零,则左轮全速正转,且右轮全速反转;若偏差角度eθ的数值符号为负号,即偏差角度eθ小于零,则左轮全速反转,且右轮全速正转。
步骤S405,根据左轮轮速和右轮轮速控制清洁机器人沿目标角度运动至目标点。
上述实施例中,在清洁机器人返回过程中,在检测到清洁机器人的朝向角度与目标角度存在偏差时,通过预设控制器计算左右轮速度差,并根据左右轮速度差调整清洁机器人的左右轮速,能够有效准确地调整清洁机器人返回过程中运行角度,进而保证清洁机器人能够准确返回起点。
步骤S50,通过检测目标点是否为关键路径队列中的第一个关键点,控制清洁机器人执行与检测结果对应的操作。其中,清洁机器人执行的操作包括在检测结果为目标点不是关键路径队列中的第一个关键点时对应的查询目标点操作,以及检测结果为目标点是关键路径队列中的第一个关键点时对应的成功返回操作。
在本实施例中,当检测到清洁机器人距离目标小于预设到达阈值时,确定清洁机器人达到目标点,若目标点不是关键路径队列中的第一个关键点,则控制清洁机器人执行查询目标点操作,查询目标点操作具体包括:确定清洁机器人当前到达的目标点所对应的关键点在关键路径队列中的索引i,从关键路径队列的头部遍历,通过检测关键路径队列中索引为0至i-1的每一个关键点到索引为i的关键点之间是否存在故障查询下个目标点,进而根据当前目标点与下个目标点的位置获取目标角度,控制清洁机器人沿目标角度运动至下个目标点,直至检测到下个目标点为关键路径队列中的第一个关键点。
而若目标点是关键路径队列中的第一个关键点,则控制清洁机器人执行成功返回操作,成功返回操作可以为向用户发出成功返回的语音提示,也可以为向用户发出成功返回的灯光提示等,本实施例中清洁机器人的预期返回路径如图6所示,图6中清洁机器人的预期返回路径为:终点→索引为7的关键点→索引为3的关键点→索引为0的关键点(即起点)。
综上所述,本实施例提供的清洁机器人返回路径规划方法,将清洁机器人清扫开始所在位置的起点,以及清扫过程中顺次获取的中间点作为关键点存入关键路径队列中,在清扫完成后,从关键路径队列的头部遍历,通过检测清洁机器人当前所在位置与关键路径队列中每一个点是否存在障碍查询目标点,并根据当前所在位置和目标点获取目标角度,进而控制清洁机器人沿目标角度方向运行至目标点,当检测到目标点为关键路径队列中的第一个关键点时,确定成功返回起点。本实施例提供的清洁机器人返回路径规划方法,通过关键路径队存储用于规划返回路径的关键点,能够减少数据存储量,降低内存开销;同时通过检测两个点是否存在障碍查询目标点,能够提高目标点查询效率,进而提高返回路径的规划速度,以及用户体验度。
如图7所示,本发明实施例还提供一种清洁机器人返回路径规划系统,包括起点确定模块110、关键点标记模块120、目标点查询模块130、角度控制模块140和返回确定模块150,各功能模块的详细说明如下:
起点确定模块110,用于确定清洁机器人清扫开始时的起点,将起点标记为关键点存入关键路径队列的头部,并将关键点的索引设置为初始索引。
关键点标记模块120,用于确定清洁机器人清扫过程中的中间点,将中间点标记为关键点存入关键路径队列中,并将关键点的索引进行累加。
目标点查询模块130,用于确定清洁机器人完成清扫后的终点,从关键路径队列开始遍历,通过检测终点与关键点之间是否存在障碍查询目标点。
角度控制模块140,用于根据终点与目标点的位置获取目标角度,控制清洁机器人沿目标角度运动至目标点。
返回确定模块150,用于通过检测目标点是否为关键路径队列中的第一个关键点,控制清洁机器人执行与检测结果对应的操作。
进一步地,所述关键点标记模块120包括以下栅格标记单元、弓形清扫单元、标记单元和清扫控制单元,各功能单元的详细说明如下:
栅格标记单元,用于在清洁机器人清扫过程中,进行占据栅格地图标记。
弓形清扫单元,用于控制清洁机器人在所在的待清扫区域进行弓形清扫。
标记单元,用于在待清扫区域清扫完成后,控制清洁机器人进行右侧沿墙运动,并在右侧沿墙运动时,将根据预设中间点标记策略获得的中间点标记为关键点存入关键路径队列中。
清扫控制单元,用于控制清洁机器人移动下一个的待清扫区域,循环执行弓形清扫单元和点标记单元,直至包含多个待清扫区域的预设空间清扫完成。
进一步地,所述目标点查询模块130包括以下位置获取单元、角度计算单元、途径点确定单元、距离检测单元、第一障碍检测单元和第二障碍检测单元,各功能单元的详细说明如下:
位置获取单元,用于获取两个点的位置。
角度计算单元,用于根据反三角函数计算第二个点相对于第一个点的移动角度。
途径点确定单元,用于根据单位移动距离和移动次数获取移动距离,并根据移动角度和移动距离确定途径点。
距离检测单元,用于检测移动距离是否大于两个点之间的距离。
第一障碍检测单元,用于若移动距离大于两个点之间的距离,则确定两个点之间没有障碍;若移动距离小于或等于两个点之间的距离,则在占据栅格地图中查询途径点是否为障碍。
第二障碍检测单元,用于若途径点为障碍,则确定两个点之间有障碍;若途径点不为障碍,则对移动次数进行累加,重复执行途径点确定单元和距离检测单元,直至移动距离大于两个点之间的距离。
进一步地,如图8所示,所述角度控制模块140包括参数获取单元141、角度检测单元142、角度调整单元143和角度控制单元144,各功能单元的详细说明如下:
参数获取单元141,用于通过计算清洁机器人的当前朝向角度与目标角度之间的差值获得偏差角度,并获取清洁机器人的当前角速度和最大轮速;
角度检测单元142,用于检测偏差角度的绝对值是否大于预设调整阈值;
角度调整单元143,用于若是,将偏差角度、当前角速度和最大轮速输入至预设控制器,通过预设控制器中的调整算法获取左右轮速度差,根据左右轮速度差和最大轮速调整左轮轮速和右轮轮速;若否,根据偏差角度的数值符号设置左轮轮速和右轮轮速。
角度控制单元144,用于根据左轮轮速和右轮轮速控制清洁机器人沿目标角度运动至目标点。
进一步地,所述返回确定模块150包括一类控制单元和二类控制单元,各功能单元的详细说明如下:
一类控制单元,用于若检测结果为目标点不是关键路径队列中的第一个关键点,则控制清洁机器人执行目标点查询操作。
二类控制单元,若检测结果为目标点是关键路径队列中的第一个关键点,则控制清洁机器人执行成功返回操作。
本实施例的清洁机器人返回路径规划系统用于执行如图1所示的清洁机器人返回路径规划方法实施例的技术方案,其实现原理和技术效果类似,在处不再赘述。
上述清洁机器人返回路径规划系统中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于清洁机器人中的处理器中,也可以以软件形式存储于清洁机器人中的存储器中,以便于处理器调用执行以上各个模块对应的操作。
此外,本发明实施例还提供一种清洁机器人,包括处理器、存储器及存储在存储器上并可在处理器上运行的返回路径规划程序,处理器执行返回路径规划程序时上述实施例中的清洁机器人返回路径规划方法的步骤。
所属领域的普通技术人员应当理解:以上任何实施例的讨论仅为示例性的,并非旨在暗示本公开的范围(包括权利要求)被限于这些例子;在本发明的思路下,以上实施例或者不同实施例中的技术特征之间也可以进行组合,并存在如上所述的本发明的不同方面的许多其它变化,为了简明它们没有在细节中提供。因此,凡在本发明的精神和原则之内,所做的任何省略、修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (9)
1.一种清洁机器人返回路径规划方法,其特征在于,包括:
确定清洁机器人清扫开始时的起点,将所述起点标记为关键点存入关键路径队列的头部,并将所述关键点的索引设置为初始索引;
确定所述清洁机器人清扫过程中的中间点,将所述中间点标记为所述关键点存入所述关键路径队列中,并将所述关键点的索引进行累加:在所述清洁机器人清扫过程中,进行占据栅格地图标记;控制所述清洁机器人在所在的待清扫区域进行弓形清扫;在所述待清扫区域清扫完成后,控制所述清洁机器人进行右侧沿墙运动,并在右侧沿墙运动时,将根据预设中间点标记策略获得的中间点标记为所述关键点存入所述关键路径队列中;控制清洁机器人移动下一个的待清扫区域,返回步骤:控制所述清洁机器人在所在的待清扫区域进行弓形清扫,直至包含多个所述待清扫区域的预设空间清扫完成,其中,预设中间点标记策略是指,清洁机器人右侧沿墙运动时,若触发墙角右转,将清洁机器人完成右转后、继续沿墙行走前的所在位置确定为第一个中间点,以及将清洁机器人沿当前朝向角度的右侧90°移动预设距离值后的所在位置确定为第二个中间点;
确定所述清洁机器人完成清扫后的终点,从所述关键路径队列开始遍历,通过检测所述终点与所述关键点之间是否存在障碍查询目标点;
根据所述终点与所述目标点的位置获取目标角度,控制所述清洁机器人沿所述目标角度运动至所述目标点;
通过检测所述目标点是否为所述关键路径队列中的第一个所述关键点,控制所述清洁机器人执行与检测结果对应的操作。
2.如权利要求1所述的清洁机器人返回路径规划方法,其特征在于,所述检测所述终点与所述关键点之间是否存在障碍,包括:
获取两个点的位置;
根据反三角函数计算第二个点相对于第一个点的移动角度;
根据单位移动距离和移动次数获取移动距离,并根据所述移动角度和所述移动距离确定途径点;
检测所述移动距离是否大于两个点之间的距离;
若所述移动距离大于两个点之间的距离,则确定两个点之间没有障碍;
若所述移动距离小于或等于两个点之间的距离,则在占据栅格地图中查询所述途径点是否为障碍;
若所述途径点为障碍,则确定两个点之间有障碍;
若所述途径点不为障碍,则对移动次数进行累加,并返回步骤:根据单位移动距离和移动次数获取移动距离,并根据所述移动角度和所述移动距离确定途径点。
3.如权利要求2所述的清洁机器人返回路径规划方法,其特征在于,所述目标角度为:
其中,θ为所述第二个点p2相对于所述第一个点p1的目标角度,且p1=(x1,y1),p2=(x2,y2)。
4.如权利要求1所述的清洁机器人返回路径规划方法,其特征在于,所述根据所述终点与所述目标点的位置获取目标角度,控制所述清洁机器人沿所述目标角度运动至所述目标点,包括:
通过计算所述清洁机器人的当前朝向角度与目标角度之间的差值获得偏差角度,并获取所述清洁机器人的当前角速度和最大轮速;
检测所述偏差角度的绝对值是否大于预设调整阈值;
若是,将所述偏差角度、所述当前角速度和所述最大轮速输入至预设控制器,通过所述预设控制器中的调整算法获取左右轮速度差,根据所述左右轮速度差和所述最大轮速调整左轮轮速和右轮轮速;
若否,根据所述偏差角度的数值符号设置左轮轮速和右轮轮速;
根据所述左轮轮速和所述右轮轮速控制所述清洁机器人沿所述目标角度运动至所述目标点。
5.如权利要求4所述的清洁机器人返回路径规划方法,其特征在于,所述预设控制器中的调整算法为:
其中,dv为所述左右轮速度差;eθ为所述偏差角度;A为所述清洁机器人的当前角速度;S为所述清洁机器人的最大轮速;δ为常数;k1、k2分别为第一调整参数和第二调整参数,且k1、k2均根据所述预设控制器的类型进行设置。
6.如权利要求1所述的清洁机器人返回路径规划方法,其特征在于,所述通过检测所述目标点是否为所述关键路径队列中的第一个所述关键点,控制所述清洁机器人执行与检测结果对应的操作,包括:
若检测结果为所述目标点不是所述关键路径队列中的第一个所述关键点,则控制所述清洁机器人执行目标点查询操作;
若检测结果为所述目标点是所述关键路径队列中的第一个所述关键点,则控制所述清洁机器人执行成功返回操作。
7.一种清洁机器人路径规划系统,其特征在于,包括:
起点确定模块,用于确定清洁机器人清扫开始时的起点,将所述起点标记为关键点存入关键路径队列的头部,并将所述关键点的索引设置为初始索引;
关键点标记模块,用于确定所述清洁机器人清扫过程中的中间点,将所述中间点标记为所述关键点存入所述关键路径队列中,并将所述关键点的索引进行累加:在所述清洁机器人清扫过程中,进行占据栅格地图标记;控制所述清洁机器人在所在的待清扫区域进行弓形清扫;在所述待清扫区域清扫完成后,控制所述清洁机器人进行右侧沿墙运动,并在右侧沿墙运动时,将根据预设中间点标记策略获得的中间点标记为所述关键点存入所述关键路径队列中;控制清洁机器人移动下一个的待清扫区域,返回步骤:控制所述清洁机器人在所在的待清扫区域进行弓形清扫,直至包含多个所述待清扫区域的预设空间清扫完成,其中,预设中间点标记策略是指,清洁机器人右侧沿墙运动时,若触发墙角右转,将清洁机器人完成右转后、继续沿墙行走前的所在位置确定为第一个中间点,以及将清洁机器人沿当前朝向角度的右侧90°移动预设距离值后的所在位置确定为第二个中间点;
目标点查询模块,用于确定所述清洁机器人完成清扫后的终点,从所述关键路径队列开始遍历,通过检测所述终点与所述关键点之间是否存在障碍查询目标点;
角度控制模块,用于根据所述终点与所述目标点的位置获取目标角度,控制所述清洁机器人沿所述目标角度运动至所述目标点;
返回确定模块,用于通过检测所述目标点是否为所述关键路径队列中的第一个所述关键点,控制所述清洁机器人执行与检测结果对应的操作。
8.如权利要求7所述的清洁机器人返回路径规划系统,其特征在于,所述角度控制模块包括:
参数获取单元,用于通过计算所述清洁机器人的当前朝向角度与所述目标角度之间的差值获得偏差角度,并获取所述清洁机器人的当前角速度和最大轮速;
角度检测单元,用于检测所述偏差角度的绝对值是否大于预设调整阈值;
角度调整单元,用于若是,将所述偏差角度、所述当前角速度和所述最大轮速输入至预设控制器,通过所述预设控制器中的调整算法获取左右轮速度差,根据所述左右轮速度差和所述最大轮速调整左轮轮速和右轮轮速;若否,根据所述偏差角度的数值符号设置左轮轮速和右轮轮速;
角度控制单元,用于根据所述左轮轮速和所述右轮轮速控制所述清洁机器人沿所述目标角度运动至所述目标点。
9.一种清洁机器人,其特征在于,所述清洁机器人包括处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的返回路径规划程序,所述处理器执行所述返回路径规划程序时实现如权利要求1至6中任一项所述的清洁机器人返回路径规划方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110270459.3A CN113031616B (zh) | 2021-03-12 | 2021-03-12 | 一种清洁机器人返回路径规划方法、系统和清洁机器人 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110270459.3A CN113031616B (zh) | 2021-03-12 | 2021-03-12 | 一种清洁机器人返回路径规划方法、系统和清洁机器人 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113031616A CN113031616A (zh) | 2021-06-25 |
CN113031616B true CN113031616B (zh) | 2023-08-01 |
Family
ID=76470234
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110270459.3A Active CN113031616B (zh) | 2021-03-12 | 2021-03-12 | 一种清洁机器人返回路径规划方法、系统和清洁机器人 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113031616B (zh) |
Families Citing this family (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114442638B (zh) * | 2022-02-14 | 2023-07-07 | 北京大学 | 一种基于大数据路径规划的清淤机器人 |
CN114427310A (zh) * | 2022-02-18 | 2022-05-03 | 智橙动力(北京)科技有限公司 | 泳池边缘清洗方法、装置、电子设备及计算机存储介质 |
CN115191890B (zh) * | 2022-07-22 | 2023-06-20 | 云鲸智能(深圳)有限公司 | 清洁机器人的控制方法、清洁机器人及存储介质 |
CN115328135B (zh) * | 2022-08-16 | 2023-03-31 | 上海联适导航技术股份有限公司 | 划线机器人校准及划线方法、装置、电子设备及划线机器人 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102023569A (zh) * | 2010-09-01 | 2011-04-20 | 重庆大学 | 两轮轮式机器人点镇定增量式智能控制方法 |
CN104950883A (zh) * | 2015-05-14 | 2015-09-30 | 西安电子科技大学 | 一种基于距离网格地图的移动机器人路径规划方法 |
CN110037619A (zh) * | 2019-06-06 | 2019-07-23 | 长沙百凤堂食品贸易有限公司 | 一种旅客列车专用扫地机器人的控制方法及扫地机器人 |
CN111580525A (zh) * | 2020-05-26 | 2020-08-25 | 珠海市一微半导体有限公司 | 沿边行走中返回起点的判断方法、芯片及视觉机器人 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN100491084C (zh) * | 2007-07-03 | 2009-05-27 | 北京控制工程研究所 | 一种基于二元环境信息的移动机器人局部路径规划方法 |
CN111596662B (zh) * | 2020-05-26 | 2023-03-10 | 珠海一微半导体股份有限公司 | 一种沿全局工作区域一圈的判断方法、芯片及视觉机器人 |
-
2021
- 2021-03-12 CN CN202110270459.3A patent/CN113031616B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102023569A (zh) * | 2010-09-01 | 2011-04-20 | 重庆大学 | 两轮轮式机器人点镇定增量式智能控制方法 |
CN104950883A (zh) * | 2015-05-14 | 2015-09-30 | 西安电子科技大学 | 一种基于距离网格地图的移动机器人路径规划方法 |
CN110037619A (zh) * | 2019-06-06 | 2019-07-23 | 长沙百凤堂食品贸易有限公司 | 一种旅客列车专用扫地机器人的控制方法及扫地机器人 |
CN111580525A (zh) * | 2020-05-26 | 2020-08-25 | 珠海市一微半导体有限公司 | 沿边行走中返回起点的判断方法、芯片及视觉机器人 |
Also Published As
Publication number | Publication date |
---|---|
CN113031616A (zh) | 2021-06-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113031616B (zh) | 一种清洁机器人返回路径规划方法、系统和清洁机器人 | |
WO2021248846A1 (zh) | 一种机器人沿边行走的区域清扫规划方法、芯片及机器人 | |
CN108507578B (zh) | 一种机器人的导航方法 | |
US11774980B2 (en) | Method for controlling cleaning of robot, chip, and robot cleaner | |
JP7085296B2 (ja) | ロボットの再測位方法 | |
JP7462244B2 (ja) | ロボットの縁沿い走行中の清掃区画領域の計画方法、チップ及びロボット | |
CN109997089A (zh) | 地面处理机和地面处理方法 | |
CN108983776B (zh) | 一种机器人控制方法及其装置、电子设备 | |
JP7408072B2 (ja) | 密集した障害物に基づく清掃制御方法 | |
WO2023155371A1 (zh) | 室内移动机器人的平稳移动全局路径规划方法 | |
CN109407675B (zh) | 机器人回座的避障方法和芯片以及自主移动机器人 | |
CN105955280A (zh) | 移动机器人路径规划和避障方法及系统 | |
WO2020187134A1 (zh) | 移动机器人的返回充电方法、装置、移动机器人、系统及存储介质 | |
CN106681320A (zh) | 一种基于激光数据的移动机器人导航控制方法 | |
WO2022000960A1 (zh) | 一种越障结束判断方法、越障控制方法、芯片及机器人 | |
CN113190010B (zh) | 一种沿边绕障路径规划方法、芯片及机器人 | |
CN110220528A (zh) | 一种基于a星算法的自动驾驶无人车双向动态路径规划方法 | |
CN113110497B (zh) | 基于导航路径的沿边绕障路径选择方法、芯片及机器人 | |
WO2022227917A1 (zh) | 机器人的导航方法、芯片及机器人 | |
CN108803602B (zh) | 障碍物自学习方法及新障碍物自学习方法 | |
CN110031015B (zh) | 一种用连续可导函数近似表示车道平面曲线的方法 | |
CN115167398A (zh) | 一种基于改进a星算法的无人船路径规划方法 | |
Yamauchi et al. | Magellan: An integrated adaptive architecture for mobile robotics | |
JP2017107275A (ja) | 自律移動装置、自律移動方法及びプログラム | |
Muñoz et al. | S-Theta: low steering path-planning algorithm |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |