CN114235006A - 一种冗余式惯性测量单元故障补位陀螺仪标定方法 - Google Patents

Abstract

本发明提供一种冗余式惯性测量单元故障补位陀螺仪标定方法，其特征在于，包括如下步骤：步骤一：确定待标定的补位陀螺仪的数量及其序号；步骤二：构建故障补位陀螺仪标定模型；步骤三：构建状态空间模型，确定初始参数；步骤四：建立序贯自适应扩展卡尔曼滤波算法，估计待标定参数；步骤五：将估计得到的参数根据补偿模型标定陀螺仪，得到精确的角速度输出。本发明具有估计精度高、通用性强、自主性强等优点，无需外界参考即可实现系统量测噪声发生改变的条件下，冗余式惯性测量单元故障补位陀螺仪精确标定。

Description

一种冗余式惯性测量单元故障补位陀螺仪标定方法

技术领域

本发明涉及导航系统标定方法，尤其是对冗余式惯性导航系统故障补位陀螺仪标定方法。

背景技术

与传统三轴惯性测量单元相比，冗余式惯性测量单元在传感器失效后仍然能够在故障模式下工作，满足了导航系统长航时连续工作应用需求。冗余式惯性测量单元最典型的应用就是美国哈勃望远镜，哈勃望远镜配备了六个陀螺仪用于瞄准和自身姿态测量，其中三个陀螺仪正常工作即可保证系统安装稳定运行。然而，2018年10月，哈勃望远镜由于陀螺仪故障而进入安全模式，被迫停止观测。此时备用陀螺仪也因为存在故障而无法开启，只能紧急停止系统运转维修故障陀螺仪。为解决这一问题，有必要在传感器故障后，选择合适的时间更换故障传感器，以提高系统下一时段工作的可靠性。相比于更换全部传感器，仅更换故障传感器有利于降低系统维修成本，保证系统连续工作。

现有研究表明，冗余式惯性测量单元工作在无故障模式下时导航精度最优，在发生故障后导航精度性能会下降。因此在合适的时机对冗余式惯性测量单元中的故障陀螺仪进行更换在保证系统可靠性的同时也有助于提高导航精度。更换后的传感器可以被称为故障补位传感器，为保证导航系统的精确性，故障补位传感器参与导航工作前需要进行标定。

目前冗余式惯性测量单元发生故障并安装补位传感器后一般会采用基于转台参考的标定方法对冗余式惯性测量单元整体进行重新标定，然而这一方案不适用于在轨卫星、深海潜艇等设备。目前国内外未见针对故障工况下冗余式惯性测量单元故障补位陀螺仪标定的研究。针对冗余式惯性测量单元故障补位陀螺仪的标定是保证载体连续稳定且安全运行的基础，具有很好的发展前景。

发明内容

本发明的目的在于提供一种冗余式惯性测量单元故障补位陀螺仪标定方法，能够在没有外界参考的情况下，快速标定冗余式惯性测量单元中故障补位陀螺仪。

为了实现上述目的，1、一种冗余式惯性测量单元故障补位陀螺仪标定方法，其特征在于，包括如下步骤：

步骤一：确定待标定的补位陀螺仪的数量及其序号；

步骤二：构建故障补位陀螺仪标定模型；

步骤三：构建状态空间模型，确定初始参数；

步骤四：建立序贯自适应扩展卡尔曼滤波算法，估计待标定参数；

步骤五：将估计得到的参数根据补偿模型标定陀螺仪，得到精确的角速度输出。

进一步的是，步骤四中的所述待标定参数包括零偏、标度因数误差和安装误差。

进一步的是，步骤二中所述故障部位陀螺仪标定模型为：

W(H^TFH)^-1H^TFωⁱ-ω_m＝(KW+θ_yU+θ_xV-Kθ_yU-Kθ_xV)(H^TFH)^-1H^TFωⁱ+b+ε

其中，U＝[u₁ u₂ … u_m]^T，V＝[v₁ v₂ … v_m]^T，W＝[w₁ w₂ … w_m]^T为坐标转换矩阵，u_i、v_i和w_i与冗余式惯性测量单元的配置方式相关，b＝[b₁ b₂ … b_m]^T为待标定补位陀螺仪零偏，K＝diag([k₁ k₂ … k_m])为补位陀螺仪标度因数误差矩阵，θ_x＝diag([θ_x1 θ_x2 …θ_xm])，θ_y＝diag([θ_y1 θ_y2 … θ_ym])为补位陀螺仪安装误差矩阵，H为冗余式惯性测量单元的冗余配置矩阵，n为冗余式惯性测量单元中陀螺仪的总数量，m为待标定故障补位陀螺仪数量，且m≤n-3，ωⁱ为无故障陀螺仪输出，F为故障矩阵，在第i个传感器发生故障时，故障矩阵的第i行和第i列元素为零，ε为陀螺仪噪声矩阵，ω_m＝[ω₁ ω₂ … ω_m]^T为补位陀螺仪输出的角速度。

进一步的是，步骤四中所述状态空间模型包含状态方程和量测方程，所述状态方程为：

x_k+1＝x_k

其中，x＝[b k θ_x θ_y]^T，b为所有待标定补位陀螺仪的零偏，k为所有待标定补位陀螺仪的标度因数误差，θ_x和θ_y为所有待标定补位陀螺仪的安装误差；

所述量测方程为：

z_k+1＝(KW+θ_yU+θ_xV-Kθ_yU-Kθ_xV)·(H^TFH)^-1H^TFωⁱ+b+ε

其中，z_k+1＝W(H^TFH)^-1H^TFωⁱ-ω_m为量测量。

进一步的是，步骤四中所述序贯自适应扩展卡尔曼滤波算法包含如下步骤：

(1)状态预测

(2)估计误差协方差预测

P_k/k-1＝P_k-1+Q_k-1

(3)量测噪声协方差R_k估计：

其中，J_k为所述量测方程的雅克比矩阵，

(4)计算扩展卡尔曼滤波增益

(5)更新状态估计

(6)更新估计误差协方差：

P_k＝(I_n-K_kJ_k)P_k/k-1。

进一步的是，步骤五中所述补偿模型为：

其中，(-θ_xV-θ_yU+W)^*为-θ_xV-θ_yU+W的广义逆矩阵，

为补偿零偏、标度因数误差和安装误差后的补位陀螺仪输出。

进一步的是，所述冗余式惯性测量单元中惯性器件数量大于或等于4个。

进一步的是，所述故障矩阵F确定方法为：系统无故障时故障矩阵为单位对角阵，在第i个传感器发生故障时，故障矩阵的第i行和第i列元素为零。

本发明方法具有的有益效果为：

本发明利用冗余式惯性测量单元中，故障补位陀螺仪与无故障陀螺仪之间特殊的几何关系构造补位陀螺仪标定模型，并设计序贯自适应卡尔曼滤波算法估计待标定参数，可以在没有外界参考且量测噪声不确定的条件下，实现补位陀螺仪的精确标定。本发明方法准确性高，自主性强，可快速实现部位陀螺仪标定。

总而言之，本发明具有估计精度高、通用性强、自主性强等优点，无需外界参考即可实现系统量测噪声发生改变的条件下，冗余式惯性测量单元故障补位陀螺仪精确标定。

附图说明

图1为本发明方法的基本流程图。

图2为s系，ts系与b系之间关系的示意图。

图3为序贯自适应卡尔曼滤波算法流程图。

图4为正十二面体配置冗余式惯性测量单元结构示意图。

图5为模拟载体运动方案曲线图。

图6～图8为1号、3号和5号陀螺仪零偏估计过程曲线。

图9～图11为1号、3号和5号陀螺仪标度因数误差估计过程曲线。

图12～图14为1号、3号和5号陀螺仪安装误差θ_x估计过程曲线。

图15～图17为1号、3号和5号陀螺仪安装误差θ_y估计过程曲线。

