CN111854793B - 惯性测量单元和全球导航系统之间杆臂的标定方法和装置 - Google Patents

Abstract

本发明实施例公开了一种惯性测量单元和全球导航系统之间杆臂的标定方法和装置，该方法包括：在GNSS的采样时刻，将IMU的运动状态信息作为预设滑动窗口中待优化的关键帧中的数据；利用关键帧中的数据，以及在相邻关键帧之间所获取的IMU的多帧采样数据的增量，构建相邻关键帧之间包含有IMU运动状态增量残差的量测方程，量测方程中还包括待优化的IMU和GNSS之间的杆臂值和IMU的安装角；基于采样得到的GNSS数据，并根据基于预设滑动窗口的优化算法，以及相邻关键帧之间的约束关系，对量测方程中待优化的数据进行优化处理，得到满足预设收敛条件的目标杆臂值和安装角。通过采用上述技术方案，提升了杆臂的测量精度。

Description

惯性测量单元和全球导航系统之间杆臂的标定方法和装置

技术领域

本发明涉及自动驾驶技术领域，具体涉及一种惯性测量单元和全球导航系统之间杆臂的标定方法和装置。

背景技术

惯性测量单元(Inertial measurement unit，IMU)可以在不依赖外部条件，依靠自身提供高频的角速度和加速度输出，从而为获取高频位置、速度和姿态提供了可能(位置、速度和姿态统称POSE)。IMU放置于汽车某处，可以获得被安装处附近的角速度和加速度量测值，汽车一般是刚性连接，因此车体各处的角速度一般认为是一致的，但线速度则不一定相同，单纯依靠IMU并不能获得汽车的绝对位置、速度和姿态，仅能获得某个时间间隔内的相对 POS增量。全球导航卫星系统(Global Navigation Satellite System，GNSS)中已经比较成熟的载波相位差分技术(Real-time kinematic，RTK)，作为另一种传感器可以提供精确且绝对的位置和速度。因此，很自然的想法就是将二者进行融合，从而提供出高频的绝对POSE输出。通常情况下，GNSS需要接收外部的卫星信号，GNSS传感器的天线一般外置于载体上方，尽可能减少载体自身对信号的遮挡，从而提供天线的位置和速度信息。因此，GNSS天线和 IMU并不一定放置在同一位置，那么两个传感器的位置和速度也不相同。一般情况下，用户想获得的是IMU的POSE信息，那么就必须知晓IMU和GNSS之间的三维距离，也就是我们所说的杆臂，从而将GNSS位置和速度补偿至IMU处。由于RTK可以提供厘米级精度的位置，因此杆臂的精度至少也应该是厘米级。

对于刚体连接的物体，利用软尺等工具直接量测杆臂是最常用也是最简单的方案。但上述方案有一定问题。以汽车为例子，IMU通常装在汽车内部，GNSS天线通常位于汽车顶部。首先，从结构上不方便量测；其次，当IMU的安装和车体底盘所代表车体坐标系有较大安装角度存在时，很难量测准确。通常的解决方案是：不考虑安装角，通过多次量测尽可能减少结构不便引起的误差，且要求后续使用杆臂时，要对安装角进行估计。否则，如果使用的是高精度GNSS板卡，或者安装误差角度较大且不估计时，将额外引入的误差。

发明内容

本发明实施例公开一种惯性测量单元和全球导航系统之间杆臂的标定方法和装置，提升了杆臂的测量精度。

第一方面，本发明实施例公开了一种惯性测量单元和全球导航系统之间杆臂的标定方法，该方法包括：

在全球导航卫星系统GNSS的采样时刻，将惯性测量单元IMU的运动状态信息作为预设滑动窗口中待优化的关键帧中的数据，所述运动状态信息包括位置、姿态和速度；

利用所述关键帧中的数据，以及在相邻关键帧之间所获取的IMU的多帧采样数据的增量，构建相邻关键帧之间包含有IMU运动状态增量残差的量测方程，所述量测方程中还包括待优化的IMU和GNSS之间的杆臂值和IMU在车体坐标系下的安装角；

基于采样得到的GNSS数据，并根据基于所述预设滑动窗口的优化算法，以及相邻关键帧之间的约束关系，对所述量测方程中待优化的数据进行优化处理，得到满足预设收敛条件的目标杆臂值和安装角。

可选的，所述相邻关键帧之间的约束关系通过如下方式建立：

将所述量测方程对所述预设滑动窗口中第一个关键帧中IMU的运动状态信息，以及除第一个关键帧之外的其他关键帧中IMU的运动状态信息进行分别求导，分别将得到的雅克比矩阵作为所述第一个关键帧中IMU的运动状态信息和所述其他关键帧中IMU的运动状态信息对应的系数矩阵；

根据所述系数矩阵及其对应关键帧中IMU的运动状态信息，以及所述IMU运动状态增量残差，建立观测方程；

在所述预设滑动窗口进行滑动的过程中，当接收到新的GNSS采样数据时，在所述观测方程中，利用消元法消除所述预设滑动窗口中的第一个关键帧中IMU的运动状态信息，得到相邻关键帧之间的约束关系。

可选的，根据所述系数矩阵及其对应关键帧中IMU的运动状态信息，以及所述IMU运动状态增量残差，建立观测方程，包括：

按照如下公式建立观测方程：

其中，x₁为预设滑动窗口中第一个关键帧中IMU的运动状态信息；x₂为预设滑动窗口中所述其他关键帧中IMU的运动状态信息；A为所述量测方程对x₁求导得到的雅克比矩阵；B 为所述量测方程对x₂求导得到的雅克比矩阵；C为所述量测方程对x₁求导得到的雅克比矩阵；和D为所述量测方程对x₂求导得到的雅克比矩阵；a为第一个关键帧中IMU的运动状态增量残差；b为所述其他关键帧中IMU的运动状态信息增量残差。

