CN111737278B

CN111737278B - 同时定位与建图方法、系统、设备及存储介质

Info

Publication number: CN111737278B
Application number: CN202010776018.6A
Authority: CN
Inventors: 陈俊宏; 郑名飞; 熊亮; 柳伟; 何震宇; 田第鸿
Original assignee: Peng Cheng Laboratory
Current assignee: Peng Cheng Laboratory
Priority date: 2020-08-05
Filing date: 2020-08-05
Publication date: 2020-12-04
Anticipated expiration: 2040-08-05
Also published as: CN111737278A

Abstract

本发明公开了一种同时定位与建图方法、系统、设备及存储介质。该方法包括：进入任一边缘节点的通信范围内时，与至少一个可通信的边缘节点建立通信；边缘节点存储有以边缘节点为中心的设定范围内的第一地图数据；将终端自身的第二地图数据及位置数据发送至已建立通信的边缘节点；其中，第一地图数据比第二地图数据具有更稠密的点云数据，且第一地图数据的点云数据具有语义标签；获取边缘节点发送的优化后的第二地图数据；根据优化后的第二地图数据更新自身的第二地图数据。解决了在与环境实时交互的高级SLAM过程中，计算能力、存储能力及能耗不足的问题，达到了满足环境的语义感知以及与环境实时交互的高级SLAM精准服务的效果。

Description

同时定位与建图方法、系统、设备及存储介质

技术领域

本发明涉及计算机视觉领域，尤其涉及一种基于边缘计算网络的SLAM方法。

背景技术

现有的SLAM（Simultaneous localization and mapping，同时定位与建图技术）方案可分为单机SLAM和多机SLAM，目前大多数的研究方案主要还是单机SLAM，即定位和对环境的感知建图是在同一台移动设备上完成的，但是由于移动平台的算力及功耗的局限，这类SLAM方案不能建立高精度的详细地图，而少量的多机SLAM方案也只能完成基础建图任务。基础地图基本可以满足对设备进行定位的需求，但是对于环境的语义感知、提供设备与环境的高级交互等应用服务的需求是远远满足不了的。

发明内容

本申请实施例通过提供一种同时定位与建图方法，解决了在与环境实时交互的高级SLAM过程中，计算能力、存储能力及能耗不足的技术问题，达到了满足环境的语义感知以及与环境实时交互的高级SLAM精准服务的技术效果。

本申请实施例提供了一种同时定位与建图方法，应用于边缘节点，其特征在于，所述方法包括：

进入任一边缘节点的通信范围内时，与至少一个可通信的边缘节点建立通信；所述边缘节点存储有以边缘节点为中心的设定范围内的第一地图数据；

将终端自身的第二地图数据及位置数据发送至已建立通信的边缘节点；其中，所述第一地图数据比第二地图数据具有更稠密的点云数据，且所述第一地图数据的点云数据具有语义标签；

获取边缘节点发送的优化后的第二地图数据；

根据优化后的第二地图数据更新自身的第二地图数据。

在一实施例中，还包括：

当终端开机时，启动自身传感器采集周围环境信息；

构建或读取所述自身的第二地图数据；

基于所述周围环境信息和自身的第二地图数据获取所述位置数据。

在一实施例中，所述获取边缘节点发送的优化后的第二地图数据的步骤，之后包括：

当终端第一次接收边缘节点发送的优化后的第二地图数据时，将自身的第二地图数据中点云的参考坐标系转至世界坐标系。

在一实施例中，所述根据优化后的第二地图数据更新自身的第二地图数据的步骤包括：

将优化后的第二地图数据中的点云替换自身的第二地图数据中相对应的点云；

对于优化后的第二地图数据中标签值为无语义的点：当所述无语义的点在优化后的第二地图数据范围内，在自身的第二地图数据中进行保留；当所述无语义的点不在优化后的第二地图数据范围内，在自身的第二地图数据中进行删除。

在一实施例中，所述与至少一个可通信的边缘节点建立通信包括：

