CN113091771B - 一种激光雷达-相机-惯导联合标定方法及系统 - Google Patents

一种激光雷达-相机-惯导联合标定方法及系统 Download PDF

Info

Publication number
CN113091771B
CN113091771B CN202110393764.1A CN202110393764A CN113091771B CN 113091771 B CN113091771 B CN 113091771B CN 202110393764 A CN202110393764 A CN 202110393764A CN 113091771 B CN113091771 B CN 113091771B
Authority
CN
China
Prior art keywords
pose
monocular camera
laser radar
sequences
sequence
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
CN202110393764.1A
Other languages
English (en)
Other versions
CN113091771A (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.)
Tsinghua University
Dongfeng Motor Corp
Original Assignee
Tsinghua University
Dongfeng Motor Corp
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 Tsinghua University, Dongfeng Motor Corp filed Critical Tsinghua University
Priority to CN202110393764.1A priority Critical patent/CN113091771B/zh
Publication of CN113091771A publication Critical patent/CN113091771A/zh
Application granted granted Critical
Publication of CN113091771B publication Critical patent/CN113091771B/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
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • 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/40—Correcting position, velocity or attitude
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种基于手眼标定的激光雷达‑相机‑惯导联合标定方法及系统，其包括：通过激光雷达、单目相机和GPS/IMU同时进行数据采集，获取激光雷达点云、单目相机图像以及GPS/IMU设备位姿信息；根据采集的数据，分别获得激光雷达位姿估计结果、单目相机位姿估计结果以及GPS/IMU设备自身在全局坐标系下的位姿；将获取的三种位姿估计结果进行数据关联，确定位姿之间的对应关系；将关联后的三个位姿序列两两之间构建手眼标定问题，并将手眼标定问题转化为优化问题；求解优化问题，将不同传感器轨迹转换到同一坐标系下。本发明能标定激光雷达、单目相机和惯性导航设备之间的外参并估计单目相机运行中的尺度。

Description

