多目鱼眼相机系统的视觉里程计解决方案

时间：2024-08-31

刘一龙，孟维亮，张晓鹏

(1.中国科学院自动化研究所模式识别国家重点实验室，北京 100190;2.中国科学院软件研究所计算机科学国家重点实验室，北京 100190;3.之江实验室，杭州 311121;4.中国科学院大学，北京 100049)

0 引言

在自动驾驶领域，里程计算法是指通过接收每帧的各车载传感器输入，不断估计车辆本身位置和姿态的增量，从而确定车身当前所处的位置和姿态的算法。里程计算法的本质是一个同时建图定位算法(Simultaneous localization and mapping, SLAM)，它工作在没有高精度地图先验的场景下，从原始输入中构建场景结构特征并解算车身位姿，其结果可以作为进一步高精度地图建图或定位的输入信息。因此，它是整个自动驾驶系统的最为基础的模块之一。

自动驾驶产品中，常用的里程计解决方案依据其使用的外观性传感器类型的不同，通常可分为使用激光雷达的方案、使用相机图像的方案以及仅使用内观性传感器(轮速或 IMU 等)的方案等。在本文中我们主要讨论使用相机图像的方案。特别地，在自动泊车等慢速行车且周围环境较复杂、行车线路较灵活的场景中，多目鱼眼相机由于其视野较大，可观测到的信息较多，因此具备一定的优势。在本文中，我们主要设计了多目鱼眼相机系统的里程计方案。

1 相关工作

1.1 基于激光雷达的里程计算法

由于激光雷达可以对周围环境扫描获得高精度的三维点云信息，使得自动驾驶中的定位和感知具有很高的可靠性。因此，在完全无人驾驶的解决方案中受到青睐。使用激光雷达信息构建里程计算法，最简单的方案是对每帧的点云使用ICP[1-2]对齐算法，但这种方案通常容易受到点云密度不均匀性、噪声和复杂物体表面点云的影响。后续的一些工作[3-4]使用了平面或线特征等几何特征，得到了更加鲁棒的结果。LOAM[5]提出了一个容易使用的从扫描帧提取线、面特征的方法，并实现了一个高效、准确的实时里程计方案。在此基础上，后人提出 LeGO-LOAM[6]，LIO-SAM[7]等不同的改进方案。LIC-Fusion[8-9]，LINS[10]，FAST-LIO[11]等工作联合使用激光雷达特征与惯性传感器数据，构建基于滤波框架的里程计。PSF-LO[12]则使用参数化的语义特征构建里程计，取得了很高的精度。总体而言，激光雷达里程计可以达到很高的准确度，但由于激光雷达的成本比较昂贵，短时间内难以大规模投入量产。而基于视觉的里程计算法，则因其低廉成本和可靠的成像质量，成为量产方案的首选。

1.2 基于视觉的里程计算法

基于视觉的里程计算法研究较多，经过工作前后大量的探索，逐渐形成了一些被广泛采用的方法和设计。其中，PTAM[13]提出了使用 Tracking 和 Mapping 两个线程对关键帧进行优化的方案。ORB-SLAM[14]使用 ORB 作为特征点的描述，构建了一个经典的单目 SLAM 解决方案，并以此为基础在后续工作[15-16]中增加双目和 IMU 等支持。SVO[17]、DSO[18]等工作不提取特征点的描述子，而是使用图像光度信息进行跟踪，形成了一系列直接法里程计的解决方案。VINS-MONO[19]使用 Lucas-Kanade 光流[20]跟踪替代描述子匹配，并使用IMU 信息实现了一个高效的视觉-惯性里程计，并在后续工作 VINS-Fusion[21]中增加了双目的支持。近年来，还有一些工作[22-23]探索使用深度学习的方法构造视觉里程计，也取得了不错的成果。这些工作为里程计算法提供了很好的经验，但它们都没有针对多目鱼眼相机系统进行特别的设计，而鱼眼相机图像所具有的较大畸变扭曲等特性，通常会使得这些方法无法适配或效果下降。同时，鱼眼相机间的数据关联也比较困难，从而限制了整个系统的精度。

1.3 数据关联算法

