CN111596299A - 反光柱跟踪定位方法、装置及电子设备 - Google Patents

反光柱跟踪定位方法、装置及电子设备 Download PDF

Info

Publication number
CN111596299A
CN111596299A CN202010428074.0A CN202010428074A CN111596299A CN 111596299 A CN111596299 A CN 111596299A CN 202010428074 A CN202010428074 A CN 202010428074A CN 111596299 A CN111596299 A CN 111596299A
Authority
CN
China
Prior art keywords
predicted
pose
laser radar
coordinate
current moment
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010428074.0A
Other languages
English (en)
Other versions
CN111596299B (zh
Inventor
王玥
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Sany Robot Technology Co Ltd
Original Assignee
Sany Robot Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Sany Robot Technology Co Ltd filed Critical Sany Robot Technology Co Ltd
Priority to CN202010428074.0A priority Critical patent/CN111596299B/zh
Publication of CN111596299A publication Critical patent/CN111596299A/zh
Application granted granted Critical
Publication of CN111596299B publication Critical patent/CN111596299B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/66—Tracking systems using electromagnetic waves other than radio waves
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/005—Tree description, e.g. octree, quadtree

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Electromagnetism (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Graphics (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • Theoretical Computer Science (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

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

Description

反光柱跟踪定位方法、装置及电子设备
技术领域
本发明涉及激光雷达定位技术领域，尤其是涉及一种反光柱跟踪定位方法、装置及电子设备。
背景技术
在现代制造业中，生产阶段自动化因其可以显著提高生产效率、降低成本，其重要性与日俱增。自动导引车(Automated Guided Vehicle，AGV)作为自动化生产中的运输工具，AGV的导航定位也收到广泛关注。常见的工业AGV导航定位方式包括磁导航、自然导航(视觉SLAM或激光SLAM)、地标导航(二维码或RFID标签等)、激光导航(反光柱)等。其中基于反光柱(或称有反)激光导航由于其定位精度高、维护成本低、安装成本相对较低、AGV的路径规划柔性高等优点在叉车AGV上获得了大量应用。但是，现有的反光柱激光雷达定位算法在定位过程中时，通常采用基于反光柱间欧氏距离在全局坐标系与雷达坐标系内不变的原理，进行反光柱与全局地标地图的匹配搜索，进行全局搜索时未有效利用激光雷达位姿先验信息，全局匹配搜索计算量较大，速度较慢。
发明内容
有鉴于此，本发明的目的在于提供一种反光柱跟踪定位方法、装置及电子设备，以减少计算量，提高激光雷达当前时刻位姿的解算速度。
第一方面，本发明实施例提供了一种反光柱跟踪定位方法，包括：当激光雷达检测到当前时刻的数据信息时，根据预先获取的激光雷达在当前时刻的前两个时刻的位姿信息预测得到激光雷达当前时刻的预测位姿；其中，数据信息包括当前时刻检测到的反光柱在激光雷达坐标系下的初始坐标；根据预测位姿，确定当前时刻检测到的反光柱在全局坐标系下的预测坐标；根据预测坐标和预先建立的KD树，查找与预测坐标匹配的目标反光柱；其中，KD树为预先根据全局地标图中所有反光柱的位置信息建立的；根据目标反光柱确定激光雷达当前时刻的位姿，并更新激光雷达前两个时刻的位姿信息。
在一种实施方式中，根据预测位姿，确定当前时刻检测到的反光柱在全局坐标系下的预测坐标的步骤，包括：根据预测位姿确定转换矩阵；根据转换矩阵将初始坐标转换为当前时刻检测到的反光柱在全局坐标系下的预测坐标。
在一种实施方式中，根据转换矩阵将初始坐标转换为当前时刻检测到的反光柱在全局坐标系下的预测坐标的步骤，包括：按照如下公式计算预测坐标：
Xglobal＝T·Xlocal
其中，Xglobal表示预测坐标的齐次坐标矩阵，Xlocal表示初始坐标的齐次坐标矩阵，T表示转换矩阵。
在一种实施方式中，根据预测坐标和预先建立的KD树，查找与预测坐标匹配的目标反光柱的步骤，包括：根据预先建立的KD树在全局地标图中进行最邻近搜索，确定预测坐标的邻近坐标；计算预测坐标与邻近坐标之间的距离；判断距离是否小于阈值，如果小于，将邻近坐标对应的反光柱确定为目标反光柱。
在一种实施方式中，当激光雷达检测到当前时刻的数据信息时，根据预先获取的激光雷达在当前时刻的前两个时刻的位姿信息预测得到激光雷达当前时刻的预测位姿的步骤，包括：根据预先获取的激光雷达在当前时刻的前两个时刻的位姿信息，基于一阶欧拉法得到激光雷达当前时刻的预测位姿。
在一种实施方式中，激光雷达的载体的移动速度小于预设值，且载体保持匀速运动；载体包括AGV小车。
第二方面，本发明实施例提供了一种反光柱跟踪定位装置，包括：预测位姿确定模块，用于当激光雷达检测到当前时刻的数据信息时，根据预先获取的激光雷达在当前时刻的前两个时刻的位姿信息预测得到激光雷达当前时刻的预测位姿；其中，数据信息包括当前时刻检测到的反光柱在激光雷达坐标系下的初始坐标；坐标转换模块，用于根据预测位姿，确定当前时刻检测到的反光柱在全局坐标系下的预测坐标；查找模块，用于根据预测坐标和预先建立的KD树，查找与预测坐标匹配的目标反光柱；其中，KD树为预先根据全局地标图中所有反光柱的位置信息建立的；更新模块，用于根据目标反光柱确定激光雷达当前时刻的位姿，并更新激光雷达前两个时刻的位姿信息。
在一种实施方式中，坐标转换模块还用于根据预测位姿确定转换矩阵；根据转换矩阵将初始坐标转换为当前时刻检测到的反光柱在全局坐标系下的预测坐标。
第三方面，本发明实施例提供了一种电子设备，包括处理器和存储器，存储器存储有能够被处理器执行的计算机可执行指令，处理器执行计算机可执行指令以实现上述第一方面提供的任一项的方法的步骤。
第四方面，本发明实施例提供了一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，计算机程序被处理器运行时执行上述第一方面提供的任一项的方法的步骤。
本发明实施例提供的上述反光柱跟踪定位方法、装置及电子设备，包括：当激光雷达检测到当前时刻的数据信息(当前时刻检测到的反光柱在激光雷达坐标系下的初始坐标)时，根据预先获取的激光雷达在当前时刻的前两个时刻的位姿信息预测得到激光雷达当前时刻的预测位姿；然后根据预测位姿，确定当前时刻检测到的反光柱在全局坐标系下的预测坐标；之后根据预测坐标和预先建立的KD树(KD树为预先根据全局地标图中所有反光柱的位置信息建立的)，查找与预测坐标匹配的目标反光柱；最后根据目标反光柱确定激光雷达当前时刻的位姿，并更新激光雷达前两个时刻的位姿信息。上述方法能够利用激光雷达当前时刻的前两个时刻的位姿预测得到当前时刻激光雷达的位姿，计算量较小；同时，上述方法能够根据反光柱全局坐标预先建立KD树用于后续反光柱的查找，基于KD树能够快速查找到匹配的反光柱，从而能够减少计算量，进一步提高激光雷达当前时刻位姿的解算速度。
本发明的其他特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点在说明书、权利要求书以及附图中所特别指出的结构来实现和获得。
为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种反光柱跟踪定位方法的流程示意图；
图2为本发明实施例提供的另一种反光柱追踪定位方法的流程示意图；
图3为本发明实施例提供的一种反光柱跟踪定位装置的结构示意图；
图4为本发明实施例提供的一种电子设备的结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
目前，有反激光雷达定位的过程可按顺序划分为几个阶段：反光柱检测、反光柱匹配/反光柱跟踪、雷达位姿解算等。在反光柱检测阶段，定位算法从当前激光雷达原始扫描数据中检测可能存在的反光柱并求解其中心坐标，当获得了当前检测到的反光柱在雷达坐标系下的坐标后，需要在已知的反光柱全局坐标地图中找到对应的匹配，用于后续的雷达位姿解算步骤。根据是否已知上一时刻的雷达位姿，可以将反光柱的匹配步骤分为两类：全局匹配与位置跟踪。
在无任何雷达位姿先验信息时，需要对当前检测到的反光柱在全局坐标地图中进行模式匹配搜索，找到其正确对应的反光柱信息用于位姿求解。而当已知前一时刻雷达位姿时，可通过位置跟踪的方法寻找反光柱在全局坐标地图中的匹配，进而解算当前时刻雷达位姿。但是，现有的方法存在以下不足：(1)每次进行全局搜索时未有效利用激光雷达位姿先验信息；(2)全局匹配搜索计算量较大，耗时较长。(3)误检测的反光柱在全部检测结果中的比例上升时，算法解算时间过长，甚至无法找到真实反光柱的匹配；(4)未对检测到的所有真实反光柱找到正确匹配，导致有用的信息未被完全用于提高定位精度，浪费反光柱检测一步得到的结果。
基于此，本发明实施例提供的一种反光柱跟踪定位方法、装置及电子设备，可以减少计算量，提高激光雷达当前时刻位姿的解算速度。
为便于对本实施例进行理解，首先对本发明实施例所公开的一种反光柱跟踪定位方法进行详细介绍，参见图1所示的一种反光柱跟踪定位方法的流程示意图，该方法可以由安装有激光雷达的AGV小车、运输车辆等执行，主要包括以下步骤S102至步骤S108：
步骤S102：当激光雷达检测到当前时刻的数据信息时，根据预先获取的激光雷达在当前时刻的前两个时刻的位姿信息预测得到激光雷达当前时刻的预测位姿。
其中，数据信息包括当前时刻检测到的反光柱在激光雷达坐标系下的初始坐标。在一种实施方式中，激光雷达当前时刻tk的前两个时刻tk-1和tk-2的位姿信息可以预先获得，记作pose(tk-1)和pose(tk-2)；进一步，可以假设激光雷达的载体(诸如AGV小车等)的移动速度小于预设值，且载体保持匀速运动，也就是假设载体低速运动，且载体在tk-1至tk时段内的运动与tk-2至tk-1时段内的运动维持不变；当接收到当前时刻tk激光雷达扫描数据时，可以根据预先获得的激光雷达的前两个时刻的位姿信息，基于一阶欧拉法得到激光雷达当前时刻的预测位姿，具体可以根据以下公式得到当前时刻的预测位姿：
Figure BDA0002499224630000061
Figure BDA0002499224630000062
Figure BDA0002499224630000063
其中，(xpred(tk)，ypred(tk)，θpred(tk))表示激光雷达当前时刻的预测位姿，
Figure BDA0002499224630000064
分别表示激光雷达当前时刻的前两个时刻tk-1和tk-2的位姿。
步骤S104：根据预测位姿，确定当前时刻检测到的反光柱在全局坐标系下的预测坐标。
在一种实施方式中，可以根据激光雷达当前时刻检测到的反光柱信息确定反光柱在激光雷达坐标系下的位置信息，得到初始坐标；然后根据预测位姿，将初始坐标转换成全局坐标系下的预测坐标。
步骤S106：根据预测坐标和预先建立的KD树，查找与预测坐标匹配的目标反光柱。
其中，本发明实施例中预先测量了所有反光柱的位置信息，并将其存储在全局地标图中，根据全局地标图中所有反光柱的位置信息可以预先建立KD树(K-dimensiontree)，用于后续的查找工作。KD树是对数据点在k维空间(如二维(x，y)，三维(x，y，z)，k维(x，y，z...))中划分的一种数据结构，主要应用于多维空间关键数据的搜索，诸如范围搜索和最近邻搜索。在一种实施方式中，根据预测坐标和KD树查找与预测坐标匹配的目标反光柱，也就是对于预测的反光柱在全局坐标系下的坐标，利用构建的KD树查找其在全局地标地图中的最近邻反光柱(即目标反光柱)。
步骤S108：根据目标反光柱确定激光雷达当前时刻的位姿，并更新激光雷达前两个时刻的位姿信息。
具体的，可以根据目标反光柱的信息通过最小二乘法求解得到激光雷达当前时刻的位姿pose(tk)，并更新前两个时刻的位姿，也就是令pose(tk-2)＝pose(tk-1)、pose(tk-1)＝pose(tk)。
本发明实施例提供的上述方法能够利用激光雷达当前时刻的前两个时刻的位姿预测得到当前时刻激光雷达的位姿，计算量较小；同时，上述方法能够根据反光柱全局坐标预先建立KD树用于后续反光柱的查找，基于KD树能够快速查找到匹配的反光柱，从而能够减少计算量，进一步提高激光雷达当前时刻位姿的解算速度。
进一步，为了便于理解，对于上述步骤S104可以按照以下步骤a1至步骤a2来执行：
步骤a1：根据预测位姿确定转换矩阵。
在一种实施方式中，转换矩阵可以是：
Figure BDA0002499224630000081
其中，(xpred(tk)，ypred(tk)，θpred(tk))表示激光雷达当前时刻的预测位姿。
步骤a2：根据转换矩阵将初始坐标转换为当前时刻检测到的反光柱在全局坐标系下的预测坐标。
具体的，可以按照如下公式计算预测坐标：
Xglobal＝T·Xlocal
其中，Xglobal表示预测坐标的齐次坐标矩阵，Xlocal表示初始坐标的齐次坐标矩阵：
Figure BDA0002499224630000082
上述矩阵Xlocal的每一列对应一个检测到的反光柱在激光雷达坐标系下的齐次坐标。
进一步，上述求解得到的反光柱在全局坐标系下的预测坐标可以用来进行反光柱匹配，也就是上述步骤S106，具体可以按照以下步骤b1至步骤b3来执行：
步骤b1：根据预先建立的KD树在全局地标图中进行最邻近搜索，确定预测坐标的邻近坐标。
步骤b2：计算预测坐标与邻近坐标之间的距离。
步骤b3：判断距离是否小于阈值，如果小于，将邻近坐标对应的反光柱确定为目标反光柱。
在一种实施方式中，对于预测的反光柱在全局坐标系下的坐标，可以利用构建的KD树在全局地标图中进行最邻近搜索，查找其在全局地标地图中的最近邻，并获得两者的距离。对于每对匹配的反光柱，判断其距离是否小于阈值。阈值可以是当前反光柱距激光雷达距离的函数，即当前反光柱距激光雷达越近，阈值越小，反之阈值越大。由于距离激光雷达越远的反光柱，在当前位姿预测不准的情况下，位置预测的误差越大，因此要使其获得成功匹配需要更大的容忍度。具体的，阈值范围可以是0-5m，以阈值为0.5m为初始值，随后线性增长。对于距离小于阈值的匹配，将其存为成功匹配(也就是确定为目标反光柱)，用于解算后续激光雷达当前时刻位姿。
进一步，本发明实施例还提供了另一种反光柱追踪定位方法，参见图2所示的另一种反光柱追踪定位方法的流程示意图，示意出该方法主要包括以下步骤S202至步骤S212：
步骤S202：根据全局地标地图中所有地标构建KD树。
在一种实施方式中，本发明实施例提供的反光柱追踪定位方法可以包括两个阶段：准备阶段和运行阶段。准备阶段即为使用全局地标地图中存储的所有反光柱在全局坐标系下的位置信息构建KD树，用于后续最近邻查找工作。
步骤S204：根据预先确定的激光雷达当前时刻的前两个时刻的位姿，通过一阶欧拉法预测得到当前时刻雷达位姿。
具体的，假定当前时刻tk激光雷达扫描数据到来时，之前两帧的位姿信息已经被求出，也就是当前时刻tk的前两个时刻tk-1和tk-2的位姿信息已经预先获得，记作pose(tk-1)和pose(tk-2)；同时，由载体低速运动的假设，可以假定载体在tk-1至tk时段内的运动与tk-2至tk-1时段内的运动维持不变，因此当前时刻位姿(即预测位姿)可以由一阶欧拉法求出：
Figure BDA0002499224630000091
Figure BDA0002499224630000092
Figure BDA0002499224630000093
步骤S206：根据预测得到的当前时刻雷达位姿将检测到的反光柱在激光雷达坐标系下的坐标转换到全局坐标系。
在一种实施方式中，可以根据当前预测位姿，将当前检测到的反光柱在全局坐标系下的坐标求解出来。假定当前检测到的反光柱在激光雷达坐标系下的齐次坐标矩阵为：
Figure BDA0002499224630000101
该矩阵每一列对应一个检测到的反光柱在激光雷达坐标系下的齐次坐标。根据当前预测位姿可以得到转换矩阵如下所示：
Figure BDA0002499224630000102
则当前检测到的反光柱在全局坐标系下的齐次坐标矩阵为：
Xglobal＝T·Xlocal
该矩阵每一列对应一个检测到的反光柱在全局坐标系下的齐次坐标，且前述反光柱在全局坐标系下的坐标为预测值，用于后续进行反光柱匹配。
步骤S208：对每个反光柱全局坐标的预测值，在全局地标地图KD树内进行最邻近搜索，确定匹配结果。
步骤S210：将每对匹配之间的距离与阈值进行比较。
在一种实施方式中，步骤S208至步骤S210也可以称为反光柱匹配，具体原理与过程与前述步骤b1至步骤b3相同，在此不再赘述。
步骤S212：将距离小于阈值的匹配记为成功匹配，并根据成功匹配的反光柱解算激光雷达当前位姿，更新缓存的前两个时刻位姿。
本发明实施例提供的上述反光柱跟踪定位方法，在准备阶段中，根据全局地标地图中存储的反光柱全局坐标构建KD树，在后续最近邻搜索步骤中可加速解算；其次，在激光雷达低速运动的假设下，利用先前时刻激光雷达的位姿，采用一阶欧拉法进行位姿预测估计递推得到当前时刻雷达位姿的预测值，计算量低，预测值相对精确；此外，上述方法仅使用激光雷达的数据进行位姿预测，无需其他传感器测量结果，操作便捷；同时，使用向量化的反光柱在雷达坐标系下的齐次坐标矩阵求解反光柱在全局坐标系下的坐标，解算便捷；然后，通过最近邻搜索的方法能够快速在全局地标地图中找到相应匹配，保证了无论误检测反光柱比例如何，均可对所有真实反光柱找到对应匹配，充分利用了反光柱检测步骤的结果，提高了算法的鲁棒性；最后，利用与距离相关的阈值作为匹配正确与否的筛选条件，提高了对误检测反光柱的剔除率，增强算法鲁棒性。综上所述，本发明实施例提供的上述方法相比于传统的全局搜索反光柱匹配的方案，解算速度高，鲁棒性强，在连续跟踪过程中，仅需检测到最少两个真实反光柱，便可找到其对应匹配，之后可通过最小二乘法获得当前位姿；同时充分利用了当前检测到的真实反光柱的冗余信息，找到所有正确匹配，从而提高位姿解算精度。
对于前述实施例提供的反光柱跟踪定位方法，本发明实施例还提供了一种反光柱跟踪定位装置，参见图3所示的一种反光柱跟踪定位装置的结构示意图，该装置可以包括以下部分：
预测位姿确定模块301，用于当激光雷达检测到当前时刻的数据信息时，根据预先获取的激光雷达在当前时刻的前两个时刻的位姿信息预测得到激光雷达当前时刻的预测位姿；其中，数据信息包括当前时刻检测到的反光柱在激光雷达坐标系下的初始坐标。
坐标转换模块302，用于根据预测位姿，确定当前时刻检测到的反光柱在全局坐标系下的预测坐标。
查找模块303，用于根据预测坐标和预先建立的KD树，查找与预测坐标匹配的目标反光柱；其中，KD树为预先根据全局地标图中所有反光柱的位置信息建立的。
更新模块304，用于根据目标反光柱确定激光雷达当前时刻的位姿，并更新激光雷达前两个时刻的位姿信息。
本发明实施例提供的上述装置能够利用激光雷达当前时刻的前两个时刻的位姿预测得到当前时刻激光雷达的位姿，计算量较小；同时，上述装置能够根据反光柱全局坐标预先建立KD树用于后续反光柱的查找，基于KD树能够快速查找到匹配的反光柱，从而能够减少计算量，进一步提高激光雷达当前时刻位姿的解算速度。
在一种实施方式中，上述坐标转换模块302还用于根据预测位姿确定转换矩阵；根据转换矩阵将初始坐标转换为当前时刻检测到的反光柱在全局坐标系下的预测坐标。
在一种实施方式中，上述坐标转换模块302进一步还用于按照如下公式计算预测坐标：
Xglobal＝T·Xlocal
其中，Xglobal表示预测坐标的齐次坐标矩阵，Xlocal表示初始坐标的齐次坐标矩阵，T表示转换矩阵。
在一种实施方式中，上述查找模块303还用于根据预先建立的KD树在全局地标图中进行最邻近搜索，确定预测坐标的邻近坐标；计算预测坐标与邻近坐标之间的距离；判断距离是否小于阈值，如果小于，将邻近坐标对应的反光柱确定为目标反光柱。
在一种实施方式中，上述预测位姿确定模块301还用于根据预先获取的激光雷达在当前时刻的前两个时刻的位姿信息，基于一阶欧拉法得到激光雷达当前时刻的预测位姿。
在一种实施方式中，上述装置中激光雷达的载体的移动速度小于预设值，且载体保持匀速运动；载体包括AGV小车。
本发明实施例所提供的装置，其实现原理及产生的技术效果和前述方法实施例相同，为简要描述，装置实施例部分未提及之处，可参考前述方法实施例中相应内容。
本发明实施例还提供了一种电子设备，具体的，该电子设备包括处理器和存储装置；存储装置上存储有计算机程序，计算机程序在被处理器运行时执行如上实施方式的任一项所述的方法。
图4为本发明实施例提供的一种电子设备的结构示意图，该电子设备100包括：处理器40，存储器41，总线42和通信接口43，所述处理器40、通信接口43和存储器41通过总线42连接；处理器40用于执行存储器41中存储的可执行模块，例如计算机程序。
其中，存储器41可能包含高速随机存取存储器(Random Access Memory，RAM)，也可能还包括非不稳定的存储器(non-volatile memory)，例如至少一个磁盘存储器。通过至少一个通信接口43(可以是有线或者无线)实现该系统网元与至少一个其他网元之间的通信连接，可以使用互联网，广域网，本地网，城域网等。
总线42可以是ISA总线、PCI总线或EISA总线等。所述总线可以分为地址总线、数据总线、控制总线等。为便于表示，图4中仅用一个双向箭头表示，但并不表示仅有一根总线或一种类型的总线。
其中，存储器41用于存储程序，所述处理器40在接收到执行指令后，执行所述程序，前述本发明实施例任一实施例揭示的过程定义的装置所执行的方法可以应用于处理器40中，或者由处理器40实现。
处理器40可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器40中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器40可以是通用处理器，包括中央处理器(Central Processing Unit，CPU)、网络处理器(Network Processor，NP)等；还可以是数字信号处理器(Digital Signal Processing，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本发明实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器41，处理器40读取存储器41中的信息，结合其硬件完成上述方法的步骤。
本发明实施例所提供的可读存储介质的计算机程序产品，包括存储了程序代码的计算机可读存储介质，所述程序代码包括的指令可用于执行前面方法实施例中所述的方法，具体实现可参见前述方法实施例，在此不再赘述。
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
最后应说明的是：以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。

Claims (10)

1.一种反光柱跟踪定位方法，其特征在于，包括：
当激光雷达检测到当前时刻的数据信息时，根据预先获取的所述激光雷达在所述当前时刻的前两个时刻的位姿信息预测得到所述激光雷达当前时刻的预测位姿；其中，所述数据信息包括当前时刻检测到的反光柱在激光雷达坐标系下的初始坐标；
根据所述预测位姿，确定所述当前时刻检测到的反光柱在全局坐标系下的预测坐标；
根据所述预测坐标和预先建立的KD树，查找与所述预测坐标匹配的目标反光柱；其中，所述KD树为预先根据全局地标图中所有所述反光柱的位置信息建立的；
根据所述目标反光柱确定所述激光雷达当前时刻的位姿，并更新所述激光雷达前两个时刻的位姿信息。
2.根据权利要求1所述的方法，其特征在于，所述根据所述预测位姿，确定当前时刻检测到的反光柱在全局坐标系下的预测坐标的步骤，包括：
根据所述预测位姿确定转换矩阵；
根据转换矩阵将所述初始坐标转换为当前时刻检测到的反光柱在全局坐标系下的预测坐标。
3.根据权利要求2所述的方法，其特征在于，所述根据转换矩阵将所述初始坐标转换为当前时刻检测到的反光柱在全局坐标系下的预测坐标的步骤，包括：
按照如下公式计算所述预测坐标：
Xglobal＝T·Xlocal
其中，Xglobal表示预测坐标的齐次坐标矩阵，Xlocal表示初始坐标的齐次坐标矩阵，T表示转换矩阵。
4.根据权利要求1所述的方法，其特征在于，根据所述预测坐标和预先建立的KD树，查找与所述预测坐标匹配的目标反光柱的步骤，包括：
根据预先建立的KD树在所述全局地标图中进行最邻近搜索，确定所述预测坐标的邻近坐标；
计算所述预测坐标与所述邻近坐标之间的距离；
判断所述距离是否小于阈值，如果小于，将所述邻近坐标对应的反光柱确定为目标反光柱。
5.根据权利要求1所述的方法，其特征在于，所述当激光雷达检测到当前时刻的数据信息时，根据预先获取的所述激光雷达在所述当前时刻的前两个时刻的位姿信息预测得到所述激光雷达当前时刻的预测位姿的步骤，包括：
根据预先获取的所述激光雷达在所述当前时刻的前两个时刻的位姿信息，基于一阶欧拉法得到所述激光雷达当前时刻的预测位姿。
6.根据权利要求1所述的方法，其特征在于，所述激光雷达的载体的移动速度小于预设值，且所述载体保持匀速运动；所述载体包括AGV小车。
7.一种反光柱跟踪定位装置，其特征在于，包括：
预测位姿确定模块，用于当激光雷达检测到当前时刻的数据信息时，根据预先获取的所述激光雷达在所述当前时刻的前两个时刻的位姿信息预测得到所述激光雷达当前时刻的预测位姿；其中，所述数据信息包括当前时刻检测到的反光柱在激光雷达坐标系下的初始坐标；
坐标转换模块，用于根据所述预测位姿，确定所述当前时刻检测到的反光柱在全局坐标系下的预测坐标；
查找模块，用于根据所述预测坐标和预先建立的KD树，查找与所述预测坐标匹配的目标反光柱；其中，所述KD树为预先根据全局地标图中所有所述反光柱的位置信息建立的；
更新模块，用于根据所述目标反光柱确定所述激光雷达当前时刻的位姿，并更新所述激光雷达前两个时刻的位姿信息。
8.根据权利要求7所述的装置，其特征在于，所述坐标转换模块还用于根据所述预测位姿确定转换矩阵；
根据转换矩阵将所述初始坐标转换为当前时刻检测到的反光柱在全局坐标系下的预测坐标。
9.一种电子设备，其特征在于，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的计算机可执行指令，所述处理器执行所述计算机可执行指令以实现权利要求1至6任一项所述的方法的步骤。
10.一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，其特征在于，所述计算机程序被处理器运行时执行上述权利要求1至6任一项所述的方法的步骤。
CN202010428074.0A 2020-05-19 2020-05-19 反光柱跟踪定位方法、装置及电子设备 Active CN111596299B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010428074.0A CN111596299B (zh) 2020-05-19 2020-05-19 反光柱跟踪定位方法、装置及电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010428074.0A CN111596299B (zh) 2020-05-19 2020-05-19 反光柱跟踪定位方法、装置及电子设备

Publications (2)

Publication Number Publication Date
CN111596299A true CN111596299A (zh) 2020-08-28
CN111596299B CN111596299B (zh) 2022-09-30

Family

ID=72189783

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010428074.0A Active CN111596299B (zh) 2020-05-19 2020-05-19 反光柱跟踪定位方法、装置及电子设备

Country Status (1)

Country Link
CN (1) CN111596299B (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112285725A (zh) * 2020-10-10 2021-01-29 杭州电子科技大学 一种基于激光雷达的单移动机器人室内定位方法
CN113375657A (zh) * 2021-04-26 2021-09-10 北京迈格威科技有限公司 电子地图的更新方法、装置和电子设备
CN113466878A (zh) * 2021-06-21 2021-10-01 三一专用汽车有限责任公司 作业机械的定位方法、装置、作业机械及电子设备
CN114593738A (zh) * 2022-05-09 2022-06-07 山东大学 基于八叉树搜索反光柱的机器人全局定位方法及系统
CN115586511A (zh) * 2022-11-25 2023-01-10 唐山百川工业服务有限公司 一种基于阵列立柱的激光雷达二维定位方法
WO2023005020A1 (zh) * 2021-07-30 2023-02-02 深圳市优必选科技股份有限公司 反光板定位方法、机器人及计算机可读存储介质

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1361433A (zh) * 2000-12-23 2002-07-31 林清芳 运载体的全融合定位方法
US20110097014A1 (en) * 2009-10-26 2011-04-28 Industrial Technology Research Institute Self-positioning device and method thereof
CN103279551A (zh) * 2013-06-06 2013-09-04 浙江大学 一种基于欧氏距离的高维数据准确近邻快速检索方法
CN105890593A (zh) * 2016-04-06 2016-08-24 浙江大学 一种mems惯性导航系统及基于该系统的轨迹重构方法
CN107123138A (zh) * 2017-04-28 2017-09-01 电子科技大学 基于vanilla‑R点对剔除策略的点云配准算法
CN108571971A (zh) * 2018-05-17 2018-09-25 北京航空航天大学 一种agv视觉定位系统及方法
EP3495837A1 (fr) * 2017-11-21 2019-06-12 Thales Procédé et système d'estimation de position par recalage collaboratif sur amers inconnus
CN110275153A (zh) * 2019-07-05 2019-09-24 上海大学 一种基于激光雷达的水面目标检测与跟踪方法
CN110471072A (zh) * 2019-08-19 2019-11-19 华晟（青岛）智能装备科技有限公司 一种反光柱位置辨识方法及系统
CN110530368A (zh) * 2019-08-22 2019-12-03 浙江大华技术股份有限公司 一种机器人定位方法及设备
CN110793512A (zh) * 2019-09-11 2020-02-14 上海宾通智能科技有限公司 位姿识别方法、装置、电子设备和存储介质
CN110838146A (zh) * 2019-10-14 2020-02-25 武汉中观自动化科技有限公司 一种共面交比约束的同名点匹配方法、系统、装置及介质
CN111108342A (zh) * 2016-12-30 2020-05-05 迪普迈普有限公司 用于高清地图创建的视觉测程法和成对对准

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1361433A (zh) * 2000-12-23 2002-07-31 林清芳 运载体的全融合定位方法
US20110097014A1 (en) * 2009-10-26 2011-04-28 Industrial Technology Research Institute Self-positioning device and method thereof
CN103279551A (zh) * 2013-06-06 2013-09-04 浙江大学 一种基于欧氏距离的高维数据准确近邻快速检索方法
CN105890593A (zh) * 2016-04-06 2016-08-24 浙江大学 一种mems惯性导航系统及基于该系统的轨迹重构方法
CN111108342A (zh) * 2016-12-30 2020-05-05 迪普迈普有限公司 用于高清地图创建的视觉测程法和成对对准
CN107123138A (zh) * 2017-04-28 2017-09-01 电子科技大学 基于vanilla‑R点对剔除策略的点云配准算法
EP3495837A1 (fr) * 2017-11-21 2019-06-12 Thales Procédé et système d'estimation de position par recalage collaboratif sur amers inconnus
CN108571971A (zh) * 2018-05-17 2018-09-25 北京航空航天大学 一种agv视觉定位系统及方法
CN110275153A (zh) * 2019-07-05 2019-09-24 上海大学 一种基于激光雷达的水面目标检测与跟踪方法
CN110471072A (zh) * 2019-08-19 2019-11-19 华晟（青岛）智能装备科技有限公司 一种反光柱位置辨识方法及系统
CN110530368A (zh) * 2019-08-22 2019-12-03 浙江大华技术股份有限公司 一种机器人定位方法及设备
CN110793512A (zh) * 2019-09-11 2020-02-14 上海宾通智能科技有限公司 位姿识别方法、装置、电子设备和存储介质
CN110838146A (zh) * 2019-10-14 2020-02-25 武汉中观自动化科技有限公司 一种共面交比约束的同名点匹配方法、系统、装置及介质

Non-Patent Citations (10)

* Cited by examiner, † Cited by third party
Title
AIXUE YE: "A vision-based guidance method for autonomous guided vehicles", 《2012 IEEE INTERNATIONAL CONFERENCE ON MECHATRONICS AND AUTOMATION》 *
KURIAN, A: "A FAST AND FLEXIBLE METHOD FOR META-MAP BUILDING FOR ICP BASED SLAM", 《23RD CONGRESS OF THE INTERNATIONAL-SOCIETY-FOR-PHOTOGRAMMETRY-AND-REMOTE-SENSING (ISPRS)》 *
LIJIN YANG: "K-Nearest Neighbor Model Based Short-Term Traffic Flow Prediction Method", 《2019 18TH INTERNATIONAL SYMPOSIUM ON DISTRIBUTED COMPUTING AND APPLICATIONS FOR BUSINESS ENGINEERING AND SCIENCE (DCABES)》 *
MENG YU: "Registration of point clouds using sample-sphere and adaptive distance restriction", 《THE VISUAL COMPUTER》 *
QINGMING ZHAN: "Locality sensitive hashing for LiDAR data processing", 《PROCEEDINGS OF THE 2011 INTERNATIONAL CONFERENCE ON REMOTE SENSING, ENVIRONMENT AND TRANSPORTATION ENGINEERING (RSETE 2011)》 *
吴禄慎等: "基于特征点的改进ICP三维点云配准技术", 《南昌大学学报(工科版)》 *
孟禹: "《基于采样球和ICP算法的点云配准方法研究》", 《中国优秀硕博士学位论文（硕士） 信息科技辑》 *
李俊杰等: "基于人工地标的移动机器人定位与调整技术", 《航空制造技术》 *
汪思迪等: "基于惯性导航、RFID及图像识别的AGV融合导航系统", 《起重运输机械》 *
蒋荣华: "地面三维激光扫描点云配准研究综述", 《科技创新与生产力》 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112285725A (zh) * 2020-10-10 2021-01-29 杭州电子科技大学 一种基于激光雷达的单移动机器人室内定位方法
CN112285725B (zh) * 2020-10-10 2023-11-07 杭州电子科技大学 一种基于激光雷达的单移动机器人室内定位方法
CN113375657A (zh) * 2021-04-26 2021-09-10 北京迈格威科技有限公司 电子地图的更新方法、装置和电子设备
CN113466878A (zh) * 2021-06-21 2021-10-01 三一专用汽车有限责任公司 作业机械的定位方法、装置、作业机械及电子设备
WO2023005020A1 (zh) * 2021-07-30 2023-02-02 深圳市优必选科技股份有限公司 反光板定位方法、机器人及计算机可读存储介质
CN114593738A (zh) * 2022-05-09 2022-06-07 山东大学 基于八叉树搜索反光柱的机器人全局定位方法及系统
CN114593738B (zh) * 2022-05-09 2022-07-26 山东大学 基于八叉树搜索反光柱的机器人全局定位方法及系统
CN115586511A (zh) * 2022-11-25 2023-01-10 唐山百川工业服务有限公司 一种基于阵列立柱的激光雷达二维定位方法
CN115586511B (zh) * 2022-11-25 2023-03-03 唐山百川工业服务有限公司 一种基于阵列立柱的激光雷达二维定位方法

Also Published As

Publication number Publication date
CN111596299B (zh) 2022-09-30

Similar Documents

Publication Publication Date Title
CN111596299B (zh) 反光柱跟踪定位方法、装置及电子设备
CN113376650B (zh) 移动机器人定位方法及装置、电子设备及存储介质
CN111536964A (zh) 机器人定位方法及装置、存储介质
CN108638062B (zh) 机器人定位方法、装置、定位设备及存储介质
JP5385105B2 (ja) 画像検索方法およびシステム
US11328516B2 (en) Apparatus and method for associating sensor data in vehicle
CN112348035B (zh) 车辆关键点检测方法、装置及电子设备
CN108363060A (zh) 一种动态障碍物检测方法及设备
Mersch et al. Building volumetric beliefs for dynamic environments exploiting map-based moving object segmentation
CN111352118A (zh) 反光柱的匹配方法、装置、激光雷达定位方法和设备终端
CN112612034B (zh) 基于激光帧和概率地图扫描的位姿匹配方法
CN117075097B (zh) 一种基于扩展目标簇划分的海事雷达目标跟踪方法及系统
CN113936045A (zh) 路侧激光雷达点云配准方法及装置
CN113375657A (zh) 电子地图的更新方法、装置和电子设备
CN111142101B (zh) 一种数据关联方法
CN116309628A (zh) 车道线识别方法和装置、电子设备和计算机可读存储介质
CN113203424B (zh) 多传感器的数据融合方法、装置及相关设备
WO2022252482A1 (zh) 机器人及其环境地图构建方法和装置
CN114964204A (zh) 地图构建方法、地图使用方法、装置、设备和存储介质
CN112305520B (zh) 单激光雷达的反光柱检测位置的修正方法和装置
CN113625249A (zh) 反光板定位方法、机器人及计算机可读存储介质
CN116134488A (zh) 点云标注方法、装置、计算机设备和存储介质
Lin et al. Map matching based on Cell-ID localization for mobile phone users
CN112924519B (zh) 一种自动寻峰谷方法、装置、介质及电子设备
CN114688992B (zh) 一种反光物的识别方法、装置、电子设备及存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant