一种多传感器融合的自动驾驶位姿估计方法

技术领域

本发明涉及自动驾驶领域，尤其是涉及一种多传感器融合的自动驾驶位姿估计方法。

背景技术

近年来，随着通讯、能源与传感器技术的不断发展，自动驾驶成为了国家科技进步的焦点，也是工业界各企业所追求的产业升级的热点方向。其中，位姿估计技术是自动驾驶研究领域的关键点之一。无人车在随时要面临的陌生环境中，首先需要解决的就是自身定位与位姿估计问题。

目前，主流位姿估计量产方案包括了三种：

第一种是通过差分信号的全球导航卫星系统获取实时的车辆绝对位姿。该方法定位误差能够达到厘米级，在野外环境中具有极高的精度。然而，全球导航卫星系统在丛林、高架路、山川、隧道、地下以及室内环境等信号不良的场景下表现不佳，易产生位姿估计飘移、失效等问题，且该设备成本较高，不利于自动驾驶量产的需求。

第二种是将高精地图与多种自身传感器数据进行融合，从而根据感知数据与地图的匹配结果获得车辆准确的位姿。该方法定位精度高、鲁棒性好，且在L2级别的辅助驾驶上已实现部分量产。然而高精地图的采集和使用涉及国家法律法规问题，必须要获取一定资质才能够进行使用。另外，高精地图的生产时间长，且目前只能覆盖部分城市主要干道及高速场景，因此自动驾驶仅能在特定道路上生效。最后，城市地图并不是一成不变的，这就要求企业在开发算法的同时，还要考虑硬件和地图维护方面的成本。

第三种是通过车辆自身所携带的传感器通过帧间的匹配及优化得到实时的位姿估计结果。这种方案最大的优势是不依赖从外界获取的信息，仅利用自身传感器观测的数据即可估计车辆位姿。目前该方案所使用的主流传感器包括相机、激光雷达以及惯性测量单元。

这些传感器各有优势，同样也具有一定的局限性。相机能够从特征点及光流的变化中计算自身位姿的变化，利用丰富的色彩信息以及语义信息建图，且具有廉价、功耗低、易于部署的优点。但是相机受到光线的影响较高，高速状态下系统鲁棒性差，且单目相机缺少尺度信息，即使用双目相机或深度相机，其探测深度的能力仍然不足。激光雷达则与其相反，通过测量激光往返运行的时间进行测距，每一个激光脉冲能够精确的测量传感器到目标的距离，而且具有极高的角度分辨率和距离分辨率。但激光雷达点云数据大、频率低，处理时所需的计算量高，因而无法输出较高频率的位姿变化结果，且容易受到雨雪沙尘等恶劣天气的影响。相较前二者，惯性测量单元不受天气的影响，抗干扰能力强，且采样频率普遍能达到100Hz甚至500Hz，具备高频位姿计算的能力。但是IMU存在一定的零偏量，在长时间的累计后系统漂移较为严重，不能单独进行长期的定位工作。

考虑到单一传感器的算法均有一定的弊端，因此后续以非线性优化为核心的视觉惯性里程计、激光惯性里程计方法被提出。然而尽管视觉惯性里程计解决了相机没有尺度的问题，且显著提升了位姿输出的频率，但面对车辆行驶速度较快的情况表现出较差的鲁棒性，且其本身无法达到自动驾驶的定位精度；激光惯性里程计方法尽管定位精度较高，但无法解决大场景下z方向飘移的问题。

因此，如何提供一种能够将多种传感器数据进行融合的位姿估计算法是技术人员亟待解决的问题。

发明内容

本发明的目的就是为了提供一种融合多种传感器数据以提升位姿估计准确性的多传感器融合的自动驾驶位姿估计方法。

本发明的目的可以通过以下技术方案来实现：

一种多传感器融合的自动驾驶位姿估计方法，包括以下步骤：

实时获取图像数据，并采用图像处理算法对所述图像数据进行增强，获得增强图像；

基于所述增强图像，采用特征提取方法提取视觉特征点并进行追踪；

实时获取激光点云以构建局部地图，并将所述视觉特征点与其进行深度关联；

基于所述视觉特征点与其深度，采用视觉重投影与IMU预积分方法优化获得初始位姿；

基于所述图像数据，采用词袋模型进行匹配，计算累积飘移量；

基于所述激光点云，采用曲率及协方差提取点云特征点；

将所述点云特征点与局部地图进行匹配，计算残差，并根据曲率赋予每个特征点不同的残差权重；

