CN111707259A - 一种校正加速度计误差的sins/cns组合导航方法 - Google Patents

Abstract

本发明公开了一种校正加速度计误差的SINS/CNS组合导航方法，属于组合导航技术领域。该组合导航方法在姿态量测的基础上，结合星光折射间接敏感地平与载体运动学约束，对惯性器件误差进行校正。主要步骤包括：通过恒星星敏感器观测星图信息，输出载体姿态信息，与惯性器件输出的姿态信息构成姿态量测；利用星光折射计算折射视高度，与惯性器件输出的位置信息计算构成视高度量测；结合载体的运动学规律，给出运动学约束量测；利用信息融合，校正陀螺仪和加速度计的误差，并对SINS导航信息进行反馈校正。本发明利用姿态、位置以及运动学约束作为组合导航的量测信息，对SINS误差进行校正，可显著提高SINS/CNS组合导航系统的导航精度。

Description

一种校正加速度计误差的SINS/CNS组合导航方法

技术领域

本发明属于组合导航技术领域，涉及一种校正加速度计误差的SINS/CNS组合导航方法。

背景技术

高精度的SINS/CNS组合导航系统，在短时、远程的高空飞行器、弹道导弹等设备上的要求越来越高。

由于设备空间限制，通常采用基于恒星敏感器的天文导航系统，通过星光折射间接敏感地平的方式，辅助校正陀螺仪和加速度计误差，以达到提高系统定位精度的目的。以弹道导弹为例，当其运行至关机点后，载体不受发动机推力等，由于飞行至大气层外，也不受大气等一系列阻力影响，因而与地球构成二体运动。因而，在此条件下，可借助导航系统输出的姿态信息、星光折射的视高度信息以及运动学约束等，进行组合导航系统量测更新。

发明内容

针对以上问题，本发明提供一种校正加速度计误差的SINS/CNS组合导航方法，利用姿态、位置以及运动学约束作为组合导航的量测信息，对SINS误差进行校正，可显著提高SINS/CNS组合导航系统的导航精度，为达此目的，本发明提供一种校正加速度计误差的SINS/CNS组合导航方法，包括以下步骤：

步骤一：惯性导航信息与天文导航信息的获取与计算；

1)惯性导航系统由初始粗对准获取初始位置信息，并通过惯性解算获取实时的载体姿态、速度和位置信息；

2)天文导航系统通过星敏感器观测导航星和折射星，进行星图识别与匹配，完成导航星捕获，输出载体姿态信息；

3)天文导航系统通过对拍摄星图进行折射星捕获，获取折射前的折射星星光单位矢量，进一步求得折射视高度；

以发射点惯性坐标系i系作为导航解算的坐标系，原点位于载体中心O，x轴指向目标点方向，y轴垂直向上，z轴构成右手坐标系；

令所捕获的折射星在折射前与折射后的星光单位矢量分别为S_ei和S_ei'，根据几何原理计算星光折射角γ：

γ＝arccos(S_ei·S_ei') (1)

另外根据星光的折射几何关系有：

其中有：

R_e为地球半径，h_a为折射视高度，r_c为载体在地心惯性系的位置矢量，η_c＝[s_cx s_cys_cz]^T为折射前观测星光在地心惯性系中的方向矢量，

为视高度误差小量；

另外，结合大气折射模型，当载体位于高于地面20km-50km时，视高度由经验公式可计算为：

h_a＝57.08107-6.44133lnγ+K(λ)ρ(h)(57.08107-6.44133lnγ+R_e) (4)

式中，散射参数K(λ)仅与光波的波长有关，有K(λ)＝2.2517×10^-7当光波波长为0.7μm，ρ(h)为大气密度；

4)利用步骤3)中天文导航获得的η_c与星光折射角γ，结合惯性导航输出的位置信息，结合式(2)可计算获得对应的折射视高度；

步骤二：组合导航状态方程与基于姿态量测、视高度量测与载体运动学约束量测的量测方程的建立；

1)状态方程建立；

组合导航的状态方程通常由捷联惯导系统的误差方程构成，状态量包括三维平台失准角误差φ_x,φ_y,φ_z、三轴速度误差δv_x,δv_y,δv_z、三轴位置误差δx,δy,δz、陀螺仪常值漂移和加速度计常值偏置，即：

系统状态模型为：

其中，状态转移矩阵为F(t)，噪声驱动矩阵为G(t)，W(t)为系统噪声向量。

2)量测方程建立；

S1.姿态量测

在SINS/CNS组合导航系统中，星敏感器与惯性导航系统均可以输出姿态信息

