CN115900701B - 一种卫导拒止环境下的轨道车组合导航方法及装置 - Google Patents

一种卫导拒止环境下的轨道车组合导航方法及装置 Download PDF

Info

Publication number
CN115900701B
CN115900701B CN202211430464.7A CN202211430464A CN115900701B CN 115900701 B CN115900701 B CN 115900701B CN 202211430464 A CN202211430464 A CN 202211430464A CN 115900701 B CN115900701 B CN 115900701B
Authority
CN
China
Prior art keywords
model
coordinate system
kalman
kalman model
equation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202211430464.7A
Other languages
English (en)
Other versions
CN115900701A (zh
Inventor
张淏酥
苗建明
郑若晗
马成
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Sun Yat Sen University
Original Assignee
Sun Yat Sen University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Sun Yat Sen University filed Critical Sun Yat Sen University
Priority to CN202211430464.7A priority Critical patent/CN115900701B/zh
Publication of CN115900701A publication Critical patent/CN115900701A/zh
Application granted granted Critical
Publication of CN115900701B publication Critical patent/CN115900701B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Navigation (AREA)

Abstract

本发明公开的一种卫导拒止环境下的轨道车组合导航方法及装置，通过惯性导航误差模型和惯性器件零偏的马尔科夫模型，构建卡尔曼模型状态方程；获取所有观测量，将所述观测量进行整合，得到观测向量，根据所述观测向量，构建卡尔曼模型观测方程；基于所述卡尔曼模型状态方程和所述卡尔曼模型观测方程，得到卡尔曼模型，基于所述卡尔曼模型对状态变量进行实时估算和输出校正，得到组合导航算法；该组合导航算法需要的外界量测信息少，大多数观测量为惯性运动本身的特征量，因此，本发明的技术方案可以提高导航精度的量测信息，鲁棒性较好。

Description

一种卫导拒止环境下的轨道车组合导航方法及装置
技术领域
本发明涉及轨道交通导航定位的技术领域，特别是涉及一种卫导拒止环境下的轨道车组合导航方法及装置。
背景技术
导航定位是轨道交通车领域的一个核心技术，目前，该领域常用的导航设备有：基于微型机电系统(MEMS)陀螺的惯性导航系统(INS)、基于中/低精度光纤陀螺的低成本INS、全球导航卫星系统(GNSS，简记为卫导)和里程计(odometer，简记为ODO)；但是由于INS中惯性器件陀螺和加速度计存在固有的零偏和测量噪声等，导航误差会随着时间积累；因此需要GNSS和ODO这类定位或测速设备进行组合导航来抑制误差的增长。
由于轨道车大多数情况下在开阔的地面上行驶，可以接收到GNSS的定位信息，所以轨道车长时间工作在INS/GNSS/ODO或者INS/GNSS组合导航模式下，由于观测信息中包含来自GNSS的位置信息，则可以较准确地估算出卡尔曼模型的状态变量中的姿态角误差、位置误差和速度误差。正是基于此原因，为了降低轨道车导航定位系统的成本，一般选用MEMS－INS和低成本的FOG－INS。然而，有些时候轨道车会在隧道、地下、山区或高楼密集耸立的城市中等非开阔区域中行驶，这类环境会造成GNSS定位误差急剧增大甚至GNSS信号无法被接收到，我们将此类环境统一称为卫导拒止环境；在这类环境中行驶的轨道车，只能工作于INS/ODO组合导航模式；来自ODO的速度观测信息与来自GNSS的位置观测信息有很大的不同，ODO安装在载体坐标系下，仅依靠速度观测信息无法较准确的估算出卡尔曼状态变量中的姿态角误差。所以，该模式的工作时间一旦较长，就会引起姿态角等导航信息误差快速增长。正是因为这个原因，对于长时间工作在卫导拒止环境中的装备，如水下机器人、地下矿井中的自动采矿装备等，只能被迫选择高精度高成本的光纤/激光INS，对于安装低精度INS的轨道车，卫导拒止环境下的高精度组合导航算法是一个棘手问题和瓶颈技术。
目前，提高卫导拒止环境下轨道车导航精度常用的手段是升级设备以便提升INS和/或ODO的测量精度。这种方法会提高导航定位系统的成本，并且依旧无法从本质上解决INS/ODO组合导航模式长时间工作导致的轨道车姿态角误差过大的问题。因此，急需一种对外界信息依赖性弱、鲁棒性好、工程实用性强、可以全面提升轨道车INS/ODO组合导航模式的导航精度的算法。
发明内容
本发明要解决的技术问题是：提供一种卫导拒止环境下的轨道车组合导航方法及装置，可以提高导航精度的量测信息，鲁棒性好。
为了解决上述技术问题，本发明提供了一种卫导拒止环境下的轨道车组合导航方法，包括：
根据惯性导航误差模型和惯性器件零偏的马尔科夫模型，构建卡尔曼模型状态方程；
获取所有观测量，将所述观测量进行整合，得到观测向量，根据所述观测向量，构建卡尔曼模型观测方程；
基于所述卡尔曼模型状态方程和所述卡尔曼模型观测方程，得到卡尔曼模型，基于所述卡尔曼模型对状态变量进行实时估算和输出校正，得到组合导航算法。
在一种可能的实现方式中，获取并根据轨道车的姿态变化率方程、速度变化率方程和位置变化率方程，建立惯性导航误差模型；
其中，轨道车的姿态变化率方程、速度变化率方程和位置变化率方程，如下所示：
式中，为载体坐标系b到导航坐标系n的姿态变换矩阵，为载体坐标系b相对于地心惯性坐标系i的旋转角速度，矢量在载体坐标系b下的投影，为导航坐标系n相对于地心惯性坐标系i的旋转角速度在载体坐标系b下的投影，fb为比力矢量在载体坐标系b下的投影，为地球坐标系e相对于地心惯性坐标系i，的旋转角速度矢量在导航坐标系n下的投影，为导航坐标系n相对于地球坐标系e的旋转角速度矢量在导航坐标系n下的投影，vn为速度矢量在导航坐标系n下的投影，gn为重力加速度矢量在导航坐标系n下的投影，为载体线速度到经纬度角速度的转换矩阵；
所述惯性导航误差模型，如下所示：
式中，为地球坐标系e相对于地心惯性坐标系i的旋转角速度，矢量在导航坐标系n下的投影，φ为姿态角误差矢量，ε为陀螺的零偏，fn为比力矢量在导航坐标系n下的投影，为加速度计的零偏。
在一种可能的实现方式中，获取并根据陀螺零偏方程和加速度计零偏方程，建立性器件零偏的马尔科夫模型；其中，所述惯性器件零偏的马尔科夫模型，如下所示：
式中，τg为陀螺的时间相关常数，wg为陀螺的测量噪声，τa为加速度计的时间相关常数，wa为加速度计的测量噪声。
在一种可能的实现方式中，根据惯性导航误差模型和惯性器件零偏的马尔科夫模型，构建卡尔曼模型状态方程，具体包括：
根据所述惯性导航误差模型和所述惯性器件零偏的马尔科夫模型，得到时间连续卡尔曼模型状态方程，其中，所述时间连续卡尔曼模型状态方程，如下所示：
式中，FSINS为时间连续卡尔曼模型的状态转移矩阵，X为时间连续卡尔曼模型的状态变量，W为时间连续卡尔曼模型的系统噪声矩阵；
对所述时间连续卡尔曼模型状态方程进行离散化处理，得到卡尔曼模型状态方程，其中，所述卡尔曼模型状态方程，如下所示：
Xk＝Φk，k-1·Xk-1+Wk-1；
式中，Xk为卡尔曼模型的状态变量，Φk，k-1为卡尔曼模型的状态转移矩阵，Wk-1为离散卡尔曼模型的系统噪声矩阵。
在一种可能的实现方式中，获取所有观测量，其中，所述所有观测量包括天向速度误差、横向速度误差、前向速度误差、向心加速度误差、天向陀螺零偏、向心角加速度差值误差。
在一种可能的实现方式中，根据所述观测向量，构建卡尔曼模型观测方程，具体包括：
根据所述观测向量，构建时间连续卡尔曼模型观测方程，其中，所述时间连续卡尔曼模型观测方程如下所示：
Z＝H·X+V；
式中，Z为时间连续卡尔曼模型的量测矢量，V为时间连续卡尔曼模型的量测噪声，H为时间连续卡尔曼模型的量测矩阵；
对所述时间连续卡尔曼模型观测方程进行离散化处理，得到卡尔曼模型观测方程，其中，所述卡尔曼模型观测方程，如下所示：
Zk＝Hk·Xk+Vk；
式中，Zk为卡尔曼模型的量测向量，Hk为卡尔曼模型的量测矩阵，Vk为卡尔曼模型的量测噪声。
在一种可能的实现方式中，基于所述卡尔曼模型状态方程和所述卡尔曼模型观测方程，得到卡尔曼模型，其中，所述卡尔曼模型如下所示：
本发明还提供了一种卫导拒止环境下的轨道车组合导航装置，包括：卡尔曼模型状态方程构建模块、卡尔曼模型观测方程构建模块和卡尔曼模型构建模块；
其中，所述卡尔曼模型状态方程构建模块，用于根据惯性导航误差模型和惯性器件零偏的马尔科夫模型，构建卡尔曼模型状态方程；
所述卡尔曼模型观测方程构建模块，用于获取所有观测量，将所述观测量进行整合，得到观测向量，根据所述观测向量，构建卡尔曼模型观测方程；
所述卡尔曼模型构建模块，用于基于所述卡尔曼模型状态方程和所述卡尔曼模型观测方程，得到卡尔曼模型，基于所述卡尔曼模型对状态变量进行实时估算和输出校正，得到组合导航算法。
在一种可能的实现方式中，所述卡尔曼模型状态方程构建模块，用于获取并根据轨道车的姿态变化率方程、速度变化率方程和位置变化率方程，建立惯性导航误差模型；
其中，轨道车的姿态变化率方程、速度变化率方程和位置变化率方程，如下所示：
式中，为载体坐标系b到导航坐标系n的姿态变换矩阵，为载体坐标系b相对于地心惯性坐标系i的旋转角速度，矢量在载体坐标系b下的投影，为导航坐标系n相对于地心惯性坐标系i的旋转角速度在载体坐标系b下的投影，fb为比力矢量在载体坐标系b下的投影，为地球坐标系e相对于地心惯性坐标系i，的旋转角速度矢量在导航坐标系n下的投影，为导航坐标系n相对于地球坐标系e的旋转角速度矢量在导航坐标系n下的投影，vn为速度矢量在导航坐标系n下的投影，gn为重力加速度矢量在导航坐标系n下的投影，为载体线速度到经纬度角速度的转换矩阵；
所述惯性导航误差模型，如下所示：
式中，为地球坐标系e相对于地心惯性坐标系i的旋转角速度，矢量在导航坐标系n下的投影，φ为姿态角误差矢量，ε为陀螺的零偏，fn为比力矢量在导航坐标系n下的投影，为加速度计的零偏。
在一种可能的实现方式中，所述卡尔曼模型状态方程构建模块，用于获取并根据陀螺零偏方程和加速度计零偏方程，建立性器件零偏的马尔科夫模型；其中，所述惯性器件零偏的马尔科夫模型，如下所示：
式中，τg为陀螺的时间相关常数，wg为陀螺的测量噪声，τa为加速度计的时间相关常数，wa为加速度计的测量噪声。
在一种可能的实现方式中，所述卡尔曼模型状态方程构建模块，用于根据惯性导航误差模型和惯性器件零偏的马尔科夫模型，构建卡尔曼模型状态方程，具体包括：
根据所述惯性导航误差模型和所述惯性器件零偏的马尔科夫模型，得到时间连续卡尔曼模型状态方程，其中，所述时间连续卡尔曼模型状态方程，如下所示：
式中，FSINS为时间连续卡尔曼模型的状态转移矩阵，X为时间连续卡尔曼模型的状态变量，W为时间连续卡尔曼模型的系统噪声矩阵；
对所述时间连续卡尔曼模型状态方程进行离散化处理，得到卡尔曼模型状态方程，其中，所述卡尔曼模型状态方程，如下所示：
Xk＝Φk，k-1·Xk-1+Wk-1；
式中，Xk为卡尔曼模型的状态变量，Φk，k-1为卡尔曼模型的状态转移矩阵，Wk-1为离散卡尔曼模型的系统噪声矩阵。
在一种可能的实现方式中，所述卡尔曼模型观测方程构建模块，用于获取所有观测量，其中，所述所有观测量包括天向速度误差、横向速度误差、前向速度误差、向心加速度误差、天向陀螺零偏、向心角加速度差值误差。
在一种可能的实现方式中，所述卡尔曼模型观测方程构建模块，用于根据所述观测向量，构建卡尔曼模型观测方程，具体包括：
根据所述观测向量，构建时间连续卡尔曼模型观测方程，其中，所述时间连续卡尔曼模型观测方程如下所示：
Z＝H·X+V；
式中，Z为时间连续卡尔曼模型的量测矢量，V为时间连续卡尔曼模型的量测噪声，H为时间连续卡尔曼模型的量测矩阵；
对所述时间连续卡尔曼模型观测方程进行离散化处理，得到卡尔曼模型观测方程，其中，所述卡尔曼模型观测方程，如下所示：
Zk＝Hk·Xk+Vk；
式中，Zk为卡尔曼模型的量测向量，Hk为卡尔曼模型的量测矩阵，Vk为卡尔曼模型的量测噪声。
在一种可能的实现方式中，所述卡尔曼模型构建模块，用于基于所述卡尔曼模型状态方程和所述卡尔曼模型观测方程，得到卡尔曼模型，其中，所述卡尔曼模型如下所示：
本发明还提供了一种终端设备，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时实现如上述任意一项所述的卫导拒止环境下的轨道车组合导航方法。
本发明还提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行如上述任意一项所述的卫导拒止环境下的轨道车组合导航方法。
本发明实施例一种卫导拒止环境下的轨道车组合导航方法及装置，与现有技术相比，具有如下有益效果：
通过惯性导航误差模型和惯性器件零偏的马尔科夫模型，构建卡尔曼模型状态方程；获取所有观测量，将所述观测量进行整合，得到观测向量，根据所述观测向量，构建卡尔曼模型观测方程；基于所述卡尔曼模型状态方程和所述卡尔曼模型观测方程，得到卡尔曼模型，基于所述卡尔曼模型对状态变量进行实时估算和输出校正，得到组合导航算法；该组合导航算法需要的外界量测信息少，大多数观测量为惯性运动本身的特征量，因此，本发明的技术方案可以提高导航精度的量测信息，鲁棒性较好。
附图说明
图1是本发明提供的一种卫导拒止环境下的轨道车组合导航方法的一种实施例的流程示意图；
图2是本发明提供的一种卫导拒止环境下的轨道车组合导航装置的一种实施例的结构示意图；
图3是本发明提供的一种实施例的轨道车载体坐标系的示意图；
图4是本发明提供的一种实施例的轨道车直线行驶示意图；
图5是本发明提供的一种实施例的轨道车转弯示意图；
图6是本发明提供的一种实施例的车体转弯时绕ozb轴的旋转角速度示意图；
图7是本发明提供的一种卫导拒止环境下的轨道车组合导航方法的一种实施例的又一流程示意图。
具体实施方式
下面将结合本发明中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
首先对本发明各公式所涉及的坐标系的中文释义进行说明：
载体坐标系b(右-前-上轴对应oxb，oyb，ozb轴)：与轨道车载体坐标系的右-前-上轴一致，坐标原点为车体的旋转中心，如图3所示，图3是轨道车载体坐标系的示意图；旋转中心为车体转弯时的中心点，轨道车的右方、前方和上方分别是x，y，z轴，也就是oxb，oyb，ozb轴。
导航坐标系n(东-北-天轴对应oxn，oyn，ozn轴)：坐标原点为车体的旋转中心。
地球坐标系e：oxe轴在赤道平面内且指向本初子午线，oze轴平行于地球自转轴，oye由右手坐标系确定，坐标原点为地球的球心。有时也将该坐标系平移至原点与载体坐标系b或导航坐标系n的原点重合。
地心惯性坐标系i：oxi轴在赤道平面内且指向春分点，ozi轴平行于地球自转轴，oyi由右手坐标系确定，坐标原点为地球的球心，有时也将该坐标系平移至原点与载体坐标系b或导航坐标系n的原点重合。
对本发明各公式所涉及的变量的中文释义进行说明：
P：载体的位置矢量；其中，L、λ、h分别为载体所处的纬度、经度和高度；
v：速度矢量，包含三个分量，加上标n和b分别表示在导航坐标系n和载体坐标系b下的投影，vb包含x，y，z三个分量，vn包含E(东)，N(北)，U(天)三个分量，其它变量加上标n和b的含义类似；
f：比力矢量，包含三个分量，加上标n和b分别表示在导航坐标系n和载体坐标系b下的投影，fb包含x，y，z三个分量，就是三个加速度计的测量值；
g：重力加速度矢量，包含三个分量，加上标n和b分别表示在导航坐标系n和载体坐标系b下的投影；
ωie：地球坐标系e相对于地心惯性坐标系i，的旋转角速度矢量，包含三个分量，加上标n和b分别表示在导航坐标系n和载体坐标系b下的投影；
ωen：导航坐标系n相对于地球坐标系e的旋转角速度矢量，包含三个分量，加上标n和b分别表示在导航坐标系n和载体坐标系b下的投影；
ωib：载体坐标系b相对于地心惯性坐标系i的旋转角速度矢量，包含三个分量，加上标n和b分别表示在导航坐标系n和载体坐标系b下的投影，包含x，y，z三个分量，就是三个MEMS陀螺的测量值；
ωin：导航坐标系n相对于地心惯性坐标系i的旋转角速度，矢量包含三个分量，加上标n和b分别表示在导航坐标系n和载体坐标系b下的投影；
载体坐标系到导航坐标系的姿态变换矩阵；
载体线速度到经纬度角速度的转换矩阵；
ε：陀螺的零偏，即陀螺测量误差；
加速度计的零偏，即加速度计测量误差；
τg：陀螺的时间相关常数，本专利将其近似为∞；
τa：加速度计的时间相关常数，本专利将其近似为∞；
wg：陀螺的测量噪声，当作高斯白噪声处理；
wa：陀螺的测量噪声，当作高斯白噪声处理；
导航系统计算出的位置矢量(包含误差矢量δP)；
导航系统计算出的速度矢量(包含误差矢量δv)；
δP：位置误差矢量，等于
δv：速度误差矢量，等于
φ：姿态角误差矢量，也叫失准角矢量，包含三个分量，φ＝[φE φN φU]T；
导航系统计算出的姿态矩阵(包含姿态角误差)，与的关系为：
ρ：轨道车的转弯半径。当轨道车沿直线行驶时，ρ取值为∞即可；
轨道车拐弯时绕自身ozb轴的旋转角速度，有关系式
为轨道车的前进速度，取0D0的测量结果即可；
ac：轨道车拐弯时的向心加速度，有关系式
J：轨道车的向心加速度差值，定义为其理论值为0。
X：时间连续卡尔曼模型的状态变量，该矢量包含的变量为
FSINS：时间连续卡尔曼模型的状态转移矩阵；
W：时间连续卡尔曼模型的系统噪声矩阵；
Z：时间连续卡尔曼模型的量测矢量；
H：时间连续卡尔曼模型的量测矩阵；
V：时间连续卡尔曼模型的量测噪声；
Xk：离散卡尔曼模型的状态变量，有关系式Xk＝X(tk)，tk为第k个时刻；
Φk，k-1：离散卡尔曼模型的状态转移矩阵，对FSINS进行离散化计算得到；
Wk-1：离散卡尔曼模型的系统噪声矩阵，有关系式Wk＝W(tk)；
Zk：离散卡尔曼模型的量测向量，有关系式Zk＝Z(tk)；
Hk：离散卡尔曼模型的量测矩阵，有关系式Hk＝H(tk)；
Vk：离散卡尔曼模型的量测噪声，有关系式Vk＝V(tk)。
实施例1
参见图1，图1是本发明提供的一种卫导拒止环境下的轨道车组合导航方法的一种实施例的流程示意图，如图1所示，该方法包括步骤101-步骤103，具体如下：
步骤101：根据惯性导航误差模型和惯性器件零偏的马尔科夫模型，构建卡尔曼模型状态方程。
一实施例中，获取轨道车的姿态变化率方程、速度变化率方程和位置变化率方程；其中，轨道车的姿态变化率方程、速度变化率方程和位置变化率方程，如下所示：
式中，为载体坐标系b到导航坐标系n的姿态变换矩阵，为载体坐标系b相对于地心惯性坐标系i的旋转角速度，矢量在载体坐标系b下的投影，为导航坐标系n相对于地心惯性坐标系i的旋转角速度在载体坐标系b下的投影，fb为比力矢量在载体坐标系b下的投影，为地球坐标系e相对于地心惯性坐标系i，的旋转角速度矢量在导航坐标系n下的投影，为导航坐标系n相对于地球坐标系e的旋转角速度矢量在导航坐标系n下的投影，vn为速度矢量在导航坐标系n下的投影，gn为重力加速度矢量在导航坐标系n下的投影，为载体线速度到经纬度角速度的转换矩阵；
一实施例中，矩阵是由向量构成的反对称矩阵。
一实施例中，的表达式，如下所示：
式中，RM是地球子午圈主曲率半径；RN是卯酉圈主曲率半径；sec(·)是正切函数；L是纬度；h是轨道车所在的海拔高度，大多数情况下，轨道车在零海拔附近的平原地区运行，而不在高海拔地区、高原、山地和盆地等地区行驶，所以h＝0。
一实施例中，对获取的轨道车的姿态变化率方程、速度变化率方程和位置变化率方程进行推导，可得惯性导航误差模型，其中，所述惯性导航误差模型，如下所示：
式中，为地球坐标系e相对于地心惯性坐标系i的旋转角速度，矢量在导航坐标系n下的投影，φ为姿态角误差矢量，ε为陀螺的零偏，fn为比力矢量在导航坐标系n下的投影，为加速度计的零偏。
一实施例中，将陀螺零偏和加速度计的零偏变化简化看作一阶马尔科夫过程；具体的，获取并根据陀螺零偏方程和加速度计零偏方程，建立性器件零偏的马尔科夫模型；其中，所述惯性器件零偏的马尔科夫模型，如下所示：
式中，τg为陀螺的时间相关常数，wg为陀螺的测量噪声，τa为加速度计的时间相关常数，wa为加速度计的测量噪声，且τg和τa近似当作∞处理。
一实施例中，根据所述惯性导航误差模型和所述惯性器件零偏的马尔科夫模型，得到时间连续卡尔曼模型状态方程，其中，所述时间连续卡尔曼模型状态方程，如下所示：
式中，FSINS为时间连续卡尔曼模型的状态转移矩阵，X为时间连续卡尔曼模型的状态变量，W为时间连续卡尔曼模型的系统噪声矩阵。
一实施例中，时间连续卡尔曼模型的状态变量
一实施例中，FSINS是一个分块矩阵，如下所示：
式中，Mab(a/b为v，φ和p中的一个)代表从FSINS中变量a到b的变换子矩阵。FSINS包含的具体元素属于惯性导航领域的基础知识，在这里不再赘述。
一实施例中，对所述时间连续卡尔曼模型状态方程进行离散化处理，得到离散化卡尔曼模型状态方程，并将所述离散化卡尔曼模型状态方程作为卡尔曼模型状态方程，其中，所述卡尔曼模型状态方程，如下所示：
Xk＝Φk，k-1·Xk-1+Wk-1；
式中，Xk为卡尔曼模型的状态变量，Φk，k-1为卡尔曼模型的状态转移矩阵，Wk-1为离散卡尔曼模型的系统噪声矩阵。
需要说明的是，离散化的方法包括但不限于为等宽离散化、等频离散化、聚类离散化，还可以根据数据的特征及意义设置离散的规则。
步骤102：获取所有观测量，将所述观测量进行整合，得到观测向量，根据所述观测向量，构建卡尔曼模型观测方程。
一实施例中，所有观测量包括天向速度误差、横向速度误差、前向速度误差、向心加速度误差、天向陀螺零偏、向心角加速度差值误差。
具体的，对于天向速度误差和横向速度误差，如下所示：
式中，为导航坐标系n到载体坐标系b的转换矩阵计算值；为导航坐标系n下速度矢量的计算值；中的下标x或z代表矢量的x或z分量；的x和y两分量的计算结果都是略微偏离理论值0的小量。上式的推导中用到了由于轨道车被约束在轨道平面上运动，所以，有
上式中和分别是和的虚拟噪声。由于观测量中一般都包含实际仪器设备的测量结果，所以需要利用量测噪声项进行滤波估算。但是此处的量测量比较特殊，不包含任何真实仪器设备的测量结果。为了使得这种特殊的观测量也具有包含真实仪器设备量测噪声的特性，本专利引入虚拟噪声技术，它可以进一步提高组合导航的精度。
虚拟噪声的具体计算方法，如下所示：
式中的虚拟噪声观测量和是零均值、方差分别为的正态分布的随机噪声，其中，方差大小根据里程计的量测噪声特性确定。R1、R2为独立存在的在(0，1)均匀分布的随机变量，由计算机利用随机数函数产生。
具体的，对于前向速度误差，如下所示：
式中，是里程计测量的轨道车前进速度。由于该前向速度误差包含了实际仪器设备的测量噪声，所以不用再添加虚拟噪声项。
具体的，对于向心加速度误差、天向陀螺零偏、向心角加速度差值误差。
轨道车直线行驶与转弯的示意图分别见图4和图5，其中，图4为轨道车直线行驶示意图，图5为轨道车转弯示意图；当轨道车直线行驶时，当相于转弯半径ρ取∞的情况。当轨道车转弯行驶时，ρ为一个有限值。
车体转弯时绕ozb轴的旋转角速度示意图见图6，如图6所示，M和M′分别是上一时刻和下一时刻的轨道车的旋转中心，一般与轨道车的重心重合；O点是弯道的旋转中心，O点与轨道车的M点之间的距离就是转弯半径ρ；假设两个时刻的时间间隔记为dt，图中的ds和分别是旋转中心的位移和轨道车航向角的变化值；由和dt可以计算旋转角速度并且可以导出与的关系，如下所示：
对于轨道车通过测绘、施工图等先验知识很容易知道整个路程中所有弯道的转弯半径。而且，轨道车的运行是有时刻表的，什么时候进入什么弯道是确定的。即使真实时刻表相比计划好的时刻表有变化，轨道车也是可以及时更新得到最新的时刻表。我们只需要记录卫导拒止的若干段轨道的转弯半径和相应的时间戳信息，当需要启用该算法时，从存储的数据中调用相应的转弯半径信息即可。即使因为测绘误差、轨道车运行时刻表不准确等原因造成输入给卡尔曼滤波器的ρ值和真实ρ值有一定偏差，卡尔曼滤波器依然可以保持稳定和一定的精度。因此，轨道车的ρ值是一个可以利用的已知量用于提高组合导航精度。
定义向心加速度差值J，如下所示：
式中，J＝0；由的定义可知：
由于地球自转很慢，相比是个小量，相差12个数量级；且由于轨道车的运动速度除以地球半径是个很小的角速度，所以相比也是个小量。因此，相比是个小量，因此，有如下关系式：
ac、的计算值分别为：
上述为包含量测噪声的z轴陀螺实际测量值，如下面表达式所示：
基于上述公式，可得旋转角速度误差观测矢量就是天向陀螺零偏：
由前面的分析，则可得出观测量的计算方法，如下所示：
式中，和分别是天向陀螺和里程计的测量值，ρ是已知量，所以可以计算出
基于向心加速度与的关系，可知：基于ac和可得：向心加速度误差观测量，如下所示：
对于向心角加速度差值误差，引入向心加速度差值J的计算值如下所示：
基于向心加速度差值J和计算值计算向心角加速度差值误差δJ，如下所示：
使用的反馈校正技术可以保证接近所以用上式计算δJ可以保证较高的精度。
将上述若干个的观测量进行集合，得到观测向量Z，其中，所述观测向量如下所示：
一实施例中，根据所述观测向量，构建时间连续卡尔曼模型观测方程，其中，所述时间连续卡尔曼模型观测方程如下所示：
Z＝H·X+V；
式中，Z为时间连续卡尔曼模型的量测矢量，V为时间连续卡尔曼模型的量测噪声，H为时间连续卡尔曼模型的量测矩阵。
一实施例中，对所述时间连续卡尔曼模型观测方程进行离散化处理，得到卡尔曼模型观测方程，其中，所述卡尔曼模型观测方程，如下所示：
Zk＝Hk·Xk+Vk；
式中，Zk为卡尔曼模型的量测向量，Hk为卡尔曼模型的量测矩阵，Vk为卡尔曼模型的量测噪声。
一实施例中，基于上述公式推导建立Z和X的关系，具体的，先计算得到δJ与X之间的关系：
不考虑加速度计随机游走，只考虑其零偏噪声的情况下，式中的就是加速度计零偏，如下式所示：
将代入δJ与X之间的关系式中就得到了δJ与X之间的关系。并将δJ与X之间的关系式中与δac相关的量保留，就可以推导出δac与X之间的关系式，如下所示：
由上面的推导结合矢量δvb与X中δvn的关系、与X的关系可以推导出H和Hk，其中，Hk的表达式如下：
式中，A3×15由的元素构成，表达式如下：
N1×15的表达式如下所示：
M1×15的表达式如下所示：
M1×15＝
[M(1，1) M(1，2) M(1，3) M(1，4) M(1，5) M(1，6) 0 0 0 0 0 M(1，12) M(1，13)0 0]；
S1×15的表达式，如下所示：
S1×15＝[0 0 0 0 0 0 0 0 0 0 0 1 0 0 0]。
步骤103：基于所述卡尔曼模型状态方程和所述卡尔曼模型观测方程，得到卡尔曼模型，基于所述卡尔曼模型对状态变量进行实时估算和输出校正，得到组合导航算法。
一实施例中，基于所述卡尔曼模型状态方程和所述卡尔曼模型观测方程，得到卡尔曼模型，其中，所述卡尔曼模型如下所示：
式中，Φk，k-1由矩阵FSINS离散化计算得到，属于惯导领域的基础知识，这里不再赘述。
一实施例中，根据卡尔曼模型递推算法对状态变量Xk进行估算，具体的，对所述状态变量进行状态一步预测、状态一步预测均方误差阵的计算、滤波增益、状态估计和状态估计均方误差阵的计算，以使重新列写离散时间系统的Kalman滤波方程，递推计算过程如下所示：
状态一步预测：
状态一步预测均方误差阵的计算：
滤波增益：
状态估计：
状态估计均方误差阵：
Pk＝(I-KkHk)Pk，k-1。
一实施例中，在递推过程加上均输出校正技术，实现对状态变量的输出校正，就可以完成整套组合导航算法。如图7所示，图7是本发明提供的一种卫导拒止环境下的轨道车组合导航方法的一种实施例的又一流程示意图，通过将观测量输入到构建好的卡尔曼模型中，得到多个估计的状态变量，并基于状态变量对构建好的卡尔曼模型进行反馈校正。
作为本实施例中的一种举例说明：利用仿真技术对该算法的导航精度进行了验证，首先利用轨迹生成技术生成一条轨迹(轨迹上每点的位置、速度、加速度、轨道车的姿态角、姿态角变化速度等都是假定的已知量，在轨迹上的运行时间假定为240s)，该轨迹由一段直线加一段弯道加一段直线和一段弯道构成。两段弯道的转弯半径都是已知的。然后，由这条轨迹生成理想的IMU和ODO数据，再对理想的IMU数据注入IMU和ODO误差。注入的IMU误差的大小取自Invensense公司的MPU6050型号的MEMS－IMU，以模拟该型号IMU的组合导航精度。注入的ODO误差取自常用的某款轨道车里程计的指标，以模拟真实的轨道车导航情形。然后利用这组生成的实验数据进行组合导航仿真计算，分别使用航位推算法、常规的组合导航方法(仅用这一个观测量)和本专利方法进行仿真。最后将三组计算结果与标准的已知位置值对比得到定位误差，定位误差除以轨迹的长度得到相对误差。三种方法的相对定位误差分别为83.4％、18.9％和4.3％左右。该结果表明本方法对轨道车在卫导拒止环境下的组合导航误差有一定的抑制效果。一般地面轨道车在卫导拒止环境下的连续运行时间很少超过240s。所以，该方法的工程实用性强，有望得到实际的工程应用。
实施例2
参见图2，图2是本发明提供的一种卫导拒止环境下的轨道车组合导航装置的一种实施例的结构示意图，如图2所示，该装置包括卡尔曼模型状态方程构建模块201、卡尔曼模型观测方程构建模块202和卡尔曼模型构建模块203，具体如下：
所述卡尔曼模型状态方程构建模块201，用于根据惯性导航误差模型和惯性器件零偏的马尔科夫模型，构建卡尔曼模型状态方程。
所述卡尔曼模型观测方程构建模块202，用于获取所有观测量，将所述观测量进行整合，得到观测向量，根据所述观测向量，构建卡尔曼模型观测方程。
所述卡尔曼模型构建模块203，用于基于所述卡尔曼模型状态方程和所述卡尔曼模型观测方程，得到卡尔曼模型，基于所述卡尔曼模型对状态变量进行实时估算和输出校正，得到组合导航算法。
一实施例中，所述卡尔曼模型状态方程构建模块201，用于获取并根据轨道车的姿态变化率方程、速度变化率方程和位置变化率方程，建立惯性导航误差模型；
其中，轨道车的姿态变化率方程、速度变化率方程和位置变化率方程，如下所示：
式中，为载体坐标系b到导航坐标系n的姿态变换矩阵，为载体坐标系b相对于地心惯性坐标系i的旋转角速度，矢量在载体坐标系b下的投影，为导航坐标系n相对于地心惯性坐标系i的旋转角速度在载体坐标系b下的投影，fb为比力矢量在载体坐标系b下的投影，为地球坐标系e相对于地心惯性坐标系i，的旋转角速度矢量在导航坐标系n下的投影，为导航坐标系n相对于地球坐标系e的旋转角速度矢量在导航坐标系n下的投影，vn为速度矢量在导航坐标系n下的投影，gn为重力加速度矢量在导航坐标系n下的投影，为载体线速度到经纬度角速度的转换矩阵；
所述惯性导航误差模型，如下所示：
式中，为地球坐标系e相对于地心惯性坐标系i的旋转角速度，矢量在导航坐标系n下的投影，φ为姿态角误差矢量，ε为陀螺的零偏，fn为比力矢量在导航坐标系n下的投影，为加速度计的零偏。
一实施例中，所述卡尔曼模型状态方程构建模块201，用于获取并根据陀螺零偏方程和加速度计零偏方程，建立性器件零偏的马尔科夫模型；其中，所述惯性器件零偏的马尔科夫模型，如下所示：
式中，τg为陀螺的时间相关常数，wg为陀螺的测量噪声，τa为加速度计的时间相关常数，wa为加速度计的测量噪声。
一实施例中，所述卡尔曼模型状态方程构建模块201，用于根据惯性导航误差模型和惯性器件零偏的马尔科夫模型，构建卡尔曼模型状态方程，具体包括：
根据所述惯性导航误差模型和所述惯性器件零偏的马尔科夫模型，得到时间连续卡尔曼模型状态方程，其中，所述时间连续卡尔曼模型状态方程，如下所示：
式中，FSINS为时间连续卡尔曼模型的状态转移矩阵，X为时间连续卡尔曼模型的状态变量，W为时间连续卡尔曼模型的系统噪声矩阵；
对所述时间连续卡尔曼模型状态方程进行离散化处理，得到卡尔曼模型状态方程，其中，所述卡尔曼模型状态方程，如下所示：
Xk＝Φk，k-1·Xk-1+Wk-1；
式中，Xk为卡尔曼模型的状态变量，Φk，k-1为卡尔曼模型的状态转移矩阵，Wk-1为离散卡尔曼模型的系统噪声矩阵。
一实施例中，所述卡尔曼模型观测方程构建模块202，用于获取所有观测量，其中，所述所有观测量包括天向速度误差、横向速度误差、前向速度误差、向心加速度误差、天向陀螺零偏、向心角加速度差值误差。
一实施例中，所述卡尔曼模型观测方程构建模块202，用于根据所述观测向量，构建卡尔曼模型观测方程，具体包括：
根据所述观测向量，构建时间连续卡尔曼模型观测方程，其中，所述时间连续卡尔曼模型观测方程如下所示：
Z＝H·X+V；
式中，Z为时间连续卡尔曼模型的量测矢量，V为时间连续卡尔曼模型的量测噪声，H为时间连续卡尔曼模型的量测矩阵；
对所述时间连续卡尔曼模型观测方程进行离散化处理，得到卡尔曼模型观测方程，其中，所述卡尔曼模型观测方程，如下所示：
Zk＝Hk·Xk+Vk；
式中，Zk为卡尔曼模型的量测向量，Hk为卡尔曼模型的量测矩阵，Vk为卡尔曼模型的量测噪声。
一实施例中，所述卡尔曼模型构建模块203，用于基于所述卡尔曼模型状态方程和所述卡尔曼模型观测方程，得到卡尔曼模型，其中，所述卡尔曼模型如下所示：
所属领域的技术人员可以清楚的了解到，为描述的方便和简洁，上述描述的装置的具体工作过程，可以参考前述方法实施例中的对应过程，在此不在赘述。
需要说明的是，上述卫导拒止环境下的轨道车组合导航装置的实施例仅仅是示意性的，其中所述作为分离部件说明的模块可以是或者也可以不是物理上分开的，作为模块显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。
在上述的卫导拒止环境下的轨道车组合导航方法的实施例的基础上，本发明另一实施例提供了一种卫导拒止环境下的轨道车组合导航终端设备，该卫导拒止环境下的轨道车组合导航终端设备，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时，实现本发明任意一实施例的卫导拒止环境下的轨道车组合导航方法。
示例性的，在这一实施例中所述计算机程序可以被分割成一个或多个模块，所述一个或者多个模块被存储在所述存储器中，并由所述处理器执行，以完成本发明。所述一个或多个模块可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述卫导拒止环境下的轨道车组合导航终端设备中的执行过程。
所述卫导拒止环境下的轨道车组合导航终端设备可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述卫导拒止环境下的轨道车组合导航终端设备可包括，但不仅限于，处理器、存储器。
所称处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field－Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是所述卫导拒止环境下的轨道车组合导航终端设备的控制中心，利用各种接口和线路连接整个卫导拒止环境下的轨道车组合导航终端设备的各个部分。
所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述卫导拒止环境下的轨道车组合导航终端设备的各种功能。所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序等；存储数据区可存储根据手机的使用所创建的数据等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart MediaCard，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
在上述卫导拒止环境下的轨道车组合导航方法的实施例的基础上，本发明另一实施例提供了一种存储介质，所述存储介质包括存储的计算机程序，其中，在所述计算机程序运行时，控制所述存储介质所在的设备执行本发明任意一实施例的卫导拒止环境下的轨道车组合导航方法。
在这一实施例中，上述存储介质为计算机可读存储介质，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read－OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。
综上，本发明一种卫导拒止环境下的轨道车组合导航方法及装置，通过惯性导航误差模型和惯性器件零偏的马尔科夫模型，构建卡尔曼模型状态方程；获取所有观测量，将所述观测量进行整合，得到观测向量，根据所述观测向量，构建卡尔曼模型观测方程；基于所述卡尔曼模型状态方程和所述卡尔曼模型观测方程，得到卡尔曼模型，基于所述卡尔曼模型对状态变量进行实时估算和输出校正，得到组合导航算法；该组合导航算法需要的外界量测信息少，大多数观测量为惯性运动本身的特征量，因此，本发明的技术方案可以提高导航精度的量测信息，鲁棒性较好。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和替换，这些改进和替换也应视为本发明的保护范围。

Claims (5)

1.一种卫导拒止环境下的轨道车组合导航方法，其特征在于，包括：
根据惯性导航误差模型和惯性器件零偏的马尔科夫模型，构建卡尔曼模型状态方程；其中，获取并根据轨道车的姿态变化率方程、速度变化率方程和位置变化率方程，建立惯性导航误差模型；其中，轨道车的姿态变化率方程、速度变化率方程和位置变化率方程，如下所示：
式中，为载体坐标系b到导航坐标系n的矩阵，为惯性测量单元坐标系b相对于地心惯性坐标系i的旋转角速度矢量在惯性测量单元坐标系b下的投影，为导航坐标系n相对于地心惯性坐标系i的旋转角速度在载体坐标系b下的投影，fb为比力矢量在载体坐标系b下的投影，为地球坐标系e相对于地心惯性坐标系i的旋转角速度矢量在导航坐标系n下的投影，为导航坐标系n相对于地球坐标系e的旋转角速度矢量在导航坐标系n下的投影，vn为速度矢量在导航坐标系n下的投影，gn为重力加速度矢量在导航坐标系n下的投影，为载体线速度到经纬度角速度的转换矩阵；矩阵 是由向量构成的反对称矩阵；所述惯性导航误差模型，如下所示：
式中，为地球坐标系e相对于地心惯性坐标系i的旋转角速度矢量在导航坐标系n下的投影，φ为失准角误差矢量，ε为陀螺的零偏，fn为比力矢量在导航坐标系n下的投影，为加速度计的零偏；获取并根据陀螺零偏方程和加速度计零偏方程，建立性器件零偏的马尔科夫模型；其中，所述惯性器件零偏的马尔科夫模型，如下所示：
式中，τg为陀螺的时间相关常数，wg为陀螺的测量噪声，τa为加速度计的时间相关常数，wa为加速度计的测量噪声；
获取所有观测量，将所述观测量进行整合，得到观测向量，根据所述观测向量，构建卡尔曼模型观测方程；其中，所述根据所述观测向量，构建卡尔曼模型观测方程，具体包括：根据所述观测向量，构建时间连续卡尔曼模型观测方程，其中，所述时间连续卡尔曼模型观测方程如下所示：
Z＝H·X+V；
式中，Z为时间连续卡尔曼模型的量测矢量，V为时间连续卡尔曼模型的量测噪声，H为时间连续卡尔曼模型的量测矩阵；对所述时间连续卡尔曼模型观测方程进行离散化处理，得到卡尔曼模型观测方程，其中，所述卡尔曼模型观测方程，如下所示：
Zk＝Hk·Xk+Vk；
式中，Zk为卡尔曼模型的量测向量，Hk为卡尔曼模型的量测矩阵，Vk为卡尔曼模型的量测噪声；
基于所述卡尔曼模型状态方程和所述卡尔曼模型观测方程，得到卡尔曼模型，基于所述卡尔曼模型对状态变量进行实时估算和输出校正，得到组合导航算法；根据惯性导航误差模型和惯性器件零偏的马尔科夫模型，构建卡尔曼模型状态方程，具体包括：
根据所述惯性导航误差模型和所述惯性器件零偏的马尔科夫模型，得到时间连续卡尔曼模型状态方程，其中，所述时间连续卡尔曼模型状态方程，如下所示：
式中，FSINS为时间连续卡尔曼模型的状态转移矩阵，X为时间连续卡尔曼模型的状态变量，W为时间连续卡尔曼模型的系统噪声矩阵；对所述时间连续卡尔曼模型状态方程进行离散化处理，得到卡尔曼模型状态方程，其中，所述卡尔曼模型状态方程，如下所示：
Xk＝Φk，k-1·Xk-1+Wk-1；
式中，Xk为卡尔曼模型的状态变量，Φk，k-1为卡尔曼模型的状态转移矩阵，Wk-1为离散卡尔曼模型的系统噪声矩阵；
基于所述卡尔曼模型状态方程和所述卡尔曼模型观测方程，得到卡尔曼模型，其中，所述卡尔曼模型如下所示：
2.如权利要求1所述的一种卫导拒止环境下的轨道车组合导航方法，其特征在于，获取所有观测量，其中，所述所有观测量包括天向速度误差、横向速度误差、前向速度误差、向心加速度误差、天向陀螺零偏、向心角加速度差值误差。
3.一种卫导拒止环境下的轨道车组合导航装置，其特征在于，包括：卡尔曼模型状态方程构建模块、卡尔曼模型观测方程构建模块和卡尔曼模型构建模块；
所述卡尔曼模型状态方程构建模块，用于根据惯性导航误差模型和惯性器件零偏的马尔科夫模型，构建卡尔曼模型状态方程；其中，获取并根据轨道车的姿态变化率方程、速度变化率方程和位置变化率方程，建立惯性导航误差模型；其中，轨道车的姿态变化率方程、速度变化率方程和位置变化率方程，如下所示：
式中，为载体坐标系b到导航坐标系n的矩阵，为惯性测量单元坐标系b相对于地心惯性坐标系i的旋转角速度，矢量在惯性测量单元坐标系b下的投影，为导航坐标系n相对于地心惯性坐标系i的旋转角速度在载体坐标系b下的投影，fb为比力矢量在载体坐标系b下的投影，为地球坐标系e相对于地心惯性坐标系i的旋转角速度矢量在导航坐标系n下的投影，为导航坐标系n相对于地球坐标系e的旋转角速度矢量在导航坐标系n下的投影，vn为速度矢量在导航坐标系n下的投影，gn为重力加速度矢量在导航坐标系n下的投影，为载体线速度到经纬度角速度的转换矩阵；矩阵是由向量构成的反对称矩阵；所述惯性导航误差模型，如下所示：
式中，为地球坐标系e相对于地心惯性坐标系i的旋转角速度，矢量在导航坐标系n下的投影，φ为姿态角误差矢量，ε为陀螺的零偏，fn为比力矢量在导航坐标系n下的投影，为加速度计的零偏；获取并根据陀螺零偏方程和加速度计零偏方程，建立性器件零偏的马尔科夫模型；其中，所述惯性器件零偏的马尔科夫模型，如下所示：
式中，τg为陀螺的时间相关常数，wg为陀螺的测量噪声，τa为加速度计的时间相关常数，wa为加速度计的测量噪声
所述卡尔曼模型观测方程构建模块，用于获取所有观测量，将所述观测量进行整合，得到观测向量，根据所述观测向量，构建卡尔曼模型观测方程；其中，所述根据所述观测向量，构建卡尔曼模型观测方程，具体包括：根据所述观测向量，构建时间连续卡尔曼模型观测方程，其中，所述时间连续卡尔曼模型观测方程如下所示：
Z＝H·X+V；
式中，Z为时间连续卡尔曼模型的量测矢量，V为时间连续卡尔曼模型的量测噪声，H为时间连续卡尔曼模型的量测矩阵；对所述时间连续卡尔曼模型观测方程进行离散化处理，得到卡尔曼模型观测方程，其中，所述卡尔曼模型观测方程，如下所示：
Zk＝Hk·Xk+Vk；
式中，Zk为卡尔曼模型的量测向量，Hk为卡尔曼模型的量测矩阵，Vk为卡尔曼模型的量测噪声；
所述卡尔曼模型构建模块，用于基于所述卡尔曼模型状态方程和所述卡尔曼模型观测方程，得到卡尔曼模型，基于所述卡尔曼模型对状态变量进行实时估算和输出校正，得到组合导航算法；根据惯性导航误差模型和惯性器件零偏的马尔科夫模型，构建卡尔曼模型状态方程，具体包括：
根据所述惯性导航误差模型和所述惯性器件零偏的马尔科夫模型，得到时间连续卡尔曼模型状态方程，其中，所述时间连续卡尔曼模型状态方程，如下所示：
式中，FSINS为时间连续卡尔曼模型的状态转移矩阵，X为时间连续卡尔曼模型的状态变量，W为时间连续卡尔曼模型的系统噪声矩阵；对所述时间连续卡尔曼模型状态方程进行离散化处理，得到卡尔曼模型状态方程，其中，所述卡尔曼模型状态方程，如下所示：
Xk＝Φk，k-1·Xk-1+Wk-1；
式中，Xk为卡尔曼模型的状态变量，Φk，k-1为卡尔曼模型的状态转移矩阵，Wk-1为离散卡尔曼模型的系统噪声矩阵；
基于所述卡尔曼模型状态方程和所述卡尔曼模型观测方程，得到卡尔曼模型，其中，所述卡尔曼模型如下所示：
4.一种终端设备，其特征在于，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至2任意一项所述的卫导拒止环境下的轨道车组合导航方法。
5.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行如权利要求1至2中任意一项所述的卫导拒止环境下的轨道车组合导航方法。
CN202211430464.7A 2022-11-14 2022-11-14 一种卫导拒止环境下的轨道车组合导航方法及装置 Active CN115900701B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211430464.7A CN115900701B (zh) 2022-11-14 2022-11-14 一种卫导拒止环境下的轨道车组合导航方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211430464.7A CN115900701B (zh) 2022-11-14 2022-11-14 一种卫导拒止环境下的轨道车组合导航方法及装置

Publications (2)

Publication Number Publication Date
CN115900701A CN115900701A (zh) 2023-04-04
CN115900701B true CN115900701B (zh) 2023-10-31

Family

ID=86473957

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211430464.7A Active CN115900701B (zh) 2022-11-14 2022-11-14 一种卫导拒止环境下的轨道车组合导航方法及装置

Country Status (1)

Country Link
CN (1) CN115900701B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101561496A (zh) * 2009-05-21 2009-10-21 北京航空航天大学 一种伪卫星和惯性组合导航系统的非线性补偿方法
CN103557876A (zh) * 2013-11-15 2014-02-05 山东理工大学 一种用于天线跟踪稳定平台的捷联惯导初始对准方法
CN110579740A (zh) * 2019-09-17 2019-12-17 大连海事大学 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
CN110715660A (zh) * 2019-11-21 2020-01-21 中国人民解放军63961部队 基于卡尔曼滤波估计的北斗/惯性组合抗欺骗干扰方法
CN110887507A (zh) * 2019-10-22 2020-03-17 中国人民解放军战略支援部队航天工程大学 一种快速估计惯性测量单元全部零偏的方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109059904A (zh) * 2018-06-01 2018-12-21 浙江亚特电器有限公司 用于移动载具的组合导航方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101561496A (zh) * 2009-05-21 2009-10-21 北京航空航天大学 一种伪卫星和惯性组合导航系统的非线性补偿方法
CN103557876A (zh) * 2013-11-15 2014-02-05 山东理工大学 一种用于天线跟踪稳定平台的捷联惯导初始对准方法
CN110579740A (zh) * 2019-09-17 2019-12-17 大连海事大学 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
CN110887507A (zh) * 2019-10-22 2020-03-17 中国人民解放军战略支援部队航天工程大学 一种快速估计惯性测量单元全部零偏的方法
CN110715660A (zh) * 2019-11-21 2020-01-21 中国人民解放军63961部队 基于卡尔曼滤波估计的北斗/惯性组合抗欺骗干扰方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
"INS/BDS integrated navigation filter algorithm based on Unscented Kalman Filter";Jie Lei等;《IEEE》;3624-3629 *
侯永利等." 自主式车载组合导航系统研究与仿真".《指挥控制与仿真》.2006,第28卷(第2期),104-107页. *
松耦合SINS/GPS组合导航技术仿真研究;刘华迪;段智敏;王磊;;沈阳理工大学学报(06);第42-45、50页 *

Also Published As

Publication number Publication date
CN115900701A (zh) 2023-04-04

Similar Documents

Publication Publication Date Title
Georgy et al. Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation
Chang et al. In-motion initial alignment for odometer-aided strapdown inertial navigation system based on attitude estimation
CN111780756A (zh) 车辆航位推算方法、装置、设备以及存储介质
Wagner et al. Integrating satellite and inertial navigation—conventional and new fusion approaches
Nourmohammadi et al. Design and experimental evaluation of indirect centralized and direct decentralized integration scheme for low-cost INS/GNSS system
CN103727941A (zh) 基于载体系速度匹配的容积卡尔曼非线性组合导航方法
CN113340298B (zh) 一种惯导和双天线gnss外参标定方法
CN106153073A (zh) 一种全姿态捷联惯导系统的非线性初始对准方法
CN110395297B (zh) 列车定位方法
CN110763872A (zh) 一种多普勒测速仪多参数在线标定方法
Sun et al. In-motion attitude and position alignment for odometer-aided SINS based on backtracking scheme
CN105352502A (zh) 一种微惯性航姿参考系统的姿态获取方法
Wen et al. Odometer aided SINS in-motion alignment method based on backtracking scheme for large misalignment angles
Yang et al. SINS/odometer/Doppler radar high-precision integrated navigation method for land vehicle
Liu et al. Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter
CN111190207B (zh) 基于pstcsdref算法的无人机ins bds组合导航方法
CN113959462A (zh) 一种基于四元数的惯性导航系统自对准方法
Iqbal et al. Experimental results on an integrated GPS and multisensor system for land vehicle positioning
CN116839591B (zh) 一种轨迹跟踪定位滤波系统及救援无人机的融合导航方法
Hu et al. Kilometer sign positioning-aided INS/odometer integration for land vehicle autonomous navigation
CN115900701B (zh) 一种卫导拒止环境下的轨道车组合导航方法及装置
RU2487318C1 (ru) Бесплатформенная инерциальная курсовертикаль на чувствительных элементах средней точности
RU2502049C1 (ru) Малогабаритная бесплатформенная инерциальная навигационная система средней точности, корректируемая от системы воздушных сигналов
CN116576849A (zh) 一种基于gmm辅助的车辆融合定位方法及系统
RU2617147C1 (ru) Способ начального ориентирования гироскопической навигационной системы для наземных подвижных объектов

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant