一种姿态角的融合解算方法和装置与流程

本发明涉及姿态角测量技术领域，具体涉及一种姿态角的融合解算方法和装置。

背景技术：

姿态角主要是指物体的俯仰角、偏航角和翻滚角，可用来表征物体的姿态，姿态角度的测量在控制、机械、通信、航空航天等技术领域得到了广泛的应用，其主要利用惯性测量单元(inertialmeasurementunit，imu)进行测量。一般而言，imu包含加速度计和陀螺仪，加速度计用于检测物体在载体坐标系中独立三轴的加速度信号，而陀螺仪用于检测物体相对于地理坐标系的角速度信号，通过测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。

在具体利用imu解算姿态角信息时，陀螺仪自身的工作原理决定了其长时间的积分会导致误差的累积，使测量值随时间而漂移，导致姿态角解算的精度降低，因而需要利用外部信息对陀螺仪的漂移进行修正。磁力计是一种直接感测地磁场信息的传感器，其没有累积误差，工作较为稳定，因而可用来修正陀螺仪的漂移。目前，在进行姿态角解算时，主要利用的是互补滤波或者卡尔曼滤波，但利用互补滤波解算出的姿态角的跟随性较差，而通过卡尔曼滤波器解算姿态角时的收敛性又较慢，在动态条件下采用这两种方法进行姿态角的解算时都不利于姿态角的更新。因此，针对imu和磁力计构成的姿态角测量单元，现有的姿态角解算方法还无法实现在提高动态条件下姿态的跟随性的同时，又能使姿态角达到较快的收敛速度。

技术实现要素：

本申请提供一种姿态角的融合解算方法和装置，以实现在姿态角的解算过程中，既提高动态条件下姿态的跟随性，又使姿态角达到较快的收敛速度。

根据第一方面，一种实施例中提供一种姿态角的融合解算方法，包括：

根据陀螺仪的测量数据解算出第一姿态角；

根据加速度计的测量数据和磁力计的测量数据解算出第二姿态角；

根据互补滤波原理对所述第一姿态角和所述第二姿态角进行融合计算，得到第三姿态角；

以所述第三姿态角为第一状态变量且互补滤波后获得的所述第二姿态角为观测量，利用扩展卡尔曼滤波计算得到融合姿态角。

根据第二方面，一种实施例中提供一种姿态角融合解算的装置，包括：

陀螺仪，用于检测物体相对于地理坐标系的角速度信号并输出；

加速度计，用于检测物体在载体坐标系中三个轴的加速度信号并输出；

磁力计，用于检测地磁场的强度和方向信息并输出；

数据处理器，用于执行如上述方案所述的姿态角的融合解算方法。

依据上述实施例的姿态角的融合解算方法和装置，首先根据陀螺仪的测量数据解算出第一姿态角，且根据加速度计的测量数据和磁力计的测量数据解算出第二姿态角；接着根据互补滤波原理对第一姿态角和第二姿态角进行融合计算，得到第三姿态角；然后以第三姿态角为状态变量且互补滤波后获得的第二姿态角为观测量，利用扩展卡尔曼滤波计算得到融合姿态角，从而将互补滤波和卡尔曼滤波相结合，融合解算出姿态角，弥补了互补滤波和卡尔曼滤波各自的缺陷，既能够提高动态条件下姿态角的跟随性，又能够达到较快的收敛速度。

附图说明

图1为一种实施例中姿态角的融合解算装置的结构示意图；

图2为一种实施例中数据处理器的结构示意图；

图3为一种具体实施例中姿态角的融合解算方法的流程图；

图4为一种具体实施例中计算融合姿态角的流程图；

图5为一种具体实施例中利用卡尔曼滤波算法求取融合姿态角的流程图；

图6为另一种实施例中数据处理器的结构示意图；

图7为一种具体实施例中对磁力计进行校准的流程图；

图8为一种具体实施例中利用卡尔曼滤波迭代对磁力计的测量数据进行校准的流程图。

具体实施方式

