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

摘要
申请专利号：	CN201110322390.0	申请日：	2011.10.21
公开号：	CN102508954A	公开日：	2012.06.20
当前法律状态：	撤回	有效性：	无权
法律详情：	发明专利申请公布后的视为撤回IPC(主分类):G06F 17/50申请公布日:20120620\|\|\|实质审查的生效IPC(主分类):G06F 17/50申请日:20111021\|\|\|公开
IPC分类号：	G06F17/50	主分类号：	G06F17/50
申请人：	天津大学
发明人：	宗群; 吴宏硕; 田栢苓; 侯志宁; 廖海林
地址：	300072 天津市南开区卫津路92号
优先权：
专利代理机构：	天津市北洋有限责任专利代理事务所 12201	代理人：	刘国威
PDF下载：	PDF下载

内容摘要

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

权利要求书

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 仿真器是模拟陀螺仪和加速度计的实际输出，将陀螺仪和加速度计的误差均视为由一阶马尔可夫过程随机误差和白噪声误差组成，而且假定陀螺仪和加速度计三个轴向的误差模型都相同：陀螺仪的理想输出为：陀螺仪的实际输出为： ε ＝ εb+εr+ωg 其中，为轨迹发生器的提供的角速度信号；为导航坐标系到载体坐标系的姿态矩阵，可由轨迹发生器提供的数据计算获得；为地理坐标系相对于惯性坐标系的地球自转角速度在地理坐标系中的投影； εb 为随机常数； εr 为一阶马尔可夫过程随机噪声； ωg 为白噪声； 2 加速度计的理想输出为：加速度计的实际输出为：其中，为轨迹发生器的提供的加速度信号；为导航坐标系到载体坐标系的姿态矩阵，可由轨迹发生器提供的数据计算获得；为地理坐标系相对于惯性坐标系的地球自转角速度和重力加速度在地理坐标系中的投影；为一阶马尔可夫过程随机噪声； ωa 为随机白噪声。
5：如权利要求 3 所述方法，其特征是， SINS 解算的算法流程如下：首先对 IMU 仿真器输出的角速度用角速度刻的和加速度数据进行误差补偿，得到误差补偿后的角速度由于求解四元素要使用把加速度计的输出利用上述求得姿态矩阵和加速度利默认采用四元素法解算姿态矩阵利用上一时进行迭代，进而求出当前时刻的后，根据矩阵元素与姿从载体坐标系变态角的对应关系进行姿态解算求出姿态信息；利用换到导航坐标系扣除有害加速度，结合给定的初始位置和速度，通过一次和两次积分就可以分别求得当前速度和位置，完成导航解算。
6：如权利要求 3 所述方法，其特征是，卡尔曼滤波器进行数据融合的流程如下：首先选择 SINS 和 GPS 数据的组合方式，松散组合或者紧密组合，依据选择的组合方式建立卡尔曼滤波误差模型，然后选择常规卡尔曼滤波、扩展卡尔曼滤波 EKF、无轨迹卡尔曼滤波 UKF 中的一种滤波方法进行融合滤波，最后利用滤波估计出载体的位置、速度和姿态误差并反馈校正 SINS，并最终输出导航信息。

