CN115326068B - 激光雷达-惯性测量单元融合里程计设计方法及系统 - Google Patents

激光雷达-惯性测量单元融合里程计设计方法及系统 Download PDF

Info

Publication number
CN115326068B
CN115326068B CN202211263739.2A CN202211263739A CN115326068B CN 115326068 B CN115326068 B CN 115326068B CN 202211263739 A CN202211263739 A CN 202211263739A CN 115326068 B CN115326068 B CN 115326068B
Authority
CN
China
Prior art keywords
state
current
point cloud
radar
loop
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202211263739.2A
Other languages
English (en)
Other versions
CN115326068A (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.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
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 Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202211263739.2A priority Critical patent/CN115326068B/zh
Publication of CN115326068A publication Critical patent/CN115326068A/zh
Application granted granted Critical
Publication of CN115326068B publication Critical patent/CN115326068B/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
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • 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
    • 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/4808—Evaluating distance, position or velocity data
    • 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/497—Means for monitoring or calibrating
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
    • G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/60—Type of objects
    • G06V20/64—Three-dimensional objects
    • 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)
  • Theoretical Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Multimedia (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Computing Systems (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Databases & Information Systems (AREA)
  • Artificial Intelligence (AREA)
  • Health & Medical Sciences (AREA)
  • Electromagnetism (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种激光雷达‑惯性测量单元融合里程计设计方法及系统，属于里程计设计技术领域。根据IMU数据计算当前状态预测值，利用卡尔曼滤波器对当前状态预测值进行修正，得到当前状态，进一步根据当前状态对激光雷达点云数据进行畸变矫正，得到当前优化后点云。然后对当前状态进行回环检测，进一步对因子图进行更新优化，即可得到当前雷达采样时刻的优化后状态，并将该优化后状态以里程计的形式发布，融合卡尔曼滤波器和因子图优化方法减少了里程计的累积误差，可设计得到在长距离工况下仍然具有高精度的里程计。

Description

激光雷达-惯性测量单元融合里程计设计方法及系统
技术领域
本发明涉及里程计设计技术领域，特别是涉及一种无人驾驶车辆的激光雷达-惯性测量单元融合里程计设计方法及系统。
背景技术
非结构化道路是当前无人驾驶车辆常面临的路况，但是这种路况通常较为恶劣，无人驾驶车辆在其上行驶时常常会产生剧烈颠簸。采用激光雷达或相机等单一传感器来设计非结构化道路上行驶的无人驾驶车辆的里程计往往会出现失效。
惯性测量单元（Inertial Measurement Unit，IMU）的频率较高并且可以利用其进行运动估计，可提高算法的鲁棒性；视觉传感器具有对光照和天气条件更为敏感的缺点，与之相比激光雷达（Laser Radar，Lidar）的效果更为稳定，受环境条件变化的影响较小。目前的研究表明Lidar和IMU紧耦合可以有效减少IMU的累积误差并提高里程计的精度，在此过程中，卡尔曼滤波器常常被用来进行车辆状态估计，卡尔曼滤波器可以有效的利用Lidar点云数据和IMU数据进行状态估计，得到较为准确的里程计。上述方法在短距离工况下具有良好的表现，但由于即便经过滤波，里程计误差依然会进行不断的累积，最终导致里程计的巨大漂移，故仅依赖卡尔曼滤波器无法得到长距离工况下的高精度里程计。因此需要采用其他优化方法消除累积误差，得到长距离工况下精准的里程计。
基于此，亟需一种可以在长距离工况下仍然具有高精度的里程计的设计方法。
发明内容
本发明的目的是提供一种激光雷达-惯性测量单元融合里程计设计方法及系统，融合卡尔曼滤波器和因子图优化方法设计得到在长距离工况下仍然具有高精度的里程计。
为实现上述目的，本发明提供了如下方案：
一种激光雷达-惯性测量单元融合里程计设计方法，包括：
获取当前雷达采样时刻的激光雷达点云数据和从上一雷达采样时刻到所述当前雷达采样时刻之间的每一惯导采样时刻的IMU数据；
根据所述上一雷达采样时刻和所述当前雷达采样时刻之间的所有所述IMU数据以及所述上一雷达采样时刻的状态计算无人驾驶车辆在所述当前雷达采样时刻的当前状态预测值，并利用卡尔曼滤波器对所述当前状态预测值进行修正，得到当前状态估计值，以所述当前状态估计值作为当前状态；将所述当前状态存储到状态数据库中，所述状态数据库包括从起始雷达采样时刻到所述当前雷达采样时刻以内的每一雷达采样时刻的状态；所述状态包括所述无人驾驶车辆的位置和姿态；
根据所述当前状态以及所述上一雷达采样时刻和所述当前雷达采样时刻之间的所有所述IMU数据进行反向传播，矫正所述激光雷达点云数据的畸变，得到当前优化后点云；将所述当前优化后点云存储到点云数据库中，所述点云数据库包括从所述起始雷达采样时刻到所述当前雷达采样时刻以内的每一雷达采样时刻的优化后点云；
基于所述点云数据库对所述当前状态进行回环检测；
若检测到回环，则确定所述状态数据库中与所述当前状态构成回环的回环状态，并计算所述当前状态和所述回环状态之间的平移旋转矩阵，得到回环因子，计算所述当前状态和所述上一雷达采样时刻的优化后状态之间的平移旋转矩阵，得到相邻约束因子；将所述当前状态以状态节点的形式添加到因子图中，并将所述回环因子和所述相邻约束因子添加到所述因子图中，得到更新后因子图；利用GTSAM库对所述更新后因子图中位于所述回环状态对应的状态节点和所述当前状态对应的状态节点以内的所有状态节点进行优化，得到优化后因子图；所述因子图包括所述当前雷达采样时刻之前的每一雷达采样时刻的状态节点和各个所述状态节点之间的回环因子和相邻约束因子；
若未检测到回环，则计算所述当前状态和所述上一雷达采样时刻的优化后状态之间的平移旋转矩阵，得到相邻约束因子；将所述当前状态以状态节点的形式添加到因子图中，并将所述相邻约束因子添加到所述因子图中，得到更新后因子图；利用GTSAM库对所述更新后因子图中位于所述上一雷达采样时刻的优化后状态对应的状态节点和所述当前状态对应的状态节点以内的所有状态节点进行优化，得到优化后因子图；
根据所述优化后因子图中所述当前状态对应的状态节点确定所述当前雷达采样时刻的优化后状态，将所述优化后状态以里程计的形式发布，并以所述优化后因子图作为下一雷达采样时刻的因子图，返回“获取当前雷达采样时刻的激光雷达点云数据和从上一雷达采样时刻到所述当前雷达采样时刻之间的每一惯导采样时刻的IMU数据”的步骤，直至所述无人驾驶车辆停止运动。
一种激光雷达-惯性测量单元融合里程计设计系统，包括：
数据获取模块，用于获取当前雷达采样时刻的激光雷达点云数据和从上一雷达采样时刻到所述当前雷达采样时刻之间的每一惯导采样时刻的IMU数据；
状态估计模块，用于根据所述上一雷达采样时刻和所述当前雷达采样时刻之间的所有所述IMU数据以及所述上一雷达采样时刻的状态计算无人驾驶车辆在所述当前雷达采样时刻的当前状态预测值，并利用卡尔曼滤波器对所述当前状态预测值进行修正，得到当前状态估计值，以所述当前状态估计值作为当前状态；将所述当前状态存储到状态数据库中，所述状态数据库包括从起始雷达采样时刻到所述当前雷达采样时刻以内的每一雷达采样时刻的状态；所述状态包括所述无人驾驶车辆的位置和姿态；
点云矫正模块，用于根据所述当前状态以及所述上一雷达采样时刻和所述当前雷达采样时刻之间的所有所述IMU数据进行反向传播，矫正所述激光雷达点云数据的畸变，得到当前优化后点云；将所述当前优化后点云存储到点云数据库中，所述点云数据库包括从所述起始雷达采样时刻到所述当前雷达采样时刻以内的每一雷达采样时刻的优化后点云；
回环检测模块，用于基于所述点云数据库对所述当前状态进行回环检测；
第一因子图优化模块，用于若检测到回环，则确定所述状态数据库中与所述当前状态构成回环的回环状态，并计算所述当前状态和所述回环状态之间的平移旋转矩阵，得到回环因子，计算所述当前状态和所述上一雷达采样时刻的优化后状态之间的平移旋转矩阵，得到相邻约束因子；将所述当前状态以状态节点的形式添加到因子图中，并将所述回环因子和所述相邻约束因子添加到所述因子图中，得到更新后因子图；利用GTSAM库对所述更新后因子图中位于所述回环状态对应的状态节点和所述当前状态对应的状态节点以内的所有状态节点进行优化，得到优化后因子图；所述因子图包括所述当前雷达采样时刻之前的每一雷达采样时刻的状态节点和各个所述状态节点之间的回环因子和相邻约束因子；
第二因子图优化模块，用于若未检测到回环，则计算所述当前状态和所述上一雷达采样时刻的优化后状态之间的平移旋转矩阵，得到相邻约束因子；将所述当前状态以状态节点的形式添加到因子图中，并将所述相邻约束因子添加到所述因子图中，得到更新后因子图；利用GTSAM库对所述更新后因子图中位于所述上一雷达采样时刻的优化后状态对应的状态节点和所述当前状态对应的状态节点以内的所有状态节点进行优化，得到优化后因子图；
里程计发布模块，用于根据所述优化后因子图中所述当前状态对应的状态节点确定所述当前雷达采样时刻的优化后状态，将所述优化后状态以里程计的形式发布，并以所述优化后因子图作为下一雷达采样时刻的因子图，返回“获取当前雷达采样时刻的激光雷达点云数据和从上一雷达采样时刻到所述当前雷达采样时刻之间的每一惯导采样时刻的IMU数据”的步骤，直至所述无人驾驶车辆停止运动。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明用于提供一种激光雷达-惯性测量单元融合里程计设计方法及系统，根据IMU数据计算当前状态预测值，利用卡尔曼滤波器对当前状态预测值进行修正，得到当前状态，进一步根据当前状态对激光雷达点云数据进行畸变矫正，得到当前优化后点云。然后对当前状态进行回环检测，以同时生成回环因子和相邻约束因子或仅生成相邻约束因子，进一步对因子图进行更新优化，即可得到当前雷达采样时刻的优化后状态，并将该优化后状态以里程计的形式发布，融合卡尔曼滤波器和因子图优化方法减少了里程计的累积误差，可设计得到在长距离工况下仍然具有高精度的里程计。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例1所提供的设计方法的原理框图；
图2为本发明实施例2所提供的设计系统的系统框图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种激光雷达-惯性测量单元融合里程计设计方法及系统，融合卡尔曼滤波器和因子图优化方法设计得到在长距离工况下仍然具有高精度的里程计。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
实施例1：
本实施例用于提供一种激光雷达-惯性测量单元融合里程计设计方法，如图1所示，包括：
步骤一：获取当前雷达采样时刻的激光雷达点云数据和从上一雷达采样时刻到所述当前雷达采样时刻之间的每一惯导采样时刻的IMU数据；
本实施例在无人驾驶车辆上安装激光雷达和惯性测量单元，以实时采集得到车辆外部环境的三维的激光雷达点云数据和车辆的IMU数据，IMU数据包括车辆的角速度和加速度，IMU测量值u可表示为
Figure 285470DEST_PATH_IMAGE001
，w m 为IMU测量的车辆的角速度，a m 为IMU测量的车辆的加速度。需要说明的是，本实施例中激光雷达的采集频率低于惯性测量单元的采集频率，即激光雷达以一个较低的频率提供激光雷达点云数据，惯性测量单元以一个较高的频率提供IMU数据，将采集到激光雷达点云数据的时刻记为雷达采样时刻，采集到IMU数据的时刻记为惯导采样时刻，则在两个雷达采样时刻之间，会存在多个惯导采样时刻。
步骤二：根据所述上一雷达采样时刻和所述当前雷达采样时刻之间的所有所述IMU数据以及所述上一雷达采样时刻的状态计算无人驾驶车辆在所述当前雷达采样时刻的当前状态预测值，并利用卡尔曼滤波器对所述当前状态预测值进行修正，得到当前状态估计值，以所述当前状态估计值作为当前状态；将所述当前状态存储到状态数据库中，所述状态数据库包括从起始雷达采样时刻到所述当前雷达采样时刻以内的每一雷达采样时刻的状态；所述状态包括所述无人驾驶车辆的位置和姿态；
步骤二中，根据上一雷达采样时刻和当前雷达采样时刻之间的所有IMU数据以及上一雷达采样时刻的状态计算无人驾驶车辆在当前雷达采样时刻的当前状态预测值可以包括：以上一雷达采样时刻的状态作为初始状态，根据上一雷达采样时刻和当前雷达采样时刻之间的所有IMU数据对初始状态进行递推，得到无人驾驶车辆在当前雷达采样时刻的当前状态预测值。
具体的，车辆状态可表示为：
Figure 952074DEST_PATH_IMAGE002
，其中，
Figure 234151DEST_PATH_IMAGE003
为根据上一惯导采样时刻的姿态和IMU测量值估计得到的当前惯导采样时刻的姿态，即欧拉角；
Figure 840451DEST_PATH_IMAGE004
为根据上一惯导采样时刻的位置和IMU测量值估计得到的当前惯导采样时刻的位置，即坐标；
Figure 546851DEST_PATH_IMAGE005
为根据上一惯导采样时刻的速度和IMU测量值估计得到的当前惯导采样时刻的速度，速度为矢量，包含大小和方向；b w ，b a 为根据n bw ，n ba 得到的IMU误差，即为n bw ，n ba 驱动下的随机游动过程的IMU误差；
Figure 700752DEST_PATH_IMAGE006
为重力加速度，其是已知的。IMU数据还包括IMU噪声，IMU噪声
Figure 786520DEST_PATH_IMAGE007
；n w ，n a 为w m ，a m 的测量噪声，其可以通过测量得到；n bw ，n ba 为IMU的偏置，其可以通过测量得到。
本实施例以IMU的采样周期构建离散的运动学模型，利用该运动学模型实现车辆状态的预测过程，运动学模型为：
Figure 342266DEST_PATH_IMAGE008
；
其中，x i+1为第i+1惯导采样时刻的车辆状态；x i 为第i惯导采样时刻的车辆状态；
Figure 488076DEST_PATH_IMAGE009
为IMU的采样周期，也即相邻两个惯导采样时刻之间的时间差；
Figure 504441DEST_PATH_IMAGE010
为车辆状态、IMU测量值和IMU噪声之间的函数关系。
Figure 128320DEST_PATH_IMAGE011
；
按照上述运动学模型，以初始状态为输入，利用第一惯导采样时刻的IMU数据即可预测得到第二惯导采样时刻的车辆状态，以第二惯导采样时刻的车辆状态为输入，利用第二惯导采样时刻的IMU数据即可预测得到第三惯导采样时刻的车辆状态，依次递推，即可得到最后一个惯导采样时刻的车辆状态，其即为当前状态预测值，本实施例即通过运动学模型的前向传播过程来获得当前状态预测值。
步骤二中，利用卡尔曼滤波器对当前状态预测值进行修正，得到当前状态估计值可以包括：将激光雷达点云数据中的每一个点投影到车辆坐标系下；对于激光雷达点云数据中的每一个点，在车辆坐标系下确定距离该点最近的N个点，并将N个点拟合成一个局部平面，N可为5；计算该点到局部平面的距离，得到点的残差；以所有点的残差以及当前状态预测值作为输入，利用卡尔曼滤波器进行迭代计算，得到当前状态估计值，并以当前状态估计值作为当前状态，即将残差值和当前状态预测值输入给迭代误差卡尔曼滤波器进行估计，收敛后的最优状态即为当前状态。卡尔曼滤波器的原理为：拟合平面计算残差的过程可以由一个测量模型表示，将该测量模型与前向传播结合就可以得到针对车辆状态的最大后验估计问题，该最大后验估计问题可以由迭代误差卡尔曼滤波器解决，收敛后的最优状态被认为是当前状态。上述卡尔曼滤波器修正过程属于现有技术，在此不再赘述。
将当前状态按照对应的索引（索引即为其所对应的雷达采样时刻k）存储到状态数据库（也可称为位姿vector）中，状态数据库包括从起始雷达采样时刻到当前雷达采样时刻以内的每一雷达采样时刻的状态，状态数据库P可表示为：
Figure 804152DEST_PATH_IMAGE012
；
其中，x k 为第k个雷达采样时刻的状态。
步骤三：根据所述当前状态以及所述上一雷达采样时刻和所述当前雷达采样时刻之间的所有所述IMU数据进行反向传播，矫正所述激光雷达点云数据的畸变，得到当前优化后点云；将所述当前优化后点云存储到点云数据库中，所述点云数据库包括从所述起始雷达采样时刻到所述当前雷达采样时刻以内的每一雷达采样时刻的优化后点云；
虽然激光雷达点云数据是在当前雷达采样时刻t k 产生的，但激光雷达点云数据中的不同的点是在不同的采集时间p j 得到的，故每一个点都会对应一个采集时间，这样就会导致在当前雷达采样时刻获得的激光雷达点云数据存在一定的运动畸变。考虑到前向传播是在IMU数据到达时进行，并会一直持续这个过程，直到到达当前雷达采样时刻，故本实施例试图通过当前雷达采样时刻的当前状态对激光雷达点云数据进行畸变校正。
为了补偿p j 到t k 之间相对运动造成的运动失真，从零位姿和静止状态开始，将运动学模型反向传播得到下式所述的反向运动学模型：
Figure 275192DEST_PATH_IMAGE013
；
本实施例则利用反向运动学模型进行反向传播，以实现矫正点云的过程。其原理如下：根据IMU数据，利用运动学模型不断递推就可以由上一雷达采样时刻的状态实现当前雷达采样时刻的状态的预测，即运动学模型是由上一雷达采样时刻的状态递推得到当前雷达采样时刻的状态，那么将运动学模型反过来所得到的反向运动学模型，就是利用当前雷达采样时刻的状态递推得到其与上一雷达采样时刻之间的各个惯导采样时刻的车辆状态，以当前状态和当前雷达采样时刻与上一雷达采样时刻之间的各个惯导采样时刻的所有IMU数据作为输入，利用反向运动学模型进行计算，就可以得到各个惯导采样时刻的位置，并以此完成点云的畸变矫正。
具体的，根据当前状态以及上一雷达采样时刻和当前雷达采样时刻之间的所有IMU数据进行反向传播，矫正激光雷达点云数据的畸变，得到当前优化后点云可以包括：根据当前状态以及上一雷达采样时刻和当前雷达采样时刻之间的所有IMU数据计算无人驾驶车辆在每一惯导采样时刻的位置，计算位置的过程利用上述反向传播过程实现；将激光雷达点云数据中的各个点按照采集时间分为N个集合，每一集合对应一惯导采样时刻；举例而言，上一雷达采样时刻为0s，下一雷达采样时刻为1s，惯导采样时刻分别为0.2s，0.4s，0.6s，0.8s和1s，则可根据激光雷达点云数据所包括的各个点的采集时间，将各个点分到0-0.2s，0.2-0.4s，0.4-0.6s，0.6-0.8s，0.8-1s这5个集合内，0-0.2s这一集合对应的惯导采样时刻即为0.2s，同理确定其他集合对应的惯导采样时刻；对于每一集合，将集合内的点的位置替换为集合对应的惯导采样时刻的位置，得到当前优化后点云。
将当前优化后点云按照索引k存储到点云数据库（也可称为点云vector）中，点云数据库包括从起始雷达采样时刻到当前雷达采样时刻以内的每一雷达采样时刻的优化后点云，点云数据库可表示为：
Figure 403685DEST_PATH_IMAGE014
；
其中，C k 为第k个雷达采样时刻的优化后点云。
步骤四：基于所述点云数据库对所述当前状态进行回环检测；
利用回环检测判断车辆是否到达历史位置，并计算车辆两次到达同一位置时位姿之间的平移旋转矩阵，进而利用该平移旋转矩阵对历史位姿进行非线性优化，是在不依赖GNSS约束的条件下常用的优化方法。目前已有许多的算法如LeGO-LOAM，ORB-SLAM3验证了这种优化方法的可靠性和良好表现。本实施例借鉴LeGO-LOAM算法中所采用的回环检测策略，采用欧几里得距离、LiDAR-Iris描述子与ICP算法相结合的回环检测方法进行回环检测，其以1hz的频率独立于主线程运行，根据设定的回环检测算法，以一定的频率在一个单独的线程不断进行回环检测，搜索与当前状态在一定距离之内并且在一定时间之前的历史状态作为潜在回环状态，将潜在回环状态对应的优化后点云作为潜在回环点云，潜在回环点云与其前N帧优化后点云和其后M帧优化后点云合并以构建目标点云，将当前优化后点云与目标点云进行匹配，得到平移旋转矩阵及匹配分数，若匹配分数满足要求，则认为产生回环，潜在回环状态即为回环状态，当前优化后点云与目标点云匹配所得到的平移旋转矩阵即为当前状态和回环状态之间的回环因子。
则步骤四可以包括：
（1）根据当前状态，在第一距离阈值r nearby 和时间阈值的约束下，利用k-dimensional tree (k-d tree)在状态数据库中搜索，直至得到一个第一状态；为保证运算实时性的同时扩大检测范围，本实施例同时根据当前状态，在第二距离阈值r lidariris 的约束下，利用k-d tree在状态数据库中搜索，得到若干个第二状态，第二距离阈值大于第一距离阈值；计算当前状态对应的描述子与每一第二状态对应的描述子的汉明距离，并判断所有汉明距离的最小值是否小于第三距离阈值，若是，则以最小值对应的第二状态作为第三状态；
在第一距离阈值r nearby 和时间阈值的约束下，利用k-d tree在状态数据库中搜索的过程为：在欧几里得空间内，计算当前状态的位置与状态数据库中的每一个状态的位置的欧几里得距离，并实时计算当前状态的索引与状态数据库中的每一个状态的索引之间的时间差，在时间差大于时间阈值且欧几里德距离小于第一距离阈值的诸多状态中选取欧几里德距离最小的状态作为第一状态，在得到一个第一状态后，则搜索过程结束。
在第二距离阈值r lidariris 的约束下，利用k-d tree在状态数据库中搜索的过程为：在欧几里得空间内，计算当前状态的位置与状态数据库中的每一个状态的位置的欧几里得距离，选取欧几里得距离小于第二距离阈值的所有状态作为第二状态。
在计算汉明距离之前，本实施例还包括：计算当前优化后点云对应的描述子，并将描述子存储到描述子数据库中，描述子数据库包括从起始雷达采样时刻到当前雷达采样时刻以内的每一雷达采样时刻的描述子，每一描述子均对应一索引。由于状态数据库、点云数据库和描述子数据库内的数据都具有索引，故状态、优化后点云和描述子可以通过索引一一对应，进而可根据当前状态的索引从描述子数据库中确定当前状态对应的描述子，根据第二状态的索引从描述子数据库中确定第二状态对应的描述子。
优选的，本实施例的描述子为LiDAR-Iris全局描述子，其可根据LiDAR-Iris这一现有算法计算得到，以优化后点云作为LiDAR-Iris这一现有算法的输入，即可得到优化后点云的LiDAR-Iris全局描述子。
（2）若先得到第一状态，则以第一状态作为潜在回环状态；若先得到第三状态，则以第三状态作为潜在回环状态；在点云数据库中选取潜在回环状态对应的优化后点云作为潜在回环点云；
本实施例的第一状态的获取过程以及第三状态的获取过程是并行进行的，在得到第一状态后，则不再进行搜索，直接以第一状态作为潜在回环状态，其对应的优化后点云作为潜在回环点云；在得到第三状态后，也不再进行搜索，直接以第三状态作为潜在回环状态，其对应的优化后点云作为潜在回环点云。
（3）根据潜在回环点云生成目标点云；
具体的，根据潜在回环点云生成目标点云可以包括：合并潜在回环点云、点云数据库中潜在回环点云的前N帧优化后点云以及点云数据库中潜在回环点云的后M帧优化后点云，得到目标点云，M、N可相同，也可不同。需要说明的是，前后是依据雷达采样时刻所定义的，潜在回环点云的前一帧优化后点云即为潜在回环点云的雷达采样时刻的前一雷达采样时刻对应的优化后点云。
（4）将当前优化后点云作为源点云，利用现有的ICP算法对源点云和目标点云进行匹配，得到匹配分数（ICP fitness Score）；若匹配分数小于分数阈值，则认为当前状态和潜在回环状态构成回环，该潜在回环状态即为与当前状态构成回环的回环状态。
步骤五：若检测到回环，则确定所述状态数据库中与所述当前状态构成回环的回环状态，并计算所述当前状态和所述回环状态之间的平移旋转矩阵，得到回环因子，计算所述当前状态和所述上一雷达采样时刻的优化后状态之间的平移旋转矩阵，得到相邻约束因子；将所述当前状态以状态节点的形式添加到因子图中，并将所述回环因子和所述相邻约束因子添加到所述因子图中，得到更新后因子图；利用GTSAM库对所述更新后因子图中位于所述回环状态对应的状态节点和所述当前状态对应的状态节点以内的所有状态节点进行优化，得到优化后因子图；所述因子图包括所述当前雷达采样时刻之前的每一雷达采样时刻的状态节点和各个所述状态节点之间的回环因子和相邻约束因子；
回环因子可直接利用ICP算法得到，ICP算法在对源点云和目标点云进行匹配时，不仅会得到匹配分数，还会得到当前状态的位姿（即位置和姿态）和潜在回环状态的位姿之间的平移旋转矩阵，如果匹配分数小于分数阈值，则认为产生回环，此时ICP算法所得到的平移旋转矩阵即为回环因子。
相邻约束因子可直接利用GTSAM库得到，将当前状态和上一雷达采样时刻的优化后状态输入到GTSAM库中，GTSAM库会自动计算出二者之间的平移旋转矩阵，得到相邻约束因子。
本实施例引入相邻约束因子和回环因子两个因子，并以车辆状态作为状态节点来构建因子图，相邻约束因子确定了相邻两状态节点之间的关系，回环因子确定了构成回环的两个状态节点之间的关系。
根据相邻状态节点之间的约束关系（相邻约束因子）和回环因子，利用GTSAM库就可以修正回环状态对应的状态节点和当前状态对应的状态节点以内的所有状态节点的位姿，完成优化目标。
步骤六：若未检测到回环，则计算所述当前状态和所述上一雷达采样时刻的优化后状态之间的平移旋转矩阵，得到相邻约束因子；将所述当前状态以状态节点的形式添加到因子图中，并将所述相邻约束因子添加到所述因子图中，得到更新后因子图；利用GTSAM库对所述更新后因子图中位于所述上一雷达采样时刻的优化后状态对应的状态节点和所述当前状态对应的状态节点以内的所有状态节点进行优化，得到优化后因子图；
步骤七：根据所述优化后因子图中所述当前状态对应的状态节点确定所述当前雷达采样时刻的优化后状态，将所述优化后状态以里程计的形式发布，并以所述优化后因子图作为下一雷达采样时刻的因子图，返回“获取当前雷达采样时刻的激光雷达点云数据和从上一雷达采样时刻到所述当前雷达采样时刻之间的每一惯导采样时刻的IMU数据”的步骤，直至所述无人驾驶车辆停止运动。
为了消除由于传感器误差和卡尔曼滤波器本身的局限性导致的累积状态误差，实现更准确的状态估计，本实施例在状态估计与因子图优化之间构成反馈关系，将因子图优化的位姿结果替换当前状态的位姿，以得到优化后状态x i ，如下：
Figure 831255DEST_PATH_IMAGE015
；
Figure 361593DEST_PATH_IMAGE016
；
Figure 583627DEST_PATH_IMAGE017
；
其中，roll，pitch，yaw，x，y，z为因子图优化后得到的欧拉角和位置坐标。
状态估计将在新的状态下继续进行，直到产生下一次优化，将里程计实时发布，用于非结构化道路下的车辆定位。
本实施例提出了一种应用于非结构化道路工况下无人驾驶车辆的激光雷达-惯性测量单元融合里程计设计方法，首先获取激光雷达提供的较低频率的激光雷达点云数据，融合高频率的IMU数据对点云的运动畸变进行矫正，在紧耦合迭代误差卡尔曼滤波的优化下得到车辆的当前位姿。通过k-d tree搜索到一定时间阈值之前的不同距离的邻近位姿及对应的点云，然后将LiDAR-Iris算法构建的描述子与ICP点云配准算法相结合判断是否构成回环，若回环检测成功则可利用ICP算法得到回环因子。随后通过相邻约束因子和回环因子在GTSAM库的帮助下实现因子图优化，将优化结果反馈给车辆位姿状态集合和当前位姿状态。最后，通过优化后的迭代误差卡尔曼滤波前向传播即可得到以准确的激光雷达-惯性里程计。本实施例采用迭代误差卡尔曼滤波器和因子图优化相结合设计高精度激光雷达-惯性测量单元融合里程计，将状态估计和位姿优化结合成反馈系统，减少了里程计累积误差，降低了优化和检测下一个回环的难度。回环检测方法结合欧氏距离、LiDAR-Iris描述子和ICP算法计算的匹配分数fitness Score，保证了实时性，减少了里程计大范围漂移的影响。状态估计结果直接作为车辆当前状态节点，通过当前状态与车辆上一雷达采样时刻的优化后状态之间的关系建立相邻约束因子，以减少优化的计算量。
实施例2：
本实施例用于提供一种激光雷达-惯性测量单元融合里程计设计系统，如图2所示，包括：
数据获取模块M1，用于获取当前雷达采样时刻的激光雷达点云数据和从上一雷达采样时刻到所述当前雷达采样时刻之间的每一惯导采样时刻的IMU数据；
状态估计模块M2，用于根据所述上一雷达采样时刻和所述当前雷达采样时刻之间的所有所述IMU数据以及所述上一雷达采样时刻的状态计算无人驾驶车辆在所述当前雷达采样时刻的当前状态预测值，并利用卡尔曼滤波器对所述当前状态预测值进行修正，得到当前状态估计值，以所述当前状态估计值作为当前状态；将所述当前状态存储到状态数据库中，所述状态数据库包括从起始雷达采样时刻到所述当前雷达采样时刻以内的每一雷达采样时刻的状态；所述状态包括所述无人驾驶车辆的位置和姿态；
点云矫正模块M3，用于根据所述当前状态以及所述上一雷达采样时刻和所述当前雷达采样时刻之间的所有所述IMU数据进行反向传播，矫正所述激光雷达点云数据的畸变，得到当前优化后点云；将所述当前优化后点云存储到点云数据库中，所述点云数据库包括从所述起始雷达采样时刻到所述当前雷达采样时刻以内的每一雷达采样时刻的优化后点云；
回环检测模块M4，用于基于所述点云数据库对所述当前状态进行回环检测；
第一因子图优化模块M5，用于若检测到回环，则确定所述状态数据库中与所述当前状态构成回环的回环状态，并计算所述当前状态和所述回环状态之间的平移旋转矩阵，得到回环因子，计算所述当前状态和所述上一雷达采样时刻的优化后状态之间的平移旋转矩阵，得到相邻约束因子；将所述当前状态以状态节点的形式添加到因子图中，并将所述回环因子和所述相邻约束因子添加到所述因子图中，得到更新后因子图；利用GTSAM库对所述更新后因子图中位于所述回环状态对应的状态节点和所述当前状态对应的状态节点以内的所有状态节点进行优化，得到优化后因子图；所述因子图包括所述当前雷达采样时刻之前的每一雷达采样时刻的状态节点和各个所述状态节点之间的回环因子和相邻约束因子；
第二因子图优化模块M6，用于若未检测到回环，则计算所述当前状态和所述上一雷达采样时刻的优化后状态之间的平移旋转矩阵，得到相邻约束因子；将所述当前状态以状态节点的形式添加到因子图中，并将所述相邻约束因子添加到所述因子图中，得到更新后因子图；利用GTSAM库对所述更新后因子图中位于所述上一雷达采样时刻的优化后状态对应的状态节点和所述当前状态对应的状态节点以内的所有状态节点进行优化，得到优化后因子图；
里程计发布模块M7，用于根据所述优化后因子图中所述当前状态对应的状态节点确定所述当前雷达采样时刻的优化后状态，将所述优化后状态以里程计的形式发布，并以所述优化后因子图作为下一雷达采样时刻的因子图，返回“获取当前雷达采样时刻的激光雷达点云数据和从上一雷达采样时刻到所述当前雷达采样时刻之间的每一惯导采样时刻的IMU数据”的步骤，直至所述无人驾驶车辆停止运动。
本实施例融合了卡尔曼滤波估计和回环优化获取激光雷达-惯性里程计，设计了在非结构化道路工况下更高精度的里程计，解决在非结构化道路下无人驾驶车辆难以获取长距离高精度里程计的问题。
本说明书中每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (8)

1.一种激光雷达-惯性测量单元融合里程计设计方法，其特征在于，包括：
步骤一：获取当前雷达采样时刻的激光雷达点云数据和从上一雷达采样时刻到所述当前雷达采样时刻之间的每一惯导采样时刻的IMU数据；
步骤二：根据所述上一雷达采样时刻和所述当前雷达采样时刻之间的所有所述IMU数据以及所述上一雷达采样时刻的状态计算无人驾驶车辆在所述当前雷达采样时刻的当前状态预测值，并利用卡尔曼滤波器对所述当前状态预测值进行修正，得到当前状态估计值，以所述当前状态估计值作为当前状态；将所述当前状态存储到状态数据库中，所述状态数据库包括从起始雷达采样时刻到所述当前雷达采样时刻以内的每一雷达采样时刻的状态；所述状态包括所述无人驾驶车辆的位置和姿态；
步骤三：根据所述当前状态以及所述上一雷达采样时刻和所述当前雷达采样时刻之间的所有所述IMU数据进行反向传播，矫正所述激光雷达点云数据的畸变，得到当前优化后点云；将所述当前优化后点云存储到点云数据库中，所述点云数据库包括从所述起始雷达采样时刻到所述当前雷达采样时刻以内的每一雷达采样时刻的优化后点云；
步骤四：基于所述点云数据库对所述当前状态进行回环检测；
所述步骤四具体包括：
根据所述当前状态，在第一距离阈值和时间阈值的约束下，利用k-d tree在所述状态数据库中搜索，直至得到一个第一状态；同时根据所述当前状态，在第二距离阈值的约束下，利用k-d tree在所述状态数据库中搜索，得到若干个第二状态；计算所述当前状态对应的描述子与每一所述第二状态对应的描述子的汉明距离，并判断所有所述汉明距离的最小值是否小于第三距离阈值，若是，则以所述最小值对应的第二状态作为第三状态；所述第二距离阈值大于所述第一距离阈值；
若先得到所述第一状态，则以所述第一状态作为潜在回环状态；若先得到所述第三状态，则以所述第三状态作为潜在回环状态；在所述点云数据库中选取所述潜在回环状态对应的优化后点云作为潜在回环点云；
根据所述潜在回环点云生成目标点云；
利用ICP算法对所述当前优化后点云和所述目标点云进行匹配，得到匹配分数；若所述匹配分数小于分数阈值，则所述当前状态和所述潜在回环状态构成回环，所述潜在回环状态即为与所述当前状态构成回环的回环状态；
步骤五：若检测到回环，则确定所述状态数据库中与所述当前状态构成回环的回环状态，并计算所述当前状态和所述回环状态之间的平移旋转矩阵，得到回环因子，利用GTSAM库计算所述当前状态和所述上一雷达采样时刻的优化后状态之间的平移旋转矩阵，得到相邻约束因子；将所述当前状态以状态节点的形式添加到因子图中，并将所述回环因子和所述相邻约束因子添加到所述因子图中，得到更新后因子图；利用GTSAM库对所述更新后因子图中位于所述回环状态对应的状态节点和所述当前状态对应的状态节点以内的所有状态节点进行优化，得到优化后因子图；所述因子图包括所述当前雷达采样时刻之前的每一雷达采样时刻的状态节点和各个所述状态节点之间的回环因子和相邻约束因子；
步骤六：若未检测到回环，则利用GTSAM库计算所述当前状态和所述上一雷达采样时刻的优化后状态之间的平移旋转矩阵，得到相邻约束因子；将所述当前状态以状态节点的形式添加到因子图中，并将所述相邻约束因子添加到所述因子图中，得到更新后因子图；利用GTSAM库对所述更新后因子图中位于所述上一雷达采样时刻的优化后状态对应的状态节点和所述当前状态对应的状态节点以内的所有状态节点进行优化，得到优化后因子图；
步骤七：根据所述优化后因子图中所述当前状态对应的状态节点确定所述当前雷达采样时刻的优化后状态，将所述优化后状态以里程计的形式发布，并以所述优化后因子图作为下一雷达采样时刻的因子图，返回步骤一，直至所述无人驾驶车辆停止运动。
2.根据权利要求1所述的一种激光雷达-惯性测量单元融合里程计设计方法，其特征在于，所述根据所述上一雷达采样时刻和所述当前雷达采样时刻之间的所有所述IMU数据以及所述上一雷达采样时刻的状态计算无人驾驶车辆在所述当前雷达采样时刻的当前状态预测值具体包括：以所述上一雷达采样时刻的状态作为初始状态，根据所述上一雷达采样时刻和所述当前雷达采样时刻之间的所有所述IMU数据对所述初始状态进行递推，得到无人驾驶车辆在所述当前雷达采样时刻的当前状态预测值。
3.根据权利要求1所述的一种激光雷达-惯性测量单元融合里程计设计方法，其特征在于，所述利用卡尔曼滤波器对所述当前状态预测值进行修正，得到当前状态估计值具体包括：
将所述激光雷达点云数据投影到车辆坐标系下；
对于所述激光雷达点云数据中的每一个点，在所述车辆坐标系下确定距离所述点最近的N个点，并将所述N个点拟合成一个局部平面；计算所述点到所述局部平面的距离，得到所述点的残差；
以所有所述点的残差以及所述当前状态预测值作为输入，利用卡尔曼滤波器进行迭代计算，得到当前状态估计值。
4.根据权利要求1所述的一种激光雷达-惯性测量单元融合里程计设计方法，其特征在于，所述根据所述当前状态以及所述上一雷达采样时刻和所述当前雷达采样时刻之间的所有所述IMU数据进行反向传播，矫正所述激光雷达点云数据的畸变，得到当前优化后点云具体包括：
根据所述当前状态以及所述上一雷达采样时刻和所述当前雷达采样时刻之间的所有所述IMU数据计算所述无人驾驶车辆在每一所述惯导采样时刻的位置；
将所述激光雷达点云数据中的各个点按照采集时间分为N个集合，每一所述集合对应一所述惯导采样时刻；
对于每一所述集合，将所述集合内的点的位置替换为所述集合对应的所述惯导采样时刻的位置，得到当前优化后点云。
5.根据权利要求1所述的一种激光雷达-惯性测量单元融合里程计设计方法，其特征在于，所述根据所述潜在回环点云生成目标点云具体包括：合并所述潜在回环点云、所述点云数据库中所述潜在回环点云的前N帧优化后点云以及所述点云数据库中所述潜在回环点云的后M帧优化后点云，得到目标点云。
6.根据权利要求1所述的一种激光雷达-惯性测量单元融合里程计设计方法，其特征在于，在所述计算所述当前状态对应的描述子与每一所述第二状态对应的描述子的汉明距离之前，还包括：计算所述当前优化后点云对应的描述子，并将所述描述子存储到描述子数据库中；所述描述子数据库包括从所述起始雷达采样时刻到所述当前雷达采样时刻以内的每一雷达采样时刻的描述子。
7.根据权利要求6所述的一种激光雷达-惯性测量单元融合里程计设计方法，其特征在于，所述描述子为LiDAR-Iris全局描述子。
8.一种激光雷达-惯性测量单元融合里程计设计系统，其特征在于，包括：
数据获取模块，用于获取当前雷达采样时刻的激光雷达点云数据和从上一雷达采样时刻到所述当前雷达采样时刻之间的每一惯导采样时刻的IMU数据；
状态估计模块，用于根据所述上一雷达采样时刻和所述当前雷达采样时刻之间的所有所述IMU数据以及所述上一雷达采样时刻的状态计算无人驾驶车辆在所述当前雷达采样时刻的当前状态预测值，并利用卡尔曼滤波器对所述当前状态预测值进行修正，得到当前状态估计值，以所述当前状态估计值作为当前状态；将所述当前状态存储到状态数据库中，所述状态数据库包括从起始雷达采样时刻到所述当前雷达采样时刻以内的每一雷达采样时刻的状态；所述状态包括所述无人驾驶车辆的位置和姿态；
点云矫正模块，用于根据所述当前状态以及所述上一雷达采样时刻和所述当前雷达采样时刻之间的所有所述IMU数据进行反向传播，矫正所述激光雷达点云数据的畸变，得到当前优化后点云；将所述当前优化后点云存储到点云数据库中，所述点云数据库包括从所述起始雷达采样时刻到所述当前雷达采样时刻以内的每一雷达采样时刻的优化后点云；
回环检测模块，用于基于所述点云数据库对所述当前状态进行回环检测；具体包括：根据所述当前状态，在第一距离阈值和时间阈值的约束下，利用k-d tree在所述状态数据库中搜索，直至得到一个第一状态；同时根据所述当前状态，在第二距离阈值的约束下，利用k-d tree在所述状态数据库中搜索，得到若干个第二状态；计算所述当前状态对应的描述子与每一所述第二状态对应的描述子的汉明距离，并判断所有所述汉明距离的最小值是否小于第三距离阈值，若是，则以所述最小值对应的第二状态作为第三状态；所述第二距离阈值大于所述第一距离阈值；
若先得到所述第一状态，则以所述第一状态作为潜在回环状态；若先得到所述第三状态，则以所述第三状态作为潜在回环状态；在所述点云数据库中选取所述潜在回环状态对应的优化后点云作为潜在回环点云；
根据所述潜在回环点云生成目标点云；
利用ICP算法对所述当前优化后点云和所述目标点云进行匹配，得到匹配分数；若所述匹配分数小于分数阈值，则所述当前状态和所述潜在回环状态构成回环，所述潜在回环状态即为与所述当前状态构成回环的回环状态；
第一因子图优化模块，用于若检测到回环，则确定所述状态数据库中与所述当前状态构成回环的回环状态，并计算所述当前状态和所述回环状态之间的平移旋转矩阵，得到回环因子，利用GTSAM库计算所述当前状态和所述上一雷达采样时刻的优化后状态之间的平移旋转矩阵，得到相邻约束因子；将所述当前状态以状态节点的形式添加到因子图中，并将所述回环因子和所述相邻约束因子添加到所述因子图中，得到更新后因子图；利用GTSAM库对所述更新后因子图中位于所述回环状态对应的状态节点和所述当前状态对应的状态节点以内的所有状态节点进行优化，得到优化后因子图；所述因子图包括所述当前雷达采样时刻之前的每一雷达采样时刻的状态节点和各个所述状态节点之间的回环因子和相邻约束因子；
第二因子图优化模块，用于若未检测到回环，则利用GTSAM库计算所述当前状态和所述上一雷达采样时刻的优化后状态之间的平移旋转矩阵，得到相邻约束因子；将所述当前状态以状态节点的形式添加到因子图中，并将所述相邻约束因子添加到所述因子图中，得到更新后因子图；利用GTSAM库对所述更新后因子图中位于所述上一雷达采样时刻的优化后状态对应的状态节点和所述当前状态对应的状态节点以内的所有状态节点进行优化，得到优化后因子图；
里程计发布模块，用于根据所述优化后因子图中所述当前状态对应的状态节点确定所述当前雷达采样时刻的优化后状态，将所述优化后状态以里程计的形式发布，并以所述优化后因子图作为下一雷达采样时刻的因子图，返回“获取当前雷达采样时刻的激光雷达点云数据和从上一雷达采样时刻到所述当前雷达采样时刻之间的每一惯导采样时刻的IMU数据”的步骤，直至所述无人驾驶车辆停止运动。
CN202211263739.2A 2022-10-17 2022-10-17 激光雷达-惯性测量单元融合里程计设计方法及系统 Active CN115326068B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211263739.2A CN115326068B (zh) 2022-10-17 2022-10-17 激光雷达-惯性测量单元融合里程计设计方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211263739.2A CN115326068B (zh) 2022-10-17 2022-10-17 激光雷达-惯性测量单元融合里程计设计方法及系统

Publications (2)

Publication Number Publication Date
CN115326068A CN115326068A (zh) 2022-11-11
CN115326068B true CN115326068B (zh) 2023-01-24

Family

ID=83914719

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211263739.2A Active CN115326068B (zh) 2022-10-17 2022-10-17 激光雷达-惯性测量单元融合里程计设计方法及系统

Country Status (1)

Country Link
CN (1) CN115326068B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117387598A (zh) * 2023-10-08 2024-01-12 北京理工大学 一种紧耦合轻量级的实时定位与建图方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113340295A (zh) * 2021-06-16 2021-09-03 广东工业大学 一种多个测距传感器的无人艇近岸实时定位与建图方法
CN113358112A (zh) * 2021-06-03 2021-09-07 北京超星未来科技有限公司 一种地图构建方法及一种激光惯性里程计
CN114966734A (zh) * 2022-04-28 2022-08-30 华中科技大学 一种结合多线激光雷达的双向深度视觉惯性位姿估计方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190079536A1 (en) * 2017-09-13 2019-03-14 TuSimple Training and testing of a neural network system for deep odometry assisted by static scene optical flow
CN111929699B (zh) * 2020-07-21 2023-05-09 北京建筑大学 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统
CN114046790A (zh) * 2021-10-22 2022-02-15 南京航空航天大学 一种因子图双重回环的检测方法
CN114018248B (zh) * 2021-10-29 2023-08-04 同济大学 一种融合码盘和激光雷达的里程计方法与建图方法
CN115077519A (zh) * 2022-06-27 2022-09-20 南京航空航天大学 基于模板匹配与激光惯导松耦合的定位建图方法和装置
CN115164887A (zh) * 2022-08-30 2022-10-11 中国人民解放军国防科技大学 基于激光雷达与惯性组合的行人导航定位方法和装置

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113358112A (zh) * 2021-06-03 2021-09-07 北京超星未来科技有限公司 一种地图构建方法及一种激光惯性里程计
CN113340295A (zh) * 2021-06-16 2021-09-03 广东工业大学 一种多个测距传感器的无人艇近岸实时定位与建图方法
CN114966734A (zh) * 2022-04-28 2022-08-30 华中科技大学 一种结合多线激光雷达的双向深度视觉惯性位姿估计方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
"Information sparsification for visual-inertial odometry by manipulating Bayes tree";Zhang, Xin 等;《Aerospace Science & Technology》;21201230;第118卷;正文第1-11页 *

Also Published As

Publication number Publication date
CN115326068A (zh) 2022-11-11

Similar Documents

Publication Publication Date Title
CN112484725B (zh) 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法
Zhao et al. A robust laser-inertial odometry and mapping method for large-scale highway environments
Liu et al. Stereo visual-inertial odometry with multiple Kalman filters ensemble
CN110375738A (zh) 一种融合惯性测量单元的单目同步定位与建图位姿解算方法
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN113654555A (zh) 一种基于多传感器数据融合的自动驾驶车辆高精定位方法
CN114323033B (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN109579840A (zh) 一种点线特征融合的紧耦合双目视觉惯性slam方法
CN114018248B (zh) 一种融合码盘和激光雷达的里程计方法与建图方法
Jia et al. Lvio-fusion: A self-adaptive multi-sensor fusion slam framework using actor-critic method
CN109579838A (zh) Agv小车的定位方法及定位系统
CN114623817B (zh) 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法
CN113516692B (zh) 一种多传感器融合的slam方法和装置
CN113358112B (zh) 一种地图构建方法及一种激光惯性里程计
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
CN116452763A (zh) 一种基于误差卡尔曼滤波和因子图的三维点云地图构建方法
CN115326068B (zh) 激光雷达-惯性测量单元融合里程计设计方法及系统
CN116429084A (zh) 一种动态环境3d同步定位与建图方法
CN115930977A (zh) 特征退化场景的定位方法、系统、电子设备和可读存介质
Yu et al. Tightly-coupled fusion of VINS and motion constraint for autonomous vehicle
Si et al. TOM-Odometry: A generalized localization framework based on topological map and odometry
CN114889606A (zh) 一种基于多传感融合的低成本高精定位方法
Liu et al. Integrated autonomous relative navigation method based on vision and IMU data fusion
Zhang et al. A robust lidar slam system based on multi-sensor fusion
CN115950414A (zh) 一种不同传感器数据的自适应多重融合slam方法

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