用于确定载运工具的位置的方法和装置

技术领域

本申请涉及一种用于确定载运工具在运输网络内的位置的方法和装置。更具体地但非排他性地，本申请涉及一种使用贝叶斯估计滤波器算法精确地确定载运工具在运输网络内的位置的方法和装置，该算法在考虑了运输网络对载运工具施加的轨道约束的同时融合了来自附接到载运工具的惯性测量单元(IMU)和非IMU传感器的传感器数据，使得由贝叶斯估计滤波器算法估计的载运工具的位置始终位于运输网络的轨道上。

背景技术

载运工具在运输网络内的准确定位依赖于绝对位置的来源，例如，全球导航卫星系统(GNSS)传感器或固定的轨道侧基础设施(例如，应答器)。GNSS传感器在许多常见的操作环境(例如，隧道)中缺乏信号可用性。固定的基础设施的安装和维护成本很高，因此由于成本原因，提供有限的测量可用性。

因此，需要提供一种低成本的定位解决方案，该解决方案提供良好的定位精度，并且可以在没有绝对位置来源的情况下，在宽范围的环境中长时间自主操作。

发明内容

根据本文描述的第一方面，提供了一种确定载运工具在运输网络内的位置的计算机实现的方法，包括：获得指示运输网络的至少一部分的轨道几何形状的轨道几何数据；从安装在载运工具上的惯性测量单元接收第一传感器数据；执行贝叶斯估计滤波器算法以预测载运工具的位置，其中，贝叶斯估计滤波器算法包括过程模型，该过程模型包括捷联惯性导航算法，并且其中，捷联惯性导航算法至少基于第一传感器数据和轨道几何数据来生成指示载运工具的预测位置的数据，使得载运工具的预测位置位于由轨道几何数据限定的轨道上；从除了惯性测量单元之外的传感器接收第二传感器数据，其中，该传感器安装在载运工具上；至少基于第二传感器数据来执行贝叶斯估计滤波器算法，以更新载运工具的预测位置；以及基于以下至少一项来生成指示载运车辆在运输网络内的位置的输出：载运工具的预测位置和载运工具的经更新的预测位置。

运输网络可以是引导式运输网络(例如，铁路、电车轨道)。引导式运输网络具有在正常操作中严格限制载运工具(例如，火车、电车)的移动的轨道。

运输网络还可以是具有供载运工具(例如，汽车、货车)遵循的轨道(例如，道路或路线)的道路或高速公路网络，或者是限定约束载运工具(例如，船)采取的路线的轨道的水路网络(例如，运河，其中轨道由运河的墙壁限定)。

一般地，运输网络对载运工具的位置和移动施加轨道约束。例如，除非出现严重的问题，否则预期载运工具沿着运输网络提供的轨道/路线的曲率运动。顾名思义，轨道几何数据是指示运输网络的轨道几何形状的数据，因此定义了运输网络对载运工具施加的轨道约束。

贝叶斯估计滤波器算法通常用于基于间接测量状态的传感器测量结果来估计系统的状态。贝叶斯估计滤波器算法在预测步骤中递归地预测状态，并且在更新步骤中基于测量数据来更新预测状态。载运工具的位置是通过贝叶斯估计滤波器算法估计的状态的一部分。术语“贝叶斯估计滤波器算法”在本文中可以与术语“传感器融合算法”或“最佳估计算法”互换使用。

应当理解，捷联惯性导航算法在预测步骤期间执行，并且通过航位推算将惯性测量单元安装到的载运工具的运动学状态(例如，位置、姿态或速度)来计算。还将理解，贝叶斯估计滤波器算法(特别是捷联惯性导航算法)可能需要载运工具的初始条件，以便在预测步骤期间预测载运工具的位置。初始条件可以包括载运工具的初始位置，例如，载运工具沿着由轨道几何数据限定的轨道的初始位置。初始条件可以由安装在载运工具上的传感器提供，该传感器提供指示载运工具绝对位置的信息，或者可以由在最新的更新步骤期间生成的载运工具的经更新的预测位置来提供。

有利地，捷联惯性导航算法通过在预测步骤期间考虑第一传感器数据和轨道几何数据来生成指示载运工具的预测位置的数据，使得载运工具的预测位置位于由轨道几何数据限定的轨道上。换言之，捷联惯性导航算法受轨道几何数据约束(即，运输网络对载运工具施加的约束)。