下面通过具体实施方式结合附图对本发明作进一步详细说明。在某些情况下，本申请相关的一些操作并没有在说明书中显示或者描述，这是为了避免本申请的核心部分被过多的描述所淹没，而对于本领域技术人员而言，详细描述这些相关操作并不是必要的，他们根据说明书中的描述以及本领域的一般技术知识即可完整了解相关操作。

另外，说明书中所描述的特点、操作或者特征可以以任意适当的方式结合形成各种实施方式。同时，方法描述中的各步骤或者动作也可以按照本领域技术人员所能显而易见的方式进行顺序调换或调整。

姿态角是指物体的俯仰角、偏航角和翻滚角，在本文中，若无特殊说明，这三个角均分别用φ、θ和ψ表示，带有下标的φ、θ和ψ也分别代表这三个角。

在本发明实施例中，先根据互补滤波原理将陀螺仪解算出的第一姿态角和加速度计与磁力计解算出第二姿态角进行融合计算，得到第三姿态角，再以第三姿态角为状态变量建立卡尔曼滤波的状态方程，以互补滤波后获得的第二姿态角为观测量进行姿态角更新，以此对互补滤波和卡尔曼滤波进行信息融合来解算姿态角。

实施例一：

图1为本发明一种实施例中姿态角的融合解算装置的结构示意图，如图1所示，该装置包括陀螺仪1、加速度计2、磁力计3和数据处理器4。陀螺仪1用于检测物体相对于地理坐标系的角速度信号并输出；加速度计2用于检测物体在载体坐标系中三个轴的加速度信号并输出；磁力计3用于检测地磁场的强度和方向信息并输出；数据处理器4用于获取陀螺仪1、加速度计2和磁力计3输出的信号，并对这些信号进行放大、去除噪声、计算等处理。

在本发明的一种实施例中，数据处理器4的结构示意图如图2所示，包括第一计算模块41、第二计算模块42和第三计算模块43。第一计算模块41用于根据陀螺仪的测量数据解算出第一姿态角，且根据加速度计的测量数据和磁力计的测量数据解算出第二姿态角；第二计算模块42用于根据互补滤波原理对第一计算模块41解算出的第一姿态角和第二姿态角进行融合计算，得到第三姿态角；第三计算模块43用于以第二计算模块42融合计算出的第三姿态角为状态变量，且以互补滤波后获得的第二姿态角为观测量，利用扩展卡尔曼滤波计算得到融合姿态角。

在一具体实施例中，图3示出了姿态角的融合解算方法的流程图，如图3所示，该方法包括如下步骤：

步骤s11：解算第一姿态角和第二姿态角。

第一计算模块41根据陀螺仪三轴方向上的测量数据解算出第一姿态角φg、θg和ψg，且根据加速度计三轴方向上的测量数据和磁力计三轴方向上的测量数据解算出第二姿态角φam、θam和ψam。

第一计算模块41在解算出第一姿态角和第二姿态角之后，由第二计算模块42根据互补滤波原理对该第一姿态角和第二姿态角进行融合计算而得到第三姿态角，其具体步骤包括步骤s12和步骤s13，具体为：

步骤s12：计算权值f。

第二计算模块42根据加速度计的测量数据、磁力计的测量数据和地球的当地磁场值，利用公式(1)计算出权值f，其中的公式(1)为：

在(1)式中，mg为地球的当地磁场值，ax、ay、az分别为加速度计的三个轴归一化后的测量数据，mx、my、mz分别为磁力计的三个轴的测量数据。

步骤s13：计算第三姿态角。

第二计算模块42根据第一姿态角φg、θg和ψg、第二姿态角φam、θam和ψam以及由公式(1)计算出的f，利用公式(2)计算出第三姿态角φ、θ和ψ，其中公式(2)为：

由公式(2)可知，第三姿态角是在第一姿态角的基础上加上第二姿态角和权值f的乘积得到的，这样，第二姿态角和权值f的乘积便可看作是第一姿态角的修正项，而第一姿态角是根据陀螺仪的测量数据解算出的姿态角，第二姿态角是根据加速度计和磁力计的测量数据解算出的姿态角，由此便可实现磁力计对陀螺仪漂移的修正，提高了姿态角解算的精度。

