CN114858156A - 即时定位与地图构建方法及无人移动设备 - Google Patents

即时定位与地图构建方法及无人移动设备 Download PDF

Info

Publication number
CN114858156A
CN114858156A CN202210316869.1A CN202210316869A CN114858156A CN 114858156 A CN114858156 A CN 114858156A CN 202210316869 A CN202210316869 A CN 202210316869A CN 114858156 A CN114858156 A CN 114858156A
Authority
CN
China
Prior art keywords
ground
optimization
stage
parameters
key frames
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
CN202210316869.1A
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.)
Autonavi Software Co Ltd
Original Assignee
Autonavi Software 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 Autonavi Software Co Ltd filed Critical Autonavi Software Co Ltd
Priority to CN202210316869.1A priority Critical patent/CN114858156A/zh
Publication of CN114858156A publication Critical patent/CN114858156A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3807—Creation or updating of map data characterised by the type of data
    • G01C21/3811—Point data, e.g. Point of Interest [POI]

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本公开实施例公开了一种即时定位与地图构建方法及无人移动设备，所述方法包括：获取多个连续关键帧，以及所述多个连续关键帧中均能够观测到的多个目标三维点；利用所述连续关键帧以及所述目标三维点进行第一阶段的局部地图优化；其中，在第一阶段的局部地图优化过程中，将所述距离误差中的地面参数固定为初始值后，对所述连续关键帧的位姿参数进行优化；利用所述连续关键帧以及所述目标三维点中的所述地面三维点进行第二阶段的平面优化；在第二阶段的平面优化过程中，将所述连续关键帧的位姿参数固定为第一阶段优化得到的优化结果后，对所述连续关键帧中的地面参数进行优化。该技术方案能够得到准确率较高的地图优化结果。

Description

即时定位与地图构建方法及无人移动设备
技术领域
本公开涉及基于位置服务的技术领域，具体涉及一种即时定位与地图构建方法及无人移动设备。
背景技术
定位技术已被广泛应用于各个行业，例如机器人、自动驾驶、AR、VR等，其中被用于定位的传感器也包括很多，单目相机由于其成本低、体积小易于安装在各种平台，引起了大量研究人员和从业人员的关注。目前主流的单目SLAM(Simultaneous Localization andMapping，即时定位及构图)系统主要使用点作为唯一的地图地标，例如ORB-SLAM和DSO技术，但是点在地图中并不能体现场景中的高级别信息和几何结构。而从扫描到的周围环境的图像中提取地面信息，进而在地图构建时加入地面对应的信息，并联合地面约束能够使得估计误差更小。
因此，如何在SLAM建图过程中加入地面约束，进而降低所构建地图的误差是当前需要解决的技术问题之一。
发明内容
本公开实施例提供一种即时定位与地图构建方法及无人移动设备。
第一方面，本公开实施例中提供了一种即时定位与地图构建方法，其中，包括：
获取多个连续关键帧，以及所述多个连续关键帧中均能够观测到的多个目标三维点；
利用所述连续关键帧以及所述目标三维点进行第一阶段的局部地图优化；其中，在第一阶段的局部地图优化过程中，将所述距离误差中的地面参数固定为初始值后，对所述连续关键帧的位姿参数进行优化；
利用所述连续关键帧以及所述目标三维点中的所述地面三维点进行第二阶段的平面优化；在第二阶段的平面优化过程中，将所述连续关键帧的位姿参数固定为第一阶段优化得到的优化结果后，对所述连续关键帧中的地面参数进行优化。
进一步地，所述第一阶段的局部地图优化和所述第二阶段的局部地图优化中均使用成本函数进行优化，成本函数包括所述目标三维点在多个连续关键帧上的重投影误差以及所述目标三维点中地面三维点距离地面的距离误差；所述重投影误差基于所述目标三维点以及所述多个连续关键帧的位姿参数得到；所述距离误差基于所述地面三维点以及所述地面参数得到。
进一步地，获取多个连续关键帧，包括：
获取当前关键帧、第一历史关键帧以及所述当前关键帧和所述第一历史关键帧之间的多个连续历史关键帧；
其中，在第一阶段的局部地图优化中，所述第一历史关键帧对应的位姿参数固定不变，而当前关键帧、当前关键帧和第一历史关键帧之间的多个历史关键帧的位姿参数为待优化参数。
进一步地，所述地面参数包括地面法向量参数和地面到世界坐标系原点的距离参数。
进一步地，所述方法还包括：
提取所述当前关键帧中的特征点；
确定所述特征点对应的当前三维点；
基于所述当前三维点拟合平面的方式，确定所述当前三维点中位于地面的地面三维点。
进一步地，基于所述当前三维点拟合平面的方式，确定所述当前三维点中位于地面的地面三维点，包括：
基于所述当前三维点拟合出一个或多个平面；
确定所述一个或多个平面中对应于地面的目标平面；
确定所述当前三维点与所述目标平面之间的距离；
基于所述距离从所述当前三维点中筛选出地面三维点。
进一步地，当前优化过程的第一阶段的局部地图优化中，所述地面参数对应的所述初始值，为上一优化过程的第二阶段的平面优化过程中得到的优化结果。
第二方面，本公开实施例中提供了一种无人移动设备，包括：图像传感器和处理器；其中，
所述图像传感器采集所述无人移动设备周边环境的图像，以及将所述图像输出至所述处理器；
所述处理器获取所述图像中的多个连续关键帧，以及所述多个连续关键帧中均能够观测到的多个目标三维点；
所述处理器还利用所述连续关键帧以及所述目标三维点进行第一阶段的局部地图优化，以及利用所述连续关键帧以及所述目标三维点中的所述地面三维点进行第二阶段的平面优化；其中，在第一阶段的局部地图优化过程中，将所述距离误差中的地面参数固定为初始值后，对所述连续关键帧的位姿参数进行优化；在第二阶段的平面优化过程中，将所述连续关键帧的位姿参数固定为第一阶段优化得到的优化结果后，对所述连续关键帧中的地面参数进行优化。
第三方面，本公开实施例中提供了一种基于位置的服务提供方法，其中，包括：利用第一方面所述的方法进行后端优化，并基于后端优化的结果构建周边环境地图，以及基于所述周边环境地图为被服务对象提供基于位置的服务，所述基于位置的服务包括：导航、路线规划、地图渲染中的一种或多种。
第四方面，本发明实施例中提供了一种即时定位地图构建装置，包括：
获取模块，被配置为获取多个连续关键帧，以及所述多个连续关键帧中均能够观测到的多个目标三维点；
第一优化模块，被配置为利用所述连续关键帧以及所述目标三维点进行第一阶段的局部地图优化；其中，在第一阶段的局部地图优化过程中，将所述距离误差中的地面参数固定为初始值后，对所述连续关键帧的位姿参数进行优化；
第二优化模块，被配置为利用所述连续关键帧以及所述目标三维点中的所述地面三维点进行第二阶段的平面优化；在第二阶段的平面优化过程中，将所述连续关键帧的位姿参数固定为第一阶段优化得到的优化结果后，对所述连续关键帧中的地面参数进行优化。
第五方面，本发明实施例中提供了一种基于位置的服务提供装置，包括：利用上述即时定位与地图构建装置进行后端优化，并基于后端优化的结果构建周边环境地图，以及基于所述周边环境地图为被服务对象提供基于位置的服务，所述基于位置的服务包括：导航、路线规划、地图渲染中的一种或多种。
所述功能可以通过硬件实现，也可以通过硬件执行相应的软件实现。所述硬件或软件包括一个或多个与上述功能相对应的模块。
在一个可能的设计中，上述装置的结构中包括存储器和处理器，所述存储器用于存储一条或多条支持上述装置执行上述对应方法的计算机指令，所述处理器被配置为用于执行所述存储器中存储的计算机指令。上述装置还可以包括通信接口，用于上述装置与其他设备或通信网络通信。
第六方面，本公开实施例提供了一种电子设备，包括存储器、处理器以及存储在存储器上的计算机程序，其中，所述处理器执行所述计算机程序以实现上述任一方面所述的方法。
第七方面，本公开实施例提供了一种计算机可读存储介质，用于存储上述任一装置所用的计算机指令，该计算机指令被处理器执行时用于实现上述任一方面所述的方法。
第八方面，本公开实施例提供了一种计算机程序产品，其包含计算机指令，该计算机指令被处理器执行时用于实现上述任一方面所述的方法。
本公开实施例提供的技术方案可以包括以下有益效果：
本公开实施例在SLAM即时定位与地图构建构成中，后端优化处理时利用地面参数以及多个目标三维点对当前帧以及当前帧之前的多个连续关键帧进行优化，优化过程分为两个阶段；第一阶段中将地面参数固定后，对多个连续关键帧的位姿参数进行优化，第二阶段在第一阶段优化后的基础上，将多个连续关键帧的位姿参数固定后，对地面参数进行优化。最终可以得到优化后的各个关键帧对应的位姿参数以及地面参数。本公开实施例在SLAM过中通过引入地面参数的约束，使得后端优化过程中地面三维点向地面靠近的方式约束SLAM系统的尺度，并且在引入地面约束后，分两个阶段进行优化，第一个阶段优化中能够保持前后优化中使用一致的尺度，而第二个阶段优化中又使得地面满足该一致尺度，最终能够得到准确率较高的局部地图优化结果。
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本公开。
附图说明
结合附图，通过以下非限制性实施方式的详细描述，本公开的其它特征、目的和优点将变得更加明显。在附图中：
图1示出根据本公开一实施方式的即时定位与地图构建方法的流程图；
图2示出根据本公开一实施方式中重投影误差的效果示意图；
图3示出根据本公开一实施方式中点到地面的距离误差效果示意图；
图4示出根据本公开一实施方式的一种无人移动设备的结构框图；
图5示出根据本公开一实施方式中即时定位地图构建方法的一种场景示意图；
图6示出根据本公开一实施方式的即时定位地图构建装置的结构框图；
图7是适于用来实现根据本公开实施方式的即时定位与地图构建方法和/或基于位置的服务提供方法的电子设备的结构示意图。
具体实施方式
下文中，将参考附图详细描述本公开的示例性实施方式，以使本领域技术人员可容易地实现它们。此外，为了清楚起见，在附图中省略了与描述示例性实施方式无关的部分。
在本公开中，应理解，诸如“包括”或“具有”等的术语旨在指示本说明书中所公开的特征、数字、步骤、行为、部件、部分或其组合的存在，并且不排除一个或多个其他特征、数字、步骤、行为、部件、部分或其组合存在或被添加的可能性。
另外还需要说明的是，在不冲突的情况下，本公开中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本公开。
下面通过具体实施例详细介绍本公开实施例的细节。
图1示出根据本公开一实施方式的即时定位与地图构建方法的流程图。如图1所示，该即时定位与地图构建方法包括以下步骤：
在步骤S101中，获取多个连续关键帧以及所述多个连续关键帧中均能够观测到的多个目标三维点；
在步骤S102中，利用所述连续关键帧以及所述目标三维点进行第一阶段的局部地图优化；其中，在第一阶段的局部地图优化过程中，将所述距离误差中的地面参数固定为初始值后，对所述连续关键帧的位姿参数进行优化；
在步骤S103中，利用所述连续关键帧以及所述目标三维点中的所述地面三维点进行第二阶段的平面优化；在第二阶段的平面优化过程中，将所述连续关键帧的位姿参数固定为第一阶段优化得到的优化结果后，对所述连续关键帧中的地面参数进行优化。
本实施例中，该即时定位地图构建方法可以在终端上执行。终端可以是任意设备，例如无人机、无人驾驶车辆、机器人等移动设备。终端上可以设置图像传感器，用于在移动过程中采集周边环境的图像。本公开实施例中设置在终端上的图像传感器可以是单目相机。
即时定位与地图构建(SLAM)技术是指某种移动设备(如机器人、无人机、手机、汽车、智能穿戴设备等)从一个未知环境里的未知地点出发，在运动过程中通过传感器(如相机、激光雷达、IMU等)观测定位自身位置和姿态，再根据自身位姿进行增量式的地图构建，从而达到同时定位和地图构建的目的。
如果SLAM过程中使用的传感器为视觉相关的传感器(如单目、双目、RGB-D、鱼眼、全景相机等)，则通常称之为视觉SLAM。
视觉SLAM的过程包括图像采集、前端定位以及后端优化等。SLAM中构图部分主要指后端优化过程，本公开实施例主要涉及的是后端优化过程。
本公开实施例中，移动设备进入未知环境后，可以利用设置在移动设备上的图像传感器实时采集周边环境中的图像，该图像被图像传感器传送至移动设备的处理设备，由处理设备对其进行前端处理。前端处理过程中，可以从当前图像上提取特征点，进而基于特征点与前一图像进行匹配，并基于匹配结果确定当前图像相对于前一图像的初始位姿，当前图像以及初始位姿等信息均可以传送至后端线程进行后端优化，前端优化可以是实时处理过程，而后端优化可以不是实时处理过程。
对于图像传感器采集到的第一帧图像，可以直接以该第一帧图像的采集位置作为所要构建的地图坐标系的原点，并将该第一帧图像上提取的特征点等作为初始地图的信息进行存储，后续采集到的图像可以不断地对该初始地图进行更新。
本公开实施例中，可以从前端处理过程获取多个连续关键帧，该多个连续关键帧可以是固定数量的多个连续关键帧。后端优化过程可以是一个迭代循环的优化过程，每一迭代优化均针对固定数量的关键帧，该固定数量的关键帧可以看做是在一个滑动窗口的多个连续关键帧。在获得一个新的关键帧，也即当前关键帧后，将滑动窗口往该当前关键帧移动一个位置，也即将该当前关键帧加入该滑动窗口，并将原滑动窗口内远离当前关键帧的一个历史关键帧剔除。加入了当前关键帧的滑动窗口内的多个连续关键帧可以共同完成当前迭代优化。
假如当前关键帧为第n个关键帧，如果每次局部优化使用10个连续关键帧，也即滑动窗口的长度为10，则可以将该第n个关键帧以及之前的第n-1、n-2、……、n-9个历史关键帧联合起进行后端优化。可以理解的是，在当前迭代优化过程中，当前关键帧的初始位姿为前端处理过程中输出的初始位姿，而其他历史关键帧的初始位姿可以是上一迭代优化过程中优化得到的结果。
在一些实施例中，多个连续关键帧可以是当前关键帧之前的历史关键帧，并且与当前关键帧在时间上是连续的，也即每一轮迭代优化时滑动窗口内的关键帧都是连续的关键帧。目标三维点可以是多个待跟踪的地图点，并且该多个目标三维点在该滑动窗口内的多个连续关键帧中均能够观测到，也即该多个目标三维点在该多个连续关键帧中均有对应的图像点。需要说明的是，每次迭代优化过程中，目标三维点可能并不完全相同，也即在当前关键帧被加入后，可以基于当前关键帧以及之前的多个历史关键帧的共视关系更新待跟踪的目标三维点集合。
例如，在前一次优化过程中，所使用的关键帧为第n-10至第n-1个连续关键帧，并且该10个连续关键帧中所观测到的目标三维点包括P1至Px，而当前优化过程中，所使用的关键帧为第n-9至第n个连续关键帧(其中第n个关键帧为当前关键帧)，则该10个连续关键帧中所观测到的目标三维点可能包括P2至Px、Py，也即当前关键帧中已经观测不到三维点Px，而新观测到了三维点Py，并且三维点Py在第n-9至第n个连续关键帧中也能够观测到。
本公开实施例中，将后端优化分为两个阶段：第一阶段为局部地图优化，第二阶段为平面优化。已有技术中，后端优化只进行一步优化，也即利用成本函数将所有待优化参数一同进行优化，该所有待优化参数可以包括但不限于关键帧的位姿参数、地面参数等。而本公开实施例中，在第一阶段的局部地图优化中，将多个连续关键帧的位姿参数作为待优化参数，而将地面参数固定为初始值，不对该地面参数进行优化；在第二阶段的平面优化时，将多个连续关键帧的位姿参数固定为第一阶段的优化结果，并对地面参数进行优化。经过两步优化之后，可以得到各个关键帧对应的位姿参数以及地面参数，该位姿参数以及地面参数可以作为下一迭代优化过程的初始值。
需要说明的是，关键帧的位姿参数包括该关键帧相对于世界坐标系的旋转参数以及平移参数；地面参数包括地面在世界坐标系下的表示参数，地面在世界坐标系下的表示方式可以由多种，不同表示方式对应的地面参数有所不同，例如最近点表示方式下，地面参数包括地面法向量参数以及地面与世界坐标系原点的距离参数。
本公开实施例在SLAM即时定位与地图构建构成中，后端优化处理时利用地面参数以及多个目标三维点对当前帧以及当前帧之前的多个连续关键帧进行优化，优化过程分为两个阶段；第一阶段中将地面参数固定后，对多个连续关键帧的位姿参数进行优化，第二阶段在第一阶段优化后的基础上，将多个连续关键帧的位姿参数固定后，对地面参数进行优化。最终可以得到优化后的各个关键帧对应的位姿参数以及地面参数。本公开实施例在SLAM过中通过引入地面参数的约束，使得后端优化过程中地面三维点向地面靠近的方式约束SLAM系统的尺度，并且在引入地面约束后，分两个阶段进行优化，第一个阶段优化中能够保持前后优化中使用一致的尺度，而第二个阶段优化中又使得地面满足该一致尺度，最终能够得到准确率较高的优化结果，基于该准确度较高的优化结果所构建的SLAM地图的精度更高。
在本实施例的一个可选实现方式中，所述第一阶段的局部地图优化和所述第二阶段的局部地图优化中均使用成本函数进行优化，成本函数包括所述目标三维点在多个连续关键帧上的重投影误差以及所述目标三维点中地面三维点距离地面的距离误差；所述重投影误差基于所述目标三维点以及所述多个连续关键帧的位姿参数得到；所述距离误差基于所述地面三维点以及所述地面参数得到。
该可选的实现方式中，第一阶段的局部地图优化以及第二阶段的平面优化均使用预先构建的成本函数进行。该成本函数可以包括两部分：重投影误差以及距离误差；重投影误差为目标三维点在关键帧上的观测图像点和该目标三维点投影到该关键帧上的投影图像点之间的误差；距离误差为地面三维点与地面之间的距离(理论上该地面三维点到该地面的距离为0)。
下面举例说明成本函数、重投影误差以及距离误差的构建过程。
在一些实施例中，成本函数可以如下表示：
Figure BDA0003569211700000071
其中，
Figure BDA0003569211700000072
Figure BDA0003569211700000073
Figure BDA0003569211700000074
其中，C表示进行局部地图优化的成本函数，在优化过程中通过使得C尽可能小的情况下，解算得到待优化参数，如位姿参数Riw、tiw，和地面参数πw。
F表示滑动窗口内的多个连续关键帧以及该多个连续关键帧能够共同观测到的目标三维点集合。
G表示该滑动窗口内的地面三维点以及第一次出现该些地面三维点的初始观测帧的集合。需要说明的是，G是F的一个子集，也即G中的地面三维点为F中位于地面上的目标三维点，而G中的初始观测帧为F中第一次出现了某个或某些地面三维点的关键帧。
ei,j表示集合F中第i个关键帧和第j个目标三维点构成的重投影误差。
Figure BDA0003569211700000081
表示集合G中第q个地面三维点与第一次出现该第q个地面三维点的该第anchorp个初始观测帧构成的距离误差。
Ω表示协方差矩阵。
ρh(·)表示鲁棒核函数。
zi,j表示集合F中第j个目标三维点在第i个关键帧中观测到的图像点。
Pj w表示集合F中第j个目标三维点在世界坐标系下的坐标。需要说明的是，世界坐标系通常是移动设备进入未知环境之后采集作为初始地图的图像时所在的初始位置作为原点，并且以该移动设备上的图像传感器当前的旋转参数为相对于世界坐标系的旋转参数，而平移参数为0所建立的世界坐标系。
Riw表示集合F中第i个关键帧的相机坐标系到世界坐标系的旋转参数。
tiw表示集合F中第i个关键帧的相机坐标系到世界坐标系的平移参数。
ci表示集合F中第j个目标三维点在第i个关键帧中的投影点。
Figure BDA0003569211700000082
表示集合G中第q个地面三维点在世界坐标系下的坐标。
Figure BDA0003569211700000083
表示集合G中第anchorp个初始观测帧的相机坐标系下的地面参数。
πw表示世界坐标系下的地面参数。
Figure BDA0003569211700000084
表示集合G中第anchorp个初始观测帧的相机坐标系与世界坐标系的旋转参数。
Figure BDA0003569211700000085
表示集合G中第anchorp个初始观测帧的相机坐标系与世界坐标系的平移参数。
图2示出根据本公开一实施方式中重投影误差的效果示意图。如图2所示，世界坐标系下的三维点
Figure BDA0003569211700000086
经过待优化参数Riw和tiw的变换过后投影到相机成像平面的坐标ci与观测到的图像点zi,j不重合(理论上应该重合)，从而构成重投影误差。
图3示出根据本公开一实施方式中点到地面的距离误差效果示意图。如图3所示，地面三维点
Figure BDA0003569211700000087
理论上应该在地面πw上，但是由于估计变量不准的原因，使其与地面有一定的距离误差，从而构建点到地面的距离误差。
在本公开实施例中，第一阶段的局部地图优化实际上就是对上述成本函数的优化，优化目的是将成本函数C对应的值最小化的情况下，得到关键帧对应的位姿参数的优化结果，该位姿参数包括关键帧的相机坐标系到世界坐标系的旋转参数以及平移参数。
在第一阶段的局部地图优化过程中，可以将点到地面的距离误差中的地面参数πw设置为固定的初始参数，该初始参数可以是上一次优化时得到的地面参数，在第一阶段的局部地图优化中，不对其进行优化；而位姿参数则会在优化过程中不断变化，直到最终得到能够使得成本函数C最小的位姿参数为止。
第一阶段的局部地图优化结束后，可以得到优化后的各个关键帧的位姿参数；在第二阶段的平面优化中，可以将第一阶段的局部地图优化过程中得到的位姿参数固定不变，而优化成本函数中的地面参数πw。也就是说，第二阶段的平面优化中，成本函数中的位姿参数为第一阶段的局部地图优化后得到的优化参数，且固定不变，而距离误差中的地面参数会在优化过程中不断发生变化，直到最终的能够使得成本函数C最小的地面参数为止。
经过第一阶段和第二阶段的优化之后，可以得到参与本次优化过程的多个连续关键帧的位姿参数以及地面参数。
在本实施例的一个可选实现方式中，步骤S101，即获取多个连续关键帧的步骤，进一步包括以下步骤：
获取当前关键帧、第一历史关键帧以及所述当前关键帧和所述第一历史关键帧之间的多个连续历史关键帧；
其中，在第一阶段的局部地图优化中，所述第一历史关键帧对应的位姿参数固定不变，而当前关键帧、当前关键帧和第一历史关键帧之间的多个历史关键帧的位姿参数为待优化参数。
该可选的实现方式中，如上文中所述，当前的迭代优化所针对的是包括当前关键帧在内的多个连续关键帧，而前次迭代优化对的是去除该当前关键帧的多个连续关键帧以及该多个连续关键帧之间的一个历史关键帧。为了能够使得优化效果更好，也即为了使得当前迭代优化针对的关键帧的优化结果与前次迭代优化针对的关键帧的优化结果的尺度(例如对于距离的衡量尺度等)保持一致，在当前迭代优化时，除了滑动窗口内包括当前关键帧在内的多个连续关键帧之外，还可以加入滑动窗口外的一个或多个历史关键帧，该一个或多个历史关键帧可以称之为第一历史关键帧，针对该第一历史关键帧在前次迭代优化时获得了优化后的位姿参数。因此，在当前迭代优化时，将该第一历史关键帧加入，并且该第一历史关键帧的位姿参数为固定值，也即前次迭代优化的结果，用于约束其他关键帧的位姿参数的优化。
在本实施例的一个可选实现方式中，所述地面参数包括地面法向量参数和地面到世界坐标系原点的距离参数。
该可选的实现方式中，平面的表示方法有多种，如Hesse形式、球坐标形式、切平面形式和最近点形式。本公开实施例在SLAM地图构建时需要引入地面作为约束，而地面作为一种平面在优化过程中的表示可以选择上述已有的多种表示之一。但是考虑到选用Hesse形式、球坐标形式和切平面形式时均存在各种问题，比如Hesse形式会时候优化结果过参数化，而球坐标形式在某种情形下会产生奇异性，切平面形式则只对法线进行微调，且每一次迭代优化需重新计算基向量；而最近点形式应用在本公开实施例中的后端优化过程中，则无明显缺点并且较为直观。因此，在构建成本函数时，通过该最近点形式表示地面参数，并且距离误差也通过地面三维点与该地面的距离大小来构建。
最近点形式的地面参数如下表示：
Figure BDA0003569211700000101
其中，π表示地面，
Figure BDA0003569211700000102
是地面的法向量参数，d是地面到世界坐标系原点的距离。需要说明的是，世界坐标系通常是移动设备进入未知环境之后采集作为初始地图的图像时所在的初始位置作为原点，并且以该移动设备上的图像传感器当前的旋转参数为相对于世界坐标系的旋转参数，而平移参数为0所建立的世界坐标系。
地面到世界坐标系原点的距离可以理解为该原点到地面的垂直距离。
在本实施例的一个可选实现方式中，所述方法进一步还包括以下步骤：
提取所述当前关键帧中的特征点；
确定所述特征点对应的当前三维点；
基于所述当前三维点拟合平面的方式，确定所述当前三维点中位于地面的地面三维点。
该可选的实现方式中，为了将地面约束引入关键帧的位姿优化过程，因此在获得当前关键帧之后，对当前关键帧进行特征点提取处理，并对所提取的特征点进行三角化等处理后，确定特征点对应的三维点。该三维点可以理解为当前关键帧中的特征点对应至所构建的地图上的点，也是周边环境中的真实点。从关键帧所提取的特征点对应的三维点通常被称之为待跟踪点，用于在各个关键帧中进行特征匹配，进而构建SLAM地图。
在确定了从当前关键帧中的特征点对应的三维点后，可以基于这些三维点拟合得到一个或多个平面，通过比较该平面的法向量与之前关键帧中确定的地面的法向量之间的关系，从而能够确定该一个或多个平面中是否存在对应于地面的平面，并且可以将对应于地面的平面上的三维点以及距离该对应于地面的平面较近的三维点确定为地面三维点。
在本实施例的一个可选实现方式中，基于所述当前三维点拟合平面的方式，确定所述当前三维点中位于地面的地面三维点的步骤，进一步包括以下步骤：
基于所述当前三维点拟合出一个或多个平面；
确定所述一个或多个平面中对应于地面的目标平面；
确定所述当前三维点与所述目标平面之间的距离；
基于所述距离从所述当前三维点中筛选出地面三维点。
该可选的实现方式中，在第二阶段的平面优化过程中，由于优化仅针对的是地面参数，而成本函数中地面参数的约束依据的是理论上位于地面的地面三维点到地面的距离误差。因此，针对新得到的当前关键帧，可以通过从中提取特征点，并匹配得到该些特征点对应的三维点，以及基于三维点拟合平面的方式得到该些三维点所构成的平面。之后，还可以基于已知的地面参数(也即利用之前的关键帧优化得到的地面参数)在世界坐标系下的法向量和拟合得到的平面的法向量之间的关系，确定拟合得到平面中是否存在对应于地面的目标平面，例如可以将与已知地面参数中的法向量平行的平面确定为对应于地面的平面。
在确定了当前关键帧中对应于地面的平面后，还可以从当前关键帧中提取并处理得到的当前三维点中筛选出地面三维点。
在一些实施例中，可以计算当前三维点与该目标平面之间的距离，在该距离小于或等于预设距离阈值时，可以认为该当前三维点可能是地面三维点，只是由于误差而导致与地面具有一定的距离，并且该距离足够小，因此可以将这类三维点确定为地面三维点，从而在第二阶段平面优化过程中，可以利用地面三维点对地面参数进行优化。
在本实施例的一个可选实现方式中，当前优化过程的第一阶段的局部地图优化中，所述地面参数对应的所述初始值，为上一优化过程的第二阶段的平面优化过程中得到的优化结果。
该可选的实现方式中，第一阶段的局部地图优化中，成本函数中基于地面参数计算的距离误差部分，地面参数被固定为初始值。SLAM系统中后端优化是一个迭代循环的过程，每一次迭代时都会加入新的关键帧，也即当前关键帧，并剔除时间最久远的一个关键帧。上一优化过程的第二阶段的平面优化得到的地面参数的优化结果，可以作为当前优化过程的第一阶段的局部地图优化时的初始值。
图4示出根据本公开一实施方式的一种无人移动设备的结构框图。如图4所示，该无人移动设备包括：图像传感器401和处理设备402；其中，
所述图像传感器401采集所述无人移动设备周边环境的图像，以及将所述图像输出至所述处理设备402；
所述处理设备402获取所述图像中的多个连续关键帧，以及所述多个连续关键帧中均能够观测到的多个目标三维点；
所述处理设备402还利用所述连续关键帧以及所述目标三维点进行第一阶段的局部地图优化，以及利用所述连续关键帧以及所述目标三维点中的所述地面三维点进行第二阶段的平面优化；其中，在第一阶段的局部地图优化过程中，将所述距离误差中的地面参数固定为初始值后，对所述连续关键帧的位姿参数进行优化；在第二阶段的平面优化过程中，将所述连续关键帧的位姿参数固定为第一阶段优化得到的优化结果后，对所述连续关键帧中的地面参数进行优化。
本实施例中，图像传感器401可以是设置在无人移动设备上的单目相机等能够采集周边环境的图像的传感器。处理设备402可以是CPU、GPU、FPGA、NPU等处理单元。处理设备402可以根据存储在无人移动设备内部的只读存储器中的程序或者从外部存储介质加载到随机访问存储器中的程序而执行本公开上述即时定位与地图构建方法中的各种处理。
本实施例中，无人移动设备进入未知环境后，图像传感器401实时采集该未知环境中的图像，并通过图像传感器401和处理设备402之间的通信链路传输至处理设备402。处理设备402接收到图像后，利用SLAM系统对其进行前端优化以及后端优化，后端优化的结果传送至服务器进而全局地图的构建。
处理设备402在后端优化过程中，采用如上文中所述的第一阶段的局部地图优化以及第二阶段的平面优化，从而获得当前迭代优化中各个关键帧的位姿参数以及地面参数
本公开实施例中处理设备执行的即时定位与地图构建方法的具体细节可以参见上文中的描述，在此不再赘述。
根据本公开一实施方式的基于位置的服务提供方法，该基于位置的服务提供方法包括：利用上述即时定位与地图构建方法进行后端优化，并基于后端优化的结果构建周边环境地图，以及基于所述周边环境地图为被服务对象提供基于位置的服务，所述基于位置的服务包括：导航、路线规划、地图渲染中的一种或多种。
本实施例中，该基于位置的服务提供方法可以在服务器上执行，被服务对象可以是无人移动设备，例如可以是无人机、无人驾驶车辆、机器人等无人系统。被服务对象上可以设置图像传感器。图像传感器采集无人移动设备周边环境的图像，无人移动设备上的处理设备可以基于图像构建周边环境地图。该构建周边环境地图时，处理设备可以对图像中的关键帧所对应的位姿参数以及地面参数进行后端优化，后端优化过程可以参见上文中即时定位与地图构建方法的描述，在此不再赘述。
基于后端优化得到的位姿参数、地面参数等可以在无人移动设备构建局部地图，而该局部地图可以发送至服务器，用于构建周边环境的全局地图，服务器可以利用该全局地图对无人移动设备进行定位，以及基于定位结果为无人移动设备提供导航、路径规划、地图渲染等位置服务。
图5示出根据本公开一实施方式中即时定位地图构建方法的一种场景示意图。如图5所示，无人移动设备501包括图像传感器5011和处理设备5012，无人移动设备501进入未知环境之后，图像传感器5011采集周边环境中的图像，处理设备5012对采集的图像进行处理，构建该周边环境的局部地图，该局部地图中包括滑动窗口内多个连续关键帧对应的位姿参数以及地面参数，该局部地图可以被发送至服务器502，该服务器502基于该局部地图以及之前从无人移动设备501接收到的其他局部地图信息，优化全局地图。
服务器502还基于该全局地图为无人移动设备501提供位置服务，例如确定无人移动设备501在未知环境中的位置，进而再基于该位置指示无人移动设备501的移动动作等。
下述为本公开装置实施例，可以用于执行本公开方法实施例。
图6示出根据本公开一实施方式的即时定位地图构建装置的结构框图。该装置可以通过软件、硬件或者两者的结合实现成为电子设备的部分或者全部。如图6所示，该即时定位地图构建装置包括：
获取模块601，被配置为获取多个连续关键帧，以及所述多个连续关键帧中均能够观测到的多个目标三维点；
第一优化模块602，被配置为利用所述连续关键帧以及所述目标三维点进行第一阶段的局部地图优化；其中，在第一阶段的局部地图优化过程中，将所述距离误差中的地面参数固定为初始值后，对所述连续关键帧的位姿参数进行优化；
第二优化模块603，被配置为利用所述连续关键帧以及所述目标三维点中的所述地面三维点进行第二阶段的平面优化；在第二阶段的平面优化过程中，将所述连续关键帧的位姿参数固定为第一阶段优化得到的优化结果后，对所述连续关键帧中的地面参数进行优化。
在本实施例的一个可选实现方式中，所述第一阶段的局部地图优化和所述第二阶段的局部地图优化中均使用成本函数进行优化，成本函数包括所述目标三维点在多个连续关键帧上的重投影误差以及所述目标三维点中地面三维点距离地面的距离误差；所述重投影误差基于所述目标三维点以及所述多个连续关键帧的位姿参数得到；所述距离误差基于所述地面三维点以及所述地面参数得到。
在本实施例的一个可选实现方式中，所述获取模块，包括：
第一获取子模块，被配置为获取当前关键帧、第一历史关键帧以及所述当前关键帧和所述第一历史关键帧之间的多个连续历史关键帧；
其中，在第一阶段的局部地图优化中，所述第一历史关键帧对应的位姿参数固定不变，而当前关键帧、当前关键帧和第一历史关键帧之间的多个历史关键帧的位姿参数为待优化参数。
在本实施例的一个可选实现方式中，所述地面参数包括地面法向量参数和地面到世界坐标系原点的距离参数。
在本实施例的一个可选实现方式中，所述装置还包括：
提取模块，被配置为提取所述当前关键帧中的特征点；
第一确定模块，被配置为确定所述特征点对应的当前三维点；
第二确定模块，被配置为基于所述当前三维点拟合平面的方式，确定所述当前三维点中位于地面的地面三维点。
在本实施例的一个可选实现方式中，所述第二确定模块，包括：
拟合子模块，被配置为基于所述当前三维点拟合出一个或多个平面；
第三确定子模块，被配置为确定所述一个或多个平面中对应于地面的目标平面；
第四确定子模块，被配置为确定所述当前三维点与所述目标平面之间的距离；
筛选子模块，被配置为基于所述距离从所述当前三维点中筛选出地面三维点。
在本实施例的一个可选实现方式中，当前优化过程的第一阶段的局部地图优化中，所述地面参数对应的所述初始值，为上一优化过程的第二阶段的平面优化过程中得到的优化结果。
本实施例中的即时定位与地图构建装置与上文中的即时定位与地图构建方法对应一致，具体细节可以参见上文中对即时定位与地图构建方法的描述，在此不再赘述。
根据本公开一实施方式的基于位置的服务提供装置，该基于位置的服务提供装置包括：利用上述即时定位与地图构建装置进行后端优化，并基于后端优化的结果构建周边环境地图，以及基于所述周边环境地图为被服务对象提供基于位置的服务，所述基于位置的服务包括：导航、路线规划、地图渲染中的一种或多种。
本实施例中，该基于位置的服务提供装置可以在服务器上执行，被服务对象可以是无人移动设备，例如可以是无人机、无人驾驶车辆、机器人等无人系统。被服务对象上可以设置图像传感器。图像传感器采集无人移动设备周边环境的图像，无人移动设备上的处理设备可以基于图像构建周边环境地图。该构建周边环境地图时，处理设备可以对图像中的关键帧所对应的位姿参数以及地面参数进行后端优化，后端优化过程可以参见上文中即时定位与地图构建装置的描述，在此不再赘述。
基于后端优化得到的位姿参数、地面参数等可以在无人移动设备构建局部地图，而该局部地图可以发送至服务器，用于构建周边环境的全局地图，服务器可以利用该全局地图对无人移动设备进行定位，以及基于定位结果为无人移动设备提供导航、路径规划、地图渲染等位置服务。
图7是适于用来实现根据本公开实施方式的即时定位与地图构建方法和/或基于位置的服务提供方法的电子设备的结构示意图。
如图7所示，电子设备700包括处理单元701，其可实现为CPU、GPU、FPGA、NPU等处理单元。处理单元701可以根据存储在只读存储器(ROM)702中的程序或者从存储部分708加载到随机访问存储器(RAM)703中的程序而执行本公开上述任一方法的实施方式中的各种处理。在RAM703中，还存储有电子设备700操作所需的各种程序和数据。处理单元701、ROM702以及RAM703通过总线704彼此相连。输入/输出(I/O)接口705也连接至总线704。
以下部件连接至I/O接口705：包括键盘、鼠标等的输入部分706；包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等的输出部分707；包括硬盘等的存储部分708；以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分709。通信部分709经由诸如因特网的网络执行通信处理。驱动器710也根据需要连接至I/O接口705。可拆卸介质711，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器710上，以便于从其上读出的计算机程序根据需要被安装入存储部分708。
特别地，根据本公开的实施方式，上文参考本公开实施方式中的任一方法可以被实现为计算机软件程序。例如，本公开的实施方式包括一种计算机程序产品，其包括有形地包含在机器可读介质上的计算机程序，所述计算机程序包含用于执行本公开实施方式中任一方法的程序代码。在这样的实施方式中，该计算机程序可以通过通信部分709从网络上被下载和安装，和/或从可拆卸介质711被安装。
附图中的流程图和框图，图示了按照本公开各种实施方式的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，路程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本公开实施方式中所涉及到的单元或模块可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的单元或模块也可以设置在处理器中，这些单元或模块的名称在某种情况下并不构成对该单元或模块本身的限定。
作为另一方面，本公开还提供了一种计算机可读存储介质，该计算机可读存储介质可以是上述实施方式中所述装置中所包含的计算机可读存储介质；也可以是单独存在，未装配入设备中的计算机可读存储介质。计算机可读存储介质存储有一个或者一个以上程序，所述程序被一个或者一个以上的处理器用来执行描述于本公开的方法。
以上描述仅为本公开的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本公开中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离所述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本公开中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。