在包括里程计算法在内的 SLAM 算法中，通常需要在帧间或相机间的进行相同特征的关联，关联的特征将形成优化问题的约束，对问题的解决非常重要。对于使用直接法的算法来讲，通过在不同帧之间使用邻域的图像光度信息的差异进行匹配。这种方法由于光度匹配在单帧上容易出现错配，通常需要多帧连续匹配，使用深度滤波器等方法不断更新结果。因此，在相机间的单帧匹配上则不易发挥作用。对于使用特征点法的算法来讲，通常使用不同的特征点描述子进行特征点的匹配。FAST[24]特征点是实时系统中最常用的角点检测算法之一。而 BRIEF[25]描述子则提供了可以快速计算的二进制描述子。它们不区分角点特征的方向，也不能很好地处理图像信息尺寸的不同，但极低的计算力要求和良好的实时性仍然是很多场合下不错的选择。SIFT[26]和 SURF[27]提供了具有旋转不变性和一定的尺度不变性的浮点描述子，可以在特征匹配中达到非常好的效果，但它们的提取和生成需要耗费较多的算力和时间。ORB[28]通过在 FAST角点和 BRIEF 描述子的基础上增加方向信息和图像金字塔，以更高的计算效率达到了与 SIFT 相当的效果。然而，尽管这些特征对于特征点旋转方向的补偿，仅能适用于图像平面上的旋转，对于鱼眼相机系统中相机间距离较远形成的视角变化，以及畸变带来的扭曲变化，都不能达到满意的效果。近年来随着神经网络算法的发展，研究者们提出了许多基于卷积神经网络等模型提取特征点和描述子的方法[29-33]，以及基于深度学习模型的匹配方法[34-35]。相比于传统的特征及匹配方法，这些方法在鲁棒性、准确度、匹配数量等方面均有明显的优势。但这些方法通常需要消耗较多的算力，甚至需要GPU等硬件支持才能高效运行。

2 里程计方案设计

2.1 算法架构设计

我们的里程计算法是一套基于优化框架的 SLAM 算法，在架构上采用业界广泛应用的前端-后端架构方案，整体架构如图1 所示。算法输入多路相机图像信息，以及可选的轮速脉冲或 IMU 等信号，持续估计每时刻车体位姿的相对变化。从而输出每帧车体位姿。其中，算法前端负责关键帧管理、特征点的提取与跟踪、地图点的生成与维护等功能，而后端维护一个关键帧的滑动窗口，并在窗口内进行地图点与车体位姿的联合优化，从而提高算法精度。此外，我们提出了两种数据关联的模块，可以用于初始化时的尺度矫正和为后端联合优化提供额外约束，以提高整个系统的输出位姿的精度。

图1 本文里程计方案系统架构总览

2.2 算法初始化

当某个相机满足以下所有条件时，将被认为初始化就绪状态：

(1)当前跟踪中的特征点数超过阈值;

(2)特征点连续跟踪帧数超过阈值;

值得注意的是，其中条件 3 是为了保证相机位置在两帧间形成的基线足够长，从而可以得到比较准确的三角化结果。但该条件无法排除相机运动接近纯旋转的情况，而这种情况下并不能达到长基线的预期。由于在本文所应用的车载系统的使用场景下，几乎不会发生各个相机的纯旋转，因此使用此条件是合理的。

当系统中有超过半数的相机进入初始化就绪状态后，可以收集各相机已三角化的地图点信息，通过构建和求解如公式(2)所示的优化问题，完成里程计系统的初始化。

2.3 前端管理

前端的管理主要涉及地图点的管理、关键帧的管理以及对每帧输入进行的特征点跟踪与简单的位姿优化。

在地图点的管理上，对于已三角化的地图点，可根据其在两帧之前的轮速或IMU 推算得到的本帧位姿下的投影位置作为光流初值的预测，利用2.2节所述的双向光流匹配策略，得到其在本帧的观测位置，加入到约束中。同时，应加入特征点和地图点的补充：将整个图像区域划分为m×n的格子，当此帧上某一格子内跟踪的特征点数少于阈值时，则在该格子中补充 FAST 特征点。对于存在两个以上观测的地图点，可以通过三角化得到地图点的三维位置。值得注意的是，随着车辆的行驶，会产生原来在图像上分布较分散的特征点逐渐聚集到一起，此时这些点消耗的算力不能为优化系统提供更多的信息，反而以较高的权重影响了优化效果。比如正常向前行驶时的后视相机或者倒车行驶时的前视相机，特征点将逐渐向图像中心聚集。为减少这种影响，我们在设定区域划分格子，每个格子中仅保留可观测帧数最多的一个地图点。

