CN110068824A

CN110068824A - 一种传感器位姿确定方法和装置

Info

Publication number: CN110068824A
Application number: CN201910310529.6A
Authority: CN
Inventors: 杨帅
Original assignee: Beijing Horizon Robotics Technology Research and Development Co Ltd
Current assignee: Beijing Horizon Robotics Technology Research and Development Co Ltd
Priority date: 2019-04-17
Filing date: 2019-04-17
Publication date: 2019-07-30
Anticipated expiration: 2039-04-17
Also published as: CN110068824B

Abstract

公开了一种传感器位姿确定方法、装置、计算机可读存储介质及电子设备，传感器位姿确定方法包括：获取传感器的当前位姿信息；获取实时三维点云的第一点集中每一个点在预设三维点云地图的第二点集中的对应最近点，组成第三点集；根据所述第一点集和所述第三点集，以及所述第一点集中每一个点及所述第三点集每一个点的语义类别，获取所述第一点集对应的第四点集；根据所述第四点集与所述第三点集，确定最终位姿信息。根据本公开的技术方案，实现了传感器的观测信息与预设三维点云地图之间的精确匹配。

Description

一种传感器位姿确定方法和装置

技术领域

本申请涉及视觉定位技术领域，且更具体地，涉及一种传感器位姿确定方法和装置。

背景技术

随着互联网技术的快速发展，地图导航成为了人们日常出行必不可少的工具，尤其是是车辆在行驶时，通常都需要开启地图进行导航。其中，由于高精度地图中包含非常丰富的细节信息，这对于车辆的导航、定位以及控制等都具有非常重要的作用，因此越来越受到市场的青睐。不仅如此，车辆在行驶的过程中需要实时定位，以便确定其在地图上的实时位置，保证车辆的行驶安全。

由于激光雷达具有较高的精确度，因此被广泛应用于高精度地图的构建以及车辆的定位中。然而，激光雷达的价格非常昂贵，因此将激光雷达应用于车辆的定位时，会极大增加车辆成本。

发明内容

为了解决上述技术问题，提出了本申请。本申请的实施例提供了一种传感器位姿确定方法、装置、计算机可读存储介质及电子设备，其实现了传感器的观测信息与预设三维点云地图之间的精确匹配。

根据本申请的第一方面，提供了一种传感器位姿确定方法，包括：

获取传感器的当前位姿信息；

获取实时三维点云的第一点集P中每一个点P_i在预设三维点云地图的第二点集Q中的对应最近点Q_i，组成第三点集Q'；

根据所述第一点集P和所述第三点集Q'，以及所述第一点集P中每一个点P_i及所述第三点集Q'每一个点Q_i的语义类别，获取所述第一点集P对应的第四点集P'；

根据所述第四点集P'与所述第三点集Q'，确定最终位姿信息。

根据本申请的第二方面，提供了一种传感器位姿确定装置，包括：

当前位姿获取模块，用于获取传感器的当前位姿信息；

第一点集确定模块，用于获取实时三维点云的第一点集P中每一个点P_i在预设三维点云地图的第二点集Q中的对应最近点Q_i，组成第三点集Q'；

第二点集确定模块，用于根据所述第一点集P和所述第三点集Q'，以及第一点集P中每一个点P_i及第三点集Q'每一个点Q_i的语义类别，获取所述第一点集P对应的第四点集P'；

最终位姿确定模块，用于根据所述第四点集P'与所述第三点集Q'，确定最终位姿信息。

根据本申请的第三方面，提供了一种计算机可读存储介质，所述存储介质存储有计算机程序，所述计算机程序用于执行上述的传感器位姿确定方法。

根据本申请的第四方面，提供了一种电子设备，所述电子设备包括：

处理器；

用于存储所述处理器可执行指令的存储器；

所述处理器，用于从所述存储器中读取所述可执行指令，并执行所述指令以实现上述的传感器位姿确定方法。

与现有技术相比，本申请提供的传感器位姿确定方法、装置、计算机可读存储介质及电子设备，至少包括以下有益效果：

(1)本实施例采用激光雷达预先建立三维点云地图，而在车辆的实际行驶过程中通过普通传感器(例如单目相机)来实时获取图像，并实时更新传感器的位姿信息，不仅有效实现了传感器所获取的实时图像与预设三维点云地图的之间的匹配，而且极大降低了车辆的制造成本。