可选的，基于采样得到的GNSS数据，并根据基于所述预设滑动窗口的优化算法，以及相邻关键帧之间的约束关系，对所述量测方程中待优化的数据进行优化处理，包括：

所述预设滑动窗口中的各个关键帧采用先进先出的方式，当每采样得到新的一帧GNSS 数据时，根据所述相邻关键帧之间的约束关系，得到包含有IMU运动状态信息的其他关键帧，所述其他关键帧为所述预设滑动窗口中除第一个关键帧之外的其他关键帧；

将所述其他关键帧，以及新的一帧GNSS数据到来时所确定的关键帧作为预设滑动窗口中新的关键帧；

利用所述新的关键帧更新所述量测方程，并采用非线性优化算法对更新后的量测方程中的杆臂值和安装角进行优化。

可选的，对所述量测方程中待优化的数据进行优化处理，得到满足预设收敛条件的目标杆臂值，包括：

采用迭代方式进行，对所述量测方程中待优化的数据进行优化，将每次迭代得到的杆臂值作为下次迭代的杆臂初值，直到相邻两次迭代所得到的杆臂值之间的误差小于预设长度阈值时，将相邻两次迭代过程中最后一次迭代所得到的杆臂值作为目标杆臂值。

可选的，在第一次迭代过程中，利用采集的GNSS的位置和速度对所述IMU的位置和速度进行初始化，并根据所述IMU所采集的加速度和角速度，得到所述IMU运动状态信息的初值；

利用所述IMU的运动状态信息的初值以及在相邻关键帧之间所获取的IMU的多帧采样数据的增量，构建相邻关键帧之间包含有IMU运动状态增量残差和杆臂初值的量测方程。

可选的，在全球导航卫星系统GNSS的采样时刻，将惯性测量单元IMU的运动状态信息作为预设滑动窗口中待优化的关键帧中的数据之前，所述方法还包括：

在车体进行绕八字运动或进行转弯时，获取所述GNSS采集的速度和位置，以及所述IMU 采集的角速度和加速度，以开始第一次迭代过程。

第二方面，本发明实施例还提供了一种惯性测量单元和全球导航系统之间杆臂的标定装置，该装置包括：

关键帧确定模块，被配置为在全球导航卫星系统GNSS的采样时刻，将惯性测量单元IMU 的运动状态信息作为预设滑动窗口中待优化的关键帧中的数据，所述运动状态信息包括位置、姿态和速度；

量测方程构建模块，被配置为利用所述关键帧中的数据，以及在相邻关键帧之间所获取的IMU的多帧采样数据的增量，构建相邻关键帧之间包含有IMU运动状态增量残差的量测方程，所述量测方程中还包括待优化的IMU和GNSS之间的杆臂值和IMU在车体坐标系下的安装角；

优化模块，被配置为基于采样得到的GNSS数据，并根据基于所述预设滑动窗口的优化算法，以及相邻关键帧之间的约束关系，对所述量测方程中待优化的数据进行优化处理，得到满足预设收敛条件的目标杆臂值和安装角。

可选的，所述相邻关键帧之间的约束关系通过如下方式建立：

系数矩阵确定模块，被配置为将所述量测方程对所述预设滑动窗口中第一个关键帧中 IMU的运动状态信息，以及除第一个关键帧之外的其他关键帧中IMU的运动状态信息进行分别求导，分别将得到的雅克比矩阵作为所述第一个关键帧中IMU的运动状态信息和所述其他关键帧中IMU的运动状态信息对应的系数矩阵；

观测方程建立模块，被配置为根据所述系数矩阵及其对应关键帧中IMU的运动状态信息，以及所述IMU运动状态增量残差，建立观测方程；

约束关系获取模块，被配置为在所述预设滑动窗口进行滑动的过程中，当接收到新的 GNSS采样数据时，在所述观测方程中，利用消元法消除所述预设滑动窗口中的第一个关键帧中IMU的运动状态信息，得到相邻关键帧之间的约束关系。

可选的，所述观测方程建立模块，具体被配置为：

按照如下公式建立观测方程：

其中，x₁为预设滑动窗口中第一个关键帧中IMU的运动状态信息；x₂为预设滑动窗口中所述其他关键帧中IMU的运动状态信息；A为所述量测方程对x₁求导得到的雅克比矩阵；B 为所述量测方程对x₂求导得到的雅克比矩阵；C为所述量测方程对x₁求导得到的雅克比矩阵；和D为所述量测方程对x₂求导得到的雅克比矩阵；a为第一个关键帧中IMU的运动状态增量残差；b为所述其他关键帧中IMU的运动状态信息增量残差。

可选的，所述优化模块，具体被配置为：

所述预设滑动窗口中的各个关键帧采用先进先出的方式，当每采样得到新的一帧GNSS 数据时，根据所述相邻关键帧之间的约束关系，得到包含有IMU运动状态信息的其他关键帧，所述其他关键帧为所述预设滑动窗口中除第一个关键帧之外的其他关键帧；

将所述其他关键帧，以及新的一帧GNSS数据到来时所确定的关键帧作为预设滑动窗口中新的关键帧；

利用所述新的关键帧更新所述量测方程，并采用非线性优化算法对更新后的量测方程中的杆臂值和安装角进行优化，得到满足预设收敛条件的目标杆臂值和安装角。

可选的，所述优化模块，具体被配置为：

