CN114563795A - 基于激光里程计和标签融合算法的定位追踪方法及系统 - Google Patents

Abstract

本发明公开了一种基于激光里程计和标签融合算法的定位追踪方法及系统，包括：步骤1，里程计初步定位；步骤2，通过预先在待定位场景中布置不同的两个标签，获取激光雷达的精确位姿，其具体包括：步骤21，通过激光雷达扫描获取标签所在区域的点云，获取两个标签在局部坐标系中的相对坐标；步骤22，根据两个标签的绝对坐标，在每一个标签的相对坐标附近搜索距离最近的绝对坐标，进行匹配；步骤23，将两个标签与激光雷达组成三角形，获取激光雷达位姿变换矩阵；步骤24，使用激光雷达位姿变换矩阵，对激光雷达的初步位姿进行变换，得到激光雷达的精确位姿。本发明可应用无GNSS信号、存在较多干扰、结构不断变化地复杂场景，为车辆提供精确的定位。

Description

基于激光里程计和标签融合算法的定位追踪方法及系统

技术领域

本发明涉及定位技术领域，特别是关于一种基于激光里程计和标签融合算法的定位追踪方法及系统。

背景技术

随着无人驾驶汽车行业的飞速发展，无人驾驶汽车具备的功能更加丰富，应用领域也愈加广泛，例如在露天矿山、物流园区、港口等封闭场景中均已出现以无人驾驶汽车为基础搭建的智慧矿山、智慧物流、智慧港口等无人作业系统。无人驾驶汽车在这些作业场景下，可以实现障碍物检测与防撞、实时定位、智能行为决策以及高精度车辆控制等功能，这使得作业过程对人力的需求减少，并且可以根据作业目标合理分配资源，在保证效率的前提下大大提升了工作过程的经济性和安全性。

在工作过程中，准确的定位可以让无人车时刻清楚自身所在的位置、规划正确的行驶路线并且及时躲避环境中存在的障碍物，通常借助GNSS、激光雷达和摄像头等对车辆进行定位，以保障无人作业系统高效运行。但是在许多特殊场景下，例如封闭的生产车间或者光线昏暗的地下矿井，这样的作业场景无GNSS信号，且场景结构会随工作进度不断变化，同时还存在干扰因素影响着激光雷达与摄像头的精度。因此，亟需设计一个可适用于复杂作业场景且精度较高的定位方法及系统，能够在类似于矿井的无人作业环境中，降低恶劣环境因素带来的干扰，为无人驾驶汽车提供精确的定位。

发明内容

本发明的目的在于提供一种基于激光里程计和标签融合算法的定位追踪方法及系统来克服或至少减轻现有技术的上述缺陷中的至少一个。

为实现上述目的，本发明提供一种基于激光里程计与标签融合算法的定位追踪方法，其包括：

步骤1，里程计初步定位，获取激光雷达的初步位姿；

步骤2，通过预先在待定位场景中布置不同的两个标签，获取所述激光雷达在全局坐标系中的绝对坐标，其中：第一所述标签的绝对坐标被描述为(m，n)，第二所述标签的绝对坐标被描述为(p，q)；

所述步骤2包括：

步骤21，通过激光雷达扫描获取标签所在区域的点云，获取所述两个标签在局部坐标系中的相对坐标，其中：所述第一标签的所述相对坐标被描述为(a，b)，所述第二标签的所述相对坐标被描述为(c，d)；

步骤22，根据所述两个标签的所述绝对坐标，在每一个所述标签的所述相对坐标附近搜索距离最近的绝对坐标，并进行匹配；

步骤23，将所述两个标签与所述激光雷达组成三角形，采用三角定位算法获取激光雷达位姿变换矩阵；

步骤24，使用所述步骤23的所述激光雷达位姿变换矩阵，对所述步骤1获取的激光雷达的初步位姿进行变换，得到所述激光雷达的精确位姿。

进一步地，所述步骤1中的布置标签的方法具体包括：

将不同的两个标签沿车辆的纵向和横向之一的预设方向A隔开布置在平行于预设方向A的平面B上。

进一步地，所述步骤21具体包括：

步骤211，提取所述两个标签所在区域的点云，在所有提取到的点云中筛选出强度较高的点云，同时设置半径过滤掉距离较远的孤立点；

步骤212，搜索获得所述两个标签对应的点云集合；

步骤213，计算出每一个所述点云集合的中心坐标，作为所述两个标签的所述相对坐标。

进一步地，所述步骤23具体包括：

步骤231，结合式(2)计算得到的所述两个标签的中点在局部坐标系中的相对坐标M_local和式(3)计算得到的所述激光雷达的在局部坐标系中的相对偏转角yaw_local，获取相对位姿变换矩阵P_local；

P_local＝[M_local，yaw_local] (1)

步骤232，结合式(5)计算得到的所述两个标签在全局坐标系中的的绝对中点坐标M_global和式(6)计算得到的所述激光雷达在全局坐标系中的的绝对偏转角yaw_global，获取绝对位姿变换矩阵P_global；

P_global＝[M_global，yaw_global] (4)

步骤233，结合所述P_local和Pg_lobal，通过式(7)获取所述激光雷达位姿变换矩阵P_current：

P_current＝P_global*P_local ^-1 (7)。

进一步地，所述步骤1具体包括：

步骤11，通过激光雷达扫描得到的车辆所处工作区域的当前帧点云，对所述当前帧点云进行特征提取，得到对应的特征信息；并对点云进行去畸变处理，防止出现尺度漂移现象，然后对去畸变后的点云进行特征提取，得到当前帧点云所对应的特征信息；

步骤12，通过里程计估计在获取下一帧点云之前激光雷达的运动状态，并在获取下一帧点云后，对前后两帧点云进行特征匹配，获取所述前后两帧点云之间的转换矩阵；

步骤13，返回步骤11，通过不断加入新的点云，对所述转换矩阵进行更新，最终得到激光雷达的初步位姿。

本发明还提供一种基于激光里程计与标签融合算法的定位追踪系统，其包括：

里程计初步定位模块，其用于获取激光雷达的初步位姿；

标签辅助定位模块，其用于通过预先在待定位场景中布置不同的两个标签，获取所述激光雷达在全局坐标系中的绝对坐标，其中：第一所述标签的绝对坐标被描述为(m，n)，第二所述标签的绝对坐标被描述为(p，q)；；

所述标签辅助定位模块包括：

标签相对位置获取单元，其用于通过激光雷达扫描获取标签所在区域的点云，获取所述两个标签在局部坐标系中的相对坐标，其中：所述第一标签的相对坐标被描述为(a，b)，所述第二标签的相对坐标被描述为(c，d)；

标签绝对位置获取单元，其用于根据所述两个标签在全局坐标系中的绝对坐标，在每一个所述标签的相对坐标附近搜索距离最近的绝对坐标，并进行匹配；

位姿变换矩阵获取单元，其用于将所述两个标签与所述激光雷达组成三角形，采用三角定位算法获取激光雷达位姿变换矩阵；

精确位姿获取单元，其用于使用所述位姿变换矩阵获取单元的所述激光雷达位姿变换矩阵，对所述里程计初步定位模块获取的激光雷达的初步位姿进行变换，得到所述激光雷达的精确位姿。

进一步地，所述标签辅助定位模块中的布置标签的方法具体包括：

将不同的两个标签沿车辆的纵向和横向之一的预设方向A隔开布置在平行于预设方向A的平面B上。

进一步地，所述标签相对位置获取单元具体包括：

点云筛选子单元，其用于提取所述两个标签所在区域的点云，在所有提取到的点云中筛选出强度较高的点云，同时设置半径过滤掉距离较远的孤立点；

点云搜索子单元，其用于搜索获得所述两个标签的点云集合；

计算子单元，其用于计算出每一个所述点云集合的中心坐标，作为所述两个标签的相对坐标。

进一步地，所述精确位姿获取单元具体包括：

相对位姿变换矩阵计算子单元，其用于结合式(2)计算得到的所述两个标签的中点在局部坐标系中的相对坐标M_local和式(3)计算得到的所述激光雷达的在局部坐标系中的相对偏转角yaw_local，获取相对位姿变换矩阵P_local；

P_local＝[M_local，yaw_local] (1)

绝对位姿变换矩阵计算子单元，其用于结合式(5)计算得到的所述两个标签在全局坐标系中的的绝对中点坐标M_global和式(6)计算得到的所述激光雷达在全局坐标系中的的绝对偏转角yaw_global，获取绝对位姿变换矩阵P_global；

P_global＝[M_global，yaw_global] (4)

激光雷达位姿变换矩阵计算子单元，其用于结合所述P_local和P_global，通过式(7)获取激光雷达位姿变换矩阵P_current：

P_current＝P_global*P_local ^-1 (7)。

进一步地，所述里程计初步定位模块具体包括：

当前帧点云特征子单元，其用于通过激光雷达扫描得到的车辆所处工作区域的当前帧点云，对所述当前帧点云进行特征提取，得到对应的特征信息；并对点云进行去畸变处理，防止出现尺度漂移现象，然后对去畸变后的点云进行特征提取，得到当前帧点云所对应的特征信息；

点云转换矩阵子单元，其用于通过里程计估计在获取下一帧点云之前激光雷达的运动状态，并在获取下一帧点云后，对前后两帧点云进行特征匹配，获取所述前后两帧点云之间的转换矩阵；

激光雷达初步位姿子单元，其用于通过不断加入新的由所述当前帧点云特征子单元获取的点云，对所述转换矩阵进行更新，最终得到激光雷达的初步位姿。

本发明由于采取以上技术方案，其具有以下优点：

针对现有的封闭环境中无GNSS信号、定位精度较低、无人驾驶汽车无法正常工作的问题，本发明仅借助两个标签，结合激光里程计对车辆进行精确定位，可应用于无GNSS信号、存在较多干扰因素且环境不断变化的复杂作业场景中，同时其算法运行速度较快、定位精度很高，能够为无人驾驶车辆提供非常精确的位置信息，保证生产工作正常地、安全地、高效地进行。

附图说明

图1为本发明实施例提供的基于激光里程计与标签融合算法的定位追踪方法的算法流程图。

图2为发明实施例提供的基于激光里程计与标签融合算法的定位追踪方法中标签的位置分布示意图。

图3为发明实施例提供的基于激光里程计与标签融合算法的定位追踪方法中标签与激光雷达的坐标关系示意图。

图4为发明实施例提供的基于激光里程计与标签融合算法的定位追踪系统的结构框架图。

具体实施方式

在附图中，使用相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面结合附图对本发明的实施例进行详细说明。

如图1和图4所示，本发明实施例提供的基于激光里程计与标签融合算法的定位追踪方法包括：

步骤1，里程计初步定位，获取激光雷达的初步位姿。其中，位姿包括坐标的平移和旋转，可表示为

R是旋转，t是平移。

步骤2，通过预先在待定位场景中布置不同的两个标签，第一所述标签的绝对坐标被描述为(m，n)，第二所述标签的绝对坐标被描述为(p，q)，获取激光雷达的精确位姿。

如图2所示，布置标签的方法可以包括：

将不同的两个标签Q、R沿车辆P的纵向和横向之一的预设方向A隔开布置在平行于预设方向A的平面B上。

需要说明的是，两个标签还可以理解为两个标定点，例如只布置一个具有长度的标签，扫描获取标签的两个端点，用两个端点与激光雷达组成三角形，然后通过几何关系求解。这种方式相比于两个标签，一个标签更容易部署，为了区别两个端点，标签需要稍长一点，但两个标签的定位精度更高。

