CN112847343B - 动态目标跟踪定位方法、装置、设备和存储介质 - Google Patents

Abstract

本申请涉及机器人领域，提供了动态目标跟踪定位方法、装置、设备和计算机可读存储介质，以提升算法的速度，快速识别场景中的动态目标并对其精确定位和跟踪，所述方法包括：通过移动机器人搭载的激光雷达对环境物进行采样，获取环境物的激光雷达点云数据；对环境物的激光雷达点云数据进行聚类，获取环境物的点云簇的坐标；将环境物的点云簇的坐标关联至已经存在的点云簇状态向量或者根据环境物的点云簇的坐标，创建点云簇状态向量；基于移动机器人的位姿、点云簇状态向量和点云簇状态向量的位置观测约束，迭代优化移动机器人的位姿和点云簇状态向量中动态目标状态。本申请的技术方案算法速度快，能够对动态目标进行精确定位和跟踪。

Description

动态目标跟踪定位方法、装置、设备和存储介质

技术领域

本发明涉及机器人领域，特别涉及一种动态目标跟踪定位方法、装置、设备和计算机可读存储介质。

背景技术

随着各个行业对移动机器人应用领域的不断扩大，使得移动机器人所面临的环境越来越复杂，尤其是在具有多种不同类型动态目标存在的场景下，例如场景中存在行人、车辆或其它机器人等，机器人不仅需要进行稳定的定位，还需要在动态场景下实现流畅的运动。

解决上述问题的关键是机器人能否从场景中提取到动态目标，对动态物体的位姿和速度进行跟踪。若能够检测并跟踪到场景中的动态目标，则基于动态物体的位姿和速度信息，可以为机器人运动控制系统提供避开动态物体的决策信息，为定位系统消除动态物体对定位精度的影响。针对上述问题，现有技术的解决方案是通过移动机器人上搭载的激光雷达对环境进行采样，对采样数据采用分类算法进行分类后，确定场景中的动态目标。

然而，对于一个只搭载了激光雷达的移动机器人而言，激光雷达提供的环境信息不够丰富，导致算法无法很好地区分潜在的动态目标，从而有针对性地跟踪动态目标。

发明内容

本申请提供一种动态目标跟踪定位方法、装置、设备和计算机可读存储介质，以提升算法的速度，快速识别场景中的动态目标并对其精确定位和跟踪。

一方面，本申请提供了一种动态目标跟踪定位方法，包括：

通过移动机器人搭载的激光雷达对环境物进行采样，获取所述环境物的激光雷达点云数据；

对所述环境物的激光雷达点云数据进行聚类，获取所述环境物的点云簇的坐标；

将所述环境物的点云簇的坐标关联至已经存在的点云簇状态向量或者根据所述环境物的点云簇的坐标，创建点云簇状态向量；

基于所述移动机器人的位姿、所述点云簇状态向量和所述点云簇状态向量的位置观测约束，迭代优化所述移动机器人的位姿和所述点云簇状态向量中动态目标状态。

另一方面，本申请提供了一种动态目标跟踪定位装置，包括：

第一获取模块，用于通过移动机器人搭载的激光雷达对环境物进行采样，获取环境物的激光雷达点云数据；

第二获取模块，用于对所述环境物的激光雷达点云数据进行聚类，获取所述环境物的点云簇的坐标；

点云簇坐标处理模块，用于将所述环境物的点云簇的坐标关联至已经存在的点云簇状态向量或者根据所述环境物的点云簇的坐标，创建点云簇状态向量；

迭代优化模块，用于基于所述移动机器人的位姿、所述点云簇状态向量和所述点云簇状态向量的位置观测约束，迭代优化所述移动机器人的位姿和所述点云簇状态向量中动态目标状态。

第三方面，本申请提供了一种设备，所述设备包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述动态目标跟踪定位方法的技术方案的步骤。

第四方面，本申请提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上述动态目标跟踪定位方法的技术方案的步骤。

