CN115164936A

CN115164936A - 高精地图制作中用于点云拼接的全局位姿修正方法及设备

Info

Publication number: CN115164936A
Application number: CN202210771681.6A
Authority: CN
Inventors: 高巍; 丁文东; 万国伟; 彭亮
Original assignee: Beijing Baidu Netcom Science and Technology Co Ltd
Current assignee: Beijing Baidu Netcom Science and Technology Co Ltd
Priority date: 2022-06-30
Filing date: 2022-06-30
Publication date: 2022-10-11
Anticipated expiration: 2042-06-30
Also published as: US20230341228A1; CN115164936B

Abstract

本公开提供了一种高精地图制作中用于点云拼接的全局位姿修正方法及设备，涉及计算机技术领域，尤其涉及自动驾驶、高精地图、导航技术领域。实现方案为：获取第一轨迹点的全局位姿信息；基于全局位姿信息，确定至少一对第二轨迹点，至少一对第二轨迹点与第一轨迹点位于同一轨迹上；获取至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息；以及基于至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息，计算第一轨迹点处的局部旋转偏移量，以修正全局位姿信息。

Description

高精地图制作中用于点云拼接的全局位姿修正方法及设备

技术领域

本公开涉及计算机技术领域，尤其涉及自动驾驶、高精地图、导航技术领域，具体涉及一种用于点云拼接的全局位姿修正方法、装置、电子设备、计算机可读存储介质和计算机程序产品。

背景技术

高精地图也称高精度地图，是由自动驾驶汽车使用的地图。高精地图拥有精确的车辆位置信息和丰富的道路元素数据信息，可以帮助汽车预知路面复杂信息，如坡度、曲率、航向等，从而更好地规避潜在的风险。

在高精地图的制作中，通常首先针对某段道路进行点云数据采集，同时通过全球导航卫星系统(Global Navigation Satellites System，GNSS)以及惯性导航测量单元(Inertial Measurement Unit，IMU)进行数据采集，并基于上述数据进行全局位姿信息估算，进而基于估算结果获取点云数据采集设备的全局位姿，并基于此对点云数据进行拼接。

在此部分中描述的方法不一定是之前已经设想到或采用的方法。除非另有指明，否则不应假定此部分中描述的任何方法仅因其包括在此部分中就被认为是现有技术。类似地，除非另有指明，否则此部分中提及的问题不应认为在任何现有技术中已被公认。

发明内容

本公开提供了一种用于点云拼接的全局位姿修正方法、装置、电子设备、计算机可读存储介质和计算机程序产品。

根据本公开的一方面，提供了一种用于点云拼接的全局位姿修正方法，包括：获取第一轨迹点的全局位姿信息；基于全局位姿信息，确定至少一对第二轨迹点，至少一对第二轨迹点与第一轨迹点位于同一轨迹上；获取至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息，其中，相对位姿估计信息基于该对第二轨迹点中的每个第二轨迹点相应的全局位姿信息获得，相对位姿测量信息通过里程测量设备获得；以及基于至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息，计算第一轨迹点处的局部旋转偏移量，以修正全局位姿信息。

根据本公开的另一方面，提供了一种用于点云拼接的全局位姿修正装置，包括：第一获取单元，被配置为获取第一轨迹点的全局位姿信息；第一确定单元，被配置为基于全局位姿信息，确定至少一对第二轨迹点，至少一对第二轨迹点与第一轨迹点位于同一轨迹上；第二获取单元，被配置为获取至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息，其中，相对位姿估计信息基于该对第二轨迹点中的每个第二轨迹点相应的全局位姿信息获得，相对位姿测量信息通过里程测量设备获得；以及计算单元，被配置为基于至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息，计算第一轨迹点处的局部旋转偏移量，以修正全局位姿信息。

根据本公开的另一方面，提供了一种电子设备，包括：至少一个处理器；以及与至少一个处理器通信连接的存储器；其中存储器存储有可被至少一个处理器执行的指令，指令被至少一个处理器执行，以使至少一个处理器能够执行上述用于点云拼接的全局位姿修正方法。

根据本公开的另一方面，提供了一种存储有计算机指令的非瞬时计算机可读存储介质，其中，计算机指令用于使计算机执行上述用于点云拼接的全局位姿修正方法。

根据本公开的另一方面，提供了一种计算机程序产品，包括计算机程序，其中，计算机程序在被处理器执行时实现上述用于点云拼接的全局位姿修正方法。

根据本公开的一个或多个实施例，通过获取待修正的第一轨迹点的全局位姿信息，并基于该信息确定多对第二轨迹点，并分别获取每对第二轨迹点的相对位姿的估计信息和测量信息，并基于上述两种信息之间的转换关系求解该第一轨迹点处的局部旋转偏移量，从而能够基于该局部旋转偏移量，对全局位姿信息进行纠正，以减少点云拼接的重叠现象，提升高精地图的制图精度。

应当理解，本部分所描述的内容并非旨在标识本公开的实施例的关键或重要特征，也不用于限制本公开的范围。本公开的其它特征将通过以下的说明书而变得容易理解。

附图说明

附图示例性地示出了实施例并且构成说明书的一部分，与说明书的文字描述一起用于讲解实施例的示例性实施方式。所示出的实施例仅出于例示的目的，并不限制权利要求的范围。在所有附图中，相同的附图标记指代类似但不一定相同的要素。

图1示出了根据本公开的实施例的可以在其中实施本文描述的各种方法的示例性系统的示意图；