本发明仅借助两个标签，通过合理安排标签的分布增加三角定位算法中的基线长度，在保证较低的计算误差与较高的定位精度的前提下，使用尽量少的标签对激光雷达进行定位。

如图3所示，所述步骤2包括：

步骤21，通过激光雷达扫描获取标签所在区域的点云，获取所述两个标签在局部坐标系中的相对坐标，其中：第一标签的相对坐标被描述为(a，b)，第二标签的相对坐标被描述为(c，d)。

步骤22，根据两个标签的绝对坐标，在每一个所述标签的相对坐标附近搜索距离最近的绝对坐标，并进行匹配。

步骤23，将两个标签与所述激光雷达组成三角形，采用三角定位算法获取激光雷达位姿变换矩阵。

步骤24，使用所述步骤23的所述激光雷达位姿变换矩阵，对所述步骤1获取的激光雷达的初步位姿进行变换，得到所述激光雷达的精确位姿。

在一个实施例中，步骤21具体包括：

步骤211，提取所述两个标签所在区域的点云，在所有提取到的点云中筛选出强度较高的点云，同时设置半径过滤掉距离较远的孤立点，其中，强度较高的点云可以以预设灰度阈值为界限，例如：灰度阈值可以但不限于为200，也就是说，大于预设灰度阈值的点云视为强度较高的点云。此外，再设置半径过滤掉距离较远的孤立点时，同样的方法，可以设置半径数值，例如：0.2m，也就是说，以该点为圆心、半径为0.2m的圆形区域内没有其他点的点，为孤立点。

步骤212，搜索获得所述两个标签对应的点云集合。可以采用欧式聚类方法进行搜索，也可以采用现有的其他方法得到。

步骤213，计算出每一个所述点云集合的中心坐标，作为两个标签在局部坐标系中的相对坐标(M，N)。

本发明借助两个标签与激光雷达构建三角形，通过相对位置关系对激光雷达进行定位。

在一个实施例中，步骤23具体包括：

步骤231，结合式(2)计算得到的所述两个标签的中点在局部坐标系中的相对坐标M_local和式(3)计算得到的所述激光雷达的在局部坐标系中的相对偏转角yaw_local，获取相对位姿变换矩阵P_local；

P_local＝[M_local，yaw_local] (1)

步骤232，结合式(5)计算得到的所述两个标签在全局坐标系中的的绝对中点坐标M_global和式(6)计算得到的所述激光雷达在全局坐标系中的的绝对偏转角yaw_global，获取绝对位姿变换矩阵P_global；

P_global＝[M_global，yaw_global] (4)

步骤233，结合所述P_local和P_global，通过式(7)获取激光雷达位姿变换矩阵P_current：

P_current＝P_global*P_local ^-1 (7)。

在一个实施例中，步骤1具体包括：

步骤11，通过激光雷达扫描得到的车辆所处工作区域的当前帧点云，对所述当前帧点云进行特征提取，得到对应的特征信息；并对点云进行去畸变处理，防止出现尺度漂移现象，然后对去畸变后的点云进行特征提取，得到当前帧点云所对应的特征信息。

其中，车辆所处工作区域可以从下面两个方面理解：

1.激光雷达安装在无人驾驶汽车上，周围环境是指无人车所在的环境。

2.周围环境限定为无人车工作的区域，例如封闭车间内的车辆运输道路、矿洞内车辆可行驶的路径等区域。

步骤12，通过里程计估计在获取下一帧点云之前激光雷达的运动状态，并在获取下一帧点云后，对前后两帧点云进行特征匹配，获取所述前后两帧点云之间的转换矩阵。

其中，激光雷达的运动状态包括激光雷达坐标的平移和旋转

步骤13，返回步骤11，直到完成对整个路径的扫描，回到初始位置，就不再重复，通过不断加入新的点云，对所述转换矩阵进行更新，最终得到激光雷达的初步位姿。

本发明实施例提供基于激光里程计与标签融合算法的定位追踪系统包括里程计初步定位模块和标签辅助定位模块，其中：

里程计初步定位模块用于获取激光雷达的初步位姿。

标签辅助定位模块用于通过预先在待定位场景中布置不同的两个标签，第一所述标签的绝对坐标被描述为(m，n)，第二所述标签的绝对坐标被描述为(p，q)，获取激光雷达的精确位姿。

所述标签辅助定位模块包括标签相对位置获取单元、标签绝对位置获取单元、位姿变换矩阵获取单元和精确位姿获取单元，其中：

标签相对位置获取单元用于通过激光雷达扫描获取标签所在区域的点云，获取所述两个标签在局部坐标系中的相对坐标，其中：第一所述标签的相对坐标被描述为(a，b)，第二所述标签的相对坐标被描述为(c，d)。

标签绝对位置获取单元用于根据所述两个标签在全局坐标系中的绝对坐标，其中：第一所述标签的绝对坐标被描述为(m，n)，第二所述标签的绝对坐标被描述为(p，q)；在每一个所述标签的相对坐标附近搜索距离最近的绝对坐标，并进行匹配。

位姿变换矩阵获取单元用于将所述两个标签与所述激光雷达组成三角形，采用三角定位算法获取激光雷达位姿变换矩阵。

精确位姿获取单元用于使用所述位姿变换矩阵获取单元的所述激光雷达位姿变换矩阵，对所述里程计初步定位模块获取的激光雷达的初步位姿进行变换，得到所述激光雷达的精确位姿。

在一个实施例中，所述标签相对位置获取单元具体包括点云筛选子单元、点云搜索子单元和计算子单元，其中：

点云筛选子单元用于提取所述两个标签所在区域的点云，在所有提取到的点云中筛选出强度较高的点云，同时设置半径过滤掉距离较远的孤立点。

点云搜索子单元用于通过欧式聚类搜索得到所述两个标签对应的点云集合。

计算子单元用于计算出每一个所述点云集合的中心坐标，作为所述两个标签的相对坐标。

在一个实施例中，所述精确位姿获取单元具体包括相对位姿变换矩阵计算子单元和绝对位姿变换矩阵计算子单元，其中：

相对位姿变换矩阵计算子单元用于结合式(2)计算得到的所述两个标签的中点在局部坐标系中的相对坐标M_local和式(3)计算得到的所述激光雷达的在局部坐标系中的相对偏转角yaw_local，获取相对位姿变换矩阵P_local；

P_local＝[M_local，yaw_local] (1)

绝对位姿变换矩阵计算子单元用于结合式(5)计算得到的所述两个标签在全局坐标系中的的绝对中点坐标M_global和式(6)计算得到的所述激光雷达在全局坐标系中的的绝对偏转角yaw_global，获取绝对位姿变换矩阵P_global；

P_global＝[M_global,yaw_global] (4)

激光雷达位姿变换矩阵计算子单元，其用于结合所述P_local和P_global，通过式(7)获取激光雷达位姿变换矩阵P_current：

P_current＝P_global*P_local ^-1 (7)。

在一个实施例中，所述里程计初步定位模块具体包括当前帧点云特征子单元、点云转换矩阵子单元和激光雷达初步位姿子单元，其中：

当前帧点云特征子单元用于通过激光雷达扫描得到的车辆所处工作区域的当前帧点云，对所述当前帧点云进行特征提取，得到对应的特征信息；并对点云进行去畸变处理，防止出现尺度漂移现象，然后对去畸变后的点云进行特征提取，得到当前帧点云所对应的特征信息。

点云转换矩阵子单元用于通过里程计估计在获取下一帧点云之前激光雷达的运动状态，并在获取下一帧点云后，对前后两帧点云进行特征匹配，获取所述前后两帧点云之间的转换矩阵。

激光雷达初步位姿子单元用于通过不断加入新的由所述当前帧点云特征子单元获取的点云，对所述转换矩阵进行更新，最终得到激光雷达的初步位姿。

综上所述，本发明借助两个标签，结合激光里程计对车辆进行精确定位，可应用于无GNSS信号、存在较多干扰因素且环境不断变化的复杂作业场景中，同时其算法运行速度较快、定位精度很高，能够为无人驾驶车辆提供非常精确的位置信息，保证生产工作正常地、安全地、高效地进行。

最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种基于激光里程计与标签融合算法的定位追踪方法，其特征在于，包括：

步骤1，里程计初步定位，获取激光雷达的初步位姿；

步骤2，通过预先在待定位场景中布置不同的两个标签，第一所述标签的绝对坐标被描述为(m，n)，第二所述标签的绝对坐标被描述为(p，q)，获取激光雷达的精确位姿，其具体包括：

步骤21，通过激光雷达扫描获取标签所在区域的点云，获取所述两个标签在局部坐标系中的相对坐标，其中：所述第一标签的所述相对坐标被描述为(a，b)，所述第二标签的所述相对坐标被描述为(c，d)；

步骤22，根据所述两个标签的所述绝对坐标，在每一个所述标签的所述相对坐标附近搜索距离最近的绝对坐标，并进行匹配；

步骤23，将所述两个标签与所述激光雷达组成三角形，采用三角定位算法获取激光雷达位姿变换矩阵；

步骤24，使用所述步骤23的所述激光雷达位姿变换矩阵，对所述步骤1获取的激光雷达的初步位姿进行变换，得到所述激光雷达的精确位姿。

2.如权利要求2所述的基于激光里程计与标签融合算法的定位追踪方法，其特征在于，所述步骤1中的布置标签的方法具体包括：

将不同的两个标签沿车辆的纵向和横向之一的方向(A)隔开布置在平行于所述方向(A)的平面(B)上。

3.如权利要求1所述的基于激光里程计与标签融合算法的定位追踪方法，其特征在于，所述步骤21具体包括：

步骤211，提取所述两个标签所在区域的点云，在所有提取到的点云中筛选出强度较高的点云，同时设置半径过滤掉距离较远的孤立点；

步骤212，搜索获得所述两个标签对应的点云集合；

步骤213，计算出每一个所述点云集合的中心坐标，作为所述两个标签的所述相对坐标。

4.如权利要求1-3中任一项所述的基于激光里程计与标签融合算法的定位追踪方法，其特征在于，所述步骤23具体包括：

步骤231，结合式(2)计算得到的所述两个标签的中点在局部坐标系中的相对坐标M_local和式(3)计算得到的所述激光雷达的在局部坐标系中的相对偏转角yaw_local，获取相对位姿变换矩阵P_local；

P_local＝[M_local,yaw_local] (1)

步骤232，结合式(5)计算得到的所述两个标签在全局坐标系中的的绝对中点坐标M_global和式(6)计算得到的所述激光雷达在全局坐标系中的的绝对偏转角yaw_global，获取绝对位姿变换矩阵P_global；

P_global＝[M_global,yaw_global] (4)

步骤233，结合所述P_local和P_global，通过式(7)获取所述激光雷达位姿变换矩阵P_current：

P_current＝P_global*P_local ^-1 (7)。

5.如权利要求4所述的基于激光里程计与标签融合算法的定位追踪方法，其特征在于，所述步骤1具体包括：

步骤11，通过激光雷达扫描得到的车辆所处工作区域的当前帧点云，对所述当前帧点云进行特征提取，得到对应的特征信息；并对点云进行去畸变处理，防止出现尺度漂移现象，然后对去畸变后的点云进行特征提取，得到当前帧点云所对应的特征信息；

步骤12，通过里程计估计在获取下一帧点云之前激光雷达的运动状态，并在获取下一帧点云后，对前后两帧点云进行特征匹配，获取所述前后两帧点云之间的转换矩阵；

步骤13，返回步骤11，通过不断加入新的点云，对所述转换矩阵进行更新，最终得到激光雷达的初步位姿。

6.一种基于激光里程计与标签融合算法的定位追踪系统，其特征在于，包括：

里程计初步定位模块，其用于获取激光雷达的初步位姿；