另一方面，在实时动态条件下，由于加速度计包含有重力加速度和运动加速度，两者无法区分，而在利用加速度计和磁力计一起解算姿态角时依托的是地球重力场，即利用的是重力加速度，这时，运动加速度便成为误差项，如果直接利用加速度计的测量数据解算姿态角，则会降低解算出的姿态角的精度；考虑到利用加速度计和磁力计解算姿态角与利用陀螺仪解算姿态角均存在信息冗余，这时通过设置权值f来调节两者解算出的姿态角的比重；由f的计算公式可知，当运动加速度的值越大时，ax、ay和az的值将越大，这时权值f将越小，利用加速度计和磁力计解算出的姿态角在第三姿态角中的比重也将越小，从而可以在动态条件下自适应的调整权值f，以抑制运动加速度对姿态角解算的影响，进而提高姿态角解算的精度。

步骤s14：计算融合姿态角。

第二计算模块42根据第一姿态角、第二姿态角和权值f融合计算出第三姿态角之后，第三计算模块43以第三姿态角φ、θ和ψ为状态变量，且以互补滤波后获得的第二姿态角为观测量，利用扩展卡尔曼滤波计算得到融合姿态角。图4示出了第三计算模块43计算融合姿态角的流程图，如图4所示，该过程可由步骤sa1至步骤sa4来实现，具体为：

步骤sa1：建立状态变量。

第三计算模块43以第三姿态角φ、θ和ψ为状态变量，令卡尔曼滤波的状态变量xk为：

xk＝(φkθkψk)^t

其中，φk、θk和ψk为k时刻的第三姿态角。

步骤sa2：建立姿态角的微分方程。

假设从地理坐标系到载体坐标系的三次旋转顺序为φ→θ→ψ，那么，载体坐标系相对于地理坐标系的角速率向量可用公式(3)表示：

其中，ωx、ωy和ωz为角速率向量，和为三个姿态角向量。由公式(3)可得到姿态角的微分方程为公式(4)：

步骤sa3：建立卡尔曼滤波的状态转移矩阵d。

接着，第三计算模块43采用一阶差分对姿态角的微分方程(4)进行离散化处理，得到离散化方程为公式(5)：

其中，φk-1、θk-1和ψk-1为k-1时刻的第三姿态角，ωxk^t、ωyk^t和ωzk^t分别为k时刻的角速率向量ωxk、ωyk和ωzk的转置。

在建立卡尔曼滤波的过程中，由于卡尔曼滤波通常处理的是线性问题，而得到的离散化方程(5)是非线性方程，因而需要利用扩展卡尔曼滤波对其进行线性化处理，通过计算离散化方程(5)的雅可比矩阵来建立卡尔曼滤波的状态转移矩阵d，其计算公式如公式(6)：

其中，dk|k-1为k-1时刻到k时刻的状态转移，即为状态转移矩阵d。

步骤sa4：利用卡尔曼滤波算法计算融合姿态角。

选取互补滤波后获得的φam、θam和ψam为观测量，由于建立的卡尔曼滤波的状态变量xk为三维姿态角，则第二量测矩阵h2k可以取为：

此时，量测方程可用如下的公式(7)来表示：

其中，z2k为估算值，vk＝(vφkvθkvψk)^t为观测噪声矩阵。

接着，第三计算模块43便可根据第一状态变量、第二量测矩阵和状态转移矩阵d，利用卡尔曼滤波算法来求取融合姿态角，图5为利用卡尔曼滤波算法求取融合姿态角的流程图，如图5所示，具体包括以下步骤：

步骤a1：更新状态变量。第三计算模块43通过状态方程更新状态变量，得到第一状态变量在k时刻的状态预测值，其中的状态方程为式(8)：

其中，为第一状态变量在k时刻的状态预测值，为k-1时刻的最优状态值，w2k-1为k-1时刻的状态噪声。

步骤a2：计算估算值。第三计算模块43根据量测方程(7)得到第一状态变量在k时刻的估算值，该估算值的计算公式为：

z2k|k-1＝h2k*x2k-1+v2k-1(9)

其中，z2k|k-1为根据k-1时刻的状态预测的第一状态变量在k时刻的估算值，x2k-1为第一状态变量在k-1时刻的状态，v2k-1为k-1时刻的估测噪声。

