多个IMU时间同步方法、装置、终端与流程

本发明涉及导航用多传感器融合技术领域，尤其涉及一种多个imu时间同步方法、装置、终端。

背景技术：

随着惯性测量单元（以下简称imu）成本的不断下降以及集成式惯性传感器的出现，越来越多的机器人等平台都配备了多个imu。通常地，这些平台的共同之处主要在于，将主imu用于定位和对齐（即安装在靠近重心的位置并与平台的主轴对齐），辅助imu安装在受阻程度较小的地方。对于大多数平台，这两个位置有很大的区别，为了融合多个imu的数据，需要对它们进行时间戳的对齐操作。

然而，就目前的常见的多个imu的时间戳对齐或称同步主要有两种：

一是采用硬件同步的方法，例如已有的专利公开号为cn110217178a的专利申请《一种基于硬件同步的无人驾驶感知系统及其工作方法》，即利用高精度的硬件来解决多个imu的数据同步问题，当然还有如专利公开号为cn102736591a的《一种分布式pos子imu数据同步采集系统》等等。这类方法虽然精度较高，但其成本也高，对于大部分用户或企业而言并不太适合等。

二是采用软件同步的方法，主要是根据两个imu的不同时间戳信息，采用线程锁的方式来对齐两组imu的数据，该方法虽然比较方便，但是却无法达到高精度需求，对于一些要求高精度的场合并不适用。此外，还有一些其他的软件与硬件结合的时间同步方法，例如专利公开号为cn109729277a的《多传感器采集时间戳同步装置》在解决时间同步的问题时，主要是通过串口传输来实现时间戳同步而且每个传感器都能获取到时间戳信息，而这一点对于相机自身不具有时间戳的场景来说无法适用，具有较大的局限性；又例如，专利公开号为cn109951248a的《一种水下传感器网络时间同步方法》其主要是利用待同步传感器节点与邻居节点交换数据包的发送/接受钟面时刻，拟合各邻居节点钟面时间与自身钟面时间的线性回归式，不断迭代更新自身钟面时刻参数，从而实现传感器节点间的时间同步。由于该方法需要传感器之间能进行通讯，而这点对于不具有相互通讯能力且彼此独立工作的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之间的外部参数；

从所述多个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时间同步方法中，所述“计算得到第一精度对应的最优时间差；其中，每次平移后均根据所述当前imu的所述角度变化值进行插值及数据截取，以计算所述当前imu与作为基准的imu之间的角度数据累计差值，然后从多个所述累计差值中获取最小累计差值对应的平移时间值，以作为所述第一精度对应的最优时间差”包括：

对于其他的每一imu，在作为基准的imu的各采样时刻处根据当前imu对应的角度变化值进行角度或角速度插值，以得到所述当前imu的含插值在内的角度数据集；

对作为基准的imu的角度数据集和当前imu各自的角度数据集进行等时间等数量截取，并在每一所述采样时刻处计算当前imu与作为基准的imu之间的角度数据的差值并累加，以得到当前平移对应的累计差值；

然后重复上述步骤，得到平移所述第一预设次数对应的所有累计差值；

从所有累计差值中选取出最小值，并将所述最小值作为所述第一精度对应的最优时间差。

进一步地，在上述的多个imu时间同步方法中，所述第二精度小于所述第一精度，且所述“然后根据所述第一精度的所述最优时间差对应的第二预设取值范围对所述当前imu进行第二预设次数的时间戳平移，以得到第二精度对应的最优时间差”包括：

从所述第二精度对应的所述第二预设取值范围中依次取值与所述第一精度的最优时间差进行累加以得到对应的平移时间值；所述第二预设次数等于当前取值的次数；

按照每次得到的所述平移时间值分别对所述当前imu的时间戳进行整体平移，然后计算每次平移后对应的所述累计差值并从中选取最小累计差值对应的平移时间值，以作为所述第二精度对应的最优时间差。