图18～图20为1号、3号和5号陀螺仪标定前后输出精度对比曲线。

具体实施方式

本发明提供一种对冗余式惯性导航系统故障补位陀螺仪标定方法，本发明的冗余式惯性测量单元故障补位陀螺仪标定方法能够在没有外界参考的情况下，快速标定冗余式惯性测量单元中故障补位陀螺仪，其首先根据待标定的补位陀螺仪、无故障陀螺仪以及载体坐标系的关系建立故障补位陀螺仪的标定模型；然后根据标定模型建立状态空间模型，并采用序贯自适应扩展卡尔曼滤波算法，估计待标定参数；最后将估计得到的参数代入补偿方程，完成故障补位陀螺仪标定，得到精确角速度输出。该方法具有估计精度高、通用性强、自主性强等优点，无需外界参考即可实现系统量测噪声发生改变的条件下，冗余式惯性测量单元故障补位陀螺仪精确标定。

下面结合附图对本发明作进一步详细描述。

本发明提供一种冗余式惯性测量单元故障补位陀螺仪标定方法，图1为本发明方法的基本流程图，其流程图如图1所示。该方法包括以下步骤：

步骤一：确定待标定的补位陀螺仪的数量及其序号；

步骤二：构建故障补位陀螺仪标定模型；

步骤三：构建状态空间模型，确定初始参数(包括零偏、标度因数误差和安装误差)；

步骤四：建立序贯自适应扩展卡尔曼滤波算法，估计待标定参数；

步骤五：将估计得到的参数根据补偿模型标定陀螺仪，得到精确的角速度输出。

在三维空间中，设O_bX_bY_bZ_b为载体坐标系(b系)，其原点O_b位于载体重心，X_b轴和Y_b轴分别于载体的横轴和纵轴的方向重合，Z_b与X_b、Y_b组成右手笛卡尔坐标系。

为描述每一个陀螺仪与载体坐标系的关系，定义传感器坐标系(s系)和理论传感器坐标系(ts系)。

假设冗余式惯性测量单元中第i个陀螺仪待标定，该传感器对应的传感器坐标系记为OX_siY_siZ_si，s系与传感器固连，原点固定在传感器的中心，Z_si轴与第i个传感器的敏感轴ω_i重合，X_si轴在Z_siOZ_b平面上且与OZ_si垂直，Y_si轴与X_si和Z_si轴组成右手笛卡尔坐标系。

由于安装误差的存在，理论上传感器的敏感轴指向与实际指向存在偏差，假设ts系的原点与s系的原点重合，第i个传感器对应的ts系记为OX_tsiY_tsiZ_tsi，Z_tsi轴为无安装误差时传感器的理论输出轴，X_tsi轴在Z_tsiOZ_b平面上且与OZ_tsi垂直，Y_tsi轴与X_tsi和Z_tsi轴组成右手笛卡尔坐标系。

图2为s系，ts系与b系之间关系的示意图。s系，ts系与b系之间关系如图2所示。b系和ts系之间的关系可表示为

其中，α_i和β_i为陀螺仪的安装角。

为便于计算，记坐标转换矩阵每一行分别为

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

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

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

ts系的OZ_tsi轴可以通过两次旋转与s系的OZ_si轴重合。第一次旋转为：ts系绕OX_tsi轴旋转θ_x，第二次旋转为：ts系绕OY_tsi′旋转θ_y。其中，θ_x和θ_y为第i个陀螺仪的安装误差，且为小量，可以近似为sinθ_x≈θ_x，sinθ_y≈θ_y，cosθ_x≈1，cosθ_y≈1。根据坐标系转换关系可以得到

由于冗余陀螺仪实际输出ω_mi与s系OZ_si轴重合，因此ω_mi可表示为

带入b系和ts系之间的关系式可得

其中，ω_t为载体三轴角速率。

考虑零偏b_i、标度因数误差k_i和陀螺仪噪声ε_i后，冗余陀螺仪实际输出与转台提供的b系角速度之间的关系为

