一种基于互相关的多传感器数据时间对齐系统及其方法与流程
本发明涉及传感器数据应用技术领域，更具体地，涉及一种基于互相关的多传感器数据时间对齐系统及其方法。
背景技术：
即时定位与地图构建(simultaneouslocalizationandmapping,slam,下文统称slam)是当前移动机器人和计算机视觉领域的热门研究方向，其研究的根本问题是机器人在未知环境下运动时对环境的观测、自身位姿和运动轨迹的检测，最终完成当前环境地图的构建。对于一个多传感器协同slam系统来说，多传感器联合标定和多传感器数据融合是整个系统数据输入端的两个关键部分，而这二者都依托于多传感器数据融合技术。在实际的slam应用中，以kalibr为代表的多传感器联合标定方法以及大多数的多传感数据融合算法都默认输入的数据帧在时域上是对齐的，只有部分算法会实时优化时间对齐。因此，如果多传感器数据融合方法所输入的不同传感器数据没有进行时间对齐，就会导致传感器组外参标定与实际存在较大偏差、不同传感器的数据帧匹配错误、算法输出精度下降、最终决策非最优等问题。
现有多传感器数据时间对齐技术主流是利用曲线拟合、滤波方法等方法进行数据的时间同步，往往要求对齐的数据中一种或多种数据具有时间戳，通过时间戳进行简单匹配完成时间对齐。中国专利公开号：cn111121755a，公开日期：2020年05月08日，该专利公开了一种多传感器的融合定位方法，包括：获取至少两个定位传感器输出的待定位物体的定位数据；将所述各定位传感器的定位数据进行时间戳同步，确定所述各定位传感器的时间定位数据；根据所述各定位传感器的时间定位数据，构建非线性优化函数，并生成非线性优化函数组；基于所述非线性优化函数组，确定所述待定位物体的目标定位数据。该专利中需要通过时间戳信息进行同步，泛用性不佳，同时输出精度受限于原始数据时间戳信息的准确度。
技术实现要素：
本发明的目的在于克服现有多传感器数据时间对齐的方法中需要使用时间戳进行同步，导致泛用性不足，准确度低的缺点，提供一种基于互相关的多传感器数据时间对齐系统。本发明不需要时间戳信息来作为参考数据，可以完全基于原始数据本身所包含的信息来进行时间对齐，应用场景更广，可适用于各类的数据采集场景，泛用性更强，准确度更高。
本发明还提供一种基于互相关的多传感器数据时间对齐方法。
为解决上述技术问题，本发明采用的技术方案是：一种基于互相关的多传感器数据时间对齐系统，包括数据处理模块、序列输入模块、视觉帧处理模块、点云帧处理模块和序列对齐模块。
数据处理模块：对相机传感器和激光雷达传感器的数据进行格式转换分别输入视觉帧处理模块和点云帧处理模块；
序列输入模块：能够直接采集三轴角度差序列输入序列对齐模块；
视觉帧处理模块：对数据处理模块输入的模块进行计算处理得出图像的三轴角度差序列输入序列对齐模块；
点云帧处理模块：对数据处理模块输入的模块进行计算处理得出点云帧的三轴角度差序列输入序列对齐模块；
序列对齐模块：将输入的各个三轴角度差序列进行对齐处理得出时域对齐的多传感器数据组。
进一步的，所述序列输入模块为能够直接将采集的数据转换为三轴角度差序列的传感器。
进一步的，所述传感器包括惯性测量仪和陀螺仪。惯性测量仪和陀螺仪能够直接获取角速度等量化信息，通过内置控制器的数学运算就可输出三轴角度差序列。
一种基于互相关的多传感器数据时间对齐方法，包括以下步骤：
s1.数据处理模块将相机传感器获取的数据和激光雷达传感器获取的数据分别转换为图像帧和点云帧；
s2.视觉帧处理模块提取图象帧的三轴角度差序列；
s3.点云帧处理模块提取点云帧的三轴角度差序列；
s4.序列对齐模块将图象帧的三轴角度差序列、点云帧的三轴角度差序列以及序列输入模块的三轴角度差序列通过互相关方法进行序列对齐，求解各个序列的时间差，完成时间对齐。
进一步的，所述步骤s1中图像帧的文件格式为jpg或png，点云帧的文件格式为pcd。
进一步的，所述步骤s2包括以下具体步骤：
s21.基于matlab的undistortiamge去畸变方法，纠正图像帧的畸变；
s22.利用harris角点提取办法，使用滑动窗口检测图像帧相邻区域的灰度变化，找到超过设定阈值的区块并提取出图像帧中的harris角点作为相机位姿变化估计参考的特征点；
s23.利用lk光流法在所有相邻帧中追踪步骤s22中找到的特征点，并估算出相机运动的位姿，得到相机传感器数据记录的整个运动过程中的所有旋转矩阵r；
s24.利用epnp方法基于步骤s23中得到的所有旋转矩阵r的集合求解出三轴角度差序列。
进一步的，所述步骤s3中包括以下具体步骤：
s31.对于所有的点云帧进行网格化形成点云；
s32.针对两个相邻的点云帧，计算每个网格中所有点的正态分布参数均值q和协方矩阵e；
s33.初始化变化参数p，基于步骤s32中正态分布参数均值q和协方矩阵e算出点云帧间转换点的概率密度，得出ndt配准的得分score；
s34.通过优化方法找到score的最大值，得到点云帧间的最优变换t和旋转矩阵r，求解出两帧之间的三轴角度差；
s35.对所有相邻的点云帧执行步骤s32-s34，得出三轴角度差序列。
进一步的，所述步骤s34中的优化方法为牛顿优化法。
进一步的，所步骤s4中包括以下具体步骤：
s41.根据加权算法及传感器类别选择三轴角度差的权值weight，定义三轴上的互相关值加权和数组cof_on_3_axis；
s42.对各组三轴角度差序列，按照轴为区分，依次计算其加权互相关值并找到当前互相关值，插入互相关值数组cof_on_3_axis中；
s43.遍历所有序列的所有元素，得到完整的互相关值数组cof_on_3_axis；
s44.遍历cof_on_3_axis数组，找到最大值max_cofs及其下标step，max_cofs即为三轴加权后最大互相关值，step为求出的时间偏移量；
s45.通过求解出的时间偏移量，对各传感器原始数据进行简单删减，得到时域对齐的多传感器数据组。
进一步的，所述步骤s43中遍历结束的标志是所有序列的所有元素都进行了计算。
与现有技术相比，本发明的有益效果是：
本发明不需要对多个传感器增加同步开始工作的触发功能，节约了数据采集过程的人力成本以及采集设备、触发功能开发的高昂成本；本发明通过对三轴角度差序列处理得到时间对齐数据，不要求某一类特定的传感器数据作为基准数据，对任意的多种传感器或是多个同类传感器(如三个相机)数据皆可完成时间对齐，不需要任一组传感器数据具有精确的时间戳信息，也不需要每组数据都带有自身标记或通过人工标注的时间戳信息，应用场景更广，可适用于各类的数据采集场景，泛用性更强，精确度更高。
附图说明
图1为本发明一种基于互相关的多传感器数据时间对齐系统的整体结构示意图。
图2为本发明一种基于互相关的多传感器数据时间对齐方法的流程图。
具体实施方式
下面结合具体实施方式对本发明作进一步的说明。其中，附图仅用于示例性说明，表示的仅是示意图，而非实物图，不能理解为对本专利的限制；为了更好地说明本发明的实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。
本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。
实施例1
如图1所示为本发明一种基于互相关的多传感器数据时间对齐系统的实施例。一种基于互相关的多传感器数据时间对齐系统，包括数据处理模块、视觉帧处理模块、点云帧处理模块和序列对齐模块，还包括序列输入模块，数据处理模块分别连接视觉帧处理模块和点云帧处理模块，序列输入模块、视觉帧处理模块和点云帧处理模块均与序列对齐模块连接。
本实施例中数据处理模块是对相机传感器和激光雷达传感器的数据进行格式转换，将数据处理模块处理后的数据分别输入视觉帧处理模块、点云帧处理模块，得出图像的三轴角度差序列和点云帧的三轴角度差序列，序列输入模块是能够直接输出三轴角度差序列的传感器，序列对齐模块对各个三轴角度差序列进行对齐处理得出时域对齐的多传感器数据组；传感器包括惯性测量仪和陀螺仪。惯性测量仪和陀螺仪能够直接获取角速度等量化信息，通过内置控制器的数学运算就可输出三轴角度差序列。
实施例2
如图2所示为本发明一种基于互相关的多传感器数据时间对齐方法的实施例。一种基于互相关的多传感器数据时间对齐方法，包括以下步骤：
s1.数据处理模块将相机传感器获取的数据和激光雷达传感器获取的数据分别转换为图像帧和点云帧；
s2.视觉帧处理模块提取图象帧的三轴角度差序列；
s3.点云帧处理模块提取点云帧的三轴角度差序列；
s4.序列对齐模块将图象帧的三轴角度差序列、点云帧的三轴角度差序列以及序列输入模块的三轴角度差序列通过互相关方法进行序列对齐，求解各个序列的时间差，完成时间对齐。
本实施例中，步骤s1中，数据预处理模块对相机传感器中bin的文件转换为jpg或png格式的图像帧文件，将激光雷达传感器中pacp文件转换为pcd格式的点云帧文件。
本实施例中，步骤s2包括以下具体提取步骤：
s21.基于matlab的undistortiamge去畸变方法，给定相机参数以纠正图像帧的畸变；
s22.harris角点提取办法，使用滑动窗口检测图像帧相邻区域的灰度变化，找到超过设定阈值的区块并提取出图像帧中的harris角点作为相机位姿变化估计参考的特征点；方法的具体提取步骤如下所示：
a.对于一张二维的图像i，计算其在x轴和y轴两个方向上的梯度。
b.计算图像在两个方向上梯度的乘积
ix2＝ix·ix,iy2＝iy·iy,ix,y＝ix·iy(2)
c.使用高斯函数对梯度乘积进行高斯加权，计算出自相关函数中矩阵m行列式中的a、b和c
d.对图中所有的像素都进行harris角点响应值的计算。设置阈值并将所有小于的响应值置为0
e.在一定范围的邻域中进行非最大值抑制，得到的局部最大值即所求的角点。一般来说，领域的范围设置成3x3或者是5x5。对图像所有区域进行处理，最终得到所有特征点的位置。
s23.使用lk光流法在所有相邻帧中追踪由上述harris角点提取方法找到的特征点，并估算出相机运动的位姿，得到传感器数据记录的整个运动过程中的所有旋转矩阵r。具体方法如下所示：
a.对于图像中的特征点v，设其在t时刻的时候的坐标为(x,y)，而t+dt时刻的坐标为(x+dx,y+dy)，基于灰度不变假设，我们可以得到两者的关系：
i(x+dx,y+dy,t+dt)＝i(x,y,t)(5)
b.对式5的左式进行泰勒展开并保留一阶项，可以得到：
c.基于灰度不变假设，联合式5和式6，可以得到：
此处的
d.基于窗口内像素运动相同假设，则一个大小为a×a的窗口内的所有像素都满足式9的等式，因此我们一共有a2个等式，即得到一个关于u和v的超定方程。此时，我们应用最小二乘解的思想，即可解出u和v，即求解出了特征点的运动。
e.按照上述方法使用左相机图像对右相机图像进行特征点追踪，即可得到右相机特征点的位置信息，继而求出左右相机之间的视差d。联合已知的相机基线长度b、相机焦距fx，即可求出特征点的深度depth。记左相机中特征点的二维坐标为(xl,yl)，右相机中特征点的二维坐标为(xr,yr)。有：
d＝xl-xr(10)
f.由于光流法存在特征点的误跟踪情况，因此需要去除误跟踪的特征点。由外参标定可以得到左右相机的相对旋转r以及相对位移t，基于r和t对一对匹配的特征点构建对极约束，将对极约束误差e大于设定阈值e_max的特征点对认为是误跟踪，将其作为外点去除。设对应的特征点归一化坐标为p0，p1对极约束误差求解如下：
e＝p1tt×rp0(12)
s24.使用epnp方法基于上述方法中得到的所有旋转矩阵r的集合求解出三轴角度差序列，具体求解公式如下：
设旋转矩阵
θx＝atan2(r32,r33)
θz＝atan2(r21,r11)
本实施例中，步骤s3包括以下具体步骤：
s31.将采集到的三维点云网格化，即对整个三维空间划分为多个立方体，每个立方体中都包含不同形态的点云；
s32.针对两个相邻的点云帧，计算每个网格中所有点的正态分布参数均值q和协方矩阵e；
s33.初始化变换参数p，格式为
s34.基于q、e算出转换点的概率密度，每个网格概率密度相加即ndt配准的得分；
s35.通过如牛顿优化法找到score的最大值，即得到点云帧间的最优变换t；
s36.对所有相邻的点云帧执行步骤s32-s34，得出三轴角度差序列。
本实施例中，所述步骤s4中包括以下具体步骤：
s41.根据加权算法及传感器类别选择三轴角度差的权值weight，定义三轴上的互相关值加权和数组cof_on_3_axis；
s42.对各组三轴角度差序列，按照轴为区分，依次计算其加权互相关值并找到当前互相关值，插入互相关值数组cof_on_3_axis中；
s43.遍历所有序列的所有元素、得到完整的互相关值数组cof_on_3_axis；
s44.遍历cof_on_3_axis数组，找到最大值max_cofs及其下标step，max_cofs即为三轴加权后最大互相关值,step为求出的时间偏移量；
s45.通过求解出的时间偏移量，对各传感器原始数据进行简单删减，得到时域对齐的多传感器数据组。
再将序列对齐模型求出的时域对齐的多传感器数据组应用到多传感器联合标定等相关应用领域中。
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。
- 还没有人留言评论。精彩留言会获得点赞！