CN109798891A - 基于高精度动作捕捉系统的惯性测量单元标定系统 - Google Patents

Abstract

一种基于高精度动作捕捉系统的惯性测量单元标定系统，包括：数据获取模块、姿态因子模块、速度因子模块和位置因子模块，数据获取模块采集来自动作捕捉系统的位置信息和姿态信息，来自惯性测量单元的加速度和角速度信息；姿态因子模块与数据获取模块相连并优化姿态因子，计算惯性测量单元的陀螺仪零偏参数和多个坐标系之间的相对旋转；速度因子模块分别与数据获取模块和姿态因子模块相连并根据陀螺仪零偏参数、坐标系间的相对旋转、位置信息和姿态信息对加速度计的零偏参数进行估计；位置因子模块分别与数据获取模块和速度因子模块相连并根据位置信息和姿态信息对速度因子模块计算得到的…参数进行进一步的优化并输出优化后的标定结果。本发明使用高精度动作捕捉系统进行标定，能够方便的用于现有的数据采集流程，无需特殊操作并提供了真值数据(位置、姿态)的开源数据集，能够达到很高的精度。

Description

基于高精度动作捕捉系统的惯性测量单元标定系统

技术领域

本发明涉及的是一种室内惯性导航领域的技术，具体是一种基于高精度动作捕捉系统的惯性测量单元标定系统。

背景技术

目前的惯性测量单元(IMU)标定方案通常要求要借助一些额外的设备和动作，如高精度转台、六面法等。标定流程通常复杂，需要人为操作且标定时间长，一次标定需要至少半个小时到一个小时。

发明内容

本发明针对现有技术存在的上述不足，提出一种基于高精度动作捕捉系统的惯性测量单元标定系统，首先采集来自动作捕捉系统的位置信息和姿态信息和来自惯性测量单元的加速度和角速度信息，然后对姿态因子进行优化并计算惯性测量单元的陀螺仪零偏参数和多个坐标系之间的相对旋转信息，最后估计加速度计的零偏，在进一步优化后，输出最终的标定结果。本发明在标定时无需特殊操作，进行正常的数据采集流程即可。

本发明是通过以下技术方案实现的：

本发明包括：数据获取模块、姿态因子模块、速度因子模块和位置因子模块，其中：数据获取模块采集来自动作捕捉系统的位置信息和姿态信息，来自惯性测量单元的加速度和角速度信息；姿态因子模块与数据获取模块相连并最小化相对姿态的误差函数，采用因子图的描述方法，即姿态因子计算惯性测量单元的陀螺仪零偏参数和多个坐标系之间的相对旋转实现标定；速度因子模块分别与数据获取模块和姿态因子模块相连并根据陀螺仪零偏参数、多传感器安装时对应坐标系间的相对旋转、位置信息和姿态信息对加速度计的零偏参数进行估计；位置因子模块分别与数据获取模块和速度因子模块相连并根据位置信息和姿态信息对速度因子模块计算得到的IMU的零偏参数、多传感器之间的旋转、平移参数进行进一步的优化并输出优化后的惯性测量单元内参标定结果。

所述的数据获取模块包括：高精度动作数据采集模块、三轴加速度计、三轴陀螺仪和时间同步模块，其中：高精度动作数据采集模块与时间同步模块相连并采集并传输贴有标记球的物体的位置与姿态信息；三轴加速度计和三轴陀螺仪分别与时间同步模块相连并测量并传输载体运动过程中的加速度和各种转动的角速度；时间同步模块接收动作捕捉数据、加速度、角速度信息并进行时间同步和输出。

所述的高精度动作数据采集模块采样频率不低于100Hz。