从上述本申请提供的技术方案可知，一方面，通过对激光雷达点云数据进行聚类，将环境物的点云簇的坐标关联至已经存在的点云簇状态向量或者创建点云簇状态向量，与现有技术相比，无需使用复杂的检测方法就可识别环境物的种类，进而能够通过点云簇状态向量中的速度向量判断环境物是否为动态目标，算法速度快，提升了激光雷达数据的利用率；另一方面，基于移动机器人的位姿、点云簇状态向量和点云簇状态向量的位置观测约束，迭代优化移动机器人的位姿和点云簇状态向量中动态目标状态，换言之，将动态目标状态和移动机器人的位姿进行联合优化，获得状态估计的全局最优，从而实现了对动态目标的精确定位和跟踪。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本申请实施例提供的动态目标跟踪定位方法的流程图；

图2是本申请实施例提供的动态目标跟踪定位装置的结构示意图；

图3是本申请实施例提供的二维KD树示意图；

图4是本申请实施例提供的设备的结构示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

在本说明书中，诸如第一和第二这样的形容词仅可以用于将一个元素或动作与另一元素或动作进行区分，而不必要求或暗示任何实际的这种关系或顺序。在环境允许的情况下，参照元素或部件或步骤(等)不应解释为局限于仅元素、部件、或步骤中的一个，而可以是元素、部件、或步骤中的一个或多个等。

在本说明书中，为了便于描述，附图中所示的各个部分的尺寸并不是按照实际的比例关系绘制的。

本申请提出了一种动态目标跟踪定位方法，可应用于机器人，该机器人可以是在餐厅作业的机器人，例如，传菜机器人，也可以是在医疗场所，例如医院作业的送药机器人，还可以是在仓库等场所作业的搬运机器人，等等。如附图1所示，动态目标跟踪定位方法主要包括步骤S101至S104，详述如下：

步骤S101：通过移动机器人搭载的激光雷达对环境物进行采样，获取环境物的激光雷达点云数据。

在本申请实施例中，环境是指机器人工作的环境，环境物是指机器人工作的环境中的一切物体，包括在环境中的静态物(例如，某种货物、一棵树、一堵墙、一张桌子)和动态目标(例如，一个人、一辆行驶的车或移动机器人，等等)。在本申请实施例中，机器人搭载的激光雷达对环境物的采样过程与现有技术一样，都是通过向周遭环境发射激光束，实时对当前环境进行扫描，采用飞行时间测距法计算得到机器人与环境中路标之间的距离，每一束激光击中环境物时，该激光束相对于机器人的角度以及激光源与被击中环境物之间的距离等信息，多个激光束击中环境物时的上述信息就构成环境物的激光雷达点云数据。该激光雷达具体可以采用二维激光雷达。

步骤S102：对环境物的激光雷达点云数据进行聚类，获取环境物的点云簇的坐标。

所谓聚类，是一种涉及数据点分组的机器学习技术，对于给定的一组数据点，可以使用聚类算法将每个数据点划分为一个特定的组。理论上，同一组中的数据点应该具有相似的属性和/或特征，而不同组中的数据点应该具有高度不同的属性和/或特征，本申请实施例中，对环境物的激光雷达点云数据进行聚类具有上述聚类算法的一般性质。作为本申请的一个实施例，对环境物的激光雷达点云数据进行聚类，获取环境物的点云簇的坐标可通过步骤S1021至少步骤S1023实现，说明如下：

步骤S1021：对环境物的激光雷达点云数据进行初次聚类，得到第一点云簇的几何中心。

