CN113721248B - 一种基于多源异构传感器的融合定位方法及*** - Google Patents

Abstract

本发明提供了一种基于多源异构传感器的融合定位方法及***，属于车辆融合定位技术领域。该方法用于定位目标车辆，所述目标车辆设置有多个激光雷达以及其他定位装置，该融合定位方法包括：将各个所述激光雷达采集的点云数据按照同一坐标系进行融合，得到融合点云数据；从所述融合点云数据识别每一所述激光雷达所对应的点云区域内的目标点云数据；针对每个所述激光雷达，对所述目标点云数据进行特征提取，得到该激光雷达对应的特征；将各个所述特征进行合并，并根据合并后的特征和所述其他定位装置所采集的定位信息确定所述目标车辆的第一位姿信息。本发明的融合定位方法及***能够在保证定位精度。

Description

一种基于多源异构传感器的融合定位方法及***

技术领域

本发明属于车辆融合定位技术领域，特别是涉及一种基于多源异构传感器的融合定位方法及***。

背景技术

车辆定位功能是实现自动驾驶最基本也是最重要的环节之一，定位的精度和可靠性会影响到自动驾驶整体功性能的表现。一般而言，自动驾驶需求的定位精度在10厘米左右，并且能够在空旷、遮挡或半遮挡等工况下应用。

当前，使用比较多的定位方法直接基于全球导航卫星***(GNSS)，与惯性测量单元(IMU)和实时动态定位(RTK)做融合，给出车辆在全局坐标系下的经纬度，与高精地图匹配，得到车辆局部坐标下相对位置，例如，车辆距离车道线、停止线的位置。这种方法在空旷区域具有高精度的特点，但在树木、楼宇或通道对卫星信号有遮挡、半遮挡的情况下，尽管有IMU递推，但其定位精度仍会大幅下降，可靠性急剧下降。为解决这种场景下的定位问题，许多方案通过增加其他传感器，例如，轮速、激光或视觉等信息源，通过递推、同步定位与地图构建(SLAM)以及信息融合，来提高遮挡、半遮挡情况下的定位稳定性。受光线环境影响，视觉SLAM主要在环境光照变化较为稳定、不突变的场景下使用，目前，使用激光SLAM作为融合源的方案较多。

对于大型车辆来说，传感器安装布置时会考虑将两个机械旋转激光雷达安装在车辆的左右前角，来提升侧方目标感知的覆盖能力。机械旋转激光雷达具有360度的感知范围，但在该安装方式下受车体遮挡，每个激光雷达都至少会有90度～180度的点云浪费，不能参与SLAM点图匹配，降低了信噪比，影响建图和实时定位精度。按照激光定位的工作原理，需要先建图，再匹配定位。因此，激光图层的精度会影响实时定位的精度。激光SLAM建图需要回环检测修正，来提高图层精度，但精度随着行驶距离会有下降。

发明内容

本发明第一方面的一个目的是提供一种基于多源异构传感器的融合定位方法，能够在保证定位精度。

本发明的进一步的一个目的是要进一步提高算法稳定性以及建图和实时定位的精度。

本发明第二方面的一个目的是提供一种基于多源异构传感器的融合定位***，能够在保证定位精度。

特别地，本发明提供了一种基于多源异构传感器的融合定位方法，用于定位目标车辆，所述目标车辆设置有多个激光雷达以及其他定位装置，所述方法包括：

将各个所述激光雷达采集的点云数据按照同一坐标系进行融合，得到融合点云数据；

从所述融合点云数据识别每一所述激光雷达所对应的点云区域内的目标点云数据；

针对每个所述激光雷达，对所述目标点云数据进行特征提取，得到该激光雷达对应的特征；

将各个所述特征进行合并，并根据合并后的特征和所述其他定位装置所采集的位姿信息确定所述目标车辆的第一位姿信息。

可选地，每一所述激光雷达所对应的点云区域内的目标点云数据包括该激光雷达采集的原始点云数据以及落入所述原始数据对应的点云区域内的其他所述激光雷达采集的点云数据。

可选地，将各个所述激光雷达采集的点云数据按照同一坐标系进行融合的步骤包括：

提取时间对齐的各个所述激光雷达的点云数据；

