CN114739415A

CN114739415A - 基于多传感器融合的多车定位方法、装置及计算机设备

Info

Publication number: CN114739415A
Application number: CN202210295963.3A
Authority: CN
Inventors: 曾宇骏; 呼晓畅; 谢广磊; 刘大学; 徐昕; 方强
Original assignee: National University of Defense Technology
Current assignee: National University of Defense Technology
Priority date: 2022-03-24
Filing date: 2022-03-24
Publication date: 2022-07-12

Abstract

上述基于多传感器融合的多车定位方法、装置及计算机设备，通过获取编队中定位无人车上安装的组合惯性导航传感器数据、感知数据以及被定位无人车的导航定位数据，对导航定位数据、感知数据以及被定位无人车上滤波器对当前时间步的预测数据进行关联，得到关联数据包；根据预先设置的自适应切换策略，选择关联数据包中的其中一种数据作为被定位无人车的观测数据，最后，将观测数据输入被定位无人车上的滤波器，得到编队中被定位无人车的定位结果。将多个传感器的数据综合分析可以更加准确地描述外界环境，由于多车通讯系统以及各种传感器数据在不同环境条件下的有效性存在差异，进行数据关联并通过环境条件自主切换定位源，提高了定位数据的可信度。

Description

基于多传感器融合的多车定位方法、装置及计算机设备

技术领域

本申请涉及无人车编队定位技术领域，特别是涉及一种基于多传感器融合的多车定位方法、装置及计算机设备。

背景技术

为了完成编队任务，编队中的任何一辆无人车都需要知道其他无人车之间的绝对位置才能完成相应的路径规划。通常，多车编队系统的位置信息交换通过主从式或分布式通讯架构完成多车之间的定位信息的交互，编队中任意无人车在获取其他任意无人车的绝对位姿、速度信息之后，才能完成相应的编队任务。

但单纯依靠电台交互GPS等信息的定位方法，使用的场景较为有限，无法胜任复杂多变的环境。当面临强电磁环境、虚假定位信息、声光电等多重环境干扰，通讯拒止、无法获取绝对位置的情况时有发生，而且是并发存在的。由此，多传感器融合技术应运而生，但是复杂环境下不同传感器数据的有效性与可信度不同，如何根据环境条件自适应的切换可信的数据是一个具有十分挑战的难题。同时，车间通讯时间坐标系原点异常、定位无人车多传感器触发周期不稳定性以及不同传感器数据的坐标系不一致也给数据融合结果的统一和滤波器的状态设计带来了挑战。

发明内容

基于此，有必要针对上述技术问题，提供一种能够依据通讯环境自主切换定位源的基于多传感器融合的多车定位方法和装置。

一种基于多传感器融合的多车定位方法，所述方法包括：

获取编队中定位无人车上安装的多个传感器的传感器数据；所述传感器数据包括：组合惯性导航传感器数据和目标感知传感器数据；

根据目标感知传感器数据确定被定位无人车的感知数据，通过无线通讯获取被定位无人车的导航定位数据；

对所述导航定位数据、所述感知数据以及被定位无人车上滤波器对当前时间步的预测数据进行关联，得到关联数据包；

根据预先设置的自适应切换策略，选择所述关联数据包中的其中一种数据作为被定位无人车的观测数据；

将所述观测数据输入所述被定位无人车上的滤波器，得到编队中被定位无人车的定位结果。

在其中一个实施例中，所述组合惯性导航传感器数据包括：所述定位无人车的导航定位数据和IMU积分定位数据；所述定位无人车的导航定位数据中包括定位协方差；所述目标感知传感器数据包括图像、毫米波雷达点云和激光雷达点云。

在其中一个实施例中，所述根据目标感知传感器数据确定被定位无人车的感知数据，包括：

将所述图像输入YOLO网络，得到带目标框的第一图像，将所述激光雷达点云投影到所述第一图像上，提取位于所述第一图像中目标框中的点云，采用最近邻聚类算法分别处理所述目标框中的点云，得到对应目标框的聚类中心点，结合所述第一图像和所述聚类中心点得到第二图像。

对所述激光雷达点云极坐标化，对极坐标化点云进行地面分割得到非地面障碍区域，采用最近邻聚类算法对所述非地面障碍区域中的正障碍点进行过分割，获得正障碍区域，对所述正障碍区域进行最近邻聚类得到第一激光雷达点云。

在其中一个实施例中，在对所述导航定位数据、所述感知数据以及被定位无人车上滤波器对当前时间步的预测数据进行关联，得到关联数据包之前，包括：

获取上一时间步被定位无人车的感知数据和导航定位数据；