基于所述初始位姿、残差和残差权重，采用李代数形式表示位姿，从而计算雅可比矩阵，获得实时位姿；

基于所述实时位姿和累积飘移量，采用增量平滑算法进行位姿优化，获得最终的位姿。

进一步地，所述提取视觉特征点的具体步骤包括：

提取所述增强图像中的FAST角点作为先验角点；

基于所述先验角点及其相邻的若干像素点构建矩形区域，并采用离散微分算子在所述矩形区域计算梯度值；

构建所述先验角点的协方差矩阵，并基于所述梯度值计算特征值；

基于所述特征值对先验角点进行排序；

按照最小距离对所述增强图像进行区域划分，并在每个区域中仅保留特征值最大的先验角点作为视觉特征点。

进一步地，采用KLT光流算法对所述视觉特征点进行追踪。

进一步地，所述视觉特征点进行深度关联的具体步骤包括：

基于获取的激光点云，采用分线程控制构建局部地图；

筛除所述局部地图中对深度关联无用的点，根据角度位置将其划分为若干区域，并将每个区域中深度最小的点作为区域的唯一选中点，以构建网格地图；

将所述网格地图和视觉特征点正则化到归一化球面上，搜索视觉特征点在球面上的若干最近邻的激光正则点；

基于所述最近邻的激光正则点查询所述局部地图上对应的原始点，并构成平面；

基于所述平面上各激光点的坐标和视觉特征点在球面的坐标计算视觉特征点的深度，从而完成视觉特征点与其进行深度关联。

进一步地，采用K-D树算法搜索所述激光正则点。

进一步地，所述点云特征点包括边缘特征点和平面特征点。

进一步地，采用LOAM方法计算激光点的曲率，根据所述曲率获得边缘特征点。

进一步地，所述采用K-D树算法和激光点的曲率计算协方差矩阵，根据协方差矩阵获得平面特征点。

进一步地，所述残差权重包括边缘特征点的残差权重和平面特征点的残差权重，计算表达式分别为：

其中，

进一步地，所述李代数为基于六自由度的李代数。

与现有技术相比，本发明具有以下有益效果：

(1)本发明利用高效提取视觉特征的方法提取视觉特征点，并用激光点云与视觉特征点进行深度关联，提升优化效率，并设计以李代数表示位姿的方法进行优化，最终输出估计的位姿。通过将多种传感器数据进行融合，显著提升了位姿估计的准确性。

(2)本发明以FAST特征提取方法为初值的GFFT特征提取方法，从而避免遍历图片上的每一个点计算卷积。该方法在不损耗特征点稳定程度的基础上，大幅度降低了视觉特征提取模块的耗时。本方案所使用的高效特征提取方法在计算机平台提取150个特征点的耗时仅为10.227毫秒，而传统GFFT特征点方法则需要27.0482毫秒，显著地提升了整体框架的实时性。在准确性方面，由于该方法以FAST角点这样具有一定几何特征的点作为候选点，因此也排除一些直接通过GFFT算法计算出来的错误点，使得系统的稳定性也略优于GFFT特征提取方法。在实际测试中，采用本发明提取特征点的视觉里程计相比于传统的VINS-Mono视觉里程计方法在744米的数据集中位姿估计的均方根误差小了1.62米。

(3)本发明通过激光点云构建局部地图，赋予视觉特征点深度，从而改进视觉惯性里程计的非线性优化问题。传统的方法需要优化出位姿的同时，还需优化视觉特征点的深度。由于每一帧图像上的视觉特征点超过100个，在需要优化的变量中占最大的比例。因此本方法，通过点云局部地图给予部分视觉特征点深度信息，从而大幅降低优化变量，减少优化时间，增加优化的精度。在实际测试中，本方案对于每一个滑动窗口优化的平均耗时为22.36毫秒，而如果优化所有特征点的深度，则需要耗时50.64毫秒。另外，通过激光雷达赋予点深度信息相比于视觉三角化方法计算的深度信息要更加精确，因此使得优化问题中的视觉特征约束更加可靠，非线性优化的稳定性更强。

(4)本发明对于不同特征点的残差，根据曲率给予不同的权重。该方法相比于传统LOAM算法的优势是，对于较为突出的特征点，例如曲率极大的边缘特征和曲率极小的平面特征，能够降低其在优化求解中的影响力。否则在多帧优化的过程中可能始终在优化这几个突出特征点，从而使得优化整体产生对这几个点的过拟合现象，导致最终位姿结果的不准确性。

