CN110361031A - 一种基于回溯理论的imu全参数误差快速标定方法 - Google Patents

Abstract

本发明公开了一种基于回溯理论的IMU全参数误差的快速标定方法，本发明设计了一种三位置下正反旋转的的标定轨迹，通过惯性组件误差传播规律分析误差状态的可观测性，基于回溯框架理论的标定方法可以短时间内快速标定IMU全参数误差。本发明的有益效果为：基于***级的标定方法可以标定出光纤陀螺与加速度计的零位误差，刻度因子误差，安装误差以及加速度计在动态环境下的杆臂误差；本发明简化了标定轨迹的编排，极大的缩短了标定时间，提高了IMU误差标定的效率。

Description

一种基于回溯理论的IMU全参数误差快速标定方法

技术领域

本发明涉及标定技术领域，特别是涉及一种基于回溯理论的IMU全参数误差快速标定方法。

背景技术

捷联惯性导航***能够在不受外界干扰的情况下提供导航信息。然而，捷联惯性导航***的误差会随着时间的推移而积累和发散，这将严重制约其导航性能。惯性传感器误差是捷联惯导误差发散的主要来源。误差参数的精确标定是保证惯性导航性能的关键。同时，IMU 的标定也是捷联惯导***运行前的关键环节之一，它对精度和速度要求很高。

目前传统的IMU标定方法有分立式标定与***级标定。分立式标定一般是依靠高精度转台提供位置和速率参考，标定精度受到转台的控制精度的影响。***级通常以导航误差作为观测量，对转台的精度要求很低。

虽然传统的***级标定方法具有不依赖外部设备精度的优点，然而由于***状态维数过高，因此计算量大，并且所需要的标定时间长，通常需要半小时甚至一小时以上的时间，才能标定出全部的IMU误差参数。同时，为了激励出所有的误差参数，标定路径的设计也相当复杂，过长的标定时间将极大的降低导航的效率。

因此，在保证精度的前提下，一种快速高效的IMU全参数误差标定方法，对于提高IMU 在导航过程中的效率至关重要。

发明内容

为了解决以上问题，本发明提供一种基于回溯理论的IMU全参数误差快速标定方法，设计了一种三位置下正反旋转的的标定轨迹，通过惯性组件误差传播规律分析误差状态的可观测性，以惯性系下的速度误差作为观测数据，结合回溯导航理论建立误差标定的卡尔曼滤波方程，到达快速标定IMU全参数误差的目的，为达此目的，本发明提供一种基于回溯理论的 IMU全参数误差快速标定方法，包括如下步骤：

(1)建立惯性组件***误差模型与惯性系下捷联惯导***误差模型；

(2)将惯性组件安装于转台上，初始朝向为东-北-天方向，使惯性组件以变角速度绕Y 轴正向旋转360°，其次再绕Y轴反向旋转360°；

(3)惯性组件绕Z轴正向旋转90°，使惯性组件的朝向为北-西-天，使惯性组件以变角速度绕X轴正向旋转360°，其次再绕X轴反向旋转360°；

(4)惯性组件绕Y轴正向旋转90°，使惯性组件的朝向为地-西-北，使惯性组件以变角速度绕Z轴正向旋转360°，其次再绕Z轴反向旋转360°；

(5)惯性组件绕Y轴反向旋转90°，再绕Z轴反向旋转90°，使惯性组件的朝向为东 -北-天方向，使惯性组件以变角速度绕Y轴正向旋转360°，其次再绕Y轴反向旋转360°；

(6)建立惯性组件***误差标定的卡尔曼滤波方程；

(7)设计基于回溯理论的***级标定方法。

本发明的进一步改进，所述步骤(1)中，建立惯性组件误差模型具体为：

将光纤陀螺的零偏误差、刻度系数误差和安装误差加入陀螺误差模型，得：

其中，g表示陀螺的安装坐标系，b表示真实的传感器坐标系，表示陀螺的***误差；表示陀螺的常值漂移误差，ω_b为陀螺真实的输出，表示陀螺安装误差矩阵，表示陀螺的刻度系数误差矩阵；

将加速度计的零偏误差，刻度系数误差，安装误差以及动态杆臂误差加入到误差模型中，得：

