CN104019828A - 高动态环境下惯性导航系统杆臂效应误差在线标定方法 - Google Patents

Abstract

本发明公开了一种高动态环境下惯性导航系统杆臂效应误差在线标定方法，属于惯性导航惯性传感器误差标定技术领域。该方法包括以下步骤：首先建立加速度计在高超声速飞行器动态飞行过程中的杆臂效应误差模型；在传统的加速度计随机误差模型的基础上，将杆臂长度扩展为卡尔曼滤波器系统状态变量，建立包含杆臂效应误差在内的滤波状态方程和位置、速度线性量测方程；最后在高超声速飞行器动态飞行过程中对杆臂效应误差进行实时动态标定与校正。本方法能够在飞行器高动态飞行过程中实现对惯性导航系统中杆臂效应误差的在线标定及校正，有效提高高超声速飞行器惯性导航系统性能，适合于工程应用。

Description

高动态环境下惯性导航系统杆臂效应误差在线标定方法

技术领域

本发明公开了一种高动态环境下惯性导航系统杆臂效应误差在线标定方法，属于惯性导航惯性传感器误差标定技术领域。

背景技术

近年来，随着高超声速飞行器的研制，对导航系统性能的要求日益提高。惯性导航系统具有短时精度高、输出连续以及完全自主等突出优点，必是高超声速飞行器的核心导航信息单元。

惯性传感器（IMU-陀螺仪和加速度计）的测量误差是影响惯性导航系统精度的主要因素。一般IMU的质心应该与飞行器质心重合，而由于飞行器飞行过程中的高频振动、以及飞行器分离时的冲击等环境影响将引起IMU的质心与飞行器质心不重合，如果飞行器存在角运动则会造成加速度计输出产生杆臂效应误差。在飞行器动态飞行过程中，如果杆臂效应误差不能得到补偿将极有可能影响导航系统精度，从而难以满足高超声速飞行器对导航系统的高精度需求。

传统的对于杆臂效应误差的标定和补偿研究多在传递对准系统误差补偿中，利用主惯导的测量输出实现对子惯导的补偿校正。常规的关于杆臂效应误差的补偿方法通常有力学补偿法和数字滤波法，力学补偿法的前提是杆臂长度已知，计算出杆臂效应误差从而实现对其补偿。数字滤波法的前提是杆臂效应误差和加速度计输出信号之间的频率特性已知，从而利用滤波的方法实现对杆臂效应误差的补偿。而在高超声速飞行器高动态飞行过程中，复杂飞行环境的影响有可能导致常规的对杆臂效应误差的补偿方法难以适用。

发明内容

本发明提出了一种高动态环境下惯性导航系统杆臂效应误差在线标定方法，以满足高超声速飞行器动态飞行过程中对导航系统的高精度要求。

本发明为解决上其技术问题采用如下技术方案，

一种高动态环境下惯性导航系统杆臂效应误差在线标定方法，包括如下步骤：

步骤一、建立动态标定杆臂效应误差模型：

杆臂效应引起的加速度计误差模型表示为：

                                                                                   (1)

式(1)中，为杆臂长度，为陀螺仪输出的角速率，为陀螺仪输出角速率的一阶微分；

取东北天地理坐标系为导航坐标系，机体坐标系取“右前上”坐标系，则杆臂效应引起的速度误差和位置误差在导航坐标系的投影为：

                                          (2)

其中，为机体坐标系到导航坐标系的转换矩阵，且。为的反对称矩阵；

设在高超声速飞行器动态飞行过程中，杆臂长度的变化为随机常数，即

                                                   (3)

考虑杆臂效应误差影响，捷联惯性导航系统的速度误差方程为：

          (4)

式(4)中，且,,分别为东北天三个方向的速度，,分别为对应的速度误差及其一阶微分；且,,分别为东北天三个方向的数学平台误差角；为加速度计输出的比力，为杆臂效应误差项；为地球自转角速率在导航坐标系的投影，为对应的误差；为导航坐标系相对地球坐标系的角速率在导航坐标系的投影，为对应的误差；且,,分别为东北天三个方向加速度计的随机误差；

