基于图优化的IMU/相机/激光雷达联合标定方法技术

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

【技术实现步骤摘要】
基于图优化的IMU/相机/激光雷达联合标定方法


[0001]本专利技术是一种基于图优化的IMU/相机/激光雷达联合标定方法，属于多传感器标定技术，特别适用于未知环境导航领域。

技术介绍

[0002]在无人导航领域中，IMU(即：惯性测量单元)凭借其高度自主性与抗干扰性应用广泛，但其误差随时间积累；相机价格低廉且具有丰富语义信息，在纹理环境中导航精度高，但无法获取精确深度信息；激光雷达可获取精确三维环境信息，特别是在GNSS(即：全球导航卫星系统)数据不可用时，可为自主无人车提供准确位姿信息。基于IMU/相机/激光雷达的组合导航是未来无人导航的发展方向之一。在IMU/相机/激光雷达系统中，旋转和平移外参会影响定位精度，需对其进行在线标定。现有标定技术分别实现了IMU/相机、IMU/激光雷达和相机/激光雷达的标定工作，无法同时标定三者外参，未考虑三者外参的相互约束性，在复杂环境下适应性与鲁棒性较低。针对以上情况亟需一种更有效的标定方法。

技术实现思路

[0003]专利技术目的：为了提高标定效率并增强标定适应性，本专利技术提出了一种基于图优化的IMU/相机/激光雷达联合标定方法。
[0004]技术方案：为实现本专利技术的目的，本专利技术所采用的技术方案是：
[0005]一种基于图优化的IMU/相机/激光雷达联合标定方法，该方法包括如下步骤：
[0006]S1：根据IMU输出角速度信息拟合旋转B样条曲线得IMU姿态信息；
[0007]S2：采用NDT(正态分布变换)算法估计激光雷达旋转，根据S1估计的IMU姿态信息变化量与激光雷达旋转初始化IMU/激光雷达旋转外参；
[0008]S3：根据S2初始化的IMU/激光雷达旋转外参进行激光雷达去畸变处理；
[0009]S4：采用特征点算法估计相机旋转，根据S1估计的IMU姿态信息变化量与相机旋转初始化IMU/相机旋转外参，并初始化重力、尺度因子、陀螺仪零偏及速度；
[0010]S5：采用棋盘格标定板，提取相机系下棋盘格点坐标，根据聚类算法提取激光雷达系下棋盘格激光点对应法向量与中心点，根据点面距离最小构建目标函数，初始化激光雷达
‑
相机外参；
[0011]S6：根据IMU约束、激光雷达约束、相机约束、基于标定板的相机和激光雷达外参约束、IMU/相机/激光雷达外参互相约束构建联合优化目标方程，求解目标方程得IMU/相机/激光雷达外参。
[0012]所述的基于图优化的IMU/相机/激光雷达联合标定方法，所述步骤S1具体包括以下过程：
[0013]根据IMU输出角速度信息拟合旋转B样条曲线，通过解最小二乘问题计算拟合曲线的四元数控制点：
[0014][0015]其中，q0,....q
N
为B样条曲线四元数控制点，k为当前时刻，M为总采样时间，为k时刻IMU输出角速度信息，为0时刻到k时刻IMU的旋转矩阵。
[0016]所述的基于图优化的IMU/相机/激光雷达联合标定方法，所述步骤S2具体包括以下过程：
[0017]S2.1采用NDT算法估计激光雷达旋转，目标函数为：
[0018][0019]其中，为k
‑
1时刻到k时刻激光雷达的变换矩阵，i为当前激光点，n为总激光点数，为k时刻第i个激光点位置，为k
‑
1时刻第i个激光点位置；
[0020]S2.2根据S1估计的IMU姿态变化与S2.1估计的激光雷达旋转，初始化IMU/激光雷达旋转外参：
[0021][0022]其中，为k
‑
1时刻到k时刻激光雷达的旋转四元数，为激光雷达到IMU的旋转四元数，为k
‑
1时刻到k时刻IMU的旋转四元数。
[0023]所述的基于图优化的IMU/相机/激光雷达联合标定方法，所述步骤S3具体包括以下过程：
[0024]根据S2初始化的IMU/激光雷达旋转外参进行激光雷达去畸变处理：
[0025][0026]其中，为k时刻第0扫描系的第i个激光雷达点，为k时刻第j扫描系的第i个激光雷达点，为激光雷达从k时刻第j扫描系到第0扫描系的旋转矩阵。
[0027]所述的基于图优化的IMU/相机/激光雷达联合标定方法，所述步骤S4具体包括以下过程：
[0028]S4.1采用特征点算法估计相机旋转，求解所有帧位姿和路标点的3D位置；
[0029]S4.2根据S1估计的IMU姿态变化与相机旋转初始化IMU/相机旋转外参：
[0030][0031]其中，为k
‑
1时刻到k时刻相机的旋转四元数，为相机到IMU的旋转四元数，为k
‑
1时刻到k时刻IMU的旋转四元数；
[0032]S4.3初始化重力、尺度因子、陀螺仪零偏及速度。
[0033]所述的基于图优化的IMU/相机/激光雷达联合标定方法，所述步骤S5具体包括以下过程：
[0034]采用棋盘格标定板，提取相机系下棋盘格点坐标及平面法向量，根据聚类算法提取激光雷达系下棋盘格激光点对应平面法向量与中心点，根据点面距离最小构建目标函数，初始化激光雷达/相机外参。
[0035]S5.1根据相机系下的平面法向量N
C
和激光雷达系下的平面法向量N
L
求解相机到激
光雷达的旋转矩阵：
[0036][0037]其中，为相机到激光雷达的旋转矩阵，N
C
为相机系下的平面法向量，N
L
为激光雷达系下的平面法向量。
[0038]S5.2根据相机系下的点到激光雷达面的距离最小构建目标函数求解：
[0039][0040]其中，t(x,y,z)为相机到激光雷达的平移外参，为相机到激光雷达的旋转矩阵，为第i帧相机系下的第j个点，为第i帧激光雷达系下的中心点，为第i帧激光雷达系下的法向量。
[0041]所述的基于图优化的IMU/相机/激光雷达联合标定方法，所述步骤S6具体包括以下过程：
[0042]根据IMU约束、激光雷达约束、相机约束、基于标定板的相机和激光雷达外参约束、IMU/相机/激光雷达外参互相约束构建联合优化目标方程：
[0043][0044][0045][0046][0047][0048][0049][0050][0051]其中，x为优化变量，为加表残差，为陀螺残差，r
lj
为激光雷达残差，为相机残差，为相机与激光点面距离残差，为外参残差，A为加表采样数，W为陀螺采样数，L为雷达采样数，C为相机采样数，O为相机与激光匹配采样数，P为外参匹配采样数，为相机到激光雷达的旋转四元数，为相机到激光雷达的平移外参，为相机到IMU的旋转四元数，为相机到IMU的平移外参，为激光雷达到IMU的旋转四元数，为激光雷达到IMU的平移外参，x
q
为连续时间轨迹旋转控制点，x
p
为连续时间轨迹旋平移控制点，b
a
为本文档来自技高网...

