一种多传感器时间空间标定方法及装置

技术领域

本发明涉及多传感器标定技术领域，特别涉及一种多传感器时间空间标定方法及装置。

背景技术

目前传感器标定大多采用离线标定的方法，即在探测系统工作前已完成标定工作，在系统工作时完全使用离线标定的结果进行工作。而探测系统要长期工作在远离地球的地外环境中，工作条件恶劣，其携带的传感器可能会由于风沙或者长期颠簸造成位置改变，此时，离线标定的时间空间偏差与系统实际情况有较大误差，大大影响了探测系统对环境的准确性。虽然部分探测系统会携带标定板来校正这些位置的变化，但是这种系统只能用于视觉传感器的空间位置校准，无法用于其他传感器，此外标定板在长期的恶劣环境中可能会被沙尘覆盖导致校准精度不够或者校正失败。

发明内容

本发明实施例的目的是提供一种多传感器时间空间标定方法及装置，以解决现有存在的相机、激光雷达和IMU传感器之间在长时间地外环境运行过程中出现的时间空间偏差问题。

根据本发明实施例的第一方面，提供一种多传感器时间空间标定方法，包括：

通过IMU传感器获取IMU数据，通过相机获取图像数据，通过激光雷达获取点云数据；

对所述图像数据进行语义分割及特征点提取，根据所述语义分割的结果对所述特征点进行匹配，构建重投影误差方程，并将IMU传感器与相机之间的第一时间偏差引入所述重投影误差方程；

引入所述IMU传感器与激光雷达之间的第二时间偏差，对所述点云数据中两帧点云进行位姿修正，对修正后的所述两帧点云进行配准，计算所述两帧点云之间的相对位姿；

获取两帧图像间的IMU数据，通过预积分计算两帧图像的第一位姿，获取两帧点云间的IMU数据，通过预积分计算两帧点云的第二位姿，计算所述第一位姿和第二位姿之间的位姿偏差；

设定滑动窗口，根据所述滑动窗口内所述重投影误差方程、所述相对位姿及所述位姿偏差进行迭代优化求解，实现多传感器时间空间标定。

根据本发明实施例的第二方面，提供一种多传感器时间空间标定装置，包括：

获取模块，用于通过IMU传感器获取IMU数据，通过相机获取图像数据，通过激光雷达获取点云数据；

图像数据处理模块，用于对所述图像数据进行语义分割及特征点提取，根据所述语义分割的结果对所述特征点进行匹配，构建重投影误差方程，并将IMU传感器与相机之间的第一时间偏差引入所述重投影误差方程；

点云数据处理模块，用于引入所述IMU传感器与激光雷达之间的第二时间偏差，对所述点云数据中两帧点云进行位姿修正，对修正后的所述两帧点云进行配准，计算所述两帧点云之间的相对位姿；

IMU数据处理模块，用于获取两帧图像间的IMU数据，通过预积分计算两帧图像的第一位姿，获取两帧点云间的IMU数据，通过预积分计算两帧点云的第二位姿，计算所述第一位姿和第二位姿之间的位姿偏差；

迭代优化模块，用于设定滑动窗口，根据所述滑动窗口内所述重投影误差方程、所述相对位姿及所述位姿偏差进行迭代优化求解，实现多传感器时间空间标定。

根据以上技术方案，本发明具有如下技术效果：

由上述实施例可知，本申请提出的多传感器时间空间标定方法可提升探测系统的鲁棒性。一方面，探测系统易受到未知环境中风暴、颠簸等环境因素的影响，本申请提出的多传感器时间空间标定方法可及时修正各传感器间发生的相对位置变化，校准时间偏差，从而提高多传感器数据融合的准确性，提升探测系统的定位精度。另一方面，由于探测系统在实际工作时难以进行人为的传感器校准和标定，因此难以进行长期稳定工作，本申请提出的多传感器时间空间标定方法可一定程度上提升探测系统的有效工作时间，从而提高探测系统的鲁棒性。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本发明的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是根据一示例性实施例示出的一种多传感器时间空间标定方法的流程图。

图2是根据一示例性实施例示出的图像数据处理的流程图。

图3是根据一示例性实施例示出的点云数据处理的流程图。

图4是根据一示例性实施例示出的IMU数据处理的流程图。

图5是根据一示例性实施例示出的一种多传感器时间空间标定装置的结构示意图。

图6是根据一示例性实施例示出的图像数据处理模块的结构示意图。

