CN108507571A - 一种高速运动学下imu姿态捕捉方法及系统 - Google Patents

Abstract

本发明公开了一种高速运动学下IMU姿态捕捉方法，包括以下步骤：(A)、获取角速度的信号(B)、获取加速度的信号(C)、获取磁信号(D)、建立初始姿态；(E)、评估加速度的不确定度；(F)、评估磁干扰的不确定度；(G)、利用卡尔曼延展滤波器更新姿态；本发明又提供了一种高速运动学下IMU姿态捕捉系统，包括有IMU单元，所述IMU单元包括有三轴加速传感器、三轴磁传感器与三轴角速度传感器，还包括有运算单元与卡尔曼延展滤波器，所述IMU单元与运算单元以及卡尔曼延展滤波器电连接。本发明的IMU动态运动捕捉方法，可以避免运动过程中因加速度骤变而引起较大姿态误差；姿态测量误差收敛快速；对不同的运动形式有很好的适应能力。

Description

一种高速运动学下IMU姿态捕捉方法及系统

技术领域

本发明涉及一种三维传感器领域，特别是IMU系统。

背景技术

惯性测量单元，即IMU传感器，是基于MEMS技术，将3轴 (X,Y,Z三个方向)的加速度传感器(accelerometer)，三轴的角速度传感器(gyroscope)，以及三轴的磁力传感器(magnetometer)封装在一起，实现物体三维姿态测量的微机电传感器系统。近年来，得益于MEMS技术的快速发展，目前IMU传感器的尺寸已经可以做到小于手表的尺寸，价格也大大降低，常规的IMU能够普遍为一般用户所接受,甚至智能手机中，都含有一个IMU传感器。其在体育锻炼，人体定位导航，娱乐，康复测试等方面都得到了广泛的应用。

然而，由于IMU传感器测量物体三维姿态时，借助于重力加速度和地磁场作为参考基准，在测量持续高速运动时的物体时，由于物体显著的加速度与重力加速度耦合在一起，因此系统失去了可靠的重力加速度的观测基准，测量的精度明显降低。同样的，在磁场干扰明显的情况下，传感器由于失去了可靠的地磁场参考基准，传感器的三维方位测量精度也会明显下降。而为了实现精准的测量，现有技术中，基本都是通过采购经标定的IMU传感器。对于严格标定的IMU传感器，如XSENS等，价格高达上千美金，虽然较一般未标定IMU准确，但价格也高出了上百倍。而且尽管标定过的IMU性能较好，然而其在测量动态运动时，在应对加速度较复杂的情况时，也出现了很明显的误差以及收敛性差的问题，导致出现上述的问题，主要是因为 IMU当中的数据融合算法不精确，常用的数据融合算法有：卡尔曼滤波方法、粒子滤波姿态估计法、梯度下降法、互补滤波方法等。卡尔曼滤波方法应用于惯性导航中取得了较大的成功，但该算法较复杂、运算量大，需要较高的硬件成本，电量耗损大。粒子滤波姿态估计法提高了滤波精度，解决了系统非线性和非白噪声对姿态解算的影响，但计算量较大，不适用于低成本的航姿系统。梯度下降法在姿态解算中性能优于卡尔曼滤波，计算量少，对初始点要求低，但收敛速度较慢。互补滤波方法原理简单、计算量小，通过平率分辨消除噪声，能较好地结合加速度计的静态精度和陀螺仪的高动态姿态数据，利用两者在频域角度的互补特性，提高姿态测量的精度和动态响应，以减少测量和估计的误差，但其高通滤波器和低通滤波器的转接频率难以确定。

发明内容

本发明要解决的技术问题是：提供一种经济效益好而且测量精度高、抗干扰的适用于高速运动捕获的IMU姿态捕捉方法及系统。

本发明解决其技术问题的解决方案是：一种高速运动学下IMU 姿态捕捉方法，包括以下步骤：(A)、获取角速度的信号，并建立数学模型；(B)、获取加速度的信号，并建立数学模型；(C)、获取磁信号，并建立数学模型；(D)、建立初始姿态；(E)、评估加速度的不确定度；(F)、评估磁干扰的不确定度；(G)、结合步骤(E)、(F)，利用卡尔曼延展滤波器更新姿态。

作为上述技术方案的进一步改进，所述步骤(A)中，具体方法如下：以ω_g，m表示角速度传感器的输出，则有

ω_g，m＝ω_t+b_w+w_g，t (1)