通过比较被定位无人车上一帧和当前帧导航定位数据中的时间戳判断无线通讯是否正常，以及根据所述定位协方差判断导航定位是否正常。

在其中一个实施例中，所述导航定位数据、所述感知数据以及被定位无人车上滤波器对当前时间步的预测数据进行关联，得到关联数据包，包括：

当无线通讯且导航定位正常时：

将当前帧被定位无人车的导航定位数据与所述第二图像进行最优匹配，若匹配成功，则存储对应的第二图像数据；

根据所述第一激光雷达点云计算点云中心，将当前帧被定位无人车的导航定位数据与所述点云中心进行最优匹配，若匹配成功，则存储对应的第一激光雷达点云中心数据和被定位无人车对应的的点云特征；

将当前帧被定位无人车的导航定位数据与所述毫米波雷达点云进行最优匹配，若匹配成功，则存储对应的毫米波雷达点云数据。

在其中一个实施例中，对所述导航定位数据、所述感知数据以及被定位无人车上滤波器对当前时间步的预测数据进行关联，得到关联数据包，还包括：

当无线通讯或导航定位异常时：

根据所述第一激光雷达点云计算点云中心，将滤波器对当前时间步的预测数据与所述点云中心进行最优匹配，若匹配成功，得到被定位无人车对应的点云特征，计算被定位无人车的上一帧和当前帧的点云特征的比值，若所述比值小于预设阈值，则存储对应的第一激光雷达点云中心数据和被定位无人车对应的的点云特征；

将当前帧和上一帧的毫米波雷达点云进行最优匹配，若匹配成功，得到对应的被定位无人车的毫米波雷达点云，将所述毫米波雷达点云与滤波器对当前时间步的预测数据进行最优匹配，若匹配成功，则存储对应的毫米波雷达点云数据，若匹配失败，则将所述毫米波雷达点云与第一激光雷达点云进行最优匹配，若匹配成功，则存储对应的毫米波雷达点云数据；

将当前帧和上一帧的第二图像进行最优匹配，若匹配成功，得到对应的被定位无人车的第二图像，将所述第二图像与滤波器对当前时间步的预测数据进行最优匹配，若匹配成功，则存储对应的第二图像数据。

在其中一个实施例中，根据预先设置的自适应切换策略，选择所述关联数据中的一种作为观测数据，包括：

当无线通讯且导航定位正常时，选择所述导航定位数据作为被定位无人车的观测数据；

当无线通讯或导航定位异常时，自动切换到基于目标感知传感器进行无人车定位的模型，根据所述模型预先设置的数据优先级，并根据所述目标感知传感器的实际运行状态，从所述第二图像数据、第一激光雷达点云中心数据或者毫米波雷达数据中选择一种数据作为当前时刻的观测数据。

一种基于多传感器融合的多车定位装置，所述装置包括：

数据获取模块，用于获取编队中定位无人车上安装的多个传感器的传感器数据；所述传感器数据包括：组合惯性导航传感器数据和目标感知传感器数据；

数据确定模块，用于根据目标感知传感器数据确定被定位无人车的感知数据，通过无线通讯获取被定位无人车的导航定位数据；

数据关联模块，用于对所述导航定位数据、所述感知数据以及被定位无人车上滤波器对当前时间步的预测数据进行关联，得到关联数据包；

数据选择模块，用于根据预先设置的自适应切换策略，选择所述关联数据包中的其中一种数据作为被定位无人车的观测数据；

结果输出模块，用于将所述观测数据输入所述被定位无人车上的滤波器，得到编队中被定位无人车的定位结果。

一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述实施例中方法的步骤。

一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述实施例中方法的步骤。

上述基于多传感器融合的多车定位方法、装置及计算机设备，通过获取编队中定位无人车上安装的组合惯性导航传感器数据、感知数据以及被定位无人车的导航定位数据，对导航定位数据、感知数据以及被定位无人车上滤波器对当前时间步的预测数据进行关联，得到关联数据包；根据预先设置的自适应切换策略，选择关联数据包中的其中一种数据作为被定位无人车的观测数据，最后，将观测数据输入被定位无人车上的滤波器，得到编队中被定位无人车的定位结果。将多个传感器的数据集中在一起综合分析可以更加准确可靠地描述外界环境，由于多车通讯系统以及各种传感器数据在不同环境条件下的有效性存在差异，进行数据关联并通过环境条件自主切换定位源，提高了定位数据的可信度。

附图说明

图1为一个实施例中基于多传感器融合的多车定位方法的流程示意图；

图2为一个实施例中第一图像与激光雷达点云的融合示意图；

图3为一个实施例中实际场景中地面分割与障碍物检测效果示意图；

