CN115585818A - 一种地图构建方法、装置、电子设备及存储介质 - Google Patents

Abstract

本申请公开了一种地图构建方法、装置、电子设备及存储介质，涉及数据融合技术领域。本申请中，从待测区域中，获取采集到的雷达点云数据和图像采集数据，再分别对雷达点云数据和图像采集数据进行解析，获得地图构建设备在标准坐标参考系中的第一测量位姿和第二测量位姿，从而基于第一测量位姿和第二测量位姿，获得地图构建设备的目标融合位姿，进而基于目标融合位姿、雷达点云数据和图像采集数据，构建待测区域的地图。采用这种方式，避免了因未考虑到激光SLAM构建的地图和视觉SLAM构建的地图，各自确定的地图构建设备的位姿不同，从而导致地图构建的精度较低的技术弊端，故而，实现了多传感器的紧耦合，并提高了地图构建的精度。

Description

一种地图构建方法、装置、电子设备及存储介质

技术领域

本申请涉及数据融合技术领域，尤其涉及一种地图构建方法、装置、电子设备及存储介质。

背景技术

随着信息技术和数据处理技术的发展，构建地图的方法也逐渐增多，其中，同步定位与地图构建(Simultaneous Localization And Mapping，SLAM)为最常用的方法，其通常是指在机器人或者其他载体上，通过对各种传感器数据进行采集和计算，来实现对其自身位置姿态(即位姿)的定位和地图的构建，被广泛应用于智能驾驶、服务型机器人、无人机等领域。

目前，主流的SLAM技术应用有激光SLAM和视觉SLAM两种，鉴于视觉SLAM极易受环境光线、相机运动等因素的影响，并且，激光SLAM垂直分辨率低，只能获取稀疏的点云信息，获取的环境特征有限。

故而，为了提高地图构建的精度，可将激光SLAM和视觉SLAM两种技术融合，来进行地图的构建，即将采用激光SLAM构建好的地图和采用视觉SLAM构建好的地图进行融合。

然而，采用上述的地图构建方式，会因并未考虑到激光SLAM构建的地图和视觉SLAM构建的地图，是在各自确定的机器人或者其他载体的位姿的基础上构建的，从而导致当两种位姿不同时，降低了地图构建的精度。

因此，采用上述方式，地图构建的精度较低。

发明内容

本申请实施例提供了一种地图构建方法、装置、电子设备及存储介质，用以提高地图构建的精度。

第一方面，本申请实施例提供了一种地图构建方法，应用于地图构建设备，所述方法包括：

从待测区域中，获取采集到的雷达点云数据和图像采集数据；

对雷达点云数据进行解析，获得地图构建设备在标准坐标参考系中的第一测量位姿；

对图像采集数据进行解析，获得地图构建设备在标准坐标参考系中的第二测量位姿；

基于第一测量位姿和第二测量位姿，获得地图构建设备的目标融合位姿；

基于目标融合位姿、雷达点云数据和图像采集数据，构建待测区域的地图。

第二方面，本申请实施例还提供了一种地图构建装置，应用于地图构建设备，所述装置包括：

获取模块，用于从待测区域中，获取采集到的雷达点云数据和图像采集数据；

解析模块，用于对雷达点云数据进行解析，获得地图构建设备在标准坐标参考系中的第一测量位姿，以及对图像采集数据进行解析，获得地图构建设备在标准坐标参考系中的第二测量位姿；

融合模块，用于基于第一测量位姿和第二测量位姿，获得地图构建设备的目标融合位姿；

构建模块，用于基于目标融合位姿、雷达点云数据和图像采集数据，构建待测区域的地图。

在一种可选的实施例中，在基于第一测量位姿和第二测量位姿，获得地图构建设备的目标融合位姿时，所述融合模块具体用于：

采用容积卡尔曼滤波，分别对第一测量位姿和第二测量位姿进行预测，获得相应的第一目标状态信息集和第二目标状态信息集；

基于第一目标状态信息集和第二目标状态信息集，获得地图构建设备的目标融合位姿。

在一种可选的实施例中，第一目标状态信息集包括：目标状态信息矩阵、目标状态信息向量、第一状态信息增量和第一关联信息矩阵，所述第二目标状态信息集包括：目标状态信息矩阵、目标状态信息向量、第二状态信息增量和第二关联信息矩阵；

则基于第一目标状态信息集和第二目标状态信息集，获得地图构建设备的目标融合位姿时，所述融合模块具体用于：

基于第一状态信息增量和第二状态信息增量，获得相应的融合状态信息增量，以及基于第一关联信息矩阵和第二关联信息矩阵，获得相应的融合关联信息矩阵；

基于目标状态信息矩阵、目标状态信息向量、融合状态信息增量和融合关联信息矩阵，获得地图构建设备的目标融合位姿。

在一种实施例中，在从待测区域中，获取采集到的雷达点云数据和图像采集数据的过程中，所述获取模块还用于：

分别获取惯性测量单元和轮式里程计从所述待测区域中，采集到的地图构建设备的第三测量位姿和第四测量位姿。

在一种可选的实施例中，在基于第一目标状态信息集和第二目标状态信息集，获得地图构建设备的目标融合位姿时，所述融合模块还用于：

基于预设的卡尔曼滤波算法，对第三测量位姿和第四测量位姿进行数据融合，获得相应的初始融合位姿；

采用容积卡尔曼滤波算法，对初始融合位姿进行预测，获得相应的初始融合状态信息集；

基于初始融合状态信息集、第一目标状态信息集和第二目标状态信息集，获得地图构建设备的目标融合位姿。