存在多个待标定的陀螺仪时，各个陀螺仪与b系之间关系的矩阵形式可以表示为

ω_m＝(I_n-K)(-θ_xV-θ_yU+W)ω_t-b-ε

其中，U＝[u₁ u₂ … u_m]^T，V＝[v₁ v₂ … v_m]^T，W＝[w₁ w₂ … w_m]^T为坐标转换矩阵，θ_x＝diag([θ_x1 θ_x2 … θ_xm])，θ_y＝diag([θ_y1 θ_y2 … θ_ym])，其中，m≤n-3。

由于未发生故障的陀螺仪在出厂时已完成标定，因此可以将未发生故障的陀螺仪输出作为参考，未发生故障的陀螺仪与载体坐标系角速率之间的关系为

ωⁱ＝FHω_t

式中，ωⁱ为无故障陀螺仪输出，F为故障矩阵且系统无故障时故障矩阵为单位对角阵，第i个陀螺仪发生故障时，故障矩阵的第i行和第i列元素作置零处理。

根据最小二乘估计，载体坐标系下的角速度可以表示为

ω_t＝(H^TFH)^-1H^TFωⁱ

已知待标定传感器与载体坐标系的关系以及无故障传感器与载体坐标系的关系，即可获得待标定传感器与无故障传感器的关系

ω_m＝(I_n-K)(-θ_xV-θ_yU+W)(H^TFH)^-1H^TFωⁱ-b-ε

将上式展开，待标定的陀螺仪标定模型可以表示为

W(H^TFH)^-1HTFωⁱ-ω_m＝(KW+θ_yU+θ_xV-Kθ_yU-Kθ_xV)(H^TFH)^-1H^TFωⁱ+b+ε

将标定模型转换为状态空间模型的形式，系统状态量选取为

x＝[b k θ_x θ_y]^T

式中，b为所有待标定陀螺仪的零偏，k为所有待标定陀螺仪的标度因数误差，θ_x和θ_y为所有待标定陀螺仪的安装误差。

由于零偏、标度因数误差和安装误差在短时间内变化较小，可以近似为常值，因此状态方程可以表示为

x_k+1＝x_k

根据所述标定模型，状态空间量测方程可以表示为

z_k+1＝(KW+θ_yU+θ_xV-Kθ_yU-Kθ_xV)·(H^TFH)^-1H^TFωⁱ+b+ε

其中，z_k+1＝W(H^TFH)^-1H^TFωⁱ-ω_m为量测量。

量测方程为非线性方程，因而在扩展卡尔曼滤波过程中需要计算其雅克比矩阵，雅克比矩阵为

为实现状态空间模型中状态量的有效估计设计自适应扩展卡尔曼滤波算法，算法流程如下：

(1)状态预测

(2)估计误差协方差预测：

P_k/k-1＝P_k-1+Q_k-1

(3)量测噪声协方差R_k估计：

其中，J_k为所述量测方程的雅克比矩阵。

(4)计算扩展卡尔曼滤波增益

(5)更新状态估计

(6)更新估计误差协方差：

P_k＝(I_n-K_kJ_k)P_k/k-1

在冗余式惯性测量单元中存在大于或等于五组传感器的条件下，可能会出现需要同时标定两个或两个以上传感器的情况，由于不同传感器之间的参数相互独立，在m个传感器同时需要标定时，可以在自适应扩展卡尔曼滤波算法的基础上将高维度的量测更新转换为m个低维度的量测更新，并对每一组量测更新单独设定滤波参数，避免了大规模矩阵计算的同时，提高了参数估计精度，这一滤波算法被称为序贯卡尔曼滤波。图3为序贯自适应卡尔曼滤波算法流程图。序贯自适应扩展卡尔曼滤波算法估计过程如图3所示。

最后，将估计得到的参数根据补偿模型标定陀螺仪，得到精确的角速度输出。

所述的补偿模型为：

其中，(-θ_xV-θ_yU+W)^*为-θ_xV-θ_yU+W的广义逆矩阵，