ψ_cns,γ_cns与

ψ_ins,γ_ins，二者做差即可获得三轴姿态误差角：

导航坐标系转换到平台坐标系的姿态角误差转换矩阵为Μ，需要将三轴姿态误差角转换为平台失准角，方能参与卡尔曼滤波计算，即有：

ΔE_ε′＝Μ·ΔE_ε (8)

式中，

进而，得到组合导航关于姿态的量测方程为：

Z₁＝h₁(X(t),V₁) (10)

h₁为对应的观测方程，h₁(·)＝Μ^-1[I_3×3O_1×2]₃(·)，状态量V₁为对应的观测噪声，且V₁＝M^-1[V_x V_y V_z]^T；

S2.视高度量测；

结合式(2)与式(4)，建立基于位置信息的量测方程，由惯性导航所输出的位姿信息求得的视高度，与天文导航所观测的折射角所求得的视高度计算获得，即

其中，

由惯性导航计算得出的位姿信息获取，

为通过天文导航观测得到的折射角真值获取，ν_h为量测信息引起的视高度误差，i＝1,2,3...n代表观测到的第i颗折射星，结合式(2)与式(4)有

h_a,cns＝57.08107-6.44133lnR+K(λ)ρ(h)(57.08107-6.44133lnR+R_e)

基于折射视高度建立的位置误差的量测方程表示为：

量测误差

其中δR为星敏感器量测误差，ν_h为前文所述视高度误差，即量测方程为：

Z₂＝h₂(X(t),V₂) (13)

S3.运动学约束量测；

载体运行过程中会收到运动学约束的影响，当弹道导弹为载体时，飞行过程包括主动飞行段与自由飞行段。在主动飞行段，发动机为导弹提供推力，在关机点处导弹已飞出大气层，开始自由飞行阶段，此时弹体不受外力作用，与地球构成二体运动，即只受地球万有引力的影响，因此，当导弹处于自由段，运动学理论上认为，加速度计的输出值应当为0，从而，此阶段的加速度计非零输出可认为是加速度计零偏以及噪声误差造成，而此时的速度方程应当为：

加速度计输出可认为满足下式：

式中，

为发射点惯性系下载体的实际比力值，

为比力真值，

为误差项，当导弹处于自由飞行段，从运动学约束的角度分析，

即可建立运动学约束模型f_Kinematic＝0；

因此，在组合导航的自由飞行段，可依据运动学约束建立量测方程为：

其中，h₃(·)＝[O_12×3 I_3×3](·)，V₃与加速度计输出误差项相关；

采用运动学约束的方式进行组合导航，使得加速度计误差可观测，从而辅助加速度计误差估计，可达到减少速度与位置误差的发散的目的；

联立式(10)、式(13)和(15)，即可获得惯性/天文组合导航的量测模型，通过卡尔曼滤波对状态量进行估计，并对惯导系统进行反馈，可达到估计惯导系统的陀螺仪误差与加速度计误差的目的，从而实现姿态信息与位置信息的共同校正；

步骤三：基于无迹卡尔曼滤波的信息融合；

由于惯性导航输出频率较高，可达100Hz，而天文导航尽管星敏感器精度高，但输出频率仅为1Hz，因此采用无迹卡尔曼滤波进行信息融合，组合导航滤波频率为1Hz，而在其他时间采用滤波器一步更新预测值进行状态量与协方差矩阵更新。

作为本发明进一步改进，所述步骤三中，由于量测输出中可能隐含了关于系统模型的相关内容，因此当系统参数模型不准确时，可根据量测输出对部分参数进行更新，由于系统噪声作为固定特性通常不易改变，而量测噪声由外部因素造成，变化较大，因此其中最主要的是针对量测噪声方差阵R的更新；

采用三维平台失准角作为量测量时，量测噪声可选择星敏感器测量偏差：

R₁＝E[V₁V₁ ^T]；

采用组合导航系统采用视高度误差作为量测模型时，

其中，i为折射星数量

式中，dh_a由下式计算并实时更新：

当采用式(15)所示运动学约束信息作为量测方程时，其对应量测噪声方差阵为

与加速度计误差相关。

本发明公开了一种校正加速度计误差的SINS/CNS组合导航方法，本发明与现有技术相比，充分结合位置信息与运动学相关信息，利用姿态信息、视高度信息和加速度计输出误差，从而实现对组合导航系统中加速度计误差的修正，进一步的优化系统位置误差。

附图说明

图1一种校正加速度计误差的SINS/CNS组合导航方法结构示意图。

具体实施方式

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

本发明提供一种校正加速度计误差的SINS/CNS组合导航方法，利用姿态、位置以及运动学约束作为组合导航的量测信息，对SINS误差进行校正，可显著提高SINS/CNS组合导航系统的导航精度。

结合附图1所示，本发明提供一种校正加速度计误差的SINS/CNS组合导航方法，包括以下步骤：

步骤一：惯性导航信息与天文导航信息的获取与计算

1)惯性导航系统由初始粗对准获取初始位置信息，并通过惯性解算获取实时的载体姿态、速度和位置信息。

2)天文导航系统通过星敏感器观测导航星和折射星，进行星图识别与匹配，完成导航星捕获，输出载体姿态信息。

3)天文导航系统通过对拍摄星图进行折射星捕获，获取折射前的折射星星光单位矢量，进一步求得折射视高度。

以发射点惯性坐标系i系作为导航解算的坐标系，原点位于载体中心O，x轴指向目标点方向，y轴垂直向上，z轴构成右手坐标系。

令所捕获的折射星在折射前与折射后的星光单位矢量分别为S_ei和S_ei'，根据几何原理计算星光折射角γ：

γ＝arccos(S_ei·S_ei')(1)

另外根据星光的折射几何关系有：

其中有：

R_e为地球半径，h_a为折射视高度，r_c为载体在地心惯性系的位置矢量，η_c＝[s_cx s_cys_cz]^T为折射前观测星光在地心惯性系中的方向矢量，

为视高度误差小量。

另外，结合大气折射模型，当载体位于高于地面20km-50km时，视高度由经验公式可计算为：

h_a＝57.08107-6.44133lnγ+K(λ)ρ(h)(57.08107-6.44133lnγ+R_e)(4)

式中，散射参数K(λ)仅与光波的波长有关，有K(λ)＝2.2517×10^-7当光波波长为0.7μm，ρ(h)为大气密度。

4)利用步骤3)中天文导航获得的η_c与星光折射角γ，结合惯性导航输出的位置信息，结合式(2)可计算获得对应的折射视高度；

步骤二：组合导航状态方程与基于姿态量测、视高度量测与载体运动学约束量测的量测方程的建立

1)状态方程建立；

组合导航的状态方程通常由捷联惯导系统的误差方程构成，状态量包括三维平台失准角误差φ_x,φ_y,φ_z、三轴速度误差δv_x,δv_y,δv_z、三轴位置误差δx,δy,δz、陀螺仪常值漂移和加速度计常值偏置，即：

2)系统状态模型为：

其中，状态转移矩阵为F(t)，噪声驱动矩阵为G(t)，W(t)为系统噪声向量。

量测方程建立

S1.姿态量测

在SINS/CNS组合导航系统中，星敏感器与惯性导航系统均可以输出姿态信息

ψ_cns,γ_cns与

ψ_ins,γ_ins，二者做差即可获得三轴姿态误差角：

导航坐标系转换到平台坐标系的姿态角误差转换矩阵为Μ，需要将三轴姿态误差角转换为平台失准角，方能参与卡尔曼滤波计算，即有：

ΔE_ε′＝Μ·ΔE_ε (8)

式中，

进而，得到组合导航关于姿态的量测方程为：

Z₁＝h₁(X(t),V₁) (10)

h₁为对应的观测方程，h₁(·)＝Μ^-1[I_3×3O_12×3](·)，状态量V₁为对应的观测噪声，且V₁＝M^-1[V_x V_y V_z]^T。

S2.视高度量测

结合式(2)与式(4)，建立基于位置信息的量测方程，由惯性导航所输出的位姿信息求得的视高度，与天文导航所观测的折射角所求得的视高度计算获得，即

其中，

由惯性导航计算得出的位姿信息获取，

为通过天文导航观测得到的折射角真值获取，ν_h为量测信息引起的视高度误差，i＝1,2,3...n代表观测到的第i颗折射星，结合式(2)与式(4)有；

h_a,cns＝57.08107-6.44133lnR+K(λ)ρ(h)(57.08107-6.44133lnR+R_e)

基于折射视高度建立的位置误差的量测方程表示为：

量测误差

其中δR为星敏感器量测误差，ν_h为前文所述视高度误差。即量测方程为：

Z₂＝h₂(X(t),V₂) (13)

S3.运动学约束量测

