CN113155123B

CN113155123B - 基于数据共享的多智能车辆协同定位追踪方法及装置

Info

Publication number: CN113155123B
Application number: CN202110364972.9A
Authority: CN
Inventors: 程翔; 李思江
Original assignee: Peking University
Current assignee: Peking University
Priority date: 2021-04-01
Filing date: 2021-04-01
Publication date: 2022-09-27
Anticipated expiration: 2041-04-01
Also published as: CN113155123A; WO2022205942A1

Abstract

本发明公布了一种基于数据共享的多智能车辆协同定位追踪方法及装置，是一种应用于智能车辆的基于数据共享的协同定位追踪技术，通过车辆上安装的多种传感器得到智能车辆自身状态，并引入环境中设备的辅助，再获得与周边车辆的相对状态，且将自身状态数据共享；本发明在环境中加入固定点位的感知设备为车辆提供定位数据，融合自身数据、共享数据、观测数据与环境辅助数据，实现可靠的多智能车辆的协同定位追踪。本发明尤其适用于单车辆难以精确自定位的情况，通过多智能车辆协同确保动态追踪的精度和鲁棒性，并且技术灵活可扩展，装置便于安装部署。

Description

基于数据共享的多智能车辆协同定位追踪方法及装置

技术领域

本发明属于无线通信定位技术领域，具体涉及一种基于数据共享的多智能车辆协同定位追踪方法及装置，是一种应用于智能车辆的基于数据共享的协同定位追踪技术，通过融合车辆上安装的多种传感器的信息，并引入环境中设备的辅助，实现可靠的多智能车辆协同定位追踪。

背景技术

随着信息技术的迅速发展，用智能化的机器代替人工成为了当下的重要方向，智能车辆作为常见的智能单位，在众多任务场景中均有应用，包括在交通、运输中应用的无人车，和在工业、物流中应用的自动导引运输车(Automated Guided Vehicle，AGV)等，而无论应用场景如何，为实现智能化执行目标任务，获得精确的定位和轨迹都是不可或缺的基础。

对于智能交通系统中的无人车，需要在相对复杂多变的开放道路场景中以中、高速运行，在定位方面，最为成熟且被广泛使用的方式为GPS定位，但由于传统GPS在精度上仅能提供米级定位，仅可以在车道层面提供辅助信息，无法支撑精确的主动控制，而在GPS基础上改进的差分GPS，通过地面站的辅助，可以在理想情况下达到厘米级精度，但建立在开阔场景，地面站和卫星信号良好的前提下，在多遮挡的城市道路，或者隧道、地下场景，定位精度会下降到米级甚至丢失信号，说明了GPS及其衍生方案在环境鲁棒性，可靠性方面难以单独支撑无人车定位。

对于智慧物流应用中的AGV，多为中、低速工程车辆，在仓储分拣中多为室内小型，在码头货运中多为户外中大型，传统的AGV由于任务固定，智能化程度低，多采用磁条、色带、二维码、电磁导轨等方式，提供固定轨迹的高精度定位，以满足精度需求，同时也存在着维护成本高，灵活性不足的问题，随着智能化需求的逐步提高，AGV需要面临开放环境下人机协同的无固定轨迹工作，原有的定位方式无法支撑，同时由于室内场景的存在，也不能采用GPS方案。

综上，传统的定位方式难以适应智能车辆日益增长的智能化需求，因此无依托定位的思路应运而生，通过智能车辆上安装的激光和视觉传感器，实现无固定轨迹的同步定位与建图(Simultaneous Localization and Mapping，SLAM)，包括激光SLAM，视觉SLAM，通过激光雷达点云或视觉图像的分析匹配，可以无需依托外部信号源，为车辆提供无轨迹的灵活定位，但是同时也存在精度不高，高精度地图计算复杂度大，对于黑暗和能见度低场景的鲁棒性不足的问题，仅通过单一方式获得高精度的定位追踪是难以通过低成本方式实现的。

