CN116500595A - 激光雷达、相机和惯性传感器相互约束的外参标定方法 - Google Patents

Abstract

本发明公开了一种激光雷达、相机和惯性传感器相互约束的外参标定方法，其包括以下步骤：制作棋盘格雷达反射板，并形成标定系统；静态标定激光雷达和相机，得到激光雷达到相机的转换外参；对惯性传感器分别从视觉角度和激光雷达角度进行动态标定；构建状态估计量；构建最小化目标，获取最优的状态估计量中相机与惯性传感器之间的转换外参；由外参间的相互转换关系获取激光雷达与惯性传感器之间的转换外参，完成外参标定。本方法通过静态标定激光雷达和视觉相机、动态标定激光雷达和IMU、动态标定视觉相机和IMU互相结合的办法，使得各传感器形成相互约束的三角结构，实现高精度多传感器间的时空一致性和稳定性。

Description

激光雷达、相机和惯性传感器相互约束的外参标定方法

技术领域

本发明涉及传感器融合领域，具体涉及一种激光雷达、相机和惯性传感器相互约束的外参标定方法。

背景技术

SLAM(Simultaneous Localization and Mapping)技术，译为"同时定位与地图构建技术"。它是指安装了传感器的主体，在没有周围环境信息的条件下，通过传感器和自身运动估计自己的位姿，同时建立环境模型的技术。里程计(Odometry)技术是SLAM技术的前端，它通过不同的传感器数据来估计主体位置和姿态的变化。

目前已经有许多使用单一测量传感器的成功里程计框架，他们大多基于激光雷达或视觉相机。但是随着人们对智能机器人需求和要求的日益增长，SLAM技术也需要应对更复杂更具有挑战性的环境，因此近年来也引入IMU(惯性测量单元)、GPS(全球定位系统)等传感器进行辅助。多传感器结合的多模态融合里程计系统能综合各传感器的工作特性和区间，扬长避短，在部分传感器退化的情况中仍能鲁棒地工作，表现出了更强更广泛的应用。

虽然多传感器融合里程计优势明显，但当其应用在SLAM算法中时也更为复杂。为充分置信并使用各传感器所测量的数据，多模态融合里程计要求传感器之间的坐标轴转换(即外参)是先验的、确定的。以此能将来自不同传感器的信息转换到一个共同的物理参考系下进行计算。现有外参校准方法大多仅适用静态系统，没有包括IMU的使用。少数包含对IMU的标定方法也只停留在LIO系统或者VIO系统中，而没有将三种传感器结合在一起的工作实例，因此未能形成完备约束的整体系统。

发明内容

针对现有技术中的上述不足，本发明提供的一种激光雷达、相机和惯性传感器相互约束的外参标定方法解决了现有技术对激光雷达、相机和惯性传感器外参标定方法的精度和鲁棒性差的问题。

为了达到上述发明目的，本发明采用的技术方案为：

提供一种激光雷达、相机和惯性传感器相互约束的外参标定方法，其包括以下步骤：

S1、制作棋盘格雷达反射板；将激光雷达、相机和惯性传感器之间的相对位置进行固定，并形成标定系统；棋盘格雷达反射板上设置有黑白相间的方形棋盘格图案，且黑白色部分图案的雷达反射率不同；

S2、基于棋盘格雷达反射板，静态标定激光雷达和相机，得到激光雷达到相机的转换外参；

S3、移动标定系统，对惯性传感器分别从视觉角度和激光雷达角度进行动态标定，分别得到相机与惯性传感器之间的外参初值、激光雷达与惯性传感器之间的外参初值、世界坐标系下一段时间内的多帧相机位姿和世界坐标系下的同一段时间下的多帧激光雷达位姿；

S4、基于相机位姿、相机与惯性传感器之间的转换外参、参考点的逆深度和激光雷达位姿构建状态估计量；其中相机与惯性传感器之间的转换外参的初始值为相机与惯性传感器之间的外参初值；

S5、选取参考点，获取融合定位过程中的边缘化残差、两帧之间惯性传感器测量残差、视觉重投影误差和LO姿态误差；

S6、根据步骤S5获得的数据和步骤S4的状态估计量构建最小化目标，获取最优的状态估计量中相机与惯性传感器之间的转换外参；

