CN109544638A - 一种用于多传感器融合的异步在线标定方法 - Google Patents

Abstract

一种用于多传感器融合的异步在线标定方法，通过标定三个传感器的坐标系之间的相互转换关系，将多个传感器的测量信息统一到相同的坐标系下，传统的标定方法局限于两个传感器之间的标定，并且要求严格的时间同步，标定过程较为复杂；本发明分别计算激光雷达、相机、惯性测量单元(IMU)的运动，然后通过使用线性插值获得相同时间间隔内不同传感器的运动，最后通过对齐三个传感器的旋转序列得到两两之间的外部旋转。本发明能够得到较为精确的激光雷达、相机、惯性测量单元(IMU)之间的外部朝向，为后续的标定提供初值。

Description

一种用于多传感器融合的异步在线标定方法

技术领域

本发明涉及机器人视觉、多传感器融合等技术领域，具体涉及多传感器融合的异步在线标定方法。

背景技术

相机、惯性测量单元(IMU)、激光雷达的融合被广泛的应用到机器人定位和建图领域，以达到比单传感器更加精确和鲁棒的效果。单个传感器存在局限性，比如相机在弱纹理、光照变化的情况下容易发生跟踪丢失的情况，惯性测量单元容易累计误差和偏移，激光雷达存在点云的畸变，而结合这三种传感器能够克服单传感器的缺点。

可信赖的多传感器融合的定位和建图算法依赖于精确的外参标定。通过标定三个传感器的坐标系之间的相互转换关系，可以将多个传感器的测量信息统一到相同的坐标系下。传统的标定算法局限于两个传感器之间的标定，并且要求严格的时间同步，标定过程较为复杂。

发明内容

为了克服现有标定方法的局限于两个传感器之间的标定、要求严格的时间同步、过程较为复杂的不足，本发明提出了一种用于多传感器融合的异步在线标定方法，不需要人工标记物和严格的时间同步，能够在线标定相机、惯性测量单元、激光雷达之间的外部朝向。

为了解决上述技术问题，本发明的技术方案如下：

一种用于多传感器融合的异步在线标定方法，包括以下步骤：

1)计算相机的旋转

设点P为相机坐标系下的一个点，其坐标为[x y z]^T，设点p₁,p₂分别是点P在图F_i,F_j中的投影，C_i,C_j分别是在时刻i和时刻j的相机坐标系，使用针孔相机模型，得到尺度无关的表达式：

p₁＝KP

p₂＝K(RP+t) (1.1)

其中K是相机的内参矩阵，R和t是从C_i到C_j的旋转矩阵和平移向量，进行转换，得到对极约束：

其中表示向量t的斜对称矩阵，t＝[a b c]^T,

定义本质矩阵则式(1.2)写作：

在已知内参矩阵的情况下，给定一系列匹配的特征点，就估计出本质矩阵E，对E进行分解，得到旋转矩阵R和平移向量t；

2)计算惯性测量单元的旋转

IMU具有较高的采样频率，通过预积分集成一定时间段内的IMU测量，以避免高频率的状态估计和大量的位姿状态，惯性积分公式写成下面的形式：

其中和分别是以t时刻IMU坐标系为参照坐标系的线性加速度和角速度，是向量的斜对称矩阵(公式1.3)，g^w是世界坐标系下的重力加速度，分别为IMU在时刻i在世界坐标系下的位置、速度和朝向；

IMU从时刻i到时刻j在i时刻IMU坐标系下的相对变化表示为：

3)计算激光雷达的旋转

定义雷达完成一次扫描覆盖为sweep，P_m表示在sweep m期间接受的点云集，假设一次sweep期间的角速度和线性加速度为常量，通过插值的方法获得不同时刻的变换，设t为当前时间，t_m为sweep m的开始时间，为雷达在[t_m,t]期间的位姿变换，给定点p_i,p_i∈P_m,t_i是点p_i的时间戳，是雷达在[t_m,t_i]期间的位姿变换，通过线性插值估计：

定义Γ_m为从P_m中提取的边缘点和平面的集合，将边缘点和平面点投影到sweep m开始的时候，即t_m，得到点集构造如下的几何关系：

其中是点p_i的坐标，是点p_i投影到t_m时刻的雷达坐标系下的坐标，在每次sweep结束的时刻，在该sweep期间接收到的点云都会被投影到该sweep结束的时刻，比如在sweep m-1期间接受到的点云P_m-1，被投影到t_m时刻的雷达坐标系下，得到点集点集是其中的边缘点和平面点的集合，在集合和中寻找匹配点，得到匹配点之间距离的计算公式：

对于Γ_m中的所有点，构造如下的非线性方程：

通过求解最小二乘解，得到的最优解；

4)异步测量的对齐

由于传感器是异步的，上面三个步骤得到相对旋转不一定位于相同的时间段，因此，基于短时间内匀速运动的假设，求得相同时间段内的不同传感器的旋转；

选择相机作为基准传感器，t_i和t_j是两帧连续图片的采集时间，在这段时间内，惯性测量单元有多次采样，t′_i是最接近且晚于或等于t_i的IMU采样时间，t′_j是最接近且晚于或等于t_j的IMU采样时间；首先用步骤2)中的方法对[t′_i，t′_j]间的IMU数据进行预积分，得到这段时间的IMU旋转雷达在期间的旋转由步骤3)中的方法计算得到，是最接近时间[t_i,t_j]的雷达sweep时间，定义分数γ表示相对旋转需要被扩展或者缩小的程度：

其中t₀和t₁是根据传感器测量直接计算得到的相对旋转的起始时间和结束时间，t′₀和t′₁是期望得到的相对旋转的起始时间和结束时间；

定义相对旋转的插值如下：

其中Exp(·)是李代数和李群之间的指数映射，Log(·)是李群和李代数之间的对数映射；5)计算传感器之间的外参

对齐相同时间的不同传感器的旋转序列可以估计传感器之间的外部朝向，通过步骤1),2)，3),4)得到[t_i,t_j]期间的相机旋转IMU旋转激光雷达的旋转对于任何i和j，i<j，下面的式子成立：

其中表示从IMU坐标系到相机坐标系的旋转，表示从相机坐标系到雷达坐标系的旋转，表示从雷达到IMU坐标系的旋转；

该线性系统的每一行都有相同的形式：

为了恢复旋转矩阵使用单位四元数代替上式中的矩阵：

符号表示四元数的乘法，定义单位四元数为：

其中

将不断到达的旋转桟入线性系统，得到超定线性方程组：

其中N是旋转序列的大小，α是异常值处理的权重；随着新的测量的增加，这个线性系统会进行重构，因此使用之前计算的外部朝向来估计误差，误差被定义为：

其中‖·‖是向量的模，权重定义为：

其中s是放大残差影响的因子。对于三个传感器之间的标定，得到下式：

通过奇异值分解求解上面的超定线性方程，最小奇异值对应的奇异向量就是最小二乘解；

由于在标定的初始阶段可能没有足够的运动，估计的值在标定的早期可能是不准确的；随着新的数据不断到来，标定的外参朝向会逐渐收敛到固定值，当局部时间段τ内的外部旋转的对应的欧拉角的方差分别小于阈值时，认为标定成功，表达式如下：

其中分别是四元数对应的偏航、俯仰和滚转角在时间段τ内的方差，η是根据经验设置的阈值，

本发明的有益效果为：不需要人工标记物和严格的时间同步，能够在线标定相机、惯性测量单元、激光雷达之间的外部朝向。

附图说明

图1是相机和IMU不同的采样频率和采样时间，其中蓝色的竖线表示相机的采样，绿色的竖线表示IMU的采样。

图2给定时间[t₀,t₁]内的相对旋转，可以通过插值的方法求得时间[t′_o，t′₁]内的相对旋转。

图3表示同一传感器在时刻i与时刻j之间的相对旋转与不同传感器之间的外部朝向之间的关系。

图4是多传感器融合的异步在线标定方法的流程图。

具体实施方式

下面结合附图对本发明作进一步描述。

参照图1～图4，一种用于多传感器融合的异步在线标定方法，包括以下步骤：

一种用于多传感器融合的异步在线标定方法，包括以下步骤：

1)计算相机的旋转

设点P为相机坐标系下的一个点，其坐标为[x y z]^T，设点p₁,p₂分别是点P在图F_i,F_j中的投影，C_i,C_j分别是在时刻i和时刻j的相机坐标系，使用针孔相机模型，得到尺度无关的表达式：

p₁＝KP

p₂＝K(RP+t) (1.1)

其中K是相机的内参矩阵，R和t是从C_i到C_j的旋转矩阵和平移向量，进行转换，得到对极约束：

其中表示向量t的斜对称矩阵，t＝[a b c]^T,

定义本质矩阵则式(1.2)写作：

在已知内参矩阵的情况下，给定一系列匹配的特征点，就估计出本质矩阵E，对E进行分解，得到旋转矩阵R和平移向量t；

2)计算惯性测量单元的旋转

IMU具有较高的采样频率，通过预积分集成一定时间段内的IMU测量，以避免高频率的状态估计和大量的位姿状态，惯性积分公式写成下面的形式：

其中和分别是以t时刻IMU坐标系为参照坐标系的线性加速度和角速度，是向量的斜对称矩阵(公式1.3)，g^w是世界坐标系下的重力加速度，分别为IMU在时刻i在世界坐标系下的位置、速度和朝向；