基于采样得到的GNSS数据，并根据基于所述预设滑动窗口的优化算法，以及相邻关键帧之间的约束关系，采用迭代方式进行，对所述量测方程中待优化的数据进行优化，将每次迭代得到的杆臂值作为下次迭代的杆臂初值，直到相邻两次迭代所得到的杆臂值之间的误差小于预设长度阈值时，将相邻两次迭代过程中最后一次迭代所得到的杆臂值作为目标杆臂值。

可选的，在第一次迭代过程中，利用采集的GNSS的位置和速度对所述IMU的位置和速度进行初始化，并根据所述IMU所采集的加速度和角速度，得到所述IMU运动状态信息的初值；

利用所述IMU的运动状态信息的初值以及在相邻关键帧之间所获取的IMU的多帧采样数据的增量，构建相邻关键帧之间包含有IMU运动状态增量残差和杆臂初值的量测方程。

可选的，所述装置还包括：

在车体进行绕八字运动或进行转弯时，获取所述GNSS采集的速度和位置，以及所述IMU 采集的角速度和加速度，以开始第一次迭代过程。

第三方面，本发明实施例还提供了一种车载终端，包括：

存储有可执行程序代码的存储器；

与所述存储器耦合的处理器；

所述处理器调用所述存储器中存储的所述可执行程序代码，执行本发明任意实施例所提供的惯性测量单元和全球导航系统之间杆臂的标定方法的部分或全部步骤。

第四方面，本发明实施例还提供了一种计算机可读存储介质，其存储计算机程序，所述计算机程序包括用于执行本发明任意实施例所提供的惯性测量单元和全球导航系统之间杆臂的标定方法的部分或全部步骤的指令。

第五方面，本发明实施例还提供了一种计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行本发明任意实施例所提供的惯性测量单元和全球导航系统之间杆臂的标定方法的部分或全部步骤。

本实施例提供的技术方案，利用前后关键帧构建了量测方程。在对量测方程的各个待优化变量进行优化的过程中，采用了基于预设滑动窗口的优化算法。该算法的优化过程以多个关键帧数据之间的约束关系作为先验约束条件，保证了滑窗过程的顺利进行，同时在滑窗过程中也未丢失任何关键帧信息。相对于现有技术中采用卡尔曼滤波或采用直接测量以对单帧数据进行的优化方式，本实施例提供的技术方案可得到更加精准的杆臂值。

本发明的发明点包括：

1、通过利用关键帧数据构建相邻关键帧之间的量测方程，并将相邻关键帧之间的约束关系作为先验约束条件，采用基于预设滑动窗口的优化算法对量测方程中各个待优化的变量进行优化，可得到满足预设收敛条件的目标杆臂值和安装角。解决了现有技术中只对单帧数据优化并不考虑相邻关键帧数据间的约束而导致的杆臂标定精度低的问题，有效提升了杆臂的标定精度，是本发明的发明点之一。

2、本发明实施例的技术方案在优化杆臂的同时，也优化了IMU的安装角，这样在后续使用杆臂时可以视情况不再考虑安装误差角度的影响，解决了现有技术中由于IMU的安装误差而导致的对杆臂精度测量的影响，是本发明的发明点之一。

3、通过采用迭代优化的方式，对量测方程中待优化的数据进行优化，将每次迭代得到的杆臂值作为下次迭代的杆臂初值，直到相邻两次迭代所得到的杆臂值之间的误差小于预设长度阈值时，将相邻两次迭代过程中最后一次迭代所得到的杆臂值作为目标杆臂值，解决了现有技术中杆臂标定精度低的问题，提升了杆臂的标定精度，是本发明的发明点之一。

4、构建观测方程，并将量测方程对第一个关键帧中IMU的POSE与对其他关键帧中IMU 的POSE求导后得到的矩阵，作为量测方程中对应关键帧的系数矩阵。在该观测方程中，通过采用消元的方式，在保留相邻关键帧间约束关系的前提下，消除了第一个关键帧，保证了滑窗的顺利进行，解决了由于直接消除变量而丢失关键帧间约束信息的问题，有助于提升窗口滑动过程中杆臂标定的精度，是本发明的发明点之一。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的一种惯性测量单元和全球导航系统之间杆臂的标定方法的流程示意图；

图2是本发明实施例提供的一种相邻关键帧之间的约束关系建立方法的流程示意图；

图3是本发明实施例提供的一种一种惯性测量单元和全球导航系统之间杆臂的标定装置的结构示意图；

图4是本发明实施例提供的一种车载终端的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，本发明实施例及附图中的术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。

实施例一

请参阅图1，图1是本发明实施例提供的一种惯性测量单元和全球导航系统之间杆臂的标定方法的流程示意图。该方法应用于自动驾驶中，可由惯性测量单元和全球导航系统之间杆臂的标定装置来执行，该装置可通过软件和/或硬件的方式实现，一般可集成在车载电脑、车载工业控制计算机(Industrial personal Computer，IPC)等车载终端中，本发明实施例不做限定。如图1所示，本实施例提供的方法具体包括：

110、在全球导航卫星系统GNSS的采样时刻，将惯性测量单元IMU的运动状态信息作为预设滑动窗口中待优化的关键帧中的数据。

其中，GNSS(Global Navigation Satellite System，全球导航卫星系统)泛指所有的卫星导航系统，包括美国的GPS、俄罗斯的Glonass、欧洲的Galileo和中国的北斗卫星导航系统。