将时间对齐的各个所述激光雷达的点云数据按照预设顺序排序，得到排序后的对齐点云数据；

将所述对齐点云数据投影至同一坐标系，以形成融合后的点云数据。

可选地，将所述对齐点云数据投影至同一坐标系的步骤包括：

通过对各个所述激光雷达标定外参数来将所有所述激光雷达的所述对齐点云数据对齐到车身坐标。

可选地，根据合并后的特征和所述其他定位装置所采集的定位信息确定所述目标车辆的第一位姿信息的步骤包括：

根据相邻两帧的点云数据所对应的合并后的特征以及预设迭代算法求解所述车辆的位姿变换量，其中所述预设迭代算法中的迭代初始变换值根据所述其他定位装置所采集的定位信息确定；

根据预设的优化算法优化所述位姿变化量，并根据优化后的所述位姿变化量确定所述第一位姿信息。

可选地，所述目标车辆还与全球导航卫星***通信，其特征在于，根据合并后的特征和所述其他定位装置所采集的定位信息确定所述目标车辆的第一位姿信息的步骤之后，还包括：

获取所述全球导航卫星***的置信度信息；

判断所述全球导航卫星***的置信度信息是否满足可靠性条件；

若是，利用所述全球导航卫星***所解析的位姿信息与所述第一位姿信息进行融合，以得出融合后的第二位姿信息；

输出所述第二位姿信息。

可选地，所述置信度信息包括所述全球导航卫星***的解的状态信息和搜星数量。

可选地，判断所述全球导航卫星***的置信度信息是否满足可靠性条件的步骤包括：

当所述全球导航卫星***的解为稳定解且所述搜星数量大于标定数量时，判定所述全球导航卫星***的置信度信息满足可靠性条件。

可选地，判断所述全球导航卫星***的置信度信息是否满足可靠性条件的步骤之后还包括：

若否，则输出所述第一位姿信息。

特别地，本发明还包括一种基于多源异构传感器的融合定位***，包括控制单元，包括存储器和处理器，所述存储器内存储有控制程序，所述控制程序被所述处理器执行时用于实现上述任一项所述的融合定位方法。

本发明通过先将各个激光雷达的点云数据进行融合，得到融合点云数据，也即进行点云级别的时空同步，然后在融合的点云数据中识别每一激光雷达的点云区域内的目标点云数据，根据每个激光雷达的目标点云数据进行特征提取，也就是说特征的提取不是基于总的融合的点云数据进行提取的，还是依据各个激光雷达来分别提取的，然后再将各个特征进行合并。这种数据处理方式解决了多激光雷达场景下建图与定位的信噪比问题，相比采用单边激光定位后再融合，稳定性和定位精度提高；而相比完全点云级融合定位，算法运算量小，易于部署实现。

进一步地，本发明通过判断全球导航卫星***的置信度来判断通过全球导航卫星***所解析出来的位姿信息是否可靠，然后在全球导航卫星***得出的位姿信息可靠时将该位姿信息与第一位姿信息进行融合，以提高算法稳定性以及实时定位的精度。

根据下文结合附图对本发明具体实施例的详细描述，本领域技术人员将会更加明了本发明的上述以及其他目的、优点和特征。

附图说明

后文将参照附图以示例性而非限制性的方式详细描述本发明的一些具体实施例。附图中相同的附图标记标示了相同或类似的部件或部分。本领域技术人员应该理解，这些附图未必是按比例绘制的。附图中：

图1是根据本发明一个实施例的基于多源异构传感器的融合定位方法的流程图；

图2是根据本发明另一个实施例的基于多源异构传感器的融合定位方法的流程图。

具体实施方式

图1是根据本发明一个实施例的基于多源异构传感器的融合定位方法的流程图。本发明提供了一种基于多源异构传感器的融合定位方法，用于定位目标车辆，目标车辆设置有多个激光雷达以及其他定位装置，其他定位装置可以包括全球导航卫星***(GNSS)的接收器和惯性测量单元(IMU)。一个实施例中，该融合定位方法包括：

步骤S100，将各个激光雷达采集的点云数据按照同一坐标系进行融合，得到融合点云数据。一般地，各个激光雷达采集数据后，通过与各个激光雷达通信的车辆控制单元来获取各个激光雷达的点云数据，例如车辆的域控制器，然后再进行点云数据的融合。这里的坐标系可以是车身坐标系，此步骤将各个激光雷达的点云数据统一到一个坐标系下，以便数据的融合。

步骤S200，从融合点云数据识别每一激光雷达所对应的点云区域内的目标点云数据。每一激光雷达都对应有一个可探测范围，这个可探测范围内的数据投影在坐标系上(例如车身坐标系)后会形成一个数据区域，即这个激光雷达的点云区域。每一激光雷达所对应的点云区域内的目标点云数据包括该激光雷达采集的原始点云数据以及落入原始数据对应的点云区域内的其他激光雷达采集的点云数据。

步骤S300，针对每个激光雷达，对所述目标点云数据进行特征提取，得到该激光雷达对应的特征。这里的特征包括面特征和线特征。

一般地，各个激光雷达会存在视角重叠区，例如车辆前部的两个具有360°感知范围的机械旋转激光雷达。一个实施例中，每一激光雷达所对应的点云区域内的数据包括其自身的点云数据以及在点云区域内的其他激光雷达的点云数据，即例如A激光雷达的点云区域内F内包括A激光雷达的全部点云数据X以及落入该点云区域F内的其他激光雷达的点云数据Y，此时对A激光雷达进行特征提取所依据的点云数据就包括了点云区域F内点云数据X和点云数据Y。

步骤S400，将各个特征进行合并，并根据合并后的特征和其他定位装置所采集的定位信息确定目标车辆的第一位姿信息。

例如应用SLAM框架进行建图和定位。其他定位装置可以是车辆的惯性测量单元，其采集的定位信息包括物体的三轴姿态角以及加速度等信息。第一位姿信息就是车辆的位置和姿态信息，例如与车辆的X轴、Y轴和Z轴的夹角所确定的姿态信息。一个实施例中，根据SLAM算法，步骤S400包括：

根据相邻两帧的点云数据所对应的合并后的特征以及预设迭代算法求解车辆的位姿变换量，其中预设迭代算法中的迭代初始变换值根据其他定位装置所采集的定位信息确定。本步骤相当于SLAM算法中的前端初步定位过程，这里的预设迭代算法可以是迭代最近邻算法，其他定位装置为惯性测量单元。

根据预设的优化算法优化位姿变化量，并根据优化后的位姿变化量确定第一位姿信息。本步骤相当于SLAM算法中的后端优化定位过程。预设的优化算法可以是较为常用的LM(Levenberg-Marquarelt)算法或高斯牛顿迭代法。

当然在后面还包括回环检测的步骤，通过从激光雷达的点云数据中提取特征后与已建立的地图特征之间进行相似性检测与匹配，通过与高相似性的地图进行匹配后求解处位姿变换，优化车辆的全局定位误差，以降低累积误差。

本实施例中，通过先将各个激光雷达的点云数据进行融合，得到融合点云数据，也即进行点云级别的时空同步，然后在融合点云数据中识别每一激光雷达的点云区域内的目标点云数据，根据每个激光雷达的目标点云数据进行特征提取，也就是说特征的提取不是基于总的融合的点云数据进行提取的，还是依据各个激光雷达来分别提取的，然后再将各个特征进行合并。这种数据处理方式解决了多激光雷达场景下建图与定位的信噪比问题，相比采用单边激光定位后再融合，稳定性和定位精度提高；而相比完全点云级融合定位，算法运算量小，易于部署实现。

图2是根据本发明另一个实施例的基于多源异构传感器的融合定位方法的流程图。如图2所示，另一个实施例中，步骤S100包括：

步骤S102，提取时间对齐的各个激光雷达的点云数据。也即提取同一时间的各个激光雷达的点云数据，可以通过车辆控制器来完成。