标签辅助定位模块，其用于通过预先在待定位场景中布置不同的两个标签，第一所述标签的绝对坐标被描述为(m，n)，第二所述标签的绝对坐标被描述为(p，q)，获取激光雷达的精确位姿，其具体包括；

所述标签辅助定位模块包括：

标签相对位置获取单元，其用于通过激光雷达扫描获取标签所在区域的点云，获取所述两个标签在局部坐标系中的相对坐标，其中：所述第一标签的相对坐标被描述为(a，b)，所述第二标签的相对坐标被描述为(c，d)；

标签绝对位置获取单元，其用于根据所述两个标签在全局坐标系中的绝对坐标，在每一个所述标签的相对坐标附近搜索距离最近的绝对坐标，并进行匹配；

位姿变换矩阵获取单元，其用于将所述两个标签与所述激光雷达组成三角形，采用三角定位算法获取激光雷达位姿变换矩阵；

精确位姿获取单元，其用于使用所述位姿变换矩阵获取单元的所述激光雷达位姿变换矩阵，对所述里程计初步定位模块获取的激光雷达的初步位姿进行变换，得到所述激光雷达的精确位姿。

7.如权利要求6所述的基于激光里程计与标签融合算法的定位追踪系统，其特征在于，所述标签辅助定位模块中的布置标签的方法具体包括：

将不同的两个标签沿车辆的纵向和横向之一的方向(A)隔开布置在平行于所述方向(A)的平面(B)上。

8.如权利要求6所述的基于激光里程计与标签融合算法的定位追踪系统，其特征在于，所述标签相对位置获取单元具体包括：

点云筛选子单元，其用于提取所述两个标签所在区域的点云，在所有提取到的点云中筛选出强度较高的点云，同时设置半径过滤掉距离较远的孤立点；

点云搜索子单元，其用于搜索获得所述两个标签对应的点云集合；

计算子单元，其用于计算出每一个所述点云集合的中心坐标，作为所述两个标签的相对坐标。

9.如权利要求6-8中任一项所述的基于激光里程计与标签融合算法的定位追踪系统，其特征在于，所述精确位姿获取单元具体包括：

相对位姿变换矩阵计算子单元，其用于结合式(2)计算得到的所述两个标签的中点在局部坐标系中的相对坐标M_local和式(3)计算得到的所述激光雷达的在局部坐标系中的相对偏转角yaw_local，获取相对位姿变换矩阵P_local；

P_local＝[M_local,yaw_local] (1)

绝对位姿变换矩阵计算子单元，其用于结合式(5)计算得到的所述两个标签在全局坐标系中的的绝对中点坐标M_global和式(6)计算得到的所述激光雷达在全局坐标系中的的绝对偏转角yaw_global，获取绝对位姿变换矩阵P_global；

P_global＝[M_global,yaw_global] (4)

激光雷达位姿变换矩阵计算子单元，其用于结合所述P_local和P_global，通过式(7)获取激光雷达位姿变换矩阵P_current：

P_current＝P_global*P_local ^-1 (7)。

10.如权利要求9所述的基于激光里程计与标签融合算法的定位追踪系统，其特征在于，所述里程计初步定位模块具体包括：

当前帧点云特征子单元，其用于通过激光雷达扫描得到的车辆所处工作区域的当前帧点云，对所述当前帧点云进行特征提取，得到对应的特征信息；并对点云进行去畸变处理，防止出现尺度漂移现象，然后对去畸变后的点云进行特征提取，得到当前帧点云所对应的特征信息；

点云转换矩阵子单元，其用于通过里程计估计在获取下一帧点云之前激光雷达的运动状态，并在获取下一帧点云后，对前后两帧点云进行特征匹配，获取所述前后两帧点云之间的转换矩阵；

激光雷达初步位姿子单元，其用于通过不断加入新的由所述当前帧点云特征子单元获取的点云，对所述转换矩阵进行更新，最终得到激光雷达的初步位姿。

Images