考虑到具有噪声的基于密度聚类(Density-Based Spatial Clustering ofApplications with Noise，DBSCAN)算法具有能够对任意形状的稠密数据集进行聚类，并且在聚类的同时发现异常点，聚类结果鲜有发生偏倚等优点，在本申请实施例中，可以采用DBSCAN算法对环境物的激光雷达点云数据进行初次聚类，得到第一点云簇的几何中心，DBSCAN算法的基本思想是，若一个区域中的数据点的密度大过某个阈值，则将其添加至与之相似的簇中。具体地，对环境物的激光雷达点云数据进行初次聚类，得到第一点云簇的几何中心可以是：设置DBSCAN算法的邻域半径和最小点云簇点数，采用DBSCAN算法对激光雷达点云数据进行初次聚类，得到初次点云簇，求取初次点云簇的几何中心作为第一点云簇的几何中心。上述实施例中，设置DBSCAN算法的邻域半径和最小点云簇点数的目的在于可以将环境物的激光雷达点云数据中异常点予以滤除，以减小聚类算法不必要的工作量，亦能提升分类的精确性。至于求取初次点云簇的几何中心，可以将初次点云簇视为一个质点，采用几何或者物理方法求取该质点的几何中心坐标作为第一点云簇的几何中心。

步骤S1022：若第一点云簇存在相似点云簇，则将第一点云簇的几何中心与相似点云簇的几何中心再次聚类，得到第二点云簇。

无论是人工制造物还是自然物，很多都具有对称性，一般而言，对称性的物体，其激光雷达点云数据经聚类后得到的点云簇具有相似性。例如，人体的两条腿或两只手、汽车的两个前轮，等等，左腿的激光雷达点云数据经聚类后得到的点云簇与右腿的激光雷达点云数据经聚类后得到的点云簇相似，左手的激光雷达点云数据经聚类后得到的点云簇与右手的激光雷达点云数据经聚类后得到的点云簇相似，等等，对于此类相似的点云簇，可以将其几何中心再次聚类，得到新的点云簇，即，在本申请实施例中，若第一点云簇存在相似点云簇，则将第一点云簇的几何中心与相似点云簇的几何中心再次聚类，得到第二点云簇。将第一点云簇的几何中心与相似点云簇的几何中心再次聚类的具体算法，也可以与对激光雷达点云数据进行初次聚类的算法，例如，也可以使用DBSCAN算法或其他聚类算法，只是再次聚类时，将第一点云簇的几何中心与相似点云簇的几何中心视为聚类对象即数据点。

步骤S1023：求取第二点云簇的几何中心坐标，将第二点云簇的几何中心坐标作为环境物的点云簇的坐标。

在本申请实施例中，求取第二点云簇的几何中心坐标可以与第一点云簇的几何中心相同，此处不做赘述。当求取了第二点云簇的几何中心坐标后，将第二点云簇的几何中心坐标作为环境物的点云簇的坐标。

步骤S103：将环境物的点云簇的坐标关联至已经存在的点云簇状态向量或者根据环境物的点云簇的坐标，创建点云簇状态向量。

如前所述，点云簇可以视为一个质点，在经典力学中，通常使用位置和速度等信息来表征一个质点的属性，因此，在本申请实施例中，若定位系统存在点云簇状态向量，则可以直接使用已经存在的点云簇状态向量，若定位系统不存在点云簇状态向量，则可以创建点云簇状态向量。无论是已经存在的点云簇状态向量还是创建的点云簇状态向量，其都可以包括点云簇的位置即点云簇的坐标和速度等信息。作为本申请一个实施例，将环境物的点云簇的坐标关联至已经存在的点云簇状态向量或者根据环境物的点云簇的坐标，创建点云簇状态向量可通过步骤S1031至步骤S1035实现，说明如下：

步骤S1031：若定位系统存在点云簇状态向量，则使用已经存在的点云簇状态向量中的位置信息构建二维KD树。

KD树的全称为k-Dimension Tree，是一种分割K维空间的数据结构，主要应用于关键信息的搜索，本申请实施例中提及的二维KD树，是指该KD树是一种分割二维空间的数据结构。若定位系统存在点云簇状态向量，则使用已经存在的点云簇状态向量中的位置信息构建二维KD树，构建完成的二维KD树，其节点由点云簇状态向量中的位置信息构成。至于构建二维KD树方法，具体可以包括步骤S1至步骤S5，简述如下：

