CN110244743B

CN110244743B - 一种融合多传感器信息的移动机器人自主脱困方法

Info

Publication number: CN110244743B
Application number: CN201910597251.5A
Authority: CN
Inventors: 周天成; 王进; 许力; 陆国栋; 郑涛
Original assignee: Zhejiang University ZJU
Current assignee: Zhejiang University ZJU
Priority date: 2019-07-03
Filing date: 2019-07-03
Publication date: 2022-02-01
Anticipated expiration: 2039-07-03
Also published as: CN110244743A

Abstract

本发明公开了一种融合多传感器信息的移动机器人自主脱困方法，属于电子信息和机器人智能控制技术领域。机器人上安装激光雷达传感器，超声波传感器，里程计，9轴陀螺仪传感器；所述移动机器人自主脱困方法步骤如下：步骤1：判断机器人是否被困；步骤2：如果被困，建立机器人当前所在区域的平面坐标系；步骤3：机器人通过超声波传感器和陀螺仪传感器标定周围“缺口”的大小，方向和数量，所述“缺口”为相邻两个障碍物端点之间的距离；步骤4：机器人选择合适的“缺口”通行；步骤5：判断是否脱困成功，如果未成功，则重复步骤1至步骤5，寻找新的“缺口”直到脱困成功。

Description

一种融合多传感器信息的移动机器人自主脱困方法

技术领域

本发明涉及电子信息和机器人智能控制技术领域，具体涉及一种融合多传感器信息的移动机器人自主脱困方法。

背景技术

移动机器人在使用激光雷达构建复杂环境地图时，在一些地方会存在激光雷达的地图盲区，而这部分环境通常是比较复杂的，有时候机器人进入这些环境中之后就会被困住，很久都出不来，这就需要机器人能够在脱离激光雷达定位功能时也能都顺利实现从这些复杂环境中脱困出来。针对该问题，现有技术提出的方法大多是需要机器人一边碰撞一边检测，这样在某种程度上会增加机器人的损耗，以及周围环境物体的破损，在一些环境中不是很适用。

发明内容

本发明的目的在于：提供一种融合多传感器信息的移动机器人自主脱困方法，以解决上述现有技术中的多次碰撞对机器人和环境造成损坏的不足，结合激光雷达传感器，超声波传感器，里程计，9轴陀螺仪传感器，红外传感器等多种传感器，得到一种搜索最大“缺口”并不断循环使用的方法，能够有效地保证机器人完成地图盲区的脱困，同时尽可能小地减少碰撞。

为了实现上述目的，本发明采用的技术方案为：

一种融合多传感器信息的移动机器人自主脱困方法，其特征在于所述移动机器人上安装激光雷达传感器，超声波传感器，里程计，9轴陀螺仪传感器；所述移动机器人自主脱困方法步骤如下：

步骤1：判断机器人是否被困；

步骤2：如果被困，建立机器人当前所在区域的平面坐标系

步骤3：机器人通过超声波传感器和陀螺仪传感器标定周围“缺口”的大小，方向和数量，所述“缺口”为相邻两个障碍物端点之间的距离；

步骤4：机器人选择合适的“缺口”通行；

步骤5：判断是否脱困成功，如果未成功，则重复步骤1至步骤5，寻找新的“缺口”直到脱困成功。

进一步的，所述的机器人为圆形，超声波传感器设置于机器人的前半部分位置，所述9轴陀螺仪传感器中心与机器人的几何中心重合；所属超声波传感器位于9轴陀螺仪传感器的正东方向；激光雷达传感器，用于机器人构建地图和脱困成功与否的判断。

进一步的，所述的步骤1中机器人是否被困的判断方法，包括如下步骤：

步骤11：机器人通过雷达检测到其已经进入了激光雷达建立的地图盲区；

步骤12：机器人设定一个激光雷达地图中的运动目标点，并设定最长运动时间；

步骤13：如果机器人在最长运动时间内无法到达运动目标点，说明机器人已经被困住。

进一步的，所述步骤2中机器人建立平面坐标系的方法，包括如下步骤：

步骤21：机器人以开始进入脱困模式时的当前位置作为平面坐标系的原点；

步骤22：机器人设定每个坐标单元对应1cm；

步骤23：机器人设定x轴正方向为正东方向，y轴正方向为正北方向；

步骤24：机器人原地旋转，调整其超声波传感器初始位置在x轴正方向上，并设定当前的绝对角度为0度。

进一步的，所述步骤3中机器人标定周围“缺口”的大小，方向的方法，包括如下步骤：

步骤31：机器人旋转一周，用超声波传感器记录每个障碍物端点与坐标原点的距离，用陀螺仪传感器记录每个障碍物端点对应的绝对角度；

步骤32：根据测量到的距离和角度，根据余弦定理计算相邻两个障碍物之间“缺口”的大小和方位；

