CN112611380B - 基于多imu融合的姿态检测方法及其姿态检测装置 - Google Patents

Abstract

本发明提供一种基于多IMU融合的姿态检测方法及其姿态检测装置，该装置中多个IMU的安装互相保持一定的角度，使得测量中总有一个IMU有一定的倾角。***初始化时主IMU下挂载的电子罗盘采集磁场强度以确定初始偏航角的有效性，测量中当电子罗盘检测到强磁干扰时，计算主IMU和辅IMU的倾斜角，使用倾斜角最接近45度或135度的一个IMU来观测偏航角增量，得到多IMU融合的弱漂移的偏航角。根据磁场干扰、加速度或角速度突变情况计算干扰系数S和突变因子I，再将AHRS姿态解算算法解算的偏航角和多IMU融合的弱漂移的偏航角融合，获得多IMU融合抗磁干扰的偏航角。本发明可以提高姿态传感器的抗磁场干扰能力，并且可以削弱无电子罗盘下IMU测量相对偏航角的漂移。

Description

基于多IMU融合的姿态检测方法及其姿态检测装置

技术领域

本发明涉及嵌入式技术领域，特别涉及一种基于多IMU融合的姿态检测方法及其姿态检测装置。

背景技术

近年来，电子惯性测量单元已经在无人机、无人车、智能手机等领域广泛应用，但在偏航角测量中存在漂移和磁干扰的问题。其中6轴的IMU，即没有电子罗盘的IMU在测量偏航角时存在很大的漂移，不宜长时间使用，在加速度或角速度快速变化时，测量到的偏航角还会出现滑动，9轴的IMU，即加了电子罗盘的IMU在测量偏航角时依赖对微弱的地球磁场的测量，很容易受到磁场环境的干扰。

目前测量偏航角的方法很多，如支持定位定向的RTK、基于地图的视觉定位、激光寻北、机械码盘，乃至光学三维动作捕捉分析***Vicon等，这些方法虽然解决了磁场干扰问题，但也引入了新的限制条件，安装和使用都不如IMU方便，价格也远高于IMU。因此解决或削弱IMU的漂移和磁干扰仍然很有价值。

发明内容

针对现有技术存在的问题，本发明提供一种基于多惯性测量单元IMU，融合的姿态检测方法及装置，主要是通过设计一种多IMU的安装方法，使得装置在测量中总存在一个IMU可以获得漂移较小的偏航角，从而削弱了无电子罗盘下IMU测量偏航角的漂移，又根据磁场干扰、加速度或角速度突变情况计算了干扰系数和突变系数，据此对多IMU的数据进行融合，提高了测量装置的抗干扰能力和检测精度，具有一定的实际应用价值。

本发明提供了一种基于多IMU融合的姿态检测方法，所述姿态检测方法运行在嵌入式***中，该嵌入式***包括硬件和软件，所述硬件包括嵌入式处理器、一个主IMU和若干辅IMU，其中主IMU挂载了电子罗盘，辅IMU无需挂载电子罗盘，所述软件运行在嵌入式微处理器上；所述姿态检测方法的具体步骤如下：

S1、姿态检测开始时，先进行初始化，分别校准主IMU和辅IMU的加速度计和陀螺仪，同时校准挂载在主IMU下的电子罗盘；

S2、根据电子罗盘采集的磁场强度，计算磁场干扰强度，并根据磁场干扰强度确定偏航角的初始值：

S21、设电子罗盘测量到其所在环境的三轴磁场强度为M(m_x,m_y,m_z)，则磁场强度m的表达式为：

S22、设地磁强度为m_地，则磁场干扰强度m_扰的表达式为：

m_扰＝m-m_地

并记录初始磁场干扰强度为m_初；

S23、如果磁场干扰强度m_扰大于设定的阈值，则偏航角有效位置0，同时将偏航角初值设为0，并记录在变量Y中；

S24、如果磁场干扰强度m_扰小于设定的阈值，则偏航角有效位置1，主IMU结合电子罗盘数据使用AHRS姿态解算算法解算得到当前偏航角，将解算得到的当前偏航角作为偏航角的初始值，并记录在变量Y中；

S3、运行多IMU融合削弱偏航角漂移的算法，计算主IMU和辅IMU的倾斜角，使用倾斜角最接近45度或135度的一个IMU来观测偏航角增量，得到多IMU融合的弱漂移的偏航角：

S31、采用6轴IMU姿态解算算法解算得到第i个IMU的三轴姿态欧拉角，即横滚角R_i、俯仰角P_i和偏航角Y_i，其中i＝0、1、2……，i＝0时代表主IMU的数据，i＝1、2……代表辅IMU的数据；

S32、根据步骤S31中每一个IMU的横滚角和偏航角计算出其Z轴与垂直于地平面向上的法向量的夹角，即倾斜角T_i：

S33、为了方便比较各IMU倾斜角的大小以选取用于观测的IMU，将步骤S32中第i个IMU的倾斜角T_i转化为用于比较的倾斜角D_i：

S34、设第i个IMU的偏航角增量为ΔY_i，选择步骤S33中用于比较的倾斜角的绝对值最小的IMU的偏航角作为观测偏航角，则观测偏航角的增量ΔY_观为：

ΔY_观＝ΔY_k(k＝index(min(|D_i|)))

则弱漂移的偏航角的增量ΔY_测为：

ΔY_测＝sign(ΔY₀ΔY_观)ΔY_观

多IMU融合的弱漂移的偏航角为：

Y＝Y+ΔY_测；

S4、运行多IMU融合抗磁干扰的偏航角测量算法，根据磁场干扰、加速度或角速度突变情况计算干扰系数S和突变因子I，再将AHRS姿态解算算法解算的偏航角和步骤S3中多IMU融合的弱漂移的偏航角融合，获得多IMU融合抗磁干扰的偏航角：

S41、根据步骤S2中方法计算磁场干扰强度m_扰，结合步骤S22中得到的初始磁场干扰强度m_初，得到磁场干扰强度变化m_变的表达式为：

m_变＝m_扰-m_初

设磁场变化影响系数为k，由此得到干扰系数S的表达式为：

S＝km_变+(1-k)m_扰；

S42、根据主IMU加速度计采集到的加速度A(a_x,a_y,a_z)和陀螺仪采集到的角速度W(w_x,w_y,w_z)，用加速度的增量dA(a_x,a_y,a_z)、加速度的突变阈值dA_Max、角速度的增量dW(w_x,w_y,w_z)和角速度的突变阈值dW_Max将突变进行量化，得到突变因子I的表达式为：

其中，h为加速度突变影响系数；

S43、根据步骤S41和步骤S42得到的干扰系数S和突变因子I，设干扰阈值为S_Max、设突变阈值为I_Max，计算融合系数v的表达式为：

S44、根据干扰系数S是否达到干扰阈值S_Max，以及突变因子I是否达到突变阈值I_Max，得到融合后的偏航角Y的表达式：

其中，ΔY_测为步骤S34中得到的弱漂移的偏航角的增量，ΔY为主IMU结合电子罗盘数据使用AHRS姿态解算算法解算得到的偏航角与当前偏航角Y的差值；

S5、主IMU使用6轴IMU姿态解算算法解算得到装置的俯仰角和横滚角；

S6、重复步骤S3至步骤S5，完成装置姿态的实时检测。

可优选的是，在步骤S1中，量化了磁场干扰，并根据量化的磁场干扰获得了初始偏航角。

可优选的是，在步骤S3中，6轴IMU姿态解算算法为使用三轴加速度计和三轴陀螺仪数据解算姿态的算法，其中可选择使用卡尔曼滤波算法或互补滤波算法。

可优选的是，在多IMU融合抗磁干扰的偏航角测量算法中，通过干扰系数和突变因子，并融合主IMU和副IMU的数据，得到检测装置的抗磁干扰的偏航角。

本发明的另一方面，提供一种利用前述基于多IMU融合的姿态检测方法的姿态检测装置，所述多IMU融合削弱偏航角漂移的算法和所述多IMU融合抗磁干扰的偏航角测量算法均运行在采用多IMU融合削弱偏航角漂移的IMU安装方法制作的装置中；其中多IMU融合削弱偏航角漂移的IMU安装方法，即各IMU的安装保持一定的角度关系，使得测量装置在运动中总有一个IMU的Z轴与垂直于地平面向上的法向量和地平面都有不小于20度的夹角，电子罗盘的坐标轴与主IMU的坐标轴平行且方向一致。

可优选的是，所述20度的夹角为多IMU融合削弱偏航角漂移的IMU安装方法中优选的一个边界角度值，可选用附近的其他角度值。

