CN102508954A - Gps/sins组合导航全数字仿真方法及装置 - Google Patents

Abstract

本发明属于组合导航领域。为提供一种低成本、高精度导航装置及方法，本发明采取的技术方案是，GPS/SINS组合导航全数字仿真装置，由轨迹发生器模块、IMU仿真器、GPS仿真器、SINS解算模块以及卡尔曼滤波器模块构成；轨迹发生器模块用于产生飞行轨迹数据；IMU仿真器用于模拟陀螺仪和加速度计的实际情况；GPS仿真器模拟GPS接收机，输出载体的伪距、伪距率、位置和速度信息；SINS解算模块将IMU仿真器的输出进行解算获得载体的位置、速度和姿态信息；卡尔曼滤波器模块将GPS和SINS可以按照松散组合和紧密组合两种不同的组合方式，采用Kalman滤波方法进行数据融合。本发明主要应用于导航。

Description

GPS/SINS组合导航全数字仿真方法及装置

技术领域

本发明属于组合导航领域，具体涉及GPS/SINS组合导航全数字仿真方法及装置，对实际的组合导航系统的设计、方案论证有着重要的意义。

背景技术

为了获得移动载体的实时位置、速度和姿态信息，多种导航方式已经被提出和采用。其中，以捷联惯性导航系统(Strapdown Inertial Navigation System，SINS)和全球卫星定位系统(Global Positioning System，GPS)的应用最为广泛。

SINS不仅能够提供载体位置、速度参数，还能提供载体的三维姿态参数，是完全自主的导航方式，在航空、航天、航海和陆地等几乎所有领域中都得到了广泛应用。但是，SINS难以克服的缺点是其导航定位误差随时间累加，难以长时间独立工作。

20世纪末发展起来的GPS具有定位和测速精度高的优势，且基本上不受时间、地区的限制，已经得到了广泛应用。但是，在卫星信号受到屏蔽或遮挡时，接收机就无法定位。因而，对导航定位可靠性要求较高的情况下GPS很难单独应用。

可以看出，GPS和SINS都存在着自身难以克服的缺点。但是，GPS和SINS又具有很强的互补性，两者的组合不仅可以充分发挥各自的优势，而且随着组合程度的加深GPS/SINS组合系统的总体性能要远远优于各独立系统。组合系统利用高精度的GPS信息作为外部量测输入，在运动过程中修正SINS，以控制其误差随时间的累积；而短时间内高精度的SINS定位结果，又可以解决GPS动态环境中的信号失锁和周跳问题。SINS还可以辅助GPS接收机增强其抗干扰能力，提高捕获和跟踪卫星信号的能力。因此，GPS/SINS组合系统被认为是目前导航领域最理想的组合方式。

高精度的器件也带来了高价格的问题。根据市场调研，国内目前高精度的惯性测量单元价格在几十万左右，能够提供伪距、伪距率信息而且定位精度到达厘米级别的GPS的价格也在10万元以上。这在一定程度上，特别是在民用领域，限制了GPS/SINS组合系统的推广应用。因此，低成本的GPS/SINS组合导航系统已成为国内外的研究热点之一。

对于低成本的GPS/SINS组合导航系统，其核心问题是如何通过改善软件算法来提高导航精度。在软件方面，捷联矩阵的更新、误差建模、卡尔曼滤波等关键技术都会影响最终导航的精度。

发明内容

为克服现有技术的不足，提供一种低成本、高精度导航装置及方法，本发明采取的技术方案是，一种GPS/SINS组合导航全数字仿真装置，由轨迹发生器模块、IMU仿真器、GPS仿真器、SINS解算模块以及卡尔曼滤波器模块构成；轨迹发生器模块用于产生飞行轨迹数据；IMU仿真器用于模拟陀螺仪和加速度计的实际情况，输出载体的三维角速度信息和三维加速度信息；GPS仿真器模拟GPS接收机，输出载体的伪距、伪距率、位置和速度信息；SINS解算模块将IMU仿真器的输出进行解算获得载体的位置、速度和姿态信息；卡尔曼滤波器模块将GPS和SINS可以按照松散组合和紧密组合两种不同的组合方式，采用Kalman滤波方法进行数据融合。

所述轨迹发生器模块采用数学方法模拟包括爬升、平飞、下降在内的各种各样的飞行轨迹，提供组合导航仿真系统需要的姿态角、姿态角速率、位置、速度、加速度五组共十五个飞行轨迹数据；

IMU仿真器将轨迹发生器模块提供的姿态角速率和加速度信息作为输入，依据陀螺仪和加速度计的误差模型，加入误差，模拟陀螺仪和加速度计的真实输出，最后将结果输出给SINS解算模块；

GPS仿真器利用软件编程实现GPS的功能，将轨迹发生器提供的位置、速度信息，以及参考卫星星历数据作为输入，输出伪距、伪距率、位置和速度信息给卡尔曼滤波器模块；

SINS解算模块利用IMU仿真器输出的带有误差的姿态角速度和加速度信息，默认采用四元素法进行解算，得到载体的位置、速度和姿态信息。

一种GPS/SINS组合导航全数字仿真方法，使用MATLAB作为工具，通过编写m文件和搭建Simulink模块交互的形式实现仿真；具体为：通过使用MATLAB软件编写m文件实现轨迹发生器模块，用于产生载体的理想轨迹；通过使用MATLAB软件编写m文件实现IMU仿真器，用于模拟惯性测量单元IMU的实际情况，输出载体的三维角速度信息和三维加速度信息；通过使用MATLAB软件编写m文件实现GPS仿真器，模拟GPS接收机，输出载体的伪距、伪距率、位置和速度信息；通过搭建Simulink模块实现SINS解算模块，用于将IMU仿真器的输出数据进行解算获得载体的位置、速度和姿态信息；通过搭建Simulink模块实现卡尔曼滤波器模块，用于将GPS和SINS分别按照松散组合和紧密组合两种不同的组合方式，采用Kalman滤波方法进行数据融合。

所述IMU仿真器是模拟陀螺仪和加速度计的实际输出，将陀螺仪和加速度计的误差均视为由一阶马尔可夫过程随机误差和白噪声误差组成，而且假定陀螺仪和加速度计三个轴向的误差模型都相同：

陀螺仪的理想输出为：

${\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{ib}}^b={\mathrm{\ω}}_{\mathrm{nb}}^b+C_n^b{\mathrm{\ω}}_{\mathrm{ib}}^b$

陀螺仪的实际输出为：

${\mathrm{\ω}}_{\mathrm{nb}}^b={\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{ib}}^b+\mathrm{\ϵ}$

ε＝ε_b+ε_r+ω_g

其中，

为轨迹发生器的提供的角速度信号；

为导航坐标系到载体坐标系的姿态矩阵，可由轨迹发生器提供的数据计算获得；为地理坐标系相对于惯性坐标系的地球自转角速度在地理坐标系中的投影；ε_b为随机常数；ε_r为一阶马尔可夫过程随机噪声；ω_g为白噪声；

加速度计的理想输出为：

${\stackrel{\‾}{f}}_{\mathrm{ib}}^b=f_{\mathrm{nb}}^b+C_n^b{f}_{\mathrm{ib}}^b$

加速度计的实际输出为：

$f_{\mathrm{ib}}^b={\stackrel{\‾}{f}}_{\mathrm{ib}}^b+\▿$

$\▿={\▿}_a+{\mathrm{\ω}}_a$

其中，

为轨迹发生器的提供的加速度信号；

为导航坐标系到载体坐标系的姿态矩阵，可由轨迹发生器提供的数据计算获得；

为地理坐标系相对于惯性坐标系的地球自转角速度和重力加速度在地理坐标系中的投影；

为一阶马尔可夫过程随机噪声；ω_a为随机白噪声。

SINS解算的算法流程如下：首先对IMU仿真器输出的角速度和加速度数据

进行误差补偿，得到误差补偿后的角速度

和加速度利用角速度

默认采用四元素法解算姿态矩阵

由于求解四元素要使用

利用上一时刻的进行迭代，进而求出当前时刻的

利用上述求得姿态矩阵

后，根据矩阵元素与姿态角的对应关系进行姿态解算求出姿态信息；利用

把加速度计的输出从载体坐标系变换到导航坐标系

扣除有害加速度，结合给定的初始位置和速度，通过一次和两次积分就可以分别求得当前速度和位置，完成导航解算。

