CN114323038B - 融合双目视觉和2d激光雷达的室外定位方法 - Google Patents

Abstract

融合双目视觉和2D激光雷达的室外定位方法，属于基于先验地图的机器人或自动驾驶汽车精确定位领域。本发明(1)将双目视觉作为里程计提供帧间的相对位姿，将一个局部时间窗口内的2D激光雷达数据融合成一幅局部子图；(2)利用DS证据理论对局部子图中的时态信息进行融合以消除局部子图中的动态噪音；(3)利用基于反向组合算法的地图匹配方式，将局部子图与预先构建的先验地图进行匹配，从而得到先验地图与局部子图之间的相对位姿；(4)根据匹配结果对局部子图的位姿进行校正，将校正后的位姿用于更新轨迹以消除双目里程计所产生的累积误差。

Description

融合双目视觉和2D激光雷达的室外定位方法

技术领域

本发明设计了一种融合双目视觉和2D激光雷达的室外定位方法。本方法使用双目视觉作为里程计以将一个局部时间窗口内的2D激光雷达数据融合成一幅局部子图，并利用DS证据理论对局部子图的时态信息进行融合，随后利用基于反向组合算法的地图匹配方式将局部子图与预先构建的全局先验地图进行匹配，最后，根据匹配结果对局部子图的位姿进行校正以消除双目里程计的累积误差。该方法可应用于基于先验地图的机器人或自动驾驶汽车精确定位领域，使用低成本的双目相机和2D激光雷达来实现室外环境下的高精度定位。

背景技术

随着移动机器人与智能车的不断发展，自动驾驶技术逐渐成为学术界和工业界的研究热点，而精确的定位则是其至关重要的部分，然而传统的定位方法往往都采用了昂贵的传感器，并且易受到动态目标的影响，这给城市交通场景下的精确定位带来了困难。

目前的室外定位算法大多都依赖于GPS***，但是民用GPS往往只能达到米级的定位精度，并且在城市建筑密集区域，GPS信号容易受到障碍物的遮挡，这将严重影响卫星定位的精度。相比之下，激光雷达是一种鲁棒而精确的测距传感器，这对于自动驾驶很重要。激光雷达按照类型可以分为2D激光雷达和3D激光雷达，3D激光雷达可以提供高精度的测量信息，但是其价格十分昂贵，不利于大规模推广使用。2D激光雷达虽然只能提供某一个平面上的测量信息，但是由于其轻便、价格低廉等特性被广泛应用于各种定位***之中。目前的2D激光雷达大多应用于室内环境下的小型机器人的定位***上，通过匹配相邻点云以获取高精度的相对位姿，但是在室外环境下，由于点云的稀疏性以及大量动态目标的存在都使得相邻点云的匹配变得尤为困难，这导致在室外环境下仅依赖2D激光雷达进行定位的精度较低甚至无法实现定位。不同于激光雷达，视觉里程计可以在室外环境下提供一个好的相对定位精度，但是其会随着一段时间运行后会产生一个很大的累积漂移。

众所周知，2D激光雷达由于其点云稀疏并且易受到动态目标的影响导致目前很少有利用2D激光雷达来完成室外定位的工作。为此，本发明提出了一种融合双目视觉和2D激光雷达的室外定位方法。为了解决2D激光雷达的稀疏性，将双目视觉作为里程计以提供帧间的相对位姿，以将一个局部时间窗口内的2D激光雷达数据融合成一个数据更为稠密的局部子图，同时利用DS证据理论(G.Shafer,“A Mathematical Theory of Evidence”,Princeton university press,vol.1,1976)融合局部子图的时态信息以消除动态目标所带来的噪音。最后，基于反向组合算法(S.Baker and I.Matthews,"Equivalence andefficiency of image alignment algorithms,"Proceedings of the 2001 IEEEComputer Society Conference on Computer Vision and Pattern Recognition.CVPR2001,2001,pp.I-I)的地图匹配算法将被用于局部子图和先验地图之间的匹配以消除里程计随着时间所产生的累积误差，从而实现高精度的定位。

发明内容

本发明通过融合视觉和2D激光雷达来实现室外环境下的高精度定位，通过视觉辅助激光雷达以构建一个数据更为稠密的局部子图，解决了室外环境下激光雷达数据稀疏以及易受到动态目标影响的问题。通过在全局先验地图中定位局部子图，以消除双目里程计所产生的累积误差，从而实现室外环境下的高精度定位。

为了实现上述目的，本发明提供了如下方案：

一种融合双目视觉和2D激光雷达的室外定位方法，所述方法包括：

步骤1：由双目里程计获取帧间的相对位姿；

步骤2：局部子图的构建和融合；

步骤3：局部子图和先验地图的匹配；

步骤4：局部子图的位姿校正。

所述的融合双目视觉和2D激光雷达的定位算法的具体流程如下：

步骤1：使用双目里程计来计算帧间的相对位姿，以将激光数据转换到同一坐标系下。ORB-SLAM2(R.Mur-Artal and J.D.Tardos,“ORB-SLAM2:An open-source SLAM systemfor monocular,stereo,and RGB-D cameras,”IEEE Transactions on Robotics,vol.33,no.5,pp.1255–1262,2017)是一个经典的视觉SLAM***，基于ORB特征点的特征匹配和重定位重复ORB-SLAM2具有很好的视角不变性。本发明选用ORB-SLAM2并去除掉闭环检测部分以将其做为双目里程计来计算相对位姿，双目里程计包括提取ORB、位姿预测以及局部地图跟踪等三个步骤。首先，对双目图像提取ORB特征点，然后使用与之前速率相同的运动模型来预测相机位姿，并搜索上一帧观测到的地图点，最后将地图点投影到当前帧，从已经生成的地图点中找到更多的对应关系进一步优化位姿，最后即可得到帧间的相对位姿。

步骤2：在双目里程计的帮助之下可以将一个局部时间窗口内每个时刻得到的2D激光地图融合成一个局部子图，然而这个局部子图中会存在许多动态目标带来的噪声，并且也很难将其与静态目标区分。为此，本发明采用了基于DS证据理论的栅格地图的融合方法，通过融合时态信息以消除局部子图中动态目标所带来的噪声。

2D激光雷达可以提供在特定位置的障碍物信息，其捕获的点云可以表示成证据栅格地图。假设2D激光雷达发射激光束在某一平面上快速旋转，对于被激光束所穿过的栅格表示为F(Free)，若激光束受到阻挡，则这个栅格表示为O(Occupied)。2D激光雷达只能区分这两种状态，根据DS证据理论每个栅格的FOD(Frame of Discernment,识别框架)可以描述为Ω＝{F，O}。因此，每个栅格的BBA(Basic Belief Assignment，基本置信度赋值)将包含四种状态的质量函数m(F)，m(O)，m(Ω)，m(φ)，分别表示该栅格为空闲、占据、全集以及空集的置信度。为此，在某一时刻t，若该激光束在该栅格被阻挡，则该栅格的BBA定义如下：

m_t(O)＝λ，m_t(F)＝0，m_t(Ω)＝1-λ，m_t(φ)＝0 (1)

若激光束穿过该栅格则定义如下：

m_t(O)＝0，m_t(F)＝λ，m_t(Ω)＝1-λ，m_t(φ)＝0 (2)

其中λ∈[0，1]，表示2D激光雷达的置信度，根据所使用的环境，需要设置不同的数值，对于室外环境，λ的值可以设置为0.9，对于室内环境，λ的值可以设置为0.95。由双目里程计计算出相对位姿之后，可以将一个局部窗口内所有的2D激光数据转换到同一坐标系下并融合成一个局部子图，随后通过融合其时态信息以消除局部子图中的动态目标。对于每个栅格地图都有与其对应的质量函数，所有的质量函数均是在同一个FOD上定义的，因此，局部子图所对应的质量函数m_LM计算方法如下：

其中m₁，m₂，…，m_n表示一个局部时间窗口内n个不同时刻得到的栅格地图所对应的质量函数。其融合标志的计算方式如下：

其中归一化系数K的计算方式如下：

k＝1-m₁(Ω)·m₂(Ω) (5)

最后，一个局部时间窗口内的所有的2D激光测量数据将融合成一个局部子图，由于时态信息的融合，局部子图所含的动态噪音将被大量的清除。和单帧激光所生成栅格地图相比，局部子图的数据更为稠密更适合用于匹配。

