CN113165678B - 用于确定载运工具的位置的方法和装置 - Google Patents
用于确定载运工具的位置的方法和装置 Download PDFInfo
- Publication number
- CN113165678B CN113165678B CN201980077621.0A CN201980077621A CN113165678B CN 113165678 B CN113165678 B CN 113165678B CN 201980077621 A CN201980077621 A CN 201980077621A CN 113165678 B CN113165678 B CN 113165678B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- bayesian estimation
- estimation filter
- computer
- track
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 49
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 112
- 238000012544 monitoring process Methods 0.000 claims abstract description 4
- 238000005259 measurement Methods 0.000 claims description 116
- 230000006870 function Effects 0.000 claims description 92
- 230000033001 locomotion Effects 0.000 claims description 18
- 238000004590 computer program Methods 0.000 claims description 4
- 230000008569 process Effects 0.000 description 13
- 238000012545 processing Methods 0.000 description 13
- 239000011159 matrix material Substances 0.000 description 12
- 238000004891 communication Methods 0.000 description 10
- 230000010354 integration Effects 0.000 description 6
- 230000004927 fusion Effects 0.000 description 5
- 230000000875 corresponding effect Effects 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 238000009434 installation Methods 0.000 description 3
- 238000012423 maintenance Methods 0.000 description 3
- 230000011664 signaling Effects 0.000 description 3
- 230000000007 visual effect Effects 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 230000010355 oscillation Effects 0.000 description 2
- 230000002093 peripheral effect Effects 0.000 description 2
- 235000021018 plums Nutrition 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 101000606504 Drosophila melanogaster Tyrosine-protein kinase-like otk Proteins 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000002596 correlated effect Effects 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005538 encapsulation Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000003909 pattern recognition Methods 0.000 description 1
- 238000000513 principal component analysis Methods 0.000 description 1
- 230000008054 signal transmission Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 239000000725 suspension Substances 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 230000002123 temporal effect Effects 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
- 230000001960 triggered effect Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B61—RAILWAYS
- B61L—GUIDING RAILWAY TRAFFIC; ENSURING THE SAFETY OF RAILWAY TRAFFIC
- B61L25/00—Recording or indicating positions or identities of vehicles or trains or setting of track apparatus
- B61L25/02—Indicating or recording positions or identities of vehicles or trains
- B61L25/025—Absolute localisation, e.g. providing geodetic coordinates
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B61—RAILWAYS
- B61L—GUIDING RAILWAY TRAFFIC; ENSURING THE SAFETY OF RAILWAY TRAFFIC
- B61L25/00—Recording or indicating positions or identities of vehicles or trains or setting of track apparatus
- B61L25/02—Indicating or recording positions or identities of vehicles or trains
- B61L25/023—Determination of driving direction of vehicle or train
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B61—RAILWAYS
- B61L—GUIDING RAILWAY TRAFFIC; ENSURING THE SAFETY OF RAILWAY TRAFFIC
- B61L25/00—Recording or indicating positions or identities of vehicles or trains or setting of track apparatus
- B61L25/02—Indicating or recording positions or identities of vehicles or trains
- B61L25/021—Measuring and recording of train speed
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B61—RAILWAYS
- B61L—GUIDING RAILWAY TRAFFIC; ENSURING THE SAFETY OF RAILWAY TRAFFIC
- B61L25/00—Recording or indicating positions or identities of vehicles or trains or setting of track apparatus
- B61L25/02—Indicating or recording positions or identities of vehicles or trains
- B61L25/026—Relative localisation, e.g. using odometer
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mechanical Engineering (AREA)
- Navigation (AREA)
Abstract
提供了一种确定载运工具在运输网络内的位置的计算机实现的方法。方法包括：获得指示运输网络的至少一部分的轨道几何形状的轨道几何数据；基于轨道几何数据来确定载运工具正在接近接合点；基于轨道几何数据来确定来自接合点的多个路线选项；生成多个贝叶斯估计滤波器算法34，每个贝叶斯估计滤波器算法与多个路线选项中的相应一个路线选项相关联，并且被配置为基于指示相关联的路线选项的轨道几何数据来估计载运工具的位置，其中，多个贝叶斯估计滤波器算法被配置为输出指示载运工具采用相关联的路线选项的概率的数据；当载运工具经过接合点时，监测多个贝叶斯估计滤波器算法的输出；以及通过基于多个贝叶斯估计滤波器算法的输出选择多个路线选项中呈现最高概率的一个路线选项来确定载运工具采用的路线选项。
Description
技术领域
本申请涉及一种用于确定载运工具在运输网络内的位置的方法和装置。更具体地但非排他性地，本申请涉及一种用于通过使用贝叶斯估计滤波器算法而不依赖轨道侧基础设施来确定载运工具通过运输网络的接合点的路线的方法和装置。
背景技术
在传输网络的网络接合点处，多个轨道彼此相交。因此，网络接合点呈现多个路线选项供载运工具采用。重要的是要知道载运工具通过网络接合点所采用的特定路线，以便允许载运工具通过网络接合点的无缝操作。当前，该信息经由轨道侧基础设施提供。
轨道侧基础设施包括例如应答器或信标，它们沿着轨道安装在固定位置。轨道侧基础设施能够检测载运工具的通过，并且一般沿着由网络接合点提供的每个路线选项可用。轨道侧基础设施可以向网络信令系统或载运工具发送指示载运工具已经通过特定位置的信号。因此，通过使用轨道侧基础设施，网络信令系统或载运工具本身能够确定载运工具通过网络接合点所采用的特定路线选项。轨道侧基础设施的安装和维护成本很高。另外，轨道侧基础设施的使用取决于与载运工具或网络信令系统的通信链路。因此，使用轨道侧基础设施的路线检测方法不是自主的。
尽管载运工具通常携带全球导航卫星系统(GNSS)传感器，但是GNSS传感器在许多常见的操作环境(例如，隧道)中缺乏信号可用性和连续性。因此，仅依赖GNSS传感器来定位载运工具一般是不够的。
因此，需要提供一种用于在不利用轨道侧基础设施的情况下可靠地确定载运工具通过运输网络的接合点所采用的路线的系统和方法。
发明内容
根据本文描述的第一方面，提供了一种确定载运工具在运输网络内的位置的计算机实现的方法，包括：获得指示运输网络的至少一部分的轨道几何形状的轨道几何数据；基于轨道几何数据来确定载运工具正在接近接合点；基于轨道几何数据来确定来自该接合点的多个路线选项；生成多个贝叶斯估计滤波器算法，每个贝叶斯估计滤波器算法与多个路线选项中的相应一个路线选项相关联，并且被配置为基于指示相关联的路线选项的轨道几何数据来估计载运工具的位置，其中，多个贝叶斯估计滤波器算法被配置为输出指示载运工具采用相关联的路线选项的概率的数据；当载运工具经过接合点时，监测多个贝叶斯估计滤波器算法的输出；以及通过基于多个贝叶斯估计滤波器算法的输出选择多个路线选项中呈现最高概率的一个路线选项来确定载运工具所采用的路线选项。
运输网络可以是引导式运输网络(例如，铁路、电车轨道)。引导式运输网络具有在正常操作中严格限制载运工具(例如，火车、电车)的移动的轨道。
运输网络还可以是具有供载运工具(例如，汽车、货车)遵循的轨道(例如，道路或路线)的道路或高速公路网络，或者是具有约束载运工具(例如，船)采用的路线的轨道(例如，运河)的水路网络。
一般地，运输网络对载运工具的位置和移动施加轨道约束。例如，除非出现严重的问题，否则预期载运工具沿着运输网络提供的轨道/路线的曲率运动。顾名思义，轨道几何数据是指示运输网络的轨道几何形状的数据，因此定义了运输网络施加给载运工具的轨道约束。
贝叶斯估计滤波器算法通常用于基于间接测量状态的传感器测量结果来估计系统的状态。贝叶斯估计滤波器算法可以与“传感器融合算法”或“最佳估计算法”互换使用。
有利地，使用多个贝叶斯估计滤波器算法来求解载运工具采用的特定路线选项，每个贝叶斯估计滤波器算法与由接合点呈现的多个路线选项中的相应一个路线选项相关联。因此，减少或消除了对提供轨道侧基础设施以消除载运工具通过接合点所采用的路线选项的歧义的需求，并且当轨道侧基础设施在网络接合点局部不可用时，可以确定路线。这允许载运工具通过接合点的无缝行进，并且可以在轨道侧基础设施的采购、安装和维护方面为网络运营商显著节省成本。
指示载运工具采用相关联的路线选项的概率的数据可以包括由多个贝叶斯估计滤波器算法计算的创新值。
创新是在贝叶斯估计滤波器算法的更新步骤中计算出的参数。一般地，创新值表示馈送给贝叶斯估计滤波器算法的测量结果和经预测的测量结果之间的差异。
多个贝叶斯估计滤波器算法可以被配置为基于相关联的路线选项的伪测量来计算创新值。可以基于指示相关联的路线选项的轨道几何数据来确定相关联的路线选项的伪测量。
伪测量可以包括载运工具沿着由指示路线选项之一的轨道几何数据限定的轨道的姿态的伪测量。
可以基于在载运工具的估计位置处由轨道几何数据限定的轨道的切线来计算载运工具的姿态的伪测量。
多个贝叶斯估计滤波器算法可以被配置为基于相关联的路线选项的伪测量和载运工具的各个估计位置来计算创新值。
将理解的是，创新值是以下二者之间的差：伪测量、以及载运工具的估计状态和测量模型的乘积。载运工具的估计状态由贝叶斯估计滤波器算法生成，并且包括载运工具的估计位置。测量模型被布置为将估计状态所处的状态空间映射到伪测量所处的测量空间。
选择多个路线选项中呈现最高概率的一个路线选项可以包括选择多个路线选项中实现最低创新值的一个路线选项。
选择多个路线选项中呈现最高概率的一个路线选项可以包括：将多个贝叶斯估计滤波器算法计算出的创新值与预定阈值进行比较，并且选择多个路线选项中呈现比预定阈值低的创新值的一个路线选项。
多个贝叶斯估计滤波器算法可以被配置为使得载运工具的估计位置沿着由轨道几何数据指示的相关联的路线选项。
也就是说，多个贝叶斯估计滤波器算法可以由轨道几何数据约束，并且载运工具的估计位置可以被约束在运输网络中由轨道几何数据限定的轨道上。这可以通过各种方式来实现。在示例中，可以将算法输出的最终估计位置(不受约束)正交投影到由轨道几何数据限定的轨道上，以便获得受约束的解决方案。在备选示例中，轨道几何数据对算法的估计状态的时间演变施加隐式的一维运动约束。
多个贝叶斯估计滤波器算法可以彼此并行地执行。
载运工具可以承载至少一个传感器，该至少一个传感器被布置为输出指示载运工具的运动的信号。多个贝叶斯估计滤波器算法可以被配置为基于至少一个传感器的输出信号来估计载运工具的位置。
至少一个传感器可以包括惯性测量单元IMU和除了IMU之外的传感器。
可以从以下至少一个组成的组中选择除了IMU之外的传感器：GNSS传感器、应答器读取器、雷达、速度传感器、里程表、摄像机和激光雷达探测器。IMU可以是低成本的MEMSIMU。
确定载运工具正在接近运输网络的接合点可以包括：执行第一贝叶斯估计滤波器算法，以基于轨道几何数据来估计载运工具的位置；以及确定载运工具的估计位置正在接近运输网络的接合点。
可以执行第一贝叶斯估计滤波器算法以：
在预测步骤中，至少基于IMU输出的第一传感器数据和轨道几何数据来预测载运工具的位置，使得载运工具的预测位置位于由轨道几何数据限定的轨道上；以及
在更新步骤中，至少基于由除了IMU之外的传感器输出的第二传感器数据来更新载运工具的预测位置，并且
其中，载运工具的估计位置基于以下至少一项：载运工具的预测位置，和载运工具的经更新的预测位置。
第一贝叶斯估计滤波器算法可以包括捷联惯性导航算法。该方法还可以包括：在预测步骤中基于轨道几何数据和第一传感器数据来执行捷联惯性导航算法。捷联惯性导航算法可以被配置为：至少基于第一传感器数据来计算IMU沿着由轨道几何数据限定的轨道的速度，随时间对速度进行积分，并且至少基于轨道几何数据和速度的积分来生成IMU的预测位置。
以这种方式通过轨道几何数据约束捷联惯性导航算法将估计载运工具的不受约束的三维位置的问题减小为估计载运工具沿着运输网络的轨道的一维位置的问题，因为载运工具沿着轨道仅有一个自由度。
有利地，通过使捷联惯性导航算法受轨道几何数据的约束，载运工具的预测位置被约束在运输网络中由轨道几何数据限定的轨道上。此外，捷联惯性导航算法对运动学状态估计误差到一维位置解空间的传播进行建模，并且允许考虑在轨道几何数据中限定的轨道的瞬时曲率和切线信息。
生成多个贝叶斯估计滤波器算法可以包括生成第一贝叶斯估计滤波器算法的多个副本。
该计算机实现的方法还可以包括：至少基于第二传感器数据和相关联的路线选项的伪测量，在更新步骤中执行贝叶斯估计滤波器算法的至少一个副本以更新载运工具的预测位置。
也就是说，从轨道几何数据导出的伪测量可以与第二传感器数据一起用于更新载运工具的预测位置。
该计算机实现的方法还可以包括删除与未选择的路线选项相关联的第一贝叶斯估计滤波器算法的副本。
以此方式，在载运工具通过接合点后，仅保留与所选择的路线选项相关联的副本。贝叶斯估计滤波器算法的其余副本用于(作为第一贝叶斯估计滤波器算法)继续估计载运工具的位置，以确定载运工具是否正在接近下一接合点。
多个贝叶斯估计滤波器算法可以包括无迹卡尔曼滤波器。
在处理非线性过程模型的所有贝叶斯估计滤波器算法中，无迹卡尔曼滤波器的优点在于，它在处理非线性方面更好，并且不需要对过程模型进行显式的或数值近似的雅可比估计。
多个贝叶斯估计滤波器算法可以包括李群无迹卡尔曼滤波器。也就是说，贝叶斯估计滤波器算法可以在由李群(尤其是矩阵李群)表示的状态空间和/或测量空间中操作。使用李群表示状态空间和测量空间是有利的，因为李群可以使用乘积矩阵李群轻松地表示包括多个子状态的复杂状态，而不会丢失状态空间的类型结构。
获得轨道几何数据可以包括：访问地图数据库，其中，地图数据库包括沿着运输网络内的轨道定位的样本点；从地图数据库中检索载运工具附近的样本点；以及通过检索到的样本点应用插值函数以获得轨道约束函数，其中，轨道几何数据包括轨道约束函数。
有利地，通过对从地图数据库中检索到的样本点应用插值函数，轨道约束函数(其包括在轨道几何数据内)包括表示运输网络内的轨道的中心线的线/曲线。因此，轨道约束函数能够表示载运工具的潜在路线、载运工具的潜在方向以及要在贝叶斯估计滤波器算法内施加的运动学约束，以在没有绝对位置信息的源可用时，改善对载运工具的运动学状态(例如，位置)的误差估计。应当理解，样本点的密度(其可以与“支撑点”互换使用)影响轨道约束函数的精度。
轨道约束函数可以包括二次微分曲线，该二次微分曲线具有检索到的样本点中的至少一个样本点的连续二阶导数。
有利地，与使用相同数量的样本点的常规分段线性插值相比，具有连续二阶导数的二次微分曲线允许更精确地对运输网络内的轨道(尤其是轨道中心线)的曲率和切线信息进行建模。因此，轨道约束函数在没有间断的情况下对要估计的载运工具的运动学状态提供了有力的约束，并且改善了对贝叶斯估计滤波器算法的输出的误差估计。
插值函数可以包括三次样条函数。
有利地，三次样条插值法隐式地提供了具有连续二阶导数的二次微分曲线。此外，在所有二次微分函数中，三次样条插值函数生成最小的应变能范数，并且允许由此获得的轨道约束函数在样本点之间具有最小振荡的曲线级数。
载运工具可以包括火车或电车之一。
根据本文描述的第二方面，提供了一种计算机程序，该计算机程序包括计算机可读指令，该计算机可读指令被配置为使计算机执行根据本文描述的第一方面所述的方法。
根据本文所描述的第三方面，提供了一种计算机可读介质，承载根据本文所描述的第二方面的计算机程序。
根据本文描述的第四方面，提供了一种用于确定载运工具在运输网络内的位置的装置，包括：存储器，存储处理器可读指令；以及处理器，被布置为读取和执行存储在存储器中的处理器可读指令，其中，处理器可读指令包括被布置为控制处理器执行根据本文描述的第一方面所述的方法的指令。
该装置可以附接到载运工具。
在适当的情况下，以上关于本文描述的各方面之一所描述的任何可选特征可以适用于本文描述的各方面中的另一个方面。
附图说明
现在参考附图仅通过示例的方式来描述实施例，在附图中：
图1示意性地示出了运输网络的模型的一部分；
图2示意性地示出了存储图1的运输网络的模型的地图数据库；
图3示意性地示出了在图1的运输网络内行进的载运工具；
图4示意性地示出了由图3的载运工具承载的控制器的结构；
图5示意性地示出了由图4的控制器执行的用于估计载运工具的位置的贝叶斯估计滤波器的示例；
图6示意性地示出了在图5的贝叶斯估计滤波器中使用的受约束的捷联惯性导航系统的功能组件；
图7是可以由服务器执行的以便促进通过弧长参数化的三次样条的计算的示例处理步骤的流程图；
图8示意性地示出了载运工具采用由接合点呈现的第一路线选项的假设；
图9示意性地示出了载运工具采用由接合点呈现的第二路线选项的假设；
图10示出了当载运工具采用第二路线选项时计算出的图8和图9的假设的概率；
图11示出了当载运工具采用第一路线选项时计算出的图8和图9的假设的概率；
图12是可以由图4的控制器执行以便解决载运工具通过接合点采用的路线选项的示例处理步骤的流程图；
图13示意性地示出了可由图4的控制器执行的用于估计载运工具的位置的贝叶斯估计滤波器的一般结构。
在附图中，相同的部分由相同的附图标记表示。
将理解的是，附图仅出于说明目的，并且未按比例绘制。
具体实施方式
图1示意性地示出了运输网络的模型。运输网络可以是引导式运输网络(例如，铁路、电车轨道)，其具有在正常操作中严格限制载运工具(例如，火车、电车)的移动的轨道。备选地，运输网络可以是道路或高速公路网络，其具有供载运工具(例如，汽车、货车)遵循的轨道(例如，道路或路线)。备选地，运输网络可以是具有约束载运工具(例如，船)采用的路线的轨道(例如，运河)的水路。在以下示例性实施例中，一般假设运输网络是铁路，但是应当理解，本文描述的技术不限于此。
运输网络由顶点1-8和边11-17表示。顶点1-8沿着运输网络的轨道定位。在示例中，顶点的位置是先验的。顶点可以与沿着铁路轨道安装的固定的轨道侧基础设施(例如，应答器)的位置无关。在以下描述中，顶点也可以被称为支撑点。边11-17与顶点1-8之间的轨道相对应。经由边11-17连接的顶点序列形成用于载运工具的路径。路径的精度由顶点的密度确定。
在当前描述的示例中，图1所示的模型以数字方式存储在地图数据库24中。地图数据库24也可以被称为支撑点数据库。图2提供了地图数据库24的配置的示例。
在图2的示例中，地图数据库24包括四种类型的数据实体，即，边连接20、边21、边顶点22和顶点23。图2中的连接都是一对多的，其中单破折号末端代表一端。数据实体20-23中的每一个具有“ID”属性。边连接实体20还包括“来自ID”属性、“来自末端”属性和“至ID”属性。边21实体还包括“名称”属性。边顶点实体还包括“边ID”属性、“顶点ID”属性、“顶点顺序”属性和“弧长”属性。顶点实体23还包括“位置”属性(存储诸如各个顶点的以地球为中心的地球固定(ECEF)位置之类的位置数据)和“名称”属性。图1的顶点1-8由顶点实体23的实例表示。边实体21的实例可以包括接合点之间的所有顶点。例如，可以为图1中所示的运输网络提供边实体21的三个实例，其中第一实例包括顶点1、2、3、4，第二实例包括顶点4、5、7，并且第三实例包括顶点4、6、8。一般地，将理解，以下对边连接20、边21、边顶点22和顶点23的引用是指对应实体的实例。
边顶点22将边21与顶点23相关联，顶点23位于边21中的每个相应边上或位于边21中的每个相应边的末端处。在示例中，边顶点实体22的“弧长”属性指示从边21的实例的起点(或终点)到包括在该实例内的顶点的弧长。在备选示例中，该参数指示从整个路径(边21的实例是该路径的一部分)的起点到顶点的弧长。弧长属性提供通过顶点23的轨道的弧长。
边连接20定义了边21之间的连接。例如，边连接实体20的实例可以在边实体21包括顶点1、2、3、4的第一实例和边实体21包括顶点4、5、7的第二实例以及边实体21包括顶点4、6、8的第三实例之间定义“连接到的”关系。
地图数据库24可以本地存储在载运工具10(在图1的模型中示出其表示)上，载运工具10在运输网络内行进，使得载运工具可以访问地图数据库，而无需无线通信信道。备选地，地图数据库24可以存储在远离载运工具10的服务器(未示出)处。将理解的是，地图数据库24可以采取任何合适的配置，并且不限于图2提供的示例。在示例中，使用符合SQL的数据库来实现地图数据库24。然而，将理解，地图数据库24可以以任何方式实现并且不需要是关系式的。例如，数据库24可以包括平面文件。
图3示意性地示出了载运工具10。载运工具10承载控制器9和传感器组。传感器组包括用于监测载运工具10的运动的传感器。传感器组包括至少一个传感器27和至少一个传感器25，传感器27提供指示载运工具的参考位置的信息，传感器25提供指示载运工具的绝对位置的信息。
传感器25的示例包括GNSS传感器、应答器读取器、摄像机和激光雷达探测器。传感器25依赖于载运工具10和外部源(例如，沿着运输网络的轨道安装的卫星或应答器，或包含用于场景匹配的信息的数据库)之间的数据传输，来提供载运工具10的绝对位置信息。
传感器27包括惯性测量单元(IMU)26，并且可以包括其他传感器(例如，雷达、速度传感器或里程表)。IMU 26可以具有六个自由度(DOF)配置，并且对于三个正交轴中的每个正交轴具有一个陀螺仪和一个加速度计。加速度计用于检测比力，陀螺仪用于检测角速率。应当理解，IMU 26可以采用更简单的配置。里程表可以包括车轮里程表、视觉里程表或雷达里程表。一般地，里程表估计载运工具10的速度。IMU 26可以是低成本的微机电系统(MEMS)IMU。
传感器27允许航位推测导航并且不依赖于载运工具10和外部源之间的信号传输。然而，在给定初始条件集的情况下，传感器27提供可用于计算载运工具10的相对位置的信息。此外，传感器27容易受累积误差的影响，可以对传感器27进行常规的校准程度以便维持质量。
图4示意性地示出了控制器9的示例性结构。控制器9包括CPU 9a，CPU 9a被配置为读取并执行存储在RAM存储器9b中的指令，RAM存储器9b可以是易失性存储器。RAM 9b存储用于由CPU 9a执行的指令和那些指令使用的数据。控制器9还包括非易失性存储设备9c，例如硬盘驱动器，但是应当理解，可以使用任何其他形式的非易失性存储设备。用于由CPU 9a执行的计算机可读指令可以存储在非易失性存储设备9c中。此外，非易失性存储设备9c可以存储地图数据库24的副本。
控制器9还包括I/O接口9d，与控制器9结合使用的外围设备可以连接到该I/O接口9d。外围设备可以包括键盘、数据存储设备等。还可以提供通信接口9i。通信接口9i可以提供到其他设备(例如，传感器25、27)的短距离连接。短距离连接可以经由蓝牙、近场通信(NFC)等。通信接口9i还可以提供到诸如互联网或卫星之类的网络的连接，以进行更长距离的通信。如果地图数据库24存储在远离载运工具10的服务器中，则更长距离的通信可以用于检索地图数据库24。更长距离的通信还可以由GNSS传感器使用以生成绝对位置信息。CPU9a、RAM 9b、非易失性存储设备9c、I/O接口9d和通信接口9i通过总线9j连接在一起。
应当理解，图4所示的组件的布置仅是示例性的，并且控制器9可以包括与图4所示的组件不同的、附加的或更少的组件。
存储在非易失性存储设备9c中的计算机可读指令提供如图5至图6所示的功能组件。计算机可读指令在由CPU 9a执行时，使控制器9执行图7和图12所示的处理步骤，以确定载运工具10通过接合点采用的路线。
具体地，如图5所示，计算机可读指令提供数据服务器31、轨道约束管理器32和导航滤波器34，导航滤波器34也可以被称为贝叶斯估计滤波器。在图5的具体示例中，导航滤波器34被实现为无迹卡尔曼滤波器。导航滤波器24包括状态预测器35和状态更新器40。
数据服务器31
数据服务器31负责收集从传感器25、27接收的传感器数据，并且向导航滤波器34分发从传感器25、27接收的传感器数据。数据服务器31输出两个数据流，即(i)从IMU 26收集并用作驱动状态预测器35的控制输入的惯性测量结果；以及(ii)用于驱动状态更新器40的所有非IMU传感器测量结果。
轨道约束管理器32
轨道约束管理器32管理导航滤波器34和地图数据库24之间的接口，并向状态预测器35的受约束的捷联惯性导航系统(INS)47(如图6所示)提供最新的轨道约束函数xE(s)。
轨道约束函数xE(s)包括三维曲线，这些曲线提供了轨道在载运工具10附近的运输网络的一部分内的中心线的近似值。轨道约束函数xE(s)也可以被视为“轨道几何数据”的示例，因为它指示运输网络的轨道的几何形状。假定预期载运工具10遵循轨道，则轨道约束函数xE(s)因此表示由运输网络的轨道施加给载运工具的运动学状态的约束。参数s是轨道的中心线上从所选择的参考点开始的弧长。上标E用于指示轨道约束函数相对于E框架(它是旋转参考框架)定义，E框架固定在地球的主体中，并且由右手笛卡尔坐标轴的集合来表示，其原点位于地球的中心(即，ECEF坐标系)。
轨道约束函数xE(s)由轨道约束管理器32根据存储在地图数据库24中的支撑点集合(即，顶点实体23的实例)即时构造。轨道约束管理器32使用状态预测器35(其包括如下描述的载运工具10的估计位置)输出的最新的估计状态
来查询数据库24，并且识别载运工具10的估计位置以及载运工具10随后可以遵循的所有可能的轨迹周围的局部邻域中的所有支撑点。所识别的支撑点表征局部轨道几何形状和拓扑，使得可以针对穿过载运工具10的估计位置的所有可能的路线构造轨道约束。如果在载运工具10的估计位置的邻域内存在多个路线，则轨道约束管理器32可以构造多个轨道约束函数。
在图1所示的示例中，当轨道约束管理器32确定载运工具10在顶点3和4之间时，轨道约束管理器32从地图数据库24中识别在当前估计的载运工具10的位置的任一侧处的三个顶点范围内的所有支撑点。所识别的支撑点是图1所示的顶点1至8。实际上，轨道约束管理器32例如可以在当前估计的载运工具10的位置附近识别在载运工具10的任一侧上的二十个支撑点。已经发现，二十个支撑点通常将提供足够的信息来理解载运工具10邻域中的轨道几何形状。根据当前估计的载运工具10的位置动态地识别支撑点。
基于与所识别的支撑点相关联的边顶点22和边连接20，可以容易地确定载运工具10可以遵循的可能轨迹。例如，当轨道约束管理器32确定载运工具10在顶点3和4之间并且朝向顶点4行进时，轨道约束管理器32可以确定在载运工具10通过顶点4之后，存在两条可能的路径(即，经由边14或边16)供载运工具10采用。考虑到运输网络中的任何隐式约束，每个轨迹由所识别的支撑点的子集定义。载运工具10的移动方向可以由传感器25、27直接测量，或者备选地可以由状态预测器35输出的最新的估计状态
指示。
轨道约束管理器32使用所识别的支撑点和可能的轨迹将轨道约束函数xE(s)构造为多项式函数。通过对所识别的支撑点进行插值来构造轨道约束函数xE(s)。在示例中，轨道约束函数xE(s)具有通过所识别的支撑点中的每个支撑点的连续二阶导数。
使用具有连续二阶导数的多项式函数对所识别的支撑点进行插值的优点在于，与使用相同数量的支撑点的常规分段线性插值相比，其允许更准确地对轨道的中心线进行建模。例如，铁路轨道(和大多数高速公路)被构造为可实现安全且舒适的运动，并且具有较低的横向晃动。因此，轨道曲率的变化是平滑的。因此，具有连续二阶导数的二次微分曲线是适于轨道的数学模型。实际上，与更常规的分段线性插值方案不同，这种模型在没有间断的情况下对载运工具的要估计的运动学状态(例如，位置、运动方向和运动方向的变化率)提供了强大的约束。
在特定示例中，轨道约束函数xE(s)是三次样条，并且参数s是从所选择的参考点沿着样条的弧长。三次样条是由分段三阶多项式构造的样条插值，分段三阶多项式通过控制点(例如，所识别的支撑点)的集合。通过构造，三次样条隐式地提供了具有连续二阶导数的二次微分曲线。此外，在具有连续二阶导数的所有二次微分函数中，三次样条产生最小的应变能范数，这些二次微分函数对支撑点集合进行插值并且满足相同的端点条件。通过三次样条插值获得的结果是支撑点之间具有最小振荡的曲线级数。
轨道约束管理器32可以基于所识别的支撑点的位置、三次样条的边界条件(自然的或固定的)以及一阶和二阶导数的连续性要求，来计算通过弧长参数化的三次样条的系数。为了便于轨道约束管理器32计算三次样条，例如可以由服务器(未示出)在基于调查数据构造地图数据库24期间，根据图7所示和以下描述的处理步骤对存储在地图数据库24中的支撑点进行处理。在示例中，调查数据包括沿着运输网络的轨道采样的离散点，并且包括指示离散点的位置的信息。执行图7的步骤的服务器可以远离载运工具10，并且负责基于调查数据来构造地图数据库24。
在步骤S11，服务器基于调查数据来计算由弦长参数化的传统三次样条的系数。具体地，该计算基于包括在调查数据内的至少一些离散点的位置、三次样条的边界条件(自然的或固定的)以及一阶和二阶导数的连续性要求。在步骤S12，服务器计算新的支撑点集合，这些支撑点沿着在步骤S11处计算出的三次样条的弧长均匀间隔。在步骤S13，服务器使用在步骤S12处计算出的新的支撑点集合、边界条件以及一阶和二阶导数的连续性要求，计算由弧长参数化的三次样条的系数。服务器可以迭代执行步骤S12和S13，以便产生更好的结果(例如，确保在步骤S12处计算出的新的支撑点集合大体上沿着在步骤S13处重新参数化的三次样条均匀间隔)。在步骤S12处计算出的新的支撑点集合可以存储在地图数据库24中作为支撑点。
一般地，轨道约束函数xE(s)表示运输网络的轨道几何形状的先验知识，并且对于提高导航滤波器34的导航精度很有用。具体地，轨道约束函数xE(s)改善了以下二者中的误差的可观察性：载运工具的导航解决方案以及由传感器25、27输出的用于计算导航解决方案的传感器测量结果中的误差(例如，IMU 26的惯性测量结果中的偏差)。此外，当在如下描述的导航滤波器34中使用时，轨道约束函数xE(s)将估计载运工具10的不受约束的三维运动的问题简化为估计载运工具10沿着运输网络的轨道的一维约束运动的问题，因为载运工具10沿着轨道仅具有一个自由度。
通过从当前估计的载运工具10的位置附近的较小的支撑点子集即时构造轨道约束函数xE(s)，轨道约束函数xE(s)着重于载运工具的邻域中的一小部分运输网络(而不是整个运输网络)对载运工具施加的局部约束，并且可以以极大的灵活性和效率来计算轨道约束函数xE(s)。局部约束为估计载运工具10的运动学状态提供了足够的信息。实际上，已经认识到，远离载运工具10的轨道的几何形状对于确定载运工具10的运动学状态不太有用(如果有的话)。
为了便于说明，图1中所示的虚线18和19表示由轨迹约束函数xE(s)限定的弯曲的轨迹中心线，该轨迹约束函数是由三次样条插值通过顶点4、6和8构造的。假设顶点1至5以及5和7之间的轨道是笔直的，因此边11至15代表由轨迹约束函数xE(s)限定的轨道中心线。
导航滤波器34的概述
导航滤波器34利用贝叶斯滤波器算法并且包括预测步骤和更新步骤，用于估计载运工具10的状态
。估计状态
包括载运工具10的运动学状态(例如，载运工具10的位置、速度和/或姿态)。估计状态
还包括表示载运工具10的运动学状态所需的状态(例如，IMU 26的惯性测量结果中的偏差)，并且可以确定这些状态以便最小化估计过程中的误差累积。
在预测步骤30中，导航滤波器34的状态预测器35基于轨道约束函数xE(s)、IMU测量数据
和sB、以及在先前的更新步骤33期间由状态更新器40生成的估计状态
及其相关联的误差协方差
(或者如果不存在如下描述的可用的非IMU测量结果，则为在先前的预测步骤30期间生成的估计状态
及其相关联的误差协方差
)，在步骤38处计算估计状态
(也被称为先验估计)的均值，并在步骤39处计算该估计状态的相关联的误差协方差
IMU测量数据可以被称为“第一传感器数据”。非IMU测量数据可以被称为“第二传感器数据”。
预测步骤30由从IMU 26收集的惯性测量结果的数据驱动。预测步骤30用于在非IMU传感器测量的更新之间及时向前传播估计状态。
当预测步骤30完成时，在步骤41处查询数据服务器31以确定是否存在任何待处理的非IMU传感器测量结果。如果没有可用的非IMU测量结果，则对每个进入的IMU测量数据重复预测步骤30，直到非IMU测量数据变得可用时为止。此外，当预测步骤30完成时，将新的估计状态
及其相关联的误差协方差
传递给轨道约束管理器32，以便提供最新的轨道约束函数xE(s)。
当可从数据服务器31获得非IMU传感器测量结果y时，触发更新步骤33。在更新步骤33中，导航滤波器34的状态更新器40基于先验估计
及其相关联的误差协方差
以及非IMU传感器测量结果y，在步骤45处计算估计状态
(也被称为后验估计)的均值和及其相关联的误差协方差
随后，将估计状态
及其相关联的误差协方差
提供给下一时期的预测步骤30。
在完成预测或更新步骤之后，估计状态
或
的均值为控制器9可用，以生成指示载运工具10在运输网络内的位置的输出。
以此方式，导航滤波器34能够通过将IMU传感器测量结果和非IMU传感器测量结果与轨道约束函数xE(s)融合，来提供对载运工具10的状态的相对精确的估计。由于贝叶斯估计滤波器的性质(其过滤噪声而不是数据)，包含在载运工具10的估计状态中的噪声的程度比包含在状态过程模型(例如，受约束的捷联INS 47)中的过程噪声或者包含在IMU传感器测量结果和非IMU传感器测量结果中的测量噪声小。
在图5提供的具体示例中，导航滤波器34被实现为无迹卡尔曼滤波器。无迹卡尔曼滤波器适于处理非线性的过程模型，并且不需要对过程模型进行显式或数值近似的雅可比估计(然而扩展的卡尔曼滤波器需要)。
在无迹卡尔曼滤波器中，当状态根据非线性过程模型随时间演变时，使用无迹变换来估计状态的均值和误差协方差。基本假设是，估计状态的分布始终可以通过高斯分布很好地近似，特别地，在状态
通过状态过程模型(在图5的示例中，由受约束的捷联INS 47表示)传播之后，以及在调整状态
以合并由数据服务器31输出的非IMU传感器测量结果y之后。
利用从估计状态(例如，
)的均值及其相关联的误差协方差(例如，
)精心构造的样本点(被称为“西格玛(sigma)点”)集合来执行无迹变换。西格玛点的数量为2N+1，其中N为状态的维数。2N+1个西格玛点包括：一个等于估计状态的均值的点，以及2N个关于均值对称布置的点。在参考文献[1]-[2]中描述了对无迹卡尔曼滤波器的西格玛点的计算，并且该计算在本领域中是众所周知的。通过相关的系统模型对西格玛点进行变换，并且使用得到的经变换的点来恢复对经变换的状态的均值和误差协方差的估计。
下面参考状态预测器35和状态更新器40更详细地描述导航滤波器34的预测步骤30和更新步骤33。
状态预测器35
在步骤36处，状态预测器35基于在前一时期的更新步骤33期间生成的估计状态
的当前均值及其相关联的误差协方差
来生成由
表示的西格玛点集合。
状态预测器35还包括用作状态过程模型的受约束的捷联INS 47(在图6中示出)，并且执行步骤37以及时向前传播估计状态。在步骤37期间，将IMU测量数据
和sB用作受约束的捷联INS 47的控制输入。受约束的捷联INS 47对IMU测量数据
和sB进行积分，以确定IMU 26相对于以地球为中心的地球固定(ECEF)的笛卡尔坐标参考框架的位置、速度和姿态。因为IMU 26被捆扎在载运工具10上，所以IMU 26的位置、速度和姿态与载运工具10的位置、速度和姿态等同或紧密相关，载运工具10的位置、速度和姿态是导航滤波器34的估计状态(例如，
)的一部分。
测量数据sB是指在B框架中解析的比力，B框架是相对于承载IMU 26的载运工具10固定的旋转参考框架。B框架由右手笛卡尔坐标轴的集合表示，其原点位于IMU 26的质心上，IMU 26本身牢固地捆扎在载运工具10上。B框架包括与载运工具10的中心线平行的x1轴，因此相对于载运工具10固定。测量数据
是指在B框架中解析出的B框架相对于I框架的角速率。I框架是牛顿惯性参考框架，其由右手笛卡尔坐标轴的集合表示，坐标原点位于地球中心，并且与E框架在一些所选择的参考时期(例如，t＝t0)处重合。
INS 47的名称中的术语“受约束的”意味着，与常规的捷联惯性导航算法不同，通过合并轨道约束函数xE(s)来修改由INS 47执行的积分，INS 47基于速度数据确定IMU 26的位置，使得产生的最终位置被约束在轨道约束函数xE(s)的曲线上。这确保了状态预测器35永远不会产生偏离轨道的位置解决方案。
在图6中示出受约束的捷联INS 47的示例。INS 47输出数据52、62和63。数据52
是B框架相对于I框架的姿态(即，朝向)，其等于载运工具10相对于I框架的姿态。数据62
是IMU 26相对于E框架的速度。数据63
是IMU 26相对于E框架的位置。
受约束的捷联INS 47具有T框架积分器50，该T框架积分器50对IMU测量数据
和sB进行积分以计算数据52和IMU 26的推力惯性速度
T框架积分器50具有初始条件51，初始条件51包括例如IMU 26的推力惯性速度的初始值和将I框架坐标分解为推力速度框架(即，T框架)的坐标的姿态旋转矩阵的初始值。T框架是正交的右手坐标参考框架，它的轴与B框架的轴平行。T框架的原点相对于I框架(即，地球中心)的平移被定义为推力速度
受约束的捷联INS 47还包括G框架积分器53，G框架积分器53通过对数据64
和gE进行积分来计算IMU 26的引力惯性速度
是地球的角速率，gE是地球的引力。
是INS 47的已知参数。在框65处使用标准地球模型基于先前由R框架积分器56计算出的数据63的值来确定gE。
G框架积分器53具有初始条件54，初始条件54包括例如引力惯性速度
的初始值和将引力速度框架(即，G框架)的坐标分解为I框架坐标的姿态旋转矩阵的初始值。G框架是正交的右手坐标参考框架，它的轴与E框架的轴平行。G框架的原点相对于I框架的平移被定义为引力速度
T框架积分器50和G框架积分器53不受轨道约束函数xE(s)的影响，并且可以使用公知的算法来实现。
然后，在加法器55处组合IMU 26的推力惯性速度
和引力惯性速度
以生成
这是IMU 26的真实惯性速度。然后，将IMU 26的真实速度
数据52和数据64提供给R框架积分器56，以用于计算数据62和数据63。
R框架积分器56对R框架沿着轨道的速度进行积分，即，沿着轨道从所选择的参考点开始的弧长s的一阶导数
随后使用
的积分利用轨道约束函数xE(s)来导出IMU 26的速度和位置。弧长s是时间t的函数。
在等式(1)中,
在等式(1)中，
是R框架积分器56在前一周期中计算的数据62的最新值。此外，
表示在B框架解析的B框架相对于R框架的位置。
被建模为固定的杠杆臂向量。实际上，例如当载运工具10绕弯道行进并且IMU 26没有直接安装在R框架上方时，由于B框架相对于R框架的旋转较小，杠杆臂向量可能不是恒定的。此外，等式(1)中的
可以如下计算：
随后，R框架积分器56在初始条件57下随时间对
进行积分，以更新弧长s的值。初始条件57包括弧长s和/或所选择的参考点的初始值。
将框58表示的轨道约束函数xE(s)提供给R框架积分器56。R框架的E框架位置坐标(即，
)等同于轨道约束函数xE(s)在
的积分值下的值。也就是说，
还可以根据等式(5)和等式(6)，使用
来更新数据62和数据63的值：
因此，R框架积分器56对IMU 26的沿轨道速度进行积分，以获得IMU 26沿着轨道约束函数xE(s)的曲线相对于所选择的参考点的行进距离。因此，受约束的捷联INS 47封装了用于感兴趣的主导航子状态的时间演进函数，即：(i)B框架相对于I框架的姿态，即数据52；(ii)B框架相对于I框架的速度，即
以及(iii)R框架的沿轨道的距离，即s(t)。
由于由R框架积分器56执行的积分结合了轨道约束函数xE(s)，因此由INS 47确定的IMU 26的位置(即，数据63)被约束在轨道约束函数xE(s)的曲线上。IMU 26的位置(即，数据63)是导航滤波器34的估计状态(例如，
)的一部分。
T框架积分器50和G框架积分器53并行执行。T框架积分器50和G框架积分器53中的每一个在接收到多个进入的IMU数据样本之后提供一个经更新的数据样本(包括，由T框架积分器50提供的
(其等于
)和
以及由G框架积分器53提供的
(其等于
)和
)。这被称为次循环更新。R框架积分器56从T框架积分器50和G框架积分器53接收多个数据样本，以便完成更新周期，这被称为主循环更新。因此，R框架积分器56以比T框架积分器50和G框架积分器53慢的速率更新。
如图5所示，在步骤36处生成的西格玛点
中的每一个都通过受约束的捷联INS 47，该受约束的捷联INS 47生成西格玛点
西格玛点
的值提供了初始条件51、54和57的至少一部分。
随后，在步骤38、39处使用得到的西格玛点
来计算新的估计状态
的均值和该估计状态的协方差
这基于估计状态可以由高斯分布近似的假设。
将新的估计状态
的均值和该估计状态的协方差
(也被称为先验估计)提供给轨道约束管理器32，以查询地图数据库24，使得轨道约束管理器32向受约束的捷联INS 47动态地提供载运工具10的估计位置附近的最新的轨道约束函数xE(s)。
如果在查询数据服务器31之后，在步骤41处确定不存在可用的非IMU测量结果，则在步骤36处理新的滤波器状态均值和协方差
以生成新的西格玛点集合
来初始化受约束的捷联INS 47的状态。
状态更新器40
如果非IMU传感器测量数据可从数据服务器31获得，则基于先验估计
及其误差协方差
来触发状态更新器40，以开始更新步骤33。
非IMU传感器测量数据可以具有不同的测量类型。例如，非IMU传感器测量数据可以由车轮里程表、视觉里程表或雷达里程表或雷达提供，车轮里程表、视觉里程表或雷达里程表或雷达是提供参考位置信息的传感器27的一部分。此外或备选地，非IMU传感器测量数据可以由应答器读取器或GNSS传感器提供，应答器读取器或GNSS传感器是提供绝对位置信息的传感器25的一部分。
对于每种测量类型，状态更新器40包括测量模型，测量模型表征在没有测量误差的情况下的测量质量以及测量结果和状态之间的函数关系二者。
因此，状态更新器40在步骤44处为来自特定非IMU传感器的每个测量结果y创建适当的测量模型。所创建的测量模型包括：(i)R，表征测量噪声过程的误差协方差，假设测量噪声具有零均值并且满足高斯分布；以及(ii)函数h(x)，其将状态空间映射到测量空间。通过使用函数h(x)和状态
可以将经预测的测量结果
计算为
函数h(x)也可以被称为观察模型。
可以基于测量类型和状态元素合适地创建测量模型。
创建了函数h(x)之后，在更新步骤33内再次使用无迹变换来计算卡尔曼增益，该卡尔曼增益用于将新信息合并到估计状态。
具体地，状态更新器40在步骤42处根据先验估计
及其误差协方差
构造2N+1个西格玛点的集合。西格玛点的集合被表示为
随后，状态更新器40通过函数h(x)传播西格玛点
中的每一个，以在步骤43处获得经预测的测量西格玛点的集合
测量西格玛点根据等式(7)计算：
状态更新器40还基于西格玛点
非IMU测量数据y和测量噪声R在步骤45处计算卡尔曼增益和后验估计(即，经更新的状态估计)。
具体地，状态更新器40在步骤45处使用
和测量噪声R来计算经预测的测量结果的误差协方差
状态更新器40还在步骤45处使用
和
来计算估计状态和经预测的测量结果的交叉协方差
然后，状态更新器40在步骤45处根据等式(8)，基于误差协方差
和交叉协方差
来计算卡尔曼增益K：
西格玛点
的测量结果y和均值(由
表示)之间的差异被称为“创新”。该创新封装了引入到更新步骤33中的新信息。该创新与卡尔曼增益以及先验估计
及其误差协方差
结合使用，以计算后验估计
及其误差协方差
具体地，根据等式(9)和等式(10)来计算后验估计
及其误差协方差
在更新步骤期间，还可以利用轨道约束函数xE(s)。如何完成此操作取决于应用以及约束可以被视为绝对的还是近似的。例如，如果载运工具10是火车，并且忽略载运工具10的振动，则可以根据表示轨道的中心线和火车位置的轨道约束函数xE(s)来确定火车沿着铁路轨道行进的朝向。应当理解，只能根据轨道约束函数xE(s)确定火车的俯仰和偏航，而不能确定横滚。然而，如果将火车轨道的倾斜度也合并到地图数据库24中，则可以将横滚约束到一定程度。应该理解，轨道的倾斜度只能提供火车的总体横滚的一部分，因为当绕弯道行进时，火车倾向于在其悬架上横滚。因此，可以基于轨道约束函数xE(s)和在先验估计状态
中指示的火车的估计位置，来导出火车的姿态(尤其是姿态的俯仰分量和偏航分量)的非常精确的伪测量。姿态的横滚分量也可以通过施加动态约束来估计-例如，火车停留在轨道上，因此，唯一的加速作用是向心加速度。
在给定弧长参数s的情况下，根据轨道约束函数xE(s)，火车的姿态的伪测量实质上是B框架的x1轴的方向。可以根据轨道约束函数xE(s)在火车的估计位置(它是估计状态
的一部分)处的切线值和曲率值构建伪测量。
术语“伪”意味着该测量结果不是从任何传感器25、27获得的真实测量结果，而是在更新步骤中与非IMU传感器测量结果一起用作测量结果。具体地，可以将火车的姿态的伪测量提供给状态更新器40作为测量结果y。状态更新器40在步骤44处计算用于伪测量的测量模型h(x)，并且使用伪测量来计算卡尔曼增益K和后验估计
及其误差协方差
应当理解，如果载运工具10是小汽车并且运输网络是道路网络，则也可以将小汽车的姿态的类似伪测量应用于状态更新器40，但是精度较低，因为小汽车在道路网络内具有更大的自由度和更高的机动性。
为了实现目的，可以间歇地应用姿态的伪测量的更新。例如，可以将姿态的伪测量的更新与每个视觉里程表测量结果更新一起应用。
如上所述，在导航滤波器34的预测步骤30和更新步骤30二者内使用轨道约束函数xE(s)构成了对轨道约束信息的最佳使用。通过在受约束的捷联INS 47中施加轨道约束函数xE(s)，载运工具10的位置估计值的演进始终与沿着由轨道约束函数xE(s)表示的轨道中心线的一维运动一致，并且状态预测器35生成的状态预测(例如，
)和状态估计误差(例如，
)的传播与轨道约束函数xE(s)保持一致。通过在更新步骤中进一步将轨道约束函数xE(s)施加为伪测量，状态更新器40仅调整载运工具10沿着由轨道约束函数xE(s)表示的轨道中心线的位置估计值。轨道约束函数xE(s)改善了估计状态(例如，
)的均值和相关联的误差协方差(例如，
)二者。
路线消歧
控制器9还执行用于确定载运工具10通过运输网络的接合点所采用的路线的处理步骤。该处理步骤在图12中示出，并在下面进行描述。
在步骤S1处，控制器9(具体地，图5的轨道约束管理器32)获得轨道几何数据(例如，轨道约束函数xE(s)和/或存储在地图数据库24中的网络拓扑)，轨道几何数据指示载运工具10附近的运输网络的至少一部分的轨道几何形状。基于载运工具10的当前位置来即时动态地获得轨道约束函数xE(s)。载运工具10的当前位置可以是载运工具10的初始位置，该初始位置由提供指示载运工具10的绝对位置的信息的传感器25确定，备选地，载运工具10的当前位置可以是由状态预测器35输出的估计状态
内指示的载运工具10的最新估计位置。
在步骤S2处，控制器9基于轨道约束函数xE(s)和载运工具10的估计位置来确定载运工具10是否正在接近网络接合点。如上所述，估计位置由图5的导航滤波器34生成。
在图1的示例中，载运工具10从顶点3向下移动到顶点4，并且顶点4是网络接合点，因为它提供了两种可能的路线选项。基于轨道约束函数xE(s)和载运工具10的移动方向，控制器9确定载运工具10正在接近由顶点4表示的网络接合点。载运工具10的移动方向可以由传感器25、27直接测量，或者备选地可以在由状态预测器35输出的估计状态
中指示。
如果在步骤S2处确定载运工具10没有接近网络接合点，则将重复步骤S1和步骤S2，直到确定载运工具10正在接近网络接合点。
如果在步骤S2处确定载运工具10正在接近网络接合点，则处理进入步骤S3，其中控制器9确定来自网络接合点的路线选项。可以基于存储在地图数据库24中的网络拓扑(具体是边连接20的实例)来确定路线选项。在图1的示例中，控制器9查询地图数据库24并确定网络接合点(即，顶点4)提供朝向顶点5的第一路线选项和朝向顶点6的第二路线选项。随后，控制器9可以获得轨道约束函数xE(s)，其分别指示每个路线选项的轨道几何形状。
在步骤S4处，控制器9生成多个贝叶斯估计滤波器算法，每个贝叶斯估计滤波器算法与在步骤S3处确定的可能的路线选项相关联。多个贝叶斯估计滤波器算法中的每一个都提供导航解决方案(包括载运工具10的估计位置)，该导航解决方案受指示相关联的路线选项的轨道几何数据的约束。
在示例中，多个贝叶斯估计滤波器算法可以是导航滤波器34的副本。也就是说，控制器9生成导航滤波器34的第一副本，其基于指示朝向顶点5的第一路线选项的轨道约束函数xE(s)来估计载运工具10的位置(就好像第二路线选项不存在)。控制器还生成导航滤波器34的第二副本，其基于指示朝向顶点6的第二路线选项的轨道约束函数xE(s)来估计载运工具10的位置(就好像第一路线选项不存在)。
图8示意性地示出了由导航滤波器34的第一副本提供的结果。第一副本估计载运工具10的位置在位置70处，误差范围为71。
图9示意性地示出了由导航滤波器34的第二副本提供的结果。第二副本估计载运工具10的位置在位置80处，误差范围为81。
因为导航滤波器34不会产生载运工具10偏离由轨道约束函数限定的轨道的估计位置，因此估计位置70、80位于与导航滤波器34的副本相关联的路线选项上。
基于载运工具10的估计位置，导航滤波器34的每个副本还确定载运工具10的假定运动方向(即，如上所述的对载运工具的姿态的伪测量)。伪测量可以被计算为轨道约束函数xE(s)在载运工具10的估计位置处的切线，并且也可以被称为“姿态伪测量”。
在图8的示例中，导航滤波器34的第一副本确定载运工具10的姿态的伪测量为沿着箭头72。在图9的示例中，导航滤波器34的第二副本确定载运工具10的姿态的伪测量为沿着箭头82。
将伪测量馈送到导航滤波器34的每个副本作为测量结果y的一部分。因此，由导航滤波器的每个副本输出的创新值受相应伪测量的影响。
如上所述，创新表示估计状态与测量结果之间的差异。当估计状态与通过测量观察到的真实状态类似时，该差异很小。相反，当估计状态与真实状态不同时，该差异很大。将理解的是，如果例如载运工具10确实采用了朝向顶点5的第一路线选项，则伪测量72将近似于载运工具10的真实姿态。因此，导航滤波器34的第一副本的创新将具有较小值。如果例如载运工具10采用了朝向顶点6的第二路线选项，则伪测量72将与载运工具10的真实姿态显著不同。因此，导航滤波器34的第一副本的创新将具有更大值。
因此，由多个贝叶斯估计滤波器算法输出的创新值指示载运工具10采用由网络接合点提供的相应路线选项的概率。
多个贝叶斯估计滤波器算法可以并行执行。在一个示例中，多个贝叶斯估计滤波器算法被实现为并行运行的独立线程。在备选示例中，在单独的处理器上实现多个贝叶斯估计滤波器算法。无论如何，多个贝叶斯估计滤波器算法中的每一个都与传感器25、27的测量结果同步。
在步骤S5处，控制器9在载运工具10通过接合点时监测多个贝叶斯估计滤波器算法的输出(具体是创新值)。也就是说，多个贝叶斯估计滤波器算法中的每一个基于IMU测量结果和指示相关联的路线选项的轨道约束函数递归地预测载运工具10的位置，确定载运工具10在预测位置处的姿态伪测量，并在更新阶段期间基于预测位置、姿态伪测量和非IMU传感器测量结果来计算创新值。
在实践中，由于不知道载运工具10要经过多长时间才能通过接合点，因此控制器9可以监测多个贝叶斯估计滤波器算法的输出一段固定时间。该固定时间段被设置为足够长，以使载运工具能够越过接合点。备选地，控制器9可以监测多个贝叶斯估计滤波器算法的输出，直到该输出明显彼此不同为止。
图10和图11示出了由导航滤波器34的第一副本和第二副本计算出的创新值的图表。每个图表的Y轴指示创新值的大小。每个图表的X轴表示时间。曲线75由导航滤波器34的第一副本输出，该导航滤波器34的第一副本被分配给朝向顶点5的第一路线选项。曲线85由导航滤波器34的第二副本输出，该导航滤波器34的第二副本被分配给朝向顶点6的第二路线选项。在图10中，载运工具10采用第二路线选项。在图11中，载运工具10采用第一路线选项。
从这些图表中可以清楚地看出，正确路线的创新值显著低于错误路线的创新值。
在步骤S6处，控制器9通过基于多个贝叶斯估计滤波器算法的输出选择多个路线选项中呈现最高概率的一个路线选项来确定载运工具10采取的路线选项。
在示例中，控制器6选择在载运工具10经过网络接合点的时间段内提供最低平均创新值的路线选项。
在另一示例中，在获得每个姿势伪测量之后，控制器9将由多个贝叶斯估计滤波器算法输出的创新与阈值进行比较。对于提供阈值以上的创新的算法，控制器9确定载运工具10不太可能采用与那些算法相关联的路线选项。那些不太可能的路线选项将被排除在考虑范围之外，并且与那些不太可能的路线选项相关联的那些贝叶斯估计滤波器算法将被剔除。已经遍历网络接合点之后的剩余路线选项具有供载运工具10采用的最高可能性。如果载运工具10通过接合点之后已经行进了预定距离，并且仍然存在一个以上不产生阈值以上的创新的路线选项，则选择提供最低平均创新值的路线选项。
在图10所示的图表中，可以清楚地看出，由导航滤波器34的第二副本输出的曲线85实现了较低的创新值。因此，控制器9将确定载运工具在通过接合点之后已经采用了朝向顶点6的第二路线选项。
在图11所示的图表中，可以清楚地看出，由导航滤波器34的第一副本输出的曲线75实现了较低的创新值。因此，控制器9将确定载运工具在通过接合点之后已经采用了朝向顶点5的第一路线选项。结果与如图10和图11的上部所示的载运工具10采用的实际路线一致。
在确定了载运工具10采用的路线之后，控制器9保留与所确定的路线相关联的贝叶斯估计滤波器算法，以用于进一步处理(例如，用于重复图12的处理步骤)。终止或删除与不正确的路线选项相关联的贝叶斯估计滤波器算法，以节省控制器9的计算资源。
图12所示的处理步骤不依赖于载运工具10的绝对位置来消除载运工具10通过网络接合点所采用的路线的歧义。相反，以上描述的处理步骤通过生成多个贝叶斯估计滤波器算法并比较由这些算法生成的创新值来计算载运工具采用每个可能的路线选项的概率，每个贝叶斯估计滤波器算法与多个路线选项中的相应一个相关联。创新值表示载运工具采用相应路线选项的概率。载运工具10实际采用的正确路线选项的创新值明显低于其他路线选项的创新值。使用根据在载运工具10的估计位置处的轨道几何形状(例如，轨道约束函数)的知识形成的对载运工具10的姿态的伪测量来计算创新值。以此方式，通过比较由这些算法生成的创新值，求解出载运工具10通过网络接合点采用的最可能的路线选项。
图12所示的处理步骤不依赖于轨道侧基础设施的设置来消除载运工具10通过接合点所采用的路线选项的歧义，因此可以确定轨道侧基础设施在网络接合点局部不可用时的路线。这允许载运工具10无缝地通过接合点。此外，图12所示的处理步骤可以在轨道侧基础设施的采购、安装和维护方面为网络运营商节省大量成本。
应当理解，在步骤S4处生成的多个贝叶斯估计滤波器算法可以采用与图5所示的导航滤波器34不同的形式。
具体地，在步骤S4处生成的多个贝叶斯估计滤波器算法可以是扩展的卡尔曼滤波器、无迹卡尔曼滤波器或任何其他合适类型的贝叶斯估计滤波器算法，只要算法能够融合传感器25、27输出的传感器数据并能够基于该传感器数据估计载运工具10的状态(包括位置和姿态)即可。图12示出了适于由本文所描述的技术使用的贝叶斯估计滤波器算法的一般工作原理。贝叶斯估计滤波算法包括系统模型M 100、观察模型h101和卡尔曼增益K 103。贝叶斯估计滤波器算法使用观测值或测量值y(例如，由传感器25、27中的至少一个输出的传感器数据)来估计载运工具10的状态
(包括位置和姿态)。观察模型h101将状态空间映射到观察空间(即，测量空间)中。在加法器102处在使用模型h映射到观察空间后，计算观察值y与估计状态
之间的差异，以生成创新值I。通过卡尔曼增益K 103放大创新值I，并将放大后的结果反馈到系统模型M 100中，以进行递归处理。
可以将轨道几何数据中定义的轨道约束(例如，轨道约束函数)适当地应用于贝叶斯估计滤波器算法，使得由贝叶斯估计滤波器算法提供的载运工具的估计位置沿着由轨道约束函数限定的轨道。
用轨道约束来约束贝叶斯估计滤波器算法可以通过多种方式来实现。在示例中，可以将算法输出的最终估计位置(不受约束)正交投影到由轨道几何数据定义的轨道上，以便获得受约束的解决方案。以上描述并且在图6中示出的捷联INS 47提供了备选示例。
在示例中，图13的贝叶斯估计滤波器算法(和图5的导航滤波器34)的状态空间和测量空间可以是欧几里得向量空间，具体是N维欧几里得向量空间。也就是说，状态(例如，
)及其误差协方差(例如，
)以及测量结果y被表示为欧几里得向量。
在备选示例中，状态(例如，
)及其误差协方差(例如，
)以及测量结果y被表示为李群(具体是矩阵李群)的元素。图13的贝叶斯估计滤波器算法的工作原理不受对李群的使用的影响。从参考文献[3]至[6]中知晓李群、李代数和使用李群机制的传感器融合算法(例如，导航滤波器34)的实现的细节。
使用矩阵李群表示状态和测量是有利的，因为矩阵李群可以使用乘积矩阵李群轻松地表示包括多个子状态的复杂状态。乘积矩阵李群封装了状态空间的拓扑结构。相反，在将状态模型转换为N维欧几里德向量空间的过程中，状态空间的拓扑结构可能丢失。
图12所示的处理步骤、图13的贝叶斯估计滤波器算法以及图5的导航滤波器34可以用C++语言或任何其他合适的编程语言来实现。
在图5提供的示例中，导航滤波器34被实现为无迹卡尔曼滤波器。应当理解，可以使用不同类型的贝叶斯估计滤波器(例如，扩展的卡尔曼滤波器等)来实现导航滤波器34。
尽管以上描述已经列出了优选实施例，但是应该理解，这些实施例仅是说明性的，并且权利要求不限于这些实施例。鉴于本发明内容，本领域技术人员将能够进行修改和替换，这些修改和替换被认为落入所附权利要求的范围内。在本说明书中公开或示出的每一个特征，无论单独或者与本文公开或示出的任意其他特征进行任意合适组合，可包含在这些实施例中。
参考文献：
[1]Van der Merwe R.、Wan E.、Julier S.的“Sigma-point Kalman filters fornon-linear estimation and sensor fusion:application to integratednavigation”，proceedings of American Institute of Aeronautics and Astronautics(AIAA)Guidance，Navigation and Control conference and exhibition，8月16日-19日，普罗维登斯,罗德岛，2004年。
[2]Van der Merwe R.、Wan的“Sigma-point Kalman filters forprobabilistic inference in dynamic state-space models”，proceedings of theWorkshop on Advances in Machine Learning，6月8日-11日，蒙特利尔，加拿大，2003年。
[3]Hall B.C.的“Lie groups,Lie algebras and representations:Anelementary introduction”，Graduate Texts in Mathematics 222(第二版)，施普林格，ISBN 0-387-40122-9，2015年。
[4]Iwasawa K.的“On the representation of Lie algebras”，JapaneseJournal of Mathematics，1948年11月9日，第405-426页。
[5]Hertzberg C.、Wagner R.、Frese U.和
L.的“IntegratingGweneric Sensor Fusion Algorithms with Sound State Representations throughEncapsulation of Manifiolds”，Information Fusion，第14卷，第1期，2013年1月，第57-77页。
[6]Fletcher P.T.、Lu,C.和Joshi,S.的“Statistics of Shape via PrincipalComponent Analysis on Lie Groups”，Proceedings of IEEE Conference on ComputerVision and Pattern Recognition，2003年，第95-101页。
Claims (20)
1.一种计算机实现的方法，用于确定载运工具在运输网络内的位置并且确定所述载运工具采用的路线选项，包括：
获得指示所述运输网络的至少一部分的轨道几何形状的轨道几何数据；
基于所述轨道几何数据和来自至少一个传感器的传感器数据来确定所述载运工具正在接近接合点，所述至少一个传感器被布置为输出指示所述载运工具的运动的信号；
基于所述轨道几何数据来确定来自所述接合点的多个路线选项；
生成多个贝叶斯估计滤波器算法，每个贝叶斯估计滤波器算法与所述多个路线选项中的相应一个路线选项相关联，并且被配置为基于指示相关联的路线选项的轨道几何数据来估计所述载运工具的位置；所述多个贝叶斯估计滤波器算法被配置为根据所述至少一个传感器的输出信号来估计所述载运工具的位置；其中，所述多个贝叶斯估计滤波器算法被配置为输出指示所述载运工具采用所述相关联的路线选项的概率的数据，输出数据包括由所述多个贝叶斯估计滤波器算法基于所述相关联的路线选项的伪测量计算出的创新值，所述相关联的路线选项的伪测量基于指示所述相关联的路线选项的轨道几何数据来确定；其中，所述伪测量包括对所述载运工具沿着由指示所述路线选项之一的轨道几何数据限定的轨道的姿态的伪测量，并且其中对所述载运工具的姿态的伪测量是基于在所述载运工具的估计位置处由所述轨道几何数据限定的轨道的切线来计算的；
当所述载运工具经过所述接合点时，监测所述多个贝叶斯估计滤波器算法的输出；以及
通过基于所述多个贝叶斯估计滤波器算法的输出选择所述多个路线选项中呈现最高概率的一个路线选项来确定所述载运工具采取的路线选项。
2.根据权利要求1所述的计算机实现的方法，其中，所述多个贝叶斯估计滤波器算法被配置为：基于所述相关联的路线选项的伪测量和所述载运工具的相应估计位置来计算所述创新值。
3.根据权利要求1或2所述的计算机实现的方法，其中，选择所述多个路线选项中呈现最高概率的一个路线选项包括：选择所述多个路线选项中实现最低创新值的一个路线选项。
4.根据权利要求1或2所述的计算机实现的方法，其中，选择所述多个路线选项中呈现最高概率的一个路线选项包括：
将所述多个贝叶斯估计滤波器算法计算出的创新值与预定阈值进行比较，以及
选择所述多个路线选项中呈现比所述预定阈值低的创新值的一个路线选项。
5.根据权利要求1或2所述的计算机实现的方法，其中，所述多个贝叶斯估计滤波器算法被配置为使得所述载运工具的估计位置沿着由所述轨道几何数据指示的相关联的路线选项。
6.根据权利要求1或2所述的计算机实现的方法，其中，所述多个贝叶斯估计滤波器算法彼此并行执行。
7.根据权利要求1或2所述的计算机实现的方法，其中，所述至少一个传感器包括惯性测量单元IMU和除了IMU之外的传感器。
8.根据前述权利要求1或2所述的计算机实现的方法，其中，确定所述载运工具正在接近所述运输网络的接合点包括：
执行第一贝叶斯估计滤波器算法，以基于所述轨道几何数据来估计所述载运工具的位置；以及
确定所述载运工具的估计位置正在接近所述运输网络的接合点。
9.根据权利要求7所述的计算机实现的方法，其中，执行第一贝叶斯估计滤波器算法以：
至少基于所述IMU输出的第一传感器数据和所述轨道几何数据来在预测步骤中预测所述载运工具的位置，使得所述载运工具的预测位置位于由所述轨道几何数据限定的轨道上；以及
至少基于由所述除了IMU之外的传感器输出的第二传感器数据来在更新步骤中更新所述载运工具的预测位置，并且
其中，所述载运工具的估计位置基于以下至少一项：所述载运工具的预测位置和所述载运工具的经更新的预测位置。
10.根据权利要求8所述的计算机实现的方法，其中，生成多个贝叶斯估计滤波器算法包括：生成所述第一贝叶斯估计滤波器算法的多个副本。
11.根据权利要求10所述的计算机实现的方法，还包括：
在更新步骤中执行所述第一贝叶斯估计滤波器算法的至少一个副本，以至少基于第二传感器数据和所述相关联的路线选项的伪测量来更新所述载运工具的预测位置。
12.根据权利要求10所述的计算机实现的方法，还包括：删除与未选择的路线选项相关联的所述第一贝叶斯估计滤波器算法的副本。
13.根据权利要求1或2所述的计算机实现的方法，其中，所述多个贝叶斯估计滤波器算法包括无迹卡尔曼滤波器。
14.根据权利要求1或2所述的计算机实现的方法，其中，获得所述轨道几何数据包括：
访问地图数据库，其中，所述地图数据库包括沿着所述运输网络内的轨道定位的样本点；
从所述地图数据库中检索所述载运工具附近的样本点；以及
通过检索到的样本点应用插值函数以获得轨道约束函数，其中，所述轨道几何数据包括所述轨道约束函数。
15.根据权利要求14所述的计算机实现的方法，其中，所述轨道约束函数包括二次微分曲线，所述二次微分曲线具有检索到的样本点中的至少一个样本点的连续二阶导数。
16.根据权利要求14所述的计算机实现的方法，其中，所述插值函数包括三次样条函数。
17.根据权利要求1或2所述的计算机实现的方法，其中，所述载运工具是火车或电车。
18.一种计算机程序，包括计算机可读指令，所述计算机可读指令被配置为使计算机执行根据权利要求1至17中任一项所述的方法。
19.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1至17中任一项所述的方法。
20.一种用于确定载运工具在运输网络内的位置的装置，包括：
存储器，存储处理器可读指令；以及
处理器，被布置为读取和执行存储在所述存储器中的所述处理器可读指令，其中，所述处理器可读指令包括被布置为控制所述处理器执行根据权利要求1至17中任一项所述的方法的指令。
Applications Claiming Priority (3)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| GB1819619.6A GB2579414B (en) | 2018-11-30 | 2018-11-30 | Method and apparatus for determining a position of a vehicle |
| GB1819619.6 | 2018-11-30 | ||
| PCT/EP2019/082929 WO2020109476A1 (en) | 2018-11-30 | 2019-11-28 | Method and apparatus for determining a position of a vehicle |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113165678A CN113165678A (zh) | 2021-07-23 |
| CN113165678B true CN113165678B (zh) | 2023-05-02 |
Family
ID=65024835
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201980077621.0A Active CN113165678B (zh) | 2018-11-30 | 2019-11-28 | 用于确定载运工具的位置的方法和装置 |
Country Status (7)
| Country | Link |
|---|---|
| US (1) | US20220024505A1 (zh) |
| EP (1) | EP3887223A1 (zh) |
| CN (1) | CN113165678B (zh) |
| AU (1) | AU2019386290B2 (zh) |
| CA (1) | CA3121377C (zh) |
| GB (1) | GB2579414B (zh) |
| WO (1) | WO2020109476A1 (zh) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE102018008624A1 (de) * | 2018-10-31 | 2020-04-30 | Trw Automotive Gmbh | Steuerungssystem und Steuerungsverfahren zum samplingbasierten Planen möglicher Trajektorien für Kraftfahrzeuge |
| GB2579415B (en) * | 2018-11-30 | 2021-11-10 | Thales Holdings Uk Plc | Method and apparatus for determining a position of a vehicle |
| CN112141164B (zh) * | 2020-09-15 | 2021-12-14 | 北京交通大学 | 一种基于贝叶斯博弈的列车自主防护方法和系统 |
| CN114184205A (zh) * | 2021-11-29 | 2022-03-15 | 江西裕丰智能农业科技有限公司 | 轨道导航方法、装置、轨道车及存储介质 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108353248A (zh) * | 2015-08-27 | 2018-07-31 | Fzc哥络普斯 | 用于定位移动设备的方法和装置 |
| CN108599737A (zh) * | 2018-04-10 | 2018-09-28 | 西北工业大学 | 一种变分贝叶斯的非线性卡尔曼滤波器的设计方法 |
Family Cites Families (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20040140405A1 (en) * | 2002-01-10 | 2004-07-22 | Meyer Thomas J. | Train location system and method |
| KR100520166B1 (ko) * | 2003-03-14 | 2005-10-10 | 삼성전자주식회사 | 네비게이션시스템에서 이동체의 위치검출장치 및 그 방법 |
| JP2007033434A (ja) * | 2005-06-20 | 2007-02-08 | Denso Corp | 車両用現在位置検出装置、及び車両制御装置 |
| GB0614530D0 (en) * | 2006-07-21 | 2006-08-30 | Trw Ltd | Determining The Location of a Vehicle on a Map |
| US8401238B2 (en) * | 2007-05-29 | 2013-03-19 | Continental Teves Ag & Co. Ohg | Map matching for security applications |
| JP5185575B2 (ja) * | 2007-08-10 | 2013-04-17 | 川崎重工業株式会社 | 列車の自車位置検出装置、車体傾斜制御システム、操舵システム、アクティブ制振システム及びセミアクティブ制振システム |
| US20110241935A1 (en) * | 2009-03-23 | 2011-10-06 | Srdjan Miocinovic | Method and apparatus for improving gps receiver accuracy using an embedded map database |
| JP5852920B2 (ja) * | 2012-05-17 | 2016-02-03 | クラリオン株式会社 | ナビゲーション装置 |
| US9226111B2 (en) * | 2012-11-21 | 2015-12-29 | Apple Inc. | Pathway matching |
| US10591608B2 (en) * | 2014-01-24 | 2020-03-17 | Savari, Inc. | Positioning quality filter for the V2X technologies |
| FR3034514B1 (fr) * | 2015-04-01 | 2017-04-21 | Sagem Defense Securite | Procede de suivi de navigation d'un porteur moblile avec un filtre de kalman etendu |
| JP6895730B2 (ja) * | 2016-10-05 | 2021-06-30 | 株式会社ゼンリンデータコム | 情報処理装置、情報処理方法及びプログラム |
| EP3339807B1 (en) * | 2016-12-20 | 2024-03-13 | HERE Global B.V. | An apparatus and associated methods for determining the location of a vehicle |
| EP3339908B1 (en) * | 2016-12-23 | 2019-10-02 | u-blox AG | Distributed kalman filter architecture for carrier range ambiguity estimation |
| AT519218B1 (de) * | 2017-02-06 | 2018-05-15 | Hp3 Real Gmbh | Verfahren zur Optimierung einer Gleislage |
| CN107330356A (zh) * | 2017-05-15 | 2017-11-07 | 开易（北京）科技有限公司 | 一种基于学习的在线车道变线预测方法及系统 |
-
2018
- 2018-11-30 GB GB1819619.6A patent/GB2579414B/en active Active
-
2019
- 2019-11-28 WO PCT/EP2019/082929 patent/WO2020109476A1/en unknown
- 2019-11-28 US US17/297,006 patent/US20220024505A1/en active Pending
- 2019-11-28 CA CA3121377A patent/CA3121377C/en active Active
- 2019-11-28 EP EP19832846.0A patent/EP3887223A1/en active Pending
- 2019-11-28 CN CN201980077621.0A patent/CN113165678B/zh active Active
- 2019-11-28 AU AU2019386290A patent/AU2019386290B2/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108353248A (zh) * | 2015-08-27 | 2018-07-31 | Fzc哥络普斯 | 用于定位移动设备的方法和装置 |
| CN108599737A (zh) * | 2018-04-10 | 2018-09-28 | 西北工业大学 | 一种变分贝叶斯的非线性卡尔曼滤波器的设计方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| GB2579414A (en) | 2020-06-24 |
| CA3121377C (en) | 2023-09-26 |
| AU2019386290A1 (en) | 2021-05-27 |
| CN113165678A (zh) | 2021-07-23 |
| GB201819619D0 (en) | 2019-01-16 |
| EP3887223A1 (en) | 2021-10-06 |
| GB2579414B (en) | 2021-11-17 |
| US20220024505A1 (en) | 2022-01-27 |
| CA3121377A1 (en) | 2020-06-04 |
| WO2020109476A1 (en) | 2020-06-04 |
| AU2019386290B2 (en) | 2023-01-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113167586B (zh) | 用于确定载运工具的位置的方法和装置 | |
| CN113165678B (zh) | 用于确定载运工具的位置的方法和装置 | |
| KR102581263B1 (ko) | 위치 추적 방법, 장치, 컴퓨팅 기기 및 컴퓨터 판독 가능한 저장 매체 | |
| US10317214B2 (en) | Inertial odometry with retroactive sensor calibration | |
| US20210310809A1 (en) | Dead Reckoning Method and Apparatus for Vehicle, Device and Storage Medium | |
| KR102628778B1 (ko) | 위치결정을 위한 방법, 컴퓨팅 기기, 컴퓨터 판독가능 저장 매체 및 컴퓨터 프로그램 | |
| CN112083726B (zh) | 一种面向园区自动驾驶的双滤波器融合定位系统 | |
| Hasberg et al. | Simultaneous localization and mapping for path-constrained motion | |
| KR102238522B1 (ko) | 3차원 공간에 대응되는 맵을 생성하는 차량 및 방법 | |
| Roth et al. | Map-supported positioning enables in-service condition monitoring of railway tracks | |
| CN115135963A (zh) | 用于在场景地图中生成3d参考点的方法 | |
| de Araujo et al. | A novel method for land vehicle positioning: Invariant kalman filters and deep-learning-based radar speed estimation | |
| EP4071740A1 (en) | Information processing apparatus, control method, program, and storage medium | |
| JP2024012160A (ja) | 目標状態推定方法、装置、電子機器及び媒体 | |
| Atia et al. | A novel systems integration approach for multi-sensor integrated navigation systems | |
| Yuan et al. | LIWO: LiDAR-Inertial-Wheel Odometry | |
| Zhang et al. | GNSS/Multi-Sensor Fusion Using Continuous-Time Factor Graph Optimization for Robust Localization | |
| Bessi | Trajectory Prediction on an ego-bike with IMU and camera using a Multi-State Constraint Kalman Filter | |
| Ruppert et al. | Map-Based Path and Pose Information for Automated and Assisted Driving of Trams | |
| Zheng et al. | A probabilistic graphic model based GPS/SINS integration algorithm for low-cost sensors on smartphone | |
| JP2024109679A (ja) | 情報処理装置、制御方法、プログラム及び記憶媒体 | |
| Akai et al. | LiDAR-Inertial Odometry Based on Extended Kalman Filter | |
| Dario | Towards general end-to-end sensor fusion for robot localization: implementa-tion of visual-inertial-wheel odometry | |
| Liang | Robust vehicle state estimation with multi-modal sensors data fusion: vehicle dynamics sensors and environmental sensors | |
| Aronsson et al. | Ego vehicle localisation using LIDAR and compressed digital maps |
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 |