CN109916393B - 一种基于机器人位姿的多重栅格值导航方法及其应用 - Google Patents

Abstract

本发明公开了一种基于机器人位姿的多重栅格值导航方法及其应用，属于机器人导航技术领域。本发明基于机器人位姿的多重栅格值导航方法，扩大了机器人在栅格地图中的可通过区域，增强了拓扑地图的联通性。同时，在基于BIM信息的室内墙面施工自主移动机器人导航方法中，基于提取的BIM信息计算工作点，再根据工作点的位置信息和工作任务顺序，基于机器人位姿的多重栅格值导航方法，设置相邻工作点之间的局部导航路线；实时定位并检测室内墙面施工自主移动机器人周围的障碍物信息，更新栅格地图，并基于更新后的栅格地图重新设置相邻工作点之间的局部导航路线。本发明的实施，提升了导航工作效率和准确率。

Description

一种基于机器人位姿的多重栅格值导航方法及其应用

技术领域

本发明涉及一种机器人导航技术领域，具体涉及一种基于机器人位姿的多重栅格值导航技术。

背景技术

随着计算机技术的发展，机器人SLAM(Simultaneous Localization AndMapping)技术愈发成熟，常用的技术包括基于计算机视觉的VSLAM(Visual SimultaneousLocalization And Mapping)和基于激光雷达的多传感器融合的SLAM。这些技术应用于诸多领域，如扫地机器人，外太空勘探机器人等。

传统2D激光SLAM技术构建的地图为栅格地图，栅格地图仅仅具有尺度信息而不具备语义信息。从而难以较好的指导机器人进行工作。

普通基于栅格地图的机器人导航中，机器人很难安全到达外切障碍区域，以及非自由空间。更进一步的描述为：常规栅格地图中，每一个栅格有一个栅格值，如图3所示。这个栅格值与地图环境以及机器人的外形尺寸相关。栅格值为254时为致命障碍，障碍物与机器人中心重合，此时机器人必然与障碍物发生碰撞。栅格值253表示内切障碍，此时障碍物处于机器人轮廓的内切圆内，栅格值252～128是外切障碍，此时机器人与障碍物临接接触，不一定发生碰撞。非自由空间为128～0，为机器人外切障碍到自由区域的缓存空间。自由区域为0，机器人可以自由运行，未知区域为255，表示为探测到的区域。常规的基于栅格地图的导航应该尽量避免机器人进入非自由空间，不能进入外切障碍。

此外，在实际工程应用中，有时机器人必须进入外切障碍区域，才能完成对应工作。实际例子就是建筑抹灰机器人进行抹灰的时候仅仅离墙2厘米，此时机器人已经进入了外切障碍区域，即现有技术无法解决室内墙面施工自主移动机器人的该导航问题。

发明内容

本发明的发明目的在于：针对上述存在的问题，提供一种基于机器人位姿的多重栅格值导航方法，其特征在于，包括下列步骤：

在栅格地图中，为每一个栅格除了存储栅格值信息外，额外定义一个表示机器人在不同角度状态下的是否碰撞标识符的数组；

对栅格地图进行预处理：将栅格地图中的所有自由区域的是否碰撞标识符标记为非碰撞标识符，以及将栅格地图中的致命障碍和内切障碍的是否碰撞标识符均置为碰撞标识符；

基于机器人的不同位姿的角度x，分别判断机器人的每个顶点是否落在致命障碍的栅格上，若全部顶点均未落在致命障碍的栅格上，则设置角度x对应的是否碰撞标识符为非碰撞标识符；否则设置是否碰撞标识符为碰撞标识符；从而得到多个与机器人角度相关的可达性地图，记为可达性地图Map[n]，其中n为对应的角度区分符；

基于机器人的当前起始点A与目标点B，设置从点A到点B的局部路径：

用Map0表示不限制机器人角度的由障碍物和自由区域构成的地图；

若A点处于自由区域，B点处于自由区域，且A点到B点能在地图Map0中查找到一条可达路径，则采用非限制角度变化的方式导航；若在地图Map0中不能查找到可达路径，则遍历搜索每个可达性地图Map[n]中是否存在从点A到点B的可达路径，其中每个可达性地图Map[n]只对应一条可达路径；再从搜索得到的所有可达路径中，查找最短路径的可达路径所对应的可达性地图，并将其对应的角度作为机器人的导航角度，通过限制机器人角度的方式导航到B点；