图2示出了根据本公开的实施例的用于点云拼接的全局位姿修正方法的流程图；

图3示出了根据本公开的示例性实施例的数据采集设备轨迹的示意图；

图4示出了根据本公开的实施例的计算第一轨迹点处的局部旋转偏移量的流程图；

图5示出了可以实现根据本公开的实施例的基于相对旋转估值矩阵、相对旋转测量值矩阵以及转换关系矩阵，构建方程的流程图；

图6示出了根据本公开的实施例的用于点云拼接的全局位姿修正装置的结构框图；

图7示出了能够用于实现本公开的实施例的示例性电子设备的结构框图。

具体实施方式

以下结合附图对本公开的示范性实施例做出说明，其中包括本公开实施例的各种细节以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施例做出各种改变和修改，而不会背离本公开的范围。同样，为了清楚和简明，以下的描述中省略了对公知功能和结构的描述。

在本公开中，除非另有说明，否则使用术语“第一”、“第二”等来描述各种要素不意图限定这些要素的位置关系、时序关系或重要性关系，这种术语只是用于将一个要素与另一要素区分开。在一些示例中，第一要素和第二要素可以指向该要素的同一实例，而在某些情况下，基于上下文的描述，它们也可以指代不同实例。

在本公开中对各种所述示例的描述中所使用的术语只是为了描述特定示例的目的，而并非旨在进行限制。除非上下文另外明确地表明，如果不特意限定要素的数量，则该要素可以是一个也可以是多个。此外，本公开中所使用的术语“和/或”涵盖所列出的项目中的任何一个以及全部可能的组合方式。

在高精地图的制作中，拼接点云数据所基于的信息包括点云数据采集设备的全局位姿信息，这一信息通常基于IMU单元的全局位姿信息通过坐标系转换的方式计算获得；而IMU单元的全局位姿信息则是基于GNSS系统以及IMU单元所采集到的数据，进行组合求解获得的，其中，全局位姿信息包括位置信息以及旋转信息。

在进行全局位姿信息组合求解过程中，可能存在局部的求解过程无法收敛的现象，此时，所获得的全局位姿信息存在局部的旋转信息的偏差，也即局部旋转偏移量。局部旋转偏移量通常不会对轨迹平滑度产生影响，但是会使点云数据在相应位置进行拼接时出现重叠，影响制图精度。

相关技术中，可以通过增加全局量测(例如增加GNSS天线)，从而在同一位置获取更多数据，以使求解过程能够更快收敛，但是这种方案的硬件成本较高，应用场景有限；另一种方案中，可以通过里程计推算出来的数据对存在局部旋转偏移量的全局位姿信息进行替换，但是当存在偏差的距离较长时，该方案会产生较大误差。

本公开的实施例给出了一种用于点云拼接的全局位姿修正方法，通过获取待修正的第一轨迹点的全局位姿信息，并基于该信息确定多对第二轨迹点，并分别获取每对第二轨迹点的相对位姿的估计信息和测量信息，并基于上述两种信息之间的转换关系求解该第一轨迹点处的局部旋转偏移量，从而能够基于该局部旋转偏移量，对全局位姿信息进行纠正，以减少点云拼接的重叠现象，提升高精地图的制图精度。

下面将结合附图详细描述本公开的实施例。

图1示出了根据本公开的实施例可以将本文描述的各种方法和装置在其中实施的示例性系统100的示意图。参考图1，该系统100包括机动车辆110、服务器120以及将机动车辆110耦接到服务器120的一个或多个通信网络130。

在本公开的实施例中，机动车辆110可以包括根据本公开实施例的计算设备和/或被配置以用于执行根据本公开实施例的方法。

服务器120可以运行用于点云拼接的全局位姿修正方法的一个或多个服务或软件应用。在某些实施例中，服务器120还可以提供其他服务或软件应用，这些服务或软件应用可以包括非虚拟环境和虚拟环境。在图1所示的配置中，服务器120可以包括实现由服务器120执行的功能的一个或多个组件。这些组件可以包括可由一个或多个处理器执行的软件组件、硬件组件或其组合。机动车辆110的用户可以依次利用一个或多个客户端应用程序来与服务器120进行交互以利用这些组件提供的服务。应当理解，各种不同的系统配置是可能的，其可以与系统100不同。因此，图1是用于实施本文所描述的各种方法的系统的一个示例，并且不旨在进行限制。

服务器120可以包括一个或多个通用计算机、专用服务器计算机(例如PC(个人计算机)服务器、UNIX服务器、中端服务器)、刀片式服务器、大型计算机、服务器群集或任何其他适当的布置和/或组合。服务器120可以包括运行虚拟操作系统的一个或多个虚拟机，或者涉及虚拟化的其他计算架构(例如可以被虚拟化以维护服务器的虚拟存储设备的逻辑存储设备的一个或多个灵活池)。在各种实施例中，服务器120可以运行提供下文所描述的功能的一个或多个服务或软件应用。

服务器120中的计算单元可以运行包括上述任何操作系统以及任何商业上可用的服务器操作系统的一个或多个操作系统。服务器120还可以运行各种附加服务器应用程序和/或中间层应用程序中的任何一个，包括HTTP服务器、FTP服务器、CGI服务器、JAVA服务器、数据库服务器等。

在一些实施方式中，服务器120可以包括一个或多个应用程序，以分析和合并从机动车辆110接收的数据馈送和/或事件更新。服务器120还可以包括一个或多个应用程序，以经由机动车辆110的一个或多个显示设备来显示数据馈送和/或实时事件。

