CN108007451A - 货物承载装置位姿的检测方法、装置、计算机设备和存储介质 - Google Patents

Abstract

本发明提供一种货物承载装置位姿的检测方法、装置计算机设备和存储介质，通过获取目标货物承载装置在世界坐标系中的目标坐标点云；获取移动机器人上里程计当前定位信息；获取障碍物地图；根据障碍物地图与当前定位信息检测预设范围内的障碍物，提取障碍物坐标点云；当障碍物坐标点云与目标坐标点云匹配成功时，判定匹配成功的障碍物为当前货物承载装置；对当前货物承载装置的坐标点云进行特征检测，根据特征检测得到当前货物承载装置的目标坐标；根据当前货物承载装置的目标坐标与里程计的当前定位信息计算当前货物承载装置与移动机器人相对位姿。通过里程计定位信息和障碍物地图、货物承载装置的标准坐标计算货物承载装置的位置，能够降低成本。

Description

货物承载装置位姿的检测方法、装置、计算机设备和存储介质

技术领域

本发明涉及智能安全驾驶技术，特别是涉及一种货物承载装置检测方法、装置、计算机设备和存储介质。

背景技术

移动机器人在现在工业中是是现代工业生产和装备工厂以及物流仓储中闭不可少的工具。移动机器人在货物搬运过程中因为货物的存放位置存在误差，或者货物承载装置位置存在误差容易造成安全隐患。为了解决上述安全隐患，现有技术具备货物承载装置位姿检测功能，能够在取货时根据货物承载装置检测结果选择是否取货入叉或是否调整取货入叉姿态。目前已有的货物承载装置检测技术方案，主要依赖二维/三维传感器以检测货物承载装置的轮廓。基于二维激光扫描仪的托盘检测方法激光扫描仪成本高昂。基于深度相机(ToF相机)的托盘检测方法，深度相机成本同样高昂。故现在的工业货物承载装置检测方法成本高昂。

发明内容

基于此，有必要针对上述工业托盘检测方法成本高昂问题，提供一种货物承载装置位姿的检测方法、装置、计算机设备和存储介质。

一种货物承载装置位姿的检测方法，所述方法包括：获取目标货物承载装置在世界坐标系中的目标坐标点云；获取移动机器人上里程计的当前定位信息；获取障碍物地图；根据所述障碍物地图与所述当前定位信息检测预设范围内的障碍物，提取障碍物坐标点云；当所述障碍物坐标点云与所述目标坐标点云匹配成功时，判定所述匹配成功的障碍物为当前货物承载装置；对所述当前货物承载装置的坐标点云进行特征检测，根据所述特征检测得到所述当前货物承载装置的目标坐标；根据所述当前货物承载装置的目标坐标与所述里程计的当前定位信息计算当前货物承载装置与所述移动机器人之间的相对位姿。

在其中一个实施例中，所述获取障碍物地图的步骤之前，还包括:获取不同位置所述距离传感器到标志物的距离集；将所述标志物投影为世界坐标系中的直线方程；根据所述距离集和直线方程计算得到不同位置时所述距离传感器与所述标志物之间的相对位置；获取与所述距离传感器所在位置对应的所述里程计与所述标志物之间的相对位置；根据所述距离传感器与所述标志物之间的相对位置与所述里程计与所述标志物之间的相对位置计算得到所述距离传感器与所述里程计的相对位置。

在其中一个实施例中，所述根据所述距离传感器与所述标志物之间的相对位置与所述里程计与所述标志物之间的相对位置计算得到所述距离传感器与所述里程计的相对位置的步骤之后，还包括:建立环境地图；获取距离传感器的测量值中的有效测量值；获取所述里程计的测量数据；根据所述里程计与所述距离传感器的相对位置与所述测量数据将所述有效测量值转换为世界坐标系的坐标，将所述坐标保存到环境地图中；将不同时刻的所述坐标保存在所述环境地图中，所述环境地图为障碍物地图。

在其中一个实施例中，所述根据所述障碍物地图与所述当前定位信息检测预设范围内的障碍物，提取障碍物坐标点云的步骤，包括：获取所述距离传感器中所述障碍物与所述传感器的距离；当所述距离在预设范围时，获取所述障碍物地图中所述障碍物坐标点云。

在其中一个实施例中，所述当所述障碍物坐标点云与所述目标坐标点云匹配成功时，判定所述匹配成功的障碍物为当前货物承载装置的步骤，包括：根据所述障碍物坐标点云与所述目标坐标点云进行匹配；当所述障碍物坐标点云通过平移和/或旋转之后能够与所述目标坐标点云重合时，所述匹配成功的障碍物判定为所述当前货物承载装置。

在其中一个实施例中，所述对所述当前货物承载装置的坐标点云进行特征检测，根据所述特征检测得到所述当前货物承载装置的坐标的步骤，包括：对所述当前货物承载装置的坐标点云进行特征检测得到特征坐标点云；对所述特征坐标点云进行直线提取或拟合得到直线集；根据所述直线集与所述目坐标点云进行匹配，得到对应的平移矩阵和旋转矩阵；根据所述平移矩阵、所述旋转矩阵计算得到所述世界坐标系中所述当前货物承载装置的目标坐标。

在其中一个实施例中，所述根据所述当前货物承载装置的目标坐标与所述里程计的当前定位信息计算当前货物承载装置与所述移动机器人之间的相对位姿的步骤之后，还包括：获取所述移动机器人第一行驶路线；将所述第一行驶路线的起点作为预设起点，将所述第一行驶路线的起点航向作为预设起点航向；根据所述第一行驶路线的终点和终点航向、所述当前货物承载装置与所述移动机器人之间的相对位姿，计算得到所述移动机器人的预设终点和预设终点航向；将所述预设起点和所述预设终点分别作为第二行驶路线的起点和终点，将所述预设起点航向和所述预设终点航向分别作为所述第二行驶路线的起点航向和终点航向，重新规划得到所述第二行驶路线。

一种货物承载装置位姿的检测装置，所述装置包括：

坐标信息获取模块，用于获取目标货物承载装置在世界坐标系中的目标坐标点云；