步骤S104，将时间对齐的各个激光雷达的点云数据按照预设顺序排序，得到排序后的对齐点云数据。一个具体的例子中，假设激光雷达1有10000个点云数据，激光雷达2有5000个点云数据，按照先激光雷达1后激光雷达2的顺序进行存储形成编号为1-15000的数据点。当需要执行步骤S200和S300时，根据点云区域的边界去识别其内部的所有点云数据的编号，再去对应的激光雷达处提取目标点云数据。例如激光雷达1的点云区域内有1到10000的点云数据和12000-14000的点云数据，那么激光雷达1就根据1到10000的点云数据和12000-14000的点云数据进行特征提取。这里的各个点云数据的编号就是计算机可以识别的分区标志，在将各个激光雷达的点云数据进行顺序编号后，通过该编号就可以分别出是哪个激光雷达的点云数据，进而方便计算机根据编号去对应的激光雷达内提取相应的点云数据。按照一定编号顺序排列的点云数据即上述的对齐点云数据。

步骤S106，将对齐点云数据投影至同一坐标系，以形成融合后的点云数据。这里将多个激光雷达的点云数据进行点云级的融合。当将对齐点云数据投影至同一坐标系后，通过识别对齐点云数据的编号就能识别出对应的激光雷达，进而方便后续的目标点云数据的识别和特征的提取。

一个实施例中，步骤S106包括：

通过对各个激光雷达标定外参数来将所有激光雷达的对齐点云数据对齐到车身坐标。

即通过根据各个激光雷达的外参数标定结果，就可以各个激光雷达的对齐点云数据融合到一起，例如车辆前端的左侧激光雷达和右侧激光雷达，可以将右侧激光雷达的点云数据投影到左侧激光雷达的点云数据中，从而合并点云。

另一个实施例中，目标车辆还与全球导航卫星***通信，如图2所示，步骤S400之后还包括：

步骤S500，获取全球导航卫星***的置信度信息。一个实施例中，置信度信息包括全球导航卫星***的解的状态信息和搜星数量，可以从全球导航卫星***的解算单元直接获取。

步骤S600，判断全球导航卫星***的置信度信息是否满足可靠性条件，若是，进入步骤S702。一个实施例中，当全球导航卫星***的解为稳定解且搜星数量大于标定数量时，判定全球导航卫星***的置信度信息满足可靠性条件。这里的标定数量可以人为设置，例如大于5颗。一般地，全球导航卫星***的解的状态可以简单的分为稳定解(RTK状态)和非稳定解(非RTK状态)，在稳定解时定位精度很高。

步骤S702，利用全球导航卫星***所解析的位姿信息与第一位姿信息进行融合，以得出融合后的第二位姿信息。例如分别对第一位姿信息和全球导航卫星***所解析的位姿信息进行权重设置，将各自的权重与相应的位姿信息相乘后进行相加即得到第二位姿信息，这里各自的权重可以根据其对应的***的解的状态确定，当全球导航卫星***的解为稳定解或固定解时，提高全球导航卫星***所解析的位姿信息的权重。

步骤S704，输出第二位姿信息。也就是最终的目标车辆的定位信息。

本实施例通过判断全球导航卫星***的置信度来判断通过全球导航卫星***所解析出来的位姿信息是否可靠，然后在全球导航卫星***得出的位姿信息可靠时将该位姿信息与第一位姿信息进行融合，以提高算法稳定性以及实时定位的精度。

当然在步骤S600判定全球导航卫星***的置信度信息不满足可靠性条件时，则将第一位姿信息作为最终的定位信息进行输出(即步骤S706)。

本发明还提供了一种基于多源异构传感器的融合定位***，包括控制单元，包括存储器和处理器，存储器内存储有控制程序，控制程序被处理器执行时用于实现上述任一实施例和实施例组合中的融合定位方法。处理器可以是一个中央处理单元(centralprocessing unit，简称CPU)，或者为数字处理单元等等。处理器通过通信接口收发数据。存储器用于存储处理器执行的程序。存储器是能够用于携带或存储具有指令或数据结构形式的期望的程序代码并能够由计算机存取的任何介质，也可以是多个存储器的组合。上述计算程序可以从计算机可读存储介质下载到相应计算/处理设备或者经由网络(例如因特网、局域网、广域网和/或无线网络)下载到计算机或外部存储设备。一个实施例中，该融合定位***可以是车辆的域控制器。

