CN111665512B - 基于3d激光雷达和惯性测量单元的融合的测距和绘图 - Google Patents

基于3d激光雷达和惯性测量单元的融合的测距和绘图 Download PDF

Info

Publication number
CN111665512B
CN111665512B CN202010104509.6A CN202010104509A CN111665512B CN 111665512 B CN111665512 B CN 111665512B CN 202010104509 A CN202010104509 A CN 202010104509A CN 111665512 B CN111665512 B CN 111665512B
Authority
CN
China
Prior art keywords
lidar
time
laser radar
measurement unit
measurement data
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
CN202010104509.6A
Other languages
English (en)
Other versions
CN111665512A (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.)
Hong Kong University of Science and Technology HKUST
Original Assignee
Hong Kong University of Science and Technology HKUST
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 Hong Kong University of Science and Technology HKUST filed Critical Hong Kong University of Science and Technology HKUST
Publication of CN111665512A publication Critical patent/CN111665512A/zh
Application granted granted Critical
Publication of CN111665512B publication Critical patent/CN111665512B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/08—Systems determining position data of a target for measuring distance only
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C9/00—Measuring inclination, e.g. by clinometers, by levels
    • 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/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • 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

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)

Abstract

本公开提供一种用于姿态追踪和三维绘图的系统和方法，系统包括：3D激光雷达，其在第一时间和第二时间处获得激光雷达扫查结果，第一时间在第二时间之前；惯性测量单元，其与所述3D激光雷达紧密耦合；和计算装置，其构造为：在所述第二时间到来之前，利用在所述第一时间和所述第二时间之间来自所述惯性测量单元的测量数据对所述惯性测量单元在所述第二时间处的状态进行更新，同时对在所述第一时间和所述第二时间之间来自所述惯性测量单元的测量数据进行预整合等。

Description

