基于多源数据和方向先验联合优化的定位方法

技术领域

本发明属于计算机视觉和机器人技术领域，涉及同步定位与地图构建技术，具体涉及一种基于多源数据和方向先验联合优化的定位方法，可应用于车载平台同步定位与地图构建，通过从灭点得到少量地图方向先验，并结合二维、三维、方向先验的能量函数并进行联合优化，用于从图像、激光点云序列和少量地图方向得到每个时刻的六自由度位姿，提高车载平台定位精度。

背景技术

同步定位与地图构建(SLAM)是当前计算机视觉和机器人领域中的热门技术。它是人类和移动智能体的基本需求，得到了相关产业界的关注和应用，成为各类机器人、无人驾驶、虚拟现实等新兴应用的核心基础算法。SLAM算法想要解决的问题是，在未知环境中利用传感器(比如相机、激光雷达、惯性测量单元等)运动时获取的观测数据，求解出自身的六自由度位姿，与此同时建立周围环境的三维地图。现有经典视觉SLAM系统仅从图像上提取颜色或几何信息作为视觉线索来求解位姿，例如文献[1](Mur-Artal,Raul,and Juan D.Tardós."Orb-slam2:An open-source slam system for monocular,stereo,and rgb-dcameras."IEEE transactions on robotics 33.5(2017):1255-1262.)，但仅依靠相机会面临单目相机的尺度漂移，剧烈光照变化，纹理缺失等挑战性场景下定位失败的问题。也有方法尝试将图像和激光雷达点云进行数据关联的融合，主要是把激光雷达点云的深度信息结合到视觉线索上，例如文献[2](Huang,Shi-Sheng,et al."Lidar-monocular visualodometry using point and line features."2020IEEE International Conference onRobotics and Automation(ICRA).IEEE,2020.)，但该方法可能受限于激光雷达的线束数量限制，图像中的点、线特征并非总能得到完全对应的质量好的深度值。上述方法都属于松耦合，没有能充分利用多源数据、多重线索的互补性，所以方法的定位精度不高，需要提升。

发明内容

为了克服上述现有技术存在的不足，本发明提出一种基于多源数据和方向先验联合优化的定位方法，更加紧密地融合多源数据和地图方向先验，从融合多种几何约束进行联合优化的角度，利用多源数据和地图方向先验所蕴含的多重几何线索设计二维、三维、方向先验的能量函数，来解决车载平台同步定位与地图构建的技术问题，能求解出更准确的车载平台定位结果。

图像能提供丰富的场景表观信息，而激光雷达点云能得到更准确的三维场景几何结构，地图的主要方向能进一步约束汽车行驶的朝向的一个分量。本发明包括以下几方面：1)对每帧激光雷达点云计算特征，基于隐式移动最小二乘(IMLS)，通过点云-地图的点云配准得到每帧点云的初始位姿，输入到视觉同步定位与地图构建(SLAM)系统ORB-SLAM2(Oriented FAST and rotated BRIEF-Simultaneous localization and mapping v2，即基于ORB的同步定位与地图构建中，建立关键帧3d点云间基于隐式移动最小二乘(IMLS)约束；2)通过ORB-SLAM2使用上述1)的点云的初始位姿来得到包含图像和点云的关键帧数据结构，并建立稀疏视觉ORB特征点地图，其中含有关键帧间共视图，与之对应的是2d特征-3d地图点之间的对应关系，即建立2d特征-3d地图点之间的重投影误差约束；3)检测图像灭点，并利用少量的地图三维方向先验信息和灭点建立三维方向的投影约束，即建立地图三维方向先验的投影约束；最终联合以上三类约束：2d特征-3d地图点之间的重投影误差约束、关键帧3d点云间基于隐式移动最小二乘(IMLS)约束、少量地图三维方向先验的投影约束一同构建能量函数并进行优化。

本发明方法输入的数据模态包括双目RGB图像和点云，均可采用车载平台的相机和激光雷达采集到的数据。本发明提供的基于多源数据和方向先验联合优化的定位方法详细技术方案包括如下步骤：

A.基于隐式移动最小二乘(IMLS)方法，进行激光点云配准，得到每帧点云的初始位姿；

A1.点云数据预处理，包括以下4个步骤：

1)使用线性运动模型对点云进行校正，解决激光雷达自运动的问题；

2)去除点云中的运动物体，这里分为两种情况：如果是将KITTI数据集(参考Geiger,Andreas,Philip Lenz,and Raquel Urtasun."Are we ready for autonomousdriving？the kitti vision benchmark suite."2012IEEE conference on computervision and pattern recognition.IEEE,2012.)中的点云作为输入，可以使用来自公开数据集SemanticKITTI(参考Behley,Jens,et al."Semantickitti:A dataset for semanticscene understanding of lidar sequences."Proceedings of the IEEE/CVFInternational Conference on Computer Vision.2019.)的点云语义标签，滤除掉点云语义标签被标记为运动物体类的点；对于其他点云数据，由于没有现有的点云语义标签，需对三维点云进行聚类得到若干点云团，去除体积较小的点云团，认为这些点云团是运动物体；

3)体素网格下采样；

4)邻域半径滤波；

A2.点云特征的计算。

对上一步预处理之后的点云，使用每点的邻域计算其特征向量，该特征向量的计算方式来自文献(Deschaud,Jean-Emmanuel."IMLS-SLAM:scan-to-model matching basedon 3D data."2018IEEE International Conference on Robotics and Automation(ICRA).IEEE,2018.)，最后对于点云中的每一个点，都得到了它的特征向量，向量长度为9；

A3.构建历史点云地图，基于隐式移动最小二乘求解点到面的ICP(IterativeClosest Point，迭代最近点)，得到当前帧点云的位姿；

B.基于ORB-SLAM2来构建包含2d特征-3d地图点之间的重投影误差约束、关键帧3d点云间基于隐式移动最小二乘(IMLS)约束的图优化(graph-based optimization)；

B1.对来自车载平台上的相机采集的双目RGB图像进行数据处理，包含转为双目灰度图像和ORB特征提取；

B2.使用步骤A中得到的每帧数据的位姿，在每帧双目灰度图像和稀疏视觉ORB特征点地图建立2d特征-3d地图点的对应关系，也就得到了图像帧之间ORB特征间的对应关系；

B3.把步骤A2处理后的含有特征的点云数据和同时刻的双目灰度图像封装为新的关键帧数据结构，其中，激光雷达点云转为相机参考系下，该关键帧数据结构还包含了B1中对双目灰度图提取的ORB特征和A2中得到的点云特征；

B4.ORB-SLAM2的局部建图(Local mapping)线程不再进行捆集调整(Bundleadjustment)，只对稀疏视觉ORB特征点地图插入新关键帧、更新共视关系、三角化建立新的地图点等；

B5.取出全部的关键帧，根据视觉地图中图像2d特征-3d地图点之间的对应关系建立地图点到关键帧上特征点的视觉重投影误差，对于任一关键帧，取出和其有共视关系且时间戳在其之前的关键帧，两两之间建立3d-3d间的对应，仍是基于隐式移动最小二乘的匹配；

C.结合少量地图三维方向先验的投影约束进行联合优化；包括：

C1.检测每帧图像的灭点(Vanishing point)并保存；

C2.对灭点根据投影几何得到三维方向，通过三维空间的mean-shift聚类，得到若干个聚类中心，这些聚类中心所代表的方向就是来自于地图的少量三维方向先验，而且只有一个自由度的信息；

C3.输入得到的少量三维方向先验到对应灭点所在图像的关键帧数据结构中，于是这些三维方向先验都对应于各自关键帧，建立三维方向到关键帧上灭点的投影误差约束；

C4.联合B5中得到的2d特征-3d地图点之间的重投影误差约束、关键帧3d点云间基于隐式移动最小二乘(IMLS)约束和C3中得到的少量地图三维方向先验的投影约束，构建出能量函数进行全局联合优化，求解出各关键帧位姿，即实现了定位。

通过上述步骤，即实现基于多源数据和方向先验联合优化的定位。

与现有技术相比，本发明的有益技术效果是：

本发明提出一种基于多源数据和方向先验联合优化的定位方法，更加紧密的融合多源数据和地图方向先验的方法，从融合多种几何约束进行联合优化的角度来求解车载平台同步定位与地图构建的问题。解决了仅使用图像时面临尺度模糊，弱纹理等困难场景下定位失败的问题，也避免了直接把激光雷达点云深度投影到图像时深度值不完全对应的情况。本发明设计的二维、三维、方向先验的能量函数是一种能比较充分的利用多源数据和地图方向先验所蕴含的多重几何线索的方法，能求解出更准确的车载平台定位结果，提升定位精度。

附图说明

图1是本发明提供的基于多源数据和方向先验联合优化的定位方法的整体流程图。

图2是本发明中灭点检测方法和方向先验方法的示意图。

图3本发明中建立联合优化的示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明中的技术方案进行清楚完整的描述，但不以任何方式限制本发明的范围。

本发明提供一种基于多源数据和方向先验联合优化的定位方法，如图1所示的本发明的整体流程图，主要包括如下步骤：

1)基于隐式移动最小二乘(IMLS)的激光点云配准；

2)基于ORB-SLAM2，接受每帧点云的位姿得到关键帧，构建图优化(graph-basedoptimization)，包含2d特征-3d地图点之间的重投影误差约束、关键帧3d点云间基于隐式移动最小二乘(IMLS)约束；

3)从图像中检测灭点，并在三维空间对方向聚类，得到少量的聚类中心方向，和图像灭点坐标建立投影约束，和2)中的2d-3d，3d-3d一同构建能量函数进行全局联合优化。

本发明具体实施时，基于隐式移动最小二乘(IMLS)的激光点云配准包括以下步骤：

