CN116817896B - 一种基于扩展卡尔曼滤波的姿态解算方法 - Google Patents

Abstract

本发明公开了一种基于扩展卡尔曼滤波的姿态解算方法，包括在线磁强计校正模块、状态变量确定模块、外推方程构建模块，加速度计与磁力计估计模块、姿态偏置状态外推模块和姿态偏置状态更新模块；本发明属于导航技术领域，尤其涉及惯性导航领域，具体是指本发明提供一种基于扩展卡尔曼滤波的姿态解算方法，本发明的优点为：可将磁强计误差和畸变矫正与姿态解算方法同时进行，考虑了传感器误差和由磁强计安装引起的磁扰动，并考虑到了与惯性传感器的轴对齐问题，进而使位姿估计更加准确；使用扩展卡尔曼滤波，使估计结果越来越可靠；将过程噪声协方差矩阵和测量误差协方差矩阵设为常值矩阵，在满足实时性要求的同时获得精度较高的结果。

Description

一种基于扩展卡尔曼滤波的姿态解算方法

技术领域

本发明属于导航技术领域，尤其涉及惯性导航领域，具体为一种基于扩展卡尔曼滤波的姿态解算方法。

背景技术

捷联式惯性导航系统 (Strapdown inertial navigation system)是把惯性器件直接固连在载体上，用计算机来完成导航平台功能的惯性导航系统；其有点包括：1.整个系统的体积、重量和成本大大降低；2.惯性仪表便于安装维护，便于更换；3.惯性器件可以给出轴向的线加速度和角速度，这些信息是控制系统所需要的。和平台式系统相比，捷联式系统可以提供更多的导航和制导信息；4.惯性器件便于采用余度配置，提高系统的性能和可靠性。

三轴陀螺仪和三轴加速度计（惯性传感器）与三轴磁强计组成9轴惯性单元，9轴惯性单元与载体“捆绑”，载体的姿态和航向可以用载体坐标系（b系）相对于导航坐标系（n系）的三个转动角决定，就是说，在捷联式惯性导航系统中，直接测得的三个方向的加速度是载体坐标系三个轴方向上的，需要用一个捷联矩阵C将其变换到导航坐标系中来。惯性传感器安装在载体；该形式的捷联式惯性导航系统的任务是解算出载体相对于当地水平坐标系的位姿，速度和位置，其中，最为关键的任务是位姿的解算。

三轴磁力计、三轴加速度计和三轴陀螺仪的测量值可以结合起来，给出三维的方向估计。为了获得准确的方向估计，必须对磁强计和惯性传感器（加速度计和陀螺仪）进行轴对齐，并对磁强计的传感器误差和磁畸变进行校正。

发明内容

（一）要解决的技术问题

为解决现有技术的上述问题，本发明提供一种基于扩展卡尔曼滤波的姿态解算方法，可以有效解决：

（1）传统的姿态估计方法需要针对不同的传感器类型将磁强计误差和畸变矫正与姿态解算方法分开进行，增加了算法部署的步骤，另一方面，传统的磁强计的校准磁强计只考虑了传感器误差和由磁强计安装引起的磁扰动，没有考虑到与惯性传感器的轴对齐问题，进而造成位姿估计不准确；

（2）传统基于四元数的位姿估计由于惯性测量单元的特点使误差逐渐增大，使估计结果越来越不可靠；

（3）传统使用扩展卡尔曼滤波的姿态估计方法需实时计算过程噪声矩阵使计算量增大，对于实时性要求较高的应用场景，该方法就显得差强人意，基于传统四元数的扩展卡尔曼姿态估计在实践中的表现并不理想。

（二）技术方案

为了解决上述问题，本发明采用的一种技术方案为：一种基于扩展卡尔曼滤波的姿态解算方法，包括在线磁强计校正模块、状态变量确定模块、外推方程构建模块，加速度计与磁力计估计模块、姿态偏置状态外推模块和姿态偏置状态更新模块；在线磁强计校正模块的作用是消除原始磁强计数据的零偏误差、交轴耦合误差和比例误差，并将磁强计的三个轴与加速度计三个轴进行轴向对齐，为实现此功能，在线磁强计校正模块的操作步骤如下：

S1、建立磁强计测量模型：

一、设使用磁强计进行测量的测量数据集为：，上标b表示磁强计的传感器坐标系（也称body系），k表示第k个采样数据，上标N表示数据集中的样本总数，每个样本包含三个分量，分别是磁强计X轴的读数，磁强计Y轴的读数和磁强计Z轴的读数；当地磁场矢量/>从n系（当地水平坐标系）到b系（传感器坐标系）的转化用旋转矩阵用/>表示，对第k个采样数据，n系的当地磁场/>转换到b系中来的公式为：