在一种可选的实施例中，在基于目标融合位姿、雷达点云数据和图像采集数据，构建待测区域的地图时，所述构建模块具体用于：

从地图构建请求中，获取相应的地图需求类型；

基于对应地图需求类型设置的地图构建方式，对雷达点云数据和图像采集数据进行调整，获得调整后的雷达点云数据和图像采集数据；

基于目标融合位姿和调整后的雷达点云数据和图像采集数据，构建待测区域的地图。

第三方面，本申请实施例还提出了一种电子设备，其包括处理器和存储器，其中，所述存储器存储有程序代码，当所述程序代码被所述处理器执行时，使得所述处理器执行上述第一方面所述的地图构建方法的步骤。

第四方面，本申请实施例还提出一种计算机可读存储介质，其包括程序代码，当所述程序代码在电子设备上运行时，所述程序代码用于使所述电子设备执行上述第一方面所述的地图构建方法的步骤。

第五方面，本申请实施例还提供一种计算机程序产品，所述计算机程序产品在被计算机调用时，使得所述计算机执行如第一方面所述的地图构建方法步骤。

本申请有益效果如下：

在本申请实施例所提供的地图构建方法中，根据第一测量位姿和第二测量位姿，获得地图构建设备的目标融合位姿，再基于目标融合位姿、雷达点云数据和图像采集数据，构建待测区域的地图，避免了相关技术中，未考虑到各传感器各自构建的地图，是在各自确定的地图构建设备的位姿基础上构建的问题，故而，实现了多传感器的紧耦合地图构建，并提高了地图构建的精度。

此外，本申请的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者，通过实施本申请而了解。本申请的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简要介绍，显而易见地，此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，并不构成对本申请的不当限定。在附图中：

图1为本申请实施例适用的系统架构的一个可选示意图；

图2为本申请实施例提供的一种地图构建设备及其各类传感器示意图；

图3为本申请实施例提供的一种地图构建方法的实施流程示意图；

图4为本申请实施例提供的一种基于图3的具体应用场景示意图；

图5为本申请实施例提供的一种获得地图构建设备的目标融合位姿的逻辑示意图；

图6为本申请实施例提供的一种构建地图的逻辑示意图；

图7为本申请实施例提供的一种地图构建装置的结构示意图；

图8为本申请实施例提供的一种电子设备的结构示意图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请技术方案的一部分实施例，而不是全部的实施例。基于本申请文件中记载的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请技术方案保护的范围。

需要说明的是，在本申请的描述中“多个”理解为“至少两个”。“和/或”，描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。A与B连接，可以表示：A与B直接连接和A与B通过C连接这两种情况。另外，在本申请的描述中，“第一”、“第二”等词汇，仅用于区分描述的目的，而不能理解为指示或暗示相对重要性，也不能理解为指示或暗示顺序。

此外，本申请技术方案中，对数据的采集、传播、使用等，均符合国家相关法律法规要求。

以下对本申请实施例中的部分技术用语进行解释说明，以便于本领域技术人员理解。

(1)惯性测量单元(Inertial Measurement Unit，IMU)：是测量物体三轴姿态角(或角速率)以及加速度的装置。

需要说明的是，一个惯性测量单元包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。

(2)紧耦合：把数据放到一起联合优化，即模块或者系统之间关系比较紧密，存在相互调用。

(3)松耦合：分别对参数做出估计，再把多个参数进行融合，例如，最简单的加权平均，或是，用卡尔曼滤波器。

(4)基于外观的实时建图(Real-Time Appearance-Based Mapping，RTAB-Map)：是一个通过内存管理方法实现回环检测的开源库，从限制地图的大小，以使得回环检测始终在固定的时间限制内处理，从而满足长期和大规模环境的线上建图要求。

进一步的，基于上述名词及相关术语解释，下面对本申请实施例的设计思想进行简要介绍：

目前，随着构建地图的方法也逐渐增多，其中，SLAM为最常用的方法，其通常是指在机器人或者其他载体上，通过对各种传感器数据进行采集和计算，来实现对其自身位置姿态(即位姿)的定位和地图的构建，被广泛应用于智能驾驶、未知环境地图构建等领域。

但现阶段采用单一传感器的SLAM均存在一定缺陷，纯视觉SLAM极易受环境光线、相机运动的影响，而纯激光SLAM垂直分辨率低，只能获取稀疏点云信息，获取的环境特征有限，并且，若仅依靠惯性测量单元和航迹推算的定位效果也存在较大的累积误差；此外，在多传感器融合方面，目前集中于视觉和IMU融合、激光雷达和IMU融合、视觉和激光雷达融合等方面。

其中，激光雷达和IMU融合可以克服激光SLAM过程中，激光雷达垂直分辨率低、更新速率慢以及由运动引起的失真等问题，但不适用于三维环境地图构建；激光雷达和视觉融合可以提高SLAM的定位精度以及鲁棒性，但在载体高速运动及转弯等情况时的定位和地图构建精度很差；视觉和IMU融合能有效提高载体在运动过程中的定位和地图构建精度，但累计误差较大。

可见，采用多传感器融合的方式，能够进一步提高定位和地图构建精度，然而，现阶段存在的多传感器融合方法大多采用松耦合的方式进行，即采用某一传感器进行定位和地图构建后，将建好的地图进行融合，如RTAB-Map，没有对过程中各传感器的位姿信息进行融合，仍然存在较大的误差。

显而易见的，当各传感器的位姿信息存在较大差异时，直接融合各传感器各自构建好的地图，获得的最终融合构建地图的精度较低。

