WO2021109166A1 - 三维激光定位方法及系统 - Google Patents

三维激光定位方法及系统 Download PDF

Info

Publication number
WO2021109166A1
WO2021109166A1 PCT/CN2019/123929 CN2019123929W WO2021109166A1 WO 2021109166 A1 WO2021109166 A1 WO 2021109166A1 CN 2019123929 W CN2019123929 W CN 2019123929W WO 2021109166 A1 WO2021109166 A1 WO 2021109166A1
Authority
WO
WIPO (PCT)
Prior art keywords
particle
pose
coefficient
particles
weight
Prior art date
Application number
PCT/CN2019/123929
Other languages
English (en)
French (fr)
Inventor
刘胜明
姜志英
芮青青
司秀芬
Original Assignee
苏州艾吉威机器人有限公司
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 苏州艾吉威机器人有限公司 filed Critical 苏州艾吉威机器人有限公司
Publication of WO2021109166A1 publication Critical patent/WO2021109166A1/zh

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Definitions

  • the invention relates to the technical field of robot positioning, in particular to a three-dimensional laser positioning method and system.
  • Positioning is the most basic link for mobile robots to achieve autonomous capabilities. It is a prerequisite for completing subsequent tasks such as navigation and path planning. Its purpose is to determine the mobile robot's pose (including position and posture) relative to the world coordinate system in the working environment.
  • the current positioning methods are mainly divided into the following three categories: relative positioning technology, absolute positioning technology, and combined positioning technology.
  • the technical problem to be solved by the present invention is to overcome the problems of low efficiency and poor positioning accuracy in the prior art, thereby providing a three-dimensional laser positioning method and system with high efficiency and effectively improving positioning accuracy.
  • a three-dimensional laser positioning method of the present invention includes the following steps: predict the predicted pose of the robot at a certain moment relative to the previous moment according to the data obtained by the encoder; Particles: Observe each particle, compare the observed environmental information data with the voxel distance map, and calculate the coefficient of each particle based on the similarity of the comparison; filter out particles with poor coefficients according to the coefficient, and loop multiple times Filter until the particle with the optimal coefficient is found; perform smooth filtering processing on the pose corresponding to the particle with the optimal coefficient, and output the final pose of the robot.
  • the coefficient of the particle is related to the speed of the robot movement and is a variable.
  • the coefficient of the particle includes any one of a weight or a variance or a combination of the weight and the variance.
  • the particles with a small weight are filtered out multiple times.
  • the weight of the particle is obtained by obtaining the distance information of the laser point on the corresponding voxel grid by the look-up table method.
  • the particles with large variance are filtered out through multiple cycles.
  • the coefficient of the particles is a combination of weight and variance
  • the particles with small weight and large variance are filtered out multiple times.
  • the method of smoothing filtering processing is: when the robot is running at a low speed, believing the predicted pose output by the encoder, and when the robot is running at a high speed, believing that the coefficient is optimal The corresponding pose of the particles.
  • the weight of the predicted pose when the final pose of the robot is output, when the predicted pose output by the encoder is believed, the weight of the predicted pose is greater than the corresponding pose of the particle with the optimal coefficient. When the pose corresponding to the particle with the optimal coefficient is believed, the weight of the predicted pose is less than the weight of the pose corresponding to the particle with the optimal coefficient.
  • the invention also discloses a three-dimensional laser positioning system, which includes a predictive pose module, a measurement module, a resampling module, and a processing module, wherein the predictive pose module predicts the robot at a certain time relative to the previous time based on the data acquired by the encoder The predicted pose; the measurement module is used to arrange multiple particles near the predicted pose, observe each particle, and compare the observed environmental information data with the voxel distance map, according to the similarity of the comparison Calculate the coefficient of each particle; the re-sampling module is used to filter out particles with poor coefficients according to the coefficients, and filter repeatedly until the particles with the best coefficients are found; The position and posture corresponding to the particles are smoothed and filtered, and the final posture of the robot is output.
  • the predictive pose module predicts the robot at a certain time relative to the previous time based on the data acquired by the encoder
  • the measurement module is used to arrange multiple particles near the predicted pose, observe each particle, and compare the observed environmental information data with the vox
  • the three-dimensional laser positioning method and system of the present invention predicts the predicted pose of the robot at a certain moment relative to the previous moment based on the data obtained by the encoder; a plurality of particles are arranged near the predicted pose, and each particle is observed , Comparing the observed environmental information data with the voxel distance map not only helps increase the number of particles, but also transfers the spatial point query process to an offline calculation process, which is beneficial to improve the query speed and calculation efficiency, according to the similarity of the comparison Calculate the coefficient of each particle to facilitate the screening of particles; filter out particles with poor coefficients according to the coefficients, and filter repeatedly until the particles with the best coefficients are found, so as to ensure that a stable posture is selected; The pose corresponding to the particle with the optimal coefficient is smoothed and filtered, and the final pose of the robot is output, thereby helping to improve the positioning accuracy and making the final pose more stable.
  • Figure 1 is a flow chart of the three-dimensional laser positioning method of the present invention
  • Figure 2 is a flow chart of positioning when applying the present invention
  • Fig. 3 is a schematic diagram of the three-dimensional laser positioning system of the present invention.
  • this embodiment provides a three-dimensional laser positioning method, including the following steps: Step S1: predict the predicted pose of the robot at a certain moment relative to the previous moment based on the data obtained by the encoder; Step S2: Predict the placement of multiple particles near the pose, observe each particle, compare the observed environmental information data with the voxel distance map, and calculate the coefficient of each particle based on the similarity of the comparison; Step S3: Filter out according to the coefficient The particles with inferior coefficients are removed, and the particles with the best coefficients are filtered out multiple times; step S4: smoothing and filtering the poses corresponding to the particles with the best coefficients, and outputting the final pose of the robot.
  • the coefficient of the particle is related to the speed of the robot movement and is a variable, which is beneficial to filter out a stable posture.
  • the coefficient of the particle includes any one of a weight or a variance or a combination of the weight and the variance.
  • the coefficient of the particle is the weight, the particles with the smaller weight are filtered out multiple times to find the particle with the largest weight, and the particle with the largest weight is the particle with the best coefficient.
  • the weight of the particle is obtained by obtaining the distance information of the laser point on the corresponding voxel grid by the look-up table method.
  • the particles with the large variance are filtered out multiple times to find the particle with the smallest variance, and the particle with the smallest variance is the particle with the best coefficient.
  • the variance of the particles is a statistic.
  • the coefficient of the particle is a combination of weight and variance
  • the particles with small weight and large variance are filtered out multiple times, and the particle with the largest weight and smallest variance is found. Then the particle with the largest weight and smallest variance is the particle with the largest coefficient. Excellent particles.
  • the possible poses at this moment can be calculated to form a predicted pose; when particles are scattered near the predicted pose, each particle will correspond to an observation.
  • the measurement can be performed according to the 3D laser, and then the observed environmental information data and the voxel distance map are compared, and the observed environmental information data and the voxel distance map are performed according to the location of the particles. Compare, judge the similarity, and then continue to calculate the weight and variance for each particle.
  • the specific method of converting the point cloud map into the voxel distance map is: first establish a voxelized point cloud map; calculate the distance from the center point to the approximate nearest neighbor for each voxel grid (using octree and artificial Neural network calculation); Finally, when calculating the particle weight, the distance information of the laser point falling on the corresponding voxel grid is obtained by the look-up table method.
  • the method of smooth filtering processing is: when the robot is running at a low speed, believing the predicted pose output by the encoder; when the robot is running at a high speed, believing the pose corresponding to the particle with the optimal coefficient, so that It is helpful to improve the positioning accuracy and make the final pose more stable.
  • the weight of the predicted pose is greater than the weight of the corresponding pose of the particle with the optimal coefficient; In the case of the pose corresponding to the particle with the optimal coefficient, the weight of the predicted pose is smaller than the weight of the corresponding pose of the particle with the optimal coefficient.
  • the formula is as follows:
  • P k represents the current pose
  • P k-1 represents the previous pose
  • spd is a variable related to speed, between 0 and 1
  • P laser is the relative displacement calculated by the laser
  • P odom is According to the relative displacement calculated by the encoder.
  • the predicted pose module 10 is used to predict the predicted pose of the robot at a certain moment relative to the previous moment according to the data obtained by the encoder;
  • the measurement module 20 is used to arrange multiple particles near the predicted pose, observe each particle, compare the observed environmental information data with the voxel distance map, and calculate the value of each particle based on the similarity of the comparison. coefficient;
  • the re-sampling module 30 is used to filter out particles with poor coefficients according to the coefficients, and filter out repeatedly until the particles with the best coefficients are found;
  • the processing module 40 is configured to perform smoothing filtering processing on the pose corresponding to the particle with the optimal coefficient, and output the final pose of the robot.
  • this application can be provided as methods, systems, or computer program products. Therefore, this application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, this application may adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program codes.
  • computer-usable storage media including but not limited to disk storage, CD-ROM, optical storage, etc.
  • These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing equipment to work in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture including the instruction device.
  • the device implements the functions specified in one process or multiple processes in the flowchart and/or one block or multiple blocks in the block diagram.
  • These computer program instructions can also be loaded on a computer or other programmable data processing equipment, so that a series of operation steps are executed on the computer or other programmable equipment to produce computer-implemented processing, so as to execute on the computer or other programmable equipment.
  • the instructions provide steps for implementing the functions specified in one process or multiple processes in the flowchart and/or one block or multiple blocks in the block diagram.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

