CN109471144B - 基于伪距/伪距率的多传感器紧组合列车组合定位方法 - Google Patents

基于伪距/伪距率的多传感器紧组合列车组合定位方法 Download PDF

Info

Publication number
CN109471144B
CN109471144B CN201811525963.8A CN201811525963A CN109471144B CN 109471144 B CN109471144 B CN 109471144B CN 201811525963 A CN201811525963 A CN 201811525963A CN 109471144 B CN109471144 B CN 109471144B
Authority
CN
China
Prior art keywords
train
speed
ins
representing
ovs
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
CN201811525963.8A
Other languages
English (en)
Other versions
CN109471144A (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.)
Jiaotong University Zhiyuan Beijing Technology Co ltd
Original Assignee
Beijing Jiaotong 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 Beijing Jiaotong University filed Critical Beijing Jiaotong University
Priority to CN201811525963.8A priority Critical patent/CN109471144B/zh
Publication of CN109471144A publication Critical patent/CN109471144A/zh
Application granted granted Critical
Publication of CN109471144B publication Critical patent/CN109471144B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Abstract

本发明提供了一种基于伪距/伪距率的多传感器紧组合列车组合定位方法。该方法包括：由INS自身递推计算出列车的位置、速度和姿态信息，根据INS自身解算的列车位置与GNSS卫星位置计算卫星与列车之间的几何距离，计算GNSS量测的伪距与INS解算的卫星与列车的几何距离之间的伪距差值，计算GNSS量测的伪距率与INS解算的列车的速度之间的伪距率差值，计算INS解算的列车的速度与OVS量测的速度之间的速度差值；将卫星与列车之间的几何距离和各种差值作为系统滤波的量测输入，采用标准卡尔曼滤波修正INS系统的误差。本发明的方法通过对GNSS原始数据、INS测量数据和OVS测量数据的采集，可以在GNSS信号失效环境下，有INS/OVS组合系统继续定位，保证列车连续无缝定位。

Description

