CN114111775A - 一种多传感器融合定位方法、装置、存储介质及电子设备 - Google Patents

一种多传感器融合定位方法、装置、存储介质及电子设备 Download PDF

Info

Publication number
CN114111775A
CN114111775A CN202111566110.0A CN202111566110A CN114111775A CN 114111775 A CN114111775 A CN 114111775A CN 202111566110 A CN202111566110 A CN 202111566110A CN 114111775 A CN114111775 A CN 114111775A
Authority
CN
China
Prior art keywords
pose
data
point cloud
positioning
sensor fusion
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.)
Granted
Application number
CN202111566110.0A
Other languages
English (en)
Other versions
CN114111775B (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.)
Guoqi Beijing Intelligent Network Association Automotive Research Institute Co ltd
Original Assignee
Guoqi Beijing Intelligent Network Association Automotive Research Institute 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 Guoqi Beijing Intelligent Network Association Automotive Research Institute Co ltd filed Critical Guoqi Beijing Intelligent Network Association Automotive Research Institute Co ltd
Priority to CN202111566110.0A priority Critical patent/CN114111775B/zh
Publication of CN114111775A publication Critical patent/CN114111775A/zh
Application granted granted Critical
Publication of CN114111775B publication Critical patent/CN114111775B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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
    • 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/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
    • 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
    • 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/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/13—Receivers
    • G01S19/22—Multipath-related issues
    • 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
    • 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
    • G01S19/46—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being of a radio-wave signal type
    • 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
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Abstract

本发明公开了一种多传感器融合定位方法、装置、存储介质及电子设备，多传感器融合定位方法包括：将多传感器的数据进行预处理，得到处理后的位姿数据和点云数据；根据多传感器数据进行定位初始化，得到初始化位姿；基于所述初始化位姿得到初始预测位姿，将预处理后的点云数据逐帧进行点云匹配，得到点云匹配定位结果；将多传感器预处理后的位姿数据以及所述点云匹配定位结果进行融合，得到融合定位位姿。通过实施本发明，解决了园区内由多路径效应引起的定位不准问题，支持园区、开发路段等多种场景的自动驾驶定位功能。而且还提供多传感器融合方案并根据数据的可靠性选择性使用，增加定位算法鲁棒性并且提高了定位精度。

Description

一种多传感器融合定位方法、装置、存储介质及电子设备
技术领域
本发明涉及自动驾驶领域，具体涉及一种多传感器融合定位的方法、装置、存储介质及电子设备。
背景技术
随着人工智能行业的兴起，汽车行业发生了翻天覆地的变化。自动驾驶相关技术的应用和落地，使得汽车已经不仅仅是一个机械结构，软件算法也成为其重要的组成部件。其中，定位功能是决策控制的前提条件，更是整个自动驾驶系统运行的基石。
传统定位模块中大多使用全球导航卫星系统(Global Navigation SatelliteSystem，GNSS)和惯性测量单元(Inertial Measurement Unit，IMU)进行定位。IMU会有累计误差的影响，而在楼宇密集的园区中，由于多路径效应的影响，实时差分定位(Real-timekinematic，RTK)无法提供准确的固定解，而导致GNSS定位输出会出现偏差。为此，如何在楼宇密集的场景中提供准确定位成为了急需解决的问题。
发明内容
有鉴于此，本发明实施例提供了一种多传感器融合定位的方法、装置、存储介质及电子设备，以解决园区内由多路径效应引起的定位不准问题，支持园区、开发路段等多种场景的自动驾驶定位功能。
本发明提出的技术方案如下：
本发明实施例第一方面提供一种多传感器融合定位方法，包括：将多传感器的数据进行预处理，得到处理后的位姿数据和点云数据；根据所述多传感器数据进行定位初始化得到初始化位姿；基于所述初始化位姿得到初始预测位姿，将预处理后的点云数据逐帧进行点云匹配，得到点云匹配定位结果；将所述多传感器预处理后的位姿数据以及所述点云匹配定位结果进行融合，得到融合定位位姿。
可选地，多传感器融合定位方法还包括：将所述融合定位位姿进行位姿转换，得到高频定位信息。
可选地，所述多传感器包括：全球导航卫星系统、惯性测量单元以及轮速计；将多传感器的数据进行预处理，包括：基于激光雷达时间戳，将全球导航卫星系统、惯性测量单元以及轮速计的数据进行时间对齐；根据所述惯性测量单元的角速度和所述轮速计的线速度计算运动位姿；将激光雷达的点云数据进行点云补偿。
可选地，所述基于所述初始化位姿得到初始预测位姿，将预处理后的点云数据逐帧进行点云匹配，得到点云匹配定位结果，包括：判断所述全球导航卫星系统的信号强度；当所述信号强度达到预设阈值时，根据所述全球导航卫星系统的数据进行初始化，得到初始化位姿；当所述信号强度未达到预设阈值时，根据当前帧点云的几何信息在点云地图上筛选候选区域，再结合点云匹配结果，得到初始化位姿。
可选地，所述将多传感器预处理后的位姿数据以及所述点云匹配定位结果进行融合，得到融合定位位姿，包括：将多传感器预处理后的位姿数据以及点云匹配定位结果作为约束建立优化函数；根据图优化算法对所述优化函数进行非线性求解，得到融合定位位姿。
可选地，所述将多传感器的位姿数据以及所述点云匹配定位结果作为约束建立优化函数中，所述约束还包括：高度因子、方位角因子和航位推算因子。
可选地，将所述融合定位位姿进行位姿转换，得到高频定位信息，包括：根据所述融合定位位姿和对应时间戳的组合导航系统数据计算转换矩阵；根据所述转换矩阵和组合导航系统数据计算高频定位信息。
本发明实施例第二方面提供一种多传感器融合定位装置，包括：数据预处理模块，用于将多传感器的数据进行预处理，得到处理后的位姿数据和点云数据；定位初始化模块，用于根据多传感器数据进行定位初始化得到初始化位姿；点云匹配模块，用于基于初始化位姿得到初始预测位姿，将预处理后的点云数据逐帧进行点云匹配，得到点云匹配定位结果；位姿融合模块，用于将多传感器的预处理后的位姿数据以及所述点云匹配定位结果进行融合，得到融合定位位姿。
本发明实施例第三方面提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机指令，所述计算机指令用于使所述计算机执行如本发明实施例第一方面及第一方面任一项所述的电动汽车电机控制器的布局优化方法。
本发明实施例第四方面提供一种电子设备，包括：存储器和处理器，所述存储器和所述处理器之间互相通信连接，所述存储器存储有计算机指令，所述处理器通过执行所述计算机指令，从而执行如本发明实施例第一方面及第一方面任一项所述的电动汽车电机控制器的布局优化方法。
本发明提供的技术方案，具有如下效果：
本发明实施例提供的多传感器融合定位的方法、装置、存储介质及电子设备，通过将多传感器的数据进行预处理，得到处理后的位姿数据和点云数据；根据多传感器数据进行定位初始化得到初始化位姿；基于初始化位姿得到初始预测位姿，将预处理后的点云数据逐帧进行点云匹配，得到点云匹配定位结果；将多传感器预处理后的位姿数据以及所述点云匹配定位结果进行融合，得到融合定位位姿。该方法采用多传感器融合策略，利用传感器之间的互补性解决园区内由多路径效应引起的定位不准问题，支持园区、开发路段等多种场景的自动驾驶定位功能。而且还提供多传感器融合方案并根据数据的可靠性选择性使用，增加定位算法鲁棒性并且提高了定位精度。
附图说明
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是根据本发明实施例的多传感器融合定位方法的流程图；
图2是根据本发明实施例的将多传感器的数据进行预处理的流程图；
图3是根据本发明实施例的多传感器融合定位方法中位姿融合优化涉及约束的因子图；
图4是根据本发明实施例的多传感器融合定位方法的具体流程图；
图5是根据本发明实施例的多传感器融合定位方法的结构框图；
图6是根据本发明实施例的多传感器融合定位方法在大族企业湾运行效果示意图；
图7是根据本发明实施例提供的计算机可读存储介质的结构示意图；
图8是根据本发明实施例提供的电子设备的结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明实施例提供一种多传感器融合定位方法，如图1所示，该方法包括如下步骤：
步骤S101：将多传感器的数据进行预处理，得到处理后的位姿数据和点云数据。在一实施例中，多传感器包括：全球导航卫星系统、惯性测量单元、轮速计以及激光雷达。
其中，在本实施例中，多传感器使用全球导航卫星系统、惯性测量单元、轮速计以及激光雷达，在实际应用中，使用的多传感器包括但不限于这四种传感器。将多传感器的数据先进行预处理，可以使得后续定位结果更加准确。
步骤S102：根据多传感器数据进行定位初始化得到初始化位姿。
其中，进行定位初始化时，可以使用GNSS信息或者利用描述子信息在点云地图上寻找几何信息相似的位置，再通过点云匹配进行二次确认，得到初始化位姿。
步骤S103：基于初始化位姿得到初始预测位姿，将预处理后的点云数据逐帧进行点云匹配，得到点云匹配定位结果。
其中，点云匹配的目的是寻找输入点云和目标点云的相对位姿关系，将当前帧点云输入到目标点云中，寻找输入点云和目标点云的相对位姿关系，目标点云是定位子图，是点云地图的一部分，将初始化位姿输入到点云地图中，得到初始化位姿与点云地图的转换关系，就得到点云匹配位姿输出。由于全部点云地图比较大，匹配时间很长，所以截取当前位姿附近的点云为定位子图，即可获得当前帧点云与点云地图的转换关系，即为点云匹配位姿输出。每当定位位姿要驶出定位子图时，则以定位位姿为中心点，一定阈值半径重新截取新的定位子图。具体匹配方法包括但不限于NDT、ICP等，也可以使用NDT＿OMP等多线程算法或GPU等并行计算工具以加快匹配速度。
步骤S104：将多传感器预处理后的位姿数据以及点云匹配定位结果进行融合，得到融合定位位姿。
其中，根据点云匹配结果，将多传感器预处理后的位姿数据进行位姿融合，融合定位位姿主要融合全球导航卫星系统、惯性测量单元、轮速计预处理后的位姿数据以及激光雷达的点云匹配结果。利用各个传感器提供的位姿数据，求解最优的位姿。
本发明实施例提供的多传感器融合定位方法，通过将多传感器的数据进行预处理，得到处理后的位姿数据和点云数据；根据多传感器数据进行定位初始化得到初始化位姿；基于初始化位姿得到初始预测位姿，将预处理后的点云数据逐帧进行点云匹配，得到点云匹配定位结果；将多传感器预处理后的位姿数据以及点云匹配定位结果进行融合，得到融合定位位姿。该定位方法采用多传感器融合策略，利用传感器之间的互补性解决园区内由多路径效应引起的定位不准问题，支持园区、开发路段等多种场景的自动驾驶定位功能。而且还提供多传感器融合方案并根据数据的可靠性选择性使用，增加定位算法鲁棒性并且提高了定位精度。
在一实施例中，多传感器融合定位方法还包括：将融合定位位姿进行位姿转换，得到高频定位信息。具体地，得到高频定位信息的步骤具体包括：根据融合定位位姿和对应时间戳的组合导航系统数据计算转换矩阵；根据转换矩阵和组合导航系统数据计算高频定位信息。
其中，位姿融合的位姿输出是基于激光雷达输出频率的，目前现有的激光雷达频率大多在5-10Hz，频率比较低，所以融合的位姿信息频率比较低，不能满足决策规划的要求，需要进行位姿转换输出高频的定位信息。位姿转换的作用即为接收低频的融合定位位姿与高频的多传感器数据，在每次融合定位位姿更新时，计算多传感器位姿到融合定位位姿的转换矩阵并且利用转换矩阵与高频多传感器数据获取高频的融合定位数据。
例如，可以采用数据插值的方式，由两个部分转换矩阵更新，实现高频定位输出。具体地，每当得到融合定位数据
Figure BDA0003420844080000071
找到其对应时间戳的多传感器数据
Figure BDA0003420844080000072
计算相应的转换矩阵。对于下一帧融合定位数据
Figure BDA0003420844080000073
之前的所有多传感器的数据
Figure BDA0003420844080000074
均乘上相应的转换矩阵
Figure BDA0003420844080000075
即获得高频定位信息。
在一实施例中，如图2所示，上述步骤S101将多传感器的数据进行预处理，得到处理后的位姿数据和点云数据具体包括如下步骤：
步骤S201：基于激光雷达时间戳，将全球导航卫星系统、惯性测量单元以及轮速计的数据进行时间对齐。
其中，在做数据预处理时，先以激光雷达时间戳为基准对全球导航卫星系统、惯性测量单元以及轮速计的数据做线性插值，使全球导航卫星系统、惯性测量单元以及轮速计与激光雷达时间对齐。具体采用如下公式计算，
Figure BDA0003420844080000076
以全球导航卫星系统的数据为例，如全球导航卫星系统的第i个数据与第i+1个数据之间存在一帧激光数据。第i个数据时间为ti，其测量数据为vi。第i+1个数据时间为ti+1，其测量数据为vi+1，激光点云数据时间为tj(ti≤tj≤ti+1)。如上述公式所示，vj即为对齐激光雷达时间戳tj的全球导航卫星系统的数据。
步骤S202：根据惯性测量单元的角速度和轮速计的线速度计算运动位姿。
具体地，根据惯性测量单元的角速度和轮速计的线速度进行一段时间内相对运动位姿的求解，得到的运动位姿可作为点云匹配时的初始位姿。
其中，惯性测量单元的角速度与轮速计的线速度可表示为下式。
Figure BDA0003420844080000081
其中wk与vk分别表示角速度与线速度，bg表示陀螺仪的偏置，可通过标定得到。在不考虑车辆侧向滑移情况下，轮速计输出仅包含x方向速度。对惯性测量单元的角速度的预积分可通过下式表示。
Figure BDA0003420844080000082
Figure BDA0003420844080000083
其中Rz、Ry、Rx分别表示绕z轴、y轴与x轴旋转一定角度所对应的旋转矩阵。ΔRij，Δtij为i到j时刻姿态与位移的预积分结果。
步骤S203：将激光雷达的点云数据进行点云补偿。
其中，在实际应用中未经过运动补偿的点云和真实世界的物理位置存在一定畸变，车辆运动越快，畸变越大。对于激光雷达，尤其是机械旋转式激光雷达，由于扫描到的每一个点时间戳不一样，当激光雷达载体运动时，得到的三维点云反应的不仅仅是障碍物的位置信息，还包括了激光雷达本身的运行信息，包括平移量和旋转量。故为了去除畸变减少点云匹配时的误差，得到障碍物的真实坐标信息，需要采用如下公式对点云进行运动补偿。
Figure BDA0003420844080000091
其中t0为激光雷达点云发布时间，通常为第一个激光点或者最后一个激光点的采集时间，ti为第i个激光点的真实采集时间，Pi为第i个激光点的位置坐标。
Figure BDA0003420844080000092
为ti时刻激光雷达的位姿，
Figure BDA0003420844080000093
为t0时刻激光雷达的位姿，
Figure BDA0003420844080000094
为运动补偿后第i个激光点的位置坐标。
在一实施例中，上述步骤103基于初始化位姿得到初始预测位姿，将预处理后的点云数据逐帧进行点云匹配，得到点云匹配定位结果，具体包括如下步骤：
判断全球导航卫星系统的信号强度；当信号强度达到预设阈值时，根据全球导航卫星系统的数据进行初始化，得到初始化位姿；当信号强度未达到预设阈值时，根据当前帧点云的几何信息在点云地图上筛选候选区域，再结合点云匹配结果，得到初始化位姿。
其中，定位初始化部分首先判断全球导航卫星系统的信号强度，在实际使用时为该信号强度定义一个预设阈值，根据预设阈值判断信号强度的强弱。具体可根据全球导航卫星系统的信号强度分为两种方案：当信号强度达到预设阈值时，可直接使用全球导航卫星系统的数据进行初始化，得到初始化位姿；当信号强度未达到预设阈值时，可以通过当前帧点云的几何特征在点云地图上选择几何特征相似的候选区域，再通过点云匹配进行初始化。几何特征的提取方式可以选择M2DP、ScanContext等方法，点云地图上不同位置的几何特征描述子可以在建立点云地图时一并保存。由于单帧匹配有一定的随机性和偶然性，使用几何特征匹配进行初始化时可以进行多帧动态匹配，以保证初始化位姿的正确性。
本实施例提供了全局地图初始化方案，在全球导航卫星系统信号不好的情况下，也可以在任意位置进行初始化。
在一实施例中，上述步骤104将多传感器预处理后的位姿数据以及点云匹配定位结果进行融合，得到融合定位位姿，如图3所示，具体包括如下步骤：将多传感器预处理后的位姿数据以及点云匹配定位结果作为约束建立优化函数；根据图优化算法对优化函数进行非线性求解，得到融合定位位姿。并且约束还包括：高度因子(Height因子)、方位角因子(PR因子)和航位推算因子(DR因子)。
其具体地，通过获取各个传感器的位姿数据，作为运动约束，对位姿数据进行融合，但是考虑到使用场景与车辆动力学模型，还可以添加高度与角度等方面的运动约束。将各项约束构成的误差项建立优化函数，通过图优化的方式进行非线性求解，求解器包括但不限于Ceres、g20、gtsam。图是由节点和边构成，位姿构成节点(node)，位姿之间的关系构成边(edge)。比如点云匹配提供的两帧间相对位姿约束和由惯性测量单元与轮速计提供的角速度与线速度进行航位推算得到的约束都是边。每一条边都是一种约束，不同约束的权重不同，通过数据的方差获得。所有的约束会构成一个误差方程，图优化的求解过程，就是调整位姿去尽量满足这些边构成的约束，使得误差最小，这时得到的位姿就是最优位姿。如图3所示为位姿融合优化涉及约束的因子图，其中，xk表示第k帧融合位姿，xk+1表示第k+1帧融合位姿；Lidar因子表示点云匹配提供的两帧间相对位姿约束；GPS因子表示由全球导航卫星系统提供当前帧的全局位置约束，防止位姿积累误差过大。可以通过全球导航卫星系统数据方差及状态值判断全球导航卫星系统信号强度，如果全球导航卫星系统信号不好，可以不添加GPS因子；Height因子表示用于约束全局关键帧的高度变化，减少定位位姿的高度漂移。可根据实际使用场景选择性添加；PR因子表示用于约束位姿的俯仰角与横滚角变化。可根据实际使用场景选择性添加；DR因子表示由惯性测量单元与轮速计提供的角速度与线速度进行航位推算得到的约束。
本发明实施例提供的多传感器融合定位方法，具体流程如图4所示：首先对多传感器做数据预处理，其中先将全球导航卫星系统、惯性测量单元、轮速计以激光雷达的时间戳为基准做时间对其，其次根据惯性测量单元的角速度和轮速计的线速度做运动状态估计，并对激光雷达做运动补偿；然后进行定位初始化，使用全球导航卫星系统信息或者利用描述子信息在点云地图上寻找几何信息相似的位置，再通过点云匹配进行二次确认，得到初始化位姿，定位初始化完成以后即可逐帧进行点云匹配；最后融合全球导航卫星系统、惯性测量单元、轮速计和激光雷达的数据，并优化涉及约束的因子，得到融合定位的位姿数据，并根据融合位姿关系将位姿数据转换成高频位姿信息。
本发明实施例提供的多传感器融合定位方法，采用多传感器融合策略，利用传感器之间的互补性，解决了园区场景由于多路径效应导致的GNSS数据不准确的问题，同时也保证了在开发道路等场景的使用。并且提供全局初始化方案，避免了在特定位置初始化的限制以及对于全球导航卫星信号的强依赖性。在对多传感器的位姿数据进行融合时，采用图优化方法进行数据融合，不仅考虑了各传感器的数据约束，还可以选择性添加运动模型、角度、高度等约束，并且根据各约束的可靠性调整权重，提高了输出结果的准确性和算法的鲁棒性，还提供了位姿插值方案，可以实时输出高频位姿信息。本发明基于点云地图实现的多传感器位姿融合，使用点云地图进行点云匹配，消除了无全局地图时，单帧匹配的位置偏移影响，提高了点云匹配的精度。
本发明实施例还提供一种多传感器融合定位装置，如图5所示，该装置包括：
数据预处理模块101，用于将多传感器的数据进行预处理，得到处理后的位姿数据。详细内容参见上述方法实施例中步骤S101的相关描述，在此不再赘述。
定位初始化模块102，用于根据多传感器数据进行定位初始化得到初始化位姿，详细内容参见上述方法实施例中步骤S102的相关描述，在此不再赘述。
点云匹配模块103，用于基于初始化位姿得到初始预测位姿，将预处理后的点云数据逐帧进行点云匹配，得到点云匹配定位结果，详细内容参见上述方法实施例中步骤S103的相关描述，在此不再赘述。
位姿融合模块104，用于将多传感器的预处理后的位姿数据以及点云匹配定位结果进行融合，得到融合定位位姿，详细内容参见上述方法实施例中步骤S104的相关描述，在此不再赘述。
如图6所示，为本方案在大族企业湾运行效果示意图。
本发明实施例提供的多传感器融合定位装置通过将多传感器的数据进行预处理，得到处理后的位姿数据和点云数据；根据多传感器数据进行定位初始化得到初始化位姿；基于初始化位姿得到初始预测位姿，将预处理后的点云数据逐帧进行点云匹配，得到点云匹配定位结果；将多传感器预处理后的位姿数据以及点云匹配定位结果进行融合，得到融合定位位姿。该方法采用多传感器融合策略，利用传感器之间的互补性解决园区内由多路径效应引起的定位不准问题，支持园区、开发路段等多种场景的自动驾驶定位功能。而且还提供多传感器融合方案并根据数据的可靠性选择性使用，增加定位算法鲁棒性并且提高了定位精度。
本发明实施例还提供一种存储介质，如图7所示，其上存储有计算机程序601，该指令被处理器执行时实现上述实施例中电动汽车电机控制器的布局优化方法的步骤。该存储介质上还存储有音视频流数据，特征帧数据、交互请求信令、加密数据以及预设数据大小等。其中，存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)、随机存储记忆体(Random Access Memory，RAM)、快闪存储器(Flash Memory)、硬盘(Hard Disk Drive，缩写：HDD)或固态硬盘(Solid-State Drive，SSD)等；存储介质还可以包括上述种类的存储器的组合。
本领域技术人员可以理解，实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)、随机存储记忆体(RandomAccessMemory，RAM)、快闪存储器(Flash Memory)、硬盘(Hard Disk Drive，缩写：HDD)或固态硬盘(Solid-StateDrive，SSD)等；存储介质还可以包括上述种类的存储器的组合。
本发明实施例还提供了一种电子设备，如图8所示，该电子设备可以包括处理器51和存储器52，其中处理器51和存储器52可以通过总线或者其他方式连接，图8中以通过总线连接为例。
处理器51可以为中央处理器(Central Processing Unit，CPU)。处理器51还可以为其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等芯片，或者上述各类芯片的组合。
存储器52作为一种非暂态计算机可读存储介质，可用于存储非暂态软件程序、非暂态计算机可执行程序以及模块，如本发明实施例中的对应的程序指令/模块。处理器51通过运行存储在存储器52中的非暂态软件程序、指令以及模块，从而执行处理器的各种功能应用以及数据处理，即实现上述方法实施例中的电动汽车电机控制器的布局优化方法。
存储器52可以包括存储程序区和存储数据区，其中，存储程序区可存储操作装置、至少一个功能所需要的应用程序；存储数据区可存储处理器51所创建的数据等。此外，存储器52可以包括高速随机存取存储器，还可以包括非暂态存储器，例如至少一个磁盘存储器件、闪存器件或其他非暂态固态存储器件。在一些实施例中，存储器52可选包括相对于处理器51远程设置的存储器，这些远程存储器可以通过网络连接至处理器51。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
一个或者多个模块存储在存储器52中，当被处理器51执行时，执行如图1-4所示实施例中的多传感器位姿融合方法。
上述电子设备具体细节可以对应参阅图1至图4所示的实施例中对应的相关描述和效果进行理解，此处不再赘述。
虽然结合附图描述了本发明的实施例，但是本领域技术人员可以在不脱离本发明的精神和范围的情况下做出各种修改和变型，这样的修改和变型均落入由所附权利要求所限定的范围之内。

Claims (10)

1.一种多传感器融合定位方法，其特征在于，包括：
将多传感器的数据进行预处理，得到处理后的位姿数据和点云数据；
根据所述多传感器数据进行定位初始化得到初始化位姿；
基于所述初始化位姿得到初始预测位姿，将预处理后的点云数据逐帧进行点云匹配，得到点云匹配定位结果；
将所述多传感器预处理后的位姿数据以及所述点云匹配定位结果进行融合，得到融合定位位姿。
2.根据权利要求1所述的多传感器融合定位方法，其特征在于，还包括：
将所述融合定位位姿进行位姿转换，得到高频定位信息。
3.根据权利要求1所述的多传感器融合定位方法，其特征在于，所述多传感器包括：全球导航卫星系统、惯性测量单元以及轮速计；
将多传感器的数据进行预处理，包括：
基于激光雷达时间戳，将全球导航卫星系统、惯性测量单元以及轮速计的数据进行时间对齐；
根据所述惯性测量单元的角速度和所述轮速计的线速度计算运动位姿；
将激光雷达的点云数据进行点云补偿。
4.根据权利要求3所述的多传感器融合定位方法，其特征在于，所述基于初始化位姿得到初始预测位姿，将预处理后的点云数据逐帧进行点云匹配，得到点云匹配定位结果，包括：
判断所述全球导航卫星系统的信号强度；
当所述信号强度达到预设阈值时，根据所述全球导航卫星系统的数据进行初始化，得到初始化位姿；
当所述信号强度未达到预设阈值时，根据当前帧点云的几何信息在点云地图上筛选候选区域，再结合点云匹配结果，得到初始化位姿。
5.根据权利要求4所述的多传感器融合定位方法，其特征在于，所述将多传感器预处理后的位姿数据以及所述点云匹配定位结果进行融合，得到融合定位位姿，包括：
将多传感器预处理后的位姿数据以及点云匹配定位结果作为约束建立优化函数；
根据图优化算法对所述优化函数进行非线性求解，得到融合定位位姿。
6.根据权利要求5所述的多传感器融合定位方法，其特征在于，所述将多传感器的位姿数据以及所述点云匹配定位结果作为约束建立优化函数中，所述约束还包括：高度因子、方位角因子和航位推算因子。
7.根据权利要求2所述的多传感器融合定位方法，其特征在于，将所述融合定位位姿进行位姿转换，得到高频定位信息，包括：
根据所述融合定位位姿和对应时间戳的组合导航系统数据计算转换矩阵；
根据所述转换矩阵和组合导航系统数据计算高频定位信息。
8.一种多传感器融合定位装置，其特征在于，包括：
数据预处理模块，用于将多传感器的数据进行预处理，得到处理后的位姿数据和点云数据；
定位初始化模块，用于根据多传感器数据进行定位初始化得到初始化位姿；
点云匹配模块，用于基于初始化位姿得到初始预测位姿，将预处理后的点云数据逐帧进行点云匹配，得到点云匹配定位结果；
位姿融合模块，用于将多传感器的预处理后的位姿数据以及所述点云匹配定位结果进行融合，得到融合定位位姿。
9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机指令，所述计算机指令用于使所述计算机执行如权利要求1－7任一项所述的多传感器融合定位的方法。
10.一种电子设备，其特征在于，包括：存储器和处理器，所述存储器和所述处理器之间互相通信连接，所述存储器存储有计算机指令，所述处理器通过执行所述计算机指令，从而执行如权利要求1－7任一项所述的多传感器融合定位方法。
CN202111566110.0A 2021-12-20 2021-12-20 一种多传感器融合定位方法、装置、存储介质及电子设备 Active CN114111775B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111566110.0A CN114111775B (zh) 2021-12-20 2021-12-20 一种多传感器融合定位方法、装置、存储介质及电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111566110.0A CN114111775B (zh) 2021-12-20 2021-12-20 一种多传感器融合定位方法、装置、存储介质及电子设备