若A点处于自由区域，B点处于非自由区域，则基于点B的位置，将的所有可达性地图Map[n]中的是否碰撞标识符为非碰撞标识符的可达性地图作为第一可达性地图子集，即基于B点的是否碰撞标识符为非碰撞标识符时，则将对应地图加入需要搜索路径的子图集；并遍历第一可达性地图子集中的每个可达性地图中是否存在从点A到点B的可达路径；再从搜索得到的所有可达路径中，查找最短路径的可达路径所对应的可达性地图，并将其对应的角度作为机器人的导航角度，通过限制机器人角度的方式导航到B点；

若A点处于非自由区域，且当前机器人角度为R，那么搜索角度R对应的可达性地图Map[R]，并判断所述可达性地图Map[R]中是否存在可达路径，若存在，则通过限制机器人角度为R的方式导航到B点；否则认为点A到点B的路径不可达，即点A到点B的局部路径规划失败。

本发明基于机器人位姿的多重栅格值导航方法，扩大了机器人在栅格地图中的可通过区域，增强了拓扑地图的联通性。

同时，本发明还提供了一种基于BIM(Building Information Modeling)信息的室内墙面施工自主移动机器人导航方法。本发明提取并利用BIM信息用于机器人导航，以增强机器人在建筑施工的场景下导航定位的准确性，优化机器人的工作顺序，提高机器人的协同工作能力。其采用的技术方案为：

提取室内墙面施工自主移动机器人作业的室内环境的BIM信息，构建作业的室内地图，并基于所述室内地图计算出每个室内墙面施工作业的工作任务，得到每个工作任务的工作点的位置信息；并为所有工作任务设置工作任务顺序；

根据工作点的位置信息和工作任务顺序，基于本发明的基于机器人位姿的多重栅格值导航方法，设置相邻工作点之间的局部导航路线；

实时定位并检测室内墙面施工自主移动机器人周围的障碍物信息，更新栅格地图，并基于更新后的栅格地图重新基于本发明的基于机器人位姿的多重栅格值导航方法，设置相邻工作点之间的局部导航路线。

进一步的，还可以通过深度摄像头探测到的施工空洞以及地面未知障碍物信息，作为室内墙面施工自主移动机器人周围的障碍物信息。

相比于现有技术，本发明具有如下有益效果：

(1)基于机器人位姿的多重栅格值导航方法，扩大了机器人在栅格地图中的可通过区域，增强了拓扑地图的联通性；

(2)首次将BIM信息应用于机器人导航，解决了在建筑施工环境下，常规导航技术难以获取施工部件信息等问题。并且可以根据BIM信息计算出最优的工作路径，提高机器人工作效率。

(3)本发明利用超声波，RGB-D深度摄像头，探测环境中未知的危险，保证了人机安全。

附图说明

图1是本发明基于BIM信息的室内墙面施工自主移动机器人导航***的总体架构图。

图2是本发明基于BIM信息的室内墙面施工自主移动机器人导航***的流程图。

图3是栅格地图取值原理示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面结合实施方式和附图，对本发明作进一步地详细描述。

本发明的基于BIM信息的室内墙面施工自主移动机器人导航方法，基于预设的提取BIM信息的插件，提取BIM信息，通过BIM信息计算工作点，规划最优路径。并且将通过BIM信息生成的地图实时显示到远程控制终端，方便人机交互。机器人通过激光雷达，IMU，实现定位，超声波探测周围障碍物，发现障碍物及时重新规划路线。并且利用RGB—D深度摄像头探测BIM信息中不包含的临时开凿的施工空洞，避免危险的发生。机器人集群将机器人工作状态信息同步到总控终端，总控终端合理分配任务。采用本发明可以简化建筑机器人的使用，降低用户操作难度，机器人的工作任务可以实现自动计算，同时提升导航工作效率。

具体实现时，其对应的导航***包括激光雷达、IMU，RGB-D深度信息摄像头，4轮机器人移动平台(简称机器人)，工控机(工业控制计算机的***)，总控终端的硬件***，并具体执行下述操作：