S7、基于相机与惯性传感器之间的转换外参和激光雷达到相机的转换外参，由相机-惯性传感器-激光雷达外参间的相互转换关系获取激光雷达与惯性传感器之间的转换外参，完成外参标定。

进一步地，步骤S2的具体方法包括以下子步骤：

S2-1、获取相机的内参；

S2-2、固定棋盘格雷达反射板，分别使激光雷达和相机识别标定板上的点，对应得到3D雷达点云数据和2维图像数据；

S2-3、将3D雷达点云数据向2D投影，得到投影后的数据；

S2-4、将投影后的数据与2维图像数据中的对应点进行对齐，获取激光雷达到相机的转换外参

进一步地，步骤S3中对惯性传感器从视觉角度进行动态标定的具体方法包括以下子步骤：

A1、移动标定系统，获取惯性传感器数据和相机数据；

A2、将惯性传感器数据向相机数据进行对齐；

A3、将第k帧相机数据和第k+1帧相机数据之间的惯性传感器数据进行积分，得到两帧图像之间标定系统的相对位置、速度和旋转变化；

A4、以惯性传感器到相机相对位置的变化值为初始值，将每一帧从图像中获取到的特征点重投影误差为优化对象，对优化对象进行非线性优化，得到采集时间内不同相机帧代表的时间点t＝[t_v1,t_v2,…,t_vK]时，世界坐标系下的相机位姿和相机与惯性传感器之间的外参初值/>

进一步地，步骤S3中对惯性传感器从激光雷达进行动态标定的具体方法包括以下子步骤：

B1、移动标定系统，获取惯性传感器数据和激光雷达数据；

B2、将惯性传感器数据向激光雷达数据进行对齐；

B3、通过NDT扫描匹配从激光雷达点云中计算出从第i帧到第i+1帧激光雷达的运动估计值并获取一段时间内不同时间节点的世界坐标系下激光雷达位姿及其对应的时间点{t_L1,t_L2·…t_LN}；

B4、根据公式：

获取激光雷达与惯性传感器之间的外参初值其中/>为惯性传感器与第i帧激光雷达数据相对应的测量值；/>为惯性传感器与第i+1帧激光雷达数据相对应的测量值。

进一步地，步骤S4中状态估计量的表达式为：

其中X表示状态估计量；x₀,x₁,…x_N分别表示在对应相机帧时的相机位姿，为相机与惯性传感器之间的转换外参；{λ₀,λ₁,…,λ_M}表示M个参考点在其第一次出现的图像中的逆深度；{l₀,l₁,…l_A}分别为对应雷达帧时的雷达位姿。

进一步地，步骤S5中融合定位过程中的边缘化残差表达式为：

‖r_P-H_PX‖²

两帧之间惯性传感器测量残差表达式为：

视觉重投影误差的表达式为：

LO姿态误差的表达式为：

其中r_P和H_P均为来自边缘化的先验参数； 为两帧之间的加速度变化量，/>为两帧之间的速度变化量，/>为两帧之间的角度变化量，δb_a为两帧之间的加速度偏移变化量，δb_w为两帧之间的角加速度偏移变化量； 为第l个像素点在第d个归一化相机坐标系中观察到的坐标，/>为切平面上正交的一对正交基，[.]^T表示矩阵的转置， 为第l个像素点在第d个初始相机坐标系中观察到的坐标，‖.‖表示范数；/> 为第m个3D坐标点在第f个归一化雷达坐标系下观察到的坐标，/>为三维空间中一组正交基，/>为第m个3D坐标点在第f个初始雷达坐标系下观察到的坐标。

进一步地，步骤S6中最小化目标的表达式为：

进一步地，步骤S7的具体方法为：

将相机与惯性传感器之间的转换外参和激光雷达到相机的转换外参的乘积作为激光雷达与惯性传感器之间的转换外参，完成外参标定。

本发明的有益效果为：本方法通过静态标定激光雷达和视觉相机、动态标定激光雷达和IMU、动态标定视觉相机和IMU互相结合的办法，使得各传感器形成相互约束的三角结构，进一步还能拓展到与更多传感器联系的多因子约束外参空间，实现高精度多传感器间的时空一致性和稳定性。

附图说明

图1为本方法的流程示意图；