Claims (10)

1.一种即时定位与地图构建方法，其中，包括：
获取多个连续关键帧，以及所述多个连续关键帧中均能够观测到的多个目标三维点；
利用所述连续关键帧以及所述目标三维点进行第一阶段的局部地图优化；其中，在第一阶段的局部地图优化过程中，将所述距离误差中的地面参数固定为初始值后，对所述连续关键帧的位姿参数进行优化；
利用所述连续关键帧以及所述目标三维点中的所述地面三维点进行第二阶段的平面优化；在第二阶段的平面优化过程中，将所述连续关键帧的位姿参数固定为第一阶段优化得到的优化结果后，对所述连续关键帧中的地面参数进行优化。
2.根据权利要求1所述的方法，其中，所述第一阶段的局部地图优化和所述第二阶段的局部地图优化中均使用成本函数进行优化，成本函数包括所述目标三维点在多个连续关键帧上的重投影误差以及所述目标三维点中地面三维点距离地面的距离误差；所述重投影误差基于所述目标三维点以及所述多个连续关键帧的位姿参数得到；所述距离误差基于所述地面三维点以及所述地面参数得到。
3.根据权利要求1或2所述的方法，其中，获取多个连续关键帧，包括：
获取当前关键帧、第一历史关键帧以及所述当前关键帧和所述第一历史关键帧之间的多个连续历史关键帧；
其中，在第一阶段的局部地图优化中，所述第一历史关键帧对应的位姿参数固定不变，而当前关键帧、当前关键帧和第一历史关键帧之间的多个历史关键帧的位姿参数为待优化参数。
4.根据权利要求1或2所述的方法，其中，所述地面参数包括地面法向量参数和地面到世界坐标系原点的距离参数。
5.根据权利要求1或2所述的方法，其中，所述方法还包括：
提取所述当前关键帧中的特征点；
确定所述特征点对应的当前三维点；
基于所述当前三维点拟合平面的方式，确定所述当前三维点中位于地面的地面三维点。
6.根据权利要求5所述的方法，其中，基于所述当前三维点拟合平面的方式，确定所述当前三维点中位于地面的地面三维点，包括：
基于所述当前三维点拟合出一个或多个平面；
确定所述一个或多个平面中对应于地面的目标平面；
确定所述当前三维点与所述目标平面之间的距离；
基于所述距离从所述当前三维点中筛选出地面三维点。
7.根据权利要求1-2、5任一项所述的方法，其中，当前优化过程的第一阶段的局部地图优化中，所述地面参数对应的所述初始值，为上一优化过程的第二阶段的平面优化过程中得到的优化结果。
8.一种无人移动设备，包括：图像传感器和处理器；其中，
所述图像传感器采集所述无人移动设备周边环境的图像，以及将所述图像输出至所述处理器；
所述处理器获取所述图像中的多个连续关键帧，以及所述多个连续关键帧中均能够观测到的多个目标三维点；
所述处理器还利用所述连续关键帧以及所述目标三维点进行第一阶段的局部地图优化，以及利用所述连续关键帧以及所述目标三维点中的所述地面三维点进行第二阶段的平面优化；其中，在第一阶段的局部地图优化过程中，将所述距离误差中的地面参数固定为初始值后，对所述连续关键帧的位姿参数进行优化；在第二阶段的平面优化过程中，将所述连续关键帧的位姿参数固定为第一阶段优化得到的优化结果后，对所述连续关键帧中的地面参数进行优化。
9.一种基于位置的服务提供方法，其中，包括：利用权利要求1-7任一项所述的方法进行后端优化，并基于后端优化的结果构建周边环境地图，以及基于所述周边环境地图为被服务对象提供基于位置的服务，所述基于位置的服务包括：导航、路线规划、地图渲染中的一种或多种。
10.一种计算机程序产品，其包括计算机指令，其中，该计算机指令被处理器执行时实现权利要求1-7任一项所述的方法。
CN202210316869.1A 2022-03-28 2022-03-28 即时定位与地图构建方法及无人移动设备 Pending CN114858156A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210316869.1A CN114858156A (zh) 2022-03-28 2022-03-28 即时定位与地图构建方法及无人移动设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210316869.1A CN114858156A (zh) 2022-03-28 2022-03-28 即时定位与地图构建方法及无人移动设备

