CN112068547B - 一种基于amcl的机器人定位方法、装置和机器人 - Google Patents

一种基于amcl的机器人定位方法、装置和机器人 Download PDF

Info

Publication number
CN112068547B
CN112068547B CN202010777182.9A CN202010777182A CN112068547B CN 112068547 B CN112068547 B CN 112068547B CN 202010777182 A CN202010777182 A CN 202010777182A CN 112068547 B CN112068547 B CN 112068547B
Authority
CN
China
Prior art keywords
particle
matching degree
map picture
laser
particles
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
CN202010777182.9A
Other languages
English (en)
Other versions
CN112068547A (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.)
Goertek Inc
Original Assignee
Goertek Inc
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 Goertek Inc filed Critical Goertek Inc
Priority to CN202010777182.9A priority Critical patent/CN112068547B/zh
Publication of CN112068547A publication Critical patent/CN112068547A/zh
Application granted granted Critical
Publication of CN112068547B publication Critical patent/CN112068547B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G05D1/0248—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means in combination with a laser
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/004—Artificial life, i.e. computing arrangements simulating life
    • G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Biophysics (AREA)
  • Computational Linguistics (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Molecular Biology (AREA)
  • Computing Systems (AREA)
  • Biomedical Technology (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Optics & Photonics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Electromagnetism (AREA)
  • Health & Medical Sciences (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本申请公开一种基于AMCL的机器人定位方法、装置和机器人。本申请的方法包括：获取机器人在当前时刻采集的激光数据，以及获取当前时刻的地图图片；利用所述激光数据与所述地图图片，对由AMCL算法预测得到的初始粒子群中的粒子分布进行优化处理，得到优化后的粒子群，优化后的粒子群中粒子分布较初始粒子群中粒子分布集中；采用AMCL算法对优化后的粒子群进行处理，实现机器人的实时定位。本申请的技术方案能够对AMCL算法预测的初始粒子群进行优化处理，使优化后的粒子群中粒子的分布更为集中，从而可以使用数量更少的粒子来表示机器人位置的概率分布，降低机器人定位所需的计算资源与计算代价。

Description

一种基于AMCL的机器人定位方法、装置和机器人
技术领域
本申请涉及计算机技术领域，具体涉及一种基于AMCL的机器人定位方法、装置和机器人。
背景技术
机器人定位技术是机器人能够实现自主导航的关键技术之一，定位的效率和准确性则是评判定位好坏的关键标准。AMCL(adaptive MonteCarlo Localization，自适应蒙特卡洛定位)作为2d平面定位技术，由于其便于操作、封装较好、便于上手等特点，被广泛应用于机器人定位。
AMCL是通过表示概率分布的大量粒子来描述机器人位置的，粒子的数量会影响定位结果的准确性。粒子越多，越能代表机器人位置的概率分布，但粒子数量越多，需要的计算资源越多，计算代价越大。
发明内容
本申请提供了一种基于AMCL的机器人定位方法、装置和机器人，用于解决或部分解决上述问题。
一方面，本申请提供了一种基于AMCL的机器人定位方法，包括：
获取机器人在当前时刻采集的激光数据，以及获取当前时刻的地图图片；
利用激光数据与所述地图图片，对由AMCL算法预测得到的初始粒子群中的粒子分布进行优化处理，得到优化后的粒子群，优化后的粒子群中粒子分布较初始粒子群中粒子分布集中；
采用AMCL算法对优化后的粒子群进行处理，实现机器人的实时定位。
另一方面，本申请提供了一种基于AMCL的机器人定位装置，该装置包括：
数据采集单元，用于获取机器人在当前时刻采集的激光数据，以及获取当前时刻的地图图片；
粒子群优化单元，用于利用激光数据与所述地图图片，对由AMCL算法预测得到的初始粒子群中的粒子分布进行优化处理，得到优化后的粒子群，优化后的粒子群中粒子分布较初始粒子群中粒子分布集中；
机器人定位单元，采用AMCL算法对优化后的粒子群进行处理，实现机器人的实时定位。
再一方面，本申请提供了一种机器人，包括存储器、处理器和激光器；
激光器，采集机器人周边的激光数据并发送给处理器；
存储器，存储计算机可执行指令；
处理器，根据计算机可执行指令执行上述基于AMCL的机器人定位方法。
又一方面，本申请提供了一种计算机可读存储介质，计算机可读存储介质上存储有一个或多个计算机程序，一个或多个计算机程序被执行时实现基于AMCL的机器人定位方法。
本申请的有益效果是：利用当前时刻采集到的激光数据与地图图片对AMCL算法预测的初始粒子群进行优化处理，使优化后的粒子群中粒子的分布更为集中，从而可以使用数量更少的粒子来表示机器人位置的概率分布，降低机器人定位所需的计算资源与计算代价。
附图说明
图1是本申请示出的基于AMCL算法得到的粒子群示意图；
图2是本申请一个实施例示出的基于AMCL的机器人定位方法流程图；
图3是本申请一个实施例示出的机器人位置的概率密度函数曲线示意图；
图4是本申请一个实施例示出的基于AMCL的机器人定位装置的功能框图；
图5是本申请一个实施例示出的机器人的结构框图；
图6是本申请一个实施例的计算机可读存储介质的结构示意图。
具体实施方式
这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本申请相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本申请的一些方面相一致的装置和方法的例子。
在本申请使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请。在本申请和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。
应当理解，尽管在本申请可能采用术语第一、第二、第三等来描述各种信息，但这些信息不应限于这些术语。这些术语仅用来将同一类型的信息彼此区分开。例如，在不脱离本申请范围的情况下，第一信息也可以被称为第二信息，类似地，第二信息也可以被称为第一信息。取决于语境，如在此所使用的词语“如果”可以被解释成为“在……时”或“当……时”或“响应于确定”。
基于粒子滤波的AMCL机器人定位算法，用粒子代表机器人的位置，通过寻找一组状态空间的随机粒子来近似表示机器人位置概率密度函数。概率密度函数可以由方差来评判，方差越大概率密度函数范围越广，方差越小概率密度函数范围越窄。
参考图1，机器人沿直线行走，刚开始机器人在图1中的起始位置，所有的粒子也都集中在起始位置，表现为一个聚集的点。机器人沿直线向前移动一步，由于里程计的误差，所有粒子不在集中在一个点上，而是变成在一个小范围聚堆的粒子群，图1中示出机器人共行走了十一步，后面几步粒子群的分布范围变得很大，已经无法准确描述机器人位置。粒子群分布范围越来越广是因为机器人行走过程中里程计引进的误差导致概率密度函数的方差越来越大，概率密度函数越来越广，此时用于描述机器人位置的粒子群中粒子的数量将非常多，需要较多的计算资源，计算代价较大。
本申请考虑利用里程计数据构建的运动模型预测到的粒子分布误差较大，引入激光数据对AMCL算法的运动模型预测的到的粒子群进行优化，如图2所示，是本申请一个实施例示出的基于AMCL的机器人定位方法流程图，包括如下步骤：
步骤S210，获取机器人在当前时刻采集的激光数据，以及获取当前时刻的地图图片。
本步骤中，每时刻采集到的激光数据表现为图像形式为一帧激光图像数据，若激光器的扫描角度为1°，则一帧激光图像数据包括360个激光散点，每个激光散点对应表示每个扫描角度下的距离信息。
本步骤中，地图图片为栅格图片。
步骤S220，利用激光数据与地图图片，对由AMCL算法预测得到的初始粒子群中的粒子分布进行优化处理，得到优化后的粒子群，优化后的粒子群中粒子分布较初始粒子群中粒子分布集中。
本步骤中，由AMCL算法预测得到的初始粒子群为由AMCL算法的运动模型预测输出的粒子群，在AMCL算法中，每个粒子表示机器人的位姿，粒子群表示机器人位姿的概率分布。
由于里程计运动模型比激光雷达观测模型误差更大，因此引入激光数据可以起到矫正里程计，优化机器人位姿概率分布的作用。
步骤S230，采用AMCL算法对优化后的粒子群进行处理，实现机器人的实时定位。
由图2所示可知，本实施例利用当前时刻采集到的激光数据与地图图片对AMCL算法预测的初始粒子群进行优化处理，使优化后的粒子群中粒子的分布更为集中，从而可以使用数量更少的粒子来表示机器人位置的概率分布，降低机器人定位所需的计算资源与计算代价。
下面以一个机器人为例，结合图3该机器人进行定位的实现步骤进行具体说明。
上述步骤S220中的优化处理过程包括：
S2210，利用激光数据对初始粒子群中的每个粒子计算粒子在地图图片的不同位置处与地图图片之间的匹配度，得到每个粒子与地图图片之间的最大匹配度与目标位置，粒子的目标位置为取得最大匹配度时粒子在地图图片中的位置；
S2220，根据粒子群中粒子的最大匹配度与预设条件之间的关系，利用粒子的目标位置对初始粒子群中该粒子的位置进行优化，得到优化后的粒子群。
在一些实施例中，可以将上述优化过程构建为一种帧间匹配优化模型，该帧间匹配优化模型的输入包括激光数据、地图图片和初始粒子群，利用激光数据与地图图片对初始粒子群(即初始粒子群的位置)进行迭代更新，该帧间匹配优化模型在更新了初始粒子群的位置后输出，在下个获取到激光数据与地图图片的时刻，再次将AMCL下一时刻输出的初始粒子群、下一时刻获取到的激光数据与地图图片输入到帧间匹配优化模型中，进行迭代更新。
上述步骤S2210计算粒子在地图图片的不同位置处与地图图片之间的匹配度，得到每个粒子与地图图片之间的最大匹配度与目标位置的方法包括：
根据粒子群中每个粒子的位置信息获取每个粒子在地图图片中的粒子初始坐标(xl_0，yl_0)；根据每个粒子的粒子初始坐标(xl_0，yl_0)获取多个激光点在地图图片中的激光点坐标，激光点为激光数据映射到地图图片中的点；根据激光点坐标计算每个粒子在粒子初始坐标下与地图图片之间的匹配度，在粒子初始坐标下计算得到的匹配度为每个粒子的参考匹配度Mref；在地图图片中每个粒子的第一预设范围内移动粒子，并计算粒子在每个移动位置处与地图图片之间匹配度，在每个移动位置处计算得到的匹配度为每个粒子的观测匹配度Mobs；根据每个粒子的参考匹配度Mref与其在每个移动位置处的观测匹配度Mobs的比较结果，确定每个粒子与地图图片之间的最大匹配度。
其中，可以按照预设步长L与预设角度α在在地图图片中每个粒子的第一预设范围内移动粒子，例如沿着预设方向按照预设步长L与预设角度α移动粒子，例如沿着前、后、左、右、左前、右前六个方向移动按照预设步长L与预设角度α移动粒子，基于移动后的粒子坐标(xl_1，yl_1)更新激光点坐标，利用更新后的激光点坐标计算出粒子在移动位置处与地图图片之间的观测匹配度Mobs_1；
比较观测匹配度Mobs_1与参考匹配度Mref，若观测匹配度Mobs_1大于参考匹配度Mref，以当前移动位置(xl_1，yl_1)为基准，收缩预设步长与预设角度，例如预设步长与预设角度均减半，得到收缩后的预设步长L/2与预设角度α/2，再沿着预设方向按照收缩后的预设步长L/2与收缩后的预设角度移α/2动粒子，基于移动后的粒子坐标(xl_2，yl_2)更新激光点坐标，利用更新后的激光点坐标计算出粒子在新的移动位置处(xl_2，yl_2)与地图图片之间的观测匹配度Mobs_2，若新的移动位置处(xl_2，yl_2)的观测匹配度Mobs_2大于各个在前移动位置处的观测匹配度，即若此时观测匹配度Mobs_2大于观测匹配度Mobs_1，则以新的移动位置(xl_2，yl_2)为基准，继续收缩预设步长与预设角度，得到收缩后的预设步长L/4与预设角度α/4，再沿着预设方向按照收缩后的预设步长L/4与收缩后的预设角度α/4移动粒子，并基于移动后的粒子坐标(xl_3，yl_3)继续更新激光点坐标，利用更新后的激光点坐标计算出粒子在移动位置处(xl_3，yl_3)与地图图片之间的观测匹配度Mobs_3，直至观测匹配度Mobs_3不大于前一移动位置处的观测匹配度(即观测匹配度Mobs_2)停止移动；若观测匹配度Mobs_1不大于参考匹配度Mref，则将粒子保持在其初始坐标位置处。
在一些实施例中，每个粒子在粒子初始坐标下与地图图片之间的匹配度的计算方法包括：
计算在每个激光点坐标的第二预设范围内，地图图片中是否存在与激光点距离小于预设距离的物点，物点为环境中可以被激光检测到并形成在地图图片中的点，例如地图图片中用于表示障碍物的点即为一种物点；若存在，判断该激光点为有效激光点，并基于该物点坐标与该有效激光点坐标计算得到初始匹配度(exp(-1/m_gaussianSigma*bestMu*bestMu))，其中，m_gaussianSigma是系统参数可以根据实际情况设定，bestMu是激光点坐标与物点坐标在水平方向和竖直方向上横纵坐标差值的平方和)；若不存在，则计算在激光数据的其他激光点中是否存在其他有效激光点，并计算其他有效激光点的初始匹配度；计算全部有效激光点的初始匹配度的和值，将所得结果作为粒子的参考匹配度。
一个示例中，当前时刻采集到的激光数据映射到地图图片中有360个激光点，在计算每个粒子在初始坐标下与地图图片之间的匹配度时，遍历这360个激光点，对每个激光点确认是否存在与该激光点距离小于预设距离的物点，如果存在，则该激光点为有效激光点，如果不存在，则该激光点为无效激光点。将所有有效激光点对应的初始匹配度累加所得即为该粒子在其初始位置与地图图片之间的匹配度。其中，预设距离可以根据经验设置。
需要说明的是，每个粒子在其他移动位置处与地图图片之间的匹配度的计算方法与上述计算每个粒子在初始坐标位置处于地图图片之间的匹配度的方法相同，在此不再赘述。
一个实施例中，基于移动后的粒子坐标更新激光点坐标的方法包括：获取激光数据每个扫描角度下的距离信息，每个扫描角度下的距离信息对应一个激光点；以每个粒子移动后的粒子坐标为基准，参考每个扫描角度下的距离信息，计算得到该扫描角度下的距离信息映射到地图图片中的激光点的坐标。
相应的，基于每个粒子的粒子初始坐标获取多个激光点在地图图片中的激光点坐标亦是获取激光数据每个扫描角度下的距离信息，每个扫描角度下的距离信息对应一个激光点；以每个粒子的粒子初始坐标为基准，参考每个扫描角度下的距离信息，计算得到该扫描角度下的距离信息映射到地图图片中的激光点的坐标。
上述步骤S2220对初始粒子群中粒子的位置进行优化的方法包括：若粒子群中每个粒子与地图图片之间的最大匹配度均大于预设阈值，将该粒子的目标位置作为该粒子优化后的分布位置；若粒子群中有至少一个粒子与地图图片之间的最大匹配度不大于预设阈值，则认为激光器信息不可靠，不优化该粒子的分布位置。
在获得优化后的粒子群之后，即可利用AMCL算法的观测模型对优化后的粒子群进行权重评估，更新优化后的粒子群的权重值；对更新权重值后的粒子群进行重采样处理，根据重采样处理后的粒子群中权重值最大的粒子的位置确定机器人当前时刻的位姿。
参考图3，虚线是基于AMCL算法的运动模型预测输出的机器人概率分布曲线，粒子分布范围很广，粒子分布较为分散；而图3中实线曲线基于上述步骤S2210与S2220对初始粒子群进行优化所得的机器人概率分布曲线，可见，优化后的粒子分布范围减少很多，粒子分布更为集中。因此基于本实施例的方法对初始粒子群进行优化后，在后续定位过程中可以使用更少的粒子来表示机器人的概率分布，进而利用较少的计算资源完成机器人的定位。
与前述方法相对应，本申请还提供了一种基于AMCL的机器人定位装置，图4是本申请一个实施例示出的基于AMCL的机器人定位装置的功能框图，如图4所述，本实施例的装置400包括：
数据采集单元410，用于获取机器人在当前时刻采集的激光数据，以及获取当前时刻的地图图片；
粒子群优化单元420，用于利用所述激光数据与所述地图图片，对由AMCL算法预测得到的初始粒子群中的粒子分布进行优化处理，得到优化后的粒子群，优化后的粒子群中粒子分布较初始粒子群中粒子分布集中；
机器人定位单元430，用于采用AMCL算法对优化后的粒子群进行处理，实现机器人的实时定位。
在一些实施例中，粒子群优化单元420包括匹配度计算模块和粒子优化模块；
匹配度计算模块，用于利用激光数据对初始粒子群中的每个粒子计算粒子在地图图片的不同位置处与地图图片之间的匹配度，得到每个粒子与地图图片之间的最大匹配度与目标位置，粒子的目标位置为取得最大匹配度时粒子在地图图片中的位置；
粒子优化模块，用于根据粒子群中粒子的最大匹配度与预设条件之间的关系，利用粒子的目标位置对初始粒子群中该粒子的位置进行优化，得到优化后的粒子群。
在一些实施例中，匹配度计算模块，还用于根据粒子群中每个粒子的位置信息获取每个粒子在地图图片中的粒子初始坐标；根据每个粒子的粒子初始坐标获取多个激光点在地图图片中的激光点坐标，所述激光点为激光数据映射到所述地图图片中的点；根据激光点坐标计算每个粒子在粒子初始坐标下与地图图片之间的匹配度，在粒子初始坐标下计算得到的匹配度为每个粒子的参考匹配度；在地图图片中每个粒子的第一预设范围内移动粒子，并计算粒子在每个移动位置处与地图图片之间匹配度，在每个移动位置处计算得到的匹配度为每个粒子的观测匹配度；根据每个粒子的参考匹配度与其在每个移动位置处的观测匹配度的比较结果，确定每个粒子与地图图片之间的最大匹配度。
在一些实施例中，匹配度计算模块，还用于沿着预设方向按照预设步长与预设角度移动粒子，基于移动后的粒子坐标更新激光点坐标，利用更新后的激光点坐标计算出粒子在移动位置处与地图图片之间的观测匹配度；比较观测匹配度与参考匹配度，若观测匹配度大于参考匹配度，以当前移动位置为基准，收缩预设步长与预设角度，再沿着预设方向按照收缩后的预设步长与收缩后的预设角度移动粒子，基于移动后的粒子坐标更新激光点坐标，利用更新后的激光点坐标计算出粒子在新的移动位置处与地图图片之间的观测匹配度，若新的移动位置处的观测匹配度大于各个在前移动位置处的观测匹配度，则以新的移动位置为基准，继续收缩预设步长与预设角度，再沿着预设方向按照收缩后的预设步长与收缩后的预设角度移动粒子，并基于移动后的粒子坐标继续更新激光点坐标，利用更新后的激光点坐标计算出粒子在移动位置处与地图图片之间的观测匹配度，直至观测匹配度不大于前一移动位置处的观测匹配度停止移动；若观测匹配度不大于参考匹配度，则将粒子保持在其初始坐标位置处。
在一些实施例中，匹配度计算模块，还用于计算在每个激光点坐标的第二预设范围内，地图图片中是否存在与激光点距离小于预设距离的物点，物点为环境中可以被激光检测到并形成在地图图片中的点；若存在，判断所述激光点为有效激光点，并基于该物点坐标与该有效激光点坐标计算得到初始匹配度；若不存在，则计算在激光数据的其他激光点中是否存在其他有效激光点，并计算其他有效激光点的初始匹配度；计算全部有效激光点的初始匹配度的和值，将所得结果作为粒子的参考匹配度。
在一些实施例中，匹配度计算模块，还用于获取激光数据每个扫描角度下的距离信息，每个扫描角度下的距离信息对应一个激光点；以每个粒子移动后的粒子坐标为基准，参考每个扫描角度下的距离信息，计算得到该扫描角度下的距离信息映射到地图图片中的激光点的坐标。
在一些实施例中，粒子优化模块，具体是用于若粒子群中每个粒子与地图图片之间的最大匹配度均大于预设阈值，将该粒子的目标位置作为该粒子优化后的分布位置；若粒子群中有至少一个粒子与地图图片之间的最大匹配度不大于预设阈值，不优化该粒子的分布位置。
在一些实施例中，机器人定位单元430，用于利用AMCL算法的观测模型对优化后的粒子群进行权重评估，更新优化后的粒子群的权重值；对更新权重值后的粒子群进行重采样处理，根据重采样处理后的粒子群中权重值最大的粒子的位置确定机器人当前时刻的位姿。
综上所述，本实施例利用当前时刻采集到的激光数据与地图图片对AMCL算法预测的初始粒子群进行优化处理，使优化后的粒子群中粒子的分布更为集中，从而可以使用数量更少的粒子来表示机器人位置的概率分布，降低机器人定位所需的计算资源与计算代价。
需要说明的是：
在此提供的算法和显示不与任何特定计算机、虚拟装置或者其它设备固有相关。各种通用装置也可以与基于在此的示教一起使用。根据上面的描述，构造这类装置所要求的结构是显而易见的。此外，本申请也不针对任何特定编程语言。应当明白，可以利用各种编程语言实现在此描述的本申请的内容，并且上面对特定语言所做的描述是为了披露本申请的最佳实施方式。
在此处所提供的说明书中，说明了大量具体细节。然而，能够理解，本申请的实施例可以在没有这些具体细节的情况下实践。在一些实例中，并未详细示出公知的方法、结构和技术，以便不模糊对本说明书的理解。
类似地，应当理解，为了精简本申请并帮助理解各个发明方面中的一个或多个，在上面对本申请的示例性实施例的描述中，本申请的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而，并不应将该公开的方法解释成反映如下意图：即所要求保护的本申请要求比在每个权利要求中所明确记载的特征更多的特征。更确切地说，如下面的权利要求书所反映的那样，发明方面在于少于前面公开的单个实施例的所有特征。因此，遵循具体实施方式的权利要求书由此明确地并入该具体实施方式，其中每个权利要求本身都作为本申请的单独实施例。
本领域那些技术人员可以理解，可以对实施例中的设备中的模块进行自适应性地改变并且把它们设置在与该实施例不同的一个或多个设备中。可以把实施例中的模块或单元或组件组合成一个模块或单元或组件，以及此外可以把它们分成多个子模块或子单元或子组件。除了这样的特征和/或过程或者单元中的至少一些是相互排斥之外，可以采用任何组合对本说明书(包括伴随的权利要求、摘要和附图)中公开的所有特征以及如此公开的任何方法或者设备的所有过程或单元进行组合。除非另外明确陈述，本说明书(包括伴随的权利要求、摘要和附图)中公开的每个特征可以由提供相同、等同或相似目的的替代特征来代替。
此外，本领域的技术人员能够理解，尽管在此所述的一些实施例包括其它实施例中所包括的某些特征而不是其它特征，但是不同实施例的特征的组合意味着处于本申请的范围之内并且形成不同的实施例。例如，在下面的权利要求书中，所要求保护的实施例的任意之一都可以以任意的组合方式来使用。
本申请的各个部件实施例可以以硬件实现，或者以在一个或者多个处理器上运行的软件模块实现，或者以它们的组合实现。本领域的技术人员应当理解，可以在实践中使用微处理器或者数字信号处理器(DSP)来实现根据本申请实施例的基于AMCL的机器人定位装置中的一些或者全部部件的一些或者全部功能。本申请还可以实现为用于执行这里所描述的方法的一部分或者全部的设备或者装置程序(例如，计算机程序和计算机程序产品)。这样的实现本申请的程序可以存储在计算机可读介质上，或者可以具有一个或者多个信号的形式。这样的信号可以从因特网网站上下载得到，或者在载体信号上提供，或者以任何其他形式提供。
例如，图5是本申请一个实施例示出的机器人的结构框图，该机器人500包括激光器540、处理器510和被安排成存储计算机可执行指令(计算机可读程序代码)的存储器520。激光器540采集机器人周边的激光数据并发送给处理器510，存储器520可以是诸如闪存、EEPROM(电可擦除可编程只读存储器)、EPROM、硬盘或者ROM之类的电子存储器。存储器520具有存储用于执行上述方法中的任何方法步骤的计算机可读程序代码531的存储空间530。例如，用于存储计算机可读程序代码的存储空间530可以包括分别用于实现上面的方法中的各种步骤的各个计算机可读程序代码531。计算机可读程序代码531可以从一个或者多个计算机程序产品中读出或者写入到这一个或者多个计算机程序产品中。这些计算机程序产品包括诸如硬盘，紧致盘(CD)、存储卡或者软盘之类的程序代码载体。这样的计算机程序产品通常为例如图6所示的计算机可读存储介质。
图6示出了根据本申请一个实施例的一种计算机可读存储介质的结构示意图。该计算机可读存储介质600存储有用于执行根据本申请的方法步骤的计算机可读程序代码531，可以机器人500的处理器510读取，当计算机可读程序代码531由机器人500运行时，导致机器人500执行上面所描述的方法中的各个步骤，具体来说，该计算机可读存储介质存储的计算机可读程序代码531可以执行上述任一实施例中示出的方法。计算机可读程序代码531可以以适当形式进行压缩。
应该注意的是上述实施例对本申请进行说明而不是对本申请进行限制，并且本领域技术人员在不脱离所附权利要求的范围的情况下可设计出替换实施例。在权利要求中，不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的元件或步骤。位于元件之前的单词“一”或“一个”不排除存在多个这样的元件。本申请可以借助于包括有若干不同元件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中，这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。

Claims (8)

1.一种基于AMCL的机器人定位方法，其特征在于，包括：
获取机器人在当前时刻采集的激光数据，以及获取当前时刻的地图图片；
利用所述激光数据与所述地图图片，对由AMCL算法预测得到的初始粒子群中的粒子分布进行优化处理，得到优化后的粒子群，优化后的粒子群中粒子分布较初始粒子群中粒子分布集中；
采用AMCL算法对优化后的粒子群进行处理，实现机器人的实时定位；
利用所述激光数据与所述地图图片，对由AMCL算法预测得到的初始粒子群中的粒子分布进行优化处理，包括：
利用激光数据对初始粒子群中的每个粒子计算粒子在地图图片的不同位置处与地图图片之间的匹配度，得到每个粒子与地图图片之间的最大匹配度与目标位置，粒子的目标位置为取得最大匹配度时粒子在地图图片中的位置；根据粒子群中粒子的最大匹配度与预设条件之间的关系，利用粒子的目标位置对初始粒子群中该粒子的位置进行优化，得到优化后的粒子群；
利用激光数据对初始粒子群中的每个粒子计算粒子在地图图片的不同位置处与地图图片之间的匹配度，得到每个粒子与地图图片之间的最大匹配度与目标位置，包括：
根据粒子群中每个粒子的位置信息获取每个粒子在地图图片中的粒子初始坐标；根据每个粒子的粒子初始坐标获取多个激光点在地图图片中的激光点坐标，所述激光点为激光数据映射到所述地图图片中的点；根据激光点坐标计算每个粒子在粒子初始坐标下与地图图片之间的匹配度，在粒子初始坐标下计算得到的匹配度为每个粒子的参考匹配度；在地图图片中每个粒子的第一预设范围内移动粒子，并计算粒子在每个移动位置处与地图图片之间匹配度，在每个移动位置处计算得到的匹配度为每个粒子的观测匹配度；根据每个粒子的参考匹配度与其在每个移动位置处的观测匹配度的比较结果，确定每个粒子与地图图片之间的最大匹配度。
2.如权利要求1所述的方法，其特征在于，根据粒子群中粒子的最大匹配度与预设条件之间的关系，利用粒子的目标位置对初始粒子群中该粒子的位置进行优化，包括：
若粒子群中每个粒子与地图图片之间的最大匹配度均大于预设阈值，将该粒子的目标位置作为该粒子优化后的分布位置；
若粒子群中有至少一个粒子与地图图片之间的最大匹配度不大于预设阈值，不优化该粒子的分布位置。
3.如权利要求1所述的方法，其特征在于，在地图图片中每个粒子的第一预设范围内移动粒子，并计算粒子在每个移动位置处与地图图片之间匹配度，包括：
沿着预设方向按照预设步长与预设角度移动粒子，基于移动后的粒子坐标更新激光点坐标，利用更新后的激光点坐标计算出粒子在移动位置处与地图图片之间的观测匹配度；
比较观测匹配度与参考匹配度，若观测匹配度大于参考匹配度，以当前移动位置为基准，收缩预设步长与预设角度，再沿着预设方向按照收缩后的预设步长与收缩后的预设角度移动粒子，基于移动后的粒子坐标更新激光点坐标，利用更新后的激光点坐标计算出粒子在新的移动位置处与地图图片之间的观测匹配度，若新的移动位置处的观测匹配度大于各个在前移动位置处的观测匹配度，则以新的移动位置为基准，继续收缩预设步长与预设角度，再沿着预设方向按照收缩后的预设步长与收缩后的预设角度移动粒子，并基于移动后的粒子坐标继续更新激光点坐标，利用更新后的激光点坐标计算出粒子在移动位置处与地图图片之间的观测匹配度，直至观测匹配度不大于前一移动位置处的观测匹配度停止移动；
若观测匹配度不大于参考匹配度，则将粒子保持在其初始坐标位置处。
4.如权利要求1所述的方法，其特征在于，根据激光点坐标计算每个粒子在粒子初始坐标下与地图图片之间的匹配度，包括：
计算在每个激光点坐标的第二预设范围内，地图图片中是否存在与激光点距离小于预设距离的物点，物点为环境中可以被激光检测到并形成在地图图片中的点；
若存在，判断所述激光点为有效激光点，并基于该物点坐标与该有效激光点坐标计算得到初始匹配度；若不存在，则计算在激光数据的其他激光点中是否存在其他有效激光点，并计算其他有效激光点的初始匹配度；
计算全部有效激光点的初始匹配度的和值，将所得结果作为粒子的参考匹配度。
5.如权利要求3所述的方法，其特征在于，基于移动后的粒子坐标更新激光点坐标，包括：
获取激光数据每个扫描角度下的距离信息，每个扫描角度下的距离信息对应一个激光点；
以每个粒子移动后的粒子坐标为基准，参考每个扫描角度下的距离信息，计算得到该扫描角度下的距离信息映射到地图图片中的激光点的坐标。
6.如权利要求1所述的方法，其特征在于，采用AMCL算法对优化后的粒子群进行处理，实现机器人的实时定位，包括：
利用AMCL算法的观测模型对优化后的粒子群进行权重评估，更新优化后的粒子群的权重值；
对更新权重值后的粒子群进行重采样处理，根据重采样处理后的粒子群中权重值最大的粒子的位置确定机器人当前时刻的位姿。
7.一种基于AMCL的机器人定位装置，其特征在于，包括：
数据采集单元，用于获取机器人在当前时刻采集的激光数据，以及获取当前时刻的地图图片；
粒子群优化单元，用于利用所述激光数据与所述地图图片，对由AMCL算法预测得到的初始粒子群中的粒子分布进行优化处理，得到优化后的粒子群，优化后的粒子群中粒子分布较初始粒子群中粒子分布集中；
机器人定位单元，采用AMCL算法对优化后的粒子群进行处理，实现机器人的实时定位；
粒子群优化单元包括匹配度计算模块和粒子优化模块；
匹配度计算模块，用于利用激光数据对初始粒子群中的每个粒子计算粒子在地图图片的不同位置处与地图图片之间的匹配度，得到每个粒子与地图图片之间的最大匹配度与目标位置，粒子的目标位置为取得最大匹配度时粒子在地图图片中的位置；还用于根据粒子群中每个粒子的位置信息获取每个粒子在地图图片中的粒子初始坐标；根据每个粒子的粒子初始坐标获取多个激光点在地图图片中的激光点坐标，所述激光点为激光数据映射到所述地图图片中的点；根据激光点坐标计算每个粒子在粒子初始坐标下与地图图片之间的匹配度，在粒子初始坐标下计算得到的匹配度为每个粒子的参考匹配度；在地图图片中每个粒子的第一预设范围内移动粒子，并计算粒子在每个移动位置处与地图图片之间匹配度，在每个移动位置处计算得到的匹配度为每个粒子的观测匹配度；根据每个粒子的参考匹配度与其在每个移动位置处的观测匹配度的比较结果，确定每个粒子与地图图片之间的最大匹配度；
粒子优化模块，用于根据粒子群中粒子的最大匹配度与预设条件之间的关系，利用粒子的目标位置对初始粒子群中该粒子的位置进行优化，得到优化后的粒子群。
8.一种机器人，包括：存储器、处理器和激光器；
激光器，采集机器人周边的激光数据并发送给处理器；
存储器，存储计算机可执行指令；
处理器，根据计算机可执行指令执行如权利要求1-6任一项所述的基于AMCL的机器人定位方法。
CN202010777182.9A 2020-08-05 2020-08-05 一种基于amcl的机器人定位方法、装置和机器人 Active CN112068547B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010777182.9A CN112068547B (zh) 2020-08-05 2020-08-05 一种基于amcl的机器人定位方法、装置和机器人

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010777182.9A CN112068547B (zh) 2020-08-05 2020-08-05 一种基于amcl的机器人定位方法、装置和机器人

Publications (2)

Publication Number Publication Date
CN112068547A CN112068547A (zh) 2020-12-11
CN112068547B true CN112068547B (zh) 2024-05-28

Family

ID=73657252

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010777182.9A Active CN112068547B (zh) 2020-08-05 2020-08-05 一种基于amcl的机器人定位方法、装置和机器人

Country Status (1)

Country Link
CN (1) CN112068547B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117570998B (zh) * 2024-01-17 2024-04-02 山东大学 基于反光柱信息的机器人定位方法及系统

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107991683A (zh) * 2017-11-08 2018-05-04 华中科技大学 一种基于激光雷达的机器人自主定位方法
CN108931245A (zh) * 2018-08-02 2018-12-04 上海思岚科技有限公司 移动机器人的局部自定位方法及设备
CN109974701A (zh) * 2017-12-28 2019-07-05 深圳市优必选科技有限公司 机器人的定位方法及装置

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111486842B (zh) * 2019-01-29 2022-04-15 深圳市优必选科技有限公司 重定位方法及装置、机器人

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107991683A (zh) * 2017-11-08 2018-05-04 华中科技大学 一种基于激光雷达的机器人自主定位方法
CN109974701A (zh) * 2017-12-28 2019-07-05 深圳市优必选科技有限公司 机器人的定位方法及装置
CN108931245A (zh) * 2018-08-02 2018-12-04 上海思岚科技有限公司 移动机器人的局部自定位方法及设备

Also Published As

Publication number Publication date
CN112068547A (zh) 2020-12-11

Similar Documents

Publication Publication Date Title
CN109300159B (zh) 位置检测方法、装置、设备、存储介质及车辆
CN108629231B (zh) 障碍物检测方法、装置、设备及存储介质
US10614324B2 (en) Method and apparatus for identifying static obstacle
CN111443359B (zh) 定位方法、装置及设备
JP2021532442A (ja) ターゲット検出方法及び装置、スマート運転方法、装置並びに記憶媒体
US11080878B2 (en) Method and apparatus for detecting 3D object from 2D image
CN109190573B (zh) 应用于无人驾驶车辆的地面检测方法、电子设备、车辆
CN110927742A (zh) 障碍物跟踪方法、装置、设备及存储介质
JP7149362B2 (ja) 環境変化を検出するための方法及び装置、計算装置、記憶媒体並びにコンピュータプログラム
CN111015656A (zh) 一种机器人主动避障的控制方法、装置及存储介质
WO2022104254A1 (en) Efficient three-dimensional object detection from point clouds
CN108367436B (zh) 针对三维空间中的对象位置和范围的主动相机移动确定
CN111142514B (zh) 一种机器人及其避障方法和装置
CN112487861A (zh) 车道线识别方法、装置、计算设备及计算机存储介质
CN114663526A (zh) 障碍物检测方法、装置、机器人及计算机可读存储介质
CN112068547B (zh) 一种基于amcl的机器人定位方法、装置和机器人
CN115326051A (zh) 一种基于动态场景的定位方法、装置、机器人及介质
CN112150538A (zh) 一种在三维地图构建过程中车辆位姿的确定方法和装置
CN114241448A (zh) 障碍物航向角的获取方法、装置、电子设备及车辆
CN112381873A (zh) 一种数据标注方法及装置
CN112162561A (zh) 一种地图构建优化方法、装置、介质及设备
CN115855026A (zh) 一种大型场景图元地图切换方法、系统、装置及存储介质
CN113670253A (zh) 空间目标姿态反演方法、装置、计算设备及存储介质
CN114266859B (zh) 数据处理方法、装置、设备和存储介质
CN117111491B (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