载体运行过程中会收到运动学约束的影响。当弹道导弹为载体时，飞行过程包括主动飞行段与自由飞行段。在主动飞行段，发动机为导弹提供推力，在关机点处导弹已飞出大气层，开始自由飞行阶段，此时弹体不受外力作用，与地球构成二体运动，即只受地球万有引力的影响。因此，当导弹处于自由段，运动学理论上认为，加速度计的输出值应当为0。从而，此阶段的加速度计非零输出可认为是加速度计零偏以及噪声误差造成。

而此时的速度方程应当为：

加速度计输出可认为满足下式：

式中，

为发射点惯性系下载体的实际比力值，

为比力真值，

为误差项。当导弹处于自由飞行段，从运动学约束的角度分析，

即可建立运动学约束模型f_Kinematic＝0。

因此，在组合导航的自由飞行段，可依据运动学约束建立量测方程为：

其中，h₃(·)＝[O_12×3I_3×3](·)，V₃与加速度计输出误差项相关。

采用运动学约束的方式进行组合导航，可使得加速度计误差可观测，从而辅助加速度计误差估计，可达到减少速度与位置误差的发散的目的。

联立式(10)、式(13)和(15)，即可获得惯性/天文组合导航的量测模型，通过卡尔曼滤波对状态量进行估计，并对惯导系统进行反馈，可达到估计惯导系统的陀螺仪误差与加速度计误差的目的，从而实现姿态信息与位置信息的共同校正

步骤三：基于无迹卡尔曼滤波的信息融合

由于惯性导航输出频率较高，可达100Hz，而天文导航尽管星敏感器精度高，但输出频率仅为1Hz，因此采用无迹卡尔曼滤波进行信息融合，组合导航滤波频率为1Hz，而在其他时间采用滤波器一步更新预测值进行状态量与协方差矩阵更新。

由于量测输出中可能隐含了关于系统模型的相关内容，因此当系统参数模型不准确时，可根据量测输出对部分参数进行更新，由于系统噪声作为固定特性通常不易改变，而量测噪声由外部因素造成，变化较大，因此其中最主要的是针对量测噪声方差阵R的更新。

采用三维平台失准角作为量测量时，量测噪声可选择星敏感器测量偏差：

R₁＝E[V₁V₁ ^T]。

采用组合导航系统采用视高度误差作为量测模型时，

其中，i为折射星数量

式中，dh_a由下式计算并实时更新：

当采用式(15)所示运动学约束信息作为量测方程时，其对应量测噪声方差阵为

与加速度计误差相关。

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

Claims (2)

1.一种校正加速度计误差的SINS/CNS组合导航方法，其特征在于，包括以下步骤：

步骤一：惯性导航信息与天文导航信息的获取与计算；

1)惯性导航系统由初始粗对准获取初始位置信息，并通过惯性解算获取实时的载体姿态、速度和位置信息；

2)天文导航系统通过星敏感器观测导航星和折射星，进行星图识别与匹配，完成导航星捕获，输出载体姿态信息；

3)天文导航系统通过对拍摄星图进行折射星捕获，获取折射前的折射星星光单位矢量，进一步求得折射视高度；

以发射点惯性坐标系i系作为导航解算的坐标系，原点位于载体中心O，x轴指向目标点方向，y轴垂直向上，z轴构成右手坐标系；

令所捕获的折射星在折射前与折射后的星光单位矢量分别为S_ei和S_ei'，根据几何原理计算星光折射角γ：

γ＝arccos(S_ei·S_ei') (1)

另外根据星光的折射几何关系有：

其中有：

R_e为地球半径，h_a为折射视高度，r_c为载体在地心惯性系的位置矢量，η_c＝[s_cx s_cy s_cz]^T为折射前观测星光在地心惯性系中的方向矢量，

为视高度误差小量；

另外，结合大气折射模型，当载体位于高于地面20km-50km时，视高度由经验公式计算为：

h_a＝57.08107-6.44133lnγ+K(λ)ρ(h)(57.08107-6.44133lnγ+R_e) (4)

式中，散射参数K(λ)仅与光波的波长有关，有K(λ)＝2.2517×10^-7当光波波长为0.7μm，ρ(h)为大气密度；

4)利用步骤3)中天文导航获得的η_c与星光折射角γ，结合惯性导航输出的位置信息，结合式(2)计算获得对应的折射视高度；

步骤二：组合导航状态方程与基于姿态量测、视高度量测与载体运动学约束量测的量测方程的建立；

1)状态方程建立；

组合导航的状态方程通常由捷联惯导系统的误差方程构成，状态量包括三维平台失准角误差φ_x,φ_y,φ_z、三轴速度误差δv_x,δv_y,δv_z、三轴位置误差δx,δy,δz、陀螺仪常值漂移和加速度计常值偏置，即：