基于伪距/伪距率的多传感器紧组合列车组合定位方法
技术领域
本发明涉及列车定位技术领域，尤其涉及一种基于伪距/伪距率的多传感器紧组合列车组合定位方法。
背景技术
轨道交通是国民经济大动脉、大众化交通工具和现代城市运行的骨架，是国家关键基础设施和重要基础产业，对我国经济社会发展、民生改善和国家安全起着不可替代的全局性支撑作用。在列车的运行过程中，列车运行控制功能的实现依赖于对列车的速度、位置、速度等信息的准确获取。目前我国的高速铁路列车运行控制系统普遍采用地面应答器辅助车轮传感器实现列车位置状态的获取，利用轨道电路实现列车占用检查，这种方式系统成本大，且固定闭塞分区的设置方式也制约着列车行车密度和效率。采用GNSS(GlobalNavigation Satellite System,全球卫星导航定位系统)获取列车定位信息不仅可以减少轨旁设备，有效降低建设和运行成本，同时还能够提高定位精度，实现高更新率的实时连续定位。
考虑到列车运行的安全性和连续性的需要，GNSS导航系统不宜作为单独的导航设备，应该与其他的导航系统组合使用。由于INS(Inertial Navigation System,惯性导航系统)信号短时间定位精度高，但长时间误差累积，系统稳定性较差，将GNSS与INS进行系统融合具有很好的互补优势。
GNSS/INS组合导航模式最常见的是松组合与紧组合。松组合将GNSS的速度和位置作为量测量引入INS中以抑制其发散，但GNSS对位置和速度的解算至少需要4颗有效卫星，当此条件不满足时GNSS将失效，此时组合系统的输出仅取决于INS，误差会快速积累。在一些环境复杂的应用中，如森林、峡谷等有遮挡、弱信号、强干扰的环境中，GNSS接收机往往无法定位导致此种组合方式失效，因此松组合的抗干扰能力较差。
为克服松组合方式在环境复杂的应用中抗干扰能力较差的缺点，采用GNSS导航解算之前的伪距和伪距率等原始信息作为观测量进行的紧组合定位方法。紧组合采用GNSS提供可见卫星的伪距和伪距率作为量测值，INS利用自身解算的速度、位置以及GNSS卫星的速度、位置反算出伪距和伪距率，利用这两者之差作为滤波器的输入对误差状态进行估计并修正。由于每颗可见卫星都对应一组观测信息，因此当可见卫星数目少于4颗时，紧组合导航系统仍然能够实现组合输出，并且可见卫星多于4颗时有助于进一步提高系统的导航精度。所以紧组合较松组合有更高的精度和鲁棒性，并且保持了结构上的简洁和一致性。
当列车运行到大长隧道、山谷长时间遮挡等环境，GNSS/INS组合系统转换为INS继续提供列车位置信息，列车定位误差随时间累积，影响行车安全。OVS(Optical VelocitySensor,光电速度传感器)作为一种光学传感器，可以测量速度、加速度以及位移等多种物理量，可以用作辅助定位。当GNSS定位系统失效时，OVS可以作为一种补偿手段，提供列车在轨道上的相对位置，提高定位系统的精度和可靠性。
目前，现有技术中还没有针对基于卫星导航定位的GNSS/INS/OVS三组合紧组合列车组合定位方法进行深入研究。
发明内容
本发明实施例提供了一种基于伪距/伪距率的多传感器紧组合列车组合定位方法，采用基于伪距、伪距率的GNSS/INS/OVS紧组合定位方法，提供高精度连续无缝的列车位置信息，保障列车行车安全。
为了实现上述目的，本发明采取了如下技术方案。
一种基于伪距/伪距率的多传感器紧组合列车组合定位方法，包括：
由INS根据列车初始位置、速度和姿态信息，结合INS中加速度计和陀螺仪量测的列车加速度和角速率，自身递推计算出列车的位置、速度和姿态信息；
在卫星信号有效环境下，根据INS自身解算的列车位置与GNSS卫星位置计算卫星与列车之间的几何距离，计算GNSS量测的伪距与INS解算的卫星与列车的几何距离之间的伪距差值，计算GNSS量测的伪距率与INS解算的列车的速度之间的伪距率差值，计算INS解算的列车的速度与OVS量测的速度之间的速度差值；
将所述计算出的卫星与列车之间的几何距离，以及所述伪距差值、伪距率差值和速度差值作为系统滤波的量测输入，采用标准卡尔曼滤波的滤波结果修正惯性导航系统的误差，得到融合后的列车位置、速度信息和姿态信息。
进一步地，所述的计算GNSS量测的伪距与INS解算的卫星与列车的几何距离之间的伪距差值，计算GNSS量测的伪距率与INS解算的列车的速度之间的伪距率差值，计算INS解算的列车的速度与OVS量测的速度之间的速度差值，包括：
INS递推计算列车的位置、速度和姿态的具体过程为：
Figure BDA0001904459520000031
Figure BDA0001904459520000032
Figure BDA0001904459520000033
其中：pn表示列车位置信息，
Figure BDA0001904459520000041
表示列车位置的变化率矢量，vn表示列车的速度信息，
Figure BDA0001904459520000042
表示列车速度的变化率矢量，fn表示加速度计测量的列车加速度矢量信息，gn表示重力，
Figure BDA0001904459520000043
表示在导航坐标系下陀螺仪测量的列车角速度矢量信息，
Figure BDA0001904459520000044
表示在导航坐标系下地球自转引起的角速度变化矢量，
Figure BDA0001904459520000045
表示姿态四元数，
Figure BDA0001904459520000046
表示姿态四元数率，
Figure BDA0001904459520000047
表示载体坐标系相对于导航坐标系的瞬时角速度矢量
Figure BDA0001904459520000048
的四元数形式；
姿态转移矩阵的计算公式如下所示：
Figure BDA0001904459520000049
初始的姿态矩阵由给定的初始俯仰角、横滚角和航向角计算得到，计算公式如下：
Figure BDA00019044595200000410
其中：θ0表示初始俯仰角，γ0表示初始横滚角，ψ0表示初始航向角，
Figure BDA00019044595200000411
表示初始姿态矩阵。
进一步地，所述的在卫星信号有效环境下，根据INS自身解算的列车位置与GNSS卫星位置计算卫星与列车之间的几何距离，包括：
卫星与列车之间的几何距离的计算公式为：
Figure BDA00019044595200000412
其中，dis表示INS自身解算的列车位置和GNSS卫星位置之间的几何距离，(sx,sy,sz)表示GNSS卫星在导航坐标系下的位置坐标，(rx,ry,rz)表示INS自身解算的列车在导航坐标系下的位置坐标。
进一步地，所述的计算OVS量测的速度与INS解算的列车的速度之间的速度差值，包括：
OVS量测的导航坐标系下的列车速度vOVS计算如下：
Figure BDA0001904459520000051
其中：vL表示光电速度传感器量测的列车在载体坐标系的左向速度，vF表示光电速度传感器量测的列车在载体坐标系的前向速度，
Figure BDA0001904459520000052
表示列车从载体坐标系转向导航坐标系的姿态转移矩阵；
将OVS量测的导航坐标系下的列车速度vOVS与INS自身解算的速度vINS做差：vOVS-vINS。
进一步地，所述的将所述计算出的卫星与列车之间的几何距离，以及所述伪距差值、伪距率差值和速度差值作为系统滤波的量测输入，采用标准卡尔曼滤波的滤波结果修正惯性导航系统的误差，得到融合后的列车位置、速度信息和姿态信息，包括：
GNSS系统状态由十七维组成，包括姿态误差、位置误差、三维的速度误差、陀螺仪误差以及加速度计误差，接收机钟差和钟漂；
GNSS系统的状态向量X(t)为：
Figure BDA0001904459520000053
其中：(δrE,δrN,δrU)表示列车位置误差，(δvE,δvN,δvU)表示列车速度误差，
Figure BDA0001904459520000054
表示列车姿态误差，
Figure BDA0001904459520000055
表示陀螺仪测量的角速度误差，(εx,εy,εz)表示加速度计测量的加速度误差，dt表示卫星接收机的钟差，
Figure BDA0001904459520000056
表示卫星接收的钟漂；
在GNSS/INS/OVS紧组合列车定位系统中，将所述计算出的卫星与列车之间的几何距离，以及所述伪距差值、伪距率差值和速度差值作为系统滤波的量测输入，系统的量测矩阵Z为：
Figure BDA0001904459520000061
其中：ρ表示GNSS原始伪距量测，
Figure BDA0001904459520000062
表示GNSS原始伪距率量测，
Figure BDA0001904459520000063
表示INS解算的卫星到列车的几何距离，
Figure BDA0001904459520000064
表示INS解算的卫星到列车的几何距离变化率，n表示可见卫星数，vINS表示惯导自身解算速度信息，vOVS表示在导航坐标系下的OVS量测速度；
采用标准卡尔曼滤波对列车的速度、位置状态进行估计，系统的量测矩阵H为：
Figure BDA0001904459520000065
其中：c表示光速，Dn×3表示从卫星到列车接收机的方向余弦矩阵，On×3表示一个n行3列的零矩阵，On×9表示一个n行9列的零矩阵，On×1表示一个n行1列的零矩阵，,In×1表示一个n行1列的单位矩阵；
系统的测量噪声协方差为：
Figure BDA0001904459520000066
进一步地，所述的方法还包括：
在卫星信号失效环境下，采用标准卡尔曼滤波算法利用OVS来修正INS系统的误差，系统的量测向量、系统量测矩阵和系统噪声协方差矩阵的计算公式如下：
系统的量测矩阵Z为：
Z＝[vOVS-vINS]T
其中：vINS表示惯导自身解算速度信息，vOVS表示在导航坐标系下的OVS量测速度；
系统的量测矩阵H为：
H＝[03×3 I3×3 03×9 03×1 03×1]
惯性导航/光电速度传感器组合系统的量测噪声协方差为：
R＝diag(0.82,0.82,0.82)
进一步地，所述的方法还包括：
所述标准卡尔曼滤波算法包括：时间更新和量测更新。
时间更新包括状态一步预测和一步预测均方误差的计算，计算公式分别如下：
Figure BDA0001904459520000071
P(k/k-1)＝F(k-1)P(k-1)F(k-1)T+Q(k-1)
其中，F(k-1)表示状态转移矩阵，
Figure BDA0001904459520000072
表示k-1时刻的估计状态；
Figure BDA0001904459520000073
表示k时刻的一步预测状态，即惯性导航系统自身递推得到的状态，包括位置、速度、姿态、加速度计和陀螺仪的偏移；Q(k-1)表示系统噪声协方差矩阵，P(k-1)表示k-1时刻估计状态误差的协方差矩阵；
量测更新包括滤波增益、状态估计和估计均方误差的计算，计算公式分别如下：
K(k)＝P(k/k-1)H(k)T[H(k)P(k/k-1)H(k)T+R(k)]-1
Figure BDA0001904459520000081
P(k)＝[I-K(k)H(k/k-1)]P(k/k-1)
其中，H(k)表示系统的量测矩阵，R(k)表示系统量测噪声协方差矩阵，K(k)表示滤波增益，P(k)表示状态估计的均方误差，
Figure BDA0001904459520000082
表示估计的状态，即为融合后的列车状态，由一步预测状态和量测更新得到的状态两部分组成，列车的状态包括列车的位置、速度、姿态、加速度计和陀螺仪的偏移。
由上述本发明的实施例提供的技术方案可以看出，本发明实施例的自适应容错的GNSS/INS紧组合列车定位方法通过列车运行过程中对GNSS原始数据和INS测量数据，基于伪距、伪距率的GNSS/INS紧组合导航定位系统获得连续的列车位置、速度信息，同时通过FDP方法进行故障检测、识别与相应处理，提高系统容错性，适用于列车实时定位和后处理位置解算分析，具有连续性强、可靠性高的特点。
本发明附加的方面和优点将在下面的描述中部分给出，这些将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种基于GNSS/INS/OVS的紧组合列车定位方法的实现原理示意图；
图2为本发明实施例提供的一种基于GNSS/INS/OVS的紧组合列车定位方法的处理流程图。
具体实施方式
下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。
本技术领域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”、“所述”和“该”也可包括复数形式。应该进一步理解的是，本发明的说明书中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组。应该理解，当我们称元件被“连接”或“耦接”到另一元件时，它可以直接连接或耦接到其他元件，或者也可以存在中间元件。此外，这里使用的“连接”或“耦接”可以包括无线连接或耦接。这里使用的措辞“和/或”包括一个或更多个相关联的列出项的任一单元和全部组合。
本技术领域技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。
为便于对本发明实施例的理解，下面将结合附图以几个具体实施例为例做进一步的解释说明，且各个实施例并不构成对本发明实施例的限定。
本发明实施例提供了一种基于GNSS/INS/OVS的紧组合列车定位方法的实现原理示意图如图1所示，处理流程如图2所示，包括如下的处理步骤：
步骤S210：由INS根据列车初始位置、速度和姿态信息，结合INS中加速度计和陀螺仪量测的列车加速度和角速率，自身递推计算出列车的位置、速度和姿态信息。
由于GNSS/INS/OVS紧组合列车定位方法采用GNSS原始伪距、伪距率与INS解算的对应的伪距、伪距率之间的差值，以及INS自身计算的速度与OVS量测的速度之间的差值作为滤波量测输入，所以首先需要获得INS自身计算的位置、速度和姿态信息，OVS量测的列车在导航坐标下的速度。
INS递推计算列车的位置、速度和姿态的具体过程可以描述为：
Figure BDA0001904459520000101
Figure BDA0001904459520000102
Figure BDA0001904459520000103
其中：pn表示列车位置信息，
Figure BDA0001904459520000104
表示列车位置的变化率矢量，vn表示列车的速度信息，
Figure BDA0001904459520000105
表示列车速度的变化率矢量，fn表示加速度计测量的列车加速度矢量信息，gn表示重力，
Figure BDA0001904459520000106
表示在导航坐标系下陀螺仪测量的列车角速度矢量信息，
Figure BDA0001904459520000107
表示在导航坐标系下地球自转引起的角速度变化矢量，
Figure BDA0001904459520000108
表示姿态四元数，
Figure BDA0001904459520000109
表示姿态四元数率，
Figure BDA00019044595200001010
表示载体坐标系相对于导航坐标系的瞬时角速度矢量
Figure BDA00019044595200001011
的四元数形式。
姿态转移矩阵的计算公式如下所示：
Figure BDA0001904459520000111
q0,q1,q2,q3分别表示姿态四元数矩阵
Figure BDA0001904459520000112
中的四个元素。
INS通过测量载体在惯性参考系的加速度和角速度，将他们对时间进行积分，之后变换到导航坐标系，得到载体在导航坐标系中的速度、航向角和位置。姿态矩阵描述了载体坐标系与导航坐标系的关系，实现载体坐标系到导航坐标系的转换。
初始位置、速度和姿态信息的精度会影响INS的自身递推结果精度，所以需要选取高精度的初始信息。初始的姿态矩阵由给定的初始俯仰角、横滚角和航向角计算得到。那么初始姿态矩阵计算公式如下所示：
Figure BDA0001904459520000113
其中：θ0表示初始俯仰角，γ0表示初始横滚角，ψ0表示初始航向角，
Figure BDA0001904459520000114
表示初始姿态矩阵。
步骤S220：计算GNSS量测的伪距与INS解算的卫星与列车的几何距离之间的伪距差值，计算GNSS量测的伪距率与INS解算的列车的速度之间的伪距率差值，计算INS解算的列车的速度与OVS量测的速度之间的速度差值。
在卫星信号有效环境下，根据INS自身解算的列车位置与GNSS卫星位置计算卫星与列车之间的几何距离，计算GNSS量测的伪距与INS解算的卫星与列车的几何距离之间的差值，计算GNSS量测的伪距率与INS解算的列车的速度之间的差值，计算INS解算的列车的速度与OVS量测的速度之间的差值，将上述计算出的卫星与列车之间的几何距离，以及伪距差值、伪距率差值和速度差值作为系统滤波的量测输入，采用标准卡尔曼滤波的滤波结果修正惯性导航系统的误差。
由于OVS原始量测的列车速度为载体坐标系下的速度，为与GNSS、INS在导航坐标系下进行组合，需要将OVS原始量测的列车速度转化到导航坐标系下。OVS量测的导航坐标系下的列车速度vOVS计算如下：
Figure BDA0001904459520000121
其中：vL表示光电速度传感器量测的列车在载体坐标系的左向速度，vF表示光电速度传感器量测的列车在载体坐标系的前向速度，
Figure BDA0001904459520000122
表示列车从载体坐标系转向导航坐标系的姿态转移矩阵。
将OVS量测的导航坐标系下的列车速度vOVS与INS自身解算的速度vINS做差：vOVS-vINS。
还需要计算GNSS量测的伪距与INS解算的卫星与列车的几何距离之间的伪距差值，计算GNSS量测的伪距率与INS解算的列车的速度之间的伪距率差值。
步骤S230：将计算出的卫星与列车之间的几何距离，以及所述伪距差值、伪距率差值和速度差值作为系统滤波的量测输入，采用标准卡尔曼滤波的滤波结果修正惯性导航系统的误差，得到融合后的列车位置、速度信息和姿态信息。
GNSS系统状态由十七维组成，包括姿态误差(横滚、俯仰和偏航角误差)，位置误差(纬度，经度和高度误差)，三维的速度误差、陀螺仪误差以及加速度计误差，接收机钟差和钟漂。
GNSS系统的状态向量X(t)为：
Figure BDA0001904459520000131
其中：(δrE,δrN,δrU)表示列车位置误差，(δvE,δvN,δvU)表示列车速度误差，
Figure BDA0001904459520000132
表示列车姿态误差，
Figure BDA0001904459520000133
表示陀螺仪测量的角速度误差，(εx,εy,εz)表示加速度计测量的加速度误差，dt表示卫星接收机的钟差，
Figure BDA0001904459520000134
表示卫星接收的钟漂。
在GNSS/INS/OVS紧组合列车定位系统中，系统需要比较GNSS卫星原始伪距、伪距率与INS解算的对应的伪距、伪距率，比较INS自身计算的列车速度与OVS量测的列车速度，系统的量测矩阵Z为：
Figure BDA0001904459520000135
其中：ρ表示GNSS原始伪距量测，
Figure BDA0001904459520000136
表示GNSS原始伪距率量测，
Figure BDA0001904459520000137
表示INS解算的卫星到列车的几何距离，
Figure BDA0001904459520000138
表示INS解算的卫星到列车的几何距离变化率，n表示可见卫星数，vINS表示惯导自身解算速度信息，vOVS表示在导航坐标系下的OVS量测速度。
在满足列车定位要求的情况下，利用上述解算的伪距差值、伪距率差值和速度差值作为量测输入，采用标准卡尔曼滤波对列车的速度、位置等状态进行估计，那么系统的量测矩阵H为：
Figure BDA0001904459520000139
其中：c表示光速，Dn×3表示从卫星到列车接收机的方向余弦矩阵，On×3表示一个n行3列的零矩阵，On×9表示一个n行9列的零矩阵，On×1表示一个n行1列的零矩阵，,In×1表示一个n行1列的单位矩阵。
由于基于GNSS接收机的伪距、伪距率量测精度分别为米级、分米/秒，OVS的速度量测精度为分米/秒，所以基于GNSS/INS/OVS的紧组合列车定位系统中的测量噪声协方差可以写为：
Figure BDA0001904459520000141
卡尔曼滤波包括两个主要信息更新过程：时间更新和量测更新。
时间更新包括状态一步预测和一步预测均方误差的计算，计算公式分别如下：
Figure BDA0001904459520000142
P(k/k-1)＝F(k-1)P(k-1)F(k-1)T+Q(k-1)
其中，F(k-1)表示状态转移矩阵，
Figure BDA0001904459520000143
表示k-1时刻的估计状态；
Figure BDA0001904459520000144
表示k时刻的一步预测状态，即惯性导航系统自身递推得到的状态，包括位置、速度、姿态、加速度计和陀螺仪的偏移；Q(k-1)表示系统噪声协方差矩阵，P(k-1)表示k-1时刻估计状态误差的协方差矩阵。
量测更新包括滤波增益、状态估计和估计均方误差的计算，计算公式分别如下：
K(k)＝P(k/k-1)H(k)T[H(k)P(k/k-1)H(k)T+R(k)]-1
Figure BDA0001904459520000145
P(k)＝[I-K(k)H(k/k-1)]P(k/k-1)
其中，H(k)表示系统的量测矩阵，R(k)表示系统量测噪声协方差矩阵，K(k)表示滤波增益，P(k)表示状态估计的均方误差，
Figure BDA0001904459520000151
表示估计的状态，即为融合后的列车状态，由一步预测状态(惯性导航系统自身递推得到的状态)和量测更新得到的状态(卡尔曼滤波估计得到的状态误差)两部分组成，列车的状态包括列车的位置、速度、姿态、加速度计和陀螺仪的偏移。
根据列车的INS/OVS组合定位结果修正惯性导航系统的误差，在卫星信号完全失效情况下，保证列车位置信息的连续性，包括：
在卫星信号失效环境下，INS/OVS进行组合，依旧采用标准卡尔曼滤波算法，利用OVS来抑制惯性器件的误差增加，修正惯性导航系统的误差。
在INS/OVS组合系统中，INS自身解算的速度与OVS量测的速度做差，速度差作为滤波器的量测输入，那么INS/OVS组合系统的量测矩阵Z为：
Z＝[vOVS-vINS]T
其中：vINS表示惯导自身解算速度信息，vOVS表示在导航坐标系下的OVS量测速度。
系统的量测矩阵H为：
H＝[03×3 I3×3 03×9 03×1 03×1]
在INS/OVS组合系统中，速度量测由OVS提供，所以系统的量测噪声协方差矩阵取决于OVS速度测量精度。OVS的速度量测精度为分米/秒，所以INS/OVS组合系统的量测噪声协方差为：
R＝diag(0.82,0.82,0.82)
利用上述卡尔曼滤波中的时间更新和量测更新过程，修正惯性导航系统的误差的具体过程和前一部分一样。
根据上述各模型并通过本发明方法可以得出以下实验结果：
在列车运行过程中，实时采集GNSS原始伪距、伪距率、INS加速度计和陀螺仪量测以及OVS速度量测，通过标准卡尔曼滤波进行实时滤波估计，得到融合后的连续无缝列车位置、速度信息。同时，在GNSS信号失效环境下，由INS/OVS组合系统继续获得列车位置。这种定位方法得到的列车位置、速度信息具有精度高、连续无缝的特点。
本发明方法适用于铁路系统中的连续列车定位系统，可以进行列车实时定位和后处理位置解算与分析。
综上所述，本发明实施例首先由惯性导航系统根据精确的列车初始位置、速度和姿态信息，结合惯性导航系统中加速度计和陀螺仪量测的列车加速度和角速率，自身递推计算列车的位置、速度和姿态信息。其次，惯性导航系统结合列车卫星接收机采集的卫星星历中的位置信息，计算出接收机到卫星的等效伪距和伪距率。卫星接收机得到原始的伪距和伪距率，光电速度传感器量测得到列车的速度。然后，将对应的伪距、伪距率与速度分别做差，得到的结果作为滤波器量测输入进行滤波估计。滤波得到的结果修正惯性导航系统自身解算的位置、速度和姿态信息，得到融合后的列车位置、速度信息和姿态信息。在卫星信号失效环境下，将光电速度传感器量测的速度与惯性导航系统自身解算的速度做差，结果作为滤波器量测输入进行滤波估计。滤波得到的结果继续修正惯性导航系统自身解算的位置、速度和姿态信息，从而实现列车高精度连续无缝定位。能解决列车在卫星信号失效环境下的定位精度下降甚至无法定位的问题，为列车提供连续无缝的高精度位置信息，保障列车行车安全。
本发明实施例提供了一种基于伪距/伪距率的多传感器紧组合列车组合定位方法，通过列车运行过程中对GNSS原始数据、INS测量数据和OVS测量数据的采集，基于伪距、伪距率的GNSS/INS/OVS紧组合导航定位系统获得连续无缝的列车位置、速度信息，同时，在GNSS信号失效环境下，有INS/OVS组合系统继续定位，保证列车连续无缝定位，适用于列车实时定位和后处理位置结算分析，具有精度高、连续无缝的特点。
本领域普通技术人员可以理解：附图只是一个实施例的示意图，附图中的模块或流程并不一定是实施本发明所必须的。
本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于装置或系统实施例而言，由于其基本相似于方法实施例，所以描述得比较简单，相关之处参见方法实施例的部分说明即可。以上所描述的装置及系统实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (5)