图4为一个实施例中毫米波雷达数据在极坐标与直角坐标系之间的数据关系图；

图5为一个实施例中一种基于多传感器融合的多车定位系统框图；

图6为一个实施例中基于多传感器融合的多车定位装置的结构框图；

图7为一个实施例中计算机设备的内部结构图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。

在一个实施例中，如图1所示，提供了一种基于多传感器融合的多车定位方法，包括以下步骤：

步骤102，获取编队中定位无人车上安装的多个传感器的传感器数据。

传感器数据包括：组合惯性导航传感器数据和目标感知传感器数据。

编队是指具有一定顺序或者某种组织形式的队伍，编队中任意单元在获取其他任意单元之间的绝对位置之后，才能完成相应的编队任务。

定位无人车可以是编队中任意无人车，被定位无人车可以是编队中定位无人车以外的任意若干无人车；多个传感器可以是激光雷达、毫米波雷达、相机、组合惯性导航等传感器的任意组合。

组合惯性导航是指包含卫星定位和惯性测量单元(IMU)的导航系统，其中IMU不受外界电磁干扰的影响。

步骤104，根据目标感知传感器数据确定被定位无人车的感知数据，通过无线通讯获取被定位无人车的导航定位数据。

导航定位数据是定位无人车通过车间无线通讯获取的被定位无人车的定位结果，该定位结果为导航定位系统绝对坐标系下的，由于IMU积分位姿不受电磁干扰影响，本车对于其他车的位姿优化采用IMU坐标系，将其转换到定位无人车IMU的全局坐标系下才能为该定位无人车定位、规划编队路径所用。

同理，目标感知传感器数据也需要转换到定位无人车IMU的全局坐标系下得到感知数据。

步骤106，对导航定位数据、感知数据以及被定位无人车上滤波器对当前时间步的预测数据进行关联，得到关联数据包。

数据关联是多传感器数据融合的关键技术，指的是建立目标的导航定位数据、多传感器对目标的若干感知数据等与某个目标的对应关系，得到不同传感器对某个特定目标的感知数据集合。

步骤108，根据预先设置的自适应切换策略，选择关联数据包中的其中一种数据作为被定位无人车的观测数据。

其中，观测数据可能为线性数据或者非线性数据，线性数据包括图像、激光雷达点云、导航定位数据或者IM积分定位数据等，非线性数据包括毫米波雷达点云等。

步骤110，将观测数据输入被定位无人车上的滤波器，得到编队中被定位无人车的定位结果。

当观测数据为线性数据时，滤波器可以选择卡尔曼滤波器(KF)，当观测数据为非线性数据时，滤波器可以选择无迹卡尔曼滤波器(UKF)或者扩展卡尔曼滤波器(EKF)。

在其中一个实施例中，组合惯性导航传感器数据包括：定位无人车的导航定位数据以及IMU积分定位数据。

导航定位数据中包括定位协方差δ_x，δ_y，可以通过该定位协方差判断导航定位是否受到电磁干扰，当满足条件：δ_x＞τ_{gps_x}&&δ_y＞τ_{gps_y}时，判定导航定位已经受到电磁干扰，即导航定位异常，导航定位数据不可信，需要自动切换到基于传感器检测的模型，否则可以采用车间无线通讯进行编队的多车定位。

目标感知传感器数据包括：图像、毫米波雷达点云和激光雷达点云。

在其中一个实施例中，根据目标感知传感器数据确定被定位无人车的感知数据，包括：

将图像输入YOLO网络，得到带目标框的第一图像，将激光雷达点云投影到第一图像上，提取位于第一图像中目标框中的点云，采用最近邻聚类算法分别处理目标框中的点云，得到对应目标框的聚类中心点，结合第一图像和聚类中心点得到第二图像。

对激光雷达点云极坐标化，对极坐标化点云进行地面分割得到非地面障碍区域，采用最近邻聚类算法对非地面障碍区域中的正障碍点进行过分割，获得正障碍区域，对正障碍区域进行最近邻聚类得到第一激光雷达点云。

在一个实施例中，在对导航定位数据、感知数据以及被定位无人车上滤波器对当前时间步的预测数据进行关联，得到关联数据包之前，包括：

获取上一时间步被定位无人车的感知数据和导航定位数据；

通过比较被定位无人车上一帧和当前帧导航定位数据中的时间戳判断无线通讯是否正常，以及根据定位协方差判断导航定位是否正常。

当上一帧被定位无人车导航定位数据中的时间戳早于被定位无人车当前帧导航定位数据中的时间戳，且两者在合理的时间差内，则车间无线通讯正常。