通过利用轨道几何数据约束捷联惯性导航算法，有利地将估计载运工具的不受约束的三维位置的问题减少为估计载运工具沿着运输网络的轨道的一维位置的问题，因为载运工具沿着轨道仅有一个自由度。此外，受约束的捷联惯性导航算法可用于对运动学状态估计误差到一维位置解空间中的传播进行建模。因此，在捷联惯性导航算法中使用轨道几何数据对于提高所确定的载运工具的位置的准确性是有用的，并且可以显著减少贝叶斯估计滤波器算法累积的误差，尤其是当贝叶斯估计滤波器算法依赖于低成本的惯性测量单元和其他低成本的传感器并且在没有绝对位置信息的情况下长时间操作时。

利用受约束的捷联惯性导航算法，该计算机实现的方法可以在不大量依赖GNSS传感器或轨道侧基础设施(例如，应答器)的情况下实现对载运工具定位精度的提高并且采用低成本的传感器。因此，该计算机实现的方法适于诸如GNSS信号可能并非始终可用的地铁和城市环境之类的环境，并且减少或消除了对提供安装和维护成本高昂的轨道侧基础设施的需求。

惯性测量单元可以是低成本的MEMS惯性测量单元。

该计算机实现的方法还可以包括：至少基于第一传感器数据来执行捷联惯性导航算法，以计算惯性测量单元沿着由轨道几何数据限定的轨道的速度，随时间对该速度进行积分，并且至少基于轨道几何数据和该速度的积分来生成指示载运工具的预测位置的数据。

以此方式，在捷联惯性导航算法中使用轨道几何数据对运动学状态估计误差到一维位置解空间中的传播进行建模，并且允许考虑轨道在几何数据中限定的轨道的瞬时曲率和切线信息。因此，可以提高所确定的载运工具的位置的准确性。

该计算机实现的方法还可以包括动态地获得指示运输网络的至少一部分的轨道几何形状的轨道几何数据，该运输网络的至少一部分位于载运工具的预测位置和/或载运工具的经更新的预测位置附近。

术语“动态地”意味着基于由贝叶斯估计滤波器算法输出的载运工具的估计位置来即时更新或构造轨道几何数据。以此方式，轨道几何数据着重于由载运工具附近的运输网络的一小部分(而不是整个运输网络)施加给载运工具的局部约束。因此，可以以极大的灵活性和效率来计算轨道几何数据。

获得轨道几何数据可以包括：访问地图数据库，其中，地图数据库包括沿着运输网络内的轨道定位的样本点；从地图数据库中检索在载运工具的预测位置和/或载运工具的经更新的预测位置附近的样本点；以及通过检索到的采样点应用插值函数以获得轨道约束函数，其中，轨道几何数据包括轨道约束函数。

有利地，通过对从地图数据库中检索到的样本点应用插值函数，轨道约束函数(其包括在轨道几何数据内)包括表示运输网络内的轨道的中心线的线/曲线。因此，轨道约束函数能够表示载运工具的潜在路线、载运工具的潜在方向以及要在贝叶斯估计滤波器算法内施加的运动学约束，以在没有绝对位置信息的源可用时，改善对载运工具的运动学状态(例如，位置)的误差估计。应当理解，样本点(其可以与“支撑点”互换使用)的密度影响轨道约束函数的精度。

轨道约束函数可以包括二次微分曲线，该二次微分曲线具有检索到的样本点中的至少一个样本点的连续二阶导数。

有利地，与使用相同数量的样本点的常规分段线性插值相比，具有连续二阶导数的二次微分曲线允许更精确地对运输网络内的轨道(尤其是轨道中心线)的曲率和切线信息进行建模。因此，轨道约束函数在没有间断的情况下对载运工具的运动学状态提供了有力的约束，并且改善了对贝叶斯估计滤波器算法的输出的误差估计。

插值函数可以包括三次样条函数。

有利地，三次样条插值法隐式地提供了具有连续二阶导数的二次微分曲线。此外，在所有二次微分函数中，三次样条插值函数产生最小的应变能范数，并且允许由此获得的轨道约束函数在采样点之间具有最小振荡的曲线级数。

轨道约束函数可以通过轨道的弧长参数化。

可以从以下至少一个组成的组中选择除了惯性测量单元之外的传感器：GNSS传感器、应答器读取器、雷达、速度传感器、里程表、摄像机和激光雷达探测器。

轨道几何数据可以通过轨道的弧长参数化。