对于每个相机，如果其满足如下的两个条件之一，则被认为需要设定关键帧。

(1)距离上个关键帧已有2帧或以上，同时满足以下条件之一：

• 该帧的平均光流距离超过阈值;

• 现存的地图点数量少于阈值;

• 该帧上发生了聚集特征点删除。

(2)距离上个关键帧已有4帧或以上，同时本帧观测到的特征点数小于阈值。

当存在任一相机需要设定关键帧时，该帧将被标记为关键帧，进入后端的优化。此外，还可设定一个强制插入关键帧的距离参数，当估计的行驶距离超过该设定参数时，即使未满足前述条件，也会将当前帧设定为关键帧。

当接收到每帧的输入后，保持各相机中当前各地图点位置，通过求解优化问题估计当前帧位姿：

(1)

式中，π(·)为投影过程，Cc和Tc分别为相机c的内参和外参；xp为地图点的三维位置，pp,i,c为地图点xp在第i帧、相机c图像上的二维观测位置；ρ(·)为鲁棒核函数，在本文中使用 Huber 函数。在前端的优化中，待优化的变量仅为当前帧(第i帧)的位姿Pi。构建约束的是当前帧所有相机的所有观测，其中所有地图点的位置xp均为固定值而不被优化。这使得前端优化可以以很小的计算量快速得到每帧的位姿估计。

2.4 后端优化

为了得到更准确的三维地图点位置和每帧位姿，在后端维护一个固定长度为W帧的关键帧滑窗，并对于滑窗内各关键帧位姿及观测到的地图点位置进行联合优化。具体而言，优化问题表示为：

(2)

式中，i的取值范围为滑窗内各帧ID；π(·)为投影过程，Cc和Tc分别为相机c的内参和外参；xp为地图点的三维位置，pp,i,c为地图点xp在第i帧、相机c图像上的二维观测位置；ρ(·)为鲁棒核函数，在本文中使用 Huber 函数。与公式(1)相比，公式(2)所表示的后端优化有两处不同：一是滑窗内的所有关键帧都参与了优化，这些帧的位姿Pi都是待优化变量；二是观测的所有地图点三维位置xp也被同时优化。后端的这种优化通常被称为光束平差优化(Bundle Adjustment)，可以进一步得到更加准确的位姿和地图点位置。

3 鱼眼相机间数据关联

相机间数据关联的目标是在两个相机图像上内容重叠区域确定若干对以像素坐标表示的匹配点，确保每对匹配点对应三维世界中相同的三维点。正确的相机间数据关联是视觉里程计算法能够充分利用多目视觉信息、获得与物理尺度相一致的优化结果的必要前提。在相机间数据关联缺失时，来自不同相机的观测仅在每个相机本身构建优化问题，不同相机的约束仅通过相对车体的相机外参联系起来，虽然绝对尺度也是可观的，但是约束并不强。实验表明这种约束下尺度仍会具有 1%～2% 水平的误差，从而影响系统结果的精度。不失一般性，我们可以将多目相机系统的相机间数据关联问题简化为双目相机图像上的匹配问题。

现有的双目匹配技术主要分为两类:基于描述子匹配的方法和基于图像灰度信息的直接法。这些方法通常适用于相机中心距离较近(短基线)场景，在长基线的鱼眼相机的配置下，图像内容重叠区域通常在图像边缘畸变明显处占据很小面积，同时图像特征差异较大，且多存在遮挡等极端条件，现有方法均不能保证匹配点的数目和质量。本节将探讨适用于长基线鱼眼相机配置下的双目数据关联问题。

3.1 基于光度的数据关联

