一种基于先验语义地图结构信息和语义信息的视觉定位算法

摘要

本发明属于视觉SLAM领域，提出了一种基于先验语义地图结构信息和语义信息的视觉定位算法。本发明通过一种融合先验语义地图语义信息和结构信息的混合约束，将先验语义地图因子引入到视觉定位中。随后，利用视觉路标与先验语义地图之间的数据关联，使用期望最大化算法同时优化数据关联和相机位姿，提高视觉定位的精度和鲁棒性。该算法能够有效限制视觉里程计的漂移误差并提高视觉定位精度，以服务于导航等应用场景。在满足实时性的要求下，取得了较高的定位精度。

说明书

技术领域

本发明涉及视觉SLAM(simultaneous localization and mapping)领域，具体涉及一种基于先验语义地图结构信息和语义信息的视觉定位算法。

背景技术

随着移动机器人和无人驾驶的应用越来越广泛，如何为自主车辆等相关硬件设备提供准确、鲁棒的位姿成为急需解决的问题。视觉SLAM技术通过相机获取图像估计自身位姿以及建图。作为当前主流的定位技术之一，视觉SLAM方法由于相机低成本、轻量化的特点，吸引了大量的研究工作。然而，相比基于激光雷达的SLAM算法，传统视觉SLAM主要依靠环境中的点、线、面等特征来完成自身的定位和建图，使其非常容易受到环境光照、视角等因素的影响。在自动驾驶或移动机器人领域中，一种可行的方案是利用离线构建的高精度地图提供约束来限制视觉里程计的累积漂移误差。这种做法既可以保证视觉定位的精度，还可以提高其鲁棒性，使视觉定位能够满足实际应用的需求。

《IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS),4588-4594,2020》中量化先验语义地图中的3D线特征，将图像上提取的2D线特征与3D线特征进行匹配。基于这种2D-3D线特征的数据关联，为视觉定位系统引入先验语义地图约束，限制视觉里程计的累积漂移误差。《Journal of Field Robotics,1003-1026,2020》提出一种ProW-NDT点云匹配算法估计局部视觉地图和先验激光地图的坐标变换。为了融合点云配准和视觉里程计的结果，利用局部位姿图优化算法对滑窗内的关键帧位姿进行优化。

《IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS),4588-4594,2020》和《Journal of Field Robotics,1003-1026,2020》中所述的视觉定位算法都仅仅利用先验语义地图的结构信息，没有考虑先验语义地图中的语义信息。在室外大场景中进行视觉定位时，仅仅利用先验语义地图中的结构信息可能导致定位鲁棒性下降甚至失效。为了保证视觉定位的精度和鲁棒性，需要引入先验语义地图中语义信息和结构信息的共同约束。

发明内容

本发明的目的就是提出一种基于先验语义地图结构信息和语义信息的视觉定位算法，提高视觉定位的精度和鲁棒性。

本发明的技术方案：一种基于先验语义地图结构信息和语义信息的视觉定位算法，具体步骤如下：

(1)以先验语义地图M获取结构信息与语义信息，以视觉图像特征U和语义分割图像C分别作为系统观测O，O＝{U,C}；设相机位姿为T，视觉图像与先验语义地图之间的坐标变换关系为S，利用先验语义地图M和系统观测O估计相机状态X＝{T,S}和语义路标的坐标P；获得相机状态、语义路标、先验语义地图和系统观测四者的后验概率估计并进行最大化估计，获得视觉观测因子、语义追踪因子和先验语义地图因子：

p(X,P|M,O)∝p(U|P,T)·p(C|Z)·p(P|M,S) (1)

其中，Z表示语义路标的语义标签，p(U|P,T)为视觉观测因子，p(C|Z)为语义追踪因子，p(P|M,S)为先验语义地图因子；

(2)基于视觉特征描述子特征匹配视觉图像，对语义路标进行连续视觉跟踪；

设第k帧图像的相机位姿为T

其中，π(·)表示重投影函数，u

(3)语义分割图像基于狄利克雷分布对语义路标进行连续语义跟踪，利用语义追踪因子p(C|Z)及语义分割图像C估计语义路标的语义标签Z；

定义g

利用多帧语义观测C

D

其中Dir(·)表示狄利克雷分布，

其中

(4)定位线程中，先验语义地图M按照其语义标签进一步分为多个子地图M

(4.1)对于式(5)中的结构信息

其中，

对于平面结构，式(6)中的残差项建模为点到直线的距离，即：

其中

对于非平面结构，采用ICP算法中的残差形式，即：

(4.2)式(5)中的p(z

其中，γ为平衡系数；

(5)在定位线程中，基于提出的混合约束，使用期望最大化算法求解先验语义地图和世界坐标系之间的坐标变换

使用期望最大化算法估计视觉图像初始帧与先验语义地图之间的坐标变换

1)E步：对于每个语义路标，使用最近邻搜索以变量Z构建语义路标与先验语义地图之间的数据关联，并用式(9)估计每个数据关联的权重w

