CN113916213A - 定位方法、装置、电子设备和计算机可读存储介质 - Google Patents

定位方法、装置、电子设备和计算机可读存储介质 Download PDF

Info

Publication number
CN113916213A
CN113916213A CN202010650773.XA CN202010650773A CN113916213A CN 113916213 A CN113916213 A CN 113916213A CN 202010650773 A CN202010650773 A CN 202010650773A CN 113916213 A CN113916213 A CN 113916213A
Authority
CN
China
Prior art keywords
depth
observation
target
positioning
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
Application number
CN202010650773.XA
Other languages
English (en)
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.)
Beijing Orion Star Technology Co Ltd
Original Assignee
Beijing Orion Star Technology Co Ltd
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 Beijing Orion Star Technology Co Ltd filed Critical Beijing Orion Star Technology Co Ltd
Priority to CN202010650773.XA priority Critical patent/CN113916213A/zh
Publication of CN113916213A publication Critical patent/CN113916213A/zh
Pending legal-status Critical Current

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
    • 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
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C3/00—Measuring distances in line of sight; Optical rangefinders
    • 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
    • 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/46—Indirect determination of position data
    • 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/50—Depth or shape recovery
    • G06T7/521—Depth or shape recovery from laser ranging, e.g. using interferometry; from the projection of structured light
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Automation & Control Theory (AREA)
  • Multimedia (AREA)
  • Optics & Photonics (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本申请提出一种定位方法、装置、电子设备和计算机可读存储介质，涉及计算机视觉技术领域，其中，方法包括：获取激光雷达测量得到的点云图，从点云图中提取第一目标区，第一目标区与同步采集的视觉图像中的感兴趣区检测相同对象，根据第一目标区域携带的第一定位信息，确定第一目标区域中各像素点的深度,根据各像素点的深度，统计各设定深度对应的像素点个数指示值，根据像素点个数指示值的峰值，从各设定深度中确定目标深度，实现了通过激光雷达与视觉图像配准，进行对象位置预测，降低了成本，提高了定位准确度，解决现有技术中采用深度传感器进行位置预测，成本高，准确度低的技术问题。

Description

定位方法、装置、电子设备和计算机可读存储介质
技术领域
本申请涉及计算机视觉技术领域，尤其涉及一种定位方法、装置、电子设备和计算机可读存储介质。
背景技术
机器人与人进行交互过程中，很重要的一个应用是对人进行跟踪，以跟随人运动到指定的位置，因此，需要实时检测人体，并估计出人体和机器人之间的相对位置关系。
而现有技术中，通常是基于相机和深度传感器的数据，计算人体与机器人之间的距离，然而，深度传感器价格较高，图像采集范围较小，确定当前人体和机器之间的相对位置关系成本高，准确度低。
发明内容
本申请旨在至少在一定程度上解决相关技术中的技术问题之一。
为此，本申请的第一个目的在于提出一种定位方法，通过同步采集的视觉图像中的感兴趣区域，确定激光雷达采集的点云图中的第一目标区域，根据第一目标区域定位对象的观测位置，实现了通过激光雷达与视觉图像配准，进行对象位置预测，降低了成本，提高了定位准确度。
本申请的第二个目的在于提出一种定位装置。
本申请的第三个目的在于提出一种电子设备。
本申请的第四个目的在于提出一种非临时性计算机可读存储介质。
为达上述目的，本申请第一方面实施例提出了一种定位方法，包括：
获取激光雷达测量得到的点云图；
从所述点云图中提取第一目标区，所述第一目标区与同步采集的视觉图像中的感兴趣区检测相同对象；
根据所述第一目标区域携带的第一定位信息，确定所述第一目标区域中各像素点的深度；
根据所述各像素点的深度，统计各设定深度对应的像素点个数指示值；
根据所述像素点个数指示值的峰值，从各设定深度中确定目标深度；
根据所述目标深度，定位对象所处的第一观测位置。
可选地，作为第一方面的第一种可能的实现方式，所述根据像素点个数指示值的峰值，从各设定深度中确定目标深度，包括：
从各设定深度中，确定前景深度和背景深度；其中，所述背景深度具有像素点个数指示值的最大峰值；所述前景深度具有按照深度从小到大顺序的像素点个数指示值的首个峰值；
根据所述前景深度和所述背景深度对应的像素点个数指示值，从所述前景深度和所述背景深度中选定所述目标深度。
可选地，作为第一方面的第二种可能的实现方式，所述根据所述前景深度和所述背景深度对应的像素点个数指示值，从所述前景深度和所述背景深度中选定所述目标深度，包括：
若所述前景深度和所述背景深度的像素点个数指示值之比大于比例阈值，则将所述前景深度作为所述目标深度；
若所述前景深度和所述背景深度的像素点个数指示值之比不大于所述比例阈值，则将所述背景深度作为所述目标深度。
可选地，作为第一方面的第三种可能的实现方式，所述根据所述各像素点的深度，统计各设定深度对应的像素点个数指示值之后，还包括：
筛选掉所述像素点个数指示值小于个数阈值的设定深度。
可选地，作为第一方面的第四种可能的实现方式，所述根据所述各像素点的深度，统计各设定深度对应的像素点个数指示值，包括：
对每一个设定深度，确定深度统计范围；
根据各像素点的深度，统计所述深度与相应深度统计范围匹配的像素点个数，以得到相应设定深度对应的像素点个数指示值。
可选地，作为第一方面的第五种可能的实现方式，所述第一观测位置包括观测距离和观测角度；所述第一定位信息包括深度和角度；
所述根据所述目标深度，定位对象所处的第一观测位置，包括：
根据所述目标深度对应的各像素点携带的角度，定位所述对象的观测角度；
根据所述目标深度对应的各像素点携带的深度，定位所述对象的观测距离。
可选地，作为第一方面的第六种可能的实现方式，所述方法还包括：
获取第二观测位置；其中，所述第二观测位置，是根据深度传感器测量得到的深度图，对所述对象定位得到的；
根据所述对象的历史运动状态，对所述第一观测位置和所述第二观测位置中的一个，执行迭代修正过程；
根据所述迭代修正过程得到的位置更新所述历史运动状态；
根据更新后的历史运动状态，对所述第一观测位置和所述第二观测位置中的另一个，执行迭代修正过程，以得到所述目标位置。
可选地，作为第一方面的第七种可能的实现方式，所述获取第二观测位置，包括：
获取深度传感器测量得到的深度图；
从所述深度图中提取第二目标区；其中，所述第二目标区与同步采集的视觉图像中的感兴趣区检测相同对象；
根据所述第二目标区携带的第二定位信息，确定所述对象所处的第二观测位置。
可选地，作为第一方面的第八种可能的实现方式，所述执行迭代修正过程，包括：
根据本次迭代修正过程采用的历史运动状态，得到预测运动状态；所述本次迭代修正过程采用的历史运动状态，是根据前一次迭代修正过程得到的位置，以及前一次迭代修正过程采用的历史运动状态生成；
根据所述预测运动状态，得到预测观测位置；
根据所述预测观测位置，对进行本次迭代修正过程的所述第一观测位置或所述第二观测位置进行修正。
可选地，作为第一方面的第九种可能的实现方式，所述根据所述目标深度，定位对象所处的第一观测位置之后，还包括：
根据所述目标深度对应的像素点个数指示值，确定所述第一观测位置的测量置信度；
若所述前景深度和所述背景深度相同，提高所述第一观测位置的测量置信度，其中，所述测量置信度，用于为所述第一观测位置的迭代修正过程中采用的观测方程，生成量测噪声项，所述观测方程用于在根据所述迭代修正过程采用的历史运动状态，得到预测运动状态之后，将所述预测运动状态代入所述观测方程，以得到预测观测位置。
可选地，作为第一方面的第十种可能的实现方式，所述根据所述对象的历史运动状态，对所述第一观测位置和所述第二观测位置中的一个，执行迭代修正过程之前，还包括：
比较获取到所述点云图的第一观测时刻，以及获取到所述深度图的第二观测时刻；
若所述第一观测时刻先于所述第二观测时刻，则确定所述第一观测位置先于所述第二观测位置进行迭代修正；
若所述第一观测时刻晚于所述第二观测时刻，则确定所述第二观测位置先于所述第一观测位置进行迭代修正；
若所述第一观测时刻等于所述第二观测时刻，则随机确定所述第一观测位置和所述第二观测位置进行迭代修正的顺序。
可选地，作为第一方面的第十一种可能的实现方式，所述历史运动状态包括所述对象的历史位置和历史速度；
对应地，针对每一次迭代修正过程，本次迭代修正过程采用的历史运动状态中的历史位置，是根据前一次迭代修正过程得到的目标位置，以及前一次迭代修正过程采用的历史位置生成；
针对每一次迭代修正过程，本次迭代修正过程采用的历史运动状态中的历史速度，是根据本次迭代修正过程采用的历史位置，以及前一次迭代修正过程采用的历史位置确定的。
可选地，作为第一方面的第十二种可能的实现方式，所述根据所述预测运动状态，得到预测观测位置，包括：
将所述预测运动状态代入观测方程，得到所述预测观测位置；
其中，所述观测方程为所述预测运动状态与转换矩阵的乘积叠加量测噪声项；
所述转换矩阵，用于指示所述预测运动状态与所述预测观测位置之间的转换关系；
所述量测噪声项，符合设定协方差的高斯白噪声分布；所述设定协方差是根据设备精度和测量置信度确定的。
可选地，作为第一方面的第十三种可能的实现方式，所述根据所述预测观测位置，对进行本次迭代修正过程的所述第一观测位置或所述第二观测位置进行修正，包括：
对进行本次迭代修正过程的所述第一观测位置或所述第二观测位置，确定测量残差；
若所述测量残差小于差异阈值，则根据所述预测观测位置，对进行本次迭代修正过程的所述第一观测位置或所述第二观测位置进行修正。
可选地，作为第一方面的第十四种可能的实现方式，所述测量残差是所述预测观测位置与进行本次迭代修正过程的所述第一观测位置或所述第二观测位置之间的差异。
可选地，作为第一方面的第十五种可能的实现方式，所述从所述点云图中提取第一目标区，包括：
对所述感兴趣区域，确定图像坐标系中的直角坐标位置；
将所述图像坐标系中的直角坐标位置，映射至所述激光雷达的极坐标系，得到极坐标位置；
根据所述极坐标位置，从所述点云图中提取所述第一目标区域。
可选地，作为第一方面的第十六种可能的实现方式，所述对所述感兴趣区域，确定图像坐标系中的直角坐标位置，包括：
对所述感兴趣区域的左右边界，确定所述直角坐标位置。
可选地，作为第一方面的第十七种可能的实现方式，所述将所述图像坐标系中的直角坐标位置，映射至所述激光雷达的极坐标系，得到极坐标位置，包括：
将所述图像坐标系中的直角坐标位置，通过相机的内参矩阵映射至相机坐标系，得到所述相机坐标系的直角坐标位置；其中，所述相机用于采集所述视觉图像；
将所述相机坐标系的直角坐标位置，通过所述相机与所述激光雷达的外参矩阵映射至所述极坐标系，得到所述极坐标位置。
为达上述目的，本申请第二方面实施例提出了一种定位装置，包括：
第一获取模块，用于获取激光雷达测量得到的点云图；
提取模块，用于从所述点云图中提取第一目标区，所述第一目标区与同步采集的视觉图像中的感兴趣区检测相同对象；
第一确定模块，用于根据所述第一目标区域携带的第一定位信息，确定所述第一目标区域中各像素点的深度；
统计模块，用于根据所述各像素点的深度，统计各设定深度对应的像素点个数指示值；
第二确定模块，用于根据所述像素点个数指示值的峰值，从各设定深度中确定目标深度；
定位模块，用于根据所述目标深度，定位对象所处的第一观测位置。
为达上述目的，本申请第三方面实施例提出了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时，实现如第一方面所述的定位方法。
为了实现上述目的，本申请第四方面实施例提出了一种非临时性计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时，实现如第一方面所述的定位方法。
本申请实施例所提供的技术方案可以包含如下的有益效果：
本申请中通过激光雷达与视觉图像配准，进行对象位置预测，降低了成本，提高了定位准确度。
本申请附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本申请的实践了解到。
附图说明
本申请上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：
图1为本申请实施例所提供的定位方法的流程示意图之一；
图2为本申请实施例提供的定位方法的流程示意图之二；
图3为本申请提供的坐标系转换的示意图；
图4为本申请实施例提供的定位方法的流程示意图之三；
图5为本实施例提供的一种直方图示意图；
图6为本申请提供的另一种直方图示意图；
图7为本申请实施例提供的定位方法的流程示意图之四；
图8为本申请实施例提供的一种迭代修正过程的流程示意图；
图9为各状态变量在极坐标系中的定义示意图；
图10为本申请实施例提供的另一种迭代修正过程的流程示意图；
图11为本申请实施例提供的定位方法的流程示意图之五；
图12为本申请实施例提供的定位方法的流程示意图之六；以及
图13为本申请实施例提供的一种定位装置的结构示意图。
具体实施方式
下面详细描述本申请的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本申请，而不能理解为对本申请的限制。
下面参考附图描述本申请实施例的定位方法、装置、电子设备和计算机可读存储介质。
图1为本申请实施例所提供的定位方法的流程示意图之一。
如图1所示，该方法包括以下步骤：
步骤101，获取激光雷达测量得到的点云图。
其中，点云图携带有第一定位信息。第一定位信息包含激光雷达测量到的各对象的角度信息和深度信息，深度信息指示了对象与激光雷达的相对距离信息。
其中，激光雷达可以为单线激光雷达、多线激光雷达等。在机器人与人交互的场景中，激光雷达可以为单线激光雷达，单线激光雷达具有较大的感知范围，同时，相比深度传感器采集的深度数据，采集的点云数据数据量较小，降低了位置定位过程中的运算量。
步骤102，从点云图中提取第一目标区，第一目标区与同步采集的视觉图像中的感兴趣区检测相同对象。
其中，感兴趣区域是指视觉图像中包含对象的区域，例如，对象是人体，则感兴趣区域则为包含人体的区域。
本实施例中，在通过激光雷达测量得到点云图时，还同步通过相机采集了视觉图像，对视觉图像进行检测确定感兴趣区域，例如，通过目标检测算法(Single ShotMultiBox Detector，SSD)或目标检测算法(You Only Look Once，YOLO)从图像中确定感兴趣的区域。由于激光雷达测量到的点云可以覆盖0-360度的区域，因此，根据确定的感兴趣区域，从点云图中提取第一目标区域，而从点云图中提取第一目标区域时，由于感兴趣区域对应的参考坐标为图像坐标系，而激光雷达测量得到的激光点云图为激光雷达的极坐标系，因此，需要转换至相同的坐标系中，为了较小计算量，将感兴趣区域从图像坐标系中转换至激光雷达的极坐标系中，实现了坐标系的统一，是实现从点云图中提取第一目标区域。其中，坐标系的转换方法下一实施例中会具体介绍，此处不再赘述。
步骤103，根据第一目标区域携带的第一定位信息，确定第一目标区域中各像素点的深度。
其中，第一定位信息包含激光雷达测量到的对象的角度信息和深度信息，深度信息指示了对象与激光雷达的相对距离信息。从而，根据第一目标区域携带的第一定位信息，可确定第一目标区域中各像素点的深度。
步骤104，根据各像素点的深度，统计各设定深度对应的像素点个数指示值。
其中，各设定深度是根据深度传感器采集的深度图中携带的深度信息，基于设定的步长确定的，步长可基于实际应用场景的需求设定。在本申请的一个实施例中，对每一个设定深度，对比第一定位信息中各像素点的深度与相应设定深度，统计与相应设定深度匹配的像素点个数，得到相应设定深度对应的像素点个数指示值。
在本申请的另一个实施例中，对每一个设定深度，确定深度统计范围，统计第一定位信息中各像素点的深度与相应深度统计范围匹配的像素点个数，以得到相应设定深度对应的像素点个数指示值。本实施例中通过将每一个设定深度，进行范围扩展得到深度统计范围，统计相应深度统计范围内的像素点个数，作为相应设定深度对应的像素点个数，实现了增加相应设定深度对应的像素点的个数，兼顾了准确性和精确性。步骤105，根据像素点个数指示值的峰值，从各设定深度中确定目标深度。
具体地，从各设定深度中，确定前景深度和背景深度，其中，背景深度具有像素点个数指示值的最大峰值，前景深度具有按照深度从小到大顺序的像素点个数指示值的首个峰值，根据前景深度和背景深度对应的像素点个数指示值，从前景深度和背景深度中选定目标深度。
步骤106，根据目标深度，定位对象所处的第一观测位置。
其中，第一观测位置包括观测距离和观测角度，第一定位信息包括深度和角度。
具体地，根据目标深度对应的各像素点对应的第一定位信息中携带的角度，定位对象的观测角度，作为一种可能的实现方式，可将目标深度对应的各像素点携带的角度，进行加权平均，计算得到对象的观测角度；根据目标深度对应的各像素点对应的第一定位信息中携带的深度，定位对象的观测距离，作为一种可能的实现方式，可将目标深度对应的各像素点携带的深度，进行加权平均，计算得到对象的深度信息，根据对象的深度信息定位对象的观测距离。进而，根据定位得到的对象的观测距离和观测角度，定位对象所处的第一观测位置。
本实施例的定位方法中，获取激光雷达测量得到的点云图，从点云图中提取第一目标区，第一目标区与同步采集的视觉图像中的感兴趣区检测相同对象，根据第一目标区域中各像素点携带的第一定位信息，确定第一目标区域中各像素点的深度,根据各像素点的深度，统计各设定深度对应的像素点个数指示值，根据像素点个数指示值的峰值，从各设定深度中确定目标深度，实现了通过激光雷达与视觉图像配准，进行对象位置预测，降低了成本，提高了定位准确度。
上一实施例中说明了，根据确定的感兴趣区域，从点云图中提取第一目标区域，而从点云图中提取第一目标区域时，由于感兴趣区域对应的参考坐标为图像坐标系，而激光雷达测量得到的点云图为激光雷达的极坐标系，因此，需要转换至相同的坐标系中，为了较小计算量，将感兴趣区域从图像坐标系中转换至激光雷达的极坐标系中，实现了坐标系的统一。为此，基于上一实施例，本实施例提供了一种实现方式，具体说明了如何从点云图像中提取第一目标区域。
如图2所示，步骤102可以包含以下步骤：
步骤201，对感兴趣区域，确定图像坐标系中的直角坐标位置。
具体地，图像坐标系为平面坐标系，因此，可保留感兴趣区域对应的左右边界，对感兴趣区域的左右边界，确定直角坐标位置，如图3所示，感兴趣区域对应的左右边界对应四个顶点，根据左右边界对应的四个顶点，确定图像坐标系中的直角坐标位置。
步骤202，将图像坐标系中的直角坐标位置，映射至激光雷达的极坐标系，得到极坐标位置。
如图3所示，将图像坐标系中感兴趣区域的直角坐标位置，通过相机的内参矩阵映射至相机坐标系，得到相机坐标系的直角坐标位置，其中，相机用于采集视觉图像，进而，将相机坐标系的直角坐标位置，通过相机与激光雷达的外参矩阵映射至激光雷达的极坐标系，得到感兴趣区域在激光雷达的极坐标系中对应的极坐标位置，实现了相同坐标系的转换，而将感兴趣区域从图像坐标系转换至激光雷达的极坐标系，降低了运算量。
步骤203，根据极坐标位置，从点云图中提取第一目标区域。
具体地，根据感兴趣区域在激光雷达的极坐标系中对应的极坐标位置，从点云图中确定极坐标位置对应的区域，将极坐标位置对应的区域作为第一目标区域，其中，第一目标区域与同步采集的视觉图像中的感兴趣区域检测相同对象。也就是说点云图可以覆盖0-360 度内的数据，而本申请中，仅保留和同步采集的视觉图像中的感兴趣区域检测相同对象的区域，如图3所示的左边界和右边界之间的区域。
本实施例的定位方法中，对通过相机采集的视觉图像中的感兴趣区域进行坐标系转换，得到激光雷达的极坐标系中对应的极坐标位置，实现了相同坐标系的转换，而将感兴趣区域从图像坐标系转换至激光雷达的极坐标系，降低了运算量，根据极坐标位置从点云图中提取第一目标区域，使得第一目标区域与同步采集的视觉图像中的感兴趣区域检测相同对象，便于进行后续与视觉图像配准确定第一观测位置。
基于上述实施例，本实施例提供了一种实现方式，如图4所示，该方法包含以下步骤：
步骤401，获取激光雷达测量得到的点云图。
步骤402，从点云图中提取第一目标区，第一目标区与同步采集的视觉图像中的感兴趣区检测相同对象。
步骤403，根据第一目标区域携带的第一定位信息，确定第一目标区域中各像素点的深度。
步骤404，根据各像素点的深度，统计各设定深度对应的像素点个数指示值。
具体地，步骤401-步骤404具体可参照上述实施例中的步骤101-104，原理相同，此处不再赘述。
其中，各设定深度是根据深度传感器采集的深度图中携带的深度信息，基于设定的步长确定的，步长可基于实际应用场景的需求设定。
本实施例中，可用直方图来展示每一个设定深度对应的像素点个数。图5为本实施例提供的一种直方图示意图，图5中示出了各设定深度对应的像素点的个数。
步骤405，筛选掉像素点个数指示值小于个数阈值的设定深度。
本实施例中，深度传感器测量得到的深度图，例如RGBD相机测量到的深度图一般都随机分布许多噪点，容易在目标位置确定的过程中，形成野值，降低控制系统的稳定性，因此，根据统计确定的相应设定深度对应的像素点个数指示值，筛选掉像素点个数指示值小于个数阈值的设定深度，实现了将属于野值的设定深度去除，不用于进行目标深度的确定，提高了目标深度确定的准确性，同时也提高了控制系统的稳定性。
步骤406，从各设定深度中，确定前景深度和背景深度。
其中，背景深度具有像素点个数指示值的最大峰值，前景深度具有按照深度从小到大顺序的像素点个数指示值的首个峰值。
步骤407，根据前景深度和背景深度对应的像素点个数指示值，从前景深度和背景深度中选定目标深度。
具体地，若前景深度和背景深度的像素点个数指示值之比大于比例阈值，则将前景深度作为目标深度；若前景深度和背景深度的像素点个数指示值之比不大于比例阈值，则将背景深度作为目标深度。
在一种场景下，图6为本申请提供的另一种直方图示意图。如图6所示，按照深度从小到大顺序，像素点个数指示值的最大峰值，即为c指示的峰值，因此，c指示的峰值对应的深度即为前景深度，而像素点个数指示值的最大峰值为b指示的峰值，因此，b指示的峰值对应的深度即为背景深度。确定c指示的前景深度对应的像素点个数指示值与d指示的背景深度对应的像素点个数指示值之比，与比例阈值比较，确定比值小于比例阈值，则认为前景是遮挡物，背景才是目标，例如目标为人脸或人体，则将d指示的背景深度作为目标深度。
在另一种场景下，如图5所示，a指示的峰值即为按照深度从小到大顺序，像素点个数指示值的最大峰值，即为前景深度。b指示的峰值为像素点个数指示值的最大峰值，即为背景深度。确定a指示的前景深度对应的像素点个数指示值与b指示的背景深度对应的像素点个数指示值之比大于比例阈值，则认为背景是遮挡物，前景是目标，则将a指示的前景深度作为目标深度。
步骤408，根据目标深度，定位对象所处的第一观测位置。
其中，第一观测位置包括观测距离和观测角度。
具体地，根据目标深度对应的各像素点携带的角度，定位对象的观测角度，作为一种可能的实现方式，可将目标深度对应的各像素点携带的角度，进行加权平均，计算得到对象的观测角度；根据目标深度对应的各像素点携带的深度，定位对象的观测距离，作为一种可能的实现方式，可将目标深度对应的各像素点携带的深度，进行加权平均，计算得到对象的深度信息，根据对象的深度信息定位对象的观测距离。进而，根据定位得到的对象的观测距离和观测角度，定位对象所处的第一观测位置。
本申请的定位方法中，针对激光雷达采集的点云图中确定的第一目标区域中各像素点携带的第一定位信息，统计第一定位信息与各设定深度匹配的像素点，得到各设定深度对应的像素点个数指示值，根据各设定深度对应的像素点个数指示值，筛选掉像素点个数指示值小于个数阈值的设定深度，以去除深度图中引入的噪声深度信息，避免对系统的影响，并根据像素点个数指示值的峰值，从各设定深度中确定目标深度，以定位对象所处的第一观测位置，提高了第一观测位置确定的准确性。
基于上述实施例，本实施例提供了一种实现方式，说明了还可以根据深度传感器测量得到深度图，对对象定位得到第二观测位置，进而，根据对象的历史运动状态，对第一观测位置和第二观测位置融合，以确定对象的目标位置，提高了目标位置确定的准确性。
需要明的是，本实施例中的下述步骤701-步骤704，可以在上述步骤106和步骤408之前执行，也可以在上述步骤106和步骤408之后执行，本实施例中不进行限定。本实施例中以在根据激光雷达测量到的点云数据，定位对象所处的第一观测位置后，还可以根据深度传感器测量得到深度图，对对象定位得到第二观测位置，进而，根据对象的历史运动状态，对第一观测位置和第二观测位置融合，以确定对象的目标位置为例进行说明。
如图7所示，步骤106和步骤408之后可以包括以下步骤：
步骤701，获取深度传感器测量得到的深度图。
其中，深度传感器，例如为(Depth Red Green Blue，RGB-D)传感器，第二定位信息包含深度传感器测量得到的深度图中各对象的深度信息，深度信息指示了对象与深度传感器的相对距离信息。
步骤702，从深度图中提取第二目标区域。
其中，第二目标区与同步采集的视觉图像中的感兴趣区检测相同对象。
本实施例中，在通过激光雷达测量得到点云图，深度传感器测量得到深度图时，还同步通过相机采集了视觉图像，对视觉图像进行检测确定感兴趣区域，也就是对象所处的区域，例如，通过目标检测算法(Single Shot MultiBox Detector，SSD)或目标检测算法(You Only Look Once，YOLO)从图像中确定感兴趣的区域，根据确定的感兴趣区域，从深度图中提取第二目标区域，也就是说点云图中的第一目标区域和深度图中的第二目标区域与同步采集的视觉图像中的感兴趣区域检测相同对象，为不同的传感器进行数据融合确定对象的位置提供了基础。
作为一种可能的实现方式，将获取到的深度图与相机采集的视觉图像进行配准，进而，根据视觉图像中感兴趣区域对深度图进行裁剪，以得到和感兴趣区域检测相同对象的第二目标区域。
步骤703，根据第二目标区域携带的第二定位信息，确定对象所处的第二观测位置。
其中，第二定位信息包含深度，第二观测位置包含观测距离。
本实施例中，根据第二目标区域中各像素点携带的第二定位信息，统计第二定位信息与各设定深度匹配的像素点，得到各设定深度对应的像素点个数指示值，根据像素点个数指示值的峰值，从各设定深度中确定目标深度，根据目标深度，定位对象所处的第二观测位置。
具体的实现方法，可参照上一实施例中对激光雷达传感器采集的点云图中的第一目标区域，确定对象所处的第一观测位置的方法，实现原理相同，此处不再赘述。
步骤704，获取第二观测位置。
步骤705，根据对象的历史运动状态，对第一观测位置和第二观测位置中的一个，执行迭代修正过程。。
步骤706，根据迭代修正过程得到的位置更新历史运动状态。
步骤707，根据更新后的历史运动状态，对第一观测位置和第二观测位置中的另一个，执行迭代修正过程，以得到目标位置。
本实施例中，可先对第一观测位置执行迭代修正过程，也可以先对第二观测位置执行迭代修正过程，其中，如何确定第一观测位置和第二观测位置执行迭代修正的过程的顺序，下一实施例中会进行详细描述。
在一个实施例中，根据对象的历史运动状态，对第一观测位置执行迭代修正过程，根据迭代修正过程得到的位置更新历史运动状态，得到更新后的历史运动状态，进而，根据更新后的历史运动状态，对第二观测位置执行迭代修正过程，以得到目标位置，也就是说目标位置是在第一观测位置迭代修正的基础上，又进行第二观测位置迭代修正后得到的，即通过融合第一定位信息和第二定位信息得到的，也就是说融合了深度传感器的测量数据和激光雷达传感器的测量数据，通过融合增加了用于确定目标位置的信息量，从而提高了目标位置确定的准确性。
同理，先对第二观测位置执行迭代修正过程，再对第一观测位置执行迭代修正过程，和上述融合确定目标位置的原理相同，此处不再赘述。
需要说明的是，本实施例中是以两个传感器，即激光雷达传感器和深度传感器采集数据的时间顺序进行融合为例，而实际应用中，还可以根据采集时间的顺序，对三个或更多个传感器进行融合，本实施例中不进行限定。
本申请实施例的定位方法中，根据深度传感器测量得到的深度图，确定对象所处的第二观测位置，根据对象的历史运动状态，对第一观测位置和第二观测位置中的一个，执行迭代修正过程，根据迭代修正过程得到的位置更新历史运动状态，根据更新后的历史运动状态，对第一观测位置和第二观测位置中的另一个，执行迭代修正过程，以得到目标位置，目标位置是通过融合第一定位信息和第二定位信息得到的，也就是说融合了深度传感器的测量数据和激光雷达传感器的测量数据，通过融合增加了用于确定目标位置的信息量，从而提高了目标位置确定的准确性。
基于上述实施例，图8为本申请实施例提供的一种迭代修正过程的流程示意图。
如图8所示，上述步骤705和步骤707中，执行迭代修正过程，可以包含以下步骤：
步骤801，根据本次迭代修正过程采用的历史运动状态，得到预测运动状态，其中，本次迭代修正过程采用的历史运动状态，是根据前一次迭代修正过程得到的位置，以及前一次迭代修正过程采用的历史运动状态生成。
在本申请的一个实施例中，将本次迭代修正过程采用的历史运动状态带入系统状态方程，得到预测运动状态。
本实施例中，系统状态方程为历史运动状态与状态转移矩阵的乘积叠加过程噪声项。
例如，系统状态方程表示为：X(t+1)＝g(t)X(t)+w(t)，其中，X(t)表示历史运动状态，w(t)为过程噪声项，符合期望为0，协方差为Q的高斯白噪声，其协方差Q可以初始化为常值。g(t)为状态转移矩阵，例如，
Figure BDA0002574875210000131
其中，X(t)为历史运动状态，历史运动状态中包含的dist(t)，指示历史状态中对象的相对距离；vel(t)指示对象的相对线速度；sita(t)指示对象的相对角度；omega(t)指示对象的角速度。X(t+1)为根据历史运动状态X(t)预测得到的预测运动状态。
以机器人场景为例，机器人中设置的激光雷达传感器和深度传感器实时采集人的图像信息，图9为各状态变量在极坐标系中的定义示意图，如图9所示，则历史运动状态的各个变量在激光雷达的极坐标系中的定义如下：
图9中，三角形处指示的为机器人的位置，五角星处指示的为对象，即人的位置，编号1指示的即为dist(t)，即人与机器人之间的相对距离，始终为正值；
编号2指示的即sita(t):人在机器人坐标系中的相对角度，正前方为0度，顺时针为正；
编号3指示的即vel(t)，即人与机器人之间的相对线速度，远离为正，即箭头指示的方向为正；
编号4指示的即为omega(t)，即人在机器人坐标系中，沿切向运动的角速度，顺时针为正，即箭头指示的方向为正。
其中，X(t+1)为根据历史运动状态X(t)预测得到的预测运动状态。
步骤802，根据预测运动状态，得到预测观测位置。
本申请的一个实施例中，将预测运动状态带入观测方程，得到预测观测位置。其中，观测方程为预测运动状态与转换矩阵的乘积叠加量测噪声项。其中，转换矩阵，用于指示预测运动状态与预测观测位置之间的转换关系；量测噪声项，符合设定协方差的高斯白噪声分布，而设定协方差是根据设备精度和测量置信度确定的。其中，测量置信度为第一观测位置的测量置信度或第二观测位置的测量置信度，而测量置信度的确定方法，后续实施例中会说明。
本实施例中，将预测运动状态代入观测方程后，可以得到预测观测位置，预测观测位置包含预测观测角度和预测观测距离。
如，预测观测距离的观测方程为：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]。
步骤803，根据预测观测位置，对进行本次迭代修正过程的第一观测位置或第二观测位置进行修正。
在本申请的一个实施例中，若本次迭代修正过程，是对第一观测位置进行修正，则根据预测观测位置，对进行本次迭代修正过程的第一观测位置进行修正，以得到修正后的位置。
其中，预测观测位置包含预测角度和预测距离，第一观测位置包含对象的第一观测角度和第二观测距离，从而，修正后得到的位置也包含观测距离和观测角度。
作为一种可能的实现方式，确定预测观测位置的权重和第一观测位置的权重，根据预测观测位置的权重和第一观测位置的权重进行加权计算，得到本次迭代修正过程得到的位置。其中，修正后得到的位置也包含观测距离和观测角度。
在本申请的另一个实施例中，若本次迭代修正过程，是对第二观测位置进行修正，则根据预测观测位置，对进行本次迭代修正过程的第二观测位置进行修正，以得到修正后的位置。
其中，预测观测位置包含预测角度和预测距离，第一观测位置包含对象的第一观测角度和第二观测距离，从而，修正后得到的位置也包含观测距离和观测角度。
作为一种可能的实现方式，确定预测观测位置的权重和第二观测位置的权重，根据预测观测位置的权重和第二观测位置的权重进行加权计算，得到本次迭代修正过程得到的位置，也就是说实现了，通过预测观测位置对第二观测位置进行修正。
需要说明的是，本次迭代修正过程采用的历史运动状态，是根据前一次迭代修正过程得到的位置，以及前一次迭代修正过程采用的历史运动状态生成。例如，本实施例中A1轮迭代修正过程是通过预测观测位置对第一观测位置或第二观测位置进行修正得到修正后的距离S，而下一次A2迭代修正过程采用的历史运动状态，则是利用A1轮修正后的距离 S对A1轮迭代采用的历史运动状态中的距离进行替换，以得到下一次A2轮迭代修正过程需要采用的历史运动状态，实现了历史运动状态的迭代修正，提高了每一轮迭代修正的准确性。
在本申请实施例的一种可能的实现方式中，历史运动状态包括对象的历史位置和历史速度。其中，历史位置包含对象的相对距离和相对角度，也就是说距离是通过对象与采集的机器的相对距离和相对角度确定的。历史速度包含对象的相对线速度和角速度。
对应地，针对每一次迭代修正过程，本次迭代修正过程采用的历史运动状态中的历史位置，是根据前一次迭代修正过程得到的位置，以及前一次迭代修正过程采用的历史位置生成。而针对每一次迭代修正过程，本次迭代修正过程采用的历史运动状态中的历史速度，是根据本次迭代修正过程采用的历史位置，以及前一次迭代修正过程采用的历史位置确定的，通过在每一次迭代修正过程中，对历史运动状态中的历史位置和历史速度的修正，实现了每次迭代修正过程均是采用修正后的历史运动状态，提高了每次迭代修正过程得到的位置的准确性。
需要说明的是，在每一次迭代过程中修正对象的速度，可以对对象的行为进行预判，例如，判断对象相对采集机器是远离还是靠近等，以实现对对象进行跟踪。
本实施例的定位方法中，通过执行迭代修正实现了根据预测观测位置，对进行本次迭代修正过程的第一观测位置或第二观测位置进行修正，以得到修正后的位置，当对第一观测位置和第二观测位置都完成修正后，则得到修正后的目标位置，使得确定的目标位置融合了第一观测位置和第二观测位置的信息，提高了目标位置确定的准确性。
实际应用中，当预测观测位置与进行本次迭代修正过程的第一观测位置或第二观测位置之间的差异较大时，则说明预测观测位置的可信度较低，若采用该预测观测位置对获取到的第一观测位置或第二观测位置进行修正，会使得修正后得到的位置误差更大。从而，为了提高迭代修正的可靠性，本实施例中根据测量残差，判断预测观测位置与进行本次迭代修正过程的第一观测位置或第二观测位置之间的差异大小，以识别本次迭代过程，是否需要对第一观测位置或第二观测位置进行修正。因此，如图10所示，上述步骤803，可以包含以下步骤：
步骤1001，对进行本次迭代修正过程的第一观测位置或第二观测位置，确定相应的测量残差。
步骤1002，若测量残差小于差异阈值，则根据预测观测位置，对进行本次迭代修正过程的第一观测位置或第二观测位置进行修正。
其中，测量残差是预测观测位置与进行本次迭代修正过程的第一观测位置或第二观测位置之间的差异。
本实施例中，由于激光雷达传感器或深度传感器获取到检测数据的时间不同步，因此，每次进行迭代修正时可能利用第一观测位置进行修正，也可能利用第二观测位置进行修正，从而，将得到的预测观测位置，与第一观测位置或第二观测位置，确定相应的测量残差。
具体地，在一种场景下，进行本次迭代修正过程的为第一观测位置，则根据第一观测位置与预测观测位置确定测量残差，并判断测量残差是否小于差异阈值，若测量残差小于差异阈值，则得到的预测观测位置可信度较高，可用于后续对第一观测位置进行修正，以确定修正后得到的位置；若测量残差大于差异阈值，则得到的预测观测位置可信度较小，即视为噪声数据，不用于对第一观测位置进行修正，以提高修正得到的位置的准确度。
在另一种场景下，进行本次迭代修正过程的为第二观测位置，则根据第二观测位置与预测观测位置确定测量残差，并判断测量残差是否小于差异阈值，若测量残差小于差异阈值，则得到的预测观测位置可信度较高，可用于后续对第二观测位置进行修正，以确定修正后得到的位置；若测量残差大于差异阈值，则得到的预测观测位置可信度较小，即视为噪声数据，不用于对第二观测位置进行修正，以提高修正得到的位置的准确度。
本实施例的定位方法中，对进行本次迭代修正过程的第一观测位置或第二观测位置，确定相应的测量残差，在测量残差小于差异阈值时，根据预测观测位置，对进行本次迭代修正过程的第一观测位置或第二观测位置进行修正，由于测量残差指示了得到的预测观测位置可信度的高低，从而提高了每次迭代修正过程中进行第一观测位置或第二观测位置修正的可靠度和准确性。
上述实施例中描述了根据对象的历史运动状态对第一观测位置和第二观测位置中的一个，执行迭代修正过程，本实施例中提供了一种确定第一观测位置和第二观测位置执行迭代修正的顺序的方法，如图11所示，上述步骤705之前，可以包含以下步骤：
步骤1101，比较获取到点云图的第一观测时刻，以及获取到深度图的第二观测时刻。
本实施例中，激光雷达在测量得到点云图时，和深度传感器在测量得到深度图时，采集时间是不同步的，为了便于区分，将获取到点云图的时刻称为第一观测时刻，将获取到深度图的时刻称为第二观测时刻，也就是说获取到点云图的第一观测时刻和获取到深度图的第二观测时刻并不相同，因此，需要比较第一观测时刻和第二观测时刻，以确定第一观测时刻和第二观测时刻的先后。
步骤1102，若第一观测时刻先于第二观测时刻，则确定第一观测位置先于第二观测位置进行迭代修正。
步骤1103，若第一观测时刻晚于第二观测时刻，则确定第二观测位置先于第一观测位置进行迭代修正。
步骤1104，若第一观测时刻等于第二观测时刻，则随机确定第一观测位置和第二观测位置进行迭代修正的顺序。
本实施例的定位方法中，若比较确定第一观测时刻先于第二观测时刻，则确定第一观测位置先于第二观测位置进行迭代修正，也就是说先根据第一观测时刻的第一观测位置进行迭代修正，再根据第二观测时刻的第二观测位置进行迭代修正；若比较确定第一观测时刻晚于第二观测时刻，则确定第二观测位置先于第一观测位置进行迭代修正；若第一观测时刻等于第二观测时刻，则随机确定第一观测位置和第二观测位置进行迭代修正的顺序，也就是说随机确定第一观测位置和第二观测位置中的一个先进行迭代修正，再对第一观测位置和第二观测位置中的另一个进行迭代修正实现了对第一观测位置和第二观测位置交替进行迭代修正，从而实现了对激光雷达测量数据和深度传感器测量数据的有效融合，提高了准确性。
基于上述实施例，本实施例提供了一种实现方式，说明了在确定第一观测位置后，如何确定第一观测位置的测量置信度，而测量置信度用于确定前述实施例中观测方程的量测噪声项。如图12所示，步骤106和步骤408之后，包含以下步骤：
步骤1201，根据目标深度对应的像素点个数指示值，确定第一观测位置的测量置信度。
其中，目标深度对应的像素点个数指示值与第一观测位置的测量置信度成正比，也就是说目标深度对应的像素点个数指示值越大，第一观测位置的测量置信度则越高。
实际场景下，目标深度可以为背景深度也可以为前景深度，因此，在确定第一观测位置的置信度时，也根据对应的背景深度或前景深度进行确定。
在一种场景下，若目标深度为背景深度，则可以根据背景深度对应的像素点个数指示值，确定目标位置的测量置信度，作为一种可能的实现方式，利用背景深度的像素点个数指示值与前景深度的像素点个数指示值之比，确定第一观测位置的测量置信度，如，置信度为C1，C1＝(back_ratio/doubless_ratio)*100，其中，back_ratio为背景深度的像素点个数指示值与前景深度的像素点个数指示值之比，doubless_ratio为置信度的满分阈值。
在另一种场景下，若目标深度为前景深度，则可以根据前景深度对应的像素点个数指示值，确定目标位置的测量置信度，作为一种可能的实现方式，利用前景深度的像素点个数指示值与背景深度的像素点个数指示值之比，确定第一观测位置的测量置信度，如，置信度为C2，C2＝(fore_ratio/doubless_ratio)*100，其中，fore_ratio前景深度的像素点个数指示值与背景深度的像素点个数指示值之比，doubless_ratio为置信度的满分阈值。
步骤1202，若前景深度和背景深度相同，提高第一观测位置的测量置信度。
其中，测量置信度，用于为图8实施例中说明的迭代修正过程中采用的观测方程，生成量测噪声项，观测方程用于在根据迭代修正过程采用的历史运动状态，得到预测运动状态之后，将预测运动状态代入观测方程，以得到预测观测位置。
具体地，若前景深度和背景深度相同，即前景和背景深度是重合的，也就是说当前确定的第一观测位置的准确度较高，则需要提高第一观测位置的测量置信度，作为一种可能的实现方式，可将利用前景深度确定的测量置信度增加为2倍，即为2C2，或者是将利用前景深度确定的测量置信度增加为2倍，即为2C1。
需要说明的是，第二观测位置的测量置信度的确定方法和第一观测位置的测量置信度的确定方法，原理相同，此处不再赘述。
本申请的定位方法中，根据目标深度，定位对象所处的第一观测位置后进一步确定第一观测位置的测量置信度，同理，确定第二观测位置的测量置信度，以根据测量置信度，用于为迭代修正过程中采用的观测方程，生成量测噪声项，提高迭代修正过程的可靠性，提高基于多个传感器的数据融合确定的目标位置的准确性。
为了实现上述实施例，本申请还提出一种定位装置。
图13为本申请实施例提供的一种定位装置的结构示意图。
如图13所示，该装置包括：第一获取模块91、提取模块92、第一确定模块93、统计模块94、第二确定模块95和定位模块96。
第一获取模块91，用于获取激光雷达测量得到的点云图。
提取模块92，用于从点云图中提取第一目标区，第一目标区与同步采集的视觉图像中的感兴趣区检测相同对象。
第一确定模块93，用于根据第一目标区域携带的第一定位信息，确定第一目标区域中各像素点的深度。
统计模块94，用于根据各像素点的深度，统计各设定深度对应的像素点个数指示值。
第二确定模块95，用于根据像素点个数指示值的峰值，从各设定深度中确定目标深度。
定位模块96，用于根据目标深度，定位对象所处的第一观测位置。
进一步地，在本申请实施例的一种可能的实现方式中，第二确定模块95，具体用于：
从各设定深度中，确定前景深度和背景深度；其中，背景深度具有像素点个数指示值的最大峰值；前景深度具有按照深度从小到大顺序的像素点个数指示值的首个峰值；根据前景深度和背景深度对应的像素点个数指示值，从前景深度和背景深度中选定目标深度。
作为一种可能的实现方式，上述第二确定模块95，具体用于：
若前景深度和背景深度的像素点个数指示值之比大于比例阈值，则将前景深度作为目标深度；若前景深度和背景深度的像素点个数指示值之比不大于比例阈值，则将背景深度作为目标深度。
作为一种可能的实现方式，该装置，还包括：筛除模块。
筛除模块，用于筛选掉像素点个数指示值小于个数阈值的设定深度。
作为一种可能的实现方式，上述统计模块94，具体用于：
对每一个设定深度，确定深度统计范围；根据各像素点的深度，统计深度与相应深度统计范围匹配的像素点个数，以得到相应设定深度对应的像素点个数指示值。
作为一种可能的实现方式，第一观测位置包括观测距离和观测角度，第一定位信息包括深度和角度，上述定位模块96，具体用于：根据目标深度对应的各像素点携带的角度，定位对象的观测角度；根据目标深度对应的各像素点携带的深度，定位对象的观测距离。
在本申请实施例的一种可能的实现方式中，该装置还包括：第二获取模块，和迭代修正模块。
第二获取模块，还用于获取第二观测位置，其中，第二观测位置，是根据深度传感器测量得到的深度图，对对象定位得到的。
迭代修正模块，用于根据对象的历史运动状态，对第一观测位置和第二观测位置中的一个，执行迭代修正过程；根据迭代修正过程得到的位置更新历史运动状态；根据更新后的历史运动状态，对第一观测位置和第二观测位置中的另一个，执行迭代修正过程，以得到目标位置。
作为一种可能的实现方式，上述第二获取模块，具体用于获取深度传感器测量得到的深度图；从深度图中提取第二目标区，其中，第二目标区与同步采集的视觉图像中的感兴趣区检测相同对象；根据第二目标区携带的第二定位信息，确定对象所处的第二观测位置。
作为一种可能的实现方式，迭代修正模块，具体用于：
根据本次迭代修正过程采用的历史运动状态，得到预测运动状态；本次迭代修正过程采用的历史运动状态，是根据前一次迭代修正过程得到的位置，以及前一次迭代修正过程采用的历史运动状态生成；根据预测运动状态，得到预测观测位置；根据预测观测位置，对进行本次迭代修正过程的第一观测位置或第二观测位置进行修正。
作为一种可能的实现方式，历史运动状态包括对象的历史位置和历史速度；对应地，针对每一次迭代修正过程，本次迭代修正过程采用的历史运动状态中的历史位置，是根据前一次迭代修正过程得到的目标位置，以及前一次迭代修正过程采用的历史位置生成；针对每一次迭代修正过程，本次迭代修正过程采用的历史运动状态中的历史速度，是根据本次迭代修正过程采用的历史位置，以及前一次迭代修正过程采用的历史位置确定的。
作为一种可能的实现方式，该装置还包括：比较确定模块。
比较确定模块，用于比较获取到点云图的第一观测时刻，以及获取到深度图的第二观测时刻；若第一观测时刻先于第二观测时刻，则确定第一观测位置先于第二观测位置进行迭代修正；若第一观测时刻晚于第二观测时刻，则确定第二观测位置先于第一观测位置进行迭代修正；若第一观测时刻等于第二观测时刻，则随机确定第一观测位置和第二观测位置进行迭代修正的顺序。
作为一种可能的实现方式，迭代修正模块，具体用于将预测运动状态代入观测方程，得到预测观测位置；其中，观测方程为预测运动状态与转换矩阵的乘积叠加量测噪声项；转换矩阵，用于指示预测运动状态与预测观测位置之间的转换关系；量测噪声项，符合设定协方差的高斯白噪声分布，设定协方差是根据设备精度和测量置信度确定的。
作为一种可能的实现方式，迭代修正模块，具体用于对进行本次迭代修正过程的第一观测位置或第二观测位置，确定测量残差；若测量残差小于差异阈值，则根据预测观测位置，对进行本次迭代修正过程的第一观测位置或第二观测位置进行修正。
作为一种可能的实现方式，测量残差是预测观测位置与进行本次迭代修正过程的第一观测位置或第二观测位置之间的差异。
在本申请实施例的一种可能的实现方式中，该装置还包括：第三确定模块。
第三确定模块，用于根据目标深度对应的像素点个数指示值，确定第一观测位置的测量置信度；若前景深度和所述背景深度相同，提高第一观测位置的测量置信度，其中，测量置信度，用于为第一观测位置的迭代修正过程中采用的观测方程，生成量测噪声项，观测方程用于在根据迭代修正过程采用的历史运动状态，得到预测运动状态之后，将预测运动状态代入观测方程，以得到预测观测位置。
在本申请实施例的一种可能的实现方式中，上述提取模块92，具体用于：
对感兴趣区域，确定图像坐标系中的直角坐标位置；将图像坐标系中的直角坐标位置，映射至激光雷达的极坐标系，得到极坐标位置；根据极坐标位置，从点云图中提取第一目标区域。
作为一种可能的实现方式，上述提取模块92，具体用于：对感兴趣区域的左右边界，确定直角坐标位置。
作为一种可能的实现方式，上述提取模块92，具体用于将图像坐标系中的直角坐标位置，通过相机的内参矩阵映射至相机坐标系，得到相机坐标系的直角坐标位置；其中，相机用于采集视觉图像；将相机坐标系的直角坐标位置，通过相机与激光雷达的外参矩阵映射至极坐标系，得到极坐标位置。
需要说明的是，前述对定位方法实施例的解释说明也适用于该实施例的定位装置，此处不再赘述。
为了实现上述实施例，本申请还提出一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时，实现如前述方法实施例所述的定位方法。
本申请提出的电子设备可以但不限于为机器人。
为了实现上述实施例，本申请还提出一种非临时性计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时，实现如前述方法实施例所述的定位方法。
应当理解，本申请的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。如，如果用硬件来实现和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。
此外，在本申请各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。
上述提到的存储介质可以是只读存储器，磁盘或光盘等。尽管上面已经示出和描述了本申请的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本申请的限制，本领域的普通技术人员在本申请的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (10)

