CN110044377A - 一种基于Vicon的IMU离线标定方法 - Google Patents

Abstract

本文发明涉及惯性导航领域，具体为一种基于Vicon的IMU离线标定方法，包括IMU及Vicon数据获取模块、数据融合模块、陀螺仪零偏估计模块、加速度计零偏估计模块、输出模块。本发明通过IMU及Vicon数据获取模块采集IMU的加速度和角速度和Vicon的位置信息和姿态信息，再经数据融合模块获得相对姿态残差、相对速度残差和相对位置残差，而后在陀螺仪零偏估计模块优化姿态残差得到陀螺仪零偏参数，之后在加速度计零偏估计模块优化速度残差和位置残差得到加速度计零偏参数，最后将陀螺仪的零偏参数和加速的计的零偏参数通过输出模块输出，从而达到离线情况下对IMU的陀螺仪零偏估计和加速度计零偏估计进行快速，高精度的估计。

Description

一种基于Vicon的IMU离线标定方法

技术领域

本发明涉及惯性导航领域，具体是一种基于Vicon的IMU离线标定方法。

背景技术

Vicon是高精度的动作捕获系统，能够稳定且实时获取目标的位置与姿态。通常我们使用Vicon数据作为数据集的真值，并用于评估其他定位系统的定位精度。惯性导航算法，除了位置、速度、姿态以外，通常还需要估计IMU的内部参数。位置、速度、姿态都可以通过Vicon系统进行评估，而估计的IMU内部参数无法直接获取。为了对IMU的内部参数估计进行评估，需要对IMU进行标定。传统的IMU标定方法存在一些缺点，具体如下。

1.传统的IMU标定方法操作复杂，一次操作流程往往需要一到两个小时，费事费力。

2.传统的IMU标定方法无法离线标定，对已经采好的数据集无法得到采集时的真正参数。

发明内容

本发明针对现有IMU标定技术存在的不足，提供一种基于Vicon的IMU离线标定方法利用预积分技术以及将IMU与Vicon数据融合，计算相对姿态残差、相对速度残差和相对位置残差，之后最小化相对姿态残差，得到陀螺仪零偏估计，最后将陀螺仪零偏估计结果用于最小化相对速度残差和相对位置残差，从而得到加速度计的零偏估计。

为了解决上述技术问题，本文采用了如下方案。

一种基于Vicon的IMU离线标定方法，包括：IMU及Vicon数据获取模块、数据融合模块、陀螺仪零偏估计模块、加速度计零偏估计模块及输出模块。

所述的IMU及Vicon数据获取模块用以采集IMU载体运动过程中的加速度、转动的角速度和载体在Vicon坐标系下的数据。

所述的数据融合模块与IMU及Vicon数据获取模块连接，用以计算IMU的预积分值和Vicon采样后的数据，然后计算出相对姿态残差、相对速度残差和相对位置残差。

所述的陀螺仪零偏估计模块与数据融合模块连接，最小化相对姿态残差，得到陀螺仪的零偏估计。

所述的加速度计零偏估计模块与数据融合模块和陀螺仪零偏估计模块连接，运用陀螺仪零偏估计模块得到的陀螺仪零偏估计，最小化相对速度残差、相对位置残差，得到加速度计零偏估计。

所述的输出模块与陀螺仪零偏估计模块和加速度计零偏估计模块连接，用以接收陀螺仪零偏估计模块和加速度计零偏估计模块发出的结果信息。

较优选的，所述的IMU及Vicon数据获取模块包扩三轴加速度计、三轴陀螺仪和Vicon数据获取模块。所述的三轴陀螺仪与三轴的加速度计用以测量载体运动过程中的加速度和转动的角速度，Vicon数据获取模块用来获取IMU载体在Vicon坐标系下的数据。