网络130可以是本领域技术人员熟知的任何类型的网络，其可以使用多种可用协议中的任何一种(包括但不限于TCP/IP、SNA、IPX等)来支持数据通信。仅作为示例，一个或多个网络110可以是卫星通信网络、局域网(LAN)、基于以太网的网络、令牌环、广域网(WAN)、因特网、虚拟网络、虚拟专用网络(VPN)、内部网、外部网、区块链网络、公共交换电话网(PSTN)、红外网络、无线网络(包括例如蓝牙、WiFi)和/或这些与其他网络的任意组合。

系统100还可以包括一个或多个数据库150。在某些实施例中，这些数据库可以用于存储数据和其他信息。例如，数据库150中的一个或多个可用于存储诸如音频文件和视频文件的信息。数据存储库150可以驻留在各种位置。例如，由服务器120使用的数据存储库可以在服务器120本地，或者可以远离服务器120且可以经由基于网络或专用的连接与服务器120通信。数据存储库150可以是不同的类型。在某些实施例中，由服务器120使用的数据存储库可以是数据库，例如关系数据库。这些数据库中的一个或多个可以响应于命令而存储、更新和检索到数据库以及来自数据库的数据。

在某些实施例中，数据库150中的一个或多个还可以由应用程序使用来存储应用程序数据。由应用程序使用的数据库可以是不同类型的数据库，例如键值存储库，对象存储库或由文件系统支持的常规存储库。

机动车辆110可以包括传感器111用于感知周围环境。传感器111可以包括下列传感器中的一个或多个：视觉摄像头、红外摄像头、超声波传感器、毫米波雷达以及激光雷达(LiDAR)。不同的传感器可以提供不同的检测精度和范围。摄像头可以安装在车辆的前方、后方或其他位置。视觉摄像头可以实时捕获车辆内外的情况并呈现给驾驶员和/或乘客。此外，通过对视觉摄像头捕获的画面进行分析，可以获取诸如交通信号灯指示、交叉路口情况、其他车辆运行状态等信息。红外摄像头可以在夜视情况下捕捉物体。超声波传感器可以安装在车辆的四周，用于利用超声波方向性强等特点来测量车外物体距车辆的距离。毫米波雷达可以安装在车辆的前方、后方或其他位置，用于利用电磁波的特性测量车外物体距车辆的距离。激光雷达可以安装在车辆的前方、后方或其他位置，用于检测物体边缘、形状信息，从而进行物体识别和追踪。由于多普勒效应，雷达装置还可以测量车辆与移动物体的速度变化。

机动车辆110还可以包括通信装置112。通信装置112可以包括能够从卫星141接收卫星定位信号(例如，北斗、GPS、GLONASS以及GALILEO)并且基于这些信号产生坐标的卫星定位模块。通信装置112还可以包括与移动通信基站142进行通信的模块，移动通信网络可以实施任何适合的通信技术，例如GSM/GPRS、CDMA、LTE等当前或正在不断发展的无线通信技术(例如5G技术)。通信装置112还可以具有车联网或车联万物(Vehicle-to-Everything，V2X)模块，被配置用于实现例如与其它车辆143进行车对车(Vehicle-to-Vehicle，V2V)通信和与基础设施144进行车辆到基础设施(Vehicle-to-Infrastructure，V2I)通信的车与外界的通信。此外，通信装置112还可以具有被配置为例如通过使用IEEE802.11标准的无线局域网或蓝牙与用户终端145(包括但不限于智能手机、平板电脑或诸如手表等可佩戴装置)进行通信的模块。利用通信装置112，机动车辆110还可以经由网络130接入服务器120。

机动车辆110还可以包括控制装置113。控制装置113可以包括与各种类型的计算机可读存储装置或介质通信的处理器，例如中央处理单元(CPU)或图形处理单元(GPU)，或者其他的专用处理器等。控制装置113可以包括用于自动控制车辆中的各种致动器的自动驾驶系统。自动驾驶系统被配置为经由多个致动器响应来自多个传感器111或者其他输入设备的输入而控制机动车辆110(未示出的)动力总成、转向系统以及制动系统等以分别控制加速、转向和制动，而无需人为干预或者有限的人为干预。控制装置113的部分处理功能可以通过云计算实现。例如，可以使用车载处理器执行某一些处理，而同时可以利用云端的计算资源执行其他一些处理。控制装置113可以被配置以执行根据本公开的方法。此外，控制装置113可以被实现为根据本公开的机动车辆侧(客户端)的计算设备的一个示例。

图1的系统100可以以各种方式配置和操作，以使得能够应用根据本公开所描述的各种方法和装置。

根据本公开的实施例，如图2所示，提供了一种用于点云拼接的全局位姿修正方法，包括：步骤S201、获取第一轨迹点的全局位姿信息；步骤S202、基于全局位姿信息，确定至少一对第二轨迹点，至少一对第二轨迹点与第一轨迹点位于同一轨迹上；步骤S203、获取至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息，其中，相对位姿估计信息基于该对第二轨迹点中的每个第二轨迹点相应的全局位姿信息获得，相对位姿测量信息通过里程测量设备获得；以及步骤S204、基于至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息，计算第一轨迹点处的局部旋转偏移量，以修正全局位姿信息。

由此，通过获取待修正的第一轨迹点的全局位姿信息，并基于该信息确定多对第二轨迹点，并分别获取每对第二轨迹点的相对位姿的估计信息和测量信息，并基于上述两种信息之间的转换关系求解该第一轨迹点处的局部旋转偏移量，从而能够基于该局部旋转偏移量，对全局位姿信息进行纠正，以减少点云拼接的重叠现象，提升高精地图的制图精度。

在一些实施例中，在基于GNSS系统和IMU单元所采集的数据进行组合求解时，在某些轨迹点处可能存在局部的求解过程无法收敛的情况，从而使该轨迹点处的全局位姿信息产生局部旋转偏移量，上述轨迹点即为所述第一轨迹点。

在一些实施例中，在确定第一轨迹点的同时，可以获取该第一轨迹点基于GNSS系统和IMU单元进行组合求解所获取的全局位姿信息，其中，全局位姿信息包括该第一轨迹点的全局位置信息以及旋转角度信息。

在一些实施例中，可以基于该第一轨迹点的全局位姿信息中的全局位置信息，确定至少一对第二轨迹点。

根据一些实施例，基于全局位姿信息，确定至少一对第二轨迹点可以包括：基于全局位姿信息中的位置信息以及第一距离阈值，确定第一范围；以及在第一范围内，确定至少一对第二轨迹点。

由于对于一段轨迹而言，其一定范围内的轨迹点对应的局部旋转偏移量是恒定的。由此，根据待修正的第一轨迹点的全局位置信息，确定第二轨迹点的选择范围(也即第一范围)，并在该范围中进行第二轨迹点的选取，能够进一步提升局部旋转偏移量的计算准确度。

图3示出了根据本公开的示例性实施例的数据采集设备轨迹的示意图，其中，数据采集设备例如可以为IMU单元。

在一些示例性实施例中，如图3所示，在该数据采集设备轨迹301上包括第一轨迹点302，基于该第一轨迹点302以及第一距离阈值，可以确定第一范围303，随后即可在该第一范围303内的轨迹301上，进行第二轨迹点的选取，例如选取第二轨迹点304和第二轨迹点305。

在一些实施例中，第一距离阈值例如可以为1～30米，可理解的，相关技术人员可以基于实际情况自行设置第一距离阈值，在此不做限制。

由于每对第二轨迹点的相对位姿测量信息可以通过里程测量设备(例如雷达惯性里程计)测量获得，而两轨迹点之间的距离超过一定范围后，测量准确度将无法保证。

根据一些实施例，对于至少一对第二轨迹点中的每对第二轨迹点，该对第二轨迹点中的两第二轨迹点之间的距离可以小于或等于第二距离阈值。

在一些示例性实施例中，如图3所示，对于一对第二轨迹点(也即第二轨迹点304和第二轨迹点305)而言，该两个第二轨迹点之间的距离需要满足小于或等于第二距离阈值的条件。

在一些实施例中，第二距离阈值例如可以为1～30米，可理解的，相关技术人员可以基于实际情况自行设置第二距离阈值，在此不做限制。

由此，通过限定两第二轨迹点之间的距离，能够确保该对第二轨迹点的相对位姿测量信息的准确度，从而提升局部旋转偏移量的准确性。

在一些实施例中，每对第二轨迹点的相对位姿测量信息可以通过里程测量设备获取。

在一些实施例中，对于每对第二轨迹点的相对位姿估计信息，可以首先通过上述组合求解的方式，获取该对第二轨迹点中的每个第二轨迹点的全局位姿信息，随后基于两第二轨迹点分别的全局位姿信息获取该对第二轨迹点的相对位姿估计信息。

根据一些实施例，如图4所示，基于至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息，计算第一轨迹点处的局部旋转偏移量可以包括：步骤S401、确定至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息之间的转换关系矩阵，转换关系矩阵包括待求解的局部旋转偏移量；步骤S402、基于至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息、相对位姿测量信息以及转换关系矩阵，构建用于求解局部旋转偏移量的方程；以及步骤S403、基于方程，计算局部旋转偏移量。

由此，基于局部旋转偏移量构建转换关系矩阵，并基于该矩阵，建立相对位姿估计值和相对位姿测量值之间的关系，从而获取用于求解局部旋转偏移量的方程。通过上述方式，能够基于相对位姿测量值，精准地获取该处的局部旋转偏移量。

在一些示例性实施例中，如图3所示，其中，设第二轨迹点304的全局位姿实际值(也即经过修正后的全局位姿信息)为

第二轨迹点305的全局位姿实际值(也即经过修正后的全局位姿信息)为

其中，t₁、t₂分别为数据采集设备经过第二轨迹点304以及第二轨迹点305对应的时刻。相应的，第二轨迹点304和第二轨迹点305的全局位姿信息分别记为

将该处(也即第一轨迹点处)的待求解的局部旋转偏移量记为ΔR(也即转换关系矩阵)，则第二轨迹点304、305的修正后的全局位姿信息

及其全局位姿信息

分别具有如下关系：

并且，两第二轨迹点之间的相对位姿估计信息

可以通过下述公式获得：

相似地，两第二轨迹点之间的相对位姿测量信息

与第二轨迹点304、305的修正后的全局位姿信息

具有如下关系：

对于上述两第二轨迹点之间的相对位姿估计信息

相对位姿测量信息

具有如下关系：

由此，即获取到上式所示的用于求解局部旋转偏移量的方程，即可基于上式以及相对位姿估计信息

相对位姿测量信息

