CN117434515A - 基于惯性里程计的车辆定位方法、装置、设备及介质 - Google Patents
基于惯性里程计的车辆定位方法、装置、设备及介质 Download PDFInfo
- Publication number
- CN117434515A CN117434515A CN202311403080.0A CN202311403080A CN117434515A CN 117434515 A CN117434515 A CN 117434515A CN 202311403080 A CN202311403080 A CN 202311403080A CN 117434515 A CN117434515 A CN 117434515A
- Authority
- CN
- China
- Prior art keywords
- current
- inertial sensor
- laser
- information
- determining
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 73
- 230000000007 visual effect Effects 0.000 claims abstract description 144
- 238000005457 optimization Methods 0.000 claims abstract description 25
- 230000008569 process Effects 0.000 claims abstract description 24
- 230000006870 function Effects 0.000 claims description 88
- 238000004590 computer program Methods 0.000 claims description 16
- 239000011159 matrix material Substances 0.000 claims description 11
- 230000015556 catabolic process Effects 0.000 claims description 10
- 238000006731 degradation reaction Methods 0.000 claims description 10
- 230000001502 supplementing effect Effects 0.000 claims description 8
- 238000000354 decomposition reaction Methods 0.000 claims description 4
- 238000004891 communication Methods 0.000 description 8
- 238000012545 processing Methods 0.000 description 7
- 230000003287 optical effect Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 239000000047 product Substances 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000001413 cellular effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 239000011521 glass Substances 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000001953 sensory effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 239000013589 supplement Substances 0.000 description 1
Classifications
-
- 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
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
-
- 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
- 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
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/50—Depth or shape recovery
- G06T7/521—Depth or shape recovery from laser ranging, e.g. using interferometry; from the projection of structured light
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/77—Determining position or orientation of objects or cameras using statistical methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- 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)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Theoretical Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Life Sciences & Earth Sciences (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Bioinformatics & Computational Biology (AREA)
- Evolutionary Biology (AREA)
- Probability & Statistics with Applications (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Optics & Photonics (AREA)
- Navigation (AREA)
Abstract
本发明实施例公开了一种基于惯性里程计的车辆定位方法、装置、设备及介质。该方法包括:从目标车辆的当前帧雷达点云信息中提取激光特征点,将激光特征点与激光点云地图进行匹配得到初始激光特征点投影误差;若基于对第一关联误差函数的优化过程确定目标车辆在当前时刻处于退化场景,则确定当前帧相机图像信息中的当前视觉特征点,并确定初始视觉特征点投影误差;根据第一关联误差函数和第二关联误差函数确定综合关联误差函数,基于初始激光特征点投影误差和初始视觉特征点投影误差对综合关联误差函数进行优化得到目标惯性传感器状态信息;根据目标惯性传感器状态信息对目标车辆进行定位。本技术方案,能够提高车辆在退化场景下的定位精度。
Description
技术领域
本发明涉及车辆定位技术领域,尤其涉及一种基于惯性里程计的车辆定位方法、装置、设备及介质。
背景技术
激光雷达惯性里程计作为当前主流的定位算法,通过惯性传感器进行车辆位姿预测。激光雷达从当前一帧点云中提取几何特征与地图进行匹配得到更精确的位姿。但是当场景退化,即激光一帧点云中缺乏几何特征时,激光雷达匹配就会失效。比如隧道等长直走廊场景,是典型的退化场景。此时点云只能提供车体横向约束,在纵向上缺乏几何约束,因此导致定位纵向误差快速累积,从而降低车辆定位精度。
目前常用的解决方法有:1、利用GNSS(Global Navigation Satellite System,全球导航卫星系统)提供绝对位姿约束,但是在隧道场景中GNSS因遮挡没有信号无法使用。2、通过提高点云地图分辨率提高匹配成功率与精度,但是在地图同样缺少几何特征的情况下失效。3、结合轮速里程计优化位姿。但是在长隧道场景,里程计精度会越来越低,整体定位精度也会降低。
发明内容
本发明提供了一种基于惯性里程计的车辆定位方法、装置、设备及介质,通过融合轻量级的视觉特征在退化场景下为车辆提供足够的定位约束,有效解决了由于缺乏几何特征导致的激光雷达惯性里程计定位误差大的问题,从而提高了退化场景下的车辆定位精度。
根据本发明的一方面,提供了一种基于惯性里程计的车辆定位方法,所述方法包括:
从目标车辆的当前帧雷达点云信息中提取激光特征点,将所述激光特征点与激光点云地图进行匹配得到初始激光特征点投影误差;其中,所述激光点云地图基于历史帧雷达点云信息确定;
基于所述初始激光特征点投影误差对第一关联误差函数的优化过程确定所述目标车辆在当前时刻是否处于退化场景;其中,所述第一关联误差函数用于表征惯性传感器状态与激光特征点投影误差之间的函数关系;
若是,则确定当前帧相机图像信息中的当前视觉特征点,并将激光点云地图中与所述当前视觉特征点关联的点云信息投影到当前帧相机图像中,根据投影结果确定初始视觉特征点投影误差;
根据所述第一关联误差函数和第二关联误差函数确定综合关联误差函数,基于所述初始激光特征点投影误差和所述初始视觉特征点投影误差对所述综合关联误差函数进行优化得到目标惯性传感器状态信息;其中,所述第二关联误差函数用于表征惯性传感器状态与视觉特征点投影误差之间的函数关系,所述目标惯性传感器状态信息为所述激光特征点投影误差与所述视觉特征点投影误差之和最小时对应的惯性传感器状态信息;
根据所述目标惯性传感器状态信息对所述目标车辆进行定位。
根据本发明的另一方面,提供了一种基于惯性里程计的车辆定位装置,包括:
初始激光特征点投影误差确定模块,用于从目标车辆的当前帧雷达点云信息中提取激光特征点,将所述激光特征点与激光点云地图进行匹配得到初始激光特征点投影误差;其中,所述激光点云地图基于历史帧雷达点云信息确定;
退化场景判断模块,用于基于所述初始激光特征点投影误差对第一关联误差函数的优化过程确定所述目标车辆在当前时刻是否处于退化场景;其中,所述第一关联误差函数用于表征惯性传感器状态与激光特征点投影误差之间的函数关系;
初始视觉特征点投影误差确定模块,用于若是,则确定当前帧相机图像信息中的当前视觉特征点,并将激光点云地图中与所述当前视觉特征点关联的点云信息投影到当前帧相机图像中,根据投影结果确定初始视觉特征点投影误差;
目标惯性传感器状态信息确定模块,用于根据所述第一关联误差函数和第二关联误差函数确定综合关联误差函数,基于所述初始激光特征点投影误差和所述初始视觉特征点投影误差对所述综合关联误差函数进行优化得到目标惯性传感器状态信息;其中,所述第二关联误差函数用于表征惯性传感器状态与视觉特征点投影误差之间的函数关系,所述目标惯性传感器状态信息为所述激光特征点投影误差与所述视觉特征点投影误差之和最小时对应的惯性传感器状态信息;
第一车辆定位模块,用于根据所述目标惯性传感器状态信息对所述目标车辆进行定位。
根据本发明的另一方面,提供了一种电子设备,所述电子设备包括:
至少一个处理器;以及
与所述至少一个处理器通信连接的存储器;其中,
所述存储器存储有可被所述至少一个处理器执行的计算机程序,所述计算机程序被所述至少一个处理器执行,以使所述至少一个处理器能够执行本发明任一实施例所述的基于惯性里程计的车辆定位方法。
根据本发明的另一方面,提供了一种计算机可读存储介质,所述计算机可读存储介质存储有计算机指令,所述计算机指令用于使处理器执行时实现本发明任一实施例所述的基于惯性里程计的车辆定位方法。
本发明实施例的技术方案,从目标车辆的当前帧雷达点云信息中提取激光特征点,将激光特征点与激光点云地图进行匹配得到初始激光特征点投影误差;其中,激光点云地图基于历史帧雷达点云信息确定;基于初始激光特征点投影误差对第一关联误差函数的优化过程确定目标车辆在当前时刻是否处于退化场景;其中,第一关联误差函数用于表征惯性传感器状态与激光特征点投影误差之间的函数关系;若是,则确定当前帧相机图像信息中的当前视觉特征点,并将激光点云地图中与当前视觉特征点关联的点云信息投影到当前帧相机图像中,根据投影结果确定初始视觉特征点投影误差;根据第一关联误差函数和第二关联误差函数确定综合关联误差函数,基于初始激光特征点投影误差和初始视觉特征点投影误差对综合关联误差函数进行优化得到目标惯性传感器状态信息;其中,第二关联误差函数用于表征惯性传感器状态与视觉特征点投影误差之间的函数关系,目标惯性传感器状态信息为激光特征点投影误差与视觉特征点投影误差之和最小时对应的惯性传感器状态信息;根据目标惯性传感器状态信息对目标车辆进行定位。本技术方案,通过融合轻量级的视觉特征在退化场景下为车辆提供足够的定位约束,有效解决了因缺乏几何特征导致的激光雷达惯性里程计定位误差大的问题,从而提高了退化场景下的车辆定位精度。
应当理解,本部分所描述的内容并非旨在标识本发明的实施例的关键或重要特征,也不用于限制本发明的范围。本发明的其它特征将通过以下的说明书而变得容易理解。
附图说明
为了更清楚地说明本发明实施例中的技术方案,下面将对实施例描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1是根据本发明实施例一提供的一种基于惯性里程计的车辆定位方法的流程图;
图2是根据本发明实施例二提供的一种基于惯性里程计的车辆定位方法的流程图;
图3是根据本发明实施例三提供的一种基于惯性里程计的车辆定位装置的结构示意图;
图4是实现本发明实施例的一种基于惯性里程计的车辆定位方法的电子设备的结构示意图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分的实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都应当属于本发明保护的范围。
需要说明的是,本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”“目标”等是用于区别类似的对象,而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换,以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外,术语“包括”和“具有”以及他们的任何变形,意图在于覆盖不排他的包含,例如,包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元,而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
实施例一
图1为本发明实施例一提供的一种基于惯性里程计的车辆定位方法的流程图,本实施例可适用于对处于退化场景下的车辆进行精准定位的情况,该方法可以由基于惯性里程计的车辆定位装置来执行,该基于惯性里程计的车辆定位装置可以采用硬件和/或软件的形式实现,该基于惯性里程计的车辆定位装置可配置于具有数据处理能力的电子设备中。如图1所示,该方法包括:
S110,从目标车辆的当前帧雷达点云信息中提取激光特征点,将激光特征点与激光点云地图进行匹配得到初始激光特征点投影误差。
其中,当前帧雷达点云信息可以是指在当前时刻下由激光雷达采集的点云信息。激光特征点可以是指雷达点云信息中含有几何特征的点。激光点云地图基于历史帧雷达点云信息确定。其中,历史帧雷达点云信息可以是指通过激光雷达采集的当前时刻之前的点云信息。具体的,将当前时刻之前获取的历史帧雷达点云信息转换到地图坐标系中,并将转换后的点云信息进行叠加即可得到激光点云地图。初始激光特征点投影误差可以是指将激光特征点从雷达坐标系转换到地图坐标系的过程中产生的投影误差。其中,雷达坐标系和地图坐标系是两个不同的坐标系,雷达坐标系是描述雷达检测数据的基础,地图坐标系是描述激光点云地图的基础。
本实施例中,预先在目标车辆上安装有激光雷达、相机、惯性传感器和时间同步装置。其中,激光雷达、相机以及惯性传感器的内参和外参是预先精确标定好的。激光雷达和相机之间要求进行硬件时间同步,即激光雷达和相机采用相同的采集帧率(如10Hz)帧率,使激光雷达数据与相机数据同步获得。需要说明的是,为提高视觉跟踪的成功率,可以适当提高相机帧率,但要求激光雷达每秒至少有5帧数据和相机数据时间对齐。
本实施例中,可选的,在从目标车辆的当前帧雷达点云信息中提取激光特征点之前,还包括:确定目标车辆的当前惯性传感器状态信息;其中,惯性传感器状态信息用于表征惯性传感器的位姿信息;根据当前惯性传感器状态信息确定目标车辆的当前雷达位姿信息和当前相机位姿信息。
其中,当前惯性传感器状态信息可以用于表征当前时刻的惯性传感器状态,惯性传感器状态信息用于表征惯性传感器的位姿信息。示例性的,惯性传感器状态信息可以包括位置、姿态、速度、陀螺仪零偏和加速度计零偏。当前雷达位姿信息可以用于表征当前时刻下的雷达位姿。当前相机位姿信息可以用于表征当前时刻下的相机位姿。
在本实施例中,可选的,确定目标车辆的当前惯性传感器状态信息,包括:获取目标车辆的上一惯性传感器状态信息和当前惯性传感器数据;其中,惯性传感器数据包括陀螺仪数据和加速度计数据;根据上一惯性传感器状态信息以及当前惯性传感器数据,确定目标车辆的当前惯性传感器状态信息。
其中,上一惯性传感器状态信息可以是指上一时刻的惯性传感器状态信息。当前惯性传感器数据可以是指当前时刻下的惯性传感器数据,具体可以包括陀螺仪数据(如角速度和方向)和加速度计数据(如加速度)。示例性的,假设当前时刻为k时刻,即上一时刻为k-1时刻。若已知k-1时刻的惯性传感器状态信息Xk-1,当获取到k时刻的惯性传感器数据时,可预测得到k时刻的惯性传感器状态信息Xk,即Xk=f(Xk-1,imuk)。其中,imuk表示k时刻的惯性传感器数据。
本实施例中,在确定目标车辆的当前惯性传感器状态信息之后,可以基于激光雷达和惯性传感器的外参,根据当前惯性传感器状态信息来确定目标车辆的当前雷达位姿信息。其中,激光雷达和惯性传感器的外参可以用于表征激光雷达与惯性传感器之间的位姿映射关系。同时,可以基于相机和惯性传感器的外参,根据当前惯性传感器状态信息来确定目标车辆的当前相机位姿信息。其中,相机和惯性传感器的外参可以用于表征相机与惯性传感器之间的位姿映射关系。
本实施例中,可选的,将激光特征点与激光点云地图进行匹配得到初始激光特征点投影误差,包括:基于当前雷达位姿信息将激光特征点投影到激光点云地图中,根据激光特征点的投影结果确定初始激光特征点投影误差。
本实施例中,确定目标车辆的当前雷达位姿信息之后,可以利用当前雷达位姿信息将激光特征点投影到激光点云地图中,并根据激光特征点的投影结果确定初始激光特征点投影误差。具体的,基于激光雷达的位置和角度信息,将处于雷达坐标系下的激光特征点投影到地图坐标系下的激光点云地图中,根据投影后的激光特征点位置信息和激光点云地图中与激光特征点对应的点云位置信息计算投影误差,得到初始激光特征点投影误差。
S120,基于初始激光特征点投影误差对第一关联误差函数的优化过程确定目标车辆在当前时刻是否处于退化场景。
其中,第一关联误差函数用于表征惯性传感器状态与激光特征点投影误差之间的函数关系。
本实施例中,确定初始激光特征点投影误差之后,可以基于初始激光特征点投影误差对第一关联误差函数的优化过程确定目标车辆在当前时刻是否处于退化场景。其中,对第一关联误差函数的优化目的是通过调整惯性传感器状态参数使激光特征点投影误差达到最小,以此对惯性传感器状态信息进行优化。可选的,基于初始激光特征点投影误差对第一关联误差函数的优化过程确定目标车辆在当前时刻是否处于退化场景,包括:基于初始激光特征点投影误差对第一关联误差函数的优化过程确定Hession矩阵;对Hession矩阵进行特征值分解,得到候选特征值;若候选特征值中的最小值小于预设特征值阈值,确定目标车辆在当前时刻处于退化场景;否则,确定目标车辆在当前时刻未处于退化场景。
本实施例中,预先构建第一关联误差函数,示例性的,可以表示如下:其中,elidar为激光特征点投影误差,Xk为k时刻的惯性传感器状态信息,Ti2l为激光雷达与惯性传感器的外参,pmap为激光点云地图中的点云位置信息,/>为激光特征点在雷达坐标系中的位置信息,g为第一关联误差函数关系。在得到初始激光特征点投影误差之后,可以将初始激光特征点投影误差作为elidar的初始值对第一关联误差函数进行优化。在优化过程中会生成Hession矩阵,通过Hession矩阵可以判断目标车辆在当前时刻是否处于退化场景。
具体的,首先需要对Hession矩阵进行特征值分解,得到候选特征值。然后将候选特征值中的最小特征值与预设特征值阈值进行大小比较,再根据比较结果判断是否处于退化场景。其中,预设特征值阈值可以根据实际应用需求设定,本实施例中对此不做具体限定。若候选特征值中的最小值小于预设特征值阈值,则可确定目标车辆在当前时刻处于退化场景;否则,确定目标车辆在当前时刻未处于退化场景。
S130,若是,则确定当前帧相机图像信息中的当前视觉特征点,并将激光点云地图中与当前视觉特征点关联的点云信息投影到当前帧相机图像中,根据投影结果确定初始视觉特征点投影误差。
其中,当前帧相机图像信息可以是指当前时刻下通过相机采集的图像信息。视觉特征点可以是指二维图像中梯度较大(如颜色或光线跳变较大)的像素点,视觉特征点的数量可以根据实际需求设定。其中,梯度大表明纹理信息明显,视觉纹理信息是对激光雷达几何信息的有效补充。在隧道场景中,充分的纹理信息可以有效维持定位的精度,使退化场景下的定位依然稳健。当前视觉特征点可以是指当前帧相机图像信息中的视觉特征点。初始视觉特征点投影误差可以是指将激光点云地图中与当前视觉特征点关联的点云信息投影到当前帧相机图像的过程中产生的投影误差。
本实施例中,若确定目标车辆在当前时刻处于退化场景,则需要确定当前帧相机图像信息中的当前视觉特征点。具体的,首先需要确定第一帧相机图像信息中的初始视觉特征点,然后基于光流跟踪算法根据初始视觉特征点确定后续每帧相机图像信息中的视觉特征点。具体过程为:在惯性里程计成功运行之后,首先获取初始时刻下的第一帧相机图像信息,然后将初始时刻下初始激光点云地图中位于相机视野的点云信息投影到第一帧相机图像中,再从投影后的第一帧相机图像中提取梯度较大的预设数量个像素点作为初始视觉特征点。由于相邻帧相机图像信息中的视觉特征点具有重叠部分,可以基于光流跟踪算法对前一帧相机图像信息中的视觉特征点进行视觉特征追踪,得到与前一帧相机图像相邻的后一帧相机图像中的视觉特征点。假设当前时刻为k时刻,则k时刻相机图像信息中的视觉特征点可以基于k-1时刻相机图像信息中的视觉特征点进行确定。
在本实施例中,可选的,将激光点云地图中与当前视觉特征点关联的点云信息投影到当前帧相机图像中,包括:基于当前相机位姿信息将激光点云地图中与当前视觉特征点关联的点云信息投影到当前帧相机图像中。
本实施例中,在确定目标车辆的当前相机位姿信息之后,可以利用当前相机位姿信息根据三维地图点到二维图像点的反投影模型将激光点云地图中与当前视觉特征点关联的点云信息投影到当前帧相机图像中,然后根据投影结果确定初始视觉特征点投影误差。需要说明的是,由于当前视觉特征点基于历史视觉特征点确定,而初始视觉特征点的确定与初始激光点云地图相关联,因此当前视觉特征点与激光点云地图具有关联关系。
S140,根据第一关联误差函数和第二关联误差函数确定综合关联误差函数,基于初始激光特征点投影误差和初始视觉特征点投影误差对综合关联误差函数进行优化得到目标惯性传感器状态信息。
其中,第二关联误差函数用于表征惯性传感器状态与视觉特征点投影误差之间的函数关系,目标惯性传感器状态信息为激光特征点投影误差与视觉特征点投影误差之和最小时对应的惯性传感器状态信息。
本实施例中,预先构建第二关联误差函数,示例性的,可以表示如下:ecam=t(Xk,Ti2c,pcam_feature,pmap)。其中,ecam为视觉特征点投影误差,Xk为k时刻的惯性传感器状态信息,Ti2c为相机与惯性传感器的外参,pmap为激光点云地图中的点云位置信息,pcam_feature为当前视觉特征点在当前帧图像中的位置信息,t表示第二关联误差函数关系。若确定目标车辆处于退化场景,则将第一关联误差函数与第二关联误差函数进行加和得到综合关联误差函数。示例性的,综合关联误差函数可以表示如下: 然后基于初始激光特征点投影误差和初始视觉特征点投影误差对综合关联误差函数进行优化,通过调整惯性传感器状态参数使激光特征点投影误差和视觉特征点投影误差之和达到最小,对应得到目标惯性传感器状态信息。
S150,根据目标惯性传感器状态信息对目标车辆进行定位。
本实施例中,确定目标惯性传感器状态信息之后,即可根据目标惯性传感器状态信息对目标车辆进行定位。示例性的,可以将目标惯性传感器状态信息中的惯性传感器位姿信息直接作为目标车辆的位姿信息,以实现目标车辆在退化场景下的精准定位。
本发明实施例的技术方案,从目标车辆的当前帧雷达点云信息中提取激光特征点,将激光特征点与激光点云地图进行匹配得到初始激光特征点投影误差;其中,激光点云地图基于历史帧雷达点云信息确定;基于初始激光特征点投影误差对第一关联误差函数的优化过程确定目标车辆在当前时刻是否处于退化场景;其中,第一关联误差函数用于表征惯性传感器状态与激光特征点投影误差之间的函数关系;若是,则确定当前帧相机图像信息中的当前视觉特征点,并将激光点云地图中与当前视觉特征点关联的点云信息投影到当前帧相机图像中,根据投影结果确定初始视觉特征点投影误差;根据第一关联误差函数和第二关联误差函数确定综合关联误差函数,基于初始激光特征点投影误差和初始视觉特征点投影误差对综合关联误差函数进行优化得到目标惯性传感器状态信息;其中,第二关联误差函数用于表征惯性传感器状态与视觉特征点投影误差之间的函数关系,目标惯性传感器状态信息为激光特征点投影误差与视觉特征点投影误差之和最小时对应的惯性传感器状态信息;根据目标惯性传感器状态信息对目标车辆进行定位。本技术方案,通过融合轻量级的视觉特征在退化场景下为车辆提供足够的定位约束,有效解决了因缺乏几何特征导致的激光雷达惯性里程计定位误差大的问题,从而提高了退化场景下的车辆定位精度。
在本实施例中,可选的,在确定目标车辆在当前时刻是否处于退化场景之后,还包括:若否,则基于初始激光特征点投影误差对第一关联误差函数的优化结果确定候选惯性传感器状态信息;其中,候选惯性传感器状态信息为最小激光特征点投影误差对应的惯性传感器状态信息;根据候选惯性传感器状态信息对目标车辆进行定位。
本实施例中,若确定目标车辆在当前时刻未处于退化场景,则不需要融合视觉特征进行辅助定位,此时只需要基于初始激光特征点投影误差对第一关联误差函数的优化结果,确定出与最小激光特征点投影误差对应的候选惯性传感器状态信息,再根据候选惯性传感器状态信息中惯性传感器位姿信息确定目标车辆的位姿信息,以在非退化场景下实现对目标车辆的快速精准定位。
本方案通过这样的设置,在确定目标车辆在当前时刻未处于退化场景的情况下,能够仅仅根据最小激光特征点投影误差对应的候选惯性传感器状态信息,实现目标车辆在非退化场景下的快速精准定位。
实施例二
图2为本发明实施例二提供的一种基于惯性里程计的车辆定位方法的流程图,本实施例以上述实施例为基础进行优化。具体优化为:在确定当前帧相机图像信息中的当前视觉特征点之后,还包括:若当前视觉特征点的数量小于预设数量阈值,则将当前时刻下位于相机视野的激光点云地图中的点云信息作为当前相机点云信息;将当前相机点云信息投影到当前帧相机图像中,得到更新后的当前帧相机图像信息;从更新后的当前帧相机图像信息中提取候选视觉特征点,并基于候选视觉特征点对当前视觉特征点进行补充。
如图2所示,本实施例的方法具体包括如下步骤:
S210,从目标车辆的当前帧雷达点云信息中提取激光特征点,将激光特征点与激光点云地图进行匹配得到初始激光特征点投影误差。
其中,激光点云地图基于历史帧雷达点云信息确定。
S220,基于初始激光特征点投影误差对第一关联误差函数的优化过程确定目标车辆在当前时刻是否处于退化场景。
其中,第一关联误差函数用于表征惯性传感器状态与激光特征点投影误差之间的函数关系。
S230,若是,则确定当前帧相机图像信息中的当前视觉特征点。
S240,若当前视觉特征点的数量小于预设数量阈值,则将当前时刻下位于相机视野的激光点云地图中的点云信息作为当前相机点云信息。
本实施例中,若当前视觉特征点的数量小于预设数量阈值,表明当前视觉特征点的数量较少,此时出现了前一帧视觉特征点跟踪丢失的情况,即当前相机图像中仅跟踪到了前一帧相机图像中的部分视觉特征点。为了避免由于当前视觉特征点的数量较少导致的退化场景下目标车辆的定位精度降低的问题,此时需要对当前视觉特征点的数量进行补充,使其重新满足预设数量阈值。首先将当前时刻下激光点云地图中位于相机视野中的点云信息作为当前相机点云信息。
S250,将当前相机点云信息投影到当前帧相机图像中,得到更新后的当前帧相机图像信息。
本实施例中,在得到当前相机点云信息之后,基于三维地图点到二维图像点的反投影模型将当前相机点云信息投影到当前帧相机图像中,得到更新后的当前帧相机图像信息。
S260,从更新后的当前帧相机图像信息中提取候选视觉特征点,并基于候选视觉特征点对当前视觉特征点进行补充。
本实施例中,得到更新后的当前帧相机图像信息之后,从更新后的当前帧相机图像信息中提取梯度较大的像素点作为候选视觉特征点,该候选视觉特征点与更新之前的当前帧相机图像信息中的当前视觉特征点并不相同,再基于候选视觉特征点对当前视觉特征点进行补充,使补充后的视觉特征点数量达到预设数量阈值。
S270,将激光点云地图中与补充后的当前视觉特征点关联的点云信息投影到当前帧相机图像中,根据投影结果确定初始视觉特征点投影误差。
S280,根据第一关联误差函数和第二关联误差函数确定综合关联误差函数,基于初始激光特征点投影误差和初始视觉特征点投影误差对综合关联误差函数进行优化得到目标惯性传感器状态信息。
其中,第二关联误差函数用于表征惯性传感器状态与视觉特征点投影误差之间的函数关系,目标惯性传感器状态信息为激光特征点投影误差与视觉特征点投影误差之和最小时对应的惯性传感器状态信息。
S290,根据目标惯性传感器状态信息对目标车辆进行定位。
本发明实施例的技术方案,在确定当前帧相机图像信息中的当前视觉特征点之后,若当前视觉特征点的数量小于预设数量阈值,则将当前时刻下位于相机视野的激光点云地图中的点云信息作为当前相机点云信息;将当前相机点云信息投影到当前帧相机图像中,得到更新后的当前帧相机图像信息;从更新后的当前帧相机图像信息中提取候选视觉特征点,并基于候选视觉特征点对当前视觉特征点进行补充。本技术方案,在通过融合轻量级的视觉特征在退化场景下为车辆提供足够的定位约束,有效解决了因缺乏几何特征导致的激光雷达惯性里程计定位误差大的问题,从而提高了退化场景下的车辆定位精度的基础上,还能在当前视觉特征点的数量小于预设数量阈值时对视觉特征点数量进行补充,从而避免由于当前视觉特征点的数量较少导致目标车辆在退化场景下定位精度降低的问题,有助于进一步提高退化场景下的车辆定位精度。
实施例三
图3为本发明实施例三提供的一种基于惯性里程计的车辆定位装置的结构示意图,该装置可执行本发明任意实施例所提供的基于惯性里程计的车辆定位方法,具备执行方法相应的功能模块和有益效果。如图3所示,该装置包括:
初始激光特征点投影误差确定模块310,用于从目标车辆的当前帧雷达点云信息中提取激光特征点,将所述激光特征点与激光点云地图进行匹配得到初始激光特征点投影误差;其中,所述激光点云地图基于历史帧雷达点云信息确定;
退化场景判断模块320,用于基于所述初始激光特征点投影误差对第一关联误差函数的优化过程确定所述目标车辆在当前时刻是否处于退化场景;其中,所述第一关联误差函数用于表征惯性传感器状态与激光特征点投影误差之间的函数关系;
初始视觉特征点投影误差确定模块330,用于若是,则确定当前帧相机图像信息中的当前视觉特征点,并将激光点云地图中与所述当前视觉特征点关联的点云信息投影到当前帧相机图像中,根据投影结果确定初始视觉特征点投影误差;
目标惯性传感器状态信息确定模块340,用于根据所述第一关联误差函数和第二关联误差函数确定综合关联误差函数,基于所述初始激光特征点投影误差和所述初始视觉特征点投影误差对所述综合关联误差函数进行优化得到目标惯性传感器状态信息;其中,所述第二关联误差函数用于表征惯性传感器状态与视觉特征点投影误差之间的函数关系,所述目标惯性传感器状态信息为所述激光特征点投影误差与所述视觉特征点投影误差之和最小时对应的惯性传感器状态信息;
第一车辆定位模块350,用于根据所述目标惯性传感器状态信息对所述目标车辆进行定位。
可选的,所述装置还包括:
当前惯性传感器状态确定模块,用于在从目标车辆的当前帧雷达点云信息中提取激光特征点之前,确定目标车辆的当前惯性传感器状态信息;其中,所述惯性传感器状态信息用于表征所述惯性传感器的位姿信息;
当前雷达与相机位姿确定模块,用于根据所述当前惯性传感器状态信息确定所述目标车辆的当前雷达位姿信息和当前相机位姿信息。
可选的,所述当前惯性传感器状态确定模块,用于:
获取目标车辆的上一惯性传感器状态信息和当前惯性传感器数据;其中,所述惯性传感器数据包括陀螺仪数据和加速度计数据;
根据所述上一惯性传感器状态信息以及所述当前惯性传感器数据,确定所述目标车辆的当前惯性传感器状态信息。
可选的,所述初始激光特征点投影误差确定模块310,用于:
基于所述当前雷达位姿信息将所述激光特征点投影到激光点云地图中,根据所述激光特征点的投影结果确定初始激光特征点投影误差;
所述初始视觉特征点投影误差确定模块330,用于:
基于所述当前相机位姿信息将激光点云地图中与所述当前视觉特征点关联的点云信息投影到当前帧相机图像中。
可选的,所述退化场景判断模块320,用于:
基于所述初始激光特征点投影误差对第一关联误差函数的优化过程确定Hession矩阵;
对所述Hession矩阵进行特征值分解,得到候选特征值;
若所述候选特征值中的最小值小于预设特征值阈值,确定所述目标车辆在当前时刻处于退化场景;
否则,确定所述目标车辆在当前时刻未处于退化场景。
可选的,所述装置还包括:
当前相机点云信息确定模块,用于若所述当前视觉特征点的数量小于预设数量阈值,则将当前时刻下位于相机视野的激光点云地图中的点云信息作为当前相机点云信息;
当前帧相机图像信息更新模块,用于将所述当前相机点云信息投影到当前帧相机图像中,得到更新后的当前帧相机图像信息;
当前视觉特征点补充模块,用于从所述更新后的当前帧相机图像信息中提取候选视觉特征点,并基于所述候选视觉特征点对所述当前视觉特征点进行补充。
可选的,所述装置还包括:
候选惯性传感器状态信息确定模块,用于在确定所述目标车辆在当前时刻是否处于退化场景之后,若否,则基于所述初始激光特征点投影误差对第一关联误差函数的优化结果确定候选惯性传感器状态信息;其中,所述候选惯性传感器状态信息为最小激光特征点投影误差对应的惯性传感器状态信息;
第二车辆定位模块,用于根据所述候选惯性传感器状态信息对所述目标车辆进行定位。
本发明实施例所提供的一种基于惯性里程计的车辆定位装置可执行本发明任意实施例所提供的一种基于惯性里程计的车辆定位方法,具备执行方法相应的功能模块和有益效果。
实施例四
图4示出了可以用来实施本发明的实施例的电子设备10的结构示意图。电子设备旨在表示各种形式的数字计算机,诸如,膝上型计算机、台式计算机、工作台、个人数字助理、服务器、刀片式服务器、大型计算机、和其它适合的计算机。电子设备还可以表示各种形式的移动装置,诸如,个人数字处理、蜂窝电话、智能电话、可穿戴设备(如头盔、眼镜、手表等)和其它类似的计算装置。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示例,并且不意在限制本文中描述的和/或者要求的本发明的实现。
如图4所示,电子设备10包括至少一个处理器11,以及与至少一个处理器11通信连接的存储器,如只读存储器(ROM)12、随机访问存储器(RAM)13等,其中,存储器存储有可被至少一个处理器执行的计算机程序,处理器11可以根据存储在只读存储器(ROM)12中的计算机程序或者从存储单元18加载到随机访问存储器(RAM)13中的计算机程序,来执行各种适当的动作和处理。在RAM 13中,还可存储电子设备10操作所需的各种程序和数据。处理器11、ROM 12以及RAM 13通过总线14彼此相连。输入/输出(I/O)接口15也连接至总线14。
电子设备10中的多个部件连接至I/O接口15,包括:输入单元16,例如键盘、鼠标等;输出单元17,例如各种类型的显示器、扬声器等;存储单元18,例如磁盘、光盘等;以及通信单元19,例如网卡、调制解调器、无线通信收发机等。通信单元19允许电子设备10通过诸如因特网的计算机网络和/或各种电信网络与其他设备交换信息/数据。
处理器11可以是各种具有处理和计算能力的通用和/或专用处理组件。处理器11的一些示例包括但不限于中央处理单元(CPU)、图形处理单元(GPU)、各种专用的人工智能(AI)计算芯片、各种运行机器学习模型算法的处理器、数字信号处理器(DSP)、以及任何适当的处理器、控制器、微控制器等。处理器11执行上文所描述的各个方法和处理,例如基于惯性里程计的车辆定位方法。
在一些实施例中,基于惯性里程计的车辆定位方法可被实现为计算机程序,其被有形地包含于计算机可读存储介质,例如存储单元18。在一些实施例中,计算机程序的部分或者全部可以经由ROM 12和/或通信单元19而被载入和/或安装到电子设备10上。当计算机程序加载到RAM 13并由处理器11执行时,可以执行上文描述的基于惯性里程计的车辆定位方法的一个或多个步骤。备选地,在其他实施例中,处理器11可以通过其他任何适当的方式(例如,借助于固件)而被配置为执行基于惯性里程计的车辆定位方法。
本文中以上描述的系统和技术的各种实施方式可以在数字电子电路系统、集成电路系统、现场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、芯片上系统(SOC)、负载可编程逻辑设备(CPLD)、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以包括:实施在一个或者多个计算机程序中,该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/或解释,该可编程处理器可以是专用或者通用可编程处理器,可以从存储系统、至少一个输入装置、和至少一个输出装置接收数据和指令,并且将数据和指令传输至该存储系统、该至少一个输入装置、和该至少一个输出装置。
用于实施本发明的方法的计算机程序可以采用一个或多个编程语言的任何组合来编写。这些计算机程序可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器,使得计算机程序当由处理器执行时使流程图和/或框图中所规定的功能/操作被实施。计算机程序可以完全在机器上执行、部分地在机器上执行,作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。
在本发明的上下文中,计算机可读存储介质可以是有形的介质,其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的计算机程序。计算机可读存储介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备,或者上述内容的任何合适组合。备选地,计算机可读存储介质可以是机器可读信号介质。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
为了提供与用户的交互,可以在电子设备上实施此处描述的系统和技术,该电子设备具有:用于向用户显示信息的显示装置(例如,CRT(阴极射线管)或者LCD(液晶显示器)监视器);以及键盘和指向装置(例如,鼠标或者轨迹球),用户可以通过该键盘和该指向装置来将输入提供给电子设备。其它种类的装置还可以用于提供与用户的交互;例如,提供给用户的反馈可以是任何形式的传感反馈(例如,视觉反馈、听觉反馈、或者触觉反馈);并且可以用任何形式(包括声输入、语音输入或者、触觉输入)来接收来自用户的输入。
可以将此处描述的系统和技术实施在包括后台部件的计算系统(例如,作为数据服务器)、或者包括中间件部件的计算系统(例如,应用服务器)、或者包括前端部件的计算系统(例如,具有图形用户界面或者网络浏览器的用户计算机,用户可以通过该图形用户界面或者该网络浏览器来与此处描述的系统和技术的实施方式交互)、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信(例如,通信网络)来将系统的部件相互连接。通信网络的示例包括:局域网(LAN)、广域网(WAN)、区块链网络和互联网。
计算系统可以包括客户端和服务器。客户端和服务器一般远离彼此并且通常通过通信网络进行交互。通过在相应的计算机上运行并且彼此具有客户端-服务器关系的计算机程序来产生客户端和服务器的关系。服务器可以是云服务器,又称为云计算服务器或云主机,是云计算服务体系中的一项主机产品,以解决了传统物理主机与VPS服务中,存在的管理难度大,业务扩展性弱的缺陷。
应该理解,可以使用上面所示的各种形式的流程,重新排序、增加或删除步骤。例如,本发明中记载的各步骤可以并行地执行也可以顺序地执行也可以不同的次序执行,只要能够实现本发明的技术方案所期望的结果,本文在此不进行限制。
上述具体实施方式,并不构成对本发明保护范围的限制。本领域技术人员应该明白的是,根据设计要求和其他因素,可以进行各种修改、组合、子组合和替代。任何在本发明的精神和原则之内所作的修改、等同替换和改进等,均应包含在本发明保护范围之内。
Claims (16)
1.一种基于惯性里程计的车辆定位方法,其特征在于,所述方法包括:
从目标车辆的当前帧雷达点云信息中提取激光特征点,将所述激光特征点与激光点云地图进行匹配得到初始激光特征点投影误差;其中,所述激光点云地图基于历史帧雷达点云信息确定;
基于所述初始激光特征点投影误差对第一关联误差函数的优化过程确定所述目标车辆在当前时刻是否处于退化场景;其中,所述第一关联误差函数用于表征惯性传感器状态与激光特征点投影误差之间的函数关系;
若是,则确定当前帧相机图像信息中的当前视觉特征点,并将激光点云地图中与所述当前视觉特征点关联的点云信息投影到当前帧相机图像中,根据投影结果确定初始视觉特征点投影误差;
根据所述第一关联误差函数和第二关联误差函数确定综合关联误差函数,基于所述初始激光特征点投影误差和所述初始视觉特征点投影误差对所述综合关联误差函数进行优化得到目标惯性传感器状态信息;其中,所述第二关联误差函数用于表征惯性传感器状态与视觉特征点投影误差之间的函数关系,所述目标惯性传感器状态信息为所述激光特征点投影误差与所述视觉特征点投影误差之和最小时对应的惯性传感器状态信息;
根据所述目标惯性传感器状态信息对所述目标车辆进行定位。
2.根据权利要求1所述的方法,其特征在于,在从目标车辆的当前帧雷达点云信息中提取激光特征点之前,所述方法还包括:
确定目标车辆的当前惯性传感器状态信息;其中,所述惯性传感器状态信息用于表征所述惯性传感器的位姿信息;
根据所述当前惯性传感器状态信息确定所述目标车辆的当前雷达位姿信息和当前相机位姿信息。
3.根据权利要求2所述的方法,其特征在于,确定目标车辆的当前惯性传感器状态信息,包括:
获取目标车辆的上一惯性传感器状态信息和当前惯性传感器数据;其中,所述惯性传感器数据包括陀螺仪数据和加速度计数据;
根据所述上一惯性传感器状态信息以及所述当前惯性传感器数据,确定所述目标车辆的当前惯性传感器状态信息。
4.根据权利要求2所述的方法,其特征在于,将所述激光特征点与激光点云地图进行匹配得到初始激光特征点投影误差,包括:
基于所述当前雷达位姿信息将所述激光特征点投影到激光点云地图中,根据所述激光特征点的投影结果确定初始激光特征点投影误差;
将激光点云地图中与所述当前视觉特征点关联的点云信息投影到当前帧相机图像中,包括:
基于所述当前相机位姿信息将激光点云地图中与所述当前视觉特征点关联的点云信息投影到当前帧相机图像中。
5.根据权利要求1所述的方法,其特征在于,基于所述初始激光特征点投影误差对第一关联误差函数的优化过程确定所述目标车辆在当前时刻是否处于退化场景,包括:
基于所述初始激光特征点投影误差对第一关联误差函数的优化过程确定Hession矩阵;
对所述Hession矩阵进行特征值分解,得到候选特征值;
若所述候选特征值中的最小值小于预设特征值阈值,确定所述目标车辆在当前时刻处于退化场景;
否则,确定所述目标车辆在当前时刻未处于退化场景。
6.根据权利要求1所述的方法,其特征在于,在确定当前帧相机图像信息中的当前视觉特征点之后,所述方法还包括:
若所述当前视觉特征点的数量小于预设数量阈值,则将当前时刻下位于相机视野的激光点云地图中的点云信息作为当前相机点云信息;
将所述当前相机点云信息投影到当前帧相机图像中,得到更新后的当前帧相机图像信息;
从所述更新后的当前帧相机图像信息中提取候选视觉特征点,并基于所述候选视觉特征点对所述当前视觉特征点进行补充。
7.根据权利要求1所述的方法,其特征在于,在确定所述目标车辆在当前时刻是否处于退化场景之后,所述方法还包括:
若否,则基于所述初始激光特征点投影误差对第一关联误差函数的优化结果确定候选惯性传感器状态信息;其中,所述候选惯性传感器状态信息为最小激光特征点投影误差对应的惯性传感器状态信息;
根据所述候选惯性传感器状态信息对所述目标车辆进行定位。
8.一种基于惯性里程计的车辆定位装置,其特征在于,所述装置包括:
初始激光特征点投影误差确定模块,用于从目标车辆的当前帧雷达点云信息中提取激光特征点,将所述激光特征点与激光点云地图进行匹配得到初始激光特征点投影误差;其中,所述激光点云地图基于历史帧雷达点云信息确定;
退化场景判断模块,用于基于所述初始激光特征点投影误差对第一关联误差函数的优化过程确定所述目标车辆在当前时刻是否处于退化场景;其中,所述第一关联误差函数用于表征惯性传感器状态与激光特征点投影误差之间的函数关系;
初始视觉特征点投影误差确定模块,用于若是,则确定当前帧相机图像信息中的当前视觉特征点,并将激光点云地图中与所述当前视觉特征点关联的点云信息投影到当前帧相机图像中,根据投影结果确定初始视觉特征点投影误差;
目标惯性传感器状态信息确定模块,用于根据所述第一关联误差函数和第二关联误差函数确定综合关联误差函数,基于所述初始激光特征点投影误差和所述初始视觉特征点投影误差对所述综合关联误差函数进行优化得到目标惯性传感器状态信息;其中,所述第二关联误差函数用于表征惯性传感器状态与视觉特征点投影误差之间的函数关系,所述目标惯性传感器状态信息为所述激光特征点投影误差与所述视觉特征点投影误差之和最小时对应的惯性传感器状态信息;
第一车辆定位模块,用于根据所述目标惯性传感器状态信息对所述目标车辆进行定位。
9.根据权利要求8所述的装置,其特征在于,所述装置还包括:
当前惯性传感器状态确定模块,用于在从目标车辆的当前帧雷达点云信息中提取激光特征点之前,确定目标车辆的当前惯性传感器状态信息;其中,所述惯性传感器状态信息用于表征所述惯性传感器的位姿信息;
当前雷达与相机位姿确定模块,用于根据所述当前惯性传感器状态信息确定所述目标车辆的当前雷达位姿信息和当前相机位姿信息。
10.根据权利要求9所述的装置,其特征在于,所述当前惯性传感器状态确定模块,用于:
获取目标车辆的上一惯性传感器状态信息和当前惯性传感器数据;其中,所述惯性传感器数据包括陀螺仪数据和加速度计数据;
根据所述上一惯性传感器状态信息以及所述当前惯性传感器数据,确定所述目标车辆的当前惯性传感器状态信息。
11.根据权利要求9所述的装置,其特征在于,所述初始激光特征点投影误差确定模块,用于:
基于所述当前雷达位姿信息将所述激光特征点投影到激光点云地图中,根据所述激光特征点的投影结果确定初始激光特征点投影误差;
所述初始视觉特征点投影误差确定模块,用于:
基于所述当前相机位姿信息将激光点云地图中与所述当前视觉特征点关联的点云信息投影到当前帧相机图像中。
12.根据权利要求8所述的装置,其特征在于,所述退化场景判断模块,用于:
基于所述初始激光特征点投影误差对第一关联误差函数的优化过程确定Hession矩阵;
对所述Hession矩阵进行特征值分解,得到候选特征值;
若所述候选特征值中的最小值小于预设特征值阈值,确定所述目标车辆在当前时刻处于退化场景;
否则,确定所述目标车辆在当前时刻未处于退化场景。
13.根据权利要求8所述的装置,其特征在于,所述装置还包括:
当前相机点云信息确定模块,用于若所述当前视觉特征点的数量小于预设数量阈值,则将当前时刻下位于相机视野的激光点云地图中的点云信息作为当前相机点云信息;
当前帧相机图像信息更新模块,用于将所述当前相机点云信息投影到当前帧相机图像中,得到更新后的当前帧相机图像信息;
当前视觉特征点补充模块,用于从所述更新后的当前帧相机图像信息中提取候选视觉特征点,并基于所述候选视觉特征点对所述当前视觉特征点进行补充。
14.根据权利要求8所述的装置,其特征在于,所述装置还包括:
候选惯性传感器状态信息确定模块,用于在确定所述目标车辆在当前时刻是否处于退化场景之后,若否,则基于所述初始激光特征点投影误差对第一关联误差函数的优化结果确定候选惯性传感器状态信息;其中,所述候选惯性传感器状态信息为最小激光特征点投影误差对应的惯性传感器状态信息;
第二车辆定位模块,用于根据所述候选惯性传感器状态信息对所述目标车辆进行定位。
15.一种电子设备,其特征在于,所述电子设备包括:
至少一个处理器;以及
与所述至少一个处理器通信连接的存储器;其中,
所述存储器存储有可被所述至少一个处理器执行的计算机程序,所述计算机程序被所述至少一个处理器执行,以使所述至少一个处理器能够执行权利要求1-7中任一项所述的基于惯性里程计的车辆定位方法。
16.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质存储有计算机指令,所述计算机指令用于使处理器执行时实现权利要求1-7中任一项所述的基于惯性里程计的车辆定位方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311403080.0A CN117434515A (zh) | 2023-10-26 | 2023-10-26 | 基于惯性里程计的车辆定位方法、装置、设备及介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311403080.0A CN117434515A (zh) | 2023-10-26 | 2023-10-26 | 基于惯性里程计的车辆定位方法、装置、设备及介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN117434515A true CN117434515A (zh) | 2024-01-23 |
Family
ID=89547533
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202311403080.0A Pending CN117434515A (zh) | 2023-10-26 | 2023-10-26 | 基于惯性里程计的车辆定位方法、装置、设备及介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117434515A (zh) |
-
2023
- 2023-10-26 CN CN202311403080.0A patent/CN117434515A/zh active Pending
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US9709404B2 (en) | Iterative Kalman Smoother for robust 3D localization for vision-aided inertial navigation | |
CN113405545B (zh) | 定位方法、装置、电子设备及计算机存储介质 | |
CN114013449B (zh) | 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆 | |
CN111623773B (zh) | 一种基于鱼眼视觉和惯性测量的目标定位方法及装置 | |
CN114217665A (zh) | 一种相机和激光雷达时间同步方法、装置及存储介质 | |
CN114018269B (zh) | 定位方法、装置、电子设备、存储介质以及自动驾驶车辆 | |
CN117232499A (zh) | 多传感器融合的点云地图构建方法、装置、设备及介质 | |
CN114662587A (zh) | 一种基于激光雷达的三维目标感知方法、装置及系统 | |
CN113932796A (zh) | 高精地图车道线生成方法、装置和电子设备 | |
CN111783611B (zh) | 无人车的定位方法、装置、无人车及存储介质 | |
CN116958452A (zh) | 三维重建方法和系统 | |
CN116777966A (zh) | 一种农田路面环境下车辆航向角的计算方法 | |
CN115773759A (zh) | 自主移动机器人的室内定位方法、装置、设备及存储介质 | |
CN117434515A (zh) | 基于惯性里程计的车辆定位方法、装置、设备及介质 | |
CN113984072B (zh) | 车辆定位方法、装置、设备、存储介质及自动驾驶车辆 | |
CN114299192B (zh) | 定位建图的方法、装置、设备和介质 | |
CN115790621A (zh) | 高精地图更新方法、装置及电子设备 | |
CN116129422A (zh) | 单目3d目标检测方法、装置、电子设备和存储介质 | |
CN115147561A (zh) | 位姿图生成方法、高精地图生成方法和装置 | |
CN116448105B (zh) | 位姿更新方法、装置、电子设备和存储介质 | |
CN114612544B (zh) | 图像处理方法、装置、设备和存储介质 | |
CN117058430B (zh) | 用于视场匹配的方法、装置、电子设备和存储介质 | |
CN117348031A (zh) | 一种车辆定位系统、方法及介质 | |
CN116380056B (zh) | 惯性定位方法、装置、电子设备及存储介质 | |
CN117058250A (zh) | 一种基于相机的3d目标检测方法、装置、设备及介质 |
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 |