系统状态模型为：

其中，状态转移矩阵为F(t)，噪声驱动矩阵为G(t)，W(t)为系统噪声向量；

2)量测方程建立；

S1.姿态量测

在SINS/CNS组合导航系统中，星敏感器与惯性导航系统均可以输出姿态信息

ψ_cns,γ_cns与

ψ_ins,γ_ins，二者做差即可获得三轴姿态误差角：

导航坐标系转换到平台坐标系的姿态角误差转换矩阵为Μ，需要将三轴姿态误差角转换为平台失准角，方能参与卡尔曼滤波计算，即有：

ΔE_ε′＝Μ·ΔE_ε (8)

式中，

进而，得到组合导航关于姿态的量测方程为：

Z₁＝h₁(X(t),V₁) (10)

h₁为对应的观测方程，h₁(·)＝Μ^-1[I_3×3O_12×3](·)，状态量V₁为对应的观测噪声，且V₁＝M^-1[V_x V_y V_z]^T；

S2.视高度量测；

结合式(2)与式(4)，建立基于位置信息的量测方程，由惯性导航所输出的位姿信息求得的视高度，与天文导航所观测的折射角所求得的视高度计算获得，即

其中，

由惯性导航计算得出的位姿信息获取，

为通过天文导航观测得到的折射角真值获取，ν_h为量测信息引起的视高度误差，i＝1,2,3...n代表观测到的第i颗折射星，结合式(2)与式(4)有

h_a,cns＝57.08107-6.44133lnR+K(λ)ρ(h)(57.08107-6.44133lnR+R_e)

基于折射视高度建立的位置误差的量测方程表示为：

量测误差

其中δR为星敏感器量测误差，ν_h为前文所述视高度误差，即量测方程为：

Z₂＝h₂(X(t),V₂) (13)

S3.运动学约束量测；

载体运行过程中会收到运动学约束的影响，当弹道导弹为载体时，飞行过程包括主动飞行段与自由飞行段，在主动飞行段，发动机为导弹提供推力，在关机点处导弹已飞出大气层，开始自由飞行阶段，此时弹体不受外力作用，与地球构成二体运动，即只受地球万有引力的影响，因此，当导弹处于自由段，运动学理论上认为，加速度计的输出值应当为0，从而，此阶段的加速度计非零输出认为是加速度计零偏以及噪声误差造成，而此时的速度方程应当为：

加速度计输出认为满足下式：

式中，

为发射点惯性系下载体的实际比力值，

为比力真值，

为误差项，当导弹处于自由飞行段，从运动学约束的角度分析，

即可建立运动学约束模型f_Kinematic＝0；因此，在组合导航的自由飞行段，依据运动学约束建立量测方程为：

其中，h₃(·)＝[O_12×3 I_3×3](·)，V₃与加速度计输出误差项相关；

采用运动学约束的方式进行组合导航，使得加速度计误差可观测，从而辅助加速度计误差估计，达到减少速度与位置误差的发散的目的；

联立式(10)、式(13)和(15)，即可获得惯性/天文组合导航的量测模型，通过卡尔曼滤波对状态量进行估计，并对惯导系统进行反馈，达到估计惯导系统的陀螺仪误差与加速度计误差的目的，从而实现姿态信息与位置信息的共同校正；

步骤三：基于无迹卡尔曼滤波的信息融合；

由于惯性导航输出频率较高，可达100Hz，而天文导航尽管星敏感器精度高，但输出频率仅为1Hz，因此采用无迹卡尔曼滤波进行信息融合，组合导航滤波频率为1Hz，而在其他时间采用滤波器一步更新预测值进行状态量与协方差矩阵更新。

2.根据权利1所述的一种校正加速度计误差的SINS/CNS组合导航方法，其特征在于，所述步骤三中，由于量测输出中可能隐含了关于系统模型的相关内容，因此当系统参数模型不准确时，可根据量测输出对部分参数进行更新，由于系统噪声作为固定特性通常不易改变，而量测噪声由外部因素造成，变化较大，因此其中最主要的是针对量测噪声方差阵R的更新；采用三维平台失准角作为量测量时，量测噪声选择星敏感器测量偏差：

R₁＝E[V₁V₁ ^T]；

采用组合导航系统采用视高度误差作为量测模型时，

其中，i为折射星数量

式中，dh_a由下式计算并实时更新：

当采用式(15)所示运动学约束信息作为量测方程时，其对应量测噪声方差阵为

与加速度计误差相关。

Images