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

摘要

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

主权项

1.一种基于双IMU/DGPS组合的相对姿态测量实时动态滤波方法，其特征在于：采用主惯导系统与从惯导系统两个光纤捷联惯导系统与一个DGPS进行组合，具体滤波方法由下述步骤来完成：步骤1：主惯导系统和从惯导系统根据各自的惯性测量单元测得的载体加速度数据和角速度数据，分别通过捷联惯导实时解算，得到主惯导系统和从惯导系统的导航信息；步骤2：判断DGPS信息是否有更新；当DGPS信息更新时，进入步骤3；当DGPS信息丢失或未更新时，则进入步骤4。步骤3：根据由DGPS获取的载体的位置、速度等信息，与步骤一中得到的主惯导系统与从惯导系统的导航信息，分别对主惯导系统与从惯导系统进行滤波修正，建立组合导航滤波器的量测方程，随后进入步骤5；步骤4：使用主惯导系统的速度、位置信息对从惯导系统进行滤波修正，建立双惯导组合滤波器方程，随后进入步骤5；步骤5：将惯导系统组合滤波器量测方程和状态方程离散化，建立离散型卡尔曼滤波器的递推方程；步骤6：通过步骤5中建立的离散型卡尔曼滤波器的递推方程，进行Kalman滤波解算，得到主惯导系统与从惯导系统的俯仰、横滚和航向角；步骤7：根据步骤6得到的主惯导系统的俯仰、横滚和航向角ψ_AE、ψ_AN和ψ_AU，与从惯导系统的俯仰、横滚和航向角ψ_BE、ψ_BM和ψ_BU，建立由地理坐标系到主惯导系统载体坐标系的转换矩阵为由地理坐标系到从惯导系统载体坐标系的转换矩阵为则：CAn=cosψANcosψAU-sinψANsinψAEsinψAU-cosψAEsinψAUsinψANcosψAU+cosψANsinψAEsinψAUcosψANsinψAU+sinψANsinψAEcosψAUcosψAEcosψAUsinψANsinψAU-cosψANsinψAEcosψAU-sinψANcosψAEsinψAEcosψANcosψAE]]>CBn=cosψBNcosψBU-sinψBNsinψBEsinψBU-cosψBEsinψBUsinψBNcosψBU+cosψBNsinψBEsinψBUcosψBNsinψBU+sinψBNsinψBEcosψBUcosψBEcosψBUsinψBNsinψBU-cosψBNsinψBEcosψBU-sinψBNcosψBEsinψBEcosψBNcosψBE]]>得到由主惯导系统载体坐标系到从惯导系统载体坐标系的坐标转换矩阵为：CAB=CnB·CAn=(CBn)T·CAn]]>则主惯导系统与从惯导相对姿态角的主值为：ψEAB0=arctanCAB(3,2)1-(CAB(3,2))2]]>ψNAB0=arctan(-CAB(3,1)CAB(3,3))]]>ψUAB0=arctan(-CAB(1,2)CAB(2,2))]]>步骤8：主惯导系统与从惯导系统间的相对姿态置信度估计。