一种激光雷达-相机-惯导联合标定方法及系统
技术领域
本发明涉及一种自动驾驶技术领域，特别是关于一种基于手眼标定的激光雷达-相机-惯导联合标定方法及系统。
背景技术
高精度地图为自动驾驶汽车提供了重要的环境先验信息，在高精度地图的构建过程中常采用搭载了激光雷达、单目相机、惯性导航设备等多种传感器的采集车进行数据采集。单一传感器获得的感知、定位结果都是在各个传感器自身坐标系下，需要依靠传感器坐标系之间的相对位姿关系，即外参(extrinsic)在空间上进行统一，之后在同一坐标系下进行融合。
传感器外参的准确标定是感知、定位结果正确融合的必要前提。在实际应用中，直接测量传感器间的外参误差较大，难以满足精度需求；根据设计图纸判断的外参会受到安装误差影响；在车辆长时间运行过程中传感器间外参可能发生变化，因此需要算法进行自动化标定。
激光雷达可以获得周围环境的高精度三维点云信息，但是远处点云较为稀疏；单目相机可以获得周围环境的颜色、纹理等信息，但是缺乏深度信息；惯性导航设备可以直接获得自身在WGS坐标系下的全局位姿。三种传感器获得的是异构数据，难以进行直接关联，需要对于异构数据进行正确时空关联进行求解。
发明内容
针对上述问题，本发明提供一种基于手眼标定的激光雷达-相机-惯导联合标定方法及系统，用于标定激光雷达、单目相机和惯性导航设备之间的外参并估计单目相机运行中的尺度。
为实现上述目的，本发明采取以下技术方案：一种激光雷达-相机-惯导联合标定方法，其包括：步骤1、通过激光雷达、单目相机和GPS/IMU同时进行数据采集，获取激光雷达点云、单目相机图像以及GPS/IMU设备位姿信息；步骤2、根据采集的数据，分别获得激光雷达位姿估计结果
Figure BDA0003017760120000021
单目相机位姿估计结果
Figure BDA0003017760120000022
以及GPS/IMU设备自身在全局坐标系下的位姿
Figure BDA0003017760120000023
步骤3、将获取的三种位姿估计结果进行数据关联，确定位姿之间的对应关系；步骤4、将关联后的三个位姿序列两两之间构建手眼标定问题，并将手眼标定问题转化为优化问题；步骤5、求解优化问题，将不同传感器轨迹转换到同一坐标系下。
进一步，所述步骤2中，采用激光SLAM算法获得激光雷达位姿估计结果
Figure BDA0003017760120000024
采用视觉SLAM算法获得单目相机位姿估计结果
Figure BDA0003017760120000025
采用惯性导航设备获得自身在全局坐标系下的位姿
Figure BDA0003017760120000026
进一步，所述单目相机位姿估计结果
Figure BDA0003017760120000027
Figure BDA0003017760120000028
为第i帧单目相机相对于自身起点的位姿；
Figure BDA0003017760120000029
Figure BDA00030177601200000210
表示视觉里程计估计出的旋转部分位姿；
Figure BDA00030177601200000211
表示视觉里程计估计出的平移部分位姿，缺乏尺度；C0表示单目相机起始位置坐标系，Ci表示第i帧单目相机坐标系。
进一步，所述视觉里程计估计出的平移部分位姿
Figure BDA00030177601200000212
缺乏尺度，需要通过估计尺度s恢复其实际位姿
Figure BDA00030177601200000213
Figure BDA00030177601200000214
进一步，所述步骤3中，采用位姿插值的方式进行数据关联：在三种位姿序列中任选两种位姿序列进行插值，获得同一时间戳下两个位姿序列中对应的位姿；然后在具有同一时间戳的两个位姿序列中任选一个位姿序列与余下的位姿序列再次进行插值，完成数据关联。
进一步，所述位姿插值方法包括以下步骤：
步骤31、输入两个位姿序列，并判断其中第一位姿序列是否遍历完成，若完成，则输出与第二位姿序列的插值结果，反之，则进入下一步；
步骤32、取第一位姿序列中的时间戳，判断其是否落在第二位姿序列中，若落入，则进入下一步，反之，则返回步骤31重新判断第一位姿序列是否遍历完成；
步骤33、将第二位姿序列的平移部分和旋转部分分别进行插值，由此获得同一时间戳下位姿序列和位姿序列中对应的位姿。
进一步，所述步骤4中，构建方法包括以下步骤：步骤41、计算位姿增量；步骤42、根据位姿增量确定手眼标定问题；步骤43、将手眼标定问题转化为优化问题。
进一步，所述步骤43中，单目相机-激光雷达之间尺度
Figure BDA0003017760120000031
单目相机-惯性导航设备之间的尺度
Figure BDA0003017760120000032
之间应当满足如下关系：
Figure BDA0003017760120000033
引入新的代价函数项：
Figure BDA0003017760120000034
完成优化问题构建；其中，
Figure BDA0003017760120000035
为激光雷达到相机的外参矩阵，
Figure BDA0003017760120000036
为激光雷达到惯性导航设备的外参矩阵，
Figure BDA0003017760120000037
为相机到惯性导航设备的外参矩阵。
进一步，所述步骤5中，通过非线性优化的方式求解优化问题，获得激光雷达、单目相机、惯性导航设备之间的外参以及单目相机运行中的尺度利用外参和尺度将不同传感器轨迹转换到同一坐标系下。
一种激光雷达-相机-惯导联合标定系统，其包括：数据采集模块、位姿估计模块、数据关联模块、优化问题构建模块和优化求解模块；
所述数据采集模块，通过激光雷达、单目相机和GPS/IMU同时进行数据采集，获取激光雷达点云、单目相机图像以及GPS/IMU设备位姿信息；
所述位姿估计模块，根据采集的数据，分别获得激光雷达位姿估计结果
Figure BDA0003017760120000041
单目相机位姿估计结果
Figure BDA0003017760120000042
以及GPS/IMU设备自身在全局坐标系下的位姿
Figure BDA0003017760120000043
所述数据关联模块，将获取的三种位姿估计结果进行数据关联，确定位姿之间的对应关系；
所述优化问题构建模块，将关联后的三个位姿序列两两之间构建手眼标定问题，并将手眼标定问题转化为优化问题；
所述优化求解模块，求解优化问题，将不同传感器轨迹转换到同一坐标系下。
本发明由于采取以上技术方案，其具有以下优点：
1、本发明在标定过程中无需标定板，在相机和激光雷达缺乏共视关系的情况下也可以进行标定。
2、本发明模块化强，不依赖具体位姿估计算法。
3、本发明采用位姿插值进行数据关联，对于异步信息鲁棒性强。
4、本发明在手眼标定问题中引入尺度约束和外参间约束提高标定精度，能够实现尺度估计和外参的联合标定。
附图说明
图1是本发明实施例中的标定方法流程示意图；
图2是本发明实施例中数据采集部分的示意图；
图3是本发明实施例中位姿插值部分的算法框图；
图4是本发明实施例中构建优化问题的示意图；
图5是本发明实施例中激光雷达-惯性导航的标定结果示意图；
图6是本发明实施例中单目相机-惯性导航的标定结果示意图；
图7是本发明实施例中激光雷达-单目相机的标定结果示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例的附图，对本发明实施例的技术方案进行清楚、完整地描述。显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于所描述的本发明的实施例，本领域普通技术人员所获得的所有其他实施例，都属于本发明保护的范围。
本发明提供一种基于手眼标定的激光雷达-相机-惯导联合标定方法及系统，具体是一种利用即时定位与建图(Simultaneous Localization And Mapping,SLAM)技术、基于手眼标定方法的激光雷达、单目相机和惯性导航设备间外参联合标定和尺度估计方法。以激光雷达采集的点云、单目相机采集的图像以及惯性导航设备获得的WGS坐标系下的全局位姿作为信息源，通过激光SLAM技术估计激光雷达位姿，通过视觉SLAM技术估计单目相机位姿。通过位姿插值进行数据关联，确定位姿间的对应关系，之后构建优化问题进行求解。获得三者之间的外参以及单目相机运行中的尺度。
在本发明的第一实施方式中，如图1所示，提供一种激光雷达-相机-惯导联合标定方法，其包括以下步骤：
步骤1、通过激光雷达、单目相机和GPS/IMU同时进行数据采集，获取激光雷达点云、单目相机图像以及GPS/IMU设备位姿信息；
具体为：选择周围几何特征明显、光照良好纹理充足、GPS信号良好的开阔场地，使车辆由静止开始按照“8”字形轨迹行驶，如图2所示；同时采集激光雷达点云、单目相机图像以及GPS/IMU设备位姿信息供后续标定过程使用。
步骤2、根据步骤1中采集的数据，分别获得激光雷达位姿估计结果
Figure BDA0003017760120000051
单目相机位姿估计结果
Figure BDA0003017760120000052
(缺乏尺度)以及GPS/IMU设备自身在全局坐标系下的位姿
Figure BDA0003017760120000061
在本实施例中，采用激光SLAM算法获得激光雷达位姿估计结果
Figure BDA0003017760120000062
采用视觉SLAM算法获得单目相机位姿估计结果
Figure BDA0003017760120000063
采用惯性导航设备获得自身在全局坐标系下的位姿
Figure BDA0003017760120000064
其中，
Figure BDA0003017760120000065
为第i帧激光雷达相对于自身起点的位姿，
Figure BDA0003017760120000066
表示激光里程计估计出的旋转部分位姿，
Figure BDA0003017760120000067
表示激光里程计估计出的平移部分位姿，L0表示激光雷达起始位置坐标系，Li表示第i帧激光雷达坐标系；
Figure BDA0003017760120000068
为第i帧单目相机相对于自身起点的位姿，
Figure BDA0003017760120000069
Figure BDA00030177601200000610
表示视觉里程计估计出的旋转部分位姿，
Figure BDA00030177601200000611
表示视觉里程计估计出的平移部分位姿(缺乏尺度)，C0表示单目相机起始位置坐标系，Ci表示第i帧单目相机坐标系；
Figure BDA00030177601200000612
为第i帧惯性导航设备在全局坐标系下的位姿，
Figure BDA00030177601200000613
表示惯性导航设备获得的旋转部分位姿，
Figure BDA00030177601200000614
表示惯性导航设备获得的平移部分位姿，W表示大地坐标系，Gi表示第i帧惯性导航设备坐标系。
需要注意的是，视觉里程计估计出的平移部分位姿
Figure BDA00030177601200000615
缺乏尺度，需要通过估计尺度s恢复其实际位姿
Figure BDA00030177601200000616
如下式所示：
Figure BDA00030177601200000617
Figure BDA00030177601200000618
步骤3、将步骤2中获取的三种位姿估计结果进行数据关联，确定位姿之间的对应关系；
在本实施例中，由于激光雷达、单目相机和GPS/IMU这三个传感器帧率、算法处理速度及传输延迟情况各不相同，实际上获得的三组位姿信息频率和数量各不相同，属于异步信息，故在构建优化问题求解前需要进行数据关联。
本实施例中采用时间戳信息进行时间同步或者位姿插值确定位姿之间的对应关系。考虑到外参标定对于精度要求较高，采用位姿插值的方式进行数据关联。
具体的，在三种位姿序列(即激光雷达位姿估计结果、单目相机位姿估计结果以及GPS/IMU设备自身在全局坐标系下的位姿)中任选两种位姿序列进行插值，获得同一时间戳下两个位姿序列中对应的位姿；然后在具有同一时间戳的两个位姿序列中任选一个位姿序列与余下的位姿序列再次进行插值，完成数据关联，进而确定位姿之间的对应关系。
如图3所示，位姿插值方法包括以下步骤：
步骤31、输入两个位姿序列1、2，并判断其中第一位姿序列1是否遍历完成，若完成，则输出与第二位姿序列2的插值结果，反之，则进入下一步；
步骤32、取第一位姿序列1中的时间戳，判断其是否落在第二位姿序列2中，若落入，则进入下一步，反之，则返回步骤31重新判断第一位姿序列1是否遍历完成；
步骤33、将第二位姿序列的平移部分和旋转部分分别进行插值，由此获得同一时间戳下位姿序列1和位姿序列2中对应的位姿；
在本实施例中，将位姿的平移部分采用线性插值，将位姿的旋转部分用单位四元数表示，采用线性球面插值(Spherical Linear Interpolation,SLERP)，由此获得同一时间戳下位姿序列1和位姿序列2中对应的位姿。具体的插值方法可以根据精度需求进行替换。
上述步骤33中，假设位姿序列1中某一位姿的时间戳为t，在位姿序列2中相邻最近的两个时间戳为t1和t2，且t1<t<t2。对应位姿的平移部分分别为t1＝[x1 y1 z1]、t2＝[x2 y2 z2]；用单位四元数表示的旋转部分分别为
Figure BDA0003017760120000071
Figure BDA0003017760120000081
插值过程如下式所示：
Figure BDA0003017760120000082
Figure BDA0003017760120000083
Figure BDA0003017760120000084
Figure BDA0003017760120000085
由此可以获得位姿序列2中在同一时间戳下的对应位姿，其中，
Figure BDA0003017760120000086
为位姿的平移部分，
Figure BDA0003017760120000087
为位姿的旋转部分，θ为q1和q2向量间夹角的一半，T为插值参数，由时间戳归一化获得，无量纲，q1和q2为用单位四元数表示的t1时刻和t2时刻的位姿旋转部分。
步骤4、将关联后的三个位姿序列两两之间构建手眼标定问题，并将手眼标定问题转化为优化问题；
具体包括以下步骤：
步骤41、计算位姿增量；
位姿增量计算公式为：
Figure BDA0003017760120000088
其中，
Figure BDA0003017760120000089
表示第i帧到第j帧的位姿增量，以此可以计算出激光雷达帧间位姿增量
Figure BDA00030177601200000810
单目相机帧间位姿增量
Figure BDA00030177601200000811
惯性导航设备帧间位姿增量
Figure BDA00030177601200000812
对于高频位姿序列，由于帧间位姿变化不明显，可以按照预先设定的时间间隔计算相对位姿。
步骤42、根据位姿增量确定手眼标定问题；
以激光雷达和单目相机为例进行说明，由外参定义可知，激光雷达位姿增量与单目相机位姿增量之间应满足如下关系：
Figure BDA00030177601200000813
其中，
Figure BDA0003017760120000091
为恢复尺度后的单目相机帧间位姿增量。
(8)式具有手眼标定问题A·X＝X·B的形式，将(8)式中的各个矩阵对应用A、B、X简化表示，则(8)式可简记为：
A·X＝X·B (9)
步骤43、将手眼标定问题转化为优化问题；
手眼标定问题可以转化为优化问题，由式(9)推导代价函数形式：
Figure BDA0003017760120000092
RA·RX＝RX·RB (11)
RA·tX+tA＝RX·tB+tX (12)
式中，RA为
Figure BDA0003017760120000093
简化形式，即RA∈SO(3)表示激光里程计估计出的旋转部分位姿；tA为
Figure BDA0003017760120000094
简化形式，
Figure BDA0003017760120000095
表示激光里程计估计出的平移部分位姿；RX∈SO(3)表示激光雷达到相机外参的旋转部分，
Figure BDA0003017760120000096
表示激光雷达到相机外参的平移部分，RB∈SO(3)表示视觉里程计估计出的旋转部分位姿，
Figure BDA0003017760120000097
表示恢复尺度后的视觉里程计估计出的平移部分位姿。
由于视觉里程计给出的平移部分位姿t′B不包含尺度信息，因此在优化过程中还需要对于尺度s进行优化，上式中tB＝s·t′B，可得代价函数：
Figure BDA0003017760120000098
在不涉及视觉里程计时，代价函数如下所示：
Figure BDA0003017760120000099
至此，可以在三组位姿序列之间两两建立手眼标定问题。
考虑到激光雷达、单目相机、惯性导航设备之间外参之间应满足如下关系：
Figure BDA00030177601200000910
式中，
Figure BDA00030177601200000911
为激光雷达到相机的外参矩阵，
Figure BDA00030177601200000912
为激光雷达到惯性导航设备的外参矩阵，
Figure BDA0003017760120000101
为相机到惯性导航设备的外参矩阵。
考虑到激光里程计位姿以及惯性导航设备位姿都具有尺度，因此单目相机-激光雷达之间尺度
Figure BDA0003017760120000102
单目相机-惯性导航设备之间的尺度
Figure BDA0003017760120000103
之间应当满足如下关系：
Figure BDA0003017760120000104
由式(15)、(16)，引入新的代价函数项：
Figure BDA0003017760120000105
至此，完成优化问题构建，如图4所示。
步骤5、求解优化问题，将不同传感器轨迹转换到同一坐标系下；
通过非线性优化的方式求解上述优化问题，获得激光雷达、单目相机、惯性导航设备之间的外参以及单目相机运行中的尺度。利用外参和尺度将不同传感器轨迹转换到同一坐标系下，结果如图5～图7所示。
在本发明的第二实施方式中，提供一种基于手眼标定的激光雷达-相机-惯导联合标定系统，其包括：数据采集模块、位姿估计模块、数据关联模块、优化问题构建模块和优化求解模块；
数据采集模块，通过激光雷达、单目相机和GPS/IMU同时进行数据采集，获取激光雷达点云、单目相机图像以及GPS/IMU设备位姿信息；
位姿估计模块，根据采集的数据，分别获得激光雷达位姿估计结果
Figure BDA0003017760120000106
单目相机位姿估计结果
Figure BDA0003017760120000107
以及GPS/IMU设备自身在全局坐标系下的位姿
Figure BDA0003017760120000108
数据关联模块，将获取的三种位姿估计结果进行数据关联，确定位姿之间的对应关系；
优化问题构建模块，将关联后的三个位姿序列两两之间构建手眼标定问题，并将手眼标定问题转化为优化问题；
优化求解模块，求解优化问题，将不同传感器轨迹转换到同一坐标系下。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