1.一种定位方法，其特征在于，所述方法包括：
获取激光雷达测量得到的点云图；
从所述点云图中提取第一目标区，所述第一目标区与同步采集的视觉图像中的感兴趣区检测相同对象；
根据所述第一目标区域携带的第一定位信息，确定所述第一目标区域中各像素点的深度；
根据所述各像素点的深度，统计各设定深度对应的像素点个数指示值；
根据所述像素点个数指示值的峰值，从各设定深度中确定目标深度；
根据所述目标深度，定位对象所处的第一观测位置。
2.根据权利要求1所述的定位方法，其特征在于，所述根据像素点个数指示值的峰值，从各设定深度中确定目标深度，包括：
从各设定深度中，确定前景深度和背景深度；其中，所述背景深度具有像素点个数指示值的最大峰值；所述前景深度具有按照深度从小到大顺序的像素点个数指示值的首个峰值；
根据所述前景深度和所述背景深度对应的像素点个数指示值，从所述前景深度和所述背景深度中选定所述目标深度。
3.根据权利要求2所述的定位方法，其特征在于，所述根据所述前景深度和所述背景深度对应的像素点个数指示值，从所述前景深度和所述背景深度中选定所述目标深度，包括：
若所述前景深度和所述背景深度的像素点个数指示值之比大于比例阈值，则将所述前景深度作为所述目标深度；
若所述前景深度和所述背景深度的像素点个数指示值之比不大于所述比例阈值，则将所述背景深度作为所述目标深度。
4.根据权利要求1所述的定位方法，其特征在于，所述根据所述各像素点的深度，统计各设定深度对应的像素点个数指示值之后，还包括：
筛选掉所述像素点个数指示值小于个数阈值的设定深度。
5.根据权利要求1所述的定位方法，其特征在于，所述根据所述各像素点的深度，统计各设定深度对应的像素点个数指示值，包括：
对每一个设定深度，确定深度统计范围；
根据各像素点的深度，统计所述深度与相应深度统计范围匹配的像素点个数，以得到相应设定深度对应的像素点个数指示值。
6.根据权利要求1所述的定位方法，其特征在于，所述第一观测位置包括观测距离和观测角度；所述第一定位信息包括深度和角度；
所述根据所述目标深度，定位对象所处的第一观测位置，包括：
根据所述目标深度对应的各像素点携带的角度，定位所述对象的观测角度；
根据所述目标深度对应的各像素点携带的深度，定位所述对象的观测距离。
7.根据权利要求1-6任一项所述的定位方法，其特征在于，所述方法还包括：
获取第二观测位置；其中，所述第二观测位置，是根据深度传感器测量得到的深度图，对所述对象定位得到的；
根据所述对象的历史运动状态，对所述第一观测位置和所述第二观测位置中的一个，执行迭代修正过程；
根据所述迭代修正过程得到的位置更新所述历史运动状态；
根据更新后的历史运动状态，对所述第一观测位置和所述第二观测位置中的另一个，执行迭代修正过程，以得到所述目标位置。
8.一种定位装置，其特征在于，所述装置包括：
第一获取模块，用于获取激光雷达测量得到的点云图；
提取模块，用于从所述点云图中提取第一目标区，所述第一目标区与同步采集的视觉图像中的感兴趣区检测相同对象；
第一确定模块，用于根据所述第一目标区域携带的第一定位信息，确定所述第一目标区域中各像素点的深度；
统计模块，用于根据所述各像素点的深度，统计各设定深度对应的像素点个数指示值；
第二确定模块，用于根据所述像素点个数指示值的峰值，从各设定深度中确定目标深度；
定位模块，用于根据所述目标深度，定位对象所处的第一观测位置。
9.一种电子设备，其特征在于，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时，实现如权利要求1-7中任一所述的定位方法。
10.一种非临时性计算机可读存储介质，其上存储有计算机程序，其特征在于，所述程序被处理器执行时实现如权利要求1-7中任一所述的定位方法。
CN202010650773.XA 2020-07-08 2020-07-08 定位方法、装置、电子设备和计算机可读存储介质 Pending CN113916213A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010650773.XA CN113916213A (zh) 2020-07-08 2020-07-08 定位方法、装置、电子设备和计算机可读存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010650773.XA CN113916213A (zh) 2020-07-08 2020-07-08 定位方法、装置、电子设备和计算机可读存储介质