(5)本发明通过使用李代数形式来表达位姿，并计算了雅可比矩阵。首先，相比于直接使用数值求导的方法，本发明给出了优化问题的雅可比矩阵，即给出了优化下降的方向，使得优化耗时大幅降低，优化结果的准确性也因此提高。另外，由于李代数能够通过左扰动、右扰动的方式更加简便地计算残差对于位姿的导数，且相比于使用欧拉角表示旋转的方法不存在奇异性问题，因此不会出现使用欧拉角优化时在奇异点处稳定性极差的问题，从而提升算法的鲁棒性。

附图说明

图1为本发明方法流程示意图；

图2为本发明方法采用的系统框架图；

图3为本发明高效提取视觉特征点的方法示意图；

图4为本发明基于点云局部地图的视觉特征点深度关联示意图；

图5为本发明视觉惯性里程计因子图优化示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

本实施例提供一种多传感器融合的自动驾驶位姿估计方法，该方法的具体实现步骤如图1所示。为实现该方法，本实施例采用如图2所示的系统结构图，将视觉惯性里程计与激光惯性里程计结合，在前端设计了一种高效提取视觉特征的方法，并用激光雷达对部分特征点赋予深度，提升优化效率，在激光里程计后端设计以李代数表示位姿的方法进行优化，最终使用增量平滑方法对视觉、激光、惯性里程计进行总体融合。接下来，本实施例将结合图1所述的方法流程图和图2所示的该方法采用的系统框架图予以描述：

S1、实时获取图像数据，并采用图像处理算法对所述图像数据进行增强，获得增强图像。

视觉惯性里程计包括相机和IMU(Inertial Measurement Unit，惯性测量单元)，首先获取相机采集的图像数据，根据消息头判断该图像的时间戳是否与其他传感器对齐，而后通过自适应直方图均衡化(Contrast Limited Adaptive Histogram Equalization，CLAHE)算法对图像进行增强，使得图像中边缘地区的清晰度上升，为后续提取特征点做准备。

S2、基于所述增强图像，采用特征提取方法提取视觉特征点并进行追踪。

如图3所示的提取视觉特征点的方法示意图，该步骤具体过程为：

1)首先提取图像中的大量FAST(Features from Accelerated Segment Test，加速分段测试特征点)角点作为先验角点，提取方式为：遍历每个非边缘像素，并选取以该像素为圆心，半径为3的一个圆上的16个像素点，若有连续12个像素的亮度大于一个阈值或小于一个阈值，则认为该中心点为局部灰度变化很快的点，则为FAST角点。使用FAST角点的原因是该方法提取速度快，能够迅速给出先验的角点位置，节省了GFFT(Generalized FastFourier Transform feature extraction，广义快速傅里叶变换特征提取)为所有像素执行卷积计算的时间。

2)将每个FAST角点和其相邻的8个像素一起构成一个3×3的区域，使用Sobel算子对每个区域进行卷积计算。该方法能够计算每个FAST角点邻域在x方向和y方向的灰度加权差，从而进行边缘检测。然后构建每个点关于x和y方向的协方差矩阵，并计算其矩阵特征值，表示当前FAST角点在x方向和y方向的变化梯度，并选出两个方向特征根的最大值。通过这个最大值对所有先验角点从大到小进行排序，特征值越大，则表示该角点成为强角点的可能性越高。

3)为了避免FAST角点中提取的特征点分布不均匀的问题，按照自定义的最小距离对图像进行区域划分，每个区域中仅保留特征根最大的点。具体操作为，将根据特征值大小排序好的FAST角点容器从前向后开始遍历，并根据像素的x坐标和y坐标计算该特征所在的区域，若该区域已经存在特征点，则舍弃当前特征点，保留已经存在的特征；若该区域无特征点，则保留当前点。以此来保证每个区域均保留了最强的特征点。

上述步骤为提取视觉特征点，设计了以FAST特征提取方法为初值的GFFT特征提取方法，从而避免遍历图片上的每一个点计算卷积。该方法在不损耗特征点稳定程度的基础上，大幅度降低了视觉特征提取模块的耗时。本方案所使用的高效特征提取方法在计算机平台提取150个特征点的耗时仅为10.227毫秒，而传统GFFT特征点方法则需要27.0482毫秒，显著地提升了整体框架的实时性。在准确性方面，由于该方法以FAST角点这样具有一定几何特征的点作为候选点，因此也排除一些直接通过GFFT算法计算出来的错误点，使得系统的稳定性也略优于GFFT特征提取方法。在实际测试中，采用本发明提取特征点的视觉里程计相比于传统的VINS-Mono视觉里程计方法在744米的数据集中位姿估计的均方根误差小了1.62米。