在一个实施例中，对导航定位数据、感知数据以及被定位无人车上滤波器对当前时间步的预测数据进行关联，得到关联数据包，包括：

当无线通讯且导航定位正常时：

将当前帧被定位无人车的导航定位数据与第二图像进行最优匹配，若匹配成功，则存储对应的第二图像数据；

根据第一激光雷达点云计算点云中心，将当前帧被定位无人车的导航定位数据与点云中心进行最优匹配，若匹配成功，则存储对应的第一激光雷达点云中心数据和被定位无人车对应的的点云特征；

将当前帧被定位无人车的导航定位数据与毫米波雷达点云进行最优匹配，若匹配成功，则存储对应的毫米波雷达点云数据。

当无线通讯或导航定位异常时：

根据第一激光雷达点云计算点云中心，将滤波器对当前时间步的预测数据与点云中心进行最优匹配，若匹配成功，得到被定位无人车对应的点云特征，计算被定位无人车的上一帧和当前帧的点云特征的比值，若比值小于预设阈值，则存储对应的第一激光雷达点云中心数据和被定位无人车对应的的点云特征；

将当前帧和上一帧的毫米波雷达点云进行最优匹配，若匹配成功，得到对应的被定位无人车的毫米波雷达点云，将毫米波雷达点云与滤波器对当前时间步的预测数据进行最优匹配，若匹配成功，则存储对应的毫米波雷达点云数据，若匹配失败，则将毫米波雷达点云与第一激光雷达点云进行最优匹配，若匹配成功，则存储对应的毫米波雷达点云数据；

将当前帧和上一帧的第二图像进行最优匹配，若匹配成功，得到对应的被定位无人车的第二图像，将第二图像与滤波器对当前时间步的预测数据进行最优匹配，若匹配成功，则存储对应的第二图像数据。

在一个实施例中，根据预先设置的自适应切换策略，选择关联数据中的一种作为观测数据，包括：

当无线通讯且导航定位正常时，选择导航定位数据作为被定位无人车的观测数据；

当无线通讯或导航定位异常时，自动切换到基于目标感知传感器进行无人车定位的模型，根据模型预先设置的数据优先级，并根据目标感知传感器的实际运行状态，从第二图像数据、第一激光雷达点云中心数据或者毫米波雷达数据中选择一种数据作为当前时刻的观测数据。

具体的，提供一个实施例对本方法进行详细说明，假设车队中一共有N辆车，我们假设当前程序位于单元i车，需要对其他N-1车的位置进行多传感器的融合定位，在此实施例中选择GPS进行导航定位。

1、根据目标感知传感器数据确定被定位无人车的感知数据：

a.第一图像为YOLO网络检测结果，结果表示为

u，v表示检测结果的像素坐标，w，h表示长宽，共计N-1个检测结果，对应其他N-1辆无人车，以及图像对应的接收时间为T_image，将激光雷达点云投影到第一图像上，将目标框中的所有点云提取出来，图2提供了第一图像与激光雷达点云的融合示意图。采用基于区域最近邻聚类算法，获得前景的聚类数据点

其中m为聚类数据点的簇数，计算聚类中心点：

此为将图像转换到点云三维坐标系下的最终结果，即第二图像可以表示为

b.毫米波雷达点云为极坐标系下生成的数据，每帧有64个雷达点，每个点保护对应的被定位无人车的id号和径向距离ρ、径向速度

和极坐标角度

即

以及雷达波点云接收时间T_radar。

c.在T_{t_lidar}时刻，采集到一帧包含C个点云的障碍物3维点云集合

首先将其极坐标化，基于高斯过程回归对其进行地面分割，其次，采用基于区域最近邻障碍物聚类算法对正障碍点进行过分割，获得正障碍区域，然后对正障碍区域进行最近邻聚类，获取对应的第一激光点云

T_lidar，其中K为聚类点云的簇数，m为某一簇聚类点云中的点云个数。图3提供了一个实际场景中地面分割与障碍物检测效果示意图。

2、获取组合惯性导航传感器数据中的GPS数据、IMU积分位姿和基于通讯获得被定位无人车的位姿数据：

获取本车即定位无人车的GPS或者IMU积分定位数据用于全局坐标和局部坐标的转换，通过通讯获得其他车辆的位姿数据用于编队队形的变化与控制。由于IMU积分位姿不受电磁干扰影响，我们本车对于其他车的位姿优化采用IMU坐标系，具体如下：