进一步地，在上述的多个imu时间同步方法中，当设置有所述多个imu的设备在地面上移动时，所述预先选取的姿态角为绕方向向上的z轴的旋转角度。

本发明的另一实施例提出一种多个imu时间同步装置，包括：

外参标定模块，用于基于相机对多个imu进行外参标定以获取各imu之间的外部参数，并利用所述外部参数将各imu获取到的角度数据转换至同一坐标系下后计算各imu在预先选取的姿态角的角度变化值；

初始同步计算模块，用于以采样频率最大的imu的时间戳为基准，根据当前imu与作为基准的imu各自的起始时刻之间的初始时间差进行时间戳粗略同步，然后根据所述初始时间差对应的第一预设取值范围对所述当前imu进行第一预设次数的时间戳平移，以计算得到第一精度对应的最优时间差；其中，每次平移后均根据所述当前imu的所述角度变化值进行插值及数据截取，以计算所述当前imu与作为基准的imu之间的角度数据累计差值，然后从多个所述累计差值中获取最小累计差值对应的平移时间值，以作为所述第一精度对应的最优时间差；

精确同步计算模块，用于然后根据所述第一精度的所述最优时间差对应的第二预设取值范围对所述当前imu进行第二预设次数的时间戳平移，以得到第二精度对应的最优时间差，如此类推，直到得到满足预设精度对应的最优时间差，以作为所述当前imu与作为基准的imu之间的最终同步时间。

本发明的又一实施例提出一种终端，包括：处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施上述的多个imu时间同步方法。

本发明的再一实施例提出一种计算机可读存储介质，其存储有计算机程序，在所述计算机程序被执行时，实施根据上述的多个imu时间同步方法。

本发明实施例提出的方法通过采用纯软件的同步方法，通过获取各imu之间的外参，然后根据该外参信息将所有的imu转换至同一坐标系下进行某一姿态角的角度变化分析；通过时间戳平移算法并基于插值算法以计算各imu之间的角度数据累计差值，并选取使差值最小的平移时间作为对应精度的最优时间差；然后按照不同的精度来逐渐缩小时间差范围，以得到的能够满足所需精度对应的最终同步时间。相对于现有技术而言，采用本发明实施例的方法，通过计算机软件技术实现所需精度的传感器同步，其也无需增加其他硬件，相对于硬件同步方式可降低硬件成本。

可以解决硬件成本的问题，时间同步上也得到了极大的提高等。

附图说明

为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。

图1示出了本发明实施例多个imu时间同步方法的第一流程示意图；

图2示出了本发明实施例多个imu时间同步方法的第二流程示意图；

图3示出了本发明实施例多个imu时间同步方法的第三流程示意图；

图4示出了本发明实施例多个imu时间同步方法的第四流程示意图；

图5示出了本发明实施例多个imu时间同步方法的第五流程示意图；

图6示出了本发明实施例多个imu时间同步装置的结构示意图。

主要元件符号说明：

10-多个imu时间同步装置；110-外参标定模块；120-初始同步计算模块；130-精确同步计算模块。

具体实施方式

下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

在下文中，可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。

此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限定。

实施例1

请参照图1，本实施例提出一种多个imu时间同步方法，可应用于需要多个传感器进行信息融合的场合，如即时定位与地图构建（slam）、自主导航定位等等。本实施例的方法采用软件的同步方式，相比现有的软件同步方法可大大提高精度，并且由于该方法无需增加任何硬件，故可以很好地解决基于硬件同步而导致存在硬件成本高等问题。

如图1所示，下面对该多个imu时间同步方法进行详细说明。

步骤s100，基于相机对多个imu进行外参标定以获取各imu之间的外部参数，并利用所述外部参数将各imu采样到的角度数据转换至同一坐标系下后计算各imu在预先选取的姿态角的角度变化值。

示范性地，上述步骤s100主要包括以下两部分：一是获取这多个imu之间的外部参数，以方便后续进行同一坐标系的转换；二是在同一坐标系下，根据各imu获取到的角度数据来分析三轴姿态角中预先选取的变化最大的姿态角的角度变化信息，即该姿态角的角速度信息。可以理解，本实施例中的imu获取到的角度数据可以是角度值或角速度值。

如图2所示，对于上述步骤s100中的第一部分，主要包括以下子步骤：

步骤s101，利用标定板并基于相机对多个imu进行外参标定，以获取每一imu与相机之间的外部参数。

可以理解，外部参数是相机和imu之间的桥梁，在进行求解相机位姿或重投影误差等处理时均需要利用该外部参数。而本实施例中，由于各imu均具有自身的坐标系，并且彼此之间也是相互独立测量的，在对不同imu的数据融合过程中，数据在不同坐标系下的转换需要使用各imu之间的坐标系的外部参数。其中，该外部参数通常包括旋转矩阵和平移矩阵。

示范性地，可采用标定板等工具先对安装有多个imu和相机的刚体进行外参标定，从而得到在该刚体中每个imu与相机之前的外部参数。例如，在标定过程中，可使该刚体对着标定板进行数据采集，同时使各imu在x、y和z方向上具有足够的激励以使对应的pitch角、roll角、yaw角（即三轴姿态角）变化明显，然后根据由相机录制的图像数据借助kalibr等工具即可计算得到每一imu与相机之间的外部参数。

步骤s102，根据每一imu与所述相机之间的外部参数计算得到各imu之间的外部参数。

在得到各imu与相机之间的外参后，将进一步计算各imu之间的外参。在已知每一imu相对同一相机之间的外参，因此可利用旋转和平移来求解一imu到另一imu之间的外参，即求解对应的旋转矩阵和平移矩阵。

示范性地，在一种实施例中，该多个imu包括第一imu和第二imu，假设该第一imu与相机之间的外部参数为，而该第二imu与相机之间的外部参数为，则该第一imu和该第二imu之间的外部参数将满足如下公式：

。

可以理解，通过上述公式进行两两imu之间的外参求解。若设备中存在更多的imu时，同样可利用上述公式计算，在此不再赘述。

步骤s103，从该多个imu中选取其中一imu的坐标系作为基准坐标系，然后根据其他的imu与作为基准坐标系的imu之间的外部参数将所述其他的imu获取到的角度数据转换至该基准坐标系中。

imu用于测量对应刚体的三轴姿态角以及加速度，通过imu内部传感器采样到的数据可以得到该刚体的姿态角信息。通常地，该姿态角信息也可用欧拉角表示，即通过分别绕不同轴旋转的旋转角进行描述。若选取imu的z轴的方向向上（或向下），则这三个姿态角分别是绕x轴旋转的翻滚角（roll）、绕y轴旋转的俯仰角（pitch）以及绕z轴旋转的偏航角（yaw）。

示范性地，对于上述步骤s103，可选取其中一imu的坐标系作为其他imu转换的基准，并将其他的imu的角度数据均转换至该基准坐标系中，以方便后续进行同一姿态角的角度变化信息的比较。

步骤s104，然后在该基准坐标系中计算每一imu在预先选取的姿态角的角度变化值。

在一种实施例中，示范性地，当设置有该多个imu及相机的设备在地面上移动时，该预先选取的姿态角将选取为绕z轴的旋转角度，即上述的绕z轴旋转的偏航角yaw。由于设备在地面移动过程中可以走各种形状，如s型，○型，或者8型等，因此此时的yaw角就是变化最大的姿态角。可以理解，该姿态角的选取可以根据该设备在具体的运用场景中的运动轨迹及imu的放置方向等来确定。

其中，在分析各imu的yaw角的角度变化，例如，可通过对yaw角的角度值进行微分来计算该yaw角的角速度信息。当然，若获取到的是角速度，则可直接计算对应时间内的角速度变化信息。

步骤s200，以采样频率最大的imu的时间戳为基准，根据当前imu与作为基准的imu各自的起始时刻之间的初始时间差进行时间戳粗略同步，然后根据该初始时间差对应的第一预设取值范围对当前imu进行第一预设次数的时间戳平移，以计算得到第一精度对应的最优时间差。

