CN117029853A - 多传感器融合定位方法、系统、车辆、设备及介质 - Google Patents

多传感器融合定位方法、系统、车辆、设备及介质 Download PDF

Info

Publication number
CN117029853A
CN117029853A CN202311013290.9A CN202311013290A CN117029853A CN 117029853 A CN117029853 A CN 117029853A CN 202311013290 A CN202311013290 A CN 202311013290A CN 117029853 A CN117029853 A CN 117029853A
Authority
CN
China
Prior art keywords
data
positioning
factor
sensor
frame
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.)
Pending
Application number
CN202311013290.9A
Other languages
English (en)
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.)
China National Heavy Duty Truck Group Jinan Power Co Ltd
Original Assignee
China National Heavy Duty Truck Group Jinan Power Co Ltd
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 China National Heavy Duty Truck Group Jinan Power Co Ltd filed Critical China National Heavy Duty Truck Group Jinan Power Co Ltd
Priority to CN202311013290.9A priority Critical patent/CN117029853A/zh
Publication of CN117029853A publication Critical patent/CN117029853A/zh
Pending legal-status Critical Current

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/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/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
    • 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
    • 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/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • 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)
  • Traffic Control Systems (AREA)

Abstract

本申请提供一种多传感器融合定位方法、系统、车辆、设备及介质，涉及自动驾驶定位技术领域。该方法包括：获取多种原始数据，并对各种原始数据分别进行失效识别；在识别到所有原始数据中的至少一种为失效数据时，将因子图模型中与失效数据对应的因子的权重值减小到第一预设权重值；其中，因子图模型包含所有原始数据对应的因子以及所有因子的权重值；对各种原始数据分别进行处理，得到各种原始数据对应的定位数据；将各种原始数据对应的定位数据作为各原始数据对应的因子，输入至因子图模型，以供因子图模型对各因子进行计算，得到车辆的融合定位结果。上述方式提高了融合定位结果的精确度。

Description