本实施例中，在杆臂的标定过程中，为了建立标定过程中前后两帧数据之间的关系，采用了基于预设滑动窗口的优化算法。该预设滑动窗口的大小可根据实际需求进行设置。本实施例对其不作具体限定。在预设滑动窗口中的每个元素均称为关键帧，该关键帧即为待优化的变量之一。本实施例中，可选取GNSS采样时刻所获取的IMU的运动状态信息作为预设滑动窗口中关键帧中的数据。其中，运动状态信息包括位置、姿态和速度(后续将位置、速度和姿态统称为POSE)。如果将预设滑动窗口的大小设置为10帧，则该窗口中则包含10个待优化的POSE。除此之外，待优化变量还包括IMU和GNSS之间的杆臂值和IMU在车体坐标系下的安装角。上述待优化的变量均存在对应的初值。

本领域技术人员可以理解的是，预设滑动窗口的滑动遵循先进先出的原则，每次从窗口的尾部添加一帧数据，头部删除一帧数据，以保证窗口的大小不变。随着预设滑动窗口的移动，对窗口中关键帧中的数据进行更新，从而对上述变量进行优化。

120、利用关键帧中的数据，以及在相邻关键帧之间所获取的IMU的多帧采样数据的增量，构建相邻关键帧之间包含有IMU运动状态增量残差的量测方程。

本实施例中，由于关键帧是以GNSS的采样频率选取的，而IMU的采样频率高于GNSS的采样频率，因此在相邻关键帧之间会采集到多帧IMU的采样数据。本实施例中，利用相邻关键帧之间所获取的IMU的多帧采样数据的增量，以及关键帧中的数据，可构建相邻关键帧之间包含有IMU运动状态增量残差的量测方程。其中，IMU的多帧采样数据的增量可通过对该多帧采样数据进行积分后得到。

示例性的，在该量测方程中，IMU运动状态增量残差可利用关键帧中IMU的运动状态信息，与IMU的多帧采样数据积分后得到的运动状态信息相减来表示。具体的，量测方程可表示如下：

其中，

表示相邻关键帧之间的IMU的位置增量；

表示相邻关键帧之间之间位置增量残差；

表示相邻关键帧之间的速度增量，

相邻关键帧之间的速度增量误差；

表示相邻关键帧之间的姿态增量，

表示相邻关键帧之间位置增量误差；

表示 IMU坐标系到世界坐标系的旋转矩阵；

和

分别表示IMU在第k+1关键帧和第k关键帧中在世界坐标系中的位置；

和

分别表示IMU在第k+1关键帧和第k关键帧中在世界坐标系中的速度；

和

分别表示IMU在第k+1关键帧和第k关键帧中的姿态；

和

分别表示IMU在第k+1关键帧和第k关键帧的加速度计零偏；

和

分别表示 IMU第k+1关键帧和第k关键帧的陀螺零偏；g^w表示世界坐标系下IMU的重力(无需优化)；Δt_k表示相连关键帧之间的时间间隔(无需优化)。

表示第k关键帧中GNSS在世界坐标系中的位置(无需优化)；

表示第k关键帧中从世界坐标系到IMU坐标系的旋转矩阵(无需优化)；

表示IMU坐标系到车体系的旋转角度，即IMU在车体坐标系下的安装角；

表示车体系下的IMU的陀螺仪采集的角速度(无需优化)；l^v表示车体系下，GNSS到IMU的杆臂；

表示第k关键帧中IMU在车体坐标系下的位置残差；

表示第k关键帧中IMU的速度残差。

在上述量测方程的各个参数中，除了已注明无需优化的变量，其他均为有初值且需要优化的变量。利用各待优化量的初值即可构建出上述量测方程，从而可开始后续的迭代优化过程。

具体的，在初始状态，即在第一次优化之前，可利用IMU的运动状态信息的初值以及在相邻关键帧之间所获取的IMU的多帧采样数据的增量，构建相邻关键帧之间包含有IMU运动状态增量残差和杆臂初值的量测方程。其中，量测方程中杆臂的初值可以设置为0，IMU运动状态信息的初值可利用GNSS的位置和速度对IMU的位置和速度进行初始化，并根据IMU所采集的加速度和角速度来确定。

130、基于采样得到的GNSS数据，并根据基于预设滑动窗口的优化算法，以及相邻关键帧之间的约束关系，对量测方程中待优化的数据进行优化处理，得到满足预设收敛条件的目标杆臂值和安装角。

本实施例中，采用了基于预设滑动窗口的优化算法。预设滑动窗口的每次滑动均会对该窗口中IMU的运动状态信息进行更新，从而也会使得量测方程中的各个待优化的变量随之得到更新。

具体的，在预设滑动窗口中的各个关键帧均遵循先进先出的原则。在预设滑动窗口滑动的过程中，由于关键帧的填充是按照GNSS的采样频率进行的，因此，为了保证滑窗的顺利进行，每当采集到新的一帧GNSS数据时，需删除预设滑动窗中的第一个关键帧，并将除第一个关键帧之外剩余的关键帧，以及新的一帧GNSS数据到来时所确定的关键帧作为预设滑动窗口中新的关键帧，这样预设滑动窗口中的关键帧数目将保持不变。

需要说明的是，由于本实施例构建了相邻关键帧之间的约束关系，该约束关系的存在并不会随着第一个关键帧数据的丢失而删除。当新的一帧GNSS数据到来时，可基于该约束关系确定出除第一个关键帧之外的其他关键帧。因此，通过采用基于滑动窗口的优化算法，随着预设滑动窗口头部关键帧的滑出，以及尾部新的关键帧的进入，关键帧中数据会不断更新。同时，由于量测方程是由关键帧中的数据构建的，量测方程也会不断得到更新，即量测方程中各个待优化的变量均会随着预设滑动窗口的滑动而得到更新。

示例性的，基于相邻两个关键帧之间的约束关系，在预设滑动窗口滑动的过程中，可通过采用如高斯牛顿或者LM(列文伯格-马夸尔特)等非线性优化算法优化对预设滑动窗口中除第一个关键帧之外其他关键帧数据中IMU的运动状态信息，以及量测方程中的杆臂值和安装角进行优化处理。上述优化过程可采用迭代优化的方式进行，例如可将每次迭代得到的杆臂值作为下次迭代的杆臂初值，直到相邻两次迭代所得到的杆臂值之间的误差小于预设长度阈值时，即满足预设收敛条件时，将相邻两次迭代过程中最后一次迭代所得到的杆臂值作为目标杆臂值。

具体的，在进行迭代之前，还可设置迭代次数，例如10次。每次迭代可以1200帧的GNSS 数据作为基础，即滑动窗口从头到尾将这1200帧GNSS数据全部解算完，表示一次迭代结束。在这次迭代过程中，预设滑动窗口每滑动一次，IMU的运动状态信息均会得到依次更新，并且也会更新出一个杆臂值，解算完1200帧GNSS数据可获得1200个杆臂值。将该次迭代过程最后得出的杆臂值作为该次迭代的最优解，并将该最优解作为下次迭代的初值。在10次迭代过程中，如果有两次迭代过程得到的杆臂最优解之间的误差小于预设长度阈值，例如5厘米，表示迭代过程结果。

需要说明的是，为了保证杆臂与其他待优化变量之间的相互影响，可在数据采集的过程中保证有足够的激励。具体可在车体绕八字运动或进行转弯时，获取GNSS采集的速度和位置，以及IMU采集的角速度和加速度，以使IMU的天向角速度可以获得较为充足的激励，避免各变量在优化过程中相互影响而导致优化精度降低。

本实施例提供的技术方案，利用前后关键帧构建量测方程。在对量测方程的各个待优化变量进行优化的过程中，采用了基于预设滑动窗口的优化算法。该算法的优化过程以多个关键帧数据之间的约束关系作为先验约束条件，保证了滑窗过程的顺利进行，同时在滑窗过程中也未丢失任何关键帧信息。相对于现有技术中对单帧数据的优化方式，本实施例提供的技术方案可得到更加精确度的杆臂值。此外，本实施例的技术方案同时优化了IMU的安装角，这样在后续使用杆臂时可以视情况不再考虑安装误差角度的影响。

本实施例中，相邻关键帧之间约束关系的存在对滑动过程的顺利进行，以及杆臂标定的精度起到至关重要的作用，下面将对该约束关系的建立进行详细说明：

请参阅图2，图2是本发明实施例提供的一种相邻关键帧之间的约束关系建立方法的流程示意图。本实施例在上述实施例的基础上进行了优化，通过对量测方程求导得到系数矩阵，利用系数矩阵和对应关键帧中IMU的POSE和运动状态增量残差来建立观测方程。如图2所示，该方法包括：

210、将量测方程对预设滑动窗口中第一个关键帧中IMU的运动状态信息，以及除第一个关键帧之外的其他关键帧中IMU的运动状态信息进行分别求导，分别将得到的雅克比矩阵作为第一个关键帧中IMU的运动状态信息和其他关键帧中IMU的运动状态信息对应的系数矩阵。

220、根据系数矩阵及其对应关键帧中IMU的运动状态信息，以及IMU运动状态增量残差，建立观测方程。

具体的，在得到系数矩阵后，可按照如下公式建立观测方程：

其中，x₁为预设滑动窗口中第一个关键帧中IMU的运动状态信息；x₂为预设滑动窗口中除第一个关键帧之外其他关键帧中IMU的运动状态信息；A为量测方程对x₁求导得到的雅克比矩阵；B为量测方程对x₂求导得到的雅克比矩阵；C为量测方程对x₁求导得到的雅克比矩阵；和D为量测方程对x₂求导得到的雅克比矩阵；a为第一个关键帧中IMU的运动状态增量残差；b为除一个关键帧之外其他关键帧中IMU的运动状态信息增量残差。

230、在预设滑动窗口进行滑动的过程中，当接收到新的GNSS采样数据时，在观测方程中，利用消元法消除预设滑动窗口中的第一个关键帧中IMU的运动状态信息，得到相邻关键帧之间的约束关系。

本实施例中，预设滑动窗口的滑动过程遵循先进先出的原则，由于预设滑动窗口的大小保持不变，因此，当有新的一帧GNSS采样数据到来时，需从原预设滑动窗口的头部删除最先进入的第一个关键帧，即上述公式中的x₁。但如果直接丢弃x₁，会导致相邻关键帧之间的约束关系也丢失。因此，本实施例采用了消元的方式，即消掉了上述观测方程中的变量x₁，同时也可以保留相邻关键帧之间的约束关系。具体消元后的方程如下：

(B-AC^-1D)x₂＝a-AC^-1b

该方程可作为先验约束条件，即相邻关键帧之间的约束关系，既保证了窗口滑动的顺利进行，同时又不丢失关键帧的测量信息。

作为一种可选的实施方式，上述约束关系建立之后，本实施例中，基于采样得到的GNSS 数据，并根据基于滑动窗口的优化算法以及相邻关键帧之间的约束关系，对量测方程中待优化的数据进行优化处理，可包括：

预设滑动窗口中的各个关键帧采用先进先出的方式，当每采样得到新的一帧GNSS数据时，根据相邻关键帧之间的约束关系，得到包含有IMU运动状态信息的其他关键帧；

将除了第一个关键帧之外的其他关键帧，以及新的一帧GNSS数据到来时所确定的关键帧作为预设滑动窗口中新的关键帧；