首先，基于预设的一个用于提取BIM信息的插件，获取施工环境的地图信息以及待施工部件的特征属性(例如门，窗，柱，墙面，等)，尺寸位置等信息。并对提取的BIM信息进行下述处理：

实时显示所述施工环境的地图信息，以及机器人在作业环境中的位置和工作状态。并且可以通过远程终端实时监控机器人。其中，所述远程终端可以是PC，移动设备，VR(Virtual Reality)设备等。

所述工控机根据BIM信息，计算出每个工作(室内墙面施工作业)的具体任务，并设置每个对应的工作顺序，按照预设的规则设置优化的工作顺序即可。

其次，通过激光雷达获取作业环境的实时信息，并发送给工控机，通过工控机实现机器人定位。其具体实现过程可以是：

实时显示激光雷达获取的地图信息，根据激光雷达获取的地图信息，设置机器人的导航路径，其中导航路径的设置方法可以采用任何现有惯有方式，通常可考虑带有避障处理的导航方式；

机器人将当前位置坐标信息、工作状态信息实时同步给总控终端，总控终端统一协调工作任务；

将RGB-D深度摄像头探测到的施工空洞以及地面未知障碍物实时显示于远程终端。

再其次，采用激光雷达+IMU多传感器融合的方法，提高机器人定位精度以及实时建图的可靠性。其中IMU包含惯导信息，地磁传感器信息等。其具体实现过程如下：

工控机根据SLAM建图信息，计算收到的机器人位置信息，以及BIM信息，基于预置的导航路径规划方式为机器人规划导航的最优路径并向机器人反馈对应的移动指令。其中，最优路径为多条可完成施工任务的路线中的最高效路线。

机器人根据工控机所发出的移动指令进行自主导航。

然后，对于BIM中不包含的地面障碍以及开凿的施工空洞，利用RGB—D深度摄像头进行探测，避免危险的发生。

接着，利用BIM信息对室内环境进行分类，利用BIM信息构建栅格地图，并且为栅格地图中的每一个栅格赋予建筑信息语义标签。利用激光雷达，实时更新地图数据，并且在栅格地图的基础之上，生成语义拓扑地图，为机器人的导航提供依据。

其中语义拓扑地图用来进行全局路径规划，栅格地图用来进行局部路径规划。由于在大型施工场景下具有较多的拓扑节点，故本具体实施方式中，使用性能较优的蚁群算法寻找较优路线。

在局部路径规划的具体实现中，为了使机器人安全到达非自由空间栅格与外切障碍栅格，本发明还提出了一种改进的导航方法。一种基于机器人位姿的多重栅格值导航方法，从而使能在x,y两个方向自由平移的机器人能安全进入非自由空间栅格与外切障碍栅格。其具体实现过程如下；

在栅格地图中，每一个栅格除了存储栅格值信息外，额外定义一个N大小的数组A[N]，数组的每个元素A[x]表示机器人在角度为x的状态下是否会与障碍物发生碰撞的碰撞标识符，本具体实施方式中，用“-1”表示发生碰撞；“1”表示不发生碰撞。即与普通栅格地图是二维数组，每一个地图节点中存的数据是栅格值相比，本发明改进后的栅格地图每一个节点不仅存了一个栅格值，还存了一个表示对应节点在x角度下是否碰撞的一维数组A[x]。相当于将1+N个地图全部存到一个二维数组中，这些地图包括一个机器人可自由移动的普通栅格地图，以及N个限制角度的地图。

然后对栅格地图进行预处理，首先将所有自由区域的碰撞标识符(A值)置为1，致命障碍和内切障碍均置为-1。遍历外切障碍与非自由空间的栅格，并且对其做出可达性判断。若A[x]＝1则表示机器人在x角度下，在当前栅格中，不会与障碍物发生碰撞。而-1则表示会与之发生碰撞。具体的判断方法为：

(1)用M表示机器人的顶点数，且中心位置表示为W0(x0,y0),其余各顶点位置分别为W1(x1,y1)…WM(xM,yM)；