有鉴于此，本申请实施例中，为了进一步提高复杂环境中，SLAM的定位精度和建图精度，提出了一种多传感器的地图构建方法，比如，激光雷达和图像采集设备的地图构建方法，针对激光雷达和图像采集设备的任意采样时刻，具体包括：分别获取激光雷达和图像采集设备，在当前采样时刻从待测区域中，采集到的雷达点云数据和图像采集数据；接着，对雷达点云数据进行解析，获得地图构建设备在标准坐标参考系中的第一测量位姿，以及对图像采集数据进行解析，获得地图构建设备在标准坐标参考系中的第二测量位姿；进一步地，基于第一测量位姿和第二测量位姿，获得地图构建设备的目标融合位姿；最终，基于目标融合位姿、雷达点云数据和图像采集数据，构建待测区域的地图，从而提高了地图构建的精度。

特别地，以下结合说明书附图对本申请的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本申请，并不用于限定本申请，并且在不冲突的情况下，本申请实施例及实施例中的特征可以相互组合。

参阅图1所示，其为本申请实施例适用的一种系统架构示意图，该系统架构包括：目标终端(101a，101b)和服务器102。目标终端(101a，101b)和服务器102之间可通过通信网络进行信息交互，其中，通信网络采用的通信方式可包括：无线通信方式和有线通信方式。

示例性的，目标终端(101a，101b)可通过蜂窝移动通信技术接入网络，与服务器102进行通信，其中，所述蜂窝移动通信技术，比如，包括第五代移动通信(5th GenerationMobile Networks，5G)技术。

可选的，目标终端(101a，101b)可通过短距离无线通信方式接入网络，与服务器102进行通信，其中，所述短距离无线通信方式，比如，包括无线保真(Wireless Fidelity，Wi-Fi)技术。

本申请实施例对上述系统架构中涉及的通信设备的数量不做任何限制，如图1所示，仅以目标终端(101a，101b)和服务器102为例进行描述，下面对上述各设备及其各自的功能进行简要介绍。

目标终端(101a，101b)，是一种可以向用户提供语音和/或数据连通性的设备，可以是支持有线和/或无线连接方式的设备。

示例性的，目标终端(101a，101b)包括但不限于：手机、平板电脑、笔记本电脑、掌上电脑、移动互联网设备(Mobile Internet Device，MID)、可穿戴设备，虚拟现实(VirtualReality，VR)设备、增强现实(Augmented Reality，AR)设备、工业控制中的无线终端设备、无人驾驶中的无线终端设备、智能电网中的无线终端设备、运输安全中的无线终端设备、智慧城市中的无线终端设备，或智慧家庭中的无线终端设备等。

此外，目标终端(101a，101b)上可以安装有相关的客户端，该客户端可以是软件，例如，应用程序(Application，APP)、浏览器、短视频软件等，也可以是网页、小程序等。在本申请实施例中，目标终端(101a，101b)可用于向服务器102发送各类传感器(比如，激光雷达、图像采集设备、惯性测量单元和轮式里程计等)采集的数据信息。

在一种可选的实施例中，参阅图2所示，目标终端可以为地图构建设备20，该地图构建设备可承载各类传感器，具体包括：激光雷达201、图像采集设备202、惯性测量单元203和轮式里程计204，用以对待测区域21进行环境信息的采集，以便后续进行相应的地图构建。

其中，激光雷达201可采集待测区域21对应平面内的稀疏点云信息(即雷达点云数据)，图像采集设备202可用于待测区域21内，环境的彩色信息和深度信息(即图像采集数据)，惯性测量单元203可用于采集地图构建设备20的三轴加速度信息和角速度，轮式里程计可通过四轮编码器采集地图构建设备20的运行速度；此外，上述所有采集数据均可实时传送到服务器102(即工控机)上进行后续处理，可选的，图像采集设备202可以为视觉相机。

服务器102可以是独立的物理服务器，也可以是多个物理服务器构成的服务器集群或者分布式系统，还可以是提供云服务、云数据库、云计算、云函数、云存储、网络服务、云通信、中间件服务、域名服务、安全服务、内容分发网络(Content Delivery Network，CDN)、以及大数据和人工智能平台等基础云计算服务的云服务器。

值得提出的是，在本申请实施例中，服务器102用于分别获取激光雷达和图像采集设备从待测区域中，采集到的雷达点云数据和图像采集数据，再对雷达点云数据进行解析，获得地图构建设备在标准坐标参考系中的第一测量位姿，以及对图像采集数据进行解析，获得地图构建设备在标准坐标参考系中的第二测量位姿，从而基于第一测量位姿和第二测量位姿，获得地图构建设备的目标融合位姿，进而基于目标融合位姿、雷达点云数据和图像采集数据，构建待测区域的地图。

下面结合上述的系统架构，以及参考附图来描述本申请示例性实施方式提供的地图构建方法，需要注意的是，上述系统架构仅是为了便于理解本申请的精神和原理而示出，本申请的实施方式在此方面不受任何限制。

参阅图3所示，其为本申请实施例提供的一种地图构建方法的实施流程图，执行主体以服务器为例，该方法的具体实施流程如下：

S301：从待测区域中，获取采集到的雷达点云数据和图像采集数据。

需要说明的是，雷达点云数据和图像采集数据，均包含地图构建设备在待测区域巡航，当前采样时刻从待测区域中，采集到的环境信息；其中，雷达点云数据用于指示：通过激光雷达采集到的待测区域中，各目标(比如，行人，树木等)各自在雷达坐标系中的目标点云位置，图像采集数据用于指示：通过图像采集设备采集到的待测区域中，各目标(比如，行人，树木等)各自在图像坐标系中的目标图像位置。

