CN115876222A - 基于图优化的imu/相机/激光雷达联合标定方法 - Google Patents

基于图优化的imu/相机/激光雷达联合标定方法 Download PDF

Info

Publication number
CN115876222A
CN115876222A CN202211582531.7A CN202211582531A CN115876222A CN 115876222 A CN115876222 A CN 115876222A CN 202211582531 A CN202211582531 A CN 202211582531A CN 115876222 A CN115876222 A CN 115876222A
Authority
CN
China
Prior art keywords
imu
camera
laser radar
rotation
point
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211582531.7A
Other languages
English (en)
Inventor
徐晓苏
侯岚华
姚逸卿
高佳誉
何宇明
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Southeast University
Original Assignee
Southeast University
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 Southeast University filed Critical Southeast University
Publication of CN115876222A publication Critical patent/CN115876222A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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

  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本发明公开了一种基于图优化的IMU/相机/激光雷达联合标定方法，具体步骤包括：首先，根据IMU信息拟合旋转B样条曲线，并采用NDT算法估计雷达旋转，初始化IMU/雷达旋转外参；接着，采用特征点算法估计相机旋转，初始化IMU/相机旋转外参、重力、尺度因子、陀螺仪零偏；然后，采用棋盘格标定板，根据视觉特征点和激光雷达面距离最小构建目标函数，初始化雷达/相机外参；最后，根据IMU约束、相机约束、激光雷达约束、基于标定板的相机和激光雷达外参约束、IMU/相机/激光雷达外参互相约束构建联合优化方程，求解得IMU/相机/激光雷达外参。与现有标定方法相比，本发明能实现IMU/相机/激光雷达两两同时标定，有效提高标定收敛速度、精度及鲁棒性。

Description

基于图优化的IMU/相机/激光雷达联合标定方法
技术领域
本发明是一种基于图优化的IMU/相机/激光雷达联合标定方法，属于多传感器标定技术，特别适用于未知环境导航领域。
背景技术
在无人导航领域中，IMU(即：惯性测量单元)凭借其高度自主性与抗干扰性应用广泛，但其误差随时间积累；相机价格低廉且具有丰富语义信息，在纹理环境中导航精度高，但无法获取精确深度信息；激光雷达可获取精确三维环境信息，特别是在GNSS(即：全球导航卫星系统)数据不可用时，可为自主无人车提供准确位姿信息。基于IMU/相机/激光雷达的组合导航是未来无人导航的发展方向之一。在IMU/相机/激光雷达系统中，旋转和平移外参会影响定位精度，需对其进行在线标定。现有标定技术分别实现了IMU/相机、IMU/激光雷达和相机/激光雷达的标定工作，无法同时标定三者外参，未考虑三者外参的相互约束性，在复杂环境下适应性与鲁棒性较低。针对以上情况亟需一种更有效的标定方法。
发明内容
发明目的：为了提高标定效率并增强标定适应性，本发明提出了一种基于图优化的IMU/相机/激光雷达联合标定方法。
技术方案：为实现本发明的目的，本发明所采用的技术方案是：
一种基于图优化的IMU/相机/激光雷达联合标定方法，该方法包括如下步骤：
S1：根据IMU输出角速度信息拟合旋转B样条曲线得IMU姿态信息；
S2：采用NDT(正态分布变换)算法估计激光雷达旋转，根据S1估计的IMU姿态信息变化量与激光雷达旋转初始化IMU/激光雷达旋转外参；
S3：根据S2初始化的IMU/激光雷达旋转外参进行激光雷达去畸变处理；
S4：采用特征点算法估计相机旋转，根据S1估计的IMU姿态信息变化量与相机旋转初始化IMU/相机旋转外参，并初始化重力、尺度因子、陀螺仪零偏及速度；
S5：采用棋盘格标定板，提取相机系下棋盘格点坐标，根据聚类算法提取激光雷达系下棋盘格激光点对应法向量与中心点，根据点面距离最小构建目标函数，初始化激光雷达-相机外参；
S6：根据IMU约束、激光雷达约束、相机约束、基于标定板的相机和激光雷达外参约束、IMU/相机/激光雷达外参互相约束构建联合优化目标方程，求解目标方程得IMU/相机/激光雷达外参。
所述的基于图优化的IMU/相机/激光雷达联合标定方法，所述步骤S1具体包括以下过程：
根据IMU输出角速度信息拟合旋转B样条曲线，通过解最小二乘问题计算拟合曲线的四元数控制点：
Figure BDA0003991918150000021
/>
其中，q0,....qN为B样条曲线四元数控制点，k为当前时刻，M为总采样时间，
Figure BDA0003991918150000022
为k时刻IMU输出角速度信息，/>
Figure BDA0003991918150000023
为0时刻到k时刻IMU的旋转矩阵。
所述的基于图优化的IMU/相机/激光雷达联合标定方法，所述步骤S2具体包括以下过程：
S2.1采用NDT算法估计激光雷达旋转，目标函数为：
Figure BDA0003991918150000024
其中，
Figure BDA0003991918150000025
为k-1时刻到k时刻激光雷达的变换矩阵，i为当前激光点，n为总激光点数，/>
Figure BDA0003991918150000026
为k时刻第i个激光点位置，/>
Figure BDA0003991918150000027
为k-1时刻第i个激光点位置；
S2.2根据S1估计的IMU姿态变化与S2.1估计的激光雷达旋转，初始化IMU/激光雷达旋转外参：
Figure BDA0003991918150000028
其中，
Figure BDA0003991918150000029
为k-1时刻到k时刻激光雷达的旋转四元数，/>
Figure BDA00039919181500000210
为激光雷达到IMU的旋转四元数，/>
Figure BDA00039919181500000211
为k-1时刻到k时刻IMU的旋转四元数。
所述的基于图优化的IMU/相机/激光雷达联合标定方法，所述步骤S3具体包括以下过程：
根据S2初始化的IMU/激光雷达旋转外参进行激光雷达去畸变处理：
Figure BDA00039919181500000212
其中，
Figure BDA00039919181500000213
为k时刻第0扫描系的第i个激光雷达点，/>
Figure BDA00039919181500000214
为k时刻第j扫描系的第i个激光雷达点，/>
Figure BDA00039919181500000215
为激光雷达从k时刻第j扫描系到第0扫描系的旋转矩阵。
所述的基于图优化的IMU/相机/激光雷达联合标定方法，所述步骤S4具体包括以下过程：
S4.1采用特征点算法估计相机旋转，求解所有帧位姿和路标点的3D位置；
S4.2根据S1估计的IMU姿态变化与相机旋转初始化IMU/相机旋转外参：
Figure BDA00039919181500000216
其中，
Figure BDA00039919181500000217
为k-1时刻到k时刻相机的旋转四元数，/>
Figure BDA00039919181500000218
为相机到IMU的旋转四元数，
Figure BDA00039919181500000219
为k-1时刻到k时刻IMU的旋转四元数；
S4.3初始化重力、尺度因子、陀螺仪零偏及速度。
所述的基于图优化的IMU/相机/激光雷达联合标定方法，所述步骤S5具体包括以下过程：
采用棋盘格标定板，提取相机系下棋盘格点坐标及平面法向量，根据聚类算法提取激光雷达系下棋盘格激光点对应平面法向量与中心点，根据点面距离最小构建目标函数，初始化激光雷达/相机外参。
S5.1根据相机系下的平面法向量NC和激光雷达系下的平面法向量NL求解相机到激光雷达的旋转矩阵：
Figure BDA0003991918150000031
其中，
Figure BDA0003991918150000032
为相机到激光雷达的旋转矩阵，NC为相机系下的平面法向量，NL为激光雷达系下的平面法向量。
S5.2根据相机系下的点到激光雷达面的距离最小构建目标函数求解：
Figure BDA0003991918150000033
其中，t(x,y,z)为相机到激光雷达的平移外参，
Figure BDA0003991918150000034
为相机到激光雷达的旋转矩阵，
Figure BDA0003991918150000035
为第i帧相机系下的第j个点，/>
Figure BDA0003991918150000036
为第i帧激光雷达系下的中心点，/>
Figure BDA0003991918150000037
为第i帧激光雷达系下的法向量。
所述的基于图优化的IMU/相机/激光雷达联合标定方法，所述步骤S6具体包括以下过程：
根据IMU约束、激光雷达约束、相机约束、基于标定板的相机和激光雷达外参约束、IMU/相机/激光雷达外参互相约束构建联合优化目标方程：
Figure BDA0003991918150000038
Figure BDA0003991918150000039
Figure BDA00039919181500000310
Figure BDA00039919181500000311
Figure BDA00039919181500000312
Figure BDA00039919181500000313
Figure BDA00039919181500000314
Figure BDA00039919181500000315
其中，x为优化变量，
Figure BDA00039919181500000316
为加表残差，/>
Figure BDA00039919181500000317
为陀螺残差，rl j为激光雷达残差，/>
Figure BDA00039919181500000318
为相机残差，/>
Figure BDA00039919181500000319
为相机与激光点面距离残差，/>
Figure BDA00039919181500000320
为外参残差，A为加表采样数，W为陀螺采样数，L为雷达采样数，C为相机采样数，O为相机与激光匹配采样数，P为外参匹配采样数，/>
Figure BDA0003991918150000041
为相机到激光雷达的旋转四元数，/>
Figure BDA0003991918150000042
为相机到激光雷达的平移外参，/>
Figure BDA0003991918150000043
为相机到IMU的旋转四元数，/>
Figure BDA0003991918150000044
为相机到IMU的平移外参，/>
Figure BDA0003991918150000045
为激光雷达到IMU的旋转四元数，/>
Figure BDA0003991918150000046
为激光雷达到IMU的平移外参，xq为连续时间轨迹旋转控制点，xp为连续时间轨迹旋平移控制点，ba为加表零偏，bg为陀螺零偏，/>
Figure BDA0003991918150000047
为初始IMU系到重力的旋转四元数，/>
Figure BDA0003991918150000048
为k时刻IMU输出加速度信息，a(tk)为k时刻拟合加速度，/>
Figure BDA0003991918150000049
为k时刻IMU输出角速度信息，w(tk)为k时刻拟合角速度，/>
Figure BDA00039919181500000410
为激光雷达系到IMU系的旋转矩阵，/>
Figure BDA00039919181500000411
为第j时刻到第0时刻IMU的旋转矩阵，/>
Figure BDA00039919181500000412
为第j时刻第i个激光雷达点坐标，/>
Figure BDA00039919181500000413
为激光雷达系到IMU系的平移外参，/>
Figure BDA00039919181500000414
为第j时刻到第0时刻IMU的平移，/>
Figure BDA00039919181500000415
为IMU到激光雷达的平移外参，/>
Figure BDA00039919181500000416
为第0时刻第j个激光雷达点坐标，/>
Figure BDA00039919181500000417
和/>
Figure BDA00039919181500000418
为正切平面上的任意两个正交基，/>
Figure BDA00039919181500000419
为估计的第l个路标点在Ci系坐标，/>
Figure BDA00039919181500000420
为第l个路标点在Ci系观测坐标，πc为相机投影矩阵，/>
Figure BDA00039919181500000421
为第l个路标点的像素横坐标，/>
Figure BDA00039919181500000422
为第l个路标点的像素纵坐标，/>
Figure BDA00039919181500000423
为相机到IMU的旋转矩阵，/>
Figure BDA00039919181500000424
为Ii到Ii′的旋转矩阵，/>
Figure BDA00039919181500000425
为第i个点在Ci系坐标，
Figure BDA00039919181500000426
为相机到IMU的平移外参，/>
Figure BDA00039919181500000427
为Ii到Ii′平移变换，/>
Figure BDA00039919181500000428
为IMU到相机的平移外参，/>
Figure BDA00039919181500000429
为第i帧相机系下的第j个点，t(x,y,z)为相机到激光雷达的平移外参，/>
Figure BDA00039919181500000430
为第i帧激光雷达系下的中心点，/>
Figure BDA00039919181500000431
为激光雷达系下的法向量。
相对于现有技术，本发明的优点如下：
1、本发明采用IMU/激光雷达、IMU/相机、激光雷达/相机三者外参相互约束，通过构建三角约束，增强外参估计鲁棒性，提高了外参估计效率；
2、本发明采用B样条插值计算IMU连续时间轨迹估计，对齐IMU/激光雷达/相机，减少线性插值误差。
附图说明
图1为本发明的基于图优化的IMU/相机/激光雷达联合标定方法流程图；
图2为本发明的因子图优化示意图。
具体实施方式
下面结合附图，对本发明的技术方案作进一步说明。应当了解，以下提供的实施例仅是为了详尽地且完全地公开本发明，并且向所属技术领域的技术人员充分传达本发明的技术构思，本发明还可以用许多不同的形式来实施，并且不局限于此处描述的实施例。对于表示在附图中的示例性实施方式中的术语并不是对本发明的限定。
实施例1：本发明提供的一种基于图优化的IMU/相机/激光雷达联合标定方法，实现原理如图1所示，其流程主要包括以下步骤：
步骤S1，根据IMU输出角速度信息拟合旋转B样条曲线得IMU姿态信息，
具体包括以下过程：
根据IMU输出角速度信息拟合旋转B样条曲线，通过解最小二乘问题计算拟合曲线的四元数控制点：
Figure BDA0003991918150000051
其中，q0,....qN为B样条曲线四元数控制点，k为当前时刻，M为总采样时间，
Figure BDA0003991918150000052
为k时刻IMU输出角速度信息，/>
Figure BDA0003991918150000053
为0时刻到k时刻IMU的旋转矩阵。/>
步骤S2，采用NDT算法估计激光雷达旋转，根据S1估计的IMU姿态信息变化量与激光雷达旋转初始化IMU/激光雷达旋转外参，
具体包括以下过程：
S2.1采用NDT算法估计激光雷达旋转，目标函数为：
Figure BDA0003991918150000054
其中，
Figure BDA0003991918150000055
为k-1时刻到k时刻激光雷达的变换矩阵，i为当前激光点，n为总激光点数，/>
Figure BDA0003991918150000056
为k时刻第i个激光点位置，/>
Figure BDA0003991918150000057
为k-1时刻第i个激光点位置；
S2.2根据S1估计的IMU姿态变化与S2.1估计的激光雷达旋转，初始化IMU/激光雷达旋转外参：
Figure BDA0003991918150000058
其中，
Figure BDA0003991918150000059
为k-1时刻到k时刻激光雷达的旋转四元数，/>
Figure BDA00039919181500000510
为激光雷达到IMU的旋转四元数，/>
Figure BDA00039919181500000511
为k-1时刻到k时刻IMU的旋转四元数。
步骤S3，根据S2初始化的IMU/激光雷达旋转外参进行激光雷达去畸变处理，
具体包括以下过程：
根据S2初始化的IMU/激光雷达旋转外参进行激光雷达去畸变处理：
Figure BDA00039919181500000512
其中，
Figure BDA00039919181500000513
为k时刻第0扫描系的第i个激光雷达点，/>
Figure BDA00039919181500000514
为k时刻第j扫描系的第i个激光雷达点，/>
Figure BDA00039919181500000515
为激光雷达从k时刻第j扫描系到第0扫描系的旋转矩阵。
步骤S4，采用特征点算法估计相机旋转，根据S1估计的IMU姿态信息变化量与相机旋转初始化IMU/相机旋转外参，并初始化重力、尺度因子、陀螺仪零偏及速度，
具体包括以下过程：
S4.1采用特征点算法估计相机旋转，求解所有帧位姿和路标点的3D位置；
S4.2根据S1估计的IMU姿态变化与相机旋转初始化IMU/相机旋转外参：
Figure BDA0003991918150000061
其中，
Figure BDA0003991918150000062
为k-1时刻到k时刻相机的旋转四元数，/>
Figure BDA0003991918150000063
为相机到IMU的旋转四元数，
Figure BDA0003991918150000064
为k-1时刻到k时刻IMU的旋转四元数；
S4.3初始化重力、尺度因子、陀螺仪零偏及速度。
步骤S5，采用棋盘格标定板，提取相机系下棋盘格点坐标，根据聚类算法提取激光雷达系下棋盘格激光点对应法向量与中心点，根据点面距离最小构建目标函数，初始化激光雷达-相机外参，
具体包括以下过程：
S5.1根据相机系下的平面法向量NC和激光雷达系下的平面法向量NL求解相机到激光雷达的旋转矩阵：
Figure BDA0003991918150000065
其中，
Figure BDA0003991918150000066
为相机到激光雷达的旋转矩阵，NL为相机系下的平面法向量，NL为激光雷达系下的平面法向量。
S5.2根据相机系下的点到激光雷达面的距离最小构建目标函数求解：
Figure BDA0003991918150000067
其中，t(x,y,z)为相机到激光雷达的平移外参，
Figure BDA0003991918150000068
为相机到激光雷达的旋转矩阵，
Figure BDA0003991918150000069
为第i帧相机系下的第j个点，/>
Figure BDA00039919181500000610
为第i帧激光雷达系下的中心点，/>
Figure BDA00039919181500000611
为第i帧激光雷达系下的法向量。
步骤S6，根据IMU约束、激光雷达约束、相机约束、基于标定板的相机和激光雷达外参约束、IMU/相机/激光雷达外参互相约束构建联合优化目标方程，求解目标方程得IMU/相机/激光雷达外参，
具体包括以下过程：
根据IMU约束、激光雷达约束、相机约束、基于标定板的相机和激光雷达外参约束、IMU/相机/激光雷达外参互相约束构建联合优化目标方程：
Figure BDA0003991918150000071
Figure BDA0003991918150000072
Figure BDA0003991918150000073
Figure BDA0003991918150000074
Figure BDA0003991918150000075
Figure BDA0003991918150000076
Figure BDA0003991918150000077
Figure BDA0003991918150000078
其中，x为优化变量，
Figure BDA0003991918150000079
为加表残差，/>
Figure BDA00039919181500000710
为陀螺残差，rl j为激光雷达残差，/>
Figure BDA00039919181500000711
为相机残差，/>
Figure BDA00039919181500000712
为相机与激光点面距离残差，/>
Figure BDA00039919181500000713
为外参残差，A为加表采样数，W为陀螺采样数，L为雷达采样数，C为相机采样数，O为相机与激光匹配采样数，P为外参匹配采样数，/>
Figure BDA00039919181500000714
为相机到激光雷达的旋转四元数，/>
Figure BDA00039919181500000715
为相机到激光雷达的平移外参，/>
Figure BDA00039919181500000716
为相机到IMU的旋转四元数，/>
Figure BDA00039919181500000717
为相机到IMU的平移外参，/>
Figure BDA00039919181500000718
为激光雷达到IMU的旋转四元数，/>
Figure BDA00039919181500000719
为激光雷达到IMU的平移外参，xq为连续时间轨迹旋转控制点，xp为连续时间轨迹旋平移控制点，ba为加表零偏，bg为陀螺零偏，/>
Figure BDA00039919181500000720
为初始IMU系到重力的旋转四元数，/>
Figure BDA00039919181500000721
为k时刻IMU输出加速度信息，a(tk)为k时刻拟合加速度，/>
Figure BDA00039919181500000722
为k时刻IMU输出角速度信息，w(tk)为k时刻拟合角速度，/>
Figure BDA00039919181500000723
为激光雷达系到IMU系的旋转矩阵，/>
Figure BDA00039919181500000724
为第j时刻到第0时刻IMU的旋转矩阵，/>
Figure BDA00039919181500000725
为第j时刻第i个激光雷达点坐标，/>
Figure BDA00039919181500000726
为激光雷达系到IMU系的平移外参，/>
Figure BDA00039919181500000727
为第j时刻到第0时刻IMU的平移，/>
Figure BDA00039919181500000728
为IMU到激光雷达的平移外参，/>
Figure BDA00039919181500000729
为第0时刻第j个激光雷达点坐标，/>
Figure BDA00039919181500000730
和/>
Figure BDA00039919181500000731
为正切平面上的任意两个正交基，/>
Figure BDA00039919181500000732
为估计的第l个路标点在Ci系坐标，/>
Figure BDA00039919181500000733
为第l个路标点在Ci系观测坐标，πc为相机投影矩阵，/>
Figure BDA00039919181500000734
为第l个路标点的像素横坐标，/>
Figure BDA00039919181500000735
为第l个路标点的像素纵坐标，/>
Figure BDA00039919181500000736
为相机到IMU的旋转矩阵，/>
Figure BDA00039919181500000737
为Ii到Ii′的旋转矩阵，/>
Figure BDA00039919181500000738
为第i个点在Ci系坐标，
Figure BDA00039919181500000739
为相机到IMU的平移外参，/>
Figure BDA00039919181500000740
为Ii到Ii′平移变换，/>
Figure BDA00039919181500000741
为IMU到相机的平移外参，/>
Figure BDA00039919181500000742
为第i帧相机系下的第j个点，t(x,y,z)为相机到激光雷达的平移外参，/>
Figure BDA00039919181500000743
为第i帧激光雷达系下的中心点，/>
Figure BDA00039919181500000744
为激光雷达系下的法向量。
本发明方案所公开的技术手段不仅限于上述实施方所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。