利用通讯系统接收被定位无人车的GPS定位数据，将被定位无人车的GPS数据中包含的时间戳与本车(定位无人车)当前时间进行对比，如果时间戳早于本车当前时间，且在合理的时间差内，说明车间的无线通讯正常，可以采用通讯获取的GPS定位数据；若时间戳晚于本车当前时间或者即使早于本车当前时间，时间差也不在合理范围内，判定通讯异常，输出通讯异常标识。当通讯异常时，不对通讯系统中被定位无人车的GPS定位数据进行关联操作。

本车的IMU积分获得的定位(x_imu，y_imu，z_imu，Heading_imu，T_imu)，本车的GPS定位数据为(x_gps，y_gps，z_gps，δ_x，δ_y，Heading_gps，T_gps)，其中δ_x，δ_y为GPS的定位协方差，用于判断GPS数据是否受干扰，当δ_x＞τ_{gps_x}&&δ_y＞τ_{gps_y}，GPS已经受到电磁干扰，需要自动切换到基于传感器检测的模型，否则可以采用通讯进行编队的多车定位，当GPS定位正常，将被定位无人车的GPS定位数据转换到本车MIU局部坐标系下，当GPS定位异常，则丢弃通讯收到的GPS数据，不予以关联。

IMU和GPS分别对应的旋转平移矩阵为

g_1，l_g代表转换顺序。

基于车间通讯获得的被定位无人车的GPS定位结果(x_{j_c}，y_{j_c}，θ_{j_c})，T_c，GPS是绝对位置，在理想情况下，本车接收其他车的GPS定位数据不会出现偏差，但是IMU会存在累计误差，而且多车间初始相对位姿误差未知，如果车间位姿通讯采用IMU积分定位，会出现不准确的问题，所以车间通讯采用GPS定位结果，而本车对于编队中其他单元车辆进行位置估计时，搜集到数据会存在通讯异常、GPS不准等问题，因此，我们对应本单元车内部间的自运动估计和特定动态车辆的跟踪采用IMU积分坐标系。

由于通讯中的其他单元车的定位结果为GPS绝对坐标系下的，将该结果转换到本车IMU的全局坐标系下才能为本车定位、规划系统所用，转换方式为：

值得说明的是，在本实施例中，以上获取的数据均为当前帧数据。

数据关联涉及到通讯获取的定位结果(x_{j_c}，y_{j_c}，θ_{j_c}，T_{j_c})与第二图像、毫米波雷达点云、第一激光雷达点云进行关联、历史数据与当前帧数据的数据关联、本车异构传感器数据之间的数据关联以及传感器数据同滤波器的预测数据之间的关联，最终输出为更新当前帧所有有效传感器关联后的结果。整体的数据关联过程如下：

1、获取上一时间步定位无人车上安装的多种传感器的数据：

a.上一帧图像检测结果中匹配的被定位无人车的id和对应的标志位，即id_{image_1}，flag_image_1；

b.上一帧毫米波雷达检测结果中的被定位无人车的id和对应的标志位，即id_{radar_1}，flag_radar_1；

c.上一帧定位无人车发布GPS定位数据的时间T_{j_c_1}；

d.滤波器在上一帧对当前帧被定位无人车的位姿预估数据。

2、当上一帧被定位无人车发布GPS定位数据的时间早于被定位无人车当前帧GPS数据中的时间，即T_{j_c_1}＜T_{j_c}，且两者在合理的时间差内，则车间通讯正常，对应的无线通讯的标志位为True，flag_c_1＝True，表示无线通讯结果可信，同时，若定位无人车当前帧GPS数据中的定位协方差均小于对应的GPS可信程度阈值参数，即满足δ_x≤τ_{gps_x}&&δ_y≤τ_{gps_y}，则GPS定位正常。

当车间无线通讯且GPS定位正常时，进行当前帧传感器数据的更新：

a.将被定位无人车当前帧的GPS数据与图像数据进行匹配，计算图像数据和GPS数据的第一最小距离min(dis((x_{j_c}，y_{j_c})，(p_{ix_image}，p_{iy_image})))，若第一最小距离小于第一距离阈值τ_image，存储图像数据p_x，p_y＝p_{iX_image}，p_{iy_image}及对应被定位无人车的id，以供下一帧计算使用，否则flag_image_1＝false，id_{image_1}＝-1；

b.计算被定位无人车对应的第一激光雷达点云的点云中心：

将被定位无人车的当前帧GPS数据与激光雷达点云中心进行匹配，计算两者的第二最小距离

若第二最小距离小于第二距离阈值τ_lidar，则存储激光雷达点云数据x，y＝x_i，y_i及对应被定位无人车的id，同时更新对应的被定位无人车的被定位无人车对应的的点云特征：点云数、点云长和点云宽N_{lidar_1}，W_{lidar_1}，H_{lidar_1}，否则flag_lidar_1＝false。