上标n表示导航坐标系（n系也叫当地水平坐标系，采用北东地NED）

进而有：

上式是对每一个样本点根据当地磁场和对应的旋转矩阵用/>估计的结果，我们希望实际测量值与估计值相等，也就是/>成立，然而，有两个原因导致它们在实践中通常不平等，需要校准；磁力计三个轴的传感器误差：每个磁力计的轴向误差不同。这些传感器误差包括：

（1）磁强计三个轴的交轴耦合，使用3×3的矩阵N表示磁强计三个轴的交轴耦合；

（2）磁强计零偏置，即使磁场为零，磁强计测量结果为非零，零偏置由3×1向量b定义；

（3）磁强计三个轴的比例因子不同，用3×3对角矩阵S _m表示；

（4）在磁强计的测量中存在高斯噪声，用3×1向量表示；

这导致了和/>之间的以下关系，只考虑传感器误差：

二、考虑硬铁效应和软铁效应：硬铁效应是由于磁性材料的永久磁化强度，会导致一个恒定的附加磁场，表示这个效应的向量记为h；软铁效应是由于外部磁场对材料的磁化作用，因此将取决于材料相对于外部磁场的方向，它可以改变被测磁场的大小和方向，表示软铁效应的3×3矩阵记为S _i ；假设软铁产生的场与外部磁场之间存在线性关系，这是一个合理的假设，因为磁滞只有在施加非常大的磁场时才会发生；在考虑了硬铁效应和软铁效应后，传感器误差模型变为：

假设满足这些要求，式（1）意味着n系中的投影到b系中的/>将位于一个半径等于局部磁场大小的球体上。对式（3），假设所有的样本的数据/>应该位于一个椭球体上，这个椭球体可以相对于上述球体进行平移、旋转和缩放。

三、考虑磁力计和惯性传感器轴不对齐：现在考虑磁强计三轴与惯性传感器的轴不对齐问题造成位姿估计不准确，使用旋转矩阵R _im 描述惯性传感器和磁力计之间的错位关系，式（4）变为：

其中：

最终的校正模型为：

上式中，D为失真矩阵，q为偏移向量，意味着为了获得正确的校准，不需要在式（5）中区分交轴耦合、零偏置、比例因子、硬铁效应、软铁效应和磁力计和惯性传感器轴不对齐，它们可以组合成一个3×3失真矩阵D和一个3×1偏移向量q；

S2、最大似然建模：

一、磁强计的原始数据通过将椭球体数据映射到球体来补偿传感器误差和磁畸变的存在；然而，磁力计和惯性传感器轴不对齐不能仅仅通过磁强计的测量来校正；将磁强计测量和惯性测量相结合以确定这种旋转是必要的，椭球的旋转可以通过使用磁强计测量和处理过的垂向参考值来确定，其中第k个样本表示为：

其中：

式（8）中，服从高斯分布，z ⁿ 为单位垂向向量，处理后的测量值/>可以通过保持加速度计在不同方向上的静止来得到，且/>模长也为1；

二、当地地球磁场建模：

当地地球磁场由一个水平分量和一个垂直分量组成，设地球磁场/>与水平面成δ角,由/>sin δ定义/>的垂直分量，由/>cos δ定义/>的水平分量，

上式中，表示/>的模长，/>和水平面之间的夹角δ取决于在地球上的位置，d表示垂向分量，由于公式（7）中的校准结果D和q只依赖于水平分量和垂直分量之间的比例，因此可以假设/>为零；

从公式（10）和的定义中可以看出，/>和/>的内积为/>的垂直分量d；因此，和/>的内积对于所有可能的旋转/>上也等于d；然而，当惯性和磁力计传感器轴不对齐时，基于磁力计和垂直测量的每个/>的估计d将会有所不同，选择一种旋转R _im 可使惯性和磁强计传感器轴对齐，而使估计的倾角d近似恒定；

S3、构建最大似然公式：磁强计测量值在校准前位于一个椭球体上，并在校准后位于一个半径为||||的球体上，将球体的半径缩放为1，即||mⁿ || = 1；

结合式（7）和式（1）得：

定义参数θ ₁为：

如前所述，上式中，D表示失真矩阵，q表示偏移向量，是第二个分量/>为0的当地磁场矢量，/>表示所有样本点从n系到b系的旋转矩阵；假设噪声是独立的和高斯分布的，有：

其中和/>为对角矩阵：

定义参数θ ₂为：

S4、求解极大似然问题，也就是使和/>尽可能相等，从而找到未知参数θ ₁和θ ₂的估计值，有：

Y _1:k定义为：

上式中，上标m表示估计值，上标z表示测量值，由于噪声服从独立的高斯分布，则将ML问题简化为：