较优选的，所述的数据融合模块包括IMU预积分模块、Vicon数据采样模块以及残差计算模块，IMU预积分模块与IMU及Vicon数据获取模块和Vicon数据采样模块连接；所述的Vicon数据采样模块使用IMU及Vicon数据获取模块所提供的Vicon数据进行采样操作得到合适频率的Vicon数据；所述的IMU预积分模块使用IMU及Vicon数据获取模块所提供的加速度和角速度和采样后的Vicon数据信息进行预积分运算得到IMU预积分值；所述的残差计算模块使用IMU预积分模块提供的预积分值和Vicon数据采样模块提供的采样后的Vicon数据进行残差计算，得到系统的相对姿态残差、相对速度残差和相对位置残差信息。

较优选的，所述的Vicon数据采样模块的采样频率为30Hz。

较优选的，所述的陀螺仪零偏估计模块使用数据融合模块提供的相对姿态残差信息进行优化，得到陀螺仪零偏估计值，并将结果输出。

较优选的，所述的加速度计零偏估计模块使用陀螺仪零偏估计模块提供的陀螺仪零偏估计值，对数据融合模块提供的相对速度残差和相对位置残差信息进行优化，得到加速度计零偏估计值，并将结果输出。

较优选的，所述的输出模块通过wifi、蓝牙或USB线缆将结果传给显示设备并显示，所述的显示设备包括手机、电脑、以及其他便携视频显示设备。

本发明由于采用以上技术方案，使之与现有技术相比，具有以下的优点和积极效果：

1.本发明通过IMU及Vicon数据获取模块获取载体在Vicon坐标系下的位姿速度和位置信息以及IMU测量的载体的加速度和角速度信息，再通过数据融合模块对载体的加速度和角速度进行预积分操作获取预积分的结果，对Vicon数据进行采样，之后通过残差计算模块计算出系统残差信息，而后通过陀螺仪零偏估计模块对相对姿态残差信息进行优化，得到陀螺仪零偏估计值，最后通过加速度计零偏估计模块对陀螺仪零偏估计值、相对速度残差和相对位置残差信息进行优化，得到加速计零偏估计，从而达到高精度的IMU内参估计值。

2.本方法可以在线标定IMU内参，也可针对已经采好的数据集进行标定，大大提高了IMU标定方法的灵活性。

3.本方法比较较传统的IMU标定方法更加简单快速，标定时间大大减少。

附图说明

图1为本发明的结构原理图；

图2为本发明中IMU及Vicon数据获取模块的结构原理图；

图3为本发明中数据融合模块的结构原理图；

图4为本发明中陀螺仪零偏估计模块结构原理图；

图5为本发明中加速度计零偏估计模块结构原理图；

图6为本发明中输出模块结构原理图；

具体实施方式

以下结合附图和具体实施例对本发明提出的技术方案进行进一步详细说明。根据下面说明和权利要求书，本发明的优点和特征将更清楚。需说明的是，附图均采用非常简化的形式且均使用非精准的比率，仅用于方便、明晰地辅助说明本发明实施例的目的。

本文针对传统IMU标定方法过程过于繁琐，无法标定离线数据的缺点，将Vicon真值数据运用到IMU标定过程中，通过IMU及Vicon数据获取模块采集IMU的加速度和角速度和Vicon的位置信息和姿态信息，再经数据融合模块相对姿态残差、相对速度残差和相对位置残差，而后在陀螺仪零偏估计模块优化姿态残差得到陀螺零偏参数，之后在加速度计零偏估计模块优化速度残差和位置残差得到加速度计零偏参数，最后将陀螺仪的零偏参数和加速的计的零偏参数通过输出模块输出，从而达到离线情况下对IMU的内参进行快速，高精度的估计，现结合附图及具体实施例对本发明的具体内容进行说明。

请参见图1，基于Vicon的IMU离线标定方法包括：IMU及Vicon数据获取模块100、数据融合模块101、陀螺仪零偏估计模块102、加速度计零偏估计模块103、输出模块104，其中：

IMU及Vicon数据获取模块100用以采集IMU的加速度和角速度和载体在Vicon坐标系下的位置信息、速度信息和姿态信息。

数据融合模块101与IMU及Vicon数据获取模块100连接，用于对IMU及Vicon数据获取模块采集的Vicon真值数据进行采样，得到IMU在Vicon坐标系下的位置、姿态和速度信息，通过IMU数据预积分，得到相对姿态残差、相对位置残差和相对位置残差。

