CN110375773A - Mems惯导系统姿态初始化方法 - Google Patents

Abstract

本发明公开了一种MEMS惯导系统姿态初始化方法，包括：根据姿态初始化的误差方程建立系统模型，并依据所述系统模型确定状态量与量测量；获取惯性测量数据；依据所述惯性测量数据计算初始姿态角；基于所述状态量、量测量以及初始姿态角预测失准角修正值；基于所述失准角修正值对所述初始姿态角进行闭环修正。通过基于状态量、量测量以及初始姿态角预测失准角修正值；并基于失准角修正值对初始姿态角进行闭环修正。从而达到提高对准精度的目的。

Description

MEMS惯导系统姿态初始化方法

技术领域

本发明涉及MEMS领域，具体地，涉及一种MEMS惯导系统姿态初始化方法。

背景技术

近年来，微机电系统(Micro-Electro-Mechanical System,MEMS)技术得到迅速发展，MEMS传感器具有低成本、轻重量、小体积、低功耗等优势，因此在工业领域得到广泛应用。同样地，MEMS惯导系统也在无人机、机器人等需要姿态控制与定位导航的载体上得到使用。惯导系统在进入导航模式之前，需要先对其进行初始对准。在MEMS惯导系统中，通常采用磁强计辅助加速度计完成这一过程。

MEMS惯导系统的初始对准过程就是姿态初始化的过程，该过程容易受到传感器测量误差与对准环境的影响。MEMS传感器受到制造工艺限制，输出信号中含有电热噪声与机械噪声，这对MEMS惯导系统姿态初始化的精度提高十分不利。并且，在初始化过程中，系统所处的环境往往存在不确定性干扰，导致传感器测量误差增大，影响初始化精度。针对这一问题，做了大量研究。其中，基于传感器信号降噪的方法得到应用。现有技术中采用离散小波变换对MEMS惯性传感器输出序列进行分解，应用阈值对分解系数消减后，对序列进行ARMA建模，提高了陀螺仪的输出精度。然而，小波分解中的小波基函数与分解层数的选取都会影响信号的降噪结果，选取方法复杂且不易得到良好的分解效果。更为重要的是，降噪过程中的时间延迟现象会影响惯导系统的实时性。或采用将递推最小二乘算法引入磁强计误差补偿，该方法无需专门设备辅助，易于实现。然而，该方法并没有考虑到系统所处环境的干扰对传感器测量的影响。上述两种方法都有一个共同的问题，都是仅针对传感器输出的数学模型中的误差进行降噪与补偿，对传感器的输出信号处理后再进行姿态初始化，没有进一步分析传感器输出误差与姿态失准角之间的关系，建立传感器输出与失准角的误差模型，从而存在对准精度低的问题。

发明内容

本发明的目的在于，针对上述问题，提出一种MEMS惯导系统姿态初始化方法，以实现提高对准精度的优点。

为实现上述目的，本发明实施例采用的技术方案是：

一种MEMS惯导系统姿态初始化方法，包括：

根据姿态初始化的误差方程建立系统模型，并依据所述系统模型确定状态量与量测量；

获取惯性测量数据；

依据所述惯性测量数据计算初始姿态角；

基于所述状态量、量测量以及初始姿态角预测失准角修正值；

基于所述失准角修正值对所述初始姿态角进行闭环修正。

作为本发明实施例的一种具体实现方式，所述惯性测量数据，至少包括：

三轴加速度计与三轴磁强计。

作为本发明实施例的一种具体实现方式，所述依据所述惯性测量数据计算初始姿态角，包括：

基于所述三轴加速度计的输出计算得到水平姿态角；

基于所述三轴磁强计的测量的地磁信息计算得到方位角；

对所述方位角进行修正。

作为本发明实施例的一种具体实现方式，对所述方位角进行修正，为：

通过磁偏角对所述方位角进行修正。

作为本发明实施例的一种具体实现方式，所述根据姿态初始化的误差方程建立系统模型，包括：

对所述误差方程进行分析；

依据分析结果建立系统模型。