当机器人角度为x时，分别判断各顶点是否落在致命障碍的栅格上，如果全部没有落在致命障碍的栅格上，则A[x]＝1,反之A[x]＝-1；这样就生成了N个与机器人角度相关的可达性地图Map[1]，Map[2]…Map[N]，机器人可以通过可达性地图找到可达路径。

(2)机器人从A点到B点的局部路径规划具体步骤为：

用Map0表示不限制机器人角度的由障碍物和自由区域构成的地图；而限制机器人角度的地图为Map[n],其中n＝1,2，……，N；

若A点处于自由区域，B点处于自由区域，且A点到B点能在Map0中找到一条可达路径，则用不限制角度变化的方式导航。如果不能，则遍历搜索Map[N]，遍历搜索每个可达性地图中是否存在从点A到点B的可达路径(即每个可达性地图中仅存在一条可达路径，也称最优可达路径)，基于搜索得到的可达路径得到可达路径集合{P₁，…,P_K}，其中K表示N个可达性地图中包括的点A到点B的可达路径数；然后再将路径最短的可达路径(可定义为P_min)所对应的可达性地图(可定义为Map[min])所对应的角度(即min)作为机器人的导航角度，通过限制机器人角度的方式导航到B点。

若A点处于自由区域，B点处于非自由区域，那么搜索B点A[x]＝1的可达性地图子集，并遍历搜索该地图子集中的每个可达性地图中是否存在从点A到点B的可达路径，在从搜索得到的所有可达路径中，查找最短路径的可达路径所对应的可达性地图，记为Map[min]，再讲该可达性地图对应的角度min作为机器人的导航角度，通过限制机器人角度的方式导航到B点。

若A点处于非自由区域，且当前机器人角度为R，那么搜索地图Map[R],判断地图是否可达(即是否存在可达路径)，若存在，则通过限制机器人角度为R的方式导航到B点；否则B点不可达。

实施例

本实例采用分布式机器人操作***，并通过该***来共享不同机器人节点之间的数据，实验环境为实际搭建的一个半成品房屋，机器人要对其进行抹灰施工作业。

为了较好的获取到建筑施工现场的环境信息，将2D Lidar安装在机器人(高1.8m)顶部。为了保证施工过程中的安全，还需要在四周安装超声波传感器，才能探测到周围人等其他障碍物，遇到其他障碍物时重新规划路线。通过地磁传感器，实时获取小车角度，当发生强磁干扰时，停用地磁传感器数据，并做归零校正。在累计误差不大时，利用机器人自身里程计以及惯导里程计对机器人进行定位。当累计误差超过2D Lidar的SLAM***精度时，利用基于2D Lidar的SLAM***以及闭环检测算法对其重新定位，消除累计误差。到达工作点时，由于机器人要进行进一步的精确定位，故可以进一步消除累计误差。

利用RGB-D深度摄像头探测室内地面障碍以及临时开凿的施工空洞，避免危险的发生。

参加图1，本实施例中，实现本发明的室内墙面施工自主移动机器人导航的导航***包括5个模块；

其中，人机交互模块，主要用于机器人的参数设定，工作目标的选定，以及工作状态的实时显示，是操作人员与机器人交互，对机器人下达指令的主要模块；

建图模块，基于所提取的BIM信息，构建对应的作业环境地图；其包括建筑信息提取与地图构建、角点特征提取与闭环检测模块；

路径规划模块，控制机器人根据操作人员下达的指令，规划最优的路径；其包括工作点与工作点任务计算，规划最优路径和接受新任务路径重新规划三个子模块；

导航模块，实时定位确保机器人在正确的航线上，并对机器人的移动平台下达正确指令；其包括估计初始状态位姿、实时计算机器人位姿和同步数据子模块，其中同步数据用于实现本地与远端的数据同步；

避障与安全***中，机器人会确保人机安全。其包括超声波避障、***重启与恢复和深度摄像头探测(探测深度信息)。