图7是根据一示例性实施例示出的点云数据处理模块的结构示意图。

图8是根据一示例性实施例示出的IMU数据处理模块的结构示意图。

具体实施方式

这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本申请相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本申请的一些方面相一致的装置和方法的例子。

在本申请使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请。在本申请和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。

图1是根据一示例性实施例出的一种多传感器时间空间标定方法的流程图，如图1所示，该方法应用于探测系统，可以包括以下步骤：

步骤S101，测量计算IMU传感器、相机和激光雷达三者之间的空间位置关系，将所述空间位置关系定义为探测系统的空间同步初始值，并定义探测系统的时间同步初始值为0，将所述空间同步初始值和所述时间同步初始值作为迭代优化求解的初始设置；

本实施例中，所述探测系统可以是各种移动机器人，也可以是轮式、履带式移动小车，包括各式移动底盘，但不限于此，本实施例以探测系统为装载有IMU传感器、相机和激光雷达的移动机构为例，各传感器的安装位置不限。

在探测系统长时间工作过程中，由于环境、机构等各种因素影响，各传感器的相对 位置不可避免地会发生变化，如果忽视位置的改变，可能会导致环境感知的精度，甚至发生 某些意想不到的危险，因此，需要对探测系统各传感器的相对位置进行实时在线修正。为了 提高在线修正的精度，需要赋予多传感器时间空间标定系统一个较高精度的初始值，因而 选择高精度的测量设备测量IMU传感器和相机的相对位置关系，并将其定义为IMU传感器和 相机空间同步的初始值，记为

由于探测系统不可避免地存在触发延时、传输延时和时钟不同步等问题，各传感 器间必然存在时间偏移，为了提高多传感器数据融合的有效性，必须保证各传感器间的时 间一致性，因此需要对探测系统的各传感器进行时间同步。但是，随着探测系统的运行，状 态变量规模不断增大，因此设置滑动窗口对相关变量进行优化，以减少计算量。定义IMU传 感器、相机和激光雷达的时间戳分别为

步骤S102，通过IMU传感器获取IMU数据，通过相机获取图像数据，通过激光雷达获取点云数据；

本实施例中，所述IMU传感器、相机和激光雷达均与探测系统刚性联结，各传感器的安装位置均没有要求，可有效采集工作空间数据即可。所述工作空间一般指月球、火星等地外无人环境，也包括城市、荒漠等地球上的户外环境，亦包括各类室内场景，本实施例以地外环境为例，因其相对于其他场景，探测系统工作中的多传感器时间空间标定更难通过人为的方式进行。

步骤S103，对所述图像数据进行语义分割及特征点提取，根据语义分割结果对所述特征点进行匹配，构建重投影误差方程，并将IMU传感器与相机之间的第一时间偏差引入所述重投影误差方程；图2是根据一示例性实施例示出的图像数据处理的流程图，该步骤包括以下子步骤：

步骤S1031，对所述图像数据进行语义分割，并提取图像中的特征点，计算所述特 征点在图像坐标系的运动速度，以获取所述特征点位置的真实值；具体地，首先使用语义分 割算法DeepLab V3+对图像进行语义分割，并使用SIFT算法提取图像的特征点。假设

式中，

步骤S1032，对两帧所述图像中具有相同语义分割结果的特征点进行匹配，根据所 述匹配的结果定义所述特征点的观测值，根据所述真实值和观测值构建重投影误差方程； 具体地，根据图像

式中，

步骤S1033，将所述IMU传感器与相机之间的第一时间偏差引入重投影误差方程， 并将其作为相机的约束条件；具体地，在探测系统中一般将IMU的时间戳作为系统的时间 戳，即上述k和k+1两个采样时间与真实的采集时间均相差

将其引入重投影误差函数：

将其进行简写，可得到相机的约束条件为：

式中，

步骤S104，引入所述IMU传感器与激光雷达之间的第二时间偏差，对所述点云数据中两帧点云进行位姿修正，对修正后的所述两帧点云进行配准，计算所述两帧点云之间的相对位姿；图3是根据一示例性实施例示出的点云数据处理的流程图，该步骤包括以下子步骤：

步骤S1041，首先计算所述两帧点云的运动速度，并引入所述IMU传感器与激光雷达之间的第二时间偏差，根据所述运动速度和所述第二时间偏差，计算所述两帧点云的真实位姿，修正所述两帧点云的真实位姿，根据每帧点云的真实位姿获取所述每帧点云中每个点的真实坐标；