a)如发明内容中A1所述进行点云的预处理。使用线性运动模型对点云进行校正、去除点云中的运动物体这两个步骤的技术细节来自参考文献(Deschaud,Jean-Emmanuel."IMLS-SLAM:scan-to-model matching based on 3D data."2018IEEE InternationalConference on Robotics and Automation(ICRA).IEEE,2018.)。使用pcl点云处理库进行保留重心的体素网格下采样，精度为0.7m；邻域半径滤波，1.5m半径邻域内点数少于3就被删掉。

b)经过预处理后的点云，进行接下来的2个步骤，如发明内容中A2～A3中所述，这些步骤的技术细节同样来自文献(Deschaud,Jean-Emmanuel."IMLS-SLAM:scan-to-modelmatching based on 3D data."2018IEEE International Conference on Robotics andAutomation(ICRA).IEEE,2018.)，故此处不再赘述。这里最重要的就是建立3d-3d对应关系时的隐式移动最小二乘距离的计算，该算法在后面联合优化时建立关键帧间3d-3d约束时还要用到。

接下来，利用ORB-SLAM2构建联合优化图结构，包含2d-3d，3d-3d的多源数据几何约束，下面具体介绍(具体地，还可进一步参考文献(Mur-Artal,Raul,and Juan D.Tardós."Orb-slam2:An open-source slam system for monocular,stereo,and rgb-dcameras."IEEE transactions on robotics 33.5(2017):1255-1262.))：

a)Tracking线程不再进行基于图像的姿态求解，而是直接输入同一时刻点云的位姿，注意要先通过激光雷达和相机之间的标定好的相对位姿把点云转换到相机参考系下，然后在图像的特征点和当前的稀疏特征点的三维地图建立2d-3d对应关系。该线程得到的图像关键帧和得到的计算过特征后的点云数据结合构建新的关键帧数据结构；

b)Local Mapping线程不再进行原本的视觉对应关系的捆集调整(Bundleadjustment)，只进行对稀疏视觉特征点地图插入新关键帧，更新存储关键帧图像间特征点对应关系的共视图，三角化建立新的地图点，剔除冗余的关键帧和地图点。

c)Loop Closing线程也不再进行回环检测和矫正，只是在最后所有帧数据处理完毕后，开启新线程进行全局的联合优化。联合优化前，就要去建立包含2d-3d，3d-3d对应关系的约束。2d图像特征和3d特征点间的对应关系建立和ORB-SLAM2中一致，关键帧间3d-3d的约束的建立步骤如下：

i.从当前地图中得到关键帧集合：

ii.对于任一关键帧k∈[1，N-1]，得到时间戳在它之前的共视关键帧{j|j∈C

最后结合来自地图的方向先验，和图像灭点构造一个自由度的方向约束，具体过程如下：

a)从每个双目图像的左目图像检测灭点(Vanishing points)，方法可参考(Tardif J.-P.,Non-iterative Approach for Fast and Accurate Vanishing PointDetection,12th IEEE International Conference on Computer Vision,Kyoto,Japan,September 27-October 4,2009)，每张图像检测到至多3个灭点，保存像素坐标；

b)利用GPS或先验地图提供的含有灭点的图像相对于世界坐标系的旋转变换矩阵R

D＝R

对所有三维方向进行三维空间上的均值漂移(Mean shift)聚类，保留分布在地图上主要直线区域的少量三维方向簇，并保存聚类中心，和图像以及灭点相对应。

c)在联合优化图的基础上，对于那些含有灭点和三维方向的关键帧，建立该聚类中心的三维方向到图像灭点的投影误差，参照图2，图中顶部为灭点检测的3个步骤，底部为先验方向约束方法的展示，世界坐标系下的方向先验D通过当前关键帧k的旋转矩阵R

注意这里的方向约束只能提供一个自由度的约束，即沿垂直于地面的轴上的旋转，而另外两个周上的旋转不受该约束限制，所以这是一个比较弱的方向约束。

d)我们还建立了来自于图像和点云数据的两类约束。对于2d-3d对应关系，每个关键帧上特征点和视觉特征点地图之间匹配关系可用集合χ

对于3d-3d对应关系，如上一步骤所述，对于一组有共视关系的关键帧对(k,j)，可以写出基于IMLS的损失函数：

其中，x

最后，我们把以上三类损失函数加权组合在一起，即为本发明提出的融合多源数据和地图方向先验的能量函数：

其中，(α,β,γ)为三类约束的权重，实际上是根据三类约束的数量来设置，属于可调的超参数，最终联合优化的图结构见图3，图中黑色箭头左侧为捆集调整，它是ORB-SLAM2原始的优化结构，右侧联合优化图为本发明提出的能量函数E

需要注意的是，公布实施例的目的在于帮助进一步理解本发明，但是本领域的技术人员可以理解：在不脱离本发明及所附权利要求的范围内，各种替换和修改都是可能的。因此，本发明不应局限于实施例所公开的内容，本发明要求保护的范围以权利要求书界定的范围为准。