步骤二、在步骤一对杆臂效应误差建模的基础上，将杆臂长度扩展为系统状态变量，构建基于杆臂效应误差模型的滤波器状态方程和位置、速度线性化量测方程：

基于杆臂效应误差建模的系统状态变量X为：

              (5)

式(5)中，为三个数学平台误差角，为三个速度误差，分别为纬度误差、经度误差及高度误差，为加速度计随机误差，分别为杆臂长度在机体坐标系三个轴上的投影；

建立惯性导航系统的误差状态方程，如式(6)所示：

                                        (6)

式(6)中，为系统状态变量；为状态变量的一阶导数；为系统矩阵；为噪声系数矩阵；为噪声矩阵；

采用地理系下位置与速度线性化观测原理，建立地理系下卫星导航位置观测量、速度观测量和惯性导航系统误差状态量之间的线性化量测方程：

                                (7)

式(7)中，和分别为位置和速度量测矢量；和分别为位置和速度量测系数矩阵；位置和速度观测量噪声矩阵分别为和；

步骤三、在步骤二的基础上，对系统状态方程和量测方程进行离散化处理以及状态量、量测量的更新，实现对杆臂效应误差的标定与补偿。

步骤三中所述对杆臂效应误差的标定与补偿，其具体步骤为：

（301）将滤波器状态方程和量测方程离散化处理：

                            (8)

                                          (9)

其中，为时刻惯性导航系统状态量；为时刻系统状态量；为时刻至时刻系统的状态转移矩阵；为时刻至时刻系统的噪声驱动矩阵；为时刻系统的噪声矩阵；为时刻系统的位置和速度观测量矩阵；为时刻的位置和速度量测系数矩阵；为时刻的位置和速度观测量的噪声矩阵；

（302）按照式(10)、式(11)、式(12)、式(13)对步骤（301）部分的惯性导航系统误差状态量和协方差信息的量测更新：

         (10)

                      (11)

                         (12)

                                   (13)

其中，为时刻至时刻一步预测协方差矩阵；为时刻滤波状态估计协方差矩阵；为时刻系统的噪声协方差矩阵；为时刻滤波增益矩阵；为时刻位置和速度量测噪声协方差阵；为时刻至时刻一步预测状态量；为时刻滤波状态估计协方差矩阵；为与维数相同的单位矩阵；

（303）对滤波输出的状态量进行判断，若状态量的预测值稳定则完成标定，得到对杆臂效应误差的标定结果，进入步骤（304）；若预测值不稳定，则返回步骤（302）继续对状态值进行预测估计；

（304）在步骤（303）得到对杆臂效应误差的标定结果后，暂存标定值，利用标定值对杆臂效应误差进行补偿校正，校正在一个导航解算周期内完成，误差补偿校正算法为：

                                            (14)

式(14)中，为误差的加速度计输出的加速度；为去除掉误差后的加速度；为加速度计的随机漂移误差，为杆臂效应误差。

本发明的有益效果如下：

采用本方法可以显著减小惯性导航系统误差，能够有效提高高超声速飞行器惯性导航系统精度，适合工程应用。本方法的优点是能够在杆臂长度、杆臂效应误差和加速度计输出信号之间的频率特性未知情况下，在高超声速飞行器飞行过程中对惯性导航系统杆臂效应误差进行在线标定与实时补偿，显著提高惯性导航系统的精度。本方法对高动态环境下杆臂效应引起的加速度计误差进行了精确建模，并将杆臂长度扩展为卡尔曼滤波器系统状态变量，通过卫星导航接收机提供的外测速度及位置量测信息，应用卡尔曼滤波方法对杆臂效应误差进行实时估计与补偿，因此高超声速飞行器惯性导航系统的精度显著提高。

附图说明

图1为本发明的高动态环境下惯性导航系统杆臂效应误差在线标定方法结构图。

图2为杆臂效应误差原理图。

图3为高超声速飞行器轨迹。

图4为第一组X轴、Y轴及Z轴杆臂长度标定结果。

图5为第二组X轴、Y轴及Z轴杆臂长度标定结果。

图6为对杆臂效应误差和惯导误差进行单点校正后的姿态误差对比曲线。

图7为对杆臂效应误差和惯导误差进行单点校正后的速度误差对比曲线。

图8为对杆臂效应误差和惯导误差进行单点校正后的位置误差对比曲线。

具体实施方式

下面结合附图对本发明创造做进一步详细说明。

如图1所示，本发明所述的高动态环境下惯性导航系统杆臂效应误差在线标定方法的原理是：加速度计的测量输出为，其中包含了随机性误差和杆臂效应误差。是经过误差补偿修正后的加速度信息用于惯性导航系统的导航解算模块。惯性导航系统误差建模模块在传统误差模型的基础上建立杆臂效应误差模型，构建惯性导航系统误差的滤波状态量及状态方程；同时，根据地理系下位置与速度线性化观测原理，建立地理系下位置和速度的线性化量测方程，实现对杆臂效应误差的在线标定；利用标定的结果在加速度计误差修正模块对杆臂效应误差进行补偿修正，能够有效提高高超声速飞行器惯性导航系统精度。

本发明的具体实施方式如下：

1.  建立动态标定杆臂效应误差模型

在捷联惯性导航系统中，由于高超声速飞行器高频振动会导致IMU的质心与飞行器质心不重合，如果飞行器存在角运动，则会造成加速度计输出产生杆臂效应误差。

杆臂效应误差原理如图2所示：和分别为惯性坐标系和机体坐标系的中心，为IMU的质心，为杆臂长度，则杆臂效应引起的加速度计误差模型可以表示为：

                                                                                (1)

式(1)中，为陀螺仪输出的角速率，为陀螺仪输出角速率的一阶微分。

取东北天地理坐标系为导航坐标系，机体坐标系取“右前上”坐标系，则杆臂效应引起的速度误差和位置误差在导航坐标系的投影为：

                                                                                                      (2)

其中，为机体坐标系到导航坐标系的转换矩阵，且。为的反对称矩阵。

假设在高超声速飞行器动态飞行过程中，杆臂长度的变化为随机常数，即

                                                   (3)

考虑杆臂效应误差影响，捷联惯导系统的速度误差方程为：

       (4)

式(4)中，且,,分别为东北天三个方向的速度，,分别为对应的速度误差及其一阶微分；且,,分别为东北天三个方向的数学平台误差角；为加速度计输出的比力，为杆臂效应误差项；为地球自转角速率在导航坐标系的投影，为对应的误差；为导航坐标系相对地球坐标系的角速率在导航坐标系的投影，为对应的误差；且,,分别为东北天三个方向加速度计的随机误差。

2.  建立基于杆臂效应误差模型的卡尔曼滤波器模型

（2.1）建立惯性导航系统误差的状态量方程

选取机体坐标系为机体的“右前上”方向，选取导航坐标系为东北天地理坐标系。将杆臂长度扩展为卡尔曼滤波器系统状态变量，则滤波状态变量为：基于杆臂效应误差建模的系统状态变量X为：

            (5)

式(5)中，为三个数学平台误差角，为三个速度误差，分别为纬度误差、经度误差及高度误差，为加速度计随机误差，分别为杆臂长度在机体坐标系三个轴上的投影。

建立惯性导航系统的误差状态方程，如式(6)所示：

                                        (6)

式(6)中，为系统状态变量；为状态变量的一阶导数；为系统矩阵；为噪声系数矩阵；为噪声矩阵，其中均为零均值白噪声。

根据步骤1中所建立的杆臂效应误差模型，与式(5)及式(6)对应的系统矩阵为：

                                       (7)

式(7)中，为典型的基本导航参数阵，其非零项元素为：

其中，为地球子午圈曲率半径，为地球卯酉圈曲率半径；为东向、北向及天向的加速度。

式(7)中，为IMU误差参数之间的关系矩阵；为IMU误差与导航参数误差间的关系矩阵，其具体形式为：

                                       (8)

式(8)中，为杆臂效应误差项对应的矩阵。其推导过程如下：

                                             (9)

                                      (10)

                                      (11)

             (12)

其中，,,。 和分别为高超声速飞行器在北向和东向的速度。为地球半径，为飞行器所处的纬度。分别为飞行器的横滚角、俯仰角及航向角。

       (13)

                     (14)