c.将被定位无人车的当前帧GPS数据与毫米波雷达数据进行匹配，计算对应的第三最小距离

若第三最小距离小于第三距离阈值τ_radar，则存储毫米波雷达点云数据

否则flag_radar_1＝false，id_{radar_1}＝-1。

当车间通讯或GPS定位异常时，切换到基于传感器定位的模型：

a.将滤波器的预估数据与对应的激光点云中心进行匹配，计算两者的第四最小距离

若第四最小距离小于第二距离阈值τ_lidar，则存储激光雷达点云数据

及对应被定位无人车的id，更新对应的被定位无人车的点云数、点云长和点云宽N_{lidar_1}，W_{lidar_1}，H_{lidar_1}，选择最优匹配点云子集对应的min_id的点云特征，与当前帧的点云特征进行进一步比较，若满足：

abs(N_{lidar_1}/N_lidar[min_id])＜ξ_N

&abs(H_{lidar_1}/H_lidar[min_id])＜ξH

&abs(W_{lidar_1}/W_lidar[min_id])＜ξw

则flag_lidar_1＝True，否则flag_lidar_1＝False。

其中，ζ_N，ζ_H，ζ_W分别为Lidar点云检测结果前后帧之间匹配时点云数、点云长和点云宽的比例阈值参数，则flag_lidar_1＝True，否则flag_lidar_1＝False

b.若上一帧毫米波雷达检测结果中满足flag_radar_1＝True&&id_{radar_1}≥0，基于上一帧的毫米波雷达检测结果与当前帧的毫米波雷达检测结果进行匹配，选择最优匹配结果对应的min_id，将滤波器的预估数据与对应的毫米波雷达检测结果进行匹配，计算两者的第五最小距离

若第五最小距离小于第三距离阈值τ_radar，则存储激光雷达点云数据

及对应被定位无人车的id，flag_radar_1＝True，否则flag_radar_1＝false，id_{radar_1}＝-1；

若上一帧毫米波雷达检测结果不满足flag_radar_1＝True&&id_{radar_1}≥0，则基于上一帧激光雷达点云数据与毫米波点云进行匹配，计算上一帧激光雷达点云数据与当前帧毫米波雷达数据的第六最小距离

若第六最小距离小于第三距离阈值τ_radar，则存储毫米波雷达点云数据

否则flag_radar_1＝false，id_{radar_1}＝-1。

c.若上一帧图像检测结果中flag_image_1＝True&&id_{image_1}≥0，基于上一帧的图像数据与当前帧的图像帧进行匹配，选择最优匹配结果对应的min_id，选择最优匹配结果对应的min_id，将滤波器的预估数据与对应的毫米波雷达检测结果进行匹配，计算两者的第七最小距离min_dis＝min(dis((p_x，p_y)，(p_{ix_image}，p_{iy_image})))，若第七最小距离小于第一距离阈值τ_image，则存储激光雷达点云数据

若上一帧图像检测结果不满足flag_image_1＝True&&id_{image_1}≥0，则将滤波器的预估数与对应的图像检测结果进行匹配，计算两者的第八最小距离

若第八最小距离小于第一距离阈值τ_image，存储图像数据p_x，p_y＝p_{ix_image}，p_{iy_image}及对应被定位无人车的id，以供下一帧计算使用，否则flag_image_1＝false，id_{image_1}＝-1。

数据关联流程如表1所示：

表1数据关联算法流程

3、根据预先设置的自适应切换策略，选择关联数据包中的其中一种数据作为被定位无人车的观测数据。

获取通过数据关联处理输出的数据，如表2所示

若flag_c_1＝True，将通过无线通讯接收到的被定位无人车的GPS数据作为对应被定位无人车当前帧的观测数据，即(p_x，p_y)^T＝(x_{j_c}，y_{j_c})^T，对应的时间差Δt＝T_{j_c_1_}T_{p_1}，其中T_{p_1}为上一帧EKF的更新时间；

若flag_c_1＝False&flag_radar_1＝True，将被定位无人车的毫米波雷达数据作为当前帧的观测数据，即

对应的时间差T_{radar_1}-T_{p_1}；

若flag_c_1＝False&flag_radar_1＝False&flag_image_1＝True，将被定位无人车的图像数据作为当前帧的观测数据，即

对应的时间差Δt＝T_{image_1_}T_{p_1}；

若flag_c_1＝False&flag_radar_1＝False&flag_image_1＝false&flag_lidar_1＝True，将被定位无人车的激光雷达点云数据作为当前帧的观测数据，即

对应的时间差Δt＝T_{Lidar_1}_T_{p_1}；

