[发明专利]反光柱跟踪定位方法、装置及电子设备

权利要求书

1.一种反光柱跟踪定位方法，其特征在于，包括：

当激光雷达检测到当前时刻的数据信息时，根据预先获取的所述激光雷达在所述当前时刻的前两个时刻的位姿信息预测得到所述激光雷达当前时刻的预测位姿；其中，所述数据信息包括当前时刻检测到的反光柱在激光雷达坐标系下的初始坐标；

根据所述预测位姿，确定所述当前时刻检测到的反光柱在全局坐标系下的预测坐标；

根据所述预测坐标和预先建立的KD树，查找与所述预测坐标匹配的目标反光柱；其中，所述KD树为预先根据全局地标图中所有所述反光柱的位置信息建立的；

根据所述目标反光柱确定所述激光雷达当前时刻的位姿，并更新所述激光雷达前两个时刻的位姿信息；

根据所述预测坐标和预先建立的KD树，查找与所述预测坐标匹配的目标反光柱的步骤，包括：

根据预先建立的KD树在所述全局地标图中进行最邻近搜索，确定所述预测坐标的邻近坐标；

计算所述预测坐标与所述邻近坐标之间的距离；

判断所述距离是否小于阈值，如果小于，将所述邻近坐标对应的反光柱确定为目标反光柱，所述阈值为当前反光柱距激光雷达距离的函数且所述距离越小所述阈值越小、反之所述阈值越大。

2.根据权利要求1所述的方法，其特征在于，所述根据所述预测位姿，确定当前时刻检测到的反光柱在全局坐标系下的预测坐标的步骤，包括：

根据所述预测位姿确定转换矩阵；

根据转换矩阵将所述初始坐标转换为当前时刻检测到的反光柱在全局坐标系下的预测坐标。

3.根据权利要求2所述的方法，其特征在于，所述根据转换矩阵将所述初始坐标转换为当前时刻检测到的反光柱在全局坐标系下的预测坐标的步骤，包括：

按照如下公式计算所述预测坐标：

Xglobal＝T·Xlocal

其中，Xglobal表示预测坐标的齐次坐标矩阵，Xlocal表示初始坐标的齐次坐标矩阵，T表示转换矩阵。

4.根据权利要求1所述的方法，其特征在于，所述当激光雷达检测到当前时刻的数据信息时，根据预先获取的所述激光雷达在所述当前时刻的前两个时刻的位姿信息预测得到所述激光雷达当前时刻的预测位姿的步骤，包括：

根据预先获取的所述激光雷达在所述当前时刻的前两个时刻的位姿信息，基于一阶欧拉法得到所述激光雷达当前时刻的预测位姿。

5.根据权利要求1所述的方法，其特征在于，所述激光雷达的载体的移动速度小于预设值，且所述载体保持匀速运动；所述载体包括AGV小车。

6.一种反光柱跟踪定位装置，其特征在于，包括：预测位姿确定模块，用于当激光雷达检测到当前时刻的数据信息时，根据预先获取的所述激光雷达在所述当前时刻的前两个时刻的位姿信息预测得到所述激光雷达当前时刻的预测位姿；其中，所述数据信息包括当前时刻检测到的反光柱在激光雷达坐标系下的初始坐标；

坐标转换模块，用于根据所述预测位姿，确定所述当前时刻检测到的反光柱在全局坐标系下的预测坐标；

查找模块，用于根据所述预测坐标和预先建立的KD树，查找与所述预测坐标匹配的目标反光柱；其中，所述KD树为预先根据全局地标图中所有所述反光柱的位置信息建立的；

更新模块，用于根据所述目标反光柱确定所述激光雷达当前时刻的位姿，并更新所述激光雷达前两个时刻的位姿信息；

所述查找模块具体用于根据预先建立的KD树在所述全局地标图中进行最邻近搜索，确定所述预测坐标的邻近坐标；计算所述预测坐标与所述邻近坐标之间的距离；判断所述距离是否小于阈值，如果小于，将所述邻近坐标对应的反光柱确定为目标反光柱，所述阈值为当前反光柱距激光雷达距离的函数且所述距离越小所述阈值越小、反之所述阈值越大。

7.根据权利要求6所述的装置，其特征在于，所述坐标转换模块还用于根据所述预测位姿确定转换矩阵；

根据转换矩阵将所述初始坐标转换为当前时刻检测到的反光柱在全局坐标系下的预测坐标。