IMU从时刻i到时刻j在i时刻IMU坐标系下的相对变化表示为：

3)计算激光雷达的旋转

定义雷达完成一次扫描覆盖为sweep，P_m表示在sweep m期间接受的点云集，假设一次sweep期间的角速度和线性加速度为常量，通过插值的方法获得不同时刻的变换，设t为当前时间，t_m为sweep m的开始时间，为雷达在[t_m,t]期间的位姿变换，给定点p_i,p_i∈P_m,t_i是点p_i的时间戳，是雷达在[t_m,t_i]期间的位姿变换，通过线性插值估计：

定义Γ_m为从P_m中提取的边缘点和平面的集合，将边缘点和平面点投影到sweep m开始的时候，即t_m，得到点集构造如下的几何关系：

其中是点p_i的坐标，p_i∈Γ_m，是点p_i投影到t_m时刻的雷达坐标系下的坐标，在每次sweep结束的时刻，在该sweep期间接收到的点云都会被投影到该sweep结束的时刻，比如在sweep m-1期间接受到的点云P_m-1，被投影到t_m时刻的雷达坐标系下，得到点集点集是其中的边缘点和平面点的集合，在集合和中寻找匹配点，得到匹配点之间距离的计算公式：

对于Γ_m中的所有点，构造如下的非线性方程：

通过求解最小二乘解，得到的最优解；

4)异步测量的对齐

由于传感器是异步的，上面三个步骤得到相对旋转不一定位于相同的时间段，因此，

基于短时间内匀速运动的假设，求得相同时间段内的不同传感器的旋转；

选择相机作为基准传感器，t_i和t_j是两帧连续图片的采集时间，在这段时间内，惯性测量单元有多次采样，t′_i是最接近且晚于或等于t_i的IMU采样时间，t′_j是最接近且晚于或等于t_j的IMU采样时间；首先用步骤2)中的方法对[t′_i，t′_j]间的IMU数据进行预积分，得到这段时间的IMU旋转雷达在期间的旋转由步骤3)中的方法计算得到，是最接近时间[t_i,t_j]的雷达sweep时间，定义分数γ表示相对旋转需要被扩展或者缩小的程度：

其中t₀和t₁是根据传感器测量直接计算得到的相对旋转的起始时间和结束时间，t′₀和t′₁是期望得到的相对旋转的起始时间和结束时间；

定义相对旋转的插值如下：

其中Exp(·)是李代数和李群之间的指数映射，Log(·)是李群和李代数之间的对数映射；

5)计算传感器之间的外参

对齐相同时间的不同传感器的旋转序列可以估计传感器之间的外部朝向，通过步骤1),2)，3),4)得到[t_i,t_j]期间的相机旋转IMU旋转激光雷达的旋转对于任何i和j，i<j，下面的式子成立：

其中表示从IMU坐标系到相机坐标系的旋转，表示从相机坐标系到雷达坐标系的旋转，表示从雷达到IMU坐标系的旋转；

该线性系统的每一行都有相同的形式：

为了恢复旋转矩阵使用单位四元数代替上式中的矩阵：

符号表示四元数的乘法，定义单位四元数为：

其中

将不断到达的旋转桟入线性系统，得到超定线性方程组：

其中N是旋转序列的大小，α是异常值处理的权重；随着新的测量的增加，这个线性系统会进行重构，因此使用之前计算的外部朝向来估计误差，误差被定义为：

其中‖·‖是向量的模，权重定义为：

其中s是放大残差影响的因子。对于三个传感器之间的标定，得到下式：

通过奇异值分解求解上面的超定线性方程，最小奇异值对应的奇异向量就是最小二乘解；

由于在标定的初始阶段可能没有足够的运动，估计的值在标定的早期可能是不准确的；随着新的数据不断到来，标定的外参朝向会逐渐收敛到固定值，当局部时间段τ内的外部旋转的对应的欧拉角的方差分别小于阈值时，认为标定成功，表达式如下：

其中分别是四元数对应的偏航、俯仰和滚转角在时间段τ内的方差，η是根据经验设置的阈值，

Claims (1)

1.一种用于多传感器融合的异步在线标定方法，其特征在于，所述方法包括以下步骤：

1)计算相机的旋转

设点P为相机坐标系下的一个点，其坐标为[x y z]^T，设点p₁,p₂分别是点P在图F_i,F_j中的投影，C_i,C_j分别是在时刻i和时刻j的相机坐标系，使用针孔相机模型，得到尺度无关的表达式：

p₁＝KP

p₂＝K(RP+t) (1.1)

其中K是相机的内参矩阵，R和t是从C_i到C_j的旋转矩阵和平移向量，进行转换，得到对极约束：

其中表示向量t的斜对称矩阵，t＝[a b c]^T,

定义本质矩阵则式(1.2)写作：

在已知内参矩阵的情况下，给定一系列匹配的特征点，就估计出本质矩阵E，对E进行分解，得到旋转矩阵R和平移向量t；

2)计算惯性测量单元的旋转

IMU具有较高的采样频率，通过预积分集成一定时间段内的IMU测量，以避免高频率的状态估计和大量的位姿状态，惯性积分公式写成下面的形式：

其中和分别是以t时刻IMU坐标系为参照坐标系的线性加速度和角速度，是向量的斜对称矩阵，g^w是世界坐标系下的重力加速度，分别为IMU在时刻i在世界坐标系下的位置、速度和朝向；

IMU从时刻i到时刻j在i时刻IMU坐标系下的相对变化表示为：

3)计算激光雷达的旋转

定义雷达完成一次扫描覆盖为sweep，P_m表示在sweep m期间接受的点云集，假设一次sweep期间的角速度和线性加速度为常量，通过插值的方法获得不同时刻的变换，设t为当前时间，t_m为sweep m的开始时间，为雷达在[t_m,t]期间的位姿变换，给定点p_i,p_i∈P_m,t_i是点p_i的时间戳，是雷达在[t_m,t_i]期间的位姿变换，通过线性插值估计：

定义Γ_m为从P_m中提取的边缘点和平面的集合，将边缘点和平面点投影到sweep m开始的时候，即t_m，得到点集构造如下的几何关系：

其中是点p_i的坐标，p_i∈Γ_m，是点p_i投影到t_m时刻的雷达坐标系下的坐标，在每次sweep结束的时刻，在该sweep期间接收到的点云都会被投影到该sweep结束的时刻，比如在sweep m-1期间接受到的点云P_m-1，被投影到t_m时刻的雷达坐标系下，得到点集点集是其中的边缘点和平面点的集合，在集合和中寻找匹配点，得到匹配点之间距离的计算公式：

对于Γ_m中的所有点，构造如下的非线性方程：

通过求解最小二乘解，得到的最优解；

4)异步测量的对齐

基于短时间内匀速运动的假设，求得相同时间段内的不同传感器的旋转；

选择相机作为基准传感器，t_i和t_j是两帧连续图片的采集时间，在这段时间内，惯性测量单元有多次采样，t′_i是最接近且晚于或等于t_i的IMU采样时间，t′_j是最接近且晚于或等于t_j的IMU采样时间；首先用步骤2)中的方法对[t′_i，t′_j]间的IMU数据进行预积分，得到这段时间的IMU旋转雷达在期间的旋转由步骤3)中的方法计算得到，是最接近时间[t_i,t_j]的雷达sweep时间，定义分数γ表示相对旋转需要被扩展或者缩小的程度：

其中t₀和t₁是根据传感器测量直接计算得到的相对旋转的起始时间和结束时间，t′₀和t′₁是期望得到的相对旋转的起始时间和结束时间；

定义相对旋转的插值如下：

其中Exp(·)是李代数和李群之间的指数映射，Log(·)是李群和李代数之间的对数映射；5)计算传感器之间的外参

对齐相同时间的不同传感器的旋转序列可以估计传感器之间的外部朝向，通过步骤1),2)，3),4)得到[t_i,t_j]期间的相机旋转IMU旋转激光雷达的旋转对于任何i和j，i<j，下面的式子成立：

其表示从IMU坐标系到相机坐标系的旋转，表示从相机坐标系到雷达坐标系的旋转，表示从雷达到IMU坐标系的旋转；

该线性系统的每一行都有相同的形式：

为了恢复旋转矩阵使用单位四元数代替上式中的矩阵：

符号表示四元数的乘法，定义单位四元数为：

其中

将不断到达的旋转桟入线性系统，得到超定线性方程组：

其中N是旋转序列的大小，α是异常值处理的权重；随着新的测量的增加，这个线性系统会进行重构，因此使用之前计算的外部朝向来估计误差，误差被定义为：

其中‖·‖是向量的模，权重定义为：

其中s是放大残差影响的因子。对于三个传感器之间的标定，得到下式：

通过奇异值分解求解上面的超定线性方程，最小奇异值对应的奇异向量就是最小二乘解；

随着新的数据不断到来，标定的外参朝向会逐渐收敛到固定值，当局部时间段τ内的外部旋转的对应的欧拉角的方差分别小于阈值时，认为标定成功，表达式如下：

其中分别是四元数对应的偏航、俯仰和滚转角在时间段τ内的方差，η是根据经验设置的阈值，