若flag_c_1＝False&flag_radar_1＝False&flag_image_1＝false&flag_lidar_1＝false，将滤波器估的预估数据作为当前帧的观测数据，即

对应的时间差Δt＝T_current_T_{p_1}；

如表3所示，提供一个实施例中根据预先设置的自适应切换策略，选择关联数据包中的其中一种数据作为被定位无人车的观测数据的数据自适应切换算法流程。

对于本单元车而言，各种传感器的触发时间的周期并不固定，因此滤波器在进行噪声估计和速度预测的时候，不能采用固定的时间步来进行更新，而需要采用变时间步的操作，时间出现异常，会导致预测尤其是速度预测，变得异常不稳定。对于由通讯传输中接收的其他车辆的定位结果，一般情况下，采用GPS时间来对每辆车的时钟进行标定，这样所有车的时间坐标系都统一在GPS坐标轴下。通常，一辆车接收的其他车辆的定位时刻不应该比本车此刻的时钟要早，但是这种情况时有发生，一般由于GPS同步受GPS接收系统限制，同时，每个单元车辆中包含多个工控机，多种因素造成多车间时间坐标系不固定，这会造成滤波器更新的时间步长出现计算不准确甚至存在无效值的问题。时间坐标系不一致最终会导致的滤波器导致的预测矩阵、噪声协方差矩阵更新异常问题。

本方法在数据关联和数据选择时通过本车时间和通讯结果接收时间进行对比来解决时间坐标不固定问题的，具体而言，时间坐标不固定问题仅出现在当数据关联时选择采用其他车辆的通讯获得的GNSS定位作为滤波器更新用的观测数据的情况，产生原因是其他车辆时间坐标原点和本车时间坐标原点未统一。

表3数据自适应切换算法流程

为了解决这个问题，将其他车辆的定位结果包含的时间戳和本车时间戳进行对比，如果时间差为正，则证明该通讯获得的GNSS结果是可信的。本车传感器数据的时间差Δt，采用本车时间坐标轴的时刻进行计算。当通讯GPS的时间与本车传感器时间交换情况下，采用仅预测一步的策略，来防止时间不一致问题的发生。

4、将观测数据输入被定位无人车上的滤波器，得到编队中被定位无人车的定位结果。在此提供一个实施例中将观测数据输入被定位无人车上的滤波器，得到编队中被定位无人车的定位结果的具体过程：

任意被定位无人车上的滤波器，包含一个状态预测方程和两个观测方程(线性和非线性)。

1、针对状态预测方程，采用线性状态预测方程：

x^·＝Fx+v

P^·＝FPF^T+Q

也即：

转换为矩阵形式

其中输出向量和输入向量分别为

转移矩阵

随机噪声矩阵v～N(0，Q)

设加速度服从分布

则预测噪声矩阵为

综上，状态预测方程、噪声协方差预测方程分别为

x^·＝Fx+v

P^·＝FPF^T+Q

2、状态观测方程，主要包括两个，一个是面向二维平面直角坐标系的线性观测方程和面向极坐标系的非线性观测方程，对于非线性方程，采用一阶近似将非线性方程转换为线性方程形式，具体如下：

y＝z-Hx^·-ω

S＝HP`H^T+R

K＝P`H^TS^-1

x＝x^·+Ky

P＝(I-KH)P`

2.1针对二维平面直角坐标系数据的观测方程：

观测噪声为二维矢量，p_x，p_y都会受到随机噪声的影响，所以

所以

S＝cov(z-Hx`)

＝cov(Hx+ω-Hx`)

＝cov(H(x-x`))+cov(ω)

＝HP`H^T+R

2.2针对基于极坐标系的数据的观测方程：

Radar的观测的数据为

位于极坐标系下。而上述预测方程输出的状态数据

位于以IMU全局坐标系下，因此，需要将该数据转换到本车的局部坐标系下：

图4提供了毫米波雷达数据在极坐标与直角坐标系之间的数据关系图，由图4可得，Radar数据的观测方程为一个非线性方程：

我们通过一阶泰勒展开对其进行线性化处理：

用海塞矩阵H作为Radar的观测矩阵。因此，Radar数据的观测偏差为：

最终，增益更新，以及位置输出为：

K＝P`H^TS^-1

x＝x^·+Ky

