基于因子图优化的众包高精度地图云端融合方法

时间：2024-06-19

谷峥，隗寒冰， LIU Zheng，2，娄路，郑国峰

（1.重庆交通大学，重庆 400074；2.University of British Columbia Okanagan，Kelowna，BC，V6T1Z4，Canada）

高精度地图（High-Definition Map，HD-MAP）定位精度可达厘米级，能提供比传统导航地图更加准确和详细的地图数据，是实现自动驾驶汽车自主定位的关键［1］。高精度地图作为自动驾驶的关键核心技术之一，在定位［2-5］、感知［6］、决策［7］方面都有广泛应用。

传统高精度地图的基本制作流程是，配备高性能位置传感器的车辆沿着目标路线行驶以获取测绘数据，然后基于测绘车辆上传的道路地图数据，对地图信息进行特征分类，最后根据地图中的特征信息进行手动细化和数据确认。该方法存在以下缺点：

1）地图测绘车辆需要配备高性能传感器，地图制作成本高昂；

2）需要对采集到的特征信息进行人工修正和标注，耗费大量人力资源；

3）道路状况发生变化时需要重新采集数据上传并验证，地图更新速度较慢。

为了克服传统高精度地图生成方法的不足，学术界开始致力于研究众包地图（Crowd-Sourced Mapping，CSM），即通过在车端布置低成本传感器，如相机、GPS、GNSS/IMU 来采集道路数据，在云端服务器中进行地图轨迹融合处理后再下发局部地图到车端以进行定位和导航。目前，视觉V-SLAM 算法趋近成熟，如基于稀疏特征的ORBSLAM3［8］相比前两代ORB-SLAM［9-10］精度提升了2～5 倍，且实现了VO 丢失后的地图再融合功能。DSM［11］是首个完全基于直接法实现回环检测和地图重用的SLAM 系统。VINS-Fusion［12］方案利用基于视觉结合IMU的紧耦合滑窗优化算法，实现了很高的定位精度，并且其对计算资源的消耗较小，能在普通CPU上实时在线运行。因此，不少众包地图研究都以V-SLAM作为车端建图基础。HERB 等［13］利用ORB-SLAM 作为车端局部地图来重建算法，利用卷积神经网络（Convolutional Neural Networks，CNN）对关键帧进行语义分割并上传特征信息，然后在云端对各关键帧进行特征匹配并对齐，最后利用上传的特征信息对道路边沿进行合并重建。但利用特征匹配并对齐的方式存在误匹配问题。KIM等［14］利用Graph- SLAM［15］方法在车端局部地图中构建新的特征层并上传，然后在云端对多车上传的特征层进行融合，在增加地图信息丰富度的同时极大地加快了地图的更新。然而，该方法极度依赖车端传感器对所需特征的检测，实用性较低。

针对传统高精度地图云端融合方法生成的地图存在置信度较低、误差较大等问题，本文开发了一种基于因子图优化的服务器云端多地图融合算法。首先，利用绝对全局位姿信息进行多个地图片段之间的匹配，并采用一致性筛选方法保证匹配对的准确率；然后，在此基础上构建了图优化模型以求得地图间的最优变换矩阵；最后，结合最优变换矩阵对原始地图进行映射以完成地图片段间的融合。此外，还提出了一种高精地图融合评价指标，并以此为标准对试验结果进行对比评估。

1 众包高精度地图基础框架

借鉴QIN Tong 等［16］提出的众包建图思想，本文采用的高精度地图生产方法主要有两部分，即车载前端和服务器云端，整体基础框架如图1所示。

图1 众包高精度地图框架

1.1 车载前端

1.1.1 图像分割

利用基于CNN 的方法［17-18］，对车辆采集的图像进行所需特征如车道线、马路边沿线、停止线、道路标识等的语义分割。然后通过逆透视变换（Inverse Perspective Mapping，IPM）将提取的语义特征从图像平面反向投影到车辆坐标系下的地面。假设地面为水平面，像素［u，v］投影到水平地面的坐标变换为：

式中：s为尺度；T为车辆后轴中心到相机的变换矩阵；C为相机投影模型。

1.1.2 局部地图构建