卡尔曼滤波器进行数据融合的流程如下：首先选择SINS和GPS数据的组合方式，松散组合或者紧密组合，依据选择的组合方式建立卡尔曼滤波误差模型，然后选择常规卡尔曼滤波、扩展卡尔曼滤波EKF、无轨迹卡尔曼滤波UKF中的一种滤波方法进行融合滤波，最后利用滤波估计出载体的位置、速度和姿态误差并反馈校正SINS，并最终输出导航信息。

本发明具有如下技术效果：

1、本发明提供的GPS/SINS组合导航全数字仿真系统，可以代替实际的GPS/SINS组合导航系统，用来验证组合导航中各种软件算法对导航精度的影响和开发新的算法。

2、本发明对低成本的GPS/SINS组合导航系统如何提高导航精度有着重要的指导作用和现实意义。

3、科研人员可以将验证后的算法和开发的新算法载入到实际的GPS/SINS组合导航系统中，进行实际的应用。

4、本发明还具有成本低廉的特点。

附图说明

图1组合导航全数字仿真系统总体结构框图。

图2轨迹发生器示意图。

图3IMU仿真器基本结构框图。

图4GPS仿真器程序流程图。

图5SINS解算基本流程图。

图6卡尔曼滤波模块基本流程图。

具体实施方式

本发明的目的在于提供一种GPS/SINS组合导航系统全数字仿真系统。该仿真系统可以代替实际的GPS/SINS组合导航系统，用来验证组合导航中各种软件算法对导航精度的影响和开发新的算法；同时也可以将这些验证后的算法和开发的新算法载入到实际的GPS/SINS组合导航系统中进行应用；本发明对低成本的GPS/SINS组合导航系统如何提高导航精度有着重要的指导作用和现实意义。本发明主要完成以下功能：

1、轨迹发生器提供整个组合导航仿真系统所需要的飞行轨迹数据。

2、IMU仿真器模拟陀螺仪和加速度计的真实输出。

3、GPS仿真器利用软件编程模拟GPS的功能，输出伪距、伪距率、位置和速度信息。

4、SINS解算模块完成对IMU仿真器输出数据的解算并获得载体的位置、速度和姿态信息；解算算法被集成为模块，用户可以对算法修改或者重新开发，本发明默认采用四元素法作为解算算法。

5、卡尔曼滤波器模块将SINS和GPS的输出数据进行融合滤波，获得载体的位置、速度和姿态信息。本发明提供GPS/SINS的松散组合和紧密组合两种不同方式供用户选择；滤波方法也被集成为一个模块，用户可以根据需要对算法进行修改或者重新开发。

本发明的GPS/SINS组合导航全数字仿真系统，由轨迹发生器模块、IMU仿真器、GPS仿真器、SINS解算模块以及卡尔曼滤波器模块构成。轨迹发生器模块用于产生飞行轨迹数据；IMU仿真器用于模拟陀螺仪和加速度计的实际情况，输出载体的三维角速度信息和三维加速度信息；GPS仿真器模拟GPS接收机，输出载体的伪距、伪距率、位置和速度信息；SINS解算模块将IMU仿真器的输出进行解算获得载体的位置、速度和姿态信息；卡尔曼滤波器模块将GPS和SINS可以按照松散组合和紧密组合两种不同的组合方式，采用Kalman滤波方法进行数据融合。

轨迹发生器模块采用数学方法模拟各种各样的飞行轨迹，比如爬升、平飞、下降等，它提供组合导航仿真系统需要的姿态角、姿态角速率、位置、速度、加速度五组共十五个飞行轨迹数据。

IMU仿真器将轨迹发生器模块提供的姿态角速率和加速度信息作为输入，依据陀螺仪和加速度计的误差模型，加入误差，模拟陀螺仪和加速度计的真实输出，最后将结果输出给SINS解算模块。

GPS仿真器利用软件编程实现GPS的功能，将轨迹发生器提供的位置、速度信息，以及参考卫星星历数据作为输入，输出伪距、伪距率、位置和速度信息给卡尔曼滤波器模块。

SINS解算模块利用IMU仿真器输出的带有误差的姿态角速度和加速度信息，默认采用四元素法(提供用户可编程模块)进行解算，得到载体的位置、速度和姿态信息。

卡尔曼滤波器模块是整个仿真系统的核心模块，它将SINS和GPS的输出按照松散组合和紧密组合两种不同的组合方式，建立误差模型，采用Kalman滤波方法(本发明提供常规卡尔曼滤波、扩展卡尔曼滤波EKF、无轨迹卡尔曼滤波UKF和用户可编程模块)进行融合滤波，获得载体的位置、速度和姿态信息。