说明书

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 仿真器是模拟陀螺仪和加速度计的实际输出，将陀螺仪和加速度计的误差均视为由一阶马尔可夫过程随机误差和白噪声误差组成，而且假定陀螺仪和加速度计三个轴向的误差模型都相同：
     陀螺仪的理想输出为：
     陀螺仪的实际输出为： ε ＝ εb+εr+ωg 其中，为轨迹发生器的提供的角速度信号；为导航坐标系到载体坐标系的姿态矩阵，可由轨迹发生器提供的数据计算获得；为地理坐标系相对于惯性坐标系的地球自转角速度在地理坐标系中的投影； εb 为随机常数； εr 为一阶马尔可夫过程随机噪声； ωg 为白噪声；
     加速度计的理想输出为：
     5加速度计的实际输出为：CN 102508954 A
     说明书3/8 页其中，为轨迹发生器的提供的加速度信号；为导航坐标系到载体坐标系的姿态矩阵，可由轨迹发生器提供的数据计算获得；为地理坐标系相对于惯性坐标系的地球 ω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 仿真器的功能是模拟陀螺仪和加速度计的实际输出。实际中的陀螺仪和加速度计受到地球自转和地心重力的影响，因此它们的输出是惯性坐标系下的角速度和加速度信号，而本发明中的轨迹发生器提供的角速度和加速度信号是载体坐标系下的信号，因此要模拟实际的陀螺仪和加速度计，首先需要加入地球自转和地心重力对惯性器件的影响。惯性器件本身也存在一定的误差，本发明中将陀螺仪和加速度计的误差均视为由一阶马尔可夫过程随机误差和白噪声误差组成，而且假定陀螺仪和加速度计三个轴向的误差模型都相同。
     陀螺仪的理想输出为：陀螺仪的实际输出为： ε ＝ εb+εr+ωg 其中，为轨迹发生器的提供的角速度信号；为导航坐标系到载体坐标系的姿态矩阵，可由轨迹发生器提供的数据计算获得；为地理坐标系相对于惯性坐标系的地球自转角速度在地理坐标系中的投影； εb 为随机常数； εr 为一阶马尔可夫过程随机噪声； ωg 为白噪声。
     加速度计的理想输出为：
     加速度计的实际输出为：其中，为轨迹发生器的提供的加速度信号；为导航坐标系到载体坐标系的姿态矩阵，可由轨迹发生器提供的数据计算获得；为地理坐标系相对于惯性坐标系的地球自转角速度和重力加速度在地理坐标系中的投影；为一阶马尔可夫过程随机噪声； ω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 表示载体坐标系。紧耦合卡尔曼滤波器的状态向量为：
     式中， φnT 为姿态误差，为速度误差， δL、 δλ、 δh 分别为纬度误差、经度误差、高度误差，为陀螺常值漂移，为加速度计常值漂移， δtu 为 GPS 接收机时钟误差等效的距离， δtru 为 GPS 时钟漂移等效的距离变化率。
     系统误差模型描述如下：
     式中， VE、 VN、 VU 为载体在导航坐标系下的东北天向速度； L 为纬度， h 为高度， RE 为地球半径；为地球自转在导航坐标系下的投影。
     状态方程可写为：
     式中， W 为系统噪声， F 为状态方程矩阵。
     紧密组合的观测向量为 SINS 推导的伪距推算值、伪距率推算值与 GPS 测量的伪距观测值、伪距率观测值之差。数学模型推导如下所示。
     设 x、 y、 z 为真实的载体坐标， xI、 yI、 zI 为惯导给出的载体坐标， xi、 yi、 zi 为编号 i 的卫星的坐标。惯导的误差为： Δx ＝ xI-x， Δy ＝ yI-y， Δz ＝ zI-z。
     惯导推算的载体到卫星 i 的伪距为：
     真实的载体到卫星 i 的距离为：对上式进行泰勒展开，取一阶误差可得：卫星给出的伪距为 ρi，满足 i ρ ＝ ri-δtu-vρi 式中， vρi 为伪距测量噪声。伪距观测量为：
     对 ri 求导得：
     对求导得：
     卫星给出的伪距率满足：式中，为伪距率测量噪声。伪距率观测量为：
     其中：
     根据上述状态方程和量测方程，进行常规的卡尔曼滤波，估计出对 SINS 进行反馈校正：式中， X 是 SINS 反馈矫正前的状态，是 SINS 反馈矫正后的状态。
     本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
     注： Simulink 是 MATLAB 最重要的组件之一，它提供一个动态系统建模、仿真和综合分析的集成环境， MATLAB 是矩阵实验室 (Matrix Laboratory) 的简称，是美国 MathWorks 公司出品的商业数学软件，用于算法开发、数据可视化、数据分析以及数值计算的高级技术计算语言和交互式环境。
     本发明具有如下效果：
     社会效益：此项发明对于中国导航系统研究的发展具有十分重要的意义。该项发明具有国际先进水平，它为科研人员提供了一个很好的理论研究平台，进而有助于提高 GPS/SINS 组合导航的导航精度，对我国的航空航天事业的发展有着积极的推动作用。
     经济效益： GPS/SINS 组合导航全数字仿真系统的研究有着重大的经济效益。高精度的 GPS 和高精度的 SINS 组合的效果很好，但价格昂贵，因此研制导航精度较高的低成本 GPS/SINS 组合导航系统已成为一个替代方案，尤其是在民用市场；低成本的 GPS/SINS 组合导航系统核心问题是如何完善软件算法来提高导航精度；而本发明提供的全数字仿真系统，为科研人员验证组合导航中各种算法的优劣提供了一个很好的平台，能够大大降低软件算法开发与验证的周期和成本，如若能产业化应用，它产生的经济效益将是非常巨大的。

资源描述