采用基于位姿图优化理论的里程计构建方法［12］，对车辆位姿进行全局优化，再利用优化后的车辆位姿将上述提取的语义特征从车辆坐标系转换到全局坐标系，如式（2）所示。

式中：R(qi)表示将i时刻车辆位姿qi转化为旋转矩阵；pi为i时刻车辆在全局坐标系下的位置。

1.2 服务器云端

服务器云端的主要工作是完成多地图间的融合，主要分为3 个步骤：多车地图矢量片段匹配、矢量片段一致性筛选和基于因子图优化的地图合并。

步骤一：多车地图矢量片段匹配。

通过建立全局位姿图模型，利用上传至云端的RTK-GPS数据对相应局部地图进行全局化处理，再根据处理后地图间位姿的相对关系完成地图片段间的粗略匹配。

步骤二：矢量片段一致性筛选。

针对步骤（1）可能会得到错误的匹配关系，本文利用里程计的全局一致性，构建地图匹配对之间的误差模型，将误差优化问题转化为求图论中的最大团问题，最后得到符合全局一致性的地图间匹配关系。

步骤三：基于因子图优化的地图合并。

将地图间目标匹配对转化为约束地图间变换关系的因子节点，构建因子图优化模型，最终求得地图间全局最优的变换矩阵。

本文的主要研究内容在服务器云端上，所以不对车载前端建图部分做过多描述。

2 基于图优化的云端地图融合

2.1 多车地图矢量片段匹配

在SLAM 位姿因子图中，通常用xi∈SE(3)表示机器人在某时刻的位姿。因子图中的因子是由车辆的观测值链接而成，即马尔可夫链。通常用zij表示与变量xi和xj相关的测量值，如果i和j在时刻上是连续的，则称其为里程计测量值；如果i和j在时刻上不连续，则称其为闭环测量值。位姿图的目标就是在给定测量值zij的情况下估计每个位姿变量xi最可能的值。

对单车SLAM问题的描述如式（3）所示。

式中：χ为所有位姿变量xi的集合；Z为所有观测值zij的集合。

对于多车SLAM 地图问题，还需要估计各车辆局部坐标系之间的相对位置变换关系。借鉴KIM等［19］与QIN Tong 等［12］的做法，本文引入GPS 测量值作为位姿图的全局测量因子，从而得到统一的全局坐标系。全局位姿结构如图2所示。

图2 全局位姿结构

图中，蓝色节点为车辆在某一时刻的位姿xi，包含了车辆的位置p 和朝向q；紫色边为GNSS 约束，其仅仅约束节点的位置；黄色边为里程计约束，其约束相邻两节点的位姿。全局位姿图优化问题可表示为：

式中：xi为位姿（位置和朝向）；ro为里程计残差；为里程计测量值，即相邻两时刻的位置变换Δpi-1，i以及朝向变换Δqi-1，i；rg为GNSS 残差；zig为GNSS测量值，即全局坐标系中的绝对位置pi。

残差ro与rg的定义如式（5）所示。

式中：R(q)为将四元数q转化为旋转矩阵；[q-i1·qi-1· Δqoi-1，i]xyz为四元数的前3个元素。

为了提高运算效率，将以上步骤得到的地图片段进行网格化处理。本文将地图片段分割为多个5 m的地图矢量片段，并且以地图矢量片段中里程计的起点位姿xs代表该地图矢量片段，最后利用角度和距离关系对地图矢量片段进行匹配。

2.2 矢量片段一致性筛选

里程计误差的存在可能导致匹配关系不符合全局一致性，而直接判断某个地图矢量片段匹配对是否为异常值较为困难。采用MANGELSON 等［20］提出的一致性筛选方法，对获得的地图矢量片段匹配对进行筛选，从而得到一个符合全局一致性的匹配对子集合最大值，算法结构如图3 所示。图中，步骤1 为待筛选的两车地图间的匹配关系示意图。其中，蓝色节点和绿色节点分别表示a、b两车在某一时刻的位姿；黄色边表示单车轨迹间的里程计约束；虚线边表示两车位姿间的匹配关系，其中包含了待剔除的匹配关系。步骤2 表示一致性矩阵，其将符合一致性度量的匹配关系对进行二值化处理并储存，即符合一致性度量的匹配关系对之间的颜色块更深。步骤3 为根据一致性矩阵建立的一致性图，其中黄色阴影部分表示一致性图中的最大团，即符合一致性度量的所有匹配关系。步骤4 为一致性筛选后两地图间的匹配关系，其剔除了不符合一致性度量的匹配关系。