图2为棋盘格雷达反射板实物示意图；

图3为标定系统实物示意图。

具体实施方式

下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

如图1、图2和图3所示，该激光雷达、相机和惯性传感器相互约束的外参标定方法包括以下步骤：

S1、制作棋盘格雷达反射板；将激光雷达、相机和惯性传感器之间的相对位置进行固定，并形成标定系统；棋盘格雷达反射板上设置有黑白相间的方形棋盘格图案，且黑白色部分图案的雷达反射率不同；

S2、基于棋盘格雷达反射板，静态标定激光雷达和相机，得到激光雷达到相机的转换外参；

S3、移动标定系统，对惯性传感器分别从视觉角度和激光雷达角度进行动态标定，分别得到相机与惯性传感器之间的外参初值、激光雷达与惯性传感器之间的外参初值、世界坐标系下一段时间内的多帧相机位姿和世界坐标系下的同一段时间下的多帧激光雷达位姿；

S4、基于相机位姿、相机与惯性传感器之间的转换外参、参考点的逆深度和激光雷达位姿构建状态估计量；其中相机与惯性传感器之间的转换外参的初始值为相机与惯性传感器之间的外参初值；

S5、选取参考点，获取融合定位过程中的边缘化残差、两帧之间惯性传感器测量残差、视觉重投影误差和LO姿态误差；

S6、根据步骤S5获得的数据和步骤S4的状态估计量构建最小化目标，获取最优的状态估计量中相机与惯性传感器之间的转换外参；

S7、基于相机与惯性传感器之间的转换外参和激光雷达到相机的转换外参，由相机-惯性传感器-激光雷达外参间的相互转换关系获取激光雷达与惯性传感器之间的转换外参，完成外参标定。

步骤S2的具体方法包括以下子步骤：

S2-1、获取相机的内参；

S2-2、固定棋盘格雷达反射板，分别使激光雷达和相机识别标定板上的点，对应得到3D雷达点云数据和2维图像数据；

S2-3、将3D雷达点云数据向2D投影，得到投影后的数据；

S2-4、将投影后的数据与2维图像数据中的对应点进行对齐，获取激光雷达到相机的转换外参

步骤S3中对惯性传感器从视觉角度进行动态标定的具体方法包括以下子步骤：

A1、移动标定系统，获取惯性传感器数据和相机数据；

A2、将惯性传感器数据向相机数据进行对齐；

A3、将第k帧相机数据和第k+1帧相机数据之间的惯性传感器数据进行积分，得到两帧图像之间标定系统的相对位置、速度和旋转变化；

A4、以惯性传感器到相机相对位置的变化值为初始值，将每一帧从图像中获取到的特征点重投影误差为优化对象，对优化对象进行非线性优化，得到采集时间内不同相机帧代表的时间点t＝[t_v1,t_v2,…,t_vK]时，世界坐标系下的相机位姿和相机与惯性传感器之间的外参初值/>

步骤S3中对惯性传感器从激光雷达进行动态标定的具体方法包括以下子步骤：

B1、移动标定系统，获取惯性传感器数据和激光雷达数据；

B2、将惯性传感器数据向激光雷达数据进行对齐；

B3、通过NDT扫描匹配从激光雷达点云中计算出从第i帧到第i+1帧激光雷达的运动估计值并获取一段时间内不同时间节点的世界坐标系下激光雷达位姿及其对应的时间点{t_L1,t_L2·…t_LN}；

B4、根据公式：

获取激光雷达与惯性传感器之间的外参初值其中/>为惯性传感器与第i帧激光雷达数据相对应的测量值；/>为惯性传感器与第i+1帧激光雷达数据相对应的测量值。

步骤S4中状态估计量的表达式为：

其中X表示状态估计量；x₀,x₁,…x_N分别表示在对应相机帧时的相机位姿，为相机与惯性传感器之间的转换外参；{λ₀,λ₁,…,λ_M}表示M个参考点在其第一次出现的图像中的逆深度；{l₀,l₁,…l_A}分别为对应雷达帧时的雷达位姿。

步骤S5中四种残差均以马氏距离表示，融合定位过程中的边缘化残差表达式为：

‖r_P-H_PX‖²