在一种可选的实现方式中，服务器在获得雷达点云数据和图像采集数据之后，便可对其进行数据预处理，例如，对雷达点云数据和图像采集数据进行杂散点剔除的操作，以减少无关数据对后续地图构建的影响。

S302：对雷达点云数据进行解析，获得地图构建设备在标准坐标参考系中的第一测量位姿，以及对图像采集数据进行解析，获得地图构建设备在标准坐标参考系中的第二测量位姿。

其中，标准坐标参考系也称之为世界坐标参考系或绝对坐标参考系，故而，在对雷达点云数据和图像采集数据的解析过程中，需要进行相应的坐标变换，即把获得的雷达点云数据和图像采集数据均统一到标准坐标系中，以便后续的数据融合和地图构建。

示例性的，在执行步骤S302时，服务器在将雷达点云数据和图像采集数据均统一到标准坐标参考系之后，便可通过激光SLAM前端，对激光雷达获取的雷达点云数据进行点云匹配，从而获得地图构建设备在标准坐标参考系中的第一测量位姿(即激光里程计信息)，记作：X_l＝(x_l,y_l,θ_l,v_xl,v_yl,w_l)，其中，x_l、y_l、θ_l、v_xl、v_yl、w_l分别表示：激光雷达获得的地图构建设备，在标准坐标参考系中的横坐标、纵坐标、角度、横向速度、纵向速度和角速度。

并且，可通过视觉SLAM前端，对图像采集设备采集的图像采集数据进行特征提取和特征匹配，得到相邻图像帧之间的位姿变换，然后，通过位姿运动估计，从而获得地图构建设备在标准坐标参考系中的第二测量位姿，记作：X_v＝(x_v,y_v,θ_v,v_xv,v_yv,w_v)，其中，x_v、y_v、θ_v、v_xv、v_yv、w_v分别表示：图像采集设备获得的地图构建设备，在标准坐标参考系中的横坐标、纵坐标、角度、横向速度、纵向速度和角速度。

S303：基于第一测量位姿和第二测量位姿，获得地图构建设备的目标融合位姿。

S304：基于目标融合位姿、雷达点云数据和图像采集数据，构建待测区域的地图。

基于上述S301～S304的地图构建方法步骤，参阅图4所示，其为本申请实施例提供的一种地图构建方法的具体应用场景示意图，服务器分别获取激光雷达Radar1和图像采集设备Video1，在当前采样时刻(比如，2022.10.18 13:11:28)从待测区域Tar.Area中，采集到的雷达点云数据Radar.Data和图像采集数据Image.Data，再对雷达点云数据Radar.Data进行解析，获得地图构建设备在标准坐标参考系中的第一测量位姿No.1.Pose，以及对图像采集数据Image.Data进行解析，获得地图构建设备在标准坐标参考系中的第二测量位姿No.2.Pose，从而基于第一测量位姿No.1.Pose和第二测量位姿No.2.Pose，获得地图构建设备的目标融合位姿Tar.Fus.Pose，进而基于目标融合位姿Tar.Fus.Pose、雷达点云数据Radar.Data和图像采集数据Image.Data，构建待测区域Tar.Area的地图，从而获得待测区域Tar.Area的地图Map。

在一种可选的实施例中，在执行步骤S303时，服务器在获得地图构建设备的第一测量位姿和第二测量位姿之后，便可采用容积卡尔曼滤波算法，分别对第一测量位姿和第二测量位姿进行预测，获得相应的第一目标状态信息集和第二目标状态信息集，其中，第一目标状态信息集和第二目标状态信息集至少包括：当前采样时刻的目标状态信息矩阵和目标状态信息向量。

示例性的，假定传感器(激光雷达或图像采集设备)的初始状态为x_k|k，其中，x_k|k可以为上述的第一测量位姿或者第二测量位姿，初始协方差矩阵为P_k|k，状态(预测)方程为x_t＝f(x_t)+Q_t，观测方程为z_t＝h(x_t)+v_t，其中，Q_t和v_t是状态噪声和观测噪声，则容积卡尔曼滤波算法具体如下：

1、状态预测。

(1)计算容积点。

其中，

表示预测获得的传感器在第k时刻的第j个容积采样点，2n表示容积采样点的数量，P_k|k表示预测获得的传感器在第k时刻的协方差矩阵，S_k|k表示协方差矩阵P_k|k对应的算数平方根(即标准差)矩阵，ξ_j表示n×2n维的矩阵，

表示预测获得的传感器在第k时刻的目标状态预测值。

(2)传播容积点并计算一步预测。

其中，

表示预测获得的传感器在第k+1时刻的第j个容积采样点，f(*)表示目标的非线性状态转移函数，

表示预测获得的传感器在第k+1时刻的目标状态预测值。

(3)计算状态误差协方差矩阵的一步预测。

其中，P_k+1|k表示预测获得的传感器在第k+1时刻的协方差矩阵，Q_k表示第k时刻目标运动过程中，状态噪声的协方差矩阵。

(4)计算状态信息和Fisher信息，即目标状态信息矩阵和目标状态信息向量。

其中，Y_k+1|k表示预测获得的传感器在第k+1时刻的目标状态信息矩阵，

表示预测获得的协方差矩阵P_k+1|k的逆矩阵，y_k+1|k表示预测获得的传感器在第k+1时刻的目标状态信息向量，

表示预测获得的传感器在第k+1时刻的目标状态预测值。

2、测量更新。

(1)计算容积点。

其中，P_k+1|k表示测量获得的传感器在第k+1时刻的协方差矩阵，S_k+1|k表示测量获得的传感器在第k+1时刻的标准差矩阵，

表示标准差矩阵S_k+1|k的转置矩阵，

表示测量获得的传感器在第k时刻的第j个容积采样点，ξ_j表示n×2n维的矩阵，

表示预测获得的传感器在第k+1时刻的目标状态预测值。

(2)传播容积点并计算一步预测。

其中，

表示测量获得的传感器在第k+1时刻的第j个容积采样点，h(*)表示目标的非线性测量转移函数，

表示测量获得的传感器在第k+1时刻的目标状态预测值。

(3)计算协方差矩阵。

其中，

表示传感器在第k+1时刻的预测值和测量值的交叉协方差矩阵，

表示预测获得的传感器在第k+1时刻的第j个容积采样点，

表示预测获得的传感器在第k+1时刻的目标状态预测值，

表示测量获得的传感器在第k+1时刻的第j个容积采样点，

表示测量获得的传感器在第k+1时刻的目标状态预测值。

(4)计算信息贡献矩阵和信息贡献向量，即融合关联信息矩阵和融合状态信息增量。

由容积卡尔曼滤波算法可知：

则有：

其中，

表示修正后的传感器在第k+1时刻的预测值和测量值的交叉协方差矩阵，P_k+1|k+1表示修正后的在第k+1时刻的协方差矩阵，

表示传感器在第k+1时刻的伪测量矩阵H_k+1的转置矩阵，I_k+1表示在第k+1时刻的融合关联信息矩阵，

表示传感器在第k+1时刻测量噪声的协方差矩阵R_k+1的逆矩阵，即融合状态信息增量，i_k+1表示在第k+1时刻的融合状态信息增量，V_k+1表示第k+1时刻目标运动过程中，观测噪声的协方差矩阵。

(5)更新信息状态和Fisher状态，即目标状态信息矩阵和目标状态信息向量。

Y_k+1|k+1＝Y_k+1|k+I_k+1

其中，Y_k+1|k+1表示修正后的传感器在第k+1时刻的目标状态信息矩阵，Y_k+1|k表示预测获得的传感器在第k+1时刻的目标状态信息矩阵，I_k+1表示在第k+1时刻的融合关联信息矩阵，

表示修正后的传感器在第k+1时刻的目标状态信息向量，y_k+1|k表示预测获得的传感器在第k+1时刻的目标状态信息向量，i_k+1表示在第k+1时刻的融合状态信息增量。

3、状态更新。

其中，P_k+1|k+1表示修正后的在第k+1时刻的协方差矩阵，

表示修正后的传感器在第k+1时刻的目标状态信息矩阵Y_k+1|k+1的逆矩阵，

表示修正后的传感器在第k+1时刻的目标状态预测值，即修正后的地图构建设备的位姿，

表示修正后的传感器在第k+1时刻的目标状态信息向量。

需要说明的是，若有N个传感器采集的数据待融合，第s个传感器初始状态为

各个传感器的观测方程为

s＝1，2，…，N，则相应的更新信息状态和Fisher状态，即目标状态信息矩阵和目标状态信息向量可表示如下：

因此，将上述表达式带入步骤3的状态更新公式即可得多传感器融合状态。

故而，基于上述的容积卡尔曼滤波算法，服务器在获得第一目标状态信息集和第二目标状态信息集之后，便可根据第一目标状态信息集和第二目标状态信息集，获得地图构建设备的目标融合位姿。

具体的，参阅图5所示，服务器基于第一目标状态信息集包含的第一状态信息增量和第一关联信息矩阵，以及第二目标状态信息集包含的第二状态信息增量和第二关联信息矩阵，获得当前采样时刻的融合状态信息增量和融合关联信息矩阵，从而基于目标状态信息矩阵、目标状态信息向量、融合状态信息增量和融合关联信息矩阵，获得地图构建设备的目标融合位姿。

需要说明的是，服务器在获得地图构建设备的目标融合位姿之后，可进行视觉SLAM的后端优化，进一步减小计算过程中的估计误差，比如，在局部地图中，采用光束平差法进行局部地图优化，在全局地图中，采用图优化进行全局地图优化，进一步减小地图构建过程中的估计误差。

在一种可选的实施例中，参阅图6所示，在执行步骤S304时，服务器在获得目标融合位姿之后，可根据接收到的业务需求方发送的地图构建请求，并从地图构建请求中，获取相应的地图需求类型；接着，基于对应地图需求类型设置的地图构建方式，对雷达点云数据和图像采集数据进行调整，获得调整后的雷达点云数据和图像采集数据；最终，基于目标融合位姿和调整后的雷达点云数据和图像采集数据，构建待测区域的地图。

示例性的，考虑到导航时要求较高实时性以及障碍物的精确估计，服务器可根据地图构建请求携带相应的地图需求类型，并结合雷达点云数据和图像采集数据，去生成用于导航的2D占栅格地图，即由于单一激光雷达仅能获取雷达扫描平面内的环境信息，不足以满足精确导航的需求，因此，这里将视觉SLAM构建的3D点云地图投影成2D占栅格地图，并将其与激光SLAM构建的2D占栅格地图进行基于加权贝叶斯的融合，最后得到高精度的融合导航地图，即构建待测区域的地图。

可选的，服务器也可以根据相应的地图需求类型，并结合雷达点云数据和图像采集数据，去生成可视化展示效果较好的3D点云地图，其中，将视觉SLAM获得的3D点云地图与激光SLAM获得的2D占栅格地图进行地图对齐，可获得较为准确的3D可视化点云地图。

在一种可选的实现方式中，在执行步骤S301的过程中，为了提高地图构建设备在高速运动及转弯等情况的定位和地图构建精度，服务器还可分别获取惯性测量单元和轮式里程计，在当前采样时刻从待测区域中，采集到的地图构建设备的第三测量位姿和第四测量位姿，从而基于第一测量位姿、第二测量位姿、第三测量位姿和第四测量位姿，获得地图构建设备的目标融合位姿。

示例性的，服务器将惯性测量单元采集的加速度信息和角速度信息进行惯性测量单元位姿解算，便可获取地图构建设备在惯性测量单元的测量坐标参考系下的惯性测量位姿，以及根据四轮编码器的脉冲数进行航迹推算，获取轮式里程计信息，即地图构建设备在轮式里程计对应的轮式坐标参考系下的轮式测量位姿，再进行相应的坐标变换，从而将惯性测量位姿和轮式测量位姿均统一到标准坐标系中，依次获得相应的第三测量位姿和第四测量位姿，以便后续的数据融合和地图构建。

在一种可选的实现方式中，服务器在获得第三测量位姿和第四测量位姿之后，便可基于预设的卡尔曼滤波算法，比如，扩展的卡尔曼滤波算法，对第三测量位姿和第四测量位姿进行数据融合，获得相应的初始融合位姿。

示例性的，假定服务器获得的，与惯性测量单元对应的第三测量位姿，记作X_l＝(x_i,y_i,θ_i,v_xi,v_yi,w_i)，其中，x_i、y_i、θ_i、v_xi、v_yi、w_i分别表示：惯性测量单元获得的地图构建设备，在标准坐标参考系中的横坐标、纵坐标、角度、横向速度、纵向速度和角速度，以及与轮式里程计对应的第四测量位姿，记作X_o＝(x_o,y_o,θ_o,v_xo,v_yo,w_o)，其中，x_o、y_o、θ_o、v_xo、v_yo、w_o分别表示：轮式里程计获得的地图构建设备，在标准坐标参考系中的横坐标、纵坐标、角度、横向速度、纵向速度和角速度，则可采用扩展的卡尔曼滤波算法，对第三测量位姿X_l和第四测量位姿进行位姿融合，获得相应的初始融合位姿，可记作X_init＝(x_init,y_init,θ_init,v_xinit,v_yinit,w_init)，其中，x_init、y_init、θ_init、v_xinit、v_yinit和w_init分别表示：地图构建设备在标准坐标参考系中的初始融合横坐标、初始融合纵坐标、初始融合角度、初始融合横向速度、初始融合纵向速度和初始融合角速度。

需要说明的是，若当前采样时刻为首次采样时刻，则可将上述获得的初始融合位姿X_init作为视觉SLAM和激光SLAM的初始位姿。

故而，服务器在获得初始融合位姿之后，便可基于上述类似的方法，即采用容积卡尔曼滤波算法，分别对初始融合位姿、第一测量位姿和第二测量位姿进行预测，获得相应的初始融合状态信息集、第一目标状态信息集和第二目标状态信息集，进而可根据初始融合状态信息集、第一目标状态信息集和第二目标状态信息集，获得地图构建设备的目标融合位姿，即进行第二次位姿融合，并从业务需求方发送的地图构建请求中，获取相应的地图需求类型；接着，基于对应地图需求类型设置的地图构建方式，对雷达点云数据和图像采集数据进行调整，获得调整后的雷达点云数据和图像采集数据；最终，基于目标融合位姿和调整后的雷达点云数据和图像采集数据，构建待测区域的地图，以提高地图构建设备在高速运动及转弯等情况的定位和地图构建精度。

还需说明的是，本申请实施例中，上述说举的几种传感器的地图构建方法，只是一种示例，并对上述地图构建方法的适用范围或使用场景不做任何限制；此外，初始融合位姿也可采用其他方式处理后，与第一测量位姿和第二测量位姿进行位姿融合，故而，本申请实施例中，不对引入初始融合位姿后，与第一测量位姿和第二测量位姿的位姿融合做任何具体限定。

综上所述，在本申请实施例所提供的地图构建方法中，从待测区域中，获取采集到的雷达点云数据和图像采集数据，再对雷达点云数据进行解析，获得地图构建设备在标准坐标参考系中的第一测量位姿，以及对图像采集数据进行解析，获得地图构建设备在标准坐标参考系中的第二测量位姿，从而基于第一测量位姿和第二测量位姿，获得地图构建设备的目标融合位姿，进而基于目标融合位姿、雷达点云数据和图像采集数据，构建待测区域的地图。

采用这种方式，避免了相关技术中，未考虑到各传感器各自构建的地图，是在各自确定的地图构建设备的位姿基础上构建的问题，故而，实现了多传感器的紧耦合地图构建，并提高了地图构建的精度。

进一步地，基于相同的技术构思，本申请实施例提供了一种地图构建装置，该地图构建装置用以实现本申请实施例的上述方法流程。参阅图7所示，该地图构建装置包括：获取模块701、解析模块702、融合模块703以及构建模块704，其中：

获取模块701，用于从待测区域中，获取采集到的雷达点云数据和图像采集数据；

解析模块702，用于对雷达点云数据进行解析，获得地图构建设备在标准坐标参考系中的第一测量位姿，以及对图像采集数据进行解析，获得地图构建设备在标准坐标参考系中的第二测量位姿；

融合模块703，用于基于第一测量位姿和第二测量位姿，获得地图构建设备的目标融合位姿；

构建模块704，用于基于目标融合位姿、雷达点云数据和图像采集数据，构建待测区域的地图。

在一种可选的实施例中，在基于第一测量位姿和第二测量位姿，获得地图构建设备的目标融合位姿时，所述融合模块703具体用于：

采用容积卡尔曼滤波算法，分别对第一测量位姿和第二测量位姿进行预测，获得相应的第一目标状态信息集和第二目标状态信息集；基于第一目标状态信息集和第二目标状态信息集，获得地图构建设备的目标融合位姿。

在一种可选的实施例中，第一目标状态信息集包括：目标状态信息矩阵、目标状态信息向量、第一状态信息增量和第一关联信息矩阵，所述第二目标状态信息集包括：目标状态信息矩阵、目标状态信息向量、第二状态信息增量和第二关联信息矩阵；

则基于第一目标状态信息集和第二目标状态信息集，获得地图构建设备的目标融合位姿时，所述融合模块703具体用于：

基于第一状态信息增量和第二状态信息增量，获得相应的融合状态信息增量，以及基于第一关联信息矩阵和第二关联信息矩阵，获得相应的融合关联信息矩阵；

基于目标状态信息矩阵、目标状态信息向量、融合状态信息增量和融合关联信息矩阵，获得地图构建设备的目标融合位姿。

在一种实施例中，在从待测区域中，获取采集到的雷达点云数据和图像采集数据的过程中，所述获取模块701还用于：

分别获取惯性测量单元和轮式里程计从所述待测区域中，采集到的地图构建设备的第三测量位姿和第四测量位姿。

在一种可选的实施例中，在基于第一目标状态信息集和第二目标状态信息集，获得地图构建设备的目标融合位姿时，所述融合模块703还用于：

基于预设的卡尔曼滤波算法，对第三测量位姿和第四测量位姿进行数据融合，获得相应的初始融合位姿；

采用容积卡尔曼滤波算法，对初始融合位姿进行预测，获得相应的初始融合状态信息集；

基于初始融合状态信息集、第一目标状态信息集和第二目标状态信息集，获得地图构建设备的目标融合位姿。

在一种可选的实施例中，在基于目标融合位姿、雷达点云数据和图像采集数据，构建待测区域的地图时，所述构建模块704具体用于：

从地图构建请求中，获取相应的地图需求类型；

基于对应地图需求类型设置的地图构建方式，对雷达点云数据和图像采集数据进行调整，获得调整后的雷达点云数据和图像采集数据；

基于目标融合位姿和调整后的雷达点云数据和图像采集数据，构建待测区域的地图。

基于相同的技术构思，本申请实施例还提供了一种电子设备，该电子设备可实现本申请上述实施例提供的地图构建方法流程。在一种实施例中，该电子设备可以是服务器，也可以是终端设备或其他电子设备。如图8所示，该电子设备可包括：

至少一个处理器801，以及与至少一个处理器801连接的存储器802，本申请实施例中不限定处理器801与存储器802之间的具体连接介质，图8中是以处理器801和存储器802之间通过总线800连接为例。总线800在图8中以粗线表示，其它部件之间的连接方式，仅是进行示意性说明，并不引以为限。总线800可以分为地址总线、数据总线、控制总线等，为便于表示，图8中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。或者，处理器801也可以称为控制器，对于名称不做限制。

在本申请实施例中，存储器802存储有可被至少一个处理器801执行的指令，至少一个处理器801通过执行存储器802存储的指令，可以执行前文论述的一种地图构建方法。处理器801可以实现图7所示的装置中各个模块的功能。

其中，处理器801是该装置的控制中心，可以利用各种接口和线路连接整个该控制设备的各个部分，通过运行或执行存储在存储器802内的指令以及调用存储在存储器802内的数据，该装置的各种功能和处理数据，从而对该装置进行整体监控。

在一种可选的设计中，处理器801可包括一个或多个处理单元，处理器801可集成应用处理器和调制解调处理器，其中，应用处理器主要处理操作系统、用户界面和应用程序等，调制解调处理器主要处理无线通信。可以理解的是，上述调制解调处理器也可以不集成到处理器801中。在一些实施例中，处理器801和存储器802可以在同一芯片上实现，在一些实施例中，它们也可以在独立的芯片上分别实现。

处理器801可以是通用处理器，例如CPU、数字信号处理器、专用集成电路、现场可编程门阵列或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件，可以实现或者执行本申请实施例中公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者任何常规的处理器等。结合本申请实施例所公开的一种地图构建方法的步骤可以直接体现为硬件处理器执行完成，或者用处理器中的硬件及软件模块组合执行完成。

存储器802作为一种非易失性计算机可读存储介质，可用于存储非易失性软件程序、非易失性计算机可执行程序以及模块。存储器802可以包括至少一种类型的存储介质，例如可以包括闪存、硬盘、多媒体卡、卡型存储器、随机访问存储器(Random AccessMemory，RAM)、静态随机访问存储器(Static Random Access Memory，SRAM)、可编程只读存储器(Programmable Read Only Memory，PROM)、只读存储器(Read Only Memory，ROM)、带电可擦除可编程只读存储器(Electrically Erasable Programmable Read-Only Memory，EEPROM)、磁性存储器、磁盘、光盘等等。存储器802是能够用于携带或存储具有指令或数据结构形式的期望的程序代码并能够由计算机存取的任何其他介质，但不限于此。本申请实施例中的存储器802还可以是电路或者其它任意能够实现存储功能的装置，用于存储程序指令和/或数据。

通过对处理器801进行设计编程，可以将前述实施例中介绍的一种地图构建方法所对应的代码固化到芯片内，从而使芯片在运行时能够执行图3所示的实施例的一种地图构建方法的步骤。如何对处理器801进行设计编程为本领域技术人员所公知的技术，这里不再赘述。

基于同一发明构思，本申请实施例还提供一种存储介质，该存储介质存储有计算机指令，当该计算机指令在计算机上运行时，使得计算机执行前文论述的一种地图构建方法。

在一些可选的实施方式中，本申请还提供了一种地图构建方法的各个方面还可以实现为一种程序产品的形式，其包括程序代码，当程序产品在装置上运行时，程序代码用于使该控制设备执行本说明书上述描述的根据本申请各种示例性实施方式的一种地图构建方法中的步骤。

应当注意，尽管在上文详细描述中提及了装置的若干单元或子单元，但是这种划分仅仅是示例性的并非强制性的。实际上，根据本申请的实施方式，上文描述的两个或更多单元的特征和功能可以在一个单元中具体化。反之，上文描述的一个单元的特征和功能可以进一步划分为由多个单元来具体化。

此外，尽管在附图中以特定顺序描述了本申请方法的操作，但是，这并非要求或者暗示必须按照该特定顺序来执行这些操作，或是必须执行全部所示的操作才能实现期望的结果。附加地或备选地，可以省略某些步骤，将多个步骤合并为一个步骤执行，和/或将一个步骤分解为多个步骤执行。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个服务器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

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

在涉及远程计算装置的情形中，远程计算装置可以通过任意种类的网络包括局域网(LAN)或广域网(WAN)连接到用户计算装置，或者，可以连接到外部计算装置(例如，利用因特网服务提供商来通过因特网连接)。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内。

Claims (10)

1.一种地图构建方法，其特征在于，应用于地图构建设备，包括：

从待测区域中，获取采集到的雷达点云数据和图像采集数据；

对所述雷达点云数据进行解析，获得地图构建设备在标准坐标参考系中的第一测量位姿；

对所述图像采集数据进行解析，获得所述地图构建设备在所述标准坐标参考系中的第二测量位姿；

基于所述第一测量位姿和所述第二测量位姿，获得所述地图构建设备的目标融合位姿；

基于所述目标融合位姿、所述雷达点云数据和所述图像采集数据，构建所述待测区域的地图。

2.如权利要求1所述的方法，其特征在于，所述基于所述第一测量位姿和所述第二测量位姿，获得所述地图构建设备的目标融合位姿，包括：

采用容积卡尔曼滤波算法，分别对所述第一测量位姿和所述第二测量位姿进行预测，获得相应的第一目标状态信息集和第二目标状态信息集；

基于所述第一目标状态信息集和所述第二目标状态信息集，获得所述地图构建设备的目标融合位姿。

3.如权利要求2所述的方法，其特征在于，所述第一目标状态信息集包括：目标状态信息矩阵、目标状态信息向量、第一状态信息增量和第一关联信息矩阵，所述第二目标状态信息集包括：所述目标状态信息矩阵、所述目标状态信息向量、第二状态信息增量和第二关联信息矩阵；

则基于所述第一目标状态信息集和所述第二目标状态信息集，获得所述地图构建设备的目标融合位姿，包括：

基于所述第一状态信息增量和所述第二状态信息增量，获得相应的融合状态信息增量，以及基于所述第一关联信息矩阵和所述第二关联信息矩阵，获得相应的融合关联信息矩阵；

基于所述目标状态信息矩阵、所述目标状态信息向量、所述融合状态信息增量和所述融合关联信息矩阵，确定所述地图构建设备的目标融合位姿。

4.如权利要求2所述的方法，其特征在于，所述从待测区域中，获取采集到的雷达点云数据和图像采集数据的过程中，还包括：

分别获取惯性测量单元和轮式里程计从所述待测区域中，采集到的所述地图构建设备的第三测量位姿和第四测量位姿。

5.如权利要求4所述的方法，其特征在于，所述基于所述第一目标状态信息集和所述第二目标状态信息集，获得所述地图构建设备的目标融合位姿，还包括：

基于预设的卡尔曼滤波算法，对所述第三测量位姿和所述第四测量位姿进行数据融合，获得相应的初始融合位姿；

采用所述容积卡尔曼滤波算法，对所述初始融合位姿进行预测，获得相应的初始融合状态信息集；

基于所述初始融合状态信息集、所述第一目标状态信息集和所述第二目标状态信息集，获得所述地图构建设备的目标融合位姿。

6.如权利要求1-5中任一项所述的方法，其特征在于，所述基于所述目标融合位姿、所述雷达点云数据和所述图像采集数据，构建所述待测区域的地图，包括：

从地图构建请求中，获取相应的地图需求类型；

基于对应所述地图需求类型设置的地图构建方式，对所述雷达点云数据和所述图像采集数据进行调整，获得调整后的雷达点云数据和图像采集数据；

基于所述目标融合位姿和所述调整后的雷达点云数据和图像采集数据，构建所述待测区域的地图。

7.一种地图构建装置，其特征在于，应用于地图构建设备，包括：

获取模块，用于从待测区域中，获取采集到的雷达点云数据和图像采集数据；

解析模块，用于对所述雷达点云数据进行解析，获得地图构建设备在标准坐标参考系中的第一测量位姿，以及对所述图像采集数据进行解析，获得所述地图构建设备在所述标准坐标参考系中的第二测量位姿；

融合模块，用于基于所述第一测量位姿和所述第二测量位姿，获得所述地图构建设备的目标融合位姿；

构建模块，用于基于所述目标融合位姿、所述雷达点云数据和所述图像采集数据，构建所述待测区域的地图。

8.一种电子设备，包括存储器，处理器及存储在存储器上并可在处理器运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1-6中任一项所述的方法。

9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-6中任一所述方法的步骤。

10.一种计算机程序产品，其特征在于，所述计算机程序产品在被计算机调用时，使得所述计算机执行如权利要求1-6中任一项所述的方法。

Images