CN112344966B - 一种定位失效检测方法、装置、存储介质及电子设备 - Google Patents
一种定位失效检测方法、装置、存储介质及电子设备 Download PDFInfo
- Publication number
- CN112344966B CN112344966B CN202011335423.0A CN202011335423A CN112344966B CN 112344966 B CN112344966 B CN 112344966B CN 202011335423 A CN202011335423 A CN 202011335423A CN 112344966 B CN112344966 B CN 112344966B
- Authority
- CN
- China
- Prior art keywords
- particle
- map
- determining
- characteristic
- straight line
- 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
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 71
- 239000002245 particle Substances 0.000 claims abstract description 188
- 230000004927 fusion Effects 0.000 claims abstract description 34
- 238000000034 method Methods 0.000 claims abstract description 31
- 238000005259 measurement Methods 0.000 claims description 24
- 230000015654 memory Effects 0.000 claims description 23
- 239000011159 matrix material Substances 0.000 claims description 22
- 230000004807 localization Effects 0.000 claims description 13
- 238000004590 computer program Methods 0.000 claims description 9
- 238000000605 extraction Methods 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000012545 processing Methods 0.000 description 21
- 238000004891 communication Methods 0.000 description 6
- 238000013507 mapping Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 230000007613 environmental effect Effects 0.000 description 5
- 230000000007 visual effect Effects 0.000 description 5
- 238000004422 calculation algorithm Methods 0.000 description 4
- 230000033001 locomotion Effects 0.000 description 4
- 230000003044 adaptive effect Effects 0.000 description 3
- 230000001413 cellular effect Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000011156 evaluation Methods 0.000 description 2
- 239000000284 extract Substances 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
- G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
- G06F16/29—Geographical information databases
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Databases & Information Systems (AREA)
- Theoretical Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Radar, Positioning & Navigation (AREA)
- Data Mining & Analysis (AREA)
- General Engineering & Computer Science (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本申请提供一种定位失效检测方法、装置、存储介质及电子设备,涉及机器人技术领域。通过获取移动机器人的激光雷达扫描数据,并提取所述激光雷达扫描数据的地图二维直线特征;根据所述地图二维直线特征,确定所述地图二维直线特征的特征定位性能预测指标;根据所述特征定位性能预测指标和获取的机器人位姿粒子集的粒子权重,确定融合特征定位性能预测指标的融合粒子集权重;若所述融合粒子集权重小于或等于设定阈值,确定所述移动机器人定位失效。该方法在机器人位姿粒子集的基础上融合了激光雷达扫描数据的地图二维直线特征的特征定位性能预测指标,更准确地反映机器人定位失效的真实情况,从而确保移动机器人的正常定位。
Description
技术领域
本申请涉及机器人技术领域,特别是涉及一种定位失效检测方法、装置、存储介质及电子设备。
背景技术
目前,激光SLAM(Simultaneous Localization And Mapping,即时定位与建图)在理论、技术和产品落地上都较为成熟,因而成为现下最为主流的定位导航方式。但是在某些场景下,如在轮胎打滑而使里程计信息产生累计误差时,又或者在拥挤场景下激光扫描数据受到干扰时,使激光SLAM定位失效。
移动机器人的内外传感器都存在噪声,会影响机器人的定位性能,严重时甚至会导致定位失败。其次,机器人所处环境中物体的尺寸、外形等因素的不同,环境地图的相关性质都会影响最终定位的精度下界。我们将上述因素对定位精度带来的影响称为定位能力。定位能力较强意味着在当前位姿处,移动机器人受到上述因素的影响较小,定位精度较高;定位能力较弱则意味着,在当前位姿处,上述因素已经严重影响了移动机器人的正常定位,导致了定位精度的下降。
针对上述问题,如何进行机器人定位失效的检测,是一个急需解决的问题。
发明内容
本申请实施例提供了一种定位失效检测方法、装置、存储介质及电子设备,实现机器人定位失效的检测,将环境质量和未知障碍物带来的影响予以剔除,确保移动机器人的正常定位。
为达到上述目的,本申请实施例的技术方案是这样实现的:
第一方面,本申请实施例提供一种定位失效检测方法,包括:
获取移动机器人的激光雷达扫描数据,并提取所述激光雷达扫描数据的地图二维直线特征;
根据所述地图二维直线特征,确定所述地图二维直线特征的特征定位性能预测指标;
根据所述特征定位性能预测指标和获取的机器人位姿粒子集的粒子权重,确定融合特征定位性能预测指标的融合粒子集权重;所述机器人位姿粒子集包含在所述移动机器人定位时用于确定所述移动机器人的位姿的粒子;所述粒子权重是所述机器人位姿粒子集中各个粒子的粒子权重;
若所述融合粒子集权重小于或等于设定阈值,确定所述移动机器人定位失效。
在一种可选的实施例中,所述提取所述激光雷达扫描数据中的地图二维直线特征,包括:
根据获取的移动机器人的激光雷达扫描数据,确定断点检测算子;
确定所述激光雷达扫描数据的各个相邻的两个测量点之间的距离;
根据所述各个相邻的两个测量点之间的距离和所述断点检测算子,将所述激光雷达扫描数据中的测量点划分为至少一个数据点集;
分别对得到的各个数据点集进行直线拟合,得到各个数据点集对应的直线段;
根据各个数据点集对应的直线段,生成所述激光雷达扫描数据的地图二维直线特征;所述地图二维直线特征是所述直线段构成的集合。
在一种可选的实施例中,所述根据所述各个相邻的两个测量点之间的距离和所述断点检测算子,将所述激光雷达扫描数据中的测量点划分为至少一个数据点集,包括:
若相邻的两个测量点之间的距离大于所述断点检测算子,则将该两个测量点划分至不同的数据点集;
若相邻的两个测量点之间的距离小于或等于所述断点检测算子,则将该两个测量点划分至同一个数据点集。
在一种可选的实施例中,所述根据所述地图二维直线特征,确定所述地图二维直线特征的特征定位性能预测指标,包括:
对于每个数据点集,执行如下步骤:分别根据所述数据点集中每个测量点的坐标数据,确定每个测量点的标量协方差;根据所述每个测量点的标量协方差,确定所述数据点集对应的地图二维直线特征的直线段位置的特征协方差矩阵;
根据所述特征协方差矩阵以及地图二维直线特征的各个直线段的特征,得到所述特征定位性能预测指标。
在一种可选的实施例中,所述根据所述特征定位性能预测指标和获取的机器人位姿粒子集的粒子权重,确定融合特征定位性能预测指标的融合粒子集权重,包括:
根据获取的机器人位姿粒子集的粒子权重,确定所述机器人位姿粒子集中的全部粒子的粒子权重的平均值,作为基本粒子集权重;
根据所述基本粒子集权重和所述特征定位性能预测指标,确定融合特征定位性能预测指标的融合粒子集权重。
第二方面,本申请实施例提供一种定位失效检测装置,包括:
特征提取单元,用于获取移动机器人的激光雷达扫描数据,并提取所述激光雷达扫描数据的地图二维直线特征;
指标计算单元,用于根据所述地图二维直线特征,确定所述地图二维直线特征的特征定位性能预测指标;
权重融合单元,用于根据所述特征定位性能预测指标和获取的机器人位姿粒子集的粒子权重,确定融合特征定位性能预测指标的融合粒子集权重;所述机器人位姿粒子集包含在所述移动机器人定位时用于确定所述移动机器人的位姿的粒子;所述粒子权重是所述机器人位姿粒子集中各个粒子的粒子权重;
失效判断单元,用于若监测到所述融合粒子集权重小于或等于设定阈值,确定所述移动机器人定位失效。
在一种可选的实施例中,所述特征提取单元,具体用于:
根据获取的移动机器人的激光雷达扫描数据,确定断点检测算子;
确定所述激光雷达扫描数据的各个相邻的两个测量点之间的距离;
根据所述各个相邻的两个测量点之间的距离和所述断点检测算子,将所述激光雷达扫描数据中的测量点划分为至少一个数据点集;
分别对得到的各个数据点集进行直线拟合,得到各个数据点集对应的直线段;
根据各个数据点集对应的直线段,生成所述激光雷达扫描数据的地图二维直线特征;所述地图二维直线特征是所述直线段构成的集合。
在一种可选的实施例中,所述指标计算单元,具体用于:
对于每个数据点集,执行如下步骤:分别根据所述数据点集中每个测量点的坐标数据,确定每个测量点的标量协方差;根据所述每个测量点的标量协方差,确定所述数据点集对应的地图二维直线特征的直线段位置的特征协方差矩阵;
根据所述特征协方差矩阵以及地图二维直线特征的各个直线段的特征,得到所述特征定位性能预测指标。
在一种可选的实施例中,所述权重融合单元,具体用于:
根据获取的机器人位姿粒子集的粒子权重,确定所述机器人位姿粒子集中的全部粒子的粒子权重的平均值,作为基本粒子集权重;
根据所述基本粒子集权重和所述特征定位性能预测指标,确定融合特征定位性能预测指标的融合粒子集权重。
第三方面,本申请实施例提供一种计算机可读存储介质,所述计算机可读存储介质内存储有计算机程序,所述计算机程序被处理器执行时,实现第一方面任一项所述的方法。
第四方面,本申请实施例还提供一种电子设备,包括存储器和处理器,所述存储器上存储有可在所述处理器上运行的计算机程序,当所述计算机程序被所述处理器执行时,使得所述处理器实现第一方面任一项所述的方法。
本申请实施例的定位失效检测方法、装置、存储介质及电子设备,通过获取移动机器人的激光雷达扫描数据,并提取所述激光雷达扫描数据的地图二维直线特征;根据所述地图二维直线特征,确定所述地图二维直线特征的特征定位性能预测指标;根据所述特征定位性能预测指标和获取的机器人位姿粒子集的粒子权重,确定融合特征定位性能预测指标的融合粒子集权重;若所述融合粒子集权重小于或等于设定阈值,确定所述移动机器人定位失效。该方法在机器人位姿粒子集的基础上融合了激光雷达扫描数据的地图二维直线特征的特征定位性能预测指标,将环境质量和未知障碍物带来的影响予以剔除,更准确地反映机器人定位失效的真实情况,从而确保移动机器人的正常定位。
附图说明
此处所说明的附图用来提供对本申请的进一步理解,构成本申请的一部分,本申请的示意性实施例及其说明用于解释本申请,并不构成对本申请的不当限定。在附图中:
图1为本申请实施例提供的一种定位失效检测方法的应用场景图;
图2为本申请实施例提供的一种定位失效检测方法的流程示意图;
图3为本申请实施例提供的一种地图二维直线特征提取的流程示意图;
图4为本申请实施例提供的一种确定特征定位性能预测指标的流程示意图;
图5为本申请实施例提供的一种确定融合粒子集权重的流程示意图;
图6为本申请实施例提供的一种定位失效检测装置的结构示意图;
图7为本申请实施例提供的一种电子设备的结构示意图。
具体实施方式
为了使本申请的目的、技术方案和优点更加清楚,下面将结合附图对本申请作进一步地详细描述,显然,所描述的实施例仅仅是本申请一部分实施例,而不是全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例,都属于本申请保护的范围。
需要说明的是,本申请的文件中涉及的术语“包括”和“具有”以及它们的变形,意图在于覆盖不排他的包含,例如,包含了一系列步骤或单元的过程、方法、***、产品或设备不必限于清楚地列出的那些步骤或单元,而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
以下对本申请实施例中的部分用语进行解释说明,以便于本领域技术人员理解。
(1)SLAM(Simultaneous Localization And Mapping,即时定位与建图):主要用于解决机器人在未知环境运动时的定位与地图构建问题。SLAM***框架一般分为五个模块,包括传感器数据、视觉里程计、后端、建图及回环检测。其中,传感器数据:主要用于采集实际环境中的各类型原始数据,包括激光扫描数据、视频图像数据、点云数据等;视觉里程计:主要用于不同时刻间移动目标相对位置的估算,包括特征匹配、直接配准等算法的应用;后端:主要用于优化视觉里程计带来的累计误差。包括滤波器、图优化等算法应用;建图:用于三维地图构建;回环检测:主要用于空间累积误差消除。其工作流程大致为:传感器读取数据后,视觉里程计估计两个时刻的相对运动(Ego-motion),后端处理视觉里程计估计结果的累积误差,建图则根据前端与后端得到的运动轨迹来建立地图,回环检测考虑了同一场景不同时刻的图像,提供了空间上约束来消除累积误差。
(2)世界坐标系(World coordinate system),也称为全局坐标系,它是大地作为参考平面。世界坐标系可以让两个或多个机器人定位到车间里的同一个位置点,在两个或多个机器人协同工作时,比如一个机器人抓取另一个机器人焊接好的部件,使用世界坐标系会特别方便。
下文中所用的词语“示例性”的意思为“用作例子、实施例或说明性”。作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。
文中的术语“第一”、“第二”仅用于描述目的,而不能理解为明示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征,在本申请实施例的描述中,除非另有说明,“多个”的含义是两个或两个以上。
为更好地理解本申请实施例提供的技术方案,下面对本申请实施例提供的技术方案适用的应用场景做一些简单介绍,需要说明的是,以下介绍的应用场景仅用于说明本申请实施例而非限定。在具体实施时,可以根据实际需要灵活地应用本申请实施例提供的技术方案。
图1示出了本申请实施例提供的定位失效检测方法的一种应用场景,参见图1所示,该应用场景中包括网络100、数据处理设备200、移动机器人300 以及数据库400。数据处理设备200与移动机器人300、数据处理设备200与数据库400之间可以通过通信网络100进行通信连接并传输数据。该通信网络可以是有线网络或无线网络。例如,数据处理设备200与移动机器人300可以通过蜂窝数据网络或WiFi无线网络传输数据;数据处理设备200与数据库400 也可以通过蜂窝数据网络或WiFi无线网络传输数据。
其中,数据处理设备200可以是一台服务器或由若干台服务器组成的服务器集群或云计算中心,或者是一个虚拟化平台,也可以是微型计算机、大中型计算机或计算机集群等。数据处理设备200可以是一个固定安装在移动机器人 300内部的构件,也可以是一个与移动机器人300相互独立的构件。
机器人激光SLAM定位失效检测时,数据处理设备200用于通过网络100 向机器人进行激光SLAM定位及定位失效检测所涉及的参数初始化设定,并通过激光SLAM定位得到用于确定所述移动机器人的位姿的机器人位姿粒子集和该机器人位姿粒子集中粒子的粒子权重并存储到数据库400中。数据处理设备还可以将机器人激光定位的其他数据,如二维栅格地图存储到数据库400中。数据处理设备获取移动机器人300的激光雷达扫描数据,并提取激光雷达扫描数据中的地图二维直线特征,根据所述地图二维直线特征和机器人位姿粒子集的粒子权重,实现机器人定位失效的检测。上述进行机器人定位失效的检测的过程将在下文中详细介绍。
目前,激光SLAM(Simultaneous Localization And Mapping,即时定位与建图)在理论、技术和产品落地上都较为成熟,因而成为现下最为主流的定位导航方式。但是在某些场景下,如在轮胎打滑而使里程计信息产生累计误差时,又或者在拥挤场景下激光扫描数据受到干扰时,使激光SLAM定位失效。
移动机器人的内外传感器都存在噪声,会影响机器人的定位性能,严重时甚至会导致定位失败。其次,机器人所处环境中物体的尺寸、外形等因素的不同,环境地图的相关性质都会影响最终定位的精度下界。我们将上述因素对定位精度带来的影响称为定位能力。定位能力较强意味着在当前位姿处,移动机器人受到上述因素的影响较小,定位精度较高;定位能力较弱则意味着,在当前位姿处,上述因素已经严重影响了移动机器人的正常定位,导致了定位精度的下降。
针对上述问题,需要提出机器人定位失效的检测方法。
本申请实施例提供了一种定位失效检测方法、装置、存储介质及电子设备,实现机器人定位失效的检测,将环境质量和未知障碍物带来的影响予以剔除,确保移动机器人的正常定位。
为了便于理解,在一个实施例中,如图2所示,为本申请实施例提供的一种定位失效检测方法的具体应用流程:
步骤S201,获取移动机器人的激光雷达扫描数据,并提取激光雷达扫描数据的地图二维直线特征。
具体地,在移动机器人激光SLAM定位时,为了进行激光SLAM定位失效检测,获取移动机器人的激光雷达扫描数据,并提取激光雷达扫描数据的地图二维直线特征。
在本申请的一些实施例中,定位失效检测是针对二维栅格地图中的移动机器人激光SLAM定位进行的定位失效检测。
在一些实施例中,移动机器人的激光雷达扫描数据可以是使用机器人自身携带的激光传感器采集地图数据,得到观测数据。基于观测数据的噪声,提取地图二维直线特征。在一些实施例中,地图二维直线特征是至少一条直线段构成的集合。
步骤S202,根据地图二维直线特征,确定地图二维直线特征的特征定位性能预测指标。
基于提取地图二维直线特征,计算获得特征定位性能预测指标。
在一些实施例中,特征定位性能预测指标是地图二维直线特征的特征定位性能评价估计矩阵。
步骤S203,根据特征定位性能预测指标和获取的机器人位姿粒子集的粒子权重,确定融合特征定位性能预测指标的融合粒子集权重。
其中,机器人位姿粒子集包含在移动机器人定位时用于确定移动机器人的位姿的粒子;粒子权重是机器人位姿粒子集中各个粒子的粒子权重。
在一些实施例中,基于机器人位姿平均粒子集权重,融合特征定位性能评价估计矩阵,计算获得融合粒子集权重。其中,机器人位姿平均粒子集权重是根据机器人位姿粒子集中各个粒子的粒子权重确定的。
在一些实施例中,机器人位姿粒子集中的粒子用于评估机器人的位姿。
步骤S204,若融合粒子集权重小于或等于设定阈值,确定移动机器人定位失效。
在一些实施例中,机器人定位时,可以通过预先试验测定机器人定位正常时的融合粒子集权重的正常值。在机器人定位失效检测时,将融合粒子集权重的正常值乘以一个(0,1)区间内的权重系数,得到该设定阈值。
在一些实施例中,在机器人定位失效检测时,可以基于机器人运行环境的复杂程度的差异,对于复杂程度不同的运行环境,将融合粒子集权重的正常值乘以不同的权重系数,得到对应于该运行环境下定位失效检测的设定阈值。
示例性地,基于机器人运行环境的复杂程度的差异,可以将运行环境区分为强特征环境和弱特征环境两种。对于强特征环境可以设定为融合粒子集权重的正常值乘以权重系数A,对于弱特征环境可以设定为融合粒子集权重的正常值乘以权重系数B,其中A和B的数值在(0,1)区间内,且A大于B。
在一些实施例中,数据处理设备在每次确定融合特征定位性能预测指标的融合粒子集权重之后,比较融合粒子集权重与设定阈值的大小关系,若融合粒子集权重小于或等于设定阈值,判定移动机器人激光SLAM定位失效。数据处理设备可以实时监测融合粒子集权重,也可以是以预设的时间间隔监测融合粒子集权重。
在另外一些实施例中,数据处理设备在每次确定融合特征定位性能预测指标的融合粒子集权重之后,还监测融合粒子集权重是否小于前一次计算得到的融合粒子集权重。在该实施例中,若融合粒子集权重小于前一次计算得到的融合粒子集权重,则计数器加1;若计数器的值超出预先设定的计数阈值,再比较融合粒子集权重与设定阈值的大小关系,若融合粒子集权重小于或等于设定阈值,判定移动机器人激光SLAM定位失效。
本申请实施例提供的定位失效检测方法,通过获取移动机器人的激光雷达扫描数据,并提取所述激光雷达扫描数据的地图二维直线特征;根据所述地图二维直线特征,确定所述地图二维直线特征的特征定位性能预测指标;根据所述特征定位性能预测指标和获取的机器人位姿粒子集的粒子权重,确定融合特征定位性能预测指标的融合粒子集权重;若所述融合粒子集权重小于或等于设定阈值,确定所述移动机器人定位失效。该方法在机器人位姿粒子集的基础上融合了激光雷达扫描数据的地图二维直线特征的特征定位性能预测指标,将环境质量和未知障碍物带来的影响予以剔除,更准确地反映机器人定位失效的真实情况,从而确保移动机器人的正常定位。
图3示出了本申请实施例提供的一种定位失效检测方法中地图二维直线特征提取的流程示意图。如图3,在一种可选的实施例中,提取激光雷达扫描数据中的地图二维直线特征,可以通过以下步骤:
步骤S301,根据获取的移动机器人的激光雷达扫描数据,确定断点检测算子。
示例性地,机器人的激光雷达输出的激光雷达扫描数据可以是物体距离扫描点。物体距离扫描点表示激光雷达扫描到的物体距离机器人位置的距离。激光雷达输出的物体距离扫描点在机器人定位的极坐标系中可以表示为 {(d1,φ1),(d2,φ2),...,(dn,φn)},其中di和φi包含均值为0,方差为和/>的高斯随机噪声εd和εφ,Di和Φi为测量值di和φi对应的真值,即:
di=Di+εd
φi=Φi+εφ
则测量数据在笛卡尔坐标系下的不确定性协方差可以表示为:
在计算中,使用测量值di和φi作为真值Di和Φi的良好估计。
基于上述测量数据噪声性协方差,得到断点检测算子:
其中,D′th为根据自适应阈值生成法得到的前后两个测量点间的距离阈值, det(Qi)为求矩阵Qi的行列式值。
步骤S302,确定激光雷达扫描数据的各个相邻的两个测量点之间的距离。
步骤S303,根据各个相邻的两个测量点之间的距离和断点检测算子,将激光雷达扫描数据中的测量点划分为至少一个数据点集。
具体地,若前后两个测量点之间的距离大于自适应断点检测算子,则认为这两点之间存在断点。将两个断点以内的数据整合成一个数据集,而后根据点集进行直线拟合。
在一种可选的实施例中,根据各个相邻的两个测量点之间的距离和断点检测算子,将激光雷达扫描数据中的测量点划分为至少一个数据点集,包括:
若相邻的两个测量点之间的距离大于断点检测算子,则将该两个测量点划分至不同的数据点集;
若相邻的两个测量点之间的距离小于或等于所述断点检测算子,则将该两个测量点划分至同一个数据点集。
步骤S304,分别对得到的各个数据点集进行直线拟合,得到各个数据点集对应的直线段。
在一些实施例中,使用分割-合并算法对基于断点分组之后的数据点集进行直线拟合,可以得到多条拟合后的直线段。直线段与数据点集是一一对应的。
步骤S305,根据各个数据点集对应的直线段,生成激光雷达扫描数据的地图二维直线特征。
其中,地图二维直线特征是直线段构成的集合。
对得到的数据点集进行直线拟合,获得各个数据点集对应的直线段。根据各个数据点集对应的直线段,生成激光雷达扫描数据的地图二维直线特征。其中,地图二维直线特征为与该至少一个数据点集一对一对应的至少一条直线段的集合。地图二维直线特征中的任一条直线段可以使用三个参数L(R,α,S)表示,其中R是直线段的法向量,α是直线段法线与激光最小入射角的夹角,S是沿着直线段切线方向的等效中点。
图4示出了本申请实施例提供的一种定位失效检测方法中确定特征定位性能预测指标的流程示意图。如图4,在一种可选的实施例中,根据地图二维直线特征,确定地图二维直线特征的特征定位性能预测指标,包括以下步骤:
步骤S401,对于每个数据点集,执行如下步骤:分别根据数据点集中每个测量点的坐标数据,确定每个测量点的标量协方差。
示例性地,激光数据第k个点uk在RS坐标系下的坐标可以表示为:
每个测量点在RS坐标系下的标量协方差为:
其中Qij为激光雷达扫描数据第k个点uk的不确定性协方差矩阵Qk第i行,第j列的元素。
步骤S402,对于每个数据点集,执行如下步骤:根据每个测量点的标量协方差,确定数据点集对应的地图二维直线特征的直线段位置的特征协方差矩阵。
示例性地,设R方向与S方向不相关,对于地图二维直线特征包含的一个直线段L,对应位置的特征协方差矩阵可以表示为:
其中:
n为直线段L对应的数据点集中的测量点的数量。
步骤S403,根据特征协方差矩阵以及地图二维直线特征的各个直线段的特征,得到特征定位性能预测指标。
在一些实施例中,地图二维直线特征的各个直线段的特征可以包括地图二维直线特征的直线段的朝向、移动机器人自身朝向、地图二维直线特征的直线段的特征尺寸与机器人到地图二维直线特征的直线段的距离的比值、激光与对应反射面法线方向所成角度、所述激光雷达扫描数据的地图二维直线特征包含直线段的数量。在一些实施例中,数据处理设备从数据库400获取上述地图二维直线特征的各个直线段的特征的数值。
可选地,根据特征协方差矩阵以及地图二维直线特征的各个直线段的特征,得到特征定位性能预测指标,包括:根据特征协方差矩阵、地图二维直线特征的直线段的朝向、移动机器人自身朝向、地图二维直线特征的直线段的特征尺寸与机器人到地图二维直线特征的直线段的距离的比值、激光与对应反射面法线方向所成角度、所述激光雷达扫描数据的地图二维直线特征包含直线段的数量,得到特征定位性能预测指标。
示例性地,设每个地图二维直线特征可以表示为feai(Li,αi,Di,Ci),其中αi为第i个地图二维直线特征在世界坐标系下的朝向,Li为该特征长度,Di为该特征协方差矩阵,Ci为该特征在世界坐标系下的中点坐标。定义Z为当前地图, disi表示机器人到第i个特征的距离,设f(disi,p)为使用特征feai对p进行位姿估计的概率分布,P为基于所有直线特征的概率分布的乘积,不失一般性,设各个特征相互独立,则位姿p处的位姿估计可以由各个特征的联合概率分布表示,则对于当前位姿下在地图中能观测到的所有特征,存在概率分布:
根据特征协方差矩阵,按以下公式得到所述地图二维直线特征的特征定位性能预测指标Fp:
其中Di为地图二维直线特征的第i个直线段位置的特征协方差矩阵,αi为地图二维直线特征的第i个直线段在世界坐标系下的朝向,θ为移动机器人在世界坐标系自身朝向,ri为地图二维直线特征的第i个直线段的特征尺寸与机器人到地图二维直线特征的第i个直线段的距离的比值,βi为第i束激光与对应反射面法线方向所成角度,N为该激光雷达扫描数据的地图二维直线特征包含直线段的数量。
图5示出了本申请实施例提供的一种定位失效检测方法中确定融合粒子集权重的流程示意图。如图5,在一种可选的实施例中,根据特征定位性能预测指标和获取的机器人位姿粒子集的粒子权重,确定融合特征定位性能预测指标的融合粒子集权重,可以通过以下步骤:
步骤S501,根据获取的机器人位姿粒子集的粒子权重,确定机器人位姿粒子集中的全部粒子的粒子权重的平均值,作为基本粒子集权重。
机器人激光SLAM定位时通常是通过由自适应蒙特卡洛定位算法得到代表机器人位姿的粒子和粒子的粒子权重进行定位。本申请的实施例中,数据处理设备从数据库获取机器人位姿粒子集,并获得机器人位姿粒子集的粒子权重。其中,机器人位姿粒子集是机器人激光SLAM定位的代表机器人位姿的粒子组成的集合。
示例性地,机器人位姿粒子集中包含的粒子的数量记为M,粒子的权重记为wj,机器人位姿粒子集中的全部粒子的粒子权重的平均值wave为:
将上述粒子权重的平均值wave作为基本粒子集权重,记为wi,
wi=wave。
步骤S502,根据基本粒子集权重和特征定位性能预测指标,确定融合特征定位性能预测指标的融合粒子集权重。
具体地,根据基本粒子集权重和特征定位性能预测指标,按以下公式计算得到融合特征定位性能预测指标的融合粒子集权重w′i,
其中,Fp为当前位姿下的所述地图二维直线特征的特征定位性能预测指标, wi为基本粒子集权重。
对于定位失效检测,在弱特征环境中例如直线走廊,随着机器人的移动,基本粒子集权重wi在降低,基于特征的Fp定位不确定性也在降低。融合后w′i降低的比例会低于基本粒子集权重wi降低的比例,通过融合权重可以更有效防止机器人定位丢失的误触发。
在强特征环境中,例如不是机器人当前的环境,随着机器人的移动,基础平均粒子权重wi在降低,而基于特征的Fp定位不确定性基本维持不变,此时随着机器人的移动,融合后w′i会逐渐降低。
若融合权重w′i,小于设定的阈值,判断激光SLAM定位失效。
本申请实施例提供的定位失效检测方法,在机器人位姿粒子集的基础上融合了激光雷达扫描数据的地图二维直线特征的特征定位性能预测指标,将环境质量和未知障碍物带来的影响予以剔除,更准确地反映机器人定位失效的真实情况,从而确保移动机器人的正常定位。
基于同一发明构思,本申请实施例中还提供了一种定位失效检测装置,该定位失效检测装置可以布设在数据处理设备中。由于该装置是本申请实施例定位失效检测方法对应的装置,并且该装置解决问题的原理与该方法相似,因此该装置的实施可以参见上述方法的实施,重复之处不再赘述。
图6示出了本申请实施例提供的一种定位失效检测装置的结构框图,如图 6所示,该定位失效检测装置包括:特征提取单元601、指标计算单元602、权重融合单元603和失效判断单元604;其中,
特征提取单元601,用于获取移动机器人的激光雷达扫描数据,并提取激光雷达扫描数据的地图二维直线特征;
指标计算单元602,用于根据地图二维直线特征,确定地图二维直线特征的特征定位性能预测指标;
权重融合单元603,用于根据特征定位性能预测指标和获取的机器人位姿粒子集的粒子权重,确定融合特征定位性能预测指标的融合粒子集权重;机器人位姿粒子集包含在移动机器人定位时用于确定移动机器人的位姿的粒子;粒子权重是机器人位姿粒子集中各个粒子的粒子权重;
失效判断单元604,用于若监测到融合粒子集权重小于或等于设定阈值,确定移动机器人定位失效。
在一种可选的实施例中,特征提取单元601,具体用于:
根据获取的移动机器人的激光雷达扫描数据,确定断点检测算子;
确定激光雷达扫描数据的各个相邻的两个测量点之间的距离;
根据各个相邻的两个测量点之间的距离和断点检测算子,将激光雷达扫描数据中的测量点划分为至少一个数据点集;
分别对得到的各个数据点集进行直线拟合,得到各个数据点集对应的直线段;
根据各个数据点集对应的直线段,生成激光雷达扫描数据的地图二维直线特征;地图二维直线特征是直线段构成的集合。
在一种可选的实施例中,指标计算单元602,具体用于:
对于每个数据点集,执行如下步骤:分别根据数据点集中每个测量点的坐标数据,确定每个测量点的标量协方差;根据每个测量点的标量协方差,确定数据点集对应的地图二维直线特征的直线段位置的特征协方差矩阵;
根据特征协方差矩阵以及地图二维直线特征的各个直线段的特征,得到特征定位性能预测指标。
在一种可选的实施例中,权重融合单元603,具体用于:
根据获取的机器人位姿粒子集的粒子权重,确定机器人位姿粒子集中的全部粒子的粒子权重的平均值,作为基本粒子集权重;
根据基本粒子集权重和特征定位性能预测指标,确定融合特征定位性能预测指标的融合粒子集权重。
基于同一发明构思,本申请实施例中还提供了一种电子设备。在一种实施例中,该电子设备可以是数据处理设备。在该实施例中,电子设备的结构可以如图7所示,包括存储器901,通讯模块903以及一个或多个处理器902。
存储器901,用于存储处理器902执行的计算机程序。存储器901可主要包括存储程序区和存储数据区,其中,存储程序区可存储操作***,以及运行即时通讯功能所需的程序等;存储数据区可存储各种即时通讯信息和操作指令集等。
存储器901可以是易失性存储器(volatile memory),例如随机存取存储器(random-access memory,RAM);存储器901也可以是非易失性存储器 (non-volatilememory),例如只读存储器,快闪存储器(flash memory),硬盘 (hard disk drive,HDD)或固态硬盘(solid-state drive,SSD)、或者存储器901 是能够用于携带或存储具有指令或数据结构形式的期望的程序代码并能够由计算机存取的任何其他介质,但不限于此。存储器901可以是上述存储器的组合。
处理器902,可以包括一个或多个中央处理单元(central processing unit,CPU)或者为数字处理单元等等。处理器902,用于调用存储器901中存储的计算机程序时实现上述定位失效检测方法。
通讯模块903用于与机器人、数据库或其他数据处理设备进行通信。
本申请实施例中不限定上述存储器901、通讯模块903和处理器902之间的具体连接介质。本申请实施例在图7中以存储器901和处理器902之间通过总线904连接,总线904在图7中以粗线表示,其它部件之间的连接方式,仅是进行示意性说明,并不引以为限。总线904可以分为地址总线、数据总线、控制总线等。为便于表示,图7中仅用一条粗线表示,但并不表示仅有一根总线或一种类型的总线。
存储器901中存储有计算机存储介质,计算机存储介质中存储有计算机可执行指令,计算机可执行指令用于实现本申请实施例的定位失效检测方法。处理器902用于执行上述的定位失效检测方法。
本申请的实施例还提供了一种计算机存储介质,计算机存储介质中存储有计算机可执行指令,计算机可执行指令用于实现本申请任一实施例所记载的定位失效检测方法。
在一些可能的实施方式中,本申请提供的定位失效检测方法的各个方面还可以实现为一种程序产品的形式,其包括程序代码,当程序产品在计算机设备上运行时,程序代码用于使计算机设备执行本说明书上述描述的根据本申请各种示例性实施方式的定位失效检测方法的步骤,例如,计算机设备可以执行如图2所示的步骤S201~S204的定位失效检测方法的流程。
本领域普通技术人员可以理解:实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成,前述的程序可以存储于一计算机可读取存储介质中,该程序在执行时,执行包括上述方法实施例的步骤;而前述的存储介质包括:移动存储设备、只读存储器(ROM,Read-Only Memory)、随机存取存储器(RAM,Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述,仅为本申请的具体实施方式,但本申请的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本申请揭露的技术范围内,可轻易想到变化或替换,都应涵盖在本申请的保护范围之内。
Claims (7)
1.一种定位失效检测方法,其特征在于,包括:
获取移动机器人的激光雷达扫描数据,并提取所述激光雷达扫描数据的地图二维直线特征;
根据所述地图二维直线特征,确定所述地图二维直线特征的特征定位性能预测指标;
根据所述特征定位性能预测指标和获取的机器人位姿粒子集的粒子权重,确定融合特征定位性能预测指标的融合粒子集权重;所述机器人位姿粒子集包含在所述移动机器人定位时用于确定所述移动机器人的位姿的粒子;所述粒子权重是所述机器人位姿粒子集中各个粒子的粒子权重;
若所述融合粒子集权重小于或等于设定阈值,确定所述移动机器人定位失效;
所述提取所述激光雷达扫描数据中的地图二维直线特征,包括:
根据获取的移动机器人的激光雷达扫描数据,确定断点检测算子;
确定所述激光雷达扫描数据的各个相邻的两个测量点之间的距离;
根据所述各个相邻的两个测量点之间的距离和所述断点检测算子,将所述激光雷达扫描数据中的测量点划分为至少一个数据点集;
分别对得到的各个数据点集进行直线拟合,得到各个数据点集对应的直线段;
根据各个数据点集对应的直线段,生成所述激光雷达扫描数据的地图二维直线特征;所述地图二维直线特征是所述直线段构成的集合;
所述根据所述地图二维直线特征,确定所述地图二维直线特征的特征定位性能预测指标,包括:
对于每个数据点集,执行如下步骤:分别根据所述数据点集中每个测量点的坐标数据,确定每个测量点的标量协方差;根据所述每个测量点的标量协方差,确定所述数据点集对应的地图二维直线特征的直线段位置的特征协方差矩阵;
根据所述特征协方差矩阵以及地图二维直线特征的各个直线段的特征,得到所述特征定位性能预测指标。
2.根据权利要求1所述的方法,其特征在于,所述根据所述各个相邻的两个测量点之间的距离和所述断点检测算子,将所述激光雷达扫描数据中的测量点划分为至少一个数据点集,包括:
若相邻的两个测量点之间的距离大于所述断点检测算子,则将该两个测量点划分至不同的数据点集;
若相邻的两个测量点之间的距离小于或等于所述断点检测算子,则将该两个测量点划分至同一个数据点集。
3.根据权利要求1所述的方法,其特征在于,所述根据所述特征定位性能预测指标和获取的机器人位姿粒子集的粒子权重,确定融合特征定位性能预测指标的融合粒子集权重,包括:
根据获取的机器人位姿粒子集的粒子权重,确定所述机器人位姿粒子集中的全部粒子的粒子权重的平均值,作为基本粒子集权重;
根据所述基本粒子集权重和所述特征定位性能预测指标,确定融合特征定位性能预测指标的融合粒子集权重。
4.一种定位失效检测装置,其特征在于,包括:
特征提取单元,用于获取移动机器人的激光雷达扫描数据,并提取所述激光雷达扫描数据的地图二维直线特征;
指标计算单元,用于根据所述地图二维直线特征,确定所述地图二维直线特征的特征定位性能预测指标;
权重融合单元,用于根据所述特征定位性能预测指标和获取的机器人位姿粒子集的粒子权重,确定融合特征定位性能预测指标的融合粒子集权重;所述机器人位姿粒子集包含在所述移动机器人定位时用于确定所述移动机器人的位姿的粒子;所述粒子权重是所述机器人位姿粒子集中各个粒子的粒子权重;
失效判断单元,用于若监测到所述融合粒子集权重小于或等于设定阈值,确定所述移动机器人定位失效;
所述特征提取单元,具体用于:
根据获取的移动机器人的激光雷达扫描数据,确定断点检测算子;
确定所述激光雷达扫描数据的各个相邻的两个测量点之间的距离;
根据所述各个相邻的两个测量点之间的距离和所述断点检测算子,将所述激光雷达扫描数据中的测量点划分为至少一个数据点集;
分别对得到的各个数据点集进行直线拟合,得到各个数据点集对应的直线段;
根据各个数据点集对应的直线段,生成所述激光雷达扫描数据的地图二维直线特征;所述地图二维直线特征是所述直线段构成的集合;
所述指标计算单元,具体用于:
对于每个数据点集,执行如下步骤:分别根据所述数据点集中每个测量点的坐标数据,确定每个测量点的标量协方差;根据所述每个测量点的标量协方差,确定所述数据点集对应的地图二维直线特征的直线段位置的特征协方差矩阵;
根据所述特征协方差矩阵以及地图二维直线特征的各个直线段的特征,得到所述特征定位性能预测指标。
5.根据权利要求4所述的装置,其特征在于,所述权重融合单元,具体用于:
根据获取的机器人位姿粒子集的粒子权重,确定所述机器人位姿粒子集中的全部粒子的粒子权重的平均值,作为基本粒子集权重;
根据所述基本粒子集权重和所述特征定位性能预测指标,确定融合特征定位性能预测指标的融合粒子集权重。
6.一种计算机可读存储介质,所述计算机可读存储介质内存储有计算机程序,其特征在于:所述计算机程序被处理器执行时,实现权利要求1~3中任一项所述的方法。
7.一种电子设备,其特征在于,包括存储器和处理器,所述存储器上存储有可在所述处理器上运行的计算机程序,当所述计算机程序被所述处理器执行时,实现权利要求1~3中任一项所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011335423.0A CN112344966B (zh) | 2020-11-24 | 2020-11-24 | 一种定位失效检测方法、装置、存储介质及电子设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011335423.0A CN112344966B (zh) | 2020-11-24 | 2020-11-24 | 一种定位失效检测方法、装置、存储介质及电子设备 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112344966A CN112344966A (zh) | 2021-02-09 |
CN112344966B true CN112344966B (zh) | 2023-06-16 |
Family
ID=74365526
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011335423.0A Active CN112344966B (zh) | 2020-11-24 | 2020-11-24 | 一种定位失效检测方法、装置、存储介质及电子设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112344966B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113558524B (zh) * | 2021-07-14 | 2022-11-29 | 北京小狗吸尘器集团股份有限公司 | 扫地机器人及其被抱起后的重新定位方法和装置 |
Family Cites Families (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106444757B (zh) * | 2016-09-27 | 2020-06-30 | 成都普诺思博科技有限公司 | 基于直线特征地图的ekf-slam方法 |
US10740645B2 (en) * | 2018-06-29 | 2020-08-11 | Toyota Research Institute, Inc. | System and method for improving the representation of line features |
CN109682382B (zh) * | 2019-02-28 | 2020-09-08 | 电子科技大学 | 基于自适应蒙特卡洛和特征匹配的全局融合定位方法 |
CN109737968B (zh) * | 2019-03-07 | 2022-11-15 | 中山大学 | 基于二维LiDAR和智能手机的室内融合定位方法 |
CN110243380B (zh) * | 2019-06-26 | 2020-11-24 | 华中科技大学 | 一种基于多传感器数据与角度特征识别的地图匹配方法 |
CN110763239B (zh) * | 2019-11-14 | 2021-08-24 | 华南智能机器人创新研究院 | 滤波组合激光slam建图方法及装置 |
CN111044036B (zh) * | 2019-12-12 | 2021-10-15 | 浙江大学 | 基于粒子滤波的远程定位方法 |
CN111060888B (zh) * | 2019-12-31 | 2023-04-07 | 芜湖哈特机器人产业技术研究院有限公司 | 一种融合icp和似然域模型的移动机器人重定位方法 |
-
2020
- 2020-11-24 CN CN202011335423.0A patent/CN112344966B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN112344966A (zh) | 2021-02-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN102175222B (zh) | 基于立体视觉的吊车避障*** | |
JP5759161B2 (ja) | 物体認識装置、物体認識方法、学習装置、学習方法、プログラム、および情報処理システム | |
CN111708047A (zh) | 机器人定位评估方法、机器人及计算机存储介质 | |
Yang et al. | On solving mirror reflection in lidar sensing | |
US8971576B2 (en) | Information processing apparatus and processing method thereof | |
US20140142891A1 (en) | Generaton of map data | |
EP3693759B1 (en) | System and method for tracking motion of target in indoor environment | |
CN111445531A (zh) | 一种多目相机导航方法、装置、设备及存储介质 | |
CN113432533B (zh) | 一种机器人定位方法、装置、机器人及存储介质 | |
KR20180087519A (ko) | 레이저 거리 센서의 측정 거리에 대해 추정된 거리 유형의 신뢰성을 평가하는 방법 및 이를 이용한 이동 로봇의 위치 추정 방법 | |
CN113688730A (zh) | 障碍物测距方法、装置、电子设备、存储介质及程序产品 | |
CN112313536B (zh) | 物体状态获取方法、可移动平台及存储介质 | |
CN112344966B (zh) | 一种定位失效检测方法、装置、存储介质及电子设备 | |
CN110426714B (zh) | 一种障碍物识别方法 | |
WO2022116831A1 (zh) | 定位方法、装置、电子设备及可读存储介质 | |
CN112731337B (zh) | 地图构建方法、装置和设备 | |
D’Adamo et al. | Registration of three‐dimensional scanning LiDAR sensors: An evaluation of model‐based and model‐free methods | |
CN111474560B (zh) | 一种障碍物定位方法、装置及设备 | |
CN113177980A (zh) | 用于自动驾驶的目标对象速度确定方法、装置及电子设备 | |
Shvets et al. | Occupancy grid mapping with the use of a forward sonar model by gradient descent | |
CN113158816B (zh) | 面向室外场景物体的视觉里程计二次曲面路标构建方法 | |
CN114861725A (zh) | 一种目标感知跟踪的后处理方法、装置、设备及介质 | |
CN111951337B (zh) | 图像检测目标空间定位方法及*** | |
CN115147561A (zh) | 位姿图生成方法、高精地图生成方法和装置 | |
CN114705223A (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 | ||
TR01 | Transfer of patent right |
Effective date of registration: 20240508 Address after: Room 6227, No. 999, Changning District, Shanghai 200050 Patentee after: Shenlan robot (Shanghai) Co.,Ltd. Country or region after: China Address before: Unit 1001, 369 Weining Road, Changning District, Shanghai, 200336 (9th floor of actual floor) Patentee before: DEEPBLUE TECHNOLOGY (SHANGHAI) Co.,Ltd. Country or region before: China |
|
TR01 | Transfer of patent right |