CN110887505A - 一种冗余式惯性测量单元实验室标定方法 - Google Patents

Abstract

本发明提供一种冗余式惯性测量单元实验室标定方法，采用圆锥描述方案建立安装角模型，并得到载体角速率与每一个陀螺仪测量角速率的关系；根据传感器与载体坐标系的关系建立通用的惯性测量单元的标定模型；建立滤波器的状态方程和量测方程；根据量测方程对量测量进行行扩增，得到行扩增后的量测方程，使得系统可观测性提升；根据扩增后的量测方程求得雅可比矩阵和黑塞矩阵，并采用基于二阶泰勒展开的扩展卡尔曼滤波算法估计误差参数，得到精确的传感器零偏、标度因数误差和安装误差；本发明估计精度高、通用性强、实验过程简单、能同时估计出零偏、标度因数误差和安装误差三种误差等优点，能够有效提高惯性测量单元精度，具有很好的工程应用价值。

Description

一种冗余式惯性测量单元实验室标定方法

技术领域

本发明涉及一种标定方法，尤其涉及一种冗余式惯性测量单元实验室标定方法，属于捷 联惯导系统领域。

背景技术

由于捷联式惯性导航系统具有自主性强、隐蔽性好等优势，因此一直是船舶、飞机、汽 车等载体的核心导航系统，为载体提供姿态、速率和位置等导航信息，捷联式惯性导航系统 的可靠性的高低直接影响了航行器的工作状态，导航系统出现故障可能会导致航行器无法工 作，甚至导致重大事故。随着科技发展，对导航系统可靠性要求逐渐提高，越来越多的场合 要求导航系统能够适应高动态、大过载、大冲击等工作环境，所以导航系统的可靠性和长航 时精度越来越成为考察其性能的重要技术指标。因此，提高惯性导航系统可靠性成为惯性导 航领域一项重要的研究内容。通过应用冗余容错技术提高惯性导航系统的可靠性是发展的主 要方向。由于惯性器件性能及安装条件限制，即使使用冗余容错技术的系统也会存在标度因 数、安装误差、常值漂移等误差，相比于传统三轴正交惯性导航系统，冗余式惯性导航系统 器件数量增多，且传感器的安装方式多为非正交安装，传感器输出数据具有耦合性，无法使 用传统的常值误差模型，而常用的四冗余式捷联惯导系统的12误差标定模型参数多，标定方 法复杂。因此，设计一种合理的标定方法，降低惯性导航系统的常值误差是提高导航信息精 度的关键。

现有的惯性测量单元实验室标定方法种类繁多，其中，基于高精度转台的多位置标定方 法，是将载体置于高精度转台上，通过设定转台在不同位置匀速转动，根据每一个位置转台 角速率与斜置陀螺仪的关系得到多个方程，并基采用最小二乘法估计方程的解，得到零偏、 标度因素误差和安装误差。Syed等人在《A new multi-position calibrationmethod for MEMS inertial navigation systems》(发表于《Measurement Science andTechnology》，2007年，18期) 提出一种多位置标定方法，能够同时估计出零偏、标度因素误差和安装误差，便于工程实现 而得到了广泛应用。但该方法仅适用于传统三轴的微惯性测量单元标定，且无法标定不能敏 感地球自转角速率的低精度惯性测量单元。刘万科等人在《基于匀速率26位置法的 iIMU–FSAS光纤陀螺仪标定》(发表于《惯性技术学报》，2017年，01期)提出基于高精度 转台的匀速率标定方案，适用于低精度惯性测量单元的标定，但仍然不适用于冗余式系统。 Mark在《Calibration and Attitude Determination withRedundant Inertial Measurement Units》(发 表于《Journal of Guidance,Control,andDynamics，2005年，04期》提出的冗余式惯性测量单 元的标定方法，但其建模不精确导致标定精度较低。

发明内容

本发明的目的是为了提高导航信息精度而提供一种一种冗余式惯性测量单元实验室标定 方法。

本发明的目的是这样实现的：

一种冗余式惯性测量单元实验室标定方法，包括如下步骤：

(1)采用圆锥描述方案建立安装角模型，并得到载体角速率与每一个陀螺仪测量角速率 的关系；

(2)根据传感器与载体坐标系的关系建立通用的惯性测量单元的标定模型；

(3)建立滤波器的状态方程和量测方程；

(4)根据步骤三(3)中量测方程，对量测量进行行扩增，得到行扩增后的量测方程，使得系统可观测性提升；

(5)根据扩增后的量测方程求得雅可比矩阵和黑塞矩阵，并采用基于二阶泰勒展开的扩 展卡尔曼滤波算法估计误差参数，得到精确的传感器零偏、标度因数误差和安装误差。

本发明还包括这样一些特征：

所述步骤(1)具体为：

以陀螺仪敏感轴与载体坐标系z轴的夹角为第一个安装角α，以陀螺仪敏感轴在载体坐 标系xoy面的投影与x轴的夹角为第二个安装角β，根据载体坐标系与陀螺仪敏感轴的关系建 立载体角速率与每一个陀螺仪测量角速率的关系，

其中，ω_gn为第n个陀螺仪输出角速率，ω_bx，ω_by，ω_bz为载体坐标系下角速率；

所述通用的惯性测量单元的标定模型为

式中b_n为第n个传感器的零偏，k_n为第n个传感器的标度因数误差，δα_n为第n个传感器 的α安装角误差，δβ_n为第n个传感器的β安装角误差，w_n、u_n、v_n为传感器在载体坐标系 安装关系矩阵；

所述传感器在载体坐标系安装关系矩阵表示为：

w_i＝[cosβ_i sinα_i sinβ_isinα_i cosα_i]

u_i＝[cosβ_isin(α_i+90°) sinβ_isin(α_i+90°) cos(α_i+90°)]

v_i＝[cos(β_i+90°) sin(β_i+90°)0]；

所述通用滤波器的状态方程为；

X_k+1＝X_k

式中X＝[b₁ ... b_n k₁... k_n δα₁ ... δα_n δβ₁ ... δβ_n]^T为滤波器状态量， 所述量测方程可表示为：

z_k＝H_inv(δβUω_t+δαVω_t+KWω_t-KδαVω_t-KδβUω_t+B)

式中z_k＝ω_t-H_invω_G为量测量；

所述行扩增后的状态方程为：

式中H_inv为最小二乘法求得的H的广义逆，N为扩增矩阵；

所涉及的最小二乘法求得的H的广义逆表达式为：

H_inv＝(H^TH)^-1H^T

所涉及的扩增矩阵表达式为：

所述雅可比矩阵为：

所述黑塞矩阵为：

C_BB＝C_BK＝C_Bδα＝C_Bδβ＝C_KB＝C_KK＝C_δαB

＝C_δαδα＝C_δαδβ＝C_δβB＝C_δβδα＝C_δβδβ＝0

其中，g_k＝H_kX_k，

与现有技术相比，本发明的有益效果是：

本发明建立了高精度的冗余式惯性测量单元的标定模型，结合二阶泰勒展开的扩展卡尔 曼滤波方法估计各个误差，具有估计精度高、通用性强、实验过程简单、能同时估计出零偏、 标度因数误差和安装误差三种误差等优点，能够有效提高惯性测量单元精度，具有很好的工 程应用价值。

附图说明

图1是本发明提出的冗余式惯性测量单元实验室标定方法的基本流程框图；

图2是传感器坐标系与载体坐标系关系示意图；

图3是四陀螺四面体冗余配置方案；

图4是模拟载体在转台上的三轴角运动方案曲线；

图5a～图5d为1～4号陀螺零偏估计曲线；

图6a～图6d为1～4号陀螺标度因数误差估计曲线；

图7a～图7d为1～4号陀螺安装角α误差估计曲线；

图8a～图8d为1～4号陀螺安装角β误差估计曲线。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

本发明的目的在于提供一种冗余式惯性测量单元实验室标定方法。

为实现上述目的，本发明采用的技术方案包括下列步骤：

(1)采用圆锥描述方案建立安装角模型，并得到载体角速率与每一个陀螺仪测量角速率 的关系；

(2)根据传感器与载体坐标系的关系建立通用的惯性测量单元的标定模型；

(3)建立滤波器的状态方程和量测方程；

(4)根据(3)中量测方程，对量测量进行行扩增，得到行扩增后的量测方程，使得系统可观测性提升；

(5)根据扩增后的量测方程求得雅可比矩阵和黑塞矩阵，并采用基于二阶泰勒展开的扩 展卡尔曼滤波算法估计误差参数，得到精确的传感器零偏、标度因数误差和安装误差。

所述采用圆锥描述方案建立安装角模型，并得到载体角速率与每一个陀螺仪测量角速率 的关系，包括：

以陀螺仪敏感轴与载体坐标系z轴的夹角为第一个安装角α，以陀螺仪敏感轴在载体坐 标系xoy面的投影与x轴的夹角为第二个安装角β。根据载体坐标系与陀螺仪敏感轴的关系建 立载体角速率与每一个陀螺仪测量角速率的关系，

其中，ω_gn为第n个陀螺仪输出角速率，ω_bx，ω_by，ω_bz为载体坐标系下角速率；

所述通用的惯性测量单元的标定模型为

式中b_n为第n个传感器的零偏，k_n为第n个传感器的标度因数误差，δα_n为第n个传感器 的α安装角误差，δβ_n为第n个传感器的β安装角误差，w_n、u_n、v_n为传感器在载体坐标系 安装关系矩阵；

所述传感器在载体坐标系安装关系矩阵表示为：

w_i＝[cosβ_i sinα_i sinβ_isinα_i cosα_i]

u_i＝[cosβ_isin(α_i+90°) sinβ_i sin(α_i+90°) cos(α_i+90°)]

v_i＝[cos(β_i+90°) sin(β_i+90°)0]；

所述通用滤波器的状态方程为；

X_k+1＝X_k

式中X＝[b₁ ... b_n k₁ ... k_n δα₁ ... δα_n δβ₁ ... δβ_n]^T为滤波器状态量。

所述量测方程可表示为：

z_k＝H_inv(δβUω_t+δαVω_t+KWω_t-KδαVω_t-KδβUω_t+B)

式中z_k＝ω_t-H_invω_G为量测量；

行扩增后的状态方程为：

式中H_inv为最小二乘法求得的H的广义逆，N为扩增矩阵；

所涉及的最小二乘法求得的H的广义逆表达式为：

H_inv＝(H^TH)^-1H^T

所涉及的扩增矩阵表达式为：

所述雅可比矩阵为：

所述黑塞矩阵为：

C_BB＝C_BK＝C_Bδα＝C_Bδβ＝C_KB＝C_KK＝C_δαB

＝C_δαδα＝C_δαδβ＝C_δβB＝C_δβδα＝C_δβδβ＝0

其中，g_k＝H_kX_k，

由此完成冗余式惯性测量单元的实验室标定。将估计得到的零偏、标度因数误差和安装 误差带入到惯性测量单元中，即可得到精确的角速率输出。

本发明提出一种冗余式惯性测量单元实验室标定方法，其流程图如附图1所示，该方法 的主要步骤如下：

(1)以四陀螺四面体冗余配置的冗余惯性测量单元为对象，将陀螺仪与加速度计成对放 置与四面体的每一个面上，本实验仅以陀螺仪为例，其配置方案如图2所示，并采集该系统 陀螺仪输出数据；

所涉及的配置方式为：

陀螺1沿载体坐标系z_b轴方向安装，陀螺仪2、3、4与z_b轴均成α＝70.53°夹角斜置安装于四面体的3个侧面，4个斜置陀螺测量轴在x_boy_b面上的投影与ox_b轴的夹角分别为 β₂＝0°，β₃＝120°，β₄＝240°；

按上述配置方式，四陀螺冗余惯性测量单元配置矩阵H为：

载体角速率与每一个陀螺仪测量角速率的关系为：

(2)建立传感器坐标系ox_gy_gz_g，以传感器敏感轴为传感器坐标系z_g轴，x_g轴在z_goz_b平 面且与z_g轴垂直，y_g与x_g，z_g构成右手笛卡尔坐标系。传感器坐标系及其与载体坐标系的关系如图2所示。由此可以得到两坐标系之间的关系：

x_gi＝[cosβ_isin(α_i+90°) sinβ_isin(α_i+90°) cos(α_i+90°)][x_b y_b z_b]^T

＝u_i[x_b y_b z_b]^T

y_gi＝[cos(β_i+90°) sin(β_i+90°) 0]·[x_b y_b z_b]^T＝v_i[x_b y_b z_b]^T

z_gi＝[cosβ_isinα_i sinβ_isinα_i cosα_i]·[x_b y_b z_b]^T＝w_i[x_b y_b z_b]^T

其中，i为陀螺仪编号。

当存在安装误差时，相当于传感器坐标系绕x_g轴和y_g轴进行两次小角度转动，令转动角 度分别为δα和δβ，根据坐标系的小角度转动公式，可以得到转动后传感器坐标系ox′_gy′_gz′_g与 ox_gy_gz_g的关系：

由于陀螺仪敏感轴与传感器坐标系z_g重合，因此存在安装误差时陀螺仪角速率输出为：

其中ω_xb，ω_yb和ω_zb为高精度转台提供的三轴载体坐标系角速率。

(3)根据步骤(2)中存在安装误差时陀螺仪角速率输出公式，并带入陀螺仪零偏和标度因 数误差，惯性测量单元的标定模型表示为:

式中i为陀螺仪编号。

在四陀螺惯性测量单元中将上式通过矩阵方式表达，表达式为：

ω_G＝(I-B)(Wω_t-δβUω_t-δαVω_t)-B

其中，ω_G＝[ω_g1 … ω_g4]^T，ω_t＝[ω_x ω_y ω_z]^T，W＝H＝[w₁ … w₄]^T， U＝[u₁ …u₄]^T，V＝[v₁ … v₄]^T，δβ＝diag([δβ₁ … δβ₄])，δα＝diag([δα₁ … δα_n])。

(4)根据步骤(3)建立的标定模型，建立滤波器的状态方程和量测方程，由于系统状态量不 随时间变化，因此状态方程为：

X_k+1＝X_k

式中X为滤波器状态量

所述滤波器状态量表达式为：

X＝[b₁ ... b₄ k₁ ... k₄ δα₁ ... δα₄ δβ₁ ... δβ₄]^T

量测方程表达式为：

z_k＝H_inv(δβUω_t+δαVω_t+KWω_t-KδαVω_t-KδβUω_t+B)

式中z_k为量测量，式中H_inv为最小二乘法求得的H的广义逆；

所涉及的量测量z_k表示为：

z_k＝ω_t-H_invω_G

所涉及的最小二乘法求得的H的广义逆表达式为：

H_inv＝(H^TH)^-1H^T；

(5)为提高系统的可观测性，对步骤(4)建立的滤波器量测方程进行行扩增，引入扩增矩阵 N

所涉及的扩增矩阵表达式为：

将扩增矩阵带入滤波器量测方程可以得到扩增后的滤波器量测方程：

(6)根据步骤(5)中的量测方程和状态方程，求得雅可比矩阵为

求得黑塞矩阵为：

C_BB＝C_BK＝C_Bδα＝C_Bδβ＝C_KB＝C_KK＝C_δαB＝C_δαδα＝C_δαδβ＝C_δβB＝C_δβδα＝C_δβδβ＝0

其中，g_k＝H_kX_k，

根据雅可比矩阵和黑塞矩阵完成以下二级泰勒展开卡尔曼滤波过程，并估计出每个陀螺 仪的零偏、标度因数误差和安装误差：

P_k|k-1＝P_k-1

P_k＝(I-K_kH_k)P_k|k-1；

由此完成惯性测量单元的实验室标定过程，将估计出的零偏、标度因数和安装误差带入 陀螺仪中，即可得到精确地角速率输出。

Matlab程序仿真的方案、条件及结果如下所示：

(1)采样点设置

模拟4000个采样点，采样频率为100Hz。

(2)转台运动设置

模拟载体在转台上的三轴角运动方案曲线如图3所示。

(3)陀螺仪零偏、标度因数误差和安装误差设置

1～4号陀螺的零偏分别设置为b₁＝0.2°/s，b₂＝0.4°/s，b₃＝0.3°/s，b₄＝0.1°/s；1～4 号陀螺的标度因数分别设置为k₁＝0.01ppm，k₂＝0.02ppm，k₃＝0.01ppm，k₄＝0.03ppm； 1～4号陀螺的安装角α的安装误差分别设置为δα₁＝0.03rad，δα₂＝0.01rad，δα₃＝0.02rad， δα₄＝0.03rad；1～4号陀螺的安装角β的安装误差分别设置为δβ₁＝0.02rad，δβ₂＝0.02rad， δβ₃＝0.04rad，δβ₄＝0.01rad。

(4)仿真结果

依上述仿真条件，对所设计的冗余式惯性测量单元实验室标定方法进行仿真验证，由图 8a～8d、图5a～5d、图6a～6d和图7a～7d可知。

结合上述分析，得到如下的分析结果：通过本发明提出的冗余式惯性测量单元实验室标 定方法，可以准确地估计陀螺仪的零偏、标度因数和安装误差，提高了惯性测量单元精度。 因此，本发明可以更为全面地提升导航系统性能，保证导航系统精度。

本发明公开了一种冗余式惯性测量单元实验室标定方法。该方法首先提出安装误差角的 圆锥描述方案，根据圆锥描述方案建立一种通用的惯性测量单元的安装误差模型，并将零偏 和标度因数误差带入安装误差模型中，得到通用的冗余式惯性测量单元的标定模型；然后根 据标定模型建立滤波器的状态方程和量测方程，采用零空间扩增的方法增加量测方程的可观 测性；最后，采用基于二阶泰勒展开的扩展卡尔曼滤波算法估计零偏、标度因数误差和安装 误差。该方法具有估计精度高、通用性强、实验过程简单、能同时估计出零偏、标度因数误 差和安装误差三种误差等优点，能够有效提高惯性测量单元精度。

Claims (6)

1.一种冗余式惯性测量单元实验室标定方法，其特征是，包括如下步骤：

(1)采用圆锥描述方案建立安装角模型，并得到载体角速率与每一个陀螺仪测量角速率的关系；

(2)根据传感器与载体坐标系的关系建立通用的惯性测量单元的标定模型；

(3)建立滤波器的状态方程和量测方程；

(4)根据步骤三(3)中量测方程，对量测量进行行扩增，得到行扩增后的量测方程，使得系统可观测性提升；

(5)根据扩增后的量测方程求得雅可比矩阵和黑塞矩阵，并采用基于二阶泰勒展开的扩展卡尔曼滤波算法估计误差参数，得到精确的传感器零偏、标度因数误差和安装误差。

2.根据权利要求1所述的冗余式惯性测量单元实验室标定方法，其特征是，所述步骤(1)具体为：

以陀螺仪敏感轴与载体坐标系z轴的夹角为第一个安装角α，以陀螺仪敏感轴在载体坐标系xoy面的投影与x轴的夹角为第二个安装角β，根据载体坐标系与陀螺仪敏感轴的关系建立载体角速率与每一个陀螺仪测量角速率的关系，

其中，ω_gn为第n个陀螺仪输出角速率，ω_bx，ω_by，ω_bz为载体坐标系下角速率。

3.根据权利要求1所述的冗余式惯性测量单元实验室标定方法，其特征是，所述通用的惯性测量单元的标定模型为

式中b_n为第n个传感器的零偏，k_n为第n个传感器的标度因数误差，δα_n为第n个传感器的α安装角误差，δβ_n为第n个传感器的β安装角误差，w_n、u_n、v_n为传感器在载体坐标系安装关系矩阵；

所述传感器在载体坐标系安装关系矩阵表示为：

w_i＝[cosβ_isinα_i sinβ_isinα_i cosα_i]

u_i＝[cosβ_isin(α_i+90°) sinβ_isin(α_i+90°) cos(α_i+90°)]

v_i＝[cos(β_i+90°) sin(β_i+90°) 0]。

4.根据权利要求1所述的冗余式惯性测量单元实验室标定方法，其特征是，所述通用滤波器的状态方程为；

X_k+1＝X_k

式中X＝[b₁...b_n k₁…k_n δα₁...δα_n δβ₁…δβ_n]^T为滤波器状态量，

所述量测方程可表示为：

z_k＝H_inv(δβUω_t+δαVω_t+KWω_t-KδαVω_t-KδβUω_t+B)

式中z_k＝ω_t-H_invω_G为量测量。

5.根据权利要求1所述的冗余式惯性测量单元实验室标定方法，其特征是，所述行扩增后的状态方程为：

式中H_inv为最小二乘法求得的H的广义逆，N为扩增矩阵；

所涉及的最小二乘法求得的H的广义逆表达式为：

H_inv＝(H^TH)^-1H^T

所涉及的扩增矩阵表达式为：

6.根据权利要求1所述的冗余式惯性测量单元实验室标定方法，其特征是，所述雅可比矩阵为：

所述黑塞矩阵为：

C_BB＝C_BK＝C_Bδα＝C_Bδβ＝C_KB＝C_KK＝C_δαB

＝C_δαδα＝C_δαδβ＝C_δβB＝C_δβδα＝C_δβδβ＝0

其中，g_k＝H_kX_k，

Images