一种基于对偶四元数的惯性/GPS组合导航方法技术

本发明专利技术公开了一种基于对偶四元数的惯性/GPS组合导航方法，属于飞行器组合导航技术领域。该方法首先建立陀螺和加速度计误差模型，并将陀螺及加速度计误差扩展为系统状态变量，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程；随后，结合GPS的量测信息及对偶四元数捷联惯导算法的计算值，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程；最后，对系统状态方程和量测方程进行离散化处理，采用卡尔曼滤波进行闭环估计并对惯导算法中的推力速度对偶四元数、引力速度对偶四元数、位置对偶四元数进行修正，从而得到载体的导航信息。本方法能够有效利用GPS的输出信息对惯导解算误差进行修正，提高惯性导航系统性能，适用于工程应用。

【技术实现步骤摘要】
一种基于对偶四元数的惯性/GPS组合导航方法
本专利技术涉及一种基于对偶四元数的惯性/GPS组合导航方法，属于飞行器组合导航

技术介绍
近年来，随着高超声速飞行器等高动态飞行器的研制发展，对导航系统性能提出了更高的要求。惯性导航系统具有短时精度高、输出连续以及完全自主等突出优点，但其误差会随时间累积，需要其他导航手段加以辅助。GPS全球定位系统是一种高精度的全球三维实时卫星系统，其导航定位的全球性和高精度使其成为一种先进的导航设备。但是GPS全球定位系统也存在一些不足之处，主要是GPS在受到遮挡的情况下容易信号丢失，且容易受到人为控制和干扰，因此主要作为一种辅助导航设备使用。惯性/GPS组合克服了各自缺点，取长补短，使组合后的导航精度高于两个系统单独工作的精度。对偶四元数捷联惯性导航算法将载体的旋转和平移统一考虑，以最简洁的形式表示一般的刚体运动，在高动态环境下，具有比传统捷联惯导算法更高的精度，更能满足高动态飞行器对高精度导航性能的要求。传统的惯性导航算法误差模型通常是基于数学平台失准角的线性误差方程，但是该模型仅适用于平台失准角为小量的情况，当载体的姿态角误差较大时，在组合导航算法中使用该模型会使滤波精度大大降低，收敛时间变长甚至会导致发散。基于对偶四元数的捷联惯性导航算法可以直接利用对偶四元数建立线性误差模型，即使在大失准角的情况也同样能具有较快的收敛速度，较好的滤波精度，从而提高了组合导航系统性能。因此，研究基于对偶四元数的惯性/GPS组合导航算法具有重要的研究意义。
技术实现思路
本专利技术提出了一种基于对偶四元数的惯性/GPS组合导航方法，在飞行器动态飞行过程中有效利用GPS提供的速度位置信息，对惯性导航解算参数误差进行修正，显著提高惯性导航系统精度。本专利技术为解决其技术问题采用如下技术方案：一种基于对偶四元数的惯性/GPS组合导航方法，包括如下步骤：步骤1，建立陀螺、加速度计误差模型，所述陀螺误差包括常值漂移误差、一阶马尔科夫过程随机噪声以及白噪声随机误差，所述加速度计误差为一阶马尔科夫过程随机噪声；步骤2，在步骤1对陀螺和加速度计误差建模的基础上，将步骤1所述的陀螺常值漂移误差、陀螺一阶马尔科夫过程随机噪声、加速度计一阶马尔科夫过程随机噪声扩展为系统状态变量，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程；步骤3，将GPS输出的地理系速度、地球系位置测量误差建模为白噪声，并将其测得的地理系速度转化为惯性系速度，结合由对偶四元数捷联惯导算法计算得到的惯性系速度及地球系位置，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程；步骤4，对系统状态方程和量测方程进行离散化处理，并采用卡尔曼滤波对状态量进行闭环估计，利用估计所得的对偶四元数误差对惯导算法中的推力对偶四元数、引力对偶四元数、位置对偶四元数进行修正，从而得到载体的速度、位置、姿态等导航信息。步骤1所述陀螺和加速度计误差模型为：其中，为陀螺误差，εb为陀螺常值漂移误差，εr为陀螺一阶马尔科夫过程随机噪声，ωg为白噪声；δfB为加速度计误差，为加速度计一阶马尔科夫过程随机噪声；对上式中的εb、εr、进行求导后可得到以下数学表达式：其中，为εb的一阶导数；为εr的一阶导数；为的一阶导数；Tg为陀螺一阶马尔科夫过程相关时间，ωr为陀螺一阶马尔科夫过程驱动白噪声；Ta为加速度计一阶马尔科夫过程相关时间，ωa为加速度计一阶马尔科夫过程驱动白噪声。步骤2所述基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程为：其中X∈R28×1为系统状态量，F∈R28×28为系统矩阵，G∈R28×12为噪声系数矩阵，w∈R12×1为系统噪声向量，为X的一阶导数，各矩阵分别表示为：系统矩阵F和噪声系数矩阵G中，若将四元数q写成q＝[q0q1q2q3]T的形式，我们定义矩阵为q在四元数乘法中的前乘矩阵，为q在四元数乘法中的后乘矩阵，其具体可表示为：系统矩阵F和噪声系数矩阵G中0均为四阶零矩阵，I4为四阶单位矩阵，各变量均为四元数，其中三维向量表示为标量部分为0的四元数。δqIT为推力速度对偶四元数误差的实数部分，δq′IT为推力速度对偶四元数误差的对偶部分，δq′IG为引力速度对偶四元数误差的对偶部分，δq′IU为位置对偶四元数误差的对偶部分；为陀螺输出信息，为在四元数乘法中的后乘矩阵；qIT为推力速度对偶四元数的实数部分，为qIT在四元数乘法中的前乘矩阵；q′IT为推力对偶四元数的对偶部分，为q′IT在四元数乘法中的前乘矩阵；为地球自转角速度，为在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，为其在四元数乘法中的前乘矩阵，为其在四元数乘法中的后乘矩阵；q*IT为qIT的共轭四元数，为其在四元数乘法中的前乘矩阵，为其在四元数乘法中的后乘矩阵；系统矩阵F和噪声系数矩阵G中M1、M2、M3可分别表示为：步骤3所述的GPS输出的地理系速度、地球系位置测量误差模型为：VG＝Vn+δV，RG＝Re+δR，其中，VG为GPS输出的地理系速度，Vn为载体真实的地理系速度，δV为GPS的速度测量误差，将其建模为白噪声；RG为GPS测得的地球系位置，Re为载体真实的地球系位置，δR为GPS的位置测量误差，将其建模为白噪声；利用如下公式将VG转换到惯性系：其中，VGI为VG转换到惯性系下的值，为地球系到惯性系的转换矩阵，ωie为四元数的矢量部分，为地理系到地球系的转换矩阵；由此可建立步骤3所述基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程：Z＝HX+v，其中，为量测向量，VI为对偶四元数惯导算法计算所得惯性系速度，RI为对偶四元数惯导算法计算所得地球系位置；H为量测系数矩阵，X为系统状态量，为系统量测噪声阵；量测系数矩阵H的具体表达式为：其中，qIG为引力速度对偶四元数的实数部分，为qIG的共轭四元数，为q*IG在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，*IU为qIU的共轭四元数，为q*IU在四元数乘法中的前乘矩阵，为q′IT在四元数乘法中的前乘矩阵。所述步骤4的具体过程为：(401)将系统状态方程和量测方程离散化处理：Xk＝Φk,k-1Xk-1+Γk,k-1Wk-1，Zk＝HkXk+Vk，其中，Xk为tk时刻系统状态量,Xk-1为tk-1时刻系统状态量，Φk,k-1为tk-1时刻至tk时刻系统的状态转移矩阵，Γk,k-1为tk-1时刻至tk时刻系统的噪声驱动矩阵，Wk-1为tk-1时刻系统的噪声矩阵，Zk为tk时刻系统的量测矩阵,Hk为tk时刻的量测系数矩阵，Vk为tk时刻的观测量噪声矩阵；(402)采用卡尔曼滤波对状态量进行闭环估计：其中，是状态量Xk-1的一步预测估计值,Pk-1为tk-1时刻滤波状态估计协方差矩阵,Qk-1为tk-1时刻系统噪声协方差矩阵,为Φk,k-1的转置，为Γk,k-1的转置，Pk,k-1为tk-1时刻到tk时刻的状态一步预测协方差矩阵,Rk为tk时刻的量测噪声协方差矩阵,Kk为tk时刻滤波增益矩阵,为Hk的转置，为状态量Xk的卡尔曼滤波估值，I为单位矩阵，为Kk的转置，Pk为tk时刻滤波状态估计协方差矩阵；(403)在(402)得到各状态量估计值后，利用估计所得的对偶四元数误差对惯导算法中的推力对偶本文档来自技高网...