因此，智能车辆定位追踪解决方案逐渐向多传感器融合发展，通过滤波算法，神经网络等手段，达到传感器优势互补，提供相对精确可靠的定位追踪效果，但在车辆上加装多种传感器，意味着在复杂度和成本上做出了妥协，并且当车辆处于不利环境中时，设备性能难以保障，因此伴随着通信网络技术的发展迭代，由万物互联思想引出的多智能体协同成为解决定位追踪问题的新思路，通过引入多智能车辆协同的新维度，可以在传感器性能一定的前提下进一步提高精度和鲁棒性，进一步提高车辆定位追踪性能。例如，公告号为CN111198567 A的发明专利以灵活可靠低成本为出发点，利用惯导、雷达等多传感器和通信模块，实现多AGV协同动态追踪，但是，考虑到AGV上计算设备与通信设备能力受体积与功率限制，上述方法难以满足较低计算复杂度和低通信负担的需求，很难更好的适应AGV平台，更好地实现安装部署，同时也难以兼顾环境更为复杂，协作对象数量不定的无人车场景。

发明内容

本发明提出了一种基于数据共享的多智能车辆协同定位追踪技术，通过融合车辆上安装的多种传感器的信息，并引入环境中设备辅助，实现可靠的多智能车辆协同定位追踪。

本发明中，多智能车辆协同定位追踪装置(系统)内的智能车辆通过包括无线定位和惯导的自身定位模块得到自身状态，通过雷达、摄像头等感知探测模块获得与周边车辆的相对状态，通过通信模块将自身状态数据共享，并在环境中加入固定点位的感知设备为车辆提供定位数据，融合自身数据、共享数据、观测数据与环境辅助数据，可以实现多车的协同定位追踪，提高定位追踪的精度和鲁棒性。该协同定位追踪装置适用于各种传感器的配置，可以应用于包括无人车，AGV等不同类型的智能车辆之上，进一步提升定位追踪的精度与可靠性。

为达到上述目的，本发明提供一种多智能车辆协同定位追踪方法及装置，多智能车辆协同定位追踪装置设置在智能车辆内部与周边环境中，共同完成多车协同定位追踪。具体实施时，该多智能车辆协同定位追踪装置包含一个自定位模块，一个感知探测模块，一个无线传输模块，一个延迟补偿模块，一个环境辅助模块，一个协同动态追踪模块；自定位模块包括无线定位模块、惯导模块；基于数据共享的多智能车辆协同定位动态追踪方法包括如下步骤：

1)全体智能车辆由安装在之上的自定位模块(惯导模块、无线定位模块(GPS、UWB、WIFI、可见光等))以一定周期连续获得车辆自身的自定位信息，包括作为自身状态的自定位估计值的车辆位置、速度、加速度信息，以及自定位方差，组成数据包。

2)全体车辆由无线传输模块发送数据包，数据包内包括步骤1)中得到的自定位估计值和自定位方差，时间戳，识别编号，同时接收可通信范围内车辆的数据包，实现自定位信息的共享。

3)通过感知探测模块(激光雷达、摄像头等)，观测周边车辆与自身的相对位置和速度，作为相对状态的估计值，并给出观测方差，将相对状态与步骤2)中接收可通信范围内车辆的数据包信息进行匹配相减，得到通过周边车辆推算出的自身状态估计和方差。

4)包括环境中安装的固定路测设备(Road Side Unit，RSU)与车辆上搭载的车载单元(OBU)的环境辅助模块，可以给环境中车辆提供更加精确的辅助定位，并将定位的结果与方差加上时间戳打包传输给车辆。

5)考虑通信的时延，延迟补偿模块利用惯导模块中的加速度数据对步骤3)、4)中的定位结果进行时延修正，与步骤1)实现不同源数据间的时间对齐，得到修正后的结果和方差。

6)协同动态追踪模块将步骤5)中对齐后的来自自身、周边车辆和环境辅助设备的定位数据，利用动态滤波算法进行融合，得到全局优化的定位估计值；

通过上述步骤，实现多车辆协同动态追踪。

具体实施时，本发明提供的多智能车辆协同定位追踪装置包括在车辆内部设置的模块和在车辆环境中部署的模块；其中，在车辆内部设置的模块有自定位模块，感知探测模块，无线传输模块，延迟补偿模块，协同定位追踪模块；在环境中部署的模块包括环境辅助模块；