多传感器融合定位方法、系统、车辆、设备及介质
技术领域
本申请涉及自动驾驶定位技术领域，尤其涉及一种多传感器融合定位方法、系统、车辆、设备及介质。
背景技术
露天矿区自动驾驶矿卡是车辆中的一种，其定位方案大多采用单一组合导航定位的方式。由于矿区地理位置偏远、网络信号薄弱、山体遮挡等原因，组合导航容易出现搜星失败、无网络信号引起的失锁问题。部分研究采用搭建5G基站的方法解决网络信号弱的问题，但是难以解决由于山体遮挡引起的全球导航卫星系统(Global Navigation SatelliteSystem，GNSS)失锁问题。
为解决上述失锁问题，现有技术提供多传感器融合定位技术，多传感器融合定位技术是指利用多个传感器的信息，通过滤波或优化的方法进行融合，对目标的位置和姿态进行估计和精确定位。
现有的多传感器融合定位技术，在涉及多种传感器的情况下，很容易出现传感器故障，并且不易被及时发现，因此在继续利用故障传感器采集到的数据进行融合定位时，存在融合定位结果准确度差的问题，严重时容易导致矿卡失控，产生安全隐患。
发明内容
本申请提供了一种多传感器融合定位方法、系统、车辆、设备及介质，用以解决现有技术存在的精确度差的技术问题。
根据本申请的第一方面，提供了一种多传感器融合定位方法，包括：
获取多种原始数据，并对各种原始数据分别进行失效识别；其中，不同的所述原始数据是由不同的目标传感器采集到的用于对车辆进行融合定位的数据；
在识别到所有所述原始数据中的至少一种为失效数据时，将因子图模型中与所述失效数据对应的因子的权重值减小到第一预设权重值；其中，所述因子图模型包含所有所述原始数据对应的因子以及所有因子的权重值；
对各种所述原始数据分别进行处理，得到各种所述原始数据对应的定位数据；
将各种所述原始数据对应的定位数据作为各所述原始数据对应的因子，输入至所述因子图模型，以供所述因子图模型对各所述因子进行计算，得到车辆的融合定位结果；其中，所述融合定位结果包括以下至少之一：位置、姿态和速度。
可选地，所述获取多种原始数据，并对各种原始数据分别进行失效识别，包括：
获取以下至少两种原始数据：激光雷达传感器采集到的激光点云、惯性导航传感器采集到的惯性导航数据和全球导航卫星系统GNSS传感器采集到的卫星定位数据；
针对所述激光点云，提取所述激光点云的角点特征，根据所述激光点云的角点特征统计出激光点云的角点数量，判断所述激光点云的角点数量是否低于预设数量；其中，所述激光点云的角点数量低于预设数量时，表示所述激光点云数据为失效数据；
针对所述卫星定位数据，判断所述GNSS传感器是否处于RTK稳定解状态；其中，所述GNSS传感器处于非RTK稳定解状态时，表示所述卫星定位数据为失效数据。
可选地，所述对各种所述原始数据分别进行处理，得到各种所述原始数据对应的定位数据，包括以下至少两项：
对当前激光雷达数据帧的激光点云进行预处理，并采用激光里程计进行所述激光点云到局部地图的点云匹配，得到第一定位数据；其中，所述第一定位数据包括当前激光雷达数据帧与前一激光雷达数据帧之间的相对变换位姿；
对惯性导航数据进行预积分计算，得到第二定位数据；其中，所述第二定位数据为所述惯性导航数据在所述当前激光雷达数据帧与所述前一激光雷达数据帧之间的预积分量；
对卫星定位数据进行解算，得到第三定位数据；其中，所述第三定位数据为单帧定位信息。
可选地，所述因子图模型包括第一帧间约束因子、第二帧间约束因子和单帧约束因子；
则所述将各种所述原始数据对应的定位数据作为各所述原始数据对应的因子，包括：
将所述第一定位数据作为第一帧间约束因子，将所述第二定位数据作为第二帧间约束因子，将所述第三定位数据作为单帧约束因子；
所述因子图模型还包括：滑动窗口和边缘化因子；所述第一帧间约束因子、所述第二帧间约束因子和所述单帧约束因子均位于所述滑动窗口内；所述边缘化因子由历史帧数据确定，并位于所述滑动窗口外。
可选地，在对各种所述原始数据分别进行处理之前，所述方法还包括：
判断所述激光雷达传感器、所述惯性导航传感器和所述GNSS传感器对应的数据存储队列是否存储有新增数据；
若多个所述数据存储队列均有新增数据，则以当前激光雷达数据帧为目标帧，分别从所述惯性导航传感器和所述GNSS传感器对应的数据存储队列中查找与所述目标帧相邻的前后两帧数据，分别计算对应的时间差，并分别建立时间戳与所述目标帧的时间戳相同的新增帧数据，以实现对所述惯性导航数据和所述卫星定位数据的数据时间同步处理。
可选地，在所述将因子图模型中与所述失效数据对应的因子的权重值减小到第一预设权重值之后，所述方法还包括：
将所述因子图模型中与正常数据对应的因子的权重值增大到第二预设权重值；其中，所述正常数据为所有所述原始数据中除失效数据以外的其他数据。
根据本申请的第二方面，提供了一种多传感器融合定位系统，包括：
故障检测模块，用于获取多种原始数据，并对各种原始数据分别进行失效识别；其中，不同的所述原始数据是由不同的目标传感器采集到的用于对车辆进行融合定位的数据；
融合定位优化模块，用于在识别到所有所述原始数据中的至少一种为失效数据时，将因子图模型中与所述失效数据对应的因子的权重值减小到第一预设权重值；其中，所述因子图模型包含所有所述原始数据对应的因子以及所有因子的权重值；
数据处理模块，用于对各种所述原始数据分别进行处理，得到各种所述原始数据对应的定位数据；
所述融合定位优化模块，还用于将各种所述原始数据对应的定位数据作为各所述原始数据对应的因子，输入至所述因子图模型，以供所述因子图模型对各所述因子进行计算，得到车辆的融合定位结果；其中，所述融合定位结果包括以下至少之一：位置、姿态和速度。
根据本申请的第三方面，提供了一种车辆，包括多传感器融合定位系统；其中，所述多传感器融合定位系统通过通信系统获取到多种原始数据，并在进行多传感器融合定位后，将车辆的融合定位结果发送至所述通信系统。
根据本申请的第四方面，提供了一种电子设备，包括：至少一个处理器和存储器；
所述存储器存储计算机执行指令；
所述至少一个处理器执行所述存储器存储的计算机执行指令，使得所述至少一个处理器执行如上第一方面所述的多传感器融合定位方法。
根据本申请的第五方面，提供了一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机执行指令，所述计算机执行指令被处理器执行时用于实现如上第一方面所述的多传感器融合定位方法。
根据本申请的第六方面，提供了一种计算机程序产品，包括计算机程序，该计算机程序被处理器执行时实现第一方面所述的多传感器融合定位方法。
本申请提供的一种多传感器融合定位方法，包括：获取多种原始数据，并对各种原始数据分别进行失效识别；其中，不同的原始数据是由不同的目标传感器采集到的用于对车辆进行融合定位的数据；在识别到所有原始数据中的至少一种为失效数据时，将因子图模型中与失效数据对应的因子的权重值减小到第一预设权重值；其中，因子图模型包含所有原始数据对应的因子以及所有因子的权重值；对各种原始数据分别进行处理，得到各种原始数据对应的定位数据；将各种原始数据对应的定位数据作为各原始数据对应的因子，输入至因子图模型，以供因子图模型对各因子进行计算，得到车辆的融合定位结果；其中，融合定位结果包括以下至少之一：位置、姿态和速度。
本申请通过对各种原始数据进行失效识别，进而在某种原始数据为失效数据的情况下减小该失效数据对应的因子在因子图模型中的权重的方式，能够基于原始数据的真实有效情况，对因子图模型中的各因子的权重值进行自适应调整，能够保证因子图模型的有效性，进而提高融合定位结果的精确度。
应当理解，本部分所描述的内容并非旨在标识本申请的实施例的关键或重要特征，也不用于限制本申请的范围。本申请的其它特征将通过以下的说明书而变得容易理解。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本申请的实施例，并与说明书一起用于解释本申请的原理。
图1为本申请实施例提供的一种多传感器融合定位系统的结构示意图；
图2为本申请实施例提供的另一种多传感器融合定位系统的结构示意图；
图3为本申请实施例提供的因子图模型的结构示意图；
图4为本申请实施例提供的激光点云在去畸变前后的对比图；
图5为本申请实施例提供的惯性导航数据的预积分示意图；
图6为激光点云和惯性导航数据各自对应的采样周期示意图；
图7为本申请实施例提供的一种多传感器融合定位方法的流程示意图；
图8为本申请实施例提供的另一种多传感器融合定位方法的流程示意图；
图9为本申请实施例提供的一种车辆的结构示意图；
图10为本申请实施例提供的一种电子设备的结构示意图。
通过上述附图，已示出本申请明确的实施例，后文中将有更详细的描述。这些附图和文字描述并不是为了通过任何方式限制本申请构思的范围，而是通过参考特定实施例为本领域技术人员说明本申请的概念。
具体实施方式
这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本申请相一致的所有实施方式。
为解决失锁问题，现有技术提供多传感器融合定位技术，多传感器融合定位技术是指利用多个传感器的信息，通过滤波或优化的方法进行融合，对目标的位置和姿态进行估计和精确定位。传统的定位技术通常只依赖于单一的传感器，如GNSS传感器、惯性导航传感器、相机、激光雷达传感器等，其定位精度和鲁棒性受限于单一传感器的性能和工作环境的限制，而利用多个传感器的信息进行融合，可以充分利用各自的优势，提高定位精度和鲁棒性。
但是，现有的几种多传感器融合定位技术，分别存在以下问题：
(1)在涉及多种传感器的情况下，该技术很容易出现传感器故障，并且不易被及时发现，因此在继续利用故障传感器采集到的数据进行融合定位时，存在融合定位结果准确度差的问题。
(2)现有的基于多传感器融合的定位技术，采用的是基于线面特征提取的点云配准方法，该方法基于预生成的点云特征地图，针对矿山场景的点云特征稀疏和环境易变化，点云地图的更新和匹配难度均较大，存在实现难度大的问题。
(3)现有的基于多传感器信息即时融合定位方法，没有考虑传感器故障或者传感器定位失效的情况。
(4)现有技术提出了一种基于超宽带(Ultra Wideband，UWB)、惯导及激光雷达的井工矿融合定位方法，该方法构建有多个模型，同样未考虑传感器失效的情况，且存在需要考虑地图的更新问题。
为了解决上述技术问题，本申请的整体发明构思为如何提供一种应用于自动驾驶定位领域，用于提高融合定位结果准确度的系统。
下面以具体地实施例对本申请的技术方案以及本申请的技术方案如何解决上述技术问题进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例中不再赘述。下面将结合附图，对本申请的实施例进行描述。
实施例1：
本实施例提供了一种多传感器融合定位系统，可以实现高精度、实时的位置和姿态估计。该系统基于激光雷达传感器、惯性导航传感器和GNSS传感器采集的原始数据，进行数据预处理、点云匹配、惯导数据积分、GNSS位姿解算、数据时间同步、传感器故障检测和融合定位优化等流程，实现多传感器数据的融合和定位精度的提高。
下面结合图1，对本实施例的具体实施方案作进一步的详细说明。
图1为本申请实施例提供的一种多传感器融合定位系统的结构示意图。如图1所示，本实施例的系统，包括以下模块：故障检测模块1、融合定位优化模块2和数据处理模块3，其中：
故障检测模块1，用于获取多种原始数据，并对各种原始数据分别进行失效识别；其中，不同的原始数据是由不同的目标传感器采集到的用于对车辆进行融合定位的数据。
在本申请实施例中，上述原始数据包括：激光点云、惯性导航数据和卫星定位数据等。上述激光点云或称为激光雷达点云信息、点云、激光雷达数据等。惯性导航数据或称为原始惯导数据、惯导数据、惯导测量值、惯导传感器数据等。GNSS传感器数据或称为GNSS数据、卫星定位数据等。
上述目标传感器包括：激光雷达传感器、惯性导航传感器和全球导航卫星系统GNSS传感器等。其中，激光雷达传感器或称为激光传感器，上述GNSS传感器或称为卫星定位传感器。
故障检测模块1或称为传感器故障检测模块，该故障检测模块1主要进行激光雷达传感器和GNSS传感器的定位功能故障检测，针对出现故障的传感器，本申请实施例在后续流程中采用降低故障传感器采集到的定位数据的置信度的方式，提高融合定位结果的精确度。
该置信度，可以表现为失效数据对应的因子在因子图模型中的权重，其中，权重越大，表明置信度越高；反之，则置信度越低。
融合定位优化模块2，用于在识别到所有原始数据中的至少一种为失效数据时，将因子图模型中与失效数据对应的因子的权重值减小到第一预设权重值；其中，因子图模型包含所有原始数据对应的因子以及所有因子的权重值。
在本申请实施例中，因子图模型或称为滑窗优化模型、因子图优化模型、位姿图模型等，用于提供统一的优化框架。
数据处理模块3，用于对各种原始数据分别进行处理，得到各种原始数据对应的定位数据。
融合定位优化模块2，还用于将各种原始数据对应的定位数据作为各原始数据对应的因子，输入至因子图模型，以供因子图模型对各因子进行计算，得到车辆的融合定位结果；其中，融合定位结果包括以下至少之一：位置、姿态和速度。
本实施例具有以下优势：第一方面，本实施例综合利用不同传感器的优点，克服单一传感器在矿区环境下存在的局限性，提高定位精度和可靠性。第二方面，本实施例具有传感器故障检测和数据相互检验功能，针对故障传感器和失效数据，进行有效的故障隔离，能够保证整体定位输出的融合定位结果更加稳定。
本申请实施例通过对各种原始数据进行失效识别，进而在某种原始数据为失效数据的情况下减小该失效数据对应的因子在因子图模型中的权重的方式，能够基于原始数据的真实有效情况，对因子图模型中的各因子的权重值进行自适应调整，能够保证因子图模型的有效性，进而提高融合定位结果的精确度。
一种可能的实现方式中，如图2所示，数据时间同步模块4、数据处理模块3、故障检测模块1、融合定位优化模块2、定位结果输出模块5用于构成后端优化模块。激光雷达传感器、惯性导航传感器、GNSS传感器等用于构成前端模块。后端优化模块接收前端模块发布的消息，进行数据时间同步，进行数据时间同步，进行传感器故障检测和数据校验，进而基于建立好的统一结构的因子图模型进行对各因子进行优化，实现多个数据帧的位姿优化，进而输出融合定位结果。
其中，各传感器具有以下优缺点：激光雷达传感器依靠外界环境，即通过外界的环境点来进行车辆的定位，其在获取到的特征点较少的情况下，其精度下降。惯性导航传感器不需要获取外界环境信息，通过车辆运动情况，能够给出一个运动变化量，其在车辆短时间内的定位精度较高，但是车辆在长时间运动中的定位精度下降，下降后需要其他传感器进行纠正。GNSS传感器需要卫星信号和网络信号，一旦两种信号中的一个无法获取到，就导致失锁，进而无法进行精确定位。
本实施例具体基于融合激光雷达传感器、惯性导航传感器、GNSS传感器采集到的原始数据，执行数据预处理、位姿解算、传感器故障检测、融合优化等操作流程，实现高精度、高可靠性的融合定位结果的输出。
一种可能的实现方式中，故障检测模块1，还用于：
获取以下至少两种原始数据：激光雷达传感器采集到的激光点云、惯性导航传感器采集到的惯性导航数据和全球导航卫星系统GNSS传感器采集到的卫星定位数据。
针对激光点云，提取激光点云的角点特征，根据激光点云的角点特征统计出激光点云的角点数量，判断激光点云的角点数量是否低于预设数量；其中，激光点云的角点数量低于预设数量时，表示激光点云数据为失效数据。
在本申请实施例中，预设数量可以为20个、30个、50、60个等，本申请实施例对该预设数量的具体数值不做具体限定。
针对GNSS传感器获取的卫星定位数据，判断GNSS传感器是否处于RTK稳定解状态；其中，GNSS传感器处于非RTK稳定解状态时，表示卫星定位数据为失效数据。
也就是说，故障检测模块1接收各个传感器采集到的数据，然后针对不同传感器，分别进行对应的检测，其中，传感器发生故障时，其采集到的数据为失效数据。
针对激光雷达传感器的故障检测，本实施例先进行激光点云的特征点提取，统计激光点云的角点数量，若小于30个，则确定该帧激光点云无法实现有效的帧间里程计功能，并将激光点云标记为失效点云。需要注意的是，本实施例可以仅保留该帧激光点云的时间，该时间便于惯导数据积分单元进行预积分计算。
针对GNSS传感器的故障检测，本实施例主要依赖GNSS传感器内部RTK算法的输出，判断RTK算法当前是否为稳定解状态。若RTK算法不是稳定解状态，则确定GNSS传感器发生失锁，且融合定位优化模块通过后端融合定位程序降低第三定位数据的置信度，和/或，提高第一定位数据和第二定位数据的置信度。
也就是说，当GNSS传感器未发生失锁时，第三定位数据的置信度为一个值a31，当GNSS传感器发生失锁时，第三定位数据的置信度为另一个值a32，并且a32小于a31。
本申请实施例可以有效识别各传感器故障情况和其对应数据的失效情况，进而保证数据的真实性，进而提高本实施例的融合定位的准确度。
一种可能的实现方式中，如图2所示，数据处理模块3包括数据预处理单元31、激光里程计单元32、惯导数据积分单元33和GNSS位姿解算单元34。在上述数据处理模块3包含上述几个单元的基础上，数据处理模块3，还用于执行以下操作：
数据预处理单元31，用于对当前激光雷达数据帧的激光点云进行预处理。
应理解，该数据预处理单元31实时从通信系统中接收消息，消息中携带激光点云。预处理包括但不限于：降采样滤波、坐标转换、去畸变处理等。其中，降采样滤波和坐标转换在去畸变处理之前完成，并且本申请实施例对降采样滤波和坐标转换的执行顺序不做具体限定。
具体的，针对降采样滤波，激光点云的降采样滤波方法可以采用基于体素栅格的方法，其中，栅格的大小选择以0.5m为边长的立方体，将每个三维的体素栅格内的所有点用其质心来表示。
其中，(xi，yi，zi)为体素栅格内第i个点的坐标，(Xcentroid，Ycentroid，Zcentroid)为体素栅格内的所有点的质心坐标，m为点的数量。
针对坐标转换，激光点云的坐标变换是将所有点云的坐标轴由雷达坐标系转到车体坐标系。
针对去畸变处理，激光点云的去畸变处理可以采用恒速模型进行补偿的方法，即利用前两帧的相对位姿预测当前帧t与前一帧t-1的相对位姿变换Tpred，t，将该位姿变换认为是当前帧t的起始点与结束点的位姿变换，然后通过点云中每个点的时间去线性插值每个点的位置：
其中，Rt-1为t-1帧的姿态、Rt-2为t-2帧的姿态，tt-1为t-1帧的位置，tt-2为t-2帧的位置。
如图4所示，当激光雷达传感器运动时，被扫描物体的点云形状发生畸变，本实施例可以采取运动补偿方法进行去畸变处理，以将点云形状恢复为原来形状，进而保证定位精度。
可选地，数据预处理单元31可以将预处理后的激光点云发送至故障检测模块1和/或激光里程计单元32，以供故障检测模块1基于该预处理后的激光点云对激光雷达传感器进行故障检测。此外，故障检测模块1还可以基于原始的激光点云实现故障检测。
激光里程计单元32，用于采用激光里程计进行激光点云到局部地图的点云匹配，得到第一定位数据；其中，第一定位数据包括当前激光雷达数据帧与前一激光雷达数据帧之间的相对变换位姿。
应理解，第一定位数据或称为激光里程计，当前激光雷达数据帧或称为当前点云、当前帧，前一激光雷达数据帧或称为上一关键帧。激光雷达数据帧或称为关键帧点云。
在本申请实施例中，激光里程计单元32在点云匹配之后，向融合定位优化模块2提供激光里程计，以使融合定位优化模块2通过建立好的因子图模型对该激光里程计进行优化。此外，局部地图可以使用八叉树的方式进行存储，用以保证点云搜索和点云匹配的效率。
具体的，在车辆前进的过程中，本实施例实时进行局部地图的构建，建立激光点云关键帧的分选机制，以当前点云和上一关键帧的距离、时间关系，判断当前帧是否加入局部地图。同时，为了保证点云匹配的效率，本实施例可以控制局部地图的大小，即设定局部地图的帧数不超过30帧。当局部地图中的点云帧数达到30帧时，每加入一帧新的关键帧点云，就自动剔除时间最久的关键帧点云，总体维持局部地图的动态平衡。
如图5所示，激光里程计单元的局部地图大小在运动过程中保持动态平衡，当矿车从P0行驶到局部地图边缘P1时，激光里程计单元中的检测程序自动更新局部地图中心点的位置。
本实施例采用的激光里程计单元32，其提供的激光里程计定位方法不需要预先采集点云地图，受环境变化的影响较小；同时，每帧激光点云不进行特征提取，在经过滤波和去畸变处理之后，直接用于点云的匹配定位，更加适用于特征点较少的矿区环境。
惯导数据积分单元33，用于对惯性导航数据进行预积分计算，得到第二定位数据；其中，第二定位数据为惯性导航数据在当前激光雷达数据帧与前一激光雷达数据帧之间的预积分量。
惯导数据积分单元33实时从通信系统中接收消息，进而从消息中解析出惯性导航传感器发布的惯性导航数据，进而对该惯性导航数据进行预积分计算，计算两帧雷达数据帧之间的预积分量。
为了将惯性导航数据加入统一的优化框架，实时进行惯性导航数据的预积分计算，惯导数据积分单元33以两帧雷达数据帧之间的惯性导航数据作为一个队列，计算队列中惯性导航数据对应的预积分量：位置变化量、速度变化量和姿态变化量：
其中，为位置变化量，/>为速度变化量，/>为姿态变化量，i和j表示预积分的起始时刻和结束时刻，/>表示i时刻到t时刻的姿态变化量，/>表示惯导加速度计的测量值，/>表示惯导陀螺仪的测量值。
如图6所示，激光雷达传感器的采集频率是10Hz，采样周期为t1到t2之间的时间段，具体是100ms，即每100ms采集一帧激光关键帧，惯性导航传感器的采集频率是100Hz，其在100ms之内采集了k＝10帧惯导测量值，即每10ms采集一帧惯导测量值，为了便于融合优化，本申请实施例计算两个激光关键帧之间的9帧惯导测量值所引起的运动变化量(即上述预积分量)。
同时，后端优化模块还用于实时估计惯性导航传感器中加速度计和陀螺仪的零偏数值，每帧优化之后将最新估计的零偏数值传递给惯导数据积分单元33。
GNSS位姿解算单元34，用于对卫星定位数据进行解算，得到第三定位数据；其中，第三定位数据为单帧定位信息。
应理解，GNSS位姿解算单元34或称为GNSS定位消息解析模块，第三定位数据或称为位姿矩阵、GNSS定位数据等。
在本申请实施例中，GNSS位姿解算单元34实时从通信系统中接收消息，并从该消息中解算出卫星定位数据，在解算完成之后，按照预设数据格式进行填充，填充完之后，发布位姿矩阵。
具体的，卫星定位数据包括但不限于：经纬度信息、海拔高度信息、姿态角信息、RTK解状态等。
本实施例可以将经纬度信息转换成平面直角坐标信息，然后将平面直角坐标信息、海拔高度信息和姿态角信息填充到表示位置和姿态的4*4矩阵中，得到位姿矩阵，进而将位姿矩阵送入到后端优化模块中的融合定位优化模块2。
一种可能的实现方式中，如图3所示，因子图模型包括第一帧间约束因子301、第二帧间约束因子302和单帧约束因子303。
融合定位优化模块2，还用于：
将第一定位数据作为第一帧间约束因子301，将第二定位数据作为第二帧间约束因子302，将第三定位数据作为单帧约束因子303。
上述因子图模型还包括：滑动窗口和边缘化因子304；第一帧间约束因子301、第二帧间约束因子302和单帧约束因子303均位于滑动窗口内；边缘化因子304由历史帧数据确定，并位于滑动窗口外。
应理解，第一定位数据用于表示激光里程计单元32发送的两帧之间的相对变换位姿，第二定位数据用于表示惯导数据积分单元33发送的两帧之间的预积分量，第三定位数据用于表示GNSS位姿解算单元34发送的单帧定位信息。
第一帧间约束因子301或称为雷达里程计因子、激光里程计因子等。上述第二帧间约束因子302或称为惯导帧间约束因子、惯导预积分因子等，单帧约束因子303或称为节点约束因子、GNSS定位因子。边缘化因子304或称为历史帧先验因子，不同时间节点采集到的xn均为窗口内优化帧305，窗口内优化帧305是指需要优化的关键帧节点。
具体的，融合定位优化模块2，用于：
接收激光里程计单元32发送的两帧之间的相对变换位姿，并转化为帧间约束因子。
接收惯导数据积分单元33发送的两帧之间的预积分量，并转化为惯导帧间约束因子。
接收GNSS位姿解算单元34发送的单帧定位信息，并转化为因子图模型中的节点约束因子。
为了保证融合定位优化模块2的运行效率，本实施例可以应用边缘化的方法实现一个具有固定大小的滑动窗口的因子图模型，设置在滑动窗口内的边缘化因子同时含有历史帧的数据信息，然后使用预设优化库进行迭代求解，给出的最优的当前位姿。
通过上述描述可知，融合定位优化模块2将多个传感器对应的定位数据进行融合，实现多个数据帧的统一优化求解，进而给出融合定位结果，该融合定位结果包含最优的当前位姿。
一种可能的实现方式中，如图2所示，该多传感器融合定位系统还包括数据时间同步模块4。该数据时间同步模块4用于解决多传感器采样频率不一致、数据时间不对齐的问题，进行惯导数据和GNSS数据的插值计算。
该数据时间同步模块4，用于在对各种原始数据分别进行处理之前，执行以下操作：
判断激光雷达传感器、惯性导航传感器和GNSS传感器对应的数据存储队列是否存储有新增数据。
若多个数据存储队列均有新增数据，则以当前激光雷达数据帧为目标帧，分别从惯性导航传感器和GNSS传感器对应的数据存储队列中查找与目标帧相邻的前后两帧数据，分别计算对应的时间差，并分别建立时间戳与目标帧的时间戳相同的新增帧数据，以实现对惯性导航数据和卫星定位数据的插值处理。应理解，新增帧数据可以简称为新帧数据。
具体的，本实施例对惯性导航数据和卫星定位数据进行数据插值处理。在处理过程中，判断各个传感器对应的数据存储队列是否采集到了新数据，若多个数据存储队列均有数据，则：以雷达帧为目标帧，找出其他传感器采集到的与目标帧的时间相邻的前后两帧，分别计算对应的时间差，进而建立与目标帧的时间一致的新帧数据。
本申请实施例可以实现数据时间同步，进而提高融合定位的准确度。
一种可能的实现方式中，融合定位优化模块2，还用于在将因子图模型中与失效数据对应的因子的权重值减小到第一预设权重值之后，执行以下操作：
将因子图模型中与正常数据对应的因子的权重值增大到第二预设权重值；其中，正常数据为所有原始数据中除失效数据以外的其他数据。
本申请实施例可以通过增大非失效数据对应的因子的权重值的方式，能够减小失效数据的影响，进一步提高融合定位的准确度。
一种可能的实现方式中，如图2所示，该多传感器融合定位系统还包括定位结果输出模块5。定位结果输出模块5将融合定位优化模型的最优解作为融合定位结果，并将融合定位结果封装成里程计的格式，发布到通信系统中。
例如，定位结果输出模块5将经过位姿图模型优化的当前最新位姿，封装成里程计的格式，发布到通信系统中。
综上，本实施例提供了一种多传感器融合定位系统，可以实现高精度、实时的位置和姿态估计。该系统基于激光雷达传感器、惯性导航传感器和GNSS传感器采集的原始数据，进行数据预处理、点云匹配、惯导数据积分、GNSS位姿解算、数据时间同步、传感器故障检测和融合定位优化等流程，实现多传感器数据的融合和定位精度的提高。
在上述实施例的基础上，下面结合具体的实施例对本申请技术方案进行更详细的描述。
实施例2：
图7为本申请实施例提供的一种多传感器融合定位方法的流程示意图。如图7所示，本实施例的方法，包括以下步骤：
S10、获取多种原始数据，并对各种原始数据分别进行失效识别。
其中，不同的原始数据是由不同的目标传感器采集到的用于对车辆进行融合定位的数据。
S20、在识别到所有原始数据中的至少一种为失效数据时，将因子图模型中与失效数据对应的因子的权重值减小到第一预设权重值。
其中，因子图模型包含所有原始数据对应的因子以及所有因子的权重值。
S30、对各种原始数据分别进行处理，得到各种原始数据对应的定位数据。
S40、将各种原始数据对应的定位数据作为各原始数据对应的因子，输入至因子图模型，以供因子图模型对各因子进行计算，得到车辆的融合定位结果。
其中，融合定位结果包括以下至少之一：位置、姿态和速度。
本实施例通过对各种原始数据进行失效识别，进而在某种原始数据为失效数据的情况下减小该失效数据对应的因子在因子图模型中的权重的方式，能够基于原始数据的真实有效情况，对因子图模型中的各因子的权重值进行自适应调整，能够保证因子图模型的有效性，进而提高融合定位结果的精确度。
一种可能的实现方式中，步骤S10、获取多种原始数据，并对各种原始数据分别进行失效识别，包括以下步骤：
S101、获取以下至少两种原始数据：激光雷达传感器采集到的激光点云、惯性导航传感器采集到的惯性导航数据和全球导航卫星系统GNSS传感器采集到的卫星定位数据。
S102、针对激光点云，提取激光点云的角点特征，根据激光点云的角点特征统计出激光点云的角点数量，判断激光点云的角点数量是否低于预设数量；其中，激光点云的角点数量低于预设数量时，表示激光点云数据为失效数据。
S103、针对卫星定位数据，判断GNSS传感器是否处于RTK稳定解状态；其中，GNSS传感器处于非RTK稳定解状态时，表示卫星定位数据为失效数据。
一种可能的实现方式中，S30、对各种原始数据分别进行处理，得到各种原始数据对应的定位数据，包括以下至少两项：
S301、对当前激光雷达数据帧的激光点云进行预处理，并采用激光里程计进行激光点云到局部地图的点云匹配，得到第一定位数据；其中，第一定位数据包括当前激光雷达数据帧与前一激光雷达数据帧之间的相对变换位姿。
S302、对惯性导航数据进行预积分计算，得到第二定位数据；其中，第二定位数据为惯性导航数据在当前激光雷达数据帧与前一激光雷达数据帧之间的预积分量。
S303、对卫星定位数据进行解算，得到第三定位数据；其中，第三定位数据为单帧定位信息。
一种可能的实现方式中，因子图模型包括第一帧间约束因子、第二帧间约束因子和单帧约束因子。
则在步骤S40中，将各种原始数据对应的定位数据作为各原始数据对应的因子，包括以下步骤：
S401、将第一定位数据作为第一帧间约束因子，将第二定位数据作为第二帧间约束因子，将第三定位数据作为单帧约束因子；其中，因子图模型还包括：滑动窗口和边缘化因子；第一帧间约束因子、第二帧间约束因子和单帧约束因子均位于滑动窗口内；边缘化因子由历史帧数据确定，并位于滑动窗口外。
一种可能的实现方式中，在对各种原始数据分别进行处理之前，方法还包括：
S50、判断激光雷达传感器、惯性导航传感器和GNSS传感器对应的数据存储队列是否存储有新增数据。
S60、若多个数据存储队列均有新增数据，则以当前激光雷达数据帧为目标帧，分别从惯性导航传感器和GNSS传感器对应的数据存储队列中查找与目标帧相邻的前后两帧数据，分别计算对应的时间差，并分别建立时间戳与目标帧的时间戳相同的新增帧数据，以实现对惯性导航数据和卫星定位数据的数据时间同步处理。
一种可能的实现方式中，在将因子图模型中与失效数据对应的因子的权重值减小到第一预设权重值之后，该方法还包括：
S70、将因子图模型中与正常数据对应的因子的权重值增大到第二预设权重值；其中，正常数据为所有原始数据中除失效数据以外的其他数据。
综上，本实施例提供了一种多传感器融合定位方法，可以实现高精度、实时的位置和姿态估计。该方法基于激光雷达传感器、惯性导航传感器和GNSS传感器采集的原始数据，进行数据预处理、点云匹配、惯导数据积分、GNSS位姿解算、数据时间同步、传感器故障检测和融合定位优化等流程，实现多传感器数据的融合和定位精度的提高。
实施例3：
图8为本申请实施例提供的另一种多传感器融合定位方法的流程示意图。如图8所示，该方法包括以下步骤：
S81、接收激光点云，对激光点云进行预处理。
S82、实现激光点云到局部地图的点云匹配，发布激光里程计。
S83、接收惯导数据，进行数据预积分。
S84、接收卫星定位数据，进行GNSS位姿解算。
S85、传感器数据时间同步。
S86、传感器故障检测。
S87、建立滑窗优化模型，进行融合定位优化。
S88、定位结果输出。
本实施例提供的多传感器融合定位方法，其实现原理和技术效果均与上述多传感器融合定位系统类似，此处不做赘述。
实施例4：
图9为本申请实施例提供的一种车辆的结构示意图。该车辆包括多传感器融合定位系统51；其中，多传感器融合定位系统51通过通信系统获取到多种原始数据，并在进行多传感器融合定位后，将车辆的融合定位结果发送至通信系统52。
在本实施例中，多传感器融合定位系统51的实现原理和技术效果见上述实施例1，由于本实施例提供的车辆包括多传感器融合定位系统51，因此车辆的实现原理和技术效果与之类似，此处不做赘述。
需要说明的是，本申请所涉及的用户信息和数据(包括但不限于用于分析的数据、存储的数据、展示的数据等)，均为经用户授权或者经过各方充分授权的信息和数据，并且相关数据的收集、使用和处理需要遵守相关国家和地区的相关法律法规和标准，并提供有相应的操作入口，供用户选择授权或者拒绝。
也就是说，本申请的技术方案中，所涉及的用户个人信息的收集、存储、使用、加工、传输、提供和公开等处理，均符合相关法律法规的规定，且不违背公序良俗。
根据本申请的实施例，本申请还提供了一种电子设备和一种可读存储介质。
图10为本申请实施例提供的一种电子设备的结构示意图。该电子设备包括接收器60、发送器61、至少一个处理器62和存储器63，由上述部件构成的该电子设备可以用来实施本申请上述几个具体的实施例，此处不再赘述。
本申请实施例还提供一种计算机可读存储介质，计算机可读存储介质中存储有计算机执行指令，当处理器执行计算机执行指令时，实现上述实施例中方法中的各个步骤。
本申请实施例还提供一种计算机程序产品，包括计算机程序，该计算机程序被处理器执行时实现上述实施例中方法中的各个步骤。
本申请以上描述的系统和技术的各种实施方式可以在数字电子电路系统、集成电路系统、场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、芯片上系统的系统(SOC)、负载可编程逻辑设备(CPLD)、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以包括：实施在一个或者多个计算机程序中，该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/或解释，该可编程处理器可以是专用或者通用可编程处理器，可以从存储系统、至少一个输入装置、和至少一个输出装置接收数据和指令，并且将数据和指令传输至该存储系统、该至少一个输入装置、和该至少一个输出装置。
用于实施本申请的方法的程序代码可以采用一个或多个编程语言的任何组合来编写。这些程序代码可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器或控制器，使得程序代码当由处理器或控制器执行时使流程图和/或框图中所规定的功能/操作被实施。程序代码可以完全在机器上执行、部分地在机器上执行，作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或电子设备上执行。
在本申请的上下文中，计算机可读存储介质可以是有形的介质，其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的程序。计算机可读存储介质可以是机器可读信号介质或机器可读储存介质。计算机可读存储介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备，或者上述内容的任何合适组合。计算机可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
为了提供与用户的交互，可以在计算机上实施此处描述的系统和技术，该计算机具有：用于向用户显示信息的显示装置(例如，CRT(阴极射线管)或者LCD(液晶显示器)监视器)；以及键盘和指向装置(例如，鼠标或者轨迹球)，用户可以通过该键盘和该指向装置来将输入提供给计算机。其它种类的装置还可以用于提供与用户的交互；例如，提供给用户的反馈可以是任何形式的传感反馈(例如，视觉反馈、听觉反馈、或者触觉反馈)；并且可以用任何形式(包括声输入、语音输入或者、触觉输入)来接收来自用户的输入。
可以将此处描述的系统和技术实施在包括后台部件的计算系统(例如，作为数据电子设备)、或者包括中间件部件的计算系统(例如，应用电子设备)、或者包括前端部件的计算系统(例如，具有图形用户界面或者网络浏览器的用户计算机，用户可以通过该图形用户界面或者该网络浏览器来与此处描述的系统和技术的实施方式交互)、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信(例如，通信网络)来将系统的部件相互连接。通信网络的示例包括：局域网(LAN)、广域网(WAN)和互联网。
应该理解，可以使用上面所示的各种形式的流程，重新排序、增加或删除步骤。例如，本申请公开中记载的各步骤可以并行地执行也可以顺序地执行也可以不同的次序执行，只要能够实现本申请公开的技术方案所期望的结果，本文在此不进行限制。
上述具体实施方式，并不构成对本申请保护范围的限制。本领域技术人员应该明白的是，根据设计要求和其他因素，可以进行各种修改、组合、子组合和替代。任何在本申请的原则之内所作的修改、等同替换和改进等，均应包含在本申请保护范围之内。

