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

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

背景技术：

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

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

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

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

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

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

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

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

技术实现思路

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

26、进一步地，采用k-d树算法搜索所述激光正则点。

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

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

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

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

31、

32、

33、其中，与分别为边缘特征点的残差权重和平面特征点的残差权重，与分别表示边缘特征点与平面特征点的曲率，pi为集合中某一个特征点，fe与fp分别为边缘特征点集合与平面特征点集合，为pi特征点的曲率。

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

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

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

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

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

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

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