如图3所示，对于上述步骤s200中的粗略同步及时间戳平移步骤，主要包括以下几个子步骤：

子步骤s201，从多个imu中选取频率最大的imu的时间戳作为基准。

由于每个imu的采样频率不同，导致在相同时间内不同的imu的采样数量也会存在不同。为方便比较同一采样时刻下不同imu之间的角度变化差值，示范性地，将选取频率最大的imu的时间戳作为时间基准。

子步骤s202，根据当前imu的起始时刻与作为基准的imu的起始时刻之间的初始时间差对当前imu的时间戳进行整体平移，以进行粗略同步。

示范性地，根据记录的该当前imu的起始时刻与基准imu的起始时刻之间的初始时间差进行粗略同步，即将当前imu的时间戳向着与基准imu的时间戳对齐。例如，若初始时间差为负值，则表示该当前imu的时间戳靠前，因此需将其向后移该初始时间差对应的数值；若初始时间差为负值，表示该当前imu的时间戳在后，因此需将其向前移动，以达到粗略同步目的。

子步骤s203，从第一精度对应的第一预设取值范围中依次取值与该初始时间差进行累加以得到对应的平移时间值；其中，第一预设次数等于从该第一预设取值范围中取值的次数。

子步骤s204，按照每次得到的平移时间值分别对当前imu的时间戳进行整体平移。

示范性地，若该初始时间差为t0，则从该第一预设取值范围中依次取值记为{-tm,…,tm}，tm>0，即从[-tm,tm]中进行依次取值，则每次计算得到的平移时间值为t0+tk，tk表示取出的第k个值，于是，将当前imu的时间戳整体移动t0+tk。通常地，该第一预设取值范围为对称取值，具体的数据可根据实际需求来选取。

例如，若imu-a的原始采样时间戳为1.0，2.0，3.0，4.0....，而imu-b的原始采样时间戳为3.5，4.0，4.5，5.0....，可知imu-b作为基准的imu，若初始时间差为2.2，第一预设取值范围为[-0.9s,0.9s]，若当前取值为-0.8s，则需要对imu-a的原始采样时间戳整体向后平移1.4。

如图4所示，对于上述步骤s200中的第一精度对应的最优时间差的计算过程，主要包括以下子步骤：

子步骤s205，对于其他的每一imu，在作为基准的imu的各采样时刻处根据当前imu对应的角度变化值进行角度或角速度插值，以得到当前imu的含插值在内的角度数据集。

可以理解，其中插入的数值可以是角度或角速度，具体根据获取到的角度数据的类型为准，这两者中的任意一个均可根据对应的imu的角速度变化信息计算得到。在插值时，优选地采用线性插值方式。

子步骤s206，对作为基准的imu的角度数据集和当前imu各自的角度数据集进行等时间等数量截取，并在每一所述采样时刻处计算当前imu与作为基准的imu之间的角度数据的差值并累加，以得到当前平移对应的累计差值。

子步骤s207，然后重复上述步骤，得到平移该第一预设次数对应的所有累计差值。可以理解，该累计差值的个数即等于该第一预设次数。

子步骤s208，从所有累计差值中选取出最小值，并将该最小值对应的平移时间值作为第一精度对应的最优时间差。

仍以上述为例，当imu-a的时间戳平移1.4s后，则需要对imu-a在3.5，4.0，4.5，5.0....这些采样时刻处进行角度数据插值，从而得到imu-a的含插值在内的角度数据集。于是，从各自的角度数据集中截取在相同时间内的该imu-a和imu-b的角度数据，两者的数量相等。然后，将imu-a与imu-b各自的角度数据进行作差，并将每一差值进行累加求和以得到上述的累计差值。接着，平移1.5s（即2.2-0.7）后，按照上述方法计算对应的累计差值，如此类推，直到得到19组的累计差值，然后从这些累计差值中选取出最小值，此时该最小值对应的平移时间即作为第一精度（0.1s）对应的最优时间差。