步骤S1：将位置信息即数据点中的x坐标和y坐标进行方差计算，选出其中方差大的，作为分割线的方向；步骤S2：将节点中的数据点按照分割方向的维度进行排序，选出其中的中位数的数据点作为数据矢量即待分割的分割点；步骤S3：同时进行空间矢量的再次划分，即在父节点的空间范围内再进行分割；步骤S4：对剩余节点进行左侧空间和右侧空间的分割，进行左孩子节点和右孩子节点的分割；步骤S5：分割的终点是最终只剩下一个数据点或一侧没有数据点时为止。

步骤S1032：将环境物的点云簇的坐标与二维KD树中的候选点坐标匹配。

在本申请实施例中，将环境物的点云簇的坐标与二维KD树中的候选点坐标匹配，具体包括步骤S1至步骤S3，简述如下：

步骤S1：从二维KD树的根节点开始，从上往下，根据构建二维KD树时候的分割方向，在二维的坐标点上，进行树的顺序查找，例如，假设环境物的点云簇的坐标为(3，1)，对于经步骤S1031构建的二维KD树如图2所示，首先查找根节点(7，2)，由于根节点的划分方向为x，因此，只比较x坐标的划分，又由于3<7，因此，往左边走，后续的节点同样的道理，最终到达叶子节点为止；

步骤S2：当以后步骤S1查找到的节点不是最近时，可以转入步骤S2；

步骤S3：回溯至父节点，回溯至父节点时，首先和父节点比，若父节点距离环境物的点云簇的坐标(以下以待查数据点称呼)更近，则更改当前找到的最近节点，然后，以待查数据点为圆心，待查数据点与最近节点的距离为半径画个圆，判断是否与父节点的分割线相交，若相交，则说明父节点另外的孩子空间存在与待查数据点距离更近的节点，进行父节点空间的又一次深度优先遍历，在局部的遍历查找完毕，再与当前的最近节点比较，比较完之后，继续往上回溯。

步骤S1033：若环境物的点云簇匹配到二维KD树的最近的点云簇，则将环境物的点云簇的坐标关联至已经存在的点云簇状态向量。

在本申请实施例中，将环境物的点云簇的坐标关联至已经存在的点云簇状态向量具体是将环境物的点云簇的坐标保存至已经存在的点云簇状态向量的观测容器中，意味着已经存在的点云簇状态向量找到了与其对应的位置观测约束。

步骤S1034：若定位系统不存在点云簇状态向量，则创建点云簇状态向量。

若定位系统不存在点云簇状态向量，则使用环境物的点云簇的坐标创建一个点云簇状态向量，该环境物的点云簇的坐标即为创建的点云簇状态向量的位置信息的初始值，速度则直接设定为0。

步骤S1035：将环境物的点云簇的坐标确定为已经存在的点云簇状态向量或创建的点云簇状态向量的位置观测约束。

所谓位置观测约束，点云簇状态向量为点云簇在环境中的估计位置，而已经存在的点云簇状态向量或创建的点云簇状态向量的坐标可以认为是通过激光雷达观测到状态向量在环境中的真实位置，位置观测约束就是用于约束观测到的移动机器人实际位姿与点云簇状态向量表示的估计位置的关系。

为了防止将环境物的点云簇的坐标错误地关联至已经存在的点云簇状态向量，在本申请实施例中，在将环境物的点云簇的坐标关联至已经存在的点云簇状态向量之前或同时，可以计算环境物的点云簇的坐标与已经存在的点云簇状态向量之间的马氏距离，若马氏距离大于预设阈值，则将与已经存在的点云簇状态向量之间的马氏距离大于预设阈值的点云簇的坐标去除。

步骤S104：基于移动机器人的位姿、点云簇状态向量和点云簇状态向量的位置观测约束，迭代优化移动机器人的位姿和点云簇状态向量中动态目标状态。