其中ω_t表示当前物体运动的角速度的真实值，以上都是相对于传感器坐标系，b_w表示当前传感器的偏移量，w_g，t表示角速度传感器的白噪音，其中ω_t,b_w,

b_w，t＝b_w，t-1+w_b，t。 (2)

作为上述技术方案的进一步改进，所述步骤(B)中，具体方法如下：

以a_f表示加速度传感器测量结果,a_t表示物体运动加速度，以

w_a表示加速度传感器测量白噪音，因此有：

a_f＝^sg+a_t+w_a。 (3)

作为上述技术方案的进一步改进，所述步骤(C)中，具体方法如下：

以m_m表示磁传感器测量结果，^sm₀表示本地地磁基准，m_d表示磁干扰，w_m表示磁传感器测量白噪音，因此有：

m_m＝^sm₀+m_d+w_m。 (4)

作为上述技术方案的进一步改进，所述步骤(D)中，具体方法如下：

以R_s(3x3的方向余弦矩阵)表示物体的三维姿态，已知R_s，k-1为前一时刻物体姿态，借助角速度ω_g，k信息积分，可以通过得到当前时刻的姿态R_s，k(一阶近似)：

其中表示ω的斜对称矩阵，dt为时间间隙，以δθ表示传感器的姿态误差,以表示传感器的实际误差，则有：

当传感器初始处于静止状态，加速度传感器测量值a_f是由重力引起的，在没有磁干扰时候，磁传感器测量值是当地的地磁场强^sm₀，据此以竖直向上为参考坐标系的z轴，以地磁北极为参考坐标系的x轴,可以得到初始状态，传感器相对参考坐标系的姿态：

作为上述技术方案的进一步改进，所述步骤(E)中，具体步骤如下：

以θ_a表示测量加速度与重力基准的角度误差(a_g＝-g，向上),同时

δa_f表示a_f的标量值与重力加速度值g的差异，

其中^sz_e＝R_s ^T[0，0，1]^T.在R_s收敛的情况下，^sz_e代表了重力参考 (反)方向；

采用如下形式，表述t时刻，运动加速度的不确定度，

S_at ²＝μ_s(δa_f ²+(δθ_ag)²) (10)

其中μ_s是一个常数；

由于a_f的大小变化很大，在姿态测量中，将其单位化，

显然，当被测物体运动加速度a_t很小时，a_t+w_z≈0,并且||a_f||≈g，因此，加速度的不确定度S_at很小，以上公式简化如下

在t时刻，在a_t是未知的情况下，将a_t用以下标准正态分布模型近似，

因此有：

其中w_n＝a_t+w_a

以R_an表示噪音的协方差矩阵，则有：

作为上述技术方案的进一步改进，所述步骤(F)中，具体步骤如下：

S_m ²＝δm_m ²+(δθ_m||m_m||)² (17)

根据公式(4)及(6)有，

m_m＝R_s ^Tm₀+R_s ^Tm₀×δθ+m_d+w_m (20)

其中m_d的离散时序控制过程被描素为以下马尔科夫过程，

m_d，k＝c_dm_d，k-1+w_d， (21)

w_d为白噪音，其协方差矩阵由磁干扰的不确定度Sm2决定

Q_md＝μ_mS_m ²I_3×3 (22)

其中，c_d为0到1之间的常数，μ_m为常数。

作为上述技术方案的进一步改进，所述步骤(G)中，具体步骤如下：

(G1).根据非线性状态方程的状态估计，

(G2).状态误差的协方差矩阵P_k可以根据状态方程更新如下,

其中是过程误差的协方差矩阵；

(G3).在EKF中，状态的更新如下

其中为测量结果的协方差矩阵

(G4).最终状态误差的协方差矩阵P_k更新如下

同时，本发明又提供了一种高速运动学下IMU姿态捕捉系统，包括有IMU单元，所述IMU单元包括有三轴加速传感器、三轴磁传感器与三轴角速度传感器，还包括有MCU单元，所述IMU单元与 MCU单元电连接。

本发明的有益效果是：本发明的IMU动态运动捕捉方法，可以避免运动过程中因加速度骤变而引起较大姿态误差；姿态测量误差收敛快速；对不同的运动形式有很好的适应能力。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单说明。显然，所描述的附图只是本发明的一部分实施例，而不是全部实施例，本领域的技术人员在不付出创造性劳动的前提下，还可以根据这些附图获得其他设计方案和附图。

图1是本发明的初始姿态原理示意图；

图2是本发明的加速度不确定度估计原理示意图；

图3是本发明的磁干扰不确定度估计原理示意图。

