一种空间占用检测方法、系统和设备

技术领域

本发明涉及移动机器人技术领域，具体涉及一种空间占用检测方法、系统和设备。

背景技术

由于机器人能够替代人员进行重复性巡逻或危险区域安防巡逻作业，机器人在安防领域应用广泛。

机器人在执行安防任务时，需要检测待检区域内环境信息，并根据采集的数据进行报警，在执行安防任务时，目前的机器人无法完成通道检测功能，例如逃生通道、安全通道等一些需要保持畅通的区域，而如果这些安全通道无法检测，则会造成一定的安全隐患。因此需要机器人在执行安防巡检任务时，能够检测前方空间是否被占用，那么，需要研究设计一种机器人空间检测方法以检测前方空间是否被占用。

发明内容

鉴于上述问题，本发明提供一种空间占用检测方法、系统和设备。

本发明为解决技术问题所采用的技术方案如下：

第一方面，本发明提供一种空间占用检测方法，包括：

获得待检测点的位置，判断机器人和待检测点之间的位置关系；

在机器人的激光雷达能够扫描到待检测目标区域的情况下，获取待检测目标区域的激光点云数据，获取机器人最新定位数据，根据所述最新定位数据获取该定位数据定位时间之前的多帧激光点云数据；

将所述激光点云数据映射至所述最新定位数据所对应的机器人坐标系，融合所有映射后的激光点云数据；

根据所述最新定位数据、待检测点的位置和融合后的激光点云数据，计算待检测点在所述最新定位数据的机器人坐标系下的待检测框；

根据待检测框判断所述待检测框所对应空间是否被占用。

在一个优选的实施例中，根据所述最新定位数据获取该定位数据定位时间之前的多帧激光点云数据具体为：从机器人定位数据的环形队列中取出最新定位数据前若干帧历史定位数据；对取出的每一帧历史定位数据，均根据其定位时间从激光点云数据的环形队列中搜索出与其最邻近的激光点云数据并进行绑定；所述映射的方法为：计算所述历史定位数据和最新定位数据之间的欧氏距离和角度差，并据此构建外参矩阵，将每帧激光点云数据与其对应的外参矩阵相乘得到映射后的激光点云数据；所述融合所有映射后的激光点云数据的过程具体为：叠加所有映射后的激光点云数据获得多帧激光融合后的点云数据。

在一个优选的实施例中，计算最新定位数据和待检测点的位置的欧式距离和角度差，并据此计算待检测框上参考点位置，根据参考点位置生成机器人坐标系下立体的待检测框。

在一个优选的实施例中，所述根据待检测框判断所述待检测框所对应空间是否被占用的具体过程为：将待检测框划分为数个体素框，遍历所述融合后的激光点云数据，若有激光点落在某个体素框内，则将该体素框标记为被占用框，当被占用框的数量大于等于N个时，N为大于等于2的整数，则该待检测框所在空间被堵塞或占用；当被占用框小于N个时，以激光雷达安装位置为中心，向待检测框前表面的四个角做连线并延长，形成锥形空间，所述前表面为朝向激光雷达的面；对于锥形空间遍历融合后的激光点云数据，统计第一数值和第二数值，所述第一数值为位于锥形空间内、且待检测框和激光雷达安装位置之间的激光点数，所述第二数值为落在锥形空间、且位于待检测框前表面后侧的激光点数；若第一数值大于第二数值，则待检测框的前方被阻挡，无法正常观测，若第一数值不大于第二数值，则该待检测框所在空间未被堵塞或占用。

第二方面，本发明提供一种空间占用检测系统，包括：

第一获取单元，用于获得待检测点的位置；

第一判断单元，用于判断机器人和待检测点之间的位置关系；

第二获取单元，用于在机器人的激光雷达能够扫描到待检测目标区域的情况下，获取待检测目标区域的激光点云数据，获取机器人最新定位数据，根据所述最新定位数据获取该定位数据定位时间之前的多帧激光点云数据；

第一数据映射单元，用于将所述激光点云数据映射至所述最新定位数据所对应的机器人坐标系，融合所有映射后的激光点云数据；