自定位模块包括无线定位模块、惯导模块，用于以一定周期连续获得车辆自身的自定位信息；其中无线定位模块包括GPS、UWB、WIFI、可见光等；

感知探测模块包括激光雷达、摄像头等；用于观测周边车辆与自身的相对位置和速度，作为相对状态的估计值，并给出观测方差；并将相对状态与接收到的可通信范围内车辆的数据包信息进行匹配，得到通过周边车辆推算出的自身状态估计和方差；

无线传输模块用于发送数据包，同时接收可通信范围内车辆的数据包，实现自定位信息的共享；

延迟补偿模块用于利用加速度数据对定位结果进行时延修正，实现不同源数据间的时间对齐，得到修正后的结果和方差；

协同动态追踪模块用于将对齐后的来自自身、周边车辆和环境辅助设备的定位数据，利用动态滤波算法进行融合，得到全局优化的定位估计值；

环境辅助模块包括固定路测设备(RSU)与车辆上搭载的车载单元(OBU)；用于给环境中车辆提供更加精确的辅助定位，并将定位的结果与方差加上时间戳打包传输给车辆。

与现有技术相比，本发明的有益效果是：本发明提供的基于数据共享的多车辆协同定位追踪方案，在不影响单车定位和功能的基础上，充分利用多种定位设备和感知设备的功能，以及来自周边车辆和环境设备的辅助，在复杂开放的环境下，尤其是单车难以精确自定位的情况下，通过协同来确保动态追踪的精度和鲁棒性，并且灵活可扩展，便于安装部署。

本发明提供的多车协同动态追踪方法具有以下优点：

(一)多机协同方面：充分利用多车上多种传感器的数据对位置信息进行融合优化，在保证单车设备低成本的前提下，提高在各种任务和环境下可靠灵活的定位追踪，实现多机协同的基本目的。

(二)算法处理方面：算法包括多源信息融合与动态滤波，在保证时间和空间低复杂度的前提下，考虑到工作场景可能较大和环境中的不稳定因素，周边车辆可灵活地加入和退出协作，而不影响算法的正常运行，在极端情况下，也可仅依靠惯单机定位和惯导数据保证单体精度。

(三)通信负担方面：各车辆采用实时广播汇报更新自身状态并接收周边车辆状态的信息共享方式，相比所有车辆间建立点对点直连链路传输观测数据，此方案由于仅需发送自身状态，单车通信负担降低，并且在具体实现上有更灵活的方案选择，适用于不同环境，不同成本的车辆。

(四)环境辅助方面：通过在环境中引入若干位置固定，性能相对车载单元较强的RSU，为车辆提供更为精确稳定的定位数据，分担了车辆上的成本和传感器精度，改善协同定位追踪性能，使车辆从单方面应对环境到环境协助下工作。

附图说明

图1是本发明具体实施提供的多智能车辆协同定位追踪装置的结构框图。

图2是本发明具体实施时感知探测模块的工作流程框图。

图3是本发明具体实施时延迟补偿模块的工作流程框图。

图4是本发明具体实施时协同定位追踪算法的流程框图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

本发明整体架构如图1所示，环境中每个车辆均配备有自定位设备和感知设备，从而获得自身定位和对周边车辆相对观测，通过无线传输实现车辆自定位信息的汇报共享，进而利用其他车辆的位置和观测数据对自定位进行优化，同时，在环境中也安装有位置固定具备定位感知能力的辅助设备，可以向车辆发送定位信息，进一步提高车辆的定位性能，共同组成协同定位追踪系统，适用于存在遮挡的物流场景，通过协同提高系统中所有车辆的定位精度与鲁棒性，确保应用需求特以满足。

本发明在车辆内部设置的模块有自定位模块，感知探测模块，无线传输模块，延迟补偿模块，协同定位追踪模块，环境中部署有环境辅助模块，具体工作步骤如下：