有利地，通过弧长而不是传统样条分段弦长来参数化轨道约束函数和/或轨道几何数据，提供了轨道中心线的更自然的表示，并且在将轨道约束施加给贝叶斯估计滤波器算法(尤其是捷联惯性导航算法)以用于估计载运工具的位置时更加方便。

该计算机实现的方法还可以包括：基于第一传感器数据、地球的角速率和地球的引力来执行捷联惯性导航算法，以计算轨道上由参考框架行进的弧长的一阶导数；其中，该参考框架被布置为附接到载运工具上的固定参考点，并且被布置为被约束为沿着由轨道几何数据限定的轨道移动。

参考框架可以具有平行于由轨道几何数据限定的轨道的中心线的轴线。以此方式，参考框架被约束为沿着由轨道几何数据限定的轨道移动。参考框架可以被布置为附接到载运工具的转向架上的固定参考点。参考框架行进的弧长的一阶导数可以等于参考框架沿着由轨迹几何数据限定的轨道的速度。

该计算机实现的方法还可以包括：执行捷联惯性导航算法以随时间对一阶导数进行积分，以便获得参考框架行进的弧长。

该计算机实现的方法还可以包括：执行捷联惯性导航算法，以至少基于轨道几何数据和参考框架行进的弧长来生成指示载运工具的预测位置的数据。

第一传感器数据可以包括由惯性测量单元测量的比力和角速率。

该计算机实现的方法还可以包括：确定第二传感器数据是否可用，以及响应于确定第二传感器数据不可用：基于载运工具的预测位置，获得指示运输网络的至少一部分的轨道几何形状的新的轨道几何数据；并且至少基于新的轨道几何数据和从惯性测量单元接收的第一传感器数据来执行贝叶斯估计滤波器算法，以预测载运工具的位置。

也就是说，贝叶斯估计滤波器算法的更新阶段由第二传感器数据触发，并且如果第二传感器数据不可用，则贝叶斯估计滤波器算法重复预测步骤。以此方式，可以将第一传感器数据和第二传感器数据异步地提供给贝叶斯估计滤波器算法。

该计算机实现的方法还可以包括：至少基于第二传感器数据和轨道几何数据来执行贝叶斯估计滤波器算法，以更新载运工具的预测位置。

通过在更新步骤中进一步施加由轨道几何数据限定的轨道约束，沿着由轨道几何数据定义的运输网络的轨道来调整或更新载运工具的预测位置。有利地，贝叶斯估计滤波器算法不会产生偏离轨道的经更新的位置，因此提高了贝叶斯估计滤波器算法的准确性。

该计算机实现的方法还可以包括：基于轨道几何数据和载运工具的预测位置，来计算对载运工具的姿态的伪测量；以及基于对载运工具的姿态的伪测量来执行贝叶斯估计滤波器算法，以更新载运工具的预测位置。

也就是说，轨道几何数据提供伪测量，该伪测量与第二传感器数据一起使用来更新载运工具的预测位置。

对载运工具的姿态的伪测量可以基于在载运工具的预测位置处的轨道几何数据的切线来计算。

贝叶斯估计滤波器算法可以包括无迹卡尔曼滤波器。

无迹卡尔曼滤波器的优点在于，它在处理非线性方面更好，并且不需要对过程模型进行显式的或数值近似的雅可比估计。

贝叶斯估计滤波器算法可以包括李群无迹卡尔曼滤波器。

也就是说，贝叶斯估计滤波器算法可以在由李群(尤其是矩阵李群)表示的状态空间和/或测量空间中操作。

捷联惯性导航算法可以包括李群捷联惯性导航算法。

也就是说，捷联惯性导航算法可以在由李群(尤其是矩阵李群)表示的状态空间中操作。使用李群表示状态空间和测量空间是有利的，因为李群可以使用乘积矩阵李群轻松地表示包括多个子状态的复杂状态，而不会丢失状态空间的类型结构。

载运工具可以包括火车或电车之一。

根据本文描述的第二方面，提供了一种计算机程序，该计算机程序包括计算机可读指令，该计算机可读指令被配置为使计算机执行根据本文描述的第一方面所述的方法。

根据本文所描述的第三方面，提供了一种计算机可读介质，承载根据本文所描述的第二方面的计算机程序。

根据本文描述的第四方面，提供了一种用于在运输网络内定位载运工具的装置，包括：存储器，存储处理器可读指令；以及处理器，被布置为读取和执行存储在存储器中的处理器可读指令，其中，处理器可读指令包括被布置为控制处理器以执行根据本文描述的第一方面所述的方法的指令。