本发明与现有技术相比，具有如下优点：

1、本发明提出了多IMU融合削弱偏航角漂移的IMU安装方法，使得装置选装过程中，总存在一个IMU可以获得漂移较小的偏航角；

2、本发明提出了一种多IMU融合削弱偏航角漂移的算法，削弱了无电子罗盘下IMU测量偏航角的漂移，提高了姿态检测算法的精度；

3、本发明提出了一种多IMU融合抗磁干扰的偏航角测量算法，提高了基于IMU测量偏航角的抗磁干扰能力，同时削弱了加速度和角速度数据突变对偏航角测量的影响，提高了姿态检测算法的容错能力。

附图说明

图1为本发明基于多IMU融合的姿态检测方法及其姿态检测装置的***框图；

图2为本发明基于多IMU融合的姿态检测方法及其姿态检测装置中IMU的安装方式图；

图3为本发明基于多IMU融合的姿态检测方法及其姿态检测装置中弱漂移偏航角可靠测量覆盖范围实例图；

图4为本发明基于多IMU融合的姿态检测方法及其姿态检测装置中嵌入式处理器程序流程图。

具体实施方式

为详尽本发明之技术内容、所达成目的及功效，以下将结合说明书附图进行详细说明。

如图1所示，姿态检测方法运行在嵌入式***中，该嵌入式***包括硬件和软件，硬件包括嵌入式处理器、一个主IMU和若干辅IMU，其中主IMU挂载了电子罗盘，辅IMU无需挂载电子罗盘，软件运行在嵌入式微处理器上；辅IMU向嵌入式处理器发送表示自身的姿态三个欧拉角，主IMU向嵌入式处理发送表示自身姿态的三个欧拉角、磁场强度、三轴加速度和角速度，嵌入式处理器可以向主IMU发送指令切换姿态解算算法。

多IMU融合削弱偏航角漂移的算法和多IMU融合抗磁干扰的偏航角测量算法均运行在采用多IMU融合削弱偏航角漂移的IMU安装方法制作的装置中；其中多IMU融合削弱偏航角漂移的IMU安装方法，即各IMU的安装保持一定的角度关系，使得测量装置在运动中总有一个IMU的Z轴与垂直于地平面向上的法向量和地平面都有不小于20度的夹角，电子罗盘的坐标轴与主IMU的坐标轴平行且方向一致。

在本发明的一个优选实施方式中，20度的夹角为多IMU融合削弱偏航角漂移的IMU安装方法中优选的一个边界角度值，可选用附近的其他角度值。

基于多IMU融合的姿态检测方法，如图4所示，具体的实施步骤如下：

S1、姿态检测开始时，先进行初始化，分别校准主IMU和辅IMU的加速度计和陀螺仪，同时校准挂载在主IMU下的电子罗盘。

S2、电子罗盘采集的磁场强度，计算磁场干扰强度，并根据磁场干扰强度确定偏航角的初始值：

S21、设电子罗盘测量到其所在环境的三轴磁场强度为M(m_x,m_y,m_z)，则磁场强度m的表达式为：

S22、设地磁强度为m_地，由于地磁强度十分微弱，而磁干扰在近场产生，强度一般远大于地磁强度，近似地，则磁场干扰强度m_扰的表达式为：

m_扰＝m-m_地

并记录初始磁场干扰强度为m_初。

S23、如果磁场干扰强度m_扰大于设定的阈值，则偏航角有效位置0，同时将偏航角初值设为0，并记录在变量Y中。

S24、如果磁场干扰强度m_扰小于设定的阈值，则偏航角有效位置1，主IMU结合电子罗盘数据使用航姿参考系AHRS姿态解算算法解算得到当前偏航角，将解算得到的当前偏航角作为偏航角的初始值，并记录在变量Y中。

S3、运行多IMU融合削弱偏航角漂移的算法，计算主IMU和辅IMU的倾斜角，使用倾斜角最接近45度或135度的一个IMU来观测偏航角增量，得到多IMU融合的弱漂移的偏航角：

S31、采用6轴IMU姿态解算算法解算得到第i个IMU的三轴姿态欧拉角，即横滚角R_i、俯仰角P_i和偏航角Y_i，其中i＝0、1、2……，i＝0时代表主IMU的数据，i＝1、2……代表辅IMU的数据；其中6轴IMU姿态解算算法为使用三轴加速度计和三轴陀螺仪数据解算姿态的算法，其中可选择使用卡尔曼滤波算法或互补滤波算法。

