一种基于双IMU/DGPS组合的相对姿态测量实时动态滤波方法

摘要

本发明公开基于双IMU/DGPS组合的相对姿态测量实时动态滤波方法，采用双光纤捷联惯导系统，通过捷联惯导实时解算，得到主惯导与从惯导系统的导航信息；判断DGPS信息是否更新，产生两种情况：当更新时，主惯导与从惯导系统进行滤波修正，建立组合导航滤波器的量测方程；当没有更新时，使用主惯导系统对从惯导系统进行滤波修正，建立组合导航滤波器的量测方程；两种情况得到的组合滤波方程通过离散化，建立离散型卡尔曼滤波器的递推方程，解算得到主惯导与从惯导系统的俯仰、横滚和航向角；随后进行相对姿态矩阵解算，得到主惯导与从惯导相对姿态角的主值。本发明提高了导航系统的稳定性，且可实时输出导航系统在测量过程中的速度、位置、姿态信息，测量范围广。

主权项

1.一种基于双IMU/DGPS组合的相对姿态测量实时动态滤波方法，其特征在于：采用主惯导系统与从惯导系统两个光纤捷联惯导系统与一个DGPS进行组合，具体滤波方法由下述步骤来完成： 步骤1：主惯导系统和从惯导系统根据各自的惯性测量单元测得的载体加速度数据和角速度数据，分别通过捷联惯导实时解算，得到主惯导系统和从惯导系统的导航信息； 设惯导系统的导航坐标系为当地地理坐标系，水平位置误差为δL,δλ,δh，δL为惯导系统经度误差，δλ为惯导系统纬度误差，δh为惯导系统高度误差，水平速度误差为δV_E,δV_N,δV_U，姿态误差为ψ_E,ψ_N,ψ_U，陀螺漂移项ε_E,ε_N,ε_U，加速度计零偏▽_E,▽_N,▽_U，则捷联惯导误差方程为： 状态矢量： X(t)＝[δL,δλ,δh,δV_E,δV_N,δV_U,ψ_E,ψ_N,ψ_U,ε_E,ε_N,ε_U,▽_E,▽_N,▽_U]^T   (1) 姿态误差方程： 速度误差方程： 水平位置误差方程： 其中，ω_ie为地球自转角速度，ω_ie＝7.29×10^-5rad/s；R_E＝6378137m，R_E为参考椭球体的长半径；f为地球扁率，f＝1/298.257；R_M＝R_E(1-2f+3fsin²L)，R_M为子午圈主曲率半径；R_N为卯酉圈主曲率半径R_N＝R_E(1+fsin²L)；L为惯导系统测量到的位置经度数据；h为惯导系统测量到的高度数据； 根据主惯导系统和从惯导系统根据各自的惯性测量单元测得的载体加速度数据和角速度数据，通过上述捷联惯导误差方程，可分别得到主惯导系统与从惯导系统的水平位置误差δL_A,δλ_A,δh_A与δL_B,δλ_B,δh_B，水平速度误差δV_AE,δV_AN,δV_AU与δV_BE,δV_BN,δV_BU，姿态误差ψ_AE,ψ_AN,ψ_AU与ψ_BE,ψ_BN,ψ_BU，并分别对主惯导系统与从惯导系统的水平位置误差，水平速度误差，姿态误差进行迭代运算，从而得到主惯导系统与从惯导系统的位置、速度和姿态值 ；步骤2：判断DGPS信息是否有更新； 当DGPS信息更新时，进入步骤3；当DGPS信息丢失或未更新时，则进入步骤4； 步骤3：根据由DGPS获取的载体的位置、速度等信息，与步骤1中得到的主惯导系统与从惯导系统的导航信息，分别对主惯导系统与从惯导系统进行滤波修正，建立组合导航滤波器的量测方程，随后进入步骤5； 上述组合导航滤波器的量测方程的建立方法为： 惯导系统的位置信息可表示为： 其中，惯导系统水平位置误差为δL,δλ,δh，L_t，λ_t，h_t为惯导系统的真实位置信息； DGPS测得的载体位置信息可表示为： 其中，N_E，N_N，N_U分别为DGPS测量得到的载体沿东、北、天方向的位置误差； 则惯导系统的位置量测矢量为： 其中，，；设DGPS接收机伪距测量误差为σ_ρ，平面位置精度因子为HDOP，高程精度因子为VDOP，将DGPS量测噪声矢量V(t)作为白噪声处理，则DGPS位置噪声方差为为：惯导系统的速度信息可表示为： 其中，惯导系统的水平速度误差为δV_E,δV_N,δV_U，V_N，V_E，V_U为惯导系统的真实速度信息； DGPS测得的载体速度信息可表示为： 其中，M_E，M_N，M_U分别为DGPS沿东、北、天方向的载体速度误差； 则惯导系统的速度量测矢量为： 其中，；设DGPS接收机伪距率测量误差为则DGPS速度噪声方差为为：综上可得在DGPS信号正常时惯导系统组合滤波器量测方程如下： 步骤4：使用主惯导系统的速度、位置信息对从惯导系统进行滤波修正，建立双惯导组合滤波器方程，随后进入步骤5； 上述使用主惯导系统的速度、位置信息对从惯导系统进行滤波修正方法为： 从惯导系统的位置量测信息表示为： 其中，δL_B、δλ_B、δh_B分别为从惯导系统经度、纬度、高度上的位置误差；L_Bt，λ_Bt，h_Bt为从惯导系统的真实位置信息； 主惯导系统的位置量测信息可表示为： 其中，δL_A,δλ_A,δh_A为主惯导系统与从惯导系统的水平位置误差； 则惯导系统位置量测矢量定义为： 其中，；主惯导系统与从惯导系统组合滤波器的位置噪声的最优估计由主惯导系统估计误差的协方差阵P_A(t)为： 式(17)中，为主惯导位置误差方差的估计值；从惯导系统的速度信息可表示为： 其中，δV_BE,δV_BN,δV_BU为主惯导系统水平速度误差；V_BNt，V_BEt，V_BUt为从惯导系统真实速度信息；主惯导系统的速度量测信息可表示为： 其中，δV_AE,δV_AN,δV_AU为主惯导系统水平速度误差； 则惯导系统位置量测速度量测矢量为： 式中，；主惯导系统与从惯导系统组合滤波器的速度量测噪声的最优估计由主惯导系统估计误差的协方差阵P_A(t)给出： 式中，为主惯导系统速度误差方差的估计值；综上可得在无DGPS信号时惯导系统组合滤波器量测方程如下： 步骤5：将惯导系统组合滤波器量测方程和状态方程离散化，建立离散型卡尔曼滤波器的递推方程； 所述离散型卡尔曼滤波器的递推方程建立过程如下： 首先，给定惯导系统的一阶线性状态方程和量测方程为： Z(t)＝H(t)X(t)+V(t)   (24) 进而将状态方程(5)和量测方程(6)离散化可得： X_k＝Φ_k,k-1X_k-1+Γ_k-1W_k-1   (25) Z_k＝H_kX_k+V_k   (26) 其中，X_k为状态向量，Φ_k,k-1为状态转移矩阵，Z_k为量测向量，H_k为量测矩阵，Γ_k-1为系统噪声矩阵，W_k-1为系统噪声向量，V_k为量测噪声向量；W_k-1、V_k是不相关高斯白噪声列； 状态预测估计方程为： 方差预测方程为： 状态预测估计方程为： 方差迭代方程： ＝(I-K_kH_k)P_k/k-1   (30) 滤波增益方程为： 初始条件为： 验前统计量为： E[W_k]＝0,Cov[W_k,W_j]＝E[W_kW_j^T]＝Q_kδ_kj   (33) E[V_k]＝0,Cov[V_k,V_j]＝E[V_kV_j^T]＝R_kδ_kj   (34) Cov[W_k,V_j]＝E[W_kV_j^T]＝0   (35) 步骤6：通过步骤5中建立的离散型卡尔曼滤波器的递推方程，进行Kalman滤波解算，得到主惯导系统与从惯导系统的俯仰、横滚和航向角； 步骤7：根据步骤6得到的主惯导系统的俯仰、横滚和航向角ψ_AE、ψ_AN和ψ_AU,与从惯导系统的俯仰、横滚和航向角ψ_BE、ψ_BN和ψ_BU，建立由地理坐标系到主惯导系统载体坐标系的转换矩阵为由地理坐标系到从惯导系统载体坐标系的转换矩阵为则：得到由主惯导系统载体坐标系到从惯导系统载体坐标系的坐标转换矩阵为：则主惯导系统与从惯导相对姿态角的主值为： 步骤8：主惯导系统与从惯导系统间的相对姿态置信度估计，具体为： 由主惯导系统与从惯导系统各自的姿态误差分别由协方差阵P_A(t)和P_B(t)给出最优估计；设主惯导系统在地理坐标系下（x、y、z）三个坐标轴方向上姿态角的噪声方差估计值分别从惯导系统为则:在主惯导系统与从惯导系统精度近似不相关的情况下，相对姿态的置信度估计值为： 其中，分别表示两惯导相对姿态置信度估计值。