CN112415992B - 一种移动机器人的快速脱困方法 - Google Patents

Abstract

本发明公开了一种移动机器人的快速脱困方法，包括：S1：获取当前时刻移动机器人与周围障碍物之间的距离及角度；S2：构建以角度为自变量，距离为因变量的函数；S3：筛选出函数值不连续的角度或角度区间；S4：移动机器人沿筛选出的角度或角度区间运动移动一定距离执行脱困动作。本发明的有益效果：在移动机器人被困时，仅通过搭载在机身的距离传感器寻找出口方向，用较低的成本达到较好的效果。

Description

一种移动机器人的快速脱困方法

技术领域

本发明属于机器人领域，尤其涉及一种移动机器人的快速脱困方法。

背景技术

目前移动机器人已经具有了一定的智能性，能够代替人来完成居室的清扫工作。但由于移动机器人主要通过传感器来检测障碍物，并且采用弓字型和沿边行走的控制方法来进行清扫，在清理一些空旷地面时没有问题，但在一些复杂的区域，四周存在多处障碍物时，如家里的桌子、椅子下面，移动机器人很容易被困。当被困在这些桌子，小椅子下面时，现有移动机器人往往会无规则的随机碰撞，甚至无法走出被困区域，需要人为施救，这大大降低了移动机器人的清洁效率。为了有效解决这些问题，有必要设计一种新的控制方法。

发明内容

为解决背景技术中的技术问题，本发明提供的技术方案如下：

一种移动机器人的快速脱困方法，包括：

S1：获取当前时刻移动机器人与周围障碍物之间的距离及角度；

S2：构建以角度为自变量，距离为因变量的函数；

S3：筛选出函数值不连续的角度或角度区间；

S4：移动机器人沿筛选出的角度或角度区间移动一定距离执行脱困动作。

进一步地，为了使移动机器人的脱困效率更高，在本发明的一个优化方案中，步骤S3和S4之间还有步骤S31：从步骤S3中筛选出的角度或角度区间中筛选出可供移动机器人通过的角度或角度区间。

具体地，为了让移动机器人尽快脱困，在本发明的一个优化方案中，步骤S31中的筛选方法是：如果不连续的函数值为单个点，则判断函数值发生跳变的量是否大于移动机器人通过所需最小宽度，若是则保留，若否则移除；

如果不连续的函数值为区间，则根据余弦定理、区间夹角、夹角两侧边长计算出口宽度，若宽度大于移动机器人通过所需最小宽度，若是则保留，若否则移除；

进一步地，为了移动机器人呢能够以旋转较小的角度便可以脱困，在本发明的一个优化方案中，步骤S4包括：

S42：尝试脱困，判断是否成功，若是进入步骤S45，若否进入步骤S43；

S43：判断筛选出的角度或角度区间中是否还存在未被尝试的角度或角度区间，若是，则进入步骤S44，若否则脱困失败进入步骤S45；

S44：移动机器人朝着未被尝试的角度或角度区间方向前进，进入步骤S42；

S45：脱困工作结束。

进一步地，为了移动机器人呢能够以旋转较小的角度便可以脱困，在本发明的一个优化方案中，步骤S41和步骤S44中，移动机器人沿前一步骤中筛选出的角度或角度区间运动时选择当前未被尝试的角度或角度区间中转动幅度最小的角度或角度区间。

进一步地，为了提高移动机器人脱困的成功率，在本发明的一个优化方案中，若移动机器人是沿着角度区间运动，则沿着改角度区间的中间角度运动。

本发明还提供了一种移动机器人，包括：测距传感器、陀螺仪、控制器、驱动装置，所述测距传感器可以测量其与障碍物之间的距离，所述陀螺仪可以测量其运动角度，所述控制器可以根据测量到的距离和角度构建函数并且对函数值进行筛选和计算，所述驱动装置可以带动移动机器人运动，控制器可以控制驱动装置

进一步地，综合成本和效果考虑，在本发明的一个优化方案中所述测距传感器可以是超声波测距传感器或激光雷达传感器或红外测距传感器。

本发明还提供了一种移动机器人控制装置，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的计算机程序指令，所述处理器执行所述计算机程序指令时，实现上述的方法步骤。