1.一种基于伪距/伪距率的多传感器紧组合列车组合定位方法，其特征在于，包括：
由INS根据列车初始位置、速度和姿态信息，结合INS中加速度计和陀螺仪量测的列车加速度和角速率，自身递推计算出列车的位置、速度和姿态信息；
在卫星信号有效环境下，根据INS自身解算的列车位置与GNSS卫星位置计算卫星与列车之间的几何距离，计算GNSS量测的伪距与INS解算的卫星与列车的几何距离之间的伪距差值，计算GNSS量测的伪距率与INS解算的列车的速度之间的伪距率差值，计算INS解算的列车的速度与OVS量测的速度之间的速度差值；
将所述计算出的卫星与列车之间的几何距离，以及所述伪距差值、伪距率差值和速度差值作为系统滤波的量测输入，采用标准卡尔曼滤波的滤波结果修正惯性导航系统的误差，得到融合后的列车位置、速度信息和姿态信息；
还包括：
在卫星信号失效环境下，采用标准卡尔曼滤波算法利用OVS来修正INS系统的误差，系统的量测向量、系统量测矩阵和系统噪声协方差矩阵的计算公式如下：
系统的量测矩阵Z为：
Z＝[vOVS-vINS]T
其中：vINS表示惯导自身解算速度信息，vOVS表示在导航坐标系下的OVS量测速度；
系统的量测矩阵H为：
H＝[03×3 I3×3 03×9 03×1 03×1]
惯性导航/光电速度传感器组合系统的量测噪声协方差为：
R＝diag(0.82,0.82,0.82)；
所述标准卡尔曼滤波算法包括：时间更新和量测更新；
时间更新包括状态一步预测和一步预测均方误差的计算，计算公式分别如下：
Figure FDA0004136478610000021
P(k/k-1)＝F(k-1)P(k-1)F(k-1)T+Q(k-1)
其中，F(k-1)表示状态转移矩阵，
Figure FDA0004136478610000022
表示k-1时刻的估计状态；
Figure FDA0004136478610000023
表示k时刻的一步预测状态，即惯性导航系统自身递推得到的状态，包括位置、速度、姿态、加速度计和陀螺仪的偏移；Q(k-1)表示系统噪声协方差矩阵，P(k-1)表示k-1时刻估计状态误差的协方差矩阵；
量测更新包括滤波增益、状态估计和估计均方误差的计算，计算公式分别如下：
K(k)＝P(k/k-1)H(k)T[H(k)P(k/k-1)H(k)T+R(k)]-1
Figure FDA0004136478610000031
P(k)＝[I-K(k)H(k/k-1)]P(k/k-1)
其中，H(k)表示系统的量测矩阵，R(k)表示系统量测噪声协方差矩阵，K(k)表示滤波增益，P(k)表示状态估计的均方误差，
Figure FDA0004136478610000032
表示估计的状态，即为融合后的列车状态，由一步预测状态和量测更新得到的状态两部分组成，列车的状态包括列车的位置、速度、姿态、加速度计和陀螺仪的偏移。
2.根据权利要求1所述的方法，其特征在于，所述的计算GNSS量测的伪距与INS解算的卫星与列车的几何距离之间的伪距差值，计算GNSS量测的伪距率与INS解算的列车的速度之间的伪距率差值，计算INS解算的列车的速度与OVS量测的速度之间的速度差值，包括：
INS递推计算列车的位置、速度和姿态的具体过程为：
Figure FDA0004136478610000033
Figure FDA0004136478610000034
Figure FDA0004136478610000035
其中：pn表示列车位置信息，
Figure FDA0004136478610000036
表示列车位置的变化率矢量，vn表示列车的速度信息，
Figure FDA0004136478610000037
表示列车速度的变化率矢量，fn表示加速度计测量的列车加速度矢量信息，gn表示重力，
Figure FDA0004136478610000038
表示在导航坐标系下陀螺仪测量的列车角速度矢量信息，
Figure FDA0004136478610000039
表示在导航坐标系下地球自转引起的角速度变化矢量，
Figure FDA00041364786100000310
表示姿态四元数，
Figure FDA00041364786100000311
表示姿态四元数率，
Figure FDA00041364786100000312
表示载体坐标系相对于导航坐标系的瞬时角速度矢量
Figure FDA00041364786100000313
的四元数形式；
姿态转移矩阵的计算公式如下所示：
Figure FDA0004136478610000041
初始的姿态矩阵由给定的初始俯仰角、横滚角和航向角计算得到，计算公式如下：
Figure FDA0004136478610000042
其中：θ0表示初始俯仰角，γ0表示初始横滚角，ψ0表示初始航向角，
Figure FDA0004136478610000043
表示初始姿态矩阵。
3.根据权利要求2所述的方法，其特征在于，所述的在卫星信号有效环境下，根据INS自身解算的列车位置与GNSS卫星位置计算卫星与列车之间的几何距离，包括：
卫星与列车之间的几何距离的计算公式为：
Figure FDA0004136478610000044
其中，dis表示INS自身解算的列车位置和GNSS卫星位置之间的几何距离，(sx,sy,sz)表示GNSS卫星在导航坐标系下的位置坐标，(rx,ry,rz)表示INS自身解算的列车在导航坐标系下的位置坐标。
4.根据权利要求3所述的方法，其特征在于，所述的计算OVS量测的速度与INS解算的列车的速度之间的速度差值，包括：
OVS量测的导航坐标系下的列车速度vOVS计算如下：
Figure FDA0004136478610000045
其中：vL表示光电速度传感器量测的列车在载体坐标系的左向速度，vF表示光电速度传感器量测的列车在载体坐标系的前向速度，
Figure FDA0004136478610000055
表示列车从载体坐标系转向导航坐标系的姿态转移矩阵；
将OVS量测的导航坐标系下的列车速度vOVS与INS自身解算的速度vINS做差：vOVS-vINS。
5.根据权利要求4所述的方法，其特征在于，所述的将所述计算出的卫星与列车之间的几何距离，以及所述伪距差值、伪距率差值和速度差值作为系统滤波的量测输入，采用标准卡尔曼滤波的滤波结果修正惯性导航系统的误差，得到融合后的列车位置、速度信息和姿态信息，包括：
GNSS系统状态由十七维组成，包括姿态误差、位置误差、三维的速度误差、陀螺仪误差以及加速度计误差，接收机钟差和钟漂；
GNSS系统的状态向量X(t)为：
Figure FDA0004136478610000051
其中：(δrE,δrN,δrU)表示列车位置误差，(δvE,δvN,δvU)表示列车速度误差，
Figure FDA0004136478610000052
表示列车姿态误差，
Figure FDA0004136478610000053
表示陀螺仪测量的角速度误差，(εx,εy,εz)表示加速度计测量的加速度误差，dt表示卫星接收机的钟差，
Figure FDA0004136478610000054
表示卫星接收的钟漂；
在GNSS/INS/OVS紧组合列车定位系统中，将所述计算出的卫星与列车之间的几何距离，以及所述伪距差值、伪距率差值和速度差值作为系统滤波的量测输入，系统的量测矩阵Z为：
Figure FDA0004136478610000061
其中：ρ表示GNSS原始伪距量测，
Figure FDA0004136478610000062
表示GNSS原始伪距率量测，
Figure FDA0004136478610000063
表示INS解算的卫星到列车的几何距离，
Figure FDA0004136478610000064
表示INS解算的卫星到列车的几何距离变化率，n表示可见卫星数，vINS表示惯导自身解算速度信息，vOVS表示在导航坐标系下的OVS量测速度；
采用标准卡尔曼滤波对列车的速度、位置状态进行估计，系统的量测矩阵H为：
Figure FDA0004136478610000065
其中：c表示光速，Dn×3表示从卫星到列车接收机的方向余弦矩阵，On×3表示一个n行3列的零矩阵，On×9表示一个n行9列的零矩阵，On×1表示一个n行1列的零矩阵，,In×1表示一个n行1列的单位矩阵；
系统的测量噪声协方差为：
Figure FDA0004136478610000066
CN201811525963.8A 2018-12-13 2018-12-13 基于伪距/伪距率的多传感器紧组合列车组合定位方法 Active CN109471144B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811525963.8A CN109471144B (zh) 2018-12-13 2018-12-13 基于伪距/伪距率的多传感器紧组合列车组合定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811525963.8A CN109471144B (zh) 2018-12-13 2018-12-13 基于伪距/伪距率的多传感器紧组合列车组合定位方法

Publications (2)

Publication Number Publication Date
CN109471144A CN109471144A (zh) 2019-03-15
CN109471144B true CN109471144B (zh) 2023-04-28

Family

ID=65675311

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811525963.8A Active CN109471144B (zh) 2018-12-13 2018-12-13 基于伪距/伪距率的多传感器紧组合列车组合定位方法

Country Status (1)

Country Link
CN (1) CN109471144B (zh)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110106755B (zh) * 2019-04-04 2020-11-03 武汉大学 利用姿态重构铁轨几何形态的高铁轨道不平顺性检测方法
CN110133700B (zh) * 2019-05-27 2023-01-31 上海海事大学 一种船载综合导航定位方法
CN110567456B (zh) * 2019-09-03 2021-06-11 兰州交通大学 基于抗差卡尔曼滤波的bds/ins组合列车定位方法
CN110645979B (zh) * 2019-09-27 2021-09-21 北京交通大学 基于gnss/ins/uwb组合的室内外无缝定位方法
CN110850447B (zh) * 2019-11-11 2022-03-25 北京交通大学 对列车定位单元的定位精度进行综合评估的方法
CN111077555B (zh) * 2020-03-24 2020-08-07 北京三快在线科技有限公司 一种定位方法及装置
CN112722013B (zh) * 2021-01-22 2022-03-25 北京交通大学 一种列车定位股道判别方法
CN113203418B (zh) * 2021-04-20 2022-09-16 同济大学 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN114353835B (zh) * 2022-01-21 2022-10-11 中国铁道科学研究院集团有限公司铁道建筑研究所 惯性轨道测量仪动态校准系统、方法及其应用
CN114964220A (zh) * 2022-04-06 2022-08-30 广东省国土资源测绘院 一种无人机定位、定姿数据采集方法、装置及无人机

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101907714A (zh) * 2010-06-25 2010-12-08 陶洋 基于多传感器数据融合的gps辅助定位系统及其定位方法
CN102292646A (zh) * 2009-01-20 2011-12-21 皇家飞利浦电子股份有限公司 调节用于测量车辆速度的自混合激光传感器系统的方法
CN107894232A (zh) * 2017-09-29 2018-04-10 湖南航天机电设备与特种材料研究所 一种gnss/sins组合导航精确测速定位方法及系统
CN108196289A (zh) * 2017-12-25 2018-06-22 北京交通大学 一种卫星信号受限条件下的列车组合定位方法
CN108226985A (zh) * 2017-12-25 2018-06-29 北京交通大学 基于精密单点定位的列车组合导航方法
CN108454652A (zh) * 2017-02-22 2018-08-28 中车株洲电力机车研究所有限公司 一种安全可靠的实时测速和连续定位的方法、装置及系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8412456B2 (en) * 2008-11-06 2013-04-02 Texas Instruments Incorporated Loosely-coupled integration of global navigation satellite system and inertial navigation system: speed scale-factor and heading bias calibration

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102292646A (zh) * 2009-01-20 2011-12-21 皇家飞利浦电子股份有限公司 调节用于测量车辆速度的自混合激光传感器系统的方法
CN101907714A (zh) * 2010-06-25 2010-12-08 陶洋 基于多传感器数据融合的gps辅助定位系统及其定位方法
CN108454652A (zh) * 2017-02-22 2018-08-28 中车株洲电力机车研究所有限公司 一种安全可靠的实时测速和连续定位的方法、装置及系统
CN107894232A (zh) * 2017-09-29 2018-04-10 湖南航天机电设备与特种材料研究所 一种gnss/sins组合导航精确测速定位方法及系统
CN108196289A (zh) * 2017-12-25 2018-06-22 北京交通大学 一种卫星信号受限条件下的列车组合定位方法
CN108226985A (zh) * 2017-12-25 2018-06-29 北京交通大学 基于精密单点定位的列车组合导航方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
一种 GNSS/INS/LiDAR 组合导航传感器安置关系快速标定方法;周阳林 等;<<中国惯性技术学报 >>;20180831;第26卷(第4期);458-463页 *

Also Published As

Publication number Publication date
CN109471144A (zh) 2019-03-15

Similar Documents

Publication Publication Date Title
CN109471144B (zh) 基于伪距/伪距率的多传感器紧组合列车组合定位方法
CN106289275B (zh) 用于改进定位精度的单元和方法
CN109471143B (zh) 自适应容错的列车组合定位方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN108931244B (zh) 基于列车运动约束的惯导误差抑制方法及系统
Jiang et al. A fault-tolerant tightly coupled GNSS/INS/OVS integration vehicle navigation system based on an FDP algorithm
US8170796B2 (en) Ins based GPS carrier phase cycle slip detection and repairing
CN109343095B (zh) 一种车载导航车辆组合定位装置及其组合定位方法
CN104729506A (zh) 一种视觉信息辅助的无人机自主导航定位方法
CN111156994A (zh) 一种基于mems惯性组件的ins/dr&amp;gnss松组合导航方法
CN102879779B (zh) 一种基于sar遥感成像的杆臂测量及补偿方法
CN109059909A (zh) 基于神经网络辅助的卫星/惯导列车定位方法与系统
CN111721250B (zh) 一种铁路轨道平顺性实时检测装置及检测方法
JP2007284013A (ja) 車両位置測位装置及び車両位置測位方法
CN115060257B (zh) 一种基于民用级惯性测量单元的车辆变道检测方法
CN114396943A (zh) 一种融合定位方法与终端
Zou et al. A nonlinear transfer alignment of distributed POS based on adaptive second-order divided difference filter
CN113758483A (zh) 一种自适应fkf地图匹配方法及系统
CN115683124A (zh) 确定行驶轨迹的方法
CN113916224A (zh) 通过多传感器融合来确定车辆位置的方法
US20230341563A1 (en) System and method for computing positioning protection levels
US20220244407A1 (en) Method for Generating a Three-Dimensional Environment Model Using GNSS Measurements
CN113203414A (zh) 一种基于gps+bds ppp/imu紧组合的列车定位方法
CN116299599A (zh) 一种ins辅助的gnss伪距粗差探测方法
CN111076718B (zh) 一种地铁列车的自主导航定位方法

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
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20240124

Address after: 13B-128, 13th Floor, Building 89, Zone 1, No. 44 Gaoliangqiao Xiejie Street, Haidian District, Beijing, 100044

Patentee after: Jiaotong University Zhiyuan (Beijing) Technology Co.,Ltd.

Country or region after: China

Address before: 100044 Beijing city Haidian District Xizhimen Shangyuan Village No. 3

Patentee before: Beijing Jiaotong University

Country or region before: China