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

说明书

本发明提供了一种反光柱跟踪定位方法、装置及电子设备，包括：当激光雷达检测到当前时刻的数据信息时，根据预先获取的激光雷达在当前时刻的前两个时刻的位姿信息预测得到激光雷达当前时刻的预测位姿；其中，数据信息包括当前时刻检测到的反光柱在激光雷达坐标系下的初始坐标；根据预测位姿，确定当前时刻检测到的反光柱在全局坐标系下的预测坐标；根据预测坐标和预先建立的KD树，查找与预测坐标匹配的目标反光柱；其中，KD树为预先根据全局地标图中所有反光柱的位置信息建立的；根据目标反光柱确定激光雷达当前时刻的位姿，并更新激光雷达前两个时刻的位姿信息。本发明可以减少计算量，提高激光雷达当前时刻位姿的解算速度。

技术领域

本发明涉及激光雷达定位技术领域，尤其是涉及一种反光柱跟踪定位方法、装置及电子设备。

背景技术

在现代制造业中，生产阶段自动化因其可以显著提高生产效率、降低成本，其重要性与日俱增。自动导引车(Automated Guided Vehicle，AGV)作为自动化生产中的运输工具，AGV的导航定位也收到广泛关注。常见的工业AGV导航定位方式包括磁导航、自然导航(视觉SLAM或激光SLAM)、地标导航(二维码或RFID标签等)、激光导航(反光柱)等。其中基于反光柱(或称有反)激光导航由于其定位精度高、维护成本低、安装成本相对较低、AGV的路径规划柔性高等优点在叉车AGV上获得了大量应用。但是，现有的反光柱激光雷达定位算法在定位过程中时，通常采用基于反光柱间欧氏距离在全局坐标系与雷达坐标系内不变的原理，进行反光柱与全局地标地图的匹配搜索，进行全局搜索时未有效利用激光雷达位姿先验信息，全局匹配搜索计算量较大，速度较慢。

发明内容

有鉴于此，本发明的目的在于提供一种反光柱跟踪定位方法、装置及电子设备，以减少计算量，提高激光雷达当前时刻位姿的解算速度。

第一方面，本发明实施例提供了一种反光柱跟踪定位方法，包括：当激光雷达检测到当前时刻的数据信息时，根据预先获取的激光雷达在当前时刻的前两个时刻的位姿信息预测得到激光雷达当前时刻的预测位姿；其中，数据信息包括当前时刻检测到的反光柱在激光雷达坐标系下的初始坐标；根据预测位姿，确定当前时刻检测到的反光柱在全局坐标系下的预测坐标；根据预测坐标和预先建立的KD树，查找与预测坐标匹配的目标反光柱；其中，KD树为预先根据全局地标图中所有反光柱的位置信息建立的；根据目标反光柱确定激光雷达当前时刻的位姿，并更新激光雷达前两个时刻的位姿信息。

在一种实施方式中，根据预测位姿，确定当前时刻检测到的反光柱在全局坐标系下的预测坐标的步骤，包括：根据预测位姿确定转换矩阵；根据转换矩阵将初始坐标转换为当前时刻检测到的反光柱在全局坐标系下的预测坐标。

在一种实施方式中，根据转换矩阵将初始坐标转换为当前时刻检测到的反光柱在全局坐标系下的预测坐标的步骤，包括：按照如下公式计算预测坐标：

X_global＝T·X_local

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

在一种实施方式中，根据预测坐标和预先建立的KD树，查找与预测坐标匹配的目标反光柱的步骤，包括：根据预先建立的KD树在全局地标图中进行最邻近搜索，确定预测坐标的邻近坐标；计算预测坐标与邻近坐标之间的距离；判断距离是否小于阈值，如果小于，将邻近坐标对应的反光柱确定为目标反光柱。

在一种实施方式中，当激光雷达检测到当前时刻的数据信息时，根据预先获取的激光雷达在当前时刻的前两个时刻的位姿信息预测得到激光雷达当前时刻的预测位姿的步骤，包括：根据预先获取的激光雷达在当前时刻的前两个时刻的位姿信息，基于一阶欧拉法得到激光雷达当前时刻的预测位姿。

在一种实施方式中，激光雷达的载体的移动速度小于预设值，且载体保持匀速运动；载体包括AGV小车。