步骤3：当局部子图生成之后，我们需要将这个局部子图与先验地图进行匹配以消除里程计所产生的累积误差。对于栅格地图，我们可以将其每个栅格的置信度作为像素值，从而将一幅地图表示成一幅图像。假设局部子图和先验地图所对应的图像分别表示为I_LM(x)和I_PM(x)，x为图像中像素的坐标，所采用的匹配算法是一种基于像素值的图像配准，其目标是找到一个几何变换，使得I_LM(x)和I_PM(x)对于每个x尽可能的相等。算法通过最大化基于图像的相似性来实现这一目标，设p＝(Δx，Δy，θ)^T表示平移和旋转相关的参数向量，则定义I(W(x；p))表示根据向量p对图像I(x)进行欧氏变换，因此该问题就变成一种经典的图像配准方法，其目标是最小化两个图像之间的误差的平方和：

其中表示遍历图像中所有的像素。针对这一问题，可以使用反向组合算法(Inverse Compositional Algorithm，ICA)来解决这一问题，通过迭代来求解参数，其方法如下所述：

第一步：根据式(7)计算海塞矩阵H：

其中表示图像I的梯度；

第二步：根据参数向量p对局部子图I_LM(x)进行欧氏变换得到I_LM(W(x；p))；

第三步：根据式(8)计算参数向量的新增值Δp：

第四步：更新参数向量p：

p＝p+Δp (9)

第五步：重复执行二到四步，直到Δp小于阈值或者超过迭代上限。

其中迭代上限可以设置为300次，阈值则可以设置为当前所在位置与上一帧所在位置之间距离的0.1％，最终输出的p表示两幅图像之间的平移和旋转值，即局部子图和先验地图之间的相对位姿。

步骤4：经过栅格地图匹配之后所得到的参数向量p表示局部子图与先验地图之间的位移量(Δx，Δy)和旋转量θ，根据这三个参数可以对当前局部子图的位姿进行校正。假设当前局部子图的位姿表示为T，T∈SE(2)，则校正后的位姿T′计算方式如下：

经过位姿校正，即可消除里程计所产生的累积误差。同时，更新后的位姿将用于更新轨迹并且作为下一个局部子图的初始位姿。

附图说明

图1是本发明提供的融合双目视觉和2D激光雷达的室外定位方法的流程图；

图2是本发明提供的局部子图的融合过程图；

图3是本发明提供的融合双目视觉和2D激光雷达的室外定位方法在KITTI数据集上的实验结果轨迹图。

具体实施方式

本发明的目的是提供融合双目视觉和2D激光雷达用于室外精确定位的算法，首先双目视觉作为里程计提供相对位姿，然后将一个局部时间容器内的2D激光雷达数据融合成一幅局部子图，并采用DS证据理论融合局部子图中的时态信息以消除其中的动态目标，最后，局部子图将与先验地图进行匹配以消除里程计所产生的累积误差，最终获得精确的定位精度。

下面将结合附图对本发明加以详细说明，应提出的是，所描述的实施例仅旨在便于对本发明的理解，而对其不起任何限定作用。

图1是本发明提供的融合双目视觉和2D激光雷达的室外定位算法的流程图；图2是本发明提供的局部子图的融合过程图；图3是本发明提供的融合双目视觉和2D激光雷达的室外定位方法在KITTI数据集上的实验结果轨迹图，KITTI是一个主要包括城市道路、乡村道路以及高速公路等室外环境下的场景的数据集，其中(a)表示KITTI00序列下的轨迹图，(b)表示KITTI02序列上的轨迹图，(c)表示KITTI05序列上的轨迹图，(d)表示KITTI06序列上的轨迹图，(e)表示KITTI08序列上的轨迹图。本方法主要与ORB-SLAM2等视觉SLAM方法在KITTI数据集部分序列上进行了对比，其中FVL-OutLoc代表本发明中所提出的方法，利用本发明所获得的轨迹明显比采用ORB-SLAM2所获得的轨迹更接近真实值，由此可知，本发明能够在室外环境下提供高精度的定位轨迹。