所述的姿态因子模块包括：姿态变化计算模块、角速度积分模块和姿态残差优化模块，其中：姿态变化计算模块与数据获取模块相连并使用高精度动作捕捉系统输出的两个时刻的姿态信息，计算物体的姿态变化量；角速度积分模块与数据获取模块相连并对IMU输出的角速度信息进行积分，输出IMU物体的姿态变化量；姿态残差优化模块分别与姿态变化计算模块和角速度积分模块相连并对姿态变化计算模块、角速度积分模块的结果进行残差计算，并通过高斯牛顿优化器迭代优化相关参数。

所述的相关参数包括：陀螺仪的零偏、动作捕捉系统的体坐标系与IMU之间的旋转参数。

所述的姿态因子，其陀螺仪的零偏参数初值为零，动作捕捉系统的体坐标系与IMU之间的旋转为单位四元数。

所述的速度因子模块包括：速度变化计算单元、加速度积分单元和速度残差优化单元，其中：速度变化计算单元与数据获取模块相连用以使用高精度动作捕捉系统输出的两个时刻的位置信息，计算物体的速度变化量；加速度积分单元与数据获取模块相连并对IMU输出的加速度信息进行积分，输出IMU物体的速度变化量；速度残差优化单元分别与速度变化计算单元、加速度积分单元和姿态因子模块相连并对速度变化计算单元、加速度积分单元的结果进行残差计算，并通过高斯牛顿优化器迭代优化相关参数。

所述的相关参数包括加速度计的零偏、动作捕获系统的体坐标系与IMU之间的平移参数，以及动作捕获系统的参考系与惯性系之间的旋转参数。

所述的位置因子模块包括：位置变化计算单元、位置积分单元和位置残差优化单元，其中：位置变化计算单元与数据获取模块相连并使用高精度动作捕捉系统输出的两个时刻的位置信息，计算物体的位置变化量；位置积分单元与数据获取模块相连并对IMU输出的加速度信息进行二重积分，输出IMU物体的位置变化量；位置残差优化单元分别与位置变化计算单元、位置积分单元和速度因子模块相连并对位置变化计算单元、位置积分单元的结果进行残差计算，并通过高斯牛顿优化器迭代优化相关参数。

所述的相关参数包括陀螺仪、加速度计的零偏参数，动作捕捉系统的体坐标系与IMU之间的旋转和平移，以及动作捕获系统的参考系与惯性系之间的旋转参数。

所述的位置因子模块进行进一步的优化是指：使用姿态因子模块、速度因子模块，已经可以估计出所有的待估计参数，在这个结果的基础上，添加相对位置的约束项，通过最小化位置残差，进一步地提高参数估计的精度。

技术效果

与现有技术相比，本发明使用高精度动作捕捉系统进行标定，能够方便的用于现有的数据采集流程，无需特殊操作。本发明具有多段、全局式的优化方法，能够达到很高的精度。

附图说明

图1为本发明结构示意图；

图2为本发明数据获取模块的结构示意图；

图3为本发明姿态因子模块的结构示意图；

图4为本发明速度因子模块的结构示意图；

图5为本发明位置因子模块的结构示意图；

图中：数据获取模块1、姿态因子模块2、速度因子模块3、位置因子模块4、输出模块5、高精度动作数据采集模块6、三轴加速度计7、三轴陀螺仪8、时间同步模块9、姿态变化计算模块10、角速度积分模块11、姿态残差优化模块12、速度变化计算单元13、加速度积分单元14、速度残差优化单元15、位置变化计算单元16、位置积分单元17、位置残差优化单元18。

具体实施方式

如图1所示，为本实施例涉及的一种基于高精度动作捕捉系统的惯性单元标定系统，包括：数据获取模块1、姿态因子模块2、速度因子模块3、位置因子模块4和输出模块5，其中：数据获取模块1用以采集来自动作捕捉系统的位置信息和姿态信息，来自惯性测量单元的加速度和角速度信息；姿态因子模块2与数据获取模块1相连并优化姿态因子，计算惯性测量单元的陀螺仪零偏参数和多个坐标系之间的相对旋转；速度因子模块3分别与数据获取模块1和姿态因子模块2相连并对姿态因子模块2输出的陀螺仪零偏参数和坐标系间的相对旋转和数据获取模块1输出的位置信息和姿态信息，对加速度计的零偏参数进行估计；位置因子模块4分别与数据获取模块1和速度因子模块3相连，采用数据获取模块3输出的位置信息和姿态信息，对速度因子模块3计算得到的参数进行进一步的优化；输出模块5与位置因子模块4相连并接收标定结果并显示在设备上。

