CN111443359B - 定位方法、装置及设备 - Google Patents

定位方法、装置及设备 Download PDF

Info

Publication number
CN111443359B
CN111443359B CN202010225362.6A CN202010225362A CN111443359B CN 111443359 B CN111443359 B CN 111443359B CN 202010225362 A CN202010225362 A CN 202010225362A CN 111443359 B CN111443359 B CN 111443359B
Authority
CN
China
Prior art keywords
point cloud
cloud data
laser radar
local map
scanning
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.)
Active
Application number
CN202010225362.6A
Other languages
English (en)
Other versions
CN111443359A (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.)
Cloudminds Robotics Co Ltd
Original Assignee
Cloudminds Shanghai Robotics 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 Cloudminds Shanghai Robotics Co Ltd filed Critical Cloudminds Shanghai Robotics Co Ltd
Priority to CN202010225362.6A priority Critical patent/CN111443359B/zh
Publication of CN111443359A publication Critical patent/CN111443359A/zh
Application granted granted Critical
Publication of CN111443359B publication Critical patent/CN111443359B/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/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/481—Constructional features, e.g. arrangements of optical elements
    • G01S7/4817—Constructional features, e.g. arrangements of optical elements relating to scanning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Theoretical Computer Science (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Computation (AREA)
  • Evolutionary Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明实施例涉及定位技术领域，公开了一种定位方法、装置及设备。其中，所述方法包括：获取激光雷达上一次扫描周围环境的第一点云数据；对所述第一点云数据进行预处理，所述预处理包括运动畸变去除、滤波以及去除动态物体；根据所述预处理后的所述第一点云数据，更新局部地图；获取所述激光雷达当前扫描所述周围环境的第二点云数据；对所述第二点云数据和更新后的所述局部地图进行匹配，以确定所述激光雷达在所述当前扫描时的位姿。通过上述方式，本发明实施例能够提高定位的精度。

Description

定位方法、装置及设备
技术领域
本发明实施例涉及定位技术领域，具体涉及一种定位方法、装置及设备。
背景技术
激光雷达传感器在自动驾驶、智能机器人等领域有着广泛的应用。例如，当激光雷达安装在机器人上时，激光雷达能够根据当前环境生成激光点云数据，创建当前环境的地图，并利用激光点云数据进行定位，确定机器人的位姿。但是，目前基于激光雷达的定位方法的定位精度并不高。
发明内容
本发明实施例一个目的旨在提供一种定位方法、装置及设备，能够提高定位的精度。
根据本发明实施例的第一方面，提供了一种定位方法，包括：获取激光雷达上一次扫描周围环境的第一点云数据；对所述第一点云数据进行预处理，所述预处理包括运动畸变去除、滤波以及去除动态物体；根据所述预处理后的所述第一点云数据，更新局部地图；获取所述激光雷达当前扫描周围环境的第二点云数据；对所述第二点云数据和更新后的所述局部地图进行匹配，以确定所述激光雷达在所述当前扫描时的位姿。
在一种可选的方式中，所述对所述第一点云数据进行预处理，具体包括：确定所述上一次扫描中的起始时刻以及除所述起始时刻以外的其他时刻；计算所述其他时刻和所述起始时刻之间的时间间隔；根据所述时间间隔以及所述激光雷达在所述其他时刻时的旋转速度和平移速度，对所述第一点云数据进行运动畸变去除。
在一种可选的方式中，在所述对所述第一点云数据进行运动畸变去除之后，所述对所述第一点云数据进行预处理，具体还包括：对所述第一点云数据进行滤波；确定滤波后的所述第一点云数据中的动态物体，并去除所述动态物体。
在一种可选的方式中，所述确定滤波后的所述第一点云数据中的动态物体，具体包括：对滤波后的所述第一点云数据进行聚类，以确定所述第一点云数据中的物体；若所述第一点云数据中的物体的长、宽、高小于预设长、宽、高阈值时，则将所述物体确定为所述动态物体。
在一种可选的方式中，所述根据所述预处理后的所述第一点云数据，更新局部地图，具体包括：根据所述预处理后的所述第一点云数据和所述局部地图，计算所述预处理后的所述第一点云数据和所述局部地图之间的变换矩阵；根据所述预处理后的所述第一点云数据和所述局部地图之间的变换矩阵，将所述预处理后的所述第一点云数据和所述局部地图变换到相同坐标系下；对变换后的所述第一点云数据和所述局部地图进行合并和滤波，以得到更新后的所述局部地图。
在一种可选的方式中，所述对所述第二点云数据和更新后的所述局部地图进行匹配，以确定所述激光雷达在所述当前扫描时的位姿，具体包括：根据所述第二点云数据和更新后的所述局部地图，基于迭代最近点算法确定所述第二点云数据和更新后的所述局部地图之间的变换关系；根据所述第二点云数据和更新后的所述局部地图之间的变换矩阵，确定所述激光雷达在所述当前扫描时的位姿。
在一种可选的方式中，所述方法还包括：根据所述激光雷达在所述上一次扫描时的第一平移速度、所述激光雷达在所述当前扫描时的第二平移速度，计算所述激光雷达的加速度；若所述加速度大于预设加速度阈值，则根据所述第一平移速度以及所述上一次扫描和所述当前扫描之间的时间差，调整确定的所述在所述当前扫描时的位姿。
根据本发明实施例的第二方面，提供了一种定位装置，包括：第一获取模块，用于获取激光雷达上一次扫描周围环境的第一点云数据；预处理模块，用于对所述第一点云数据进行预处理，所述预处理包括运动畸变去除、滤波以及去除动态物体；更新模块，用于根据所述预处理后的所述第一点云数据，更新局部地图；第二获取模块，用于获取所述激光雷达当前扫描周围环境的第二点云数据；位姿确定模块，用于对所述第二点云数据和更新后的所述局部地图进行匹配，以确定所述激光雷达在所述当前扫描时的位姿。
根据本发明实施例的第三方面，提供了一种定位设备，包括：处理器和存储器，所述存储器用于存放至少一可执行指令，当所述定位设备运行时，所述处理器执行所述可执行指令，使所述处理器执行如上所述的定位方法的操作。
根据本发明实施例的第四方面，提供了一种计算机可读存储介质，所述存储介质中存储有至少一可执行指令，所述可执行指令使处理器执行根据如上所述的定位方法的步骤。
本发明实施例通过获取激光雷达上一次扫描周围环境的第一点云数据，对所述第一点云数据进行预处理，所述预处理包括运动畸变去除、滤波以及去除动态物体，根据预处理后的第一点云数据，更新局部地图，获取激光雷达当前扫描周围环境的第二点云数据，对第二点云数据和更新后的局部地图进行匹配，以确定激光雷达在当前扫描时的位姿，能够通过第一点云数据对局部地图进行补充，完善局部地图的几何观测信息，从而能够提高定位的精度。
上述说明仅是本发明实施例技术方案的概述，为了能够更清楚了解本发明实施例的技术手段，而可依照说明书的内容予以实施，并且为了让本发明实施例的上述和其它目的、特征和优点能够更明显易懂，以下特举本发明的具体实施方式。
附图说明
通过阅读下文优选实施方式的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。而且在整个附图中，用相同的参考符号表示相同的部件。在附图中：
图1示出了本发明实施例提供的一种定位方法的流程示意图；
图2示出了本发明另一实施例提供的一种定位方法的流程示意图；
图3示出了本发明又一实施例提供的一种定位方法的流程示意图；
图4示出了本发明实施例提供的一种定位装置的结构示意图；
图5示出了本发明实施例提供的计算设备的结构示意图。
具体实施方式
下面将参照附图更详细地描述本发明的示例性实施例。虽然附图中显示了本发明的示例性实施例，然而应当理解，可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本发明，并且能够将本发明的范围完整的传达给本领域的技术人员。
激光雷达传感器在自动驾驶、智能机器人等领域有着广泛的应用。例如，当激光雷达安装在机器人上时，激光雷达能够根据当前环境生成激光点云数据，创建当前环境的地图，并利用激光点云数据进行定位，确定机器人的位姿。但是，目前基于激光雷达的定位方法的定位精度并不高。
基于此，本发明实施例提供一种定位方法、装置及设备，能够提高定位的精度。
具体地，下面结合附图，对本发明实施例作进一步阐述。
其中，应当理解的是，本发明提供的下述实施例之间，只要不冲突，均可相互结合以形成新的实施方式。
图1示出了本发明实施例提供的一种定位方法的流程示意图。该定位方法可以应用于定位设备。如图1所示，该定位方法包括：
步骤110、获取激光雷达上一次扫描周围环境的第一点云数据。
其中，激光雷达可以单线激光雷达或者多线激光雷达。激光雷达可以设置在移动平台上，从而随着移动平台的运动而运动。移动平台可以为移动的物体，例如车辆、机器人、飞行器等等，本实施例不对移动平台的结构作具体限定。
其中，获取激光雷达的周围环境的激光点云数据的方式，可以为：激光雷达向周围环境发射一束或者多束激光进行扫描，并在接收到周围环境反射的激光后，根据该激光所携带的数据描绘出周围环境，从而激光雷达生成激光点云数据，从而能够获取激光雷达生成的激光点云数据。其中，激光点云数据为三维数据。
其中，上一次扫描是指激光雷达完成一次扫描，例如激光雷达旋转360°，此时，激光雷达生成一帧激光点云数据。则第一点云数据是指激光雷达在上一次扫描中获取的激光点云数据。例如，激光雷达从第0秒到第1秒旋转了10圈，则激光雷达完成了10次扫描，生成10帧激光点云数据，激光雷达在第0秒时从0°开始旋转，在第50毫秒时旋转到180°，在第100毫秒时旋转到360°，则激光雷达从第0秒到第100毫秒完成了上一次扫描，则第一点云数据为激光雷达从第0秒到第100毫秒获取的激光点云数据。
步骤120、对第一点云数据进行预处理，预处理包括运动畸变去除、滤波以及去除动态物体。
其中，对第一点云数据进行预处理，具体可以为：先对第一点云数据进行运动畸变去除，然后对第一点云数据进行滤波，确定滤波后的第一点云数据中的动态物体，并去除动态物体。通过对第一点云数据进行预处理，能够得到更有效的环境信息。
步骤130、根据预处理后的第一点云数据，更新局部地图。
其中，局部地图为激光雷达在历史扫描中获取的周围环境的地图。例如，历史扫描可以为激光雷达在某个环境首次启用的时刻，当激光雷达在该个环境首次启用时，激光雷达对该环境进行识别，生成该环境的激光点云数据，则将此时的激光点云数据作为局部地图。
在本实施例中，激光雷达上一次扫描周围环境获取的第一点云数据并不是激光雷达首次扫描周围环境获取的激光点云数据，即在上一次扫描之前，激光雷达就已经对周围环境进行过扫描，生成了激光点云数据(即局部地图)，因此能够获取到周围环境的局部地图。则在步骤130中，获取到局部地图后，通过第一点云数据对局部地图进行更新。
具体地，步骤130可以包括：
步骤131、根据预处理后的第一点云数据和局部地图，计算预处理后的第一点云数据和局部地图之间的变换矩阵；
步骤132、根据预处理后的第一点云数据和局部地图之间的变换矩阵，将预处理后的第一点云数据和局部地图变换到相同坐标系下；
步骤133、对变换后的第一点云数据和局部地图进行合并和滤波，以得到更新后的局部地图。
在步骤131中，可以通过获取激光雷达完成上一次扫描时的平移速度，计算激光雷达在上一次扫描时的位姿，从而根据激光雷达在上一次扫描时的位姿计算得到预处理后的第一点云数据和局部地图之间的变换矩阵。
其中，相同坐标系可以为第一点云数据的坐标系，也可以为局部地图的坐标系，还可以为世界坐标系，等等。则在步骤132中，将预处理后的第一点云数据和局部地图变换到相同坐标系下，具体可以为：根据预处理后的第一点云数据和局部地图之间的变换矩阵，对预处理后的第一点云数据进行旋转和平移，将预处理后的第一点云数据变换到局部地图坐标系下，从而使预处理后的第一点云数据和局部地图在相同坐标系下。
在步骤133中，具体可以为：将在相同坐标系下的第一点云数据和局部地图进行合并，然后在保持局部地图尺寸固定的情况下，对第一点云数据和局部地图合并后的地图进行修剪，并通过统计滤波和体素滤波对修剪后的地图进行处理，从而得到更新后的局部地图。通过统计滤波和体素滤波对地图进行处理，能够使得地图更加准确。
步骤140、获取激光雷达当前扫描周围环境的第二点云数据。
其中，当前扫描是指与上一次扫描相邻的下一次扫描。第二点云数据是指激光雷达在当前扫描中获取的激光点云数据。例如，激光雷达从第0秒到第100毫秒完成了上一次扫描，生成第一点云数据，激光雷达从第100毫秒到第200毫秒完成了当前扫描，生成第二点云数据。又例如，激光雷达在第一帧内完成了上一次扫描，生成第一点云数据，激光雷达在第二帧内完成了当前扫描，生成第二点云数据。
需要说明的是，步骤130和步骤140可以同时执行，也可以先后执行，例如，先执行步骤130再执行步骤140，或者，先执行步骤140再执行步骤130。
步骤150、对第二点云数据和更新后的局部地图进行匹配，以确定激光雷达在当前扫描时的位姿。
具体地，步骤150包括：
步骤151、根据第二点云数据和更新后的局部地图，基于迭代最近点算法确定第二点云数据和更新后的局部地图之间的变换关系；
步骤152、根据第二点云数据和更新后的局部地图之间的变换关系，确定激光雷达在当前扫描时的位姿。
其中，用迭代最近点(Iterative Closest Point，ICP)算法进行第二点云数据和更新后的局部地图的匹配，从而计算得到第二点云数据和更新后的局部地图之间的变换关系。ICP算法具体实施方式可以为：一、在第二点云数据中选取点；二、在局部地图中找出点对应的点，并使得点与点之间的距离最小(例如可以遍历局部地图，计算点与第二点云数据中各个点的距离，通过不断比较，找到距离点最近的点)；三、计算旋转矩阵R和平移矩阵T，使得误差函数最小；四、对点使用上一步求得的旋转矩阵R和平移矩阵T进行旋转和平移变换，得到新的对应点集；五、计算点与点的平均距离d；六、若平均距离d小于给定的阈值或者大于预设的最大迭代次数，则停止迭代计算，否则返回第二步，直到满足收敛条件为止。
在本实施例中，记第二点云数据为源点云Ps＝{xi|i∈[1,Ns]}，更新后的局部地图为参考点云Pr＝{yi|i∈[1,Nr]}，其中，Ns和Nr为整数。首先，Ps中的点xi在Pr中搜索最近点得到yi，从而得到点对(xi,yi)；当点对(xi,yi)的距离小于预设阈值时，将该点对保留，直至保留的点对个数达到预设值ne，则可以按照以下步骤求旋转矩阵R和平移向量t：
计算源点云和参考点云中参与配对的点的各自的几何中心：
Figure BDA0002427459990000071
Figure BDA0002427459990000072
计算协方差矩阵W：
Figure BDA0002427459990000073
对协方差矩阵W进行奇异值分解，求出矩阵U和V：
W＝UDV
计算旋转矩阵R：
R＝USVT
其中，S是一个三阶的对角矩阵，其对角元素为{1,1,detU,detV}，其中，detU和detV分别为矩阵U和V的行列式。
计算平移向量t：
t＝xc-RTyc
在步骤152中，在计算得到平移向量t后，结合激光雷达在上一次扫描时的位姿，得到激光雷达在当前扫描时的位姿。例如，假设平移向量
Figure BDA0002427459990000081
激光雷达在上一次扫描时在世界坐标系中的坐标为(1,1,1)，则激光雷达在当前扫描在世界坐标系中的坐标为(1+x,1+y,1+z)，从而得到激光雷达在当前扫描时的位姿。
需要说明的是，在本实施例中，上一次扫描和当前扫描可以为激光雷达在扫描过程中的任意两次相邻的扫描，从而通过输出激光雷达的位姿不断追踪激光雷达的运动轨迹。
本发明实施例通过获取激光雷达上一次扫描周围环境的第一点云数据，对第一点云数据进行预处理，预处理包括运动畸变去除、滤波以及去除动态物体，根据预处理后的第一点云数据，更新局部地图，获取激光雷达当前扫描周围环境的第二点云数据，对第二点云数据和更新后的局部地图进行匹配，以确定激光雷达在当前扫描时的位姿，能够通过第一点云数据对局部地图进行补充，完善局部地图的几何观测信息，从而能够提高定位的精度。
图2示出了本发明另一实施例提供的一种定位方法的流程示意图。如图2所示，该方法包括：
步骤210、获取激光雷达上一次扫描周围环境的第一点云数据。
其中，步骤210与上述实施例中的步骤110的实施方式相同，此处不再赘述。
步骤221、确定上一次扫描中的起始时刻以及除起始时刻以外的其他时刻。
其中，起始时刻为激光雷达在进行上一次扫描时的开始时刻，而其他时刻为上一次扫描中除了起始时刻的若干其他时刻。例如，假设激光雷达从第0秒到第100毫秒完成了上一次扫描，则起始时刻为第0秒，第1毫秒、第2毫秒……第100毫秒等为其他时刻。当然，各个其他时刻之间的时间间隔可以根据实际情况决定，例如，可以为1毫秒，也可以为10毫秒等等。
步骤222、计算其他时刻和起始时刻之间的时间间隔。
其中，计算各个其他时刻和起始时刻之间的时间间隔。
步骤223、根据时间间隔以及激光雷达在其他时刻时的旋转速度和平移速度，对第一点云数据进行运动畸变去除。
其中，激光点云数据不是瞬时获得的，在激光雷达进行激光测量时伴随着移动平台的运动。当激光雷达处于移动状态下时，其扫描周期内的扫描运动将不仅仅包含激光雷达旋转部分的旋转运动，还有其相对被测物体因其固定的移动平台所产生的平移及旋转运动。因此，需要将激光雷达移动状态下这些运动考虑其中，矫正激光雷达在一个扫描周期内的点云数据，使其扫描平面的点云数据更为准确地反映真实情况，从而提高激光雷达点云数据的准确度。
其中，其他时刻的旋转速度是指其他时刻的瞬时的旋转速度，其他时刻的平移速度是指其他时刻的瞬时的平移速度。则在步骤223中，获取激光雷达在各个其他时刻的旋转速度和平移速度，将旋转速度乘以对应的时间间隔，得到需要校正的角度，平移速度乘以对应的时间间隔，得到需要校正的位移，对各个其他时刻的激光点云进行校正后，从而进行运动畸变去除，并将校正后的所有激光点云重新封装成一帧激光点云数据，得到第一点云数据。
步骤224、对运动畸变去除后的第一点云数据进行滤波。
其中，为了减少数据处理量，则对运动畸变去除后的第一点云数据进行滤波。具体可以为：将第一点云数据的空间按照预设长宽高的体素栅格进行划分，用栅格内的重心表示栅格内的所有点，各个栅格的重心组成滤波后的第一点云数据。其中，重心的计算方法可以为：对栅格内的每个点的坐标求和，然后除以点的个数。可选地，预设长宽高可以为1m、0.6m和0.2m。
步骤225、确定滤波后的第一点云数据中的动态物体，并去除动态物体。
其中，为了减少环境中的运动物体的干扰，提高第一点云数据中有效环境信息的比例，则去除第一点云数据中的动态物体。确定滤波后的第一点云数据中的动态物体，具体可以包括：对滤波后的第一点云数据进行聚类，以确定第一点云数据中的物体；若第一点云数据中的物体的长宽高均小于预设长宽高阈值时，则将物体确定为动态物体。
其中，可以对滤波后的第一点云数据进行欧式聚类，欧式聚类是指当距离某个类的点的距离小于预设聚类半径时，则将该点归为该类，如果不存在满足条件的类，则将该点视为新的类，依次遍历，直至所有点都有对应的类。可选地，预设聚类半径可以为1.1m。
其中，将聚类后得到的每个类作为一个物体，从而确定第一点云数据中的物体。确定各个物体的长、宽、高，并分别比较物体的长、宽、高和预设长、宽、高阈值，当物体的长、宽、高均小于预设长、宽、高阈值时，则确定该物体为动态物体，从而去除第一点云数据中的动态物体。可选地，预设长、宽、高阈值可以为4m、3m、1.5m，这样设置可以去掉大多数行人和车辆。
步骤230、根据预处理后的第一点云数据，更新局部地图。
步骤240、获取激光雷达当前扫描周围环境的第二点云数据。
步骤250、对第二点云数据和更新后的局部地图进行匹配，以确定激光雷达在当前扫描时的位姿。
其中，步骤230至步骤250与上一实施例中的步骤130至步骤150的实施方式相同，此处不再赘述。
步骤261、根据激光雷达在上一次扫描的第一平移速度、激光雷达在当前扫描的第二平移速度，计算激光雷达的加速度。
其中，第一平移速度可以为激光雷达在上一次扫描中的任意时刻的瞬时平移速度，也可以为激光雷达在上一次扫描中的平均平移速度。同样的，第二平移速度可以为激光雷达在当前扫描中的任意时刻的瞬时平移速度，也可以为激光雷达在当前扫描中的平均平移速度。计算激光雷达的加速度，具体可以为：确定上一次扫描与当前扫描之间的时间差(一般为一个扫描周期)，将第二平移速度与第一平移速度的差除以上一次扫描与当前扫描之间的时间差，计算得到激光雷达的加速度。
步骤262、若加速度大于预设加速度阈值，则根据第一平移速度以及上一次扫描和当前扫描之间的时间差，调整确定的在当前扫描时的位姿。
其中，预设加速度阈值用于判断步骤250中确定的激光雷达在当前扫描的位姿是否准确。由于运动物体的加速度有一定的限制，在短时间内，物体的速度无法大幅度上升或者下降，因此，当激光雷达的加速度没有超过预设加速度阈值时，认为激光雷达观测正常，即步骤250中确定的激光雷达在当前扫描的位姿准确，则正常输出激光雷达在当前扫描的位姿；当激光雷达的加速度超过预设加速度阈值时，则认为激光雷达观测无法用于求解正确的运动，即步骤250中确定的激光雷达在当前扫描的位姿不准确，则将第一平移速度乘以上一次扫描与当前扫描之间的时间差，作为激光雷达在当前扫描的位姿并输出。可选地，预设加速度阈值可以为8m/s2。
本发明实施例通过获取激光雷达上一次扫描周围环境的第一点云数据，对第一点云数据进行运动畸变去除、滤波以及去除动态物体，然后再根据第一点云数据，更新局部地图，获取激光雷达当前扫描周围环境的第二点云数据，对第二点云数据和更新后的局部地图进行匹配，以确定激光雷达在当前扫描时的位姿，根据激光雷达在上一次扫描时的第一平移速度、激光雷达在当前扫描时的第二平移速度，计算激光雷达的加速度，若加速度大于预设加速度阈值，则根据第一平移速度以及上一次扫描和当前扫描之间的时间差，调整激光雷达在当前扫描的位姿，能够对第一点云数据进行处理，以得到更有效的地图信息，再通过第一点云数据对局部地图进行补充，完善局部地图的几何观测信息，从而提高定位的精度，并且，还通过判断观测退化来调整当前扫描的位姿，从而得到更加准确的定位，提高了准确性和鲁棒性。
图3示出了本发明又一实施例提供的一种定位方法的流程示意图。该定位方法应用于载有激光雷达的扫地机器人。如图3所示，该方法包括：
步骤301、开启扫地机器人，扫地机器人速度为零，激光雷达第一次旋转360°扫描周围环境，生成第一帧激光点云数据，作为局部地图；
步骤302、记录扫地机器人的原点位姿，扫地机器人开始运动，同时激光雷达第二次旋转360°扫描周围环境，生成第二帧激光点云数据，作为第一点云数据；
步骤303、根据原点位姿以及扫地机器人生成第一点云数据时的平移速度，计算扫地机器人生成第一点云数据时的第一位姿；
步骤304、对第一点云数据进行预处理，预处理包括运动畸变去除、滤波以及去除动态物体；
步骤305、根据预处理后的第一点云数据，更新局部地图；
步骤306、激光雷达第三次旋转360°扫描周围环境，生成第三帧激光点云数据，作为第二点云数据；
步骤307、对第二点云数据和更新后的局部地图进行匹配，并结合第一位姿，计算生成第二点云数据时的第二位姿；
步骤308、将扫地机器人生成第一点云数据时的平移速度与扫地机器人生成第二点云数据时的平移速度的差除以扫地机器人生成第一点云数据和生成第二点云数据的时间差，计算得到扫地机器人的加速度；
步骤309、若加速度大于预设加速度阈值，则将扫地机器人生成第一点云数据时的平移速度乘以扫地机器人生成第一点云数据和生成第二点云数据的时间差，并结合第一位姿，计算得到第三位姿，并将步骤307得到的第二位姿的值调整为第三位姿的值，输出第二位姿。
步骤310、若加速度不大于预设加速度阈值，则直接输出步骤307得到的第二位姿。
本发明实施例能够对第一点云数据进行预处理，以得到更有效的地图信息，再通过第一点云数据对局部地图进行补充，完善局部地图的几何观测信息，从而提高定位的精度，并且，还通过判断观测退化来调整当前扫描的位姿，从而得到更加准确的定位，提高了准确性和鲁棒性。
图4示出了本发明实施例提供的一种定位装置的结构示意图。如图4所示，该装置400包括：第一获取模块410、预处理模块420、更新模块430、第二获取模块440和位姿确定模块450。
其中，第一获取模块410用于获取激光雷达上一次扫描周围环境的第一点云数据；预处理模块420用于对所述第一点云数据进行预处理，所述预处理包括运动畸变去除、滤波以及去除动态物体；更新模块430用于根据所述第一点云数据，更新局部地图；第二获取模块440用于获取所述激光雷达当前扫描周围环境的第二点云数据；位姿确定模块450用于对所述第二点云数据和更新后的所述局部地图进行匹配，以确定所述激光雷达在所述当前扫描时的位姿。
在一种可选的方式中，预处理模块420具体用于：确定所述上一次扫描中的起始时刻以及除所述起始时刻以外的第一时刻；计算所述第一时刻和所述起始时刻之间的时间间隔；根据所述时间间隔以及所述激光雷达在所述第一时刻时的旋转速度和平移速度，对所述第一点云数据进行运动畸变去除。
在一种可选的方式中，预处理模块420还用于对所述第一点云数据进行滤波；确定滤波后的所述第一点云数据中的动态物体，并去除所述动态物体。
在一种可选的方式中，预处理模块420还具体用于：对滤波后的所述第一点云数据进行聚类，以确定所述第一点云数据中的物体；若所述第一点云数据中的物体的长、宽、高小于预设长、宽、高阈值时，则将所述物体确定为所述动态物体。
在一种可选的方式中，更新模块430具体用于：根据所述预处理后的所述第一点云数据和所述局部地图，计算所述预处理后的所述第一点云数据和所述局部地图之间的变换矩阵；根据所述预处理后的所述第一点云数据和所述局部地图之间的变换矩阵，将所述预处理后的所述第一点云数据和所述局部地图变换到相同坐标系下；对变换后的所述第一点云数据和所述局部地图进行合并和滤波，以得到更新后的所述局部地图。
在一种可选的方式中，位姿确定模块450具体用于：根据所述第二点云数据和更新后的所述局部地图，基于迭代最近点算法确定所述第二点云数据和更新后的所述局部地图之间的变换关系；根据所述第二点云数据和更新后的所述局部地图之间的变换矩阵，确定所述激光雷达在所述当前扫描时的位姿。
在一种可选的方式中，该装置还包括：位姿调整模块。位姿调整模块用于：根据所述激光雷达在所述上一次扫描的第一平移速度、所述激光雷达在所述当前扫描的第二平移速度，计算所述激光雷达的加速度；若所述加速度大于预设加速度阈值，则根据所述第一平移速度以及所述上一次扫描和所述当前扫描之间的时间差，调整确定的所述在所述当前扫描时的位姿。
需要说明的是，本发明实施例提供的定位装置是能够执行上述定位方法的装置，则上述基于定位方法的所有实施例均适用于该装置，且均能达到相同或相似的有益效果。
本发明实施例通过获取激光雷达上一次扫描周围环境的第一点云数据，对所述第一点云数据进行预处理，所述预处理包括运动畸变去除、滤波以及去除动态物体，根据预处理后的第一点云数据，更新局部地图，获取激光雷达当前扫描周围环境的第二点云数据，对第二点云数据和更新后的局部地图进行匹配，以确定激光雷达在当前扫描时的位姿，能够通过第一点云数据对局部地图进行补充，完善局部地图的几何观测信息，从而能够提高定位的精度。
本发明实施例提供了一种计算机可读存储介质，所述存储介质中存储有至少一可执行指令，所述可执行指令使处理器执行上述任意方法实施例中的定位方法。
本发明实施例提供了一种计算机程序产品，所述计算机程序产品包括存储在计算机存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被计算机执行时，使所述计算机执行上述任意方法实施例中的定位方法。
图5示出了本发明实施例提供的计算设备的结构示意图，本发明具体实施例并不对计算设备的具体实现做限定。
其中，计算设备包括：处理器和存储器。存储器用于存放至少一可执行指令，当计算设备运行时，处理器执行可执行指令，使处理器执行根据上述任意方法实施例中的定位方法的步骤。
可选地，如图5所示，该云端服务器500可以包括：处理器(processor)502、通信接口(Communications Interface)504、存储器(memory)506、以及通信总线508。
其中：处理器502、通信接口504、以及存储器506通过通信总线508完成相互间的通信。通信接口504，用于与其它设备比如客户端或其它服务器等的网元通信。处理器502，用于执行程序510，具体可以执行上述任意方法实施例中的定位方法。
具体地，程序510可以包括程序代码，该程序代码包括计算机操作指令。
处理器502可能是中央处理器CPU，或者是特定集成电路ASIC(ApplicationSpecific Integrated Circuit)，或者是被配置成实施本发明实施例的一个或多个集成电路。云端服务器包括的一个或多个处理器，可以是同一类型的处理器，如一个或多个CPU；也可以是不同类型的处理器，如一个或多个CPU以及一个或多个ASIC。
存储器506，用于存放程序510。存储器506可能包含高速RAM存储器，也可能还包括非易失性存储器(non-volatile memory)，例如至少一个磁盘存储器。
本发明实施例通过获取激光雷达上一次扫描周围环境的第一点云数据，对所述第一点云数据进行预处理，所述预处理包括运动畸变去除、滤波以及去除动态物体，根据预处理后的第一点云数据，更新局部地图，获取激光雷达当前扫描周围环境的第二点云数据，对第二点云数据和更新后的局部地图进行匹配，以确定激光雷达在当前扫描时的位姿，能够通过第一点云数据对局部地图进行补充，完善局部地图的几何观测信息，从而能够提高定位的精度。
在此提供的算法或显示不与任何特定计算机、虚拟***或者其它设备固有相关。各种通用***也可以与基于在此的示教一起使用。根据上面的描述，构造这类***所要求的结构是显而易见的。此外，本发明实施例也不针对任何特定编程语言。应当明白，可以利用各种编程语言实现在此描述的本发明的内容，并且上面对特定语言所做的描述是为了披露本发明的最佳实施方式。
在此处所提供的说明书中，说明了大量具体细节。然而，能够理解，本发明的实施例可以在没有这些具体细节的情况下实践。在一些实例中，并未详细示出公知的方法、结构和技术，以便不模糊对本说明书的理解。
类似地，应当理解，为了精简本发明并帮助理解各个发明方面中的一个或多个，在上面对本发明的示例性实施例的描述中，本发明实施例的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而，并不应将该公开的方法解释成反映如下意图：即所要求保护的本发明要求比在每个权利要求中所明确记载的特征更多的特征。更确切地说，如下面的权利要求书所反映的那样，发明方面在于少于前面公开的单个实施例的所有特征。因此，遵循具体实施方式的权利要求书由此明确地并入该具体实施方式，其中每个权利要求本身都作为本发明的单独实施例。
本领域那些技术人员可以理解，可以对实施例中的设备中的模块进行自适应性地改变并且把它们设置在与该实施例不同的一个或多个设备中。可以把实施例中的模块或单元或组件组合成一个模块或单元或组件，以及此外可以把它们分成多个子模块或子单元或子组件。除了这样的特征和/或过程或者单元中的至少一些是相互排斥之外，可以采用任何组合对本说明书(包括伴随的权利要求、摘要和附图)中公开的所有特征以及如此公开的任何方法或者设备的所有过程或单元进行组合。除非另外明确陈述，本说明书(包括伴随的权利要求、摘要和附图)中公开的每个特征可以由提供相同、等同或相似目的的替代特征来代替。
此外，本领域的技术人员能够理解，尽管在此的一些实施例包括其它实施例中所包括的某些特征而不是其它特征，但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如，在下面的权利要求书中，所要求保护的实施例的任意之一都可以以任意的组合方式来使用。
应该注意的是上述实施例对本发明进行说明而不是对本发明进行限制，并且本领域技术人员在不脱离所附权利要求的范围的情况下可设计出替换实施例。在权利要求中，不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的元件或步骤。位于元件之前的单词“一”或“一个”不排除存在多个这样的元件。本发明可以借助于包括有若干不同元件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中，这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。上述实施例中的步骤，除有特殊说明外，不应理解为对执行顺序的限定。

Claims (9)

1.一种定位方法，其特征在于，包括：
获取激光雷达上一次扫描周围环境的第一点云数据；
对所述第一点云数据进行预处理，所述预处理包括运动畸变去除、滤波以及去除动态物体；
根据所述预处理后的所述第一点云数据，更新局部地图；
获取所述激光雷达当前扫描所述周围环境的第二点云数据；
对所述第二点云数据和更新后的所述局部地图进行匹配，以确定所述激光雷达在所述当前扫描时的位姿；
根据激光雷达在上一次扫描的第一平移速度、激光雷达在当前扫描的第二平移速度，计算激光雷达的加速度；
若所述加速度大于预设加速度阈值，则根据第一平移速度以及上一次扫描和当前扫描之间的时间差，调整所述当前扫描时的位姿。
2.根据权利要求1所述的方法，其特征在于，所述对所述第一点云数据进行预处理，具体包括：
确定所述上一次扫描中的起始时刻以及除所述起始时刻以外的其他时刻；
计算所述其他时刻和所述起始时刻之间的时间间隔；
根据所述时间间隔以及所述激光雷达在所述其他时刻时的旋转速度和平移速度，对所述第一点云数据进行运动畸变去除。
3.根据权利要求2所述的方法，其特征在于，在所述对所述第一点云数据进行运动畸变去除之后，所述对所述第一点云数据进行预处理，具体还包括：
对运动畸变去除后的所述第一点云数据进行滤波；
确定滤波后的所述第一点云数据中的动态物体，并去除所述动态物体。
4.根据权利要求3所述的方法，其特征在于，所述确定滤波后的所述第一点云数据中的动态物体，具体包括：
对滤波后的所述第一点云数据进行聚类，以确定所述第一点云数据中的物体；
若所述第一点云数据中的物体的长、宽、高小于预设长、宽、高阈值时，则将所述物体确定为所述动态物体。
5.根据权利要求1所述的方法，其特征在于，所述根据所述预处理后的所述第一点云数据，更新局部地图，具体包括：
根据所述预处理后的所述第一点云数据和所述局部地图，计算所述预处理后的所述第一点云数据和所述局部地图之间的变换矩阵；
根据所述预处理后的所述第一点云数据和所述局部地图之间的变换矩阵，将所述预处理后的所述第一点云数据和所述局部地图变换到相同坐标系下；
对变换后的所述第一点云数据和所述局部地图进行合并和滤波，以得到更新后的所述局部地图。
6.根据权利要求1所述的方法，其特征在于，所述对所述第二点云数据和更新后的所述局部地图进行匹配，以确定所述激光雷达在所述当前扫描时的位姿，具体包括：
根据所述第二点云数据和更新后的所述局部地图，基于迭代最近点算法确定所述第二点云数据和更新后的所述局部地图之间的变换关系；
根据所述第二点云数据和更新后的所述局部地图之间的变换矩阵，确定所述激光雷达在所述当前扫描时的位姿。
7.一种定位装置，其特征在于，包括：
第一获取模块，用于获取激光雷达上一次扫描周围环境的第一点云数据；
预处理模块，用于对所述第一点云数据进行预处理，所述预处理包括运动畸变去除、滤波以及去除动态物体；
更新模块，用于根据所述预处理后的所述第一点云数据，更新局部地图；
第二获取模块，用于获取所述激光雷达当前扫描所述周围环境的第二点云数据；
位姿确定模块，用于对所述第二点云数据和更新后的所述局部地图进行匹配，以确定所述激光雷达在所述当前扫描时的位姿；根据激光雷达在上一次扫描的第一平移速度、激光雷达在当前扫描的第二平移速度，计算激光雷达的加速度；若所述加速度大于预设加速度阈值，则根据第一平移速度以及上一次扫描和当前扫描之间的时间差，调整所述当前扫描时的位姿。
8.一种定位设备，其特征在于，包括：处理器和存储器，所述存储器用于存放至少一可执行指令，当所述定位设备运行时，所述处理器执行所述可执行指令，使所述处理器执行根据权利要求1-6任一项所述的定位方法的步骤。
9.一种计算机可读存储介质，其特征在于，所述存储介质中存储有至少一可执行指令，所述可执行指令使处理器执行根据权利要求1-6任一项所述的定位方法的步骤。
CN202010225362.6A 2020-03-26 2020-03-26 定位方法、装置及设备 Active CN111443359B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010225362.6A CN111443359B (zh) 2020-03-26 2020-03-26 定位方法、装置及设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010225362.6A CN111443359B (zh) 2020-03-26 2020-03-26 定位方法、装置及设备

Publications (2)

Publication Number Publication Date
CN111443359A CN111443359A (zh) 2020-07-24
CN111443359B true CN111443359B (zh) 2022-06-07

Family

ID=71649086

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010225362.6A Active CN111443359B (zh) 2020-03-26 2020-03-26 定位方法、装置及设备

Country Status (1)

Country Link
CN (1) CN111443359B (zh)

Families Citing this family (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113776530B (zh) * 2020-09-11 2024-06-18 北京京东乾石科技有限公司 一种点云地图构建方法、装置、电子设备和存储介质
CN112484738B (zh) * 2020-11-24 2023-04-28 深圳市优必选科技股份有限公司 机器人建图方法、装置、计算机可读存储介质及机器人
CN112946683A (zh) * 2021-01-13 2021-06-11 北京理工大学 一种单线激光雷达的地图构建方法
CN112988079B (zh) * 2021-05-07 2021-11-26 成都奥伦达科技有限公司 一种面向超海量点云的管理方法及***
CN113492405B (zh) * 2021-06-17 2022-08-02 达闼机器人股份有限公司 机器人开门方法、装置、存储介质及电子设备
CN113432533B (zh) * 2021-06-18 2023-08-15 北京盈迪曼德科技有限公司 一种机器人定位方法、装置、机器人及存储介质
WO2023000294A1 (zh) * 2021-07-23 2023-01-26 深圳市大疆创新科技有限公司 位姿估计方法、激光雷达-惯性里程计、可移动平台及存储介质
CN114089365B (zh) * 2022-01-20 2022-05-17 智道网联科技(北京)有限公司 一种自动驾驶车辆定位方法、装置及电子设备、存储介质
CN114720993A (zh) * 2022-03-30 2022-07-08 上海木蚁机器人科技有限公司 机器人定位方法、装置、电子设备以及存储介质
CN115507861A (zh) * 2022-09-02 2022-12-23 智己汽车科技有限公司 一种自动驾驶定位方法、装置、计算机设备、介质和程序产品
CN117804423A (zh) * 2022-09-26 2024-04-02 华为云计算技术有限公司 一种重定位方法以及装置
CN116359938B (zh) * 2023-05-31 2023-08-25 未来机器人（深圳）有限公司 对象检测方法、装置及运载装置

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109870157A (zh) * 2019-02-20 2019-06-11 苏州风图智能科技有限公司 确定车***姿的方法及装置、制图方法
CN110533055A (zh) * 2018-05-25 2019-12-03 北京京东尚科信息技术有限公司 一种点云数据的处理方法和装置
JP2020042409A (ja) * 2018-09-07 2020-03-19 村田機械株式会社 走行車システム

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110533055A (zh) * 2018-05-25 2019-12-03 北京京东尚科信息技术有限公司 一种点云数据的处理方法和装置
JP2020042409A (ja) * 2018-09-07 2020-03-19 村田機械株式会社 走行車システム
CN109870157A (zh) * 2019-02-20 2019-06-11 苏州风图智能科技有限公司 确定车***姿的方法及装置、制图方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Localization and mapping in urban area based on 3D point cloud of autonomous vehicles;WANG Mei-ling等;《Journal of Beijing Institute of Technology》;20161231;第25卷(第4期);第473-481页 *
基于三维激光扫描仪的三维点云地图构建研究;蒋盛锋;《中国优秀硕士学位论文全文数据库 (工程科技Ⅰ辑)》;20171115(第11期);正文第16-18、21-30、35-36、41-44页 *
室内移动机器人SLAM算法设计;宋书亮;《中国优秀硕士学位论文全文数据库 (信息科技辑)》;20200215(第2期);正文第22-24页 *

Also Published As

Publication number Publication date
CN111443359A (zh) 2020-07-24

Similar Documents

Publication Publication Date Title
CN111443359B (zh) 定位方法、装置及设备
CN109579849B (zh) 机器人定位方法、装置和机器人及计算机存储介质
US20210270609A1 (en) Method, apparatus, computing device and computer-readable storage medium for positioning
CN113223091B (zh) 三维目标检测方法、抓取方法、装置及电子设备
CN113748357A (zh) 激光雷达的姿态校正方法、装置和***
US11045953B2 (en) Relocalization method and robot using the same
CN111699410B (zh) 点云的处理方法、设备和计算机可读存储介质
CN112179357B (zh) 基于单目相机的平面运动目标视觉导航方法及***
CN113128272B (zh) 一种碰撞预测方法、装置、存储介质和服务器
CN115482255A (zh) 障碍物追踪方法、装置、设备及存储介质
CN110991534A (zh) 点云数据处理方法、装置、设备及计算机可读存储介质
CN110515060B (zh) 多线激光雷达反射率标定的方法
Hu et al. R-CNN based 3D object detection for autonomous driving
CN117269952A (zh) 一种4d成像毫米波雷达动目标点云半自动标注方法及装置
CN114648639B (zh) 一种目标车辆的检测方法、***及装置
CN117029817A (zh) 一种二维栅格地图融合方法及***
CN112068547B (zh) 一种基于amcl的机器人定位方法、装置和机器人
CN115619954A (zh) 稀疏语义地图的构建方法、装置、设备及存储介质
WO2022017129A1 (zh) 目标对象检测方法、装置、电子设备及存储介质
CN109711363B (zh) 车辆定位方法、装置、设备和存储介质
CN113219486B (zh) 一种定位方法及装置
CN114549605B (zh) 基于点云匹配的图像优化方法、装置、设备及存储介质
CN115147612B (zh) 一种基于累加点云对车辆尺寸进行实时估计的处理方法
CN113029166B (zh) 定位方法、装置、电子设备及存储介质
WO2022198603A1 (en) Real-time simultaneous localization and mapping using an event camera

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
TA01 Transfer of patent application right

Effective date of registration: 20210125

Address after: 200000 second floor, building 2, no.1508, Kunyang Road, Minhang District, Shanghai

Applicant after: Dalu Robot Co.,Ltd.

Address before: 610094 West Section of Fucheng Avenue, Chengdu High-tech District, Sichuan Province

Applicant before: CLOUDMINDS (CHENGDU) TECHNOLOGIES Co.,Ltd.

TA01 Transfer of patent application right
CB02 Change of applicant information

Address after: 201111 Building 8, No. 207, Zhongqing Road, Minhang District, Shanghai

Applicant after: Dayu robot Co.,Ltd.

Address before: 200000 second floor, building 2, no.1508, Kunyang Road, Minhang District, Shanghai

Applicant before: Dalu Robot Co.,Ltd.

CB02 Change of applicant information
GR01 Patent grant
GR01 Patent grant