本发明提供的融合双目视觉和2D激光雷达的室外定位方法的具体包括：

步骤1：由双目里程计获取帧间的相对位姿；

将ORB-SLAM2去除掉回环检测部分以作为双目里程计来使用，主要包括ORB-SLAM2的跟踪和建图模块。该过程中，将双目图像作为输入，分别对双目图像提取ORB特征点并计算其描述子，通过三角测量获取ORB特征点的三维坐标，随后，使用与之前速率相同的运动模型来初步的估计位姿，最后，将前面提取到的ORB特征点投影到当前帧，从已经生成的特征点中找到更多的对应关系对位姿进行优化，最终输出当前位姿。

步骤2：局部子图的构建和融合；

对于2D激光雷达，其数据将会被表示为栅格地图的形式，所构建的栅格地图中每个单元格的长度设置为0.5m。因此，对于被激光束所穿越的栅格其BBA可以定义如下：

m_t(O)＝0.9，m_t(F)＝0，m_t(Ω)＝0.1，m_t(φ)＝0 (11)

若激光束在该栅格处被阻挡，则定义如下：

m_t(O)＝0，m_t(F)＝0.9，m_t(Ω)＝0.1，m_t(φ)＝0 (12)

由于双目里程计提供了帧间的相对位姿，因而可以将一个局部时间窗口内的2D激光雷达数据转换到同一坐标系下，局部时间窗口所包含的帧数设置为10，因而每一个局部子图都是由连续10帧的栅格地图所融合而成的。在一个局部时间窗口内，10帧栅格地图的坐标都将转换到与第一帧栅格地图相同的坐标系下，随后按照证据理论对其时态信息进行融合。经过融合之后所形成的局部子图相比单帧栅格地图数据更为丰富，轮廓更为丰富，因而更适合用于地图匹配。

步骤3：局部子图和先验地图的匹配；

先验地图是一幅预先构建的全局栅格地图，是由无人车或机器人在已知环境下提前生成的全局地图，在上一步生成局部子图之后，可以将其与先验地图进行匹配以消除里程计所产生的累积误差。由于局部子图的位姿已经由双目里程计提供，因而可以在先验地图中找到与局部子图相对应的部分，随后，基于反向组合算法的地图匹配算法将被用于局部子图和先验地图之间的匹配。其中，反向组合算法中的阈值设置为当前所在位置与上一帧所在位置之间距离的0.1％，迭代次数上限设置为300次，经过地图匹配之后即可得到局部子图与先验地图之间的相对位姿。

步骤4：局部子图的位姿校正；

由于视觉里程计会随着时间推移产生漂移，因此需要每隔一段时间对其进行位姿校正。对于每个局部子图，当其与先验地图进行匹配之后所得到的相对位姿都可以将其用于位姿校正。经过位姿校正之后，所得到的位姿将覆盖局部子图先前的位姿。最后，将所有位姿输出即可得到定位轨迹。

Claims (3)

1.融合双目视觉和2D激光雷达的室外定位方法，其特征在于，包括以下步骤：

步骤1：由双目里程计获取帧间的相对位姿；使用双目里程计来计算帧间的相对位姿，以将激光数据转换到同一坐标系下；利用ORB-SLAM2并去除掉闭环检测部分以将其作为双目里程计来计算相对位姿，双目里程计包括提取ORB、位姿预测以及局部地图跟踪等三个步骤；

步骤2：局部子图的构建和融合；通过双目里程计来提供相对位姿将一个局部时间窗口内每个时刻得到的2D激光地图融合成一个局部子图，采用基于DS证据理论的栅格地图的融合方法，通过融合局部子图的时态信息以消除动态目标所带来的噪声；

步骤3：局部子图和先验地图的匹配；在局部子图融合完成之后，需要将其与预先构建的先验地图进行匹配以消除里程计所产生的累积误差；由于局部子图的位姿已经由双目里程计提供，因而直接在先验地图中找到局部子图所对应的位置，基于反向组合算法的地图匹配算法被用于局部子图和先验地图之间的匹配；

步骤4：局部子图的位姿校正；经过地图匹配之后，得到局部子图和先验地图之间的相对位姿，根据相对位姿对局部子图进行位姿校正，校正之后的位姿将被用于更新轨迹并且作为下一个局部子图和初始位姿；