Claims (10)

1.一种多传感器融合定位方法，其特征在于，包括：
获取多种原始数据，并对各种原始数据分别进行失效识别；其中，不同的所述原始数据是由不同的目标传感器采集到的用于对车辆进行融合定位的数据；
在识别到所有所述原始数据中的至少一种为失效数据时，将因子图模型中与所述失效数据对应的因子的权重值减小到第一预设权重值；其中，所述因子图模型包含所有所述原始数据对应的因子以及所有因子的权重值；
对各种所述原始数据分别进行处理，得到各种所述原始数据对应的定位数据；
将各种所述原始数据对应的定位数据作为各所述原始数据对应的因子，输入至所述因子图模型，以供所述因子图模型对各所述因子进行计算，得到车辆的融合定位结果；其中，所述融合定位结果包括以下至少之一：位置、姿态和速度。
2.根据权利要求1所述的方法，其特征在于，所述获取多种原始数据，并对各种原始数据分别进行失效识别，包括：
获取以下至少两种原始数据：激光雷达传感器采集到的激光点云、惯性导航传感器采集到的惯性导航数据和全球导航卫星系统GNSS传感器采集到的卫星定位数据；
针对所述激光点云，提取所述激光点云的角点特征，根据所述激光点云的角点特征统计出激光点云的角点数量，判断所述激光点云的角点数量是否低于预设数量；其中，所述激光点云的角点数量低于预设数量时，表示所述激光点云数据为失效数据；
针对所述卫星定位数据，判断所述GNSS传感器是否处于RTK稳定解状态；其中，所述GNSS传感器处于非RTK稳定解状态时，表示所述卫星定位数据为失效数据。
3.根据权利要求2所述的方法，其特征在于，所述对各种所述原始数据分别进行处理，得到各种所述原始数据对应的定位数据，包括以下至少两项：
对当前激光雷达数据帧的激光点云进行预处理，并采用激光里程计进行所述激光点云到局部地图的点云匹配，得到第一定位数据；其中，所述第一定位数据包括当前激光雷达数据帧与前一激光雷达数据帧之间的相对变换位姿；
对惯性导航数据进行预积分计算，得到第二定位数据；其中，所述第二定位数据为所述惯性导航数据在所述当前激光雷达数据帧与所述前一激光雷达数据帧之间的预积分量；
对卫星定位数据进行解算，得到第三定位数据；其中，所述第三定位数据为单帧定位信息。
4.根据权利要求3所述的方法，其特征在于，所述因子图模型包括第一帧间约束因子、第二帧间约束因子和单帧约束因子；
则所述将各种所述原始数据对应的定位数据作为各所述原始数据对应的因子，包括：
将所述第一定位数据作为第一帧间约束因子，将所述第二定位数据作为第二帧间约束因子，将所述第三定位数据作为单帧约束因子；
所述因子图模型还包括：滑动窗口和边缘化因子；所述第一帧间约束因子、所述第二帧间约束因子和所述单帧约束因子均位于所述滑动窗口内；所述边缘化因子由历史帧数据确定，并位于所述滑动窗口外。
5.根据权利要求3所述的方法，其特征在于，在对各种所述原始数据分别进行处理之前，所述方法还包括：
判断所述激光雷达传感器、所述惯性导航传感器和所述GNSS传感器对应的数据存储队列是否存储有新增数据；
若多个所述数据存储队列均有新增数据，则以当前激光雷达数据帧为目标帧，分别从所述惯性导航传感器和所述GNSS传感器对应的数据存储队列中查找与所述目标帧相邻的前后两帧数据，分别计算对应的时间差，并分别建立时间戳与所述目标帧的时间戳相同的新增帧数据，以实现对所述惯性导航数据和所述卫星定位数据时间同步处理。
6.根据权利要求1所述的方法，其特征在于，在所述将因子图模型中与所述失效数据对应的因子的权重值减小到第一预设权重值之后，所述方法还包括：
将所述因子图模型中与正常数据对应的因子的权重值增大到第二预设权重值；其中，所述正常数据为所有所述原始数据中除失效数据以外的其他数据。
7.一种多传感器融合定位系统，其特征在于，包括：
故障检测模块，用于获取多种原始数据，并对各种原始数据分别进行失效识别；其中，不同的所述原始数据是由不同的目标传感器采集到的用于对车辆进行融合定位的数据；
融合定位优化模块，用于在识别到所有所述原始数据中的至少一种为失效数据时，将因子图模型中与所述失效数据对应的因子的权重值减小到第一预设权重值；其中，所述因子图模型包含所有所述原始数据对应的因子以及所有因子的权重值；
数据处理模块，用于对各种所述原始数据分别进行处理，得到各种所述原始数据对应的定位数据；
所述融合定位优化模块，还用于将各种所述原始数据对应的定位数据作为各所述原始数据对应的因子，输入至所述因子图模型，以供所述因子图模型对各所述因子进行计算，得到车辆的融合定位结果；其中，所述融合定位结果包括以下至少之一：位置、姿态和速度。
8.一种车辆，其特征在于，包括多传感器融合定位系统；其中，所述多传感器融合定位系统通过通信系统获取到多种原始数据，并在进行多传感器融合定位后，将车辆的融合定位结果发送至所述通信系统。
9.一种电子设备，其特征在于，包括：至少一个处理器和存储器；
所述存储器存储计算机执行指令；
所述至少一个处理器执行所述存储器存储的计算机执行指令，使得所述至少一个处理器执行如权利要求1至6任一项所述的多传感器融合定位方法。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质中存储有计算机执行指令，所述计算机执行指令被处理器执行时用于实现如权利要求1至6任一项所述的多传感器融合定位方法。
CN202311013290.9A 2023-08-11 2023-08-11 多传感器融合定位方法、系统、车辆、设备及介质 Pending CN117029853A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311013290.9A CN117029853A (zh) 2023-08-11 2023-08-11 多传感器融合定位方法、系统、车辆、设备及介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311013290.9A CN117029853A (zh) 2023-08-11 2023-08-11 多传感器融合定位方法、系统、车辆、设备及介质

