基于单点TOF的机器人脱困方法与流程

本发明涉及机器人技术领域，具体涉及一种基于单点tof的机器人脱困方法。

背景技术：

因为视觉和惯导机器人不能像激光一样能直接获取到周围环境的信息，如果视觉和惯导的机器人在移动时进入了一桌六椅或者有多个小障碍物围绕的地方，它们只能通过沿边的方式来走出去，但是沿边又存在很大的不确定性，会在障碍物中出现反复沿边的现象，从而使得整体清扫规划变得很糟糕，而且有可能被困在障碍中，就算反复沿边也无法脱困。

技术实现要素：

为解决上述问题，本发明提供了基于单点tof的机器人脱困方法，机器人在判断自身被困时，可以通过tof模块获取周围环境信息来获取脱困信息，从而脱困，提高机器人的脱困能力，使机器人的整体清扫规划变的更佳。本发明的具体技术方案如下：

一种基于单点tof的机器人脱困方法，该方法包括以下步骤：s1：机器人判断自身被困，然后通过单点tof模块来获取自身周围的障碍物的信息点；s2：机器人对障碍物的信息点进行处理来获得若干脱困出口；s3：机器人根据设定的条件选择进行脱困的脱困出口，并通过脱困出口进行脱困。机器人在自身判断被困后，通过tof模块来获取环境信息，再从环境信息中获取脱困信息，然后根据脱困信息快速进行脱困，提高机器人的整体性能；采用固定设置的单点tof模块，成本较低。

进一步地，步骤s1中，在设定时间内，机器人在地图上的移动位置小于设定距离，则判断自身被困。

进一步地，步骤s1中，机器人原地旋转一圈来获取障碍物的信息点。

进一步地，机器人在原地旋转过程中获取信息点与机器人的距离和信息点相对于机器人的位置。

进一步地，步骤s2中，机器人将信息点拟合为直线，获取相邻拟合直线的端点之间的点间距离，然后将点间距离与机器人的宽度进行比较，若点间距离大于机器人的宽度，则该两点之间设为脱困出口。

进一步地，步骤s3中，机器人选择距离机器人最近的脱困出口来进行脱困。机器人选择最近的脱困出口进行脱困，提高机器人的脱困速度。

进一步地，步骤s3中，机器人进行脱困的步骤包括：机器人选择最近的脱困出口，移动至脱困出口的其中一个信息点前方预设距离处，然后绕信息点进行沿边脱困。

进一步地，机器人绕信息点进行沿边脱困时，若两个信息点均位于机器人的左侧，则机器人绕距离机器人最近的信息点进行左沿边来脱困；若两个信息点均位于机器人的右侧，则机器人绕距离机器人最近的信息点进行右沿边来脱困。

进一步地，机器人绕信息点进行沿边脱困时，若两个信息点分别位于机器人的两侧且距离机器人最近的信息点位于机器人的左侧，则机器人绕该信息点进行右沿边来脱困；若两个信息点分别位于机器人的两侧且距离机器人最近的信息点位于机器人的右侧，则机器人绕该信息点进行左沿边来脱困。

进一步地，机器人进行沿边脱困时，若机器人在该脱困出口无法脱困，则会通过沿边行走至最近的脱困出口。机器人可以根据实际情况来更改脱困出口，提高机器人的灵活性。

附图说明

图1为本发明一种实施例基于单点tof的机器人脱困方法的流程图；

图2为本发明一种实施例所述的障碍物信息图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行详细描述。应当理解，下面所描述的具体实施例仅用于解释本发明，并不用于限定本发明。

tof是timeofflight的简写，直译为飞行时间的意思。所谓飞行时间法3d成像，是通过给目标连续发送光脉冲，然后用传感器接收从物体返回的光，通过探测光脉冲的飞行(往返)时间来得到目标物距离。如果单点tof放在slam机器上，此时单点tof可以获取到远处障碍物离机器的距离，再利用slam机器在进行规划的过程中计算出机器的世界坐标以及当前机器的倾角，则可以计算出当前环境中障碍物在世界坐标下的位置。此时的障碍物只是一个点，但而tof在机器不断行走规划的过程中一直在获取这些障碍物的信息，那么这些信息就能形成一片二维点云，这就是单点tof结合slam能给slam提供的信息。