图4是本发明的系统的电器原理示意图。

具体实施方式

以下将结合实施例和附图对本发明的构思、具体结构及产生的技术效果进行清楚、完整地描述，以充分地理解本发明的目的、特征和效果。显然，所描述的实施例只是本发明的一部分实施例，而不是全部实施例，基于本发明的实施例，本领域的技术人员在不付出创造性劳动的前提下所获得的其他实施例，均属于本发明保护的范围。另外，文中所提到的所有联接/连接关系，并非单指构件直接相接，而是指可根据具体实施情况，通过添加或减少联接辅件，来组成更优的联接结构。本发明创造中的各个技术特征，在不互相矛盾冲突的前提下可以交互组合。

一种高速运动学下IMU姿态捕捉方法，包括以下步骤：(A)、获取角速度的信号，并建立数学模型；(B)、获取加速度的信号，并建立数学模型；(C)、获取磁信号，并建立数学模型；(D)、建立初始姿态；(E)、评估加速度的不确定度；(F)、评估磁干扰的不确定度； (G)、结合步骤(E)、(F)，利用卡尔曼延展滤波器更新姿态。

其中，角速度传感器测量信号的步骤如下，以ω_g，m表示角速度传感器的输出，则有

ω_g，m＝ω_t+b_w+w_g，t (28)

其中ω_t表示当前物体运动的角速度的真实值，以上都是相对于传感器坐标系，b_w表示当前传感器的偏移量，w_g，t表示角速度传感器的白噪音，其中ω_t,b_w,

b_w，t＝b_w，t-1+w_b，t。 (29)

其中，加速度传感器测量信号的步骤如下：

以a_f表示加速度传感器测量结果,a_t表示物体运动加速度，以 w_a表示加速度传感器测量白噪音，因此有：

a_f＝^sg+a_t+w_a。 (30)

理论上，可以通过在加速度传感器输出中移除被测物体的加速度来获取重力加速度的参考^sg,^sg=a_f-a_t。然而一方面，在一般性的高动态运动下，被测物体自身的加速度信号非常复杂，难于借助模型进行预测。同时，随着测量物体运动加速度的增加，加速度传感器的测量结果与参考重力加速度的差异也随之增加。

然后，磁传感器测量信号的步骤如下：

以m_m表示磁传感器测量结果，^sm₀表示本地地磁基准，m_d表示磁干扰，w_m表示磁传感器测量白噪音，因此有：

m_m＝^sm₀+m_d+w_m。 (31)

具体的，确定IMU单元的初始姿态，具体如下：

以R_s(3x3的方向余弦矩阵)表示物体的三维姿态，已知R_s，k-1为前一时刻物体姿态，借助角速度ω_g，k信息积分，可以通过得到当前时刻的姿态R_s，k(一阶近似)：

其中表示ω的斜对称矩阵，dt为时间间隙，由于积分过程及滤波等过程，传感器的姿态存在误差，以δθ表示传感器的姿态误差, 以表示传感器的实际误差，则有：

参见图1，当传感器初始处于静止状态，加速度传感器测量值a_f是由重力引起的，在没有磁干扰时候，磁传感器测量值是当地的地磁场强^sm₀，据此以竖直向上为参考坐标系的z轴，以地磁北极为参考坐标系的x轴,可以得到初始状态，传感器相对参考坐标系的姿态：

具体地，评估加速度的不确定度步骤如下：

介于一般运动过程中，待测物体运动加速度与重力加速度耦合在加速度传感器的测量结果中，没有有效的方法可以移除运动加速度，因此本技术中，将此加速度作为扰动噪音处理。

在t时刻，加速度传感器测量值为a_f,被测物体运动加速度a_t的范围可以通过传感器测量信息进行估计，如图2所示。

以θ_a表示测量加速度与重力基准的角度误差(a_g=-g，向上),同时

δa_f表示a_f的标量值与重力加速度值g的差异，

其中^sz_e＝R_s ^T[0，0，1]^T.在R_s收敛的情况下，^sz_e代表了重力参考 (反)方向；

由图2可知，当θ_a和δa_f较小时，有如下关系,

||a_t||²≈δa_f ²+(δθ_ag)² (37)

采用如下形式，表述t时刻，运动加速度的不确定度，

S_at ²＝μ_s(δa_f ²+(δθ_ag)²) (38)

其中μ_s是一个常数；

由于a_f的大小变化很大，在姿态测量中，将其单位化，

显然，当被测物体运动加速度a_t很小时，a_t+w_z≈0,并且||a_f||≈g，因此，加速度的不确定度S_at很小，以上公式简化如下