S10：参考图1中所示，每台AGV上由无线定位模块和惯导模块组成的自定位模块，以一定的时间周期连续地获得自身状态的估计，并且给出估计的方差，作为车辆基础定位，等待后续优化。

S20：全体车辆由图1中车辆搭载的无线传输模块发送数据包，包内包括(1)中得到的自定位估计值和方差，时间戳，识别编号，同时接收可通信范围内车辆的数据包，实现自定位信息的共享

S30：车辆通过如图2所示的感知探测模块，观测周边车辆与自身的相对位置和速度，作为相对状态的估计值，并给出观测方差，将相对状态与S20中接收的信息进行匹配相减，得到通过周边AGV推算出的自身状态估计和方差。

S40：环境中安装的固定路测设备(RSU)由于位置固定，对体积和功耗限制更小，可以对其覆盖范围内的车辆进行高精度定位，并且将定位结果发送给对应车辆，优化车辆协同系统的性能。

S50：车辆通过图3所示的延迟补偿模块，对S10，S30，S40得到的不同来源的自身状态估计值进行时间上的对齐，补偿通信带来的时延，以备优化算法使用。

S60：通过S50得到的更新后的各部分估计值，可以利用流程如图4所示的动态滤波算法进行融合，得到全局优化的定位估计值。

在步骤S10：需要得到车辆的二维位置、速度和加速度信息，并给出每个维度上独立的方差，该步骤包含以下流程S11～S12：

S11：无线定位模块(GPS、WIFI，UWB，可见光等)可根据信号传播时间，强度，到达角度等，根据信号源与接收机的相对位置解算出车辆的状态，可以得到车辆的二维位置、速度信息，表示为状态向量

和方差P_s，以及状态

对应的时间t_s；

S12：惯导模块的加速度计可以几乎不受环境影响地测得车辆的二维加速度信息，表示为控制向量u；

在步骤S20：车辆需要通过蓝牙、WIFI、4/5G等无线通信方式，发送自身位置信息，该步骤包含以下流程S21～S22：

S21：车辆将自定位模块得到的状态向量

方差P_s和时间t_s打包，以与自定位相同的周期向周边车辆进行一对多广播，汇报自身状态，包内包含车辆的编号i；

S22：收取周边车辆发送的数据包，记录接收时间t_r,得到周边车辆在发送时的状态

和P_i

在步骤S30：车辆需要通过感知设备(激光雷达、摄像头等)，对周边车辆的状态进行观测，并且结合S20中接收到的信息得到对自己状态的估计值，该步骤包含以下流程S31～S33：

S31：车辆借助安装的传感器，通过激光点云分析，视觉图像处理等方式对环境进行感知探测，可以得到本AGV与相邻车辆之间的二维相对位置和相对速度，表示为相对状态向量

以及观测方差P_s→i

S32：将观测智能车辆自身与到周边i号智能车辆的相对状态向量

与接收到的i号智能车辆的数据包内的自定位状态向量

进行匹配相减，可以得到与i号车协作获得的自身状态估计

并根据数据包中时间t_s和计算耗时，得到状态

对应的时间t_si

S33：同时可以计算出这一估计的方差P_si＝P_i+P_s→i，方差P_si包含i号车的自定位方差P_i和观测方差P_s→i两部分；

在步骤S40：环境中的RSU为周边的车辆提供辅助定位，发送给车辆优化其定位性能，该步骤包含以下流程S41～S42：

S41：RSU可以通过激光雷达、摄像头等感知设备对周边环境进行观测，也可通过与车辆上OBU间的无线测距等手段，利用多点定位的方式，得到周边环境中车辆的位置，速度信息

S42：RSU将得到的车辆的位置、速度信息，以状态向量

方差P_r和状态

对应时间t_r的形式，发送给车辆，等待后续对其自定位进行优化

在步骤S50：由于通信时延，在进行数据融合前，通过周边车辆得到的状态估计

通过环境中RSU得到状态估计

通过自定位模块得到的状态估计

与当前时刻车辆的状态之间存在时间差，车辆可以通过时延补偿模块，对时间差进行补偿，该步骤包含以下流程S51～S53：

