[发明专利]采用发射惯性系下的多组合导航处理装置的导航方法

权利要求书

1.一种发射惯性系下的多组合导航处理装置，其特征在于包括传感器模块、导航处理装置和显示模块，其中传感器模块包括惯性传感器、卫星导航接收机和星敏感器，导航处理装置包括接口模块、导航计算机、电源模块；惯性传感器、卫星导航接收机和星敏感器分别与接口模块连接，接口模块与电源模块连接，接口模块与导航计算机连接，导航计算机与显示模块连接。

2.一种如权利要求1所述的发射惯性系下的多组合导航处理装置的导航方法，其特征在于，包括以下步骤：

(1)对于SINS/GPS/CNS多组合导航系统，其发射惯性系下的捷联惯导解算过程如下，速度计算公式为：

Cbffb=v.-Cifg---(2.1)]]>

式中，v为发射惯性系下的速度值，为v对时间的导数，g为地球的万有引力，为发射惯性系相对于本体系的姿态矩阵，为发射惯性系相对于地心惯性系的姿态矩阵，f_b为加速度真值；

位置计算公式为：

p.=v---(2.2)]]>

式中，p为发射惯性系下的位置值，为p对时间的导数；

姿态运动有四元数描述，计算公式为：

式中，q为姿态四元数，为q对时间的导数，ω_b为为角速度真值；

陀螺和加速度计的测量模型如下：

ωc=ωb+ωr+ωϵfc=fb+fr+fϵω.r=ωnf.r=fn---(2.4)]]>

式中，ω_b为角速度真值，ω_r为陀螺随机游走误差，ω_ε为陀螺测量噪声，f_r为加速度计随机游走误差，f_ε为加速度计测量噪声，ω_n为陀螺随机游走驱动噪声，f_n为加速度计随机游走驱动噪声，f_c为为加速度的实际量测值，ω_c为角速度的实际量测值，为ω_r对时间的导数，为f_r对时间的导数；

组合导航系统为非线性对象，采用扩展卡尔曼滤波进行数值计算，根据公式(2.1)、(2.2)、(2.3)和(2.4)建立发射惯性系下的误差增量方程如下：

δq.13=-[ω^b×]·δq13-0.5·δωr-0.5·ωϵδp.=δυδυ.=2Cbf·[f^b×]·δq13-Cbf·δfr-Cbf·fϵδω.r=ωnδf·r=fn---(2.5)]]>

式中，δq₁₃为数学平台误差的四元数矢量部分，δp为位置误差状态量，δv为速度误差状态量，为δq₁₃对时间的导数，为δp对时间的导数，为δv对时间的导数；表示陀螺角速率的反对称矩阵；为加速度的反对称矩阵，[ω^b×]=0ωbz-ωby-ωbz0ωbxωby-ωbx0,]]>[f^b×]=0fbz-fby-fbz0fbxfby-fbx0,]]>ω_bx为角速度真值在X轴的分量；ω_by为角速度真值在Y轴的分量；ω_bz为角速度真值在Z轴的分量；f_bx为加速度真值在X轴的分量；f_by为加速度真值在Y轴的分量；f_bz为加速度真值在Z轴的分量，δω_r为陀螺随机游走误差增量，为δω_r对时间的导数，δf_r为加速度计随机游走误差增量，为δf_r对时间的导数；

(2)基于上述，获得SINS/GPS/CNS多组合导航系统状态方程为：

X.(t)15×1=A(t)15×15X(t)+G(t)15×12W(t)12×1---(2.6)]]>

其中，状态变量为：

X＝[δq₁  δq₂  δq₃  δp_x  δp_y  δp_z  δv_x  δv_y  δv_z  δω_rx  δω_ry  δω_rz  δf_rx  δf_ry  δf_rz]^T

δq₁,δq₂,δq₃表示惯性导航系统误差状态量中数学平台误差的四元数矢量部分；δp_x,δp_y,δp_z分别表示惯性导航系统误差状态量中的X轴位置误差状态量、Y轴位置误差状态量和Z轴位置误差状态量；δv_x,δv_y,δv_z分别表示惯性导航系统误差状态量中的X轴速度误差状态量、Y轴速度误差状态量和Z轴速度误差状态量；δω_rx,δω_ry,δω_rz分别表示惯性导航系统误差状态量中的X轴陀螺随机游走误差增量状态量、Y轴陀螺随机游走误差增量状态量和Z轴陀螺随机游走误差增量状态量；δf_rx,δf_ry,δf_rz分别表示惯性导航系统误差状态量中的X轴加速度计随机游走误差增量状态量、Y轴加速度计随机游走误差增量状态量和Z轴加速度计随机游走误差增量状态量；上标T为转置；A(t)_15×15为系统的状态转移矩阵；G(t)_15×12为姿态噪声系数矩阵；W(t)_12×1为系统的白噪声矢量；

(3)根据各子系统不同工作特性，建立发射惯性系下各子系统的量测方程，SINS/GPS子系统量测为捷联惯性导航系统给出的三轴位置参数和GPS卫星接收机给出的相应信息的差值；SINS/CNS子系统量测为惯导给出的姿态信息和天文星敏感器给出的姿态差值；GPS和CNS的输出信息需要先转换成发射惯性系下的相关参数，再和惯导信息相减；

a、SINS/GPS子系统

定义观测矢量为：

ZG(t)=xG-xIyG-yIzG-zI=δxδyδz+pϵxpϵypϵz=Hp(t)X(t)+pϵ---(2.7)]]>

式中：x_I和x_G分别表示惯导系统和GPS的X轴位置参数，y_I和y_G分别表示惯导系统和GPS的Y轴位置参数，z_I和z_G分别表示惯导系统和GPS的Z轴位置参数，p_ε为GPS量测白噪声，观测矩阵H_p(t)＝[0_3×3  eye(3)  0_3×9]_3×15，δx为X轴的位置误差量测值；δy为Y轴的位置误差量测值；δz为Z轴的位置误差量测值；p_εx为GPS量测误差在X轴的分量；p_εy为GPS量测误差在Y轴的分量；p_εz为GPS量测误差在Z轴的分量；

b、SINS/CNS子系统

定义子系统观测矢量为姿态四元数的矢量部分，如下：

Z_s(t)＝δq₁₃+q_η,13＝H_a(t)X(t)+N_S(t)     (2.8)

式中：观测矩阵H_a(t)＝[I_3×3  0_3×3  0_3×3  0_3×6]_3×15，N_S(t)为量测噪声，δq₁₃为数学平台误差四元数的矢量部分，q_η,13为量测白噪声；

(4)将步骤(3)所述的各子系统量测方程中的子系统误差状态量进行卡尔曼滤波，并将子系统卡尔曼滤波结果送入联邦滤波模块；

(5)联邦主滤波模块对步骤(4)中子系统送来的滤波结果进行数据融合，输出全局最优估计值，从而对惯性导航系统的导航误差进行修正。

3.根据权利要求2中所述的发射惯性系下的多组合导航处理装置的导航方法，其特征在于所述步骤(3)中GPS和CNS的输出信息需要先转换成发射惯性系下的相关参数，其转换过程如下：

对于GPS，位置转换为

pos_f=Cef·pos_e=Cif·Cei·pos_e---(3.1)]]>

式中，pos_f为GPS在发射惯性系下的位置量测值，pos_e为GPS在地固系下的原始位置量测值，为地心惯性系到发射惯性系的姿态矩阵，为地固系到地心惯性系的姿态矩阵，为地固系到发射惯性系的姿态矩阵；

对CNS，姿态转换为

Cfb=Csb·Cis·Cfi---(3.2)]]>

式中，为地心惯性系到星敏测量系的姿态矩阵，根据星敏感器的输出求得，为星敏测量系到本体系的姿态矩阵，为发射惯性系到地心惯性系的姿态矩阵，根据公式(3.1)求得的求出星敏感器在发射惯性系下的姿态量测值。