CN113984044A - 一种基于车载多感知融合的车辆位姿获取方法及装置 - Google Patents

一种基于车载多感知融合的车辆位姿获取方法及装置 Download PDF

Info

Publication number
CN113984044A
CN113984044A CN202111170986.3A CN202111170986A CN113984044A CN 113984044 A CN113984044 A CN 113984044A CN 202111170986 A CN202111170986 A CN 202111170986A CN 113984044 A CN113984044 A CN 113984044A
Authority
CN
China
Prior art keywords
pose
vehicle
imu
gps
difference value
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
CN202111170986.3A
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.)
Hangzhou Hopechart Iot Technology Co ltd
Original Assignee
Hangzhou Hopechart Iot Technology 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 Hangzhou Hopechart Iot Technology Co ltd filed Critical Hangzhou Hopechart Iot Technology Co ltd
Priority to CN202111170986.3A priority Critical patent/CN113984044A/zh
Publication of CN113984044A publication Critical patent/CN113984044A/zh
Pending legal-status Critical Current

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
    • 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/20—Instruments for performing navigational calculations
    • 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

本发明提供一种基于车载多感知融合的车辆位姿获取方法及装置，包括：由车辆的GPS和激光雷达分别获取车辆当前时刻位姿；计算各个位姿与上一时刻相应位姿的位姿差值，并得到车辆的IMU的位姿预测值；当前时刻GPS数据若更新，在GPS的位姿差值与IMU的位姿差值的差值小于预设阈值时设置GPS的观测噪声矩阵；若未更新或差值大于阈值，设置激光雷达的观测噪声矩阵；根据观测噪声矩阵计算GPS的位姿与IMU的位姿预测值，或激光雷达的位姿与IMU的位姿预测值，作为当前时刻车辆位姿的权重；根据权重更新当前时刻车辆位姿，并纠正IMU的当前时刻位姿。用以解决现有技术中采用单一传感器为车辆进行定位误差较大的缺陷，利用多传感器融合，实现定位精度和鲁棒性的提高。

Description