为补偿零偏、标度因数误差和安装误差后的补位陀螺仪输出。

下面以六陀螺仪正十二面体配置的冗余式惯性测量单元为对象，模拟其中存在三个未标定故障补位陀螺仪，且其他陀螺仪正常工作。图4为正十二面体配置冗余式惯性测量单元结构示意图。所涉及的冗余配置方案为：六个陀螺仪分别置于正十二面体的其中六个面上，如图4所示。

本发明方法实施的步骤如下：

(1)对六个陀螺仪进行编号，分别记为1号陀螺仪、2号陀螺仪、…、6号陀螺仪，其中1号、3号和5号陀螺仪为故障补位陀螺仪，其他陀螺仪均正常工作。分别采集六个陀螺仪输出的角速度数据。

按照正十二面体配置方式，冗余配置矩阵为

坐标转换矩阵为

故障矩阵为

F＝diag([0 1 0 1 0 1])

(2)将以上参数代入标定模型中，可得1号、3号和5号陀螺仪的标定模型表达式

(3)判断待标定陀螺仪数量大于1，则进入序贯自适应扩展卡尔曼滤波流程，估计3个补位陀螺仪的零偏、标度因数和安装误差。

(4)讲估计的到的补位陀螺仪的零偏、标度因数和安装误差代入到补偿模型中，即可得到标定后补位陀螺仪输出，采集标定后陀螺仪输出数据，并对比理论值，即可分析标定方法的精确性。

下面通过仿真例说明本发明方法的有效性。

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

(1)采样点设置

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

(2)载体运动设置

图5为模拟载体运动方案曲线图。模拟载体运动方案曲线如图5所示。

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

设置1号陀螺仪的零偏为0.5°/s，标度因数误差为0.02，安装误差为0.01和0.02；3号陀螺仪的零偏为0.3°/s标度因数误差为0.01，安装误差为0.02和0.04；5号陀螺仪的零偏为0.4°/s标度因数误差为0.02，安装误差为0.05和0.01。标定过程中MEMS-RIMU的转速设置为20°/s。MEMS-RIMU在第12秒时因振动而导致冗余陀螺仪的量测噪声增大，在第30秒震动结束，陀螺仪量测噪声恢复正常。

(4)仿真结果

依上述仿真条件，对所设计的冗余式惯性测量单元实验室标定方法进行仿真验证。图6～图8为1号、3号和5号陀螺仪零偏估计过程曲线。图9～图11为1号、3号和5号陀螺仪标度因数误差估计过程曲线。图12～图14为1号、3号和5号陀螺仪安装误差θ_x估计过程曲线。图15～图17为1号、3号和5号陀螺仪安装误差θ_y估计过程曲线。由图6～17可知，提出的标定方法能够在35秒内有效估计出三个补位陀螺仪的零偏、标度因数误差和安装误差，且在量测噪声发生改变时，参数估计过程波动较小，不影响估计过程的收敛性。

图18～图20为1号、3号和5号陀螺仪标定前后输出精度对比曲线。由图18～图20可知，将估计参数代入补偿方程后，标定够的陀螺仪输出精度提高显著，所提出的标定方法能够有效标定补位陀螺仪。

结合上述分析，得到如下的分析结果：通过本发明提出的冗余式惯性测量单元故障部位陀螺仪标定方法，在量测噪声不确定的条件下，可以准确地估计故障补位陀螺仪的零偏、标度因数和安装误差，提高了更换补位陀螺仪后惯性测量单元精度，也保证了系统长航时工作的可靠性。因此，本发明可以更为全面地提升导航系统性能，保证导航系统长航时工作的精度。

需要说明的是，上文只是对本发明进行示意性说明和阐述，本领域的技术人员应当明白，对本发明的任意修改和替换都属于本发明的保护范围。

Claims (8)

1.一种冗余式惯性测量单元故障补位陀螺仪标定方法，其特征在于，包括如下步骤：

步骤一：确定待标定的补位陀螺仪的数量及其序号；

步骤二：构建故障补位陀螺仪标定模型；

步骤三：构建状态空间模型，确定初始参数；

步骤四：建立序贯自适应扩展卡尔曼滤波算法，估计待标定参数；

步骤五：将估计得到的参数根据补偿模型标定陀螺仪，得到精确的角速度输出。

2.如权利要求1所述的冗余式惯性测量单元故障补位陀螺仪标定方法，其特征在于，步骤四中的所述待标定参数包括零偏、标度因数误差和安装误差。

3.如权利要求1所述的冗余式惯性测量单元故障补位陀螺仪标定方法，其特征在于，步骤二中所述故障部位陀螺仪标定模型为：

W(H^TFH)^-1H^TFωⁱ-ω_m＝(KW+θ_yU+θ_xV-Kθ_yU-Kθ_xV)(H^TFH)^-1H^TFωⁱ+b+ε

其中，H为冗余式惯性测量单元的冗余配置矩阵，U＝[u₁ u₂…u_m]^T，V＝[v₁ v₂…v_m]^T，W＝[w₁ w₂…w_m]^T为坐标转换矩阵，u_i、v_i和w_i与冗余式惯性测量单元的配置方式相关，b＝[b₁b₂…b_m]^T为待标定补位陀螺仪零偏，K＝diag([k₁ k₂…k_m])为补位陀螺仪标度因数误差矩阵，θ_x＝diag([θ_x1 θ_x2…θ_xm])，θ_y＝diag([θ_y1 θ_y2…θ_ym])为补位陀螺仪安装误差矩阵，n为冗余式惯性测量单元中陀螺仪的总数量，m为待标定故障补位陀螺仪数量，且m≤n-3，ωⁱ为无故障陀螺仪输出，F为故障矩阵，在第i个传感器发生故障时，故障矩阵的第i行和第i列元素为零，ε为陀螺仪噪声矩阵，ω_m＝[ω₁ ω₂…ω_m]^T为补位陀螺仪输出的角速度。

4.如权利要求1所述的冗余式惯性测量单元故障补位陀螺仪标定方法，其特征在于，步骤四中所述状态空间模型包含状态方程和量测方程，所述状态方程为：

x_k+1＝x_k

其中，x＝[b k θ_x θ_y]^T，b为所有待标定补位陀螺仪的零偏，k为所有待标定补位陀螺仪的标度因数误差，θ_x和θ_y为所有待标定补位陀螺仪的安装误差；

所述量测方程为：

z_k+1＝(KW+θ_yU+θ_xV-Kθ_yU-Kθ_xV)·(H^TFH)^-1H^TFωⁱ+b+ε

其中，z_k+1＝W(H^TFH)^-1H^TFωⁱ-ω_m为量测量。

5.如权利要求1所述的状态空间模型，其特征在于，步骤四中所述序贯自适应扩展卡尔曼滤波算法包含如下步骤：

第一、状态预测

第二、估计误差协方差预测

P_k/k-1＝P_k-1+Q_k-1

第三、量测噪声协方差R_k估计：

其中，J_k为所述量测方程的雅克比矩阵，

第四、计算扩展卡尔曼滤波增益

第五、更新状态估计

第六、更新估计误差协方差：

P_k＝(I_n-K_kJ_k)P_k/k-1。

6.如权利要求1所述的状态空间模型，其特征在于，步骤五中所述补偿模型为：

其中，(-θ_xV-θ_yU+W)^*为-θ_xV-θ_yU+W的广义逆矩阵，

为补偿零偏、标度因数误差和安装误差后的补位陀螺仪输出。

7.如权利要求1所述的状态空间模型，其特征在于，所述冗余式惯性测量单元中惯性器件数量大于或等于4个。

8.如权利要求3所述的状态空间模型，其特征在于，所述故障矩阵F确定方法为：系统无故障时故障矩阵为单位对角阵，在第i个传感器发生故障时，故障矩阵的第i行和第i列元素为零。

Images