下面结合附图和具体实施方式对本发明作进一步的说明。

参见图1，为组合导航全数字仿真系统总体结构框图。本发明的GPS/SINS组合导航全数字仿真系统由轨迹发生器模块、IMU仿真器、GPS仿真器、SINS解算模块以及卡尔曼滤波器模块构成。本发明使用MATLAB作为工具，通过编写m文件和搭建Simulink模块交互的形式实现整个系统的仿真，并且由于MATLAB软件支持将m文件和Simulink模块转化为C语言，后期可以很方便的将算法载入到实际的组合导航系统中。轨迹发生器模块用于产生载体的理想轨迹，通过使用MATLAB软件编写m文件实现；IMU仿真器用于模拟惯性测量单元IMU的实际情况，输出载体的三维角速度信息和三维加速度信息，通过使用MATLAB软件编写m文件实现；GPS仿真器模拟GPS接收机，输出载体的伪距、伪距率、位置和速度信息，通过使用MATLAB软件编写m文件实现；SINS解算模块将IMU仿真器的输出数据进行解算获得载体的位置、速度和姿态信息，通过搭建Simulink模块实现；卡尔曼滤波器模块将GPS和SINS分别按照松散组合和紧密组合两种不同的组合方式，采用Kalman滤波方法进行数据融合，通过搭建Simulink模块实现。

参见图2，为轨迹发生器示意图。轨迹发生器部分产生载体运动轨迹，输出仿真系统需要的姿态角、姿态角速率、位置、速度、加速度五组共十五个飞行轨迹数据。由于该部分涉及速度姿态矩阵等的解算，用MATLAB数值分析上的优势能达到事半功倍的效果，所以轨迹的产生通过编制m文件来实现，并且将生成的数据保存为dat文件，方便后期的仿真过程调用。

参见图3，为IMU仿真器基本结构框图。IMU仿真器的功能是模拟陀螺仪和加速度计的实际输出。实际中的陀螺仪和加速度计受到地球自转和地心重力的影响，因此它们的输出是惯性坐标系下的角速度和加速度信号，而本发明中的轨迹发生器提供的角速度和加速度信号是载体坐标系下的信号，因此要模拟实际的陀螺仪和加速度计，首先需要加入地球自转和地心重力对惯性器件的影响。惯性器件本身也存在一定的误差，本发明中将陀螺仪和加速度计的误差均视为由一阶马尔可夫过程随机误差和白噪声误差组成，而且假定陀螺仪和加速度计三个轴向的误差模型都相同。

陀螺仪的理想输出为：

${\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{ib}}^b={\mathrm{\ω}}_{\mathrm{nb}}^b+C_n^b{\mathrm{\ω}}_{\mathrm{ib}}^b$

陀螺仪的实际输出为：

${\mathrm{\ω}}_{\mathrm{nb}}^b={\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{ib}}^b+\mathrm{\ϵ}$

ε＝ε_b+ε_r+ω_g

其中，

为轨迹发生器的提供的角速度信号；

为导航坐标系到载体坐标系的姿态矩阵，可由轨迹发生器提供的数据计算获得；

为地理坐标系相对于惯性坐标系的地球自转角速度在地理坐标系中的投影；ε_b为随机常数；ε_r为一阶马尔可夫过程随机噪声；ω_g为白噪声。

加速度计的理想输出为：

${\stackrel{\‾}{f}}_{\mathrm{ib}}^b=f_{\mathrm{nb}}^b+C_n^b{f}_{\mathrm{ib}}^b$

加速度计的实际输出为：

$f_{\mathrm{ib}}^b={\stackrel{\‾}{f}}_{\mathrm{ib}}^b+\▿$

$\▿={\▿}_a+{\mathrm{\ω}}_a$

其中，

为轨迹发生器的提供的加速度信号；

为导航坐标系到载体坐标系的姿态矩阵，可由轨迹发生器提供的数据计算获得；

为地理坐标系相对于惯性坐标系的地球自转角速度和重力加速度在地理坐标系中的投影；为一阶马尔可夫过程随机噪声；ω_a为随机白噪声。