P＝(I-KH)P`

将局部下状态更新转换到IMU坐标系的全局下，作为状态的最终输出

为：

如图5所示，提供了一种基于多传感器融合的多车定位系统框图。

在一个实施例中，如图6所示，提供了一种基于多传感器融合的多车定位装置，装置包括：数据获取模块、数据确定模块、数据关联模块、数据选择模块以及结果输出模块，其中：

数据获取模块，用于获取编队中定位无人车上安装的多个传感器的传感器数据。

数据确定模块，用于根据目标感知传感器数据确定被定位无人车的感知数据，通过无线通讯获取被定位无人车的导航定位数据。

数据关联模块，对导航定位数据、感知数据以及被定位无人车上滤波器对当前时间步的预测数据进行关联，得到关联数据包。

数据选择模块，用于根据预先设置的自适应切换策略，选择关联数据包中的其中一种数据作为被定位无人车的观测数据。

结果输出模块，用于将观测数据输入被定位无人车上的滤波器，得到编队中被定位无人车的定位结果。

在其中一个实施例中，数据确定模块还用于将图像输入YOLO网络，得到带目标框的第一图像，将激光雷达点云投影到第一图像上，提取位于第一图像中目标框中的点云，采用最近邻聚类算法分别处理目标框中的点云，得到对应目标框的聚类中心点，结合第一图像和聚类中心点得到第二图像。

在其中一个实施例中，数据关联模块还用于当无线通讯且导航定位正常时：

当无线通讯或导航定位异常时：根据第一激光雷达点云计算点云中心，将滤波器对当前时间步的预测数据与点云中心进行最优匹配，若匹配成功，得到被定位无人车对应的点云特征，计算被定位无人车的上一帧和当前帧的点云特征的比值，若比值小于预设阈值，则存储对应的第一激光雷达点云中心数据和被定位无人车对应的的点云特征；

在其中一个实施例中，数据选择模块还用于当无线通讯且导航定位正常时，选择导航定位数据作为被定位无人车的观测数据，当无线通讯或导航定位异常时，自动切换到基于目标感知传感器进行无人车定位的模型，根据模型预先设置的数据优先级，并根据目标感知传感器的实际运行状态，从第二图像数据、第一激光雷达点云中心数据或者毫米波雷达数据中选择一种数据作为当前时刻的观测数据。

关于基于多传感器融合的多车定位装置的具体限定可以参见上文中对于基于多传感器融合的多车定位方法的限定，在此不再赘述。上述基于多传感器融合的多车定位装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。

在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端，其内部结构图可以如图7所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口、显示屏和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种基于多传感器融合的多车定位方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是多个传感器等。本领域技术人员可以理解，图7所示的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。

在一个实施例中，提供了一种计算机设备，包括存储器和处理器，该存储器存储有计算机程序，该处理器执行计算机程序时实现上述实施例中方法的步骤。

在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述实施例中方法的步骤。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synch link)DRAM(SLDRAM)、存储器总线(Ram bus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。

以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims

1.一种基于多传感器融合的多车定位方法，其特征在于，所述方法包括：

根据所述目标感知传感器数据确定被定位无人车的感知数据，通过无线通讯获取被定位无人车的导航定位数据；

2.根据权利要求1所述的方法，其特征在于，所述组合惯性导航传感器数据包括：所述定位无人车的导航定位数据和IMU积分定位数据；所述定位无人车的导航定位数据中包括定位协方差；

所述目标感知传感器数据包括图像、毫米波雷达点云和激光雷达点云。

3.根据权利要求2所述的方法，其特征在于，所述根据目标感知传感器数据确定被定位无人车的感知数据，包括：

将所述图像输入YOLO网络，得到带目标框的第一图像，将所述激光雷达点云投影到所述第一图像上，提取位于所述第一图像中目标框中的点云，采用最近邻聚类算法分别处理所述目标框中的点云，得到对应目标框的聚类中心点，结合所述第一图像和所述聚类中心点得到第二图像；

4.根据权利要求3所述的方法，其特征在于，在对所述导航定位数据、所述感知数据以及被定位无人车上滤波器对当前时间步的预测数据进行关联，得到关联数据包之前，包括：

获取上一时间步被定位无人车的感知数据和导航定位数据；

5.根据权利要求4所述的方法，其特征在于，对所述导航定位数据、所述感知数据以及被定位无人车上滤波器对当前时间步的预测数据进行关联，得到关联数据包，包括：

当无线通讯且导航定位正常时：

6.根据权利要求4所述的方法，其特征在于，对所述导航定位数据、所述感知数据以及被定位无人车上滤波器对当前时间步的预测数据进行关联，得到关联数据包，还包括：

当无线通讯或导航定位异常时：

7.根据权利要求1至6任一项所述的方法，其特征在于，根据预先设置的自适应切换策略，选择所述关联数据中的一种作为观测数据，包括：

8.一种基于多传感器融合的多车定位装置，其特征在于，所述装置包括：

9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述方法的步骤。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法的步骤。