本发明的有益效果：在移动机器人被困时，仅通过搭载在机身的距离传感器寻找出口方向，用较低的成本达到较好的效果。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单的介绍，显而易见地，下面描述中的附图是本发明实施例的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明的普适控制流程图；

图2为本发明的优化控制流程图；

图3为本发明步骤S4的控制流程图；

图4为本发明的第一实施例的场景图；

图5为本发明的第一实施例的坐标图；

图6为本发明的第二实施例的场景图；

图7为本发明的第二实施例的场景图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。另外，为了便于说明，本实施例说明中使用了左、右、内、外等方位词，但这并不限制本发明的保护范围。

本发明方法的移动机器人包括：坐标系建立系统，包括里程计、陀螺仪。其中，坐标系建立系统可以记住起点和行走路径的坐标，构建坐标系，同时计算规划行走线路。里程计用于获取移动机器人的行走距离，所述陀螺仪用于获取移动机器人的行走角度，纠正移动机器人的行进路线让移动机器人不跑偏。

此外，移动机器人还包括电源模块、行走模块、传感器模块、控制器、存储模块及功能模块、计时器等。

电源模块可以向移动机器人供电，电源可以反复充、放电，由于锂电池输出功率大、低自放电、工作温度范围宽、充放电快等优点，所以较优选择锂电池作为移动机器人的电源。

行走模块包括驱动电机和行走轮，用于使移动机器人在工作区域内运动，驱动电机用于驱动行走轮，除了行走轮以外还可以增加适当个万向轮，用来使移动机器人保持平衡。

传感器模块用于接收工作区域的环境信息并反馈至控制模块，传感器可以包括测距传感器、碰撞传感器、位移传感器、电池电量检测传感器等，其中测距传感器可以为激光雷达传感器、超声波测距传感器、红外测距传感器等。

控制模块用于根据传感器模块反馈的信息控制移动机器人运动；

存储模块用于存储移动机器人的控制程序以及从外界获取的传感器信息；

功能模块指移动机器人特定的功能，比如移动机器人具有清扫功能，割草机器人具有割草功能等。

由于移动机器人的工作环境较为复杂，在某些区域可能会被困住，判断移动机器人是否被困住可以采用以下判断标准：

在一定时间内，移动机器人碰撞次数超过阈值，并且根据里程计和陀螺仪获取到的位移量和角度量得出的运动区域范围的最大宽度小于阈值时，则可以判定移动机器人处于被困状态。

阈值的设定可依据扫地机清扫速度进行设置，这里设置的一定时间为120秒，运动区域范围的最大宽度为1米，碰撞次数阈值可设为5次，若扫地机在120秒内，碰撞5次且运动呢范围的最大宽度为1米，则表示移动机器人被困，此时向控制器发送信号，使控制器驱动距离传感器工作，进入脱困模式。

本发明提供了一种移动机器人快速脱困的方法：

S1：获取当前时刻移动机器人与周围障碍物之间的距离及角度；

测量移动机器人与障碍物之间的距离的方式可以为两种：

(1)移动机器人旋转一周

采用这种方法的移动机器人搭载的测距传感器仅需固定在移动机器人机身上，不要求传感器自身可以旋转，采用这种方法测得的距离为机身一周与障碍物的距离；

(2)搭载在移动机器人上的测距传感器旋转一周

采用这种方法的移动机器人搭载的测距传感器自身需要移动，可以为设置在移动机器人的顶部的激光雷达测距传感器，激光雷达可以360度旋转，便可实现移动机器人机身不运动而测得与周围障碍物的距离，采用这种方法测的距离为传感器与障碍物的距离。

移动机器人将测得的距离与对应旋转的角度记录在存储模块中。

移动机器人处于如图4所示的环境中。

S2：构建以角度为自变量，距离为因变量的函数；

根据步骤S1的中测量的距离和旋转角度，以旋转角度为横轴，以距离为纵轴画出函数ρ(θ)的图像；在这里竖直向上的方向为θ＝0的方向，绘制出的图像如图5所示。

S3：筛选出函数值不连续的角度或角度区间；

根据图5所示，函数值发生不连续的角度或角度区间有三个，分别是θ_a、θ_b、[θ_c,θ_d]。

S31：从步骤S3中筛选出的角度或角度区间中筛选出可供移动机器人通过的角度或角度区间

