多传感器的时钟同步方法、装置及计算设备
技术领域
本发明实施例涉及传感器标定技术领域,具体涉及一种多传感器的时钟同步方法、装置及计算设备。
背景技术
在无人驾驶领域和机器人导航领域中,环境感知技术是最为关键的技术之一。环境感知通过各种感知传感器感知环境信息,但各种传感器具有其有缺点,例如,惯性测量仪可以高频快速的测量自身运动,但是随时间增长误差会迅速累积;激光雷达虽无法测量自身运动,但可以提供稳定的时间信息。因此,多个传感器融合可以取长补短,提高***的感知能力。
然而,在多个传感器融合时,各个传感器之间会存在时间偏差,从而对感知结果造成较大影响。
发明内容
本发明实施例一个目的旨在提供一种多传感器的时钟同步方法、装置及计算设备,能够计算出多传感器之间的时间偏差,从而提高感知结果的准确性。
根据本发明实施例的一个方面,提供了一种多传感器的时钟同步方法,所述方法包括:获取第一传感器在第一传感器坐标系下的第一角速度;获取第二传感器在第二传感器坐标系下的第二角速度以及所述第一传感器坐标系与所述第二传感器坐标系的变换关系;根据所述第二角速度以及所述第一传感器坐标系与所述第二传感器坐标系的变换关系,计算所述第二传感器在所述第一传感器坐标系下的第三角速度;根据所述第一角速度和所述第三角速度,计算在各个预设偏差时间下所述第一传感器和所述第二传感器的角速度残差和;将所述角速度残差和中的最小角速度残差和对应的预设偏差时间确定为所述第一传感器和所述第二传感器的时间偏差;根据所述第一传感器和所述第二传感器的时间偏差,对所述第一传感器和所述第二传感器进行时钟同步。
在一种可选的方式中,所述获取第一传感器在第一传感器坐标系下的第一角速度,具体包括:获取第一时刻所述第一传感器在所述第一传感器坐标系下的第一点云;获取第二时刻所述第一传感器在所述第一传感器坐标系下的第二点云;基于迭代最近点算法计算得到所述第一点云和所述第二点云之间的旋转矩阵;根据所述第一点云和所述第二点云之间的旋转矩阵、所述第一时刻、所述第二时刻,计算所述第一角速度。
在一种可选的方式中,所述根据所述第一点云和所述第二点云之间的旋转矩阵、所述第一时刻、所述第二时刻,计算所述第一角速度,具体包括:将所述第一点云和所述第二点云之间的旋转矩阵转化为欧拉角;将所述欧拉角除以所述第一时刻与所述第二时刻的差,得到所述第一传感器在所述第一时刻和所述第二时刻的中间时刻的角速度;将所述第一传感器在所述第一时刻和所述第二时刻的中间时刻的角速度确定为所述第一角速度。
在一种可选的方式中,所述第一传感器坐标系与所述第二传感器坐标系的变换关系包括所述第一传感器坐标系与所述第二传感器坐标系的旋转矩阵;所述根据所述第二角速度以及所述第一传感器坐标系与所述第二传感器坐标系的变换关系,计算所述第二传感器在所述第一传感器坐标系下的第三角速度,具体包括:将所述第二角速度与所述旋转矩阵相乘,计算得到所述第三角速度。
在一种可选的方式中,所述根据所述第一角速度和所述第三角速度,计算在各个预设偏差时间下所述第一传感器和所述第二传感器的角速度残差和,具体包括:确定第一参考时刻以及所述第一参考时刻对应的所述第一角速度;将所述第一参考时刻加上所述预设偏差时间,得到第二参考时刻;确定所述第二参考时刻对应的所述第三角速度;根据所述第一参考时刻对应的所述第一角速度与所述第二参考时刻对应的所述第三角速度,确定角速度对;计算全部所述角速度对的差值的向量模长的和,作为所述预设偏差时间下所述第一传感器和所述第二传感器的角速度残差和。
在一种可选的方式中,所述确定所述第二参考时刻对应的第三角速度,具体包括:若查找不存在所述第二参考时刻对应的所述第三角速度,则获取所述第二参考时刻的上一时间戳对应的所述第三角速度、下一时间戳对应的所述第三角速度;根据上一时间戳对应的所述第三角速度、下一时间戳对应的所述第三角速度,计算所述第二传感器在所述第二参考时刻的第四角速度;将所述第四角速度确定为所述第二参考时刻对应的所述第三角速度。
在一种可选的方式中,所述第一传感器包括激光雷达、毫米波雷达、微波雷达、图像传感器中的至少一种,所述第二传感器包括惯性测量仪。
根据本发明实施例的另一方面,提供了一种多传感器的时钟同步装置,所述装置包括:第一获取模块,用于获取第一传感器在第一传感器坐标系下的第一角速度;第二获取模块,用于获取第二传感器在第二传感器坐标系下的第二角速度以及所述第一传感器坐标系与所述第二传感器坐标系的变换关系;角速度转换模块,用于根据所述第二角速度以及所述第一传感器坐标系与所述第二传感器坐标系的变换关系,计算所述第二传感器在所述第一传感器坐标系下的第三角速度;角速度残差和计算模块,用于根据所述第一角速度和所述第三角速度,计算在各个预设偏差时间下所述第一传感器和所述第二传感器的角速度残差和;时间偏差确定模块,用于将所述角速度残差和中的最小角速度残差和对应的预设偏差时间确定为所述第一传感器和所述第二传感器的时间偏差;时钟同步模块,用于根据所述第一传感器和所述第二传感器的时间偏差,对所述第一传感器和所述第二传感器进行时钟同步。
根据本发明实施例的又一方面,提供了一种计算设备,包括:处理器和存储器,当所述计算设备运行时,所述处理器执行所述可执行指令,使所述处理器执行如上所述的多传感器的时钟同步方法的操作。
根据本发明实施例的另又一方面,提供了一种计算机可读存储介质,所述存储介质中存储有至少一可执行指令,所述可执行指令使处理器执行根据如上所述的多传感器的时钟同步方法的步骤。
本发明实施例通过获取第一传感器在第一传感器坐标系下的第一角速度,获取第二传感器在第二传感器坐标系下的第二角速度以及第一传感器坐标系与第二传感器坐标系的变换关系,根据第二角速度以及第一传感器坐标系与第二传感器坐标系的变换关系,计算第二传感器在第一传感器坐标系下的第三角速度,根据第一角速度和第三角速度,计算在各个预设偏差时间下第一传感器和第二传感器的角速度残差和,将角速度残差和中的最小角速度残差和对应的预设偏差时间确定为第一传感器和第二传感器的时间偏差,根据第一传感器和第二传感器的时间偏差,对第一传感器和第二传感器进行时钟同步,能够准确地计算出第一传感器和第二传感器的时间偏差,从而计算出多个传感器之间的时间偏差,解决多传感器的时间同步问题,从而提高感知结果的准确性。
上述说明仅是本发明实施例技术方案的概述,为了能够更清楚了解本发明实施例的技术手段,而可依照说明书的内容予以实施,并且为了让本发明实施例的上述和其它目的、特征和优点能够更明显易懂,以下特举本发明的具体实施方式。
附图说明
通过阅读下文优选实施方式的详细描述,各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的,而并不认为是对本发明的限制。而且在整个附图中,用相同的参考符号表示相同的部件。在附图中:
图1示出了本发明实施例提供的一种多传感器的时钟同步方法的流程示意图;
图2示出了图1的步骤110的流程示意图;
图3示出了图1的步骤140的流程示意图;
图4示出了本发明实施例提供的一种多传感器的时钟同步方法的应用实例的流程示意图;
图5示出了预设偏差时间与激光雷达和IMU的角速度残差和的关系示意图;
图6示出了本发明实施例提供的一种多传感器的时钟同步装置的结构示意图;
图7示出了本发明实施例提供的计算设备的结构示意图。
具体实施方式
下面将参照附图更详细地描述本发明的示例性实施例。虽然附图中显示了本发明的示例性实施例,然而应当理解,可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反,提供这些实施例是为了能够更透彻地理解本发明,并且能够将本发明的范围完整的传达给本领域的技术人员。
惯性测量仪(Inertial Measurement Unit,IMU)是一种常用的传感器,其可以高频快速地测量自身运动,但是随着时间增长,IMU的误差会逐渐积累,需要靠其他传感器的观测来修正累积的误差。外部传感器(例如雷达、相机等)可以测量出真实环境的轮廓、颜色等信息,配合IMU可以完成机器人建图、定位、感知等任务。但是,IMU和外部传感器之间往往存在时间偏差,这个偏差会对感知结果造成较大影响。
基于此,本发明实施例提供一种多传感器的时钟同步方法、装置及计算设备,能够计算出多个传感器之间的时间偏差,从而提高感知结果的准确性。
具体地,下面结合附图,对本发明实施例作进一步阐述。
其中,应当理解的是,本发明提供的下述实施例之间,只要不冲突,均可相互结合以形成新的实施方式。
图1示出了本发明实施例提供的一种多传感器的时钟同步方法的流程示意图。该方法用于对第一传感器和第二传感器进行时钟同步,其中,第一传感器和第二传感器固定于所需的应用设备上。第一传感器可以包括雷达传感器或者图像传感器,雷达传感器包括但不限于激光雷达、毫米波雷达、微波雷达、Flash雷达、MEMS雷达、相控阵雷达等能够生成三维点云的传感器,图像传感器包括但不限于单目相机、环视相机、双目相机、结构光相机等可以用于拍摄的图像传感器。第二传感器可以包括IMU,则本发明实施例能够用于计算IMU与雷达传感器,或者IMU与图像传感器之间的时间偏差。
其中,本实施例以第一传感器为激光雷达、第二传感器为IMU为例举例说明。如图1所示,该方法包括:
步骤110、获取第一传感器在第一传感器坐标系下的第一角速度。
其中,第一传感器坐标系可以为以激光雷达中心建立的三维坐标系。由于激光雷达在旋转运动,第一角速度是指激光雷达在第一传感器坐标系下的平均角速度。
具体地,如图2所示,步骤110包括:
步骤111、获取第一时刻第一传感器在第一传感器坐标系下的第一点云;
步骤112、获取第二时刻第一传感器在第一传感器坐标系下的第二点云;
步骤113、基于迭代最近点算法计算得到第一点云和第二点云之间的旋转矩阵;
步骤114、根据第一点云和第二点云之间的旋转矩阵、第一时刻、第二时刻,计算第一角速度。
其中,第一时刻和第二时刻是两个不同的时刻,例如,可以每隔预设时刻td获取第一传感器在第一传感器坐标系下的点云数据,则第一时刻与第二时刻之间可以相隔预设时间,即在第一时刻ti获取第一点云数据Pi,在第二时刻ti+d获取第二点云数据Pi+d。例如,预设时刻为1秒,则第一时刻与第二时刻之间可以相隔1秒,第一时刻为i秒,则第二时刻为i+1秒。
其中,采用迭代最近点(Iterative Closest Point,ICP)算法进行第一点云和第二点云的匹配,从而计算得到第一点云和第二点云之间的旋转矩阵。ICP算法具体实施方式可以为:一、在第一点云Pi中选取点pi;二、在第二点云Pi+d中找出点pi对应的点qi,并使得点pi与点qi之间的距离最小(例如可以遍历第二点云Pi+d,计算点pi与第二点云Pi+d中各个点的距离,通过不断比较,找到距离点pi最近的点qi);三、计算旋转矩阵R和平移矩阵T,使得误差函数最小;四、对点pi使用上一步求得的旋转矩阵R和平移矩阵T进行旋转和平移变换,得到新的对应点集pi';五、计算点pi'与点qi的平均距离d;六、若平均距离d小于给定的阈值或者大于预设的最大迭代次数,则停止迭代计算,否则返回第二步,直到满足收敛条件为止。
在本实施例中,首先,在第一点云Pi中选取点pi∈Pi,并在第二点云Pi+d中找出点pi对应的点qi∈Pi+d;然后通过使得误差函数E(R,t)最小计算旋转矩阵R和平移矩阵T,其中,可以设置一个误差函数阈值,当误差函数小于误差函数阈值,则认为误差函数最小,其中:
再者,对pi使用旋转矩阵R和平移矩阵T进行旋转和平移转换,得到新的对应点集pi':
p′i=R·pi+t,pi∈Pi
计算点pi'与点qi的平均距离d:
若d小于设定的阈值或者达到预设的迭代次数,则停止迭代,否则重复进行迭代,直至满足收敛条件。
在步骤114中,计算得到第一点云和第二点云之间的旋转矩阵后,根据第一点云和第二点云之间的旋转矩阵、第一时刻、第二时刻,计算第一角速度,具体可以包括:将第一点云和第二点云之间的旋转矩阵转化为欧拉角;将欧拉角除以第一时刻与第二时刻的差,得到第一传感器在第一时刻和第二时刻的中间时刻的角速度;将第一传感器在第一时刻和第二时刻的中间时刻的角速度确定为第一角速度。
其中,将第一点云和第二点云之间的旋转矩阵R转化为欧拉角
具体可以为:
将旋转矩阵R表示为:
则第一传感器在第一时刻ti和第二时刻ti+d的中间时刻的角速度为:
步骤120、获取第二传感器在第二传感器坐标系下的第二角速度以及第一传感器坐标系与第二传感器坐标系的变换关系。
其中,第二传感器坐标系可以为以IMU中心建立的三维坐标系。由于IMU也在在旋转运动,第二角速度是指IMU在第二传感器坐标系下的平均角速度。IMU能够用于测量角速度和加速度,则第二角速度可以直接从IMU测得的角速度中获取。
其中,第一传感器坐标系与第二传感器坐标系的变换关系可以包括第一传感器坐标系与第二传感器坐标系的旋转矩阵和平移矩阵。在第一传感器和第二传感器进行时钟同步之前,第一传感器和第二传感器进行了外参标定,则第一传感器坐标系与第二传感器坐标系的变换关系可以从第一传感器和第二传感器的外参标定结果中获取。
步骤130、根据第二角速度以及第一传感器坐标系与第二传感器坐标系的变换关系,计算第二传感器在第一传感器坐标系下的第三角速度。
其中,第三角速度是指将第二角速度转换到第一传感器坐标系下的角速度。则根据第二角速度以及第一传感器坐标系与第二传感器坐标系的变换关系,计算第二传感器在第一传感器坐标系下的第三角速度,具体可以为:将第二角速度与旋转矩阵相乘,计算得到第三角速度。即,第三角速度为:
其中,
为第二传感器在第一传感器坐标系下的第三角速度,
为第二传感器在第二传感器坐标系下的第二角速度,R'为旋转矩阵。
步骤140、根据第一角速度和第三角速度,计算在各个预设偏差时间下第一传感器和第二传感器的角速度残差和。
其中,预设偏差时间是预先设置的第一传感器和第二传感器之间的时间偏差。预设偏差时间有若干个,例如,设置偏差范围为-200毫秒至200毫秒,搜索步长为1毫秒,则预设偏差时间为-200毫秒、-199毫秒、-198毫秒……199毫秒、200毫秒。在每个预设偏差时间都根据第一传感器和第二传感器的对应采样点的角速度计算第一传感器和第二传感器的角速度残差和,则得到在各个预设偏差时间下第一传感器和第二传感器的角速度残差和。
具体地,如图3所示,步骤140包括:
步骤141、确定第一参考时刻以及第一参考时刻对应的第一角速度;
步骤142、将第一参考时刻加上预设偏差时间,得到第二参考时刻;
步骤143、确定第二参考时刻对应的第三角速度;
步骤144、根据第一参考时刻对应的第一角速度与第二参考时刻对应的第三角速度,确定角速度对;
步骤145、计算全部角速度对的差值的向量模长的和,作为预设偏差时间下第一传感器和第二传感器的角速度残差和。
其中,第一参考时刻是指第一传感器的时间戳,即激光雷达的时间。例如,激光雷达每间隔1秒获取一组点云数据,共采集到4组点云数据,则第一参考时刻可以为1s、2s、3s、4s,第一参考时刻对应的第一角速度是指1s、2s、3s、4s时刻激光雷达对应的角速度。
其中,第二参考时刻是指第二传感器的时间戳,即IMU的时间。例如,假设第一参考时刻为1s、2s、3s、4s,若其中一预设偏差时间为-200ms,则第二参考时刻为0.8s、1.8s、2.8s、3.8s,则0.8s、1.8s、2.8s、3.8s时刻IMU对应的角速度为第二参考时刻对应的第三角速度;又例如,假设第一参考时刻为1s、2s、3s、4s,若其中一预设偏差时间为100ms,则第二参考时刻为1.1s、2.1s、3.1s、4.1s,则1.1s、2.1s、3.1s、4.1s时刻IMU对应的角速度为第二参考时刻对应的第三角速度。
在步骤144和步骤145中,在确定第一参考时刻对应的第一角速度与第二参考时刻对应的第三角速度后,在第一参考时刻对应的第一角速度中确定对应的第二参考时刻对应的第三角速度,组成角速度对,从而确定角速度对;根据角速度对,计算全部角速度对的差值的向量模长的和。例如,假设第一参考时刻为1s、2s、3s、4s,第二参考时刻为1.1s、2.1s、3.1s、4.1s,则激光雷达的时间为1s时对应的第一角速度和IMU的时间为1.1s时对应的第三角速度组成第一个角速度对,激光雷达的时间为2s时对应的第一角速度和IMU的时间为2.1s时对应的第三角速度组成第二个角速度对,激光雷达的时间为3s时对应的第一角速度和IMU的时间为3.1s时对应的第三角速度组成第三个角速度对,激光雷达的时间为4s时对应的第一角速度和IMU的时间为4.1s时对应的第三角速度组成第四个角速度对,则全部共有四个角速度对。
其中,计算全部角速度对的差值的向量模长的和为:
其中,t为预设偏差时间,δ
t为在预设偏差时间t时的全部角速度对的差值的向量模长的和(即预设偏差时间t时第一传感器和第二传感器的角速度残差和),n为角速度对的数量,k∈[1,n],且k为整数,
是指第k个角速度对的差值的向量模长。
步骤150、将角速度残差和中的最小的角速度残差和对应的预设偏差时间确定为第一传感器和第二传感器的时间偏差。
计算得到各个预设偏差时间下的角速度残差和后,比较各个角速度残差和的大小,确定角速度残差和中的最小的角速度残差和,并确定最小的角速度残差和对应的预设偏差时间,将这个预设偏差时间确定为第一传感器和第二传感器的时间偏差。例如,若确定当预设偏差时间为12毫秒时角速度残差和最小,则将12毫秒确定为第一传感器和第二传感器的时间偏差。
步骤160、根据第一传感器和第二传感器的时间偏差,对第一传感器和第二传感器进行时钟同步。
根据第一传感器和第二传感器的时间偏差,调整第一传感器或者第二传感器的时钟,使得第一传感器和第二传感器的时钟同步。
本发明实施例通过获取第一传感器在第一传感器坐标系下的第一角速度,获取第二传感器在第二传感器坐标系下的第二角速度以及第一传感器坐标系与第二传感器坐标系的变换关系,根据第二角速度以及第一传感器坐标系与第二传感器坐标系的变换关系,计算第二传感器在第一传感器坐标系下的第三角速度,根据第一角速度和第三角速度,计算在各个预设偏差时间下第一传感器和第二传感器的角速度残差和,将角速度残差和中的最小角速度残差和对应的预设偏差时间确定为第一传感器和第二传感器的时间偏差,根据第一传感器和第二传感器的时间偏差,对第一传感器和第二传感器进行时钟同步,能够准确地计算出第一传感器和第二传感器的时间偏差,从而计算出多个传感器之间的时间偏差,解决多传感器的时间同步问题,从而提高感知结果的准确性。
在一些实施例中,在步骤143中,当无法从测量数据中直接得到第二参考时刻对应的第三角速度时,则需要计算第二参考时刻对应的第三角速度。则步骤143具体可以包括:
步骤1431、若查找不存在第二参考时刻对应的第三角速度,则获取第二参考时刻的上一时间戳对应的第三角速度、下一时间戳对应的第三角速度;
步骤1432、根据上一时间戳对应的第三角速度、下一时间戳对应的第三角速度,计算第二传感器在第二参考时刻的第四角速度;
步骤1433、将第四角速度确定为第二参考时刻对应的第三角速度。
其中,在采集得到的IMU测量数据中查找是否存在第二参考时刻对应的第三角速度,若在第二参考时刻时,恰好采集到IMU数据,则查找存在第二参考时刻对应的第三角速度,若在第二参考时刻时,没有采集到IMU数据,则查找不存在第二参考时刻对应的第三角速度。
其中,第二参考时刻的上一时间戳是指第二参考时刻的前一个采集到IMU数据的时刻,第二参考时刻的下一时间戳是指第二参考时刻的下一个采集到IMU数据的时刻。根据这两个时刻的第三角速度,计算第二传感器在第二参考时刻的第四角速度。其中,假设第二参考时刻t2的上一时间戳为t21,第二参考时刻的下一时间戳为t22(则t21<t2<t22),计算得到第二参考时刻t2对应的第四角速度为:
其中,
为第四角速度,
为IMU在时间t
21的角速度,
为IMU在时间t
22的角速度。
图4示出了本发明实施例提供的一种多传感器的时钟同步方法的应用实例的流程示意图。将10HZ的多线激光雷达和200HZ的IMU固定在机器人车体上,机器人移动到特征明显且没有明显退化场景的厂房里,控制机器人运动(包括各个方向的旋转和平移)并开始记录数据,一分钟后结束运动,保存数据,共600帧激光雷达数据,12000帧IMU数据。
如图4所示,该方法包括:
步骤201、计算激光雷达在激光雷达坐标系下的第一角速度。
其中,获取激光雷达第i帧的第一点云P
i和第i+1帧的第二点云P
i+1,并通过ICP算法计算P
i和P
i+1之间的旋转矩阵R。其中,通过ICP算法计算P
i和P
i+1之间的旋转矩阵的方法在上述实施例中已经详细描述,此处不再赘述。将旋转矩阵R转化为欧拉角
并根据欧拉角
计算第一传感器的第一角速度
计算得到的第一角速度为
步骤202、获取IMU在IMU坐标系下的第二角速度,以及激光雷达坐标系和IMU坐标系的变换关系。
其中,获取得到IMU在IMU坐标系下的第二角速度为
激光雷达坐标系和IMU坐标系的变换关系为R'。
步骤203、根据第二角速度以及激光雷达坐标系和IMU坐标系的变换关系,计算IMU在激光雷达坐标系下的第三角速度。
其中,根据
计算得到IMU在激光雷达坐标系下的第三角速度为
步骤204、根据第一角速度和第三角速度,计算在各个预设偏差时间下激光雷达和IMU的角速度残差和。
其中,设置偏差范围为-200毫秒至200毫秒,搜索步长为1毫秒,则预设偏差时间为-200毫秒、-199毫秒、-198毫秒……199毫秒、200毫秒。计算得到各个预设偏差时间下激光雷达和IMU的角速度残差和如图5所示。
步骤205、将角速度残差和中的最小角速度残差和对应的预设偏差时间确定为激光雷达和IMU的时间偏差。
其中,在图5中,当预设偏差时间为12毫秒时,激光雷达和IMU的角速度残差和最小,则将12毫秒确定为激光雷达和IMU的时间偏差。
步骤206、根据激光雷达和IMU的时间偏差,调整激光雷达或者IMU的时钟,使得第一传感器和第二传感器的时钟同步。
本发明实施例通过获取激光雷达在激光雷达坐标系下的第一角速度,获取IMU在IMU坐标系下的第二角速度以及激光雷达坐标系与IMU坐标系的变换关系,根据第二角速度以及激光雷达坐标系与IMU坐标系的变换关系,计算IMU在激光雷达坐标系下的第三角速度,根据第一角速度和第三角速度,计算在各个预设偏差时间下激光雷达和IMU的角速度残差和,将角速度残差和中的最小角速度残差和对应的预设偏差时间确定为激光雷达和IMU的时间偏差,根据激光雷达和IMU的时间偏差,对激光雷达和IMU进行时钟同步,能够准确地计算出激光雷达和IMU的时间偏差,从而计算出多个传感器之间的时间偏差,解决多传感器的时间同步问题,从而提高感知结果的准确性。
图6示出了本发明实施例提供的一种多传感器的时钟同步装置的结构示意图。如图6所示,该装置300包括:第一获取模块310、第二获取模块320、角速度转换模块330、角速度残差和计算模块340、时间偏差确定模块350、时钟同步模块360。
其中,第一获取模块310用于获取第一传感器在第一传感器坐标系下的第一角速度;第二获取模块320用于获取第二传感器在第二传感器坐标系下的第二角速度以及所述第一传感器坐标系与所述第二传感器坐标系的变换关系;角速度转换模块330用于根据所述第二角速度以及所述第一传感器坐标系与所述第二传感器坐标系的变换关系,计算所述第二传感器在所述第一传感器坐标系下的第三角速度;角速度残差和计算模块340用于根据所述第一角速度和所述第三角速度,计算在各个预设偏差时间下所述第一传感器和所述第二传感器的角速度残差和;时间偏差确定模块350用于将所述角速度残差和中的最小角速度残差和对应的所述预设偏差时间确定为所述第一传感器和所述第二传感器的时间偏差;时钟同步模块360用于根据所述第一传感器和所述第二传感器的时间偏差,对所述第一传感器和所述第二传感器进行时钟同步。
在一种可选的方式中,第一获取模块310具体用于:获取第一时刻所述第一传感器在所述第一传感器坐标系下的第一点云;获取第二时刻所述第一传感器在所述第一传感器坐标系下的第二点云;基于迭代最近点算法计算得到所述第一点云和所述第二点云之间的旋转矩阵;根据所述第一点云和所述第二点云之间的旋转矩阵、所述第一时刻、所述第二时刻,计算所述第一角速度。
在一种可选的方式中,第一获取模块310具体还用于:将所述第一点云和所述第二点云之间的旋转矩阵转化为欧拉角;将所述欧拉角除以所述第一时刻与所述第二时刻的差,得到所述第一传感器在所述第一时刻和所述第二时刻的中间时刻的角速度;将所述第一传感器在所述第一时刻和所述第二时刻的中间时刻的角速度确定为所述第一角速度。
在一种可选的方式中,所述第一传感器坐标系与所述第二传感器坐标系的变换关系包括所述第一传感器坐标系与所述第二传感器坐标系的旋转矩阵;角速度转换模块330具体用于:将所述第二角速度与所述旋转矩阵相乘,计算得到所述第三角速度。
在一种可选的方式中,角速度残差和计算模块340具体用于:确定第一参考时刻以及所述第一参考时刻对应的所述第一角速度;将所述第一参考时刻加上所述预设偏差时间,得到第二参考时刻;确定所述第二参考时刻对应的所述第三角速度;根据所述第一参考时刻对应的所述第一角速度与所述第二参考时刻对应的所述第三角速度,确定角速度对;计算全部所述角速度对的差值的向量模长的和,作为所述预设偏差时间下所述第一传感器和所述第二传感器的角速度残差和。
在一种可选的方式中,角速度残差和计算模块340具体还用于:若查找不存在所述第二参考时刻对应的所述第三角速度,则获取所述第二参考时刻的上一时间戳对应的所述第三角速度、下一时间戳对应的所述第三角速度;根据上一时间戳对应的所述第三角速度、下一时间戳对应的所述第三角速度,计算所述第二传感器在所述第二参考时刻的第四角速度;将所述第四角速度确定为所述第二参考时刻对应的所述第三角速度。
在一种可选的方式中,所述第一传感器包括激光雷达、毫米波雷达、微波雷达、图像传感器中的至少一种,所述第二传感器包括惯性测量仪。
需要说明的是,本发明实施例提供的多传感器的时钟同步装置是能够执行上述多传感器的时钟同步方法的装置,则上述基于多传感器的时钟同步方法的所有实施例均适用于该装置,且均能达到相同或相似的有益效果。
本发明实施例通过获取第一传感器在第一传感器坐标系下的第一角速度,获取第二传感器在第二传感器坐标系下的第二角速度以及第一传感器坐标系与第二传感器坐标系的变换关系,根据第二角速度以及第一传感器坐标系与第二传感器坐标系的变换关系,计算第二传感器在第一传感器坐标系下的第三角速度,根据第一角速度和第三角速度,计算在各个预设偏差时间下第一传感器和第二传感器的角速度残差和,将角速度残差和中的最小角速度残差和对应的预设偏差时间确定为第一传感器和第二传感器的时间偏差,根据第一传感器和第二传感器的时间偏差,对第一传感器和第二传感器进行时钟同步,能够准确地计算出第一传感器和第二传感器的时间偏差,从而计算出多个传感器之间的时间偏差,解决多传感器的时间同步问题,从而提高感知结果的准确性。
本发明实施例提供了一种计算机可读存储介质,所述存储介质中存储有至少一可执行指令,所述可执行指令使处理器执行上述任意方法实施例中的多传感器的时钟同步方法。
本发明实施例提供了一种计算机程序产品,所述计算机程序产品包括存储在计算机存储介质上的计算机程序,所述计算机程序包括程序指令,当所述程序指令被计算机执行时,使所述计算机执行上述任意方法实施例中的多传感器的时钟同步方法。
图7示出了本发明实施例提供的计算设备的结构示意图,本发明具体实施例并不对计算设备的具体实现做限定。
其中,计算设备包括:处理器和存储器。存储器用于存放至少一可执行指令,当计算设备运行时,处理器执行可执行指令,使处理器执行根据上述任意方法实施例中的多传感器的时钟同步方法的步骤。
可选地,如图7所示,该计算设备可以包括:处理器(processor)402、通信接口(Communications Interface)404、存储器(memory)406、以及通信总线408。
其中:处理器402、通信接口404、以及存储器406通过通信总线408完成相互间的通信。通信接口404,用于与其它设备比如客户端或其它服务器等的网元通信。处理器402,用于执行程序410,具体可以执行上述任意方法实施例中的多传感器的时钟同步方法。
具体地,程序410可以包括程序代码,该程序代码包括计算机操作指令。
处理器402可能是中央处理器CPU,或者是特定集成电路ASIC(ApplicationSpecific Integrated Circuit),或者是被配置成实施本发明实施例的一个或多个集成电路。计算设备包括的一个或多个处理器,可以是同一类型的处理器,如一个或多个CPU;也可以是不同类型的处理器,如一个或多个CPU以及一个或多个ASIC。
存储器406,用于存放程序410。存储器406可能包含高速RAM存储器,也可能还包括非易失性存储器(non-volatile memory),例如至少一个磁盘存储器。
本发明实施例通过获取第一传感器在第一传感器坐标系下的第一角速度,获取第二传感器在第二传感器坐标系下的第二角速度以及第一传感器坐标系与第二传感器坐标系的变换关系,根据第二角速度以及第一传感器坐标系与第二传感器坐标系的变换关系,计算第二传感器在第一传感器坐标系下的第三角速度,根据第一角速度和第三角速度,计算在各个预设偏差时间下第一传感器和第二传感器的角速度残差和,将角速度残差和中的最小角速度残差和对应的预设偏差时间确定为第一传感器和第二传感器的时间偏差,根据第一传感器和第二传感器的时间偏差,对第一传感器和第二传感器进行时钟同步,能够准确地计算出第一传感器和第二传感器的时间偏差,从而计算出多个传感器之间的时间偏差,解决多传感器的时间同步问题,从而提高感知结果的准确性。
在此提供的算法或显示不与任何特定计算机、虚拟***或者其它设备固有相关。各种通用***也可以与基于在此的示教一起使用。根据上面的描述,构造这类***所要求的结构是显而易见的。此外,本发明实施例也不针对任何特定编程语言。应当明白,可以利用各种编程语言实现在此描述的本发明的内容,并且上面对特定语言所做的描述是为了披露本发明的最佳实施方式。
在此处所提供的说明书中,说明了大量具体细节。然而,能够理解,本发明的实施例可以在没有这些具体细节的情况下实践。在一些实例中,并未详细示出公知的方法、结构和技术,以便不模糊对本说明书的理解。
类似地,应当理解,为了精简本发明并帮助理解各个发明方面中的一个或多个,在上面对本发明的示例性实施例的描述中,本发明实施例的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而,并不应将该公开的方法解释成反映如下意图:即所要求保护的本发明要求比在每个权利要求中所明确记载的特征更多的特征。更确切地说,如下面的权利要求书所反映的那样,发明方面在于少于前面公开的单个实施例的所有特征。因此,遵循具体实施方式的权利要求书由此明确地并入该具体实施方式,其中每个权利要求本身都作为本发明的单独实施例。
本领域那些技术人员可以理解,可以对实施例中的设备中的模块进行自适应性地改变并且把它们设置在与该实施例不同的一个或多个设备中。可以把实施例中的模块或单元或组件组合成一个模块或单元或组件,以及此外可以把它们分成多个子模块或子单元或子组件。除了这样的特征和/或过程或者单元中的至少一些是相互排斥之外,可以采用任何组合对本说明书(包括伴随的权利要求、摘要和附图)中公开的所有特征以及如此公开的任何方法或者设备的所有过程或单元进行组合。除非另外明确陈述,本说明书(包括伴随的权利要求、摘要和附图)中公开的每个特征可以由提供相同、等同或相似目的的替代特征来代替。
此外,本领域的技术人员能够理解,尽管在此的一些实施例包括其它实施例中所包括的某些特征而不是其它特征,但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如,在下面的权利要求书中,所要求保护的实施例的任意之一都可以以任意的组合方式来使用。
应该注意的是上述实施例对本发明进行说明而不是对本发明进行限制,并且本领域技术人员在不脱离所附权利要求的范围的情况下可设计出替换实施例。在权利要求中,不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的元件或步骤。位于元件之前的单词“一”或“一个”不排除存在多个这样的元件。本发明可以借助于包括有若干不同元件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中,这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。上述实施例中的步骤,除有特殊说明外,不应理解为对执行顺序的限定。