CN116051635A - 一种位姿确定方法和装置 - Google Patents

一种位姿确定方法和装置 Download PDF

Info

Publication number
CN116051635A
CN116051635A CN202211711839.7A CN202211711839A CN116051635A CN 116051635 A CN116051635 A CN 116051635A CN 202211711839 A CN202211711839 A CN 202211711839A CN 116051635 A CN116051635 A CN 116051635A
Authority
CN
China
Prior art keywords
grid
point cloud
target point
grids
characteristic
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
CN202211711839.7A
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.)
Neolix Technologies Co Ltd
Original Assignee
Neolix Technologies 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 Neolix Technologies Co Ltd filed Critical Neolix Technologies Co Ltd
Priority to CN202211711839.7A priority Critical patent/CN116051635A/zh
Publication of CN116051635A publication Critical patent/CN116051635A/zh
Pending legal-status Critical Current

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/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • 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
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/77—Determining position or orientation of objects or cameras using statistical 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/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Probability & Statistics with Applications (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)

Abstract

本发明公开了一种位姿确定方法和装置，涉及移动机器人技术领域。该方法的一具体实施方式包括：将参考点云划分至多个网格；计算网格中参考点云在正态分布下的均值和协方差矩阵；对协方差矩阵进行特征分解，得到网格的特征值；基于当前位姿对目标点云进行变换，得到目标点云所在的网格；根据网格的特征值，在目标点云所在的网格中确定有效网格；根据均值、协方差矩阵和有效网格中的目标点云，计算目标函数的值，当满足预设的终止条件时，确定当前位姿为目标位姿，否则，基于目标函数的值更新当前位姿，并执行基于当前位姿对目标点云进行变换。该实施方式能够降低计算量，提高定位精度。

Description