在此情况下，加速度传感器的测量值可以非常有效的更正传感器姿态的倾角误差。

在t时刻，在a_t是未知的情况下，将a_t用以下标准正态分布模型近似，

因此有：

其中w_n＝a_t+w_a

以R_an表示噪音的协方差矩阵，则有：

在后续的滤波过程中会发现，当很大时，由于加速度测量的协方差矩阵很大，因此在姿态更正中会被忽略，不会产生大的姿态误差。

具体的，评估磁干扰的不确定度的方法如下：

如图3所示，对于磁干扰，与加速度不确定度同样，其不确定度可以估计如下：

S_m ²＝δm_m ²+(δθ_m||m_m||)² (45)

根据公式(4)及(6)有，

m_m=R_s ^Tm₀+R_s ^Tm₀×δθ+m_d+w_m (48)

其中m_d的离散时序控制过程被描素为以下马尔科夫过程，

m_d，k＝c_dm_d，k-1+w_d， (49)

w_d为白噪音，其协方差矩阵由磁干扰的不确定度S_m ²决定

θ_md＝μ_mS_m ²I_3×3 (50)

其中，c_d为0到1之间的常数，μ_m为常数。

卡尔曼延展滤波(EKF)介绍

卡尔曼延展滤波是用来解决在非线性状态方程定义的离散时序控制过程下

x_k＝f(x_k-1，u_k-1,w_k-1) (51)

通过测量z

z_k＝h(x_k,v_k) (52)

来估计状态变量x值的问题。

具体的，本实施例中，卡尔曼延展滤波(EKF)的过程如下：

(G1).根据非线性状态方程的状态估计，

(G2).状态误差的协方差矩阵P_k可以根据状态方程更新如下,

其中是过程误差的协方差矩阵；

(G3).在EKF中，状态的更新如下

其中为测量结果的协方差矩阵

(G4).最终状态误差的协方差矩阵P_k更新如下

本实施例中，结合了步骤E、F后，状态变量如下，

x＝[δθ^T，b_w ^T，m_d ^T]^T (58)

基于方程(2),(22),和以下方程

δθ_k＝δθ_k-1+dt(b_w，k-1+w_g，t+w_b，t) (59)

状态更新方程如下，

x_k＝Ax_k-1+w_k-1, (60)

其中

在此δθ和b_w初始估计值被设置为零，因此，状态预先估计值如下，

x_＝[0_6×1：m_d0] (62)

状态更新过程的协方差矩阵Q如下，

P_k的初始值给定为零矩阵

根据方程(15)及(21)有，

其中

根据EKF过程的阐述有,

测量白噪音w_n和w_m的协方差矩阵如下，

在IMU进行动态运动的姿态跟踪时，借助计算物体运动加速度的不确定度，有效的在加速度较低的阶段进行姿态更正，加速度测量值与重力加速度参考越来越靠近，越有利于IMU倾角误差的更正。

在动态运动中，物体运动加速度值有事一直较大，采用阈值 (threshold)捕获其加速度很小的阶段往往行不通，因此以此方法可以有效地解决连续捕获高速运动物体姿态的问题。

根据公式(26)-(30),以卡尔曼延展滤波器更新其状态变量。.

而，在其他阶段，则根据公式(26)-(27)估计状态的值，并更新状态误差的协方差矩阵。

传感器的姿态信息可以通过公式(6)更新。.

参照图4，同时，本发明又提供了一种高速运动学下IMU姿态捕捉系统，包括有IMU单元，所述IMU单元包括有三轴加速传感器、三轴磁传感器与三轴角速度传感器，还包括有MCU单元，所述IMU 单元与MCU单元电连接。通过MCU单元读取三轴加速传感器、三轴磁传感器与三轴角速度传感器的数据，然后MCU单元中通过卡尔曼延展滤波算法等完成运算，然后将数据传输出来。

以上对本发明的较佳实施方式进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可作出种种的等同变型或替换，这些等同的变型或替换均包含在本申请权利要求所限定的范围内。

Claims (9)

1.一种高速运动学下IMU姿态捕捉方法，其特征在于：包括以下步骤：

(A)、获取角速度的信号，并建立数学模型；

(B)、获取加速度的信号，并建立数学模型；

(C)、获取磁信号，并建立数学模型；

(D)、建立初始姿态；

(E)、评估加速度的不确定度；

(F)、评估磁干扰的不确定度；

(G)、结合步骤(E)、(F)，利用卡尔曼延展滤波器更新姿态。

