[发明专利]动态目标跟踪定位方法、装置、设备和存储介质

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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