(2)本实施例在将传感器获取的实时图像与预设三维点云地图进行匹配时，充分考虑了坐标与语义类别信息，使得两者之间的匹配结果更加精确。

附图说明

通过结合附图对本申请实施例进行更详细的描述，本申请的上述以及其他目的、特征和优势将变得更加明显。附图用来提供对本申请实施例的进一步理解，并且构成说明书的一部分，与本申请实施例一起用于解释本申请，并不构成对本申请的限制。在附图中，相同的参考标号通常代表相同部件或步骤。

图1是本申请一示例性实施例提供的传感器位姿确定方法的流程示意图。

图2是如图1所示的实施例中获取传感器的当前位姿信息步骤的流程示意图。

图3是如图1所示的实施例中获取第三点集步骤的流程示意图。

图4是如图1所示的实施例中获取第一点集对应的第四点集步骤的流程示意图。

图5是如图4所示的实施例中获取中间位姿信息步骤的流程示意图。

图6是如图5所示的实施例中根据第一点集与第三点集的距离差以及语义类别差，获取中间位姿信息步骤的流程示意图。

图7是如图1所示的实施例中确定最终位姿信息步骤的流程示意图。

图8是本申请一示例性实施例提供的传感器位姿确定装置的结构示意图一。

图9是本申请一示例性实施例提供的传感器位姿确定装置的结构示意图二。

图10是本申请一示例性实施例提供的电子设备的结构图。

具体实施方式

下面，将参考附图详细地描述根据本申请的示例实施例。显然，所描述的实施例仅仅是本申请的一部分实施例，而不是本申请的全部实施例，应理解，本申请不受这里描述的示例实施例的限制。

申请概述

车辆在行驶过程中通常都需要开启地图来进行导航，地图的精度越高，包含的细节信息通常也就越丰富，和对于车辆的导航、定位以及控制等都具有非常重要的作用。不仅如此，车辆在行驶的过程中需要实时定位，以便确定其在地图上的实时位置，保证车辆的形式安全，因而需要实时更新车辆的位置信息。激光雷达由于具有较高的精确度，因此被广泛应用于高精度地图的构建以及车辆的定位中。然而，激光雷达的价格非常昂贵，将激光雷达应用于车辆的实时定位时，会极大增加车辆成本。

除了激光雷达可以用于车辆的定位以外，一些传感器(例如普通的单目相机)也可以用于实时获取车辆行驶过程中的路面图像，从而可以用于获取车辆的实时位置信息。然而，由于普通相机无法直接获得图像深度信息，因此其用于建立地图的精度以及效果均无法与激光雷达相比。

考虑到车辆行驶过程中应用的地图需要较高的精度，而车辆在行驶过程中无须频繁更新地图，只需要更新车辆的实时位置信息即可，本发明实施例中应用激光雷达预先建立高精度、低更新频率的地图，而采用普通传感器(例如单目相机)完成车辆的实时定位。而采用该种方式的一大挑战在于如何实现传感器的观测信息与地图之间的匹配。有鉴于此，本发明实施例提出一种传感器位姿确定方法，以实现对传感器位姿的实时更新，从而可以实现传感器的观测信息与地图之间的匹配。

在介绍本申请的基本构思之后，下面将结合附图来具体介绍本申请所述提供技术方案的各种非限制性实施例。

示例性方法

本实施例可应用在电子设备上，具体可以应用于服务器或一般计算机上。如图2所示，本申请一示例性实施例提供的传感器位姿确定方法至少包括如下步骤：

步骤10：获取传感器的当前位姿信息。

为了获取车辆行驶过程中的路面信息，车辆上设置有传感器，利用传感器来对路面进行实时观测。传感器的类型可以根据需要进行设置，例如传感器可以是普通的单目相机，其成本低廉，同时可以通过对其拍摄频率进行控制，使得其能够以较快频率实时获取图像信息。

步骤20：获取实时三维点云的第一点集P中每一个点P_i在预设三维点云地图的第二点集Q中的对应最近点Q_i，组成第三点集Q'。