其中，a表示加速度计安装坐标系，b表示真实的传感器坐标系，表示加速度计的***误差，表示加速度计的常值零偏误差，f_b表示加速度计的真实输出；表示加速度计的安装误差矩阵，表示加速度计的刻度系数误差矩阵；

表示加速度计的杆臂误差矩阵；

建立惯性系下姿态误差方程为：

其中，ib0表示载体坐标系，是在标定过程开始时将载体坐标系的轴线固定在惯性空间中形成的，in0表示导航坐标系，是在标定过程开始时将导航坐标系的轴线固定在惯性空间中形成的，为从载体坐标系至初始时刻载体坐标系的变换矩阵，表示初始时刻的姿态矩阵，φ＝[φ_E φ_N φ_U]表示载体的姿态误差角；

建立惯性系下速度误差方程为：

其中，ib0表示载体坐标系，是在标定过程开始时将载体坐标系的轴线固定在惯性空间中形成的，in0表示导航坐标系，是在标定过程开始时将导航坐标系的轴线固定在惯性空间中形成的，f^b表示载体坐标系下加速度计真实的输出。

本发明的进一步改进，所述步骤(2)中，将惯性组件安装于转台上，初始朝向为东-北- 天方向，使惯性组件以变角速度绕Y轴正向旋转360°，其次再绕Y轴反向旋转360°，具体为：

在东-北-天的朝向时，绕Y轴旋转，陀螺与加速度计的理论输出为：

其中，θ为旋转角度，L是当地纬度，ω_T为旋转角速度，ω_ie为地球自转角速度，由于ω_ie＜＜ω_T，因此，陀螺的输出可以简化为：

因此，由加速度计和陀螺引起的***误差可以表达为：

根据状态和量测之间的关系可以得到：

其中，

式中，γ表示横滚角；

因此得到：

式中，

根据以上分析，得到，是不可观测的，由于 α_yx和β_xy是相互耦合的状态量.如果将α_yx视作不可观测状态,那么β_xy是可观测的；

在光纤陀螺中,由于ε_z＜＜β_zyω_T并且ε_x＜＜β_xyω_T，对于状态β_zy和β_xy更加敏感；因此,ε_x和ε_z的可观测性很弱；

在该旋转方案下，有如下状态是可观测的：

本发明的进一步改进，所述步骤(3)中，惯性组件绕Z轴正向旋转90°，使惯性组件的朝向为北-西-天，使惯性组件以变角速度绕X轴正向旋转360°，其次再绕X轴反向旋转360°，具体为：

在北-西-天的朝向时，绕X轴旋转，陀螺与加速度计的理论输出为：

其中，θ为旋转角度，ω_T为旋转角速度，

由加速度计和陀螺引起的***误差可以表达为：

因此，得到导航系下等效的偏置误差为：

其中，β表示俯仰角；

因此，得到在该旋转方案下，有如下状态是可观测的：

本发明的进一步改进，所述步骤(4)中，惯性组件绕Y轴正向旋转90°，使惯性组件的朝向为地-西-北，使惯性组件以变角速度绕Z轴正向旋转360°，其次再绕Z轴反向旋转360°，具体为：

在地-西-北的朝向时，绕Z轴旋转，陀螺与加速度计的理论输出为：

由加速度计和陀螺引起的***误差可以表达为：

因此，可以得到导航系下等效的偏置误差为：

其中，表示航向角；

因此，得到在该旋转方案下，有如下状态是可观测的：

本发明的进一步改进，所述步骤(5)中，惯性组件绕Y轴反向旋转90°，再绕Z轴反向旋转90°，使惯性组件的朝向为东-北-天方向，使惯性组件以变角速度绕Y轴正向旋转360°，其次再绕Y轴反向旋转360°，具体为：

在东-北-天的朝向时，绕Y轴旋转，陀螺与加速度计的理论输出为：

其中，θ为旋转角度，ω_T为旋转角速度；

由加速度计和陀螺引起的***误差可以表达为：

因此，可以得到导航系下等效的偏置误差为：

式中，γ表示横滚角；

因此，得到在该旋转方案下，有如下状态是可观测的：

本发明的进一步改进，所述步骤(6)中，建立惯性组件***误差标定的卡尔曼滤波方程，具体为：