定位信息获取模块，用于获取移动机器人上里程计的当前定位信息；

障碍物地图获取模块，用于获取障碍物地图；

障碍物坐标点云提取模块，用于根据所述障碍物地图与所述当前定位信息检测预设范围内的障碍物，提取障碍物坐标点云；

判定模块，用于当所述障碍物坐标点云与所述目标坐标点云匹配成功时，判定所述匹配成功的障碍物为当前货物承载装置；

目标坐标获取模块，用于对所述当前货物承载装置的坐标点云进行特征检测，根据所述特征检测得到所述当前货物承载装置的目标坐标；

相对位姿计算模块，用于根据所述当前货物承载装置的目标坐标点云与所述里程计的当前定位信息计算当前货物承载装置与所述移动机器人之间的相对位姿。

一种计算机设备，所述计算机设备包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：获取目标货物承载装置在世界坐标系中的目标坐标点云；获取移动机器人上里程计的当前定位信息；获取障碍物地图；根据所述障碍物地图与所述当前定位信息检测预设范围内的障碍物，提取障碍物坐标点云；当所述障碍物坐标点云与所述目标坐标点云匹配成功时，判定所述匹配成功的障碍物为当前货物承载装置；对所述当前货物承载装置的坐标点云进行特征检测，根据所述特征检测得到所述当前货物承载装置的目标坐标；根据所述当前货物承载装置的目标坐标与所述里程计的当前定位信息计算当前货物承载装置与所述移动机器人之间的相对位姿。

在一个实施例中，所述获取障碍物地图的步骤之前，所述处理器执行所述计算机程序时还用于实现以下步骤:获取不同位置所述距离传感器到标志物的距离集；将所述标志物投影为世界坐标系中的直线方程；根据所述距离集和直线方程计算得到不同位置时所述距离传感器与所述标志物之间的相对位置；获取与所述距离传感器所在位置对应的所述里程计与所述标志物之间的相对位置；根据所述距离传感器与所述标志物之间的相对位置与所述里程计与所述标志物之间的相对位置计算得到所述距离传感器与所述里程计的相对位置。

在一个实施例中，所述根据所述距离传感器与所述标志物之间的相对位置与所述里程计与所述标志物之间的相对位置计算得到所述距离传感器与所述里程计的相对位置的步骤之后，所述处理器执行所述计算机程序时还用于实现以下步骤:建立环境地图；获取距离传感器的测量值中的有效测量值；获取所述里程计的测量数据；根据所述里程计与所述距离传感器的相对位置与所述测量数据将所述有效测量值转换为世界坐标系的坐标，将所述坐标保存到环境地图中；将不同时刻的所述坐标保存在所述环境地图中，所述环境地图为障碍物地图。

在一个实施例中，所述处理器所执行的所述根据所述障碍物地图与所述当前定位信息检测预设范围内的障碍物，提取障碍物坐标点云，包括：获取所述距离传感器中所述障碍物与所述传感器的距离；当所述距离在预设范围时，获取所述障碍物地图中所述障碍物坐标点云。

在一个实施例中，所述处理器所执行的所述当所述障碍物坐标点云与所述目标坐标点云匹配成功时，判定所述匹配成功的障碍物为当前货物承载装置，包括：根据所述障碍物坐标点云与所述目标坐标点云进行匹配；当所述障碍物坐标点云通过平移和/或旋转之后能够与所述目标坐标点云重合时，所述匹配成功的障碍物判定为所述当前货物承载装置。

在一个实施例中，所述处理器所执行的所述对所述当前货物承载装置的坐标点云进行特征检测，根据所述特征检测得到所述当前货物承载装置的坐标，包括：对所述所述当前货物承载装置的坐标点云进行特征检测得到特征坐标点云；对所述特征坐标点云进行直线提取或拟合得到直线集；根据所述直线集与所述目坐标点云进行匹配，得到对应的平移矩阵和旋转矩阵；根据所述平移矩阵、所述旋转矩阵计算得到所述世界坐标系中所述当前货物承载装置的目标坐标。

在一个实施例中，所述根据所述当前货物承载装置的目标坐标与所述里程计的当前定位信息计算当前货物承载装置与所述移动机器人之间的相对位姿的步骤之后，所述处理器执行所述计算机程序时还用于实现以下步骤：获取所述移动机器人第一行驶路线；将所述第一行驶路线的起点作为预设起点，将所述第一行驶路线的起点航向作为预设起点航向；根据所述第一行驶路线的终点和终点航向、所述当前货物承载装置与所述移动机器人之间的相对位姿，计算得到所述移动机器人的预设终点和预设终点航向；将所述预设起点和所述预设终点分别作为第二行驶路线的起点和终点，将所述预设起点航向和所述预设终点航向分别作为所述第二行驶路线的起点航向和终点航向，重新规划得到所述第二行驶路线。

一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时执行以下步骤：获取目标货物承载装置在世界坐标系中的目标坐标点云；获取移动机器人上里程计的当前定位信息；获取障碍物地图；根据所述障碍物地图与所述当前定位信息检测预设范围内的障碍物，提取障碍物坐标点云；当所述障碍物坐标点云与所述目标坐标点云匹配成功时，判定所述匹配成功的障碍物为当前货物承载装置；对所述当前货物承载装置的坐标点云进行特征检测，根据所述特征检测得到所述当前货物承载装置的目标坐标；根据所述当前货物承载装置的目标坐标与所述里程计的当前定位信息计算当前货物承载装置与所述移动机器人之间的相对位姿。

在一个实施例中，所述获取障碍物地图的步骤之前，所述计算机程序被处理器执行时还执行以下步骤:获取不同位置所述距离传感器到标志物的距离集；将所述标志物投影为世界坐标系中的直线方程；根据所述距离集和直线方程计算得到不同位置时所述距离传感器与所述标志物之间的相对位置；获取与所述距离传感器所在位置对应的所述里程计与所述标志物之间的相对位置；根据所述距离传感器与所述标志物之间的相对位置与所述里程计与所述标志物之间的相对位置计算得到所述距离传感器与所述里程计的相对位置。

在一个实施例中，所述根据所述距离传感器与所述标志物之间的相对位置与所述里程计与所述标志物之间的相对位置计算得到所述距离传感器与所述里程计的相对位置的步骤之后，所述计算机程序被处理器执行时还执行以下步骤：获取距离传感器的测量值中的有效测量值；获取所述里程计的测量数据；根据所述里程计与所述距离传感器的相对位置与所述测量数据将所述有效测量值转换为世界坐标系的坐标，将所述坐标保存到环境地图中；将不同时刻的所述坐标保存在所述环境地图中，所述环境地图为障碍物地图。

在一个实施例中，所述计算机程序被处理器执行的所述根据所述障碍物地图与所述当前定位信息检测预设范围内的障碍物，提取障碍物坐标点云，包括：获取所述距离传感器中所述障碍物与所述传感器的距离；当所述距离在预设范围时，获取所述障碍物地图中所述障碍物坐标点云。

在一个实施例中，所述计算机程序被处理器执行的所述当所述障碍物坐标点云与所述目标坐标点云匹配成功时，判定所述匹配成功的障碍物为当前货物承载装置的步骤，包括：根据所述障碍物坐标点云与所述目标坐标点云进行匹配；当所述障碍物坐标点云通过平移和/或旋转之后能够与所述目标坐标点云重合时，所述匹配成功的障碍物判定为所述当前货物承载装置。

在一个实施例中，所述计算机程序被处理器执行的所述对所述当前货物承载装置的坐标点云进行特征检测，根据所述特征检测得到所述当前货物承载装置的坐标，包括：对所述所述当前货物承载装置的坐标点云进行特征检测得到特征坐标点云；对所述特征坐标点云进行直线提取或拟合得到直线集；根据所述直线集与所述目坐标点云进行匹配，得到对应的平移矩阵和旋转矩阵；根据所述平移矩阵、所述旋转矩阵计算得到所述世界坐标系中所述当前货物承载装置的目标坐标。

在一个实施例中，所述根据所述当前货物承载装置的目标坐标与所述里程计的当前定位信息计算当前货物承载装置与所述移动机器人之间的相对位姿的步骤之后，所述计算机程序被处理器执行时还执行以下步骤：获取所述移动机器人第一行驶路线；将所述第一行驶路线的起点作为预设起点，将所述第一行驶路线的起点航向作为预设起点航向；根据所述第一行驶路线的终点和终点航向、所述当前货物承载装置与所述移动机器人之间的相对位姿，计算得到所述移动机器人的预设终点和预设终点航向；将所述预设起点和所述预设终点分别作为第二行驶路线的起点和终点，将所述预设起点航向和所述预设终点航向分别作为所述第二行驶路线的起点航向和终点航向，重新规划得到所述第二行驶路线。

上述货物承载装置位姿的检测方法、装置计算机设备和存储介质，通过获取目标货物承载装置在世界坐标系中的目标坐标点云；获取移动机器人上里程计的当前定位信息；获取障碍物地图；根据所述障碍物地图与所述当前定位信息检测预设范围内的障碍物，提取障碍物坐标点云；当所述障碍物坐标点云与所述目标坐标点云匹配成功时，判定所述匹配成功的障碍物为当前货物承载装置；对所述当前货物承载装置的坐标点云进行特征检测，根据所述特征检测得到所述当前货物承载装置的目标坐标；根据所述当前货物承载装置的目标坐标与所述里程计的当前定位信息计算当前货物承载装置与所述移动机器人之间的相对位姿。上述方法通过低成本里程计获得机器人的定位，根据定位与地图进行一定范围内的障碍物搜索，得到与货物承载装置的相似的坐标点云，对坐标点云进行特征检测得到货物承载装置的坐标，根据货物承载装置的坐标与里程计的定位信息计算得到货物承载装置与移动机器人的相对位姿降低成本。

附图说明

图1为一个实施例中货物承载装置位姿的检测的应用场景图；

图2为一个实施例中货物承载装置位姿的检测的流程图；

图3为一个实施例中获取里程计与距离传感器相对位姿的流程图；

图4为一个实施例中建立障碍物地图的流程图；

图5为一个实施例中获取障碍物坐标点云的流程图；

图6为一个实施例中当前货物承载装置判定的流程图；

图7为一个实施例中获取当前货物承载装置坐标的流程图；

图8为一个实施例中移动机器人行驶路线规划的流程图；

图9为一个实施例中货物承载装置的检测装置的框架图；

图10为一个实施例中获取里程计与距离传感器相对位姿的框架图；

图11为一个实施例中建立障碍物地图的框架图；

图12为一个实施例中获取障碍物坐标点云的框架图；

图13为一个实施例中当前货物承载装置判定的框架图；

图14为一个实施例中获取当前货物承载装置坐标的框架图；

图15为一个实施例中移动机器人行驶路线规划的框架图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步的详细说明。应当理解，此处所描述的具体实施方式仅仅用以解释本发明，并不限定本发明的保护范围。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。

如图1所示，在一个实施例中，提供应用场景图，货物承载装置10，移动机器人20，托臂30，距离传感器40，里程计50。货物承载装置10用来承载货物，包括托盘，装货物的篮子等可以用来装载货物的装置，移动机器人20用来搬运货物，托臂30用于承载货物承载装置10，且安装在所述移动机器20人上，距离传感器40的成本低，用一个或者多个距离传感器40测量距离传感器与障碍物或者标志物或承载物之间的距离，里程计50的成本低，可以选择一个或者多个里程计530用于计算移动机器人的相对位置。通过距离传感器40测量到的距离数据与里程计测量得到的相对位置数据计算得到货物承载装置与移动机器人之间的相位位姿。

如图2所示，在一个实施例中，提供了一种货物承载装置位姿的检测方法，上述方法包括：

步骤S110，获取目标货物承载装置在世界坐标系中的目标坐标点云。

其中，目标货物承载装置是用来装载货物的装置，包括托盘、篮子等。世界坐标系是是系统的绝对坐标系，在环境中选择一个基准坐标系来描述移动机器人所在的位置。点云数据可以包含几何位置和颜色信息。在这里采用点云数据的几何位置信息。具体地，将目标货物承载装置的经过扫描或者其他的能够获得目标货物承载装置的几何位置信息的方式得到在世界坐标系的目标坐标点云。