步骤S3中筛选出的角度或角度区间是有限个，在本实施例中是3个，分别是θ_a、θ_b、[θ_c,θ_d]。

筛选方式根据函数值不连续方式的不同分为两种：

(1)不连续的函数值为单个点

即这个函数值与左右两边均不连续，故对应的子区间中只有一个角度值，函数值发生跳变的量L＝ρ跳变前后的差值，也就是角度θ_a、θ_b处对应发生跳变的函数值变化量，如果L不大于2R(2R为移动机器人通过缝隙所需的最小宽度，如果移动机器人是圆形的扫地机器人，则2R扫地机器人的直径)，则将该角度排除。

可以从图4中看出，在障碍物1和障碍物2之间存在出口，跳变值为La，如果La>2R，则保留该角度θ_a，如果La≤2R，则将该角度排除。对于θ＝θ_b的情况筛选方法同θ＝θ_a。

故筛选后的角度中只保留对应跳变值L大于2R的角度。

(2)不连续的函数值为区间

即有一段函数值发生了跳变，故对应的子区间中为包含一段连续角度值的子集，由于有两个端点，故有两段跳变值，移动机器人上的某一点与分别与出口两侧边缘的距离，两个距离值Lc、Ld和夹角(θd-θc)均已知，根据余弦定理可以求出夹角所对的边的长度，即拟缝隙的最大宽度(由于还存在其他极端的情况，这里的拟缝隙最大宽度有可能不是实际的最大宽度)，将拟缝隙的最大宽度W与移动机器人的宽度2R进行比较，如果W>2R，则将该区间保留至可供移动机器人通过的角度区间。

可以理解的是，测量移动机器人与障碍物的距离无论是按照传感器自身旋转还是按照移动机器人旋转的方法均可以得到两个距离值Lc、Ld，如果是按照传感器自身旋转，则计算所需的距离值Lc、Ld即传感器的测量结果，所以上述的移动机器人上的某一点即为该传感器的测量点；如果是按照移动机器人旋转的方法，由于此时传感器是位于移动机器人边缘的，所以每次测量的起点都是变动的，将测距线反向延长相交后的夹角为(θ_d-θ_c)，而延长至交点的距离为移动机器人边缘至传感器的距离，为已知，所以上述的移动机器人上的某一点即为移动机器人原地旋转时锁绕的点。故无论采用哪种方法均可以得出所需的两个距离值Lc、Ld。

故筛选后的角度区间中只保留对应W大于2R的角度区间。

需要注意的是，这里保留角度或角度区间仅是移动机器人可能可以通过的角度或角度区间。

S4：移动机器人沿前一步骤中筛选出的角度或角度区间运动移动一定距离执行脱困动作。

步骤S4包括：

S41：移动机器人沿前一步骤中筛选出的角度或角度区间运动；

在步骤S41中，移动机器人沿前一步骤中筛选出的角度或角度区间运动时选择当前未被尝试的角度或角度区间中转动幅度最小的角度或角度区间，若移动机器人是沿着角度区间运动时，则沿着改角度区间的中间角度运动。

S42：尝试脱困，判断是否成功，若是进入步骤S45，若否进入步骤S43；

由于在步骤S3中筛选出的区间只是一个大致的方向，移动机器人沿着前一步骤中筛选出的角度或角度区间运动尝试脱困，如果仍然发生了碰撞，则朝被碰撞的反方向运动，如左侧被碰撞，则小角度向右旋转，如果这时右边也被碰撞了，则说明这里不是出口，移动机器人在该区间脱困失败。如果移动机器人脱困成功，则进入步骤S45，如果脱困失败，则进入步骤S43，

脱困成功的标志是从起点开始行进距离大于阈值，阈值可以设置为1米。

S43：判断筛选出的角度或角度区间中是否还存在未被尝试的角度或角度区间，若是，则进入步骤S44，若否则脱困失败进入步骤S45；

S44：移动机器人朝着未被尝试的角度或角度区间方向前进，进入步骤S42；

继续旋转，进入未被尝试的角度或角度区间，旋转角度的确定方式和步骤S41相同，旋转到对应角度后返回S42。

S45：脱困工作结束。