求解局部旋转偏移量。

根据一些实施例，相对位姿估计信息可以包括相对位置估值向量，相对位姿测量信息可以包括相对位置测量值向量，至少一对第二轨迹点可以包括至少三对第二轨迹点，并且其中，基于至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息、相对位姿测量信息以及转换关系矩阵，构建用于求解局部旋转偏移量的方程可以包括：基于至少一对第二轨迹点中的每对第二轨迹点的相对位置估值向量、相对位置测量值向量以及转换关系矩阵，构建方程。

由此，通过相对位姿信息中的位置信息进行局部旋转偏移量的求解，从而能够进一步简化运算，节省计算资源，提升计算效率。

在一些实施例中，由于相对位姿估计信息

相对位姿测量信息

分别包括位置信息和旋转信息，因此可以将

分别用下式表示：

其中，

为相对位姿估计信息

中的相对位置估值向量，

为相对位姿测量信息

中的相对位置测量值向量；

为相对位姿估计信息

中的相对旋转估值矩阵，

为相对位姿测量信息

中的相对旋转测量值矩阵。

进一步的，基于上述方程，则可获得如下关系：

在一些实施例中，可以仅通过位置信息部分的等式关系，进行局部旋转偏移量的求解，从而简化求解过程。

在一些实施例中，可以分别获取不同的n对第二轨迹点(其中n≥3)，并分别获取每对第二轨迹点的相对位置估值向量以及相对位置测量值向量，并分别基于上述关系构建线性非齐次方程，并求解其最小二乘解，从而获得局部旋转偏移量。

在一些示例性实施例中，相对位置估值向量

相对位置测量值向量

例如可以分别记为：

ΔR^T(即转换关系矩阵的逆)可以记为：

为进一步简化计算，可以将上述矩阵及向量进行变换，从而获得如下关系式：

记为，

随后通过将上述n对第二轨迹点中的每对第二轨迹点的相对位置估值向量以及相对位置测量值向量分别进行堆叠，从而获取下式：

其中，

由此，通过求解上述线性非齐次方程的最小二乘解，即可获取ΔR^T。

在一些实施例中，由于通过上述方式无法获得严格的正交矩阵，因此可以通过矩阵QR分解或者利用下述公式，对ΔR^T进行正交变换，从而获取正交化后的ΔR：

由此，即可获取该第一轨迹点处的局部旋转偏移量ΔR。

在一些实施例中，可以基于该局部旋转偏移量ΔR对该第一轨迹点处的全局位姿信息

进行如下修正，从而获得修正后的全局位姿信息

其中，t_i为第一范围内的各轨迹点对应的数据采集设备经过的时刻。

根据一些实施例，相对位姿估计信息可以包括相对旋转估值矩阵，相对位姿测量信息可以包括相对旋转测量值矩阵，并且其中，基于至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息、相对位姿测量信息以及转换关系矩阵，构建用于求解局部旋转偏移量的方程可以包括：基于至少一对第二轨迹点中的每对第二轨迹点的相对旋转估值矩阵、相对旋转测量值矩阵以及转换关系矩阵，构建方程。

由此，通过相对位姿信息中的旋转信息部分进行局部旋转偏移量的计算，能够进一步简化运算，节省计算资源，提升计算效率。

由上述公式(1)可以获得，旋转信息部分的等式关系如下：

在一些实施例中，可以分别获取不同的n对第二轨迹点(其中n≥3)，并分别获取每对第二轨迹点的相对旋转估值矩阵以及相对旋转测量值矩阵，并分别基于上述关系构建线性非齐次方程，并求解其最小二乘解，从而获得局部旋转偏移量。

根据一些实施例，如图5所示，基于至少一对第二轨迹点中的每对第二轨迹点的相对旋转估值矩阵、相对旋转测量值矩阵以及转换关系矩阵，构建方程可以包括：步骤S501、针对至少一对第二轨迹点中的每对第二轨迹点，对该对第二轨迹点相应的相对旋转估值矩阵和相对旋转测量值矩阵进行对数映射，以获取该对第二轨迹点相应的相对旋转估值向量和相对旋转测量值向量；步骤S502、对转换关系矩阵进行对数映射，以获取转换关系向量；以及步骤S503、基于至少一对第二轨迹点中的每对第二轨迹点的相对旋转估值向量、相对旋转测量值向量以及转换关系向量，构建第一方程，以作为用于求解局部旋转偏移量的方程。

由此，通过将旋转矩阵及偏移量矩阵(也即转换关系矩阵)进行对数映射，获取相应的旋转向量和转换关系向量，从而简化运算，节省计算资源，提高计算效率。

在一些实施例中，为进一步简化运算，可以首先对相对旋转估值矩阵、相对旋转测量值矩阵以及转换关系矩阵分别进行对数映射，以

为例，对上述公式(2)的两边分别进行对数映射：

其中，将相对旋转估值向量记为

将相对旋转测量值向量记为

将转换关系向量记为δ，则上述三个向量分别可以通过如下公式表示：

δ＝Log(ΔR)

则上述等式关系可以进一步被表示为：

对上式进行进一步变换，从而获取下述方程：

其中，

其中，θ为一个三维向量，[θ]_×用于求θ的反对称矩阵，θ为向量θ对应的标量。

由此，根据上述方程，即可通过一对第二轨迹点的相对旋转估值向量和相对旋转测量值向量，求解得到该第一轨迹点处的转换关系向量，随后即可通过对数映射的方式获得相应的转换关系矩阵，具体公式如下：