使用三个分量参数化旋转矩阵和mⁿ时，假设Σ _m和Σ _z是对角矩阵，求解给定θ ₁的θ ₂是通过先求解假设θ ₂为恒等式的θ ₁，然后求解给定θ ₁的θ ₂，最后求解给定θ ₂的θ ₁，而不是一次性求解式（14）。

进一步地，所述状态变量确定模块的处理步骤如下：

S1、定义状态变量：

上式中，q ₀ 是四元数的实部，q ₁ ，q ₂ ，q ₃ 分别是四元数虚部的三个分量；b ₁ ，b ₂ ，b ₃ 分别是陀螺仪在x，y，z 三个方向的偏置；偏置指的是陀螺仪在单位时间内的漂移量，可将式（15）写成如下形式：

上式中，q表示姿态四元数，b表示偏置向量。

S2、定义四元数求导法则：

其中：S(ω)、q、S(q)、ω的表达式分别为：

S3、为了补偿陀螺仪的偏差，在方程中加入偏差项：

S4、使用一阶线性化模型将上述方程进行离散化：

上式中，T是k时刻到k+1时刻的采样时间，q(k)是k时刻的姿态四元数，q(k+1)是k+1时刻的姿态四元数，是k时刻的姿态四元数关于时间的导数；

将式（18）代入式（19）中得：

因此，系统状态方程可以写成：

也可以写成：

上式可简写为：

矩阵A为状态转移矩阵，矩阵B为输入转移矩阵，为k时刻的状态变量，/>为k时刻的输入向量，/>为外推的状态向量；上式就是扩展卡尔曼滤波中的状态外推方程。

进一步地，所述加速度计与磁力计估计模块包括加速度估计和磁场估计两个步骤：

S1、首先其中加速度估计将重力矢量作为参考矢量，重力矢量总是指向当地水平坐标系的下方，有：

上式中，是加速度计在传感器坐标系（b系）下估计的测量加速度；/>是当地水平坐标系到传感器坐标系的旋转矩阵；g是当地水平坐标系下的重力向量，g = [0 0 1]^T；/>是传感器坐标系下的加速度计零偏，当对加速度计做了校正后/>是0；因为可以确定等号右边的所有变量，因此可以估计加速度计的测量值/>，然后使用加速度计的预测测量值与实际测量值进行比较，以确定方向误差，从单位四元数到旋转矩阵的定义如下：

由于重力向量为g = [0 0 1]^T；因此有：

上式中，含有四元数的矩阵是非线性的，因此，为使用卡尔曼滤波，将上式进行线性化，并写成形如的形式，/>是状态向量，/>是余项，扩展卡尔曼滤波的处理方式是将上式进行线性化，根据泰勒展开，有：

其中为雅可比矩阵：

将上式写成的形式，C _a称为线性系数矩阵，并忽略/>矩阵有：

S2、其次，磁场估计的参考矢量是地球磁场提供的一个始终指向磁北方向的矢量，这个矢量在地球表面上的变化(矢量的高度取决于海拔) ，假设北参考矢量的变化是可以忽略不计的，因为传感器不会移动太多(只在房间内)；假设磁强计数据在水平面上提供的参考矢量是准确的，因此要从磁强计参考矢量中去掉1维；

上式中，h表示真实的磁场，h _m表示磁强计测得的磁场；D ^-1表示失真矩阵的逆矩阵，q是偏移向量，都是在线磁强计校正模块的处理结果；接下来，为了从磁强计矢量中移除1维(垂直平面) ，由于想移除的磁场垂直分量存在于当地水平坐标系中，因此首先将坐标从传感器坐标系转换为当地水平坐标系，因此，由四元数生成旋转矩阵，如下所示：

上式中，

，/>

上式中，r的每个分量是传感器坐标系下校正之后的磁强计数据，r’的每个分量是在当地水平坐标系下校正之后的磁强计数据，令r’的第二个分量等于零，然后重新标准化向量，使它保持为一个单位向量，然后将旋转它回到传感器坐标系，并使用结果向量代替实际校准的测量数据。

使用北矢量作为磁强计的参考，因为北矢量恰好指向x轴的正方向，所以只是把正x轴(当地水平坐标系)作为参考矢量；

其中，是磁强计在传感器坐标系下的估计的磁场强度值，/>是当地水平坐标系到传感器坐标系的旋转矩阵，/>是当地水平坐标系下参考磁场的北向分量，且有/>=[1 00 ]^T；/>是传感器坐标系下的磁强计零偏值，当对磁强计进行校正后，/>为零，旋转矩阵/>的表达式为：

化简得：

上式依然是一个非线性方程，利用泰勒展开可以化简为：

因此，线性化之后的表达式为：

