CN118115597A - 双目相机、imu和激光雷达联合标定方法及*** - Google Patents
双目相机、imu和激光雷达联合标定方法及*** Download PDFInfo
- Publication number
- CN118115597A CN118115597A CN202410317337.9A CN202410317337A CN118115597A CN 118115597 A CN118115597 A CN 118115597A CN 202410317337 A CN202410317337 A CN 202410317337A CN 118115597 A CN118115597 A CN 118115597A
- Authority
- CN
- China
- Prior art keywords
- imu
- laser radar
- binocular camera
- coordinate system
- camera
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 55
- 239000011159 matrix material Substances 0.000 claims abstract description 81
- 230000009466 transformation Effects 0.000 claims abstract description 42
- 238000005457 optimization Methods 0.000 claims abstract description 16
- 238000004364 calculation method Methods 0.000 claims abstract description 10
- 239000013598 vector Substances 0.000 claims description 56
- 238000006243 chemical reaction Methods 0.000 claims description 22
- 230000001131 transforming effect Effects 0.000 claims description 15
- 238000005259 measurement Methods 0.000 claims description 11
- 230000036544 posture Effects 0.000 claims description 9
- 230000004927 fusion Effects 0.000 claims description 7
- 238000005070 sampling Methods 0.000 claims description 6
- 230000001360 synchronised effect Effects 0.000 claims description 5
- 230000005484 gravity Effects 0.000 claims description 3
- 230000006870 function Effects 0.000 description 11
- 238000004590 computer program Methods 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Landscapes
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明涉及一种双目相机、IMU和激光雷达联合标定方法及***。本方法获取双目相机、IMU的外参、IMU的噪声，对双目相机和IMU的位姿进行状态估计，并进行双目相机和IMU的外参标定；获取激光雷达、IMU的外参、IMU的噪声，对激光雷达和IMU的位姿进行状态估计，并进行激光雷达和IMU的外参标定；利用外参标定结果，计算双目相机到激光雷达的初始变换矩阵；联合标定双目相机和激光雷达，获得标定板数据，从标定板数据中提取线和面的特征点并进行关联约束计算；根据外参标定结果，进行双目相机、IMU和激光雷达联合优化标定。本发明利用外参相互约束，可消除传感器间误差，提高标定精度。
Description
技术领域
本发明涉及自动驾驶技术领域，具体涉及一种双目相机、IMU和激光雷达联合标定方法及***。
背景技术
在自动驾驶***中，传感器数据的准确性对于***的性能和安全性至关重要。其中，双目相机、IMU(Inertial measurement unit，惯性测量单元)和激光雷达是自动驾驶***中常用的传感器，分别提供视觉信息、运动姿态信息和三维点云信息，通过算法求解各个传感器之间的相对位置和姿态关系，可实现异源多传感器数据的融合和利用。
然而，不同传感器之间的数据是存在误差和偏差的，在数据融合中，会对结果的精度和可靠性造成严重影响。
因此，有必要提出异源多传感器的联合标定方法，对双目相机、IMU和激光雷达进行准确标定。
发明内容
本发明的目的是提供一种双目相机、IMU和激光雷达联合标定方法及***，以解决由于异源多传感器存在数据误差导致的***精度和可靠性较差的问题。
为了达到上述目的，本发明所采用的技术方案为：
双目相机、IMU和激光雷达联合标定方法，所述方法包括：
获取双目相机的外参、IMU的外参、IMU的噪声，作为状态向量对双目相机和IMU的位姿进行状态估计，并进行双目相机和IMU的外参标定；
获取激光雷达的外参、IMU的外参、IMU的噪声，作为状态向量对激光雷达和IMU的位姿进行状态估计，并进行激光雷达和IMU的外参标定；
利用双目相机和IMU的外参标定结果以及激光雷达和IMU的外参标定结果，计算双目相机到激光雷达的初始变换矩阵；
联合标定双目相机和激光雷达，获得标定板数据，从标定板数据中提取线和面的特征点并进行关联约束计算；
根据双目相机和IMU的外参标定结果、激光雷达和IMU的外参标定结果，进行双目相机、IMU和激光雷达联合优化标定。
进一步地，所述方法还包括：
首先对双目相机、IMU、激光雷达分别进行标定；
通过拍摄标定板对双目相机进行标定，获取内参矩阵和畸变系数；
对重力加速度计和陀螺仪进行数据融合从而对IMU进行标定，获取偏移量和比例因子；
通过在多个不同姿态下测量目标物体的方式对激光雷达进行标定，获得激光雷达的坐标系与双目相机、IMU之间的相对位置和姿态。
进一步地，获取双目相机的外参、IMU的外参、IMU的噪声，作为状态向量对双目相机和IMU的位姿进行状态估计，并进行双目相机和IMU的外参标定，包括：
双目相机的坐标系坐标和IMU的坐标系坐标的变换关系为：
Twb＝Twc·Tcb；
其中：
Twb为从世界坐标系转换到IMU坐标系的转换矩阵；
Twc为从世界坐标系转换到相机坐标系的转换矩阵；
Tcb为从相机坐标系转换到IMU坐标系的转换矩阵；
Rwb为从世界坐标系转换到IMU坐标系的旋转矩阵；
Rwc为从世界坐标系转换到相机坐标系的转换矩阵；
Rcb为从相机坐标系转换到IMU坐标系的旋转矩阵；
twb为从世界坐标系转换到IMU坐标系的平移向量；
twc为从世界坐标系转换到相机坐标系的平移向量；
tcb为从相机坐标系转换到IMU坐标系的平移向量；
考虑尺度因子s，则双目相机的坐标系和IMU的坐标系之间旋转角和平移量的变换关系为：
Rwb＝Rwc·Rcb；
Twb＝Rwc·Tcb+s·Twc；
其中：
Twb为从世界坐标系转换到IMU坐标系的转换矩阵；
Tcb为从相机坐标系转换到IMU坐标系的转换矩阵；
Twc为从世界坐标系转换到相机坐标系的转换矩阵；
对双目相机与IMU进行同步。
进一步地，对双目相机与IMU进行同步，包括：
双目相机和IMU之间存在时间差td：
tIMU＝tcam+td；
其中：
tIMU为IMU的采样时间；
tcam为相机的采样时间；
将双目相机的时间戳平移td后，双目相机与IMU被同步。
进一步地，获取激光雷达的外参、IMU的外参、IMU的噪声，作为状态向量对激光雷达和IMU的位姿进行状态估计，并进行激光雷达和IMU的外参标定，包括：
通过手眼标定法粗略估计激光雷达和IMU的旋转矩阵：
其中：
为t0时刻激光雷达转换到IMU的转换矩阵；
为tn时刻激光雷达坐标系转换到t0时刻相机坐标系的转换矩阵；
为tn时刻IMU坐标系转换到tn时刻激光雷达坐标系的转换矩阵；
为tn时刻IMU坐标系转换到t0时刻IMU坐标系的转换矩阵；
设定目标优化函数为：
f(ξLI)＝argmin{||γIMU(z,X)||2+|γPlane(z,X)||2+||γLine(z,X)||2}；
其中：
γIMU(z,X)为IMU测量误差；
γPlane(z,X)为激光雷达平面点到地图的距离残差；
γLine(z,X)为激光雷达直线点到地图的距离残差。
进一步地，利用双目相机和IMU的外参标定结果以及激光雷达和IMU的外参标定结果，计算双目相机到激光雷达的初始变换矩阵，包括：
其中：
为IMU到相机坐标系的转换矩阵；
为激光雷达到IMU的转换矩阵，/>为激光雷达到相机坐标系的转换矩阵；
然后，从图像和激光雷达中提取标定板中的三维线和平面特征点，将激光雷达上的特征点转换到相机坐标系上，并利用最近邻原理找到激光雷达和相机点的关联点。
进一步地，联合标定双目相机和激光雷达，获得标定板数据，从标定板数据中提取线和面的特征点并进行关联约束计算，包括：
由双目相机捕获的图像数据为(u,v)，由激光雷达捕获的3维点阵云为(x,y,z)，建立转化矩阵M，将(x,y,z)映射到(u,v)：
其中：
矩阵(fu，fv，u0，v0)为双目相机参数，fu和fv是X和Y轴方向尺度因子，即水平方向和垂直方向的有效焦距，u0，v0是像平面的中心点，即主点坐标；
R为旋转矩阵；
t为平移矢量；
mij为转化矩阵M的元素，i＝1,2,3,4，j＝1,2,3,4；
进一步地，根据双目相机和IMU的外参标定结果、激光雷达和IMU的外参标定结果，进行双目相机、IMU和激光雷达联合优化标定，包括：
载体的运动状态为：
其中：
XICL为载体的状态向量；
x0和xm是载体在某一个时刻的位置和速度向量；
是IMU到双目相机的旋转四元数；
是IMU到双目相机的旋转矩阵；
是IMU到激光雷达的旋转四元数；
是IMU到激光雷达的旋转矩阵；
建立优化目标函数：
f(ξICL)＝argmin{||γIMU(z,X)||2+||γchessboard_L(z,X)||2}；
其中：
误差项γIMU(z,X)为IMU测量残差；
γchessboard_L(z,X)为IMU测量残差；
激光雷达平面点到地图的距离残差和激光雷达直线点到地图的距离残差；
根据优化目标函数，估计双目相机和IMU、激光雷达和IMU、激光雷达和双目相机的外参，优化的残差为：
其中：
为激光雷达直线点在激光雷达坐标系下的点坐标向量；
为激光雷达直线点在相机坐标系下的点坐标向量；
为激光雷达平面点在激光雷达坐标系下的点坐标向量；
为激光雷达平面点在相机坐标系下的点坐标向量；
另一方面，提供双目相机、IMU和激光雷达联合标定***，所述***用于实施所述的方法，包括：
第一外参标定模块，用于获取双目相机的外参、IMU的外参、IMU的噪声，作为状态向量对双目相机和IMU的位姿进行状态估计，并进行双目相机和IMU的外参标定；
第二外参标定模块，用于获取激光雷达的外参、IMU的外参、IMU的噪声，作为状态向量对激光雷达和IMU的位姿进行状态估计，并进行激光雷达和IMU的外参标定；
计算模块，用于利用双目相机和IMU的外参标定结果以及激光雷达和IMU的外参标定结果，计算双目相机到激光雷达的初始变换矩阵；
约束模块，用于联合标定双目相机和激光雷达，获得标定板数据，从标定板数据中提取线和面的特征点并进行关联约束计算；
联合标定模块，用于根据双目相机和IMU的外参标定结果、激光雷达和IMU的外参标定结果，进行双目相机、IMU和激光雷达联合优化标定。
进一步地，所述***还包括：
标定模块，用于对双目相机、IMU、激光雷达分别进行标定。
与现有技术相比，本发明的有益效果如下：
本发明提供了一种双目相机、IMU和激光雷达联合标定方法，基于双目相机和IMU的外参标定结果和激光雷达和IMU的外参标定结果，进行双目相机、IMU和激光雷达联合优化标定，同时确定激光雷达的坐标系与双目相机、IMU之间的相对位置和姿态，利用双目相机和IMU、双目相机和激光雷达、激光雷达和IMU的外参之间的相互约束，消除不同传感器之间的误差，提高双目相机、IMU和激光雷达标定精度，提高了传感器之间的协同性和准确性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他实施例的附图。
图1是本发明的方法流程图。
具体实施方式
为了便于理解本发明，下面将参照相关附图对本发明进行更全面的描述。附图中给出了本发明的较佳实施方式。但是，本发明可以以许多不同的形式来实现，并不限于本文所描述的实施方式。相反地，提供这些实施方式的目的是使对本发明的公开内容理解的更加透彻全面。
应注意到，相似的标号和字母表示类似项，因此，一旦某一项在一个实施例中被定义，则在随后的实施例中不需要对其进行进一步定义和解释。此外，术语“包括”等以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、***、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
还应注意到，虽然在方法描述中涉及了步骤顺序，但是在某些情况下，可以以不同于此处的顺序执行，不应被理解为对步骤顺序的限制。
具体实施方式中，“内参”表示内部参数，“外参”表示外部参数。
本发明提供了一种双目相机、IMU和激光雷达联合标定方法，首先需要对双目相机、IMU、激光雷达分别进行标定，具体包括：
通过拍摄标定板对双目相机进行标定，获取内参矩阵和畸变系数。相机需要标定的参数通常分为内参和外参两部分，内参即为相机内部参数。光学镜头和光电传感器安装时总会存在误差，这种误差需要用内参来描述。获取内参矩阵和畸变系数可以实现从相机坐标系转换到像素坐标系中。
对重力加速度计和陀螺仪进行数据融合从而对IMU进行标定，获取偏移量和比例因子。获取这些结果的目的是获得IMU的精确姿态和旋转角。
通过在多个不同姿态下测量目标物体的方式对激光雷达进行标定，获得激光雷达的坐标系与双目相机、IMU之间的相对位置和姿态。该操作，可以得到三者之间的相对位置和姿态，为下一步进行三者之间的融合定位作数据准备，相当于把三者放在了同一个坐标系下，方便后面进行定位导航时进行位置和姿态计算。
上述标定方法均为现有方法，本发明不做改进。
如图1，所述方法包括：
S1：获取双目相机的外参、IMU的外参、IMU的噪声，作为状态向量对双目相机和IMU的位姿进行状态估计，并进行双目相机和IMU的外参标定，包括：
S101：双目相机的坐标系坐标和IMU的坐标系坐标的变换关系为：
Twb＝Twc·Tcb；
其中：
Twb为从世界坐标系转换到IMU坐标系的转换矩阵；
Twc为从世界坐标系转换到相机坐标系的转换矩阵；
Tcb为从相机坐标系转换到IMU坐标系的转换矩阵；
Rwb为从世界坐标系转换到IMU坐标系的旋转矩阵；
Rwc为从世界坐标系转换到相机坐标系的转换矩阵；
Rcb为从相机坐标系转换到IMU坐标系的旋转矩阵；
twb为从世界坐标系转换到IMU坐标系的平移向量；
twc为从世界坐标系转换到相机坐标系的平移向量；
tcb为从相机坐标系转换到IMU坐标系的平移向量；
S102：考虑尺度因子s，则双目相机的坐标系和IMU的坐标系之间旋转角和平移量的变换关系为：
Rwb＝Rwc·Rcb；
Twb＝Rwc·Tcb+s·Twc；
其中：
Twb为从世界坐标系转换到IMU坐标系的转换矩阵；
Tcb为从相机坐标系转换到IMU坐标系的转换矩阵；
Twc为从世界坐标系转换到相机坐标系的转换矩阵；
S103：对双目相机与IMU进行同步，包括：
双目相机和IMU之间存在时间差td：
tIMU＝tcam+td；
其中：
tIMU为IMU的采样时间；
tcam为相机的采样时间；
将双目相机的时间戳平移td后，双目相机与IMU被同步。
以上获得双目相机和IMU的外参标定结果。
S2：获取激光雷达的外参、IMU的外参、IMU的噪声，作为状态向量对激光雷达和IMU的位姿进行状态估计，并进行激光雷达和IMU的外参标定，包括：
通过手眼标定法粗略估计激光雷达和IMU的旋转矩阵：
其中：
为t0时刻激光雷达转换到IMU的转换矩阵；
为tn时刻激光雷达坐标系转换到t0时刻相机坐标系的转换矩阵；
为tn时刻IMU坐标系转换到tn时刻激光雷达坐标系的转换矩阵；
为tn时刻IMU坐标系转换到t0时刻IMU坐标系的转换矩阵；
设定目标优化函数为：
f(ξLI)＝argmin{||γIMU(z,X)||2+|γPlane(z,X)||2+||γLine(z,X)||2}；
其中：
γIMU(z,X)为IMU测量误差；
γPlane(z,X)为激光雷达平面点到地图的距离残差；
γLine(z,X)为激光雷达直线点到地图的距离残差。
其中，激光雷达与地图之间的平面点残差和线点残差由IMU和激光雷达的外部参数组成。通过优化上式目标优化函数，可以在线实时地估计IMU/激光雷达的外部参数和机动车辆的姿态。
S3：利用双目相机和IMU的外参标定结果以及激光雷达和IMU的外参标定结果，计算双目相机到激光雷达的初始变换矩阵，包括：
其中：
为IMU到相机坐标系的转换矩阵；
为激光雷达到IMU的转换矩阵，/>为激光雷达到相机坐标系的转换矩阵；
然后，从图像和激光雷达中提取标定板中的三维线和平面特征点。线特征点的引入可以提高标定***的可观测性。最后，可以将激光雷达上的特征点转换到相机坐标系上，并利用最近邻原理找到激光雷达和相机点的关联点。
S4：联合标定双目相机和激光雷达，获得标定板数据，从标定板数据中提取线和面的特征点并进行关联约束计算，包括：
由双目相机捕获的图像数据为(u,v)，由激光雷达捕获的3维点阵云为(x,y,z)，建立转化矩阵M，将(x,y,z)映射到(u,v)：
其中：
矩阵(fu，fv，u0，v0)为双目相机参数；
fu和fv是X和Y轴方向尺度因子，即水平方向和垂直方向的有效焦距；
u0，v0是像平面的中心点，即主点坐标；
R为旋转矩阵；
t为平移矢量；
mij为转化矩阵M的元素，i＝1,2,3,4，j＝1,2,3,4；
标定板数据指的是双目相机拍摄标定板数据、激光雷达扫描标定板数据，双目相机和激光雷达形成标定板数据时用的是同一块标定板，且标定板处于同一位置。线的特征点指的是比如长方形或者正方形的边，面的特征点指的是长方形或者正方形。关联约束计算指的是比如长方形的边上的一系列点应该为直线，长方形或者正方形上的一系列点应该组成的图形是长方形或者正方形。
S5：根据双目相机和IMU的外参标定结果、激光雷达和IMU的外参标定结果，进行双目相机、IMU和激光雷达联合优化标定，包括：
载体的运动状态为：
其中：
XICL为载体的状态向量；
x0和xm是载体在某一个时刻的位置和速度向量；
是IMU到双目相机的旋转四元数；
是IMU到双目相机的旋转矩阵；
是IMU到激光雷达的旋转四元数；
是IMU到激光雷达的旋转矩阵；
建立优化目标函数：
f(ξICL)＝argmin{||γIMU(z,X)||2+||γchessboard_L(z,X)||2}；
误差项γIMU(z,X)为IMU测量残差；
γchessboard_L(z,X)为IMU测量残差；
激光雷达平面点到地图的距离残差和激光雷达直线点到地图的距离残差；
根据优化目标函数，估计双目相机和IMU、激光雷达和IMU、激光雷达和双目相机的外参，优化的残差为：
其中：
为激光雷达直线点在激光雷达坐标系下的点坐标向量；
为激光雷达直线点在相机坐标系下的点坐标向量；
为激光雷达平面点在激光雷达坐标系下的点坐标向量；
为激光雷达平面点在相机坐标系下的点坐标向量。
将双目相机、IMU和激光雷达的数据进行融合，通过数据融合算法计算出传感器之间的准确关系和坐标转换。
另外，本发明还提供了一种双目相机、IMU和激光雷达联合标定***，所述***用于实施上述方法，包括：
第一外参标定模块，用于获取双目相机的外参、IMU的外参、IMU的噪声，作为状态向量对双目相机和IMU的位姿进行状态估计，并进行双目相机和IMU的外参标定，对应上述方法的S1；
第二外参标定模块，用于获取激光雷达的外参、IMU的外参、IMU的噪声，作为状态向量对激光雷达和IMU的位姿进行状态估计，并进行激光雷达和IMU的外参标定，对应上述方法的S2；
计算模块，用于利用双目相机和IMU的外参标定结果以及激光雷达和IMU的外参标定结果，计算双目相机到激光雷达的初始变换矩阵，对应上述方法的S3；
约束模块，用于联合标定双目相机和激光雷达，获得标定板数据，从标定板数据中提取线和面的特征点并进行关联约束计算，对应上述方法的S4；
联合标定模块，用于根据双目相机和IMU的外参标定结果、激光雷达和IMU的外参标定结果，进行双目相机、IMU和激光雷达联合优化标定，对应上述方法的S5。
本领域技术人员可以理解，本发明实施例的全部或部分功能可以通过硬件的方式实现，也可以通过计算机程序的方式实现。当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器、随机存储器、磁盘、光盘、硬盘等，通过计算机执行该程序以实现上述功能。例如，将程序存储在设备的存储器中，当通过处理器执行存储器中程序，即可实现上述全部或部分功能。另外，当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序也可以存储在服务器、另一计算机、磁盘、光盘、闪存盘或移动硬盘等存储介质中，通过下载或复制保存到本地设备的存储器中，或对本地设备的***进行版本更新，当通过处理器执行存储器中的程序时，即可实现上述实施方式中全部或部分功能。
以上应用了具体个例对本发明进行阐述，只是用于帮助理解本发明，并不用以限制本发明。对于本发明所属技术领域的技术人员，依据本发明的思想，还可以做出若干简单推演、变形或替换。
Claims (10)
1.双目相机、IMU和激光雷达联合标定方法，其特征在于：
所述方法包括：
获取双目相机的外参、IMU的外参、IMU的噪声，作为状态向量对双目相机和IMU的位姿进行状态估计，并进行双目相机和IMU的外参标定；
获取激光雷达的外参、IMU的外参、IMU的噪声，作为状态向量对激光雷达和IMU的位姿进行状态估计，并进行激光雷达和IMU的外参标定；
利用双目相机和IMU的外参标定结果以及激光雷达和IMU的外参标定结果，计算双目相机到激光雷达的初始变换矩阵；
联合标定双目相机和激光雷达，获得标定板数据，从标定板数据中提取线和面的特征点并进行关联约束计算；
根据双目相机和IMU的外参标定结果、激光雷达和IMU的外参标定结果，进行双目相机、IMU和激光雷达联合优化标定。
2.根据权利要求1所述的双目相机、IMU和激光雷达联合标定方法，其特征在于：
所述方法还包括：
首先对双目相机、IMU、激光雷达分别进行标定；
通过拍摄标定板对双目相机进行标定，获取内参矩阵和畸变系数；
对重力加速度计和陀螺仪进行数据融合从而对IMU进行标定，获取偏移量和比例因子；
通过在多个不同姿态下测量目标物体的方式对激光雷达进行标定，获得激光雷达的坐标系与双目相机、IMU之间的相对位置和姿态。
3.根据权利要求1所述的双目相机、IMU和激光雷达联合标定方法，其特征在于：
获取双目相机的外参、IMU的外参、IMU的噪声，作为状态向量对双目相机和IMU的位姿进行状态估计，并进行双目相机和IMU的外参标定，包括：
双目相机的坐标系坐标和IMU的坐标系坐标的变换关系为：
Twb＝Twc·Tcb；
其中：
Twb为从世界坐标系转换到IMU坐标系的转换矩阵；
Twc为从世界坐标系转换到相机坐标系的转换矩阵；
Tcb为从相机坐标系转换到IMU坐标系的转换矩阵；
Rwb为从世界坐标系转换到IMU坐标系的旋转矩阵；
Rwc为从世界坐标系转换到相机坐标系的转换矩阵；
Rcb为从相机坐标系转换到IMU坐标系的旋转矩阵；
twb为从世界坐标系转换到IMU坐标系的平移向量；
twc为从世界坐标系转换到相机坐标系的平移向量；
tcb为从相机坐标系转换到IMU坐标系的平移向量；
考虑尺度因子s，则双目相机的坐标系和IMU的坐标系之间旋转角和平移量的变换关系为：
Rwb＝Rwc·Rcb；
Twb＝Rwc·Tcb+s·Twc；
其中：
Twb为从世界坐标系转换到IMU坐标系的转换矩阵；
Tcb为从相机坐标系转换到IMU坐标系的转换矩阵；
Twc为从世界坐标系转换到相机坐标系的转换矩阵；
对双目相机与IMU进行同步。
4.根据权利要求3所述的双目相机、IMU和激光雷达联合标定方法，其特征在于：
对双目相机与IMU进行同步，包括：
双目相机和IMU之间存在时间差td：
tIMU＝tcam+td；
其中：
tIMU为IMU的采样时间；
tcam为相机的采样时间；
将双目相机的时间戳平移td后，双目相机与IMU被同步。
5.根据权利要求4所述的双目相机、IMU和激光雷达联合标定方法，其特征在于：
获取激光雷达的外参、IMU的外参、IMU的噪声，作为状态向量对激光雷达和IMU的位姿进行状态估计，并进行激光雷达和IMU的外参标定，包括：
通过手眼标定法粗略估计激光雷达和IMU的旋转矩阵：
其中：
为t0时刻激光雷达转换到IMU的转换矩阵；
为tn时刻激光雷达坐标系转换到t0时刻相机坐标系的转换矩阵；
为tn时刻IMU坐标系转换到tn时刻激光雷达坐标系的转换矩阵；
为tn时刻IMU坐标系转换到t0时刻IMU坐标系的转换矩阵；
设定目标优化函数为：
f(ξLI)＝argmin{||γIMU(z,X)||2+|γPlane(z,X)||2+|γLine(z,X)||2}；
其中：
γIMU(z,X)为IMU测量误差；
γPlane(z,X)为激光雷达平面点到地图的距离残差；
γLine(z,X)为激光雷达直线点到地图的距离残差。
6.根据权利要求5所述的双目相机、IMU和激光雷达联合标定方法，其特征在于：
利用双目相机和IMU的外参标定结果以及激光雷达和IMU的外参标定结果，计算双目相机到激光雷达的初始变换矩阵，包括：
其中：
TI C为IMU到相机坐标系的转换矩阵；
为激光雷达到IMU的转换矩阵，/>为激光雷达到相机坐标系的转换矩阵；
然后，从图像和激光雷达中提取标定板中的三维线和平面特征点，将激光雷达上的特征点转换到相机坐标系上，并利用最近邻原理找到激光雷达和相机点的关联点。
7.根据权利要求6所述的双目相机、IMU和激光雷达联合标定方法，其特征在于：
联合标定双目相机和激光雷达，获得标定板数据，从标定板数据中提取线和面的特征点并进行关联约束计算，包括：
由双目相机捕获的图像数据为(u,v)，由激光雷达捕获的3维点阵云为(x,y,z)，建立转化矩阵M，将(x,y,z)映射到(u,v)：
其中：
矩阵(fu，fv，u0，v0)为双目相机参数；
fu和fv是X和Y轴方向尺度因子，即水平方向和垂直方向的有效焦距；
u0，v0是像平面的中心点，即主点坐标；
R为旋转矩阵；
t为平移矢量；
mij为转化矩阵M的元素，i＝1,2,3,4，j＝1,2,3,4；
8.根据权利要求7所述的双目相机、IMU和激光雷达联合标定方法，其特征在于：
根据双目相机和IMU的外参标定结果、激光雷达和IMU的外参标定结果，进行双目相机、IMU和激光雷达联合优化标定，包括：
载体的运动状态为：
其中：
XICL为载体的状态向量；
x0和xm是载体在某一个时刻的位置和速度向量；
是IMU到双目相机的旋转四元数；
是IMU到双目相机的旋转矩阵；
是IMU到激光雷达的旋转四元数；
是IMU到激光雷达的旋转矩阵；
建立优化目标函数：
f(ξICL)＝argmin{||γIMU(z,X)||2+|γchessboard_L(z,X)||2}；
其中：
误差项γIMU(z,X)为IMU测量残差；
γchessboard_L(z,X)为IMU测量残差；
激光雷达平面点到地图的距离残差和激光雷达直线点到地图的距离残差；
根据优化目标函数，估计双目相机和IMU、激光雷达和IMU、激光雷达和双目相机的外参，优化的残差为：
其中：
为激光雷达直线点在激光雷达坐标系下的点坐标向量；
为激光雷达直线点在相机坐标系下的点坐标向量；
为激光雷达平面点在激光雷达坐标系下的点坐标向量；
为激光雷达平面点在相机坐标系下的点坐标向量。
9.双目相机、IMU和激光雷达联合标定***，其特征在于：
所述***用于实施权利要求2-8任一项所述的方法，包括：
第一外参标定模块，用于获取双目相机的外参、IMU的外参、IMU的噪声，作为状态向量对双目相机和IMU的位姿进行状态估计，并进行双目相机和IMU的外参标定；
第二外参标定模块，用于获取激光雷达的外参、IMU的外参、IMU的噪声，作为状态向量对激光雷达和IMU的位姿进行状态估计，并进行激光雷达和IMU的外参标定；
计算模块，用于利用双目相机和IMU的外参标定结果以及激光雷达和IMU的外参标定结果，计算双目相机到激光雷达的初始变换矩阵；
约束模块，用于联合标定双目相机和激光雷达，获得标定板数据，从标定板数据中提取线和面的特征点并进行关联约束计算；
联合标定模块，用于根据双目相机和IMU的外参标定结果、激光雷达和IMU的外参标定结果，进行双目相机、IMU和激光雷达联合优化标定。
10.根据权利要求9所述的双目相机、IMU和激光雷达联合标定***，其特征在于：
所述***还包括：
标定模块，用于对双目相机、IMU、激光雷达分别进行标定。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410317337.9A CN118115597A (zh) | 2024-03-20 | 2024-03-20 | 双目相机、imu和激光雷达联合标定方法及*** |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410317337.9A CN118115597A (zh) | 2024-03-20 | 2024-03-20 | 双目相机、imu和激光雷达联合标定方法及*** |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN118115597A true CN118115597A (zh) | 2024-05-31 |
Family
ID=91212179
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410317337.9A Pending CN118115597A (zh) | 2024-03-20 | 2024-03-20 | 双目相机、imu和激光雷达联合标定方法及*** |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN118115597A (zh) |
-
2024
- 2024-03-20 CN CN202410317337.9A patent/CN118115597A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP6760114B2 (ja) | 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム | |
| Panahandeh et al. | Vision-aided inertial navigation based on ground plane feature detection | |
| CN102472609B (zh) | 位置和姿势校准方法及设备 | |
| CN110081881B (zh) | 一种基于无人机多传感器信息融合技术的着舰引导方法 | |
| CN109708649B (zh) | 一种遥感卫星的姿态确定方法及*** | |
| US20060256200A1 (en) | Method and system for improving video metadata through the use of frame-to-frame correspondences | |
| US20180075614A1 (en) | Method of Depth Estimation Using a Camera and Inertial Sensor | |
| JP6877293B2 (ja) | 位置情報記録方法および装置 | |
| CN111665512B (zh) | 基于3d激光雷达和惯性测量单元的融合的测距和绘图 | |
| CN112230242A (zh) | 位姿估计***和方法 | |
| CN112050806B (zh) | 一种移动车辆的定位方法及装置 | |
| CN111524194A (zh) | 一种激光雷达和双目视觉相互融合的定位方法及终端 | |
| KR102490521B1 (ko) | 라이다 좌표계와 카메라 좌표계의 벡터 정합을 통한 자동 캘리브레이션 방법 | |
| CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
| CN116184430B (zh) | 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法 | |
| CN115371665A (zh) | 一种基于深度相机和惯性融合的移动机器人定位方法 | |
| CN114964276A (zh) | 一种融合惯导的动态视觉slam方法 | |
| JP6922348B2 (ja) | 情報処理装置、方法、及びプログラム | |
| CN114777768A (zh) | 一种卫星拒止环境高精度定位方法、***及电子设备 | |
| CN113137973A (zh) | 一种图像语义特征点真值确定方法及装置 | |
| CN112419427A (zh) | 用于提高飞行时间相机精度的方法 | |
| WO2002012830A1 (en) | Height measurement apparatus | |
| CN115930948A (zh) | 一种果园机器人融合定位方法 | |
| CN118115597A (zh) | 双目相机、imu和激光雷达联合标定方法及*** | |
| CN114842224A (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 |