CN115014352A - 一种基于建议分布地图的室内全局定位方法 - Google Patents

一种基于建议分布地图的室内全局定位方法 Download PDF

Info

Publication number
CN115014352A
CN115014352A CN202210619188.2A CN202210619188A CN115014352A CN 115014352 A CN115014352 A CN 115014352A CN 202210619188 A CN202210619188 A CN 202210619188A CN 115014352 A CN115014352 A CN 115014352A
Authority
CN
China
Prior art keywords
point cloud
map
distribution map
laser radar
particle
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
CN202210619188.2A
Other languages
English (en)
Other versions
CN115014352B (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.)
Zhejiang Qiantang Robot And Intelligent Equipment Research Co ltd
Zhejiang University ZJU
Original Assignee
Zhejiang Qiantang Robot And Intelligent Equipment Research Co ltd
Zhejiang University ZJU
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 Zhejiang Qiantang Robot And Intelligent Equipment Research Co ltd, Zhejiang University ZJU filed Critical Zhejiang Qiantang Robot And Intelligent Equipment Research Co ltd
Priority to CN202210619188.2A priority Critical patent/CN115014352B/zh
Publication of CN115014352A publication Critical patent/CN115014352A/zh
Application granted granted Critical
Publication of CN115014352B publication Critical patent/CN115014352B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
    • 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/20—Finite element generation, e.g. wire-frame surface description, tesselation
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation

Abstract

本发明公开了一种基于建议分布地图的室内全局定位方法，包括：对环境栅格地图进行预处理，获取可到达区域，得到可到达区域每个栅格点处模拟激光雷达扫描点云的集合；计算模拟激光雷达扫描点云集合中每个点云的旋转不变性几何特征集合以及质心方向角集合，计算当前状态下的激光雷达扫描点云相同的几何特征集合以及质心方向角；由上一步数据计算xy‑建议分布地图中对应栅格点处的分布概率值、以及计算θ‑建议分布地图中对应栅格点处的概率分布；采用加权随机采样的方法获得粒子滤波器的初始粒子集；用AMCL算法进行运动更新、权重计算、粒子集重采样的过程，直到粒子集收敛，收敛后的粒子集均值位姿即为全局定位结果。本发明提高了全局定位成功率和计算效率。

Description