一种三维激光定位方法及系统包括根据编码器获取的数据预测机器人某一时刻相对上一时刻的预测位姿；在所述预测位姿附近布置多个粒子，对每个粒子进行观测，将观测到的环境信息数据与体素距离地图作对比，根据对比的相似度计算每个粒子的系数；根据系数滤除掉系数不优的粒子，多次循环滤除直至找出系数最优的粒子；对系数最优的粒子对应的位姿进行平滑滤波处理，输出所述机器人的最终位姿。既可以提升查询速度，还可以提高定位精度。

Description

三维激光定位方法及系统 技术领域
本发明涉及机器人定位的技术领域，尤其是指一种三维激光定位方法及系统。
背景技术
定位是移动机器人实现自主能力的最基本环节，是完成导航、路径规划等后续任务的前提，其目的就是确定移动机器人在工作环境中相对于世界坐标系的位姿(包括位置和姿态)。目前定位方法主要分为如下三类：相对定位技术、绝对定位技术、组合定位技术。
在定位过程中，由于感知传感器本身存在着性能限制，运行环境中各种不确定因素干扰，移动机器人的定位研究开始依靠基于概率的模型对不确切的运动和感知信息进行建模，实现机器人的自定位。目前常用的概率定位方法包括扩展卡尔曼滤波、马尔科夫定位、多假设跟踪以及蒙特卡罗定位。而若应用在变电站环境中，由于场景范围大，且特征较为稀疏，对定位算法的时效性和精度都带来了很大的挑战，具体地体现为：观测噪声大、定位时效性、环境特征稀少，很难达到较高的定位精度。
为了克服上述问题，现有采用粒子滤波定位算法，所述粒子滤波定位算法是应用粒子集表示定位后验概率分布的蒙特卡洛方法，因此又被称为蒙特卡洛定位算法(MCL)。在移动机器人领域，粒子滤波定位就是利用机器人的输入数据，传感器的观测数据，结合运动学模型和观测模型来迭代，估计当前机器人位姿信度的最优化问题，即Bayesian滤波器问题的变体，主要思想就是用一个随机采样获得的具有权重的样本集合表示并估计后验概率密度。现有的粒子滤波定位算法框架中，在计算每个粒子权重时，需要对当前 帧激光点查询已知地图中的最近邻，但是过程是整个算法中最耗时的过程，不但速度低，而且定位精度差。
发明内容
为此，本发明所要解决的技术问题在于克服现有技术中效率低，且定位精度差的问题，从而提供一种效率高，且有效提高定位精度的三维激光定位方法及系统。
为解决上述技术问题，本发明的一种三维激光定位方法，包括如下步骤：根据编码器获取的数据预测机器人某一时刻相对上一时刻的预测位姿；在所述预测位姿附近布置多个粒子，对每个粒子进行观测，将观测到的环境信息数据与体素距离地图作对比，根据对比的相似度计算每个粒子的系数；根据系数滤除掉系数不优的粒子，多次循环滤除直至找出系数最优的粒子；对系数最优的粒子对应的位姿进行平滑滤波处理，输出所述机器人的最终位姿。
在本发明的一个实施例中，所述粒子的系数与所述机器人运动的速度有关，且是变量。
在本发明的一个实施例中，所述粒子的系数包括权重或方差或所述权重和方差的组合中的任意一种。
在本发明的一个实施例中，所述粒子的系数为权重时，多次循环滤除掉权重小的粒子。
在本发明的一个实施例中，所述粒子的权重，是由查表法获取激光点落在对应体素栅格的距离信息得出。
在本发明的一个实施例中，所述粒子的系数为方差时，多次循环滤除掉方差大的粒子。
在本发明的一个实施例中，所述粒子的系数为权重和方差的组合时，多次循环滤除掉权重小且方差大的粒子。
在本发明的一个实施例中，所述平滑滤波处理的方法为：所述机器人低速运行时，置信所述编码器输出的预测位姿，当所述机器人高速行驶时，置 信所述系数最优的粒子对应的位姿。
在本发明的一个实施例中，所述输出机器人的最终位姿时，当置信所述编码器输出的预测位姿时，所述预测位姿的权重大于所述系数最优的粒子对应位姿的权重；当置信所述系数最优的粒子对应的位姿时，所述预测位姿的权重小于所述系数最优的粒子对应位姿的权重。
本发明还公开了一种三维激光定位系统，包括预测位姿模块、测量模块、重采样模块、处理模块，其中所述预测位姿模块根据编码器获取的数据预测机器人某一时刻相对上一时刻的预测位姿；所述测量模块用于在所述预测位姿附近布置多个粒子，对每个粒子进行观测，将观测到的环境信息数据与体素距离地图作对比，根据对比的相似度计算每个粒子的系数；所述重采样模块用于根据系数滤除掉系数不优的粒子，多次循环滤除直至找出系数最优的粒子；所述处理模块用于对系数最优的粒子对应的位姿进行平滑滤波处理，输出所述机器人的最终位姿。
本发明的上述技术方案相比现有技术具有以下优点：
本发明所述的三维激光定位方法及系统，根据编码器获取的数据预测机器人某一时刻相对上一时刻的预测位姿；在所述预测位姿附近布置多个粒子，对每个粒子进行观测，将观测到的环境信息数据与体素距离地图作对比，不但有利于增加粒子数，而且可以将空间点查询过程转移为离线计算过程，有利于提升查询速度以及计算效率，根据对比的相似度计算每个粒子的系数，从而有利于筛选粒子；根据系数滤除掉系数不优的粒子，多次循环滤除直至找出系数最优的粒子，从而可以保证筛选出一个稳定的位姿；对系数最优的粒子对应的位姿进行平滑滤波处理，输出所述机器人的最终位姿，从而有利于提高定位精度，使最终位姿更加稳定。
附图说明
为了使本发明的内容更容易被清楚的理解，下面根据本发明的具体实施例并结合附图，对本发明作进一步详细的说明，其中
图1是本发明三维激光定位方法的流程图；
图2是应用本发明时的一种定位流程图；
图3是本发明三维激光定位系统的示意图。
说明书附图标记说明：10-预测位姿模块，20-测量模块，30-重采样模块，40-处理模块。
具体实施方式
实施例一：
如图1所示，本实施例提供一种三维激光定位方法，包括如下步骤：步骤S1:根据编码器获取的数据预测机器人某一时刻相对上一时刻的预测位姿；步骤S2：在所述预测位姿附近布置多个粒子，对每个粒子进行观测，将观测到的环境信息数据与体素距离地图作对比，根据对比的相似度计算每个粒子的系数；步骤S3：根据系数滤除掉系数不优的粒子，多次循环滤除直至找出系数最优的粒子；步骤S4：对系数最优的粒子对应的位姿进行平滑滤波处理，输出所述机器人的最终位姿。
本实施例所述三维激光定位方法，所述步骤S1中，根据编码器获取的数据预测机器人某一时刻相对上一时刻的预测位姿；所述步骤S2中，在所述预测位姿附近布置多个粒子，对每个粒子进行观测，将观测到的环境信息数据与体素距离地图作对比，不但有利于增加粒子数，而且可以将空间点查询过程转移为离线计算过程，有利于提升查询速度以及计算效率，根据对比的相似度计算每个粒子的系数，从而有利于筛选粒子；所述步骤S3中，根据系数滤除掉系数不优的粒子，多次循环滤除直至找出系数最优的粒子，从而可以保证筛选出一个稳定的位姿；所述步骤S4中，对系数最优的粒子对应的位姿进行平滑滤波处理，输出所述机器人的最终位姿，从而有利于提高定位精度，使最终位姿更加稳定。
所述粒子的系数与所述机器人运动的速度有关，且是变量，从而有利于筛选出一个稳定的位姿。具体地，所述粒子的系数包括权重或方差或所述权重和方差的组合中的任意一种。所述粒子的系数为权重时，多次循环滤除掉权重小的粒子，找出权重最大对应的粒子，则权重最大对应的粒子就是系数 最优的粒子。另外，所述粒子的权重，是由查表法获取激光点落在对应体素栅格的距离信息得出。所述粒子的系数为方差时，多次循环滤除掉方差大的粒子，找出方差最小对应的粒子，则方差最小对应的粒子就是系数最优的粒子。所述粒子的方差是个统计量。所述粒子的系数为权重和方差的组合时，多次循环滤除掉权重小且方差大的粒子，找出权重最大且方差最小对应的粒子，则权重最大且方差最小对应的粒子就是系数最优的粒子。
如图2所示，本实施例的一种应用中，若已知上一刻的位姿，加上编码器(如里程计)获取的数据，根据当前时刻和前一时刻的时间差值和里程计的速度、角速度，就可以计算出这一时刻可能的位姿，从而形成预测位姿；在所述预测位姿附近撒粒子，每个粒子均会对应一个观测，对每个粒子进行观测时，可以根据3D激光器去进行测量，再将所述观测到的环境信息数据和所述体素距离地图去作对比，根据所述粒子所在位置观测到的环境信息数据和所述体素距离地图作比，判断相似度，再继续给每个粒子计算权重和方差，由于总有存在算错的可能，所以需要对粒子进行重采样，通过不停筛选粒子直至找出方差最小的粒子，但是由于机器人运动的过程中速度不同，因此还需要对方差最小的粒子对应的位姿进行一个平滑滤波处理，从而可以保证输出的位姿更稳定。
另外，本实施例中，由于需要采用体素距离地图，因此需要提前判断是否有体素距离地图，若有，可以直接应用；若没有，则需要将点云地图转换成体素距离地图，从而可以将空间点查询过程转移为离线计算过程，有利于提升计算效率。将所述点云地图转换成所述体素距离地图的具体方法为：先建立体素化点云地图；对每个体素栅格计算中心点到近似最近邻的距离(利用八叉树以及人工神经网络计算)；最后，计算粒子权重时，由查表法获取激光点落在对应体素栅格的距离信息。
所述平滑滤波处理的方法为：所述机器人低速运行时，置信所述编码器输出的预测位姿，当所述机器人高速行驶时，置信所述系数最优的粒子对应的位姿，从而有利于提高定位精度，使最终位姿更加稳定。具体地，所述输出机器人的最终位姿时，当置信所述编码器输出的预测位姿时，所述预测位 姿的权重大于所述系数最优的粒子对应位姿的权重；当置信所述系数最优的粒子对应的位姿时，所述预测位姿的权重小于所述系数最优的粒子对应位姿的权重。具体地，用公式表达如下：
P k＝spd·(P laser-VP odom)+(1-spd)·P k-1
其中，P k代表当前位姿，P k-1代表上一时刻位姿，spd是和速度有关的变量，介于0到1之间，P laser是根据激光器计算出的相对位移，P odom是根据所述编码器计算出的相对位移。
实施例二
基于同一发明构思，如图3所示，本实施例提供一种三维激光定位系统，其解决问题的原理与所述三维激光定位方法类似，重复之处不再赘述。
本实施例所述三维激光定位系统包括：
预测位姿模块10，用于根据编码器获取的数据预测机器人某一时刻相对上一时刻的预测位姿；
测量模块20，用于在所述预测位姿附近布置多个粒子，对每个粒子进行观测，将观测到的环境信息数据与体素距离地图作对比，根据对比的相似度计算每个粒子的系数；
重采样模块30，用于根据系数滤除掉系数不优的粒子，多次循环滤除直至找出系数最优的粒子；
处理模块40，用于对系数最优的粒子对应的位姿进行平滑滤波处理，输出所述机器人的最终位姿。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序 产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
显然，上述实施例仅仅是为清楚地说明所作的举例，并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引伸出的显而易见的变化或变动仍处于本发明创造的保护范围之中。