作为本发明实施例的一种具体实现方式，所述对所述误差方程进行分析，包括：

获取姿态转移矩阵与真实姿态转移阵之间的偏差；

获取所述三轴加速度计的输出误差；

获取三轴磁强计的测量输出误差；

基于所述姿态转移矩阵与真实姿态转移阵之间的偏差、三轴加速度计的输出误差和三轴磁强计的测量输出误差获取所述误差方程的误差。

作为本发明实施例的一种具体实现方式，所述系统模型，为：

式中，其中，Φ_k/k-1表示k时刻系统状态转移阵，Γ_k-1表示系统噪声分配矩阵，H_k表示量测状态转移矩阵，W_k-1为k-1时刻的系统噪声，W_k和V_k分别是系统噪声与量测噪声，X_k表示k时刻的系统状态向量，X_k-1表示k-1时刻的系统状态向量，Z_k表示k时刻的量测向量。

作为本发明实施例的一种具体实现方式，所述基于所述状态量、量测量以及初始姿态角预测失准角修正值，为：

利用自适应卡尔曼滤波基于所述状态量、量测量以及初始姿态角预测失准角修正值。

作为本发明实施例的一种具体实现方式，所述自适应卡尔曼滤波中，

采用自适应指数渐消记忆的加权方式，设置多渐消因子调整量测噪声参数。

作为本发明实施例的一种具体实现方式，所述自适应指数渐消记忆的加权方式中，

记加权系数为β_k,i，b为遗忘因子，0＜b＜1；

所述加权系数β_k,i对历史数据的利用率将以遗忘因子b的指数次方递减。

本发明实施例通过基于状态量、量测量以及初始姿态角预测失准角修正值；并基于失准角修正值对初始姿态角进行闭环修正。从而达到提高对准精度的目的。

另外本发明实施例分析了MEMS初始对准中，加速度计与磁强计的输出误差，根据误差方法建立了失准角模型，提高了对准精度。

本发明实施例将自适应卡尔曼滤波引入对准过程，对量测噪声在线实时估计，有效避免了应用环境中的干扰对初始对准的影响。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为本发明实施例所述的MEMS惯导系统姿态初始化方法的流程图；

图2为本发明实施例所述的MEMS惯导姿态初始化算法示意图；

图3为本发明实施例所述的基于AKF的MEMS惯导姿态初始化示意图；

图4为本发明实施例所述的指数渐消加权自适应卡尔曼滤波示意图。

具体实施方式

以下结合附图对本发明的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。

如图1所示，一种MEMS惯导系统姿态初始化方法，包括：

S101：根据姿态初始化的误差方程建立系统模型，并依据所述系统模型确定状态量与量测量；

S102：获取惯性测量数据；

S103：依据所述惯性测量数据计算初始姿态角；

S104：基于所述状态量、量测量以及初始姿态角预测失准角修正值；

S105：基于所述失准角修正值对所述初始姿态角进行闭环修正。

作为本发明实施例的一种具体实现方式，所述惯性测量数据，至少包括：

三轴加速度计与三轴磁强计。

在具体的应用场景中采集惯性测量数据，具体为采集的惯性测量单元的数据包括：三轴加速度计、三轴磁强计与三轴陀螺仪数据。其中，用于进行MEMS初始对准计算的数据包括：三轴加速度计与三轴磁强计。

作为本发明实施例的一种具体实现方式，所述依据所述惯性测量数据计算初始姿态角，包括：

基于所述三轴加速度计的输出计算得到水平姿态角；

基于所述三轴磁强计的测量的地磁信息计算得到方位角；

对所述方位角进行修正。

作为本发明实施例的一种具体实现方式，对所述方位角进行修正，为：

通过磁偏角对所述方位角进行修正。

作为本发明实施例的一种具体实现方式，所述根据姿态初始化的误差方程建立系统模型，包括：

对所述误差方程进行分析；

依据分析结果建立系统模型。

作为本发明实施例的一种具体实现方式，所述对所述误差方程进行分析，包括：