参见图4，为GPS仿真器程序流程图。利用某一已知时刻的GPS卫星星历数据对GPS系统赋初值以后，将轨迹发生器提供的位置和速度作为输入，根据流程图的步骤计算出载体的位置、速度、伪距和伪距率信息。由于该部分涉及到大量的矩阵计算和超越方程的求解，采用MATLAB数值分析上的优势能达到事半功倍的效果。该算法通过编写m文件实现，并将输出的数据结果保存为dat格式的文件，以便后期仿真的调用。

参见图5，为SINS解算基本流程图。SINS解算的算法流程如下：首先对IMU仿真器输出的角速度

和加速度数据

进行误差补偿，得到误差补偿后的角速度

和加速度

利用角速度

默认采用四元素法解算姿态矩阵

由于求解四元素要使用

因此，须要先由

求出

这其中又要使用到

解决这个问题可以利用上一时刻的

进行迭代，进而求出当前时刻的

利用上述求得姿态矩阵后，根据矩阵元素与姿态角的对应关系进行姿态解算求出姿态信息；利用

把加速度计的输出

从载体坐标系变换到导航坐标系扣除有害加速度，结合给定的初始位置和速度，通过一次和两次积分就可以分别求得当前速度和位置，完成导航解算。

参见图6，为卡尔曼滤波模块基本流程图。本模块进行数据融合的流程如下：首先选择SINS和GPS数据的组合方式，松散组合或者紧密组合，依据选择的组合方式建立卡尔曼滤波误差模型，然后用户可选择不同的滤波方法(常规卡尔曼滤波、扩展卡尔曼滤波EKF、无轨迹卡尔曼滤波UKF和用户可编程模块)进行融合滤波，最后利用滤波估计出载体的位置、速度和姿态误差并反馈校正SINS，SINS最终输出导航信息。下面以紧密组合的常规卡尔曼滤波为例进行一下详细的说明。

紧密组合采用GPS的伪距观测量、伪距率观测量与根据SINS生成的伪距计算量、伪距率计算量的差值作为卡尔曼滤波器的量测方程，SINS的误差方程作为卡尔曼滤波器的状态方程。

坐标系定义：i表示地心惯性坐标系，e表示地球坐标系，n表示导航坐标系(采用北东地天理坐标系)，b表示载体坐标系。紧耦合卡尔曼滤波器的状态向量为：

$\mathrm{\ΔX}=\left[\begin{array}{ccccccccc}{\mathrm{\φ}}^{\mathrm{nT}}& \mathrm{\δ}{V}_e^{\mathrm{nT}}& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\mathrm{\ϵ}}_b^{\mathrm{bT}}& {\▿}_b^{\mathrm{bT}}& \mathrm{\δ}{t}_u& \mathrm{\δ}{t}_{\mathrm{ru}}\end{array}\right]$

式中，φ^nT为姿态误差，

为速度误差，δL、δλ、δh分别为纬度误差、经度误差、高度误差，

为陀螺常值漂移，

为加速度计常值漂移，δt_u为GPS接收机时钟误差等效的距离，δt_ru为GPS时钟漂移等效的距离变化率。

系统误差模型描述如下：

$\mathrm{\δ}{V}_e^{\mathrm{nT}}=f^n\×{\mathrm{\φ}}^n-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×\mathrm{\δ}{V}_e^{\mathrm{nT}}+V_e^{\mathrm{nT}}\×(2\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ie}}^n+\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{en}}^n)C_n^b{\stackrel{\‾}{\▿}}^b$

式中，V_E、V_N、V_U为载体在导航坐标系下的东北天向速度；L为纬度，h为高度，R_E为地球半径；

为地球自转在导航坐标系下的投影。

状态方程可写为：

式中，W为系统噪声，F为状态方程矩阵。

紧密组合的观测向量为SINS推导的伪距推算值、伪距率推算值与GPS测量的伪距观测值、伪距率观测值之差。数学模型推导如下所示。

设x、y、z为真实的载体坐标，x_I、y_I、z_I为惯导给出的载体坐标，x_i、y_i、z_i为编号i的卫星的坐标。惯导的误差为：Δx＝x_I-x，Δy＝y_I-y，Δz＝z_I-z。

惯导推算的载体到卫星i的伪距为：

${\mathrm{\ρ}}_I^i=\sqrt{{(x_I-x_I)}^2+{(y_I-y_I)}^2+{(z_I-z_I)}^2}$

真实的载体到卫星i的距离为：

$r_i=\sqrt{{(x-x_i)}^2+{(y-y_i)}^2+{(z-z_i)}^2}$

