CN109471144A - 基于伪距/伪距率的多传感器紧组合列车组合定位方法 - Google Patents
基于伪距/伪距率的多传感器紧组合列车组合定位方法 Download PDFInfo
- Publication number
- CN109471144A CN109471144A CN201811525963.8A CN201811525963A CN109471144A CN 109471144 A CN109471144 A CN 109471144A CN 201811525963 A CN201811525963 A CN 201811525963A CN 109471144 A CN109471144 A CN 109471144A
- Authority
- CN
- China
- Prior art keywords
- train
- indicate
- speed
- ins
- satellite
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 50
- 230000010354 integration Effects 0.000 title claims abstract description 25
- 238000005259 measurement Methods 0.000 claims abstract description 62
- 238000004364 calculation method Methods 0.000 claims abstract description 25
- 239000011159 matrix material Substances 0.000 claims description 59
- 238000001914 filtration Methods 0.000 claims description 21
- 230000001133 acceleration Effects 0.000 claims description 14
- 230000008569 process Effects 0.000 claims description 11
- 230000008859 change Effects 0.000 claims description 9
- 230000005611 electricity Effects 0.000 claims description 8
- 238000012546 transfer Methods 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims 1
- 235000008331 Pinus X rigitaeda Nutrition 0.000 description 5
- 235000011613 Pinus brutia Nutrition 0.000 description 5
- 241000018646 Pinus brutia Species 0.000 description 5
- 230000004807 localization Effects 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 230000008878 coupling Effects 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 230000001419 dependent effect Effects 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000005622 photoelectricity Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 241000208340 Araliaceae Species 0.000 description 1
- 102000002274 Matrix Metalloproteinases Human genes 0.000 description 1
- 108010000684 Matrix Metalloproteinases Proteins 0.000 description 1
- 235000005035 Panax pseudoginseng ssp. pseudoginseng Nutrition 0.000 description 1
- 235000003140 Panax quinquefolius Nutrition 0.000 description 1
- 210000001367 artery Anatomy 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000001447 compensatory effect Effects 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 235000013399 edible fruits Nutrition 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 235000008434 ginseng Nutrition 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000012805 post-processing Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Computer Networks & Wireless Communication (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
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递推计算列车的位置、速度和姿态的具体过程为：
其中：pn表示列车位置信息，表示列车位置的变化率矢量，vn表示列车的速度信息，表示列车速度的变化率矢量，fn表示加速度计测量的列车加速度矢量信息，gn表示重力，表示在导航坐标系下陀螺仪测量的列车角速度矢量信息，表示在导航坐标系下地球自转引起的角速度变化矢量，表示姿态四元数，表示姿态四元数率，表示载体坐标系相对于导航坐标系的瞬时角速度矢量的四元数形式；
姿态转移矩阵的计算公式如下所示：
初始的姿态矩阵由给定的初始俯仰角、横滚角和航向角计算得到，计算公式如下：
其中：θ0表示初始俯仰角，γ0表示初始横滚角，ψ0表示初始航向角，表示初始姿态矩阵。
进一步地，所述的在卫星信号有效环境下，根据INS自身解算的列车位置与GNSS卫星位置计算卫星与列车之间的几何距离，包括：
卫星与列车之间的几何距离的计算公式为：
其中，dis表示INS自身解算的列车位置和GNSS卫星位置之间的几何距离，(sx,sy,sz)表示GNSS卫星在导航坐标系下的位置坐标，(rx,ry,rz)表示INS自身解算的列车在导航坐标系下的位置坐标。
进一步地，所述的计算OVS量测的速度与INS解算的列车的速度之间的速度差值，包括：
OVS量测的导航坐标系下的列车速度vOVS计算如下：
其中：vL表示光电速度传感器量测的列车在载体坐标系的左向速度，vF表示光电速度传感器量测的列车在载体坐标系的前向速度，表示列车从载体坐标系转向导航坐标系的姿态转移矩阵；
将OVS量测的导航坐标系下的列车速度vOVS与INS自身解算的速度vINS做差：vOVS-vINS。
进一步地，所述的将所述计算出的卫星与列车之间的几何距离，以及所述伪距差值、伪距率差值和速度差值作为***滤波的量测输入，采用标准卡尔曼滤波的滤波结果修正惯性导航***的误差，得到融合后的列车位置、速度信息和姿态信息，包括：
GNSS***状态由十七维组成，包括姿态误差、位置误差、三维的速度误差、陀螺仪误差以及加速度计误差，接收机钟差和钟漂；
GNSS***的状态向量X(t)为：
其中：(δrE,δrN,δrU)表示列车位置误差，(δvE,δvN,δvU)表示列车速度误差，表示列车姿态误差，表示陀螺仪测量的角速度误差，(εx,εy,εz)表示加速度计测量的加速度误差，dt表示卫星接收机的钟差，表示卫星接收的钟漂；
在GNSS/INS/OVS紧组合列车定位***中，将所述计算出的卫星与列车之间的几何距离，以及所述伪距差值、伪距率差值和速度差值作为***滤波的量测输入，***的量测矩阵Z为：
其中：ρ表示GNSS原始伪距量测，表示GNSS原始伪距率量测，表示INS解算的卫星到列车的几何距离，表示INS解算的卫星到列车的几何距离变化率，n表示可见卫星数，vINS表示惯导自身解算速度信息，vOVS表示在导航坐标系下的OVS量测速度；
采用标准卡尔曼滤波对列车的速度、位置状态进行估计，***的量测矩阵H为：
其中：c表示光速，Dn×3表示从卫星到列车接收机的方向余弦矩阵，On×3表示一个n行3列的零矩阵，On×9表示一个n行9列的零矩阵，On×1表示一个n行1列的零矩阵，,In×1表示一个n行1列的单位矩阵；
***的测量噪声协方差为：
进一步地，所述的方法还包括：
在卫星信号失效环境下，采用标准卡尔曼滤波算法利用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)
进一步地，所述的方法还包括：
所述标准卡尔曼滤波算法包括：时间更新和量测更新。
时间更新包括状态一步预测和一步预测均方误差的计算，计算公式分别如下：
P(k/k-1)＝F(k-1)P(k-1)F(k-1)T+Q(k-1)
其中，F(k-1)表示状态转移矩阵，表示k-1时刻的估计状态；表示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
P(k)＝[I-K(k)H(k/k-1)]P(k/k-1)
其中，H(k)表示***的量测矩阵，R(k)表示***量测噪声协方差矩阵，K(k)表示滤波增益，P(k)表示状态估计的均方误差，表示估计的状态，即为融合后的列车状态，由一步预测状态和量测更新得到的状态两部分组成，列车的状态包括列车的位置、速度、姿态、加速度计和陀螺仪的偏移。
由上述本发明的实施例提供的技术方案可以看出，本发明实施例的自适应容错的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递推计算列车的位置、速度和姿态的具体过程可以描述为：
其中：pn表示列车位置信息，表示列车位置的变化率矢量，vn表示列车的速度信息，表示列车速度的变化率矢量，fn表示加速度计测量的列车加速度矢量信息，gn表示重力，表示在导航坐标系下陀螺仪测量的列车角速度矢量信息，表示在导航坐标系下地球自转引起的角速度变化矢量，表示姿态四元数，表示姿态四元数率，表示载体坐标系相对于导航坐标系的瞬时角速度矢量的四元数形式。
姿态转移矩阵的计算公式如下所示：
q0,q1,q2,q3分别表示姿态四元数矩阵中的四个元素。
INS通过测量载体在惯性参考系的加速度和角速度，将他们对时间进行积分，之后变换到导航坐标系，得到载体在导航坐标系中的速度、航向角和位置。姿态矩阵描述了载体坐标系与导航坐标系的关系，实现载体坐标系到导航坐标系的转换。
初始位置、速度和姿态信息的精度会影响INS的自身递推结果精度，所以需要选取高精度的初始信息。初始的姿态矩阵由给定的初始俯仰角、横滚角和航向角计算得到。那么初始姿态矩阵计算公式如下所示：
其中：θ0表示初始俯仰角，γ0表示初始横滚角，ψ0表示初始航向角，表示初始姿态矩阵。
步骤S220：计算GNSS量测的伪距与INS解算的卫星与列车的几何距离之间的伪距差值，计算GNSS量测的伪距率与INS解算的列车的速度之间的伪距率差值，计算INS解算的列车的速度与OVS量测的速度之间的速度差值。
在卫星信号有效环境下，根据INS自身解算的列车位置与GNSS卫星位置计算卫星与列车之间的几何距离，计算GNSS量测的伪距与INS解算的卫星与列车的几何距离之间的差值，计算GNSS量测的伪距率与INS解算的列车的速度之间的差值，计算INS解算的列车的速度与OVS量测的速度之间的差值，将上述计算出的卫星与列车之间的几何距离，以及伪距差值、伪距率差值和速度差值作为***滤波的量测输入，采用标准卡尔曼滤波的滤波结果修正惯性导航***的误差。
由于OVS原始量测的列车速度为载体坐标系下的速度，为与GNSS、INS在导航坐标系下进行组合，需要将OVS原始量测的列车速度转化到导航坐标系下。OVS量测的导航坐标系下的列车速度vOVS计算如下：
其中：vL表示光电速度传感器量测的列车在载体坐标系的左向速度，vF表示光电速度传感器量测的列车在载体坐标系的前向速度，表示列车从载体坐标系转向导航坐标系的姿态转移矩阵。
将OVS量测的导航坐标系下的列车速度vOVS与INS自身解算的速度vINS做差：vOVS-vINS。
还需要计算GNSS量测的伪距与INS解算的卫星与列车的几何距离之间的伪距差值，计算GNSS量测的伪距率与INS解算的列车的速度之间的伪距率差值。
步骤S230：将计算出的卫星与列车之间的几何距离，以及所述伪距差值、伪距率差值和速度差值作为***滤波的量测输入，采用标准卡尔曼滤波的滤波结果修正惯性导航***的误差，得到融合后的列车位置、速度信息和姿态信息。
GNSS***状态由十七维组成，包括姿态误差(横滚、俯仰和偏航角误差)，位置误差(纬度，经度和高度误差)，三维的速度误差、陀螺仪误差以及加速度计误差，接收机钟差和钟漂。
GNSS***的状态向量X(t)为：
其中：(δrE,δrN,δrU)表示列车位置误差，(δvE,δvN,δvU)表示列车速度误差，表示列车姿态误差，表示陀螺仪测量的角速度误差，(εx,εy,εz)表示加速度计测量的加速度误差，dt表示卫星接收机的钟差，表示卫星接收的钟漂。
在GNSS/INS/OVS紧组合列车定位***中，***需要比较GNSS卫星原始伪距、伪距率与INS解算的对应的伪距、伪距率，比较INS自身计算的列车速度与OVS量测的列车速度，***的量测矩阵Z为：
其中：ρ表示GNSS原始伪距量测，表示GNSS原始伪距率量测，表示INS解算的卫星到列车的几何距离，表示INS解算的卫星到列车的几何距离变化率，n表示可见卫星数，vINS表示惯导自身解算速度信息，vOVS表示在导航坐标系下的OVS量测速度。
在满足列车定位要求的情况下，利用上述解算的伪距差值、伪距率差值和速度差值作为量测输入，采用标准卡尔曼滤波对列车的速度、位置等状态进行估计，那么***的量测矩阵H为：
其中：c表示光速，Dn×3表示从卫星到列车接收机的方向余弦矩阵，On×3表示一个n行3列的零矩阵，On×9表示一个n行9列的零矩阵，On×1表示一个n行1列的零矩阵，,In×1表示一个n行1列的单位矩阵。
由于基于GNSS接收机的伪距、伪距率量测精度分别为米级、分米/秒，OVS的速度量测精度为分米/秒，所以基于GNSS/INS/OVS的紧组合列车定位***中的测量噪声协方差可以写为：
卡尔曼滤波包括两个主要信息更新过程：时间更新和量测更新。
时间更新包括状态一步预测和一步预测均方误差的计算，计算公式分别如下：
P(k/k-1)＝F(k-1)P(k-1)F(k-1)T+Q(k-1)
其中，F(k-1)表示状态转移矩阵，表示k-1时刻的估计状态；表示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
P(k)＝[I-K(k)H(k/k-1)]P(k/k-1)
其中，H(k)表示***的量测矩阵，R(k)表示***量测噪声协方差矩阵，K(k)表示滤波增益，P(k)表示状态估计的均方误差，表示估计的状态，即为融合后的列车状态，由一步预测状态(惯性导航***自身递推得到的状态)和量测更新得到的状态(卡尔曼滤波估计得到的状态误差)两部分组成，列车的状态包括列车的位置、速度、姿态、加速度计和陀螺仪的偏移。
根据列车的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 (7)
1.一种基于伪距/伪距率的多传感器紧组合列车组合定位方法，其特征在于，包括：
由INS根据列车初始位置、速度和姿态信息，结合INS中加速度计和陀螺仪量测的列车加速度和角速率，自身递推计算出列车的位置、速度和姿态信息；
在卫星信号有效环境下，根据INS自身解算的列车位置与GNSS卫星位置计算卫星与列车之间的几何距离，计算GNSS量测的伪距与INS解算的卫星与列车的几何距离之间的伪距差值，计算GNSS量测的伪距率与INS解算的列车的速度之间的伪距率差值，计算INS解算的列车的速度与OVS量测的速度之间的速度差值；
将所述计算出的卫星与列车之间的几何距离，以及所述伪距差值、伪距率差值和速度差值作为***滤波的量测输入，采用标准卡尔曼滤波的滤波结果修正惯性导航***的误差，得到融合后的列车位置、速度信息和姿态信息。
2.根据权利要求1所述的方法，其特征在于，所述的计算GNSS量测的伪距与INS解算的卫星与列车的几何距离之间的伪距差值，计算GNSS量测的伪距率与INS解算的列车的速度之间的伪距率差值，计算INS解算的列车的速度与OVS量测的速度之间的速度差值，包括：
INS递推计算列车的位置、速度和姿态的具体过程为：
其中：pn表示列车位置信息，表示列车位置的变化率矢量，vn表示列车的速度信息，表示列车速度的变化率矢量，fn表示加速度计测量的列车加速度矢量信息，gn表示重力，表示在导航坐标系下陀螺仪测量的列车角速度矢量信息，表示在导航坐标系下地球自转引起的角速度变化矢量，表示姿态四元数，表示姿态四元数率，表示载体坐标系相对于导航坐标系的瞬时角速度矢量的四元数形式；
姿态转移矩阵的计算公式如下所示：
初始的姿态矩阵由给定的初始俯仰角、横滚角和航向角计算得到，计算公式如下：
其中：θ0表示初始俯仰角，γ0表示初始横滚角，ψ0表示初始航向角，表示初始姿态矩阵。
3.根据权利要求2所述的方法，其特征在于，所述的在卫星信号有效环境下，根据INS自身解算的列车位置与GNSS卫星位置计算卫星与列车之间的几何距离，包括：
卫星与列车之间的几何距离的计算公式为：
其中，dis表示INS自身解算的列车位置和GNSS卫星位置之间的几何距离，(sx,sy,sz)表示GNSS卫星在导航坐标系下的位置坐标，(rx,ry,rz)表示INS自身解算的列车在导航坐标系下的位置坐标。
4.根据权利要求3所述的方法，其特征在于，所述的计算OVS量测的速度与INS解算的列车的速度之间的速度差值，包括：
OVS量测的导航坐标系下的列车速度vOVS计算如下：
其中：vL表示光电速度传感器量测的列车在载体坐标系的左向速度，vF表示光电速度传感器量测的列车在载体坐标系的前向速度，表示列车从载体坐标系转向导航坐标系的姿态转移矩阵；
将OVS量测的导航坐标系下的列车速度vOVS与INS自身解算的速度vINS做差：vOVS-vINS。
5.根据权利要求4所述的方法，其特征在于，所述的将所述计算出的卫星与列车之间的几何距离，以及所述伪距差值、伪距率差值和速度差值作为***滤波的量测输入，采用标准卡尔曼滤波的滤波结果修正惯性导航***的误差，得到融合后的列车位置、速度信息和姿态信息，包括：
GNSS***状态由十七维组成，包括姿态误差、位置误差、三维的速度误差、陀螺仪误差以及加速度计误差，接收机钟差和钟漂；
GNSS***的状态向量X(t)为：
其中：(δrE,δrN,δrU)表示列车位置误差，(δvE,δvN,δvU)表示列车速度误差，表示列车姿态误差，表示陀螺仪测量的角速度误差，(εx,εy,εz)表示加速度计测量的加速度误差，dt表示卫星接收机的钟差，表示卫星接收的钟漂；
在GNSS/INS/OVS紧组合列车定位***中，将所述计算出的卫星与列车之间的几何距离，以及所述伪距差值、伪距率差值和速度差值作为***滤波的量测输入，***的量测矩阵Z为：
其中：ρ表示GNSS原始伪距量测，表示GNSS原始伪距率量测，表示INS解算的卫星到列车的几何距离，表示INS解算的卫星到列车的几何距离变化率，n表示可见卫星数，vINS表示惯导自身解算速度信息，vOVS表示在导航坐标系下的OVS量测速度；
采用标准卡尔曼滤波对列车的速度、位置状态进行估计，***的量测矩阵H为：
其中：c表示光速，Dn×3表示从卫星到列车接收机的方向余弦矩阵，On×3表示一个n行3列的零矩阵，On×9表示一个n行9列的零矩阵，On×1表示一个n行1列的零矩阵，,In×1表示一个n行1列的单位矩阵；
***的测量噪声协方差为：
6.根据权利要求5所述的方法，其特征在于，所述的方法还包括：
在卫星信号失效环境下，采用标准卡尔曼滤波算法利用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)。
7.根据权利要求5或者6所述的方法，其特征在于，所述的方法还包括：
所述标准卡尔曼滤波算法包括：时间更新和量测更新。
时间更新包括状态一步预测和一步预测均方误差的计算，计算公式分别如下：
P(k/k-1)＝F(k-1)P(k-1)F(k-1)T+Q(k-1)
其中，F(k-1)表示状态转移矩阵，表示k-1时刻的估计状态；表示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
P(k)＝[I-K(k)H(k/k-1)]P(k/k-1)
其中，H(k)表示***的量测矩阵，R(k)表示***量测噪声协方差矩阵，K(k)表示滤波增益，P(k)表示状态估计的均方误差，表示估计的状态，即为融合后的列车状态，由一步预测状态和量测更新得到的状态两部分组成，列车的状态包括列车的位置、速度、姿态、加速度计和陀螺仪的偏移。
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 true CN109471144A (zh) | 2019-03-15 |
| CN109471144B 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) |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110106755A (zh) * | 2019-04-04 | 2019-08-09 | 武汉大学 | 利用姿态重构铁轨几何形态的高铁轨道不平顺性检测方法 |
| CN110133700A (zh) * | 2019-05-27 | 2019-08-16 | 上海海事大学 | 一种船载综合导航定位方法 |
| CN110567456A (zh) * | 2019-09-03 | 2019-12-13 | 兰州交通大学 | 基于抗差卡尔曼滤波的bds/ins组合列车定位方法 |
| CN110645979A (zh) * | 2019-09-27 | 2020-01-03 | 北京交通大学 | 基于gnss/ins/uwb组合的室内外无缝定位方法 |
| CN110850447A (zh) * | 2019-11-11 | 2020-02-28 | 北京交通大学 | 对列车定位单元的定位精度进行综合评估的方法 |
| CN111077555A (zh) * | 2020-03-24 | 2020-04-28 | 北京三快在线科技有限公司 | 一种定位方法及装置 |
| CN112722013A (zh) * | 2021-01-22 | 2021-04-30 | 北京交通大学 | 一种列车定位股道判别方法 |
| CN113203418A (zh) * | 2021-04-20 | 2021-08-03 | 同济大学 | 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及*** |
| CN114353835A (zh) * | 2022-01-21 | 2022-04-15 | 中国铁道科学研究院集团有限公司铁道建筑研究所 | 惯性轨道测量仪动态校准***、方法及其应用 |
| CN114964220A (zh) * | 2022-04-06 | 2022-08-30 | 广东省国土资源测绘院 | 一种无人机定位、定姿数据采集方法、装置及无人机 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20100109945A1 (en) * | 2008-11-06 | 2010-05-06 | Texas Instruments Incorporated | Loosely-coupled integration of global navigation satellite system and inertial navigation system: speed scale-factor and heading bias calibration |
| 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 | 中车株洲电力机车研究所有限公司 | 一种安全可靠的实时测速和连续定位的方法、装置及*** |
-
2018
- 2018-12-13 CN CN201811525963.8A patent/CN109471144B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20100109945A1 (en) * | 2008-11-06 | 2010-05-06 | Texas Instruments Incorporated | Loosely-coupled integration of global navigation satellite system and inertial navigation system: speed scale-factor and heading bias calibration |
| 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)
| Title |
|---|
| 周阳林 等: "一种 GNSS/INS/LiDAR 组合导航传感器安置关系快速标定方法", <<中国惯性技术学报 >> * |
Cited By (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110106755A (zh) * | 2019-04-04 | 2019-08-09 | 武汉大学 | 利用姿态重构铁轨几何形态的高铁轨道不平顺性检测方法 |
| CN110133700A (zh) * | 2019-05-27 | 2019-08-16 | 上海海事大学 | 一种船载综合导航定位方法 |
| CN110133700B (zh) * | 2019-05-27 | 2023-01-31 | 上海海事大学 | 一种船载综合导航定位方法 |
| CN110567456A (zh) * | 2019-09-03 | 2019-12-13 | 兰州交通大学 | 基于抗差卡尔曼滤波的bds/ins组合列车定位方法 |
| CN110645979B (zh) * | 2019-09-27 | 2021-09-21 | 北京交通大学 | 基于gnss/ins/uwb组合的室内外无缝定位方法 |
| CN110645979A (zh) * | 2019-09-27 | 2020-01-03 | 北京交通大学 | 基于gnss/ins/uwb组合的室内外无缝定位方法 |
| CN110850447B (zh) * | 2019-11-11 | 2022-03-25 | 北京交通大学 | 对列车定位单元的定位精度进行综合评估的方法 |
| CN110850447A (zh) * | 2019-11-11 | 2020-02-28 | 北京交通大学 | 对列车定位单元的定位精度进行综合评估的方法 |
| CN111077555A (zh) * | 2020-03-24 | 2020-04-28 | 北京三快在线科技有限公司 | 一种定位方法及装置 |
| CN112722013A (zh) * | 2021-01-22 | 2021-04-30 | 北京交通大学 | 一种列车定位股道判别方法 |
| CN112722013B (zh) * | 2021-01-22 | 2022-03-25 | 北京交通大学 | 一种列车定位股道判别方法 |
| CN113203418A (zh) * | 2021-04-20 | 2021-08-03 | 同济大学 | 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及*** |
| CN114353835A (zh) * | 2022-01-21 | 2022-04-15 | 中国铁道科学研究院集团有限公司铁道建筑研究所 | 惯性轨道测量仪动态校准***、方法及其应用 |
| CN114964220A (zh) * | 2022-04-06 | 2022-08-30 | 广东省国土资源测绘院 | 一种无人机定位、定姿数据采集方法、装置及无人机 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109471144B (zh) | 2023-04-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109471144A (zh) | 基于伪距/伪距率的多传感器紧组合列车组合定位方法 | |
| US9921065B2 (en) | Unit and method for improving positioning accuracy | |
| CN109059909A (zh) | 基于神经网络辅助的卫星/惯导列车定位方法与*** | |
| CN108426574A (zh) | 一种基于zihr的航向角修正算法的mems行人导航方法 | |
| CN101858748B (zh) | 高空长航无人机的多传感器容错自主导航方法 | |
| CN100585602C (zh) | 惯性测量***误差模型验证试验方法 | |
| CN108267135A (zh) | 用于轨道自动测量车的精确定位方法及*** | |
| CN108931244B (zh) | 基于列车运动约束的惯导误差抑制方法及*** | |
| Jiang et al. | A fault-tolerant tightly coupled GNSS/INS/OVS integration vehicle navigation system based on an FDP algorithm | |
| JP4090852B2 (ja) | Ｇｐｓ測位による列車走行情報検出装置及びその列車走行情報検出方法 | |
| CN104991266B (zh) | 一种基于协同完好性监测的列车卫星定位方法及*** | |
| CN108226985A (zh) | 基于精密单点定位的列车组合导航方法 | |
| CN104181572A (zh) | 一种弹载惯性/卫星紧组合导航方法 | |
| CN106405670A (zh) | 一种适用于捷联式海洋重力仪的重力异常数据处理方法 | |
| CN107525505A (zh) | 机车轮对空转及滑行检测方法及*** | |
| JP2007284013A (ja) | 車両位置測位装置及び車両位置測位方法 | |
| CN109471143A (zh) | 自适应容错的列车组合定位方法 | |
| Gao et al. | Development of precise GPS/INS/wheel speed sensor/yaw rate sensor integrated vehicular positioning system | |
| US20230182790A1 (en) | Method for calculating an instantaneous velocity vector of a rail vehicle and corresponding system | |
| CN106123917B (zh) | 考虑外杆臂效应的捷联惯导***罗经对准方法 | |
| CN111044051A (zh) | 一种复合翼无人机容错组合导航方法 | |
| CN105758427A (zh) | 一种基于动力学模型辅助的卫星完好性监测方法 | |
| US20230026395A1 (en) | System and method for computing positioning protection levels | |
| CN113758483A (zh) | 一种自适应fkf地图匹配方法及*** | |
| CN103575297A (zh) | 基于卫星导航接收机的gnss和mimu组合导航航向角估计方法 |
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 |