步骤S1042，根据所述真实坐标分别计算所述每帧点云中每两个连续点的连线与 水平面的夹角，并设定夹角阈值，根据所述夹角阈值和所述夹角，将所述每帧点云中的点分 为地面点和非地面点；具体地，激光雷达扫描得到的点云数据中含有大部分的地面点，这对 于后续障碍物点云的分类、识别和跟踪任务不利，因此需要将点云数据中的地面点进行滤 波。首先，设定地面点集的夹角判定阈值

步骤S1043，对每个所述非地面点进行主成分分析，将其分类为面点和其他点；具体地，为了降低点云匹配的误差，提高点云配准的准确性，对非地面点集进行进一步处理。根据每个非地面点及其邻近的N个最近点构建点云数据集，保证点云数据集中所有的点不在同一个环形上，并利用主成分分析算法思想，计算其均值和协方差矩阵，从而判断每个点的属性。点云中的每个点都是三维点，对于一个三维点组成的数据集，可以表示为：

其均值可表示为：

由此可得到该点与其附近点形成的点集的协方差矩阵：

根据点集的协方差矩阵，进行SVD奇异值分解：

其中，

步骤S1044，对所述地面点、所述面点和所述其他点分别进行点云配准，计算所述 两帧点云的位姿变换，将其作为激光雷达的约束条件；具体地，点云配准即找到使两点云尽 可能重叠的位姿变换

式中，

对于地面点云和面属性点云，可忽略切向上的位移，而仅关注其在法向上的误差， 因此可设置

按照相机约束的书写方式将其进行简写，可得激光雷达的约束条件：

式中，

步骤S105，获取两帧图像间的IMU数据，通过预积分计算两帧图像的第一位姿，获取两帧点云间的IMU数据，通过预积分计算两帧点云的第二位姿，计算所述第一位姿和第二位姿之间的位姿偏差；图4是根据一示例性实施例示出的IMU数据处理的流程图，该步骤可以包括以下子步骤：

步骤S1051，分析所述图像数据和所述IMU数据，获取两帧图像间所有的IMU数据， 通过预积分计算所述两帧图像间的第一位姿；具体地， 通过系统初始状态和惯性测量数据 可以传递系统下一时刻的状态估计，但是在非线性优化中，系统状态在每次迭代过程中都 会发生变化，需要重复积分，为了解决这一问题，使用预积分的方法处理IMU数据。IMU传感 器以固定频率输出在IMU坐标系下的三轴角速度

式中，

分析图像数据和IMU数据，假定图像采集时刻为k和k+1，已知k时刻IMU传感器的状 态量和两时刻间的IMU数据，则可通过预积分计算k+1时刻的状态量，即探测系统的第一位 姿

式中，IMU传感器在j时刻的位姿

步骤S1052，分析所述点云数据和所述图像数据，标记与所述两帧图像采集时刻相 同的两帧点云，获取所述两帧点云间所有的IMU数据，通过预积分计算两帧点云间的第二位 姿；具体地，分析点云数据和图像数据，找到图像采集时刻k和k+1对应的点云数据，再根据 找出的点云数据找出对应时刻的IMU数据，通过预积分计算此时k+1时刻的状态量，即第二 位姿

式中，IMU传感器在n时刻的位姿

步骤S1053，将所述第一位姿和所述第二位姿之间的位姿偏差作为 IMU的约束条 件；具体地，理论上，如果不存在时间偏差，图像数据和点云数据对应的IMU数据应该完全一 致，即第一位姿与第二位姿相等，

按照相机约束的书写方式将其进行简写，可得IMU传感器的约束条件：

式中，

步骤S106，设定滑动窗口，根据所述滑动窗口内所述重投影误差方程、所述相对位姿、所述位姿偏差进行迭代优化求解，实现多传感器时间空间标定；具体地，在迭代优化过程中，如果使用所有数据进行迭代优化，不仅会增加系统的计算量，甚至引入较大的误差，导致多传感器时间空间标定失败。因此，需要设置一个时间窗口，即滑动窗口，仅处理该段时间内的数据，将该段时间内需要进行优化的状态定义为：

式中，

联立上述相机约束、激光雷达约束和IMU传感器约束，并引入空间同步初始值和时间同步初始值，构建代价函数：

式中，

与前述的一种多传感器时间空间标定方法的实施例相对应，本申请还提供了一种多传感器时间空间标定装置的实施例。图5是根据一示例性实施例示出的一种多传感器时间空间标定装置框图。参照图5，该装置可以包括：