具体的，本实施例中，在建图，路径规划，和导航模块中，本具体实施方式基于ROS(Robot Operating System，机器人操作***)框架开发。小车控制方面，通过RS232串口实现通信控制。在ROS框架下，速度指令控制为linear，用于控制XYZ方向的线速度，单位是m/s；速度指令控制angular，用于控制XYZ方向上的角速度，单位是rad/s。pose表示机器人当前位置坐标，包括机器人XYZ三轴位置与方向参数，以及用于校正误差的协方差矩阵。twist为机器人当前的运动状态，包括XYZ三轴的线速度与角速度，以及用于校正误差的协方差矩阵。ROS框架通过订阅和发表话题进行通信，订阅的话题包括：tf-用于激光雷达坐标系，基坐标系，里程计坐标系之间的变换。scan-激光雷达扫描的数据。发布的话题包括：map_metadata-发布地图Meta数据。map-发布地图栅格数据。entropy-发布机器人姿态分布熵的估计。

参加图2，为本发明的机器人导航处理流程图。其中，本发明的控制终端采用跨平台的形式，包括移动端和PC端。小车的位置信息，实时地图信息等，通过TCP并且采用轻量级的数据交换格式Json的格式传输给人机交互模块，用于对机器人工作状态的实时显示。

地图构建方面，分为BIM信息地图与激光雷达SLAM构建的实时环境地图。

即首先预设一个可以提取BIM信息的插件，将提取的信息作为构建地图的依据。根据建筑信息和建筑施工的实际要求确定工作点，并计算出每个工作点具体的工作任务以及工作时间，本具体实施方式中，利用迪杰斯特拉算法计算出最短路径，每个工作点之间通过导航网格算法规划路线。

激光雷达SLAM构建的实时环境地图，是基于粒子滤波器的SLAM算法，它通过指定一組在状态向量空间中传播的随机样本来近似化实际的概率分布，并用样本的加权平均代替积分运算，进而实现***状态的最小方差估计。

栅格地图信息用一个包含节点的二维数组储存，节点的信息包括：栅格值，数组A[N],A[N]＝1表示机器人在角度N时会与障碍物发生碰撞，-1表示不会。

然后对栅格地图进行预处理：将所有自由区域的A值置为1，致命障碍和内切障碍均置为-1。

遍历外切障碍与非自由空间的栅格，并且对其做出可达性判断：

当机器人角度为x时，分别判断机器人的各顶点是否落在致命障碍的栅格上，如果全部没有落在致命障碍的栅格上，则A[x]＝1,反之A[x]＝-1；这样就生成了N个与机器人角度相关的可达性地图Map[1]，Map[2]…Map[N]，机器人可以通过可达性地图找到可达路径。

机器人从A点到B点的局部路径规划处理：

用Map0表示不限制机器人角度的由障碍物和自由区域构成的地图；而限制机器人角度的地图为Map[n],其中n＝1,2，……，N；

基于机器人的起始点(A)与目标点(B)所处的位置信息，规划起始点到目标点之间的局部路径：

若A点处于自由区域，B点处于自由区域，且A点到B点能在Map0中找到一条可达路径，则用不限制角度变化的方式导航。如果不能，则遍历搜索Map[N]，遍历搜索每个可达性地图中是否存在从点A到点B的可达路径，再从搜索得到的所有可达路径中，查找最短路径的可达路径所对应的可达性地图，记为Map[min]，再讲该可达性地图对应的角度min作为机器人的导航角度，通过限制机器人角度的方式导航到B点。

若A点处于自由区域，B点处于非自由区域，那么搜索B点A[x]＝1的可达性地图子集，并遍历搜索该地图子集中的每个可达性地图中是否存在从点A到点B的可达路径，再从搜索得到的所有可达路径中，查找最短路径的可达路径所对应的可达性地图，记为Map[min]，再讲该可达性地图对应的角度min作为机器人的导航角度，通过限制机器人角度的方式导航到B点。

若A点处于非自由区域，且当前机器人角度为R，那么搜索地图Map[R],判断地图是否可达，若存在，则通过限制机器人角度为R的方式导航到B点；否则表示两点间的路径不可达。

相比于现有技术，本发明具有如下有益效果：

(1)基于机器人位姿的多重栅格值导航方法，扩大了机器人在栅格地图中的可通过区域，增强了拓扑地图的联通性；

(2)首次将BIM信息应用于机器人导航，解决了在建筑施工环境下，常规导航技术难以获取施工部件信息等问题。并且可以根据BIM信息计算出最优的工作路径，提高机器人工作效率。