Claims (8)

1.一种激光雷达-相机-惯导联合标定方法，其特征在于，包括：
步骤1、通过激光雷达、单目相机和GPS/IMU同时进行数据采集，获取激光雷达点云、单目相机图像以及GPS/IMU设备位姿信息；
步骤2、根据采集的数据，分别获得激光雷达位姿估计结果
Figure FDA0003741998010000011
单目相机位姿估计结果
Figure FDA0003741998010000012
以及GPS/IMU设备自身在全局坐标系下的位姿
Figure FDA0003741998010000013
步骤3、将获取的三种位姿估计结果进行数据关联，确定位姿之间的对应关系；
步骤4、将关联后的三个位姿序列两两之间构建手眼标定问题，并将手眼标定问题转化为优化问题；
步骤5、求解优化问题，将不同传感器轨迹转换到同一坐标系下；
所述步骤3中，采用位姿插值的方式进行数据关联：在三种位姿序列中任选两种位姿序列进行插值，获得同一时间戳下两个位姿序列中对应的位姿；然后在具有同一时间戳的两个位姿序列中任选一个位姿序列与余下的位姿序列再次进行插值，完成数据关联；
所述的位姿插值包括以下步骤：
步骤31、输入两个位姿序列，并判断其中第一位姿序列是否遍历完成，若完成，则输出与第二位姿序列的插值结果，反之，则进入下一步；
步骤32、取第一位姿序列中的时间戳，判断其是否落在第二位姿序列中，若落入，则进入下一步，反之，则返回步骤31重新判断第一位姿序列是否遍历完成；
步骤33、将第二位姿序列的平移部分和旋转部分分别进行插值，由此获得同一时间戳下位姿序列和位姿序列中对应的位姿。
2.如权利要求1所述联合标定方法，其特征在于，所述步骤2中，采用激光SLAM算法获得激光雷达位姿估计结果
Figure FDA0003741998010000014
采用视觉SLAM算法获得单目相机位姿估计结果
Figure FDA0003741998010000021
采用惯性导航设备获得自身在全局坐标系下的位姿
Figure FDA0003741998010000022
3.如权利要求2所述联合标定方法，其特征在于，所述单目相机位姿估计结果
Figure FDA0003741998010000023
Figure FDA0003741998010000024
为第i帧单目相机相对于自身起点的位姿；
Figure FDA0003741998010000025
Figure FDA0003741998010000026
表示视觉里程计估计出的旋转部分位姿；
Figure FDA0003741998010000027
表示视觉里程计估计出的平移部分位姿，缺乏尺度；C0表示单目相机起始位置坐标系，Ci表示第i帧单目相机坐标系。
4.如权利要求3所述联合标定方法，其特征在于，所述视觉里程计估计出的平移部分位姿
Figure FDA0003741998010000028
缺乏尺度，需要通过估计尺度s恢复其实际位姿
Figure FDA0003741998010000029
Figure FDA00037419980100000210
5.如权利要求1所述联合标定方法，其特征在于，所述步骤4中，构建方法包括以下步骤：
步骤41、计算位姿增量；
步骤42、根据位姿增量确定手眼标定问题；
步骤43、将手眼标定问题转化为优化问题。
6.如权利要求5所述联合标定方法，其特征在于，所述步骤43中，单目相机-激光雷达之间尺度
Figure FDA00037419980100000211
单目相机-惯性导航设备之间的尺度
Figure FDA00037419980100000212
之间应当满足如下关系：
Figure FDA00037419980100000213
引入新的代价函数项：
Figure FDA00037419980100000214
完成优化问题构建；其中，
Figure FDA00037419980100000215
为激光雷达到相机的外参矩阵，
Figure FDA00037419980100000216
为激光雷达到惯性导航设备的外参矩阵，
Figure FDA00037419980100000217
为相机到惯性导航设备的外参矩阵。
7.如权利要求1所述联合标定方法，其特征在于，所述步骤5中，通过非线性优化的方式求解优化问题，获得激光雷达、单目相机、惯性导航设备之间的外参以及单目相机运行中的尺度利用外参和尺度将不同传感器轨迹转换到同一坐标系下。
8.一种激光雷达-相机-惯导联合标定系统，其特征在于，包括：数据采集模块、位姿估计模块、数据关联模块、优化问题构建模块和优化求解模块；
所述数据采集模块，通过激光雷达、单目相机和GPS/IMU同时进行数据采集，获取激光雷达点云、单目相机图像以及GPS/IMU设备位姿信息；
所述位姿估计模块，根据采集的数据，分别获得激光雷达位姿估计结果
Figure FDA0003741998010000031
单目相机位姿估计结果
Figure FDA0003741998010000033
以及GPS/IMU设备自身在全局坐标系下的位姿
Figure FDA0003741998010000032
所述数据关联模块，将获取的三种位姿估计结果进行数据关联，确定位姿之间的对应关系；
所述优化问题构建模块，将关联后的三个位姿序列两两之间构建手眼标定问题，并将手眼标定问题转化为优化问题；
所述优化求解模块，求解优化问题，将不同传感器轨迹转换到同一坐标系下；
所述数据关联模块中，采用位姿插值的方式进行数据关联：在三种位姿序列中任选两种位姿序列进行插值，获得同一时间戳下两个位姿序列中对应的位姿；然后在具有同一时间戳的两个位姿序列中任选一个位姿序列与余下的位姿序列再次进行插值，完成数据关联；
所述的位姿插值包括以下步骤：
步骤31、输入两个位姿序列，并判断其中第一位姿序列是否遍历完成，若完成，则输出与第二位姿序列的插值结果，反之，则进入下一步；
步骤32、取第一位姿序列中的时间戳，判断其是否落在第二位姿序列中，若落入，则进入下一步，反之，则返回步骤31重新判断第一位姿序列是否遍历完成；
步骤33、将第二位姿序列的平移部分和旋转部分分别进行插值，由此获得同一时间戳下位姿序列和位姿序列中对应的位姿。
CN202110393764.1A 2021-04-13 2021-04-13 一种激光雷达-相机-惯导联合标定方法及系统 Active CN113091771B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110393764.1A CN113091771B (zh) 2021-04-13 2021-04-13 一种激光雷达-相机-惯导联合标定方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110393764.1A CN113091771B (zh) 2021-04-13 2021-04-13 一种激光雷达-相机-惯导联合标定方法及系统