基于光度的方法主要是在给定三维点深度下计算其投影在两个相机图像位置上局部区域像素光度值的相关性，从而在对极线上进行搜索，以确定正确的深度值。但由于长基线鱼眼相机配置情况下，可能出现两相机曝光情况不同和存在严重畸变或遮挡等，对于单一点的光度匹配搜索往往会被噪音或局部极值影响而得到错误匹配。可以观察到，在缺少相机间数据关联的多目系统里，虽然其尺度可能存在误差，但在里程计初始化结束后，已经得到了针对每个单独相机(如相机1)的二维特征点和三维地图点，利用这些信息可以为匹配提供可靠的线索。这些地图点具有如下特性：

• 如果向相机1投影，则每个三维点可以投影到其相应的二维特征点;

• 各地图点深度不准确，但相对的深度关系较准确。即通过一个统一的尺度修正，可以同时变成准确的三维位置;

• 各点沿相机1的投影方向进行尺度改变，不影响其仍然投影到原始二维特征点。

基于这些特性，只需要估计一个最佳尺度值，使得在其作用下，这些三维点能够达到其真实位置，此时投影到相机2也可以到达正确的二维位置。整体步骤如图2所示。

图2 通过光度数据关联修正系统尺度流程

具体来讲，可以通过以下步骤得到整个系统的尺度修正值。

(1)相机1的图像通过当前里程计初始化得到二维特征点和三维地图点；

(2)初始化尺度为最大可能值，然后在其达到最小值之前循环以下步骤：

a.将当前尺度应用于所有原始三维地图点，得到更新后的地图点三维位置；

(3)

i.假设图像上局部小区域内深度相同，将每个地图点对应的二维特征点及其周围n×n方块像素点均按更新后的深度值投影到相机2，得到在相机2图像上的n×n个像素位置;

ii.根据公式(3)计算 NCC 值;

iii.将所有三维点的 NCC 值进行平均并记录。

c.沿对极线方向尺度步进更新：

i.寻找到所有三维点中深度最小(离相机1最近)的点;

ii.将其尺度减小一个极小量，将新的点投影到相机2得到其在相机2上投影的改变方向，即对极线的切线方向;

iii.沿该方向步进一个像素的距离，此时深度与原始深度的比值作为新的尺度。

(3)循环结束后，将具有最大 NCC 值的匹配和相应的尺度值输出。

由于该方法综合考虑了每帧上所有的地图点，因此，对于曝光差异、畸变和遮挡的鲁棒性较好。图3 中展示了一些极端情况下仍可得到正确的匹配和尺度值的场景。在图3(a)中，来自两个鱼眼相机的图像，其内容视角相差较大，但得到了正确匹配；图3(b)中展示了由于遮挡造成图像内容被掩盖，曝光情况也发生了差异，右图中可见的远处的面包车在左图中完全被遮挡，但匹配点仍然出现在了相应位置，得到的尺度值也是正确的。

图3 极端场景下光度数据关联可以得到正确尺度值与匹配结果

得到正确的尺度值后，可以将新的尺度应用到当前系统，从而修复系统的尺度偏差。该方法在里程计系统中轮速计、IMU 等具备尺度的观测传感器不可用时，用于在初始化后进行尺度的修正，可以有效地减少尺度偏差，从而提高系统整体精度。另一方面，在尺度误差较小的前提下，直接将 NCC 误差构建入后端的非线性优化问题中，作为优化约束之一参与每帧优化也是可行的。但由于NCC 求导数比较复杂，每一迭代中的搜索匹配也需要消耗大量计算，因此，通常并不将其加入每帧优化中，而是在此时使用基于描述子的数据关联。

3.2 基于描述子的数据关联

使用 ORB、SIFT 等具有一定的旋转、尺度不变性的描述子，在鱼眼相机长基线、大畸变的情况下，匹配结果相对较好，但其描述子的提取计算量较大，对车载实时系统算力造成不小压力。

业界通常使用PolyN模型表述鱼眼相机，可以认为是在针孔相机成像的基础上增加了一个径向角度的映射变化。如图4(a)所示，在相机坐标系中的一个点P=(xc,yc,zc)T，其投影到图像的入射光线的角度为:

图4 本文使用的两种相机模型

(4)