《GPS/SINS组合导航全数字仿真方法及装置.pdf》由会员分享，可在线阅读，更多相关《GPS/SINS组合导航全数字仿真方法及装置.pdf（15页珍藏版）》请在专利查询网上搜索。

1、(10)申请公布号 CN 102508954 A (43)申请公布日 2012.06.20 C N 1 0 2 5 0 8 9 5 4 A *CN102508954A* (21)申请号 201110322390.0 (22)申请日 2011.10.21 G06F 17/50(2006.01) (71)申请人天津大学地址 300072 天津市南开区卫津路92号 (72)发明人宗群吴宏硕田栢苓侯志宁廖海林 (74)专利代理机构天津市北洋有限责任专利代理事务所 12201 代理人刘国威 (54) 发明名称 GPS/SINS组合导航全数字仿真方法及装置 (57) 摘要本发明属于组合导航领。

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

3、Int.Cl. 权利要求书2页说明书8页附图4页 (19)中华人民共和国国家知识产权局 (12)发明专利申请权利要求书 2 页说明书 8 页附图 4 页 1/2页 2 1.一种GPS/SINS组合导航全数字仿真装置，其特征是，由轨迹发生器模块、IMU仿真器、GPS仿真器、SINS解算模块以及卡尔曼滤波器模块构成；轨迹发生器模块用于产生飞行轨迹数据；IMU仿真器用于模拟陀螺仪和加速度计的实际情况，输出载体的三维角速度信息和三维加速度信息；GPS仿真器模拟GPS接收机，输出载体的伪距、伪距率、位置和速度信息；SINS解算模块将IMU仿真器的输出进行解算获得载体的位置、速度和姿态信。

4、息；卡尔曼滤波器模块将GPS和SINS可以按照松散组合和紧密组合两种不同的组合方式，采用Kalman 滤波方法进行数据融合。 2.如权利要求1所述装置，其特征是，所述轨迹发生器模块采用数学方法模拟包括爬升、平飞、下降在内的各种各样的飞行轨迹，提供组合导航仿真系统需要的姿态角、姿态角速率、位置、速度、加速度五组共十五个飞行轨迹数据； IMU仿真器将轨迹发生器模块提供的姿态角速率和加速度信息作为输入，依据陀螺仪和加速度计的误差模型，加入误差，模拟陀螺仪和加速度计的真实输出，最后将结果输出给 SINS解算模块； GPS仿真器利用软件编程实现GPS的功能，将轨迹发生器提供的位置、速度信息，以及。

5、参考卫星星历数据作为输入，输出伪距、伪距率、位置和速度信息给卡尔曼滤波器模块； SINS解算模块利用IMU仿真器输出的带有误差的姿态角速度和加速度信息，默认采用四元素法进行解算，得到载体的位置、速度和姿态信息。 3.一种GPS/SINS组合导航全数字仿真方法，其特征是，使用MATLAB作为工具，通过编写m文件和搭建Simulink模块交互的形式实现仿真；具体为：通过使用MATLAB软件编写m 文件实现轨迹发生器模块，用于产生载体的理想轨迹；通过使用MATLAB软件编写m文件实现IMU仿真器，用于模拟惯性测量单元IMU的实际情况，输出载体的三维角速度信息和三维加速度信息；通过使用MAT。

6、LAB软件编写m文件实现GPS仿真器，模拟GPS接收机，输出载体的伪距、伪距率、位置和速度信息；通过搭建Simulink模块实现SINS解算模块，用于将IMU 仿真器的输出数据进行解算获得载体的位置、速度和姿态信息；通过搭建Simulink模块实现卡尔曼滤波器模块，用于将GPS和SINS分别按照松散组合和紧密组合两种不同的组合方式，采用Kalman滤波方法进行数据融合。 4.如权利要求3所述方法，其特征是，所述IMU仿真器是模拟陀螺仪和加速度计的实际输出，将陀螺仪和加速度计的误差均视为由一阶马尔可夫过程随机误差和白噪声误差组成，而且假定陀螺仪和加速度计三个轴向的误差模型都相同：陀螺。

7、仪的理想输出为：陀螺仪的实际输出为： b + r + g 其中，为轨迹发生器的提供的角速度信号；为导航坐标系到载体坐标系的姿态矩阵，可由轨迹发生器提供的数据计算获得；为地理坐标系相对于惯性坐标系的地球自转角速度在地理坐标系中的投影； b 为随机常数； r 为一阶马尔可夫过程随机噪声； g 为白噪声；权利要求书CN 102508954 A 2/2页 3 加速度计的理想输出为：加速度计的实际输出为：其中，为轨迹发生器的提供的加速度信号；为导航坐标系到载体坐标系的姿态矩阵，可由轨迹发生器提供的数据计算获得；为地理坐标系相对于惯性坐标系的地球自转角速度和重力加速度在地理坐标系。