一种基于车载多感知融合的车辆位姿获取方法及装置
技术领域
本发明涉及自动驾驶技术领域，尤其涉及一种基于车载多感知融合的车辆位姿获取方法及装置。
背景技术
自动驾驶技术的核心之一是高精定位，即在道路上实现车辆的自动驾驶时，需要车辆本身的位置达到厘米级的定位精度。高精定位的精度受到传感器的噪声、卫星导航的多路径传播效应、车辆自身定位问题，以及车辆运动学问题等诸多因素的影响，这些问题都会导致误差的积累，而当误差积累到一定程度，得到的最终位姿则不可信。即便是所使用的传感器性能很好，也仍旧存在自身的误差，并会受到环境影响。
传感器将物理世界按照一定的数据编码，然后将编码后的数据传送给感知模块，则感知模块可以通过相关的算法提取物理世界模型。定位模块就是利用感知模块的传感器来确定车辆在世界坐标系中的位置。其中，定位模块用到的核心传感器有实时动态卫星定位系统RTK、全球定位系统GPS、惯性测量单元IMU、激光雷达LiDAR、相机Camera等。定位模块利用这些数据，通过定位算法输出车辆在全球坐标系中的位置。理想条件下，定位模块利用RTK或GPS系统就可以对无人车进行导航及定位。然而，RTK或GPS系统是通过计算电磁波的传播时间来判断车辆的位置，在现实应用中电磁波的传播会因为大气层、高楼大厦、天气等因素产生时间误差，这意味着计算的距离数据不够准确，从而导致无法确定无人车处于哪个位置。而事实上，无人车在道路中的定位要求误差小于50cm，超过该数据，定位模块就无法用于实现自动驾驶。
因此，需要一种能够解决这些误差问题以及单个传感器不够准确的方法来用于无人车的定位。
发明内容
本发明提供一种基于车载多感知融合的车辆位姿获取方法及装置，用以解决现有技术中采用单一传感器为车辆进行定位误差较大的缺陷，利用冗余传感器，实现定位精度和鲁棒性的提高。
本发明提供一种基于车载多感知融合的车辆位姿获取方法，包括：
由车辆的GPS和激光雷达分别获取所述车辆的当前时刻位姿；
计算各个所述位姿与上一时刻的相应位姿间的位姿差值；并将所述车辆的IMU在上一时刻的位姿与所述IMU的位姿差值求和，得到所述IMU的位姿预测值；
判断在当前时刻GPS数据是否更新；
若是，则在所述GPS的位姿差值与所述IMU的位姿差值间的差值小于预设的阈值时，设置所述GPS的观测噪声矩阵；
若否，或所述GPS的位姿差值与所述IMU的位姿差值间的差值大于所述阈值，则设置所述激光雷达的观测噪声矩阵；
根据所述GPS的观测噪声矩阵或激光雷达的观测噪声矩阵计算所述GPS的位姿与所述IMU的位姿预测值，或所述激光雷达的位姿与所述IMU的位姿预测值，作为当前时刻所述车辆位姿的权重；
基于扩展卡尔曼滤波，根据所述权重利用所述GPS的位姿或IMU的位姿预测值，以及所述激光雷达的位姿或IMU的位姿预测值更新当前时刻所述车辆的位姿，并纠正IMU的当前时刻位姿。
根据本发明所述的一种基于车载多感知融合的车辆位姿获取方法，所述由车辆的GPS和激光雷达分别获取所述车辆的当前时刻位姿前，还包括：
以所述GPS提供的位置信息作为初值，利用所述激光雷达的观测信息和先验地图基于NDT匹配方法进行匹配，确定所述车辆在所述先验地图上的初始位姿。
根据本发明所述的一种基于车载多感知融合的车辆位姿获取方法，所述由车辆的激光雷达获取所述车辆的当前时刻位姿，具体包括：
利用所述激光雷达的观测信息和所述先验地图基于NDT匹配方法进行匹配，得到当前时刻所述激光雷达的位姿。
根据本发明所述的一种基于车载多感知融合的车辆位姿获取方法，所述利用所述激光雷达的观测信息和先验地图基于NDT匹配方法进行匹配前，还包括：
利用体素滤波器对所述激光雷达的观测信息进行过滤。
根据本发明所述的一种基于车载多感知融合的车辆位姿获取方法，所述由车辆的IMU获取所述车辆的当前时刻位姿，具体包括：
获取当前时刻所述车辆的IMU的检测数据；
由所述检测数据得到当前时刻所述IMU的速度和角速度，基于所述速度和角速度利用小车运动模型计算得到当前时刻所述车辆的IMU的位姿。
根据本发明所述的一种基于车载多感知融合的车辆位姿获取方法，所述设置所述激光雷达的观测噪声矩阵，具体包括：
根据当前时刻所述车辆相对激光雷达的位姿和所述IMU的位姿预测值，得到所述激光雷达的偏差值；
根据所述偏差值和上一时刻所述IMU的位姿构建所述激光雷达的观测噪声矩阵。
本发明还提供一种基于车载多感知融合的车辆位姿获取装置，包括：
数据获取模块，用于由车辆的GPS和激光雷达分别获取所述车辆的当前时刻位姿；
数据处理模块，用于计算各个所述位姿与上一时刻的相应位姿间的位姿差值；并将所述车辆的IMU在上一时刻的位姿与所述IMU的位姿差值求和，得到所述IMU的位姿预测值；
判断处理模块，用于判断在当前时刻GPS数据是否更新；
若是，则在所述GPS的位姿差值与所述IMU的位姿差值间的差值小于预设的阈值时，设置所述GPS的观测噪声矩阵；
若否，或所述GPS的位姿差值与所述IMU的位姿差值间的差值大于所述阈值，则设置所述激光雷达的观测噪声矩阵；
计算模块，用于根据所述GPS的观测噪声矩阵或激光雷达的观测噪声矩阵计算所述GPS的位姿与所述IMU的位姿预测值，或所述激光雷达的位姿与所述IMU的位姿预测值，作为当前时刻所述车辆位姿的权重；
执行模块，用于基于扩展卡尔曼滤波，根据所述权重利用所述GPS的位姿或IMU的位姿预测值，以及所述激光雷达的位姿或IMU的位姿预测值更新当前时刻所述车辆的位姿，并纠正IMU的当前时刻位姿。
本发明还提供一种电子设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述任一种所述一种基于车载多感知融合的车辆位姿获取方法的步骤。
本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如上述任一种所述一种基于车载多感知融合的车辆位姿获取方法的步骤。
本发明还提供一种计算机程序产品，包括计算机程序，所述计算机程序被处理器执行时实现如上述任一种所述一种基于车载多感知融合的车辆位姿获取方法的步骤。
本发明提供的一种基于车载多感知融合的车辆位姿获取方法及装置，通过车辆的GPS和激光雷达分别获取车辆当前时刻的位姿，并由此分别获得GPS和激光雷达的位姿差值，以及基于上一时刻预测和所述IMU的位姿差值得到的IMU的位姿预测值，基于当前时刻GPS数据是否更新，根据获得的位姿差值由所述GPS的位姿、激光雷达的位姿或IMU的位姿预测值中选择用于所述车辆当前时刻位姿的更新，即在判断车辆相对GPS的位姿精度较高时，由GPS的位姿和IMU的位姿预测值中选择作为所述车辆当前时刻的位姿，而在车辆相对GPS的位姿精度较差或GPS未更新时，由激光雷达的位姿和IMU的位姿预测值中选择作为车辆当前时刻的位姿，实现了采用低成本传感器的冗余配置，提高车辆的定位精度，以为车辆提供车道级定位，有利于自动驾驶技术推进，同时能保证定位鲁棒性，使用更加方便，为环境感知和车身控制提供基准数据。
附图说明
为了更清楚地说明本发明或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明提供的一种基于车载多感知融合的车辆位姿获取方法的流程示意图之一；
图2是本发明提供的利用体素滤波器对所述激光雷达的观测信息进行过滤时的体素栅格化的示意图；
图3是本发明提供的一种基于车载多感知融合的车辆位姿获取装置的结构示意图；
图4是本发明提供的电子设备的结构示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要提前说明的是，本发明的一种基于车载多感知融合的车辆位姿获取方法及装置中的IMU、激光雷达和GPS的安装位置、安装误差角度和外观等均以实际安装情况为准，并已做了外参标定，因而在本发明的一种基于车载多感知融合的车辆位姿获取方法的流程中包括：初始化激光雷达、IMU，以及GPS和车体之间的外参；初始化位置，速度，以及世界坐标系对齐。
下面结合图1-图2描述本发明的一种基于车载多感知融合的车辆位姿获取方法，如图1所示，所述方法包括以下步骤：
101、由车辆的GPS和激光雷达分别获取所述车辆的当前时刻位姿；
需要说明的是，正常情况下，在车辆定位的应用中GPS的精度较高，但是其数据更新频率较慢，通常为1Hz，且容易受到隧道、高楼等建筑物的影响，而激光雷达的定位精度在正常情况下虽然不如GPS高，但是更新频率较快，通常为10Hz，且不易受到建筑物的影响，所以GPS和激光雷达各有各的优缺点，然而，目前的无人驾驶车辆定位中，一般仅采用一种观测传感器，即仅采用激光雷达或GPS中的一种，基于上述的优缺点，很容易造成定位不准，因而，在本发明中采用GPS和激光雷达两种观测传感器的冗余设置。
102、计算各个所述位姿与上一时刻的相应位姿间的位姿差值；并将所述车辆的IMU在上一时刻的位姿与所述IMU的位姿差值求和，得到所述IMU的位姿预测值；
103、判断在当前时刻GPS数据是否更新；若是，进入步骤104；若否，进入步骤106；
104、判断相对GPS的位姿差值与所述IMU的位姿差值间的差值是否小于预设阈值；若是，则进入步骤105；若否，则进入步骤106；
105、设置所述GPS的观测噪声矩阵；
106、设置所述激光雷达的观测噪声矩阵；
107、根据所述GPS的观测噪声矩阵或激光雷达的观测噪声矩阵计算所述GPS的位姿与所述IMU的位姿预测值，或所述激光雷达的位姿与所述IMU的位姿预测值，作为当前时刻所述车辆位姿的权重；
108、基于扩展卡尔曼滤波，根据所述权重利用所述GPS的位姿或IMU的位姿预测值，以及所述激光雷达的位姿或IMU的位姿预测值更新当前时刻所述车辆的位姿，并纠正IMU的当前时刻位姿后，返回步骤102。
具体地，本发明所述的一种基于车载多感知融合的车辆位姿获取方法，首先由车辆的GPS、激光雷达以及IMU获取车辆当前时刻的位姿，并以GPS、激光雷达以及IMU在当前时刻的位姿与上一时刻的位姿间的位姿差值为数据基础，然后，在当前GPS数据是否更新的两种情况下，以位姿差值间的关系判断由GPS、IMU或激光雷达得到的位姿作为车辆在当前时刻的位姿的精度，即在当前时刻GPS数据已更新时，在GPS的位姿差值与所述IMU的位姿差值间的差值小于预设阈值时，由在正常情况下定位精度相对较高的GPS的位姿和IMU的位姿预测值中选择作为所述车辆在当前时刻的位姿；而在当前时刻GPS数据未更新，或相对GPS的位姿差值与所述IMU的位姿差值间的差值大于预设阈值时，即GPS可能受到影响，精度较差时，由激光雷达的位姿和IMU的位姿预测值中选择作为所述车辆在当前时刻的位姿，进而实现了利用冗余观测传感器提高车辆定位精度的效果。
进一步地，对IMU的当前时刻位姿的纠正，使得在对下一时刻车辆的位姿进行获取时，以更为准确的IMU的位姿为基础，对相对GPS的位姿精度是否满足要求的判断进一步保证了定位精度。
可以理解的是，扩展卡尔曼滤波器EKF以形式简洁、应用广泛著称。当需要在某段时间内估计某个不确定量时，一般均会使用EKF，但是当观测信息有误时，按照给定的观测噪声矩阵得到的位姿计算是不准确的，即便是冗余的传感器观测信息也有可能传感器都失效，进而导致定位失败。本发明在此基础上提出了实时地根据观测信息来对原本定值的观测噪声进行设置，不仅实现了对多传感器的数据进行融合，更提高了定位的精度也提高了定位的鲁棒性。
需要说明的是，当车辆首次进入先验地图范围后，在对车辆的每一运动时刻进行精准的定位前，首先需要对车辆的初始位姿进行确定，否则，后续对车辆的定位都将不够准确。
基于此，在本发明的一个实施例中，在由车辆的GPS和激光雷达分别获取所述车辆的当前时刻位姿前，以所述GPS提供的位置信息作为初值，利用所述激光雷达的观测信息和先验地图基于NDT匹配方法进行匹配，确定所述车辆在所述先验地图上的初始位姿，并将所述初始位置赋值给所述IMU作为所述IMU的位姿的初始值。
具体地，GPS对车辆位置的确定较为准确，但是GPS数据仅能定位位置，对于处于实时移动中的车辆来说，其数据中并不包含方向信息，所以以位置信息较为准确的GPS提供的位置信息作为初值，然后利用具有方向信息的激光雷达的观测信息，在先验地图上将GPS提供的位置信息和由激光雷达的观测信息得到的方向信息利用NDT匹配方法进行匹配，进而得到较为准确的车辆在所述先验地图上的初始位姿。
需要说明的是，基于匹配准确度的提升，本发明所用的先验地图优选基于开源包lio_sam进行构建，但是，也可以基于其他方式构建，在这里不做赘述。
NDT匹配方法是目前三维配准中使用较多的匹配方法中的一种，相较于另一种常用的ICP迭代方法，NDT匹配方法耗时更为稳定，且跟初值关系不大，即使在初值误差较大时，也能够纠正，进而得到很好的配准结果。
基于此，在本发明的另一个实施例中，利用所述激光雷达的观测信息和所述先验地图基于NDT匹配方法进行匹配，得到当前时刻所述激光雷达的位姿。
需要说明的是，基于NDT匹配方法对激光雷达的观测信息和先验地图进行匹配，相较于其他匹配方法，能够得到更为准确的激光雷达的位姿，但是，采用其他匹配方法得到的激光雷达的位姿，以及上一实施例中的车辆在所述先验地图上的初始位姿，也能实现本发明所述的一种车载多感知融合的车辆位姿获取方法。
进一步地，在利用激光雷达的观测信息和先验地图进行匹配时，观测信息的数据量直接影响匹配的速率，因此，在匹配前，对激光雷达的观测信息中的冗余信息的删除，有利于提高匹配效率。
基于此，在本发明的另一个实施例中，如图2所示，在利用激光雷达的观测信息和先验地图基于NDT匹配方法进行匹配前，先利用体素滤波器对所述激光雷达的观测信息进行过滤。
具体地，采用体素滤波器对匹配前的激光雷达的观测信息进行点云降采样，即通过输入的点云数据(即激光雷达的观测信息)创建一个三维体素栅格，然后在每个体素内用所有点的中心来近似代表所有点，最后利用体素处理后得到过滤的点云去作NDT匹配，大大加快匹配速度，同时保证准确性。
在本发明的另一个实施例中，具体说明了由车辆的IMU获取所述车辆的当前时刻位姿的方法：
获取当前时刻所述车辆的IMU的检测数据；
由所述检测数据得到当前时刻所述IMU的速度和角速度，基于所述速度和角速度利用小车运动模型计算得到当前时刻所述车辆的IMU的位姿。
具体地，位姿包括位置信息和角度信息，在本发明中，由当前时刻所述车辆的IMU的检测数据中得到当前时刻所述IMU的速度和角速度，基于所述速度和角速度利用小车运动模型，即运用阿克曼转向几何计算得到当前时刻车辆的IMU的位姿。
在本发明的另一个实施例中，具体说明了设置所述激光雷达的观测噪声矩阵的具体方法，即根据当前时刻所述车辆相对激光雷达的位姿和所述IMU的位姿预测值，得到所述激光雷达的偏差值，然后根据所述偏差值和上一时刻所述IMU的位姿构建所述激光雷达的观测噪声矩阵。
具体地，激光雷达的观测噪声的设置，即激光雷达的观测噪声矩阵的构建，需要引入一个如公式1所示的超参数dt，并根据如公式2所示的偏差值biast计算得到当前时刻观测噪声矩阵Rt，如公式3所示：
Figure BDA0003293164690000101
Figure BDA0003293164690000102
Figure BDA0003293164690000103
其中，b为遗忘因子，取值一般在0.5-0.99之间；k为当前时刻对应的次数；Zt为激光雷达的位姿；Ht为车辆系统的观测矩阵，此处为单位矩阵；
Figure BDA0003293164690000104
为IMU的位姿预测值；Kt-1为上一时刻的增益矩阵；Pt-1为上一时刻的车辆的位姿；T为矩阵的转置。
通过引入一个超参数dt，并根据偏差值biast计算得到当前时刻的观测噪声矩阵，实现根据激光雷达的观测信息来对原本定值的观测噪声矩阵进行构建，提高了定位的精度也提高了定位的鲁棒性。
下面对本发明提供的一种基于车载多感知融合的车辆位姿获取装置进行描述，下文描述的一种基于车载多感知融合的车辆位姿获取装置与上文描述的一种基于车载多感知融合的车辆位姿获取方法可相互对应参照。
如图3所示，本发明提供的一种基于车载多感知融合的车辆位姿获取装置，包括数据获取模块310、数据处理模块320、判断处理模块330、计算模块340和执行模块350；其中，
数据获取模块310用于由车辆的GPS和激光雷达分别获取所述车辆的当前时刻位姿；
数据处理模块320用于计算各个所述位姿与上一时刻的相应位姿间的位姿差值；并将所述车辆的IMU在上一时刻的位姿与所述IMU的位姿差值求和，得到所述IMU的位姿预测值；
判断处理模块330用于判断在当前时刻GPS数据是否更新；
若是，则在所述GPS的位姿差值与所述IMU的位姿差值间的差值小于预设的阈值时，设置所述GPS的观测噪声矩阵；
若否，或所述GPS的位姿差值与所述IMU的位姿差值间的差值大于所述阈值，则设置所述激光雷达的观测噪声矩阵；
计算模块340用于根据所述GPS的观测噪声矩阵或激光雷达的观测噪声矩阵计算所述GPS的位姿与所述IMU的位姿预测值，或所述激光雷达的位姿与所述IMU的位姿预测值，作为当前时刻所述车辆位姿的权重；
执行模块350用于基于扩展卡尔曼滤波，根据所述权重利用所述GPS的位姿或IMU的位姿预测值，以及所述激光雷达的位姿或IMU的位姿预测值更新当前时刻所述车辆的位姿，并纠正IMU的当前时刻位姿。
具体地，本发明所述的一种基于车载多感知融合的车辆位姿获取装置，通过车辆的GPS和激光雷达分别获取车辆当前时刻的位姿，并由此分别获得GPS和激光雷达的位姿差值，以及基于上一时刻预测得到的IMU的位姿预测值，基于当前时刻GPS数据是否更新，根据获得的位姿差值由所述GPS的位姿、激光雷达的位姿或IMU的位姿预测值中选择用于所述车辆当前时刻位姿的更新，实现了采用低成本传感器的冗余配置，提高车辆的定位精度，以为车辆提供车道级定位，有利于自动驾驶技术推进，同时能保证定位鲁棒性，使用更加方便，为环境感知和车身控制提供基准数据。
可以选择的是，所述数据获取模块310以所述GPS提供的位置信息作为初值，利用所述激光雷达的观测信息和先验地图基于NDT匹配方法进行匹配，确定所述车辆在所述先验地图上的初始位姿，并将所述初始位姿赋值给所述IMU作为所述IMU的位姿的初始值。
还可以选择的是，所述数据获取模块310利用所述激光雷达的观测信息和所述先验地图基于NDT匹配方法进行匹配，得到当前时刻所述激光雷达的位姿。
进一步可以选择的是，所述一种基于车载多感知融合的车辆位姿获取装置中，还包括数据过滤模块；
所述数据过滤模块用于利用体素滤波器对所述激光雷达的观测信息进行过滤。
还可以选择的是，所述数据获取模块310还包括获取单元和位姿计算单元；其中，
所述获取单元用于获取当前时刻所述车辆的IMU的检测数据；
所述位姿计算单元用于由所述检测数据得到当前时刻所述IMU的速度和角速度，基于所述速度和角速度利用小车运动模型计算得到当前时刻所述车辆的IMU的位姿。
还可以选择的是，所述判断处理模块320中包括偏差计算单元和矩阵构建单元；其中，
所述偏差计算单元用于根据当前时刻所述车辆相对激光雷达的位姿和所述IMU的位姿预测值，得到所述激光雷达的偏差值；
所述矩阵构建单元用于根据所述偏差值和上一时刻所述IMU的位姿构建所述激光雷达的观测噪声矩阵。
本发明实施例提供的一种基于车载多感知融合的车辆位姿获取装置用于签署各实施例的基于车载多感知融合的车辆位姿获取方法。该基于车载多感知融合的车辆位姿获取装置包括的各模块实现相应功能的具体方法和流程详见上述基于车载多感知融合的车辆位姿获取方法的实施例，此处不再赘述。
本发明的一种基于车载多感知融合的车辆位姿获取装置用于前述各实施例的基于车载多感知融合的车辆位姿获取方法。因此，在前述各实施例中的基于车载多感知融合的车辆位姿获取方法中的描述和定义，可以用于本发明实施例中各执行模块的理解。
图4示例了一种电子设备的实体结构示意图，如图4所示，该电子设备可以包括：处理器(processor)410、通信接口(Communications Interface)420、存储器(memory)430和通信总线440，其中，处理器410，通信接口420，存储器430通过通信总线440完成相互间的通信。处理器410可以调用存储器430中的逻辑指令，以执行基于车载多感知融合的车辆位姿获取方法，该方法包括：由车辆的GPS和激光雷达分别获取所述车辆的当前时刻位姿；计算各个所述位姿与上一时刻的相应位姿间的位姿差值；并将所述车辆的IMU在上一时刻的位姿与所述IMU的位姿差值求和，得到所述IMU的位姿预测值；判断在当前时刻GPS数据是否更新；若是，则在所述GPS的位姿差值与所述IMU的位姿差值间的差值小于预设的阈值时，设置所述GPS的观测噪声矩阵；若否，或所述GPS的位姿差值与所述IMU的位姿差值间的差值大于所述阈值，则设置所述激光雷达的观测噪声矩阵；根据所述GPS的观测噪声矩阵或激光雷达的观测噪声矩阵计算所述GPS的位姿与所述IMU的位姿预测值，或所述激光雷达的位姿与所述IMU的位姿预测值，作为当前时刻所述车辆位姿的权重；基于扩展卡尔曼滤波，根据所述权重利用所述GPS的位姿或IMU的位姿预测值，以及所述激光雷达的位姿或IMU的位姿预测值更新当前时刻所述车辆的位姿，并纠正IMU的当前时刻位姿。
此外，上述的存储器430中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
另一方面，本发明还提供一种计算机程序产品，所述计算机程序产品包括计算机程序，计算机程序可存储在非暂态计算机可读存储介质上，所述计算机程序被处理器执行时，计算机能够执行上述各方法所提供的基于车载多感知融合的车辆位姿获取方法，该方法包括：由车辆的GPS和激光雷达分别获取所述车辆的当前时刻位姿；计算各个所述位姿与上一时刻的相应位姿间的位姿差值；并将所述车辆的IMU在上一时刻的位姿与所述IMU的位姿差值求和，得到所述IMU的位姿预测值；判断在当前时刻GPS数据是否更新；若是，则在所述GPS的位姿差值与所述IMU的位姿差值间的差值小于预设的阈值时，设置所述GPS的观测噪声矩阵；若否，或所述GPS的位姿差值与所述IMU的位姿差值间的差值大于所述阈值，则设置所述激光雷达的观测噪声矩阵；根据所述GPS的观测噪声矩阵或激光雷达的观测噪声矩阵计算所述GPS的位姿与所述IMU的位姿预测值，或所述激光雷达的位姿与所述IMU的位姿预测值，作为当前时刻所述车辆位姿的权重；基于扩展卡尔曼滤波，根据所述权重利用所述GPS的位姿或IMU的位姿预测值，以及所述激光雷达的位姿或IMU的位姿预测值更新当前时刻所述车辆的位姿，并纠正IMU的当前时刻位姿。
又一方面，本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现以执行上述各方法提供的基于车载多感知融合的车辆位姿获取方法，该方法包括：由车辆的GPS和激光雷达分别获取所述车辆的当前时刻位姿；计算各个所述位姿与上一时刻的相应位姿间的位姿差值；并将所述车辆的IMU在上一时刻的位姿与所述IMU的位姿差值求和，得到所述IMU的位姿预测值；判断在当前时刻GPS数据是否更新；若是，则在所述GPS的位姿差值与所述IMU的位姿差值间的差值小于预设的阈值时，设置所述GPS的观测噪声矩阵；若否，或所述GPS的位姿差值与所述IMU的位姿差值间的差值大于所述阈值，则设置所述激光雷达的观测噪声矩阵；根据所述GPS的观测噪声矩阵或激光雷达的观测噪声矩阵计算所述GPS的位姿与所述IMU的位姿预测值，或所述激光雷达的位姿与所述IMU的位姿预测值，作为当前时刻所述车辆位姿的权重；基于扩展卡尔曼滤波，根据所述权重利用所述GPS的位姿或IMU的位姿预测值，以及所述激光雷达的位姿或IMU的位姿预测值更新当前时刻所述车辆的位姿，并纠正IMU的当前时刻位姿。
以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种基于车载多感知融合的车辆位姿获取方法，其特征在于，包括：
由车辆的GPS和激光雷达分别获取所述车辆的当前时刻位姿；
计算各个所述位姿与上一时刻的相应位姿间的位姿差值；并将所述车辆的IMU在上一时刻的位姿与所述IMU的位姿差值求和，得到所述IMU的位姿预测值；
判断在当前时刻GPS数据是否更新；
若是，则在所述GPS的位姿差值与所述IMU的位姿差值间的差值小于预设的阈值时，设置所述GPS的观测噪声矩阵；
若否，或所述GPS的位姿差值与所述IMU的位姿差值间的差值大于所述阈值，则设置所述激光雷达的观测噪声矩阵；
根据所述GPS的观测噪声矩阵或激光雷达的观测噪声矩阵计算所述GPS的位姿与所述IMU的位姿预测值，或所述激光雷达的位姿与所述IMU的位姿预测值，作为当前时刻所述车辆位姿的权重；
基于扩展卡尔曼滤波，根据所述权重利用所述GPS的位姿或IMU的位姿预测值，以及所述激光雷达的位姿或IMU的位姿预测值更新当前时刻所述车辆的位姿，并纠正IMU的当前时刻位姿。
2.根据权利要求1所述的一种基于车载多感知融合的车辆位姿获取方法，其特征在于，所述由车辆的GPS和激光雷达分别获取所述车辆的当前时刻位姿前，还包括：
以所述GPS提供的位置信息作为初值，利用所述激光雷达的观测信息和先验地图基于NDT匹配方法进行匹配，确定所述车辆在所述先验地图上的初始位姿，并将所述初始位姿赋值给所述IMU作为所述IMU的位姿的初始值。
3.根据权利要求1所述的一种基于车载多感知融合的车辆位姿获取方法，其特征在于，所述由车辆的激光雷达获取所述车辆的当前时刻位姿，具体包括：
利用所述激光雷达的观测信息和所述先验地图基于NDT匹配方法进行匹配，得到当前时刻所述激光雷达的位姿。
4.根据权利要求2或3所述的一种基于车载多感知融合的车辆位姿获取方法，其特征在于，所述利用所述激光雷达的观测信息和先验地图基于NDT匹配方法进行匹配前，还包括：
利用体素滤波器对所述激光雷达的观测信息进行过滤。
5.根据权利要求1所述的一种基于车载多感知融合的车辆位姿获取方法，其特征在于，所述由车辆的IMU获取所述车辆的当前时刻位姿，具体包括：
获取当前时刻所述车辆的IMU的检测数据；
由所述检测数据得到当前时刻所述IMU的速度和角速度，基于所述速度和角速度利用小车运动模型计算得到当前时刻所述车辆的IMU的位姿。
6.根据权利要求1所述的一种基于车载多感知融合的车辆位姿获取方法，其特征在于，所述设置所述激光雷达的观测噪声矩阵，具体包括：
根据当前时刻所述车辆相对激光雷达的位姿和所述IMU的位姿预测值，得到所述激光雷达的偏差值；
根据所述偏差值和上一时刻所述IMU的位姿构建所述激光雷达的观测噪声矩阵。
7.一种基于车载多感知融合的车辆位姿获取装置，其特征在于，包括：
数据获取模块，用于由车辆的GPS和激光雷达分别获取所述车辆的当前时刻位姿；
数据处理模块，用于计算各个所述位姿与上一时刻的相应位姿间的位姿差值；并将所述车辆的IMU在上一时刻的位姿与所述IMU的位姿差值求和，得到所述IMU的位姿预测值；
判断处理模块，用于判断在当前时刻GPS数据是否更新；
若是，则在所述GPS的位姿差值与所述IMU的位姿差值间的差值小于预设的阈值时，设置所述GPS的观测噪声矩阵；
若否，或所述GPS的位姿差值与所述IMU的位姿差值间的差值大于所述阈值，则设置所述激光雷达的观测噪声矩阵；
计算模块，用于根据所述GPS的观测噪声矩阵或激光雷达的观测噪声矩阵计算所述GPS的位姿与所述IMU的位姿预测值，或所述激光雷达的位姿与所述IMU的位姿预测值，作为当前时刻所述车辆位姿的权重；
执行模块，用于基于扩展卡尔曼滤波，根据所述权重利用所述GPS的位姿或IMU的位姿预测值，以及所述激光雷达的位姿或IMU的位姿预测值更新当前时刻所述车辆的位姿，并纠正IMU的当前时刻位姿。
8.一种电子设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1至7任一项所述一种基于车载多感知融合的车辆位姿获取方法的步骤。
9.一种非暂态计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7任一项所述一种基于车载多感知融合的车辆位姿获取方法的步骤。
10.一种计算机程序产品，包括计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7任一项所述一种基于车载多感知融合的车辆位姿获取方法的步骤。
CN202111170986.3A 2021-10-08 2021-10-08 一种基于车载多感知融合的车辆位姿获取方法及装置 Pending CN113984044A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111170986.3A CN113984044A (zh) 2021-10-08 2021-10-08 一种基于车载多感知融合的车辆位姿获取方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111170986.3A CN113984044A (zh) 2021-10-08 2021-10-08 一种基于车载多感知融合的车辆位姿获取方法及装置

Publications (1)

Publication Number Publication Date
CN113984044A true CN113984044A (zh) 2022-01-28

Family

ID=79737765

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111170986.3A Pending CN113984044A (zh) 2021-10-08 2021-10-08 一种基于车载多感知融合的车辆位姿获取方法及装置

Country Status (1)

Country Link
CN (1) CN113984044A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113566833A (zh) * 2021-07-28 2021-10-29 上海工程技术大学 一种多传感器融合的车辆定位方法及系统
CN114838726A (zh) * 2022-04-20 2022-08-02 哈尔滨理工大学 一种基于多传感器数据融合的gps数据修正算法
CN115204314A (zh) * 2022-08-12 2022-10-18 西南交通大学 基于车载obu的多源数据融合方法和车载obu
CN117346768A (zh) * 2023-11-03 2024-01-05 昆明理工大学 同时适用于室内外的多传感器融合感知定位方法

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101192825B1 (ko) * 2011-06-30 2012-10-18 서울시립대학교 산학협력단 Ｇｐｓ／ｉｎｓ／영상ａｔ를 통합한 라이다 지오레퍼린싱 장치 및 방법
CN106950586A (zh) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机作业的gnss/ins/车辆组合导航方法
WO2018063245A1 (en) * 2016-09-29 2018-04-05 The Charles Stark Draper Laboratory, Inc. Autonomous vehicle localization
US20180299273A1 (en) * 2017-04-17 2018-10-18 Baidu Online Network Technology (Beijing) Co., Ltd. Method and apparatus for positioning vehicle
WO2020048394A1 (zh) * 2018-09-04 2020-03-12 腾讯科技（深圳）有限公司 定位方法、装置、设备和计算机可读存储介质
CN110906923A (zh) * 2019-11-28 2020-03-24 重庆长安汽车股份有限公司 车载多传感器紧耦合融合定位方法、系统、存储介质及车辆
CN111272165A (zh) * 2020-02-27 2020-06-12 清华大学 一种基于特征点标定的智能车定位方法
CN111504311A (zh) * 2020-05-15 2020-08-07 杭州鸿泉物联网技术股份有限公司 多传感器融合实时定位导航装置及方法
CN111679308A (zh) * 2019-03-11 2020-09-18 武汉小狮科技有限公司 一种基于多传感器融合的无人车定位方法
CN111949943A (zh) * 2020-07-24 2020-11-17 北京航空航天大学 一种面向高级自动驾驶的v2x和激光点云配准的车辆融合定位方法
CN113029137A (zh) * 2021-04-01 2021-06-25 清华大学 一种多源信息自适应融合定位方法及系统
US20210215832A1 (en) * 2020-05-13 2021-07-15 Beijing Baidu Netcom Science Technology Co., Ltd. Positioning method and apparatus
CN113359167A (zh) * 2021-04-16 2021-09-07 电子科技大学 一种通过惯性测量参数将gps与激光雷达融合定位的方法

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101192825B1 (ko) * 2011-06-30 2012-10-18 서울시립대학교 산학협력단 Ｇｐｓ／ｉｎｓ／영상ａｔ를 통합한 라이다 지오레퍼린싱 장치 및 방법
WO2018063245A1 (en) * 2016-09-29 2018-04-05 The Charles Stark Draper Laboratory, Inc. Autonomous vehicle localization
CN106950586A (zh) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机作业的gnss/ins/车辆组合导航方法
US20180299273A1 (en) * 2017-04-17 2018-10-18 Baidu Online Network Technology (Beijing) Co., Ltd. Method and apparatus for positioning vehicle
WO2020048394A1 (zh) * 2018-09-04 2020-03-12 腾讯科技（深圳）有限公司 定位方法、装置、设备和计算机可读存储介质
CN111679308A (zh) * 2019-03-11 2020-09-18 武汉小狮科技有限公司 一种基于多传感器融合的无人车定位方法
CN110906923A (zh) * 2019-11-28 2020-03-24 重庆长安汽车股份有限公司 车载多传感器紧耦合融合定位方法、系统、存储介质及车辆
CN111272165A (zh) * 2020-02-27 2020-06-12 清华大学 一种基于特征点标定的智能车定位方法
US20210215832A1 (en) * 2020-05-13 2021-07-15 Beijing Baidu Netcom Science Technology Co., Ltd. Positioning method and apparatus
CN111504311A (zh) * 2020-05-15 2020-08-07 杭州鸿泉物联网技术股份有限公司 多传感器融合实时定位导航装置及方法
CN111949943A (zh) * 2020-07-24 2020-11-17 北京航空航天大学 一种面向高级自动驾驶的v2x和激光点云配准的车辆融合定位方法
CN113029137A (zh) * 2021-04-01 2021-06-25 清华大学 一种多源信息自适应融合定位方法及系统
CN113359167A (zh) * 2021-04-16 2021-09-07 电子科技大学 一种通过惯性测量参数将gps与激光雷达融合定位的方法

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113566833A (zh) * 2021-07-28 2021-10-29 上海工程技术大学 一种多传感器融合的车辆定位方法及系统
CN114838726A (zh) * 2022-04-20 2022-08-02 哈尔滨理工大学 一种基于多传感器数据融合的gps数据修正算法
CN115204314A (zh) * 2022-08-12 2022-10-18 西南交通大学 基于车载obu的多源数据融合方法和车载obu
CN115204314B (zh) * 2022-08-12 2023-05-30 西南交通大学 基于车载obu的多源数据融合方法和车载obu
CN117346768A (zh) * 2023-11-03 2024-01-05 昆明理工大学 同时适用于室内外的多传感器融合感知定位方法
CN117346768B (zh) * 2023-11-03 2024-04-19 昆明理工大学 同时适用于室内外的多传感器融合感知定位方法

Similar Documents

Publication Publication Date Title
CN108732603B (zh) 用于定位车辆的方法和装置
CN113984044A (zh) 一种基于车载多感知融合的车辆位姿获取方法及装置
CN111812658B (zh) 位置确定方法、装置、系统和计算机可读存储介质
CN109270545B (zh) 一种定位真值校验方法、装置、设备及存储介质
KR101625486B1 (ko) 지도 기반 측위 시스템 및 그 방법
KR20210111180A (ko) 위치 추적 방법, 장치, 컴퓨팅 기기 및 컴퓨터 판독 가능한 저장 매체
JP2020042024A (ja) レーザレーダの内部パラメータ精度検証方法とその装置、機器及び媒体
CN108362288B (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
CN112835085B (zh) 确定车辆位置的方法和装置
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
KR102331312B1 (ko) 차량 내부 센서, 카메라, 및 gnss 단말기를 이용한 3차원 차량 항법 시스템
CN112596089B (zh) 融合定位方法、装置、电子设备及存储介质
CN112946681B (zh) 融合组合导航信息的激光雷达定位方法
CN114167470A (zh) 一种数据处理方法和装置
US10928206B2 (en) Vehicle control system, own vehicle position calculation apparatus, vehicle control apparatus, own vehicle position calculation program, and non-transitory computer readable storage medium
KR20180038154A (ko) 라이다 센서를 이용한 차량의 자세 추정 방법
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN113238251B (zh) 一种基于车载激光雷达的目标级语义定位方法
CN113551666A (zh) 自动驾驶多传感器融合定位方法和装置、设备及介质
CN116047565A (zh) 一种多传感器数据融合定位系统
Gao et al. An integrated land vehicle navigation system based on context awareness
CN110989619A (zh) 用于定位对象的方法、装置、设备和存储介质
CN110794434B (zh) 一种位姿的确定方法、装置、设备及存储介质
CN112835086B (zh) 确定车辆位置的方法和装置
CN114264301A (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