具体地，基于移动机器人的位姿、点云簇状态向量和点云簇状态向量的位置观测约束，迭代优化移动机器人的位姿和点云簇状态向量中动态目标状态可以是：基于移动机器人的多个连续位姿、点云簇状态向量和点云簇状态向量的位置观测约束，构建非线性最小二乘化问题；以移动机器人的位姿误差和动态目标状态的状态误差作为非线性最小二乘化问题的误差约束条件，迭代优化移动机器人的位姿和点云簇状态向量中动态目标状态，直至移动机器人的位姿误差和动态目标状态的状态误差最小时为止。

上述实施例的方法还包括：判断点云簇状态向量中状态向量对应环境物是否为动态目标，若点云簇状态向量中状态向量对应环境物为动态目标，则将动态目标的状态反馈至移动机器人的运动控制系统。

从上述附图1示例的动态目标跟踪定位方法可知，一方面，通过对激光雷达点云数据进行聚类，将环境物的点云簇的坐标关联至已经存在的点云簇状态向量或者创建点云簇状态向量，与现有技术相比，无需使用复杂的检测方法就可识别环境物的种类，进而能够通过点云簇状态向量中的速度向量判断环境物是否为动态目标，算法速度快，提升了激光雷达数据的利用率；另一方面，基于移动机器人的位姿、点云簇状态向量和点云簇状态向量的位置观测约束，迭代优化移动机器人的位姿和点云簇状态向量中动态目标状态，换言之，将动态目标状态和移动机器人的位姿进行联合优化，获得状态估计的全局最优，从而实现了对动态目标的精确定位和跟踪。

请参阅附图3，是本申请实施例提供的一种动态目标跟踪定位装置，可以包括第一获取模块301、第二获取模块302、点云簇坐标处理模块303和迭代优化模块304，详述如下：

第一获取模块301，用于通过移动机器人搭载的激光雷达对环境物进行采样，获取环境物的激光雷达点云数据；

第二获取模块302，用于对环境物的激光雷达点云数据进行聚类，获取环境物的点云簇的坐标；

点云簇坐标处理模块303，用于将环境物的点云簇的坐标关联至已经存在的点云簇状态向量或者根据环境物的点云簇的坐标，创建点云簇状态向量；

迭代优化模块304，用于基于移动机器人的位姿、点云簇状态向量和点云簇状态向量的位置观测约束，迭代优化移动机器人的位姿和点云簇状态向量中动态目标状态。

可选地，附图3示例的第二获取模块302可以包括第一聚类单元、第二聚类单元和求取单元，其中：

第一聚类单元，用于对激光雷达点云数据进行初次聚类，得到第一点云簇的几何中心；

第二聚类单元，用于若第一点云簇存在相似点云簇，则将第一点云簇的几何中心与相似点云簇的几何中心再次聚类，得到第二点云簇；

求取单元，用于求取第二点云簇的几何中心坐标，将第二点云簇的几何中心坐标作为环境物的点云簇的坐标。

可选地，上述第一聚类单元可以包括设置单元和几何中心求取单元，其中：

设置单元，用于设置具有噪声的基于密度聚类DBSCAN算法的邻域半径和最小点云簇点数，采用DBSCAN算法对激光雷达点云数据进行初次聚类，得到初次点云簇；

几何中心求取单元，用于求取初次点云簇的几何中心作为第一点云簇的几何中心。

可选地，附图3示例的点云簇坐标处理模块303可以包括二维KD树构建单元、匹配单元、关联单元、创建单元和确定单元，其中：

二维KD树构建单元，用于若定位系统存在点云簇状态向量，则使用已经存在的点云簇状态向量中的位置信息构建二维KD树；

匹配单元，用于将环境物的点云簇的坐标与二维KD树中的候选点坐标匹配；

关联单元，用于若匹配单元匹配到二维KD树的最近的点云簇，则将环境物的点云簇的坐标关联至已经存在的点云簇状态向量；

创建单元，用于若定位系统不存在点云簇状态向量，则创建点云簇状态向量；

确定单元，用于将环境物的点云簇的坐标确定为已经存在的点云簇状态向量或创建的点云簇状态向量的位置观测约束。

可选地，附图3示例的迭代优化模块304可以包括二维KD树构建单元和确定单元，其中：

优化问题构建单元，用于基于移动机器人的位姿、点云簇状态向量和点云簇状态向量的位置观测约束，构建非线性最小二乘化问题；

位姿状态迭代单元，用于以移动机器人的位姿误差和动态目标状态的状态误差作为非线性最小二乘化问题的误差约束条件，迭代优化移动机器人的位姿和点云簇状态向量中动态目标状态，直至移动机器人的位姿误差和动态目标状态的状态误差最小时为止。

可选地，附图3示例的装置还可以包括马氏距离计算模块和剔除模块，其中：

马氏距离计算模块，用于点云簇坐标处理模块303将环境物的点云簇的坐标关联至已经存在的点云簇状态向量之前或同时，计算环境物的点云簇的坐标与已经存在的点云簇状态向量之间的马氏距离；

剔除模块，用于若马氏距离大于预设阈值，则将与已经存在的点云簇状态向量之间的马氏距离大于预设阈值的点云簇的坐标去除。

可选地，附图3示例的装置还可以包括判断模块和状态反馈模块，其中：

判断模块，用于判断点云簇状态向量中状态向量对应环境物是否为动态目标；

状态反馈模块，用于若点云簇状态向量中状态向量对应环境物为动态目标，则将动态目标的状态反馈至所述移动机器人的运动控制系统。

从以上技术方案的描述中可知，一方面，通过对激光雷达点云数据进行聚类，将环境物的点云簇的坐标关联至已经存在的点云簇状态向量或者创建点云簇状态向量，与现有技术相比，无需使用复杂的检测方法就可识别环境物的种类，进而能够通过点云簇状态向量中的速度向量判断环境物是否为动态目标，算法速度快，提升了激光雷达数据的利用率；另一方面，基于移动机器人的位姿、点云簇状态向量和点云簇状态向量的位置观测约束，迭代优化移动机器人的位姿和点云簇状态向量中动态目标状态，换言之，将动态目标状态和移动机器人的位姿进行联合优化，获得状态估计的全局最优，从而实现了对动态目标的精确定位和跟踪。

请参阅图4，是本申请一实施例提供的设备的结构示意图。如图4所示，该实施例的设备4主要包括：处理器40、存储器41以及存储在存储器41中并可在处理器40上运行的计算机程序42，例如动态目标跟踪定位方法的程序。处理器40执行计算机程序42时实现上述动态目标跟踪定位方法实施例中的步骤，例如图1所示的步骤S101至S104。或者，处理器40执行计算机程序42时实现上述各装置实施例中各模块/单元的功能，例如图3所示第一获取模块301、第二获取模块302、点云簇坐标处理模块303和迭代优化模块304的功能。

示例性地，动态目标跟踪定位方法的计算机程序42主要包括：通过移动机器人搭载的激光雷达对环境物进行采样，获取环境物的激光雷达点云数据；对环境物的激光雷达点云数据进行聚类，获取环境物的点云簇的坐标；将环境物的点云簇的坐标关联至已经存在的点云簇状态向量或者根据环境物的点云簇的坐标，创建点云簇状态向量；基于移动机器人的位姿、点云簇状态向量和点云簇状态向量的位置观测约束，迭代优化移动机器人的位姿和点云簇状态向量中动态目标状态。计算机程序42可以被分割成一个或多个模块/单元，一个或者多个模块/单元被存储在存储器41中，并由处理器40执行，以完成本申请。一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述计算机程序42在设备4中的执行过程。例如，计算机程序42可以被分割成第一获取模块301、第二获取模块302、点云簇坐标处理模块303和迭代优化模块304(虚拟装置中的模块)的功能，各模块具体功能如下：通过移动机器人搭载的激光雷达对环境物进行采样，获取环境物的激光雷达点云数据；对环境物的激光雷达点云数据进行聚类，获取环境物的点云簇的坐标；将环境物的点云簇的坐标关联至已经存在的点云簇状态向量或者根据环境物的点云簇的坐标，创建点云簇状态向量；基于移动机器人的位姿、点云簇状态向量和点云簇状态向量的位置观测约束，迭代优化移动机器人的位姿和点云簇状态向量中动态目标状态。