根据前述可知，脱困工作结束时移动机器人可能脱困成功，如果脱困成功则可以继续工作，如果脱困失败，则可以原地待机，保存电量，等待人工干预。

如果仅按本发明的普适控制方法，即步骤S3完成后直接进入步骤S4，不进行步骤S31的筛选过程。

如图4所示，如果不筛选过程而直接朝着步骤S3筛选出的角度或角度区间运动，根据朝着当前转动幅度最小的角度或角度区间的原则，首先会朝着角度θ_a运动，但是这里的La≤2R，则移动机器人即使朝着角度θ_a运动也不能脱困，需要退回原点，在开始朝着下一个角度或角度区间运动尝试脱困。

如果按照本发明的优化控制方法，增加了S31则可以通过比较La和移动机器人宽度2R的大小排除出口方向θ_a，而仅需朝着可能性更大的出口θ_b和[θ_c,θ_d]前进尝试脱困，提高了脱困效率。

下面具体结合不同的场景提供两个具体实施例详细说明本发明的方法步骤：

第一实施例；

场景如图4所示，移动机器人周围存在障碍物1、2、3，移动机器人在短时间内经过多次碰撞后进入脱困模式。

获取当前时刻移动机器人与周围障碍物之间的角度和距离后构建的函数如图5所示。

由图5可知，函数值不连续的角度或角度区间有θ_a、θ_b、[θ_c,θ_d]，然后根据优化方案中的步骤S31的原则筛选可能可以供移动机器人通过的角度或角度区间。

如图4中的跳变值La；跳变值Lb；移动机器人中点到障碍物3边缘距离Lc；移动机器人中点到障碍物2边缘距离Ld；Lc和Ld的夹角θ_d-θ_c和移动机器人宽度2R均已知。

首先筛选不连续的函数值为单个点的情况，单独La和Lb分别与2R进行比较，La不大于2R，Lb大于2R，故这里仅保留角度θ_b。

然后筛选不连续的函数值为区间的情况，用余弦定理计算障碍物2和障碍物3之间的出口宽度这里W小于2R，故不保留角度区间[θ_c,θ_d]。

所以经过筛选之后的角度或角度区间仅剩角度θ_b，移动机器人仅需朝着角度θ_b的方向运动，尝试脱困，最终脱困成功，继续进行被困之前的任务。

如果仅用普适控制方法，由图5可知，函数值不连续的角度或角度区间有θ_a、θ_b、[θ_c,θ_d]，然后移动机器人首先朝着转动幅度最小的θ_a移动，失败后回到脱困起点，朝着未尝试角度或角度区间中转动幅度最小的[θ_c,θ_d]的中间角度移动，失败后回到脱困起点，最后才朝着可能性最大的角度θ_b移动，而如果采用了优化方案在运动之前进行了筛选，则可以有效避免无用的尝试，提高脱困效率。

第二实施例：

场景如图6所示，移动机器人周围存在障碍物4、5、6，移动机器人在短时间内经过多次碰撞后进入脱困模式。

获取当前时刻移动机器人与周围障碍物之间的角度和距离后构建的函数如图5所示。

由图7可知，函数值不连续的角度或角度区间有θ_e、θ_f、[θ_g,θ_h]，然后根据优化方案中的步骤S31的原则筛选可能可以供移动机器人通过的角度或角度区间。

如图4中的跳变值Le；跳变值Lf；移动机器人中点到障碍物6边缘距离Lg；移动机器人中点到障碍物5边缘距离Lh；Lg和Lh的夹角θ_g-θ_h和移动机器人宽度2R均已知。

首先筛选不连续的函数值为单个点的情况，单独Le和Lf分别与2R进行比较，Le大于2R，Lf不大于2R，故这里仅保留角度θ_e。

然后筛选不连续的函数值为区间的情况，用余弦定理计算障碍物5和障碍物6之间的出口宽度这里W大于2R，故保留角度区间[θ_g,θ_h]。

所以经过筛选之后的角度或角度区间剩角度θ_e和区间[θ_g,θ_h]，移动机器人首先朝着移动角度最小的θ_e方向运动，从图6可以得出的是出口逐渐变窄，故移动机器人朝着该出口还是无法脱困，于是回到拖轮起点，朝着区间[θ_g,θ_h]的中间角度运动，尝试脱困，由于W已经是最窄的宽度，故最终脱困成功，移动机器人继续进行被困之前的任务。

