本申请涉及一种用于确定载运工具在运输网络内的位置的方法和装置。更具体地但非排他性地,本申请涉及一种用于通过使用贝叶斯估计滤波器算法而不依赖轨道侧基础设施来确定载运工具通过运输网络的接合点的路线的方法和装置。
背景技术:
在传输网络的网络接合点处,多个轨道彼此相交。因此,网络接合点呈现多个路线选项供载运工具采用。重要的是要知道载运工具通过网络接合点所采用的特定路线,以便允许载运工具通过网络接合点的无缝操作。当前,该信息经由轨道侧基础设施提供。
轨道侧基础设施包括例如应答器或信标,它们沿着轨道安装在固定位置。轨道侧基础设施能够检测载运工具的通过,并且一般沿着由网络接合点提供的每个路线选项可用。轨道侧基础设施可以向网络信令系统或载运工具发送指示载运工具已经通过特定位置的信号。因此,通过使用轨道侧基础设施,网络信令系统或载运工具本身能够确定载运工具通过网络接合点所采用的特定路线选项。轨道侧基础设施的安装和维护成本很高。另外,轨道侧基础设施的使用取决于与载运工具或网络信令系统的通信链路。因此,使用轨道侧基础设施的路线检测方法不是自主的。
尽管载运工具通常携带全球导航卫星系统(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,并且可以包括其他传感器(例如,雷达、速度传感器或里程表)。imu26可以具有六个自由度(dof)配置,并且对于三个正交轴中的每个正交轴具有一个陀螺仪和一个加速度计。加速度计用于检测比力,陀螺仪用于检测角速率。应当理解,imu26可以采用更简单的配置。里程表可以包括车轮里程表、视觉里程表或雷达里程表。一般地,里程表估计载运工具10的速度。imu26可以是低成本的微机电系统(mems)imu。
传感器27允许航位推测导航并且不依赖于载运工具10和外部源之间的信号传输。然而,在给定初始条件集的情况下,传感器27提供可用于计算载运工具10的相对位置的信息。此外,传感器27容易受累积误差的影响,可以对传感器27进行常规的校准程度以便维持质量。
图4示意性地示出了控制器9的示例性结构。控制器9包括cpu9a,cpu9a被配置为读取并执行存储在ram存储器9b中的指令,ram存储器9b可以是易失性存储器。ram9b存储用于由cpu9a执行的指令和那些指令使用的数据。控制器9还包括非易失性存储设备9c,例如硬盘驱动器,但是应当理解,可以使用任何其他形式的非易失性存储设备。用于由cpu9a执行的计算机可读指令可以存储在非易失性存储设备9c中。此外,非易失性存储设备9c可以存储地图数据库24的副本。
控制器9还包括i/o接口9d,与控制器9结合使用的外围设备可以连接到该i/o接口9d。外围设备可以包括键盘、数据存储设备等。还可以提供通信接口9i。通信接口9i可以提供到其他设备(例如,传感器25、27)的短距离连接。短距离连接可以经由蓝牙、近场通信(nfc)等。通信接口9i还可以提供到诸如互联网或卫星之类的网络的连接,以进行更长距离的通信。如果地图数据库24存储在远离载运工具10的服务器中,则更长距离的通信可以用于检索地图数据库24。更长距离的通信还可以由gnss传感器使用以生成绝对位置信息。cpu9a、ram9b、非易失性存储设备9c、i/o接口9d和通信接口9i通过总线9j连接在一起。
应当理解,图4所示的组件的布置仅是示例性的,并且控制器9可以包括与图4所示的组件不同的、附加的或更少的组件。
存储在非易失性存储设备9c中的计算机可读指令提供如图5至图6所示的功能组件。计算机可读指令在由cpu9a执行时,使控制器9执行图7和图12所示的处理步骤,以确定载运工具10通过接合点采用的路线。
具体地,如图5所示,计算机可读指令提供数据服务器31、轨道约束管理器32和导航滤波器34,导航滤波器34也可以被称为贝叶斯估计滤波器。在图5的具体示例中,导航滤波器34被实现为无迹卡尔曼滤波器。导航滤波器24包括状态预测器35和状态更新器40。
数据服务器31
数据服务器31负责收集从传感器25、27接收的传感器数据,并且向导航滤波器34分发从传感器25、27接收的传感器数据。数据服务器31输出两个数据流,即(i)从imu26收集并用作驱动状态预测器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的估计位置)输出的最新的估计状态
在图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输出的用于计算导航解决方案的传感器测量结果中的误差(例如,imu26的惯性测量结果中的偏差)。此外,当在如下描述的导航滤波器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的状态
在预测步骤30中,导航滤波器34的状态预测器35基于轨道约束函数xe(s)、imu测量数据
imu测量数据可以被称为“第一传感器数据”。非imu测量数据可以被称为“第二传感器数据”。
预测步骤30由从imu26收集的惯性测量结果的数据驱动。预测步骤30用于在非imu传感器测量的更新之间及时向前传播估计状态。
当预测步骤30完成时,在步骤41处查询数据服务器31以确定是否存在任何待处理的非imu传感器测量结果。如果没有可用的非imu测量结果,则对每个进入的imu测量数据重复预测步骤30,直到非imu测量数据变得可用时为止。此外,当预测步骤30完成时,将新的估计状态
当可从数据服务器31获得非imu传感器测量结果y时,触发更新步骤33。在更新步骤33中,导航滤波器34的状态更新器40基于先验估计
在完成预测或更新步骤之后,估计状态
以此方式,导航滤波器34能够通过将imu传感器测量结果和非imu传感器测量结果与轨道约束函数xe(s)融合,来提供对载运工具10的状态的相对精确的估计。由于贝叶斯估计滤波器的性质(其过滤噪声而不是数据),包含在载运工具10的估计状态中的噪声的程度比包含在状态过程模型(例如,受约束的捷联ins47)中的过程噪声或者包含在imu传感器测量结果和非imu传感器测量结果中的测量噪声小。
在图5提供的具体示例中,导航滤波器34被实现为无迹卡尔曼滤波器。无迹卡尔曼滤波器适于处理非线性的过程模型,并且不需要对过程模型进行显式或数值近似的雅可比估计(然而扩展的卡尔曼滤波器需要)。
在无迹卡尔曼滤波器中,当状态根据非线性过程模型随时间演变时,使用无迹变换来估计状态的均值和误差协方差。基本假设是,估计状态的分布始终可以通过高斯分布很好地近似,特别地,在状态
利用从估计状态(例如,
下面参考状态预测器35和状态更新器40更详细地描述导航滤波器34的预测步骤30和更新步骤33。
状态预测器35
在步骤36处,状态预测器35基于在前一时期的更新步骤33期间生成的估计状态
状态预测器35还包括用作状态过程模型的受约束的捷联ins47(在图6中示出),并且执行步骤37以及时向前传播估计状态。在步骤37期间,将imu测量数据
测量数据sb是指在b框架中解析的比力,b框架是相对于承载imu26的载运工具10固定的旋转参考框架。b框架由右手笛卡尔坐标轴的集合表示,其原点位于imu26的质心上,imu26本身牢固地捆扎在载运工具10上。b框架包括与载运工具10的中心线平行的x1轴,因此相对于载运工具10固定。测量数据
ins47的名称中的术语“受约束的”意味着,与常规的捷联惯性导航算法不同,通过合并轨道约束函数xe(s)来修改由ins47执行的积分,ins47基于速度数据确定imu26的位置,使得产生的最终位置被约束在轨道约束函数xe(s)的曲线上。这确保了状态预测器35永远不会产生偏离轨道的位置解决方案。
在图6中示出受约束的捷联ins47的示例。ins47输出数据52、62和63。数据52
受约束的捷联ins47具有t框架积分器50,该t框架积分器50对imu测量数据
受约束的捷联ins47还包括g框架积分器53,g框架积分器53通过对数据64
g框架积分器53具有初始条件54,初始条件54包括例如引力惯性速度
t框架积分器50和g框架积分器53不受轨道约束函数xe(s)的影响,并且可以使用公知的算法来实现。
然后,在加法器55处组合imu26的推力惯性速度
r框架积分器56对r框架沿着轨道的速度进行积分,即,沿着轨道从所选择的参考点开始的弧长s的一阶导数
在等式(1)中,
在等式(1)中,
随后,r框架积分器56在初始条件57下随时间对
将框58表示的轨道约束函数xe(s)提供给r框架积分器56。r框架的e框架位置坐标(即,
还可以根据等式(5)和等式(6),使用
因此,r框架积分器56对imu26的沿轨道速度进行积分,以获得imu26沿着轨道约束函数xe(s)的曲线相对于所选择的参考点的行进距离。因此,受约束的捷联ins47封装了用于感兴趣的主导航子状态的时间演进函数,即:(i)b框架相对于i框架的姿态,即数据52;(ii)b框架相对于i框架的速度,即
由于由r框架积分器56执行的积分结合了轨道约束函数xe(s),因此由ins47确定的imu26的位置(即,数据63)被约束在轨道约束函数xe(s)的曲线上。imu26的位置(即,数据63)是导航滤波器34的估计状态(例如,
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),基于误差协方差
西格玛点
在更新步骤期间,还可以利用轨道约束函数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)构成了对轨道约束信息的最佳使用。通过在受约束的捷联ins47中施加轨道约束函数xe(s),载运工具10的位置估计值的演进始终与沿着由轨道约束函数xe(s)表示的轨道中心线的一维运动一致,并且状态预测器35生成的状态预测(例如,
路线消歧
控制器9还执行用于确定载运工具10通过运输网络的接合点所采用的路线的处理步骤。该处理步骤在图12中示出,并在下面进行描述。
在步骤s1处,控制器9(具体地,图5的轨道约束管理器32)获得轨道几何数据(例如,轨道约束函数xe(s)和/或存储在地图数据库24中的网络拓扑),轨道几何数据指示载运工具10附近的运输网络的至少一部分的轨道几何形状。基于载运工具10的当前位置来即时动态地获得轨道约束函数xe(s)。载运工具10的当前位置可以是载运工具10的初始位置,该初始位置由提供指示载运工具10的绝对位置的信息的传感器25确定,备选地,载运工具10的当前位置可以是由状态预测器35输出的估计状态
在步骤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示出了适于由本文所描述的技术使用的贝叶斯估计滤波器算法的一般工作原理。贝叶斯估计滤波算法包括系统模型m100、观察模型h101和卡尔曼增益k103。贝叶斯估计滤波器算法使用观测值或测量值y(例如,由传感器25、27中的至少一个输出的传感器数据)来估计载运工具10的状态
可以将轨道几何数据中定义的轨道约束(例如,轨道约束函数)适当地应用于贝叶斯估计滤波器算法,使得由贝叶斯估计滤波器算法提供的载运工具的估计位置沿着由轨道约束函数限定的轨道。
用轨道约束来约束贝叶斯估计滤波器算法可以通过多种方式来实现。在示例中,可以将算法输出的最终估计位置(不受约束)正交投影到由轨道几何数据定义的轨道上,以便获得受约束的解决方案。以上描述并且在图6中示出的捷联ins47提供了备选示例。
在示例中,图13的贝叶斯估计滤波器算法(和图5的导航滤波器34)的状态空间和测量空间可以是欧几里得向量空间,具体是n维欧几里得向量空间。也就是说,状态(例如,
在备选示例中,状态(例如,
使用矩阵李群表示状态和测量是有利的,因为矩阵李群可以使用乘积矩阵李群轻松地表示包括多个子状态的复杂状态。乘积矩阵李群封装了状态空间的拓扑结构。相反,在将状态模型转换为n维欧几里德向量空间的过程中,状态空间的拓扑结构可能丢失。
图12所示的处理步骤、图13的贝叶斯估计滤波器算法以及图5的导航滤波器34可以用c++语言或任何其他合适的编程语言来实现。
在图5提供的示例中,导航滤波器34被实现为无迹卡尔曼滤波器。应当理解,可以使用不同类型的贝叶斯估计滤波器(例如,扩展的卡尔曼滤波器等)来实现导航滤波器34。
尽管以上描述已经列出了优选实施例,但是应该理解,这些实施例仅是说明性的,并且权利要求不限于这些实施例。鉴于本发明内容,本领域技术人员将能够进行修改和替换,这些修改和替换被认为落入所附权利要求的范围内。在本说明书中公开或示出的每一个特征,无论单独或者与本文公开或示出的任意其他特征进行任意合适组合,可包含在这些实施例中。
参考文献:
[1]vandermerwer.、wane.、juliers.的“sigma-pointkalmanfiltersfornon-linearestimationandsensorfusion:applicationtointegratednavigation”,proceedingsofamericaninstituteofaeronauticsandastronautics(aiaa)guidance,navigationandcontrolconferenceandexhibition,8月16日-19日,普罗维登斯,罗德岛,2004年。
[2]vandermerwer.、wan的“sigma-pointkalmanfiltersforprobabilisticinferenceindynamicstate-spacemodels”,proceedingsoftheworkshoponadvancesinmachinelearning,6月8日-11日,蒙特利尔,加拿大,2003年。
[3]hallb.c.的“liegroups,liealgebrasandrepresentations:anelementaryintroduction”,graduatetextsinmathematics222(第二版),施普林格,isbn0-387-40122-9,2015年。
[4]iwasawak.的“ontherepresentationofliealgebras”,japanesejournalofmathematics,1948年11月9日,第405-426页。
[5]hertzbergc.、wagnerr.、freseu.和
[6]fletcherp.t.、lu,c.和joshi,s.的“statisticsofshapeviaprincipalcomponentanalysisonliegroups”,proceedingsofieeeconferenceoncomputervisionandpatternrecognition,2003年,第95-101页。