Publications (1)

Publication Number Publication Date
CN113916213A true CN113916213A (zh) 2022-01-11

Family

ID=79231533

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010650773.XA Pending CN113916213A (zh) 2020-07-08 2020-07-08 定位方法、装置、电子设备和计算机可读存储介质

Country Status (1)

Country Link
CN (1) CN113916213A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022262594A1 (zh) * 2021-06-15 2022-12-22 同方威视技术股份有限公司 跟随目标的方法、装置、机器人及计算机可读存储介质

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103843335A (zh) * 2011-10-14 2014-06-04 索尼公司 图像处理装置、图像处理方法和程序
CN104133339A (zh) * 2013-05-02 2014-11-05 聚晶半导体股份有限公司 自动对焦方法及自动对焦装置
WO2016182964A1 (en) * 2015-05-08 2016-11-17 5D Robotics, Inc. Adaptive positioning system
CN107796397A (zh) * 2017-09-14 2018-03-13 杭州迦智科技有限公司 一种机器人双目视觉定位方法、装置和存储介质
KR20180054151A (ko) * 2016-11-15 2018-05-24 에스케이텔레콤 주식회사 3d 영상에서 배경과 전경을 분리하는 장치 및 방법
CN109634279A (zh) * 2018-12-17 2019-04-16 武汉科技大学 基于激光雷达与单目视觉的物体定位方法
CN109767452A (zh) * 2018-12-24 2019-05-17 深圳市道通智能航空技术有限公司 一种目标定位方法和装置、无人机
CN109945856A (zh) * 2019-02-18 2019-06-28 天津大学 基于惯性/雷达的无人机自主定位与建图方法
CN111986245A (zh) * 2019-05-23 2020-11-24 北京猎户星空科技有限公司 深度信息评估方法、装置、电子设备及存储介质
KR20210042279A (ko) * 2020-04-29 2021-04-19 베이징 바이두 넷컴 사이언스 앤 테크놀로지 코., 엘티디. 포인트 클라우드를 업데이트하는 방법, 장치, 전자 기기, 저장 매체 및 프로그램
CN113917475A (zh) * 2020-07-08 2022-01-11 北京猎户星空科技有限公司 定位方法、装置、电子设备和计算机可读存储介质

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103843335A (zh) * 2011-10-14 2014-06-04 索尼公司 图像处理装置、图像处理方法和程序
CN104133339A (zh) * 2013-05-02 2014-11-05 聚晶半导体股份有限公司 自动对焦方法及自动对焦装置
WO2016182964A1 (en) * 2015-05-08 2016-11-17 5D Robotics, Inc. Adaptive positioning system
KR20180054151A (ko) * 2016-11-15 2018-05-24 에스케이텔레콤 주식회사 3d 영상에서 배경과 전경을 분리하는 장치 및 방법
CN107796397A (zh) * 2017-09-14 2018-03-13 杭州迦智科技有限公司 一种机器人双目视觉定位方法、装置和存储介质
CN109634279A (zh) * 2018-12-17 2019-04-16 武汉科技大学 基于激光雷达与单目视觉的物体定位方法
CN109767452A (zh) * 2018-12-24 2019-05-17 深圳市道通智能航空技术有限公司 一种目标定位方法和装置、无人机
CN109945856A (zh) * 2019-02-18 2019-06-28 天津大学 基于惯性/雷达的无人机自主定位与建图方法
CN111986245A (zh) * 2019-05-23 2020-11-24 北京猎户星空科技有限公司 深度信息评估方法、装置、电子设备及存储介质
KR20210042279A (ko) * 2020-04-29 2021-04-19 베이징 바이두 넷컴 사이언스 앤 테크놀로지 코., 엘티디. 포인트 클라우드를 업데이트하는 방법, 장치, 전자 기기, 저장 매체 및 프로그램
CN113917475A (zh) * 2020-07-08 2022-01-11 北京猎户星空科技有限公司 定位方法、装置、电子设备和计算机可读存储介质

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
刘毅;孙立峰;杨士强;: "立体图像重定技术研究综述", 计算机辅助设计与图形学学报, no. 07 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022262594A1 (zh) * 2021-06-15 2022-12-22 同方威视技术股份有限公司 跟随目标的方法、装置、机器人及计算机可读存储介质

Similar Documents

Publication Publication Date Title
TWI536318B (zh) 深度測量之品質提升
US11830216B2 (en) Information processing apparatus, information processing method, and storage medium
CN108955718B (zh) 一种视觉里程计及其定位方法、机器人以及存储介质
KR102054455B1 (ko) 이종 센서 간의 캘리브레이션 장치 및 방법
CN109211277B (zh) 视觉惯性里程计的状态确定方法、装置和电子设备
WO2017077925A1 (en) Method and system for estimating three-dimensional pose of sensor
JP6744747B2 (ja) 情報処理装置およびその制御方法
CN109471096B (zh) 多传感器目标匹配方法、装置及汽车
JP2016099941A (ja) オブジェクト位置推定システム、及びそのプログラム
JP7272024B2 (ja) 物体追跡装置、監視システムおよび物体追跡方法
EP3593322B1 (en) Method of detecting moving objects from a temporal sequence of images
JP2014523572A (ja) 地図データの生成
Schramm et al. Data fusion for 3D thermal imaging using depth and stereo camera for robust self-localization
CN115376109A (zh) 障碍物检测方法、障碍物检测装置以及存储介质
CN113916213A (zh) 定位方法、装置、电子设备和计算机可读存储介质
CN110706257B (zh) 有效特征点对的识别方法、相机状态的确定方法及装置
CN113917475A (zh) 定位方法、装置、电子设备和计算机可读存储介质
CN117197245A (zh) 一种位姿修复方法及装置
CN109816726B (zh) 一种基于深度滤波器的视觉里程计地图更新方法和系统
CN117115434A (zh) 数据分割装置和方法
KR102547333B1 (ko) 깊이 영상 기반 실시간 바닥 검출방법
JP2019046278A (ja) 情報処理装置、制御方法、コンピュータプログラム、記憶媒体、及びモデル作成装置
CN117197182B (zh) 雷视标定方法、设备及存储介质
CN117649619B (zh) 无人机视觉导航定位回收方法、系统、装置及可读存储介质
CN114983302B (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