在 PolyN 模型中，认为入射角度为θ的光线将被向内偏折，使得最终投影点发生在距主点为r的位置p：

(5)

(6)

式中，阶数N可以选择不同的数量，从而在复杂程度与精度之间做出权衡，相应的模型即可被具体地称为 Poly1/Poly3/Poly5 等。在我们的实际使用中，通常使用 Poly5 的模型。

为了提升相机间描述子的一致性，需要构建另一种假想的相机模型——柱面相机模型，如图4(b)所示。在相机坐标系中的一个点P=(xc,yc,zc)T，其投影点的图像坐标如下确定：

(7)

将鱼眼图像转换为柱面图像，保证了图像内容的方向具备了一定的一致性，同时yaw角上均匀分布的角分辨率也大大提升了简单描述子的一致性。因此，我们对于多目鱼眼相机，使用柱面图像和基本的 FAST 角点加BRIEF描述子，可以得到不错的匹配效果。

通过这种方法得到每帧相机间的数据关联匹配后，其在匹配帧上的重投影误差也可以与原单相机约束相同的方式加入到公式(2)所示的原优化问题中，从而提高系统的精度。

图5 中对比了开启数据关联尺度矫正及优化配置下不同的轨迹估计结果。测试数据在地下停车场中采集，从给定车位出发，经过较长距离后回到原车位并泊入。这使得从轨迹形状上很容易分辨起始和终点的位移偏差。由图5可见，未开启数据关联时，轨迹存在一定的尺度偏差与漂移，整体误差累积也较高；而加入了数据关联尺度矫正及优化后，误差的大小及累积速度都得到了较好的控制。

图5 开启数据关联尺度优化前后的轨迹结果

4 结果与讨论

本节将对本文所述里程计方案的结果进行展示，并进行定量的评估。

4.1 公开数据集量化评测

使用公开数据集 EuRoC[36]对本文里程计算法的结果进行评测，以验证我们方案的正确性与可用性。EuRoC 数据主要是短距离的无人机拍摄数据，在各个方向上均有不同模式的运动。采用双目视觉、无 IMU 的配置，并将结果与开源算法 VINS-Fusion[21]进行对比。两者的 APE误差统计如表 1 所示。

同时，在图6中展示了本文方法得到的轨迹在各个测试数据上与真值轨迹相比的APE误差分布情况。从表1和图6轨迹结果中可见，在 EuRoC 数据集上，本文的方法比 VINS-Fusion 得到的结果误差更小。由于该评测数据集并非本文算法研发时主要考虑的场景，这证明了本文算法具有良好的普适性。

图6 本文方法在EuRoC 数据集上的轨迹结果与误差分布

表1 与VINS-Fusion 在 EuRoC 数据集APE对比

4.2 自采实车数据量化评测

为了证明我们的算法在主要应用场景，即车辆行驶场景下的精度，我们使用自有的数据采集车，在GPS/RTK信号良好的城区和高速道路采集了若干段长距离的实车数据。其中真值轨迹使用RTK轨迹，并进行了正确的外参补偿。由于里程计不包含全局约束，误差会随距离累积。对于大范围长距离数据而言，在未修正全局漂移时，APE数值的参考价值不大，故省略，仅评测RPE。同时，我们还基于 MSCKF[37]实现了一个滤波版本的里程计作为对比。3个数据集下误差情况统计如表2所示。

表2 自采实车数据集下 RPE 精度结果

从结果可以看出，本文里程计达到了 200 m 平均相对误差 < 5‰ 的较高水平，且与常用的滤波框架下的里程计精度相当，在位移精度上占优，具备较好的可用性。

为了进一步证明本文结果在自动驾驶场景下可以满足下游模块的精度要求，我们将得到的轨迹位姿接入GPS信息，通过简单的位姿图优化将其对齐到真实地理坐标上，在城市地图上将相关测试轨迹与RTK真值轨迹的200 m相对误差可视化，显示在图上。如图7所示，绝大多数区域的相对误差均在 6‰ 以下，可以很好地满足下游进一步进行高精度建图或定位的精度要求。

图7 本文方法在实车数据上的结果(GPS对齐后)200m相对误差分布