8、中的投影；为一阶马尔可夫过程随机噪声； a 为随机白噪声。 5.如权利要求3所述方法，其特征是，SINS解算的算法流程如下：首先对IMU仿真器输出的角速度和加速度数据进行误差补偿，得到误差补偿后的角速度和加速度利用角速度默认采用四元素法解算姿态矩阵由于求解四元素要使用利用上一时刻的进行迭代，进而求出当前时刻的利用上述求得姿态矩阵后，根据矩阵元素与姿态角的对应关系进行姿态解算求出姿态信息；利用把加速度计的输出从载体坐标系变换到导航坐标系扣除有害加速度，结合给定的初始位置和速度，通过一次和两次积分就可以分别求得当前速度和位置，完成导航解算。 6.如权利要求3所述方法，其特征是，卡尔曼。

9、滤波器进行数据融合的流程如下：首先选择SINS和GPS数据的组合方式，松散组合或者紧密组合，依据选择的组合方式建立卡尔曼滤波误差模型，然后选择常规卡尔曼滤波、扩展卡尔曼滤波EKF、无轨迹卡尔曼滤波UKF中的一种滤波方法进行融合滤波，最后利用滤波估计出载体的位置、速度和姿态误差并反馈校正SINS，并最终输出导航信息。权利要求书CN 102508954 A 1/8页 4 GPS/SINS 组合导航全数字仿真方法及装置技术领域 0001 本发明属于组合导航领域，具体涉及GPS/SINS组合导航全数字仿真方法及装置，对实际的组合导航系统的设计、方案论证有着重要的意义。背景技术。

10、0002 为了获得移动载体的实时位置、速度和姿态信息，多种导航方式已经被提出和采用。其中，以捷联惯性导航系统(Strapdown Inertial Navigation System，SINS)和全球卫星定位系统(Global Positioning System，GPS)的应用最为广泛。 0003 SINS不仅能够提供载体位置、速度参数，还能提供载体的三维姿态参数，是完全自主的导航方式，在航空、航天、航海和陆地等几乎所有领域中都得到了广泛应用。但是，SINS 难以克服的缺点是其导航定位误差随时间累加，难以长时间独立工作。 0004 20世纪末发展起来的GPS具有定位和测速精度高的优势，。

11、且基本上不受时间、地区的限制，已经得到了广泛应用。但是，在卫星信号受到屏蔽或遮挡时，接收机就无法定位。因而，对导航定位可靠性要求较高的情况下GPS很难单独应用。 0005 可以看出，GPS和SINS都存在着自身难以克服的缺点。但是，GPS和SINS又具有很强的互补性，两者的组合不仅可以充分发挥各自的优势，而且随着组合程度的加深GPS/ SINS组合系统的总体性能要远远优于各独立系统。组合系统利用高精度的GPS信息作为外部量测输入，在运动过程中修正SINS，以控制其误差随时间的累积；而短时间内高精度的 SINS定位结果，又可以解决GPS动态环境中的信号失锁和周跳问题。SINS还可以辅助G。

12、PS 接收机增强其抗干扰能力，提高捕获和跟踪卫星信号的能力。因此，GPS/SINS组合系统被认为是目前导航领域最理想的组合方式。 0006 高精度的器件也带来了高价格的问题。根据市场调研，国内目前高精度的惯性测量单元价格在几十万左右，能够提供伪距、伪距率信息而且定位精度到达厘米级别的GPS 的价格也在10万元以上。这在一定程度上，特别是在民用领域，限制了GPS/SINS组合系统的推广应用。因此，低成本的GPS/SINS组合导航系统已成为国内外的研究热点之一。 0007 对于低成本的GPS/SINS组合导航系统，其核心问题是如何通过改善软件算法来提高导航精度。在软件方面，捷联矩阵的更新、。

13、误差建模、卡尔曼滤波等关键技术都会影响最终导航的精度。发明内容 0008 为克服现有技术的不足，提供一种低成本、高精度导航装置及方法，本发明采取的技术方案是，一种GPS/SINS组合导航全数字仿真装置，由轨迹发生器模块、IMU仿真器、GPS 仿真器、SINS解算模块以及卡尔曼滤波器模块构成；轨迹发生器模块用于产生飞行轨迹数据；IMU仿真器用于模拟陀螺仪和加速度计的实际情况，输出载体的三维角速度信息和三维加速度信息；GPS仿真器模拟GPS接收机，输出载体的伪距、伪距率、位置和速度信息； SINS解算模块将IMU仿真器的输出进行解算获得载体的位置、速度和姿态信息；卡尔曼滤说明书CN。

14、 102508954 A 2/8页 5 波器模块将GPS和SINS可以按照松散组合和紧密组合两种不同的组合方式，采用Kalman 滤波方法进行数据融合。 0009 所述轨迹发生器模块采用数学方法模拟包括爬升、平飞、下降在内的各种各样的飞行轨迹，提供组合导航仿真系统需要的姿态角、姿态角速率、位置、速度、加速度五组共十五个飞行轨迹数据； 0010 IMU仿真器将轨迹发生器模块提供的姿态角速率和加速度信息作为输入，依据陀螺仪和加速度计的误差模型，加入误差，模拟陀螺仪和加速度计的真实输出，最后将结果输出给SINS解算模块； 0011 GPS仿真器利用软件编程实现GPS的功能，将轨迹发生器提供的。

15、位置、速度信息，以及参考卫星星历数据作为输入，输出伪距、伪距率、位置和速度信息给卡尔曼滤波器模块； 0012 SINS解算模块利用IMU仿真器输出的带有误差的姿态角速度和加速度信息，默认采用四元素法进行解算，得到载体的位置、速度和姿态信息。 0013 一种GPS/SINS组合导航全数字仿真方法，使用MATLAB作为工具，通过编写m文件和搭建Simulink模块交互的形式实现仿真；具体为：通过使用MATLAB软件编写m文件实现轨迹发生器模块，用于产生载体的理想轨迹；通过使用MATLAB软件编写m文件实现IMU 仿真器，用于模拟惯性测量单元IMU的实际情况，输出载体的三维角速度信息和三维。

16、加速度信息；通过使用MATLAB软件编写m文件实现GPS仿真器，模拟GPS接收机，输出载体的伪距、伪距率、位置和速度信息；通过搭建Simulink模块实现SINS解算模块，用于将IMU仿真器的输出数据进行解算获得载体的位置、速度和姿态信息；通过搭建Simulink模块实现卡尔曼滤波器模块，用于将GPS和SINS分别按照松散组合和紧密组合两种不同的组合方式，采用Kalman滤波方法进行数据融合。 0014 所述IMU仿真器是模拟陀螺仪和加速度计的实际输出，将陀螺仪和加速度计的误差均视为由一阶马尔可夫过程随机误差和白噪声误差组成，而且假定陀螺仪和加速度计三个轴向的误差模型都相同： 0。

17、015 陀螺仪的理想输出为： 0016 0017 陀螺仪的实际输出为： 0018 0019 b + r + g 0020 其中，为轨迹发生器的提供的角速度信号；为导航坐标系到载体坐标系的姿态矩阵，可由轨迹发生器提供的数据计算获得；为地理坐标系相对于惯性坐标系的地球自转角速度在地理坐标系中的投影； b 为随机常数； r 为一阶马尔可夫过程随机噪声； g 为白噪声； 0021 加速度计的理想输出为： 0022 0023 加速度计的实际输出为： 0024 说明书CN 102508954 A 3/8页 6 0025 0026 其中，为轨迹发生器的提供的加速度信号；为导航坐标系到载体坐标系的姿。

18、态矩阵，可由轨迹发生器提供的数据计算获得；为地理坐标系相对于惯性坐标系的地球自转角速度和重力加速度在地理坐标系中的投影；为一阶马尔可夫过程随机噪声； a 为随机白噪声。 0027 SINS解算的算法流程如下：首先对IMU仿真器输出的角速度和加速度数据进行误差补偿，得到误差补偿后的角速度和加速度利用角速度默认采用四元素法解算姿态矩阵由于求解四元素要使用利用上一时刻的进行迭代，进而求出当前时刻的利用上述求得姿态矩阵后，根据矩阵元素与姿态角的对应关系进行姿态解算求出姿态信息；利用把加速度计的输出从载体坐标系变换到导航坐标系扣除有害加速度，结合给定的初始位置和速度，通过一次和两次积分就可。