在本实施例中，传感器获取车辆行驶过程中的实时图像信息，并根据该实时图像信息，获取其对应的实时三维点云，该实时三维点云中每一个点P_i组成了第一点集P。根据传感器获取的实时图像信息不同，三维点云的第一点集P也不相同。

为了保证地图的精确度，本实施例通过离线的方式，使用激光雷达建立了高精度的预设三维点云地图，该预设三维点云地图可以预先存储于车辆的相关处理器中，在需要时直接进行调用即可。

在获得了实时图像信息对应的实时三维点云后，需要将实时三维点云与预设三维点云地图进行匹配，以使得第一点集P中每一个点P_i均能在第二点集Q中找到其对应最近点Q_i，所有的对应最近点组成第三点集Q'，此时即可认为第三点集Q'为预设三维点云地图中与第一点集P相匹配的点集，从而实现实时三维点云与预设三维点云地图的匹配。

步骤S30：根据所述第一点集P和所述第三点集Q'，以及所述第一点集P中每一个点P_i及所述第三点集Q'每一个点Q_i的语义类别，获取所述第一点集P对应的第四点集P'。

在本实施例中，在获取与第一点集P匹配的第三点集Q'时，不仅需要考虑坐标的匹配，还要考虑语义类别的匹配。预设三维点云地图中每一个点Q_i的信息至少包括该点的坐标信息以及语义类别信息。语义类别信息指的是三维点云地图中每一个点所可能属于的类别，类别包括但不限于地面、树、路灯等。在本实施例中，进行三维点云地图的构建时，每一个点的语义类别L_Qi可以是一个向量，该向量包括多个分量，每一个分量对应一个类别的概率，例如L_Qi＝[L_Qi1，L_Qi2，。。。L_Qin]，其中L_Qi1为点Q_i属于地面的概率，L_Qi1为点Q_i属于树的概率，。。。L_Qi1为点Q_i属于路灯的概率等。

当获得与第一点集P匹配的第三点集Q'时，可以同时获得摄像头坐标系与世界坐标系的实时坐标变换关系，进而根据该实时坐标变换关系，获得第一点集P对应的第四点集P'。

步骤S40：根据所述第四点集P'与所述第三点集Q'，确定最终位姿信息。

在本实施例中，在确定最终位姿信息时，可以是根据第四点集P'和第三点集Q'的位置关系来确定。当然，在其他实施例中，也可以是根据第四点集P'和第三点集Q'之间的其他关系来确定，并不仅限于上述的情形。根据传感器的最终位姿信息，可以实现传感器所获取的实时图像与预设三维点云地图的之间的匹配。

本实施例提供的传感器位姿确定方法的有益技术效果至少在于：

(2)本实施例在根据第一点集P获取第三点集Q'和第四点集P'时，不仅实现了传感器获取的实时图像与预设三维点云地图之间的匹配，而且由于充分考虑了第一点集P中每一个点P_i与第二点集Q中每一个点Q_i的坐标信息和语义类别信息，使得两者之间的匹配结果更加精确。

图2示出了如图1所示的实施例中获取传感器的当前位姿信息步骤的流程示意图。

如图2所示，在上述图1所示实施例的基础上，本申请一个示例性实施例中，步骤10所示获取传感器的当前位姿信息步骤，具体可以包括如下步骤：

步骤11：传感器采集实时图像信息。

在本实施例中，传感器可选为单目相机，其可以是朝向车辆行驶前方，用于获取车辆行驶前方的实时图像信息。当然，在其他实施例中，传感器也可以为其他类型，只要其能够用于获取实时图像信息即可，此处不做限制。

单目相机获取实时图像信息的频率可以是固定值，也可以进行动态调整，例如当车辆进行高速行驶时，需要快速更新车辆的实时位置，此时可以将单目相机的拍摄频率设置为高频。而当车辆行驶的速度较慢时，则不需要快速更新车辆的实时位置，此时可以将单目相机的拍摄频率调低，降低其工作负荷。

步骤12：通过逆透视变换获取当前传感器坐标系下所述实时图像信息对应的三维点云，获得所述传感器的当前位姿信息。