2)M步：基于数据关联以及基于式(9)估计得到的权重w

其中，S

重复E步和M步过程，直到收敛或者达到设定的迭代次数；最后，当前相机在先验语义地图中的位姿通过

基于求解得到的坐标变换

其中，

所述平面结构为道路、人行道、墙壁。

本发明的有益效果：针对以往基于先验语义地图的算法仅仅利用先验语义地图中的几何结构信息的不足，本发明提出一种基于先验语义地图结构信息和语义信息的视觉定位算法。该发明提取先验语义地图的结构信息和语义信息作为先验约束。不同于其它利用先验语义地图中几何结构特征约束相机位姿的方式，本发明通过一种融合先验语义地图语义信息和结构信息的混合约束，将先验语义地图因子引入到视觉定位中。随后，利用视觉路标与先验语义地图之间的数据关联，使用期望最大化(EM)算法同时优化数据关联和相机位姿，提高视觉定位的精度和鲁棒性。该算法能够有效限制视觉里程计的漂移误差并提高视觉定位精度，以服务于导航等应用场景。在满足实时性的要求下，取得了较高的定位精度。

附图说明

图1基于先验语义地图结构信息和语义信息的视觉定位算法流程图

具体实施方式

图1是本发明技术方案的主流程图。本发明提出的基于先验语义地图结构信息和语义信息的视觉定位算法，以先验语义地图M作为输入获取结构信息与语义信息，以视觉特征U和语义分割图像C分别作为系统观测O＝{U,C}。设相机位姿为T，视觉图像与先验语义地图之间的坐标变换为S，该系统利用先验语义地图M和系统观测估计相机状态X＝{T,S}和视觉路标P。整个问题可以建模为最大后验概率估计，将其作为视觉定位算法的先验约束，限制视觉里程计的漂移误差。其最大后验概率估计的表示形式为：

p(X,P|M,O)∝p(U|P,T)·p(C|Z)·p(P|M,S)， (1)

其中，Z表示视觉路标的语义标签。在式(1)中，p(U|P,T)为视觉观测因子，p(C|Z)为语义追踪因子，p(P|M,S)为先验语义地图因子。

如图1所示，本发明提出的基于先验语义地图结构信息和语义信息的视觉定位算法，包括下列步骤：

(1)在追踪线程中，将视觉图像和语义分割图像作为系统观测，基于视觉特征描述子和Dirichlet分布对语义路标进行连续跟踪。

在前端中，根据特征匹配建立视觉路标和ORB(Oriented FAST and RotatedBRIEF)特征之间的数据关联，利用PnP(Perspective-n-Point)算法估计每帧图像的初始位姿T

其中，π(·)表示重投影函数；u

对于式(1)中的语义追踪因子p(C|Z)，它将语义分割结果作为语义分割图像C来估计视觉路标的语义标签Z。系统并不直接量化分布p(C|Z)，而是估计语义属性Z的概率分布。我们定义g

D

其中Dir(·)表示狄利克雷分布，

其中

(2)在定位线程中，提取先验语义地图中的结构信息和语义信息构建一种混合约束，作为先验约束：

利用式(1)中的先验语义地图因子p(P|M,S)进一步引出融合了先验语义地图语义信息p(z

先验语义地图M按照语义标签进一步分为多个子地图M

另外一个方面，由于实际室外场景中存在大量的平面特征，所以将先验语义地图中的结构信息进一步分为平面结构和非平面结构两方面进行考虑。

对于先验语义地图的道路、人行道等平面结构，式(5)中的结构信息建模为点到直线的距离，即：

其中

对于其余的非结构化场景，采用ICP算法中的残差形式，即：

(3)在定位线程中，基于提出的混合约束使用EM(Expectation-Maximization)算法求解相机在先验语义地图中的6自由度位姿。

基于式(5)中的混合约束，对于第k帧图像，利用EM算法求解先验语义地图和视觉图像对应的世界坐标系之间的坐标变换

其中，γ为平衡系数。

因此，权重w

1)E步：对于每个视觉路标，使用最近邻搜索以变量Z构建视觉路标与先验语义地图之间的数据关联并用式(8)估计每个数据关联的权重w

2)M步：基于一系列的数据关联以及估计的权重，可以通过以下优化模型求解S：

其中，

另外，为了提高追踪线程中位姿估计的精度。本系统基于求解得到的坐标变换S进一步对语义路标的3D坐标进行求精。设优化前的路标点坐标为

其中，

本发明在室外KITTI数据集的9个序列上分别对双目和单目系统进行了测试。在室外KITTI数据集上，双目系统的平均定位误差为0.5216m，单目系统的平均定位误差为2.1838m。另外，对系统定位的耗时进行了实验测试，单帧定位耗时在78.77ms左右。根据实验结果，本发明提出的系统在满足实时性的要求下，取得了较高的定位精度。

表1系统ATE误差测试结果，单位:米