图3 一致性筛选算法结构

2.2.1 成对一致性

定义：对于一个测量值集合Z͂，如果其满足成对连续一致性，如式（6）所示。

式中：C为度量函数；zi，zj为集合͂中不同的匹配测量值；γ为给定的阈值条件。

度量函数C的定义如式（7）所示。

式中：为车辆a 在i时刻与车辆b 在k时刻的匹配关系测量值；为车辆a 在i，j时刻间的里程计测量值；||·||∑为马氏距离。

2.2.2 一致性图模型建立与求解

根据成对一致性的定义，对得到的匹配集合Z͂进行筛选，并将筛选结果进行二值化处理，即满足阈值条件的匹配对置1，否则置为0，并将结果存储在如图3（步骤2）所示的二值化矩阵中。

根据以上步骤得到的矩阵，可以明确得到任意一对匹配测量值的一致性关系，以此关系建立一致性图模型，如图3（步骤3）所示。图中，节点表示某一匹配关系测量值，边表示两匹配关系测量值之间的一致性关系。满足成对一致性阈值的两节点相连，否则不相连。

图论中的团被定义为顶点的子集，此子集中任意两顶点之间必有一条边。最大团问题（Maximum Clique Problem，MCP）是指在图中寻找包含顶点个数最多的一个团。因此，寻找集合Z͂中的最大一致性子集问题就是寻找一致性图中的最大团问题。本文采用PATTABIRAMAN 等［21］提出的搜索方法作为最大团问题解决法，最终得到符合全局一致性的匹配关系，如图3（步骤4）所示。

2.3 多约束因子图

得到最佳地图矢量片段匹配对之后，需要对矢量片段中车道线以及标志牌等目标进行匹配，并进行位姿化建模处理，然后建立多约束因子图模型，其流程如图4 所示。图中，步骤1 表示对匹配上的两地图片段中的目标进行匹配；步骤2 为车道线位姿化建模过程中的示意图；步骤3 表示根据匹配对建立的因子图模型。具体含义以及构建过程将在下文介绍。

图4 多约束因子图建立流程

本文采用贪心算法分别从角度和距离两方面对地图矢量片段匹配对中的目标进行匹配，匹配结果如图4（步骤1）所示。

为了构建图优化模型，需要对车道线进行位姿化处理。为便于理解，以二维平面坐标系为例，如图4（步骤2）所示。图中，紫色线段表示实线车道线片段；绿色线段表示虚线车道线片段；pl为实线段中点坐标；pb为虚线段起点坐标；θ为虚线段与坐标轴间的夹角。因此，设旋转矩阵R=平移矢量t=(x2，y2)T。则虚线段的李群表示为：

实线段的李群表示与式（8）类似，仅仅在平移矢量的选择上有区别。

通过上述步骤，对于两地图矢量片段而言，就得到了一组匹配好的n个李群的集合T和T′：

接下来建立如图4（步骤3）所示的多约束因子图。图中，节点为待估计的地图矢量片段的变换矩阵T，紫色块为实线对节点的约束，绿色块为虚线对节点的约束，虚线块为其他目标对节点的约束。

与视觉SLAM 问题类似，该图优化问题的本质也是最大似然估计问题，由多轨迹间变换关系的联合概率分布组成。假设所有测量概率独立，该问题可以表示为：

式中：T为所有待估计变量即地图片段的变换矩阵集合；S为所有观测值的集合，包括虚线因子、实线因子。

2.3.1 实线因子

由于目标遮挡、路面破损等原因，车端对于实线的检测往往存在漏检。然而，根据车端试验数据验证，仅有极小概率会出现车道线类型误检，如实线车道线检测为虚线车道线，因此，本文不考虑此类型误检造成的微小误差。考虑式（9）中的一对实线匹配对Tl1与T'l1，分别属于第t-1 个和第t个地图片段，其约束两实线段中点以及朝向相同。

