阅读说明：本技术 一种应用于低成本芯片的扫地机器人清扫路径规划导航算法 (sweeping robot sweeping path planning navigation algorithm applied to low-cost chip ) 是由 陈振兵 詹伟 苟潇华 于 2019-09-30 设计创作，主要内容包括：本发明公开了一种应用于低成本芯片的扫地机器人清扫路径规划导航算法,其特征在于,包括控制层和算法层,包括以下步骤：步骤1：开启扫地机器人；步骤2：所述扫地机器人的控制层初始化算法模块中的plan模块,所述算法层返回所述plan模块的值至所述控制层中的pathPlan结构体,并保存pathPlan结构体；步骤3：所述扫地机器人进入弓扫模式；步骤4：计算当前坐标,检测所述扫地机器人是否会发生碰撞,根据检测结果将算法层获取行进方式并返回Action；步骤5：将Action导入所述控制层,根据method使用沿边或沿线控制；步骤6：扫地机器人判断清扫是否结束,若扫地机器人判断清扫未结束,则返回步骤4。(the invention discloses a sweeping path planning navigation algorithm of a sweeping robot applied to a low-cost chip, which is characterized by comprising a control layer and an algorithm layer, and the method comprises the following steps: step 1: starting the sweeping robot; step 2: a plan module in an algorithm module is initialized by a control layer of the sweeping robot, the algorithm layer returns the value of the plan module to a pathPlan structure in the control layer, and the pathPlan structure is stored; and step 3: the sweeping robot enters a bow sweeping mode; and 4, step 4: calculating current coordinates, detecting whether the sweeping robot collides or not, acquiring a traveling mode of an algorithm layer according to a detection result and returning to an Action; and 5: importing an Action into the control layer, and controlling along the edge or the line according to the method; step 6: and (4) judging whether the cleaning is finished or not by the cleaning robot, and if the cleaning is not finished by the cleaning robot, returning to the step 4.)

一种应用于低成本芯片的扫地机器人清扫路径规划导航算法

技术领域

本发明涉及机器人技术领域，具体为一种应用于低成本芯片的扫地机器人清扫路径规划导航算法。

背景技术

随着科技不断发展进步，越来越多的人家开始使用扫地机器人，扫地机器人在进行清扫工作时，需要规划一条清扫路径，来避免与工作区间内的障碍物发生碰撞，而当前应用于扫地机器人的清扫路径规划方法有以下缺陷：高成本，现行技术由于需要对地图进行存储，内存消耗大，必须使用512kb以上芯片，导致计算设备成本堆高；扫地机器人需要事先探索工作空间构建地图之后才能开始清扫任务，在清扫大型空间时需要花费很久时间进行探索；目前应用于低成本芯片的清扫路径规划算法多为随机碰撞式，清扫覆盖率及清扫效率低，无法满足正常使用需求，用户体验感差；通常采用的大多数方案对堆放大量杂物的房间边缘覆盖率较低。

发明内容

(一)解决的技术问题

针对现有技术的不足，本发明提供了一种应用于低成本芯片的扫地机器人清扫路径规划导航算法，解决了扫地机器人成本高、清扫覆盖率及清扫效率较低、用户体验感差的问题。

(二)技术方案

为实现以上目的，本发明通过以下技术方案予以实现：一种应用于低成本芯片的扫地机器人清扫路径规划导航算法，包括控制层和算法层，包括以下步骤：

步骤1：开启扫地机器人；

步骤2：所述扫地机器人的控制层初始化算法模块中的plan模块，所述算法层返回所述plan模块的值至所述控制层中的pathPlan 结构体，并保存pathPlan结构体；

步骤3：所述扫地机器人进入弓扫模式；

步骤4：计算当前坐标，检测所述扫地机器人是否会发生碰撞，根据检测结果将算法层获取行进方式并返回Action；

步骤5：将Action导入所述控制层，根据method使用沿边或沿线控制；

步骤6：扫地机器人判断清扫是否结束，若扫地机器人判断清扫未结束，则返回步骤4。

优选的，所述扫地机器人获取行进方式包括以下步骤：

步骤1：所述扫地机器人接收到起始信号，判断当前是否为沿边模式，若是，则执行步骤2，否则执行步骤9；

步骤2：所述扫地机器人判断是否需要重建坐标系，若是，则执行步骤3，否则执行步骤4；

步骤3：所述扫地机器人以所在位置为原点重建坐标系，并重新创建3条预规划路径，输出预规划路径中的第一条，needRebuild标识为1，结束跳出，返回Action；