从公式（13）及公式（14）可以得到：

                   (15)

式(15)中， 且其元素为：

（2.2）量测方程的确定

杆臂效应误差体现在速度误差方程中，故利用速度量测的方式对杆臂效应误差进行标定和补偿。通常IMU的质心与卫星天线的质心不重合，但是卫星天线和IMU质心之间的距离可以通过测量补偿，故假设卫星天线质心始终与IMU质心重合。在高超声速飞行器动态飞行过程中，IMU质心有可能与飞行器质心偏离，但IMU质心及卫星天线质心始终重合。故系统的量测方程为：

    (16)

式(16)中，下标表示惯性导航系统，表示卫星导航系统。表示东北天方向。和分别表示地球参考椭球子午圈和卯酉圈上各点的曲率半径。-为量测噪声，均为零均值随机白噪声。

3.  杆臂效应误差的实时标定补偿

（3.1）将滤波器状态方程和量测方程离散化处理：

                           (17)

                                         (18)

其中，为时刻系统状态量；为时刻系统状态量；为时刻至时刻系统的状态转移矩阵；为时刻至时刻系统的噪声驱动矩阵；为时刻系统的噪声矩阵；为时刻系统的位置和速度观测量矩阵；为时刻的位置和速度量测系数矩阵；为时刻的位置和速度观测量的噪声矩阵。

（3.2）按照式(19)、式(20)、式(21)、式(22)对子步骤（3.1）部分的惯性导航系统误差状态量和协方差信息的量测更新：

         (19)

                      (20)

                         (21)

                                   (22)

其中，为时刻至时刻一步预测协方差矩阵；为时刻滤波状态估计协方差矩阵；为时刻系统的噪声协方差矩阵；为时刻滤波增益矩阵；为时刻位置和速度量测噪声协方差阵；为时刻至时刻一步预测状态量；为时刻滤波状态估计协方差矩阵；为与维数相同的单位矩阵；

（3.3）对滤波输出的状态量进行判断，若状态量的预测值稳定则完成标定，得到对杆臂效应误差的标定结果，进入步骤（3.4）；若预测值不稳定，则返回步骤（3.2）继续对状态值进行预测估计。

（3.4）在步骤（3.3）得到对杆臂效应误差的标定结果后，暂存标定值，利用标定值对杆臂效应误差进行补偿校正，校正在一个导航解算周期内完成，误差补偿校正算法为：

                                            (23)

式(23)中，为误差的加速度计输出的加速度；为去除掉误差后的加速度；为加速度计的随机漂移误差，为杆臂效应误差。

（3.5）在步骤（3.4）对杆臂效应误差补偿校正时刻，同时利用卡尔曼滤波器对累积的惯性导航系统导航参数误差进行单点的闭环校正。

为了验证发明所提出的高动态环境下惯性导航系统杆臂效应误差建模及标定方法的正确性及有效性，采用本发明方法建立模型，进行matlab仿真验证。图3为验证时采用的高超声速飞行器航迹，对杆臂效应误差的两组标定结果如图4、图5所示。

在高超声速飞行器动态飞行过程中利用标定结果对杆臂效应误差进行补偿修正，同时对累积的惯性导航系统导航参数误差进行单点闭环校正后的纯惯性导航结果与未经过补偿的纯惯性导航结果进行对比验证，验证结果如图6-8所示。

图4、图5中实线代表本发明的标定结果，点划线为真实值。从图中可以看出对杆臂效应误差标定所用时间在50s左右，标定结果与真实值一致。图6-8中实线代表未对杆臂效应误差和惯导误差进行单点闭环校正的导航结果，点划线代表对杆臂效应误差和惯导误差进行单点闭环校正后的导航结果。从图6-8中可以看出，利用对杆臂效应误差的标定结果对杆臂效应误差进行校正，同时对累积的惯性导航系统导航参数误差进行单点闭环校正后，高超声速飞行器惯性导航系统误差明显减小，具有有益的工程应用价值。 

Claims (2)

1.一种高动态环境下惯性导航系统杆臂效应误差在线标定方法，其特征在于，包括如下步骤：