S51：通过当前时间t₀与状态估计

对应的时间t_si,t_r,t_s相减，可以得出时间差τ_si＝t_si-t₀,τ_r＝t_r-t₀,τ_s＝t_s-t₀

S52：通过惯导模块的加速度计，车辆可以得到任意时间t时自身的加速度信息u_t

S53：根据加速度u_t和运动学定律,可以对车辆在时间差τ_si,τ_r,τ_s中位置、速度的变化进行计算，将计算结果加在状态估计

上作为时延补偿，得到的结果均与当前时间t₀对应，不会因为时延过程中车辆的运动引入误差。

在步骤S60：车辆通过协同动态追踪算法，将不同来源的信息整合优化，得到全局最优的状态估计，该步骤包含S61～S63：

S61：将时延补偿模块补偿后的状态估计向量

以及状态估计对应的方差矩阵P_s,P_si,P_r进行拼接整合，得到当前时间t₀时刻车辆的状态估计

和方差矩阵

作为滤波算法的输入数据；

S62：利用S61中整合的状态估计数据和惯导模块测得的加速度信息u，作为动态滤波算法的输入，算法可选择包括卡尔曼滤波、扩展卡尔曼滤波、粒子滤波在内的常见算法，根据所选算法的执行方法利用输入数据对车辆的运动轨迹进行滤波处理，并以优化后得到的全局定位追踪结果

作为输出，全局定位追踪结果包含车辆二维位置，二维速度，其最优性体现在位置和速度的方差小于S61中输入数据，精度得到进一步提升；

S63：由于车辆处于持续运动状态，滤波算法需要持续运行来连续输出车辆的全局状态估计，在系统循环执行步骤S1-S6的过程中，协同动态追踪模块会存储S62中计算得到的全局最优结果，形成车辆运行的轨迹。

需要注意的是，公布实施例的目的在于帮助进一步理解本发明，但是本领域的技术人员可以理解：在不脱离本发明及所附权利要求的范围内，各种替换和修改都是可能的。因此，本发明不应局限于实施例所公开的内容，本发明要求保护的范围以权利要求书界定的范围为准。

Claims

1.一种基于数据共享的多智能车辆协同定位追踪方法，通过得到智能车辆自身状态，再获得与周边车辆的相对状态，并将自身状态数据共享；在环境中加入固定点位的感知设备为车辆提供定位数据，融合自身数据、共享数据、观测数据与环境辅助数据，实现多智能车辆的协同定位追踪；包括如下步骤：

1)以一定周期连续获得智能车辆自身的自定位信息，包括作为自身状态的自定位估计值的车辆位置、速度、加速度信息，以及自定位方差，组成数据包；数据包内包括得到的自定位估计值、自定位方差、时间戳、识别编号；

以一定周期连续获得智能车辆自身的自定位信息，包括车辆的二维位置、速度和加速度信息，并给出每个维度上独立的方差；包含以下流程S11～S12：

S11：通过无线定位模块，根据信号的传播特征，根据信号源与接收机的相对位置解算出车辆的状态，得到车辆的二维位置、速度信息，表示为状态向量

和方差Ps，以及状态

对应的时间t_s；

S12：通过惯导模块的加速度计测得车辆的二维加速度信息，表示为控制向量u；

2)智能车辆自身发送数据包，同时接收可通信范围内智能车辆的数据包，实现自定位信息的共享；

智能车辆具体通过无线通信方式发送和接收位置信息，包含以下流程S21～S22：

S21：智能车辆将得到的状态向量

S22：收取周边车辆发送的数据包，记录接收时间t_r，得到周边车辆在发送时的状态

和P_i；

3)智能车辆自身通过观测得到周边智能车辆与智能车辆自身的相对位置和速度，作为相对状态的估计值，并给出观测方差；将相对状态与步骤2)中接收可通信范围内车辆的数据包信息进行匹配，得到通过周边车辆推算出的自身状态估计和方差；

具体包含以下流程S31～S33：

S31：智能车辆借助安装的传感器，通过对环境进行感知探测，得到智能车辆自身与相邻智能车辆之间的二维相对位置和相对速度，表示为相对状态向量