单目相机所获取的实时图像中每一个点的坐标都是以摄像头坐标系进行标注的，而摄像头坐标系与世界坐标系并不相同，因此需要在摄像头坐标系和世界坐标系之间建立坐标变换关系，以便将单目相机获取的图像中每一点通过该坐标变换关系转换至世界坐标系中。而该坐标变换关系则与传感器的位姿信息直接相关，因此获取了摄像头坐标系与世界坐标系的坐标变换关系，就可以获得传感器的位姿信息。

为了获取实时图像对应的三维点云，本实施例采用逆透视变换建立摄像头坐标系与世界坐标系之间的关系，从而使得实时图像中的一个点转换为三维点云中对应的一个点。

在本实施例中，在通过逆透视变换获取当前传感器坐标系下实时图像对应的三维点云时，进行转换的点可以是单目相机所获得的实时图像中的所有点，也可以只是实时图像中的部分点，可根据需要进行确定。例如，单目相机拍摄的实时图像中包括路面图像，在进行转换时，可以只对路面区域对应的图像进行逆透视变换，获得对应的路面三维点云，有效提高逆透视变换的效率。当然，在其他实施例中，进行逆透视变换的也可以是实时图像中的其他感兴趣区域，并不仅限于上述的情形。

在本实施例中，传感器的当前位姿信息包括当前旋转矩阵R₀和当前平移矩阵t₀，其可以是车辆完成上一次重定位时记录下的位姿信息，也可以是车辆启动时预设的位姿信息。根据传感器的当前位姿信息，可以获得当前传感器与车辆之间坐标系的相互转换关系。因此，通过实时更新传感器的位姿信息，可以实时更新传感器与车辆之间坐标系的相互转换关系。

本实施例通过普通传感器采集实时图像信息，并通过逆透视变换获取实时图像信息对应的三维点云，可以获得实时图像中感兴趣的区域对应的三维点云，可以有效提高逆透视变换的效率。同时，传感器的当前位姿信息可以作为后续传感器位姿信息确定过程中的初始值，有助于更快、更准确地实现传感器的观测信息与预设三维点云地图之间的匹配。

图3示出了如图1所示的实施例中获取第三点集Q'步骤的流程示意图。

如图3所示，在上述图1所示实施例的基础上，本申请一个示例性实施例中，步骤20所示获取第三点集Q'步骤，具体可以包括如下步骤：

步骤21：获取所述第一点集P中每一个点P_i与所述第二点集Q中一个点Q_i的距离差之和，组成点集距离差。

具体地，首先，在第一点集P中选取一个点P_i，对应在第二点集Q中寻找一个点Q_i，计算两个点P_i和Q_i之间的距离差||Q_i-P_i||。

然后依次地，选取第一点集P中的其他点P_i，对于每一个点P_i，均在第二点集Q中对应寻找一个点Q_i，计算两个点P_i和Q_i之间的距离差||Q_i-P_i||，且每一个点P_i对应不同的一个点Q_i。

将上述第一点集P中所有点P_i对应的距离差||Q_i-P_i||相加，组成点集距离差。

在本实施例中，在获取点集距离差时，可以是只考虑第一点集P中每一个点P_i的坐标与第二点集Q中一个点Q_i的坐标之间的距离差之和，也可以同时考虑第一点集P中每一个点P_i的坐标与第二点集Q中一个点Q_i的坐标之间的距离差之和以及语义类别之间的距离差之和。

步骤22：将所述点集距离差最小时的点Q_i所组成的点集确定为所述第三点集Q'。

在获取点集距离差时，随着点Q_i选择的不同，所得到的点集距离差的值也会有所不同。当点集距离差的值取最小时，即第一点集P中所有点P_i所对应的点Q_i所组成的点集与第一点集P之间的距离最小，则意味着此时第一点集P与点P_i所对应的点Q_i所组成的点集最相近。因此将此时的点Q_i所组成的点集确定为第三点集Q'(两个点集之间的距离差之和||Q'-P||最小)，确保了第三点集Q'与第一点集P之间的匹配程度最高，有效保障了后续计算的准确度，从而有助于最终实现传感器的观测信息与预设三维点云地图之间的匹配。

应当理解的是，在其他实施例中，也可以通过其他方式获取第一点集P中每一个点P_i的对应最近点Q_i，并不仅限于上述的方式。