建立***级标定卡尔曼滤波状态方程：

式中，

[φ_E φ_N φ_U]表示姿态角误差，表示in0坐标系下的速度误差，ε＝[ε_x ε_y ε_z]表示陀螺零偏，K_g＝[K_gx K_gy K_gz]表示陀螺刻度因子，β＝[β_xy β_xz β_yxβ_yz δβ_zx β_zy]表示陀螺的安装误差，表示加速度计的零偏误差，K_a＝[K_axK_ay K_az]表示加速度计的刻度因子，α＝[α_yx α_zx α_zy]表示加速度计的安装误差，表示加速度计的杆臂误差；

式中，W表示过程噪声，T_ij(i，j＝1，2，3)为姿态矩阵对应元素，(·)×表示反对称矩阵；

相应的量测方程：

Z＝HX+V

其中，V表示量测噪声，H＝[0_3×3 I_3×3 0_3×30]，Z＝δvⁱⁿ⁰表示以导航速度误差为观测量。

本发明的进一步改进，所述步骤(7)中，设计基于回溯理论的***级标定方法，具体为：

当一次滤波完成后，得到***各状态的估计值：

因此得到初始的姿态误差矩阵初始速度vⁱⁿ⁰以及IMU***误差的估计值，以此滤波估计的稳态值作为下一次回溯导航的误差状态初值，进行回溯导航过程。

本申请一种基于回溯理论的IMU全参数误差快速标定方法，具有以下有益效果：

根据以上步骤所建立的基于回溯理论的IMU全参数误差快速标定方法，可以估计出 IMU的所有***误差参数。根据步骤2-5所设计的标定轨迹，与传统的标定轨迹相比，极大降低了标定轨迹的复杂度，利用三位置下的正反旋转即可激励出所有误差参数。利用步骤6-7所设计的基于回溯理论的卡尔曼滤波标定模型，可以极大的缩短标定时间，提高采集的IMU数据利用率，在保证估计精度的同时，加快了标定的速度，提高了导航效率。

附图说明

图1为本发明的标定方法中转台旋转路径编排示意图。

具体实施方式

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

本发明提供一种基于回溯理论的IMU全参数误差快速标定方法，设计了一种三位置下正反旋转的的标定轨迹，通过惯性组件误差传播规律分析误差状态的可观测性，以惯性系下的速度误差作为观测数据，结合回溯导航理论建立误差标定的卡尔曼滤波方程，到达快速标定IMU全参数误差的目的。

如图1所示的转台旋转路径编排示意图，本发明通过设计转台的三位置旋转，以惯性系下的速度误差作为观测量，同时为了缩短标定时间，设计基于回溯导航理论的快速标定方法，通过卡尔曼滤波器估计惯性组件误差参数。具体标定方法如下：

步骤一：建立惯性组件***误差模型与惯性系下捷联惯导***误差模型，具体为：

将光纤陀螺的零偏误差、刻度系数误差和安装误差加入陀螺误差模型，得：

其中，g表示陀螺的安装坐标系，b表示真实的传感器坐标系。表示陀螺的***误差。表示陀螺的常值漂移误差。ω_b为陀螺真实的输出。表示陀螺安装误差矩阵。表示陀螺的刻度系数误差矩阵。

将加速度计的零偏误差，刻度系数误差，安装误差以及动态杆臂误差加入到误差模型中，得：

其中，a表示加速度计安装坐标系，b表示真实的传感器坐标系。表示加速度计的***误差。表示加速度计的常值零偏误差。f_b表示加速度计的真实输出。表示加速度计的安装误差矩阵。表示加速度计的刻度系数误差矩阵。

表示加速度计的杆臂误差矩阵。

建立惯性系下姿态误差方程为：

其中，ib0表示载体坐标系，它是在标定过程开始时将载体坐标系的轴线固定在惯性空间中形成的。in0表示导航坐标系，它是在标定过程开始时将导航坐标系的轴线固定在惯性空间中形成的。为从载体坐标系至初始时刻载体坐标系的变换矩阵。表示初始时刻的姿态矩阵。φ＝[φ_E φ_N φ_U]表示载体的姿态误差角。

建立惯性系下速度误差方程为：

其中，ib0表示载体坐标系，它是在标定过程开始时将载体坐标系的轴线固定在惯性空间中形成的。in0表示导航坐标系，它是在标定过程开始时将导航坐标系的轴线固定在惯性空间中形成的。f^b表示载体坐标系下加速度计真实的输出。

步骤2：将惯性组件安装于转台上，初始朝向为东-北-天方向。使惯性组件以变角速度绕Y轴正向旋转360°，其次再绕Y轴反向旋转360°，具体为：

在东-北-天的朝向时，绕Y轴旋转，陀螺与加速度计的理论输出为：

其中，θ为旋转角度。L是当地纬度，ω_T为旋转角速度，ω_ie为地球自转角速度。由于ω_ie＜＜ω_T，因此，陀螺的输出可以简化为：

因此，由加速度计和陀螺引起的***误差可以表达为：

根据状态和量测之间的关系可以得到：

其中，

式中，γ表示横滚角。

因此可以得到：

式中，

根据以上分析，可以得到，是不可观测的。由于 α_yx和β_xy是相互耦合的状态量.如果将α_yx视作不可观测状态,那么β_xy是可观测的。

在光纤陀螺中,由于ε_z＜＜β_zyω_T并且ε_x＜＜β_xyω_T.对于状态β_zy和β_xy更加敏感。因此,ε_x和ε_z的可观测性很弱。

在该旋转方案下，有如下状态是可观测的：

步骤3：将惯性组件绕Z轴正向旋转90°，使惯性组件的朝向为北-西-天。使惯性组件以变角速度绕X轴正向旋转360°，其次再绕X轴反向旋转360°，具体为；

在北-西-天的朝向时，绕X轴旋转，陀螺与加速度计的理论输出为：

其中，θ为旋转角度。ω_T为旋转角速度，

由加速度计和陀螺引起的***误差可以表达为：

因此，可以得到导航系下等效的偏置误差为：

其中，β表示俯仰角。

因此，可以得到在该旋转方案下，有如下状态是可观测的：

步骤4：将惯性组件绕Y轴正向旋转90°，使惯性组件的朝向为地-西-北。使惯性组件以变角速度绕Z轴正向旋转360°，其次再绕Z轴反向旋转360°，具体为；

在地-西-北的朝向时，绕Z轴旋转，陀螺与加速度计的理论输出为：

由加速度计和陀螺引起的***误差可以表达为：

因此，可以得到导航系下等效的偏置误差为：

其中，表示航向角。

因此，可以得到在该旋转方案下，有如下状态是可观测的：

步骤5：将惯性组件绕Y轴反向旋转90°，再绕Z轴反向旋转90°，使惯性组件的朝向为东-北-天方向。使惯性组件以变角速度绕Y轴正向旋转360°，其次再绕Y轴反向旋转360°，具体为；

在东-北-天的朝向时，绕Y轴旋转，陀螺与加速度计的理论输出为：

其中，θ为旋转角度。ω_T为旋转角速度。

由加速度计和陀螺引起的***误差可以表达为：

因此，可以得到导航系下等效的偏置误差为：

式中，γ表示横滚角。

因此，可以得到在该旋转方案下，有如下状态是可观测的：

步骤6：建立惯性组件***误差标定的卡尔曼滤波方程，具体为：

建立***级标定卡尔曼滤波状态方程：

式中，φ_E φ_N φ_U]表示姿态角误差，表示in0坐标系下的速度误差，ε＝[ε_x ε_y ε_z]表示陀螺零偏。K_g＝[K_gx K_gy K_gz]表示陀螺刻度因子。β＝[β_xy β_xz β_yx β_yz δβ_zxβ_zy]表示陀螺的安装误差。表示加速度计的零偏误差。K_a＝[K_ax K_ay K_az]表示加速度计的刻度因子。

α＝[α_yx α_zx α_zy]表示加速度计的安装误差。

表示加速度计的杆臂误差。

式中。W表示过程噪声。T_ij(i，j＝1，2，3)为姿态矩阵对应元素。(·)×表示反对称矩阵。

相应的量测方程：

Z＝HX+V

其中，V表示量测噪声。H＝[0_3×3 I_3×3 0_3×30]。Z＝δvⁱⁿ⁰表示以导航速度误差为观测量。