Claims (7)

1.一种基于图优化的IMU/相机/激光雷达联合标定方法，其特征在于，包括以下步骤：
S1：根据IMU输出角速度信息拟合旋转B样条曲线得IMU姿态信息；
S2：采用NDT算法估计激光雷达旋转，根据S1估计的IMU姿态信息变化量与激光雷达旋转初始化IMU/激光雷达旋转外参；
S3：根据S2初始化的IMU/激光雷达旋转外参进行激光雷达去畸变处理；
S4：采用特征点算法估计相机旋转，根据S1估计的IMU姿态信息变化量与相机旋转初始化IMU/相机旋转外参，并初始化重力、尺度因子、陀螺仪零偏及速度；
S5：采用棋盘格标定板，提取相机系下棋盘格点坐标，根据聚类算法提取激光雷达系下棋盘格激光点对应法向量与中心点，根据点面距离最小构建目标函数，初始化激光雷达-相机外参；
S6：根据IMU约束、激光雷达约束、相机约束、基于标定板的相机和激光雷达外参约束、IMU/相机/激光雷达外参互相约束构建联合优化目标方程，求解目标方程得IMU/相机/激光雷达外参。
2.根据权利要求1所述的基于图优化的IMU/相机/激光雷达联合标定方法，其特征在于，所述步骤S1具体包括以下过程：
根据IMU输出角速度信息拟合旋转B样条曲线，通过解最小二乘问题计算拟合曲线的四元数控制点：
Figure FDA0003991918140000011
其中，q0,....qN为B样条曲线四元数控制点，k为当前时刻，M为总采样时间，
Figure FDA0003991918140000012
为k时刻IMU输出角速度信息，/>
Figure FDA0003991918140000013
为0时刻到k时刻IMU的旋转矩阵。
3.根据权利要求2所述的基于图优化的IMU/相机/激光雷达联合标定方法，其特征在于，所述步骤S2具体包括以下过程：
S2.1采用NDT算法估计激光雷达旋转，目标函数为：
Figure FDA0003991918140000014
其中，
Figure FDA0003991918140000015
为k-1时刻到k时刻激光雷达的变换矩阵，i为当前激光点，n为总激光点数，/>
Figure FDA0003991918140000016
为k时刻第i个激光点位置，/>
Figure FDA0003991918140000017
为k-1时刻第i个激光点位置；
S2.2根据S1估计的IMU姿态变化与S2.1估计的激光雷达旋转，初始化IMU/激光雷达旋转外参：
Figure FDA0003991918140000021
其中，
Figure FDA0003991918140000022
为k-1时刻到k时刻激光雷达的旋转四元数，/>
Figure FDA0003991918140000023
为激光雷达到IMU的旋转四元数，/>
Figure FDA0003991918140000024
为k-1时刻到k时刻IMU的旋转四元数。
4.根据权利要求2所述的基于图优化的IMU/相机/激光雷达联合标定方法，其特征在于，所述步骤S3具体包括以下过程：
根据S2初始化的IMU/激光雷达旋转外参进行激光雷达去畸变处理：
Figure FDA0003991918140000025
其中，
Figure FDA0003991918140000026
为k时刻第0扫描系的第i个激光雷达点，/>
Figure FDA0003991918140000027
为k时刻第j扫描系的第i个激光雷达点，/>
Figure FDA0003991918140000028
为激光雷达从k时刻第j扫描系到第0扫描系的旋转矩阵。
5.根据权利要求2所述的基于图优化的IMU/相机/激光雷达联合标定方法，其特征在于，所述步骤S4具体包括以下过程：
S4.1采用特征点算法估计相机旋转，求解所有帧位姿和路标点的3D位置；
S4.2根据S1估计的IMU姿态变化与相机旋转初始化IMU/相机旋转外参：
Figure FDA0003991918140000029
其中，
Figure FDA00039919181400000210
为k-1时刻到k时刻相机的旋转四元数，/>
Figure FDA00039919181400000211
为相机到IMU的旋转四元数，/>
Figure FDA00039919181400000212
为k-1时刻到k时刻IMU的旋转四元数；
S4.3初始化重力、尺度因子、陀螺仪零偏及速度。
6.根据权利要求2所述的基于图优化的IMU/相机/激光雷达联合标定方法，其特征在于，所述步骤S5具体包括以下过程：
采用棋盘格标定板，提取相机系下棋盘格点坐标及平面法向量，根据聚类算法提取激光雷达系下棋盘格激光点对应平面法向量与中心点，根据点面距离最小构建目标函数，初始化激光雷达/相机外参；
S5.1根据相机系下的平面法向量NC和激光雷达系下的平面法向量NL求解相机到激光雷达的旋转矩阵：
Figure FDA00039919181400000213
其中，
Figure FDA00039919181400000214
为相机到激光雷达的旋转矩阵，NC为相机系下的平面法向量，NL为激光雷达系下的平面法向量；
S5.2根据相机系下的点到激光雷达面的距离最小构建目标函数求解：
Figure FDA0003991918140000031
其中，t(x,y,z)为相机到激光雷达的平移外参，
Figure FDA0003991918140000032
为相机到激光雷达的旋转矩阵，/>
Figure FDA0003991918140000033
为第i帧相机系下的第j个点，/>
Figure FDA0003991918140000034
为第i帧激光雷达系下的中心点，/>
Figure FDA0003991918140000035
为第i帧激光雷达系下的法向量。
7.根据权利要求2所述的基于图优化的IMU/相机/激光雷达联合标定方法，其特征在于，所述步骤S6具体包括以下过程：
根据IMU约束、激光雷达约束、相机约束、基于标定板的相机和激光雷达外参约束、IMU/相机/激光雷达外参互相约束构建联合优化目标方程：
Figure FDA0003991918140000036
Figure FDA0003991918140000037
/>
Figure FDA0003991918140000038
Figure FDA0003991918140000039
Figure FDA00039919181400000310
Figure FDA00039919181400000311
Figure FDA00039919181400000312
Figure FDA00039919181400000313
其中，x为优化变量，
Figure FDA00039919181400000314
为加表残差，/>
Figure FDA00039919181400000315
为陀螺残差，rl j为激光雷达残差，/>
Figure FDA00039919181400000316
为相机残差，
Figure FDA00039919181400000317
为相机与激光点面距离残差，/>
Figure FDA00039919181400000318
为外参残差，A为加表采样数，W为陀螺采样数，L为雷达采样数，C为相机采样数，O为相机与激光匹配采样数，P为外参匹配采样数，/>
Figure FDA00039919181400000319
为相机到激光雷达的旋转四元数，/>
Figure FDA00039919181400000320
为相机到激光雷达的平移外参，/>
Figure FDA00039919181400000321
为相机到IMU的旋转四元数，/>
Figure FDA00039919181400000322
为相机到IMU的平移外参，/>
Figure FDA00039919181400000323
为激光雷达到IMU的旋转四元数，/>
Figure FDA00039919181400000324
为激光雷达到IMU的平移外参，xq为连续时间轨迹旋转控制点，xp为连续时间轨迹旋平移控制点，ba为加表零偏，bg为陀螺零偏，/>
Figure FDA00039919181400000325
为初始IMU系到重力的旋转四元数，/>
Figure FDA00039919181400000326
为k时刻IMU输出加速度信息，a(tk)为k时刻拟合加速度，/>
Figure FDA00039919181400000327
为k时刻IMU输出角速度信息，w(tk)为k
时刻拟合角速度，
Figure FDA0003991918140000041
为激光雷达系到IMU系的旋转矩阵，/>
Figure FDA0003991918140000042
为第j时刻到第0时刻IMU的旋转矩阵，/>
Figure FDA0003991918140000043
为第j时刻第i个激光雷达点坐标，/>
Figure FDA0003991918140000044
为激光雷达系到IMU系的平移外参，
Figure FDA0003991918140000045
为第j时刻到第0时刻IMU的平移，/>
Figure FDA0003991918140000046
为IMU到激光雷达的平移外参，/>
Figure FDA0003991918140000047
为第0时刻第j个激光雷达点坐标，/>
Figure FDA0003991918140000048
和/>
Figure FDA0003991918140000049
为正切平面上的任意两个正交基，/>
Figure FDA00039919181400000410
为估计的第l个路标点在Ci系坐标，/>
Figure FDA00039919181400000411
为第l个路标点在Ci系观测坐标，πc为相机投影矩阵，/>
Figure FDA00039919181400000412
为第l个路标点的像素横坐标，/>
Figure FDA00039919181400000413
为第l个路标点的像素纵坐标，/>
Figure FDA00039919181400000414
为相机到IMU的旋转矩阵，/>
Figure FDA00039919181400000415
为Ii到Ii′的旋转矩阵，/>
Figure FDA00039919181400000416
为第i个点在Ci系坐标，/>
Figure FDA00039919181400000417
为相机到IMU的平移外参，/>
Figure FDA00039919181400000418
为Ii到Ii′平移变换，/>
Figure FDA00039919181400000419
为IMU到相机的平移外参，/>
Figure FDA00039919181400000420
为第i帧相机系下的第j个点，t(x,y,z)为相机到激光雷达的平移外参，/>
Figure FDA00039919181400000421
为第i帧激光雷达系下的中心点，/>
Figure FDA00039919181400000422
为激光雷达系下的法向量。/>
CN202211582531.7A 2022-06-15 2022-12-09 基于图优化的imu/相机/激光雷达联合标定方法 Pending CN115876222A (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN2022106752641 2022-06-15
CN202210675264 2022-06-15

Publications (1)

Publication Number Publication Date
CN115876222A true CN115876222A (zh) 2023-03-31

Family

ID=85766900

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211582531.7A Pending CN115876222A (zh) 2022-06-15 2022-12-09 基于图优化的imu/相机/激光雷达联合标定方法

Country Status (1)

Country Link
CN (1) CN115876222A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117761663A (zh) * 2023-10-31 2024-03-26 新石器慧通（北京）科技有限公司 外参标定方法、装置及自动驾驶车辆

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117761663A (zh) * 2023-10-31 2024-03-26 新石器慧通（北京）科技有限公司 外参标定方法、装置及自动驾驶车辆

Similar Documents

Publication Publication Date Title
JP7404010B2 (ja) 位置推定装置及び方法
Barczyk et al. Invariant EKF design for scan matching-aided localization
Konovalenko et al. UAV Navigation On The Basis Of The Feature Points Detection On Underlying Surface.
CN110411457B (zh) 基于行程感知与视觉融合的定位方法、系统、终端和存储介质
CN112230242A (zh) 位姿估计系统和方法
CN111380514A (zh) 机器人位姿估计方法、装置、终端及计算机存储介质
US11158065B2 (en) Localization of a mobile unit by means of a multi hypothesis kalman filter method
CN110458885B (zh) 基于行程感知与视觉融合的定位系统和移动终端
CN113580134B (zh) 视觉定位方法、设备、机器人、存储介质及程序产品
Samadzadegan et al. Autonomous navigation of Unmanned Aerial Vehicles based on multi-sensor data fusion
Tjahjadi et al. Single frame resection of compact digital cameras for UAV imagery
CN115876222A (zh) 基于图优化的imu/相机/激光雷达联合标定方法
Kinnari et al. GNSS-denied geolocalization of UAVs by visual matching of onboard camera images with orthophotos
CN114608568B (zh) 一种基于多传感器信息即时融合定位方法
Gao et al. MGG: Monocular global geolocation for outdoor long-range targets
CN114690229A (zh) 一种融合gps的移动机器人视觉惯性导航方法
WO2021143664A1 (zh) 在载运工具中测量目标对象距离的方法、装置和载运工具
Gupta et al. Terrain‐based vehicle orientation estimation combining vision and inertial measurements
Setterfield et al. Feature-based scanning LiDAR-inertial odometry using factor graph optimization
CN117387604A (zh) 基于4d毫米波雷达和imu融合的定位与建图方法及系统
CN115930948A (zh) 一种果园机器人融合定位方法
CN114897942B (zh) 点云地图的生成方法、设备及相关存储介质
Shmatko et al. Estimation of rotation measurement error of objects using computer simulation
CN114842224A (zh) 一种基于地理底图的单目无人机绝对视觉匹配定位方案
CN115775242A (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