设备4可包括但不仅限于处理器40、存储器41。本领域技术人员可以理解，图4仅仅是设备4的示例，并不构成对设备4的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如计算设备还可以包括输入输出设备、网络接入设备、总线等。

所称处理器40可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

存储器41可以是设备4的内部存储单元，例如设备4的硬盘或内存。存储器41也可以是设备4的外部存储设备，例如设备4上配备的插接式硬盘，智能存储卡(Smart MediaCard，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。进一步地，存储器41还可以既包括设备4的内部存储单元也包括外部存储设备。存储器41用于存储计算机程序以及设备所需的其他程序和数据。存储器41还可以用于暂时地存储已经输出或者将要输出的数据。

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

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。

在本申请所提供的实施例中，应该理解到，所揭露的装置/设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/设备实施例仅仅是示意性的，例如，模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个装置，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。

作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个非临时性计算机可读取存储介质中。基于这样的理解，本申请实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，动态目标跟踪定位方法的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤，即，通过移动机器人搭载的激光雷达对环境物进行采样，获取环境物的激光雷达点云数据；对环境物的激光雷达点云数据进行聚类，获取环境物的点云簇的坐标；将环境物的点云簇的坐标关联至已经存在的点云簇状态向量或者根据环境物的点云簇的坐标，创建点云簇状态向量；基于移动机器人的位姿、点云簇状态向量和点云簇状态向量的位置观测约束，迭代优化移动机器人的位姿和点云簇状态向量中动态目标状态。其中，计算机程序包括计算机程序代码，计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。非临时性计算机可读介质可以包括：能够携带计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读内存(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，非临时性计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，非临时性计算机可读介质不包括电载波信号和电信信号。以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。以上所述的具体实施方式，对本申请的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本申请的具体实施方式而已，并不用于限定本申请的保护范围，凡在本申请的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种动态目标跟踪定位方法，其特征在于，所述方法包括：

通过移动机器人搭载的激光雷达对环境物进行采样，获取所述环境物的激光雷达点云数据；

对所述环境物的激光雷达点云数据进行聚类，获取所述环境物的点云簇的坐标；

将所述环境物的点云簇的坐标关联至已经存在的点云簇状态向量，或者根据所述环境物的点云簇的坐标来创建点云簇状态向量，并确定已经存在的或创建的所述点云簇状态向量的位置观测约束；所述点云簇状态向量包括点云簇的坐标和速度信息；所述位置观测约束用于约束观测到的移动机器人实际位姿与点云簇状态向量表示的估计位置的关系；

基于所述移动机器人的位姿、所述点云簇状态向量和所述点云簇状态向量的位置观测约束，迭代优化所述移动机器人的位姿和所述点云簇状态向量中动态目标状态。

2.如权利要求1所述动态目标跟踪定位方法，其特征在于，所述对所述环境物的激光雷达点云数据进行聚类，获取所述环境物的点云簇的坐标，包括：

对所述激光雷达点云数据进行初次聚类，得到第一点云簇的几何中心；

若所述第一点云簇存在相似点云簇，则将所述第一点云簇的几何中心与所述相似点云簇的几何中心再次聚类，得到第二点云簇；

求取所述第二点云簇的几何中心坐标，将所述第二点云簇的几何中心坐标作为所述环境物的点云簇的坐标。

3.如权利要求2所述动态目标跟踪定位方法，其特征在于，所述对所述激光雷达点云数据进行初次聚类，得到第一点云簇的几何中心，包括：

设置具有噪声的基于密度聚类DBSCAN算法的邻域半径和最小点云簇点数，采用所述DBSCAN算法对所述激光雷达点云数据进行初次聚类，得到初次点云簇；

求取所述初次点云簇的几何中心作为所述第一点云簇的几何中心。

4.如权利要求1所述动态目标跟踪定位方法，其特征在于，所述将所述环境物的点云簇的坐标关联至已经存在的点云簇状态向量，或者根据所述环境物的点云簇的坐标来创建点云簇状态向量，并确定已经存在的或创建的所述点云簇状态向量的位置观测约束，包括：

若定位系统存在点云簇状态向量，则使用所述已经存在的点云簇状态向量中的位置信息构建二维KD树；

将所述环境物的点云簇的坐标与所述二维KD树中的候选点坐标匹配；

若匹配到所述二维KD树的最近的点云簇，则将所述环境物的点云簇的坐标关联至所述已经存在的点云簇状态向量；

若所述定位系统不存在点云簇状态向量，则创建所述点云簇状态向量；

将所述环境物的点云簇的坐标确定为所述已经存在的点云簇状态向量或创建的点云簇状态向量的位置观测约束。

5.如权利要求1所述动态目标跟踪定位方法，其特征在于，所述基于所述移动机器人的位姿、所述点云簇状态向量和所述点云簇状态向量的位置观测约束，迭代优化所述移动机器人的位姿和所述点云簇状态向量中动态目标状态，包括：

基于所述移动机器人的位姿、所述点云簇状态向量和所述点云簇状态向量的位置观测约束，构建非线性最小二乘化问题；

以所述移动机器人的位姿误差和所述动态目标状态的状态误差作为所述非线性最小二乘化问题的误差约束条件，迭代优化所述移动机器人的位姿和所述点云簇状态向量中动态目标状态，直至所述移动机器人的位姿误差和所述动态目标状态的状态误差最小时为止。

6.如权利要求1所述动态目标跟踪定位方法，其特征在于，所述将所述环境物的点云簇的坐标关联至已经存在的点云簇状态向量之前或同时，所述方法还包括：

计算所述环境物的点云簇的坐标与所述已经存在的点云簇状态向量之间的马氏距离；

若所述马氏距离大于预设阈值，则将所述与所述已经存在的点云簇状态向量之间的马氏距离大于所述预设阈值的点云簇的坐标去除。

7.如权利要求1至6任意一项所述动态目标跟踪定位方法，其特征在于，所述方法还包括：

判断所述点云簇状态向量中状态向量对应环境物是否为动态目标；

若所述点云簇状态向量中状态向量对应环境物为动态目标，则将所述动态目标的状态反馈至所述移动机器人的运动控制系统。

8.一种动态目标跟踪定位装置，其特征在于，所述装置包括：

第一获取模块，用于通过移动机器人搭载的激光雷达对环境物进行采样，获取所述环境物的激光雷达点云数据；

第二获取模块，用于对所述环境物的激光雷达点云数据进行聚类，获取所述环境物的点云簇的坐标；

点云簇坐标处理模块，用于将所述环境物的点云簇的坐标关联至已经存在的点云簇状态向量，或者根据所述环境物的点云簇的坐标来创建点云簇状态向量，并确定所述已经存在的点云簇状态向量或创建的点云簇状态向量的位置观测约束；所述点云簇状态向量包括点云簇的坐标和速度信息；所述位置观测约束用于约束观测到的移动机器人实际位姿与点云簇状态向量表示的估计位置的关系；

迭代优化模块，用于基于所述移动机器人的位姿、所述点云簇状态向量和所述点云簇状态向量的位置观测约束，迭代优化所述移动机器人的位姿和所述点云簇状态向量中动态目标状态。

9.一种设备，所述设备包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至7任意一项所述方法的步骤。

10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7任意一项所述方法的步骤。

Images