其中r_P和H_P均为来自边缘化的先验参数；由于整个优化过程采用了滑动窗口的方式，在窗口滑动时需要对最老帧进行剔除，将联合概率分布分解为边缘概率分布和条件概率分布，对此部分有一先验的残差计算和优化。

两帧之间惯性传感器测量残差表达式为：

其表示了两帧之间PVQ和bias的变化量的差；表示惯性传感器测量残差由15维向量组成，包含了加速度变化量(三维)/>速度变化量(三维)/>角度变化量(三维)/>加速度偏移变化量(三维)δb_a和角加速度偏移变化量(三维)δb_w。表示IMU测量值预积分得到的两帧之间PVQ增量的预测/>和待优化量X的残差。通过最小化残差参与整体求解。

视觉残差是重投影误差，对于第l个像素点P，将P从第一次观看到它的相机坐标系，转换到当前的第d个相机坐标系下的像素坐标，视觉重投影误差的表达式为：

为第l个像素点在第d个归一化坐标系中观察到的坐标，/>为切平面上正交的一对正交基，[.]^T表示矩阵的转置， 为第l个像素点在第d个初始相机坐标系中观察到的坐标，‖.‖表示范数。

与视觉约束类似，激光雷达对标定系统的约束表示为雷达点云投影的残差。对于第m个雷达点L，将L从第一次观看到它的雷达坐标系，转换到当前的第f个雷达坐标系下的三维坐标，LO姿态误差的表达式为：

为第m个3D坐标点在第f个激光雷达的归一化雷达坐标系下观察到的坐标，/>为三维空间中一组正交基，/>为第m个3D坐标点在第f个初始雷达坐标系下观察到的坐标。像素点和参考点均是在图像中提取出的特征点。3D坐标点为从雷达点云中提取出的特征点。

步骤S6中最小化目标的表达式为：

在实现最小化目标的过程中会得到最佳X，也就会得到最佳X中的即得到相机与惯性传感器之间的转换外参。

步骤S7的具体方法为：将相机与惯性传感器之间的转换外参和激光雷达到相机的转换外参/>的乘积作为激光雷达与惯性传感器之间的转换外参/>完成外参标定，即/>

在本发明的一个实施例中，三种传感器两两标定的精度大概在厘米级左右，其中动态效果比静态略差，受标定环境的影响还会有较为明显的波动。本方法通过由相机和雷达标定来指导另外两组传感器互标定的方式，将对IMU的标定精度提高一倍(误差减半)。同时多传感器约束将带来更高的稳定性，在结果的方差呈现上有有效提升。

Claims (8)

1.一种激光雷达、相机和惯性传感器相互约束的外参标定方法，其特征在于，包括以下步骤：

S1、制作棋盘格雷达反射板；将激光雷达、相机和惯性传感器之间的相对位置进行固定，并形成标定系统；棋盘格雷达反射板上设置有黑白相间的方形棋盘格图案，且黑白色部分图案的雷达反射率不同；

S2、基于棋盘格雷达反射板，静态标定激光雷达和相机，得到激光雷达到相机的转换外参；

S3、移动标定系统，对惯性传感器分别从视觉角度和激光雷达角度进行动态标定，分别得到相机与惯性传感器之间的外参初值、激光雷达与惯性传感器之间的外参初值、世界坐标系下一段时间内的多帧相机位姿和世界坐标系下的同一段时间下的多帧激光雷达位姿；

S4、基于相机位姿、相机与惯性传感器之间的转换外参、参考点的逆深度和激光雷达位姿构建状态估计量；其中相机与惯性传感器之间的转换外参的初始值为相机与惯性传感器之间的外参初值；

S5、选取参考点，获取融合定位过程中的边缘化残差、两帧之间惯性传感器测量残差、视觉重投影误差和LO姿态误差；

S6、根据步骤S5获得的数据和步骤S4的状态估计量构建最小化目标，获取最优的状态估计量中相机与惯性传感器之间的转换外参；

S7、基于相机与惯性传感器之间的转换外参和激光雷达到相机的转换外参，由相机-惯性传感器-激光雷达外参间的相互转换关系获取激光雷达与惯性传感器之间的转换外参，完成外参标定。

2.根据权利要求1所述的激光雷达、相机和惯性传感器相互约束的外参标定方法，其特征在于，步骤S2的具体方法包括以下子步骤：