步骤7：设计基于回溯理论的***级标定方法，具体为：

当一次滤波完成后，可以得到***各状态的估计值：

因此可以得到初始的姿态误差矩阵初始速度vin0以及IMU***误差的估计值。以此滤波估计的稳态值作为下一次回溯导航的误差状态初值，进行回溯导航过程。可以重复利用IMU数据，到达缩短标定时间，提高标定精度的目的。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims (8)

1.一种基于回溯理论的IMU全参数误差快速标定方法，包括如下步骤，其特征在于：

(1)建立惯性组件***误差模型与惯性系下捷联惯导***误差模型；

(2)将惯性组件安装于转台上，初始朝向为东-北-天方向，使惯性组件以变角速度绕Y轴正向旋转360°，其次再绕Y轴反向旋转360°；

(3)惯性组件绕Z轴正向旋转90°，使惯性组件的朝向为北-西-天，使惯性组件以变角速度绕X轴正向旋转360°，其次再绕X轴反向旋转360°；

(4)惯性组件绕Y轴正向旋转90°，使惯性组件的朝向为地-西-北，使惯性组件以变角速度绕Z轴正向旋转360°，其次再绕Z轴反向旋转360°；

(5)惯性组件绕Y轴反向旋转90°，再绕Z轴反向旋转90°，使惯性组件的朝向为东-北-天方向，使惯性组件以变角速度绕Y轴正向旋转360°，其次再绕Y轴反向旋转360°；

(6)建立惯性组件***误差标定的卡尔曼滤波方程；

(7)设计基于回溯理论的***级标定方法。

2.根据权利要求1所述的一种基于回溯理论的IMU全参数误差快速标定方法，其特征在于：所述步骤(1)中，建立惯性组件误差模型具体为：

将光纤陀螺的零偏误差、刻度系数误差和安装误差加入陀螺误差模型，得：

其中，g表示陀螺的安装坐标系，b表示真实的传感器坐标系，表示陀螺的***误差；表示陀螺的常值漂移误差，ω_b为陀螺真实的输出，表示陀螺安装误差矩阵，表示陀螺的刻度系数误差矩阵；

将加速度计的零偏误差，刻度系数误差，安装误差以及动态杆臂误差加入到误差模型中，得：

其中，a表示加速度计安装坐标系，b表示真实的传感器坐标系，表示加速度计的***误差，表示加速度计的常值零偏误差，f_b表示加速度计的真实输出；表示加速度计的安装误差矩阵，表示加速度计的刻度系数误差矩阵；

表示加速度计的杆臂误差矩阵；

建立惯性系下姿态误差方程为：

其中，ib0表示载体坐标系，是在标定过程开始时将载体坐标系的轴线固定在惯性空间中形成的，in0表示导航坐标系，是在标定过程开始时将导航坐标系的轴线固定在惯性空间中形成的，为从载体坐标系至初始时刻载体坐标系的变换矩阵，表示初始时刻的姿态矩阵，φ＝[φ_E φ_N φ_U]表示载体的姿态误差角；

建立惯性系下速度误差方程为：

其中，ib0表示载体坐标系，是在标定过程开始时将载体坐标系的轴线固定在惯性空间中形成的，in0表示导航坐标系，是在标定过程开始时将导航坐标系的轴线固定在惯性空间中形成的，f^b表示载体坐标系下加速度计真实的输出。

3.根据权利要求1所述的一种基于回溯理论的IMU全参数误差快速标定方法，其特征在于：所述步骤(2)中，将惯性组件安装于转台上，初始朝向为东-北-天方向，使惯性组件以变角速度绕Y轴正向旋转360°，其次再绕Y轴反向旋转360°，具体为：

在东-北-天的朝向时，绕Y轴旋转，陀螺与加速度计的理论输出为：

其中，θ为旋转角度，L是当地纬度，ω_T为旋转角速度，ω_is为地球自转角速度，由于ω_ie＜＜ω_T，因此，陀螺的输出可以简化为：

因此，由加速度计和陀螺引起的***误差可以表达为：

根据状态和量测之间的关系可以得到：

其中，

式中，γ表示横滚角；

因此得到：

