[发明专利]一种校正加速度计误差的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_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，二者做差即可获得三轴姿态误差角：

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

ΔE_ε′＝M·Δ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)所示运动学约束信息作为量测方程时，其对应量测噪声方差阵为与加速度计误差相关。