19、以分别求得当前速度和位置，完成导航解算。 0028 卡尔曼滤波器进行数据融合的流程如下：首先选择SINS和GPS数据的组合方式，松散组合或者紧密组合，依据选择的组合方式建立卡尔曼滤波误差模型，然后选择常规卡尔曼滤波、扩展卡尔曼滤波EKF、无轨迹卡尔曼滤波UKF中的一种滤波方法进行融合滤波，最后利用滤波估计出载体的位置、速度和姿态误差并反馈校正SINS，并最终输出导航信息。 0029 本发明具有如下技术效果： 0030 1、本发明提供的GPS/SINS组合导航全数字仿真系统，可以代替实际的GPS/SINS 组合导航系统，用来验证组合导航中各种软件算法对导航精度的影响和开发新的算法。 00。

20、31 2、本发明对低成本的GPS/SINS组合导航系统如何提高导航精度有着重要的指导作用和现实意义。 0032 3、科研人员可以将验证后的算法和开发的新算法载入到实际的GPS/SINS组合导航系统中，进行实际的应用。 0033 4、本发明还具有成本低廉的特点。附图说明 0034 图1组合导航全数字仿真系统总体结构框图。 0035 图2轨迹发生器示意图。 0036 图3IMU仿真器基本结构框图。 0037 图4GPS仿真器程序流程图。 0038 图5SINS解算基本流程图。 0039 图6卡尔曼滤波模块基本流程图。具体实施方式 0040 本发明的目的在于提供一种GPS/SINS组合导航系。

21、统全数字仿真系统。该仿真系统可以代替实际的GPS/SINS组合导航系统，用来验证组合导航中各种软件算法对导航精度的影响和开发新的算法；同时也可以将这些验证后的算法和开发的新算法载入到实际的 GPS/SINS组合导航系统中进行应用；本发明对低成本的GPS/SINS组合导航系统如何提高说明书CN 102508954 A 4/8页 7 导航精度有着重要的指导作用和现实意义。本发明主要完成以下功能： 0041 1、轨迹发生器提供整个组合导航仿真系统所需要的飞行轨迹数据。 0042 2、IMU仿真器模拟陀螺仪和加速度计的真实输出。 0043 3、GPS仿真器利用软件编程模拟GPS的功能，输出伪。

22、距、伪距率、位置和速度信息。 0044 4、SINS解算模块完成对IMU仿真器输出数据的解算并获得载体的位置、速度和姿态信息；解算算法被集成为模块，用户可以对算法修改或者重新开发，本发明默认采用四元素法作为解算算法。 0045 5、卡尔曼滤波器模块将SINS和GPS的输出数据进行融合滤波，获得载体的位置、速度和姿态信息。本发明提供GPS/SINS的松散组合和紧密组合两种不同方式供用户选择；滤波方法也被集成为一个模块，用户可以根据需要对算法进行修改或者重新开发。 0046 本发明的GPS/SINS组合导航全数字仿真系统，由轨迹发生器模块、IMU仿真器、 GPS仿真器、SINS解算模块以及。

23、卡尔曼滤波器模块构成。轨迹发生器模块用于产生飞行轨迹数据；IMU仿真器用于模拟陀螺仪和加速度计的实际情况，输出载体的三维角速度信息和三维加速度信息；GPS仿真器模拟GPS接收机，输出载体的伪距、伪距率、位置和速度信息；SINS解算模块将IMU仿真器的输出进行解算获得载体的位置、速度和姿态信息；卡尔曼滤波器模块将GPS和SINS可以按照松散组合和紧密组合两种不同的组合方式，采用Kalman 滤波方法进行数据融合。 0047 轨迹发生器模块采用数学方法模拟各种各样的飞行轨迹，比如爬升、平飞、下降等，它提供组合导航仿真系统需要的姿态角、姿态角速率、位置、速度、加速度五组共十五个飞行轨迹数。

24、据。 0048 IMU仿真器将轨迹发生器模块提供的姿态角速率和加速度信息作为输入，依据陀螺仪和加速度计的误差模型，加入误差，模拟陀螺仪和加速度计的真实输出，最后将结果输出给SINS解算模块。 0049 GPS仿真器利用软件编程实现GPS的功能，将轨迹发生器提供的位置、速度信息，以及参考卫星星历数据作为输入，输出伪距、伪距率、位置和速度信息给卡尔曼滤波器模块。 0050 SINS解算模块利用IMU仿真器输出的带有误差的姿态角速度和加速度信息，默认采用四元素法(提供用户可编程模块)进行解算，得到载体的位置、速度和姿态信息。 0051 卡尔曼滤波器模块是整个仿真系统的核心模块，它将SINS。