利用新的关键帧更新量测方程，并采用非线性优化算法对更新后的量测方程中的杆臂值和安装角进行优化。

具体的，在量测方程建立之后，可通过求导得到系数矩阵A、B、C和D。当新的一帧GNSS数据到来时，为了保证预设滑动窗口的大小保持不变，随着窗口的滑动，需将预设滑动窗口中的第一个关键帧删除，即删除上述方程中的x₁，然后再补入一个关键帧。由于上述消元后的方程既消除了x₁，同时也保留了相邻关键帧之间的约束关系，因此，通过对上述消元后的方程求解，可得到预设滑动窗口中除了第一个关键帧之外其他关键帧中IMU的运动状态信息，即x₂。由于删除了第一个关键帧x₁，在新的一帧GNSS数据到来时，还需再确定一个关键帧补入预设滑动窗口，这样得出的x₂和新补入的关键帧可作为预设滑动窗口新的关键帧。每当有新的一帧GNSS数据到来时，通过执行上述迭代过程中，并结合非线性优化算法，可使得x₂得到更新，从而由关键帧所构建的量测方程也会得到更新，通过采用非线性优化算法可对更新后的量测方程中的杆臂值和安装角进行优化。

本实施例的技术方案，通过将量测方程对第一个关键帧中的POSE和其他关键帧中的 POSE分别求导得到系数矩阵，并根据对应关键帧中IMU的POSE以及IMU运动状态增量残差，可建立观测方程。在该观测方程中，通过采用消元的方式，可在保留约束关系的前提下，消除第一个关键帧，从而可保证滑窗的顺利进行，以提升杆臂标定的精度。

实施例三

请参阅图3，图3是本发明实施例提供的一种一种惯性测量单元和全球导航系统之间杆臂的标定装置的结构示意图。如图3所示，该装置包括：关键帧确定模块310、量测方程构建模块320和优化模块330；其中，

关键帧确定模块310，被配置为在全球导航卫星系统GNSS的采样时刻，将惯性测量单元IMU的运动状态信息作为预设滑动窗口中待优化的关键帧中的数据，所述运动状态信息包括位置、姿态和速度；

量测方程构建模块320，被配置为利用所述关键帧中的数据，以及在相邻关键帧之间所获取的IMU的多帧采样数据的增量，构建相邻关键帧之间包含有IMU运动状态增量残差的量测方程，所述量测方程中还包括待优化的IMU和GNSS之间的杆臂值和IMU在车体坐标系下的安装角；

优化模块330，被配置为基于采样得到的GNSS数据，并根据基于所述预设滑动窗口的优化算法，以及相邻关键帧之间的约束关系，对所述量测方程中待优化的数据进行优化处理，得到满足预设收敛条件的目标杆臂值和安装角。

本实施例提供的技术方案，利用前后关键帧构建量测方程。在对量测方程的各个待优化变量进行优化的过程中，采用了基于预设滑动窗口的优化算法。该算法的优化过程以多个关键帧数据之间的约束关系作为先验约束条件，保证了滑窗过程的顺利进行，同时在滑窗过程中也未丢失任何关键帧信息。相对于现有技术中对单帧数据的优化方式，本实施例提供的技术方案可得到更加精确度的杆臂值。此外，本实施例的技术方案同时优化了IMU的安装角，这样在后续使用杆臂时可以视情况不再考虑安装误差角度的影响。

可选的，所述相邻关键帧之间的约束关系通过如下方式建立：

系数矩阵确定模块，被配置为将所述量测方程对所述预设滑动窗口中第一个关键帧中 IMU的运动状态信息，以及除第一个关键帧之外的其他关键帧中IMU的运动状态信息进行分别求导，分别将得到的雅克比矩阵作为所述第一个关键帧中IMU的运动状态信息和所述其他关键帧中IMU的运动状态信息对应的系数矩阵；

观测方程建立模块，被配置为根据所述系数矩阵及其对应关键帧中IMU的运动状态信息，以及所述IMU运动状态增量残差，建立观测方程；

约束关系获取模块，被配置为在所述预设滑动窗口进行滑动的过程中，当接收到新的 GNSS采样数据时，在所述观测方程中，利用消元法消除所述预设滑动窗口中的第一个关键帧中IMU的运动状态信息，得到相邻关键帧之间的约束关系。

可选的，所述观测方程建立模块，具体被配置为：

按照如下公式建立观测方程：

其中，x₁为预设滑动窗口中第一个关键帧中IMU的运动状态信息；x₂为预设滑动窗口中所述其他关键帧中IMU的运动状态信息；A为所述量测方程对x₁求导得到的雅克比矩阵；B 为所述量测方程对x₂求导得到的雅克比矩阵；C为所述量测方程对x₁求导得到的雅克比矩阵；和D为所述量测方程对x₂求导得到的雅克比矩阵；a为第一个关键帧中IMU的运动状态增量残差；b为所述其他关键帧中IMU的运动状态信息增量残差。

可选的，所述优化模块，具体被配置为：

所述预设滑动窗口中的各个关键帧采用先进先出的方式，当每采样得到新的一帧GNSS 数据时，根据所述相邻关键帧之间的约束关系，得到包含有IMU运动状态信息的其他关键帧，所述其他关键帧为所述预设滑动窗口中除第一个关键帧之外的其他关键帧；

将所述其他关键帧，以及新的一帧GNSS数据到来时所确定的关键帧作为预设滑动窗口中新的关键帧；

利用所述新的关键帧更新所述量测方程，并采用非线性优化算法对更新后的量测方程中的杆臂值和安装角进行优化，得到满足预设收敛条件的目标杆臂值和安装角。