Publications (2)

Publication Number Publication Date
CN113091771A CN113091771A (zh) 2021-07-09
CN113091771B true CN113091771B (zh) 2022-09-23

Family

ID=76676627

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110393764.1A Active CN113091771B (zh) 2021-04-13 2021-04-13 一种激光雷达-相机-惯导联合标定方法及系统

Country Status (1)

Country Link
CN (1) CN113091771B (zh)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113570663A (zh) * 2021-07-20 2021-10-29 上海云易航空科技有限公司 基于单线激光雷达和顶视摄像头融合的室内定位方法
CN113721260B (zh) * 2021-08-26 2023-12-12 南京邮电大学 一种激光雷达、双目相机和惯导的在线联合标定方法
CN114046787B (zh) * 2021-10-29 2024-01-30 广州文远知行科技有限公司 基于传感器的位姿优化方法、装置、设备及存储介质
CN114459500B (zh) * 2021-12-01 2024-05-24 岱悟智能科技(上海)有限公司 激光雷达与姿态传感器的相对位姿动态标定方法、装置、设备和介质
CN114252099B (zh) * 2021-12-03 2024-02-23 武汉科技大学 一种智能车辆多传感器融合自标定方法及系统
CN114279434A (zh) * 2021-12-27 2022-04-05 驭势科技（北京）有限公司 一种建图方法、装置、电子设备和存储介质
CN116558545A (zh) * 2022-01-29 2023-08-08 北京三快在线科技有限公司 一种传感器数据的标定方法及装置
CN115435816B (zh) * 2022-11-07 2023-04-11 山东大学 在线双舵轮agv内外参标定方法、系统、介质及设备
CN116148883B (zh) * 2023-04-11 2023-08-08 锐驰智慧科技(安吉)有限公司 基于稀疏深度图像的slam方法、装置、终端设备及介质
CN117168441B (zh) * 2023-11-02 2024-02-20 西安因诺航空科技有限公司 一种多传感器融合的slam定位和重建方法及系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109270534A (zh) * 2018-05-07 2019-01-25 西安交通大学 一种智能车激光传感器与相机在线标定方法
CN109483516A (zh) * 2018-10-16 2019-03-19 浙江大学 一种基于空间距离和极线约束的机械臂手眼标定方法
CN109544638A (zh) * 2018-10-29 2019-03-29 浙江工业大学 一种用于多传感器融合的异步在线标定方法
CN112577517A (zh) * 2020-11-13 2021-03-30 上汽大众汽车有限公司 一种多元定位传感器联合标定方法和系统
CN112598757A (zh) * 2021-03-03 2021-04-02 之江实验室 一种多传感器时间空间标定方法及装置

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109345596A (zh) * 2018-09-19 2019-02-15 百度在线网络技术（北京）有限公司 多传感器标定方法、装置、计算机设备、介质和车辆
WO2021042376A1 (zh) * 2019-09-06 2021-03-11 罗伯特·博世有限公司 用于工业机器人的标定方法及装置、三维环境建模方法及设备、计算机存储介质以及工业机器人操作平台
CN112051591B (zh) * 2020-08-31 2022-11-29 广州文远知行科技有限公司 一种激光雷达与惯性测量单元的检测方法及相关装置

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109270534A (zh) * 2018-05-07 2019-01-25 西安交通大学 一种智能车激光传感器与相机在线标定方法
CN109483516A (zh) * 2018-10-16 2019-03-19 浙江大学 一种基于空间距离和极线约束的机械臂手眼标定方法
CN109544638A (zh) * 2018-10-29 2019-03-29 浙江工业大学 一种用于多传感器融合的异步在线标定方法
CN112577517A (zh) * 2020-11-13 2021-03-30 上汽大众汽车有限公司 一种多元定位传感器联合标定方法和系统
CN112598757A (zh) * 2021-03-03 2021-04-02 之江实验室 一种多传感器时间空间标定方法及装置

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
3D LiDAR-GPS/IMU Calibration Based on Hand-Eye Calibration Model for Unmanned Vehicle;Chen Chen,et al.;《IEEE：PROCEEDINGS OF 2020 3RD INTERNATIONAL CONFERENCE ON UNMANNED SYSTEMS (ICUS)》;20201128;全文 *
面向自动驾驶系统的相机—激光雷达的自动标定;许涵博;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》;20200215(第02期);第50-60页 *