步骤S120，获取移动机器人上里程计的当前定位信息。

其中，里程计是一种利用从移动传感器获得的数据来估计物体位置随时间的变化而改变的方法。该方法被用在许多种机器人系统(轮式或者腿式)上面，来估计，而不是确定这些机器人相对于初始位置移动的距离。这种方法对由速度对时间积分来求得位置的估计时所产生的误差十分敏感。快速、精确的数据采集，设备标定以及处理过程对于高效的使用该方法是十分必要的。具体地，根据移动传感器获得数据计算得到里程计的测量数据，从里程计的测量数据得到里程计所在的当前位置。

步骤S130，获取障碍物地图。

其中，障碍物地图是在统一的水平面内的二维环境地图。障碍物地图是点云的集合。点云中的每个数据点，都是距离传感器在某一时刻检测到的点。具体地，根据距离传感器测量得到的障碍物与距离传感器之间的距离，将上述距离通过传感器的外参转换为障碍物与移动机器人的相对位置，将该相对位置保存到障碍物地图上。其中，距离传感器测量距离的传感器，包括但不限于激光距离传感器,红外距离传感器，超声波距离传感器。距离传感器的工作原理是通过发射特别短的脉冲信号(激光、红外光、超声波等)，并测量此脉冲从发射到被物体反射回来的时间，通过测时间间隔来计算与物体之间的距离。

步骤S140，根据障碍物地图与当前定位信息检测预设范围内的障碍物，提取障碍物坐标点云。

其中，预设范围根据障碍物与距离传感器之间的距离确定，预设范围可以是移动机器人的行驶方向一定角度内的障碍物与距离传感器之间的距离。具体地，根据里程计的定位信息得到移动机器人当前定位，根据移动机器人的中心坐标为起点，将预设范围内的障碍物的坐标提取出来的到障碍物坐标点云。

步骤S150，当障碍物坐标点云与目标坐标点云匹配成功时，判定匹配成功的障碍物为当前货物承载装置。

具体地，将步骤S140中的障碍物坐标点云与步骤S110中的目标坐标点云进行匹配，根据障碍物坐标点云的目标点云坐标的重合度达到一定范围时表示匹配成功。将匹配成功的障碍物判定为当前货物承载装置。也就是认为检测到了货物承载装置。

步骤S160，对当前货物承载装置的坐标点云进行特征检测，根据特征检测得到当前货物承载装置的目标坐标。

其中，特征是一个客体或一组客体特性的抽象结果。特征是用来描述概念的。任一客体或一组客体都具有众多特性，人们根据客体所共有的特性抽象出某一概念，该概念便成为了特征。具体地，将当前货物承载装置的坐标点云按照预定的方式进行处理得到用于描述货物承载装置的特征，根据得到的特征计算得到当前货物承载装置的目标坐标。其中当前货物承载装置的目标坐标为世界坐标系中的坐标。

步骤S170，根据当前货物承载装置的目标坐标与里程计的当前定位信息计算当前货物承载装置与移动机器人之间的相对位姿。

具体地，已知两个坐标点，求解两个坐标点的相对位姿，在本实施例中，即为根据步骤S160中得到的当前货物承载装置的目标坐标和根据里程计的定位信息确定的移动机器人的当前定位信息计算当前货物承载装置与移动机器人的相对位姿。

上述方法通过低成本里程计获得机器人的定位，根据定位与地图进行一定范围内的障碍物搜索，得到与货物承载装置的相似的坐标点云，对坐标点云进行特征检测得到货物承载装置的坐标，根据货物承载装置的坐标与里程计的定位信息计算得到货物承载装置与移动机器人的相对位姿降低成本。

如图3所示，在一个实施例中，在步骤S110之前，还包括距离传感器与里程计之间的几何位置标定，具体步骤如下：

步骤S210，获取不同位置距离传感器到标志物的距离集。

其中，距离传感器是又叫做位移传感器，是传感器的一种，用于感应其与某物体间的距离。标志物可以是环境中的一堵墙或者是为了进行标定设定的物体，标志物只要能够被距离传感器检测到即可，能够得到标志物与传感器之间的相对位置即可。距离集为距离传感器与标志物的多个位置的距离组成的集合。具体地，根据距离传感器在不同位置测量到的距离传感器与标志物在之间的距离组成距离集。

步骤S230，将标志物投影为世界坐标系中的直线方程。

具体地，在世界坐标系中可以将标志物当做一条直线，根据标志物在世界坐标系的坐标计算得到标志物投影的直线方程。

在本实施例中，将环境中的一面墙当做标志物，将墙与地平面的交线作为世界坐标线的直线。

步骤S250，根据距离集和直线方程计算得到不同位置时距离传感器与标志物之间的相对位置。

其中，已知直线方程，以及两点到直线的距离，计算点与直线的相对位置。在本实施中，已知距离传感器到标志物的距离和标志物的直线方程，求解距离传感器与标志物的相对位置。

步骤S270，获取与距离传感器所在位置对应的里程计与标志物之间的相对位置。

具体地，获取与距离传感器同一个时刻时里程计的定位信息，根据里程计的定位信息得到在世界坐标系中的坐标，根据在世界坐标系中的里程计坐标与世界坐标系中标志物的直线方程计算得到里程计与标志物之间的相对位置。

步骤S290，根据距离传感器与标志物之间的相对位置与里程计与标志物之间的相对位置计算得到距离传感器与里程计的相对位置。

具体地，根据步骤S250得到距离传感器与标志物之间的相对位置和步骤S270得到里程计与标志物之间的相对位置，根据上述两个相对位置计算里程计与距离传感器之间的相对位置。

在本实施例中，利用已知环境信息对距离传感器和里程计系统的几何关系进行精确标定(计算里程计与距离传感器之间的相对位置)，可以提高二维环境地图的精度。由于距离传感器仅为一维信号，数据量少，故稳定性高，计算简便。

如图4所示，在一个实施中，在步骤S290之后，还包括建立障碍物地图，具体的步骤如下：

步骤S310，建立环境地图