基于3D激光雷达和惯性测量单元的融合的测距和绘图
技术领域
本发明涉及3D激光雷达和惯性测量单元(IMU)的融合，以用于姿态追踪和3D环境绘图。
背景技术
环境的三维(3D)地图可以用于自主导航或工程测量。一些传感器(例如3D激光雷达)可用于重建三维地图。传感器可以固定在系统上或在系统上移动。移动绘图系统需要姿态追踪(测距)和三维地图生成，与固定在系统上相比，移动绘图系统更灵活，更有效地捕获三维空间。
在移动绘图系统的其他示例中，应用了不同的传感器来估计姿态并生成三维图。一些系统基于一个或多个相机来估计量距和地图。间接目测法从捕获的图像中提取特征点，并估计这些点的三维位置，这可能会导致地图上的点稀疏。直接目测法可以提供更密集的地图，但是每个图像中的点位置的准确性都与估计的姿态有关。可以进一步应用立体或视觉惯性系统来解决度量三维空间中的点的规模的问题。大多数基于相机的方法都会受到照度变化的困扰，这可能会限制其应用。
基于三维激光雷达的系统是姿态追踪和绘图的替代方法，因为三维激光雷达可以提供度量点测量并且照度不变。当系统移动时，由于三维激光雷达的点的接收时间不同，这些点可能会失真。可以通过外部传感器(例如GNSS/INS)提供的运动模型或自身估计(激光雷达测距)来补偿失真。外部传感器辅助方法取决于外部传感器的可实现性。自身估计的方法(基于三维激光雷达的姿态追踪)在快速运动的情况下可能会失败，因为传感器具有相率较低的速率，或在一些类似狭窄走廊场景(三维激光雷达的重复特征)下可能会失败。
有一些基于三维激光雷达和惯性测量单元的方法可以提高快速运动的情况下的性能。大多数方法将三维激光雷达和惯性测量单元解耦，形成松散耦合的系统。这些系统将三维激光雷达和惯性测量单元视为独立的模块，因此融合后的结果无法改善单独的各模块并影响系统的性能。
发明内容
一方面，本公开提供一种用于姿态追踪和三维绘图的系统，包括：
3D激光雷达，其在第一时间和第二时间处获得激光雷达扫查结果，第一时间在第二时间之前；
惯性测量单元，其与所述3D激光雷达紧密耦合；和
计算装置，其构造为：
在所述第二时间到来之前，利用在所述第一时间和所述第二时间之间来自所述惯性测量单元的测量数据对所述惯性测量单元在所述第二时间处的状态进行更新，同时对在所述第一时间和所述第二时间之间来自所述惯性测量单元的测量数据进行预整合；
根据预整合后的所述惯性测量单元的测量数据以及惯性测量单元与激光雷达之间的外部参数来预测所述3D激光雷达的姿态，并将预测的所述3D激光雷达的姿态应用于在所述第二时间处接收的来自所述3D激光雷达的测量数据，以得到所述3D激光雷达的去偏移的测量数据；
提取在所述第二时间处接收的来自所述3D激光雷达的测量数据的特征点；
将在所述第一时间之前的来自所述3D激光雷达的测量数据的特征点融合为局部地图；
利用预测的激光雷达的姿态和所述去偏移的测量数据，得到相对激光雷达测量；
利用所述相对激光雷达测量和所述预整合，以得到优化后的3D激光雷达的姿态，进而得到局部时间窗口内的状态的绘图估计，并将估计结果用于所述更新中。
在实施例中，所述计算装置还构造为：
利用3D激光雷达的去偏移的测量数据和优化后的3D激光雷达的姿态来将多帧去偏移的数据融合而得到全局地图。
在实施例中，对优化后的3D激光雷达的姿态添加旋转约束来优化所述全局地图使得所述全局地图与所述3D激光雷达的去偏移的测量数据相匹配，以得到具有高一致性的全局地图。
在实施例中，所述相对激光雷达测量是所述局部地图与所述3D激光雷达的去偏移的测量数据之间的对应关系。
另一方面，本公开还提供一种用于姿态追踪和三维绘图的方法，包括：
通过3D激光雷达在第一时间和第二时间处获得激光雷达扫查结果，第一时间在第二时间之前；
将惯性测量单元与所述3D激光雷达紧密耦合；
在所述第二时间到来之前，利用在所述第一时间和所述第二时间之间来自所述惯性测量单元的测量数据对所述惯性测量单元在所述第二时间处的状态进行更新，同时对在所述第一时间和所述第二时间之间来自所述惯性测量单元的测量数据进行预整合；
根据预整合后的所述惯性测量单元的测量数据以及惯性测量单元与激光雷达之间的外部参数来预测所述3D激光雷达的姿态，并将预测的所述3D激光雷达的姿态应用于在所述第二时间处接收的来自所述3D激光雷达的测量数据，以得到所述3D激光雷达的去偏移的测量数据；
提取在所述第二时间处接收的来自所述3D激光雷达的测量数据的特征点；
将在所述第一时间之前的来自所述3D激光雷达的测量数据的特征点融合为局部地图；
利用预测的激光雷达的姿态和所述去偏移的测量数据，得到相对激光雷达测量；
利用所述相对激光雷达测量和所述预整合，以得到优化后的3D激光雷达的姿态，进而得到局部时间窗口内的状态的绘图估计，并将估计结果用于所述更新中。
在实施例中，所述方法还包括：
利用3D激光雷达的去偏移的测量数据和优化后的3D激光雷达的姿态来将多帧去偏移的数据融合而得到全局地图。
在实施例中，对优化后的3D激光雷达的姿态添加旋转约束来优化所述全局地图使得所述全局地图与所述3D激光雷达的去偏移的测量数据相匹配，以得到具有高一致性的全局地图。
在实施例中，所述相对激光雷达测量是所述局部地图与所述3D激光雷达的去偏移的测量数据之间的对应关系。
附图说明
图1示出了根据本公开实施例的包括激光雷达和IMU的姿态追踪和绘图系统的框图。
图2示出了计算系统的操作过程的示例。
图3示出了姿态追踪和绘图的详细过程的框图。
图4示出了系统的时间戳的示例。
图5示出了3D激光雷达和IMU绘图流程。
具体实施方式
本公开的实施例提供了一种通过紧密耦合方法使用3D激光雷达和惯性测量单元(IMU)的系统，用于姿态追踪和3D环境绘图，该系统仍可在快速运动或狭窄场景中提供稳定的结果。
在一个示例中，参照图1，其示出了姿态追踪和绘图系统的示例。该系统包括具有3D激光雷达和IMU的传感器以及收集传感器数据并处理姿态追踪和地图生成的计算系统。在实施例中，传感器还可以为3D激光雷达和IMU组成的传感器对。计算系统例如是笔记本计算机。该计算系统可以在环境中移动时收集点云和惯性测量。
通过将3D激光雷达测量和IMU测量紧密耦合，测距比现有技术有所改进，这联合优化了来自3D激光雷达测量和IMU测量的残差。该系统可以提高快速运动和类似狭窄走廊场景下的稳定性。旋转约束的绘图利用了根据3D激光雷达与IMU融合的测距(通过优化IMU残差使该测距与重力匹配)来生成一致的3D环境地图。也就是说，通过利用3D激光雷达的去偏移的数据和预测的3D激光雷达的姿态来将多帧去偏移的数据融合而得到3D环境绘图。
图2示出了计算系统的操作过程的示例。大体上，操作过程可以包括数据收集、3D激光雷达和IMU融合的测距(即，姿态追踪)和绘图。
下面将描述3D激光雷达和IMU融合的测距。
将3D激光雷达捕获的每行测量表示为scan C，并将包含一次测量中所有扫描(scan)的扫查(sweep)表示为sweep S。例如，一次扫查中的16行3D激光雷达包含16个扫描。
在以下部分中，将转换矩阵表示为其将坐标系/>中的点转换至坐标系/> 是由IMU预测的转换。/>和/>分别是的旋转矩阵和平移矢量。使用汉密尔顿符号下的四元数/>其对应于/> 用于两个四元数相乘。/>和/>表示IMU在时间k的原始测量。提取的特征在原始的捕获坐标系/>中表示为Fa，其可以转换至坐标系/>作为/>
在离散时间i处获得激光雷达扫查结果的同时，体坐标系/>和/>分别是IMU主体的参考和激光雷达中心的参考。待估计的状态是世界坐标系/>中的IMU状态/>以及激光雷达和IMU传感器之间的外部参数/>详细地，将i处的IMU状态和外部参数书写为：
它们形成的完整状态为：
是IMU状态在四元数中的位置、速度和方向。ba和bg是IMU加速器和陀螺仪的偏差。
图3示出了3D激光雷达和IMU测距流程，其简要概述了根据本发明的3D激光雷达-IMU测距。利用先前的估计状态，可以使用上一个时间i到当前时间j的当前激光雷达原始点云和IMU原始输入/>以具有对状态进行优化的新步骤。测距估计执行如下：
1)在到来之前，经由以下公式更新IMU状态::
上述等式可以通过迭代求解。其中，将简写为(·)i，/>并且，/>作为四元数乘法的序列。R为与q对应的旋转矩阵，gW是世界坐标系中的重力矢量，和/>是来自IMU的测量。
也就是说，如等式(2)所示，利用从IMU的加速器和陀螺仪的输入，可通过离散估计将前一个IMU状态更新为当前IMU状态/>其中，Δt是两次连续IMU测量之间的间隔，并且将激光雷达扫描时间k＝i和k＝j之间的所有IMU测量整合。通过简化符号使用，将k＝j-1用作k＝j之前的在前IMU时间。
2)同时，IMU原始输入被预整合为下面的Δpij,,Δvij和Δqij，以在后续的联合优化中使用。
时间i和时间j之间的主体运动可以通过预整合测量来表示：
3)3D激光雷达内部具有旋转机构以接收整个周围环境的数据。当3D激光雷达移动时，来自该3D激光雷达的原始数据会遭受运动失真，使扫查中的点与真实位置不同。为了解决该问题，在本公开中，根据IMU传播来预测3D激光雷达运动/>并且假设在扫查过程中采用线性运动模型。在本公开的实施例中，当接收到最新的激光雷达扫查结果/>时，进行去偏移，即将使用预测的3D激光雷达的运动/>的运动失真校正步骤应用于原始数据。随后，每个点/>通过/>的线性差值进行校正，以获得去偏移的扫描/>得到扫查的最终姿态，其中t∈(tj′，tj]是扫查中的点的时间，tj′和tj分别是扫查开始和扫查结束的时间。
因此，通过以下公式产生去偏移的激光雷达扫描
4)接下来，应用特征提取步骤以减小数据的维数并提取中最重要的特征点/>存在许多有效的激光雷达点提取，例如可以选择曲率较小的点，例如，平面点。
5)根据先前对应的优化状态和/>将局部窗口内的先前激光雷达特征点/>合并为局部地图/>
6)利用针对的预测的激光雷达姿态，可以通过找到每个约束来找到相对激光雷达测量/>其中/>是点，ω、d是在时间p处定义的平面系数。
利用IMU和另一传感器的融合，能够提供传感器对的相对姿态，如果固定第一参考系，待估计的状态和/>将是局部可观测的。为了适当地合并来自IMU的预整合，采用各扫查之间的相对激光雷达测量来约束激光雷达姿态，参见下面的算法。在找到点对应关系之前，建立局部地图，这是因为单次扫查的点对于计算准确的对应关系来说不够密集。
局部地图包含来自Nm个离散时间{o，…，p，…，i}的激光雷达特征点。其中o、p和i分别是窗口内第一次激光雷达扫查的时间、基准激光雷达扫查的时间和上一次处理过的激光雷达扫查的时间，如图4所示。局部地图根据特征/>建立在基准激光雷达扫查的坐标系中，特征/>经由先前优化的激光雷达姿势/>被转换(为了简便，在此部分中，将预测的转换/>表示为/>并且经由/>转换/>)。待估计的状态是Ns个时间{p+1，…，i，j}处的状态，其中p+1和j是窗口中的紧接着基准激光雷达扫查的下一次激光雷达扫描的时间和当前激光扫描的时间。
下表为相对激光雷达测量的算法。
通过建立局部地图，可以在和原始/>之间找到对应关系。将这种对应关系定义为相对激光雷达测量，因为他们是相对于基准姿态的，并且基准姿态会随着滑动窗口而变化。提取的特征点可以是/>中最平坦的点或边缘的点。在下文中，以平坦点为例进行讨论。将KNN(k-Nearest Neighbor)用于每个转换的特征点/>以找到/>中K个最近的点/>随后，，对于平坦点，将这些相邻的点拟合为/>中的平面。通过由/>限定的线性方程可以解出平坦点的系数，其中，ω是平面法线方向，d是到/>的原点的距离。将针对每个平坦特征点/>的表示为相对测量点中的一个。需要指出的是，在每个相对激光雷达测量/>中，x在/>中被定义，ω和d在/>中被定义。
7)联合非线性优化，利用相对激光雷达测量和IMU预整合，获得局部窗口内状态的绘图(MAP)估计。将最终结果应用于更新步骤1)中的预测状态，避免IMU预测的偏移。成本函数可以形成为
其中，激光雷达残差、惯性残差和边缘化残差分别定义如下，
其中，是边缘化的先验项。/>是相对激光雷达约束的残差，/>是IMU约束的残差。非线性最小二乘形式的成本函数可以通过Gauss-Newton算法求解，其采用HδX＝-b形式。可以采用Ceres Solver求解这一问题。
可以根据等式(4)针对每个相对激光雷达测量得到激光雷达约束通过激光雷达准确性确定协方差矩阵/>可以根据状态和IMU预整合获得IMU约束通过Schur分量可以获得/>
图5示出了旋转约束绘图的示例并示出了3D激光雷达和IMU绘图流程，其将先前测距得到的姿态、特征点和来自3D激光雷达的全点云作为输入。测距姿态首先用作全局点云登记的先验值。随后，来自测距的旋转分量作为虚拟测量应用以约束优化。然后，将以上具有马哈拉诺比斯范数(Mahalanobis norm)的成本函数最小化，以获得状态X的绘图(MAP)估计。
可以通过先前的激光雷达特征点和激光雷达-IMU测距得到的激光雷达姿态来形成全局地图。将特征点登记到全局地图(而不是局部地图)可以将激光雷达姿态约束到一致的世界坐标系。根据本公开的绘图方法将相对激光雷达测量mL用作3D激光雷达-IMU测距中的相对激光雷达测量。可以形成将最新的激光雷达特征点与全局地图匹配的成本函数，如下所示：
其中
其中是最新的待估计的激光雷达姿态，m是具有/>中的特征点x和限定在/>中的系数ω和d的相对激光测量。随后可以使用类似的Gauss-Newton方法来最小化通过残差/>和雅可比矩阵(Jacobian)/>和/>执行优化，其中θ是相应四元数q的误差状态。随着累积的旋转误差和长期操作后，融合后的全局地图无法与重力精确匹配。这可能进一步导致绘图与倾斜的地图错误地匹配。在本公开中，提供了约束的绘图方法。该方法利用来自激光雷达-IMU测距的约束，这确保了最终的地图与重力匹配。
给定以下特性：沿z轴的方向具有较高的不确定性，并且该方向的其他两个自由度更加接近真实值，因此可以通过将该方向的雅可比矩阵修改为以下矩阵来约束成本函数：
其中表示最后一次迭代中的状态和信息矩阵的估计，并且/>是关于/>的方向的信息矩阵的近似值，∈x和∈y可以通过/>中x轴方向和y轴方向到z轴方向的信息比率获得。
之后，使用和/>作为雅可比行列式，并将其用于优化步骤。可以获得递增的激光雷达姿态作为δθz和δp。
利用旋转约束的最终更新的激光雷达状态将是
在实施例中，在应用于室内应用的手持系统时，3D激光雷达和IMU彼此靠近并紧密耦合。在应用于室外应用的车载系统中时，3D激光雷达和IMU通常彼此相距较远，例如IMU安装在汽车的基础连杆上方，3D激光雷达则安装在汽车的前部。与自动校准所有平移参数不同的是，将外部平移参数的先验项添加到联合非线性优化步骤中的成本函数中，以在汽车上测试。
综上所述，在本公开中，提出了一种紧密耦合的3D激光雷达-IMU姿态估计算法，以克服上述问题。来自激光雷达和IMU的测量都用于联合优化。为实现实时和更一致的估计，应用先前姿态的固定滞后平滑和边缘化，然后进行旋转约束的优化。本公开所带来的技术效果如下：
提出了一种紧密耦合的激光雷达-IMU测距算法。它提供具有高更新速率的实时准确状态估计。
基于激光雷达-IMU测距的先验技术，旋转约束优化方法进一步优化了最终姿态和生成的点云图。即使在某些激光雷达退化的情况下，它也可以确保一致且可靠的估计。
该算法已通过广泛的室内和室外测试验证。它优于传统的仅使用激光雷达或松散耦合的激光雷达-IMU算法。