Claims (10)

  1. 一种三维激光定位方法，其特征在于，包括如下步骤：
    步骤S1：根据编码器获取的数据预测机器人某一时刻相对上一时刻的预测位姿；
    步骤S2：在所述预测位姿附近布置多个粒子，对每个粒子进行观测，将观测到的环境信息数据与体素距离地图作对比，根据对比的相似度计算每个粒子的系数；
    步骤S3：根据系数滤除掉系数不优的粒子，多次循环滤除直至找出系数最优的粒子；
    步骤S4：对系数最优的粒子对应的位姿进行平滑滤波处理，输出所述机器人的最终位姿。
  2. 根据权利要求1所述的三维激光定位方法，其特征在于：所述粒子的系数与所述机器人运动的速度有关，且是变量。
  3. 根据权利要求1或2所述的三维激光定位方法，其特征在于：所述粒子的系数包括权重或方差或所述权重和方差的组合中的任意一种。
  4. 根据权利要求3所述的三维激光定位方法，其特征在于：所述粒子的系数为权重时，多次循环滤除掉权重小的粒子。
  5. 根据权利要求4所述的三维激光定位方法，其特征在于：所述粒子的权重，是由查表法获取激光点落在对应体素栅格的距离信息得出。
  6. 根据权利要求3所述的三维激光定位方法，其特征在于：所述粒子的系数为方差时，多次循环滤除掉方差大的粒子。
  7. 根据权利要求3所述的三维激光定位方法，其特征在于：所述粒子的系数为权重和方差的组合时，多次循环滤除掉权重小且方差大的粒子。
  8. 根据权利要求1所述的三维激光定位方法，其特征在于：所述平滑滤波处理的方法为：所述机器人低速运行时，置信所述编码器输出的预测位姿，当所述机器人高速行驶时，置信所述系数最优的粒子对应的位姿。
  9. 根据权利要求8所述的三维激光定位方法，其特征在于：所述输出机器人的最终位姿时，当置信所述编码器输出的预测位姿时，所述预测位姿的权重大于所述系数最优的粒子对应位姿的权重；当置信所述系数最优的粒子对应的位姿时，所述预测位姿的权重小于所述系数最优的粒子对应位姿的权重。
  10. 一种三维激光定位系统，其特征在于：包括：
    预测位姿模块，用于根据编码器获取的数据预测机器人某一时刻相对上一时刻的预测位姿；
    测量模块，用于在所述预测位姿附近布置多个粒子，对每个粒子进行观测，将观测到的环境信息数据与体素距离地图作对比，根据对比的相似度计算每个粒子的系数；
    重采样模块，用于根据系数滤除掉系数不优的粒子，多次循环滤除直至找出系数最优的粒子；
    处理模块，用于对系数最优的粒子对应的位姿进行平滑滤波处理，输出所述机器人的最终位姿。