Also Published As

Publication number Publication date
CN113091771A (zh) 2021-07-09

Similar Documents

Publication Publication Date Title
CN113091771B (zh) 一种激光雷达-相机-惯导联合标定方法及系统
CN110009681B (zh) 一种基于imu辅助的单目视觉里程计位姿处理方法
US20190371003A1 (en) Monocular vision tracking method, apparatus and non-volatile computer-readable storage medium
CN107941217B (zh) 一种机器人定位方法、电子设备、存储介质、装置
Zhang et al. A lightweight and accurate localization algorithm using multiple inertial measurement units
US20160260250A1 (en) Method and system for 3d capture based on structure from motion with pose detection tool
CN109766757B (zh) 一种融合车辆和视觉信息的泊车位高精度定位方法及系统
Li et al. Real-time 3D motion tracking and reconstruction system using camera and IMU sensors
WO2015134795A2 (en) Method and system for 3d capture based on structure from motion with pose detection tool
CN101513062A (zh) 车载图像处理装置及其视点变换信息生成方法
CN110751123B (zh) 一种单目视觉惯性里程计系统及方法
CN109507706B (zh) 一种gps信号丢失的预测定位方法
CN115728753A (zh) 激光雷达与组合导航的外参标定方法、装置及智能车辆
EP4224841A1 (en) System and method for dynamic stereoscopic calibration
CN115218906A (zh) 面向室内slam的视觉惯性融合定位方法及系统
CN114444284A (zh) 一种车端实时仿真场景道路底图生成方法及系统
CN113137973A (zh) 一种图像语义特征点真值确定方法及装置
CN113701750A (zh) 一种井下多传感器的融合定位系统
Irmisch et al. Simulation framework for a visual-inertial navigation system
CN117388830A (zh) 激光雷达与惯性导航的外参标定方法、装置、设备及介质
CN112729283A (zh) 一种基于深度相机/mems惯导/里程计组合的导航方法
CN116907469A (zh) 多模态数据联合优化的同步定位及建图方法及系统
CN111145267A (zh) 基于imu辅助的360度全景视图多相机标定方法
CN116691659A (zh) 一种无gps信号下车辆行驶轨迹的评估方法、装置
CN115760636A (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
GR01 Patent grant
GR01 Patent grant