获取姿态转移矩阵与真实姿态转移阵之间的偏差；

获取所述三轴加速度计的输出误差；

获取三轴磁强计的测量输出误差；

基于所述姿态转移矩阵与真实姿态转移阵之间的偏差、三轴加速度计的输出误差和三轴磁强计的测量输出误差获取所述误差方程的误差。

作为本发明实施例的一种具体实现方式，所述系统模型，为：

式中，其中，Φ_k/k-1表示k时刻系统状态转移阵，Γ_k-1表示系统噪声分配矩阵，H_k表示量测状态转移矩阵，W_k-1为k-1时刻的系统噪声，W_k和V_k分别是系统噪声与量测噪声，X_k表示k时刻的系统状态向量，X_k-1表示k-1时刻的系统状态向量，Z_k表示k时刻的量测向量。

作为本发明实施例的一种具体实现方式，所述基于所述状态量、量测量以及初始姿态角预测失准角修正值，为：

利用自适应卡尔曼滤波基于所述状态量、量测量以及初始姿态角预测失准角修正值。

作为本发明实施例的一种具体实现方式，所述自适应卡尔曼滤波中，

采用自适应指数渐消记忆的加权方式，设置多渐消因子调整量测噪声参数。

作为本发明实施例的一种具体实现方式，所述自适应指数渐消记忆的加权方式中，

记加权系数为β_k,i，b为遗忘因子，0＜b＜1；

所述加权系数β_k,i对历史数据的利用率将以遗忘因子b的指数次方递减。

在一个具体的应用中，

基于AKF的MEMS惯导系统姿态初始化方法，其特征在于，包括：

步骤1：根据姿态初始化的误差方程建立系统模型，确定状态量与量测量；

步骤2：采集惯性测量单元数据；

步骤3：计算初始姿态角；

步骤4：利用自适应卡尔曼滤波(AKF)预测失准角修正值；

步骤5：对步骤2惯性测量单元数据中的姿态角进行闭环修正；

步骤6：进入下一个循环。

步骤3中计算初始姿态角具体为：

①通过加速度计的输出计算水平姿态角。

在静态环境下，载体线运动速度及其加速度都为0，可解得水平姿态角。

其中，θ、γ表示俯仰角与横滚角，分别表示加计在y轴与x轴的输出，g表示当地重力加速度大小。

②方位角由磁强计测量地磁信息来计算。

其中，表示在地平坐标系(记为h系)下x轴与y轴的磁场强度，磁航向角的范围为0°～360°，计算公式如下。

在得到水平姿态角和方位角后，

③方位角修正。

为磁偏角，载体的方位角需要经过的修正。磁偏角，是指地球表面任一点的磁子午圈同地理子午圈的夹角。根据规定，磁针指北极N向东偏则磁偏角为正，向西偏则磁偏角为负。磁偏角可根据“中国各地磁偏角”文档得知。

对所述误差方程进行分析，即MEMS惯导初始对准方法的误差方程可有如下分析得到。

①计算得到的姿态转移矩阵与真实姿态转移阵之间存在偏差，记为

其中，表示真实姿态转移阵，表示通过该方法计算得到的姿态转移矩阵，表示姿态转移阵误差。

②分析加速度计的输出误差。

其中，下标a表示加速度计，B表示零偏，S表示刻度因数，w_a表示加速度计随机噪声，假设该噪声为均值为零且独立分布的高斯白噪声。

③分析磁强计的测量输出误差。

磁强计的测量输出数学模型为：

其中，下标m表示磁强计，E_NO表示正交误差，这是由于传感器在制造与安装过程中三个敏感轴没有完全正交引入的误差，E_se为灵敏度误差，H_e表示地磁场矢量，E_HI表示硬磁误差，E_SF表示软磁误差，b₀表示零位误差，w_m表示磁强计随机噪声，本文假设该噪声为均值为零且独立分布的高斯白噪声。