式中，

根据以上分析，得到，是不可观测的，由于α_yx和β_xy是相互耦合的状态量.如果将α_yx视作不可观测状态,那么β_xy是可观测的；

在光纤陀螺中,由于ε_z＜＜β_zyω_T并且ε_x＜＜β_xyω_T，对于状态β_zy和β_xy更加敏感；因此,ε_x和ε_z的可观测性很弱；

在该旋转方案下，有如下状态是可观测的：

4.根据权利要求1所述的一种基于回溯理论的IMU全参数误差快速标定方法，其特征在于：所述步骤(3)中，惯性组件绕Z轴正向旋转90°，使惯性组件的朝向为北-西-天，使惯性组件以变角速度绕X轴正向旋转360°，其次再绕X轴反向旋转360°，具体为：

在北-西-天的朝向时，绕X轴旋转，陀螺与加速度计的理论输出为：

其中，θ为旋转角度，ω_T为旋转角速度，

由加速度计和陀螺引起的***误差可以表达为：

因此，得到导航系下等效的偏置误差为：

其中，β表示俯仰角；

因此，得到在该旋转方案下，有如下状态是可观测的：

5.根据权利要求1所述的一种基于回溯理论的IMU全参数误差快速标定方法，其特征在于：所述步骤(4)中，惯性组件绕Y轴正向旋转90°，使惯性组件的朝向为地-西-北，使惯性组件以变角速度绕Z轴正向旋转360°，其次再绕Z轴反向旋转360°，具体为：

在地-西-北的朝向时，绕Z轴旋转，陀螺与加速度计的理论输出为：

由加速度计和陀螺引起的***误差可以表达为：

因此，可以得到导航系下等效的偏置误差为：

其中，表示航向角；

因此，得到在该旋转方案下，有如下状态是可观测的：

6.根据权利要求1所述的一种基于回溯理论的IMU全参数误差快速标定方法，其特征在于：所述步骤(5)中，惯性组件绕Y轴反向旋转90°，再绕Z轴反向旋转90°，使惯性组件的朝向为东-北-天方向，使惯性组件以变角速度绕Y轴正向旋转360°，其次再绕Y轴反向旋转360°，具体为：

在东-北-天的朝向时，绕Y轴旋转，陀螺与加速度计的理论输出为：

其中，θ为旋转角度，ω_T为旋转角速度；

由加速度计和陀螺引起的***误差可以表达为：

因此，可以得到导航系下等效的偏置误差为：

式中，γ表示横滚角；

因此，得到在该旋转方案下，有如下状态是可观测的：

7.根据权利要求1所述的一种基于回溯理论的IMU全参数误差快速标定方法，其特征在于：所述步骤(6)中，建立惯性组件***误差标定的卡尔曼滤波方程，具体为：

建立***级标定卡尔曼滤波状态方程：

式中，

[φ_E φ_N φ_U]表示姿态角误差,表示in0坐标系下的速度误差,ε＝[ε_x ε_y ε_z]表示陀螺零偏，K_g＝[K_gx K_gy K_gz]表示陀螺刻度因子，β＝[β_xy β_xz β_yx β_yz δβ_zx β_zy]表示陀螺的安装误差，表示加速度计的零偏误差，K_a＝[K_ax K_ay K_az]表示加速度计的刻度因子，α＝[α_yx α_zx α_zy]表示加速度计的安装误差，表示加速度计的杆臂误差；

式中，W表示过程噪声，T_ij(i，j＝1，2，3)为姿态矩阵对应元素，(·)×表示反对称矩阵；

相应的量测方程：

Z＝HX+V

其中，V表示量测噪声，H＝[O_3×3 I_3×3 O_3×30]，Z＝δvⁱⁿ⁰表示以导航速度误差为观测量。

8.根据权利要求1所述的一种基于回溯理论的IMU全参数误差快速标定方法，其特征在于：所述步骤(7)中，设计基于回溯理论的***级标定方法，具体为：

当一次滤波完成后，得到***各状态的估计值：

因此得到初始的姿态误差矩阵初始速度vⁱⁿ⁰以及IMU***误差的估计值，以此滤波估计的稳态值作为下一次回溯导航的误差状态初值，进行回溯导航过程。