第一计算单元，用于根据所述最新定位数据、待检测点的位置和融合后的激光点云数据，计算待检测点在所述最新定位数据的机器人坐标系下的待检测框；

第二判断单元，用于根据待检测框判断所述待检测框所对应空间是否被占用。

第三方面，本发明提供一种电子设备，包括：存储器；一个或多个处理器；一个或多个程序，其中所述一个或多个程序存储在所述存储器中并被配置为由所述一个或多个处理器执行，所述一个或多个程序包括用于执行根据所述一种空间占用检测方法中的任一方法的指令。

本发明一种空间占用检测方法、系统和设备，通过获取待检测点和机器人的定位，将多帧激光点云数据转换到当前帧，再多帧融合，寻找待检测框，来判断待检测框对应空间是否被占用。本发明实现了通道占用的检测，同时避免因为点云稀疏导致的无法检测问题。基于该方法、系统和设备，使得机器人在执行安防任务时能够完成通道检测功能，提高了机器人安防的安防检测准确性。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为实施例1提供的一种空间占用检测方法的流程图；

图2为实施例2提供的一种空间占用检测方法的流程图；

图3为实施例2中锥形空间的示意图；

图4为实施例3提供的一种空间占用检测系统的框架图；

图5实施例4提供的一种电子设备的框架图；

图6实施例5提供的机器人的示意图。

具体实施方式

为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。

需要说明的是，在本申请中涉及“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。另外，各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本申请要求的保护范围之内。

实施例一

本实施例提供了一种空间占用检测方法，用于解决亟需一种能够检测前方空间是否被占用的检测方法的问题。

一种空间占用检测方法，流程如图1所示，该方法包括：

获得待检测点的位置，判断机器人和待检测点之间的位置关系；

在机器人的激光雷达能够扫描到待检测目标区域的情况下，获取待检测目标区域的激光点云数据，获取机器人最新定位数据，根据所述最新定位数据获取该定位数据定位时间之前的多帧激光点云数据；

将多帧激光点云数据一一映射至所述最新定位数据所对应的机器人坐标系，得到多帧映射后的激光点云数据，融合所有映射后的激光点云数据得到融合后的激光点云数据；

根据所述最新定位数据、待检测点的位置和融合后的激光点云数据，计算待检测点在所述最新定位数据的机器人坐标系下的待检测框；

根据待检测框判断所述待检测框所对应空间是否被占用。

本实施例通过获取待检测点和机器人的定位，将多帧激光点云数据转换到当前帧，再多帧融合，寻找待检测框，来判断待检测框对应空间是否被占用。本发明实现了通道占用的检测，同时避免因为点云稀疏导致的无法检测问题。

实施例二

本实施例提供了一种空间占用检测方法，如图2，下面对该方法进行详述。

步骤一、接收任务，获得空间占用待检测点位置2d坐标(detection_point)，记录2d坐标。每间隔一定时间，例如每100ms，判断一次机器人实时位置(robot_point)和待检测点位置之间的位置关系，根据机器人和待检测点之间的这一位置关系能够判断机器人是否到达待检测点或是否能够观察到待检测点，也就是根据位置关系获知机器人能否通过激光雷达扫描到待检测目标区域。其中，待检测点位置detection_point包括待检测点在2d地图坐标系x轴的坐标信息detection_point.x和待检测点在2d地图坐标系y轴的坐标信息detection_point.y；机器人实时位置包括机器人在2d地图坐标系x轴的坐标信息robot_point.x和机器人在2d地图坐标系y轴的坐标信息robot_point.y。

机器人实时位置和待检测点之间位置关系包括机器人实时位置和待检测点这两点间的欧氏距离和两点间的角度差(即两点之间的角度)。

例如：当发现机器人2d定位位置(robot_point)和待检测点之间欧氏距离相距5m以内且夹角小于40°(机器人朝向方向与2d地图坐标系x轴夹角为机器人theta角，即robot_point.theta，由机器人位置指向待检测点位置的向量方向与2d地图坐标系x轴夹角为待检测点theta角，即待检测点角度)，则认为机器人的激光可以扫描到该区域，可以进行检测。作为一种优选实施例，在实际应用中，待检测点位置2d坐标、待检测点角度、robot_point以及robot_point.theta均可直接获得。