步骤33：将各个“缺口”大小与机器人的直径相比较，如果大于机器人直径，则标记为“可通行缺口”，如果小于机器人直径，则标记为“不可通行缺口”；

步骤34：机器人通过检测到的“缺口”的两个端点到原点的距离和角度即可通过余弦公式计算出“缺口”所对应的角度。

进一步的，所述步骤4中，合适“缺口”的选择方式为：

如果当前区域存在“可通行缺口”，则机器人选择最大的“可通行缺口”通行；

如果当前区域不存在“可通行缺口”，则机器人按照从大到小的原则依次选择“不可通行缺口”，通过推动，看机器人是否能扩大“不可通行缺口”，如果可扩大则直接通行，否则就退回尝试下一个“不可通行缺口”，直到机器人找到可以扩大通行的“缺口”，之后机器人进入下一个区域，重新开始查找新的“缺口”。

进一步的，所述的“可通行缺口”的计算方法为机器人测量到OA和OB的长度，所述OA、OB分别为机器人到“缺口”两侧的距离，并通过9轴陀螺仪传感器测得OA和OB之间的夹角∠θ可得：

AB²＝OA²+OB²-20A·OB·cosθ

设机器人直径为d，则如果AB²>d²则说明该“缺口”为“可通行缺口”，不需要求平方根运算来算出AB的长度。

进一步的，所述的机器人计算出“缺口”的大小后需要移动到合适的位置，这样才能最大限度不碰触到障碍物的边缘；其中三角形OAB的∠a的计算如下：

机器人旋转∠a相同角度之后朝着这一方向前进，前进的过程中机器人通过超声波传感器不断侦测OA和OB的长度，当OA与OB相等时机器人停下来，再右转90度，此时就到达了机器人“缺口”AB的最佳位置，机器人当前正对着的方向即为AB中垂线所在方向，这时候机器人能够最大限度地保证不碰到“缺口”AB两端障碍物的情况下通过“缺口”AB。

进一步的，如果所有的“不可通行缺口”都无法扩大，则机器人回退到上一个区域，并屏蔽掉回退到上一个区域所经过的“缺口”，继续选择新的“缺口”通行；如果机器人的当前区域没有上一个区域，说明机器人在起始区域就无法动弹，因此在当前区域内无法脱困。

进一步的，所述的机器人检测脱困成功的方法为：机器人在进入新的区域，就检测机器人是否出现在激光雷达所建立的地图中，如果出现则说明机器人脱困成功。

本发明的方法能够有效地保证机器人完成地图盲区的脱困，同时尽可能小地减少碰撞。

附图说明

图1机器人各个传感器的布局图；

图2机器人脱困模拟图；

图3机器人缺口选择图；

图4机器人脱困流程图；

其中1-机器人；2-超声波传感器；3-9轴陀螺仪传感器；4-激光雷达传感器。

具体实施方式

为了更好得理解本发明的技术方案，以下结合附图对本发明的实施方式进一步的描述：

图1中机器人1为圆形，机器人1上安装激光雷达传感器4，超声波传感器2，里程计，9轴陀螺仪传感器3，所述超声波传感器2设置于机器人1的前半部分位置，9轴陀螺仪传感器3中心与机器人1的几何中心重合，能够更加准确地测量机器人1转动的角度；超声波传感器2位于9轴陀螺仪传感器3正东方向；激光雷达传感器4，用于机器人构建地图和脱困成功与否的判断。

本发明的程序编译平台Windows7系统64位，编程开发环境为ArmKeil5。下位机使用的是stm32F103ZET6单片机，采集并处理超声波传感器2，9轴陀螺仪传感器3的信息，上位机使用树莓派3b开发板，采集并处理激光雷达信息。具体的实施步骤如下：

(1)机器人在进入激光雷达地图盲区，机器人设定一个激光雷达地图中的运动目标点，并设定最长运动时间。如果机器人在最长运动时间内无法到达运动目标点，说明机器人已经被困住。此时机器人开始进入脱困模式。

(2)当检测到机器人已经被困住时，机器人通过陀螺仪和里程计进行平面定位，确定机器人当前位置为坐标原点(0,0)，以1cm为一个坐标单元，y轴正方向为正北方向，x轴正方向为正东方向。机器人原地旋转，调整其超声波传感器2初始位置在x轴正方向上，并设定当前的绝对角度为0度。

(3)机器人通过不断旋转超声波传感器2来侦测坐标原点周围的障碍物，并与9轴陀螺仪传感器3相结合标定各个障碍物的平面坐标，机器人1各个传感器的布局如图1所示。