一种位姿确定方法和装置
技术领域
本发明涉及移动机器人技术领域，尤其涉及一种位姿确定方法和装置。
背景技术
对于自动驾驶车辆等移动机器人来说，定位系统是保证其正常运行的必要条件。基于定位传感器的不同，定位方法可以分为卫星定位、激光雷达定位和视觉定位。特别地，由于对城市建筑结构具有良好的适应性，激光雷达定位被广泛使用，其中包括基于点云匹配的方法。基于点云匹配的方法，是将激光雷达实时扫描得到的目标点云与预先建立的参考点云进行匹配，得到移动机器人的位姿。目前使用的点云匹配的方法包括NDT(NormalDistributions Transform，正态分布变换法)。
但是，该方法在确定位姿时，不仅会使用线、面这样特征明显的点，还会使用点、球这样无效特征较多的点，使得计算量较大、且定位精度降低。
发明内容
有鉴于此，本发明实施例提供一种位姿确定方法和装置，能够避免使用点、球等无效特征较多的点，进而降低计算量，提高定位精度。
第一方面，本发明实施例提供了一种位姿确定方法，包括：
将参考点云划分至多个网格；
计算所述网格中参考点云在正态分布下的均值和协方差矩阵；
对所述协方差矩阵进行特征分解，得到所述网格的特征值；
基于当前位姿对目标点云进行变换，得到所述目标点云所在的网格；
根据所述网格的特征值，在所述目标点云所在的网格中确定有效网格；其中，所述有效网格，由分布特征为面的面网格和/或分布特征为线的线网格构成；
根据所述均值、所述协方差矩阵和所述有效网格中的目标点云，计算目标函数的值，当满足预设的终止条件时，确定所述当前位姿为目标位姿，否则，基于所述目标函数的值更新所述当前位姿，并执行所述基于当前位姿对目标点云进行变换。
第二方面，本发明实施例提供了一种位姿确定装置，包括：
划分模块，配置为将参考点云划分至多个网格；计算所述网格中参考点云在正态分布下的均值和协方差矩阵；
特征分解模块，配置为对所述协方差矩阵进行特征分解，得到所述网格的特征值；
变换模块，配置为基于当前位姿对目标点云进行变换，得到所述目标点云所在的网格；
确定模块，配置为根据所述网格的特征值，在所述目标点云所在的网格中确定有效网格；其中，所述有效网格，由分布特征为面的面网格和/或分布特征为线的线网格构成；根据所述均值、所述协方差矩阵和所述有效网格中的目标点云，计算目标函数的值，当满足预设的终止条件时，确定所述当前位姿为目标位姿，否则，基于所述目标函数的值更新所述当前位姿，并执行所述基于当前位姿对目标点云进行变换。
第三方面，本发明实施例提供了一种电子设备，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序，
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如上述任一实施例所述的方法。
第四方面，本发明实施例提供了一种计算机可读介质，其上存储有计算机程序，所述程序被处理器执行时实现如上述任一实施例所述的方法。
上述发明中的一个实施例具有如下优点或有益效果：通过协方差矩阵的特征分解，在目标点云所在的网格中筛选出线网格和面网格，仅使用特征明显的线网格和面网格进行匹配，避免使用点、球等无效特征较多的点，能够降低计算量，提高定位精度。
上述的非惯用的可选方式所具有的进一步效果将在下文中结合具体实施方式加以说明。
附图说明
附图用于更好地理解本发明，不构成对本发明的不当限定。其中：
图1是本发明的一个实施例提供的一种位姿确定方法的流程图；
图2是本发明的另一个实施例提供的一种位姿确定方法的流程图；
图3是本发明的一个实施例提供的一种位姿确定装置的示意图；
图4是适于用来实现本发明实施例的终端设备或服务器的计算机系统的结构示意图。
具体实施方式
以下结合附图对本发明的示范性实施例做出说明，其中包括本发明实施例的各种细节以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施例做出各种改变和修改，而不会背离本发明的范围和精神。同样，为了清楚和简明，以下的描述中省略了对公知功能和结构的描述。
现有方法在确定位姿时会使用点、球这样无效特征较多的点，使得计算量较大、且定位精度较低。
鉴于此，如图1所示，本发明实施例提供了一种位姿确定方法，包括：
步骤101：将参考点云划分至多个网格。
参考点云即预先建立的点云地图。网格的尺寸越小，定位精度越高，但计算量越大，网格的尺寸越大，计算量降低，但定位精度较低。因此，在实际应用场景中，可以根据业务需求确定网格的尺寸。
步骤102：计算网格中参考点云在正态分布下的均值和协方差矩阵。
根据网格中参考点云，计算网格的正态分布参数，包括均值和协方差矩阵。
步骤103：对协方差矩阵进行特征分解，得到网格的特征值。
特征值与特征向量的数量与具体的业务场景相关。例如，在三维场景下，可以分解得到3个特征值和3个特征向量，在二维场景下，可以分解得到2个特征值和2个特征向量。因此，可以根据场景的需求，采用不同的方式对协方差矩阵进行特征分解，为了描述方便，后续实施例以三维场景为例进行说明。
步骤104：基于当前位姿对目标点云进行变换，得到目标点云所在的网格。
第一次变换使用的是预先估计的初始位姿，后续迭代过程将对初始位姿进行更新，直至满足终止条件。需要说明的是，各步骤的执行顺序并不唯一，例如，步骤104可以在步骤103之后执行，也可以在步骤102之前执行。
步骤105：根据网格的特征值，在目标点云所在的网格中确定有效网格；其中，有效网格，由分布特征为面的面网格和/或分布特征为线的线网格构成。
特征值能够反应点云在特征向量方向上的分布特征，因此，基于特征值及其对应的特征向量，能够筛选出线网格和面网格。
步骤106：根据均值、协方差矩阵和有效网格中的目标点云，计算目标函数的值。
步骤107：确定是否满足预设的终止条件，如果是，执行步骤108，否则，基于目标函数的值更新当前位姿，并执行步骤104。
步骤108：确定当前位姿为目标位姿。
终止条件可以是迭代次数达到一定的阈值，还可以是目标函数的值达到设定的阈值，本发明实施例对此不做限定。
本发明实施例通过协方差矩阵的特征分解，在目标点云所在的网格中筛选出线网格和面网格，仅使用特征明显的线网格和面网格进行匹配，避免使用点、球等无效特征较多的点，能够降低计算量，提高定位精度。
为了保证定位精确性，优选地，网格中点的数量一般不小于6。
在本发明的一个实施例中，对协方差矩阵进行特征分解，得到网格的特征值和特征值对应的特征向量，包括：
基于第一公式对协方差矩阵进行特征分解，得到网格的特征值和特征值对应的特征向量；
第一公式包括：
Figure BDA0004026436460000041
其中，Σ用于表征协方差矩阵，λ1、λ2和λ3分别用于表征特征值，q1、q2和q3分别用于表征与特征值对应的特征向量，q1、q2和q3两两正交。
为了保证线网格和面网格识别的精确度，特征向量两两正交。通过本发明实施例，可以获得三维场景下的特征值及特征向量，类似地，还可以获得其他维度场景下的特征值和特征向量。
在本发明的一个实施例中，根据网格的特征值，在目标点云所在的网格中确定有效网格，包括：
根据网格的特征值及特征值对应的特征向量，计算目标点云所在的各个网格的线特征值和面特征值；
根据线特征值和面特征值，在目标点云所在的多个网格中确定线网格和面网格；
在目标点云所在的线网格和面网格中，确定有效网格。
本发明实施例可以通过特征值从多个网格中筛选出线网格和面网格，相比于点、球形式的点云分布，线网格和面网格中的点云具有更加显著的特征，能够提高位姿的准确度。另外，筛除点、球形分布点云，能够降低计算量，提高位姿确定效率。
考虑到只有地面点时，无法对水平方向具有良好的约束，在本发明的一个实施例中，
对协方差矩阵进行特征分解，得到网格的特征值，包括：
对协方差矩阵进行特征分解，得到网格的特征值和特征值对应的特征向量；
在目标点云所在的线网格和面网格中，确定有效网格，包括：
根据面网格的特征值及特征值对应的特征向量，将目标点云所在的面网格划分为地面网格和非地面网格；
在目标点云所在的线网格、地面网格和非地面网格中，确定有效网格。
本发明实施例通过特征值确定网格属于线网格还是面网格，并基于特征值和特征向量，进一步将面网格划分为地面网格和非地面网格，从而保证有效网格中不仅包含线网格和地面网格，还包含非地面网格，能够提高定位精度。需要说明的是，上述确定有效网格的方法不仅适用于三维场景下，还适用于二维场景等其他场景中。
在实际应用场景中，可以先对目标点云进行变换，再根据网格的特征值确定网格的类型，还可以先根据网格的特征值确定网格的类型，再对目标点云进行变换。
在本发明的一个实施例中，根据网格的特征值，计算目标点云所在的各个网格的线特征值和面特征值，包括：
根据第二公式，计算目标点云所在的各个网格的线特征值；
根据第三公式，计算目标点云所在的各个网格的面特征值；
第二公式包括：
Figure BDA0004026436460000061
第三公式包括：
Figure BDA0004026436460000062
其中，c用于表征线特征值，p用于表征面特征值，λ1、λ2和λ3分别用于表征特征值，λ1≤λ2≤λ3。
其中，0≤c<1，c值越大，分布特征越趋向于线，0≤p<1，p值越大，分布特征越趋向于面。
本发明通过λ1、λ2和λ3能够准确描述网格在特征向量方向上的分布特征，进而准确划分网格的类型，提高定位精度。
在实际应用场景中，可以通过c值和p值大小排序，筛选出线网格和面网格，还可以通过预设的阈值筛选出线网格和面网格，例如，c值大于0.8的网格为线网格。另外，可以先计算网格的c值，从多个网格中筛选出线网格，再计算剩余网格的p值，筛选出面网格，反之亦可。当然，也可以既计算网格的c值，也计算网格的p值，通过比较c值和p值确定网格的类型。c值和p值的计算方法并不局限于式(2)和式(3)，例如，c＝λ3-λ2，
Figure BDA0004026436460000063
p值类似，p＝λ2-λ1，
Figure BDA0004026436460000064
在本发明的一个实施例中，根据线特征值和面特征值，在目标点云所在的多个网格中确定线网格和面网格，包括：
当线特征值大于预设的第一阈值时，确定目标点云所在的网格为线网格，当面特征阈值大于预设的第二阈值时，确定目标点云所在的网格为面网格。
考虑到线网格的线特征值较大，面网格的面特征值较大，因此，本发明实施例通过预设的第一阈值和第二阈值确定网格的类型。如果网格的线特征值大于第一阈值且面特征值大于第二阈值，则可以将网格作为线网格，也可以作为面网格。在实际应用场景中，还可以按照由大到小的顺序，将线特征值排在前第一百分比的网格作为线网格，将面特征值排在前第二百分比的网格作为面网格。
本发明实施例能够综合考虑网格的线特征和面特征，提高网格划分的准确性。
在本发明的一个实施例中，根据面网格的特征值及特征值对应的特征向量，将目标点云所在的面网格划分为地面网格和非地面网格，包括：
计算λ1对应的特征向量与地面法向量的夹角，当夹角小于设定的角度阈值时，面网格为地面网格，否则，面网格为非地面网格。
由于λ1≤λ2≤λ3，因此，λ1对应的特征向量是面网格的法向量，通过计算其与地面法向量的夹角，可以确定面网格是否为地面网格。具体地，可以将λ1对应的特征向量与向量(0,0,1)点乘，再除以λ1的模长，从而确定面网格是否为地面网格。在实际应用场景中，还可以计算面网格的平面方程，基于面网格的平面方程和地面方程，确定面网格是否为地面网格。
通过本发明实施例，可以进一步将面网格划分为地面网格和非地面网格，保证有效网格中包含非地面网格，避免只有地面点时水平方向无法有效约束。
在本发明的一个实施例中，在目标点云所在的线网格、地面网格和非地面网格中，确定有效网格，包括：
按照线特征值由大到小的顺序，在目标点云所在的线网格中选择第一数量的线网格作为有效网格；
按照面特征值由大到小的顺序，在目标点云所在的地面网格中选择第二数量的地面网格作为有效网格；
按照面特征值由大到小的顺序，在目标点云所在的非地面网格中选择第三数量的非地面网格作为有效网格。
考虑到只有地面点无法对水平方向起到很好的约束，本发明实施例确定的有效网格包括线网格、地面网格和非地面网格。考虑到c值越大越趋近于线，p值越大越趋近于面，因此，本发明实施例按照线特征值和面特征值由大到小的顺序选择有效网格。在实际应用场景中，还可以将全部的线网格和面网格作为有效网格。
本发明实施例通过网格筛选，进一步减少计算量，提到定位精度。
优选地，第二数量与第三数量相同，第一数量等于第二数量与第三数量之和。
在实际应用场景中，第一数量、第二数量与第三数量还可以为其他的比例关系，如1:1:1。
在本发明的一个实施例中，目标函数包括：
Figure BDA0004026436460000081
其中，score(Pvehicle)用于表征当前位姿的得分，tki用于表征第k个有效网格的目标点云中第i个点的坐标，μk用于表征第k个有效网格中参考点云在正态分布下的均值，∑k用于表征第k个有效网格的参考点云在正态分布下的协方差矩阵，Mk用于表征第k个有效网格的目标点云中点的数量。
在本发明实施例中，目标函数中包括有效网格中参考点云在正态分布下的均值和协方差矩阵，以及有效网格中目标点云中点的坐标。
在实际应用场景中，可以采用牛顿法、梯度下降法等非线性优化方法对-score(Pvehicle)进行优化，寻找使score(Pvehicle)最大的Pvehicle。
目标函数为正态分布的概率密度函数或其变形。例如，目标函数还可以为式(5)。
Figure BDA0004026436460000082
本发明实施例在优化的过程中，仅使用有效网格，避免使用无效特征较多的点或球形点云，能够提高定位精度和效率。
如图2所示，本发明实施例以自动驾驶车辆为例，对三维场景下的位姿确定方法进行说明，该方法包括：
步骤201：将参考点云划分至多个网格。
通过自动驾驶车辆的车载激光雷达，对需要进行定位的区域进行扫描，得到参考点云。将参考点云划分成50cm×50cm×50cm的网格。
步骤202：计算网格中参考点云在正态分布下的均值和协方差矩阵。
针对点数不小于6的网格，分别计算各个网格中的参考点云在正态分布下的坐标均值，并基于坐标均值计算协方差矩阵。
步骤203：对协方差矩阵进行特征分解，得到网格的特征值和特征值对应的特征向量。
基于式(1)对协方差矩阵进行特征分解，得到网格的特征值λ1、λ2和λ3，以及与特征值对应的特征向量q1、q2和q3，q1、q2和q3两两正交。
步骤204：基于当前位姿对目标点云进行变换，得到目标点云所在的网格。
当扫描得到目标点云时，根据预先估计的初始车辆位姿或上一次迭代确定的当前位姿对目标点云进行变换，得到目标点云所在的网格。
步骤205：根据网格的特征值，计算目标点云所在的各个网格的线特征值和面特征值。
基于式(2)和式(3)，计算网格的c值和p值。
步骤206：当线特征值大于预设的第一阈值时，确定目标点云所在的网格为线网格，当面特征阈值大于预设的第二阈值时，确定目标点云所在的网格为面网格。
步骤207：计算λ1对应的特征向量与地面法向量的夹角，当夹角小于设定的角度阈值时，面网格为地面网格，否则，面网格为非地面网格。
步骤208：按照线特征值由大到小的顺序，在目标点云所在的线网格中选择第一数量的线网格作为有效网格。
按照c值由大到小的顺序，选择h个线网格作为有效网格。
步骤209：按照面特征值由大到小的顺序，在目标点云所在的地面网格中选择第二数量的地面网格作为有效网格。
按照p值由大到小的顺序，选择u1个地面网格作为有效网格。
步骤210：按照面特征值由大到小的顺序，在目标点云所在的非地面网格中选择第三数量的非地面网格作为有效网格。
按照p值由大到小的顺序，选择u2个非地面网格作为有效网格。
u1＝u2，h＝u1+u2。
步骤211：根据均值、协方差矩阵和有效网格中的目标点云，计算目标函数的值。
步骤212：确定是否满足终止条件，如果是，执行步骤213，否则，基于目标函数的值更新当前位姿，执行步骤204。
步骤213：确定当前位姿为目标位姿。
将选定的有效网格作为最终参与匹配的网格，基于式(4)的目标函数进行优化迭代，得到最终车辆位姿。
通过本发明实施例可以筛除一些影响定位精度的点云，如树冠等，选取灯杆、树干和墙面等分布特征为线和面的点云，不仅提高了计算效率，还提高了定位精度。
如图3所示，本发明实施例提供了一种位姿确定装置，包括：
划分模块301，配置为将参考点云划分至多个网格；计算网格中参考点云在正态分布下的均值和协方差矩阵；
特征分解模块302，配置为对协方差矩阵进行特征分解，得到网格的特征值；
变换模块303，配置为基于当前位姿对目标点云进行变换，得到目标点云所在的网格；
确定模块304，配置为根据网格的特征值，在目标点云所在的网格中确定有效网格；其中，有效网格，由分布特征为面的面网格和/或分布特征为线的线网格构成；根据均值、协方差矩阵和有效网格中的目标点云，计算目标函数的值，当满足预设的终止条件时，确定当前位姿为目标位姿，否则，基于目标函数的值更新当前位姿，并触发变换模块303。
在本发明的一个实施例中，特征分解模块302，配置为基于第一公式对协方差矩阵进行特征分解，得到网格的特征值和特征值对应的特征向量；
第一公式包括：
Figure BDA0004026436460000101
其中，Σ用于表征协方差矩阵，λ1、λ2和λ3分别用于表征特征值，q1、q2和q3分别用于表征与特征值对应的特征向量，q1、q2和q3两两正交。
在本发明的一个实施例中，确定模块304，配置为根据网格的特征值，计算目标点云所在的各个网格的线特征值和面特征值；根据线特征值和面特征值，在目标点云所在的多个网格中确定线网格和面网格；在目标点云所在的线网格和面网格中，确定有效网格。
在本发明的一个实施例中，特征分解模块302，配置为对协方差矩阵进行特征分解，得到网格的特征值和特征值对应的特征向量；确定模块304，配置为根据面网格的特征值及特征值对应的特征向量，将目标点云所在的面网格划分为地面网格和非地面网格；在目标点云所在的线网格、地面网格和非地面网格中，确定有效网格。
在本发明的一个实施例中，确定模块304，配置为根据第二公式，计算目标点云所在的各个网格的线特征值；根据第三公式，计算目标点云所在的各个网格的面特征值；
第二公式包括：
Figure BDA0004026436460000111
第三公式包括：
Figure BDA0004026436460000112
其中，c用于表征线特征值，p用于表征面特征值，λ1、λ2和λ3分别用于表征特征值，λ1≤λ2≤λ3。
在本发明的一个实施例中，确定模块304，配置为当线特征值大于预设的第一阈值时，确定目标点云所在的网格为线网格，当面特征阈值大于预设的第二阈值时，确定目标点云所在的网格为面网格。
在本发明的一个实施例中，确定模块304，配置为计算λ1对应的特征向量与地面法向量的夹角，当夹角小于设定的角度阈值时，面网格为地面网格，否则，面网格为非地面网格。
在本发明的一个实施例中，确定模块304，配置为按照线特征值由大到小的顺序，在目标点云所在的线网格中选择第一数量的线网格作为有效网格；按照面特征值由大到小的顺序，在目标点云所在的地面网格中选择第二数量的地面网格作为有效网格；按照面特征值由大到小的顺序，在目标点云所在的非地面网格中选择第三数量的非地面网格作为有效网格。
在本发明的一个实施例中，目标函数包括：
Figure BDA0004026436460000113
其中，score(Pvehicle)用于表征当前位姿的得分，tki用于表征第k个有效网格的目标点云中第i个点的坐标，μk用于表征第k个有效网格中参考点云在正态分布下的均值，∑k用于表征第k个有效网格的参考点云在正态分布下的协方差矩阵，Mk用于表征第k个有效网格的目标点云中点的数量。
本发明实施例提供了一种电子设备，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序，
当一个或多个程序被一个或多个处理器执行，使得一个或多个处理器实现如上述任一实施例的方法。
本发明实施例提供了一种计算机可读介质，其上存储有计算机程序，程序被处理器执行时实现如上述任一实施例的方法。
下面参考图4，其示出了适于用来实现本发明实施例的终端设备的计算机系统400的结构示意图。图4示出的终端设备仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。
如图4所示，计算机系统400包括中央处理单元(CPU)401，其可以根据存储在只读存储器(ROM)402中的程序或者从存储部分408加载到随机访问存储器(RAM)403中的程序而执行各种适当的动作和处理。在RAM403中，还存储有系统400操作所需的各种程序和数据。CPU 401、ROM 402以及RAM 403通过总线404彼此相连。输入/输出(I/O)接口405也连接至总线404。
以下部件连接至I/O接口405：包括键盘、鼠标等的输入部分406；包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等的输出部分407；包括硬盘等的存储部分408；以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分409。通信部分409经由诸如因特网的网络执行通信处理。驱动器410也根据需要连接至I/O接口405。可拆卸介质411，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器410上，以便于从其上读出的计算机程序根据需要被安装入存储部分408。
特别地，根据本发明公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本发明公开的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分409从网络上被下载和安装，和/或从可拆卸介质411被安装。在该计算机程序被中央处理单元(CPU)401执行时，执行本发明的系统中限定的上述功能。
需要说明的是，本发明所示的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本发明中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本发明中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、RF等等，或者上述的任意合适的组合。
附图中的流程图和框图，图示了按照本发明各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，上述模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图或流程图中的每个方框、以及框图或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本发明实施例中所涉及到的模块可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的模块也可以设置在处理器中，例如，可以描述为：一种处理器包括发送模块、获取模块、确定模块和第一处理模块。其中，这些模块的名称在某种情况下并不构成对该模块本身的限定，例如，发送模块还可以被描述为“向所连接的服务端发送图片获取请求的模块”。
上述具体实施方式，并不构成对本发明保护范围的限制。本领域技术人员应该明白的是，取决于设计要求和其他因素，可以发生各种各样的修改、组合、子组合和替代。任何在本发明的精神和原则之内所作的修改、等同替换和改进等，均应包含在本发明保护范围之内。