【技术保护点】

【技术特征摘要】
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样条曲线，通过解最小二乘问题计算拟合曲线的四元数控制点：其中，q0,....q
N
为B样条曲线四元数控制点，k为当前时刻，M为总采样时间，为k时刻IMU输出角速度信息，为0时刻到k时刻IMU的旋转矩阵。3.根据权利要求2所述的基于图优化的IMU/相机/激光雷达联合标定方法，其特征在于，所述步骤S2具体包括以下过程：S2.1采用NDT算法估计激光雷达旋转，目标函数为：其中，为k
‑
1时刻到k时刻激光雷达的变换矩阵，i为当前激光点，n为总激光点数，为k时刻第i个激光点位置，为k
‑
1时刻第i个激光点位置；S2.2根据S1估计的IMU姿态变化与S2.1估计的激光雷达旋转，初始化IMU/激光雷达旋转外参：其中，为k
‑
1时刻到k时刻激光雷达的旋转四元数，为激光雷达到IMU的旋转四元数，为k
‑
1时刻到k时刻IMU的旋转四元数。4.根据权利要求2所述的基于图优化的IMU/相机/激光雷达联合标定方法，其特征在于，所述步骤S3具体包括以下过程：根据S2初始化的IMU/激光雷达旋转外参进行激光雷达去畸变处理：
其中，为k时刻第0扫描系的第i个激光雷达点，为k时刻第j扫描系的第i个激光雷达点，为激光雷达从k时刻第j扫描系到第0扫描系的旋转矩阵。5.根据权利要求2所述的基于图优化的IMU/相机/激光雷达联合标定方法，其特征在于，所述步骤S4具体包括以下过程：S4.1采用特征点算法估计相机旋转，求解所有帧位姿和路标点的3D位置；S4.2根据S1估计的IMU姿态变化与相机旋转初始化IMU/相机旋转外参：其中，为k
‑
1时刻到k时刻相机的旋转四元数，为相机到IMU的旋转四元数，为k
‑
1时刻到k时刻IMU的旋转四元数；S4.3初始化重力、尺度因子、陀螺仪零偏及速度。6.根据权利要求2所述的基于图优化的IMU/相机/激光雷达联合标定方法，其特征在于，所述步骤S5具体包括以下过程：采用棋盘格标定...

【专利技术属性】
技术研发人员：徐晓苏，侯岚华，姚逸卿，高佳誉，何宇明，
申请(专利权)人：东南大学，
类型：发明
国别省市：