(4)机器人1通过测量到的障碍物与机器人1的距离，以及每个障碍物所在的坐标方位，分别计算两个障碍物之间的“缺口”的大小，如果“缺口”大小大于机器人1的直径，则可将其标记为一个“可通行缺口”，如果“缺口”大小小于机器人1的直径，则将其标记为“不可通行缺口”，记录机器人1原始坐标周围360度范围内的“可通行缺口”与“不可通行缺口”的大小和位置，并对其标记，机器人1的脱困模拟情况如图2所示。

机器人可以测量到超声波传感器2到各个障碍物之间的距离，在加上超声波传感器2到机器人1圆心的距离，就可以计算出机器人1圆心与障碍物之间的距离。例如，在图2中机器人可以测量到OA和OB的长度，并通过9轴陀螺仪传感器3测得OA和OB之间的夹角∠θ可得：

AB²＝OA²+OB²-20A·OB·cosθ

设机器人直径为d，则如果AB²>d²则说明该“缺口”为“可通行缺口”就不需要求平方根运算来算出AB的长度，这样就在一定程度上减少了计算量。机器人计算出“缺口”的大小后还需要移动到合适的位置，这样才能最大限度不碰触到障碍物的边缘。其中三角形OAB的∠a的计算如下：

机器人旋转∠a相同角度之后朝着这一方向前进，前进的过程中机器人通过超声波传感器2不断侦测OA和OB的长度，当OA与OB相等时机器人停下来，再右转90度，此时就到达了机器人“缺口”AB的最佳位置，机器人当前正对着的方向即为AB中垂线所在方向，这时候机器人能够最大限度地保证不碰到“缺口”AB两端障碍物的情况下通过“缺口”AB。

另外，当机器人在当前区域内检测到多个“可通行缺口”时，虽然只选择了最大的“可通行缺口”通过，但是仍然需要按照从大到小的原则标记所有“可通行缺口”，如图3所示。

图3中每个区域代表有若干个障碍物分割的机器人移动区域，区域0代表机器人收到脱困命令时所在的当前区域。每一个双向箭头代表一个“缺口”，这个“缺口”可以是“可通行缺口”，也可以是可以被机器人推动扩张的“不可通行缺口”，但不是机器人无法推开的“不可通行缺口”。“缺口”的编号越小，说明“缺口”越大，机器人在进入下一个区域时，优先从最大的“缺口”通过。区域的编号由与区域连接的“缺口”的编号决定，每次都在上一个区域编号后加上进入该区域的“缺口”的编号。每进入一个区域机器人会通过9轴陀螺仪传感器3和里程计对经过的每个区域的单元格进行标记，当机器人没有进入回退模式时，不重复进入已进入区域的单元格。

当机器人进入某一区域时，发现没有“可通行缺口”和可以被机器人推动扩张的“不可通行缺口”，机器人就进入回退模式，沿着进入该区域的“缺口”，回退到上一个区域，并从第二大的“可通行缺口”通行，机器人的脱困流程图如图4所示。

这样当机器人在当前区域通过最大“可通行缺口”到达下一个区域时，如果发现下一个区域中没有“可通行缺口”(不包括从上一个区域到本区域的“可通行缺口”)，并且所有的“不可通行缺口”都无法通过机器人推开扩大，这样就需要返回到上一个区域，从上一个区域的第二大“可通行缺口”通行。这样依次遍历，就能够遍历当前区域内所有可能脱困的路径。

(5)如果机器人在坐标原点360度范围内发现没有“可通行缺口”，只有“不可通行缺口”，则优先朝着最大的“不可通行缺口”中间减速前进，如果机器人可以推开该障碍物，则“不可通行缺口”可扩大为“可通行缺口”。机器人到达下一个位置。

(6)如果机器人在坐标原点360度范围内检测到多个“可通行缺口”，则优先选择最大的“可通行缺口”，朝着其中间前进。

(7)机器人在通过“缺口”后到达下一个位置，在下一个位置后同样旋转360度查找“缺口”，重复上一个位置的运动步骤，但是确保不返回上一次的位置。

(8)机器人如果在某一个位置没有查找到“可通行缺口”，而且“不可通行缺口”都尝试过了，仍旧无法通行，就返回上一个位置，从上一个位置的第二大的“可通行缺口”前进。

(9)最终，当激光雷达检测到机器人从盲区中出来，出现在地图中时，说明脱困成功。

Claims

1.一种融合多传感器信息的移动机器人自主脱困方法，其特征在于所述移动机器人(1)安装激光雷达传感器(4)，超声波传感器(2)，里程计，9轴陀螺仪传感器(3)；

所述移动机器人自主脱困方法步骤如下：

步骤1：判断机器人(1)是否被困；

步骤2：如果被困，建立机器人(1)当前所在区域的平面坐标系

步骤3：机器人通过超声波传感器(2)和9轴陀螺仪传感器(3)标定周围“缺口”的大小，方向和数量，所述“缺口”为相邻两个障碍物端点之间的距离；

步骤4：机器人(1)选择合适的“缺口”通行，合适“缺口”的选择方式为：