进一步地，所述姿态偏置状态外推模块的处理过程如下：

S1、进行状态外推：；在状态外推中，/>表示由k-1时刻的四元数生成的状态转移矩阵、/>是k-1时刻的状态变量、/>表示由k-1时刻的四元数生成的输入转移矩阵、/>为k时刻输入，也就是k时刻的陀螺仪的测量值，/>是外推结果，也就是外推得到的状态变量；

S2、进行协方差外推：；在协方差外推中，Q为过程噪声协方差矩阵，/>是k - 1时刻的状态误差协方差矩阵，/>表示由k-1时刻的四元数生成的状态转移矩阵；/>表示外推得到的k时刻的状态误差协方差矩阵。

进一步地，所述姿态偏置状态更新模块的处理过程如下：

S1、计算卡尔曼增益矩阵：

上式中R是测量协方差矩阵，C是线性系数矩阵（也称为测量矩阵），表示外推得到的k时刻的状态误差协方差矩阵，K _k 为卡尔曼增益矩阵；

S2、进行状态更新：

上式中是外推得到的状态变量，C是线性系数矩阵（也称为测量矩阵），K _k 为卡尔曼增益矩阵，/>是k时刻的加速度计测量和磁强计测量的组合向量，/>是更新得到的状态变量；

S3、进行协方差更新：

上式中，I是7×7的单位矩阵，K _k 为卡尔曼增益矩阵，表示外推得到的k时刻的状态误差协方差矩阵，C是线性系数矩阵，R _k 是测量误差协方差矩阵，/>是更新后的状态误差协方差矩阵。

（三）有益效果

本发明提供一种基于扩展卡尔曼滤波的姿态解算方法，可有效解决：

（1）将磁强计误差和畸变矫正与姿态解算方法同时进行，考虑了传感器误差和由磁强计安装引起的磁扰动，并考虑到了与惯性传感器的轴对齐问题，进而使位姿估计更加准确；

（2）使用扩展卡尔曼滤波，使估计结果越来越可靠；

（3）将过程噪声协方差矩阵和测量误差协方差矩阵设为常值矩阵，在满足实时性要求的同时获得精度较高的结果。

附图说明

图1为本发明了一种基于扩展卡尔曼滤波的姿态解算方法的处理流程图；

图2为本发明提出的在线磁强计校正模块的处理流程图；

图3为本发明提出的加速度计与磁强计估计模块的加速度线性估计表达式和磁场计算表达式；

图4为本发明提出的姿态偏置状态外推模块的状态外推表达式和协方差外推表达式；

图5为本发明提出的姿态偏置状态更新模块的卡尔曼增益计算、状态更新和协方差更新的计算步骤。

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。

具体实施方式

下面将结合本方案实施例中的附图，对本方案实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本方案一部分实施例，而不是全部的实施例；基于本方案中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本方案保护的范围。

一种基于扩展卡尔曼滤波的姿态解算方法，包括在线磁强计校正模块、状态变量确定模块、外推方程构建模块、加速度计与磁力计估计模块、姿态偏置状态外推模块和姿态偏置状态更新模块；在线磁强计校正模块的作用是消除原始磁强计数据的零偏误差、交轴耦合误差和比例误差，并将磁强计的三个轴与加速度计三个轴进行轴向对齐，为实现此功能，在线磁强计校正模块的操作步骤如下：

S1、建立磁强计测量模型：

一、设使用磁强计进行测量的测量数据集为：，上标b表示磁强计的传感器坐标系（也称body系），k表示第k个采样数据，上标N表示数据集中的样本总数，每个样本包含三个分量，分别是磁强计X轴的读数，磁强计Y轴的读数和磁强计Z轴的读数；当地磁场矢量/>从n系（当地水平坐标系）到b系（传感器坐标系）的转化用旋转矩阵用/>表示，对第k个采样数据，n系的当地磁场/>转换到b系中来的公式为：

上标n表示导航坐标系（n系也叫当地水平坐标系，采用北东地NED）

进而有：

上式是对每一个样本点根据当地磁场和对应的旋转矩阵用/>估计的结果，我们希望实际测量值与估计值相等，也就是/>成立，然而，有两个原因导致它们在实践中通常不平等，需要校准；磁力计三个轴的传感器误差：每个磁力计的轴向误差不同。这些传感器误差包括：

（1）磁强计三个轴的交轴耦合，使用3×3的矩阵N表示磁强计三个轴的交轴耦合；

（2）磁强计零偏置，即使磁场为零，磁强计测量结果为非零，零偏置由3×1向量b定义；

（3）磁强计三个轴的比例因子不同，用3×3对角矩阵S _m表示；

（4）在磁强计的测量中存在高斯噪声，用3×1向量表示；