当存在多个可通信的边缘节点时，获取信号最强的边缘节点；

与所述信号最强的边缘节点建立通信，并将所述信号最强的边缘节点标记为已连接。

在一实施例中，还包括：通过关键帧扩展第二地图数据，并发送扩展后的第二地图数据和位置数据至边缘节点。

在一实施例中，获取所述关键帧的步骤，包括：

设置终端的定位初始化使用的帧为初始关键帧；

当前时刻的位置和姿态与上一关键帧的差值达到预设阈值，则确定当前时刻的位置和姿态为关键帧。

在一实施例中，所述方法包括：

获取并存储以边缘节点为中心的设定范围内的第一地图数据；

接收进入通信范围内并建立连接的终端发送的第二地图数据；其中，所述第一地图数据比第二地图数据具有更稠密的点云数据，且所述第一地图数据的点云数据具有语义标签；

结合第一地图数据和第二地图数据完成所述终端的定位初始化；

根据所述第一地图数据优化修正第二地图数据，并为第二地图数据添加语义标签后发送至所述终端；其中根据所述第一地图数据优化修正第二地图数据，并为第二地图数据添加语义标签得到的数据为优化后的第二地图数据。

在一实施例中，所述根据所述第一地图数据优化修正第二地图数据，并为第二地图数据添加语义标签的步骤，包括：

将第二地图数据中点云的参考坐标系转至世界坐标系；

根据特征匹配算法，将第二地图数据范围内的点与第一地图数据范围内的点进行匹配；

将第二地图数据范围内的能够与第一地图数据范围内匹配成功的点，用第一地图数据的相应点替换，并添加语义标签。

在一实施例中，所述根据所述第一地图数据优化修正第二地图数据，并为第二地图数据添加语义标签的步骤，还包括：

将匹配成功的点的状态标签设置为表示存在语义的标签值，将匹配不成功的点的状态标签设置为表示失效的标签值；

将优化后的第二地图数据存储在边缘节点的终端临时地图中。

在一实施例中，所述根据特征匹配算法，将第二地图数据范围内的点与第一地图数据范围内的点进行匹配的步骤，之前包括：

遍历第二地图数据中点云上的每个点的状态标签，根据状态标签的不同执行以下操作的至少之一：

对于标签值表示失效或者有语义的点不做操作；

对于状态标签为空的点或者超出第二地图数据范围的点在第一地图数据范围内进行筛选；其中筛选是确定所述状态标签为空的点或者超出第二地图数据范围的点是否在第一地图数据范围内；

对于在第一地图数据范围内的点利用特征匹配算法进行匹配；

对于在第一地图数据范围外的点将状态标签设置为表示超出范围的标签值。

在一实施例中，所述方法包括

边缘节点获取并存储以边缘节点为中心的设定范围内的第一地图数据；

终端进入任一边缘节点的通信范围内时，与至少一个可通信的边缘节点建立通信；

边缘节点接收进入通信范围内并建立连接的终端发送的第二地图数据；

边缘节点结合第一地图数据和第二地图数据完成所述终端的定位初始化；

边缘节点根据所述第一地图数据优化修正第二地图数据，并为第二地图数据添加语义标签后发送至所述终端；其中根据所述第一地图数据优化修正第二地图数据，并为第二地图数据添加语义标签得到的数据为优化后的第二地图数据。

终端获取边缘节点发送的优化后的第二地图数据；

终端根据优化后的第二地图数据更新自身的第二地图数据。

为实现上述目的，还提供了一种系统，所述系统包括：

终端，被配置为：

进入任一边缘节点的通信范围内时，与至少一个可通信的边缘节点建立通信；将终端自身的第二地图数据及位置数据发送至已建立通信的边缘节点；获取边缘节点发送的优化后的第二地图数据；根据优化后的第二地图数据更新自身的第二地图数据；

边缘节点，被配置为：

获取并存储以边缘节点为中心的设定范围内的第一地图数据；接收进入通信范围内并建立连接的终端发送的第二地图数据；结合第一地图数据和第二地图数据完成所述终端的定位初始化；根据所述第一地图数据优化修正第二地图数据，并为第二地图数据添加语义标签后发送至所述终端；

其中，所述第一地图数据比第二地图数据具有更稠密的点云数据，且所述第一地图数据的点云数据具有语义标签。

为实现上述目的，还提供了一种计算机可读存储介质，其特征在于，其上存储有同时定位与建图程序，其特征在于，该同时定位与建图程序被处理器执行时实现上述任一所述的方法。

为实现上述目的，还提供了一种同时定位与建图设备，其特征在于，包括存储器、处理器及存储在存储器上并可在处理器上运行同时定位与建图程序，所述处理器执行所述同时定位与建图程序时实现上述任一所述的方法。

本申请实施例中提供的一个或多个技术方案，至少具有如下技术效果或优点：进入任一边缘节点的通信范围内时，与至少一个可通信的边缘节点建立通信；所述边缘节点存储有以边缘节点为中心的设定范围内的第一地图数据；解决了在与环境实时交互的高级SLAM过程中，计算能力、存储能力及能耗不足的问题。将终端自身的第二地图数据及位置数据发送至已建立通信的边缘节点；其中，所述第一地图数据比第二地图数据具有更稠密的点云数据，且所述第一地图数据的点云数据具有语义标签；将终端实时传输第二地图数据和位置数据至边缘节点，可以保证在高端SLAM过程中的实时性。获取边缘节点发送的优化后的第二地图数据；优化后的第二地图数据添加了语义信息，其中优化修正及添加语义标签的过程达到了满足环境的语义感知以及与环境实时交互的高级SLAM精准服务的技术效果。根据优化后的第二地图数据更新自身的第二地图数据；将得到的带有语义信息的地图数据更新至终端中，可以在终端提供高级SLAM精准的服务。

附图说明

图1为本申请同时定位与建图方法应用于终端的具体实施例；

图2为本申请同时定位与建图方法应用于边缘节点的具体实施例；

图3为本申请同时定位与建图方法的硬件结构示意图；

图4为本申请同时定位与建图方法的终端与边缘节点的通信连接示意图。

具体实施方式

为了更好的理解上述技术方案，下面将结合说明书附图以及具体的实施方式对上述技术方案进行详细的说明。

本申请涉及一种同时定位与建图方法，该系统包括：

终端300，配置基于无线通信的方式连接边缘节点；所述终端被配置为：传感器单元310获取终端周围环境数据及自身运动状态；计算存储单元320利用传感器单元310获取到的信息计算终端的位置和姿态，同时计算得出基础地图；通信单元330将计算得到的第二地图数据上传至边缘节点上。

该计算机存储单元320包括如图3所示：至少一个处理器322、存储器321。

处理器322可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器322中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器322可以是通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC）现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器321，处理器322读取存储器321中的信息，结合其硬件完成上述方法的步骤。

可以理解，本发明实施例中的存储器321可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(ReadOnlyMemory，ROM)、可编程只读存储器(ProgrammableROM，PROM)、可擦除可编程只读存储器(ErasablePROM，EPROM)、电可擦除可编程只读存储器(ElectricallyEPROM，EEPROM)或闪存。易失性存储器可以是随机存取存储器(RandomAccessMemory，RAM)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的 RAM可用，例如静态随机存取存储器(StaticRAM，SRAM)、动态随机存取存储器(DynamicRAM，DRAM)、同步动态随机存取存储器(SynchronousDRAM，SDRAM)、双倍数据速率同步动态随机存取存储器(DoubleDataRateSDRAM，DDRSDRAM)、增强型同步动态随机存取存储器(Enhanced SDRAM，ESDRAM)、同步连接动态随机存取存储器 (Synchlink DRAM，SLDRAM) 和直接内存总线随机存取存储器(DirectRambusRAM，DRRAM)。本发明实施例描述的系统和方法的存储器321旨在包括但不限于这些和任意其它适合类型的存储器。