Publications (1)

Publication Number Publication Date
CN114858156A true CN114858156A (zh) 2022-08-05

Family

ID=82628874

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210316869.1A Pending CN114858156A (zh) 2022-03-28 2022-03-28 即时定位与地图构建方法及无人移动设备

Country Status (1)

Country Link
CN (1) CN114858156A (zh)

Similar Documents

Publication Publication Date Title
CN111862126B (zh) 深度学习与几何算法结合的非合作目标相对位姿估计方法
US11176701B2 (en) Position estimation system and position estimation method
CN111174799B (zh) 地图构建方法及装置、计算机可读介质、终端设备
CN107980150B (zh) 对三维空间建模
CN111899282B (zh) 基于双目摄像机标定的行人轨迹跟踪方法及装置
CN105809687B (zh) 一种基于图像中边沿点信息的单目视觉测程方法
EP2575104B1 (en) Enhancing video using super-resolution
EP2423871B1 (en) Apparatus and method for generating an overview image of a plurality of images using an accuracy information
CN111829532B (zh) 一种飞行器重定位系统和重定位方法
JPH11252440A (ja) 画像を整列させて目標点にカメラを固定する方法及び装置
KR102321998B1 (ko) 환경 변화에 강인한 이미지의 위치 및 방향 추정 방법 및 시스템
WO2018235923A1 (ja) 位置推定装置、位置推定方法、及びプログラム
CN113496503B (zh) 点云数据的生成及实时显示方法、装置、设备及介质
Bao et al. Robust tightly-coupled visual-inertial odometry with pre-built maps in high latency situations
CN110688500B (zh) 一种数据库构建方法、一种定位方法及其相关设备
CN117876608A (zh) 三维图像重建方法、装置、计算机设备及存储介质
CN111709990A (zh) 一种相机重定位方法和系统
CN114858156A (zh) 即时定位与地图构建方法及无人移动设备
CN112085842A (zh) 深度值确定方法及装置、电子设备和存储介质
CN111581322B (zh) 视频中兴趣区域在地图窗口内显示的方法和装置及设备
CN111489439B (zh) 一种三维线图重建方法、装置和电子设备
CN115014324A (zh) 定位方法、装置、介质、设备及车辆
CN111260544B (zh) 数据处理方法及装置、电子设备和计算机存储介质
CN114119973A (zh) 一种基于图像语义分割网络的空间距离预测方法及系统
CN112991388A (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