S2-1、获取相机的内参；

S2-2、固定棋盘格雷达反射板，分别使激光雷达和相机识别标定板上的点，对应得到3D雷达点云数据和2维图像数据；

S2-3、将3D雷达点云数据向2D投影，得到投影后的数据；

S2-4、将投影后的数据与2维图像数据中的对应点进行对齐，获取激光雷达到相机的转换外参

3.根据权利要求1所述的激光雷达、相机和惯性传感器相互约束的外参标定方法，其特征在于，步骤S3中对惯性传感器从视觉角度进行动态标定的具体方法包括以下子步骤：

A1、移动标定系统，获取惯性传感器数据和相机数据；

A2、将惯性传感器数据向相机数据进行对齐；

A3、将第k帧相机数据和第k+1帧相机数据之间的惯性传感器数据进行积分，得到两帧图像之间标定系统的相对位置、速度和旋转变化；

A4、以惯性传感器到相机相对位置的变化值为初始值，将每一帧从图像中获取到的特征点重投影误差为优化对象，对优化对象进行非线性优化，得到采集时间内不同相机帧代表的时间点t＝[t_v1,t_v2,…,t_vK]时，世界坐标系下的相机位姿和相机与惯性传感器之间的外参初值/>

4.根据权利要求3所述的激光雷达、相机和惯性传感器相互约束的外参标定方法，其特征在于，步骤S3中对惯性传感器从激光雷达进行动态标定的具体方法包括以下子步骤：

B1、移动标定系统，获取惯性传感器数据和激光雷达数据；

B2、将惯性传感器数据向激光雷达数据进行对齐；

B3、通过NDT扫描匹配从激光雷达点云中计算出从第i帧到第i+1帧激光雷达的运动估计值并获取一段时间内不同时间节点的世界坐标系下激光雷达位姿及其对应的时间点{t_L1,t_L2·…t_LN}；

B4、根据公式：

获取激光雷达与惯性传感器之间的外参初值其中/>为惯性传感器与第i帧激光雷达数据相对应的测量值；/>为惯性传感器与第i+1帧激光雷达数据相对应的测量值。

5.根据权利要求4所述的激光雷达、相机和惯性传感器相互约束的外参标定方法，其特征在于，步骤S4中状态估计量的表达式为：

其中X表示状态估计量；x₀,x₁,…x_N分别表示在对应相机帧时的相机位姿，为相机与惯性传感器之间的转换外参；{λ₀,λ₁,…,λ_M}表示M个参考点在其第一次出现的图像中的逆深度；{l₀,l₁,…l_A}分别为对应雷达帧时的雷达位姿。

6.根据权利要求5所述的激光雷达、相机和惯性传感器相互约束的外参标定方法，其特征在于，步骤S5中融合定位过程中的边缘化残差表达式为：

‖r_P-H_PX‖²

两帧之间惯性传感器测量残差表达式为：

视觉重投影误差的表达式为：

LO姿态误差的表达式为：

其中r_P和H_P均为来自边缘化的先验参数； 为两帧之间的加速度变化量，/>为两帧之间的速度变化量，/>为两帧之间的角度变化量，δb_a为两帧之间的加速度偏移变化量，δb_w为两帧之间的角加速度偏移变化量； 为第l个像素点在第d个归一化相机坐标系中观察到的坐标，/>为切平面上正交的一对正交基，[.]^T表示矩阵的转置， 为第l个像素点在第d个初始相机坐标系中观察到的坐标，‖.‖表示范数；/> 为第m个3D坐标点在第f个归一化雷达坐标系下观察到的坐标，/>为三维空间中一组正交基，/>为第m个3D坐标点在第f个初始雷达坐标系下观察到的坐标。

7.根据权利要求6所述的激光雷达、相机和惯性传感器相互约束的外参标定方法，其特征在于，步骤S6中最小化目标的表达式为：

8.根据权利要求7所述的激光雷达、相机和惯性传感器相互约束的外参标定方法，其特征在于，步骤S7的具体方法为：

将相机与惯性传感器之间的转换外参和激光雷达到相机的转换外参的乘积作为激光雷达与惯性传感器之间的转换外参，完成外参标定。