以及观测方差P_s→i；

与接收到的i号智能车辆的数据包内的自定位状态向量

进行匹配相减，得到与i号车协作获得的自身状态估计

并根据数据包中时间t_s和计算耗时，得到状态

对应的时间t_si；

S33：计算得到自身状态估计的方差P_si：P_si＝P_i+P_s→i；方差包含i号车的自定位方差P_i和观测方差P_s→i两部分；

4)为环境中的智能车辆提供辅助定位，并将辅助定位的结果与方差加上时间戳打包传输给智能车辆自身；辅助定位包含以下流程S41～S42：

S41：通过感知设备对周边环境进行观测，或通过利用多点定位的方式，得到环境中智能车辆的位置和速度信息；

S42：将得到的车辆的位置、速度信息，以状态向量

方差P_r和状态

对应时间t_r的形式发送给智能车辆自身，后续再进行优化；

5)智能车辆自身利用加速度数据对步骤3)、4)中的定位结果进行时延修正，对时间差进行补偿，与步骤1)实现不同源数据间的时间对齐，得到修正后的结果和方差；对时间差进行补偿具体包含以下流程S51～S53：

S51：通过当前时间t₀与状态估计

对应的时间t_si,t_r,t_s相减，得出时间差τ_si＝t_si-t₀,τ_r＝t_r-t₀,τ_s＝t_s-t₀；

S52：通过惯导模块的加速度计，得到车辆任意时间t时自身的加速度信息u_t；

S53：根据加速度u_t和运动学定律，对车辆在时间差τ_si,τ_r,τ_s中位置、速度的变化进行计算，将计算结果加在状态估计

上作为时延补偿，得到的结果均与当前时间t₀对应，不会引入时延过程中车辆的运动误差；

6)将步骤5)中对齐后的来自自身、周边车辆和环境辅助设备的定位数据，利用动态滤波算法进行融合，得到全局优化的定位估计值；具体包含流程S61～S63：

S61：将自我状态估计

与时延补偿过的

数据，以及状态估计对应的方差矩阵P_s,P_si,P_r进行拼接整合，得到当前时间t₀时刻车辆的状态估计和方差矩阵，作为动态滤波算法的输入数据；

S62：利用动态滤波算法，将S61中整合的状态估计数据和惯导模块测得的加速度信息u作为动态滤波算法的输入，对车辆的轨迹进行滤波处理，以优化后得到的全局定位追踪结果

作为输出，得到全局最优的定位追踪结果，全局定位追踪结果包含车辆二维位置和二维速度；

S63：根据全局最优的定位追踪结果，在时间上进行迭代，根据S62中计算得到的全局最优结果，输出车辆的优化状态估计，形成车辆运行的轨迹；

通过上述步骤，实现多车辆协同动态追踪。

2.如权利要求1所述基于数据共享的多智能车辆协同定位追踪方法，其特征是，步骤S31具体通过激光点云分析和/或视觉图像处理方式对环境进行感知探测。

3.一种实现权利要求1所述基于数据共享的多智能车辆协同定位追踪方法的多智能车辆协同定位追踪装置，包括在车辆内部设置的模块和在车辆环境中部署的模块；其中，在车辆内部设置的模块包括自定位模块，感知探测模块，无线传输模块，延迟补偿模块，协同定位追踪模块；在环境中部署的模块包括环境辅助模块；

自定位模块包括无线定位模块、惯导模块，用于以一定周期连续获得车辆自身的自定位信息；其中无线定位模块包括GPS、UWB、WIFI、可见光；

感知探测模块包括激光雷达、摄像头；用于观测周边车辆与自身的相对位置和速度，作为相对状态的估计值，并给出观测方差；并将相对状态与接收到的可通信范围内车辆的数据包信息进行匹配，得到通过周边车辆推算出的自身状态估计和方差；

环境辅助模块包括固定路测设备RSU与车辆上搭载的车载单元OBU，用于给环境中车辆提供辅助定位，并将定位的结果与方差加上时间戳打包传输给智能车辆自身。