一种基于建议分布地图的室内全局定位方法
技术领域
发明属于移动机器人技术领域，涉及一种基于建议分布地图的室内全局定位方法。
背景技术
近年来，机器人技术迅速发展和成熟，得益于人工智能技术的进步和突破，服务机器人智能化水平不断提高，应用场景逐渐增加，包括配送机器人、扫地机器人、新零售机器人、物流仓储机器人等，应用领域不断拓展。室内移动机器人产业市场广阔，年度销量持续创新高，其中，室内扫地机器人作为率先落地应用的家庭服务机器人，走进千家万户。
在移动机器人的相关技术中，定位技术和导航技术是十分重要的两大技术。全局定位是其中定位技术要解决的关键问题之一。全局定位算法中，环境地图是已知的，机器人在环境中的初始位姿是未知的，机器人会出现在任何可能的位置，这导致搜索空间非常大，是算法的最大难点，全局定位算法需要在这样的条件下求解机器人的当前位姿。此外，机器人定位绑架问题也可以转化为全局定位问题，机器人定位绑架是指机器人在运行过程中被移动到另一未知位置，导致之前的定位信息失效，当机器人被绑架时，机器人也可能出现在地图中的任何位置。
在现实场景中，真实环境的结构性差和传感器存在测量噪声，常常难以提取明晰的环境观测特征，不利于匹配。因此，提高全局定位算法的成功率和鲁棒性，仍具有很大的挑战性。目前的全局定位方法存在以下问题：
(1)常见的粒子滤波算法如自适应蒙特卡洛定位算法通过随机采样获得粒子集，均匀分布在整个位姿空间，需要大量的初始粒子，增加了算法的计算时间，计算量较大。而且大量的粒子分布在后验概率较低的地方，浪费了计算资源，定位效率低。
(2)基于结构化点线特征提取和机器学习的全局定位技术适用性差，对于一些非结构化场景，其点线结构特征不明显，且只能确定机器人大致区域。
发明内容
本发明为克服现有的全局定位方法中存在定位效率低和在非结构化场景中效果差的问题，提供一种基于建议分布地图的室内全局定位方法。
为解决上述技术问题，本发明的技术方案如下：
一种基于建议分布地图的室内全局定位方法，所述方法包括：
输入：环境栅格地图m＝{statei|(xi,yi)∈m},i＝1,...,Nmap，当前状态下的激光雷达扫描点云z*，一帧激光雷达扫描点云的点数Nr；
S1，对环境栅格地图进行预处理，获取环境栅格地图中的可到达区域，得到可到达区域每个栅格点处模拟激光雷达扫描点云的集合；
S2，计算模拟激光雷达扫描点云集合中每个点云的旋转不变性几何特征集合，计算当前状态下的激光雷达扫描点云相同的几何特征集合；
S3，由步骤S2数据计算xy-建议分布地图中对应栅格点处的分布概率值；
S4，计算模拟激光雷达扫描点云集合中每个点云的质心方向角集合，计算当前状态下的激光雷达扫描点云的质心方向角；
S5，由步骤S4数据计算θ-建议分布地图中对应栅格点处的分布概率值；
S6，采用加权随机采样的方法从建议分布地图中采样Nsample个粒子，构成粒子滤波的初始粒子集；
S7，用AMCL算法进行运动更新、权重计算、粒子集重采样的过程，直到粒子集收敛，收敛后的粒子集均值位姿即为全局定位结果。
所述步骤S1具体为：对环境栅格地图进行预处理，获取环境栅格地图中的可到达区域，遍历可到达区域中的每个点，获取该点处的模拟激光雷达扫描点云zi＝{b1,b2,...,bNr}，其中，每个激光束bi是包含该激光束相对于机器人的角度ρi和激光束长度li的元组(ρi,li)。所有点的模拟激光雷达扫描点云构成集合Z＝{z1,z2,...,zN}。
所述步骤S2具体为：
针对集合Z＝{z1,z2,...,zN}中的每个点云zi＝{b1,b2,...,bNr}，统计激光束长度li的分布直方图，组距为参数lbin，根据直方图获得点云zi的几何特征：激光束长度最大值、激光束长度最小值
Figure BDA0003674449340000021
激光束长度众数
Figure BDA0003674449340000022
集合Z中所有点云的几何特征构成集合
Figure BDA0003674449340000023
同样的方法，针对当前状态下的激光雷达扫描点云z*，计算相同的几何特征集合
Figure BDA0003674449340000024
所述步骤S3具体为：
原始栅格地图为m＝{statei|(xi,yi)∈m},i＝1,...,Nmap，xy-建议分布地图为mp＝{pi|(xi,yi)∈m}，与原始栅格地图m大小相等，pi的意义为栅格地图中(xi,yi)点处产生观测z*的概率，由该点的模拟激光雷达扫描点云zi与实际的激光雷达扫描点云z*的相似程度给出，由式(a)-(e)给出：
Figure BDA0003674449340000031
wmax+wmin+wm＝1 (b)
Figure BDA0003674449340000032
Figure BDA0003674449340000033
Figure BDA0003674449340000034
式(a)为计算xy-建议分布地图的第i点值pi的计算公式，其中wmax、wmin、wm分别为几何特征lmax、lmin、lm的权重参数，pmax(z*|xi,yi,m)、pmin(z*|xi,yi,m)、pm(z*|xi,yi,m)分别为几何特征
Figure BDA0003674449340000035
与
Figure BDA0003674449340000036
的相似度决定的概率。对集合Z＝{z1,z2,...,zN}中每个模拟激光雷达扫描点云zi进行以上计算，可以得到xy-建议分布地图为mp。
所述步骤S4具体为：
针对集合Z＝{z1,z2,...,zN}中的每个点云zi＝{b1,b2,...,bNr}，其几何特征质心方向角βi由公式(f)(g)给出：
Figure BDA0003674449340000037
Figure BDA0003674449340000038
式(f)中，(xc,yc)为zi的质心，(xo,yo)为产生点云zi的模拟激光雷达中心坐标。集合Z中所有点云的质心方向角构成集合Β＝{βi|i＝1,...,N}，同样的方法，针对当前状态下的激光雷达扫描点云z*，计算质心方向角β*。
所述步骤S5具体为：
θ-建议分布地图mθ与原始栅格地图m大小相等，由该点的模拟激光雷达扫描点云zi的质心方向角βi与实际的激光雷达扫描点云z*的质心方向角β*计算得到，θ-建议分布地图为mθ＝{N(βi-β*,σ)|(xi,yi)∈m}，N(βi-β*,σ)表示均值为βi-β*，标准差为σ的高斯分布，σ为参数。
所述步骤S6具体为：
S6.1，初始粒子数Nsample由xy-建议分布地图mp＝{pi|(xi,yi)∈m}和式(h)计算得到：
Figure BDA0003674449340000041
S6.2，由xy-建议分布地图mp＝{pi|(xi,yi)∈m}和式(i)计算地图中每个栅格点处的累加权重值，形成累加权重数组W＝{wi|i＝0,...,Nmap}：
wi＝wi-1+pi (i)
S6.3，进行Nsample次粒子采样获得粒子集S，每次采样的过程为，记权重总和为累加权重数组的最后一个数
Figure BDA0003674449340000042
生成一个[0,1]之间的随机数r，在W＝{wi|i＝0,...,Nmap}中寻找第一个使wi＞wsum·r成立的i，从栅格地图m中获得第i点坐标(xi,yi)作为该粒子xy坐标。从θ-建议分布地图mθ中获取该粒子的θ建议分布N(θ,σ)，从中随机采样获取θi作为粒子的角度坐标，组成(xi,yi,θi)粒子加入粒子集S中。
所述步骤S7具体为：将粒子集S作为初始粒子集，根据AMCL算法循环进行运动更新、权重计算、KLD重采样的过程，直到粒子集收敛，收敛后的粒子集均值位姿即为全局定位结果。
本发明提供的一种基于建议分布地图的室内全局定位方法具有如下优点：
(1)构建建议分布地图近似描述机器人位姿的后验分布，利用加权随机采样的方法，获取更合理的初始粒子集，提高了全局定位的成功率和计算效率。
(2)根据建议分布地图的有效栅格数量动态调整初始粒子数，避免固定的较大规模的粒子数对不同分布的表达精度不一致，造成计算资源的浪费。
附图说明
下面结合附图和实施例对本发明进一步说明。
图1是本申请实施例提供的一种基于建议分布地图的室内全局定位方法实现流程图；
图2是本申请实施例使用的场景栅格地图；
图3是本申请实施例中机器人在五角星所示位置时获取的当前激光雷达扫描点云；
图4是本申请实施例中计算的xy-建议分布地图；
图5是本申请实施例中采样得到的初始粒子集；
图6是本申请实施例中定位结果；
具体实施方法
为了使本发明实现的技术方案、创作特征、达成目的易于明白了解，下面结合具体图示，进一步阐述本发明。下面参照附图说明本发明的具体实施方式。为了全面理解本发明，下面描述到许多细节,但对于本领域技术人员来说,无需这些细节也可实现本发明。
如图1所示，本申请实施例提供了一种基于建议分布地图的室内全局定位方法，能够使得室内移动机器人根据自身携带的激光雷达的测量数据，获得在地图中的全局定位。所述方法可以包括步骤S1-S7，其中步骤S1、S2、S4中关于栅格地图和模拟激光雷达扫描点云的计算为预处理阶段，可以提前计算好保存为离线数据，减少在线处理计算量。
输入：环境栅格地图m＝{statei|(xi,yi)∈m},i＝1,...,Nmap，如图2所示。当前状态下的激光雷达扫描点云z*，如图3所示。一帧激光雷达扫描点云的点数Nr，在本实施例中为360。
步骤S1：对环境栅格地图进行预处理，获取环境栅格地图中的可到达区域，得到可到达区域每个栅格点处模拟激光雷达扫描点云的集合。具体为：对环境栅格地图进行预处理，获取环境栅格地图中的可到达区域，遍历可到达区域中的每个点，获取该点处的模拟激光雷达扫描点云zi＝{b1,b2,...,bNr}，其中，每个激光束bi是包含该激光束相对于机器人的角度ρi和激光束长度li的元组(ρi,li)。所有点的模拟激光雷达扫描点云构成集合Z＝{z1,z2,...,zN}。
步骤S2：计算模拟激光雷达扫描点云集合中每个点云的旋转不变性几何特征集合，计算当前状态下的激光雷达扫描点云相同的几何特征集合。具体为：针对集合Z＝{z1,z2,...,zN}中的每个点云zi＝{b1,b2,...,bNr}，统计激光束长度li的分布直方图，组距为参数lbin，根据直方图获得点云zi的几何特征：激光束长度最大值、激光束长度最小值
Figure BDA0003674449340000061
激光束长度众数
Figure BDA0003674449340000062
集合Z中所有点云的几何特征构成集合
Figure BDA0003674449340000063
同样的方法，针对当前状态下的激光雷达扫描点云z*，计算相同的几何特征集合
Figure BDA0003674449340000064
步骤S3，由步骤S2数据计算xy-建议分布地图中对应栅格点处的分布概率值。具体为：原始栅格地图为m＝{statei|(xi,yi)∈m},i＝1,...,Nmap，xy-建议分布地图为mp＝{pi|(xi,yi)∈m}，与原始栅格地图m大小相等，pi的意义为栅格地图中(xi,yi)点处产生观测z*的概率，由该点的模拟激光雷达扫描点云zi与实际的激光雷达扫描点云z*的相似程度给出，由式(a)-(e)给出：
Figure BDA0003674449340000065
wmax+wmin+wm＝1 (b)
Figure BDA0003674449340000066
Figure BDA0003674449340000067
Figure BDA0003674449340000068
式(a)为计算xy-建议分布地图的第i点值pi的计算公式，其中wmax、wmin、wm分别为几何特征lmax、lmin、lm的权重参数，pmax(z*|xi,yi,m)、pmin(z*|xi,yi,m)、pm(z*|xi,yi,m)分别为几何特征
Figure BDA0003674449340000069
与
Figure BDA00036744493400000610
的相似度决定的概率。对集合Z＝{z1,z2,...,zN}中每个模拟激光雷达扫描点云zi进行以上计算，可以得到xy-建议分布地图为mp。如图4为归一化后的xy-建议分布地图，图中除墙体外颜色越深的位置概率越大，白色位置处概率为0。
步骤S4，计算模拟激光雷达扫描点云集合中每个点云的质心方向角集合，计算当前状态下的激光雷达扫描点云的质心方向角。具体为：针对集合Z＝{z1,z2,...,zN}中的每个点云zi＝{b1,b2,...,bNr}，其几何特征质心方向角βi由公式(f)(g)给出：
Figure BDA0003674449340000071
Figure BDA0003674449340000072
式(f)中，(xc,yc)为zi的质心，(xo,yo)为产生点云zi的模拟激光雷达中心坐标。集合Z中所有点云的质心方向角构成集合Β＝{βi|i＝1,...,N}，同样的方法，针对当前状态下的激光雷达扫描点云z*，计算质心方向角β*。
步骤S5，由步骤S4数据计算θ-建议分布地图中对应栅格点处的分布概率值。具体为：θ-建议分布地图mθ与原始栅格地图m大小相等，由该点的模拟激光雷达扫描点云zi的质心方向角βi与实际的激光雷达扫描点云z*的质心方向角β*计算得到，θ-建议分布地图为mθ＝{N(βi-β*,σ)|(xi,yi)∈m}，N(βi-β*,σ)表示均值为βi-β*，标准差为σ的高斯分布，σ为参数。
步骤S6，采用加权随机采样的方法从建议分布地图中采样Nsample个粒子，构成粒子滤波的初始粒子集。具体为：
S6.1，初始粒子数Nsample由xy-建议分布地图mp＝{pi|(xi,yi)∈m}和式(h)计算得到：
Figure BDA0003674449340000073
S6.2，由xy-建议分布地图mp＝{pi|(xi,yi)∈m}和式(i)计算地图中每个栅格点处的累加权重值，形成累加权重数组W＝{wi|i＝0,...,Nmap}：
wi＝wi-1+pi (i)
S6.3，进行Nsample次粒子采样获得粒子集S，每次采样的过程为，记权重总和为累加权重数组的最后一个数
Figure BDA0003674449340000074
生成一个[0,1]之间的随机数r，在W＝{wi|i＝0,...,Nmap}中寻找第一个使wi＞wsum·r成立的i，从栅格地图m中获得第i点坐标(xi,yi)作为该粒子xy坐标。从θ-建议分布地图mθ中获取该粒子的θ建议分布N(θ,σ)，从中随机采样获取θi作为粒子的角度坐标，组成(xi,yi,θi)粒子加入粒子集S中。如图5为采样得到的初始粒子集，该粒子集覆盖了真实位姿，且相比全图的均匀分布，使用的粒子数更少，更精确。
步骤S7，用AMCL算法进行运动更新、权重计算、粒子集重采样的过程，直到粒子集收敛，收敛后的粒子集均值位姿即为全局定位结果。具体为：将粒子集S作为初始粒子集，根据AMCL算法循环进行运动更新、权重计算、KLD重采样的过程，直到粒子集收敛，收敛后的粒子集均值位姿即为全局定位结果。如图6所示收敛后的粒子集为真实位姿。
需要说明的是，以上仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换，但是凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种基于建议分布地图的室内全局定位方法，其特征在于，所述方法包括：
输入：环境栅格地图m＝{statei|(xi,yi)∈m},i＝1,...,Nmap，其中Nmap为栅格地图的栅格总数，当前状态下的激光雷达扫描点云z*，一帧激光雷达扫描点云的点数Nr；
S1，对环境栅格地图进行预处理，获取环境栅格地图中的可到达区域，得到可到达区域所有栅格点处模拟激光雷达扫描点云的集合；
S2，计算模拟激光雷达扫描点云集合中每个点云的旋转不变性几何特征集合，计算当前状态下的激光雷达扫描点云相同的几何特征集合；
S3，由步骤S2数据计算xy-建议分布地图中对应栅格点处的分布概率值；
S4，计算模拟激光雷达扫描点云集合中每个点云的质心方向角集合，计算当前状态下的激光雷达扫描点云的质心方向角；
S5，由步骤S4数据计算θ-建议分布地图中对应栅格点处的概率分布；
S6，采用加权随机采样的方法从建议分布地图中采样粒子，构成粒子滤波器的初始粒子集；
S7，用AMCL算法进行运动更新、权重计算、粒子集重采样的过程，直到粒子集收敛，收敛后的粒子集均值位姿即为全局定位结果。
2.根据权利要求1所述的基于建议分布地图的室内全局定位方法，其特征在于，所述步骤S1具体为：对环境栅格地图进行预处理，获取环境栅格地图中的可到达区域，遍历可到达区域中的每个点，获取该点处的模拟激光雷达扫描点云zi＝{b1,b2,...,bNr}，其中，每个激光束bi是包含该激光束相对于机器人的角度ρi和激光束长度li的元组(ρi,li)，Nr为模拟激光雷达扫描点云的点数；所有点的模拟激光雷达扫描点云构成集合Z＝{z1,z2,...,zN}，N为可到达区域中点数。
3.根据权利要求2所述的基于建议分布地图的室内全局定位方法，其特征在于，所述步骤S2具体为：
针对集合Z＝{z1,z2,...,zN}中的每个点云zi＝{b1,b2,...,bNr}，统计激光束长度li的分布直方图，组距为参数lbin，根据直方图获得点云zi的几何特征：激光束长度最大值、激光束长度最小值
Figure FDA0003674449330000021
激光束长度众数
Figure FDA0003674449330000022
集合Z中所有点云的几何特征构成集合
Figure FDA0003674449330000023
同样的方法，针对当前状态下的激光雷达扫描点云z*，计算相同的几何特征集合
Figure FDA0003674449330000024
4.根据权利要求3所述的基于建议分布地图的室内全局定位方法，其特征在于，所述步骤S3具体为：
原始栅格地图为m＝{statei|(xi,yi)∈m},i＝1,...,Nmap，xy-建议分布地图为mp＝{pi|(xi,yi)∈m}，与原始栅格地图m大小相等，pi的意义为栅格地图中(xi,yi)点处产生观测z*的概率，由该点的模拟激光雷达扫描点云zi与实际的激光雷达扫描点云z*的相似程度给出，由式(a)-(e)给出：
Figure FDA0003674449330000025
wmax+wmin+wm＝1 (b)
Figure FDA0003674449330000026
Figure FDA0003674449330000027
Figure FDA0003674449330000028
式(a)为计算xy-建议分布地图的第i点值pi的计算公式，其中wmax、wmin、wm分别为几何特征lmax、lmin、lm的权重参数，pmax(z*|xi,yi,m)、pmin(z*|xi,yi,m)、pm(z*|xi,yi,m)分别为几何特征
Figure FDA0003674449330000029
与
Figure FDA00036744493300000210
的相似度决定的概率。对集合Z＝{z1,z2,...,zN}中每个模拟激光雷达扫描点云zi进行以上计算，可以得到xy-建议分布地图为mp。
5.根据权利要求2所述的基于建议分布地图的室内全局定位方法，其特征在于，所述步骤S4具体为：
针对集合Z＝{z1,z2,...,zN}中的每个点云zi＝{b1,b2,...,bNr}，其几何特征质心方向角βi由公式(f)(g)给出：
Figure FDA0003674449330000031
Figure FDA0003674449330000032
式(f)中，(xc,yc)为zi的质心，(xo,yo)为产生点云zi的模拟激光雷达中心坐标。集合Z中所有点云的质心方向角构成集合Β＝{βi|i＝1,...,N}，同样的方法，针对当前状态下的激光雷达扫描点云z*，计算质心方向角β*。
6.根据权利要求5所述的基于建议分布地图的室内全局定位方法，其特征在于，所述步骤S5具体为：
θ-建议分布地图mθ与原始栅格地图m大小相等，由该点的模拟激光雷达扫描点云zi的质心方向角βi与实际的激光雷达扫描点云z*的质心方向角β*计算得到，θ-建议分布地图为mθ＝{N(βi-β*,σ)|(xi,yi)∈m}，N(βi-β*,σ)表示均值为βi-β*，标准差为σ的高斯分布，σ为参数。
7.根据权利要求6所述的基于建议分布地图的室内全局定位方法，其特征在于，所述步骤S6具体为：
S6.1，初始粒子数Nsample由xy-建议分布地图mp＝{pi|(xi,yi)∈m}和式(h)计算得到，式(h)中，c为固定比例参数。
Figure FDA0003674449330000033
S6.2，由xy-建议分布地图mp＝{pi|(xi,yi)∈m}和式(i)计算地图中每个栅格点处的累加权重值，形成累加权重数组W＝{wi|i＝0,...,Nmap}：
wi＝wi-1+pi (i)
S6.3，进行Nsample次粒子采样获得粒子集S，每次采样的过程为，记权重总和为累加权重数组的最后一个数
Figure FDA0003674449330000041
生成一个[0,1]之间的随机数r，在W＝{wi|i＝0,...,Nmap}中寻找第一个使wi＞wsum·r成立的i，从栅格地图m中获得第i点坐标(xi,yi)作为该粒子xy坐标，从θ-建议分布地图mθ中获取该粒子的θ建议分布N(θ,σ)，从中随机采样获取θi作为粒子的角度坐标，组成(xi,yi,θi)粒子加入粒子集S中。
8.根据权利要求7所述的基于建议分布地图的室内全局定位方法，其特征在于，所述步骤S7具体为：将粒子集S作为初始粒子集，根据AMCL算法循环进行运动更新、权重计算、KLD重采样的过程，直到粒子集收敛，收敛后的粒子集均值位姿即为全局定位结果。
CN202210619188.2A 2022-06-01 2022-06-01 一种基于建议分布地图的室内全局定位方法 Active CN115014352B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210619188.2A CN115014352B (zh) 2022-06-01 2022-06-01 一种基于建议分布地图的室内全局定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210619188.2A CN115014352B (zh) 2022-06-01 2022-06-01 一种基于建议分布地图的室内全局定位方法