【技术保护点】
一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，包括如下步骤：步骤1，建立陀螺、加速度计误差模型，所述陀螺误差包括常值漂移误差、一阶马尔科夫过程随机噪声以及白噪声随机误差，所述加速度计误差为一阶马尔科夫过程随机噪声；步骤2，在步骤1对陀螺和加速度计误差建模的基础上，将步骤1所述的陀螺常值漂移误差、陀螺一阶马尔科夫过程随机噪声、加速度计一阶马尔科夫过程随机噪声扩展为系统状态变量，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程；步骤3，将GPS输出的地理系速度、地球系位置测量误差建模为白噪声，并将其测得的地理系速度转化为惯性系速度，结合由对偶四元数捷联惯导算法计算得到的惯性系速度及地球系位置，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程；步骤4，对系统状态方程和量测方程进行离散化处理，并采用卡尔曼滤波对状态量进行闭环估计，利用估计所得的对偶四元数误差对惯导算法中的推力对偶四元数、引力对偶四元数、位置对偶四元数进行修正，从而得到载体的速度、位置、姿态等导航信息。

【技术特征摘要】
1.一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，包括如下步骤：步骤1，建立陀螺、加速度计误差模型，所述陀螺误差包括常值漂移误差、一阶马尔科夫过程随机噪声以及白噪声随机误差，所述加速度计误差为一阶马尔科夫过程随机噪声；步骤2，在步骤1对陀螺和加速度计误差建模的基础上，将步骤1所述的陀螺常值漂移误差、陀螺一阶马尔科夫过程随机噪声、加速度计一阶马尔科夫过程随机噪声扩展为系统状态变量，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程；步骤3，将GPS输出的地理系速度、地球系位置测量误差建模为白噪声，并将其测得的地理系速度转化为惯性系速度，结合由对偶四元数捷联惯导算法计算得到的惯性系速度及地球系位置，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程；步骤4，对系统状态方程和量测方程进行离散化处理，并采用卡尔曼滤波对状态量进行闭环估计，利用估计所得的对偶四元数误差对惯导算法中的推力对偶四元数、引力对偶四元数、位置对偶四元数进行修正，从而得到载体的速度、位置、姿态等导航信息。2.根据权利要求1所述的一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，步骤1所述陀螺和加速度计误差模型为：δfB＝▽a，其中，为陀螺误差，εb为陀螺常值漂移误差，εr为陀螺一阶马尔科夫过程随机噪声，ωg为白噪声；δfB为加速度计误差，▽a为加速度计一阶马尔科夫过程随机噪声；对上式中的εb、εr、▽a进行求导后可得到以下数学表达式：其中，为εb的一阶导数；为εr的一阶导数；为▽a的一阶导数；Tg为陀螺一阶马尔科夫过程相关时间，ωr为陀螺一阶马尔科夫过程驱动白噪声；Ta为加速度计一阶马尔科夫过程相关时间，ωa为加速度计一阶马尔科夫过程驱动白噪声。3.根据权利要求1所述的一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，步骤2所述基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程为：其中X∈R28×1为系统状态量，F∈R28×28为系统矩阵，G∈R28×12为噪声系数矩阵，w∈R12×1为系统噪声向量，为X的一阶导数，各矩阵分别表示为：系统矩阵F和噪声系数矩阵G中，若将四元数q写成q＝[q0q1q2q3]T的形式，我们定义矩阵为q在四元数乘法中的前乘矩阵，为q在四元数乘法中的后乘矩阵，其具体可表示为：系统矩阵F和噪声系数矩阵G中0均为四阶零矩阵，I4为四阶单位矩阵，各变量均为四元数，其中三维向量表示为标量部分为0的四元数；δqIT为推力速度对偶四元数误差的实数部分，δq′IT为推力速度对偶四元数误差的对偶部分，δq′IG为引力速度对偶四元数误差的对偶部分，δq′IU为位置对偶四元数误差的对偶部分；为陀螺输出信息，为在四元数乘法中的后乘矩阵；qIT为推力速度对偶四元数的实数部分，为qIT在四元数乘法...

【专利技术属性】
技术研发人员：闵艳玲，熊智，邢丽，刘建业，王融，殷德全，许建新，施丽娟，景羿铭，万众，鲍雪，黄欣，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：江苏,32