S32、根据步骤S31中每一个IMU的横滚角和偏航角计算出其Z轴与垂直于地平面向上的法向量的夹角，即倾斜角T_i：

S33、为了方便比较各IMU倾斜角的大小以选取用于观测的IMU，将步骤S32中第i个IMU的倾斜角T_i转化为用于比较的倾斜角D_i，比较倾斜角D_i可以通过比较其绝对值，来直接地选取用于观测的IMU：

S34、设第i个IMU的偏航角增量为ΔY_i，选择步骤S33中用于比较的倾斜角的绝对值最小的IMU的偏航角作为观测偏航角，则观测偏航角的增量ΔY_观为：

ΔY_观＝ΔY_k(k＝index(min(|D_i|)))

则弱漂移的偏航角的增量ΔY_测为：

ΔY_测＝sign(ΔY₀ΔY_观)ΔY_观

多IMU融合的弱漂移的偏航角为：

Y＝Y+ΔY_测。

S4、运行多IMU融合抗磁干扰的偏航角测量算法，根据磁场干扰、加速度或角速度突变情况计算干扰系数S和突变因子I，再将AHRS姿态解算算法解算的偏航角和步骤S3中多IMU融合的弱漂移的偏航角融合，获得多IMU融合抗磁干扰的偏航角，其中AHRS姿态解算算法即使用三轴加速度计、三轴陀螺仪、三轴电子罗盘数据解算姿态的算法：

S41、根据步骤S2中方法计算磁场干扰强度m_扰，结合步骤S22中得到的初始磁场干扰强度m_初，得到磁场干扰强度变化m_变的表达式为：

m_变＝m_扰-m_初

设磁场变化影响系数为k，由此得到干扰系数S的表达式为：

S＝km_变+(1-k)m_扰。

S42、根据主IMU加速度计采集到的加速度A(a_x,a_y,a_z)和陀螺仪采集到的角速度W(w_x,w_y,w_z)，用加速度的增量dA(a_x,a_y,a_z)、加速度的突变阈值dA_Max、角速度的增量dW(w_x,w_y,w_z)和角速度的突变阈值dW_Max将突变进行量化，得到突变因子I的表达式为：

其中，h为加速度突变影响系数。

S43、根据步骤S41和步骤S42得到的干扰系数S和突变因子I，设干扰阈值为S_Max、设突变阈值为I_Max，计算融合系数v的表达式为：

S44、根据干扰系数S是否达到干扰阈值S_Max，以及突变因子I是否达到突变阈值I_Max，得到融合后的偏航角Y的表达式：

其中，ΔY_测为步骤S34中得到的弱漂移的偏航角的增量，ΔY为主IMU结合电子罗盘数据使用AHRS姿态解算算法解算得到的偏航角与当前偏航角Y的差值。

S5、主IMU使用6轴IMU姿态解算算法解算得到装置的俯仰角和横滚角。

S6、重复步骤S3至步骤S5，完成装置姿态的实时检测。

进一步，为了获得装置的初始偏航角，在步骤S1中，对磁场干扰进行量化，并根据量化的磁场干扰获得了初始偏航角。

在多IMU融合抗磁干扰的偏航角测量算法中，通过计算干扰系数和突变因子，融合了主IMU和副IMU的数据，得到检测装置的抗磁干扰的偏航角。

以下结合实施例对本发明一种基于多IMU融合的姿态检测方法及其姿态检测装置做进一步描述：

当本发明应用于无人机、无人车等载体的姿态检测时，姿态检测方法运行在嵌入式***中，该嵌入式***包括硬件和软件，如图1所示，硬件包括嵌入式处理器STM32、一个主IMU和三个辅IMU，其中主IMU选用MPU9250，辅IMU选用MPU6050，软件运行在嵌入式微处理器上。

嵌入式处理器通过1个IIC接口读取主IMU的三轴加速度、角速度和磁场强度，通过3个IIC接口读取3个辅IMU的三轴加速度和角速度，嵌入式处理器可通过串口等接口与控制器相连，并通过该接口向控制器发送融合后的偏航角。