所述的数据获取模块1包括：高精度动作数据采集模块6、三轴加速度计7、三轴陀螺仪8和时间同步模块9，其中：高精度动作数据采集模块6与时间同步模块9相连并采集并传输贴有标记球的物体的位置与姿态信息；三轴加速度计7和三轴陀螺仪8分别与时间同步模块9相连并测量并传输载体运动过程中的加速度和各种转动的角速度；时间同步模块接9收动作捕捉数据、加速度、角速度信息并进行时间同步和输出。

所述的高精度动数据采集模块6采用ROS系统进行通信用于采集Vicon光学动作捕捉系统的数据；三轴加速度计7与三轴陀螺仪8使用了载体为一架F330四旋翼无人机的继承芯片MPU6050。

所述的高精度动作数据采集模块6的标记球均匀贴放在载体上，采样频率不低于100Hz。

所述的姿态因子模块2包括：姿态变化计算模块10、角速度积分模块11和姿态残差优化模块12，其中：姿态变化计算模块10与数据获取模块1相连并使用高精度动作捕捉系统输出的两个时刻的姿态信息，计算物体的姿态变化量；角速度积分模块11与数据获取模块1相连并对IMU输出的角速度信息进行积分，输出IMU物体的姿态变化量；姿态残差优化模块12分别与姿态变化计算模块10和角速度积分模块11相连并对姿态变化计算模块10、角速度积分模块11的结果进行残差计算，并通过高斯牛顿优化器进行迭代计算以优化相关参数，当达到收敛时估计出陀螺仪的零偏参数和多传感器之间的相对旋转。

所述的IMU输出的角速度信息包含通过最小化残差获取准确的参数零偏参数的信息，具体通过以下步骤得到：

1)使用陀螺仪测量得到的角速度计算的相对姿态变化量：

2)根据Vicon直接测量出k和k+1时刻姿态：

3)采用Levenberg-Marquardt方法最小化姿态残差估计陀螺仪的零偏参数和传感器之间的相对旋转：其中：代表由a坐标系到b坐标系的操作，可以是旋转、平移等，具体的代表由k时刻IMU的体坐标系到世界系w的旋转，q代表旋转采用了四元数的形式，代表k时刻和k+1时刻之间的IMU坐标系的相对姿态变化量，通过角速度积分得到，为IMU的角速度测量值，下标m代表其为测量值，t代表测量值是时变的，为陀螺仪的零偏参数，下标g代表陀螺仪，t表示零偏是时变的，为通过Vicon测量值得到的到k时刻的IMU的姿态信息，并且隐含了两个传感器的相关旋转信息，其中，为Vicon的测量值，为Vicon标记球和IMU安装坐标系的相对旋转。

所述的残差计算是指：即IMU计算得到的旋转与Vicon测量得到的旋转之间的差值，通过四元数的乘法来表述。

所述的速度因子模块3包括：速度变化计算单元13、加速度积分单元14和速度残差优化单元15，其中：速度变化计算单元13与数据获取模块1相连用以使用高精度动作捕捉系统输出的两个时刻的位置信息，计算物体的速度变化量；加速度积分单元14与数据获取模块1相连并对IMU输出的加速度信息进行积分，输出IMU物体的速度变化量；速度残差优化单元15分别与速度变化计算单元13、加速度积分单元14和姿态因子模块2相连并对速度变化计算单元13、加速度积分单元14的结果进行残差计算，并通过高斯牛顿优化器进行迭代计算以优化相关参数，当达到收敛时估计出加速度计的零偏参数和重力方向。