对上式进行泰勒展开，取一阶误差可得：

${\mathrm{\ρ}}_I^i=r_i+\frac{x-x_i}{r_i}\mathrm{\Δx}+\frac{y-y_i}{r_i}\mathrm{\Δy}+\frac{z-z_i}{r_i}\mathrm{\Δz}$

卫星给出的伪距为ρⁱ，满足

ρⁱ＝r_i-δt_u-v_ρi

式中，v_ρi为伪距测量噪声。

伪距观测量为：

$\mathrm{\δ}{\mathrm{\ρ}}^i={\mathrm{\ρ}}_I^i-{\mathrm{\ρ}}^i=\left[\begin{array}{cccc}\frac{x-x_i}{r_i}& \frac{y-y_i}{r_i}& \frac{z-z_i}{r_i}& 1\end{array}\right]\left[\begin{array}{c}\mathrm{\Δx}\\ \mathrm{\Δy}\\ \mathrm{\Δz}\\ \mathrm{\δ}{t}_u\end{array}\right]+v_{\mathrm{\ρi}}$

对r_i求导得：

对求导得：

卫星给出的伪距率

满足：

式中，

为伪距率测量噪声。

伪距率观测量为：

其中：

$B=\left[\begin{array}{ccc}\frac{x-x_i}{r_i}& \frac{y-y_i}{r_i}& \frac{z-z_i}{r_i}\end{array}\right]$

根据上述状态方程和量测方程，进行常规的卡尔曼滤波，估计出对SINS进行反馈校正：

$\stackrel{\‾}{X}=X-\mathrm{\Δ}\hat{X}$

式中，X是SINS反馈矫正前的状态，

是SINS反馈矫正后的状态。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

注：Simulink是MATLAB最重要的组件之一，它提供一个动态系统建模、仿真和综合分析的集成环境，MATLAB是矩阵实验室(Matrix Laboratory)的简称，是美国MathWorks公司出品的商业数学软件，用于算法开发、数据可视化、数据分析以及数值计算的高级技术计算语言和交互式环境。

本发明具有如下效果：

社会效益：此项发明对于中国导航系统研究的发展具有十分重要的意义。该项发明具有国际先进水平，它为科研人员提供了一个很好的理论研究平台，进而有助于提高GPS/SINS组合导航的导航精度，对我国的航空航天事业的发展有着积极的推动作用。

经济效益：GPS/SINS组合导航全数字仿真系统的研究有着重大的经济效益。高精度的GPS和高精度的SINS组合的效果很好，但价格昂贵，因此研制导航精度较高的低成本GPS/SINS组合导航系统已成为一个替代方案，尤其是在民用市场；低成本的GPS/SINS组合导航系统核心问题是如何完善软件算法来提高导航精度；而本发明提供的全数字仿真系统，为科研人员验证组合导航中各种算法的优劣提供了一个很好的平台，能够大大降低软件算法开发与验证的周期和成本，如若能产业化应用，它产生的经济效益将是非常巨大的。

Claims (6)

1.一种GPS/SINS组合导航全数字仿真装置，其特征是，由轨迹发生器模块、IMU仿真器、GPS仿真器、SINS解算模块以及卡尔曼滤波器模块构成；轨迹发生器模块用于产生飞行轨迹数据；IMU仿真器用于模拟陀螺仪和加速度计的实际情况，输出载体的三维角速度信息和三维加速度信息；GPS仿真器模拟GPS接收机，输出载体的伪距、伪距率、位置和速度信息；SINS解算模块将IMU仿真器的输出进行解算获得载体的位置、速度和姿态信息；卡尔曼滤波器模块将GPS和SINS可以按照松散组合和紧密组合两种不同的组合方式，采用Kalman滤波方法进行数据融合。

2.如权利要求1所述装置，其特征是，所述轨迹发生器模块采用数学方法模拟包括爬升、平飞、下降在内的各种各样的飞行轨迹，提供组合导航仿真系统需要的姿态角、姿态角速率、位置、速度、加速度五组共十五个飞行轨迹数据；

IMU仿真器将轨迹发生器模块提供的姿态角速率和加速度信息作为输入，依据陀螺仪和加速度计的误差模型，加入误差，模拟陀螺仪和加速度计的真实输出，最后将结果输出给SINS解算模块；