上述两点间欧式距离：

待检测点角度：

degree ＝ (rad /π* 180 + 360°) ％ 360° (2)

其中，(α+360°)％360°为了防止负数影响后续计算，若角度α为负，则(α+360°)％360°＝α+360°，若角度α不为负，则(α+360°)％360°＝α，α＝rad/π*180，例如(-1°+360°)％360°＝359°，例如(1°

+360°)％360°＝1°。

机器人实时位置和待检测点两点间角度差为：

distance_theta ＝ |degree - robot_point.theta| (3)

待检测点弧度：

rad＝arctan((detection_point.y-robot_point.y),(detection_point.x-

robot_point.x))(4)

因为机器人感知robot sense的激光点云数据过于稀疏，可能落在空间内的点不足以判断空间是否被占用，所以当发现满足进行空间占用检测条件后，开始融合多帧激光点云数据以解决稀疏问题，因此设计了下述步骤二和步骤三。

步骤二、在机器人开始能够看到待检测点后的1s内(激光和定位数据频率为10fps)，通过激光雷达扫描待检测目标区域累计足够多的激光点云数据；然后取最后一帧当前机器人位置(last_robot_point)，即最新定位数据，为真正检测位置开始进行检测，根据所述最新定位数据获取该定位数据的定位时间之前的多帧激光点云数据。其中，机器人的定位数据保持实时获取。

作为一种实施例，持续通过websocket监听来自激光雷达的3d激光点云数据，并记录对应的时间戳。

根据所述最新定位数据获取该定位数据的定位时间之前的多帧激光点云数据的过程具体为：

从机器人定位数据的环形队列(pos_ringbuffer)中取出当前机器人位置(最新定位数据)前若干帧(例如共5帧)的定位数据，在此称为历史定位数据(point)，机器人的定位数据为二维。对于每一帧历史定位数据，均以其时间戳(定位的cpu时间戳)从激光点云数据的环形队列(laser_ringbuffer)中搜索出与其最邻近的3d激光点云数据进行绑定，每帧激光点云数据都是以其绑定的机器人位置和激光雷达安装角度为其坐标。

步骤三、将步骤二最后得到的若干帧激光点云数据一一映射至所述最新定位数据所对应的机器人坐标系，得到多帧映射后的激光点云数据，融合所有映射后的激光点云数据得到融合后的激光点云数据。也就是根据机器人最新定位数据和三维激光雷达安装位置构建三维的机器人坐标系并进行激光点云数据的融合累计。

具体为：针对5帧历史定位数据(point)分别计算其和last_robot_point之间在2d地图坐标系下的欧氏距离和角度差。三维的机器人坐标系中，历史定位数据相对于last_robot_point的角度差即为yaw角(偏航角)上的旋转量(绕z轴旋转，z轴为偏航轴)，机器人的平移量x,y为欧氏距离在角度差上的分量，z(高度)为0，据此构建外参矩阵。也就是每帧历史定位数据有其对应的激光点云数据和外参矩阵，即激光点云数据具有对应的外参矩阵。将每帧激光点云数据与其对应的外参矩阵相乘，即可将每帧激光都投影到last_robot_point的机器人坐标系下，得到映射后的激光点云数据。再将所有映射后的激光点云数据叠加在一起，获得多帧激光融合后的点云数据laser_cloud_acc。

步骤四、根据last_robot_point、laser_cloud_acc和detection_point，计算待检测点在last_robot_point的机器人坐标系下的待检测框位置。

S4.1、计算last_robot_point和detection_point的欧式距离distance和角度差distance_theta，欧式距离和角度差的计算方法同步骤三中历史定位数据和last_robot_point欧氏距离和角度差的计算；

S4.2、根据S4.1得到的欧式距离和角度差，计算待检测框。