2.根据权利要求1所述的高速运动学下IMU姿态捕捉方法，其特征在于：所述步骤(A)中，具体方法如下：以ω_g，m表示角速度传感器的输出，则有

ω_g，m＝ω_t+b_w+w_g，t (1)

其中ω_t表示当前物体运动的角速度的真实值，以上都是相对于传感器坐标系，b_w表示当前传感器的偏移量，w_g，t表示角速度传感器的白噪音，其中ω_t,b_w,

b_w，t＝b_w，t-1+w_b，t (2) 。

3.根据权利要求2所述的高速运动学下IMU姿态捕捉方法，其特征在于：所述步骤(B)中，具体方法如下：

以a_f表示加速度传感器测量结果,a_t表示物体运动加速度，以w_a表示加速度传感器测量白噪音，因此有：

a_f＝s_g+a_t+w_a (3) 。

4.根据权利要求3所述的高速运动学下IMU姿态捕捉方法，其特征在于：所述步骤(C)中，具体方法如下：

以m_m表示磁传感器测量结果，表示本地地磁基准，m_d表示磁干扰，w_m表示磁传感器测量白噪音，因此有：

m_m＝s_m0+m_d+w_m (4) 。

5.根据权利要求4所述的高速运动学下IMU姿态捕捉方法，其特征在于：所述步骤(D)中，具体方法如下：

以R_s(3x3的方向余弦矩阵)表示物体的三维姿态，已知R_s，k-1为前一时刻物体姿态，借助角速度ω_g，k信息积分，可以通过得到当前时刻的姿态R_s，k(一阶近似)：

其中表示ω的斜对称矩阵，dt为时间间隙，以δθ表示传感器的姿态误差,以表示传感器的实际误差，则有：

当传感器初始处于静止状态，加速度传感器测量值af是由重力引起的，在没有磁干扰时候，磁传感器测量值是当地的地磁场强据此以竖直向上为参考坐标系的z轴，以地磁北极为参考坐标系的x轴,可以得到初始状态，传感器相对参考坐标系的姿态：

6.根据权利要求5所述的高速运动学下IMU姿态捕捉方法，其特征在于：所述步骤(E)中，具体步骤如下：

以θ_a表示测量加速度与重力基准的角度误差(a_g＝-g，向上),同时δa_f表示a_f的标量值与重力加速度值g的差异，

其中在R_s收敛的情况下，代表了重力参考(反)方向；

采用如下形式，表述t时刻，运动加速度的不确定度，

S_at ²＝μ_s(δa_f ²+(δθ_ag)²) (10)

其中μ_s是一个常数；

由于a_f的大小变化很大，在姿态测量中，将其单位化，

显然，当被测物体运动加速度a_t很小时，a_t+w_z≈0,并且||a_f||≈g，因此，加速度的不确定度S_at很小，以上公式简化如下

在t时刻，在a_t是未知的情况下，将a_t用以下标准正态分布模型近似，

因此有：

其中w_n＝a_t+w_a

以R_an表示噪音的协方差矩阵，则有：

7.根据权利要求6所述的高速运动学下IMU姿态捕捉方法，其特征在于：所述步骤(F)中，具体步骤如下：

S_m ²＝δm_m ²+(δθ_m||m_m||)² (17)

根据公式(4)及(6)有，

m_m＝R_s ^Tm₀+R_s ^Tm₀×δθ+m_d+w_m (20)

其中m_d的离散时序控制过程被描素为以下马尔科夫过程，

m_d，k＝c_dm_d，k-1+w_d， (21)

w_d为白噪音，其协方差矩阵由磁干扰的不确定度Sm2决定

Q_md＝μ_mS_m ²I_3×3 (22)

其中，c_d为0到1之间的常数，μ_m为常数。

8.根据权利要求7所述的高速运动学下IMU姿态捕捉方法，其特征在于：所述步骤(G)中，具体步骤如下：

(G1).根据非线性状态方程的状态估计，

(G2).状态误差的协方差矩阵P_k可以根据状态方程更新如下,

其中Q_k-1是过程误差的协方差矩阵；

(G3).在EKF中，状态的更新如下

其中R_k-1为测量结果的协方差矩阵

(G4).最终状态误差的协方差矩阵P_k更新如下

9.一种高速运动学下IMU姿态捕捉系统，其特征在于：包括有IMU单元，所述IMU单元包括有三轴加速传感器、三轴磁传感器与三轴角速度传感器，还包括有MCU单元，所述IMU单元与MCU单元电连接。