步骤a3：计算滤波增益矩阵和误差协方差矩阵。第三计算模块43分别根据公式(10)和公式(11)计算滤波增益矩阵k2k和对应的误差协方差矩阵p2k|k-1，公式(10)和公式(11)为：

k2k＝p2k|k-1*h2k^t*(h2k*p2k|k-1*h2k^t+r2k)^-1(10)

p2k|k-1＝e2k|k-1*p2k-1*e2^tk|k-1+q2k-1(11)

其中，h2k^t为h2k的转置，r2k为测量噪声协方差矩阵，e2k|k-1为状态方程系数，e2^tk|k-1为e2k|k-1的转置，q2k-1为系统噪声协方差矩阵。

步骤a4：计算融合姿态角。第三计算模块43根据公式(12)计算k时刻的融合姿态角公式(12)为：

其中，y2k为k时刻互补滤波后获得的φam、θam和ψam，用来进行姿态角的更新。

步骤a5：更新误差协方差矩阵。第三计算模块43根据公式(13)对误差协方差矩阵p2k|k-1进行更新，用于下一时刻的更新计算，公式(13)为：

p2k＝(i2-k2k*h2k)*p2k|k-1(13)

其中，p2k为更新得到的k时刻的误差协方差矩阵，i2为单位矩阵。

如此便可在动态条件下不断对融合姿态角进行更新，得到动态条件下的融合姿态角。

本实施例提供的姿态角的融合解算方法，首先，第一计算模块根据陀螺仪的测量数据解算出第一姿态角，并根据加速度计和磁力计的测量数据解算出第二姿态角，接着，第二计算模块确定权值，并根据互补滤波原理利用该权值对第一姿态角和第二姿态角进行融合计算，得到第三姿态角，以修正陀螺仪解算出的第一姿态角，其中的权值可根据加速度计测量值的变化而自适应调整，以自适应调节第二姿态角在第三姿态角中所占的比重，从而抑制运动加速度对姿态角解算的影响，提高了姿态角解算的精度；进一步的，第三计算模块以第三姿态角为状态变量建立姿态角微分方程，且以互补滤波后获得的第二姿态角为观测量，利用扩展卡尔曼滤波计算得到融合姿态角，从而将互补滤波和卡尔曼滤波相结合，融合解算出姿态角，弥补了互补滤波和卡尔曼滤波各自的缺陷，既能够提高动态条件下姿态角的跟随性，又达到了较快的收敛速度。

实施例二：

基于实施例一，图6为另一种实施例中数据处理器4的结构示意图，如图6所示，包括第一计算模块41、第二计算模块42、第三计算模块43和校准模块44。其中，校准模块44用于以磁力计的零偏为第二状态变量，利用卡尔曼滤波算法对磁力计的测量数据进行校准，得到磁力计校准后的测量数据；第一计算模块41、第二计算模块42和第三计算模块43的功能分别与实施例一中第一计算模块41、第二计算模块42和第三计算模块43的功能一一类同，与实施例一唯一不同的是，在本实施例中，磁力计的测量数据采用的是磁力计校准后的测量数据。

在一具体实施例中，图7示出了校准模块44对磁力计进行校准的流程图，如图7所示，包括以下步骤：

步骤sb1：建立零偏状态方程

在静态条件下，磁力计的误差模型可以用公式(14)来表示：

mm＝c1c2mi+b(14)

其中，为理想状态下三轴磁力计的输出，mm＝[mbxmbymbz]^t为三轴磁力计实际的测量输出值，

为磁力计三轴不正交误差，

为磁力计的三轴的灵敏度误差，

b＝[bxbybz]^t为磁力计的三轴的零偏误差。

以上三种误差在初始静态校准时需要利用转台进行全空间的采点，利用椭球拟合法进行参数的求解，得到校准矩阵写入磁力计中，从而对磁力计进行修正。但在实际使用中，利用磁力计来纠正陀螺仪的漂移时，磁力计工作在动态变化的磁场条件下，这时，磁力计会受到软磁干扰和硬磁干扰，从而引入额外的误差；软磁干扰对磁力计的影响相当于在公式(14)的左边左乘一项，而硬磁干扰对磁力计的影响又相当于在公式(14)的右边加上一项偏置项。因此，在动态条件下对磁力计的校准就是对这两种误差的校准。