可选的，所述优化模块，具体被配置为：

基于采样得到的GNSS数据，并根据基于所述预设滑动窗口的优化算法，以及相邻关键帧之间的约束关系，采用迭代方式进行，对所述量测方程中待优化的数据进行优化，将每次迭代得到的杆臂值作为下次迭代的杆臂初值，直到相邻两次迭代所得到的杆臂值之间的误差小于预设长度阈值时，将相邻两次迭代过程中最后一次迭代所得到的杆臂值作为目标杆臂值。

可选的，在第一次迭代过程中，利用采集的GNSS的位置和速度对所述IMU的位置和速度进行初始化，并根据所述IMU所采集的加速度和角速度，得到所述IMU运动状态信息的初值；

利用所述IMU的运动状态信息的初值以及在相邻关键帧之间所获取的IMU的多帧采样数据的增量，构建相邻关键帧之间包含有IMU运动状态增量残差和杆臂初值的量测方程。

可选的，所述装置还包括：

在车体进行绕八字运动或进行转弯时，获取所述GNSS采集的速度和位置，以及所述IMU 采集的角速度和加速度，以开始第一次迭代过程。

本发明实施例所提供的惯性测量单元和全球导航系统之间杆臂的标定装置可执行本发明任意实施例所提供的惯性测量单元和全球导航系统之间杆臂的标定方法，具备执行方法相应的功能模块和有益效果。未在上述实施例中详尽描述的技术细节，可参见本发明任意实施例所提供的惯性测量单元和全球导航系统之间杆臂的标定方法。

实施例四

请参阅图4，图4是本发明实施例提供的一种车载终端的结构示意图。如图4所示，该车载终端可以包括：

存储有可执行程序代码的存储器701；

与存储器701耦合的处理器702；

其中，处理器702调用存储器701中存储的可执行程序代码，执行本发明任意实施例所提供的惯性测量单元和全球导航系统之间杆臂的标定方法。

本发明实施例公开一种计算机可读存储介质，其存储计算机程序，其中，该计算机程序使得计算机执行本发明任意实施例所提供的惯性测量单元和全球导航系统之间杆臂的标定方法。

本发明实施例公开一种计算机程序产品，其中，当计算机程序产品在计算机上运行时，使得计算机执行本发明任意实施例所提供的惯性测量单元和全球导航系统之间杆臂的标定方法的部分或全部步骤。

在本发明的各种实施例中，应理解，上述各过程的序号的大小并不意味着执行顺序的必然先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。

在本发明所提供的实施例中，应理解，“与A相应的B”表示B与A相关联，根据A可以确定B。但还应理解，根据A确定B并不意味着仅仅根据A确定B，还可以根据A和/或其他信息确定B。

另外，在本发明各实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

上述集成的单元若以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可获取的存储器中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或者部分，可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储器中，包括若干请求用以使得一台计算机设备(可以为个人计算机、服务器或者网络设备等，具体可以是计算机设备中的处理器)执行本发明的各个实施例上述方法的部分或全部步骤。

本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质包括只读存储器(Read-Only Memory，ROM)、随机存储器(Random Access Memory，RAM)、可编程只读存储器(Programmable Read-only Memory，PROM)、可擦除可编程只读存储器 (Erasable Programmable Read Only Memory，EPROM)、一次可编程只读存储器(One-time Programmable Read-Only Memory，OTPROM)、电子抹除式可复写只读存储器(Electrically-Erasable Programmable Read-Only Memory，EEPROM)、只读光盘(CompactDisc Read-Only Memory，CD-ROM)或其他光盘存储器、磁盘存储器、磁带存储器、或者能够用于携带或存储数据的计算机可读的任何其他介质。

以上对本发明实施例公开的一种惯性测量单元和全球导航系统之间杆臂的标定方法和装置进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种惯性测量单元和全球导航系统之间杆臂的标定方法，其特征在于，包括：

在GNSS的采样时刻，将IMU的运动状态信息作为预设滑动窗口中待优化的关键帧中的数据，所述运动状态信息包括位置、姿态和速度；

利用所述关键帧中的数据，以及在相邻关键帧之间所获取的IMU的多帧采样数据的增量，构建相邻关键帧之间包含有IMU运动状态增量残差的量测方程，所述量测方程中还包括待优化的IMU和GNSS之间的杆臂值和IMU在车体坐标系下的安装角；

基于采样得到的GNSS数据，并根据基于预设滑动窗口的优化算法，以及相邻关键帧之间的约束关系，对所述量测方程中待优化的数据进行优化处理，得到满足预设收敛条件的目标杆臂值和安装角；其中，所述约束关系用于在新的一帧GNSS数据到来时，确定除第一个关键帧之外的其他关键帧，以形成预设滑动窗口中新的关键帧。

2.根据权利要求1所述的方法，其特征在于，所述相邻关键帧之间的约束关系通过如下方式建立：

将所述量测方程对所述预设滑动窗口中第一个关键帧中IMU的运动状态信息，以及除第一个关键帧之外的其他关键帧中IMU的运动状态信息进行分别求导，分别将得到的雅克比矩阵作为所述第一个关键帧中IMU的运动状态信息和所述其他关键帧中IMU的运动状态信息对应的系数矩阵；

根据所述系数矩阵及其对应关键帧中IMU的运动状态信息，以及所述IMU运动状态增量残差，建立观测方程；

在所述预设滑动窗口进行滑动的过程中，当接收到新的GNSS采样数据时，在所述观测方程中，利用消元法消除所述预设滑动窗口中的第一个关键帧中IMU的运动状态信息，得到相邻关键帧之间的约束关系。