本实施例的定位***通过先将各个激光雷达的点云数据进行融合，合并成一组点云，也即进行点云级别的时空同步，然后在融合的点云数据中提取每一激光雷达的点云区域的数据，根据所提取的数据再提取相应的特征，也就是说特征的提取不是基于总的融合的点云数据进行提取的，还是依据各个激光雷达来分别提取的，然后再将各个特征进行合并。这种数据处理方式解决了多激光雷达场景下建图与定位的信噪比问题，相比采用单边激光定位后再融合，稳定性和定位精度提高；而相比完全点云级融合定位，算法运算量小，易于部署实现。

至此，本领域技术人员应认识到，虽然本文已详尽示出和描述了本发明的多个示例性实施例，但是，在不脱离本发明精神和范围的情况下，仍可根据本发明公开的内容直接确定或推导出符合本发明原理的许多其他变型或修改。因此，本发明的范围应被理解和认定为覆盖了所有这些其他变型或修改。

Claims (8)

1.一种基于多源异构传感器的融合定位方法，用于定位目标车辆，所述目标车辆设置有多个激光雷达以及其他定位装置，其特征在于，所述方法包括：

将各个所述激光雷达采集的点云数据按照同一坐标系进行融合，得到融合点云数据；

从所述融合点云数据识别每一所述激光雷达所对应的点云区域内的目标点云数据；

针对每个所述激光雷达，对所述目标点云数据进行特征提取，得到该激光雷达对应的特征；

将各个所述特征进行合并，并根据合并后的特征和所述其他定位装置所采集的位姿信息确定所述目标车辆的第一位姿信息；

每一所述激光雷达所对应的点云区域内的目标点云数据包括该激光雷达采集的原始点云数据以及落入所述原始点云数据对应的点云区域内的其他所述激光雷达采集的点云数据；

将各个所述激光雷达采集的点云数据按照同一坐标系进行融合的步骤包括：

提取时间对齐的各个所述激光雷达的点云数据；

将时间对齐的各个所述激光雷达的点云数据按照预设顺序排序，得到排序后的对齐点云数据；

将所述对齐点云数据投影至同一坐标系，以形成融合后的点云数据。

2.根据权利要求1所述的融合定位方法，其特征在于，将所述对齐点云数据投影至同一坐标系的步骤包括：

通过对各个所述激光雷达标定外参数来将所有所述激光雷达的所述对齐点云数据对齐到车身坐标。

3.根据权利要求2所述的融合定位方法，其特征在于，根据合并后的特征和所述其他定位装置所采集的定位信息确定所述目标车辆的第一位姿信息的步骤包括：

根据相邻两帧的点云数据所对应的合并后的特征以及预设迭代算法求解所述车辆的位姿变化量，其中所述预设迭代算法中的迭代初始变换值根据所述其他定位装置所采集的定位信息确定；

根据预设的优化算法优化所述位姿变化量，并根据优化后的所述位姿变化量确定所述第一位姿信息。

4.根据权利要求1-3中任一项所述的融合定位方法，所述目标车辆还与全球导航卫星***通信，其特征在于，根据合并后的特征和所述其他定位装置所采集的定位信息确定所述目标车辆的第一位姿信息的步骤之后，还包括：

获取所述全球导航卫星***的置信度信息；

判断所述全球导航卫星***的置信度信息是否满足可靠性条件；

若是，利用所述全球导航卫星***所解析的位姿信息与所述第一位姿信息进行融合，以得出融合后的第二位姿信息；

输出所述第二位姿信息。

5.根据权利要求4所述的融合定位方法，其特征在于，

所述置信度信息包括所述全球导航卫星***的解的状态信息和搜星数量。

6.根据权利要求5所述的融合定位方法，其特征在于，判断所述全球导航卫星***的置信度信息是否满足可靠性条件的步骤包括：

当所述全球导航卫星***的解为稳定解且所述搜星数量大于标定数量时，判定所述全球导航卫星***的置信度信息满足可靠性条件。

7.根据权利要求4所述的融合定位方法，其特征在于，判断所述全球导航卫星***的置信度信息是否满足可靠性条件的步骤之后还包括：

若否，则输出所述第一位姿信息。

8.一种基于多源异构传感器的融合定位***，其特征在于，包括控制单元，包括存储器和处理器，所述存储器内存储有控制程序，所述控制程序被所述处理器执行时用于实现根据权利要求1-7中任一项所述的融合定位方法。