本实施例在获取第三点集Q'时，通过对第二点集Q中一个点Q_i的选取，使得第三点集与第一点集之间的距离差之和||Q'-P||最小，有效确保了第三点集Q'与第一点集P之间的匹配程度最高。

图4示出了如图1所示的实施例中获取第一点集P对应的第四点集P'步骤的流程示意图。

如图4所示，在上述图1所示实施例的基础上，本申请一个示例性实施例中，步骤30所示获取第一点集P对应的第四点集P'步骤，具体可以包括如下步骤：

步骤31：根据第一点集P和第三点集Q'，获取中间位姿信息。

传感器的中间位姿信息包括中间旋转矩阵R'和中间平移矩阵t'。

如前文所述，获取的第三点集Q'中，满足每一个点Q_i与第一点集P中每一个点P_i之间的距离差之和为最小。可以理解的是，此时第一点集P中每一个点P_i与第三点集Q'中的对应点Q_i可以通过传感器的位姿信息进行相互转换，例如Q_i＝P_iR'+t'，但是不同的点对应的旋转矩阵和平移矩阵并不完全相同，从而第一点集P中的所有点并不能通过一个中间位姿信息转换为第三点集Q'中的对应点。因此需要对第一点集P和第三点集Q'之间的转换关系进行优化，当符合预设条件时，可以获得传感器的中间位姿信息。

步骤32：根据所述中间位姿信息，获取所述第一点集P对应的第四点集P'。

第四点集P'中每一个点P'_i与第一点集P中每一个点P_i之间的关系为：

P'_i＝P_iR'+t'

其中，R'和t'分别为传感器对应的中间旋转矩阵和中间平移矩阵，将第一点集P中所有的点P_i所对应的点P'_i组成第四点集P'，从而实现了根据传感器的中间位姿信息，获得第一点集P对应的第四点集P'。

本实施例通过对第一点集P和第三点集Q'之间的转换关系进行优化，从而可以获取中间位姿信息，并可以根据该中间位姿信息获得对应的第四点集，从而使得第一点集P中的所有点均可以通过同一个中间位姿信息(R'和t')转换为第四点集P'中的对应点，从而有助于后续获取最终位姿信息，确保最终实现传感器的观测信息与预设三维点云地图之间的匹配。

在对第一点集P和第三点集Q'之间的转换关系进行优化时，可以通过第一点集P与所述第三点集Q'的距离差与语义类别差来进行。图5示出了如图4所示的实施例中获取中间位姿信息步骤的流程示意图。

如图5所示，在上述图4所示实施例的基础上，本申请一个示例性实施例中，步骤31所示获取中间位姿信息步骤，具体可以包括如下步骤：

步骤311：获取所述第一点集P与所述第三点集Q'的距离差以及所述语义类别差。

在本实施例中，第一点集P与第三点集Q'的距离差可以表示为：

其中，n为第一点集P中的点的个数。

第一点集P与第三点集Q'的语义类别差可以表示为：

其中，L_Qi和L_Pi分别为点Q_i和点P_i对应的语义类别。

步骤312：根据所述第一点集P与所述第三点集Q'的距离差以及语义类别差，获取中间位姿信息。

在获取中间位姿信息时，可以是根据上述距离差和语义类别差之和，也可以是根据上述距离差和语义类别差之差，还可以是上述距离差和语义类别差的线性组合，或者上述距离差和语义类别差之间建立其他的相互关系，只要能够根据两者之间的关系获取中间位姿信息即可。

图6示出了一种根据上述距离差和语义类别差来获取中间位姿信息的方式。如图6所示，在上述图5所示实施例的基础上，本申请一个示例性实施例中，步骤312所示获取中间位姿信息步骤，具体可以包括如下步骤：

步骤3121：获取所述第一点集P与所述第三点集Q'的距离差以及所述语义别差之和，记为目标函数f(R,t)。

在本实施例中，目标函数f(R,t)的表达式可以为：

其中，n为第一点集P中的点的个数；λ为语义类别差的权重，值可以根据实验进行设定，例如可以为0～1范围内的任意值；L_Qi和L_Pi分别为点Q_i和点P_i对应的语义类别。