所述的IMU输出的加速度信息包含通过最小化残差获取准确的参数零偏参数的信息，具体通过以下步骤得到：

i)使用加速度计和陀螺仪的测量值计算相对的速度变化量：

ii)通过Vicon测量得到相对的速度变化量：由于Vicon测量的速度是在世界系的速度，因此需要考虑重力和姿态的影响，即计算

iii)采用Levenberg-Marquardt方法最小化速度残差估计加速度计的零偏参数、重力方向：其中：表示k和k+1时刻之间，体坐标系的速度变化量，：为IMU的加速度测量值，下标m代表其为测量值，下标t代表测量值是时变的，为加速度计的零偏参数，下标a代表加速度计，t代表零偏是时变的，为角速度积分得到的姿态，与姿态因子中中的是等价的，采用离散积分时其计算过程是完全一致的；为通过Vicon测量值得到的到k时刻的IMU的姿态信息，为通过Vicon的位置测量微分得到的k时刻IMU的速度信息，g^w为世界系的重力向量。

所述的残差计算是指：即IMU计算得到的速度变化量与Vicon测量得到的速度变化量的差值。

所述的位置因子模块4包括：位置变化计算单元16、位置积分单元17和位置残差优化单元18，其中：位置变化计算单元16与数据获取模块1相连并使用高精度动作捕捉系统输出的两个时刻的位置信息进行微分，计算物体的位置变化量；位置积分单元17与数据获取模块1相连并对IMU输出的加速度信息进行二重积分，输出IMU物体的位置变化量；位置残差优化单元18分别与位置变化计算单元16、位置积分单元17和速度因子模块3相连并对位置变化计算单元16、位置积分单元17的结果进行残差计算，并通过高斯牛顿优化器进行迭代计算以优化相关参数，当达到收敛时实现估计精度的进一步提高，得到最终的IMU内参标定结果，并将IMU内参标定结果发送至输出模块5。

所述的IMU内参标定结果，具体通过以下步骤得到：

A.使用加速度计和陀螺仪的测量值计算相对的位置变化量：

B.通过Vicon测量得到的相对位置变化量：由于Vicon测量的位置是在世界系的位置，因此需要考虑重力和姿态的影响，进行相应的坐标系变换，即计算

C.采用Levenberg-Marquardt方法最小化最小化位置残差进一步提高IMU的内参(零偏)的估计精度：其中：表示k和k+1时刻之间，体坐标系的位置变化量，为IMU的加速度测量值，为角速度积分得到的姿态，与姿态因子中的是等价的，采用离散积分时其计算过程是完全一致的；为通过Vicon测量值得到的到k时刻的IMU的姿态信息，为通过Vicon的位置测量微分得到的k时刻IMU的速度信息，为Vicon测量值得到的k时刻的IMU的位置信息，g^w为世界系的重力向量。

所述的残差计算是指：即IMU计算得到的位置变化量与Vicon测量得到的位置变化量的差值。

所述的输出模块5通过wifi、蓝牙、2.4G或usb线缆将结果传给上位机并显示，显示设备包括：手机、电脑和其他便携视频显示设备。

本实施例涉及上述系统的惯性测量单元标定方法，通过采集来自动作捕捉系统的位置信息和姿态信息和来自惯性测量单元的加速度和角速度信息，然后对姿态因子进行优化并计算惯性测量单元的陀螺仪零偏参数和多个坐标系之间的相对旋转信息，从而估计加速度计的零偏，进一步优化后，输出标定结果并将结果在显示设备上进行显示。

本实施例的主要优势在于，操作简单，方便易用，不需要任何附加的流程和操作，正常采集定位测试数据即可，因此，本实施例可以直接应用于一些现有的数据集，这在传统方法上是不可能实现的(除非数据集专门提供了标定数据)。本实施例在开源的无人机定位系统的数据集EuRoc上进行了测试，具体的实验结果如下：