式中：Tt-1和Tt分别为待估计的第t-1个和t个地图片段的变换矩阵。

2.3.2 虚线因子

由于车端大部分时候对于虚线的检测都是完整而准确的，所以考虑式（9）中的一对虚线匹配对Tb1与T'b1，分别属于第t-1 个和第t个地图片段，其约束两虚线段起点以及朝向相同。

式中：Tt-1和Tt分别为待估计的第t-1个和第t个地图片段的变换矩阵。

值得注意的是，除了实线因子、虚线因子，本文算法框架还可以增加其他全局残差因子，如标识牌、红绿灯等。

2.4 因子图优化

参考文献［22］，考虑一含有n条边的因子图，其目标函数为：

式中：ei(·)为误差，作为优化变量xi和观测值zi符合程度的一个度量；Ωi为信息矩阵，是协方差矩阵的逆；xi为因子图中的一个顶点，在传统SLAM 问题中表示位姿。

进一步，将总体优化问题改写为误差和的形式，又因为观测值zi本身已知，所以为了形式上的简洁，将ei(xi，zi)改写为ei(xi)，如式（15）所示。

为了求解优化问题，需要知道初值和迭代方向，考虑初始值x͂i与增量Δx，对于第i条边的目标函数，有：

式中：Ji为e i关于xi的导数；Ci为与Δx无关的常数项，实则为Fi变化前的值；2bi为一次项系数；二次项系数Hi为Hessian矩阵。

则增量Δx令目标函数发生改变的量为：

由于目标是找到增量Δx，使增量变为极小值，所以令目标函数的增量对于增量Δx的导数为0。最终优化为：

以上为因子图优化的基本原理，出于算法复杂度的考虑，本文选用g2o 优化库［23］完成因子图优化的迭代过程，最终寻找到极小值增量Δx，使优化结果收敛。

3 试验结果及分析

为验证提出的基于因子图优化的众包地图云端融合方法的可行性与有效性，本文使用安装有前置摄像头、IMU、RTK-GPS的车辆在真实城市道路上进行数据采集建图并上传云端服务器，再将本文算法结果与迭代最近邻算法（Iterative Closest Point，ICP）结果进行对比。ICP算法是目前最经典的配准算法，其不依赖于环境特征，具有较高的配准精度。车端采集并上传至云端的试验数据如图5 所示，全长约为20 km。图5a 为上传的车端数据总览，可以看出试验场景较复杂，满足评价算法的试验标准，其包含了直道、弯道、十字路口等；图5b为总览数据的局部放大图。由图5b 可知，车端上传的车道地图数据包含马路边沿（标号1，紫色线段）、虚线车道线（标号2）、实线车道线（标号3）、地面箭头标识（标号4）、停止线（标号5）、人行横道（标号6）和标识牌（标号7）。不难看出，车端上传的数据中，实线车道线聚集度较好，并且与谷歌卫星底图较符合；而虚线车道线的聚集度稍差，与卫星底图有些许差距。

图5 谷歌影像下车端上传数据

3.1 聚集度评价指标

为了更好地评价本算法的聚集结果，根据车道地图的特点，提出以下3 个评价标准，如图6所示。

图6 聚集度评价标准

3.1.1 点距离指标

点的距离指标是指空间中两点p1和p2的绝对距离d12。定义点的距离指标得分如式（19）所示。

式中：γ1为点的距离评价指标。

3.1.2 线段距离指标

线段的距离指标是指空间中两线段l1和l2在xy平面的投影线段l'1和l'2重叠部分的距离d12。定义线的距离指标得分如式（20）所示。

式中：γ2为线段的距离评价指标。

3.1.3 线段角度指标

线段的角度指标是指空间中两线段l1和l2在xy平面的投影线段l'1和l'2的夹角θ12。定义线段的角度指标得分如式（21）所示。

式中：γ3为线段的角度评价指标。

3.1.4 虚线、实线匹配对得分计算公式

根据实际需要，分别对点的距离评价指标γ1取值为1 m、线段的距离评价指标γ2取值为1.5 m、线段的角度评价指标γ3取值为10°。对于虚线车道线匹配对，分别从点的距离以及线段的角度两方面进行打分，具体得分计算如式（22）所示；对于实线车道线匹配对，分别从线段的距离以及线段的角度两方面进行打分，具体得分计算如式（23）所示。