如果仅用普适控制方法，由图7可知，函数值不连续的角度或角度区间有θ_e、θ_f、[θ_g,θ_h]，然后移动机器人首先朝着转动幅度最小的θ_e移动，失败后回到脱困起点，朝着未尝试角度或角度区间中转动幅度最小的[θ_g,θ_h]的中间角度移动，脱困成功，而如果采用了优化方案在运动之前进行了筛选，则可以有效避免无用的尝试，提高脱困效率。

上述两个实施例脱困工作结束时移动机器人均脱困成功，如果在实施例2中，障碍物5、6之间的宽度W小于2R，则移动机器人无法脱困，这时移动机器人停止尝试脱困，保存电量，等待人工干预。

本发明一种移动机器人，其特征在于，包括：碰撞传感器、测距传感器、坐标系建立系统、控制器、驱动装置、计时器，所述碰撞传感器可以检测移动机器人发生了碰撞并将信息发送至控制器，所述测距传感器用于检测移动机器人与障碍物之间的距离并将信息记录在控制器，其中坐标系建立系统包括里程计和陀螺仪。

本发明还提供了一种移动机器人控制装置，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的计算机程序指令，所述处理器执行所述计算机程序指令时，实现实施例一或二的方法步骤。

与现有技术相比，本发明的有益效果：在移动机器人被困时，仅通过搭载在机身的距离传感器寻找出口方向，用较低的成本达到较好的效果。

所属领域的普通技术人员应当理解：以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种移动机器人的快速脱困方法，其特征在于，包括：

S1：获取当前时刻移动机器人与周围障碍物之间的距离及角度；

S2：构建以角度为自变量，距离为因变量的函数；

S3：筛选出函数值不连续的角度或角度区间；

S4：移动机器人沿筛选出的角度或角度区间移动一定距离执行脱困动作；

其中，所述步骤S3和S4之间还有步骤S31：从步骤S3中筛选出的角度或角度区间中筛选出可供移动机器人通过的角度或角度区间；

所述步骤S31中的筛选方法是：如果不连续的函数值为单个点，则判断函数值发生跳变的量是否大于移动机器人通过所需最小宽度，若是则保留，若否则移除；

如果不连续的函数值为区间，则根据余弦定理、区间夹角、夹角两侧边长计算出口宽度，若宽度大于移动机器人通过所需最小宽度，若是则保留，若否则移除。

2.根据权利要求1所述的一种移动机器人的快速脱困方法，其特征在于，所述步骤S4包括：

S41：移动机器人沿前一步骤中筛选出的角度或角度区间运动；

S42：尝试脱困，判断是否成功，若是进入步骤S45，若否进入步骤S43；

S43：判断筛选出的角度或角度区间中是否还存在未被尝试的角度或角度区间，若是，则进入步骤S44，若否则脱困失败进入步骤S45；

S44：移动机器人朝着未被尝试的角度或角度区间方向前进，进入步骤S42；

S45：脱困工作结束。

3.根据权利要求2所述的一种移动机器人的快速脱困方法，其特征在于，所述步骤S41和步骤S44中，移动机器人沿筛选出的角度或角度区间运动时选择当前未被尝试的角度或角度区间中转动幅度最小的角度或角度区间。

4.根据权利要求3所述的一种移动机器人的快速脱困方法，其特征在于，若移动机器人是沿着角度区间运动，则沿着该角度区间的中间角度运动。

5.一种移动机器人，包括上述权利要求1-4任意一项所述的一种移动机器人的快速脱困方法，其特征在于，包括：测距传感器、陀螺仪、控制器、驱动装置，所述测距传感器可以测量其与障碍物之间的距离，所述陀螺仪可以测量其运动角度，所述控制器可以根据测量到的距离和角度构建函数并且对函数值进行筛选和计算，所述驱动装置可以带动移动机器人运动，控制器可以控制驱动装置。

6.根据权利要求5所述的移动机器人，其特征在于，所述测距传感器为超声波测距传感器或激光雷达传感器或红外测距传感器。

7.一种移动机器人控制装置，其特征在于，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的计算机程序指令，所述处理器执行所述计算机程序指令时，实现权利要求1-4任一所述的方法步骤。