应当理解的是，在其他实施例中，目标函数f(R,t)的表达式也可以为其他形式，并不仅限于上述的情形，此处不做限制。

步骤3122：将所述第一点集P与所述第三点集Q'的距离差与所述语义类别差之和取最小值时的位姿信息确定为所述中间位姿信息。

在本实施例中，中间位姿信息的表达式可以是：

R',t'＝argminf(R,t)

由目标函数f(R,t)的表达式可知，目标函数f(R,t)的取值反映了第一点集P和第三点集Q'之间的匹配度，目标函数f(R,t)的值越小，则意味着第一点集P和第三点集Q'之间的匹配度越高。随着旋转矩阵R和平移矩阵t取值的变化，目标函数f(R,t)的取值也会发生变化，当旋转矩阵R和平移矩阵t变化至使得目标函数f(R,t)的取值为最小时(此即为预设条件)，意味着此时的旋转矩阵R和平移矩阵t对应着最佳的转换关系，因此将目标函数f(R,t)取最小值时的位姿信息确定为中间位姿信息(包括中间旋转矩阵R'和中间平移矩阵t')，从而可以有效保证后续通过该中间位姿信息获取第一点集P对应的第四点集P'时，所得到的第四点集P'与第一点集P之间的匹配度最高。

图7示出了如图1所示的实施例中确定最终位姿信息步骤的流程示意图。

如图7所示，在上述图1所示实施例的基础上，本申请一个示例性实施例中，步骤40所示根据所述第四点集P'与所述第三点集Q'，确定最终位姿信息步骤，具体可以包括如下步骤：

步骤41：判断所述第四点集P'与所述第三点集Q'之间的平均距离d是否小于预设阈值。

平均距离d反映了第四点集P'和第三点集Q'之间的匹配度，若平均距离d的值满足预设阈值条件，则意味着两者之间的匹配度满足要求，同时意味着第四点集P'对应的第一点集P与第三点集Q'之间的匹配度满足要求。

在本实施例的一个具体实施例中，平均距离d可以只考虑第四点集P'和第三点集Q'中点的坐标之间的距离差，其表达式如下：

当平均距离d小于预设阈值d₁时，则意味着两者之间的匹配度满足要求。

在本实施例的一个具体实施例中，平均距离d可以考虑第四点集P'和第三点集Q'中点的坐标之间的距离差以及语义类别差，其表达式如下：

当平均距离d小于预设阈值d₂时，则意味着两者之间的匹配度满足要求。

若平均距离d小于预设阈值，则进行下述步骤42：

步骤42：将中间位姿信息确定为最终位姿信息。

由于根据此时第一点集P与第三点集Q'之间的匹配度满足要求，因此可以将此时的中间位姿信息确定为最终位姿信息，即最终旋转矩阵R的取值为此时的中间旋转矩阵R'，最终平移矩阵t的的取值为此时的中间平移矩阵t'。

在本实施例中，在确定了最终位姿信息后，可以将传感器的当期位姿信息替换为所获得的最终位姿信息，从而确保传感器的位姿信息可以实时更新。

若平均距离d不小于预设阈值，意味着通过上述步骤优化得到的中间位姿信息并未达到预设要求，需要进一步对中间位姿信息进行优化，则进行下述步骤43：

步骤43：将所述第四点集P'替换所述第一点集P，并返回上述步骤20，从而重复上述步骤20至步骤40，直到再次获得的第四点集P'与第三点集Q'之间的平均距离d小于预设阈值。

通过将第一点集P的值替换为第四点集P'的值，改变了第一点集P的初值，而第四点集P'与预设三维点云地图之间的匹配度要高于第一点集P与预设三维点云地图之间的匹配度，因此在重复进行上述步骤20至步骤40时，可以获得匹配度更高的第四点集P'，有助于最终实现传感器的观测信息与预设三维点云地图之间的匹配。

示例性装置

基于与本申请方法实施例相同的构思，本申请实施例还提供了一种传感器位姿确定装置。

图8示出了本申请一示例性实施例提供的传感器位姿确定装置的结构示意图。

如图8所示，本申请一示例性实施例提供的传感器位姿确定装置，包括：

当前位姿获取模块71，用于获取传感器的当前位姿信息；

第一点集确定模块72，用于获取实时三维点云的第一点集P中每一个点P_i在预设三维点云地图的第二点集Q中的对应最近点Q_i，组成第三点集Q'；

第二点集确定模块73，用于根据所述第一点集P和所述第三点集Q'，以及第一点集P中每一个点P_i及第三点集Q'每一个点Q_i的语义类别，获取所述第一点集P对应的第四点集P'；

最终位姿确定模块74，用于根据所述第四点集P'与所述第三点集Q'，确定最终位姿信息。

如图9所示，在一个示例性实施例中，所述当前位姿获取模块71包括：

图像采集单元711，用于采集实时图像信息，该图像采集单元711可以是能够获取实时图像信息的传感器，例如可以是单目相机，当然也可以是其他类型的传感器。

逆透视变换单元712，用于通过逆透视变换获取当前传感器坐标系下所述实时图像信息对应的三维点云，并获得所述传感器的当前位姿信息。

如图9所示，在一个示例性实施例中，所述第一点集确定模块72包括：

点集距离差获取单元721，用于获取所述第一点集P中每一个点P_i与所述第二点集Q中一个点Q_i的距离差之和，组成点集距离差；

第一点集获取单元722，用于将所述点集距离差最小时的点Q_i所组成的点集确定为所述第三点集Q'。

如图9所示，在一个示例性实施例中，所述第二点集确定模块73包括：

中间位姿获取单元731，用于根据第一点集P和第三点集Q'，获取中间位姿信息。具体地，中间位姿信息获取单元731可以获取所述第一点集P与所述第三点集Q'的距离差以及所述语义别差之和，记为目标函数f(R,t)，并且将目标函数取最小值时的位姿信息确定为中间位姿信息。

第二点集确定单元732，用于根据所述中间位姿信息，获取所述第一点集P对应的第四点集P'。

如图9所示，在一个示例性实施例中，所述最终位姿确定模块74包括：

判断单元741，用于判断所述第四点集P'与所述第三点集Q'之间的平均距离d是否小于预设阈值；

最终位姿确定单元742，用于当平均距离d小于预设阈值时，将中间位姿信息确定为最终位姿信息；

赋值单元743，用于将所述第四点集P'替换所述第一点集P，并返回第一点集确定模块72。

示例性电子设备

图10图示了根据本申请实施例的电子设备的框图。

如图10所示，电子设备100包括一个或多个处理器101和存储器102。

处理器101可以是中央处理单元(CPU)或者具有数据处理能力和/或指令执行能力的其他形式的处理单元，并且可以控制电子设备100中的其他组件以执行期望的功能。

存储器102可以包括一个或多个计算机程序产品，所述计算机程序产品可以包括各种形式的计算机可读存储介质，例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器(RAM)和/或高速缓冲存储器(cache)等。所述非易失性存储器例如可以包括只读存储器(ROM)、硬盘、闪存等。在所述计算机可读存储介质上可以存储一个或多个计算机程序指令，处理器101可以运行所述程序指令，以实现上文所述的本申请的各个实施例的传感器位姿确定方法以及/或者其他期望的功能。在所述计算机可读存储介质中还可以存储诸如输入信号、信号分量、噪声分量等各种内容。

在一个示例中，电子设备100还可以包括：输入装置103和输出装置104，这些组件通过总线系统和/或其他形式的连接机构(未示出)互连。

例如，该输入装置103可以是通信网络连接器。此外，该输入设备13还可以包括例如键盘、鼠标等等。

该输出装置104可以向外部输出各种信息，输出装置104可以包括例如显示器、扬声器、打印机、以及通信网络及其所连接的远程输出设备等等。

当然，为了简化，图10中仅示出了该电子设备100中与本申请有关的组件中的一些，省略了诸如总线、输入/输出接口等等的组件。除此之外，根据具体应用情况，电子设备100还可以包括任何其他适当的组件。

示例性计算机程序产品和计算机可读存储介质

除了上述方法和设备以外，本申请的实施例还可以是计算机程序产品，其包括计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请各种实施例的声源定位方法中的步骤。

所述计算机程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本申请实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如Java、C++等，还包括常规的过程式程序设计语言，诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。