如图1所示，一种基于单点tof的机器人脱困方法，该方法包括以下步骤：s1：机器人判断自身被困，然后通过单点tof模块来获取自身周围的障碍物的信息点；s2：机器人对障碍物的信息点进行处理来获得若干脱困出口；s3：机器人根据设定的条件选择进行脱困的脱困出口，并通过脱困出口进行脱困。机器人在自身判断被困后，通过tof模块来获取环境信息，再从环境信息中获取脱困信息，然后根据脱困信息快速进行脱困，提高机器人的整体性能；采用固定设置的单点tof模块，成本较低。

进一步地，步骤s1中，在设定时间内，机器人在地图上的移动位置小于设定距离，则判断自身被困。机器人在一段时间内，一直被困在一处地方，或机器人一直进行沿边，且在地图上的移动位置较小时，就会触发被困模式。机器人原地旋转一圈来获取障碍物的信息点。机器人可以进行多次旋转来获取更多和更准确的障碍物信息，可以有效提高机器人的数据准确度；还可以将没有多次获取到的信息点去除来降低噪点对机器人计算的影响。机器人在原地旋转过程中获取信息点与机器人的距离和信息点相对于机器人的位置。机器人在旋转时通过单点tof模块获取信息点与机器人的距离，然后再获取信息点时，通过陀螺仪来记录机器人转动的角度，就可以知道信息点在机器人的哪个方位。

进一步地，步骤s2中，机器人将信息点拟合为直线，获取相邻拟合直线的端点之间的点间距离，然后将点间距离与机器人的宽度进行比较，若点间距离大于机器人的宽度，则该两点之间设为脱困出口。机器人先将信息点拟合为直线，单独的信息点也可以作为一条直线看待，然后计算相连两条直线两个相近的端点的距离。可以采用三角函数来进行距离计算，机器人已知这两个端点与机器人的距离，也就是信息点与机器人的距离，也知道这两个距离所在直线之间的夹角，然后通过余弦函数，任何一边的平方等于其他两边平方的和减去这两边与它们夹角的余弦的两倍积，则两端点之间的距离的平方就等于两个端点与机器人的距离的平方和减去这两个距离与它们夹角的余弦的两倍积，就可以求出点间距离。

进一步地，步骤s3中，机器人选择距离机器人最近的脱困出口来进行脱困。机器人选择最近的脱困出口进行脱困，提高机器人的脱困速度。机器人进行脱困的步骤包括：机器人选择最近的脱困出口，移动至脱困出口的其中一个信息点前方预设距离处，然后绕信息点进行沿边脱困。机器人绕信息点进行沿边脱困时，若两个信息点均位于机器人的左侧，则机器人绕距离机器人最近的信息点进行左沿边来脱困；若两个信息点均位于机器人的右侧，则机器人绕距离机器人最近的信息点进行右沿边来脱困。机器人绕信息点进行沿边脱困时，若两个信息点分别位于机器人的两侧且距离机器人最近的信息点位于机器人的左侧，则机器人绕该信息点进行右沿边来脱困；若两个信息点分别位于机器人的两侧且距离机器人最近的信息点位于机器人的右侧，则机器人绕该信息点进行左沿边来脱困。机器人进行沿边脱困时，若机器人在该脱困出口无法脱困，则会通过沿边行走至最近的脱困出口。机器人可以根据实际情况来更改脱困出口，提高机器人的灵活性。

如图2所示，该图为机器人被困后通过单点tof模块获取的周围障碍物信息图，图中的直线为机器人拟合信息点后形成的拟合直线，机器人在拟合出直线后就计算相邻拟合直线的两个相近的端点之间的距离，一共计算了6次，其中，只有第3和第5的点间距离大于机器人的宽度，则机器人将该两处的出口设定为脱困出口，而第3处的脱困出口离机器人更近，则机器人向组成第3处的脱困出口的两个端点中的一个进行移动，移动到该端点前方预设距离前，然后绕该端点进行沿边行走，沿边行走设定距离或者沿边行走中不碰到障碍物，则判断自身脱困成功，不然就移动到第5脱困出口处进行脱困。

显然，上述的实施例仅仅是本发明一部分实施例，而不是全部的实施例，各个实施例之间的技术方案可以相互结合。此外，如果实施例中出现“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等术语，其指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位或以特定的方位构造和操作，因此不能理解为对本发明的限制。如果实施例中出现“第一”、“第二”、“第三”等术语，是为了便于相关特征的区分，不能理解为指示或暗示其相对重要性、次序的先后或者技术特征的数量。

本领域普通技术人员可以理解：实现上述各方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成。这些程序可以存储于计算机可读取存储介质（比如rom、ram、磁碟或者光盘等各种可以存储程序代码的介质）中。该程序在执行时，执行包括上述各方法实施例的步骤。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。