Claims (10)

1.一种位姿确定方法，其特征在于，包括：
将参考点云划分至多个网格；
计算所述网格中参考点云在正态分布下的均值和协方差矩阵；
对所述协方差矩阵进行特征分解，得到所述网格的特征值；
基于当前位姿对目标点云进行变换，得到所述目标点云所在的网格；
根据所述网格的特征值，在所述目标点云所在的网格中确定有效网格；其中，所述有效网格，由分布特征为面的面网格和/或分布特征为线的线网格构成；
根据所述均值、所述协方差矩阵和所述有效网格中的目标点云，计算目标函数的值，当满足预设的终止条件时，确定所述当前位姿为目标位姿，否则，基于所述目标函数的值更新所述当前位姿，并执行所述基于当前位姿对目标点云进行变换。
2.如权利要求1所述的方法，其特征在于，
根据所述网格的特征值，在所述目标点云所在的网格中确定有效网格，包括：
根据所述网格的特征值，计算所述目标点云所在的各个网格的线特征值和面特征值；
根据所述线特征值和所述面特征值，在所述目标点云所在的多个网格中确定所述线网格和所述面网格；
在所述目标点云所在的线网格和面网格中，确定所述有效网格。
3.如权利要求2所述的方法，其特征在于，
对所述协方差矩阵进行特征分解，得到所述网格的特征值，包括：
对所述协方差矩阵进行特征分解，得到所述网格的特征值和所述特征值对应的特征向量；
在所述目标点云所在的线网格和面网格中，确定所述有效网格，包括：
根据所述面网格的特征值及所述特征值对应的特征向量，将所述目标点云所在的面网格划分为地面网格和非地面网格；
在所述目标点云所在的线网格、地面网格和非地面网格中，确定所述有效网格。
4.如权利要求3所述的方法，其特征在于，
对所述协方差矩阵进行特征分解，得到所述网格的特征值和所述特征值对应的特征向量，包括：
基于第一公式对所述协方差矩阵进行特征分解，得到所述网格的特征值和所述特征值对应的特征向量；
所述第一公式包括：
Figure FDA0004026436450000021
其中，Σ用于表征所述协方差矩阵，λ1、λ2和λ3分别用于表征所述特征值，q1、q2和q3分别用于表征与所述特征值对应的特征向量，q1、q2和q3两两正交；
根据所述网格的特征值，计算所述目标点云所在的各个网格的线特征值和面特征值，包括：
根据第二公式，计算所述目标点云所在的各个网格的线特征值；
根据第三公式，计算所述目标点云所在的各个网格的面特征值；
所述第二公式包括：
Figure FDA0004026436450000022
所述第三公式包括：
Figure FDA0004026436450000023
其中，c用于表征所述线特征值，p用于表征所述面特征值，λ1、λ2和λ3分别用于表征所述特征值，λ1≤λ2≤λ3。
5.如权利要求4所述的方法，其特征在于，
根据所述线特征值和所述面特征值，在所述目标点云所在的多个网格中确定所述线网格和所述面网格，包括：
当所述线特征值大于预设的第一阈值时，确定所述目标点云所在的网格为线网格，当所述面特征阈值大于预设的第二阈值时，确定所述目标点云所在的网格为面网格。
6.如权利要求4所述的方法，其特征在于，
根据所述面网格的特征值及所述特征值对应的特征向量，将所述目标点云所在的面网格划分为地面网格和非地面网格，包括：
计算λ1对应的特征向量与地面法向量的夹角，当所述夹角小于设定的角度阈值时，所述面网格为地面网格，否则，所述面网格为非地面网格。
7.如权利要求4所述的方法，其特征在于，
在所述目标点云所在的线网格、地面网格和非地面网格中，确定所述有效网格，包括：
按照所述线特征值由大到小的顺序，在所述目标点云所在的线网格中选择第一数量的线网格作为所述有效网格；
按照所述面特征值由大到小的顺序，在所述目标点云所在的地面网格中选择第二数量的地面网格作为所述有效网格；
按照所述面特征值由大到小的顺序，在所述目标点云所在的非地面网格中选择第三数量的非地面网格作为所述有效网格。
8.如权利要求1所述的方法，其特征在于，
所述目标函数包括：
Figure FDA0004026436450000031
其中，score(Pvehicle)用于表征所述当前位姿的得分，
Figure FDA0004026436450000032
用于表征第k个有效网格的目标点云中第i个点的坐标，μk用于表征第k个有效网格中参考点云在正态分布下的均值，∑k用于表征第k个有效网格的参考点云在正态分布下的协方差矩阵，Mk用于表征第k个有效网格的目标点云中点的数量。
9.一种位姿确定装置，其特征在于，包括：
划分模块，配置为将参考点云划分至多个网格；计算所述网格中参考点云在正态分布下的均值和协方差矩阵；
特征分解模块，配置为对所述协方差矩阵进行特征分解，得到所述网格的特征值；
变换模块，配置为基于当前位姿对目标点云进行变换，得到所述目标点云所在的网格；
确定模块，配置为根据所述网格的特征值，在所述目标点云所在的网格中确定有效网格；其中，所述有效网格，由分布特征为面的面网格和/或分布特征为线的线网格构成；根据所述均值、所述协方差矩阵和所述有效网格中的目标点云，计算目标函数的值，当满足预设的终止条件时，确定所述当前位姿为目标位姿，否则，基于所述目标函数的值更新所述当前位姿，并触发变换模块。
10.一种电子设备，其特征在于，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序，
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-8中任一所述的方法。
CN202211711839.7A 2022-12-29 2022-12-29 一种位姿确定方法和装置 Pending CN116051635A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211711839.7A CN116051635A (zh) 2022-12-29 2022-12-29 一种位姿确定方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211711839.7A CN116051635A (zh) 2022-12-29 2022-12-29 一种位姿确定方法和装置