25、和GPS的输出按照松散组合和紧密组合两种不同的组合方式，建立误差模型，采用Kalman滤波方法(本发明提供常规卡尔曼滤波、扩展卡尔曼滤波EKF、无轨迹卡尔曼滤波UKF和用户可编程模块)进行融合滤波，获得载体的位置、速度和姿态信息。 0052 下面结合附图和具体实施方式对本发明作进一步的说明。 0053 参见图1，为组合导航全数字仿真系统总体结构框图。本发明的GPS/SINS组合导航全数字仿真系统由轨迹发生器模块、IMU仿真器、GPS仿真器、SINS解算模块以及卡尔曼滤波器模块构成。本发明使用MATLAB作为工具，通过编写m文件和搭建Simulink模块交互的形式实现整个系统的仿真，。

26、并且由于MATLAB软件支持将m文件和Simulink模块转化为C语言，后期可以很方便的将算法载入到实际的组合导航系统中。轨迹发生器模块用于产生载体的理想轨迹，通过使用MATLAB软件编写m文件实现；IMU仿真器用于模拟惯性测说明书CN 102508954 A 5/8页 8 量单元IMU的实际情况，输出载体的三维角速度信息和三维加速度信息，通过使用MATLAB 软件编写m文件实现；GPS仿真器模拟GPS接收机，输出载体的伪距、伪距率、位置和速度信息，通过使用MATLAB软件编写m文件实现；SINS解算模块将IMU仿真器的输出数据进行解算获得载体的位置、速度和姿态信息，通过搭建Si。

27、mulink模块实现；卡尔曼滤波器模块将 GPS和SINS分别按照松散组合和紧密组合两种不同的组合方式，采用Kalman滤波方法进行数据融合，通过搭建Simulink模块实现。 0054 参见图2，为轨迹发生器示意图。轨迹发生器部分产生载体运动轨迹，输出仿真系统需要的姿态角、姿态角速率、位置、速度、加速度五组共十五个飞行轨迹数据。由于该部分涉及速度姿态矩阵等的解算，用MATLAB数值分析上的优势能达到事半功倍的效果，所以轨迹的产生通过编制m文件来实现，并且将生成的数据保存为dat文件，方便后期的仿真过程调用。 0055 参见图3，为IMU仿真器基本结构框图。IMU仿真器的功能是模拟陀。

28、螺仪和加速度计的实际输出。实际中的陀螺仪和加速度计受到地球自转和地心重力的影响，因此它们的输出是惯性坐标系下的角速度和加速度信号，而本发明中的轨迹发生器提供的角速度和加速度信号是载体坐标系下的信号，因此要模拟实际的陀螺仪和加速度计，首先需要加入地球自转和地心重力对惯性器件的影响。惯性器件本身也存在一定的误差，本发明中将陀螺仪和加速度计的误差均视为由一阶马尔可夫过程随机误差和白噪声误差组成，而且假定陀螺仪和加速度计三个轴向的误差模型都相同。 0056 陀螺仪的理想输出为： 0057 0058 陀螺仪的实际输出为： 0059 0060 b + r + g 0061 其中，为轨迹发生器的。

29、提供的角速度信号；为导航坐标系到载体坐标系的姿态矩阵，可由轨迹发生器提供的数据计算获得；为地理坐标系相对于惯性坐标系的地球自转角速度在地理坐标系中的投影； b 为随机常数； r 为一阶马尔可夫过程随机噪声； g 为白噪声。 0062 加速度计的理想输出为： 0063 0064 加速度计的实际输出为： 0065 0066 0067 其中，为轨迹发生器的提供的加速度信号；为导航坐标系到载体坐标系的姿态矩阵，可由轨迹发生器提供的数据计算获得；为地理坐标系相对于惯性坐标系的地球自转角速度和重力加速度在地理坐标系中的投影；为一阶马尔可夫过程随机噪声； a 为随机白噪声。 0068 参见图4，为。

30、GPS仿真器程序流程图。利用某一已知时刻的GPS卫星星历数据对 GPS系统赋初值以后，将轨迹发生器提供的位置和速度作为输入，根据流程图的步骤计算出说明书CN 102508954 A 6/8页 9 载体的位置、速度、伪距和伪距率信息。由于该部分涉及到大量的矩阵计算和超越方程的求解，采用MATLAB数值分析上的优势能达到事半功倍的效果。该算法通过编写m文件实现，并将输出的数据结果保存为dat格式的文件，以便后期仿真的调用。 0069 参见图5，为SINS解算基本流程图。SINS解算的算法流程如下：首先对IMU仿真器输出的角速度和加速度数据进行误差补偿，得到误差补偿后的角速度和加速度利。

31、用角速度默认采用四元素法解算姿态矩阵由于求解四元素要使用因此，须要先由求出这其中又要使用到解决这个问题可以利用上一时刻的进行迭代，进而求出当前时刻的利用上述求得姿态矩阵后，根据矩阵元素与姿态角的对应关系进行姿态解算求出姿态信息；利用把加速度计的输出从载体坐标系变换到导航坐标系扣除有害加速度，结合给定的初始位置和速度，通过一次和两次积分就可以分别求得当前速度和位置，完成导航解算。 0070 参见图6，为卡尔曼滤波模块基本流程图。本模块进行数据融合的流程如下：首先选择SINS和GPS数据的组合方式，松散组合或者紧密组合，依据选择的组合方式建立卡尔曼滤波误差模型，然后用户可选择不同的滤。

32、波方法(常规卡尔曼滤波、扩展卡尔曼滤波 EKF、无轨迹卡尔曼滤波UKF和用户可编程模块)进行融合滤波，最后利用滤波估计出载体的位置、速度和姿态误差并反馈校正SINS，SINS最终输出导航信息。下面以紧密组合的常规卡尔曼滤波为例进行一下详细的说明。 0071 紧密组合采用GPS的伪距观测量、伪距率观测量与根据SINS生成的伪距计算量、伪距率计算量的差值作为卡尔曼滤波器的量测方程，SINS的误差方程作为卡尔曼滤波器的状态方程。 0072 坐标系定义：i表示地心惯性坐标系，e表示地球坐标系，n表示导航坐标系(采用北东地天理坐标系)，b表示载体坐标系。紧耦合卡尔曼滤波器的状态向量为： 007。

33、3 0074 式中， nT 为姿态误差，为速度误差，L、h分别为纬度误差、经度误差、高度误差，为陀螺常值漂移，为加速度计常值漂移，t u 为GPS接收机时钟误差等效的距离，t ru 为GPS时钟漂移等效的距离变化率。 0075 系统误差模型描述如下： 0076 0077 0078 0079 0080 0081 说明书CN 102508954 A 7/8页 10 0082 式中，V E 、V N 、V U 为载体在导航坐标系下的东北天向速度；L为纬度，h为高度，R E 为地球半径；为地球自转在导航坐标系下的投影。 0083 状态方程可写为： 0084 0085 式中，W为系统噪声，F为。

34、状态方程矩阵。 0086 紧密组合的观测向量为SINS推导的伪距推算值、伪距率推算值与GPS测量的伪距观测值、伪距率观测值之差。数学模型推导如下所示。 0087 设x、y、z为真实的载体坐标，x I 、y I 、z I 为惯导给出的载体坐标，x i 、y i 、z i 为编号i 的卫星的坐标。惯导的误差为：xx I -x，yy I -y，zz I -z。 0088 惯导推算的载体到卫星i的伪距为： 0089 0090 真实的载体到卫星i的距离为： 0091 0092 对上式进行泰勒展开，取一阶误差可得： 0093 0094 卫星给出的伪距为 i ，满足 0095 i r i -t u -v 。

35、i 0096 式中，v i 为伪距测量噪声。 0097 伪距观测量为： 0098 0099 对r i 求导得： 0100 0101 对求导得： 0102 0103 卫星给出的伪距率满足： 0104 说明书CN 102508954 A 10 8/8页 11 0105 式中，为伪距率测量噪声。 0106 伪距率观测量为： 0107 0108 其中： 0109 0110 0111 根据上述状态方程和量测方程，进行常规的卡尔曼滤波，估计出对SINS进行反馈校正： 0112 0113 式中，X是SINS反馈矫正前的状态，是SINS反馈矫正后的状态。 0114 本发明说明书中未作详细描述的内容属于本。

36、领域专业技术人员公知的现有技术。 0115 注：Simulink是MATLAB最重要的组件之一，它提供一个动态系统建模、仿真和综合分析的集成环境，MATLAB是矩阵实验室(Matrix Laboratory)的简称，是美国MathWorks 公司出品的商业数学软件，用于算法开发、数据可视化、数据分析以及数值计算的高级技术计算语言和交互式环境。 0116 本发明具有如下效果： 0117 社会效益：此项发明对于中国导航系统研究的发展具有十分重要的意义。该项发明具有国际先进水平，它为科研人员提供了一个很好的理论研究平台，进而有助于提高 GPS/SINS组合导航的导航精度，对我国的航空航天事业的。

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

展开阅读全文