Publications (2)

Publication Number Publication Date
CN114111775A true CN114111775A (zh) 2022-03-01
CN114111775B CN114111775B (zh) 2024-03-29

Family

ID=80361550

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111566110.0A Active CN114111775B (zh) 2021-12-20 2021-12-20 一种多传感器融合定位方法、装置、存储介质及电子设备

Country Status (1)

Country Link
CN (1) CN114111775B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115060276A (zh) * 2022-06-10 2022-09-16 江苏集萃清联智控科技有限公司 一种多环境适应性自动驾驶车辆定位设备、系统及方法
CN116222544A (zh) * 2023-05-09 2023-06-06 浙江大学湖州研究院 一种面向饲养场的投料车自动导航定位方法及装置
CN117346768A (zh) * 2023-11-03 2024-01-05 昆明理工大学 同时适用于室内外的多传感器融合感知定位方法
CN117471513A (zh) * 2023-12-26 2024-01-30 合众新能源汽车股份有限公司 一种车辆定位方法、定位装置、电子设备及存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180356526A1 (en) * 2017-06-13 2018-12-13 TuSimple Sensor calibration and time method for ground truth static scene sparse flow generation
CN110879400A (zh) * 2019-11-27 2020-03-13 炬星科技（深圳）有限公司 激光雷达与imu融合定位的方法、设备及存储介质
CN111611855A (zh) * 2020-04-17 2020-09-01 广东电网有限责任公司 一种变电站三维可视化机器人智能巡检系统
CN111983636A (zh) * 2020-08-12 2020-11-24 深圳华芯信息技术股份有限公司 位姿融合方法、系统、终端、介质以及移动机器人
CN113269878A (zh) * 2021-05-26 2021-08-17 上海新纪元机器人有限公司 一种基于多传感器的建图方法及系统
CN113721248A (zh) * 2021-08-30 2021-11-30 浙江吉利控股集团有限公司 一种基于多源异构传感器的融合定位方法及系统

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180356526A1 (en) * 2017-06-13 2018-12-13 TuSimple Sensor calibration and time method for ground truth static scene sparse flow generation
CN110879400A (zh) * 2019-11-27 2020-03-13 炬星科技（深圳）有限公司 激光雷达与imu融合定位的方法、设备及存储介质
CN111611855A (zh) * 2020-04-17 2020-09-01 广东电网有限责任公司 一种变电站三维可视化机器人智能巡检系统
CN111983636A (zh) * 2020-08-12 2020-11-24 深圳华芯信息技术股份有限公司 位姿融合方法、系统、终端、介质以及移动机器人
CN113269878A (zh) * 2021-05-26 2021-08-17 上海新纪元机器人有限公司 一种基于多传感器的建图方法及系统
CN113721248A (zh) * 2021-08-30 2021-11-30 浙江吉利控股集团有限公司 一种基于多源异构传感器的融合定位方法及系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
戴福全，等: "基于视觉引导的机器人抓取分类系统设计", 福建工程学院学报, vol. 18, no. 6, pages 530 - 534 *

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115060276A (zh) * 2022-06-10 2022-09-16 江苏集萃清联智控科技有限公司 一种多环境适应性自动驾驶车辆定位设备、系统及方法
CN115060276B (zh) * 2022-06-10 2023-05-12 江苏集萃清联智控科技有限公司 一种多环境适应性自动驾驶车辆定位设备、系统及方法
CN116222544A (zh) * 2023-05-09 2023-06-06 浙江大学湖州研究院 一种面向饲养场的投料车自动导航定位方法及装置
CN116222544B (zh) * 2023-05-09 2023-08-04 浙江大学湖州研究院 一种面向饲养场的投料车自动导航定位方法及装置
CN117346768A (zh) * 2023-11-03 2024-01-05 昆明理工大学 同时适用于室内外的多传感器融合感知定位方法
CN117346768B (zh) * 2023-11-03 2024-04-19 昆明理工大学 同时适用于室内外的多传感器融合感知定位方法
CN117471513A (zh) * 2023-12-26 2024-01-30 合众新能源汽车股份有限公司 一种车辆定位方法、定位装置、电子设备及存储介质
CN117471513B (zh) * 2023-12-26 2024-03-15 合众新能源汽车股份有限公司 一种车辆定位方法、定位装置、电子设备及存储介质