Publications (1)

Publication Number Publication Date
CN116051635A true CN116051635A (zh) 2023-05-02

Family

ID=86130665

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211711839.7A Pending CN116051635A (zh) 2022-12-29 2022-12-29 一种位姿确定方法和装置

Country Status (1)

Country Link
CN (1) CN116051635A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116968022A (zh) * 2023-07-14 2023-10-31 武汉纺织大学 一种基于视觉引导的机械臂抓取目标物体方法及系统
CN117011486A (zh) * 2023-09-11 2023-11-07 腾讯科技（深圳）有限公司 栅格地图构建的方法、装置、电子设备和计算机存储介质
CN117421434A (zh) * 2023-09-08 2024-01-19 广州汽车集团股份有限公司 点云定位地图的存储方法、装置、电子设备及存储介质

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116968022A (zh) * 2023-07-14 2023-10-31 武汉纺织大学 一种基于视觉引导的机械臂抓取目标物体方法及系统
CN116968022B (zh) * 2023-07-14 2024-05-28 武汉纺织大学 一种基于视觉引导的机械臂抓取目标物体方法及系统
CN117421434A (zh) * 2023-09-08 2024-01-19 广州汽车集团股份有限公司 点云定位地图的存储方法、装置、电子设备及存储介质
CN117011486A (zh) * 2023-09-11 2023-11-07 腾讯科技（深圳）有限公司 栅格地图构建的方法、装置、电子设备和计算机存储介质
CN117011486B (zh) * 2023-09-11 2024-01-09 腾讯科技（深圳）有限公司 栅格地图构建的方法、装置、电子设备和计算机存储介质