初始化模块21，用于测量计算所述IMU传感器、相机和激光雷达三者之间的空间位置关系，将所述空间位置关系定义为探测系统的空间同步初始值，并定义整个系统的时间同步初始值为0，将所述空间同步初始值和所述时间同步初始值作为所述迭代优化求解的初始设置；

获取模块22，用于通过IMU传感器获取IMU数据，通过相机获取图像数据，通过激光雷达获取点云数据；

图像数据处理模块23，用于对所述图像数据进行语义分割及特征点提取，根据所述语义分割的结果对所述特征点进行匹配，构建重投影误差方程，并将IMU传感器与相机之间的第一时间偏差引入所述重投影误差方程；

点云数据处理模块24，用于引入所述IMU传感器与激光雷达之间的第二时间偏差，对所述点云数据中两帧点云进行位姿修正，对修正后的所述两帧点云进行配准，计算所述两帧点云之间的相对位姿；

IMU数据处理模块25，用于获取两帧图像间的IMU数据，通过预积分计算两帧图像的第一位姿，获取两帧点云间的IMU数据，通过预积分计算两帧点云的第二位姿，计算所述第一位姿和第二位姿之间的位姿偏差；

迭代优化模块26，用于设定滑动窗口，根据所述滑动窗口内所述重投影误差方程、所述相对位姿及所述位姿偏差进行迭代优化求解，实现多传感器时间空间标定。

图6是根据一示例性实施例示出的图像数据处理模块的结构示意图，上述图像数据处理模块23，包括：

特征提取子模块231，用于对所述图像数据进行语义分割，并提取图像中的特征点，计算所述特征点在图像坐标系的运动速度，以获取所述特征点位置的真实值；

特征匹配子模块232，用于对两帧所述图像中具有相同语义分割结果的特征点进行匹配，根据所述匹配的结果定义所述特征点的观测值，根据所述真实值和观测值构建重投影误差方程；

重投影子模块233，用于将所述IMU传感器与相机之间的第一时间偏差引入重投影误差方程，并将其作为相机的约束条件。

图7是根据一示例性实施例示出的点云数据处理模块的结构示意图，上述点云数据处理模块24，包括：

位姿修正子模块241，用于计算所述两帧点云的运动速度，并引入所述IMU传感器与激光雷达之间的第二时间偏差，根据所述运动速度和所述第二时间偏差，计算所述两帧点云的真实位姿，修正所述两帧点云的真实位姿；

坐标获取子模块242，用于根据每帧点云的真实位姿获取所述每帧点云中每个点的真实坐标；

夹角计算子模块243，用于根据所述真实坐标分别计算所述每帧点云中每两个连续点的连线与水平面的夹角；

地面滤波子模块244，用于设定夹角阈值，根据所述夹角阈值和所述夹角，将所述每帧点云中的点分为地面点和非地面点；

面点评定子模块245，用于对每个所述非地面点进行主成分分析，将其分类为面点和其他点；

位姿计算子模块246，用于对所述地面点、所述面点和所述其他点分别进行点云配准，计算所述两帧点云的相对位姿，将其作为激光雷达的约束条件。

图8是根据一示例性实施例示出的IMU数据处理模块的结构示意图，上述IMU数据处理模块25，具体包括：

第一位姿计算子模块251，用于分析所述图像数据和所述IMU数据，获取两帧图像间所有的IMU数据，通过预积分计算所述两帧图像间的第一位姿；

第二位姿计算子模块252，用于分析所述点云数据和所述图像数据，标记与所述两帧图像采集时刻相同的两帧点云，获取所述两帧点云间所有的IMU数据，通过预积分计算两帧点云间的第二位姿；

位姿偏差计算子模块253，用于将所述第一位姿和所述第二位姿之间的位姿偏差作为 IMU的约束条件。

关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。

对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本申请方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

相应的，本申请还提供一种电子设备，包括：一个或多个处理器；存储器，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如上述的一种多传感器时间空间标定方法。

相应的，本申请还提供一种计算机可读存储介质，其上存储有计算机指令，其特征在于，该指令被处理器执行时实现如上述的一种多传感器时间空间标定方法。

本领域技术人员在考虑说明书及实践这里公开的内容后，将容易想到本申请的其它实施方案。本申请旨在涵盖本申请的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本申请的一般性原理并包括本申请未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本申请的真正范围和精神由下面的权利要求指出。

应当理解的是，本申请并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本申请的范围仅由所附的权利要求来限制。