具体地，建立一张空白的环境地图。随着移动机器人的移动，将距离传感器采集到的点逐个转换成环境地图中的坐标点完善环境地图。

步骤S330，获取距离传感器的测量值中的有效测量值。

具体地，距离传感器获取到的测量值中会存在数据的重复和噪声，故对测量值进行去重和过滤得到有效测量值。例，通过对测量值进行滤波处理得到有效测量值。

步骤S350，获取里程计的测量数据。

具体地，里程计与上述里程计是同一个里程计，获取该里程计的测量数据。

步骤S370，根据里程计与距离传感器的相对位置与测量数据将有效测量值转换为世界坐标系的坐标，将坐标保存到环境地图中。

具体地，里程计的测量数据能够得到移动机器人进行定位得到移动机机器人的定位信息，根据步骤S290中计算得到里程计与距离传感器的相对位置，距离传感的有效测量值是距离传感器与障碍物的距离集，根据距离传感器的外参将距离集转换为移动机器人中心与障碍物的相对位置，根据里程计得到的移动机器人定位信息将上述位置转换为世界坐标系中的坐标，将该坐标保存到上述环境地图中，更新环境地图。

在本实施例中，距离传感器在某一时刻检测到一个障碍物点，返回距离d。根据传感器外参(即距离传感器与里程计的相对位置)，可以将数值d变换为这个障碍物点相对于该时刻移动机器人中心的相对位置。根据移动机器人的里程计定位和障碍物在移动机器人坐标系的坐标计算得到障碍物在世界坐标系的坐标P。距离传感器在不同时刻检测到多个障碍物点，可以得到一系列的障碍物坐标P1，P2，...，Pn。将上述障碍物坐标都添加到环境地图中得到障碍物地图。障碍物地图能帮助机器人更好的识别障碍物。

步骤S390，将不同时刻的坐标保存在环境地图中，环境地图为障碍物地图。

具体地，随着移动机器人的移动，不断的获得里程计数据和距离传感器的测量值，在不同时刻重复上述步骤的计算过程获得新的障碍物坐标，将得到的所有的障碍物坐标都保存到环境地图中，形成障碍物地图。障碍物地图中包含了障碍物的坐标点云。

如图5所示，在一个实施例中，步骤S140包括：

步骤S142，获取距离传感器中障碍物与传感器的距离。

步骤S144，当距离在预设范围时，获取障碍物地图中障碍物坐标点云。

具体地，在移动机机器人移动过程中，距离传感器能够感应到一定范围内的障碍物，返回障碍物与距离传感器的距离。获取障碍物与距离传感器在预设范围内的障碍物地图中的障碍物坐标点云。距离传感器感应到的障碍物存在多个，在判定是否获取障碍地图中的障碍物坐标点云时，需要选取部分障碍物以便更好的判定障碍物是否为货物承载装置。

如图6所示，在一个实施例中，步骤S150包括：

步骤S152,根据障碍物坐标点云与目标坐标点云进行匹配。

步骤S154，当障碍物坐标点云通过平移和/或旋转之后能够与目标坐标点云重合时，匹配成功的障碍物判定为当前货物承载装置。

具体地，将步骤S144中得到的障碍物坐标点云与步骤S110中得到的货物承载装置的坐标点云进行匹配，以货物承载装置的坐标点云作为模板，查找通过旋转和/或平移后能够上述模板尽量重合的障碍物坐标点云，将能够重合的坐标点云对应的障碍物判定为当前货物承载装置。

如图7所示，在一个实施例中，步骤S160包括：

步骤S162，对当前货物承载装置的坐标点云进行特征检测得到特征坐标点云。

其中，特征坐标点云是能够描述物体的特征的坐标点云。具体地，对步骤S154中判定得到的当前货物承载装置的坐标点云进行特征提取，得到能够描述当前货物承载装置的坐标点云作为特征坐标点云。

步骤S164，对特征坐标点云进行直线提取或拟合得到直线集。

具体地，根据步骤S162中得到的特征坐标点云进行拟合或提取得到直线集。所述直线集能够描述当前货物承载装置。

步骤S166，根据直线集与目坐标点云进行匹配，得到对应的平移矩阵和旋转矩阵。

具体地，将步骤S164中得到的直线集再次与货物承载装置的目标坐标点云进行匹配得到一个更为准确的平移矩阵和旋转矩阵。

步骤S168，根据平移矩阵、旋转矩阵计算得到世界坐标系中当前货物承载装置的目标坐标。

具体地，根据步骤S166中得到的平移矩阵和旋转计算货物承载装置在世界坐标系中的与移动机器人的相对位置和角度，得到移动机器人获取货物承载装置前的相对位姿。根据坐标变换将上述相对位置转换为移动机器人取货时的移动机器人坐标系下的位置，也就是移动机器人获取货物承载装置后货物承载装置相对于移动机机器人的托臂的位姿。

如图8所示，在一个实施例中，步骤S170之后，包括：

步骤S410，获取移动机器人第一行驶路线。

其中，第一行驶路线是按照货物承载装置放在预定位置时，移动机器人用于获取货物承载装置规划的路线。具体地，获取根据移动机器人与货物承载装置的初始相对位置规划好的行驶路线。

步骤S430，将第一行驶路线的起点作为预设起点，将第一行驶路线的起点航向作为预设起点航向。

其中，预设起点和预设起点航向表示检测到货物承载装置时，需要根据货物承载装置与移动机器人的相对位姿调整移动机器人的行驶路线时的起点和起点航向。具体地，获取移动机器人的第一行驶路线的起点和起点航向分别作为预设起点和预设起点航向。

步骤S450，根据第一行驶路线的终点和终点航向、当前货物承载装置与移动机器人之间的相对位姿，计算得到移动机器人的预设终点和预设终点航向。

具体地，获取移动机器人的第一行驶路线的终点和终点航向。在步骤S170中得到了当前货物承载装置与移动机器人之间的相对位姿。因为第一行驶路线中终点时货物承载装置在世界坐标系的位姿与第二行驶路线终点时货物承载装置在世界坐标系的位姿是一致的，根据移动机器人的第一行驶路线的终点和终点航向、当前货物承载装置与移动机器人之间的相对位姿计算得到移动机器人的预设终点和预设终点航向。