如果当前区域存在“可通行缺口”，则机器人(1)选择最大的“可通行缺口”通行；

如果当前区域不存在“可通行缺口”，则机器人(1)按照从大到小的原则依次选择“不可通行缺口”，通过推动，看机器人是否能扩大“不可通行缺口”，如果可扩大则直接通行，否则就退回尝试下一个“不可通行缺口”，直到机器人找到可以扩大通行的“缺口”，之后机器人进入下一个区域，重新开始查找新的“缺口”；

2.根据权利要求1所述的一种融合多传感器信息的移动机器人自主脱困方法，其特征在于所述的机器人(1)为圆形，超声波传感器(2)设置于机器人的前半部分位置，所述9轴陀螺仪传感器(3)中心与机器人(1)的几何中心重合；所属超声波传感器(2)位于9轴陀螺仪传感器(3)的正东方向；激光雷达传感器(4)，用于机器人构建地图和脱困成功与否的判断。

3.根据权利要求1所述的一种融合多传感器信息的移动机器人自主脱困方法，其特征在于，所述的步骤1中机器人(1)是否被困的判断方法，包括如下步骤：

步骤11：机器人(1)通过雷达检测到其已经进入了激光雷达建立的地图盲区；

步骤12：机器人(1)设定一个激光雷达地图中的运动目标点，并设定最长运动时间；

步骤13：如果机器人(1)在最长运动时间内无法到达运动目标点，说明机器人(1)已经被困住。

4.根据权利要求1所述的一种融合多传感器信息的移动机器人自主脱困方法，其特征在于，所述步骤2中机器人建立平面坐标系的方法，包括如下步骤：

步骤21：机器人(1)以开始进入脱困模式时的当前位置作为平面坐标系的原点；

步骤22：机器人(1)设定每个坐标单元对应1cm；

步骤23：机器人(1)设定x轴正方向为正东方向，y轴正方向为北方向；

步骤24：机器人(1)原地旋转，调整其超声波传感器初始位置在x轴正方向上，并设定当前的绝对角度为0度。

5.根据权利要求1所述的一种融合多传感器信息的移动机器人自主脱困方法，其特征在于，所述步骤3中机器人标定周围“缺口”的大小，方向的方法，包括如下步骤：

步骤31：机器人(1)旋转一周，用超声波传感器(2)记录每个障碍物端点与坐标原点的距离，用9轴陀螺仪传感器(3)记录每个障碍物端点对应的绝对角度；

步骤33：将各个“缺口”大小与机器人(1)的直径相比较，如果大于机器人(1)直径，则标记为“可通行缺口”，如果小于机器人(1)直径，则标记为“不可通行缺口”：

步骤34：机器人(1)通过检测到的“缺口”的两个端点到原点的距离和角度即可通过余弦公式计算出“缺口”所对应的角度。

6.根据权利要求1所述的一种融合多传感器信息的移动机器人自主脱困方法，其特征在于所述的“可通行缺口”的计算方法为机器人测量到OA和OB的长度，所述OA、OB分别为机器人到“缺口”两侧的距离，并通过9轴陀螺仪传感器(3)测得OA和OB之间的夹角∠θ可得：

AB²＝OA²+OB²-2OA·OB·cosθ

设机器人直径为d，则如果AB²＞d²则说明该“缺口”为“可通行缺口”，不需要求平方根运算来算出AB的长度。

7.根据权利要求6所述的一种融合多传感器信息的移动机器人自主脱困方法，其特征在于所述的机器人计算出“缺口”的大小后需要移动到合适的位置，这样才能最大限度不碰触到障碍物的边缘；其中三角形OAB的∠a的计算如下：

机器人旋转∠a相同角度之后朝着这一方向前进，前进的过程中机器人通过超声波传感器(2)不断侦测OA和OB的长度，当OA与OB相等时机器人停下来，再右转90度，此时就到达了机器人“缺口”AB的最佳位置，机器人当前正对着的方向即为AB中垂线所在方向，这时候机器人能够最大限度地保证不碰到“缺口”AB两端障碍物的情况下通过“缺口”AB。

8.根据权利要求1所述的一种融合多传感器信息的移动机器人自主脱困方法，其特征在于，如果所有的“不可通行缺口”都无法扩大，则机器人回退到上一个区域，并屏蔽掉回退到上一个区域所经过的“缺口”，继续选择新的“缺口”通行；如果机器人的当前区域没有上一个区域，说明机器人在起始区域就无法动弹，因此在当前区域内无法脱困。

9.根据权利要求1所述的一种融合多传感器信息的移动机器人自主脱困方法，其特征在于，所述的机器人检测脱困成功的方法为：机器人在进入新的区域，就检测机器人是否出现在激光雷达传感器(4)所建立的地图中，如果出现则说明机器人脱困成功。