磁强计在实际使用中，由于缺少专用的实验测试设备，对各个误差源逐个分析并分别进行补偿是十分困难的。因此，通常将磁强计的各误差源视为一个整体，进行综合建模与校准。通常地，采用椭球拟合法对磁强计进行校准。因此，磁强计输出的整体表达式如下所示。

其中，

K＝E_NOE_se(I_3×3+C_SI) (9)，

O＝KE_HI+b₀ (10)，

I_3×3表示维度为3×3的单位阵，C_SI表示软磁系数矩阵。

④对初始对准方法进行误差分析，

水平姿态角θ和γ，是加速度计输出直接计算得出的，因此水平方向失准角只与加速度计输出有关，关系式如下所示。

其中，φ_E、φ_N表示系统在东向与北向的姿态失准角，表示加速度计在北向的偏置，表示加速度计在东向的偏置，可以看到，东向失准角只与北向加速度计偏置有关，北向失准角只与东向加速度计偏置有关。

在方位对准中，计算方位角时，磁强计与加速度计的信息均参与了计算。因此，天向失准角不仅与加速度计偏置有关，还与磁强计测量误差相关，关系式如下。

φ_U表示天向的姿态失准角，表示加速度计在东向的偏置，与分别表示z轴与y轴的磁强计输出值，上标n表示导航坐标系n。g表示当地重力加速度大小。δM_E表示磁强计在东向的测量误差。

建立的系统模型如下。

忽略IMU与磁强计的安装偏差角，建立系统的状态量X如下。

将磁强计的磁场强度信息作为量测值引入系统，由于磁倾角与磁偏角均为小量误差，可以忽略不计。将量测值定义如下。

其中，表示的是n系到m系的姿态转移矩阵，m系为磁场坐标系，地磁在m系的投影为M^m＝[0 M 0]^T，其磁场大小记为M＝|M|。mⁿ为归一化后的磁场测量值，mⁿ＝Mⁿ/|Mⁿ|，δM_E表示磁强计在东向的测量误差。

离散随机线性系统模型如下所示。

其中，Φ_k/k-1表示k时刻系统状态转移阵，Γ_k-1表示系统噪声分配矩阵，H_k表示量测状态转移矩阵，W_k和V_k分别是系统噪声与量测噪声。其中，W_k＝[0 0 0 w_ay w_ax w_mx]^T，V_k＝[w_mx w_my w_mz]^T。W_k和V_k满足如下方程。

其中δ_kj为狄拉克函数，

Q_k表示系统噪声协方差阵，R_k表示量测噪声协方差阵。

Φ_k/k-1可表示为，

H_k＝[(mⁿ×)0_3×3] (19)，

采用的自适应卡尔曼滤波器中，量测噪声具有随机性与时变性，采用对历史数据求取算数平均值的方法不能减少对早期信息的使用，增加新近信息的权重，使得估计值不能及时反映噪声参数的变化。本发明实施例采用自适应指数渐消记忆的加权方式，设置多渐消因子调整量测噪声参数，有效增大新近信息的使用率，防止滤波发散。指数加权方法如下。

若记加权系数β_k,i，b为遗忘因子(0＜b＜1)，则有，

可以看出，，β_k,i体现了新旧信息对滤波过程中的贡献程度，对陈旧数据的利用率将以遗忘因子b的指数次方递减，可以根据使用的磁强计输出动态取值。

如图4所示，采用的自适应卡尔曼滤波具体实现过程如下。

其中，量测噪声协方差阵的估计值。在此方法中通过时变噪声估计器实时估计。

图2为MEMS惯导姿态初始化算法示意图，该算法利用加速度计与磁强计的输出信息求出系统的初始的姿态角。在静态环境下，载体线运动速度及其加速度都为0，利用加速度计输出的载体的比力信息，可以求出水平姿态角θ与γ。然后，根据水平姿态角可以求出载体系到地平坐标系的姿态转移矩阵，进一步地，根据地平坐标系下的x轴与y轴的磁场强度 求出磁方位角，再根据当地的磁偏角修正，得到最终的方位角。

本发明实施例提出的基于AKF的MEMS惯导系统姿态初始化方法示意图如图3所示。整个方法包括：MEMS初始对准方法误差分析，失准角建模，自适应卡尔曼滤波，姿态角闭环修正步骤。首先根据MEMS惯导系统姿态初始化方法的误差方程建立系统模型，确定状态量与量测量。然后采集惯性测量单元的数据，包括加速度计与磁强计的数据。计算出初始姿态角。再利用自适应卡尔曼滤波器计算姿态失准角，最后对初始姿态角进行闭环修正。

图4为数渐消加权自适应卡尔曼滤波示意图。方法具体过程为：①状态一步预测；②计算状态一步预测军方误差阵；③计算出一步预测误差；④计算量测的一步预测误差；⑤量测噪声协方差阵的实时估计；⑥综合考虑状态方程与量测方程的影响，利用量测信息修正状态量，计算出滤波增益；⑦修正一步预测的状态量，作为状态量的估计值；⑧计算状态估计均方差。

最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种MEMS惯导系统姿态初始化方法，其特征在于，包括：

根据姿态初始化的误差方程建立系统模型，并依据所述系统模型确定状态量与量测量；

获取惯性测量数据；

依据所述惯性测量数据计算初始姿态角；

基于所述状态量、量测量以及初始姿态角预测失准角修正值；

基于所述失准角修正值对所述初始姿态角进行闭环修正。

2.根据权利要求1所述的MEMS惯导系统姿态初始化方法，其特征在于，所述惯性测量数据，至少包括：

三轴加速度计与三轴磁强计。

3.根据权利要求2所述的MEMS惯导系统姿态初始化方法，其特征在于，所述依据所述惯性测量数据计算初始姿态角，包括：

基于所述三轴加速度计的输出计算得到水平姿态角；

基于所述三轴磁强计的测量的地磁信息计算得到方位角；

对所述方位角进行修正。

4.根据权利要求3所述的MEMS惯导系统姿态初始化方法，其特征在于，对所述方位角进行修正，为：

通过磁偏角对所述方位角进行修正。

5.根据权利要求2所述的MEMS惯导系统姿态初始化方法，其特征在于，所述根据姿态初始化的误差方程建立系统模型，包括：

对所述误差方程进行分析；

依据分析结果建立系统模型。

6.根据权利要求5所述的MEMS惯导系统姿态初始化方法，其特征在于，所述对所述误差方程进行分析，包括：

获取姿态转移矩阵与真实姿态转移阵之间的偏差；

获取所述三轴加速度计的输出误差；

获取三轴磁强计的测量输出误差；

基于所述姿态转移矩阵与真实姿态转移阵之间的偏差、三轴加速度计的输出误差和三轴磁强计的测量输出误差获取所述误差方程的误差。

7.根据权利要求1所述的MEMS惯导系统姿态初始化方法，其特征在于，所述系统模型，为：

式中，其中，Φ_k/k-1表示k时刻系统状态转移阵，Γ_k-1表示系统噪声分配矩阵，H_k表示量测状态转移矩阵，W_k-1为k-1时刻的系统噪声，W_k和V_k分别是系统噪声与量测噪声，X_k表示k时刻的系统状态向量，X_k-1表示k-1时刻的系统状态向量，Z_k表示k时刻的量测向量。

8.根据权利要求2所述的MEMS惯导系统姿态初始化方法，其特征在于，所述基于所述状态量、量测量以及初始姿态角预测失准角修正值，为：

利用自适应卡尔曼滤波基于所述状态量、量测量以及初始姿态角预测失准角修正值。

9.根据权利要求8所述的MEMS惯导系统姿态初始化方法，其特征在于，所述自适应卡尔曼滤波中，

采用自适应指数渐消记忆的加权方式，设置多渐消因子调整量测噪声参数。

10.根据权利要求9所述的MEMS惯导系统姿态初始化方法，其特征在于，所述自适应指数渐消记忆的加权方式中，

记加权系数为β_k,i，b为遗忘因子，0＜b＜1；

所述加权系数β_k,i对历史数据的利用率将以遗忘因子b的指数次方递减。