PCT/CN2019/123929 2019-12-06 2019-12-09 三维激光定位方法及系统 WO2021109166A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201911244257.0 2019-12-06
CN201911244257.0A CN111076724B (zh) 2019-12-06 2019-12-06 三维激光定位方法及系统

Publications (1)

Publication Number Publication Date
WO2021109166A1 true WO2021109166A1 (zh) 2021-06-10

Family

ID=70313321

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2019/123929 WO2021109166A1 (zh) 2019-12-06 2019-12-09 三维激光定位方法及系统

Country Status (2)

Country Link
CN (1) CN111076724B (zh)
WO (1) WO2021109166A1 (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113865588B (zh) * 2021-08-24 2024-03-26 上海交通大学 一种机器人定位方法和装置

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104764457A (zh) * 2015-04-21 2015-07-08 北京理工大学 一种用于无人车的城市环境构图方法
CN105892461A (zh) * 2016-04-13 2016-08-24 上海物景智能科技有限公司 一种机器人所处环境与地图的匹配识别方法及系统
CN107991683A (zh) * 2017-11-08 2018-05-04 华中科技大学 一种基于激光雷达的机器人自主定位方法
WO2018140701A1 (en) * 2017-01-27 2018-08-02 Kaarta, Inc. Laser scanner with real-time, online ego-motion estimation

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107908185A (zh) * 2017-10-14 2018-04-13 北醒（北京）光子科技有限公司 一种机器人自主全局重定位方法及机器人
WO2019127347A1 (zh) * 2017-12-29 2019-07-04 深圳前海达闼云端智能科技有限公司 三维建图方法、装置、系统、云端平台、电子设备和计算机程序产品
CN108287550B (zh) * 2018-02-01 2020-09-11 速感科技（北京）有限公司 基于数据关联及错误检测的slam系统和构造数据关联的方法
CN108871341B (zh) * 2018-07-05 2021-12-24 内江市下一代互联网数据处理技术研究所 一种全局优化的并发定位与建图方法
CN108931245B (zh) * 2018-08-02 2021-09-07 上海思岚科技有限公司 移动机器人的局部自定位方法及设备
CN109387204B (zh) * 2018-09-26 2020-08-28 东北大学 面向室内动态环境的移动机器人同步定位与构图方法
CN110260856A (zh) * 2019-06-26 2019-09-20 北京海益同展信息科技有限公司 一种建图方法和装置
CN110412596A (zh) * 2019-07-10 2019-11-05 上海电机学院 一种基于图像信息和激光点云的机器人定位方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104764457A (zh) * 2015-04-21 2015-07-08 北京理工大学 一种用于无人车的城市环境构图方法
CN105892461A (zh) * 2016-04-13 2016-08-24 上海物景智能科技有限公司 一种机器人所处环境与地图的匹配识别方法及系统
WO2018140701A1 (en) * 2017-01-27 2018-08-02 Kaarta, Inc. Laser scanner with real-time, online ego-motion estimation
CN107991683A (zh) * 2017-11-08 2018-05-04 华中科技大学 一种基于激光雷达的机器人自主定位方法

Also Published As

Publication number Publication date
CN111076724A (zh) 2020-04-28
CN111076724B (zh) 2023-12-22

Similar Documents

Publication Publication Date Title
CN110927740B (zh) 一种移动机器人定位方法
CN112859859B (zh) 一种基于三维障碍物体素对象映射的动态栅格地图更新方法
Ding et al. An efficient b-spline-based kinodynamic replanning framework for quadrotors
US9361590B2 (en) Information processing apparatus, information processing method, and program
CN107972034B (zh) 一种基于ros平台的复杂工件轨迹规划仿真系统
Trevor et al. Omnimapper: A modular multimodal mapping framework
CN112284376A (zh) 一种基于多传感器融合的移动机器人室内定位建图方法
CN103901891A (zh) 一种基于层次结构的动态粒子树slam算法
US20220019939A1 (en) Method and system for predicting motion-outcome data of a robot moving between a given pair of robotic locations
CN112880674A (zh) 一种行驶设备的定位方法、装置、设备及存储介质
Kirsanov et al. Discoman: Dataset of indoor scenes for odometry, mapping and navigation
CN110515382A (zh) 一种智能设备及其定位方法
CN112612290A (zh) 一种考虑洋流的水下航行器三维多任务路径规划方法
Chen et al. Multilayer mapping kit for autonomous UAV navigation
Flottmann et al. Energy-efficient FPGA-accelerated LiDAR-based SLAM for embedded robotics
WO2021109166A1 (zh) 三维激光定位方法及系统
Jiang et al. 3D SLAM based on NDT matching and ground constraints for ground robots in complex environments
Dai Research on robot positioning and navigation algorithm based on SLAM
Yu et al. A hybrid point cloud alignment method combining particle swarm optimization and iterative closest point method
Chen et al. Research on adaptive Monte Carlo location method based on fusion posture estimation
CN112947481B (zh) 一种家庭服务机器人自主定位控制方法
Malhan et al. Fast, accurate, and automated 3d reconstruction using a depth camera mounted on an industrial robot
CN112747752A (zh) 基于激光里程计的车辆定位方法、装置、设备和存储介质
Chen et al. A GPU mapping system for real-time robot motion planning
CN116929338B (zh) 地图构建方法、设备及存储介质

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 19955058

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 19955058

Country of ref document: EP

Kind code of ref document: A1