CN113917475A - 定位方法、装置、电子设备和计算机可读存储介质 - Google Patents
定位方法、装置、电子设备和计算机可读存储介质 Download PDFInfo
- Publication number
- CN113917475A CN113917475A CN202010651193.2A CN202010651193A CN113917475A CN 113917475 A CN113917475 A CN 113917475A CN 202010651193 A CN202010651193 A CN 202010651193A CN 113917475 A CN113917475 A CN 113917475A
- Authority
- CN
- China
- Prior art keywords
- observation
- depth
- correction process
- motion state
- observation position
- 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.)
- Pending
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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/42—Simultaneous measurement of distance and other co-ordinates
-
- 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/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
-
- 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/4808—Evaluating distance, position or velocity data
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30196—Human being; Person
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Electromagnetism (AREA)
- Multimedia (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本申请提出一种定位方法、装置、电子设备和计算机可读存储介质,涉及计算机视觉技术领域,其中,方法包括:获取激光雷达测量得到的点云图,获取深度传感器测量得到的深度图,从点云图中提取第一目标区域,以及从深度图中提取第二目标区域,其中,第一目标区域和第二目标区域与同步采集的视觉图像中的感兴趣区域检测相同对象,根据对象的历史运动状态,对第一目标区域携带的第一定位信息,以及第二目标区域携带的第二定位信息进行融合,得到对象的目标位置,实现了对多个时间不同步的传感器测量数据的融合,融合后提高了目标位置确定的准确度,解决现有技术中对于时间不同步的传感器数据无法进行有效融合的技术问题。
Description
技术领域
本申请涉及计算机视觉技术领域,尤其涉及一种定位方法、装置、电子设备和计算机可读存储介质。
背景技术
机器人与人进行交互过程中,很重要的一个应用是对人进行跟踪,以跟随人运动到指定的位置,因此,需要实时检测人体,并估计出人体和机器人之间的相对位置关系。
而现有技术中,通常是基于相机和深度传感器的数据,计算人体与机器人之间的距离,然而,相机和深度传感器的测量时时间通常是无法同步的,使得相机数据和深度传感器数据无法进行融合,计算得到的人体和机器人之间的相对位置关系准确度较差。
发明内容
本申请旨在至少在一定程度上解决相关技术中的技术问题之一。
为此,本申请的第一个目的在于提出一种定位方法,根据视觉图像中感兴趣区域包含的对象,从点云图中提取第一目标区域,以及从深度图中提取第二目标区域,根据对象的历史运动状态,对多个时间不同步的传感器测量数据进行了融合,融合后提高了目标位置确定的准确度。
本申请的第二个目的在于提出一种定位装置。
本申请的第三个目的在于提出一种电子设备。
本申请的第四个目的在于提出一种非临时性计算机可读存储介质。
为达上述目的,本申请第一方面实施例提出了一种定位方法,包括:
获取激光雷达测量得到的点云图;获取深度传感器测量得到的深度图;从所述点云图中提取第一目标区域,以及从所述深度图中提取第二目标区域;其中,所述第一目标区域和所述第二目标区域与同步采集的视觉图像中的感兴趣区域检测相同对象;
根据所述对象的历史运动状态,对所述第一目标区域携带的第一定位信息,以及所述第二目标区域携带的第二定位信息进行融合,得到所述对象的目标位置。
可选地,作为第一方面的第一种可能的实现方式,所述根据所述对象的历史运动状态,对所述第一目标区域携带的第一定位信息,以及所述第二目标区域携带的第二定位信息进行融合,得到所述对象的目标位置,包括:
根据所述第一目标区域携带的第一定位信息,确定所述对象所处的第一观测位置;
根据所述第二目标区域携带的第二定位信息,确定所述对象所处的第二观测位置;
根据所述对象的历史运动状态,对所述第一观测位置和所述第二观测位置中的一个,执行迭代修正过程;
根据所述迭代修正过程得到的位置更新所述历史运动状态;
根据更新后的历史运动状态,对所述第一观测位置和所述第二观测位置中的另一个,执行迭代修正过程,以得到所述目标位置。
可选地,作为第一方面的第二种可能的实现方式,所述执行迭代修正过程,包括:
根据本次迭代修正过程采用的历史运动状态,得到预测运动状态;所述本次迭代修正过程采用的历史运动状态,是根据前一次迭代修正过程得到的位置,以及前一次迭代修正过程采用的历史运动状态生成;
根据所述预测运动状态,得到预测观测位置;
根据所述预测观测位置,对进行本次迭代修正过程的所述第一观测位置或所述第二观测位置进行修正。
可选地,作为第一方面的第三种可能的实现方式,所述根据所述对象的历史运动状态,对所述第一观测位置和所述第二观测位置中的一个,执行迭代修正过程之前,还包括:
比较获取到所述点云图的第一观测时刻,以及获取到所述深度图的第二观测时刻;
若所述第一观测时刻先于所述第二观测时刻,则确定所述第一观测位置先于所述第二观测位置进行迭代修正;
若所述第一观测时刻晚于所述第二观测时刻,则确定所述第二观测位置先于所述第一观测位置进行迭代修正;
若所述第一观测时刻等于所述第二观测时刻,则随机确定所述第一观测位置和所述第二观测位置进行迭代修正的顺序。
可选地,作为第一方面的第四种可能的实现方式,所述历史运动状态包括所述对象的历史位置和历史速度;
对应地,针对每一次迭代修正过程,本次迭代修正过程采用的历史运动状态中的历史位置,是根据前一次迭代修正过程得到的位置,以及前一次迭代修正过程采用的历史位置生成;
针对每一次迭代修正过程,本次迭代修正过程采用的历史运动状态中的历史速度,是根据本次迭代修正过程采用的历史位置,以及前一次迭代修正过程采用的历史位置确定的。
可选地,作为第一方面的第五种可能的实现方式,所述根据所述预测运动状态,得到预测观测位置,包括:
将所述预测运动状态代入观测方程,得到所述预测观测位置;
其中,所述观测方程为所述预测运动状态与转换矩阵的乘积叠加量测噪声项;
所述转换矩阵,用于指示所述预测运动状态与所述预测观测位置之间的转换关系;
所述量测噪声项,符合设定协方差的高斯白噪声分布;所述设定协方差是根据设备精度和测量置信度确定的。
可选地,作为第一方面的第六种可能的实现方式,所述根据所述预测观测位置,对进行本次迭代修正过程的所述第一观测位置或所述第二观测位置进行修正,包括:
对进行本次迭代修正过程的所述第一观测位置或所述第二观测位置,确定测量残差;
若所述测量残差小于差异阈值,则根据所述预测观测位置,对进行本次迭代修正过程的所述第一观测位置或所述第二观测位置进行修正。
可选地,作为第一方面的第七种可能的实现方式,所述测量残差是所述预测观测位置与进行本次迭代修正过程的所述第一观测位置或所述第二观测位置之间的差异。
可选地,作为第一方面的第八种可能的实现方式,所述从所述点云图中提取第一目标区域,包括:
对所述感兴趣区域,确定图像坐标系中的直角坐标位置;
将所述图像坐标系中的直角坐标位置,映射至所述激光雷达的极坐标系,得到极坐标位置;
根据所述极坐标位置,从所述点云图中提取所述第一目标区域。
可选地,作为第一方面的第九种可能的实现方式,所述对所述感兴趣区域,确定图像坐标系中的直角坐标位置,包括:
对所述感兴趣区域的左右边界,确定所述直角坐标位置。
可选地,作为第一方面的第十种可能的实现方式,所述将所述图像坐标系中的直角坐标位置,映射至所述激光雷达的极坐标系,得到极坐标位置,包括:
将所述图像坐标系中的直角坐标位置,通过相机的内参矩阵映射至相机坐标系,得到所述相机坐标系的直角坐标位置;其中,所述相机用于采集所述视觉图像;
将所述相机坐标系的直角坐标位置,通过所述相机与所述激光雷达的外参矩阵映射至所述极坐标系,得到所述极坐标位置。
可选地,作为第一方面的第十一种可能的实现方式,所述根据所述第一目标区域携带的第一定位信息,确定所述对象所处的第一观测位置,包括:
根据所述第一目标区域携带的第一定位信息,确定所述第一目标区域中各像素点的深度;
根据所述各像素点的深度,统计各设定深度对应的像素点个数指示值;
根据所述像素点个数指示值的峰值,从各设定深度中确定目标深度;
根据所述目标深度,定位对象所处的第一观测位置。
可选地,作为第一方面的第十二种可能的实现方式,所述根据像素点个数指示值的峰值,从各设定深度中确定目标深度,包括:
从各设定深度中,确定前景深度和背景深度;其中,所述背景深度具有像素点个数指示值的最大峰值;所述前景深度具有按照深度从小到大顺序的像素点个数指示值的首个峰值;
根据所述前景深度和所述背景深度对应的像素点个数指示值,从所述前景深度和所述背景深度中选定所述目标深度。
可选地,作为第一方面的第十三种可能的实现方式,所述根据所述前景深度和所述背景深度对应的像素点个数指示值,从所述前景深度和所述背景深度中选定所述目标深度,包括:
若所述前景深度和所述背景深度的像素点个数指示值之比大于比例阈值,则将所述前景深度作为所述目标深度;
若所述前景深度和所述背景深度的像素点个数指示值之比不大于所述比例阈值,则将所述背景深度作为所述目标深度。
可选地,作为第一方面的第十四种可能的实现方式,所述根据所述目标深度,定位对象所处的第一观测位置之后,还包括:
根据所述目标深度对应的像素点个数指示值,确定所述第一观测位置的测量置信度;
若所述前景深度和所述背景深度相同,提高所述第一观测位置的测量置信度,其中,所述测量置信度,用于为所述迭代修正过程中采用的观测方程,生成量测噪声项,所述观测方程用于在根据所述迭代修正过程采用的历史运动状态,得到预测运动状态之后,将所述预测运动状态代入所述观测方程,以得到预测观测位置。
可选地,作为第一方面的第十五种可能的实现方式,所述根据所述各像素点的深度,统计各设定深度对应的像素点个数指示值之后,还包括:
筛选掉所述像素点个数指示值小于个数阈值的设定深度。
可选地,作为第一方面的第十六种可能的实现方式,所述根据所述各像素点的深度,统计各设定深度对应的像素点个数指示值,包括:
对每一个设定深度,确定深度统计范围;
根据各像素点的深度,统计所述深度与相应深度统计范围匹配的像素点个数,以得到相应设定深度对应的像素点个数指示值。
可选地,作为第一方面的第十七种可能的实现方式,所述第一观测位置包括观测距离和观测角度;所述第一定位信息包括深度和角度;
所述根据所述目标深度,定位对象所处的第一观测位置,包括:
根据所述目标深度对应的各像素点携带的角度,定位所述对象的观测角度;
根据所述目标深度对应的各像素点携带的深度,定位所述对象的观测距离。
为达上述目的,本申请第二方面实施例提出了一种定位装置,包括:
获取模块,用于获取激光雷达测量得到的点云图;
所述获取模块,还用于获取深度传感器测量得到的深度图;
提取模块,用于从所述点云图中提取第一目标区域,以及从所述深度图中提取第二目标区域;其中,所述第一目标区域和所述第二目标区域与同步采集的视觉图像中的感兴趣区域检测相同对象;
融合模块,用于根据所述对象的历史运动状态,对所述第一目标区域携带的第一定位信息,以及所述第二目标区域携带的第二定位信息进行融合,得到所述对象的目标位置。
为达上述目的,本申请第三方面实施例提出了一种电子设备,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,所述处理器执行所述程序时,实现如第一方面所述的定位方法。
为了实现上述目的,本申请第四方面实施例提出了一种其上存储有计算机程序,其特征在于,所述程序被处理器执行时实现如第一方面所述的定位方法。
本申请实施例所提供的技术方案可以包含如下的有益效果:
根据视觉图像中感兴趣区域包含的对象,从点云图中提取第一目标区域,以及从深度图中提取第二目标区域,根据对象的历史运动状态,对多个时间不同步的传感器测量数据进行了融合,融合后提高了目标位置确定的准确度。
本申请附加的方面和优点将在下面的描述中部分给出,部分将从下面的描述中变得明显,或通过本申请的实践了解到。
附图说明
本申请上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解,其中:
图1为本申请实施例所提供的一种定位方法的流程示意图;
图2为本申请实施例提供的另一种定位方法的流程示意图;
图3为坐标系转换的示意图;
图4为本申请实施例提供的又一种定位方法的流程示意图;
图5为本申请实施例提供的一种迭代修正过程的流程示意图;
图6为各状态变量在极坐标系中的定义示意图;
图7为本申请实施例提供的另一种迭代修正过程的流程示意图;
图8为本申请实施例提供的又一种定位方法的流程示意图;
图9为本申请实施例提供的又一种定位方法的流程示意图;
图10为本申请提供的一种直方图示意图;
图11为本申请提供的另一种直方图示意图;以及
图12为本申请实施例提供的一种定位装置的结构示意图。
具体实施方式
下面详细描述本申请的实施例,所述实施例的示例在附图中示出,其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的,旨在用于解释本申请,而不能理解为对本申请的限制。
下面参考附图描述本申请实施例的定位方法、装置、电子设备和计算机可读存储介质。
图1为本申请实施例所提供的一种定位方法的流程示意图。
如图1所示,该方法包括以下步骤:
步骤101,获取激光雷达测量得到的点云图。
步骤102,获取深度传感器测量得到的深度图。
本实施例中,利用激光雷达测量得到的点云图和深度传感器测量得到的深度图均携带有相应的定位信息。
其中,点云图携带的定位信息包含激光雷达测量到的各对象的角度信息和深度信息,深度信息指示了对象与激光雷达的相对距离信息。
其中,深度传感器,例如为(Depth Red Green Blue,RGB-D)传感器,深度图携带的定位信息包含深度传感器测量得到的各对象的深度信息,深度信息指示了对象与深度传感器的相对距离信息。
步骤103,从点云图中提取第一目标区域,以及从深度图中提取第二目标区域,其中,第一目标区域和第二目标区域与同步采集的视觉图像中的感兴趣区域检测相同对象。
其中,感兴趣区域是指视觉图像中包含对象的区域,例如,对象是人体,则感兴趣区域则为包含人体的区域。
本实施例中,在通过激光雷达测量得到点云图,深度传感器测量得到深度图时,还同步通过相机采集了视觉图像,对视觉图像进行检测确定感兴趣区域,例如,通过目标检测算法(Single Shot MultiBox Detector,SSD)或目标检测算法(You Only Look Once,YOLO)从图像中确定感兴趣区域。
根据确定的感兴趣区域,从点云图中提取第一目标区域,而从点云图中提取第一目标区域时,由于感兴趣区域对应的参考坐标为图像坐标系,而激光雷达测量得到的点云图为激光雷达的极坐标系,因此,需要转换至相同的坐标系中,为了较小计算量,将感兴趣区域从图像坐标系中转换至激光雷达的极坐标系中,实现了坐标系的统一,从而实现了从点云图中提取第一目标区域。其中,坐标系的转换方法下一实施例中会具体介绍。
根据确定的感兴趣区域,还从深度图中提取第二目标区域,也就是说第一目标区域和第二目标区域与同步采集的视觉图像中的感兴趣区域检测相同对象,为不同的传感器进行数据融合确定对象的位置提供了基础。
步骤104,根据对象的历史运动状态,对第一目标区域携带的第一定位信息,以及第二目标区域携带的第二定位信息进行融合,得到对象的目标位置。
具体地,根据第一目标区域携带的第一定位信息,确定对象所处的第一观测位置,根据第二目标区域携带的第二定位信息,确定对象所处的第二观测位置,根据对象的历史运动状态,对第一观测位置和第二观测位置中的一个,执行迭代修正过程,根据迭代修正过程得到的位置更新历史运动状态,进而,根据更新后的历史运动状态,对第一观测位置和第二观测位置中的另一个,执行迭代修正过程,以得到目标位置,在迭代修正的过程中,实现了将第一观测位置和第二观测位置进行融合,得到目标位置,也就是说在迭代修正的过程中融合了深度传感器的测量数据和激光雷达传感器的测量数据,融合后增加了用于确定目标位置的信息量,提高了目标位置确定的准确性。
本申请实施例的定位方法中,获取激光雷达测量得到的点云图,获取深度传感器测量得到的深度图,从点云图中提取第一目标区域,以及从深度图中提取第二目标区域,其中,第一目标区域和第二目标区域与同步采集的视觉图像中的感兴趣区域检测相同对象,根据对象的历史运动状态,对第一目标区域携带的第一定位信息,以及第二目标区域携带的第二定位信息进行融合,得到对象的目标位置,根据视觉图像中感兴趣区域包含的对象,从点云图中提取第一目标区域,以及从深度图中提取第二目标区域,根据对象的历史运动状态,对多个时间不同步的传感器测量数据进行了融合,融合后提高了目标位置确定的准确度。
基于上一实施例,本实施例提供了一种实现方式,具体说明了如何从点云图像中提取第一目标区域。
如图2所示,步骤103中从点云图中提取第一目标区域,可以包含以下步骤:
步骤201,对感兴趣区域,确定图像坐标系中的直角坐标位置。
具体地,图像坐标系为平面坐标系,因此,可保留感兴趣区域对应的左右边界,对感兴趣区域的左右边界,确定直角坐标位置,如图3所示,感兴趣区域对应的左右边界对应四个顶点,根据左右边界对应的四个顶点,确定图像坐标系中的直角坐标位置。
步骤202,将图像坐标系中的直角坐标位置,映射至激光雷达的极坐标系,得到极坐标位置。
如图3所示,将图像坐标系中感兴趣区域的直角坐标位置,通过相机的内参矩阵映射至相机坐标系,得到相机坐标系的直角坐标位置,其中,相机用于采集视觉图像,进而,将相机坐标系的直角坐标位置,通过相机与激光雷达的外参矩阵映射至激光雷达的极坐标系,得到感兴趣区域在激光雷达的极坐标系中对应的极坐标位置,实现了相同坐标系的转换,而将感兴趣区域从图像坐标系转换至激光雷达的极坐标系降低了运算量。
步骤203,根据极坐标位置,从点云图中提取第一目标区域。
具体地,根据感兴趣区域在激光雷达的极坐标系中对应的极坐标位置,从点云图中确定极坐标位置对应的区域,将极坐标位置对应的区域作为第一目标区域,其中,第一目标区域与同步采集的视觉图像中的感兴趣区域检测相同对象。
本实施例的定位方法中,对通过相机采集的视觉图像中的感兴趣区域进行坐标系转换,得到激光雷达的极坐标系中对应的极坐标位置,实现了相同坐标系的转换,而将感兴趣区域从图像坐标系转换至激光雷达的极坐标系降低了运算量,根据极坐标位置从点云图中提取第一目标区域,使得第一目标区域与同步采集的视觉图像中的感兴趣区域检测相同对象,便于进行后续与深度传感器采集的数据进行融合。
本申请的一个实施例中,对于步骤103中从深度图中提取第二目标区域,作为一种可能的实现方式,将获取到的深度图与相机采集的视觉图像进行配准,进而,根据视觉图像中感兴趣区域对深度图进行裁剪,以得到和感兴趣区域检测相同对象的第二目标区域。
基于上述实施例,本实施例提供了一种实现方式,详细说明了如何实现将多个时间不同步的传感器的测量数据进行融合,以提高目标位置确定的准确性,其中,融合的算法可以是基于无迹卡尔曼滤波器(Unscented Kalman Filter,UKF)实现的,也可以是基于经典卡尔曼滤波器,或者扩展卡尔曼滤波器(ExtendedKalmanFilter,EKF)实现的,而本实施例中,以基于无迹卡尔曼滤波器(Unscented Kalman Filter)实现为例进行说明,但不构成对本实施例的限定。
如图4所示,上述步骤104可以包括以下步骤:
步骤401,根据第一目标区域携带的第一定位信息,确定对象所处的第一观测位置。
其中,第一定位信息包含深度和角度,第一观测位置包含观测角度和观测距离。
具体地,根据第一目标区域中各像素点携带的第一定位信息,统计第一定位信息与各设定深度匹配的像素点,得到各设定深度对应的像素点个数指示值,根据像素点个数指示值的峰值,从各设定深度中确定目标深度,根据目标深度,定位对象所处的第一观测位置。其中,针对第一观测位置的确定方法,后续实施例中会详细描述。
步骤402,根据第二目标区域携带的第二定位信息,确定对象所处的第二观测位置。
其中,第二定位信息包含深度,第二观测位置包含观测距离。
具体地,根据第二目标区域中各像素点携带的第二定位信息,统计第二定位信息与各设定深度匹配的像素点,得到各设定深度对应的像素点个数指示值,根据像素点个数指示值的峰值,从各设定深度中确定目标深度,根据目标深度,定位对象所处的第二观测位置。其中,针对第二观测位置的确定方法,后续实施例中会详细描述。
步骤403,根据对象的历史运动状态,对第一观测位置和第二观测位置中的一个,执行迭代修正过程。
步骤404,根据迭代修正过程得到的位置更新历史运动状态。
步骤405,根据更新后的历史运动状态,对第一观测位置和第二观测位置中的另一个,执行迭代修正过程,以得到目标位置。
本实施例中,可先对第一观测位置执行迭代修正过程,也可以先对第二观测位置执行迭代修正过程,其中,如何确定第一观测位置和第二观测位置执行迭代修正的过程的顺序,下一实施例中会进行详细描述。
在一个实施例中,根据对象的历史运动状态,对第一观测位置执行迭代修正过程,根据迭代修正过程得到的位置更新历史运动状态,得到更新后的历史运动状态,进而,根据更新后的历史运动状态,对第二观测位置执行迭代修正过程,以得到目标位置,也就是说目标位置是在第一观测位置迭代修正的基础上,又进行第二观测位置迭代修正后得到的,即通过融合第一定位信息和第二定位信息得到的,也就是说融合了深度传感器的测量数据和激光雷达传感器的测量数据,通过融合增加了用于确定目标位置的信息量,从而提高了目标位置确定的准确性。
同理,先对第二观测位置执行迭代修正过程,再对第一观测位置执行迭代修正过程,和上述融合确定目标位置的原理相同,此处不再赘述。
需要说明的是,本实施例中是以两个传感器,即激光雷达传感器和深度传感器采集数据的时间顺序进行融合为例,而实际应用中,还可以根据采集时间的顺序,对三个或更多个传感器进行融合,本实施例中不进行限定。
本申请实施例的定位方法中,根据对象的历史运动状态,对第一观测位置和第二观测位置中的一个,执行迭代修正过程,根据迭代修正过程得到的位置更新历史运动状态,根据更新后的历史运动状态,对第一观测位置和第二观测位置中的另一个,执行迭代修正过程,以得到目标位置,目标位置是通过融合第一定位信息和第二定位信息得到的,也就是说融合了深度传感器的测量数据和激光雷达传感器的测量数据,通过融合增加了用于确定目标位置的信息量,从而提高了目标位置确定的准确性。
基于上述实施例,图5为本申请实施例提供的一种迭代修正过程的流程示意图。
如图5所示,上述步骤403和步骤405中,执行迭代修正过程,可以包含以下步骤:
步骤501,根据本次迭代修正过程采用的历史运动状态,得到预测运动状态,其中,本次迭代修正过程采用的历史运动状态,是根据前一次迭代修正过程得到的位置,以及前一次迭代修正过程采用的历史运动状态生成。
在本申请的一个实施例中,将本次迭代修正过程采用的历史运动状态带入系统状态方程,得到预测运动状态。
本实施例中,系统状态方程为历史运动状态与状态转移矩阵的乘积叠加过程噪声项。
例如,系统状态方程表示为:X(t+1)=g(t)X(t)+w(t),其中,X(t)表示历史运动状态,w(t)为过程噪声项,符合期望为0,协方差为Q的高斯白噪声,其协方差Q可以初始化为常值。g(t)为状态转移矩阵,例如,其中,X(t)为历史运动状态,历史运动状态中包含的dist(t),指示历史状态中对象的相对距离;vel(t)指示对象的相对线速度;sita(t)指示对象的相对角度;omega(t)指示对象的角速度。X(t+1)为根据历史运动状态X(t)预测得到的预测运动状态。
以机器人场景为例,机器人中设置的激光雷达传感器和深度传感器实时采集人的图像信息,图6为各状态变量在极坐标系中的定义示意图,如图6所示,则历史运动状态的各个变量在激光雷达的极坐标系中的定义如下:
图6中,三角形处指示的为机器人的位置,五角星处指示的为对象,即人的位置,编号1指示的即为dist(t),即人与机器人之间的相对距离,始终为正值;
编号2指示的即sita(t):人在机器人坐标系中的相对角度,正前方为0度,顺时针为正;
编号3指示的即vel(t),即人与机器人之间的相对线速度,远离为正,即箭头指示的方向为正;
编号4指示的即为omega(t),即人在机器人坐标系中,沿切向运动的角速度,顺时针为正,即箭头指示的方向为正。
其中,X(t+1)为根据历史运动状态X(t)预测得到的预测运动状态。
步骤502,根据预测运动状态,得到预测观测位置。
本申请的一个实施例中,将预测运动状态带入观测方程,得到预测观测位置。其中,观测方程为预测运动状态与转换矩阵的乘积叠加量测噪声项。其中,转换矩阵,用于指示预测运动状态与预测观测位置之间的转换关系;量测噪声项,符合设定协方差的高斯白噪声分布,而设定协方差是根据设备精度和测量置信度确定的,其中,测量置信度包含第一观测位置的测量置信度或第二观测位置的测量置信度,而测量置信度的确定方法,后续实施例中会说明。。
本实施例中,将预测运动状态代入观测方程后,可以得到预测观测位置,预测观测位置包含预测观测角度和预测观测距离。
如,预测观测距离的观测方程为:dist(t+1)=h1*X(t+1)+v1(t+1);
预测观测角度的观测方程为:sita(t+1)=h2*X(t+1)+v2(t+1)。
其中,dist(t+1)表示观测距离,sita(t+1)表示观测角度,h1和h2表示转换矩阵,v1(t+1)和v2(t+1)表示量测噪声项,例如,h1=[1 0 0 0],h2=[0 0 1 0]。步骤503,根据预测观测位置,对进行本次迭代修正过程的第一观测位置或第二观测位置进行修正。
在本申请的一个实施例中,若本次迭代修正过程,是对第一观测位置进行修正,则根据预测观测位置,对进行本次迭代修正过程的第一观测位置进行修正,以得到修正后的位置。
其中,预测观测位置包含预测角度和预测距离,第一观测位置包含对象的第一观测角度和第二观测距离,从而,修正后得到的位置也包含观测距离和观测角度。
作为一种可能的实现方式,确定预测观测位置的权重和第一观测位置的权重,根据预测观测位置的权重和第一观测位置的权重进行加权计算,得到本次迭代修正过程得到的位置。其中,修正后得到的位置也包含观测距离和观测角度。
在本申请的另一个实施例中,若本次迭代修正过程,是对第二观测位置进行修正,则根据预测观测位置,对进行本次迭代修正过程的第二观测位置进行修正,以得到修正后的位置。
其中,预测观测位置包含预测角度和预测距离,第一观测位置包含对象的第一观测角度和第二观测距离,从而,修正后得到的位置也包含观测距离和观测角度。
作为一种可能的实现方式,确定预测观测位置的权重和第二观测位置的权重,根据预测观测位置的权重和第二观测位置的权重进行加权计算,得到本次迭代修正过程得到的位置,也就是说实现了,通过预测观测位置对第二观测位置进行修正。
需要说明的是,本次迭代修正过程采用的历史运动状态,是根据前一次迭代修正过程得到的位置,以及前一次迭代修正过程采用的历史运动状态生成。例如,本实施例中A1轮迭代修正过程是通过预测观测位置对第一观测位置或第二观测位置进行修正得到修正后的距离S,而下一次A2迭代修正过程采用的历史运动状态,则是利用A1轮修正后的距离S对A1轮迭代采用的历史运动状态中的距离进行替换,以得到下一次A2轮迭代修正过程需要采用的历史运动状态,实现了历史运动状态的迭代修正,提高了每一轮迭代修正的准确性。
在本申请实施例的一种可能的实现方式中,历史运动状态包括对象的历史位置和历史速度。其中,历史位置包含对象的相对距离和相对角度,也就是说距离是通过对象与采集的机器的相对距离和相对角度确定的。历史速度包含对象的相对线速度和角速度。
对应地,针对每一次迭代修正过程,本次迭代修正过程采用的历史运动状态中的历史位置,是根据前一次迭代修正过程得到的位置,以及前一次迭代修正过程采用的历史位置生成。而针对每一次迭代修正过程,本次迭代修正过程采用的历史运动状态中的历史速度,是根据本次迭代修正过程采用的历史位置,以及前一次迭代修正过程采用的历史位置确定的,通过在每一次迭代修正过程中,对历史运动状态中的历史位置和历史速度的修正,实现了每次迭代修正过程均是采用修正后的历史运动状态,提高了每次迭代修正过程得到的位置的准确性。
需要说明的是,在每一次迭代过程中修正对象的速度,可以对对象的行为进行预判,例如,判断对象相对采集机器是远离还是靠近等,以实现对对象进行跟踪。
本实施例的定位方法中,通过执行迭代修正实现了根据预测观测位置,对进行本次迭代修正过程的第一观测位置或第二观测位置进行修正,以得到修正后的位置,当对第一观测位置和第二观测位置都完成修正后,则得到修正后的目标位置,使得确定的目标位置融合了第一观测位置和第二观测位置的信息,提高了目标位置确定的准确性。
实际应用中,当预测观测位置与进行本次迭代修正过程的第一观测位置或第二观测位置之间的差异较大时,则说明预测观测位置的可信度较低,若采用该预测观测位置对获取到的第一观测位置或第二观测位置进行修正,会使得修正后得到的位置误差更大。从而,为了提高迭代修正的可靠性,本实施例中根据测量残差,判断预测观测位置与进行本次迭代修正过程的第一观测位置或第二观测位置之间的差异大小,以识别本次迭代过程,是否需要对第一观测位置或第二观测位置进行修正。因此,如图7所示,上述步骤503,可以包含以下步骤:
步骤701,对进行本次迭代修正过程的第一观测位置或第二观测位置,确定相应的测量残差。
步骤702,若所述测量残差小于差异阈值,则根据所述预测观测位置,对进行本次迭代修正过程的所述第一观测位置或所述第二观测位置进行修正。
其中,测量残差是预测观测位置与进行本次迭代修正过程的第一观测位置或第二观测位置之间的差异。
本实施例中,由于激光雷达传感器或深度传感器获取到检测数据的时间不同步,因此,每次进行迭代修正时可能利用第一观测位置进行修正,也可能利用第二观测位置进行修正,从而,将得到的预测观测位置,与第一观测位置或第二观测位置,确定相应的测量残差。
具体地,在一种场景下,进行本次迭代修正过程的为第一观测位置,则根据第一观测位置与预测观测位置确定测量残差,并判断测量残差是否小于差异阈值,若测量残差小于差异阈值,则得到的预测观测位置可信度较高,可用于后续对第一观测位置进行修正,以确定修正后得到的位置;若测量残差大于差异阈值,则得到的预测观测位置可信度较小,即视为噪声数据,不用于对第一观测位置进行修正,以提高修正得到的位置的准确度。
在另一种场景下,进行本次迭代修正过程的为第二观测位置,则根据第二观测位置与预测观测位置确定测量残差,并判断测量残差是否小于差异阈值,若测量残差小于差异阈值,则得到的预测观测位置可信度较高,可用于后续对第二观测位置进行修正,以确定修正后得到的位置;若测量残差大于差异阈值,则得到的预测观测位置可信度较小,即视为噪声数据,不用于对第二观测位置进行修正,以提高修正得到的位置的准确度。
本实施例的定位方法中,对进行本次迭代修正过程的第一观测位置或第二观测位置,确定相应的测量残差,在测量残差小于差异阈值时,根据预测观测位置,对进行本次迭代修正过程的第一观测位置或第二观测位置进行修正,由于测量残差指示了得到的预测观测位置可信度的高低,从而提高了每次迭代修正过程中进行第一观测位置或第二观测位置修正的可靠度和准确性。
上述实施例中描述了根据对象的历史运动状态对第一观测位置和第二观测位置中的一个,执行迭代修正过程,本实施例中提供了一种确定第一观测位置和第二观测位置执行迭代修正的顺序的方法,如图8所示,上述步骤403之前,可以包含以下步骤:
步骤801,比较获取到点云图的第一观测时刻,以及获取到深度图的第二观测时刻。
本实施例中,激光雷达在测量得到点云图时,和深度传感器在测量得到深度图时,采集时间是不同步的,为了便于区分,将获取到点云图的时刻称为第一观测时刻,将获取到深度图的时刻称为第二观测时刻,也就是说获取到点云图的第一观测时刻和获取到深度图的第二观测时刻并不相同,因此,需要比较第一观测时刻和第二观测时刻,以确定第一观测时刻和第二观测时刻的先后。
步骤802,若第一观测时刻先于第二观测时刻,则确定第一观测位置先于第二观测位置进行迭代修正。
步骤803,若第一观测时刻晚于第二观测时刻,则确定第二观测位置先于第一观测位置进行迭代修正。
步骤804,若第一观测时刻等于第二观测时刻,则随机确定第一观测位置和第二观测位置进行迭代修正的顺序。
本实施例的定位方法中,若比较确定第一观测时刻先于第二观测时刻,则确定第一观测位置先于第二观测位置进行迭代修正,也就是说先根据第一观测时刻的第一观测位置进行迭代修正,再根据第二观测时刻的第二观测位置进行迭代修正;若比较确定第一观测时刻晚于第二观测时刻,则确定第二观测位置先于第一观测位置进行迭代修正;若第一观测时刻等于第二观测时刻,则随机确定第一观测位置和第二观测位置进行迭代修正的顺序,也就是说随机确定第一观测位置和第二观测位置中的一个先进行迭代修正,再对第一观测位置和第二观测位置中的另一个进行迭代修正实现了对第一观测位置和第二观测位置交替进行迭代修正,从而实现了对激光雷达测量数据和深度传感器测量数据的有效融合,提高了准确性。
基于上述实施例,本实施例提供了一种实现方式,说明了如何根据激光雷达测量到的点云图中确定出的第一目标区域,确定对象的第一观测位置。如图9所示,上述步骤401可以包含以下步骤:
步骤901,根据第一目标区域中各像素点携带的第一定位信息,确定第一目标区域中各像素点的深度。
步骤902,根据各像素点的深度,统计各设定深度对应的像素点个数指示值。
本申请的一个实施例中,统计各像素点的深度与各设定深度匹配的像素点,得到各设定深度对应的像素点个数指示值。具体地,对每一个预先确定的设定深度,确定深度统计范围,统计各像素点的深度与相应深度统计范围匹配的像素点个数,以得到相应设定深度对应的像素点个数指示值,通过将每一个设定深度,进行范围扩展得到深度统计范围,统计相应深度统计范围内的像素点个数,作为相应设定深度对应的像素点个数,实现了增加相应设定深度对应的像素点的个数,兼顾了准确性和精确性。
本实施例中,可用直方图来展示每一个设定深度对应的像素点个数。图10为本实施例提供的一种直方图示意图,图10中示出了各设定深度对应的像素点的个数。
步骤903,筛选掉像素点个数指示值小于个数阈值的设定深度。
具体地,深度传感器测量得到的深度图,例如RGBD相机测量到的深度图一般都随机分布许多噪点,容易在目标位置确定的过程中,形成野值,降低控制系统的稳定性,因此,根据统计确定的相应设定深度对应的像素点个数指示值,筛选掉像素点个数指示值小于个数阈值的设定深度,实现了将属于野值的设定深度去除,不用于进行目标深度的确定,提高了目标深度确定的准确性,同时也提高了控制系统的稳定性。
步骤904,根据像素点个数指示值的峰值,从各设定深度中确定目标深度。
具体地,从各设定深度中,确定前景深度和背景深度,其中,背景深度具有像素点个数指示值的最大峰值,前景深度具有按照深度从小到大顺序的像素点个数指示值的首个峰值,根据前景深度和背景深度对应的像素点个数指示值,从前景深度和背景深度中选定目标深度。
作为本申请实施例的一种可能的实现方式,若前景深度和背景深度的像素点个数指示值之比大于比例阈值,则将前景深度作为目标深度;若前景深度和背景深度的像素点个数指示值之比不大于比例阈值,则将背景深度作为目标深度。
在一种场景下,图11为本申请提供的另一种直方图示意图。如图11所示,按照深度从小到大顺序,像素点个数指示值的最大峰值,即为c指示的峰值,因此,c指示的峰值对应的深度即为前景深度,而像素点个数指示值的最大峰值为b指示的峰值,因此,b指示的峰值对应的深度即为背景深度。确定c指示的前景深度对应的像素点个数指示值与d指示的背景深度对应的像素点个数指示值之比,与比例阈值比较,确定比值小于比例阈值,则认为前景是遮挡物,背景才是目标,例如目标为人脸或人体,则将d指示的背景深度作为目标深度。
在另一种场景下,如图10所示,a指示的峰值即为按照深度从小到大顺序,像素点个数指示值的最大峰值,即为前景深度。b指示的峰值为像素点个数指示值的最大峰值,即为背景深度。确定a指示的前景深度对应的像素点个数指示值与b指示的背景深度对应的像素点个数指示值之比大于比例阈值,则认为背景是遮挡物,前景是目标,则将a指示的前景深度作为目标深度。
步骤905,根据目标深度,定位对象所处的第一观测位置。
其中,第一观测位置包括观测距离和观测角度。
具体地,根据目标深度对应的各像素点携带的角度,定位对象的观测角度,作为一种可能的实现方式,可将目标深度对应的各像素点携带的角度,进行加权平均,计算得到对象的观测角度;根据目标深度对应的各像素点携带的深度,定位对象的观测距离。作为一种可能的实现方式,可将目标深度对应的各像素点携带的深度,进行加权平均,计算得到对象的深度信息,根据对象的深度信息定位对象的观测距离。进而,根据定位得到的对象的观测距离和观测角度,定位对象所处的第一观测位置。
步骤906,根据目标深度对应的像素点个数指示值,确定第一观测位置的测量置信度。
其中,目标深度对应的像素点个数指示值与第一观测位置的测量置信度成正比,也就是说目标深度对应的像素点个数指示值越大,第一观测位置的测量置信度则越高。
实际场景下,目标深度可以为背景深度也可以为前景深度,因此,在确定第一观测位置的置信度时,也根据对应的背景深度或前景深度进行确定。
在一种场景下,若目标深度为背景深度,则可以根据背景深度对应的像素点个数指示值,确定目标位置的测量置信度,作为一种可能的实现方式,利用背景深度的像素点个数指示值与前景深度的像素点个数指示值之比,确定第一观测位置的测量置信度,如,置信度为C1,C1=(back_ratio/doubless_ratio)*100,其中,back_ratio为背景深度的像素点个数指示值与前景深度的像素点个数指示值之比,doubless_ratio为置信度的满分阈值。
在另一种场景下,若目标深度为前景深度,则可以根据前景深度对应的像素点个数指示值,确定目标位置的测量置信度,作为一种可能的实现方式,利用前景深度的像素点个数指示值与背景深度的像素点个数指示值之比,确定第一观测位置的测量置信度,如,置信度为C2,C2=(fore_ratio/doubless_ratio)*100,其中,fore_ratio前景深度的像素点个数指示值与背景深度的像素点个数指示值之比,doubless_ratio为置信度的满分阈值。
步骤907,若前景深度和背景深度相同,提高第一观测位置的测量置信度。
其中,测量置信度,用于为图5实施例中说明的迭代修正过程中采用的观测方程,生成量测噪声项,观测方程用于在根据迭代修正过程采用的历史运动状态,得到预测运动状态之后,将预测运动状态代入观测方程,以得到预测观测位置。
具体地,若前景深度和背景深度相同,即前景和背景深度是重合的,也就是说当前确定的第一观测位置的准确度较高,则需要提高第一观测位置的测量置信度,作为一种可能的实现方式,可将利用前景深度确定的第一观测位置的测量置信度增加为2倍,即为2C2,或者是将利用前景深度确定的第一观测位置的测量置信度增加为2倍,即为2C1。
需要说明的是,定位对象所处的第二观测位置的方法,以及第二观测位置的测量置信度的确定方法和第一观测位置的测量置信度的确定方法,原理相同,此处不再赘述。
本申请的定位方法中,针对激光雷达采集的点云图中确定的第一目标区域中各像素点携带的第一定位信息,统计第一定位信息与各设定深度匹配的像素点,得到各设定深度对应的像素点个数指示值,根据各设定深度对应的像素点个数指示值,筛选掉像素点个数指示值小于个数阈值的设定深度,以去除深度图中引入的噪声深度信息,避免对系统的影响。并根据像素点个数指示值的峰值,从各设定深度中确定目标深度,以定位对象所处的第一观测位置,和确定第一观测位置的测量置信度,实现了对激光雷达采集数据的处理,以提高后续基于多个传感器的数据融合确定的目标位置的准确性。
基于上述实施例,在本申请的一个实施例中,根据第二目标区域携带的第二定位信息,确定对象所处的第二观测位置,具体的实现方法,可参照上一实施例中根据第一目标区域携带的第一定位信息,确定对象所处的第一观测位置的解释说明,实现原理相同,此处不再赘述。
为了实现上述实施例,本申请还提出一种定位装置。
图12为本申请实施例提供的一种定位装置的结构示意图。
如图12所示,该装置包括:获取模块81、提取模块82和融合模块83。
获取模块81,用于获取激光雷达测量得到的点云图。
上述获取模块81,还用于获取深度传感器测量得到的深度图。
提取模块82,用于从点云图中提取第一目标区域,以及从深度图中提取第二目标区域,其中,第一目标区域和第二目标区域与同步采集的视觉图像中的感兴趣区域检测相同对象。
融合模块83,用于根据对象的历史运动状态,对第一目标区域携带的第一定位信息,以及第二目标区域携带的第二定位信息进行融合,得到对象的目标位置。
进一步地,在本申请实施例的一种可能的实现方式中,上述融合模块83,具体用于:
根据第一目标区域携带的第一定位信息,确定对象所处的第一观测位置,根据第二目标区域携带的第二定位信息,确定对象所处的第二观测位置;根据对象的历史运动状态,对第一观测位置和第二观测位置中的一个,执行迭代修正过程,根据迭代修正过程得到的位置更新历史运动状态,根据更新后的历史运动状态,对第一观测位置和第二观测位置中的另一个,执行迭代修正过程,以得到目标位置。
作为一种可能的实现方式,上述融合模块83,具体用于:
根据本次迭代修正过程采用的历史运动状态,得到预测运动状态;本次迭代修正过程采用的历史运动状态,是根据前一次迭代修正过程得到的位置,以及前一次迭代修正过程采用的历史运动状态生成;根据预测运动状态,得到预测观测位置;根据预测观测位置,对进行本次迭代修正过程的第一观测位置或第二观测位置进行修正。
作为一种可能的实现方式,历史运动状态包括对象的历史位置和历史速度;
对应地,针对每一次迭代修正过程,本次迭代修正过程采用的历史运动状态中的历史位置,是根据前一次迭代修正过程得到的位置,以及前一次迭代修正过程采用的历史位置生成;针对每一次迭代修正过程,本次迭代修正过程采用的历史运动状态中的历史速度,是根据本次迭代修正过程采用的历史位置,以及前一次迭代修正过程采用的历史位置确定的。
作为一种可能的实现方式,上述融合模块83,还用于:
比较获取到点云图的第一观测时刻,以及获取到深度图的第二观测时刻;若第一观测时刻先于第二观测时刻,则确定第一观测位置先于第二观测位置进行迭代修正;若第一观测时刻晚于第二观测时刻,则确定第二观测位置先于第一观测位置进行迭代修正;若第一观测时刻等于第二观测时刻,则随机确定第一观测位置和第二观测位置进行迭代修正的顺序。
作为一种可能的实现方式,上述融合模块83,具体用于:
将预测运动状态代入观测方程,得到预测观测位置;
其中,观测方程为预测运动状态与转换矩阵的乘积叠加量测噪声项;转换矩阵,用于指示预测运动状态与预测观测位置之间的转换关系;量测噪声项,符合设定协方差的高斯白噪声分布;设定协方差是根据设备精度和测量置信度确定的。
作为一种可能的实现方式,上述融合模块83,具体用于:
对进行本次迭代修正过程的第一观测位置或第二观测位置,确定测量残差;若测量残差小于差异阈值,则根据预测观测位置,对进行本次迭代修正过程的第一观测位置或第二观测位置进行修正。
作为一种可能的实现方式,测量残差是预测观测位置与进行本次迭代修正过程的第一观测位置或第二观测位置之间的差异。
进一步地,在本申请实施例的一种可能的实现方式中,上述提取模块82,具体用于:
对感兴趣区域,确定图像坐标系中的直角坐标位置;将图像坐标系中的直角坐标位置,映射至激光雷达的极坐标系,得到极坐标位置;根据极坐标位置,从点云图中提取第一目标区域。
作为一种可能的实现方式,上述提取模块82,具体用于:
对感兴趣区域的左右边界,确定直角坐标位置。
作为一种可能的实现方式,上述提取模块82,具体用于:
将图像坐标系中的直角坐标位置,通过相机的内参矩阵映射至相机坐标系,得到相机坐标系的直角坐标位置,其中,相机用于采集视觉图像,将相机坐标系的直角坐标位置,通过相机与激光雷达的外参矩阵映射至极坐标系,得到极坐标位置。
进一步地,在本申请实施例的一种可能的实现方式中,上述融合模块83,具体用于:
根据第一目标区域携带的第一定位信息,确定第一目标区域中各像素点的深度,根据各像素点的深度,统计各设定深度对应的像素点个数指示值;根据像素点个数指示值的峰值,从各设定深度中确定目标深度;根据目标深度,定位对象所处的第一观测位置。
作为一种可能的实现方式,上述融合模块83,具体用于:
从各设定深度中,确定前景深度和背景深度;其中,背景深度具有像素点个数指示值的最大峰值;前景深度具有按照深度从小到大顺序的像素点个数指示值的首个峰值,根据前景深度和背景深度对应的像素点个数指示值,从前景深度和背景深度中选定目标深度。
作为一种可能的实现方式,上述融合模块83,具体用于:
若前景深度和背景深度的像素点个数指示值之比大于比例阈值,则将前景深度作为目标深度;若前景深度和背景深度的像素点个数指示值之比不大于比例阈值,则将背景深度作为目标深度。
作为一种可能的实现方式,上述融合模块83,还用于:
根据目标深度对应的像素点个数指示值,确定第一观测位置的测量置信度;若前景深度和背景深度相同,提高第一观测位置的测量置信度,其中,测量置信度,用于为迭代修正过程中采用的观测方程,生成量测噪声项,观测方程用于在根据迭代修正过程采用的历史运动状态,得到预测运动状态之后,将预测运动状态代入观测方程,以得到预测观测位置。
作为一种可能的实现方式,上述融合模块83,还用于:
筛选掉像素点个数指示值小于个数阈值的设定深度。
作为一种可能的实现方式,上述融合模块83,具体用于:
对每一个设定深度,确定深度统计范围,根据各像素点的深度,统计深度与相应深度统计范围匹配的像素点个数,以得到相应设定深度对应的像素点个数指示值。
作为一种可能的实现方式,第一观测位置包括观测距离和观测角度,第一定位信息包括深度和角度;
上述融合模块83,具体用于根据目标深度对应的各像素点携带的角度,定位对象的观测角度,根据目标深度对应的各像素点携带的深度,定位对象的观测距离。
需要说明的是,前述对定位方法实施例的解释说明也适用于该实施例的定位装置,此处不再赘述。
为了实现上述实施例,本申请还提出一种电子设备,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,所述处理器执行所述程序时,实现如前述方法实施例所述的定位方法。
本申请提出的电子设备可以但不限于为机器人。
为了实现上述实施例,本申请还提出一种非临时性计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时,实现如前述方法实施例所述的定位方法。
在本说明书的描述中,参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施例或示例中。在本说明书中,对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且,描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外,在不相互矛盾的情况下,本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
此外,术语“第一”、“第二”仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中,“多个”的含义是至少两个,例如两个,三个等,除非另有明确具体的限定。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为,表示包括一个或更多个用于实现定制逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分,并且本申请的优选实施方式的范围包括另外的实现,其中可以不按所示出或讨论的顺序,包括根据所涉及的功能按基本同时的方式或按相反的顺序,来执行功能,这应被本申请的实施例所属技术领域的技术人员所理解。
在流程图中表示或在此以其他方式描述的逻辑和/或步骤,例如,可以被认为是用于实现逻辑功能的可执行指令的定序列表,可以具体实现在任何计算机可读介质中,以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用,或结合这些指令执行系统、装置或设备而使用。就本说明书而言,"计算机可读介质"可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下:具有一个或多个布线的电连接部(电子装置),便携式计算机盘盒(磁装置),随机存取存储器(RAM),只读存储器(ROM),可擦除可编辑只读存储器(EPROM或闪速存储器),光纤装置,以及便携式光盘只读存储器(CDROM)。另外,计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质,因为可以例如通过对纸或其他介质进行光学扫描,接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序,然后将其存储在计算机存储器中。
应当理解,本申请的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中,多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。如,如果用硬件来实现和在另一实施方式中一样,可用本领域公知的下列技术中的任一项或他们的组合来实现:具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路,具有合适的组合逻辑门电路的专用集成电路,可编程门阵列(PGA),现场可编程门阵列(FPGA)等。
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成,所述的程序可以存储于一种计算机可读存储介质中,该程序在执行时,包括方法实施例的步骤之一或其组合。
此外,在本申请各个实施例中的各功能单元可以集成在一个处理模块中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现,也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时,也可以存储在一个计算机可读取存储介质中。
上述提到的存储介质可以是只读存储器,磁盘或光盘等。尽管上面已经示出和描述了本申请的实施例,可以理解的是,上述实施例是示例性的,不能理解为对本申请的限制,本领域的普通技术人员在本申请的范围内可以对上述实施例进行变化、修改、替换和变型。
Claims (10)
1.一种定位方法,其特征在于,所述方法包括:
获取激光雷达测量得到的点云图;
获取深度传感器测量得到的深度图;
从所述点云图中提取第一目标区域,以及从所述深度图中提取第二目标区域;其中,所述第一目标区域和所述第二目标区域与同步采集的视觉图像中的感兴趣区域检测相同对象;
根据所述对象的历史运动状态,对所述第一目标区域携带的第一定位信息,以及所述第二目标区域携带的第二定位信息进行融合,得到所述对象的目标位置。
2.根据权利要求1所述的定位方法,其特征在于,所述根据所述对象的历史运动状态,对所述第一目标区域携带的第一定位信息,以及所述第二目标区域携带的第二定位信息进行融合,得到所述对象的目标位置,包括:
根据所述第一目标区域携带的第一定位信息,确定所述对象所处的第一观测位置;
根据所述第二目标区域携带的第二定位信息,确定所述对象所处的第二观测位置;
根据所述对象的历史运动状态,对所述第一观测位置和所述第二观测位置中的一个,执行迭代修正过程;
根据所述迭代修正过程得到的位置更新所述历史运动状态;
根据更新后的历史运动状态,对所述第一观测位置和所述第二观测位置中的另一个,执行迭代修正过程,以得到所述目标位置。
3.根据权利要求2所述的定位方法,其特征在于,所述执行迭代修正过程,包括:
根据本次迭代修正过程采用的历史运动状态,得到预测运动状态;所述本次迭代修正过程采用的历史运动状态,是根据前一次迭代修正过程得到的位置,以及前一次迭代修正过程采用的历史运动状态生成;
根据所述预测运动状态,得到预测观测位置;
根据所述预测观测位置,对进行本次迭代修正过程的所述第一观测位置或所述第二观测位置进行修正。
4.根据权利要求2所述的定位方法,其特征在于,所述根据所述对象的历史运动状态,对所述第一观测位置和所述第二观测位置中的一个,执行迭代修正过程之前,还包括:
比较获取到所述点云图的第一观测时刻,以及获取到所述深度图的第二观测时刻;
若所述第一观测时刻先于所述第二观测时刻,则确定所述第一观测位置先于所述第二观测位置进行迭代修正;
若所述第一观测时刻晚于所述第二观测时刻,则确定所述第二观测位置先于所述第一观测位置进行迭代修正;
若所述第一观测时刻等于所述第二观测时刻,则随机确定所述第一观测位置和所述第二观测位置进行迭代修正的顺序。
5.根据权利要求3所述的定位方法,其特征在于,所述历史运动状态包括所述对象的历史位置和历史速度;
对应地,针对每一次迭代修正过程,本次迭代修正过程采用的历史运动状态中的历史位置,是根据前一次迭代修正过程得到的位置,以及前一次迭代修正过程采用的历史位置生成;
针对每一次迭代修正过程,本次迭代修正过程采用的历史运动状态中的历史速度,是根据本次迭代修正过程采用的历史位置,以及前一次迭代修正过程采用的历史位置确定的。
6.根据权利要求3所述的定位方法,其特征在于,所述根据所述预测运动状态,得到预测观测位置,包括:
将所述预测运动状态代入观测方程,得到所述预测观测位置;
其中,所述观测方程为所述预测运动状态与转换矩阵的乘积叠加量测噪声项;
所述转换矩阵,用于指示所述预测运动状态与所述预测观测位置之间的转换关系;
所述量测噪声项,符合设定协方差的高斯白噪声分布;所述设定协方差是根据设备精度和测量置信度确定的。
7.根据权利要求3所述的定位方法,其特征在于,所述根据所述预测观测位置,对进行本次迭代修正过程的所述第一观测位置或所述第二观测位置进行修正,包括:
对进行本次迭代修正过程的所述第一观测位置或所述第二观测位置,确定测量残差;
若所述测量残差小于差异阈值,则根据所述预测观测位置,对进行本次迭代修正过程的所述第一观测位置或所述第二观测位置进行修正。
8.一种定位装置,其特征在于,所述装置包括:
获取模块,用于获取激光雷达测量得到的点云图;
所述获取模块,还用于获取深度传感器测量得到的深度图;
提取模块,用于从所述点云图中提取第一目标区域,以及从所述深度图中提取第二目标区域;其中,所述第一目标区域和所述第二目标区域与同步采集的视觉图像中的感兴趣区域检测相同对象;
融合模块,用于根据所述对象的历史运动状态,对所述第一目标区域携带的第一定位信息,以及所述第二目标区域携带的第二定位信息进行融合,得到所述对象的目标位置。
9.一种电子设备,其特征在于,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,所述处理器执行所述程序时,实现如权利要求1-7中任一所述的定位方法。
10.一种非临时性计算机可读存储介质,其上存储有计算机程序,其特征在于,所述程序被处理器执行时实现如权利要求1-7中任一所述的定位方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010651193.2A CN113917475A (zh) | 2020-07-08 | 2020-07-08 | 定位方法、装置、电子设备和计算机可读存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010651193.2A CN113917475A (zh) | 2020-07-08 | 2020-07-08 | 定位方法、装置、电子设备和计算机可读存储介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN113917475A true CN113917475A (zh) | 2022-01-11 |
Family
ID=79231710
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010651193.2A Pending CN113917475A (zh) | 2020-07-08 | 2020-07-08 | 定位方法、装置、电子设备和计算机可读存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113917475A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113916213A (zh) * | 2020-07-08 | 2022-01-11 | 北京猎户星空科技有限公司 | 定位方法、装置、电子设备和计算机可读存储介质 |
CN115239769A (zh) * | 2022-06-02 | 2022-10-25 | 中国国家博物馆 | 对象的分布位置确定方法、装置、电子设备和介质 |
-
2020
- 2020-07-08 CN CN202010651193.2A patent/CN113917475A/zh active Pending
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113916213A (zh) * | 2020-07-08 | 2022-01-11 | 北京猎户星空科技有限公司 | 定位方法、装置、电子设备和计算机可读存储介质 |
CN113916213B (zh) * | 2020-07-08 | 2024-07-23 | 北京猎户星空科技有限公司 | 定位方法、装置、电子设备和计算机可读存储介质 |
CN115239769A (zh) * | 2022-06-02 | 2022-10-25 | 中国国家博物馆 | 对象的分布位置确定方法、装置、电子设备和介质 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110555901B (zh) | 动静态场景的定位和建图方法、装置、设备和存储介质 | |
US11830216B2 (en) | Information processing apparatus, information processing method, and storage medium | |
CN108955718B (zh) | 一种视觉里程计及其定位方法、机器人以及存储介质 | |
CN109949347B (zh) | 人体跟踪方法、装置、系统、电子设备和存储介质 | |
EP2538242A1 (en) | Depth measurement quality enhancement. | |
KR102054455B1 (ko) | 이종 센서 간의 캘리브레이션 장치 및 방법 | |
CN110084133B (zh) | 障碍物检测方法、装置、车辆、计算机设备和存储介质 | |
US7764284B2 (en) | Method and system for detecting and evaluating 3D changes from images and a 3D reference model | |
EP3236424B1 (en) | Information processing apparatus and method of controlling the same | |
EP3593322B1 (en) | Method of detecting moving objects from a temporal sequence of images | |
JP6860620B2 (ja) | 情報処理装置、情報処理方法、及びプログラム | |
US9816786B2 (en) | Method for automatically generating a three-dimensional reference model as terrain information for an imaging device | |
CN113917475A (zh) | 定位方法、装置、电子设备和计算机可读存储介质 | |
CN111369495A (zh) | 一种基于视频的全景图像的变化检测方法 | |
CN111144213A (zh) | 一种对象检测方法和相关设备 | |
CN110956603A (zh) | 深度图像边缘飞点的检测方法、装置及电子设备 | |
CN114035187B (zh) | 一种自动驾驶系统的感知融合方法 | |
CN110738703A (zh) | 定位方法及装置、终端、存储介质 | |
CN116503566B (zh) | 一种三维建模方法、装置、电子设备及存储介质 | |
CN115830142A (zh) | 相机标定和相机的目标检测与定位方法、装置及电子设备 | |
CN115376109A (zh) | 障碍物检测方法、障碍物检测装置以及存储介质 | |
Schramm et al. | Data fusion for 3D thermal imaging using depth and stereo camera for robust self-localization | |
CN115100616A (zh) | 点云目标检测方法、装置、电子设备及存储介质 | |
KR102547333B1 (ko) | 깊이 영상 기반 실시간 바닥 검출방법 | |
CN113916213B (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 |