ΔR＝Exp(δ)

在一些实施例中，也可以分别获取多对第二轨迹点中每对第二轨迹点的相对旋转估值向量和相对旋转测量值向量，并基于公式(3)的约束关系以及与上述操作类似的堆叠方法，从而构建线性非齐次方程，并求解其最小二乘解，通过基于多对第二轨迹点的旋转信息进行求解，能够使所获得的转换关系矩阵更加精确。

在一些实施例中，由于通过上述方式无法获得严格的正交矩阵，因此可以进一步通过矩阵QR分解或者利用下述公式，对ΔR进行正交变换，从而获取正交化后的ΔR：

由此，即可获取该第一轨迹点处的局部旋转偏移量ΔR。

进行如下修正，从而获得修正后的全局位姿信息

根据一些实施例，如图6所示，提供了一种用于点云拼接的全局位姿修正装置600，包括：第一获取单元610，被配置为获取第一轨迹点的全局位姿信息；第一确定单元620，被配置为基于全局位姿信息，确定至少一对第二轨迹点，至少一对第二轨迹点与第一轨迹点位于同一轨迹上；第二获取单元630，被配置为获取至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息，其中，相对位姿估计信息基于该对第二轨迹点中的每个第二轨迹点相应的全局位姿信息获得，相对位姿测量信息通过里程测量设备获得；以及计算单元640，被配置为基于至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息，计算第一轨迹点处的局部旋转偏移量，以修正全局位姿信息。

其中，用于点云拼接的全局位姿修正装置600中的单元610-单元640的操作与上述用于点云拼接的全局位姿修正方法的步骤S201-步骤S204的操作类似，在此不做赘述。

根据一些实施例，计算单元可以包括：第一确定子单元，被配置为确定至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息之间的转换关系矩阵，转换关系矩阵包括待求解的局部旋转偏移量；构建子单元，被配置为基于至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息、相对位姿测量信息以及转换关系矩阵，构建用于求解局部旋转偏移量的方程；以及计算子单元，被配置为基于方程，计算局部旋转偏移量。

根据一些实施例，相对位姿估计信息可以包括相对位置估值向量，相对位姿测量信息可以包括相对位置测量值向量，至少一对第二轨迹点包括至少三对第二轨迹点，并且其中，构建子单元可以被配置为：基于至少一对第二轨迹点中的每对第二轨迹点的相对位置估值向量、相对位置测量值向量以及转换关系矩阵，构建方程。

根据一些实施例，相对位姿估计信息可以包括相对旋转估值矩阵，相对位姿测量信息可以包括相对旋转测量值矩阵，并且其中，构建子单元可以被配置为：基于至少一对第二轨迹点中的每对第二轨迹点的相对旋转估值矩阵、相对旋转测量值矩阵以及转换关系矩阵，构建方程。

根据一些实施例，构建子单元可以被进一步配置为：针对至少一对第二轨迹点中的每对第二轨迹点，对该对第二轨迹点相应的相对旋转估值矩阵和相对旋转测量值矩阵进行对数映射，以获取该对第二轨迹点相应的相对旋转估值向量和相对旋转测量值向量；对转换关系矩阵进行对数映射，以获取转换关系向量；以及基于至少一对第二轨迹点中的每对第二轨迹点的相对旋转估值向量、相对旋转测量值向量以及转换关系向量，构建第一方程，以作为用于求解局部旋转偏移量的方程。

根据一些实施例，第一确定单元可以包括：第二确定子单元，被配置为基于全局位姿信息中的位置信息以及第一距离阈值，确定第一范围；以及第三确定子单元，被配置为在第一范围内，确定至少一对第二轨迹点。

根据本公开的实施例，还提供了一种电子设备、一种可读存储介质和一种计算机程序产品。

参考图7，现将描述可以作为本公开的服务器或客户端的电子设备700的结构框图，其是可以应用于本公开的各方面的硬件设备的示例。电子设备旨在表示各种形式的数字电子的计算机设备，诸如，膝上型计算机、台式计算机、工作台、个人数字助理、服务器、刀片式服务器、大型计算机、和其它适合的计算机。电子设备还可以表示各种形式的移动装置，诸如，个人数字处理、蜂窝电话、智能电话、可穿戴设备和其它类似的计算装置。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示例，并且不意在限制本文中描述的和/或者要求的本公开的实现。

如图7所示，电子设备700包括计算单元701，其可以根据存储在只读存储器(ROM)702中的计算机程序或者从存储单元708加载到随机访问存储器(RAM)703中的计算机程序，来执行各种适当的动作和处理。在RAM 703中，还可存储电子设备700操作所需的各种程序和数据。计算单元701、ROM 702以及RAM 703通过总线704彼此相连。输入/输出(I/O)接口705也连接至总线704。

电子设备700中的多个部件连接至I/O接口705，包括：输入单元706、输出单元707、存储单元708以及通信单元709。输入单元706可以是能向电子设备700输入信息的任何类型的设备，输入单元706可以接收输入的数字或字符信息，以及产生与电子设备的用户设置和/或功能控制有关的键信号输入，并且可以包括但不限于鼠标、键盘、触摸屏、轨迹板、轨迹球、操作杆、麦克风和/或遥控器。输出单元707可以是能呈现信息的任何类型的设备，并且可以包括但不限于显示器、扬声器、视频/音频输出终端、振动器和/或打印机。存储单元708可以包括但不限于磁盘、光盘。通信单元709允许电子设备700通过诸如因特网的计算机网络和/或各种电信网络与其他设备交换信息/数据，并且可以包括但不限于调制解调器、网卡、红外通信设备、无线通信收发机和/或芯片组，例如蓝牙TM设备、802.11设备、WiFi设备、WiMax设备、蜂窝通信设备和/或类似物。

