[发明专利]基于激光里程计和标签融合算法的定位追踪方法及系统

权利要求书

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

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

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

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

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

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

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

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

将不同的两个标签沿预设方向隔开布置在平行于该预设方向的平面上，所述预设方向为车辆的纵向或横向。

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，通过不断加入新的点云，对所述转换矩阵进行更新，最终得到激光雷达的初步位姿。