步骤s300，然后根据该第一精度的最优时间差对应的第二预设取值范围对当前imu进行第二预设次数的时间戳平移，以得到第二精度对应的最优时间差，如此类推，直到得到满足预设精度对应的最优时间差，以作为当前imu与作为基准的imu之间的最终同步时间。

示范性地，该第二精度小于第一精度，如可为0.05s和0.1s。可以理解，该预设精度可根据实际需求进行设定，可以是0.05s、0.01s等等。在计算到时满足需求的高精度对应的最优时间差，将其作为两者的最终时间差以便进行时间同步。

如图5所示，对于上述步骤s300的第二精度对应的最优时间差的计算，主要包括有：

步骤s301，从该第二精度对应的第二预设取值范围中依次取值与该第一精度的最优时间差进行累加，以得到对应的平移时间值。其中，该第二预设次数等于该第二预设取值范围对应的取值次数。

步骤s302，按照每次得到的平移时间值分别对当前imu的时间戳进行整体平移，然后计算每次平移后对应的累计差值并从中选取最小累计差值对应的平移时间值，以作为该第二精度对应的最优时间差。

与上述第一精度的最优时间差的求解不同之处仅在于，第二精度是以该第一精度的最优时间差为基础，将其与取出的值进行累加以作为平移时间值。而其他步骤则可参见上述第一精度的最优时间差的求解过程，故在此不再详述。

本实施例提出的多个imu时间同步方法通过获取各imu之间的外参，然后根据该外参信息将所有的imu转换至同一坐标系下进行某一姿态角的角度变化分析；通过时间戳平移算法及插值算法等对各imu之间的角度数据进行差值比较并求得累计差值，通过选取使该累计差值最小的平移时间值作为最终的同步时间。其中，在平移过程中，按照不同的精度来逐渐缩小时间差的范围，直到得到满足所需精度的时间差值为止，这样可大大提高时间同步的精度。另外，由于采用的是软件同步方式，也无需增加其他硬件，相对于硬件同步方式可降低硬件成本。

实施例2

请参照图6，基于上述实施例1的多个imu时间同步方法，本实施例提出一种多个imu时间同步装置10，包括：

外参标定模块110，用于基于相机对多个imu进行外参标定以获取各imu之间的外部参数，并利用所述外部参数将各imu获取到的角度数据转换至同一坐标系下后计算各imu在预先选取的姿态角的角度变化值；

初始同步计算模块120，用于以采样频率最大的imu的时间戳为基准，根据当前imu与作为基准的imu各自的起始时刻之间的初始时间差进行时间戳粗略同步，然后根据所述初始时间差对应的第一预设取值范围对所述当前imu进行第一预设次数的时间戳平移，以计算得到第一精度对应的最优时间差；其中，每次平移后均根据所述当前imu的所述角度变化值进行插值及数据截取，以计算所述当前imu与作为基准的imu之间的角度数据累计差值，然后从多个所述累计差值中获取最小累计差值对应的平移时间值，以作为所述第一精度对应的最优时间差；

精确同步计算模块130，用于然后根据所述第一精度的所述最优时间差对应的第二预设取值范围对所述当前imu进行第二预设次数的时间戳平移，以得到所述第二精度对应的最优时间差，如此类推，直到得到满足预设精度对应的最优时间差，以作为所述当前imu与作为基准的imu之间的最终同步时间。

可以理解，上述的多个imu时间同步装置10对应于实施例1的多操作系统的同屏显示方法。实施例1中的任何可选项也适用于本实施例，这里不再详述。

本发明还提供了一种终端，如计算机、服务器等，该终端包括存储器和处理器，存储器存储有计算机程序，处理器通过运行所述计算机程序，从而使终端设备执行上述的多个imu时间同步方法或者上述的多个imu时间同步装置中的各个模块的功能。

本发明还提供了一种计算机可读存储介质，用于储存上述终端中使用的所述计算机程序。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。