Also Published As

Publication number Publication date
CN114111775B (zh) 2024-03-29

Similar Documents

Publication Publication Date Title
JP7186607B2 (ja) 電子地図を更新する方法、装置、およびコンピュータ読み取り可能な記憶媒体
US11802769B2 (en) Lane line positioning method and apparatus, and storage medium thereof
CN108732603B (zh) 用于定位车辆的方法和装置
CN114111775B (zh) 一种多传感器融合定位方法、装置、存储介质及电子设备
CN110146909B (zh) 一种定位数据处理方法
CN110033489B (zh) 一种车辆定位准确性的评估方法、装置及设备
CN109901138B (zh) 激光雷达标定方法、装置、设备和存储介质
CN109410735B (zh) 反射值地图构建方法和装置
US10996072B2 (en) Systems and methods for updating a high-definition map
WO2015173034A1 (en) Method and system for determining a position relative to a digital map
CN111650598A (zh) 一种车载激光扫描系统外参标定方法和装置
CN113989451B (zh) 高精地图构建方法、装置及电子设备
CN114252082B (zh) 车辆定位方法、装置和电子设备
CN114279453B (zh) 基于车路协同的自动驾驶车辆定位方法、装置及电子设备
CN110243364B (zh) 无人机航向确定方法、装置、无人机及存储介质
EP3789797B1 (en) Method and apparatus for evaluating data, device, and computer-readable storage medium
US20200249695A1 (en) Method for localizing a vehicle
CN114323050A (zh) 车辆定位方法、装置和电子设备
JP2019174191A (ja) データ構造、情報送信装置、制御方法、プログラム及び記憶媒体
CN116399324A (zh) 建图方法、装置及控制器、无人驾驶车辆
JP6819441B2 (ja) 物標位置推定方法及び物標位置推定装置
CN113503883B (zh) 采集用于构建地图的数据的方法、存储介质及电子设备
CN115494533A (zh) 车辆定位方法、装置、存储介质及定位系统
CN114755663A (zh) 车辆传感器的外参校准方法、装置及计算机可读存储介质
WO2019188874A1 (ja) データ構造、情報処理装置、及び地図データ生成装置

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