步骤一、建立动态标定杆臂效应误差模型：

杆臂效应引起的加速度计误差模型表示为：

                                                      (1)

式(1)中，为杆臂长度，为陀螺仪输出的角速率，为陀螺仪输出角速率的一阶微分；

取东北天地理坐标系为导航坐标系，机体坐标系取“右前上”坐标系，则杆臂效应引起的速度误差和位置误差在导航坐标系的投影为：

    (2)

其中，为机体坐标系到导航坐标系的转换矩阵，且，为的反对称矩阵；

设在高超声速飞行器动态飞行过程中，杆臂长度的变化为随机常数，即

      (3)

考虑杆臂效应误差影响，捷联惯性导航系统的速度误差方程为：

    (4)

式(4)中，且,,分别为东北天三个方向的速度，,分别为对应的速度误差及其一阶微分；且,,分别为东北天三个方向的数学平台误差角；为加速度计输出的比力，为杆臂效应误差项；为地球自转角速率在导航坐标系的投影，为对应的误差；为导航坐标系相对地球坐标系的角速率在导航坐标系的投影，为对应的误差；且,,分别为东北天三个方向加速度计的随机误差；

步骤二、在步骤一对杆臂效应误差建模的基础上，将杆臂长度扩展为系统状态变量，构建基于杆臂效应误差模型的滤波器状态方程和位置、速度线性化量测方程：

基于杆臂效应误差建模的系统状态变量X为：

      (5)

式(5)中，为三个数学平台误差角，为三个速度误差，分别为纬度误差、经度误差及高度误差，为加速度计随机误差，分别为杆臂长度在机体坐标系三个轴上的投影；

建立惯性导航系统的误差状态方程，如式(6)所示：

    (6)

式(6)中，为系统状态变量；为状态变量的一阶导数；为系统矩阵；为噪声系数矩阵；为噪声矩阵；

采用地理系下位置与速度线性化观测原理，建立地理系下卫星导航位置观测量、速度观测量和惯性导航系统误差状态量之间的线性化量测方程：

    (7)

式(7)中，和分别为位置和速度量测矢量；和分别为位置和速度量测系数矩阵；位置和速度观测量噪声矩阵分别为和；

步骤三、在步骤二的基础上，对系统状态方程和量测方程进行离散化处理以及状态量、量测量的更新，实现对杆臂效应误差的标定与补偿。

2.如权利要求1所述的高动态环境下惯性导航系统杆臂效应误差在线标定方法，其特征在于，步骤三中所述对杆臂效应误差的标定与补偿，其具体步骤为：

（301）将滤波器状态方程和量测方程离散化处理：

       (8)

       (9)

其中，为时刻惯性导航系统状态量；为时刻系统状态量；为时刻至时刻系统的状态转移矩阵；为时刻至时刻系统的噪声驱动矩阵；为时刻系统的噪声矩阵；为时刻系统的位置和速度观测量矩阵；为时刻的位置和速度量测系数矩阵；为时刻的位置和速度观测量的噪声矩阵；

（302）按照式(10)、式(11)、式(12)、式(13)对步骤（301）部分的惯性导航系统误差状态量和协方差信息的量测更新：

    (10)

       (11)

    (12)

      (13)

其中，为时刻至时刻一步预测协方差矩阵；为时刻滤波状态估计协方差矩阵；为时刻系统的噪声协方差矩阵；为时刻滤波增益矩阵；为时刻位置和速度量测噪声协方差阵；为时刻至时刻一步预测状态量；为时刻滤波状态估计协方差矩阵；为与维数相同的单位矩阵；

（303）对滤波输出的状态量进行判断，若状态量的预测值稳定则完成标定，得到对杆臂效应误差的标定结果，进入步骤（304）；若预测值不稳定，则返回步骤（302）继续对状态值进行预测估计；

（304）在步骤（303）得到对杆臂效应误差的标定结果后，暂存标定值，利用标定值对杆臂效应误差进行补偿校正，校正在一个导航解算周期内完成，误差补偿校正算法为：

        (14)

式(14)中，为误差的加速度计输出的加速度；为去除掉误差后的加速度；为加速度计的随机漂移误差，为杆臂效应误差。