但与静态磁力计的校准相比，磁力计在动态条件下没有静态时的依托转台，而且在校准采点过程中只能采取部分空间角的数据，无法对全空间进行采点，因此，本发明对磁力计的误差模型进行简化，认为磁力计的软磁干扰在大部分情况下影响较小，可以将其等效为磁力计的偏置来进行处理，而磁力计的硬磁干扰也可以看成零偏来进行修正，因而可以在动态条件下直接对磁力计的零偏进行校准。

由此，可以取磁力计的零偏为第二状态变量，该第二状态变量为：

x＝[bxbybz]^t

其中，x为第二状态变量，即磁力计的偏置项，反映的是动态条件下磁力计的软磁干扰和硬磁干扰。这时，对动态条件下磁力计的校准即为对该偏置项的校准；bx、by、bz分别为磁力计在x、y、z三轴方向上的零偏，t代表转置。

在实际应用中，磁力计的零偏为常值，一些外部缓变干扰在短时间内也可看成是常值，这时，对状态变量的微分值为零，故校准模块44建立的磁力计的零偏状态方程可以表示为式(15)：

x＝f(x)+w(15)

其中，f(x)＝0，为x的微分值，w为磁力计零偏的随机噪声，可近似认为是高斯白噪声。

步骤sb2：计算状态转移矩阵a。

对零偏状态方程(15)进行离散化处理，可得到卡尔曼滤波的状态转移矩阵a为：

步骤sb3：建立观测方程。

磁力计感测的是地磁场的值，所以对地球上某一特定区域，理想磁力计的输出总磁场强度为恒定值，该值可以作为误差估计的观测值，校准模块44以此建立的观测方程为式(16)：

y＝(mbx-bx)²+(mby-by)²+(mbz-bz)²(16)

其中，y为当地磁场值，mbx、mby、mbz为磁力计三轴的实际测量输出值。

步骤sb4：计算第一量测矩阵。

在建立卡尔曼滤波的过程中，需要对方程(16)进行离散化处理，得到量测方程h(xk)，但此时的量测方程是非线性方程，需要进一步利用扩展卡尔曼滤波进行线性化处理，以满足卡尔曼滤波的线性需求。具体的，校准模块44求取量测方程h(xk)的雅可比矩阵，从而得到第一量测矩阵h1k，h1k的计算公式为式(17)：

其中，代表时h(xk)对xk的偏导值，

代表h(xk)对bx求偏导；

代表h(xk)对by求偏导；

代表h(xk)对bz求偏导。

步骤sb5：利用卡尔曼滤波迭代校准磁力计。

校准模块44根据第二状态变量x、状态转移矩阵a、磁力计零偏的观测方程和第一量测矩阵h1k，利用卡尔曼滤波迭代来对磁力计的测量数据进行校准，该校准过程如图8所示，具体包括以下步骤：

步骤b1：更新状态变量。校准模块44通过状态方程(18)更新状态变量，得到x在k时刻的状态预测值，状态方程(18)为：

其中，为x在k时刻的状态预测值，为x在k-1时刻的最优状态值，w1k-1为k-1时刻的状态噪声。

步骤b2：计算估算值。校准模块44根据磁力计的测量输出值应为当地磁场值，采用公式(19)得到x在k时刻的估算值，该估算值的计算公式(19)为：

z1k|k-1＝h1k*x1k-1+v1k-1(19)

其中，z1k|k-1为根据k-1时刻的状态预测的x在k时刻的估算值，x1k-1为x在k-1时刻的状态，v1k-1为k-1时刻的估测噪声。

步骤b3：计算滤波增益矩阵和误差协方差矩阵。校准模块44分别根据公式(20)和公式(21)计算滤波增益矩阵k1k和对应的误差协方差矩阵p1k|k-1，计算公式分别为：

p1k|k-1＝e1k|k-1*p1k-1*e1^tk|k-1+q1k-1(20)

k1k＝p1k|k-1*h1k^t*(h1k*p1k|k-1*h1k^t+r1k)^-1(21)