获取视觉特征点后，仅对特征点去除畸变以代替对整张图像去除畸变，从而降低算法的耗时。在特征跟踪方面，使用Kanade-Lucas-Tomasi光流算法对多帧中相同的特征进行追踪，在细节方面，使用3层金字塔的KLT光流法，目的是能够在不同分辨率的情况下检测特征移动的情况，使得结果更精确。

S3、实时获取激光点云以构建局部地图，并将所述视觉特征点与其进行深度关联。

该步骤具体分为3步，如下所示：

1)通过一个分线程控制激光雷达局部地图的构建。为了控制数据的处理速度和内存占有的大小，本实施例每间隔0.5秒获取一帧激光雷达点云数据，而后将单帧点云进行体素降采样，以此降低内存消耗，继而把降采样后的点云通过上一帧位姿和IMU的观测，投影到世界坐标系中。

其中，

2)处理点云地图，筛除掉对视觉特征深度关联无用的点。无用点主要包括：x方向小于0的点，即在相机成像平面后方的点；y/x的绝对值大于10的点，即水平视场角绝对值大于84.289°的点；z/x的绝对值大于10的点，即垂直视场角绝对值大于84.289°的点。这样做的目的是减少后续需要处理的点的数量，提升关联效率。

将剩余的点云局部地图根据角度位置进行划分，具体方法为：将前视方向的点云以水平视场角3°、垂直视场角3°的分辨率划分，由于前视方向水平视场角和垂直视场角约为180°，因此总共划分成3600个区域。考虑到相机更容易准确提取到近处点作为特征，因此每个区域中取深度最小的点作为该区域的唯一选中点，从而构建网格地图。

3)将上述点云网格地图和视觉特征点正则化到相机的归一化球面上，而后使用K-D树算法搜索视觉特征点在球面上的3个最近邻的激光雷达正则点，继而通过最近邻点的索引找到激光点云地图中的原始点A、B、C，并判断3点是否共线，如果非共线关系，则连接三点构成一个平面，并认为视觉特征点的原始点P处于该平面上，如图4所示。若通过K-D树搜索出来的激光点超出了设定的搜索阈值，认为是无效的搜索点，即该视觉特征未成功被深度关联。

对于成功关联的第i个视觉特征，目前已知视觉特征在单位球平面上的坐标

已知圆心O到P点的单位向量为

即：

通过上述公式，即可计算出第i个特征点的深度，从而完成特征点的深度关联。

该步骤通过激光点云构建局部地图，赋予视觉特征点深度，从而改进视觉惯性里程计的非线性优化问题。传统的方法需要优化出位姿的同时，还需优化视觉特征点的深度。由于每一帧图像上的视觉特征点超过100个，在需要优化的变量中占最大的比例。因此本方法，通过点云局部地图给予部分视觉特征点深度信息，从而大幅降低优化变量，减少优化时间，增加优化的精度。在实际测试中，本方案对于每一个滑动窗口优化的平均耗时为22.36毫秒，而如果优化所有特征点的深度，则需要耗时50.64毫秒。另外，通过激光雷达赋予点深度信息相比于视觉三角化方法计算的深度信息要更加精确，因此使得优化问题中的视觉特征约束更加可靠，非线性优化的稳定性更强。

S4、基于所述视觉特征点与其深度，采用视觉重投影与IMU预积分方法优化获得初始位姿。

该步骤主要通过非线性优化的方式解决视觉惯性里程计的问题。如图4所示的视觉惯性里程计因子图优化示意图，圆表示的变量为待优化变量，包括相机关键帧的初始位姿x

S5、基于所述图像数据，采用词袋模型进行匹配，计算累积飘移量。

本实施例采用词袋模型(Bag-of-Words，BoW)来进行图像的匹配，是指运行过程中，将第一次拍摄到某一个位置的图像和间隔几分钟后第二次拍摄到同一位置的图像的进行匹配。该匹配过程具体为：当新的关键帧加入时，首先提取描述子，然后根据描述子从字典种查找相应的单词，并与之前的关键帧对比，计算相似度得分。若检测到两个关键帧的时间戳相差超过50秒且相似度得分超过一定阈值，则认为旧的关键帧与新的关键帧匹配成功。在完成新、旧帧的关联后，便能够计算累计飘移量，即匹配的新、旧两帧所在的不同世界坐标系之间的位姿变换，记为