Publications (1)

Publication Number Publication Date
CN117029853A true CN117029853A (zh) 2023-11-10

Family

ID=88642589

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311013290.9A Pending CN117029853A (zh) 2023-08-11 2023-08-11 多传感器融合定位方法、系统、车辆、设备及介质

Country Status (1)

Country Link
CN (1) CN117029853A (zh)

Similar Documents

Publication Publication Date Title
JP6812404B2 (ja) 点群データを融合させるための方法、装置、コンピュータ読み取り可能な記憶媒体、及びコンピュータプログラム
EP3920003A1 (en) Positioning method and apparatus, autonomous driving vehicle, electronic device and storage medium
CN111551186A (zh) 一种车辆实时定位方法、系统及车辆
CN110187375A (zh) 一种基于slam定位结果提高定位精度的方法及装置
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN113269878B (zh) 一种基于多传感器的建图方法及系统
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN114323033B (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN110851545A (zh) 地图绘制方法、装置及设备
CN112147632A (zh) 车载激光雷达感知算法的测试方法、装置、设备和介质
CN114119886A (zh) 高精地图点云重建方法、装置、车辆、设备和存储介质
CN111540027B (zh) 一种检测方法、装置、电子设备及存储介质
CN115183762A (zh) 一种机场仓库内外建图方法、系统、电子设备及介质
CN117388830A (zh) 激光雷达与惯性导航的外参标定方法、装置、设备及介质
CN115900697B (zh) 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆
CN117029853A (zh) 多传感器融合定位方法、系统、车辆、设备及介质
CN115937449A (zh) 高精地图生成方法、装置、电子设备和存储介质
CN114861725A (zh) 一种目标感知跟踪的后处理方法、装置、设备及介质
CN114281832A (zh) 基于定位结果的高精地图数据更新方法、装置和电子设备
CN114442133A (zh) 无人机定位方法、装置、设备及存储介质
CN116817928B (zh) 基于因子图优化的卫导/惯导列车多源融合定位的方法
CN117387610A (zh) 视觉式车辆定位融合系统及其方法
CN115658833A (zh) 高精地图生成方法、装置、电子设备和存储介质
CN117168449A (zh) 一种配电网工作人员定位方法、装置、设备及存储介质
CN116164777A (zh) 传感器标定方法、装置、设备及自动驾驶车辆

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