CN114877883B - 车路协同环境下考虑通信延迟的车辆定位方法及系统 - Google Patents

车路协同环境下考虑通信延迟的车辆定位方法及系统 Download PDF

Info

Publication number
CN114877883B
CN114877883B CN202210286541.XA CN202210286541A CN114877883B CN 114877883 B CN114877883 B CN 114877883B CN 202210286541 A CN202210286541 A CN 202210286541A CN 114877883 B CN114877883 B CN 114877883B
Authority
CN
China
Prior art keywords
vehicle
point cloud
target
road
lidar
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202210286541.XA
Other languages
English (en)
Other versions
CN114877883A (zh
Inventor
张红娟
李必军
钱闯
招倩莹
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuhan University WHU
Original Assignee
Wuhan University WHU
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Wuhan University WHU filed Critical Wuhan University WHU
Priority to CN202210286541.XA priority Critical patent/CN114877883B/zh
Publication of CN114877883A publication Critical patent/CN114877883A/zh
Application granted granted Critical
Publication of CN114877883B publication Critical patent/CN114877883B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/36—Input/output arrangements for on-board computers
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3807—Creation or updating of map data characterised by the type of data
    • G01C21/3815—Road data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/43—Determining position using carrier phase measurements, e.g. kinematic positioning; using long or short baseline interferometry
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Navigation (AREA)

Abstract

本发明提供一种车路协同环境下考虑通信延迟的车辆定位方法及系统，直接将滞后的路侧端测量信息和车载端实时数据进行融合，以消除高度不稳定的通信延迟对车辆定位的影响，通过因子图实现异步数据的融合，避免引入数据拟合误差；实现过程包括利用车载LiDAR和GNSS‑RTK构建路侧周围环境的点云地图；将路侧LiDAR点云与点云地图进行配准，基于预建点云地图结合背景差分滤波提取动态目标点云；基于DBSCAN方法对预处理后的目标点云进行聚类，将属于不同目标的点云聚合成单独的簇；选取目标特征，通过支持向量机完成分类任务，获得车辆目标，现目标车辆在导航坐标系下的定位；基于因子图，通过增量平滑推理获得当前时刻的车辆状态最优估计，支持在GNSS信号失效下实时定位。

Description

车路协同环境下考虑通信延迟的车辆定位方法及系统
技术领域
本发明属于导航定位技术领域，尤其涉及一种基于因子图的车路协同环境下考虑通信延迟的车辆高精度定位方法。
背景技术
近年来，智能车路协同系统(Intelligent Vehicle-InfrastructureCooperation Systems，i-VICS)逐渐成为智能交通系统(Intelligent TransportationSystem，ITS)的主要发展方向，通过先进的无线通信技术和互联网技术，实施车-车、车-路的动态实时信息交互，实现车、路的有效协同。高精度的车辆定位是实现智能车路协同的关键技术之一。
传统车辆定位主要依赖于全球导航卫星系统(Global Navigation SatelliteSystem,GNSS)，在开阔场地、可见卫星数量不少于4个，且卫星信号良好的情况下，借助通过多基站网络RTK技术建立的连续运行(卫星定位服务)参考站(CORS)以及实时动态差分技术(RTK)达到厘米级的精度，但是在信号有遮挡、干扰区域(比如城市峡谷中、树荫下、高架桥下等区域)，GNSS定位精度会快速退化，甚至无法定位。
惯性导航系统(Inertial Navigation System，INS)的航位推算功能在GNSS信号失效时仍能对车辆的位置、姿态以及速度进行估计，然而惯性测量系统本身极易产生累积误差，随着车辆运动定位误差将不断累积，随着运动时间的增长，若不对INS误差进行修正，误差将超出可接受的范围，无法满足定位要求。
i-VICS和通讯技术的发展，为车辆高精度定位提供了新方向，从车载传感器定位逐渐转向车路协同定位。智能车路协同系统中的智能路侧单元集成多种通信和感知定位设备，可为车辆的高精度定位提供新的数据源，在GNSS失效情况下，通过i-VICS实现车端与路侧的信息交互，通过多源融合定位技术实现车载INS数据与路侧定位信息的融合，从而对车辆状态进行最优估计。然而路侧定位信息相对于车端的INS信息存在延迟，若对同一量测时刻的信息进行融合则融合时刻必定滞后于量测时刻，严重影响融合定位系统的实时性。
传统的异步问题解决策略是采用时间配准法将延迟量测信息统一到同一融合时刻，在融合估计前消除时间上的不同步。然而各时间配准法都存在一定局限性，难以避免引入数据拟合的误差。另一种策略则是针对异步的量测信息采用异步融合的方法，非等间隔滤波方法、多尺度滤波方法、增广状态方法以及基于新息重组最优滤波方法等等都存在不同程度的限制，难以满足车路协同环境下的异步融合。
因子图算法可以在一个融合时刻处理不同时刻的量测信息，能适应异步以及时延信息的融合，同时灵活的扩展性能实现传感器的即插即用，能适应多源组合导航系统的变化与扩展要求。因此，基于因子图实现车路协同环境下通信延迟的车辆高精度融合定位方法是可行的。
发明内容
有鉴于此，本发明提出了一种基于因子图的融合定位系统，它结合了惯性导航系统得到的车辆加速度以及角速度信息，路侧LiDAR识别得到的车辆位置信息，使用因子图融合算法，通过增量平滑法对因子图进行推断，实现对车辆当前状态的最优估计，在GNSS信号失效的情况下为车辆提供高精度的实时定位。
为了实现上述目的，本发明提出的技术方案提出一种车路协同环境下考虑通信延迟的车辆定位方法，直接将滞后的路侧端测量信息和车载端实时数据进行融合，以消除高度不稳定的通信延迟对车辆定位的影响，通过因子图实现异步数据的融合，避免引入数据拟合误差；实现过程包括以下步骤，
步骤1，利用车载LiDAR和GNSS-RTK构建路侧周围环境的点云地图；
步骤2，将路侧LiDAR点云与步骤1所得点云地图进行配准，计算安装位置和姿态角；
步骤3，基于预建点云地图结合背景差分滤波提取动态目标点云；
步骤4，基于DBSCAN方法对预处理后的目标点云进行聚类，将属于不同目标的点云聚合成单独的簇；
步骤5，选取目标点云簇长、宽、高、长宽比作为目标特征，通过支持向量机完成分类任务，获得车辆目标；计算目标簇内所有点云坐标平均值作为目标中心在激光雷达坐标系的坐标，结合路侧LiDAR标定结果，将目标车辆位置从激光雷达坐标系转换到导航坐标系下的位置，实现目标车辆在导航坐标系下的定位；
步骤6，通过4G通信网络，将时间和位置信息发送给车辆；车辆通过INS预积分构建当前时刻的等效IMU因子和偏差因子；车辆通过无线通信网络接收到因通信延迟造成的滞后时刻的位置信息构建为Position因子；构建INS/Position因子图；通过增量平滑推理获得当前时刻的车辆状态最优估计。
而且，步骤1的实现方式为，首先将LiDAR安装在车上，通过LIDAR/GNSS-RTK/INS融合，对若干帧场景中无动态目标的扫描点云数据进行拼接，预先构建路侧周围环境的高精度点云地图作为背景点云，得到预建的LiDAR点云地图。
而且，步骤2的实现方式为，将路侧激光点云作为待匹配点云，高精度点云地图作为目标点云，将LiDAR的测定坐标作为正态分布变换算法的迭代初值，求解激光点云在点云地图中的位置和姿态，作为路侧LiDAR的标定结果。
而且，步骤3的实现方式为，先根据点云坐标的抖动范围进行栅格分辨率设置；通过设定的栅格分辨率将背景点云映射至对应的栅格，根据栅格内环境点的数量，对每个栅格判定是否为环境栅格；当路侧LiDAR获取到新一帧点云时，映射至对应的栅格，将映射在环境栅格中的点进行滤除。
而且，步骤4中，在首次DBSCAN聚类结果基础上，对距离较远的目标，再次进行聚类，确保远处的目标也能准确地聚类为一个整体；聚类后的点云簇使用盒式模型来表示检测的目标。
本发明提供一种车路协同环境下考虑通信延迟的车辆定位系统，用于实现如上所述的一种车路协同环境下考虑通信延迟的车辆定位方法。
而且，用于直接将滞后的路侧端测量信息和车载端实时数据进行融合，以消除高度不稳定的通信延迟对车辆定位的影响，通过因子图实现异步数据的融合，避免引入数据拟合误差；包括以下模块，
第一模块，用于利用车载LiDAR和GNSS-RTK构建路侧周围环境的点云地图；
第二模块，用于将路侧LiDAR点云与第一模块所得点云地图进行配准，计算安装位置和姿态角，将路侧激光点云与点云地图通过点云配准实现标定；
第三模块，用于基于预建点云地图结合背景差分滤波提取动态目标点云；
第四模块，用于基于DBSCAN方法对预处理后的目标点云进行聚类，将属于不同目标的点云聚合成单独的簇；
第五模块，用于选取目标点云簇长、宽、高、长宽比作为目标特征，通过支持向量机完成分类任务，获得车辆目标；计算目标簇内所有点云坐标平均值作为目标中心在激光雷达坐标系的坐标，结合路侧LiDAR标定结果，将目标车辆位置从激光雷达坐标系转换到导航坐标系下的位置，实现目标车辆在导航坐标系下的定位；
第六模块，用于通过4G通信网络，将时间和位置信息发送给车辆；车辆通过INS预积分构建当前时刻的等效IMU因子和偏差因子；车辆通过无线通信网络接收到因通信延迟造成的滞后时刻的位置信息构建为Position因子；构建INS/Position因子图；通过增量平滑推理获得当前时刻的车辆状态最优估计。
或者，包括处理器和存储器，存储器用于存储程序指令，处理器用于调用存储器中的存储指令执行如上所述的一种车路协同环境下考虑通信延迟的车辆定位方法。
或者，包括可读存储介质，所述可读存储介质上存储有计算机程序，所述计算机程序执行时，实现如上所述的一种车路协同环境下考虑通信延迟的车辆定位方法。
本发明提出以下改进：
针对车路协同定位中存在的通信延迟问题，提出了一种基于因子图的车路协同定位方法，直接将滞后的路侧端测量信息和车载端实时数据进行融合，可消除高度不稳定的通信延迟对车辆定位的影响，通过因子图实现异步数据的融合，避免引入数据拟合误差。路侧端采用LiDAR对车体进行识别和定位，车载端利用INS进行自定位，并构建顾及通信延迟的INS/Position因子图模型，实现车路协同环境下的车辆高精度定位。
本发明方案实施简单方便，实用性强，解决了相关技术存在的实用性低及实际应用不便的问题，能够提高用户体验，具有重要的市场价值。
附图说明
图1为本发明实施例方法流程图。
图2为本发明实施例INS/Position融合定位因子图框架图。
具体实施方式
以下结合附图和实施例具体说明本发明的技术方案。
参见图1，本发明实施例一提供一种基于因子图的车路协同环境下考虑通信延迟的车辆高精度定位方法，包括以下步骤：
步骤1：利用车载LiDAR和GNSS-RTK构建路侧周围环境的点云地图。首先将LiDAR安装在车上，通过LIDAR/GNSS-RTK/INS融合，对若干帧场景中无动态目标的扫描点云数据进行拼接，预先构建路侧周围环境的高精度点云地图(背景点云)，即可得到预建的LiDAR点云地图；
步骤2：将路侧LiDAR点云与步骤1所得点云地图进行配准，计算安装位置和姿态角。将路侧激光点云与点云地图通过点云配准实现标定。本发明将路侧激光点云作为待匹配点云，高精度点云地图作为目标点云，将LiDAR的测定坐标作为正态分布变换(NormalDistributions Transform，NDT)算法的迭代初值，利用该现有算法可以方便地求解激光点云在点云地图中的位置和姿态，作为路侧LiDAR的标定结果；
步骤3：基于预建点云地图结合背景差分滤波提取动态目标点云。本发明采用背景差分滤波方法提取动态目标点云。对于图像差分滤波，相同的背景在不同帧中的图像像素点位置相对固定，因此在差分时能较好地滤除属于环境的像素点，但LiDAR由于测距误差的存在，同一背景在不同帧中的点云并不是完全重合的，其点云坐标会发生微弱抖动，因此无法直接通过背景点云对环境背景进行滤除。为将不同帧点云对应起来，需先将点云进行栅格化，栅格大小根据点云坐标的抖动范围进行设置，由LiDAR测距误差特性，通常将栅格设置为10cm以上。首先通过设定的栅格分辨率将背景点云映射至对应的栅格，根据栅格内环境点的数量，对每个栅格判定是否为环境栅格，当路侧LiDAR获取到新一帧点云时，将点云映射至对应的栅格，将映射在环境栅格中的点进行滤除；
步骤4：基于DBSCAN方法对预处理后的目标点云进行聚类。将属于不同目标的点云聚合成单独的簇。由于LiDAR扫描点越远密度越低，对于距离LiDAR较远的目标，可能被聚类为多个簇，因此，在首次DBSCAN聚类结果基础上，对距离较远的目标，再次进行聚类，确保远处的目标也能准确地聚类为一个整体。聚类后的点云簇需要按照某种规则识别为目标，由于目标车辆形状较为规则，因此使用盒式模型来表示检测的目标；
步骤5：选取目标点云簇长、宽、高、长宽比作为目标特征，通过支持向量机(Support Vector Machines，SVM)完成分类任务，获得车辆目标。计算目标簇内所有点云坐标平均值作为目标中心在激光雷达坐标系的坐标。结合路侧LiDAR标定结果，将目标车辆位置从激光雷达坐标系转换到导航坐标系下的位置，实现目标车辆在导航坐标系下的定位。
步骤6：通过4G通信网络，将时间和位置信息发送给车辆；车辆通过INS预积分构建当前时刻的等效IMU因子和偏差因子；车辆通过无线通信网络接收到因通信延迟造成的滞后时刻的位置信息构建为Position因子；构建INS/Position因子图；通过增量平滑推理获得当前时刻的车辆状态最优估计。
具体实施时，为了便于实现步骤6，可以利用现有因子图算法去处理延迟的异步问题。为了方便实施参考起见，本发明实施例二提供一种基于因子图的车路协同环境下考虑通信延迟的车辆高精度定位方法，包括以下步骤：
步骤1：基于因子图，定义INS/Position融合定位系统t时刻的状态变量包括位姿变量pt、速度变量vt以及惯性传感器偏差变量αt：
其中，t下标代表t时刻，n上标代表导航坐标系。xn、yn、zn分别表示载体在导航坐标系下的三维位置，γ、θ、ψ分别表示载体的横滚角、俯仰角和航向角，分别表示载体在导航坐标系下的三维速度，/>分别表示IMU“前右下”三轴加速度计偏差值，εx、εy、εz分别表示IMU“前右下”三轴陀螺仪偏差值。
步骤2：定义在k时刻INS/Position融合定位系统的状态输出序列用集合表示为：
Xk＝{p0,v0,α0,p1,v1,α1,…,pk,vk,αk}
步骤3：定义t时刻的测量值为：为IMU的测量值，/>为路侧LiDAR的对目标车辆进行识别与定位后得到的位置信息测量值。则在k时刻融合定位系统接收到的所有测量值用集合表示为：
Zk＝{z0,z1,……,zk}
步骤4：INS/Position融合定位问题表示为联合概率分布P(Xk|Zk)的最大后验概率估计问题：
步骤5：将P(Xk|Zk)进行因式分解得到和/>分别表示zk和Xk的子集，/>将/>抽象为因子/>与成比例；n代表因式分解得到的局部函数的数量，i＝1，2，…,n；
步骤6：因子表示由测量值构建的代价函数/> d(·)表示因子的代价函数，∑表示协方差矩阵，h(·)表示观测函数，zi表示测量值；
步骤7：定义位姿先验因子速度先验因子 IMU偏差先验因子/>μp为位姿先验信息高斯分布的均值，μv为速度先验信息高斯分布的均值，μα为IMU偏差先验信息高斯分布的均值；
定义Position因子hPos(·)表示路侧LiDAR的观测函数，nPos表示量测噪声，/>表示路侧LiDAR的位置测量值；
步骤8：定义等效IMU因子
hIMU(·)表示IMU的观测函数，Δ[pT vT]T k→k+1表示通过车载IMU采集的加速度信息和角速度信息，在系统输出的上一时刻k与当前时刻k+1之间进行INS预积分所得到两时刻之间的相对运动增量；
步骤9：定义偏差因子g(·)表示IMU偏差的变化函数，/>表示通过g(·)对αk+1进行预测得到的预测值
步骤10：对取负对数并舍去1/2项，转换为非线性最小二乘问题/>
步骤11：利用车载LiDAR和GNSS-RTK构建路侧周围环境的点云地图。首先将LiDAR安装在车上，通过LIDAR/GNSS-RTK/INS融合，对若干帧场景中无动态目标的扫描点云数据进行拼接，预先构建路侧周围环境的高精度点云地图(背景点云)，即可得到预建的LiDAR点云地图；
步骤12：将路侧LiDAR点云与步骤1所得点云地图进行配准，计算安装位置和姿态角。将路侧激光点云与点云地图通过点云配准实现标定。本发明将路侧激光点云作为待匹配点云，高精度点云地图作为目标点云，将LiDAR的测定坐标作为正态分布变换(NormalDistributions Transform，NDT)算法的迭代初值，利用该现有算法可以方便地求解激光点云在点云地图中的位置和姿态，作为路侧LiDAR的标定结果；
步骤13：基于预建点云地图结合背景差分滤波提取动态目标点云。本发明采用背景差分滤波方法提取动态目标点云。对于图像差分滤波，相同的背景在不同帧中的图像像素点位置相对固定，因此在差分时能较好地滤除属于环境的像素点，但LiDAR由于测距误差的存在，同一背景在不同帧中的点云并不是完全重合的，其点云坐标会发生微弱抖动，因此无法直接通过背景点云对环境背景进行滤除。为将不同帧点云对应起来，需先将点云进行栅格化，栅格大小根据点云坐标的抖动范围进行设置，由LiDAR测距误差特性，通常将栅格设置为10cm以上。首先通过设定的栅格分辨率将背景点云映射至对应的栅格，根据栅格内环境点的数量，对每个栅格判定是否为环境栅格，当路侧LiDAR获取到新一帧点云时，将点云映射至对应的栅格，将映射在环境栅格中的点进行滤除；
步骤14：基于DBSCAN方法对预处理后的目标点云进行聚类。将属于不同目标的点云聚合成单独的簇。由于LiDAR扫描点越远密度越低，对于距离LiDAR较远的目标，可能被聚类为多个簇，因此，在首次DBSCAN聚类结果基础上，对距离较远的目标，再次进行聚类，确保远处的目标也能准确地聚类为一个整体。聚类后的点云簇需要按照某种规则识别为目标，由于目标车辆形状较为规则，因此使用盒式模型来表示检测的目标；
步骤15：选取目标点云簇长、宽、高、长宽比作为目标特征，通过支持向量机(Support Vector Machines，SVM)完成分类任务，获得车辆目标。计算目标簇内所有点云坐标平均值作为目标中心在激光雷达坐标系的坐标。结合路侧LiDAR标定结果，将目标车辆位置从激光雷达坐标系转换到导航坐标系下的位置，实现目标车辆在导航坐标系下的定位。
步骤16：通过4G通信网络，将时间和位置信息发送给车辆；
步骤17：微处理器通过车载INS采集的加速度信息和角速度信息进行INS预积分，构建当前时刻k+1的IMU因子以及偏差因子；
步骤18：车辆通过无线通信网络接收到因通信延迟造成的滞后时刻的位置信息构建为Position因子，接收到k时刻的位置信息就构建接收到k-1时刻的位置信息就构建/>不对延迟值进行推算，避免引入拟合误差；
步骤19：添加当前时刻k+1状态变量pk+1,vk+1,αk+1，使用INS预积分从上一时刻对当前时刻的状态进行解算，并作为变量在最优估计中的迭代初始值；
步骤20：将构建的先验因子、IMU因子、偏差因子、Position因子构建INS/Position因子图：
步骤21：通过增量平滑推理获得当前时刻的车辆状态最优估计。通过变量消元将因子图转化为贝叶斯网络，再基于贝叶斯网络构建贝叶斯树，当新测量值到来时，新建因子，并更新贝叶斯树，通过贝叶斯树快速识别受影响变量，并对其增量进行重新估计，通过高斯牛顿迭代求解最优估计值。
参见图2，当路侧单元测量频率与系统输出频率相同且完全同步时，每一个系统状态量输出时刻都存在路侧单元测量值，则i+1时刻的INS/Position融合定位因子图框架如图所示。图中圆形表示待估计的变量，包括系统的位姿p、速度v以及IMU偏差α，其下标i表示系统解算输出时刻i，0表示0时刻，1表示1时刻，依此类推；方形表示各因子节点，包括位姿先验因子速度先验因子/>IMU偏差先验因子/>等效IMU因子fIMU、偏差因子fBias以及Position因子fPos。fIMU下标i-1,i表示fIMU因子包含了i-1以及i时刻的变量pi-1、vi-1、αi-1、pi、vi。fBias下标i-1,i表示fBias因子包含了i-1以及i时刻的变量αi-1、αi。fPos下标i表示fBias因子包含了i时刻的变量pi。图中的边表示因子与变量之间的依赖关系，一个因子代表一个局部函数，与之相连的变量即局部函数所包含的变量。
具体实施时，本发明技术方案提出的方法可由本领域技术人员采用计算机软件技术实现自动运行流程，实现方法的系统装置例如存储本发明技术方案相应计算机程序的计算机可读存储介质以及包括运行相应计算机程序的计算机设备，也应当在本发明的保护范围内。
在一些可能的实施例中，提供一种车路协同环境下考虑通信延迟的车辆定位系统，包括以下模块，
第一模块，用于利用车载LiDAR和GNSS-RTK构建路侧周围环境的点云地图；
第二模块，用于将路侧LiDAR点云与第一模块所得点云地图进行配准，计算安装位置和姿态角；
第三模块，用于基于预建点云地图结合背景差分滤波提取动态目标点云；
第四模块，用于基于DBSCAN方法对预处理后的目标点云进行聚类，将属于不同目标的点云聚合成单独的簇；
第五模块，用于选取目标点云簇长、宽、高、长宽比作为目标特征，通过支持向量机完成分类任务，获得车辆目标；计算目标簇内所有点云坐标平均值作为目标中心在激光雷达坐标系的坐标，结合路侧LiDAR标定结果，将目标车辆位置从激光雷达坐标系转换到导航坐标系下的位置，实现目标车辆在导航坐标系下的定位；
第六模块，用于通过4G通信网络，将时间和位置信息发送给车辆；车辆通过INS预积分构建当前时刻的等效IMU因子和偏差因子；车辆通过无线通信网络接收到因通信延迟造成的滞后时刻的位置信息构建为Position因子；构建INS/Position因子图；通过增量平滑推理获得当前时刻的车辆状态最优估计。
在一些可能的实施例中，提供一种车路协同环境下考虑通信延迟的车辆定位系统，包括可读存储介质，所述可读存储介质上存储有计算机程序，所述计算机程序执行时，实现如上所述的一种车路协同环境下考虑通信延迟的车辆定位方法。
本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (9)

1.一种车路协同环境下考虑通信延迟的车辆定位方法，其特征在于：直接将滞后的路侧端测量信息和车载端实时数据进行融合，以消除高度不稳定的通信延迟对车辆定位的影响，通过因子图实现异步数据的融合，避免引入数据拟合误差；实现过程包括以下步骤，
步骤1，利用车载LiDAR和GNSS-RTK构建路侧周围环境的点云地图；
步骤2，将路侧LiDAR点云与步骤1所得点云地图进行配准，计算安装位置和姿态角；
步骤3，基于预建点云地图结合背景差分滤波提取动态目标点云；
步骤4，基于DBSCAN方法对预处理后的目标点云进行聚类，将属于不同目标的点云聚合成单独的簇；
步骤5，选取目标点云簇长、宽、高、长宽比作为目标特征，通过支持向量机完成分类任务，获得车辆目标；计算目标簇内所有点云坐标平均值作为目标中心在激光雷达坐标系的坐标，结合路侧LiDAR标定结果，将目标车辆位置从激光雷达坐标系转换到导航坐标系下的位置，实现目标车辆在导航坐标系下的定位；
步骤6，通过4G通信网络，将时间和位置信息发送给车辆；车辆通过INS预积分构建当前时刻的等效IMU因子和偏差因子；车辆通过无线通信网络接收到因通信延迟造成的滞后时刻的位置信息构建为Position因子；构建INS/Position因子图；通过增量平滑推理获得当前时刻的车辆状态最优估计。
2.根据权利要求1所述车路协同环境下考虑通信延迟的车辆定位方法，其特征在于：步骤1的实现方式为，首先将LiDAR安装在车上，通过LIDAR/GNSS-RTK/INS融合，对若干帧场景中无动态目标的扫描点云数据进行拼接，预先构建路侧周围环境的高精度点云地图作为背景点云，得到预建的LiDAR点云地图。
3.根据权利要求1所述车路协同环境下考虑通信延迟的车辆定位方法，其特征在于：步骤2的实现方式为，将路侧激光点云作为待匹配点云，高精度点云地图作为目标点云，将LiDAR的测定坐标作为正态分布变换算法的迭代初值，求解激光点云在点云地图中的位置和姿态，作为路侧LiDAR的标定结果。
4.根据权利要求1所述车路协同环境下考虑通信延迟的车辆定位方法，其特征在于：步骤3的实现方式为，先根据点云坐标的抖动范围进行栅格分辨率设置；通过设定的栅格分辨率将背景点云映射至对应的栅格，根据栅格内环境点的数量，对每个栅格判定是否为环境栅格；当路侧LiDAR获取到新一帧点云时，映射至对应的栅格，将映射在环境栅格中的点进行滤除。
5.根据权利要求1或2或3或4所述车路协同环境下考虑通信延迟的车辆定位方法，其特征在于：步骤4中，在首次DBSCAN聚类结果基础上，对距离较远的目标，再次进行聚类，确保远处的目标也能准确地聚类为一个整体；聚类后的点云簇使用盒式模型来表示检测的目标。
6.一种车路协同环境下考虑通信延迟的车辆定位系统，其特征在于：用于实现如权利要求1-5任一项所述的一种车路协同环境下考虑通信延迟的车辆定位方法。
7.根据权利要求6所述车路协同环境下考虑通信延迟的车辆定位系统，其特征在于：用于直接将滞后的路侧端测量信息和车载端实时数据进行融合，以消除高度不稳定的通信延迟对车辆定位的影响，通过因子图实现异步数据的融合，避免引入数据拟合误差；包括以下模块，
第一模块，用于利用车载LiDAR和GNSS-RTK构建路侧周围环境的点云地图；
第二模块，用于将路侧LiDAR点云与第一模块所得点云地图进行配准，计算安装位置和姿态角；
第三模块，用于基于预建点云地图结合背景差分滤波提取动态目标点云；
第四模块，用于基于DBSCAN方法对预处理后的目标点云进行聚类，将属于不同目标的点云聚合成单独的簇；
第五模块，用于选取目标点云簇长、宽、高、长宽比作为目标特征，通过支持向量机完成分类任务，获得车辆目标；计算目标簇内所有点云坐标平均值作为目标中心在激光雷达坐标系的坐标，结合路侧LiDAR标定结果，将目标车辆位置从激光雷达坐标系转换到导航坐标系下的位置，实现目标车辆在导航坐标系下的定位；
第六模块，用于通过4G通信网络，将时间和位置信息发送给车辆；车辆通过INS预积分构建当前时刻的等效IMU因子和偏差因子；车辆通过无线通信网络接收到因通信延迟造成的滞后时刻的位置信息构建为Position因子；构建INS/Position因子图；通过增量平滑推理获得当前时刻的车辆状态最优估计。
8.根据权利要求6所述车路协同环境下考虑通信延迟的车辆定位系统，其特征在于：包括处理器和存储器，存储器用于存储程序指令，处理器用于调用存储器中的存储指令执行如权利要求1-5任一项所述的一种车路协同环境下考虑通信延迟的车辆定位方法。
9.根据权利要求6所述车路协同环境下考虑通信延迟的车辆定位系统，其特征在于：包括可读存储介质，所述可读存储介质上存储有计算机程序，所述计算机程序执行时，实现如权利要求1-5任一项所述的一种车路协同环境下考虑通信延迟的车辆定位方法。
CN202210286541.XA 2022-03-22 2022-03-22 车路协同环境下考虑通信延迟的车辆定位方法及系统 Active CN114877883B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210286541.XA CN114877883B (zh) 2022-03-22 2022-03-22 车路协同环境下考虑通信延迟的车辆定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210286541.XA CN114877883B (zh) 2022-03-22 2022-03-22 车路协同环境下考虑通信延迟的车辆定位方法及系统

Publications (2)

Publication Number Publication Date
CN114877883A CN114877883A (zh) 2022-08-09
CN114877883B true CN114877883B (zh) 2024-04-26

Family

ID=82666509

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210286541.XA Active CN114877883B (zh) 2022-03-22 2022-03-22 车路协同环境下考虑通信延迟的车辆定位方法及系统

Country Status (1)

Country Link
CN (1) CN114877883B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115451976B (zh) * 2022-09-15 2023-06-02 北京四维远见信息技术有限公司 一种高精度定位方法、装置和设备
CN116192691B (zh) * 2022-12-23 2024-04-23 海信集团控股股份有限公司 一种数据包延时判断方法和计算设备
CN115900732A (zh) * 2023-01-05 2023-04-04 武汉大学 基于路侧相机和车载单元的组合导航方法及系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111949943A (zh) * 2020-07-24 2020-11-17 北京航空航天大学 一种面向高级自动驾驶的v2x和激光点云配准的车辆融合定位方法
US11002859B1 (en) * 2020-02-27 2021-05-11 Tsinghua University Intelligent vehicle positioning method based on feature point calibration
CN113029137A (zh) * 2021-04-01 2021-06-25 清华大学 一种多源信息自适应融合定位方法及系统
CN113537362A (zh) * 2021-07-20 2021-10-22 中国第一汽车股份有限公司 一种基于车路协同的感知融合方法、装置、设备及介质
CN113734176A (zh) * 2021-09-18 2021-12-03 重庆长安汽车股份有限公司 智能驾驶车辆的环境感知系统、方法、车辆及存储介质

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11002859B1 (en) * 2020-02-27 2021-05-11 Tsinghua University Intelligent vehicle positioning method based on feature point calibration
CN111949943A (zh) * 2020-07-24 2020-11-17 北京航空航天大学 一种面向高级自动驾驶的v2x和激光点云配准的车辆融合定位方法
CN113029137A (zh) * 2021-04-01 2021-06-25 清华大学 一种多源信息自适应融合定位方法及系统
CN113537362A (zh) * 2021-07-20 2021-10-22 中国第一汽车股份有限公司 一种基于车路协同的感知融合方法、装置、设备及介质
CN113734176A (zh) * 2021-09-18 2021-12-03 重庆长安汽车股份有限公司 智能驾驶车辆的环境感知系统、方法、车辆及存储介质

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Cooperative GNSS-RTK Ambiguity Resolution with GNSS, INS, and LiDAR Data for Connected Vehicles;Chuang Qian 等;remote sensing;20200315;全文 *
基于缩微车的智能驾驶设计与研究;程港;李必军;;测绘通报;20130125(第01期);全文 *
顾及通信延迟的车路协同高精度定位;张红娟 等;测绘学报;20240131;第53卷(第1期);全文 *

Also Published As

Publication number Publication date
CN114877883A (zh) 2022-08-09

Similar Documents

Publication Publication Date Title
CN114877883B (zh) 车路协同环境下考虑通信延迟的车辆定位方法及系统
KR102581263B1 (ko) 위치 추적 방법, 장치, 컴퓨팅 기기 및 컴퓨터 판독 가능한 저장 매체
CN102398598B (zh) 使用前视和后视摄像机的车道融合系统
CN111949943A (zh) 一种面向高级自动驾驶的v2x和激光点云配准的车辆融合定位方法
WO2021026705A1 (zh) 匹配关系确定方法、重投影误差计算方法及相关装置
CN108362294B (zh) 一种应用于自动驾驶的多车协同建图方法
US20220292711A1 (en) Pose estimation method and device, related equipment and storage medium
CN109100730A (zh) 一种多车协同快速建图方法
CN104677361B (zh) 一种综合定位的方法
CN102700548A (zh) 具有前摄像机和后摄像机的稳健的车辆侧向控制
CN114636993A (zh) 一种激光雷达与imu的外参标定方法、装置及设备
KR101909953B1 (ko) 라이다 센서를 이용한 차량의 자세 추정 방법
US11158065B2 (en) Localization of a mobile unit by means of a multi hypothesis kalman filter method
CN112414417B (zh) 自动驾驶地图生成方法、装置、电子设备及可读存储介质
CN110187375A (zh) 一种基于slam定位结果提高定位精度的方法及装置
CN114018248A (zh) 一种融合码盘和激光雷达的里程计方法与建图方法
CN114111775A (zh) 一种多传感器融合定位方法、装置、存储介质及电子设备
CN114323050A (zh) 车辆定位方法、装置和电子设备
CN112596089A (zh) 融合定位方法、装置、电子设备及存储介质
CN110989619B (zh) 用于定位对象的方法、装置、设备和存储介质
CN114485698A (zh) 一种交叉路口引导线生成方法及系统
CN111829514A (zh) 一种适用于车辆底盘集成控制的路面工况预瞄方法
CN115494533A (zh) 车辆定位方法、装置、存储介质及定位系统
CN113608556B (zh) 一种基于多传感器融合的多机器人相对定位方法
US20220390607A1 (en) Collaborative estimation and correction of lidar boresight alignment error and host vehicle localization error

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant