[发明专利]一种适合车载光纤航姿系统的姿态测量方法

摘要

本发明涉及属于惯性导航技术领域，具体涉及一种适合车载光纤航姿系统的姿态测量方法。本发明采用基于卡尔曼滤波器的低成本光纤航姿系统与GPS组合导航系统，在SINS初始对准前，对GPS故障信号进行检测与隔离，保证SINS/GPS组合系统的可靠性，当GPS输出有效后，选取SINS输出的航向角、北向、东向速度与GPS接收机输出相应信息的差值作为观测量，构成3维观测方程。本发明能够大大加快计算速度，实现实时滤波计算，而且与速度+位置作为观测量的组合系统相比，其导航精度尤其是姿态计算机的精度有很大的提高。

主权项

1.一种适合车载光纤航姿系统的姿态测量方法，采用基于卡尔曼滤波器的低成本光纤航姿系统与GPS组合导航系统，其特征在于，按照系统解算流程，当GPS输出有效后，利用GPS输出的姿态、速度和SINS输出的姿态、速度之差作为观测量，具体包括如下步骤：首先建立以SINS误差方程为基础的组合导航系统状态方程，并在误差方程的基础上建立组合系统的量程方程，用一阶线性随机微分方程来描述系统的误差状态传播特性和系统的量测误差状态，方程如下：X·(t)=F(t)X(t)+G(t)W(t)---(1)]]>Z(t)＝H(t)X(t)+V(t)……………………………………(2)选取SINS导航参数计算误差、光纤陀螺仪以及加速度计误差作为系统的状态变量，共12维，即：XT=[φE,φN,φU,δVE,δVN,δL,δλ,ϵbx,ϵby,ϵbz,▿bx,▿by]---(3)]]>式中：φ_E，φ_N，φ_U为SINS姿态计算误差，δV_E和δV_N为SINS的速度误差；δL和δλ为S INS位置误差；ε_bx、ε_by和ε_bz为3个轴向陀螺漂移误差为加速度计测量误差；选取SINS输出的航向角、北向、东向速度与GPS接收机输出相应信息的差值作为观测量，构成3维观测方程，即：Z(t)=ZvZψ=Hv(t)Hψ(t)X(t)+Vv(t)Vψ(t)---(4)]]>式中，H_V(t)是观测量为速度时的量测矩阵，H_ψ(t)是观测量为姿态时的量测矩阵，其中速度量测矢量为：Zv=vSINS.N-vGPS.NvSINSE-vGPS.E=HvXv+Vv---(5)]]>式中，v_SINS.N、v_SINS.E是SINS输出的北向和东向速度信息，v_SINS.N＝v_N+δv_N，v_SINS.E＝v_E+δv_E，其中v_N、v_E是载体沿地理坐标系统北向和东向的真实速度；v_GPS.N、v_GPS.E是GPS接收机输出的北向和东向的速度信息，v_GPS.N＝v_N-M_N、v_GPS.E＝v_E-M_E，其中M_N、M_E是GPS接收机测速误差，设为白噪声，H_v是观测量为速度时的量测矩阵；H_v＝[diag[1 1]0_2×10]，V_v是GPS接收机输出速度的量测噪声：V_v＝[M_N M_E]^T，取量测噪声为白噪声，Z_ψ＝ψ_SINS.yaw-ψ_GPS.yaw＝H_ψX_ψ+V_ψ……………………………(6)式中，ψ_SINS.yaw为SINS输出的航向角，ψ_SINS.yaw＝ψ_aw+δψ_SINS.yaw，其中ψ_aw为真实航向角，δψ_SINS.yaw为SINS航向角测量误差；ψ_GPS.yaw为双GPS输出的航向角，ψ_GPS.yaw＝ψ_yaw+δψ_GPS.yaw，其中δψ_GPS.yaw是双GPS航向角测量误差。视双GPS接收机测量精度和基线长度而定；H_ψ是观测量为姿态时的量测矩阵，H_ψ＝[0_1×2 1 0_1×9]；V_ψ为双GPS输出航向角的量测噪声，V_ψ＝δψ_GPS.yaw，取量测噪声为白噪声，设GPS航向测量白噪声方差为则R_ψ＝(HDOP.δ_ψ)²离散后的状态方程和观测方程为：X_K＝Φ_k，k-1X_k-1+Γ_k-1W_k-1……………………………….…(8)Z_K＝H_kK_k+V_k卡尔曼滤波算法如下，状态下一步预测：状态估计：滤波增益：下一步预测均方差：估计均方差：Pk=(I-KkHk)Pk/k-1(I-KkHk)T+KkRkKkT.]]>