采用多IMU融合削弱偏航角漂移的IMU安装方法设计该测量装置，即各IMU的安装保持一定的角度关系，使得测量装置在运动中总有一个IMU的Z轴与垂直于地平面向上的法向量和地平面都有不小于20度的夹角，电子罗盘的坐标轴与主IMU的坐标轴平行且方向一致，该安装方式保证了在低速运动时受到强磁干扰，总有一个IMU可以获得漂移较小的相对偏航角。

如图2所示，将四个IMU安装在一个具有特殊位置关系的四面体的四个面上，四面体的底面为正三角形，四面体的上面三个面与底面的夹角均为60度，主IMU安装在底面上，Z轴垂直底面向上，X轴与一边平行，即图2所示的坐标系C0，辅IMU贴在四面体上面的三个面上，Z轴垂直于所在的面向外，X轴与底面平行，即图2所示坐标系C1、C2、C3，设主IMU的安装位置的欧拉角为(0,0,0)，则三个辅IMU的安装位置的欧拉角为(0,60,60)、(0,-60,-60)和(60,0,90)。

按着上述方法设计的装置的弱漂移偏航角可靠测量的覆盖范围可在三维制图软件中表示出来，如图3所示。将C1、C2、C3的坐标原点与C0重合，以各坐标系的Z轴为轴，原点为顶点，有效倾斜角为母线与轴的夹角，绘制对角圆锥，再将圆锥内无效倾斜角覆盖的圆锥挖去，图3中绘制的圆锥母线与轴的夹角为30度至60度，空间基本被覆盖，当夹角扩大到20度至70度时，可完全覆盖。

对于无人机、无人车等载体的多IMU融合的姿态检测方法，如图4所示，具体的实施步骤如下：

S1、姿态检测开始时，先进行初始化，各IMU采集温漂用于加速度计和陀螺仪的校准，主IMU的电子罗盘采集磁场强度，若超出地磁强度范围内，即0.5至0.6高斯，则初始偏航角无效，偏航角初值有效位置0，偏航角初值为0，否则初始偏航角有效，偏航角初值有效位置1，使用电子罗盘解算当前偏航角作为初值。

S2、初始化完成后，根据电子罗盘采集的磁场强度，计算磁场干扰强度，并根据磁场干扰强度确定偏航角的初始值：

S21、设电子罗盘测量到其所在环境的三轴磁场强度为M(m_x,m_y,m_z)，则磁场强度m的表达式为：

S22、设地磁强度为m_地，由于地磁强度十分微弱，而磁干扰在近场产生，强度一般远大于地磁强度，近似地，则磁场干扰强度m_扰的表达式为：

m_扰＝m-m_地

并记录初始磁场干扰强度为m_初。

S23、如果磁场干扰强度m_扰大于设定的阈值，则偏航角有效位置0，同时将偏航角初值设为0，并记录在变量Y中。

S24、如果磁场干扰强度m_扰小于设定的阈值，则偏航角有效位置1，主IMU结合电子罗盘数据使用AHRS(航姿参考系)姿态解算算法解算得到当前偏航角，将解算得到的当前偏航角作为偏航角的初始值，并记录在变量Y中。

S3、运行多IMU融合削弱偏航角漂移的算法，计算主IMU和辅IMU的倾斜角，使用倾斜角最接近45度或135度的一个IMU来观测偏航角增量，得到多IMU融合的弱漂移的偏航角：

S31、采用6轴IMU姿态解算算法解算得到第i个IMU的三轴姿态欧拉角，即横滚角R_i、俯仰角P_i和偏航角Y_i，其中i＝0、1、2……，i＝0时代表主IMU的数据，i＝1、2……代表辅IMU的数据；其中6轴IMU姿态解算算法为使用三轴加速度计和三轴陀螺仪数据解算姿态的算法，其中可选择使用卡尔曼滤波算法或互补滤波算法。

S32、根据步骤S31中每一个IMU的横滚角和偏航角计算出其Z轴与垂直于地平面向上的法向量的夹角，即倾斜角T_i：

S33、为了方便比较各IMU倾斜角的大小以选取用于观测的IMU，采样周期内各IMU的偏航角增量为ΔY_i，将步骤S32中第i个IMU的倾斜角T_i转化为用于比较的倾斜角D_i，：

S34、设第i个IMU的偏航角增量为ΔY_i，选择步骤S33中用于比较的倾斜角的绝对值最小的IMU的偏航角作为观测偏航角，则观测偏航角的增量ΔY_观为：

ΔY_观＝ΔY_k(k＝index(min(|D_i|)))

则弱漂移的偏航角的增量ΔY_测为：

ΔY_测＝sign(ΔY₀ΔY_观)ΔY_观。

S4、运行多IMU融合抗磁干扰的偏航角测量算法，根据磁场干扰、加速度或角速度突变情况计算干扰系数S和突变因子I，再将AHRS姿态解算算法解算的偏航角和步骤S3中多IMU融合的弱漂移的偏航角融合，获得多IMU融合抗磁干扰的偏航角，其中AHRS姿态解算算法即使用三轴加速度计、三轴陀螺仪、三轴电子罗盘数据解算姿态的算法：

S41、根据步骤S2中方法计算磁场干扰强度m_扰，结合步骤S22中得到的初始磁场干扰强度m_初，得到磁场干扰强度变化m_变的表达式为：

m_变＝m_扰-m_初

设磁场变化影响系数为k，由此得到干扰系数S的表达式为：

S＝km_变+(1-k)m_扰。

S42、根据主IMU加速度计采集到的加速度A(a_x,a_y,a_z)和陀螺仪采集到的角速度W(w_x,w_y,w_z)，用加速度的增量dA(a_x,a_y,a_z)、加速度的突变阈值dA_Max、角速度的增量dW(w_x,w_y,w_z)和角速度的突变阈值dW_Max将突变进行量化，得到突变因子I的表达式为：

I＝h|dA|+(1-h)|dW|

其中，h为加速度突变影响系数。

S43、根据步骤S41和步骤S42得到的干扰系数S和突变因子I，设干扰阈值为S_Max、设突变阈值为I_Max，计算融合系数v的表达式为：

S44、干扰系数S达到干扰阈值S_Max，且突变因子I未达到突变阈值I_Max，则应运行步骤S3的多IMU融合削弱偏航角漂移的算法来计算弱漂移的偏航角Y的增量，详细计算过程见步骤S3；当干扰系数S达到干扰阈值S_Max，且突变因子I也达到了突变阈值I_Max，此时需要计算融合系数v，详细计算过程见步骤S43，将电子罗盘观测角度和多IMU的弱漂移的偏航角融合为：

S5、主IMU使用6轴IMU姿态解算算法解算得到装置的俯仰角和横滚角。

S6、重复步骤S3至步骤S5，完成装置姿态的实时检测。

以上所述的实施例仅是对本发明的优选实施方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (5)

1.一种基于多IMU融合的姿态检测方法，其特征在于，所述姿态检测方法运行在嵌入式***中，该嵌入式***包括硬件和软件，所述硬件包括嵌入式处理器、一个主IMU和若干辅IMU，其中主IMU挂载了电子罗盘，辅IMU无需挂载电子罗盘，所述软件运行在嵌入式微处理器上；所述姿态检测方法的具体步骤如下：

S1、姿态检测开始时，先进行初始化，分别校准主IMU和辅IMU的加速度计和陀螺仪，同时校准挂载在主IMU下的电子罗盘；

S2、根据电子罗盘采集的磁场强度，计算磁场干扰强度，并根据磁场干扰强度确定偏航角的初始值：

S21、设电子罗盘测量到其所在环境的三轴磁场强度为M(m_x,m_y,m_z)，则磁场强度m的表达式为：

S22、设地磁强度为m_地，则磁场干扰强度m_扰的表达式为：

m_扰＝m-m_地

并记录初始磁场干扰强度为m_初；

S23、如果磁场干扰强度m_扰大于设定的阈值，则偏航角有效位置0，同时将偏航角初值设为0，并记录在变量Y中；

S24、如果磁场干扰强度m_扰小于设定的阈值，则偏航角有效位置1，主IMU结合电子罗盘数据使用AHRS姿态解算算法解算得到当前偏航角，将解算得到的当前偏航角作为偏航角的初始值，并记录在变量Y中；

S3、运行多IMU融合削弱偏航角漂移的算法，计算主IMU和辅IMU的倾斜角，使用倾斜角最接近45度或135度的一个IMU来观测偏航角增量，得到多IMU融合的弱漂移的偏航角：