步骤S470，将预设起点和预设终点分别作为第二行驶路线的起点和终点，将预设起点航向和预设终点航向分别作为第二行驶路线的起点航向和终点航向，重新规划得到所述第二行驶路线。

具体地，根据步骤S450计算得到预设终点、终点航向、第一行驶路线的起点和起点航向规划移动机器人的行驶路线。在实际生活中，货物承载装置可能因为人为误差或者随着时间的机器的累计误差等客观因素存在导致移动机器人与货物承载装置的相对位姿发生变化，而原来的行驶路线则难以保证获取货物承载装置时位姿是准确的。故重新规划路线能够减少误差，减少货物运行的安全隐患。

如图9所示，在一个实施中，还提供一种货物承载装置位姿的检测装置100，上述装置包括：

坐标信息获取模块110，用于获取目标货物承载装置在世界坐标系中的目标坐标点云。

定位信息获取模块120，用于获取移动机器人上里程计的当前定位信息。

障碍物地图获取模块130，用于获取障碍物地图。

障碍物坐标点云提取模块140，用于根据所述障碍物地图与所述当前定位信息检测预设范围内的障碍物，提取障碍物坐标点云。

判定模块150，用于当所述障碍物坐标点云与所述目标坐标点云匹配成功时，判定所述匹配成功的障碍物为当前货物承载装置。

目标坐标获取模块160，用于对所述当前货物承载装置的坐标点云进行特征检测，根据所述特征检测得到所述当前货物承载装置的目标坐标。

相对位姿计算模块170，用于根据所述当前货物承载装置的目标坐标点云与所述里程计的当前定位信息计算当前货物承载装置与所述移动机器人之间的相对位姿。

如图10所示，在一个实施例中，货物承载装置位姿的检测装置100，还包括：

距离获取模块210，用于获取不同位置所述距离传感器到标志物的距离集。

直线方程获取模块230，用于将所述标志物投影为世界坐标系中的直线方程。

相对位置计算模块250，获取与所述距离传感器所在位置对应的所述里程计与所述标志物之间的相对位置，根据所述距离传感器与所述标志物之间的相对位置与所述里程计与所述标志物之间的相对位置计算得到所述距离传感器与所述里程计的相对位置。

如图11所示，在一个实施例中，货物承载装置位姿的检测装置100，还包括：

环境地图建立模块310，用于建立环境地图。

数据获取模块330，用于获取距离传感器的测量值中的有效测量值和所述里程计的测量数据。

障碍物地图建立模块350，用于据所述里程计与所述距离传感器的相对位置与所述测量数据将所述有效测量值转换为世界坐标系的坐标，将所述坐标保存到环境地图中，将不同时刻的所述坐标保存在所述环境地图中，所述环境地图为障碍物地图。

如图12所示，在一个实施例中，模块140，包括：

距离获取单元142，用于获取所述距离传感器中所述障碍物与所述传感器的距离。

障碍物坐标点云获取单元144，用于当所述距离在预设范围时，获取所述障碍物地图中所述障碍物坐标点云。

如图13所示，在一个实施例中，模块150，包括：

匹配单元152，用于根据所述障碍物坐标点云与所述目标坐标点云进行匹配。

判定单元154，用于当所述障碍物坐标点云通过平移和/或旋转之后能够与所述目标坐标点云重合时，所述匹配成功的障碍物判定为所述当前货物承载装置。

如图14所示，在一个实施例中，模块160还用于：

特征检测单元162，用于对所述当前货物承载装置的坐标点云进行特征检测得到特征坐标点云。

直线获取单元164，用于对所述特征坐标点云进行直线提取或拟合得到直线集。

当前货物承载装置的目标坐标获取单元166，根据所述直线集与所述目坐标点云进行匹配，得到对应的平移矩阵和旋转矩阵，根据所述平移矩阵、所述旋转矩阵计算得到所述世界坐标系中所述当前货物承载装置的目标坐标。

如图15所示，在一个实施例中，货物承载装置位姿的检测装置100，还包括：

行驶路线获取模块410，用于获取所述移动机器人第一行驶路线。

端点和航向获取模块430，用于将所述第一行驶路线的起点作为预设起点，将所述第一行驶路线的起点航向作为预设起点航向，根据所述第一行驶路线的终点和终点航向、所述当前货物承载装置与所述移动机器人之间的相对位姿，计算得到所述移动机器人的预设终点和预设终点航向。

路线规划模块450，用于将所述预设起点和所述预设终点分别作为第二行驶路线的起点和终点，将所述预设起点航向和所述预设终点航向分别作为所述第二行驶路线的起点航向和终点航向，重新规划得到所述第二行驶路线。

一种计算机设备，所述计算机设备包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：获取目标货物承载装置在世界坐标系中的目标坐标点云；获取移动机器人上里程计的当前定位信息；获取障碍物地图；根据所述障碍物地图与所述当前定位信息检测预设范围内的障碍物，提取障碍物坐标点云；当所述障碍物坐标点云与所述目标坐标点云匹配成功时，判定所述匹配成功的障碍物为当前货物承载装置；对所述当前货物承载装置的坐标点云进行特征检测，根据所述特征检测得到所述当前货物承载装置的目标坐标；根据所述当前货物承载装置的目标坐标与所述里程计的当前定位信息计算当前货物承载装置与所述移动机器人之间的相对位姿。

在一个实施例中，所述获取障碍物地图的步骤之前，所述处理器执行所述计算机程序时还用于实现以下步骤:获取不同位置所述距离传感器到标志物的距离集；将所述标志物投影为世界坐标系中的直线方程；根据所述距离集和直线方程计算得到不同位置时所述距离传感器与所述标志物之间的相对位置；获取与所述距离穷癌晚期传感器所在位置对应的所述里程计与所述标志物之间的相对位置；根据所述距离传感器与所述标志物之间的相对位置与所述里程计与所述标志物之间的相对位置计算得到所述距离传感器与所述里程计的相对位置。