陀螺仪零偏估计模块102与数据融合模块101连接，用以优化姿态残差项，得到精确的陀螺仪零偏参数估计。

加速度计零偏估计模块103与数据融合模块101和陀螺仪零偏估计模块102连接，用以优化速度残差项和位置残差项，得到精确的加速度计零偏参数估计。

输出模块104与陀螺仪零偏估计模块102和加速度计零偏估计模块103连接，用以输出陀螺仪零偏估计模块和加速度计零偏估计模块计算出来的陀螺仪零偏参数和加速度计零偏参数输出信息显示在设备上。

本发明IMU及Vicon数据获取模块采集IMU的加速度和角速度和Vicon的位置信息和姿态信息，再经数据融合模块获得相对姿态残差、相对速度残差和相对位置残差，而后在陀螺仪零偏估计模块优化姿态残差得到陀螺零偏参数，之后在加速度计零偏估计模块优化速度残差和位置残差得到加速度计零偏参数，从而达到离线情况下对IMU的内参进行快速，高精度的估计。

现结合附图分别对IMU及Vicon数据获取模块100、数据融合模块101、陀螺仪零偏估计模块102、及速度计估计模块103、输出模块104和整个基于Vicon的IMU离线标定法系统的工作原理和流程进行介绍。

IMU及Vicon数据获取模块100主要用来采集IMU载体的三轴加速度和角速度以及在Vicon坐标下载体的位姿，速度和位置信息，见图2，IMU及Vicon数据获取模块100包括三轴陀螺仪1001、三轴加速度计1002和Vicon数据采集模块1003，在安装数据采集装置时，认为imu和vicon物体之间相对位置、姿态固定，惯性参考系和Vicon的参考系之间相对位置、姿态固定。其中惯性参考系和Vicon参考系的平移为0，旋转以Vicon的z轴为标准。

在本发明中，Vicon数据采集模块使用深圳市瑞立视多媒体科技有限公司自主研发出RTS系列光学运动捕捉系统，其数据采集频率可自行设置；三轴的陀螺仪1001与三轴的加速度计1002使用Xsense公司的MTw Wireless Motion Tracker，采集频率为100Hz。本发明中提供的三轴陀螺仪1001、三轴加速度计1002和Vicon数据采集模块1003的产品型号，但是也可采用其他能实现Vicon信息、加速度、角速度信息测量的芯片，在此不作为限定。

数据融合模块101与IMU及Vicon数据获取模块100连接，主要用于融合IMU预积分信息与Vicon采样后的信息，数据融合模块101结构原理示意图请参见图3，其中包括IMU预积分模块1011、Vicon数据采样模块1012以及残差计算模块1013，其中IMU预积分模块1011连接三轴陀螺仪1001、三轴加速度计1002和Vicon数据采样模块，由于IMU的频率为100HZ，Vicon采样后只有30Hz，因此使用Christian Forster的预积分技术，将IMU与Vicon数据融合。Vicon数据采样模块1012将Vicon的频率降为30Hz，残差计算模块与IMU预积分模块和Vicon数据采样模块连接用于计算系统的相对姿态残差，相对速度残差以及相对位置残差。

陀螺仪零偏估计模块102与数据融合模块101连接，主要用于优化相对位姿残差得到陀螺仪零偏估计，如图4所示。陀螺仪零偏估计模块102通过求解一个非线性最小二乘问题，最小化相对姿态残差得到精确得陀螺仪零偏参数B_g，其公式为 残差计算模块1013计算得到的相对位置残差，最后将陀螺仪零偏参数B_g输出到输出模块104。

加速度计零偏估计模块103与陀螺仪零偏估计模块102和数据融合模块101连接，主要用于优化相对速度残差和相对位置残差得到陀螺仪零偏估计，如图5所示。加速度计零偏估计模块103接收数据融合模块101提供的相对速度残差和相对位置残差以及陀螺仪零偏估计模块102提供的陀螺仪零偏估计B_g，通过求解非线性最小二乘问题分别最小化相对速度残差和相对位置残差得到加速度计零偏估计B_a，其最小化相对速度残差公式为最小化相对位置残差公式为 分别为相对速度残差和相对位置残差，最后将加速度计零偏参数B_a输出到输出模块104。