计算单元701可以是各种具有处理和计算能力的通用和/或专用处理组件。计算单元701的一些示例包括但不限于中央处理单元(CPU)、图形处理单元(GPU)、各种专用的人工智能(AI)计算芯片、各种运行机器学习模型算法的计算单元、数字信号处理器(DSP)、以及任何适当的处理器、控制器、微控制器等。计算单元701执行上文所描述的各个方法和处理，例如上述用于点云拼接的全局位姿修正方法。例如，在一些实施例中，上述用于点云拼接的全局位姿修正方法可被实现为计算机软件程序，其被有形地包含于机器可读介质，例如存储单元708。在一些实施例中，计算机程序的部分或者全部可以经由ROM 702和/或通信单元709而被载入和/或安装到电子设备700上。当计算机程序加载到RAM 703并由计算单元701执行时，可以执行上文描述的上述用于点云拼接的全局位姿修正方法的一个或多个步骤。备选地，在其他实施例中，计算单元701可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行上述用于点云拼接的全局位姿修正方法。

本文中以上描述的系统和技术的各种实施方式可以在数字电子电路系统、集成电路系统、场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、芯片上系统的系统(SOC)、负载可编程逻辑设备(CPLD)、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以包括：实施在一个或者多个计算机程序中，该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/或解释，该可编程处理器可以是专用或者通用可编程处理器，可以从存储系统、至少一个输入装置、和至少一个输出装置接收数据和指令，并且将数据和指令传输至该存储系统、该至少一个输入装置、和该至少一个输出装置。

用于实施本公开的方法的程序代码可以采用一个或多个编程语言的任何组合来编写。这些程序代码可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器或控制器，使得程序代码当由处理器或控制器执行时使流程图和/或框图中所规定的功能/操作被实施。程序代码可以完全在机器上执行、部分地在机器上执行，作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。

在本公开的上下文中，机器可读介质可以是有形的介质，其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备，或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。

为了提供与用户的交互，可以在计算机上实施此处描述的系统和技术，该计算机具有：用于向用户显示信息的显示装置(例如，CRT(阴极射线管)或者LCD(液晶显示器)监视器)；以及键盘和指向装置(例如，鼠标或者轨迹球)，用户可以通过该键盘和该指向装置来将输入提供给计算机。其它种类的装置还可以用于提供与用户的交互；例如，提供给用户的反馈可以是任何形式的传感反馈(例如，视觉反馈、听觉反馈、或者触觉反馈)；并且可以用任何形式(包括声输入、语音输入或者、触觉输入)来接收来自用户的输入。

可以将此处描述的系统和技术实施在包括后台部件的计算系统(例如，作为数据服务器)、或者包括中间件部件的计算系统(例如，应用服务器)、或者包括前端部件的计算系统(例如，具有图形用户界面或者网络浏览器的用户计算机，用户可以通过该图形用户界面或者该网络浏览器来与此处描述的系统和技术的实施方式交互)、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信(例如，通信网络)来将系统的部件相互连接。通信网络的示例包括：局域网(LAN)、广域网(WAN)和互联网。

计算机系统可以包括客户端和服务器。客户端和服务器一般远离彼此并且通常通过通信网络进行交互。通过在相应的计算机上运行并且彼此具有客户端-服务器关系的计算机程序来产生客户端和服务器的关系。服务器可以是云服务器，也可以为分布式系统的服务器，或者是结合了区块链的服务器。

应该理解，可以使用上面所示的各种形式的流程，重新排序、增加或删除步骤。例如，本公开中记载的各步骤可以并行地执行、也可以顺序地或以不同的次序执行，只要能够实现本公开公开的技术方案所期望的结果，本文在此不进行限制。

虽然已经参照附图描述了本公开的实施例或示例，但应理解，上述的方法、系统和设备仅仅是示例性的实施例或示例，本发明的范围并不由这些实施例或示例限制，而是仅由授权后的权利要求书及其等同范围来限定。实施例或示例中的各种要素可以被省略或者可由其等同要素替代。此外，可以通过不同于本公开中描述的次序来执行各步骤。进一步地，可以以各种方式组合实施例或示例中的各种要素。重要的是随着技术的演进，在此描述的很多要素可以由本公开之后出现的等同要素进行替换。

Claims

1.一种用于点云拼接的全局位姿修正方法，所述方法包括：

获取第一轨迹点的全局位姿信息；

基于所述全局位姿信息，确定至少一对第二轨迹点，所述至少一对第二轨迹点与所述第一轨迹点位于同一轨迹上；

获取所述至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息，其中，所述相对位姿估计信息基于该对第二轨迹点中的每个第二轨迹点相应的全局位姿信息获得，所述相对位姿测量信息通过里程测量设备获得；以及

基于所述至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息，计算所述第一轨迹点处的局部旋转偏移量，以修正所述全局位姿信息。

2.根据权利要求1所述的方法，其中，所述基于所述至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息，计算所述第一轨迹点处的局部旋转偏移量包括：

确定所述至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息之间的转换关系矩阵，所述转换关系矩阵包括待求解的所述局部旋转偏移量；

基于所述至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息、相对位姿测量信息以及所述转换关系矩阵，构建用于求解所述局部旋转偏移量的方程；以及

基于所述方程，计算所述局部旋转偏移量。

3.根据权利要求2所述的方法，其中，所述相对位姿估计信息包括相对位置估值向量，所述相对位姿测量信息包括相对位置测量值向量，所述至少一对第二轨迹点包括至少三对第二轨迹点，并且其中，所述基于所述至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息、相对位姿测量信息以及所述转换关系矩阵，构建用于求解所述局部旋转偏移量的方程包括：

基于所述至少一对第二轨迹点中的每对第二轨迹点的相对位置估值向量、相对位置测量值向量以及所述转换关系矩阵，构建所述方程。

4.根据权利要求2所述的方法，其中，所述相对位姿估计信息包括相对旋转估值矩阵，所述相对位姿测量信息包括相对旋转测量值矩阵，并且其中，所述基于所述至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息、相对位姿测量信息以及所述转换关系矩阵，构建用于求解所述局部旋转偏移量的方程包括：

基于所述至少一对第二轨迹点中的每对第二轨迹点的相对旋转估值矩阵、相对旋转测量值矩阵以及所述转换关系矩阵，构建所述方程。

5.根据权利要求4所述的方法，其中，所述基于所述至少一对第二轨迹点中的每对第二轨迹点的相对旋转估值矩阵、相对旋转测量值矩阵以及所述转换关系矩阵，构建所述方程包括：

针对所述至少一对第二轨迹点中的每对第二轨迹点，对该对第二轨迹点相应的相对旋转估值矩阵和相对旋转测量值矩阵进行对数映射，以获取该对第二轨迹点相应的相对旋转估值向量和相对旋转测量值向量；

对所述转换关系矩阵进行对数映射，以获取转换关系向量；以及

基于所述至少一对第二轨迹点中的每对第二轨迹点的相对旋转估值向量、相对旋转测量值向量以及所述转换关系向量，构建第一方程，以作为用于求解所述局部旋转偏移量的所述方程。

6.根据权利要求1-5中任一项所述的方法，其中，所述基于所述全局位姿信息，确定至少一对第二轨迹点包括：

基于所述全局位姿信息中的位置信息以及第一距离阈值，确定第一范围；以及

在所述第一范围内，确定所述至少一对第二轨迹点。

7.根据权利要求1-6所述的方法，其中，对于所述至少一对第二轨迹点中的每对第二轨迹点，该对第二轨迹点中的两第二轨迹点之间的距离小于或等于第二距离阈值。

8.一种用于点云拼接的全局位姿修正装置，所述装置包括：

第一获取单元，被配置为获取第一轨迹点的全局位姿信息；

第一确定单元，被配置为基于所述全局位姿信息，确定至少一对第二轨迹点，所述至少一对第二轨迹点与所述第一轨迹点位于同一轨迹上；

第二获取单元，被配置为获取所述至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息，其中，所述相对位姿估计信息基于该对第二轨迹点中的每个第二轨迹点相应的全局位姿信息获得，所述相对位姿测量信息通过里程测量设备获得；以及

计算单元，被配置为基于所述至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息，计算所述第一轨迹点处的局部旋转偏移量，以修正所述全局位姿信息。

9.根据权利要求8所述的装置，其中，所述计算单元包括：

第一确定子单元，被配置为确定所述至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息和相对位姿测量信息之间的转换关系矩阵，所述转换关系矩阵包括待求解的所述局部旋转偏移量；

构建子单元，被配置为基于所述至少一对第二轨迹点中的每对第二轨迹点的相对位姿估计信息、相对位姿测量信息以及所述转换关系矩阵，构建用于求解所述局部旋转偏移量的方程；以及

计算子单元，被配置为基于所述方程，计算所述局部旋转偏移量。

10.根据权利要求9所述的装置，其中，所述相对位姿估计信息包括相对位置估值向量，所述相对位姿测量信息包括相对位置测量值向量，所述至少一对第二轨迹点包括至少三对第二轨迹点，并且其中，所述构建子单元被配置为：

11.根据权利要求9所述的装置，其中，所述相对位姿估计信息包括相对旋转估值矩阵，所述相对位姿测量信息包括相对旋转测量值矩阵，并且其中，所述构建子单元被配置为：

12.根据权利要求11所述的装置，其中，所述构建子单元被进一步配置为：

13.根据权利要求8-12中任一项所述的装置，其中，所述第一确定单元包括：

第二确定子单元，被配置为基于所述全局位姿信息中的位置信息以及第一距离阈值，确定第一范围；以及

第三确定子单元，被配置为在所述第一范围内，确定所述至少一对第二轨迹点。

14.根据权利要求8-13所述的装置，其中，对于所述至少一对第二轨迹点中的每对第二轨迹点，该对第二轨迹点中的两第二轨迹点之间的距离小于或等于第二距离阈值。

15.一种电子设备，包括：

至少一个处理器；以及

与所述至少一个处理器通信连接的存储器；其中

所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行权利要求1-7中任一项所述的方法。

16.一种存储有计算机指令的非瞬时计算机可读存储介质，其中，所述计算机指令用于使所述计算机执行根据权利要求1-7中任一项所述的方法。

17.一种计算机程序产品，包括计算机程序，其中，所述计算机程序在被处理器执行时实现权利要求1-7中任一项所述的方法。