步骤2所述的局部子图采用了基于DS证据理论的地图融合方法，具体步骤如下：

首先，对每个栅格所对应的四个状态的质量函数m(F),m(O),m(Ω),m(φ)进行定义；对于任意时刻t，若激光束在该栅格被阻挡，则该栅格的基本置信度赋值定义如下：

m_t(O)＝λ,m_t(F)＝0,m_t(Ω)＝1-λ,m_t(φ)＝0 (1)

若激光束穿过该栅格则定义如下：

m_t(O)＝0,m_t(F)＝λ,m_t(Ω)＝1-λ,m_t(φ)＝0 (2)

其中λ∈[0,1]，表示2D激光雷达的置信度，根据所使用的环境，需要设置不同的数值，对于室外环境，λ的值设置为0.9，对于室内环境，λ的值设置为0.95；

随后，根据双目里程计所提供的相对位姿，将一个局部时间窗口内的2D激光雷达数据转换到同一坐标系下，以便于进行地图融合；

最后，对每个栅格地图的时态信息进行融合，设局部子图所对应的质量函数为m_LM，则其计算方法如下：

其中m₁,m₂,…,m_n表示一个局部时间窗口内n个时刻得到的栅格地图所对应的质量函数；其融合标志定义如下：

其中归一化系数K的计算方式如下：

k＝1-m₁(Ω)·m₂(Ω) (5)

最后，一个局部时间窗口内的所有的2D激光测量数据将融合成一个局部子图；

步骤3对需要匹配的局部子图和先验地图采用反向组合算法，以获取两幅地图之间的相对位姿，设局部子图和先验地图所对应的图像分别表示为I_LM(x)和I_PM(x)，x为图像中像素的坐标，所采用的反向组合算法，其目标是找到一个几何变换，使得I_LM(x)和I_PM(x)对于每个x尽可能的相等；设p＝(Δx,Δy,θ)^T表示平移和旋转相关的参数向量，则定义I(W(x；p))表示根据向量p对图像I(x)进行欧氏变换，因此问题的目标是最小化两个图像之间的误差的平方和:

其中表示遍历图像中所有的像素；针对这一问题，使用反向组合算法(InverseCompositional Algorithm,ICA)来解决这一问题，通过迭代来求解参数，其方法如下所述：

第一步：根据式(7)计算海塞矩阵H:

其中表示表示图像I的梯度；

第二步：根据参数向量p对局部子图I_LM(x)进行欧氏变换得到I_LM(W(x；p))；

第三步：根据式(8)计算参数向量的新增值Δp：

第四步：更新参数向量p：

p＝p+Δp (9)

第五步：重复执行第二到第四步，直到Δp小于阈值或者超过迭代上限；

迭代上限设置为300次，阈值则设置为当前所在位置与上一帧所在位置之间距离的0.1％，最终输出的p表示两幅图像之间的平移和旋转值，即局部子图和先验地图之间的相对位姿。

2.根据权利要求1所述的融合双目视觉和2D激光雷达的室外定位方法，其特征在于，步骤1将去除掉回环检测的ORBSLAM2作为双目里程计，双目里程计包括提取ORB、位姿预测以及局部地图跟踪等三个步骤；首先，对双目图像提取ORB特征点，然后使用与之前速率相同的运动模型进行位姿预测，并搜索上一帧观测到的地图点，最后将地图点投影到当前帧，从已经生成的地图点中找到更多的对应关系进一步优化位姿，最后即可得到帧间的相对位姿。

3.根据权利要求1所述的融合双目视觉和2D激光雷达的室外定位方法，其特征在于，步骤4所述的位姿校正利用地图之间的欧氏变换进行校正，经过栅格地图匹配之后所得到的参数向量p表示局部子图与先验地图之间的位移量(Δx,Δy)和旋转量θ，根据这三个参数对当前局部子图的位姿进行校正；假设当前局部子图的位姿表示为T,T∈SE(2)，则校正后的位姿T′计算方式如下：

经过位姿校正，即可消除里程计所产生的累积误差；同时，更新后的位姿将用于更新轨迹并且作为下一个局部子图的初始位姿。