3.根据权利要求2所述的方法，其特征在于，根据所述系数矩阵及其对应关键帧中IMU的运动状态信息，以及所述IMU运动状态增量残差，建立观测方程，包括：

按照如下公式建立观测方程：

其中，x₁为预设滑动窗口中第一个关键帧中IMU的运动状态信息；x₂为预设滑动窗口中所述其他关键帧中IMU的运动状态信息；A为所述量测方程对x₁求导得到的雅克比矩阵；B为所述量测方程对x₂求导得到的雅克比矩阵；C为所述量测方程对x₁求导得到的雅克比矩阵；和D为所述量测方程对x₂求导得到的雅克比矩阵；a为第一个关键帧中IMU的运动状态增量残差；b为其他关键帧中IMU的运动状态信息增量残差。

4.根据权利要求2或3所述的方法，其特征在于，基于采样得到的GNSS数据，并根据基于预设滑动窗口的优化算法，以及相邻关键帧之间的约束关系，对所述量测方程中待优化的数据进行优化处理，包括：

所述预设滑动窗口中的各个关键帧采用先进先出的方式，当每采样得到新的一帧GNSS数据时，根据所述相邻关键帧之间的约束关系，得到包含有IMU运动状态信息的其他关键帧，所述其他关键帧为所述预设滑动窗口中除第一个关键帧之外的其他关键帧；

将所述其他关键帧，以及新的一帧GNSS数据到来时所确定的关键帧作为预设滑动窗口中新的关键帧；

利用所述新的关键帧更新所述量测方程，并采用非线性优化算法对更新后的量测方程中的杆臂值和安装角进行优化。

5.根据权利要求1所述的方法，其特征在于，对所述量测方程中待优化的数据进行优化处理，得到满足预设收敛条件的目标杆臂值，包括：

采用迭代方式进行，对所述量测方程中待优化的数据进行优化，将每次迭代得到的杆臂值作为下次迭代的杆臂初值，直到相邻两次迭代所得到的杆臂值之间的误差小于预设长度阈值时，将相邻两次迭代过程中最后一次迭代所得到的杆臂值作为目标杆臂值。

6.根据权利要求5所述的方法，其特征在于：

在第一次迭代过程中，利用采集的GNSS的位置和速度对IMU的位置和速度进行初始化，并根据所述IMU所采集的加速度和角速度，得到所述IMU运动状态信息的初值；

利用所述IMU的运动状态信息的初值以及在相邻关键帧之间所获取的IMU的多帧采样数据的增量，构建相邻关键帧之间包含有IMU运动状态增量残差和杆臂初值的量测方程。

7.根据权利要求6所述的方法，其特征在于，在GNSS的采样时刻，将IMU的运动状态信息作为预设滑动窗口中待优化的关键帧中的数据之前，所述方法还包括：

在车体进行绕八字运动或进行转弯时，获取所述GNSS采集的速度和位置，以及所述IMU采集的角速度和加速度，以开始第一次迭代过程。

8.一种惯性测量单元和全球导航系统之间杆臂的标定装置，其特征在于，包括：

关键帧确定模块，被配置为在GNSS的采样时刻，将IMU的运动状态信息作为预设滑动窗口中待优化的关键帧中的数据，所述运动状态信息包括位置、姿态和速度；

量测方程构建模块，被配置为利用所述关键帧中的数据，以及在相邻关键帧之间所获取的IMU的多帧采样数据的增量，构建相邻关键帧之间包含有IMU运动状态增量残差的量测方程，所述量测方程中还包括待优化的IMU和GNSS之间的杆臂值和IMU在车体坐标系下的安装角；

优化模块，被配置为基于采样得到的GNSS数据，并根据基于预设滑动窗口的优化算法，以及相邻关键帧之间的约束关系，对所述量测方程中待优化的数据进行优化处理，得到满足预设收敛条件的目标杆臂值和安装角；其中，所述约束关系用于在新的一帧GNSS数据到来时，确定除第一个关键帧之外的其他关键帧，以形成预设滑动窗口中新的关键帧。

9.根据权利要求8所述的装置，其特征在于，所述相邻关键帧之间的约束关系通过如下方式建立：

系数矩阵确定模块，被配置为将所述量测方程对所述预设滑动窗口中第一个关键帧中IMU的运动状态信息，以及除第一个关键帧之外的其他关键帧中IMU的运动状态信息进行分别求导，分别将得到的雅克比矩阵作为所述第一个关键帧中IMU的运动状态信息和所述其他关键帧中IMU的运动状态信息对应的系数矩阵；

观测方程建立模块，被配置为根据所述系数矩阵及其对应关键帧中IMU的运动状态信息，以及所述IMU运动状态增量残差，建立观测方程；

约束关系获取模块，被配置为在所述预设滑动窗口进行滑动的过程中，当接收到新的GNSS采样数据时，在所述观测方程中，利用消元法消除所述预设滑动窗口中的第一个关键帧中IMU的运动状态信息，得到相邻关键帧之间的约束关系。

10.根据权利要求9所述的装置，其特征在于，所述观测方程建立模块，具体被配置为：

按照如下公式建立观测方程：

其中，x₁为预设滑动窗口中第一个关键帧中IMU的运动状态信息；x₂为预设滑动窗口中所述其他关键帧中IMU的运动状态信息；A为所述量测方程对x₁求导得到的雅克比矩阵；B为所述量测方程对x₂求导得到的雅克比矩阵；C为所述量测方程对x₁求导得到的雅克比矩阵；和D为所述量测方程对x₂求导得到的雅克比矩阵；a为第一个关键帧中IMU的运动状态增量残差；b为其他关键帧中IMU的运动状态信息增量残差。

Images