这导致了和/>之间的以下关系，只考虑传感器误差：

二、考虑硬铁效应和软铁效应：硬铁效应是由于磁性材料的永久磁化强度，会导致一个恒定的附加磁场，表示这个效应的向量记为h；软铁效应是由于外部磁场对材料的磁化作用，因此将取决于材料相对于外部磁场的方向，它可以改变被测磁场的大小和方向，表示软铁效应的3×3矩阵记为S _i ；假设软铁产生的场与外部磁场之间存在线性关系，这是一个合理的假设，因为磁滞只有在施加非常大的磁场时才会发生；在考虑了硬铁效应和软铁效应后，传感器误差模型变为：

假设满足这些要求，式（1）意味着n系中的投影到b系中的/>将位于一个半径等于局部磁场大小的球体上。对式（3），假设所有的样本的数据/>应该位于一个椭球体上，这个椭球体可以相对于上述球体进行平移、旋转和缩放。

三、考虑磁力计和惯性传感器轴不对齐：现在考虑磁强计三轴与惯性传感器的轴不对齐问题造成位姿估计不准确，使用旋转矩阵R _im 描述惯性传感器和磁力计之间的错位关系，式（4）变为：

/>

其中：

最终的校正模型为：

上式中，D为失真矩阵，q为偏移向量，意味着为了获得正确的校准，不需要在式（5）中区分交轴耦合、零偏置、比例因子、硬铁效应、软铁效应和磁力计和惯性传感器轴不对齐，它们可以组合成一个3×3失真矩阵D和一个3×1偏移向量q；

S2、最大似然建模：

一、磁强计的原始数据通过将椭球体数据映射到球体来补偿传感器误差和磁畸变的存在；然而，磁力计和惯性传感器轴不对齐不能仅仅通过磁强计的测量来校正；将磁强计测量和惯性测量相结合以确定这种旋转是必要的，椭球的旋转可以通过使用磁强计测量和处理过的垂向参考值来确定，其中第k个样本表示为：

其中：

式8中，服从高斯分布，z ⁿ 为单位垂向向量，处理后的测量值/>可以通过保持加速度计在不同方向上的静止来得到，且/>模长也为1；

二、当地地球磁场建模：

当地地球磁场由一个水平分量和一个垂直分量组成，设地球磁场/>与水平面成δ角,由/>sin δ定义/>的垂直分量，由/>cos δ定义/>的水平分量，

上式中，表示/>的模长，/>和水平面之间的夹角δ取决于在地球上的位置，d表示垂向分量，由于公式（7）中的校准结果D和q只依赖于水平分量和垂直分量之间的比例，因此可以假设/>为零；

从公式（10）和的定义中可以看出，/>和/>的内积为/>的垂直分量d；因此，和/>的内积对于所有可能的旋转/>上也等于d；然而，当惯性和磁力计传感器轴不对齐时，基于磁力计和垂直测量的每个/>的估计d将会有所不同，选择一种旋转R _im 可使惯性和磁强计传感器轴对齐，而使估计的倾角d近似恒定；

S3、构建最大似然公式：磁强计测量值在校准前位于一个椭球体上，并在校准后位于一个半径为||||的球体上，将球体的半径缩放为1，即||mⁿ || = 1；

结合式（7）和式（1）得：

定义参数θ ₁为：

如前所述，上式中，D表示失真矩阵，q表示偏移向量，是第二个分量/>为0的当地磁场矢量，/>表示所有样本点从n系到b系的旋转矩阵；假设噪声是独立的和高斯分布的，有：

其中和/>为对角矩阵：

定义参数θ ₂为：

S4、求解极大似然问题，也就是使和/>尽可能相等，从而找到未知参数θ ₁和θ ₂的估计值，有：

Y _1:k定义为：

上式中，上标m表示估计值，上标z表示测量值，由于噪声服从独立的高斯分布，则将ML问题简化为：

使用三个分量参数化旋转矩阵和mⁿ时，假设Σ _m和Σ _z是对角矩阵，求解给定θ ₁的θ ₂是通过先求解假设θ ₂为恒等式的θ ₁，然后求解给定θ ₁的θ ₂，最后求解给定θ ₂的θ ₁，而不是一次性求解式（14）。

进一步地，所述状态变量确定模块的处理步骤如下：

S1、定义状态变量：

上式中，q ₀ 是四元数的实部，q ₁ ，q ₂ ，q ₃ 分别是四元数虚部的三个分量；b ₁ ，b ₂ ，b ₃ 分别是陀螺仪在x，y，z 三个方向的偏置；偏置指的是陀螺仪在单位时间内的漂移量，可将式（15）写成如下形式：

上式中，q表示姿态四元数，b表示偏置向量。

S2、定义四元数求导法则：

其中：S(ω)、q、S(q)、ω的表达式分别为：

S3、补偿陀螺仪的偏差，在方程中加入偏差项：

S4、使用一阶线性化模型将上述方程进行离散化：

上式中，T是k时刻到k+1时刻的采样时间，q(k)是k时刻的姿态四元数，q(k+1)是k+1时刻的姿态四元数。

将式（18）代入式（19）中得：

式（20）也可以表达为：

所述状态方程构建模块的计算步骤如下：

S1、结合方程（15）、（18）和（21）系统状态方程可以写成：

式（22）也可以写成：

式（23）的展开形式为：

S2、将方程（24）化简为：

矩阵A为状态转移矩阵，矩阵B为输入转移矩阵，为k时刻的状态变量，/>为k时刻的输入向量，/>为外推的状态向量；上式就是扩展卡尔曼滤波中的状态外推方程。

所述加速度计与磁力计估计模块包括加速度估计和磁场估计两个步骤：

S1、加速度估计，将重力矢量作为参考矢量，重力矢量总是指向当地水平坐标系的下方，有：

上式中，是加速度计在传感器坐标系（b系）测量的加速度；/>是当地水平坐标系到传感器坐标系的旋转矩阵；g是当地水平坐标系下的重力向量，g = [0 0 1]^T；/>是传感器坐标系下的加速度计零偏，当对加速度计做了校正后/>是0；因为可以确定等号右边的所有变量 ，因此可以预测加速度计的测量值，然后使用加速度计的预测测量值与实际测量值进行比较，以确定方向误差，从单位四元数到旋转矩阵的定义如下：

由于重力向量为g = [0 0 1]^T；因此有：

整理有：

上式中，含有四元数的矩阵是非线性的，因此，为使用卡尔曼滤波，将上式进行线性化，并写成形如的形式，/>是状态向量，/>是余项，也就是预测的加速度计的测量值，扩展卡尔曼滤波的处理方式是将上式进行线性化：

根据泰勒展开，有：

其中为雅可比矩阵，表达式为：

将上式写成的形式，C _a称为加速度线性系数矩阵，并忽略/>矩阵有：

上式中，令：

S2、磁场估计，磁场估计的参考矢量是地球磁场提供的一个始终指向磁北方向的矢量，这个矢量在地球表面上的变化 ，假设北参考矢量的变化是可以忽略不计的，因为传感器不会移动太多；磁强计数据在水平面上提供的参考矢量是准确的；因此要从磁强计参考矢量中去掉1维；

上式中，h表示真实的磁场，h _m表示磁强计测得的磁场；D ^-1表示失真矩阵的逆矩阵，q是偏移向量，都是在线磁强计校正模块的处理结果。

接下来，为了从磁强计矢量中移除1维，由于想移除的磁场垂直分量存在于当地水平坐标系中，因此首先将坐标从传感器坐标系转换为当地水平坐标系，因此，由四元数生成旋转矩阵，如下所示：

上式中，

，/>

上式中，r的每个分量是传感器坐标系下校正之后的磁强计数据，r’的每个分量是在当地水平坐标系下校正之后的磁强计数据，令r’的第二个分量等于零，然后重新标准化向量，使它保持为一个单位向量，然后将旋转它回到传感器坐标系，并使用结果向量代替实际校准的测量数据。

在加速度计部分，重力矢量作为参考矢量；另一方面，北矢量作为磁强计的参考；因为北矢量恰好指向x轴的正方向，所以只是把正x轴(当地水平坐标系)作为参考矢量；

其中，是磁强计在传感器坐标系下的测量的磁场强度值，/>是当地水平坐标系到传感器坐标系的旋转矩阵，/>是当地水平坐标系下参考磁场的北向分量，且有/>=[1 00 ]^T；/>是传感器坐标系下的磁强计零偏值，当对磁强计进行校正后，/>为零；

化简得：

上式依然是一个非线性方程，利用泰勒展开可以化简为：

其中为雅可比矩阵，表达式为：/>

进一步有：

进一步有：

因此，线性化之后得表达式为：

上式中，令：

其中，C _m称为磁场线性系数矩阵。

所述姿态偏置状态外推模块的处理过程如下：

S1、进行状态外推：；在状态外推中，/>表示由k-1时刻的四元数生成的状态转移矩阵、/>是k-1时刻的状态变量、/>表示由k-1时刻的四元数生成的输入转移矩阵、/>为k时刻输入，也就是k时刻的陀螺仪的测量值，/>是外推结果，也就是外推得到的状态变量；

S2、进行协方差外推：；在协方差外推中，Q为过程噪声协方差矩阵，/>是k-1时刻的状态误差协方差矩阵，/>表示由k-1时刻的四元数生成的状态转移矩阵；/>表示外推得到的k时刻的状态误差协方差矩阵。

所述姿态偏置状态更新模块的处理过程如下：

S1、计算卡尔曼增益矩阵：；其中R是测量误差协方差矩阵，C是线性系数矩阵（也称为测量矩阵），/>表示外推得到的k时刻的状态误差协方差矩阵，K _k为卡尔曼增益矩阵；测量矩阵C的表达式如下定义：/>

S2、进行状态更新：；其中/>是外推得到的状态变量，C是线性系数矩阵（也称为测量矩阵），K _k为卡尔曼增益矩阵，/>是k时刻的加速度计测量和磁强计测量的组合向量，/>是更新得到的状态变量；

S3、进行协方差更新：；其中，I是7×7的单位矩阵，K _k为卡尔曼增益矩阵，/>表示外推得到的k时刻的状态误差协方差矩阵，C是线性系数矩阵，R _k是测量误差协方差矩阵，/>是更新后的状态误差协方差矩阵。

实施例一：

一种基于扩展卡尔曼滤波的姿态解算方法，包括在线磁强计校正模块、加速度计与磁力计估计模块、姿态偏置状态外推模块和姿态偏置状态更新模块；实施方式如下：

S1、使用在线磁强计校正模块进行在线磁强计校正，操作步骤如下：

（1）、建立磁强计测量模型；

（2）、最大似然建模：

（3）、构建最大似然公式：

（4）、求解极大似然问题；

S2、使用状态变量确定模块的确定状态变量：

（1）、定义状态变量；

（2）、定义四元数求导法则：

S3、使用状态方程构建模块构建状态方程：

S4、使用加速度计与磁力计估计模块估计加速度和磁场：

（1）、加速度估计，最终结果为：

上式中，令：

其中，C _a称为加速度线性系数矩阵。

（2）、磁场估计，最终结果为：

上式中，令：

其中，C _m称为磁场线性系数矩阵。

S5、使用姿态偏置状态外推模块进行状态外推和协方差外推，步骤如下：

（1）、进行状态外推：；在状态外推中，/>表示由k-1时刻的四元数生成的状态转移矩阵、/>是k-1时刻的状态变量、/>表示由k-1时刻的四元数生成的输入转移矩阵、/>为k时刻输入，也就是k时刻的陀螺仪的测量值，/>是外推结果，也就是外推得到的状态变量；

（2）、进行协方差外推：；在协方差外推中，Q为过程噪声协方差矩阵，/>是k-1时刻的状态误差协方差矩阵，/>表示由k-1时刻的四元数生成的状态转移矩阵；/>表示外推得到的k时刻的状态误差协方差矩阵。

S6、使用姿态偏置状态更新模块进行状态更新和协方差更新，步骤如下：

（1）、计算卡尔曼增益矩阵：；

（2）、进行状态更新：；

（3）、进行协方差更新：；

S7、步骤S4、S5和S6循环进行。

以上便是本发明具体的工作流程，下次使用时重复此步骤即可。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

以上对本发明及其实施方式进行了描述，这种描述没有限制性，附图中所示的也只是本发明的实施方式之一，实际的结构并不局限于此。总而言之如果本领域的普通技术人员受其启示，在不脱离本发明创造宗旨的情况下，不经创造性的设计出与该技术方案相似的结构方式及实施例，均应属于本发明的保护范围。

Claims (1)

1.一种基于扩展卡尔曼滤波的姿态解算方法，包括在线磁强计校正模块、状态变量确定模块、外推方程构建模块，加速度计与磁力计估计模块、姿态偏置状态外推模块和姿态偏置状态更新模块；在线磁强计校正模块的操作步骤如下：

S1、建立磁强计测量模型：

S2、最大似然建模：

S3、构建最大似然公式：

S4、求解极大似然问题；

所述状态变量确定模块的处理步骤如下：

S1、定义状态变量：

，

上式中，q ₀ 是四元数的实部，q ₁，q ₂，q ₃分别是四元数虚部的三个分量；b ₁，b ₂，b ₃ 分别是陀螺仪在x，y，z 三个方向的偏差；偏置项指的是陀螺仪在单位时间内的漂移量，可将式（15）写成如下形式：

，

上式中，q表示姿态四元数，b表示偏执置向量；

S2、定义四元数求导法则：

，

其中：S(ω)、q、S(q)、ω的表达式分别为：

，

，

S3、补偿陀螺仪的偏差，在方程中加入偏差项；

，

S4、使用一阶线性化模型将上述方程进行离散化：

，

上式中，T是k时刻到k+1时刻的采样时间，q(k)是k时刻的姿态四元数，q(k+1)是k+1时刻的姿态四元数，是k时刻的姿态四元数关于时间的导数；

，

将式（18）代入式（19）中得：

，

进一步有：

，

状态方程构建模块的计算步骤如下：

S1、结合方程（15）、（18）和（21）系统状态方程可以写成：

，

因此，系统状态方程可以写成：

，

式（23）的展开形式为：

，

S2、将式（24）可化简为：

，

矩阵A为状态转移矩阵，矩阵B为输入转移矩阵，为k时刻的状态变量，/>为k时刻的输入向量，/>为外推的状态向量；上式就是扩展卡尔曼滤波中的状态外推方程；

所述加速度计与磁力计估计模块包括加速度估计和磁场估计两个步骤：

S1、加速度估计，加速度估计的线性化结果为：首先其中加速度估计将重力矢量作为参考矢量，重力矢量总是指向当地水平坐标系的下方，有：

，

上式中，是加速度计在传感器坐标系下估计的测量加速度；/>是当地水平坐标系到传感器坐标系的旋转矩阵；g是当地水平坐标系下的重力向量，g = [0 0 1]^T；/>是传感器坐标系下的加速度计零偏，当对加速度计做了校正后/>是0；因为可以确定等号右边的所有变量，因此可以估计加速度计的测量值/>，然后使用加速度计的预测测量值与实际测量值进行比较，以确定方向误差，

，

S2、其次，磁场估计的参考矢量是地球磁场提供的一个始终指向磁北方向的矢量，这个矢量在地球表面上会发生变化，矢量的高度取决于海拔，假设北参考矢量的变化是可以忽略不计的，因为传感器不会移动太多，只在房间内；假设磁强计数据在水平面上提供的参考矢量是准确的，因此要从磁强计参考矢量中去掉1维；

，

上式中，h表示真实的磁场，h _m表示磁强计测得的磁场；D ^-1表示失真矩阵的逆矩阵，q是偏移向量，都是在线磁强计校正模块的处理结果；接下来，为了从磁强计矢量中移除1维，由于想移除的磁场垂直分量存在于当地水平坐标系中，因此首先将坐标从传感器坐标系转换为当地水平坐标系，因此，由四元数生成旋转矩阵，如下所示：

，

上式中，

，/> ，

上式中，r的每个分量是传感器坐标系下校正之后的磁强计数据，r’的每个分量是在当地水平坐标系下校正之后的磁强计数据，令r’的第二个分量等于零，然后重新标准化向量，使它保持为一个单位向量，然后将旋转它回到传感器坐标系，并使用结果向量代替实际校准的测量数据；

使用北矢量作为磁强计的参考，因为北矢量恰好指向x轴的正方向，所以只是把正x轴作为参考矢量；

，

其中，是磁强计在传感器坐标系下的估计的磁场强度值，/>是当地水平坐标系到传感器坐标系的旋转矩阵，/>是当地水平坐标系下参考磁场的北向分量，且有/>=[1 0 0 ]^T；是传感器坐标系下的磁强计零偏值，当对磁强计进行校正后，/>为零，旋转矩阵/>的表达式为：

，

化简得：

，

上式依然是一个非线性方程，利用泰勒展开可以化简为：

，

因此，线性化之后的表达式为：

，

所述姿态偏置状态外推模块的处理过程如下：

S1、进行状态外推：；在状态外推中，/>表示由k-1时刻的四元数生成的状态转移矩阵、/>是k-1时刻的状态变量、/>表示由k-1时刻的四元数生成的输入转移矩阵、/>为k时刻输入，也就是k时刻的陀螺仪的测量值，/>是外推结果，也就是外推得到的状态变量；

S2、进行协方差外推：；在协方差外推中，Q为过程噪声协方差矩阵，/>是k - 1时刻的状态误差协方差矩阵，/>表示由k-1时刻的四元数生成的状态转移矩阵；/>表示外推得到的k时刻的状态误差协方差矩阵；

所述姿态偏置状态更新模块的处理过程如下：

S1、计算卡尔曼增益矩阵：，

上式中R是测量协方差矩阵，C是线性系数矩阵，也称为测量矩阵，表示外推得到的k时刻的状态误差协方差矩阵，K _k为卡尔曼增益矩阵；

S2、进行状态更新： ，

上式中是外推得到的状态变量，C是线性系数矩阵，也称为测量矩阵，K _k为卡尔曼增益矩阵，/>是k时刻的加速度计测量和磁强计测量的组合向量，/>是更新得到的状态变量；

S3、进行协方差更新： ，

上式中，I是7×7的单位矩阵，K _k为卡尔曼增益矩阵，表示外推得到的k时刻的状态误差协方差矩阵，C是线性系数矩阵，R _k是测量误差协方差矩阵，/>是更新后的状态误差协方差矩阵。