此外，本申请的实施例还可以是计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请各种实施例的声源定位方法中的步骤。

所述计算机可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。

以上结合具体实施例描述了本申请的基本原理，但是，需要指出的是，在本申请中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本申请的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本申请为必须采用上述具体的细节来实现。

本申请中涉及的器件、装置、设备、系统的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的，可以按任意方式连接、布置、配置这些器件、装置、设备、系统。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇，指“包括但不限于”，且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”，且可与其互换使用，除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“诸如但不限于”，且可与其互换使用。

还需要指出的是，在本申请的装置、设备和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本申请的等效方案。

提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本申请。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的，并且在此定义的一般原理可以应用于其他方面而不脱离本申请的范围。因此，本申请不意图被限制到在此示出的方面，而是按照与在此公开的原理和新颖的特征一致的最宽范围。

为了例示和描述的目的已经给出了以上描述。此外，此描述不意图将本申请的实施例限制到在此公开的形式。尽管以上已经讨论了多个示例方面和实施例，但是本领域技术人员将认识到其某些变型、修改、改变、添加和子组合。

Claims

1.一种传感器位姿确定方法，包括：

获取传感器的当前位姿信息；

根据所述第四点集P'与所述第三点集Q'，确定最终位姿信息。

2.根据权利要求1所述的方法，其中，所述根据所述第一点集P和所述第三点集Q'，以及所述第一点集P中每一个点P_i及所述第三点集Q'每一个点Q_i的语义类别，获取所述第一点集P对应的第四点集P'，包括：

根据所述第一点集P和所述第三点集Q'，获取中间位姿信息；

根据所述中间位姿信息，获取所述第一点集P对应的第四点集P'。

3.根据权利要求2所述的方法，其中，所述根据所述第四点集P'与所述第三点集Q'，确定最终位姿信息，包括：

判断所述第四点集P'与所述第三点集Q'之间的平均距离d是否小于预设阈值；

若所述平均距离d小于预设阈值，则：

将所述中间位姿信息确定为最终位姿信息。

4.根据权利要求3所述的方法，其中，若所述平均距离d不小于预设阈值，则：

将所述第四点集P'替换所述第一点集P，并返回所述获取实时三维点云的第一点集P中每一个点P_i在预设三维点云地图的第二点集Q中的对应最近点Q_i，组成第三点集Q'。

5.根据权利要求2所述的方法，其中，所述根据所述第一点集P和所述第三点集Q'，获取中间位姿信息，包括：

获取所述第一点集P中每一个点P_i及第三点集Q'每一个点Q'_i的距离差和语义类别差；

根据所述距离差和所述语义类别差，获取所述中间位姿信息。

6.根据权利要求5所述的方法，其中，所述根据所述距离差和所述语义类别差，获取所述中间位姿信息包括：

获取所述距离差以及所述语义类别差之和；

将所述第一点集P与所述第三点集Q'的距离差与所述语义类别差之和取最小值时的位姿信息确定为所述中间位姿信息。

7.根据权利要求1所述的方法，其中，所述获取实时三维点云的第一点集P中每一个点P_i在预设三维点云地图的第二点集Q中的对应最近点Q_i，组成第三点集Q'，包括：

获取所述第一点集P中每一个点P_i与所述第二点集Q中一个点Q_i的距离差之和，组成点集距离差；

将所述点集距离差最小时的点Q_i所组成的点集确定为所述第三点集Q'。

8.根据权利要求1-7任一项所述的方法，其中，所述获取所述传感器的当前位姿信息包括：

传感器采集实时图像信息；

通过逆透视变换获取当前传感器坐标系下所述实时图像信息对应的三维点云，获得所述传感器的当前位姿信息。

9.一种传感器位姿确定装置，包括：

当前位姿获取模块，用于获取传感器的当前位姿信息；

10.一种计算机可读存储介质，所述存储介质存储有计算机程序，所述计算机程序用于执行上述权利要求1-8任一所述的传感器位姿确定方法。

11.一种电子设备，所述电子设备包括：

处理器；

用于存储所述处理器可执行指令的存储器；

所述处理器，用于从所述存储器中读取所述可执行指令，并执行所述指令以实现上述权利要求1-8任一所述的传感器位姿确定方法。