GPS仿真器利用软件编程实现GPS的功能，将轨迹发生器提供的位置、速度信息，以及参考卫星星历数据作为输入，输出伪距、伪距率、位置和速度信息给卡尔曼滤波器模块；

SINS解算模块利用IMU仿真器输出的带有误差的姿态角速度和加速度信息，默认采用四元素法进行解算，得到载体的位置、速度和姿态信息。

3.一种GPS/SINS组合导航全数字仿真方法，其特征是，使用MATLAB作为工具，通过编写m文件和搭建Simulink模块交互的形式实现仿真；具体为：通过使用MATLAB软件编写m文件实现轨迹发生器模块，用于产生载体的理想轨迹；通过使用MATLAB软件编写m文件实现IMU仿真器，用于模拟惯性测量单元IMU的实际情况，输出载体的三维角速度信息和三维加速度信息；通过使用MATLAB软件编写m文件实现GPS仿真器，模拟GPS接收机，输出载体的伪距、伪距率、位置和速度信息；通过搭建Simulink模块实现SINS解算模块，用于将IMU仿真器的输出数据进行解算获得载体的位置、速度和姿态信息；通过搭建Simulink模块实现卡尔曼滤波器模块，用于将GPS和SINS分别按照松散组合和紧密组合两种不同的组合方式，采用Kalman滤波方法进行数据融合。

4.如权利要求3所述方法，其特征是，所述IMU仿真器是模拟陀螺仪和加速度计的实际输出，将陀螺仪和加速度计的误差均视为由一阶马尔可夫过程随机误差和白噪声误差组成，而且假定陀螺仪和加速度计三个轴向的误差模型都相同：

陀螺仪的理想输出为：

${\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{ib}}^b={\mathrm{\ω}}_{\mathrm{nb}}^b+C_n^b{\mathrm{\ω}}_{\mathrm{ib}}^b$

陀螺仪的实际输出为：

${\mathrm{\ω}}_{\mathrm{nb}}^b={\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{ib}}^b+\mathrm{\ϵ}$

ε＝ε_b+ε_r+ω_g

其中，

为轨迹发生器的提供的角速度信号；为导航坐标系到载体坐标系的姿态矩阵，可由轨迹发生器提供的数据计算获得；

为地理坐标系相对于惯性坐标系的地球自转角速度在地理坐标系中的投影；ε_b为随机常数；ε_r为一阶马尔可夫过程随机噪声；ω_g为白噪声；

加速度计的理想输出为：

${\stackrel{\‾}{f}}_{\mathrm{ib}}^b=f_{\mathrm{nb}}^b+C_n^b{f}_{\mathrm{ib}}^b$

加速度计的实际输出为：

$f_{\mathrm{ib}}^b={\stackrel{\‾}{f}}_{\mathrm{ib}}^b+\▿$

$\▿={\▿}_a+{\mathrm{\ω}}_a$

其中，

为轨迹发生器的提供的加速度信号；

为导航坐标系到载体坐标系的姿态矩阵，可由轨迹发生器提供的数据计算获得；为地理坐标系相对于惯性坐标系的地球自转角速度和重力加速度在地理坐标系中的投影；为一阶马尔可夫过程随机噪声；ω_a为随机白噪声。

5.如权利要求3所述方法，其特征是，SINS解算的算法流程如下：首先对IMU仿真器输出的角速度

和加速度数据进行误差补偿，得到误差补偿后的角速度

和加速度利用角速度

默认采用四元素法解算姿态矩阵

由于求解四元素要使用利用上一时刻的

进行迭代，进而求出当前时刻的

利用上述求得姿态矩阵

后，根据矩阵元素与姿态角的对应关系进行姿态解算求出姿态信息；利用

把加速度计的输出

从载体坐标系变换到导航坐标系

扣除有害加速度，结合给定的初始位置和速度，通过一次和两次积分就可以分别求得当前速度和位置，完成导航解算。

6.如权利要求3所述方法，其特征是，卡尔曼滤波器进行数据融合的流程如下：首先选择SINS和GPS数据的组合方式，松散组合或者紧密组合，依据选择的组合方式建立卡尔曼滤波误差模型，然后选择常规卡尔曼滤波、扩展卡尔曼滤波EKF、无轨迹卡尔曼滤波UKF中的一种滤波方法进行融合滤波，最后利用滤波估计出载体的位置、速度和姿态误差并反馈校正SINS，并最终输出导航信息。

Images