步骤4：所述扫地机器人选择判断通过沿边跃迁到3条预规划路径，若是接近第一条，执行步骤5，若是接近第二条，执行步骤6，若是接近第三条，执行步骤7，否则执行步骤8；

步骤5：所述扫地机器人输出生成后预规划路径中的第一条，且 needRebuild标识为0，修改当前模式为沿直线行走，结束跳出，返回Action；

步骤6：所述扫地机器人进行1次新路径生成，且needRebuild 标识为0，修改当前模式为沿直线行走，结束跳出，返回Action；

步骤7：所述扫地机器人进行2次新路径生成，且needRebuild 标识为0，修改当前模式为沿直线行走，结束跳出，返回Action；

步骤8：所述扫地机器人输出上一次的Action，跳出结束；

步骤9：所述扫地机器人判断当前是否发生碰撞，若是，所述扫地机器人根据当前行进路径获取相应边沿的方向，输出沿边模式，修改当前为沿边行走模式，结束跳出，否则执行步骤10；

步骤10：所述扫地机器人判断是否到达当前路径的终点，若是则所述扫地机器人进行1次新路径生成，并输出预规划路径的第一条， needRebuid标识为0，跳出结束，否则执行步骤11；

步骤11：所述扫地机器人输出预规划路径的第一条，needRebuid 标识为0，跳出结束。

优选的，当机器抵达当前线路终点后，或通过沿边跃迁到预规划路径中任意一条后，调用路径生成方法生成一条与预规划路径最后一条连续的新路径，并将预规划路径中原第二条、原第三条一次提升到第一、第二的位置，新的路径放置于第三个位置，新路径的生成规则如下：

(1)设定新路径的起点为最后一条预规划路线的终点，记为 (startX，startY)；

(2)若最后一条预规划路线类型不为2，则新路径终点坐标为 (startX+offset，startY)，类型为2，角度为0，否则执行第3步与第4步；

(3)如果第二条预规划路径的类型为1，则新路径终点坐标为 (startX，startY-maxLen)，类型为3，角度为-90；

(4)如果第二条与规划路径的类型为3，则新路径终点坐标为 (startX，startY+maxLen)，类型为1，角度为90。

优选的，所述扫地机器人每次构建3条预规划路径，且当所述扫地机抵达每条路径终点后构建新的规划路径、重定向坐标以实现掉头继续覆盖未清扫区域。

优选的，在沿规划路线前进过程中遇到障碍进入沿边模式清扫障碍或墙壁边缘，并在沿线过程中计算是否跃迁到预规划路线中的线路，进行沿边的退出。

(三)有益效果

本发明提供了一种应用于低成本芯片的扫地机器人清扫路径规划导航算法。具备以下益效果：本发明执行效率高，对内存依赖极小；成本低，对内存依赖极小，可在STM芯片成本最低的STM32F0x0芯片中与其他机器人算法一同流畅运行；无需先探索地图，可在清扫过程中有规划地进行避障和转向；使用不断折返的弓字形线路，覆盖效率远高于所有随机乱撞清扫算法；在接触到房间边缘或障碍物时，通过沿墙行走提高障碍物或房间边缘的覆盖率。

附图说明

图1为本发明的算法调用流程示意图；

图2为本发明的单步获取运动方式示意图；

图3为本发明的初始化构建预规划路线示意图；

图4为本发明的清扫尽头判断示意图；

图5为本发明的清扫尽头重新构建预规划路线示意图；

图6为本发明的沿边、沿线模式切换示意图；

图7为本发明的退出沿边判断情况示意图一；

图8为本发明的退出沿边判断情况示意图二；

图9为本发明的退出沿边判断情况示意图三；

图10为本发明的退出沿边判断情况示意图四。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1-10，本发明实施例提供一种技术方案一种应用于低成本芯片的扫地机器人清扫路径规划导航算法，包括控制层和算法层，包括以下步骤：

步骤1：开启扫地机器人；

步骤2：所述扫地机器人的控制层初始化算法模块中的plan模块，所述算法层返回所述plan模块的值至所述控制层中的pathPlan 结构体，并保存pathPlan结构体；

步骤3：所述扫地机器人进入弓扫模式；

步骤4：计算当前坐标，检测所述扫地机器人是否会发生碰撞，根据检测结果将算法层获取行进方式并返回Action；

步骤5：将Action导入所述控制层，根据method使用沿边或沿线控制；