其中Ba代表加速度计的零偏，Bg代表陀螺仪的零偏。Euroc数据集提供了一套零偏参数的真值。具体对比下来，本方法估计的陀螺仪零偏参数非常接近真值，加速度计与数据集提供的真值略有偏差，但事实上Euroc是采用的滤波方法估计的参数真值，其结果也发现了大幅的变动。从原理上讲，其滤波方法的估计精度并不如本发明所采用的全局优化方法。总而言之，本发明能够有效的标定IMU的零偏参数，达到有效、可用的程度。

上述具体实施可由本领域技术人员在不背离本发明原理和宗旨的前提下以不同的方式对其进行局部调整，本发明的保护范围以权利要求书为准且不由上述具体实施所限，在其范围内的各个实现方案均受本发明之约束。

Claims (8)

1.一种基于高精度动作捕捉系统的惯性测量单元标定系统，其特征在于，包括：数据获取模块、姿态因子模块、速度因子模块和位置因子模块，其中：数据获取模块采集来自动作捕捉系统的位置信息和姿态信息，来自惯性测量单元的加速度和角速度信息；姿态因子模块与数据获取模块相连并最小化相对姿态的误差函数，采用因子图的描述方法，即姿态因子计算惯性测量单元的陀螺仪零偏参数和多个坐标系之间的相对旋转实现标定；速度因子模块分别与数据获取模块和姿态因子模块相连并根据陀螺仪零偏参数、多传感器安装时对应坐标系间的相对旋转、位置信息和姿态信息对加速度计的零偏参数进行估计；位置因子模块分别与数据获取模块和速度因子模块相连并根据位置信息和姿态信息对速度因子模块计算得到的IMU的零偏参数、多传感器之间的旋转、平移参数进行进一步的优化并输出优化后的惯性测量单元内参标定结果。

2.根据权利要求1所述的系统，其特征是，所述的数据获取模块包括：高精度动作数据采集模块、三轴加速度计、三轴陀螺仪和时间同步模块，其中：高精度动作数据采集模块与时间同步模块相连并采集并传输贴有标记球的物体的位置与姿态信息；三轴加速度计和三轴陀螺仪分别与时间同步模块相连并测量并传输载体运动过程中的加速度和各种转动的角速度；时间同步模块接收动作捕捉数据、加速度、角速度信息并进行时间同步和输出。

3.根据权利要求1所述的系统，其特征是，所述的姿态因子模块包括：姿态变化计算模块、角速度积分模块和姿态残差优化模块，其中：姿态变化计算模块与数据获取模块相连并使用高精度动作捕捉系统输出的两个时刻的姿态信息，计算物体的姿态变化量；角速度积分模块与数据获取模块相连并对IMU输出的角速度信息进行积分，输出IMU物体的姿态变化量；姿态残差优化模块分别与姿态变化计算模块和角速度积分模块相连并对姿态变化计算模块、角速度积分模块的结果进行残差计算，并通过高斯牛顿优化器迭代优化相关参数。

4.根据权利要求3所述的系统，其特征是，所述的IMU输出的角速度信息包含通过最小化残差获取准确的参数零偏参数的信息，具体通过以下步骤得到：

1)使用陀螺仪测量得到的角速度计算的相对姿态变化量：

2)根据Vicon直接测量出k和k+1时刻姿态：

3)采用Levenberg-Marquardt方法最小化姿态残差估计陀螺仪的零偏参数和传感器之间的相对旋转：其中：代表由a坐标系到b坐标系的操作，可以是旋转、平移等，具体的代表由k时刻IMU的体坐标系到世界系w的旋转，q代表旋转采用了四元数的形式，代表k时刻和k+1时刻之间的IMU坐标系的相对姿态变化量，通过角速度积分得到，为IMU的角速度测量值，下标m代表其为测量值，t代表测量值是时变的，为陀螺仪的零偏参数，下标g代表陀螺仪，t表示零偏是时变的，为通过Vicon测量值得到的到k时刻的IMU的姿态信息，并且隐含了两个传感器的相关旋转信息，其中，为Vicon的测量值，为Vicon标记球和IMU安装坐标系的相对旋转；

所述的残差计算是指：即IMU计算得到的旋转与Vicon测量得到的旋转之间的差值，通过四元数的乘法来表述。

5.根据权利要求1所述的系统，其特征是，所述的速度因子模块包括：速度变化计算单元、加速度积分单元和速度残差优化单元，其中：速度变化计算单元与数据获取模块相连用以使用高精度动作捕捉系统输出的两个时刻的位置信息，计算物体的速度变化量；加速度积分单元与数据获取模块相连并对IMU输出的加速度信息进行积分，输出IMU物体的速度变化量；速度残差优化单元分别与速度变化计算单元、加速度积分单元和姿态因子模块相连并对速度变化计算单元、加速度积分单元的结果进行残差计算，并通过高斯牛顿优化器迭代优化相关参数。

6.根据权利要求5所述的系统，其特征是，所述的IMU输出的加速度信息包含通过最小化残差获取准确的参数零偏参数的信息，具体通过以下步骤得到：

i)使用加速度计和陀螺仪的测量值计算相对的速度变化量：

ii)通过Vicon测量得到相对的速度变化量：由于Vicon测量的速度是在世界系的速度，因此需要考虑重力和姿态的影响，即计算

iii)采用Levenberg-Marquardt方法最小化速度残差估计加速度计的零偏参数、重力方向：其中：表示k和k+1时刻之间，体坐标系的速度变化量，：为IMU的加速度测量值，下标m代表其为测量值，下标t代表测量值是时变的，为加速度计的零偏参数，下标a代表加速度计，t代表零偏是时变的，为角速度积分得到的姿态，与姿态因子中中的是等价的，采用离散积分时其计算过程是完全一致的；为通过Vicon测量值得到的到k时刻的IMU的姿态信息，为通过Vicon的位置测量微分得到的k时刻IMU的速度信息，g^w为世界系的重力向量；

所述的残差计算是指：即IMU计算得到的速度变化量与Vicon测量得到的速度变化量的差值。

7.根据权利要求1所述的系统，其特征是，所述的位置因子模块包括：位置变化计算单元、位置积分单元和位置残差优化单元，其中：位置变化计算单元与数据获取模块相连并使用高精度动作捕捉系统输出的两个时刻的位置信息，计算物体的位置变化量；位置积分单元与数据获取模块相连并对IMU输出的加速度信息进行二重积分，输出IMU物体的位置变化量；位置残差优化单元分别与位置变化计算单元、位置积分单元和速度因子模块相连并对位置变化计算单元、位置积分单元的结果进行残差计算，并通过高斯牛顿优化器迭代优化相关参数。

8.根据权利要求7所述的系统，其特征是，所述的IMU内参标定结果，具体通过以下步骤得到：

A.使用加速度计和陀螺仪的测量值计算相对的位置变化量：

B.通过Vicon测量得到的相对位置变化量：由于Vicon测量的位置是在世界系的位置，因此需要考虑重力和姿态的影响，进行相应的坐标系变换，即计算

C.采用Levenberg-Marquardt方法最小化最小化位置残差进一步提高IMU的内参(零偏)的估计精度：其中：表示k和k+1时刻之间，体坐标系的位置变化量，为IMU的加速度测量值，为角速度积分得到的姿态，与姿态因子中的是等价的，采用离散积分时其计算过程是完全一致的；为通过Vicon测量值得到的到k时刻的IMU的姿态信息，为通过Vicon的位置测量微分得到的k时刻IMU的速度信息，为Vicon测量值得到的k时刻的IMU的位置信息，g^w为世界系的重力向量；

所述的残差计算是指：即IMU计算得到的位置变化量与Vicon测量得到的位置变化量的差值。