Claims (8)

1.一种用于姿态追踪和三维绘图的系统，包括：
3D激光雷达，其在第一时间和第二时间处获得激光雷达扫查结果，第一时间在第二时间之前；
惯性测量单元，其与所述3D激光雷达紧密耦合；和
计算装置，其构造为：
在所述第二时间到来之前，利用在所述第一时间和所述第二时间之间来自所述惯性测量单元的测量数据对所述惯性测量单元在所述第二时间处的状态进行更新，同时对在所述第一时间和所述第二时间之间来自所述惯性测量单元的测量数据进行预整合；
根据预整合后的所述惯性测量单元的测量数据以及所述惯性测量单元与所述3D激光雷达之间的外部参数来预测所述3D激光雷达的姿态，并将预测的所述3D激光雷达的姿态应用于在所述第二时间处接收的来自所述3D激光雷达的测量数据，以得到所述3D激光雷达的去偏移的测量数据；
提取在所述第二时间处接收的来自所述3D激光雷达的测量数据的特征点；
将在所述第二时间之前的来自所述3D激光雷达的测量数据的特征点融合为局部地图；
利用预测的3D激光雷达的姿态和所述去偏移的测量数据，得到相对激光雷达测量；
利用所述相对激光雷达测量和所述预整合，进行联合非线性优化，以得到优化后的3D激光雷达的姿态，进而得到局部时间窗口内的状态的绘图估计，并将估计结果用于所述更新中,
其中，基于以下公式进行所述预整合：
其中，作为四元数乘法的序列，R为与q对应的旋转矩阵，gW是世界坐标系中的重力矢量，/>和/>是来自惯性测量单元的测量，Δt是两次连续惯性测量单元测量之间的间隔，ba和bg是惯性测量单元的加速器和陀螺仪的偏差,Δpij,Δvij、Δqij是惯性测量单元的状态在四元数中的位置、速度和方向的变化量。
2.根据权利要求1所述的系统，其中，所述计算装置还构造为：
利用3D激光雷达的去偏移的测量数据和优化后的3D激光雷达的姿态来将多帧去偏移的数据融合而得到全局地图。
3.根据权利要求2所述的系统，其中，所述计算装置还构造为：
对优化后的3D激光雷达的姿态添加旋转约束来优化所述全局地图使得所述全局地图与所述3D激光雷达的去偏移的测量数据相匹配，以得到具有高一致性的全局地图。
4.根据权利要求1所述的系统，其中，所述相对激光雷达测量是所述局部地图与所述3D激光雷达的去偏移的测量数据之间的对应关系。
5.一种用于姿态追踪和三维绘图的方法，包括：
将惯性测量单元与3D激光雷达紧密耦合；
通过所述3D激光雷达在第一时间和第二时间处获得激光雷达扫查结果，第一时间在第二时间之前；
在所述第二时间到来之前，利用在所述第一时间和所述第二时间之间来自所述惯性测量单元的测量数据对所述惯性测量单元在所述第二时间处的状态进行更新，同时对在所述第一时间和所述第二时间之间来自所述惯性测量单元的测量数据进行预整合；
根据预整合后的所述惯性测量单元的测量数据以及惯性测量单元与激光雷达之间的外部参数来预测所述3D激光雷达的姿态，并将预测的所述3D激光雷达的姿态应用于在所述第二时间处接收的来自所述3D激光雷达的测量数据，以得到所述3D激光雷达的去偏移的测量数据；
提取在所述第二时间处接收的来自所述3D激光雷达的测量数据的特征点；
将在所述第二时间之前的来自所述3D激光雷达的测量数据的特征点融合为局部地图；
利用预测的激光雷达的姿态和所述去偏移的测量数据，得到相对激光雷达测量；
利用所述相对激光雷达测量和所述预整合，进行联合非线性优化，以得到优化后的3D激光雷达的姿态，进而得到局部时间窗口内的状态的绘图估计，并将估计结果用于所述更新中，
其中，基于以下公式进行所述预整合：
其中，作为四元数乘法的序列，R为与q对应的旋转矩阵，gW是世界坐标系中的重力矢量，/>和/>是来自惯性测量单元的测量，Δt是两次连续惯性测量单元测量之间的间隔，ba和bg是惯性测量单元的加速器和陀螺仪的偏差,Δpij,Δvij、Δqij是惯性测量单元的状态在四元数中的位置、速度和方向的变化量。
6.根据权利要求5所述的方法，还包括：
利用3D激光雷达的去偏移的测量数据和优化后的3D激光雷达的姿态来将多帧去偏移的数据融合而得到全局地图。
7.根据权利要求6所述的方法，其中，
对优化后的3D激光雷达的姿态添加旋转约束来优化所述全局地图使得所述全局地图与所述3D激光雷达的去偏移的测量数据相匹配，以得到具有高一致性的全局地图。
8.根据权利要求5所述的方法，其中，所述相对激光雷达测量是所述局部地图与所述3D激光雷达的去偏移的测量数据之间的对应关系。
CN202010104509.6A 2019-02-21 2020-02-20 基于3d激光雷达和惯性测量单元的融合的测距和绘图 Active CN111665512B (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
US201962918932P 2019-02-21 2019-02-21
US62/918,932 2019-02-21

Publications (2)

Publication Number Publication Date
CN111665512A CN111665512A (zh) 2020-09-15
CN111665512B true CN111665512B (zh) 2024-04-30

Family

ID=72382548

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010104509.6A Active CN111665512B (zh) 2019-02-21 2020-02-20 基于3d激光雷达和惯性测量单元的融合的测距和绘图

Country Status (1)

Country Link
CN (1) CN111665512B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112067007B (zh) * 2020-11-12 2021-01-29 湖北亿咖通科技有限公司 地图生成方法、计算机存储介质及电子设备
CN112578394B (zh) * 2020-11-25 2022-09-27 中国矿业大学 附有几何约束的LiDAR/INS融合定位与制图方法
CN113777580A (zh) * 2021-07-19 2021-12-10 长春理工大学 一种基于可旋转单线激光雷达的三维激光扫描仪
CN114241749B (zh) * 2021-11-26 2022-12-13 深圳市戴升智能科技有限公司 一种基于时间序列的视频信标数据关联方法和系统
CN116964599A (zh) * 2022-02-25 2023-10-27 厦门大学 基于激光雷达的三维人体动作捕捉方法、训练方法及系统
CN115683170B (zh) * 2023-01-04 2023-03-14 成都西物信安智能系统有限公司 基于雷达点云数据融合误差的校准方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101192825B1 (ko) * 2011-06-30 2012-10-18 서울시립대학교 산학협력단 Ｇｐｓ／ｉｎｓ／영상ａｔ를 통합한 라이다 지오레퍼린싱 장치 및 방법
CN105783875A (zh) * 2016-05-10 2016-07-20 绵阳天眼激光科技有限公司 一种集成非扫描激光雷达的航空摄影测量系统及方法
US9870624B1 (en) * 2017-01-13 2018-01-16 Otsaw Digital Pte. Ltd. Three-dimensional mapping of an environment
CN108051002A (zh) * 2017-12-04 2018-05-18 上海文什数据科技有限公司 基于惯性测量辅助视觉的运输车空间定位方法及系统
CN109341706A (zh) * 2018-10-17 2019-02-15 张亮 一种面向无人驾驶汽车的多特征融合地图的制作方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108732584B (zh) * 2017-04-17 2020-06-30 百度在线网络技术（北京）有限公司 用于更新地图的方法和装置

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101192825B1 (ko) * 2011-06-30 2012-10-18 서울시립대학교 산학협력단 Ｇｐｓ／ｉｎｓ／영상ａｔ를 통합한 라이다 지오레퍼린싱 장치 및 방법
CN105783875A (zh) * 2016-05-10 2016-07-20 绵阳天眼激光科技有限公司 一种集成非扫描激光雷达的航空摄影测量系统及方法
US9870624B1 (en) * 2017-01-13 2018-01-16 Otsaw Digital Pte. Ltd. Three-dimensional mapping of an environment
CN108051002A (zh) * 2017-12-04 2018-05-18 上海文什数据科技有限公司 基于惯性测量辅助视觉的运输车空间定位方法及系统
CN109341706A (zh) * 2018-10-17 2019-02-15 张亮 一种面向无人驾驶汽车的多特征融合地图的制作方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
LiDAR and Inertial Fusion for Pose Estimation by Non-linear Optimization;Haoyang Ye 等;《 ICRA 2018》;1-7 *
基于惯性／磁力传感器与单目视觉融合的SLAM 方法;王泽华 等;《机器人》;第40卷(第6期);933-941 *
基于惯性测量单元的激光雷达点云融合方法;张艳国 等;系统仿真学报(第11期);311-316 *

Also Published As

Publication number Publication date
CN111665512A (zh) 2020-09-15

Similar Documents

Publication Publication Date Title
CN111665512B (zh) 基于3d激光雷达和惯性测量单元的融合的测距和绘图
Zuo et al. Lic-fusion 2.0: Lidar-inertial-camera odometry with sliding-window plane-feature tracking
Zuo et al. Lic-fusion: Lidar-inertial-camera odometry
Ye et al. Tightly coupled 3d lidar inertial odometry and mapping
CN109993113B (zh) 一种基于rgb-d和imu信息融合的位姿估计方法
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
CN111561923B (zh) 基于多传感器融合的slam制图方法、系统
EP3679549B1 (en) Visual-inertial odometry with an event camera
JP6760114B2 (ja) 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム
CN112649016B (zh) 一种基于点线初始化的视觉惯性里程计方法
US9071829B2 (en) Method and system for fusing data arising from image sensors and from motion or position sensors
CN112862894A (zh) 一种机器人三维点云地图构建与扩充方法
CN108225327B (zh) 一种顶标地图的构建与定位方法
JP2015532077A (ja) 少なくとも１つの画像を撮影する撮影装置に関連する装置の位置及び方向の決定方法
CN111932674A (zh) 一种线激光视觉惯性系统的优化方法
CN113052908A (zh) 一种基于多传感器数据融合的移动机器人位姿估计算法
CN113066127B (zh) 一种在线标定设备参数的视觉惯性里程计方法和系统
CN111623773B (zh) 一种基于鱼眼视觉和惯性测量的目标定位方法及装置
CN112781594A (zh) 基于imu耦合的激光雷达迭代最近点改进算法
CN114608554B (zh) 一种手持slam设备以及机器人即时定位与建图方法
CN104848861A (zh) 一种基于图像消失点识别技术的移动设备姿态测量方法
CN113516692A (zh) 一种多传感器融合的slam方法和装置
CN108613675B (zh) 低成本无人飞行器移动测量方法及系统
Zheng et al. SE (2)-constrained visual inertial fusion for ground vehicles
Pöppl et al. Integrated trajectory estimation for 3D kinematic mapping with GNSS, INS and imaging sensors: A framework and review

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