该装置可以附接到载运工具。

该装置还可以包括地图数据库，该地图数据库包括沿着运输网络的至少一部分内的轨道定位的样本点。

在适当的情况下，以上关于本文描述的各方面之一所描述的任何可选特征可以适用于本文描述的各方面中的另一个方面。

附图说明

现在参考附图仅通过示例的方式来描述实施例，在附图中：

图1示意性地示出了运输网络的模型的一部分；

图2示意性地示出了存储图1的运输网络的模型的地图数据库；

图3示意性地示出了在图1的运输网络内行进的载运工具；

图4示意性地示出了由图3的载运工具承载的控制器的结构；

图5示意性地示出了由图4的控制器执行的用于估计载运工具的位置的贝叶斯估计滤波器的示例；

图6示意性地示出了在图5的贝叶斯估计滤波器中使用的受约束的捷联惯性导航系统的功能组件；

图7是可以由服务器执行的以便促进通过弧长参数化的三次样条的计算的示例处理步骤的流程图；

图8是可以由图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的运动的传感器。传感器组包括至少一个第一传感器25和至少一个第二传感器27，第一传感器25提供指示载运工具的绝对位置的信息，第二传感器27提供指示载运工具的参考位置的信息。

第一传感器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)的短距离连接。短距离连接可以经由蓝牙(RTM)、近场通信(NFC)等。通信接口9i还可以提供到诸如互联网或卫星之类的网络的连接，以进行更长距离的通信。如果地图数据库24存储在远离载运工具10的服务器中，则更长距离的通信可以用于检索地图数据库24。更长距离的通信还可以由GNSS传感器使用以生成绝对位置信息。CPU 9a、RAM 9b、非易失性存储设备9c、I/O接口9d和通信接口9i通过总线9j连接在一起。

应当理解，图4所示的组件的布置仅是示例性的，并且控制器9可以包括与图4所示的组件不同的、附加的或更少的组件。

存储在非易失性存储设备9c中的计算机可读指令提供如图5至图6所示的功能组件。计算机可读指令在由CPU 9a执行时，使控制器9执行图7和图8所示的处理步骤，以确定载运工具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所示)提供最新的轨道约束函数x

轨道约束函数x

轨道约束函数x

在图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使用所识别的支撑点和可能的轨迹将轨道约束函数x

使用具有连续二阶导数的多项式函数对所识别的支撑点进行插值的优点在于，与使用相同数量的支撑点的常规分段线性插值相比，其允许更准确地对轨道的中心线进行建模。例如，铁路轨道(和大多数高速公路)被构造为可实现安全且舒适的运动，并且具有较低的横向晃动。因此，轨道曲率的变化是平滑的。因此，具有连续二阶导数的二次微分曲线是适于轨道的数学模型。实际上，与更常规的分段线性插值方案不同，这种模型在没有间断的情况下对载运工具的要估计的运动学状态(例如，位置、运动方向和运动方向的变化率)提供了强大的约束。

在特定示例中，轨道约束函数x

轨道约束管理器32可以基于所识别的支撑点的位置、三次样条的边界条件(自然的或固定的)以及一阶和二阶导数的连续性要求，来计算通过弧长参数化的三次样条的系数。为了便于轨道约束管理器32计算三次样条，例如可以由服务器(未示出)在基于调查数据构造地图数据库24期间，根据图7所示和以下描述的处理步骤对存储在地图数据库24中的支撑点进行处理。在示例中，调查数据包括沿着运输网络的轨道采样的离散点，并且包括指示离散点的位置的信息。执行图7的步骤的服务器可以远离载运工具10，并且负责基于调查数据来构造地图数据库24。

在步骤S11，服务器基于调查数据来计算由弦长参数化的传统三次样条的系数。具体地，该计算基于包括在调查数据内的至少一些离散点的位置、三次样条的边界条件(自然的或固定的)以及一阶和二阶导数的连续性要求。在步骤S12，服务器计算新的支撑点集合，这些支撑点沿着在步骤S11处计算出的三次样条的弧长均匀间隔。在步骤S13，服务器使用在步骤S12处计算出的新的支撑点集合、边界条件以及一阶和二阶导数的连续性要求，计算由弧长参数化的三次样条的系数。服务器可以迭代执行步骤S12和S13，以便产生更好的结果(例如，确保在步骤S12处计算出的新的支撑点集合大体上沿着在步骤S13处重新参数化的三次样条均匀分布)。在步骤S12处计算出的新的支撑点集合可以存储在地图数据库24中作为支撑点。

一般地，轨道约束函数x

通过从当前估计的载运工具10的位置附近的较小的支撑点子集即时构造轨道约束函数x

导航滤波器34的概述

导航滤波器34利用贝叶斯滤波器算法并且递归地执行预测步骤30和更新步骤33，以估计载运工具10的状态

在预测步骤30中，导航滤波器34的状态预测器35基于轨道约束函数x

IMU测量数据可以被称为“第一传感器数据”。非IMU测量数据可以被称为“第二传感器数据”。

预测步骤30由从IMU 26收集的惯性测量结果的数据驱动。预测步骤30用于在非IMU传感器测量的更新之间及时向前传播估计状态。

当预测步骤30完成时，在步骤41处查询数据服务器31以确定是否存在任何待处理的非IMU传感器测量结果。如果没有可用的非IMU测量结果，则对每个进入的IMU测量数据重复预测步骤30，直到非IMU测量数据变得可用时为止。此外，当预测步骤30完成时，将新的估计状态

当可从数据服务器31获得非IMU传感器测量结果y时，触发更新步骤33。在更新步骤33中，导航滤波器34的状态更新器40基于先验估计

在完成预测或更新步骤之后，估计状态

以此方式，导航滤波器34能够通过将IMU传感器测量结果和非IMU传感器测量结果与轨道约束函数x

在图5提供的具体示例中，导航滤波器34被实现为无迹卡尔曼滤波器。无迹卡尔曼滤波器适于处理可能与过程模型和/或测量模型相关联的非线性，并且不需要对过程模型进行显式或数值近似的雅可比估计(然而扩展的卡尔曼滤波器需要)。

在无迹卡尔曼滤波器中，当状态根据非线性过程模型随时间演变时，使用无迹变换来估计状态的均值和误差协方差。基本假设是，估计状态的分布始终可以通过高斯分布很好地近似，特别地，在状态

利用从估计状态(例如，

下面参考状态预测器35和状态更新器40更详细地描述导航滤波器34的预测步骤30和更新步骤33。

状态预测器35

在步骤36处，状态预测器35基于在前一时期的更新步骤33期间生成的估计状态

状态预测器35还包括用作状态过程模型的受约束的捷联INS 47(在图6中示出)，并且执行步骤37以及时向前传播估计状态。在步骤37期间，将IMU测量数据

测量数据s

INS 47的名称中的术语“受约束的”意味着，与常规的捷联惯性导航算法不同，通过合并轨道约束函数x

在图6中示出受约束的捷联INS 47的示例。INS 47输出数据52、62和63。数据52

受约束的捷联INS 47具有T框架积分器50，该T框架积分器50对IMU测量数据

受约束的捷联INS 47还包括G框架积分器53，G框架积分器53通过对数据64

G框架积分器53具有初始条件54，初始条件54包括例如引力惯性速度

T框架积分器50和G框架积分器53不受轨道约束函数x

然后，在加法器55处组合IMU 26的推力惯性速度

R框架积分器56对R框架沿着轨道的速度进行积分，即，沿着轨道从所选择的参考点开始的弧长s的一阶导数

在等式(1)中,

在等式(1)中，

随后，R框架积分器56在初始条件57下随时间对

将框58表示的轨道约束函数x

还可以根据等式(5)和等式(6)，使用

因此，R框架积分器56对IMU 26的沿轨道速度进行积分，以获得IMU 26沿着轨道约束函数x

由于由R框架积分器56执行的积分结合了轨道约束函数x

T框架积分器50和G框架积分器53并行执行。T框架积分器50和G框架积分器53中的每一个在接收到多个进入的IMU数据样本之后提供一个经更新的数据样本(包括，由T框架积分器50提供的

如图5所示，在步骤36处生成的西格玛点

随后，在步骤38、39处使用得到的西格玛点

将新的估计状态

如果在查询数据服务器31之后，在步骤41处确定不存在可用的非IMU测量结果，则在步骤36处理新的滤波器状态均值和协方差

状态更新器40

如果非IMU传感器测量数据可从数据服务器31获得，则基于先验估计

非IMU传感器测量数据可以具有不同的测量类型。例如，非IMU传感器测量数据可以由车轮里程表、视觉里程表或雷达里程表或雷达提供，车轮里程表、视觉里程表或雷达里程表或雷达是提供参考位置信息的第二传感器27的一部分。此外或备选地，非IMU传感器测量数据可以由应答器读取器或GNSS传感器提供，应答器读取器或GNSS传感器是提供绝对位置信息的第一传感器25的一部分。

对于每种测量类型，状态更新器40包括测量模型，测量模型表征在没有测量误差的情况下的测量质量以及测量结果和状态之间的函数关系二者。

因此，状态更新器40在步骤44处为来自特定非IMU传感器的每个测量结果y创建适当的测量模型。所创建的测量模型包括：(i)R，表征测量噪声过程的误差协方差，假设测量噪声具有零均值并且满足高斯分布；以及(ii)函数h(x)，其将状态空间映射到测量空间。通过使用函数h(x)和状态

可以基于测量类型和状态元素合适地创建测量模型。

创建了函数h(x)之后，在更新步骤33内再次使用无迹变换来计算卡尔曼增益，该卡尔曼增益用于将新信息合并到估计状态。

具体地，状态更新器40在步骤42处根据先验估计

随后，状态更新器40通过函数h(x)传播西格玛点

状态更新器40还基于西格玛点

具体地，状态更新器40在步骤45处使用

然后，状态更新器40在步骤45处根据等式(8)，基于误差协方差

测量结果y和西格玛点

在更新步骤期间，还可以利用轨道约束函数x

在给定弧长参数s的情况下，根据轨道约束函数x

术语“伪”意味着该测量结果不是从任何传感器25、27获得的真实测量结果，而是在更新步骤中与非IMU传感器测量结果一起用作测量结果。具体地，可以将对火车的姿态的伪测量提供给状态更新器40作为测量结果y。状态更新器40在步骤44处计算用于伪测量的测量模型h(x)，并且使用伪测量来计算卡尔曼增益K以及后验估计

应当理解，如果载运工具10是小汽车并且运输网络是道路网络，则也可以将对小汽车的姿态的类似伪测量应用于状态更新器40，但是精度较低，因为小汽车在道路网络内具有更大的自由度和更高的机动性。

为了实现目的，可以间歇地应用对姿态的伪测量的更新。例如，可以将对姿态的伪测量的更新与每个视觉里程表测量结果更新一起应用。

如上所述，在导航滤波器34的预测步骤30和更新步骤30二者内使用轨道约束函数x

以此方式，载运工具10可以沿着由轨道约束函数x

载运工具定位

控制器9还执行用于确定载运工具10的位置的处理步骤。该处理步骤在图8中示出，并在下面进行描述。

在步骤S1处，控制器9(具体地，图5的轨道约束管理器32)获得轨道约束函数x

在步骤S2处，导航滤波器34的状态预测器35从捆扎在载运工具10上的IMU 26接收第一传感器数据。

在步骤S3处，执行导航滤波器34以执行预测步骤30。也就是说，执行导航滤波器34的状态预测器35以生成载运工具10的估计状态

具体地，如上所述，状态预测器35的受约束的捷联INS 47包括R框架积分器56，该R框架积分器56至少基于第一传感器数据和轨道约束函数x

在步骤S4处，导航滤波器34的状态更新器40从安装到载运工具10的非IMU传感器(例如，除了IMU 26之外的传感器25、27内的传感器)接收第二传感器数据。

在步骤S5处，在接收到第二传感器数据之后，执行导航滤波器34以执行更新步骤33。具体地，执行状态更新器40以基于第二传感器数据来更新估计状态

在步骤S5处，状态更新器40可以利用轨道约束函数x

随后可以将后验估计

在步骤S6处，控制器9基于以下中至少一项来生成指示载运工具10在运输网络内的位置的输出：包括在估计状态

在步骤S3和S4之间，控制器9可以执行确定第二传感器数据(即，非IMU测量结果)是否可用的可选步骤。

如果第二传感器数据可用，则处理可以进行到步骤S4和步骤S5以触发导航滤波器34的更新步骤33。

然而，如果第二传感器数据不可用，则可以基于在先前步骤S3处生成的估计状态

在示例中，导航滤波器34的状态空间和测量空间可以是欧几里得向量空间，具体是N维欧几里得向量空间。也就是说，状态(例如，

在备选示例中，状态(例如，

使用矩阵李群表示状态和测量是有利的，因为矩阵李群可以使用乘积矩阵李群轻松地表示包括多个子状态的复杂状态。乘积矩阵李群封装了状态空间的拓扑结构。相反，在将状态模型转换为N维欧几里德向量空间的过程中，状态空间的拓扑结构可能丢失。

图7和图8所示的处理步骤以及导航滤波器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.和

[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页。