Similar Documents

Publication Publication Date Title
CN116051635A (zh) 一种位姿确定方法和装置
CN109545072B (zh) 地图构建的位姿计算方法、装置、存储介质和系统
CN109146976B (zh) 用于定位无人车的方法和装置
US20160377438A1 (en) System and method for grid-based geo-fencing service
US20160133048A1 (en) Method and Apparatus for Shadow Estimation and Spreading
CN110139359B (zh) 一种干扰源定位处理方法及装置
CN112652036A (zh) 道路数据的处理方法、装置、设备及存储介质
Lund et al. Models for point processes observed with noise
WO2023165220A1 (zh) 一种目标物体的检测方法和装置
CN108986218A (zh) 一种基于pmvs的建筑物密集点云快速重建方法
CN113936046A (zh) 一种物体定位方法、装置、电子设备及计算机可读介质
CN114926549B (zh) 三维点云处理方法、装置、设备以及存储介质
CN115797565A (zh) 三维重建模型训练方法、三维重建方法、装置及电子设备
CN116189147A (zh) 一种基于yolo的三维点云低功耗快速目标检测方法
JP4217251B2 (ja) 三次元構造物形状の自動生成装置、自動生成方法、そのプログラム、及びそのプログラムを記録した記録媒体
CN115311418B (zh) 一种多细节层次树木模型单体化重建方法及装置
US20230036294A1 (en) Method for processing image, electronic device and storage medium
CN116030131A (zh) 一种位姿确定方法和装置
CN116912321A (zh) 道路平面的确定方法、装置、设备、介质和车辆
CN114463512B (zh) 点云数据的处理方法、矢量化方法及装置
US20220132270A1 (en) Evaluation of device placement
CN113670253A (zh) 空间目标姿态反演方法、装置、计算设备及存储介质
CN113761090A (zh) 一种基于点云地图的定位方法和装置
CN114419250B (zh) 点云数据矢量化方法及装置、矢量地图生成方法及装置
CN113408835B (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