结合实线、虚线的位姿化标准，设有一虚线匹配对l1、l2，其端点在xy平面的坐标分别为ps1、pe1、ps2、pe2，现定义虚线车道线匹配对聚集度得分如式（22）所示。

式中：s1( · )为两点间距离指标得分；s3( · )为两线段间角度指标得分；θ12为虚线车道线匹配对在xy平面投影的夹角。

设有一实线匹配对l1、l2，其在xy平面的投影线段l'1、l'2，现定义实线车道线匹配对聚集度得分如式（23）所示。

式中：s2( · )为两线段间距离指标得分；s3( · )为两线段间角度指标得分；θ12为实线车道线匹配对在xy平面投影的夹角。

3.2 试验结果对比

本文算法和ICP 算法对复杂路口的地图优化聚合结果，如图7 所示。从局部放大图可以看出，两算法对于实线的优化聚合结果差别不大，但本文算法在虚线的表现上明显优于ICP算法。

图7 复杂路口聚合结果对比

为了得到聚集度得分评价对比结果，首先对真实道路数据进行如图4（步骤1）所示的目标间的匹配，即得到实线车道线以及虚线车道线的匹配对。然后利用3.1 节提出的聚集度评价指标对原始车道线匹配对进行聚集度打分计算，再对多地图进行聚合优化，并对优化后的地图进行聚集度打分计算。最终，聚集度得分对比柱状图结果如图8所示。

图8 聚集度得分柱状分布对比

图8a为虚线车道线匹配对聚集度得分柱状对比图，图8b 为实线车道线匹配对聚集度得分柱状对比图。图中，紫色表示原始数据聚集度得分的匹配对数目；红色表示ICP 算法优化结果聚集度得分的匹配对数目；绿色表示本文算法优化结果聚集度得分的匹配对数目。由图8a可知，原始虚线车道线匹配对聚集度较差，体现为超过50%的虚线车道线匹配对聚集度得分分布为0～30 分；本文算法虚线得分结果基本符合正态分布，得分中位数约为40～50分；而ICP 算法得分结果为右偏分布，高分相对较少，得分中位数约为30～40 分。两种算法对于低分段（0～20 分）的优化结果差异较大，可以看出ICP算法对于低分段虚线匹配对的减少量小于50%，且远小于本文算法，这也说明了ICP 算法对于优化初始值的依赖性。由图8b 可知，两种算法优化结果的聚集度得分柱状分布图都呈现左偏分布，并且得分差异性较小，表明两算法对于实线车道线匹配对聚集度的优化效果较好。

为了更精确地衡量本文算法，现对两算法优化结果的聚集度得分取平均值，见表1。由表可知，两算法对于实线车道线匹配对的聚集度得分都提高了10 分以上，差距相对较小；而本文算法在虚线车道线匹配对的聚集度得分结果上高出ICP 算法10分左右。综合车道线聚集度平均得分结果，本文算法相较于原始车道线聚集度得分提升了44.7%。结合原始虚线车道线匹配对聚集度平均得分结果，以及虚线车道线匹配对聚集度得分柱状分布图可知，车端上传的虚线车道线聚集度较差，即原始虚线车道线聚集度平均得分较低。又由于传统ICP 算法对于虚线车道线匹配对优化结果的平均聚集度得分的提升相较于本文算法有较大差距，结合两算法对于聚集度较好的实线车道线优化结果的差异性较小这一事实，以及两算法对于低分段虚线车道线匹配对减少量的较大差异性，来进一步证实本文算法对于优化初值的较低依赖性。

表1 虚、实线车道线匹配对聚集度平均得分结果对比

此外，还利用RTK-GPS 对一条总长约为0.6 km的路口车道线进行真值采集，如图9红色线段所示。通过计算比较聚合后车道线与真值车道线在东方向以及北方向的绝对值误差来评估本文算法，对比结果见表2。由表2 可知，本文算法聚合后地图相对于真值的误差均小于1 m，尤其在虚线车道线的误差上，相较于传统ICP 算法具有明显优势，进一步体现了本文算法的有效性。

表2 东、北方向车道线绝对误差对比

图9 真值采集