边缘节点301，与进入通信范围内的终端进行通信连接。

所述边缘节点被配置为：具备较强大的计算和存储能力并且能够提供无限的供能。

参照图1，图1为本申请同时定位与建图方法应用于终端的具体实施例。

步骤S100：终端开机。

其中，终端设备开机后，首先将自己的身份识别码（ID）设置为空值，并开始启动传感器采集周围环境信息。

终端，配置基于无线通信的方式连接边缘节点；所述终端被配置为：传感器单元获取终端周围环境数据及自身运动状态；计算存储单元利用传感器单元获取到的信息计算终端的位置和姿态，同时计算得出基础地图；通信单元将计算得到的第二地图数据上传至边缘节点上。终端是可以各种可以应用SLAM的地图和定位数据的设备，例如VR（虚拟现实）设备、AR（增强现实）设备、自动驾驶设备等。

步骤S110：进入任一边缘节点的通信范围时，与至少一个可通信的边缘节点建立通信。

边缘节点分布于一定的区域范围内，终端可在边缘节点通信范围覆盖的区域内运行。在终端处于不同的边缘节点的通信范围内时，与可通信的边缘节点进行通信。

其中，所述与至少一个可通信的边缘节点建立通信可以包括：

终端通过WIFI/5G基站连接最近的（信号最强的）边缘节点，并在终端保存的连接关系表中将此边缘节点标记为已连接。

其中，终端与边缘节点使用无线通信的方式连接，无线通信可以使用WIFI或是5G或是未来的无线通信技术。具体可以如图4所示，通过边缘节点中第一地图数据分布式存储的设计可以控制局部地图规模，保证获取地图的实时性，同时通过多边缘节点的方式可以扩展地图范围，从而扩展了终端的使用范围的同时控制了地图的复杂度。且终端通过通信模块与边缘节点建立连接，在初始化完成之后，终端向边缘节点传输终端的定位初始化结果、第二地图数据和地图关键帧等。边缘节点中对终端传输第二地图数据进行处理，并将带有语义信息的地图传回终端。终端与边缘节点间只需要传输地图信息，其数据量小，因而可以很好的满足系统时性，同时提供的语义信息足够终端获取带有语义信息的高级地图。

其中，边缘节点，与进入通信范围内的终端进行通信连接；所述边缘节点被配置为：具备较强大的计算和存储能力并且能够提供无限的供能；可以使用固定供电的单机PC工作站，存储空间可以为1T至100T，且可以更大。

步骤S110中，终端与边缘节点建立通信连接的步骤比较灵活，可以发生在具体实施例的任何步骤之间，但需要在与边缘节点通信传递数据之前。

步骤S120：启动自身传感器采集周围环境信息。

传感器采集到当前时刻周围的环境信息数据（例如IMU（InertialMeasurementUnit）的运动状态，或相机拍摄的照片，或者激光雷达扫描得到的点云）后将环境信息数据作为输入传输给终端SLAM系统。

其中，IMU是移动机器人、移动智能设备上常见的传感器，是测量物体三轴姿态角（或角速率）以及加速度的装置。常见的IMU为六轴传感器，配备输出三轴加速度的加速度计和输出三轴角速度的陀螺仪。九轴IMU还会配备输出三轴姿态角的磁力计。

SLAM技术是一种实现机器人的自主定位导航及获取周边环境信息的技术，是AR（Augmented Reality，增强现实）/MR（Mixed Reality混合现实）、自动驾驶汽车/无人机的关键基础技术之一，主要研究的是将机器人放入未知环境中的未知位置中，如何使机器人能够一边移动一边逐步描绘出所处环境的地图及定位机器人的相对位置的方法。

点云数据：扫描资料以点的形式记录，每一个点包含有三维坐标，有些可能含有颜色信息（RGB）或反射强度信息（Intensity）。

步骤S130：判断终端自身的第二地图是否完成初始化：

当未完成初始化，进入步骤S140：构建自身的第二地图数据（完成自身的第二地图数据的初始化）。

终端的初始化可得到最初初始化后的第二地图数据，此第二地图数据以初始化时刻终端设备的位置和姿态作为地图参考坐标系原点，称此坐标参考系为终端临时参考系，具体的初始化方法可以参考ORB-SLAM2、VINS、HectorSLAM等开源算法所使用的方法。

其中，ORB-SLAM2的单目初始化流程简述如下：

一、选取可行的起始两帧图像作为初始帧，选取标准如下

1、构造单目初始器，若第一帧所提取的ORB（Oriented Fast and Rotated Brief）特征点个数大于100个，则开始构建单目初始器；其中ORB可以用来对图像中的关键点快速创建特征向量，这些特征向量可以用来识别图像中的对象。

2、若第二帧提起到的ORB特征点个数也大于100个，则开始初始化流程，否则回到1；

3、在满足1和2后，对第一帧和第二帧提起到的特征点进行匹配，匹配到的点数大于100则继续初始化流程，否则回到1。

二、计算两帧之间的位姿信息

1、两个线程并行的计算基础矩阵F和单应矩阵H，并且选择其一来恢复两帧之间的相对位姿和点云信息（F用于非平面，H用于平面）；

2、再根据计算的RH得分来选择F和H，若RH大于0.45选择H，否则选择F来恢复位姿R和t；

3、并把初始化成功的第一帧当作世界坐标系，并设置其变换矩阵为单位矩阵I，由恢复出的和组合得到，即为第一帧的世界坐标系到第二帧的相对位姿信息。

三、三角化计算点云深度信息，并得到地图点

1、对匹配点进行三角化处理（对于不能够进行三角化的匹配点只需进行删除即可），计算得到匹配点的点云深度，从而得到3D点，进而构造一个初始的3D稀疏地图；

2、设置第一帧和第二帧为关键帧，将关键帧的描述子转换为BoW（Bag-of -wordsmodel,词袋模型），并把关键帧插入到地图当中；

四、更新关键帧之间的连接关系，并在3D点和关键帧之间建立优化边，进行BA优化（SLAM中的BA优化，先根据相机模型和A,B图像特征匹配好的像素坐标，求出A图像上的像素坐标对应的归一化的空间点坐标，然后根据该空间点的坐标计算重投影到B图像上的像素坐标，重投影的像素坐标(估计值)与匹配好的B图像上的像素坐标(测量值),不会完全重合，BA的目的就是每一个匹配好的特征点建立方程，然后联立，形成超定方程，解出最优的位姿矩阵或空间点坐标(两者可以同时优化)）。

五、将地图点中的深度值进行归一化处理，和诡异出处理两帧之间的变换。

六、将初始化的关键帧和当前的关键帧插入到局部地图和局部关键帧容器中，并将当前的关键帧设置为参考帧，从而完成了整个初始化的过程。

当完成自身第二地图的初始化，进入步骤S150。

步骤S150：判断边缘节点是否有优化的第二地图数据下发：

当有优化的第二地图数据下发时，进入步骤S151：获取边缘节点发送的优化后的第二地图数据；根据优化后的第二地图数据更新自身的第二地图数据。

使用边缘节点下发的优化后的第二地图数据更新终端中的第二地图数据，具体操作是将下发的优化后的第二地图数据中的点云替换掉第二地图数据中相对应的点云，若下发的点云地图有不包含语义信息的地图点，则通过状态点标签来对此地图点进行操作，若标签未超出范围则继续保留该基础地图点，若标签标记为无效地图点则在终端基础地图中删除该点，此步骤可保证终端地图上传过的点云均包含相关语义信息，对于不包含语义信息的点可以认为是环境中会发生运动的物体，例如路过的行人和动物等等。若当前是终端第一次接收到边缘节点下发的地图则首先需要将所有基础地图点云的位置由终端临时参考系更新到协同SLAM全局坐标系（以下称为世界坐标系），既整个边缘计算SLAM系统使用的坐标参考系。具体的转换方法是使用一个4*4的矩阵T乘以所有第二地图点云中点坐标的齐次坐标。矩阵T由边缘节点具体实施例步骤S230中得到。完成转换后终更新终端中保存的连接关系表。将第二地图数据中点云的参考坐标系转至世界坐标系，坐标系的转换使得地图数据中的点云的位置具有唯一性。

其中，得到最终的带有语义信息的高级地图，并在过程中删除了优化后的第二地图数据范围外的点，将地图中发生运动的物体过滤掉，使得发生运动的物体不会影响到建图的过程，得到的高级地图更加的精确，也体现了良好的实时性。

当无优化的第二地图数据下发时，执行步骤S152。

步骤S152：读取自身的第二地图数据。当自身的第二地图数据已经存在时，可以读取自身的第二地图数据来提供SLAM应用所需要的数据。

步骤S160：基于所述周围环境信息和自身的第二地图数据获取所述位置数据。

其中，计算得到的6自由度位姿和第二地图数据作为输出，传输给应用（如MR/AR应用，自动驾驶决策层应用等等）。

6自由度位姿：物体在空间具有六个自由度，即沿x、y、z三个直角坐标轴方向的移动自由度和绕这三个坐标轴的转动自由度。因此，要完全确定物体的位置，就必须清楚这六个自由度。

利用已有地图计算当前设备6自由度位姿，若现有终端地图还没有语义信息，如刚完成初始化还未接收到边缘节点下发的高级地图时，则可使用普通的SLAM跟踪算法（如ORB-SLAM/VINS等）计算当前设备的6自由度位姿，若是终端地图参考坐标系就转换成整个边缘计算网络SLAM系统使用的坐标参考系了。使用边缘节点下发的连接关系表端的地图点云已有高级语义地图，则可使用地图的语义增加配准约束，用以更好的消除误匹配，增加定位准确度及提升系统鲁棒性。

步骤S170：判断当前帧是否为关键帧：若是，则说明发现新的地图点，可执行步骤S171，将自身的第二地图数据进行扩展，并可以更新发送给边缘节点，以获得边缘节点的优化。若否，则说明地图数据变化较小，可以执行步骤S172，仅将目前的第二地图数据和位置数据发送给边缘节点。

关键帧的定义：SLAM终端初始化时使用的帧一定是关键帧，其他关键帧的判断依据可以是当前时刻的位置和姿态与上一关键帧的差值是否达到预设阈值，如当前时刻设备与上一关键帧的姿态偏差大于3度或者设备与上一关键帧的距离偏差大于10cm。

步骤S171：通过关键帧扩展第二地图数据，并发送扩展后的第二地图数据和位置数据至边缘节点。用新的关键帧扩展第二地图数据，具体方法可以参考ORB-SLAM或是VINS中构建新地图点的方法。

步骤S172：终端自身的第二地图数据及位置数据发送至已建立通信的边缘节点。

步骤S171或S172之后，终端可以继续采集周围环境信息，并重复上述流程。

在本实施例中，进入任一边缘节点的通信范围内时，与至少一个可通信的边缘节点建立通信；所述边缘节点存储有以边缘节点为中心的设定范围内的第一地图数据；解决了在与环境实时交互的高级SLAM过程中，计算能力、存储能力及能耗不足的问题。将终端自身的第二地图数据及位置数据发送至已建立通信的边缘节点；其中，所述第一地图数据比第二地图数据具有更稠密的点云数据，且所述第一地图数据的点云数据具有语义标签；将终端实时传输第二地图数据和位置数据至边缘节点，可以保证在高端SLAM过程中的实时性。获取边缘节点发送的优化后的第二地图数据；优化后的第二地图数据添加了语义信息，其中优化修正及添加语义标签的过程达到了满足环境的语义感知以及与环境实时交互的高级SLAM精准服务的技术效果。根据优化后的第二地图数据更新自身的第二地图数据；将得到的带有语义信息的地图数据更新至终端中，可以在终端提供高级SLAM精准的服务。

参照图2，图2为本申请同时定位与建图方法应用于边缘节点的具体实施例。

步骤S210：获取并存储以边缘节点为中心的设定范围内的第一地图数据。

步骤S220：接收进入通信范围内并建立连接的终端发送的第二地图数据；其中，所述第一地图数据比第二地图数据具有更稠密的点云数据，且所述第一地图数据的点云数据具有语义标签。

步骤S230：结合第一地图数据和第二地图数据完成所述终端的定位初始化

其中，在得到终端SLAM初始关键帧的传感器原始数据以及终端初始化基础地图点云等相关信息后，利用闭环检测和重定位的方法（例如ORB-SLAM/VINS等传统SLAM算法中的闭环检测和重定位方法），可以计算出终端设备在初始关键帧时刻的位置和姿态，此时将计算得到的姿态转为3*3的旋转矩阵R，并与设备位置坐标t（3*1的列向量）按下列方式组合成4*4的矩阵T：

在传输经边缘节点优化后的地图时，矩阵T将被一同传输给终端。

步骤S240：根据所述第一地图数据优化修正第二地图数据，并为第二地图数据添加语义标签后发送至所述终端；其中根据所述第一地图数据优化修正第二地图数据，并为第二地图数据添加语义标签得到的数据为优化后的第二地图数据。

首先将所有第二地图数据上的点云的参考坐标系转到世界坐标系，再遍历接收到的点云上的每个点的状态标签，对于失效和有语义的点不做操作，对于标签为空及超出范围的点进行本边缘节点地图范围筛选，即根据点的坐标判断此地图点是否在本边缘节点地图范围内，对于范围外的点将其状态标签置为超出范围，对于地图范围内的点利用特征匹配，如SURF，SIFT等描述子匹配方法将基础地图上的点与边缘节点中保存的高级地图进行匹配，对于匹配上的点用高级地图直接替换其位置坐标并增加语义和更改状态为有语义，对于位置在本节点地图范围内，但是未能有效匹配的基础地图点则增加失效标志。完成优化处理后，将地图存储到节点上的相应终端临时地图中并将优化后的地图传送给终端。

其中，世界坐标系是系统的绝对坐标系，在没有建立用户坐标系之前画面上所有点的坐标都是以该坐标系的原点来确定各自的位置的。

特征匹配算法可以是如SURF（Speeded Up Robust Features，加速稳健特征）是一种稳健的局部特征点检测和描述算法或者SIFT（Scale-invariant feature transform，尺度不变特征转换）是一种基于尺度空间的，对图像缩放、旋转、甚至仿射变换保持不变性的图像局部特征描述算法。

语义标签内容代表的可以是语义信息，其中，对于自动驾驶而言，语义信息是指那些能让无人车更好地理解行车规则、感知路面交通状况、规划行车路线，且被涵盖在高精度地图里的多层次、富维度的信息。

相对于典型的点云地图而言，语义地图能够很好的表示出无人车到的地方是什么，无人车“看”到的东西是什么。比如当无人车进入一条马路，点云地图中，无人车并不能识别显示出来的一块块的点云到底是什么。

也有人把图像的语义分为视觉层、对象层和概念层。视觉层即通常所理解的底层，即颜色、纹理和形状等等，这些特征都被称为底层特征语义；对象层即中间层，通常包含了属性特征等，就是某一对象在某一时刻的状态；概念层则是高层，是图像表达出的最接近人类理解的东西。

在上述实施例中存在的有益效果，获取并存储以边缘节点为中心的设定范围内的第一地图数据；将第一地图数据存储在具备较强大的计算和存储能力并且能够提供无限的供能的边缘节点，解决了在与环境实时交互的高级SLAM过程中，计算能力、存储能力及能耗不足的问题。接收进入通信范围内并建立连接的终端发送的第二地图数据；其中所述第一地图数据比第二地图数据具有更稠密的点云数据，且所述第一地图数据的点云数据具有语义标签；获得第二地图数据，为之后根据所述第一地图数据优化修正第二地图数据做好数据准备；根据所述第一地图数据优化修正第二地图数据，并为第二地图数据添加语义标签后发送至所述终端。优化修正及添加语义标签的过程达到了满足环境的语义感知以及与环境实时交互的高级SLAM精准服务的技术效果。

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

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

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

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

应当注意的是，在权利要求中，不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的部件或步骤。位于部件之前的单词“一”或“一个”不排除存在多个这样的部件。本发明可以借助于包括有若干不同部件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中，这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发

明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims

1.一种同时定位与建图方法，应用于终端，其特征在于，所述方法包括：

获取边缘节点发送的优化后的第二地图数据；

根据优化后的第二地图数据更新自身的第二地图数据；

通过关键帧扩展第二地图数据，并发送扩展后的第二地图数据和位置数据至边缘节点。

2.如权利要求1所述的同时定位与建图方法，其特征在于，还包括：

当终端开机时，启动自身传感器采集周围环境信息；

构建或读取所述自身的第二地图数据；

3.如权利要求1所述的同时定位与建图方法，其特征在于，所述获取边缘节点发送的优化后的第二地图数据的步骤，之后包括：

4.如权利要求1所述的同时定位与建图方法，其特征在于，所述根据优化后的第二地图数据更新自身的第二地图数据的步骤包括：

5.如权利要求1所述的同时定位与建图方法，其特征在于，所述与至少一个可通信的边缘节点建立通信包括：

6.如权利要求1所述的同时定位与建图方法，其特征在于，获取所述关键帧的步骤，包括：

设置终端的定位初始化使用的帧为初始关键帧；

7.一种同时定位与建图方法，应用于边缘节点，其特征在于，所述方法包括：

根据所述第一地图数据优化修正第二地图数据，并为第二地图数据添加语义标签后发送至所述终端；其中，将第二地图数据中点云的参考坐标系转至世界坐标系；根据特征匹配算法，将第二地图数据范围内的点与第一地图数据范围内的点进行匹配；将第二地图数据范围内的能够与第一地图数据范围内匹配成功的点，用第一地图数据的相应点替换，并添加语义标签；根据所述第一地图数据优化修正第二地图数据，并为第二地图数据添加语义标签得到的数据为优化后的第二地图数据。

8.如权利要求7所述的同时定位与建图方法，其特征在于，所述根据所述第一地图数据优化修正第二地图数据，并为第二地图数据添加语义标签的步骤，还包括：

9.如权利要求7所述的同时定位与建图方法，其特征在于，所述根据特征匹配算法，将第二地图数据范围内的点与第一地图数据范围内的点进行匹配的步骤，之前包括：

对于标签值表示失效或者有语义的点不做操作；

10.一种同时定位与建图方法，其特征在于，所述方法包括

通过关键帧扩展第二地图数据，并发送扩展后的第二地图数据和位置数据至边缘节点；

边缘节点根据所述第一地图数据优化修正第二地图数据，并为第二地图数据添加语义标签后发送至所述终端；其中，将第二地图数据中点云的参考坐标系转至世界坐标系；根据特征匹配算法，将第二地图数据范围内的点与第一地图数据范围内的点进行匹配；将第二地图数据范围内的能够与第一地图数据范围内匹配成功的点，用第一地图数据的相应点替换，并添加语义标签；根据所述第一地图数据优化修正第二地图数据，并为第二地图数据添加语义标签得到的数据为优化后的第二地图数据；

终端获取边缘节点发送的优化后的第二地图数据；

终端根据优化后的第二地图数据更新自身的第二地图数据。

11.一种同时定位与建图系统，其特征在于，所述系统包括：

终端，被配置为：

边缘节点，被配置为：

12.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有同时定位与建图程序，同时定位与建图程序被处理器执行时实现权利要求1-11任一所述的方法。

13.一种同时定位与建图设备，其特征在于，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行同时定位与建图程序，所述处理器执行所述同时定位与建图程序时实现权利要求1-11任一所述的方法。