(3)本发明的机器人采用分布式***，根据BIM信息能给机器人集群合理分配任务，机器人也能将自己的工作状态同步到总控终端，总控统一协调工作任务，避免冲突，提升效率。

(4)本发明利用超声波，RGB-D深度摄像头，探测环境中未知的危险，保证了人机安全。

(5)本发明能生成包含建筑信息语义标签的栅格地图，指导建筑施工，并且栅格地图的基础之上，生成语义拓扑地图，提高了路径规划的效率。而常规的激光雷达SLAM只能生成不包含语义标签的普通栅格地图。

以上所述，仅为本发明的具体实施方式，本说明书中所公开的任一特征，除非特别叙述，均可被其他等效或具有类似目的的替代特征加以替换；所公开的所有特征、或所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以任何方式组合。

Claims (3)

1.一种基于机器人位姿的多重栅格值导航方法，其特征在于，包括下列步骤：

在栅格地图中，为每一个栅格除了存储栅格值信息外，额外定义一个表示机器人在不同角度状态下的是否碰撞标识符的数组；

对栅格地图进行预处理：将栅格地图中的所有自由区域的是否碰撞标识符标记为非碰撞标识符，以及将栅格地图中的致命障碍和内切障碍的是否碰撞标识符均置为碰撞标识符；

基于机器人的不同位姿的角度x，分别判断机器人的每个顶点是否落在致命障碍的栅格上，若全部顶点均未落在致命障碍的栅格上，则设置角度x对应的是否碰撞标识符为非碰撞标识符；否则设置是否碰撞标识符为碰撞标识符；从而得到多个与机器人角度相关的可达性地图，记为可达性地图Map[n]，其中n为对应的角度区分符；

基于机器人的当前起始点A与目标点B，设置从点A到点B的局部路径：

用Map0表示不限制机器人角度的由障碍物和自由区域构成的地图；

若A点处于自由区域，B点处于自由区域，且A点到B点能在地图Map0中查找到一条可达路径，则采用非限制角度变化的方式导航；若在地图Map0中不能查找到可达路径，则遍历搜索每个可达性地图Map[n]中是否存在从点A到点B的可达路径，其中每个可达性地图Map[n]只对应一条可达路径；再从搜索得到的所有可达路径中，查找最短路径的可达路径所对应的可达性地图，并将其对应的角度作为机器人的导航角度，通过限制机器人角度的方式导航到B点；

若A点处于自由区域，B点处于非自由区域，则基于点B的位置，将所有可达性地图Map[n]中的是否碰撞标识符为非碰撞标识符的可达性地图作为第一可达性地图子集；并遍历第一可达性地图子集中的每个可达性地图中是否存在从点A到点B的可达路径；再从搜索得到的所有可达路径中，查找最短路径的可达路径所对应的可达性地图，并将其对应的角度作为机器人的导航角度，通过限制机器人角度的方式导航到B点；

若A点处于非自由区域，且当前机器人角度为R，那么搜索角度R对应的可达性地图Map[R]，并判断所述可达性地图Map[R]中是否存在可达路径，若存在，则通过限制机器人角度为R的方式导航到B点；否则认为点A到点B的路径不可达。

2.基于BIM信息的室内墙面施工自主移动机器人导航方法，其特征在于，包括下列步骤：

提取室内墙面施工自主移动机器人作业的室内环境的BIM信息，构建作业的室内地图，并基于所述室内地图计算出每个室内墙面施工作业的工作任务，得到每个工作任务的工作点的位置信息；并为所有工作任务设置工作任务顺序；

根据工作点的位置信息和工作任务顺序，根据权利要求1所述的基于机器人位姿的多重栅格值导航方法，设置相邻工作点之间的局部导航路线；

实时定位并检测室内墙面施工自主移动机器人周围的障碍物信息，更新栅格地图，并基于更新后的栅格地图重新根据权利要求1所述的基于机器人位姿的多重栅格值导航方法，设置相邻工作点之间的局部导航路线。

3.如权利要求2所述的方法，其特征在于，通过深度摄像头探测到的施工空洞以及地面未知障碍物信息，作为室内墙面施工自主移动机器人周围的障碍物信息。

Images