Publications (2)

Publication Number Publication Date
CN115014352A true CN115014352A (zh) 2022-09-06
CN115014352B CN115014352B (zh) 2023-05-26

Family

ID=83072942

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210619188.2A Active CN115014352B (zh) 2022-06-01 2022-06-01 一种基于建议分布地图的室内全局定位方法

Country Status (1)

Country Link
CN (1) CN115014352B (zh)

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106949895A (zh) * 2017-04-13 2017-07-14 杭州申昊科技股份有限公司 一种适用于变电站环境下的巡检机器人定位方法
CN107991683A (zh) * 2017-11-08 2018-05-04 华中科技大学 一种基于激光雷达的机器人自主定位方法
CN109682382A (zh) * 2019-02-28 2019-04-26 电子科技大学 基于自适应蒙特卡洛和特征匹配的全局融合定位方法
CN110568447A (zh) * 2019-07-29 2019-12-13 广东星舆科技有限公司 视觉定位的方法、装置及计算机可读介质
DE102018219773A1 (de) * 2018-11-19 2020-05-20 Audi Ag Verfahren zum Kartographieren einer örtlichen Verteilung von Ereignissen eines vorbestimmten Ereignistyps in einem vorbestimmten Umgebungsbereich eines Kraftfahrzeugs sowie dazu ausgelegtes Steuergerät und Kraftfahrzeug
CN111830985A (zh) * 2020-07-24 2020-10-27 中南大学 一种多机器人定位方法、系统及集中式通信系统
CN112082553A (zh) * 2020-07-24 2020-12-15 广州易来特自动驾驶科技有限公司 基于wifi和激光雷达的室内定位方法、定位装置和机器人
CN112882056A (zh) * 2021-01-15 2021-06-01 西安理工大学 基于激光雷达的移动机器人同步定位与地图构建方法
JP2021144149A (ja) * 2020-03-12 2021-09-24 ヤンマーパワーテクノロジー株式会社 地図生成方法および地図生成装置
CN113483747A (zh) * 2021-06-25 2021-10-08 武汉科技大学 基于具有墙角信息的语义地图改进amcl定位方法及机器人
CN114043479A (zh) * 2021-11-24 2022-02-15 泉州装备制造研究所 一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106949895A (zh) * 2017-04-13 2017-07-14 杭州申昊科技股份有限公司 一种适用于变电站环境下的巡检机器人定位方法
CN107991683A (zh) * 2017-11-08 2018-05-04 华中科技大学 一种基于激光雷达的机器人自主定位方法
DE102018219773A1 (de) * 2018-11-19 2020-05-20 Audi Ag Verfahren zum Kartographieren einer örtlichen Verteilung von Ereignissen eines vorbestimmten Ereignistyps in einem vorbestimmten Umgebungsbereich eines Kraftfahrzeugs sowie dazu ausgelegtes Steuergerät und Kraftfahrzeug
CN109682382A (zh) * 2019-02-28 2019-04-26 电子科技大学 基于自适应蒙特卡洛和特征匹配的全局融合定位方法
CN110568447A (zh) * 2019-07-29 2019-12-13 广东星舆科技有限公司 视觉定位的方法、装置及计算机可读介质
JP2021144149A (ja) * 2020-03-12 2021-09-24 ヤンマーパワーテクノロジー株式会社 地図生成方法および地図生成装置
CN111830985A (zh) * 2020-07-24 2020-10-27 中南大学 一种多机器人定位方法、系统及集中式通信系统
CN112082553A (zh) * 2020-07-24 2020-12-15 广州易来特自动驾驶科技有限公司 基于wifi和激光雷达的室内定位方法、定位装置和机器人
CN112882056A (zh) * 2021-01-15 2021-06-01 西安理工大学 基于激光雷达的移动机器人同步定位与地图构建方法
CN113483747A (zh) * 2021-06-25 2021-10-08 武汉科技大学 基于具有墙角信息的语义地图改进amcl定位方法及机器人
CN114043479A (zh) * 2021-11-24 2022-02-15 泉州装备制造研究所 一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
张恒;樊晓平;瞿志华;: "基于多假设跟踪的移动机器人自适应蒙特卡罗定位研究" *
章弘凯;陈年生;范光宇;: "基于粒子滤波的智能机器人定位算法" *