S31、采用6轴IMU姿态解算算法解算得到第i个IMU的三轴姿态欧拉角，即横滚角R_i、俯仰角P_i和偏航角Y_i，其中i＝0、1、2……，i＝0时代表主IMU的数据，i＝1、2……代表辅IMU的数据；

S32、根据步骤S31中每一个IMU的横滚角和俯仰角计算出其Z轴与垂直于地平面向上的法向量的夹角，即倾斜角T_i：

S33、为了方便比较各IMU倾斜角的大小以选取用于观测的IMU，将步骤S32中第i个IMU的倾斜角T_i转化为用于比较的倾斜角D_i：

S34、设第i个IMU的偏航角增量为ΔY_i，选择步骤S33中用于比较的倾斜角的绝对值最小的IMU的偏航角作为观测偏航角，则观测偏航角的增量ΔY_观为：

ΔY_观＝ΔY_k(k＝index(min(|D_i|)))

则弱漂移的偏航角的增量ΔY_测为：

ΔY_测＝sign(ΔY₀ΔY_观)ΔY_观

多IMU融合的弱漂移的偏航角为：

Y＝Y+ΔY_测；

S4、运行多IMU融合抗磁干扰的偏航角测量算法，根据磁场干扰、加速度或角速度突变情况计算干扰系数S和突变因子I，再将AHRS姿态解算算法解算的偏航角和步骤S3中多IMU融合的弱漂移的偏航角融合，获得多IMU融合抗磁干扰的偏航角：

S41、根据步骤S2中方法计算磁场干扰强度m_扰，结合步骤S22中得到的初始磁场干扰强度m_初，得到磁场干扰强度变化m_变的表达式为：

m_变＝m_扰-m_初

设磁场变化影响系数为k，由此得到干扰系数S的表达式为：

S＝km_变+(1-k)m_扰；

S42、根据主IMU加速度计采集到的加速度A(a_x,a_y,a_z)和陀螺仪采集到的角速度W(w_x,w_y,w_z)，用加速度的增量dA(a_x,a_y,a_z)、加速度的突变阈值dA_Max、角速度的增量dW(w_x,w_y,w_z)和角速度的突变阈值dW_Max将突变进行量化，得到突变因子I的表达式为：

其中，h为加速度突变影响系数；

S43、根据步骤S41和步骤S42得到的干扰系数S和突变因子I，设干扰阈值为S_Max、设突变阈值为I_Max，计算融合系数v的表达式为：

S44、根据干扰系数S是否达到干扰阈值S_Max，以及突变因子I是否达到突变阈值I_Max，得到融合后的偏航角Y的表达式：

其中，ΔY_测为步骤S34中得到的弱漂移的偏航角的增量，ΔY为主IMU结合电子罗盘数据使用AHRS姿态解算算法解算得到的偏航角与当前偏航角Y的差值；

S5、主IMU使用6轴IMU姿态解算算法解算得到装置的俯仰角和横滚角；

S6、重复步骤S3至步骤S5，完成装置姿态的实时检测。

2.根据权利要求1所述的基于多IMU融合的姿态检测方法，其特征在于，在步骤S1中，量化了磁场干扰，并根据量化的磁场干扰获得了初始偏航角。

3.根据权利要求1所述的基于多IMU融合的姿态检测方法，其特征在于，在步骤S3中，6轴IMU姿态解算算法为使用三轴加速度计和三轴陀螺仪数据解算姿态的算法，其中也选择使用卡尔曼滤波算法或互补滤波算法。

4.根据权利要求1所述的基于多IMU融合的姿态检测方法，其特征在于，在多IMU融合抗磁干扰的偏航角测量算法中，通过计算干扰系数和突变因子，融合主IMU和副IMU的数据，得到检测装置的抗磁干扰的偏航角。

5.一种利用权利要求1-4中任一项所述的基于多IMU融合的姿态检测方法的姿态检测装置，其特征在于，所述多IMU融合削弱偏航角漂移的算法和所述多IMU融合抗磁干扰的偏航角测量算法均运行在采用多IMU融合削弱偏航角漂移的IMU安装方法制作的装置中；其中多IMU融合削弱偏航角漂移的IMU安装方法，即各IMU的安装保持一定的角度关系，使得测量装置在运动中总有一个IMU的Z轴与垂直于地平面向上的法向量和地平面都有不小于20度的夹角，电子罗盘的坐标轴与主IMU的坐标轴平行且方向一致。

Images