S4.2具体为：根据S4.1得到的欧式距离和角度差，计算待检测框上参考点位置(center_x,center_y)(机器人坐标系)，以(center_x,center_y)为待检测框一条底边的中心点，生成机器人坐标系下，底面边长为1m、高为2m的待检测框，待检测框为立体框体。本发明不限定(center_x,center_y)作为待检测框一条底边的中心点，例如可以是待检测框底面的中点，也可以是待检测框前表面的中点等。

步骤五、按10cm

作为一种实施例，小于N则直接判定待检测框所在空间未被堵塞或占用。

作为另一种实施例，当被占用框小于3个时，存在两种情况，一种情况是：待检测框前方被阻挡，导致无法观测到待检测区域内是否存在足量的体素框存在激光点，也就是无法确定待检测框所在空间是否未被堵塞或占用；另一种情况是：待检测框前方未被阻挡，可以正常观测待检测框，即待检测框所在空间未被阻挡，可以确定待检测框所在空间未被堵塞或占用。因此需要进一步的判断，作为一种实施例，判断待检测框前方是否被阻挡的方法为：以激光雷达安装位置为中心，向待检测框正面(朝向激光雷达的面，称为前表面)的四个角做连线并延长，产生无限远的锥形空间约束，如图3，锥形为四棱锥。此时对于锥形空间遍历laser_cloud_acc，统计第一数值，即位于锥形空间、且待检测框和激光雷达安装位置之间的激光点数；统计第二数值，即落在锥形空间、且位于待检测框前表面后侧的激光点数；若第一数值大于第二数值，则待检测框的前方被阻挡，无法正常观测，即待检测框被阻挡。

需要说明的是，实施例二中的部分实施例，也可对应适用于实施例一，故未在实施例一中展开说明。

实施例三

一种空间占用检测系统，包括：第一获取单元、第一判断单元、第二获取单元、第一数据映射单元、第一计算单元、第二判断单元，参见图4。

所述第一获取单元用于获得待检测点的位置。第一判断单元用于判断机器人和待检测点之间的位置关系。第二获取单元用于在机器人的激光雷达能够扫描到待检测目标区域的情况下，获取待检测目标区域的激光点云数据，获取机器人最新定位数据，根据所述最新定位数据获取该定位数据定位时间之前的多帧激光点云数据。第一数据映射单元用于将所述激光点云数据映射至所述最新定位数据所对应的机器人坐标系，以及融合所有映射后的激光点云数据。第一计算单元用于根据所述最新定位数据、待检测点的位置和融合后的激光点云数据，计算待检测点在所述最新定位数据的机器人坐标系下的待检测框。第二判断单元，用于根据待检测框判断所述待检测框所对应空间是否被占用。

所述一种空间占用检测系统在具体实施时，可以参照上述任一实施例中的一种空间占用检测方法实现空间占用检测，具体实现步骤不再赘述。

实施例四

根据本发明的方法可以实现一种电子设备，参见图5，电子设备包括：存储器；一个或多个处理器；一个或多个程序，其中一个或多个程序存储在所述存储器中并被配置为由所述一个或多个处理器执行，所述一个或多个程序包括用于执行根据上述任一实施例中一种空间占用检测方法的指令。

处理器可以是中央处理单元(CPU)，还可以是其他通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

存储器可以包括各种类型的存储单元，例如系统内存、只读存储器(ROM)，和永久存储装置。此外，存储器可以包括任意计算机可读存储媒介的组合，存储器可以为半导体存储芯片、磁盘、光盘。

存储器上存储有可执行代码，当可执行代码被处理器处理时，可以使处理器执行上文述及的方法中的部分或全部。

本发明所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

实施例五

本发明还提供了一种机器人，如图6，所述机器人具有定位器、激光雷达、存储器、处理器。存储器中存储有若干计算机程序，所述计算机程序包括一种空间占用检测方法中的指令。所述处理器连接存储器，用于执行存储器中存储的计算机程序。

需要说明的是，在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详细描述的部分，可以参见其它实施例的相关描述。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；在本发明的思路下，以上实施例或者不同实施例中的技术特征之间也可以进行组合，步骤可以以任意顺序实现，并存在如上所述的本发明的不同方面的许多其它变化，为了简明，它们没有在细节中提供；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包括这些改动和变型在内。