输出模块104与陀螺仪零偏估计模块102和加速度计零偏估计模块103连接，主要用以IMU标定结果(B_g、B_a)，并将结果在显示设备上进行显示，输出模块具体是通过蓝牙、wifi或USB线缆将结果传给显示设备并显示，显示设备包括手机、电脑等便携视频显示设备，以使整个基于Vicon的IMU离线标定系统可将处理形成后得到的标定结果显示在手机、电脑等其他便携显示设备上，如图6所示。

显然，本领域的技术人员可以对发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (8)

1.一种基于Vicon的IMU离线标定方法，其特征在于，包括：IMU及Vicon数据获取模块、数据融合模块、陀螺仪零偏估计模块、加速度计估计模块、输出模块，其中所述的IMU及Vicon数据获取模块用以采集IMU的加速度和角速度和载体在Vicon坐标系下的位置信息、速度信息和姿态信息；

所述的数据融合模块与IMU及Vicon数据获取模块连接，用于对IMU及Vicon数据获取模块采集的Vicon真值数据进行采样，得到IMU在Vicon坐标系下的位置、姿态和速度信息，通过IMU数据预积分，得到相对姿态残差、相对位置残差和相对位置残差；

所述的陀螺仪零偏估计模块与数据融合模块连接，用以优化姿态残差项，得到精确的陀螺仪零偏参数估计；

所述的加速度计零偏估计模块与数据融合模块和陀螺仪零偏估计模块连接，用以优化速度残差项和位置残差项，得到精确的加速度计零偏参数估计；

所述的输出模块与陀螺仪零偏估计模块和加速度计零偏估计模块连接，用以输出陀螺仪零偏估计模块和加速度计零偏估计模块计算出来的陀螺仪零偏参数和加速度计零偏参数，输出信息显示在设备上。

2.根据权利要求1所述的一种基于Vicon的IMU离线标定方法，其特征在于，所述的IMU及Vicon数据获取模块包三轴加速度计、三轴陀螺仪和Vicon数据采集模块。

3.根据权利要求2所述的一种基于Vicon的IMU离线标定方法，其特征在于，所述的三轴加速度计和所述三轴陀螺仪用以采集IMU载体运动过程中的加速度和转动的角速度，Vicon数据获取模块用来获取载体在Vicon坐标系下的数据。

4.根据权利要求3所述的一种基于Vicon的IMU离线标定方法，其特征在于，所述的数据融合模块包括IMU预积分模块、Vicon数据采样模块以及残差计算模块。

5.根据权利要求4所述的一种基于Vicon的IMU离线标定方法，其特征在于，所述的IMU预积分模块与三轴加速度计模块、三轴陀螺仪模块连接和Vicon数据采样模块连接，所述预积分模块使用加速度和角速度求解预积分值。

6.根据权利要求4所述的一种基于Vicon的IMU离线标定方法，其特征在于，所述Vicon数据采样模块使用IMU及Vicon数据获取模块所提供的Vicon数据进行采样操作得到一定频率的Vicon数据，所述的残差计算模块与IMU预积分模块和Vicon数据采样模块连接，使用预积分值与Vicon采样后的数据计算相对姿态残差、相对速度残差和相对位置残差。

7.根据权利要求1所述的一种基于Vicon的IMU离线标定方法，其特征在于，所述的陀螺仪零偏估计模块与数据融合模块连接，使用相对姿态残差进行计算，得到陀螺仪的零偏估计。

8.根据权利要求1所述的一种基于Vicon的IMU离线标定方法，其特征在于，所述的加速度计零偏估计模块与残差计算模块和陀螺仪零偏估计模块连接，使用陀螺仪估计模块提供的陀螺仪零偏估计以及数据融合模块提供的相对速度残差信息和相对位置残差信息进行计算，得到加速度计零偏估计。