在一个实施例中，所述根据所述距离传感器与所述标志物之间的相对位置与所述里程计与所述标志物之间的相对位置计算得到所述距离传感器与所述里程计的相对位置的步骤之后，所述处理器执行所述计算机程序时还用于实现以下步骤:建立环境地图；获取距离传感器的测量值中的有效测量值；获取所述里程计的测量数据；根据所述里程计与所述距离传感器的相对位置与所述测量数据将所述有效测量值转换为世界坐标系的坐标，将所述坐标保存到环境地图中；将不同时刻的所述坐标保存在所述环境地图中，所述环境地图为障碍物地图。

在一个实施例中，所述处理器所执行的所述根据所述障碍物地图与所述当前定位信息检测预设范围内的障碍物，提取障碍物坐标点云，包括：获取所述距离传感器中所述障碍物与所述传感器的距离；当所述距离在预设范围时，获取所述障碍物地图中所述障碍物坐标点云。

在一个实施例中，所述处理器所执行的所述当所述障碍物坐标点云与所述目标坐标点云匹配成功时，判定所述匹配成功的障碍物为当前货物承载装置，包括：根据所述障碍物坐标点云与所述目标坐标点云进行匹配；当所述障碍物坐标点云通过平移和/或旋转之后能够与所述目标坐标点云重合时，所述匹配成功的障碍物判定为所述当前货物承载装置。

在一个实施例中，所述处理器所执行的所述对所述当前货物承载装置的坐标点云进行特征检测，根据所述特征检测得到所述当前货物承载装置的坐标，包括：对所述所述当前货物承载装置的坐标点云进行特征检测得到特征坐标点云；对所述特征坐标点云进行直线提取或拟合得到直线集；根据所述直线集与所述目坐标点云进行匹配，得到对应的平移矩阵和旋转矩阵；根据所述平移矩阵、所述旋转矩阵计算得到所述世界坐标系中所述当前货物承载装置的目标坐标。

在一个实施例中，所述根据所述当前货物承载装置的目标坐标与所述里程计的当前定位信息计算当前货物承载装置与所述移动机器人之间的相对位姿的步骤之后，所述处理器执行所述计算机程序时还用于实现以下步骤：获取所述移动机器人第一行驶路线；将所述第一行驶路线的起点作为预设起点，将所述第一行驶路线的起点航向作为预设起点航向；根据所述第一行驶路线的终点和终点航向、所述当前货物承载装置与所述移动机器人之间的相对位姿，计算得到所述移动机器人的预设终点和预设终点航向；将所述预设起点和所述预设终点分别作为第二行驶路线的起点和终点，将所述预设起点航向和所述预设终点航向分别作为所述第二行驶路线的起点航向和终点航向，重新规划得到所述第二行驶路线。

一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时执行以下步骤：获取目标货物承载装置在世界坐标系中的目标坐标点云；获取移动机器人上里程计的当前定位信息；获取障碍物地图；根据所述障碍物地图与所述当前定位信息检测预设范围内的障碍物，提取障碍物坐标点云；当所述障碍物坐标点云与所述目标坐标点云匹配成功时，判定所述匹配成功的障碍物为当前货物承载装置；对所述当前货物承载装置的坐标点云进行特征检测，根据所述特征检测得到所述当前货物承载装置的目标坐标；根据所述当前货物承载装置的目标坐标与所述里程计的当前定位信息计算当前货物承载装置与所述移动机器人之间的相对位姿。

在一个实施例中，所述获取障碍物地图的步骤之前，所述计算机程序被处理器执行时还执行以下步骤:获取不同位置所述距离传感器到标志物的距离集；将所述标志物投影为世界坐标系中的直线方程；根据所述距离集和直线方程计算得到不同位置时所述距离传感器与所述标志物之间的相对位置；获取与所述距离传感器所在位置对应的所述里程计与所述标志物之间的相对位置；根据所述距离传感器与所述标志物之间的相对位置与所述里程计与所述标志物之间的相对位置计算得到所述距离传感器与所述里程计的相对位置。

在一个实施例中，所述根据所述距离传感器与所述标志物之间的相对位置与所述里程计与所述标志物之间的相对位置计算得到所述距离传感器与所述里程计的相对位置的步骤之后，所述计算机程序被处理器执行时还执行以下步骤：获取距离传感器的测量值中的有效测量值；获取所述里程计的测量数据；根据所述里程计与所述距离传感器的相对位置与所述测量数据将所述有效测量值转换为世界坐标系的坐标，将所述坐标保存到环境地图中；将不同时刻的所述坐标保存在所述环境地图中，所述环境地图为障碍物地图。

在一个实施例中，所述计算机程序被处理器执行的所述根据所述障碍物地图与所述当前定位信息检测预设范围内的障碍物，提取障碍物坐标点云，包括：获取所述距离传感器中所述障碍物与所述传感器的距离；当所述距离在预设范围时，获取所述障碍物地图中所述障碍物坐标点云。

在一个实施例中，所述计算机程序被处理器执行的所述当所述障碍物坐标点云与所述目标坐标点云匹配成功时，判定所述匹配成功的障碍物为当前货物承载装置的步骤，包括：根据所述障碍物坐标点云与所述目标坐标点云进行匹配；当所述障碍物坐标点云通过平移和/或旋转之后能够与所述目标坐标点云重合时，所述匹配成功的障碍物判定为所述当前货物承载装置。

在一个实施例中，所述计算机程序被处理器执行的所述对所述当前货物承载装置的坐标点云进行特征检测，根据所述特征检测得到所述当前货物承载装置的坐标，包括：对所述所述当前货物承载装置的坐标点云进行特征检测得到特征坐标点云；对所述特征坐标点云进行直线提取或拟合得到直线集；根据所述直线集与所述目坐标点云进行匹配，得到对应的平移矩阵和旋转矩阵；根据所述平移矩阵、所述旋转矩阵计算得到所述世界坐标系中所述当前货物承载装置的目标坐标。

在一个实施例中，所述根据所述当前货物承载装置的目标坐标与所述里程计的当前定位信息计算当前货物承载装置与所述移动机器人之间的相对位姿的步骤之后，所述计算机程序被处理器执行时还执行以下步骤：获取所述移动机器人第一行驶路线；将所述第一行驶路线的起点作为预设起点，将所述第一行驶路线的起点航向作为预设起点航向；根据所述第一行驶路线的终点和终点航向、所述当前货物承载装置与所述移动机器人之间的相对位姿，计算得到所述移动机器人的预设终点和预设终点航向；将所述预设起点和所述预设终点分别作为第二行驶路线的起点和终点，将所述预设起点航向和所述预设终点航向分别作为所述第二行驶路线的起点航向和终点航向，重新规划得到所述第二行驶路线。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一非易失性的计算机可读取存储介质中，如本发明实施例中，该程序可存储于计算机系统的存储介质中，并被该计算机系统中的至少一个处理器执行，以实现包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)或随机存储记忆体(Random Access Memory，RAM)等。

据此，在一个实施例中还提供一种存储介质，其上存储有计算机程序，其中，该程序被处理器执行时实现如上述各实施例中的任意一个实施例的方法。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种货物承载装置位姿的检测方法，所述方法包括：

获取目标货物承载装置在世界坐标系中的目标坐标点云；

获取移动机器人上里程计的当前定位信息；

获取障碍物地图；

根据所述障碍物地图与所述当前定位信息检测预设范围内的障碍物，提取障碍物坐标点云；

当所述障碍物坐标点云与所述目标坐标点云匹配成功时，判定所述匹配成功的障碍物为当前货物承载装置；

对所述当前货物承载装置的坐标点云进行特征检测，根据所述特征检测得到所述当前货物承载装置的目标坐标；

根据所述当前货物承载装置的目标坐标与所述里程计的当前定位信息计算当前货物承载装置与所述移动机器人之间的相对位姿。

2.根据权利要求1所述的方法，其特征在于，所述获取障碍物地图的步骤之前，还包括：

获取不同位置所述距离传感器到标志物的距离集；

将所述标志物投影为世界坐标系中的直线方程；

根据所述距离集和直线方程计算得到不同位置时所述距离传感器与所述标志物之间的相对位置；

获取与所述距离传感器所在位置对应的所述里程计与所述标志物之间的相对位置；

根据所述距离传感器与所述标志物之间的相对位置与所述里程计与所述标志物之间的相对位置计算得到所述距离传感器与所述里程计的相对位置。

3.根据权利要求2所述的方法，其特征在于，所述根据所述距离传感器与所述标志物之间的相对位置与所述里程计与所述标志物之间的相对位置计算得到所述距离传感器与所述里程计的相对位置的步骤之后，还包括:

建立环境地图；

获取距离传感器的测量值中的有效测量值；

获取所述里程计的测量数据；

根据所述里程计与所述距离传感器的相对位置与所述测量数据将所述有效测量值转换为世界坐标系的坐标，将所述坐标保存到环境地图中；

将不同时刻的所述坐标保存在所述环境地图中，所述环境地图为障碍物地图。

4.根根据权利要求1所述的方法，其特征在于，所述根据所述障碍物地图与所述当前定位信息检测预设范围内的障碍物，提取障碍物坐标点云的步骤，包括：

获取所述距离传感器中所述障碍物与所述传感器的距离；

当所述距离在预设范围时，获取所述障碍物地图中所述障碍物坐标点云。

5.根据权利要求1所述的方法，其特征在于，所述当所述障碍物坐标点云与所述目标坐标点云匹配成功时，判定所述匹配成功的障碍物为当前货物承载装置的步骤，包括：

根据所述障碍物坐标点云与所述目标坐标点云进行匹配；

当所述障碍物坐标点云通过平移和/或旋转之后能够与所述目标坐标点云重合时，所述匹配成功的障碍物判定为所述当前货物承载装置。

6.根据权利要求1所述的方法，其特征在于，所述对所述当前货物承载装置的坐标点云进行特征检测，根据所述特征检测得到所述当前货物承载装置的坐标的步骤，包括：

对所述当前货物承载装置的坐标点云进行特征检测得到特征坐标点云；

对所述特征坐标点云进行直线提取或拟合得到直线集；

根据所述直线集与所述目坐标点云进行匹配，得到对应的平移矩阵和旋转矩阵；

根据所述平移矩阵、所述旋转矩阵计算得到所述世界坐标系中所述当前货物承载装置的目标坐标。

7.根据权利要求1所述的方法，其特征在于，所述根据所述当前货物承载装置的目标坐标与所述里程计的当前定位信息计算当前货物承载装置与所述移动机器人之间的相对位姿的步骤之后，还包括：

获取所述移动机器人第一行驶路线；

将所述第一行驶路线的起点作为预设起点，将所述第一行驶路线的起点航向作为预设起点航向；

根据所述第一行驶路线的终点和终点航向、所述当前货物承载装置与所述移动机器人之间的相对位姿，计算得到所述移动机器人的预设终点和预设终点航向；

将所述预设起点和所述预设终点分别作为第二行驶路线的起点和终点，将所述预设起点航向和所述预设终点航向分别作为所述第二行驶路线的起点航向和终点航向，重新规划得到所述第二行驶路线。

8.一种货物承载装置检测装置，其特征在于，所述装置包括：

坐标信息获取模块，用于获取目标货物承载装置在世界坐标系中的目标坐标点云；

定位信息获取模块，用于获取移动机器人上里程计的当前定位信息；

障碍物地图获取模块，用于获取障碍物地图；

障碍物坐标点云提取模块，用于根据所述障碍物地图与所述当前定位信息检测预设范围内的障碍物，提取障碍物坐标点云；

判定模块，用于当所述障碍物坐标点云与所述目标坐标点云匹配成功时，判定所述匹配成功的障碍物为当前货物承载装置；

目标坐标获取模块，用于对所述当前货物承载装置的坐标点云进行特征检测，根据所述特征检测得到所述当前货物承载装置的目标坐标；

相对位姿计算模块，用于根据所述当前货物承载装置的目标坐标点云与所述里程计的当前定位信息计算当前货物承载装置与所述移动机器人之间的相对位姿。

9.一种计算机设备，所述计算机设备包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至7中任一项的一种货物承载装置位姿的检测方法。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7中任一项所述的一种货物承载装置位姿的检测方法。