其中，e1k|k-1为状态方程系数，e1^tk|k-1为e1k|k-1的转置，q1k-1为系统噪声协方差矩阵，h1k^t为h1k的转置，r1k为测量噪声协方差矩阵。

步骤b4：计算磁力计的零偏。校准模块44根据公式(22)计算k时刻磁力计的零偏值计算公式(22)为：

其中，yk为k时刻由公式(16)计算得到的观测值。

步骤b5：更新误差协方差矩阵。校准模块44根据公式(23)更新误差协方差矩阵，用于下一时刻的更新计算，公式(23)为：

p1k＝(i1-k1k*h1k)*p1k|k-1(23)

其中，p1k为k时刻的误差协方差矩阵，i1为单位矩阵。

步骤b6：校准磁力计。用校准k时刻磁力计的测量数据，即从k时刻磁力计的测量数据中减去便可得到k时刻磁力计校准后的测量数据。

如此便可在动态条件下通过不断更新磁力计的零偏来不断对磁力计的测量数据进行校准，得到动态条件下磁力计校准后的测量数据。

与原有的利用mems(microelectromechanicalsystems，微机械)陀螺仪提供的角度信息来修正磁力计得到的测量数据的方法相比，本发明提供的磁力计的校准方法不用依赖mems陀螺仪的精度，避免了使用mems陀螺仪进行磁力计校准过程中温漂的影响，该校准过程无需引入多余的传感器，校准简单，可以大大减小校准的成本。

校准模块44在对磁力计进行校准，得到磁力计校准后的测量数据之后，第一计算模块41、第二计算模块42和第三计算模块43以磁力计校准后的测量数据为磁力计的测量数据执行如实施例一中步骤s11至步骤s14所述的姿态角的融合解算方法，得到最终的融合姿态角。

本实施例提供的姿态角的融合解算方法，首先，校准模块以磁力计的零偏为第二状态变量，利用卡尔曼滤波迭代对磁力计的测量数据进行校准，得到磁力计校准后的测量数据；接着，第一计算模块根据陀螺仪的测量数据解算出第一姿态角，并根据加速度计和磁力计校准后的测量数据解算出第二姿态角；然后，第二计算模块确定权值，并根据互补滤波原理利用该权值对第一姿态角和第二姿态角进行融合计算，得到第三姿态角，以修正陀螺仪解算出的第一姿态角；最后，第三计算模块以第三姿态角为状态变量且互补滤波后获得的第二姿态角为观测量，利用扩展卡尔曼滤波计算得到融合姿态角。由于对动态条件下磁力计的测量数据进行了校准，避免了磁力计将因外界变化磁场影响而产生的误差传递到姿态角的解算过程中，提高了姿态角解算的精度，且该校准过程无需引入多余的传感器，校准简单，可以大大减小校准的成本；同时，该权值可根据加速度计测量值的变化而自适应调整，以自适应调节第二姿态角在第三姿态角中所占的比重，从而抑制运动加速度对姿态角解算的影响，进一步提高了姿态角解算的精度；进一步的，通过将互补滤波和卡尔曼滤波相结合，融合解算出姿态角，弥补了互补滤波和卡尔曼滤波各自的缺陷，既能够提高动态条件下姿态角的跟随性，又达到了较快的收敛速度。

本领域技术人员可以理解，上述实施方式中各种方法的全部或部分功能可以通过硬件的方式实现，也可以通过计算机程序的方式实现。当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器、随机存储器、磁盘、光盘、硬盘等，通过计算机执行该程序以实现上述功能。例如，将程序存储在设备的存储器中，当通过处理器执行存储器中程序，即可实现上述全部或部分功能。另外，当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序也可以存储在服务器、另一计算机、磁盘、光盘、闪存盘或移动硬盘等存储介质中，通过下载或复制保存到本地设备的存储器中，或对本地设备的系统进行版本更新，当通过处理器执行存储器中的程序时，即可实现上述实施方式中全部或部分功能。

以上应用了具体个例对本发明进行阐述，只是用于帮助理解本发明，并不用以限制本发明。对于本发明所属技术领域的技术人员，依据本发明的思想，还可以做出若干简单推演、变形或替换。