其中，

S6、基于所述激光点云，采用曲率及协方差提取点云特征点。

在激光里程计中，首先通过激光雷达获取当前帧点云，而后进行点云去畸变，具体方法为根据每一个扫描点的时间戳和当前帧中首个扫描点的时间戳，将所有的点投影到该帧首个扫描点所在的时间戳下，其表达形式为：

其中，

该步骤为通过曲率及协方差提取点云特征点，包括边缘特征点和平面特征点。首先采用经典的LOAM(Lidar Odometry and Mapping，激光雷达里程计与建图)方法，针对每一条扫描线，从第五个点开始遍历直到倒数第5个点，根据每个点序号前、后各五个点的坐标计算当前位置的曲率，计算方法如下：

其中，C

平面特征点的提取则是在上述曲率计算的基础上先找出一些候选点，而后通过K-D(K-Dimensional)树算法一个平面候选点以及该点投影到局部点云地图上的五个最近点，记为p

其中，

对cov矩阵进行特征值分解，得到从大到小排列的三个特征值λ

S7、将所述点云特征点与局部地图进行匹配，计算残差，并根据曲率赋予每个特征点不同的残差权重。

根据视觉惯性里程计的初始位姿将当前帧特征点投影到世界坐标系下，并与局部点云地图通过距离进行匹配。在完成匹配后，计算目标函数即残差，并根据曲率给予每个特征点不同的残差权重。本实施例首先借鉴LOAM算法的思路，以当前帧中边缘特征点到边缘特征地图中的最近邻目标线的距离，以及当前帧中平面特征点到平面特征地图中最近邻平面的距离作为残差的基础值。边缘特征的残差基础值如下所示：

其中，d

其中，p

根据距离计算了基础残差后，本实施例基于步骤8中计算的曲率给每一个特征点赋予一个权重，如下所示：

其中，

本步骤对于不同特征点的残差，根据曲率给予不同的权重。该方法相比于传统LOAM算法的优势是，对于较为突出的特征点，例如曲率极大的边缘特征和曲率极小的平面特征，能够降低其在优化求解中的影响力。否则在多帧优化的过程中可能始终在优化这几个突出特征点，从而使得优化整体产生对这几个点的过拟合现象，导致最终位姿结果的不准确性。

S8、基于所述初始位姿、残差和残差权重，采用李代数形式表示位姿，从而计算雅可比矩阵，获得实时位姿。

在位姿优化问题中，本实施例设计了基于六自由度的李代数ξ表示位姿的优化方法，其雅可比矩阵J为距离残差对于位姿求导的结果，即

其中，

其中，X

其中，X

在完成优化，输出位姿后，本实施例还基于ICP(Iterative Closest Point，迭代最近点)算法设计了一套激光回环算法，来检测相机无共视情况下的回环因子，输入到因子图优化中。

本步骤使用李代数形式来表达位姿，并计算了雅可比矩阵。首先，相比于直接使用数值求导的方法，本发明给出了优化问题的雅可比矩阵，即给出了优化下降的方向，使得优化耗时大幅降低，优化结果的准确性也因此提高。另外，由于李代数能够通过左扰动、右扰动的方式更加简便地计算残差对于位姿的导数，且相比于使用欧拉角表示旋转的方法不存在奇异性问题，因此不会出现使用欧拉角优化时在奇异点处稳定性极差的问题，从而提升算法的鲁棒性。

S9、基于所述实时位姿和累积飘移量，采用增量平滑算法进行位姿优化，获得最终的位姿。

对所述位姿进行优化，获得最终的位姿。

在获得了视觉惯性因子、激光里程计因子、IMU预积分因子、视觉回环因子以及激光回环因子之后，采用基于贝叶斯树的增量平滑算法进行位姿优化，在实际操作中，本实施例使用GTSAM库进行实现，最终输出一个准确、实时的位姿。

将视觉、激光、IMU预积分里程计结合视觉回环、激光回环因子一同输入增量平滑优化中，该方案的优势是即使其中部分传感器失效，仍然能够在短时间输出一个较为精确的里程计。在时速超过60km/h的高速国道场景位姿估计试验中，本发明的平移平均精度误差仅有0.543米，远远高于传统的视觉惯性里程计、激光惯性里程计等；尤其是在高度z方向上，本发明的精度误差仅有0.308米，而其他激光惯性里程计则超过10米，从而证明了本发明能够在达到实时性的基础上，鲁棒地输出高精度的位姿估计结果。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。