Also Published As

Publication number Publication date
CN115014352B (zh) 2023-05-26

Similar Documents

Publication Publication Date Title
CN106970375B (zh) 一种机载激光雷达点云中自动提取建筑物信息的方法
CN111060888B (zh) 一种融合icp和似然域模型的移动机器人重定位方法
CN111696210A (zh) 一种基于三维点云数据特征轻量化的点云重构方法及系统
CN109284677B (zh) 一种贝叶斯滤波目标跟踪算法
CN109323697B (zh) 一种针对室内机器人任意点启动时粒子快速收敛的方法
CN113345008B (zh) 考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法
CN113628263A (zh) 一种基于局部曲率及其近邻特征的点云配准方法
CN114200477A (zh) 一种激光三维成像雷达地面目标点云数据处理方法
CN113483747A (zh) 基于具有墙角信息的语义地图改进amcl定位方法及机器人
CN111415379A (zh) 一种基于布谷鸟优化的三维点云数据配准方法
CN115222625A (zh) 一种基于多尺度噪声的激光雷达点云去噪方法
CN111783722B (zh) 一种激光点云的车道线提取方法和电子设备
CN114663373A (zh) 一种用于零件表面质量检测的点云配准方法及装置
CN113450269A (zh) 一种基于3d视觉的点云关键点提取方法
CN114066773B (zh) 一种基于点云特征与蒙特卡洛扩展法的动态物体去除
CN113034554A (zh) 基于混沌反向学习的鲸鱼优化的破损俑体碎片配准方法
CN117274339A (zh) 一种基于改进的iss-3dsc特征结合icp的点云配准方法
CN116109601A (zh) 一种基于三维激光雷达点云的实时目标检测方法
CN111765883B (zh) 机器人蒙特卡罗定位方法、设备及存储介质
CN114137562A (zh) 一种基于改进全局最邻近的多目标跟踪方法
CN112070787B (zh) 基于对立推理理论的航空三维点云平面分割方法
CN115661218B (zh) 一种基于虚拟超点的激光点云配准方法和系统
CN115014352B (zh) 一种基于建议分布地图的室内全局定位方法
CN116563354A (zh) 一种结合特征提取和聚类算法的激光点云配准方法
CN116452604A (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