步骤6：扫地机器人判断清扫是否结束，若扫地机器人判断清扫未结束，则返回步骤4。

所述扫地机器人获取行进方式包括以下步骤：

步骤1：所述扫地机器人接收到起始信号，判断当前是否为沿边模式，若是，则执行步骤2，否则执行步骤9；

步骤2：所述扫地机器人判断是否需要重建坐标系，若是，则执行步骤3，否则执行步骤4；

步骤3：所述扫地机器人以所在位置为原点重建坐标系，并重新创建3条预规划路径，输出预规划路径中的第一条，needRebuild标识为1，结束跳出，返回Action；

步骤4：所述扫地机器人选择判断通过沿边跃迁到3条预规划路径，若是接近第一条，执行步骤5，若是接近第二条，执行步骤6，若是接近第三条，执行步骤7，否则执行步骤8；

步骤5：所述扫地机器人输出生成后预规划路径中的第一条，且 needRebuild标识为0，修改当前模式为沿直线行走，结束跳出，返回Action；

步骤6：所述扫地机器人进行1次新路径生成，且needRebuild 标识为0，修改当前模式为沿直线行走，结束跳出，返回Action；

步骤7：所述扫地机器人进行2次新路径生成，且needRebuild 标识为0，修改当前模式为沿直线行走，结束跳出，返回Action；

步骤8：所述扫地机器人输出上一次的Action，跳出结束；

步骤9：所述扫地机器人判断当前是否发生碰撞，若是，所述扫地机器人根据当前行进路径获取相应边沿的方向，输出沿边模式，修改当前为沿边行走模式，结束跳出，否则执行步骤10；

步骤10：所述扫地机器人判断是否到达当前路径的终点，若是则所述扫地机器人进行1次新路径生成，并输出预规划路径的第一条， needRebuid标识为0，跳出结束，否则执行步骤11；

步骤11：所述扫地机器人输出预规划路径的第一条，needRebuid 标识为0，跳出结束。

当机器抵达当前线路终点后，或通过沿边跃迁到预规划路径中任意一条后，调用路径生成方法生成一条与预规划路径最后一条连续的新路径，并将预规划路径中原第二条、原第三条一次提升到第一、第二的位置，新的路径放置于第三个位置，新路径的生成规则如下：

(1)设定新路径的起点为最后一条预规划路线的终点，记为 (startX，startY)；

(2)若最后一条预规划路线类型不为2，则新路径终点坐标为 (startX+offset，startY)，类型为2，角度为0，否则执行第3步与第4步；

(3)如果第二条预规划路径的类型为1，则新路径终点坐标为 (startX，startY-maxLen)，类型为3，角度为-90；

(4)如果第二条与规划路径的类型为3，则新路径终点坐标为 (startX，startY+maxLen)，类型为1，角度为90。

所述扫地机器人每次构建3条预规划路径，且当所述扫地机抵达每条路径终点后构建新的规划路径、重定向坐标以实现掉头继续覆盖未清扫区域。

在沿规划路线前进过程中遇到障碍进入沿边模式清扫障碍或墙壁边缘，并在沿线过程中计算是否跃迁到预规划路线中的线路，进行沿边的退出。

本发明路径生成以及路径更换：

(1)初始化时的预构建：由于大多数情况下，用户使用扫地机器人时，会将扫地机器人放置于通道中央部分，故设定开始弓扫时，扫地机器人当前朝向(x轴)，即为覆盖区域正中间。在初始阶段，根据传入配置将生成如下未来3条线路(见图3)。

(2)当前路径终点判断：在清扫过程中，控制层将反复调用get_current_action()方法获取建议路线，如果扫地机器人没有碰撞障碍，也没有抵达终点，将快速速出上一次返回的路线，控制扫地机器人继续沿当前线路行走。如果判断到扫地机器人抵达该条线路的终点，将输出该条线路的下一条路线，并生成一条新的路线放置于预规划路线数组。当前线路的终点判断方法如下：

已知当前线路的起点坐标为(x1，y1)，当前扫地机器人所在坐标为(x2，y2)；

如果当前线路类型为2(侧)，判断x2-x1是否大于等于offset，如果是，则到达，否则没有到达；

如果当前线路类型不为2(侧)，则判断y2-y1的绝对值是否大于等于maxLen，如果是，则到达，否则没有到达。

C++代码如下：

(3)清扫过程中路径动态生成：当扫地机器人抵达当前线路终点后，或通过沿边跃迁到预规划路径中任意一条后，调用路径生成方法生成一条与预规划路径最后一条连续的新路径，并将预规划路径中原第二条提升到第一条的位置，原第3条提升到第二的位置，新的路径放置于第三个位置。新路径的生成规则如下：

设定新路径的起点为最后一条预规划路线的终点，记为(startX， startY)；

如果最后一条预规划路线类型不为2，则新路径终点坐标为 (startX+offset，startY)，类型为2，角度为0。否则执行第3步与第4步；

如果第二条预规划路径的类型为1，则新路径终点坐标为(startX， startY-maxLen)，类型为3，角度为-90；

如果第二条与规划路径的类型为3，则新路径终点坐标为(startX， startY+maxLen)，类型为1，角度为90。

(4)尽头的判断：扫地机器人沿弓字形折返不断沿X轴进行覆盖，最终将抵达障碍物，导致无法进入下一条规划路线，此时，扫地机器人必将进入沿边模式，并最终进入未覆盖区域，如图4所示。通过判断当前机器坐标，当扫地机器人进入未覆盖区域时，输出needRebuild为1，并重新规划尽头之后的导航线路，控制层在判断 needRebuild为1后，将当前坐标即码盘重置为0，从而继续覆盖未清扫区域。

尽头之后的生成：当控制层获取needRebuild为1后，将对坐标系进行重建，将旧坐标系(灰色)根据当前扫地机器人坐标为原点，头部朝向为x轴构建新坐标(图中蓝色坐标)，并沿远离墙体的方向构建3条预规划路线(图中0，1，2)，如下图5所示，如果当前为左沿墙，生成3条路线的方法如下：

如果达能前为右沿墙，生成3条路线的方法如下：

本发明沿边沿线控制模式的切换：扫地机器人在实际运行过程中，将面对各种不规则的房间边缘，在清扫时通过沿边沿线两种工作模式的合理切换，使扫地机器人能因对各种形状或倾斜的边缘，行走路线将如图6所示：

进入沿边并确定沿边方式，如果当前为沿直线行走模式，即 nowMethod为1，且控制层在调用获取建议行进方式 (get_current_action)时，传入obstacle为1，即有碰撞发生，则当前扫地机器人所在点为沿墙进入点，设定nowMethod为2，通过如下逻辑确定沿边方向：

如果预规划路径中第一条的类型为1(去)，则沿边方向为左沿边；

如果预规划路径中第一条的类型为3(返)，则沿边方向为右沿边；

如果预规划路径中的第一条类型为2，且后一条类型为1，则沿边方向为右沿边；

如果预规划路径中的第一条类型为2，且后一条类型为3，则沿边方向为左沿边。

本发明推出沿边的方式为：如果当前行进模式为沿边模式，即 nowMethod为2，且无须重建坐标系，将按如下逻辑判断是否退出沿边进入沿线模式。

如果第一条预规化路线类型为1，第二条为2，如图7判断跃迁到那条线路；

如果第一条预规化路线类型为2，第二条为1，如图8判断跃迁到那条线路；

如果第一条预规化路线类型为3，第二条为2，如图9判断跃迁到那条线路；

如果第一条预规化路线类型为2，第二条为3，如图10判断跃迁到那条线路。

如果跃迁到预规划路线中的第一条，同时满足下列任意一个条件即跃迁成功，否则为无效跃迁：a如果第一条预规划路线类型为1，且当前Y坐标减去开始进入沿边的Y坐标大于320(机器直径)；b如果第一条预规化路线类型为2，且当前x坐标值减去开始沿边的x坐标值大于320；c如果第一条预规划路线类型为3，且开始沿边的Y 坐标减去当前Y坐标值大于320。

本发明的相关数据结构体的定义：

Point坐标点：

类型	字段	说明
			int	x	x坐标
int	y	y坐标

Path路线：

Action输出行为：

pathPlan规划算法模块存储参数：

API的设计，path_plan_init模块初始化方法：

输入：

输出pathPlan结构体，在获取建议路线之前只需依次调用，并将返回的pathPlan结构体保存在一个全局变量中，后续每次调用获取建议行为时，都需要传入该变量的指针。

get_current_action获取当前建议行进方式：

输入：

输出：Action结构体。

路线生成以及路线更换，初始化时的预构建：

序号	起点	终点	类型	角度
					0	(0,0)	(offset,0)	2(侧)	0
1	(offset,0)	(offset,-maxLen/2)	1(去)	90
					2	(offset,-maxLen/2)	(offset×2,-maxLen/2)	2(侧)	0

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。