CN101563625A - 用于二维和三维精确位置和定向确定的布置和方法 - Google Patents

Abstract

用于根据以下动作来确定一组GPS样本中不准确的GPS样本的方法和设备：a)获得由交通工具载有的全球定位系统在沿着轨迹行进时取得的GPS样本；b)基于所述GPS样本，获得所述轨迹的第一估计；c)至少基于由交通工具载有的惯性测量单元在沿着所述轨迹行进时做出的测量而获得所述轨迹的第二估计；d)将所述第一估计与第二估计进行比较；e)建立其中所述第一估计与所述第二估计相比展现高于预定阈值的变化的位置；f)如果不能建立此些位置，那么以动作j)继续，否则以动作g)继续；g)将与具有高变化的位置相关联的GPS样本作为不准确的GPS样本而去除，从而形成一组剩余的GPS样本；h)基于所述剩余的GPS样本再计算所述轨迹的所述第一估计，且再计算所述第二估计；i)重复动作d)到h)；j)结束所述动作。

Description

用于二维和三维精确位置和定向确定的布置和方法

技术领域

本发明涉及二维和三维精确位置和定向确定的布置和方法。

背景技术

位置确定装置是现有技术中已知的。现今位置确定装置越来越多地应用于(例如)交通上具，例如车辆、轮船和飞机。为此目的，此交通工具可包括不同的测量单元，如全球定位系统(GPS)、惯性测量单元(IMU)和距离测量仪器(DMI)。

在行进时，此些测量单元的输出数据由处理器用来计算交通工具的位置和/或定向。取决于应用，在线或离线使用这些测量单元做出的测量。

一般需要从3个测量单元：GPS、IMU和DMI提供尽可能准确的位置和定向测量。为此目的应解决许多问题，例如多路径问题、测量信号中的噪声以及IMU的输出信号中的漂移(或其它移位)。在当前技术水平下，卡尔曼(Kalman)滤波器广泛用于补偿来自IMU的输出信号中的漂移以及补偿其它效应。而且，其它统计方法，如移动平均技术和高斯(Gaussian)白噪声滤波，可用于从信号去除许多噪声以使信号变得纯净。

然而，有效地去除高斯噪声的一些滤波技术和平均技术对经偏移信号将低效地起作用，所述信号展示对测量系列(如在GPS测量中)的较长时间常数移位。

第5,311,195号和第5,948,043号美国专利揭示具有其它传感器和用于识别不应考虑的不准确GPS测量的非卡尔曼构件的GPS系统。

发明内容

本发明的目标是提供一种位置和定向确定系统以及位置和定向确定方法，以改进基于在沿着轨迹移动的交通工具上执行的测量的位置和定向计算中的至少一者。

为此，本发明提供一种如独立设备技术方案中所主张的计算机布置。

而且，本发明涉及一种如独立方法技术方案中所主张的方法。

本发明提供非常准确且可靠的方法来从在交通工具沿着其轨迹移动时收集到的一组GPS样本中去除不准确的GPS样本。通过使用所主张的设备或方法，不准确的GPS样本以非线性方式被消除。

在一实施例中，关于位置和定向的计算可离线进行，即，在已收集到来自轨迹的所有测量数据之后进行。这(例如)在所谓的移动测绘系统(MMS系统)中是真实的，其中通过沿着道路网行进的交通工具来收集位置和定向数据，所述位置和定向数据稍后用于产生2D和/或3D道路地图，或捕捉可在一区域(如城市中的道路)的3D显示中使用的地理数据，所述3D显示还展示沿着所述道路的建筑物的立面。可应用本发明的其它领域是在道路资产细目数据库创建中，其中MMS系统可提供重要的支持。同样可使用其它传感器，如用于路面管理的作为地理雷达的传感器、基于激光的粗糙度系数测量单元、广泛用于裂纹检测应用的频闪观测器增强高速垂直相机，以及用于对象确定、3D视图应用等的激光扫描仪。

在此实施例中测得的位置和定向数据无需实时使用而是仅在后来使用的事实向离线处理器提供了执行不同于在实时情况下是可能的校正机制的校正机制的能力。本发明的离线方法提供检查如在沿着道路网行进期间收集到的所有GPS样本的甚至更准确的选择，从而识别不准确的GPS样本，且不再考虑这些不准确的GPS样本来计算所行进的轨迹。如以此方式计算出的轨迹具有较高的准确度，因为其不再受经偏移GPS信号影响。

因此，本发明提供准确的结果，其可非常成本有效地实施且可在离线环境中使用，在所述离线环境中，位置/定向测量是从用例如已以平稳方式(因此，没有例如由比赛车辆做出的突然移动)沿着道路行驶的交通工具收集到的数据计算的。

在离线环境中，本发明可用于产生轨迹的更敏感确定，且因此最终产生略微更精确的最终结果。这是因为在离线实施例中，可应用形状滤波器，所述形状滤波器为全局的、回归的且自适应的，且可将GPS样本分类为准确的或不准确的。

在一实施例中，本发明提供对用于计算交通工具的随着时间而变的位置和定向的惯性测量单元(IMU)的输出信号中的漂移(或其它移位)进行计算的选择。这是通过使用GPS样本来完成的，且随后校正IMU信号的以此方式确定的漂移。随后将经漂移校正的IMU信号用作用于计算位置和定向的主要基础，而GPS样本主要用于漂移校正。用于此漂移补偿的GPS样本的数目可高达25％。

附图说明

将参看一些图式来详细阐释本发明，所述图式既定用于说明本发明而不是限制其范围，本发明的范围由附加的权利要求书及其等效实施例界定。

在图式中：

图1展示交通工具中的现有技术位置和定向测量系统；

图2a展示局部交通工具坐标系；

图2b展示所谓的wgs坐标系；

图3展示计算机布置的示意性实例；

图4是用于阐释多路径问题的示意图；

图5展示GPS系统和惯性测量单元(IMU)的输出信号的图；

图6展示用于阐释本发明的流程图；

图7a到图7d展示用于从一组GPS样本中去除不准确的GPS样本的方法中的相继动作；

图8a、图8b和图8c展示用于阐明可如何估计IMU信号中的漂移(和其它移位)的曲线；

图9a和图9b展示可如何通过回旋曲线来近似表示道路交通工具所行进的轨迹；

图10a、图10b和图10c展示用于阐明可如何估计IMU纵摇信号中的漂移(和其它移位)的曲线；

图11展示如何相对于局部重力向量来界定斜率。

具体实施方式

图1展示具备现存的高准确度位置确定装置的交通工具。图1展示交通工具1，其具备多个车轮2。而且，交通工具1具备高准确度位置确定装置。如图1所示，位置确定装置包括以下组件：

·全球定位系统(GPS)单元，其连接到天线8，且经布置以与多个卫星SLi(i＝1、2、3、…)通信并根据从卫星SLi接收到的信号来计算位置信号。GPS系统还提供交通工具1的航向数据(即，行进的方向)和速度。GPS系统可为差分GPS(DGPS)系统，其提供1∑/1米的准确度(除可能的白噪声之外)。GPS系统连接到微处理器μP，所述微处理器经布置以处理GPS输出信号。

·距离测量仪器(DMI)。此仪器是通过感测车轮2中的一者或一者以上的旋转数目来测量交通工具1所行进的距离的里程表。因此，DMI将测量后车轮中的一者的旋转数目，因为后车轮相对于交通工具的主体不旋转。DMI可以10Hz或10Hz以上的取样频率操作。DMI也连接到微处理器μP，以允许微处理器μP在处理来自GPS单元的输出信号时考虑如DMI所测量到的距离。

·惯性测量单元(IMU)。此IMU可实施为：3个陀螺仪单元，其经布置以测量旋转加速度；以及三个加速度计，其经布置以沿着3个正交方向测量平移加速度。IMU/陀螺仪和加速度计可以200Hz的取样频率操作。IMU也连接到微处理器μP，以允许微处理器μP在处理来自GPS单元的输出信号时考虑IMU的测量。在本发明的一些实施例中观察到，1轴或2轴IMU或者1轴陀螺仪和用于2D定位支持的DMI可以是足够的。

·DMI、IMU和GPS系统连接到微处理器μP。微处理器μP用于确定这三个测量系统中的任一者做出测量的时间：微处理器时钟可用于所述目的。可通过接收来自GPS信号的PPS信号(PPS＝精确定位服务)来改进所述时间的线性度。基于从DMI、IMU和GPS系统接收到的信号，微处理器μP可确定将在交通工具1的监视器4上显示的合适显示信号，从而告知驾驶员交通工具所在的位置以及可能交通工具正行进所在的方向。

如图1所示的系统可应用于所谓的“移动测绘系统”中，“移动测绘系统”(例如)通过用安装在交通工具11上的一个或一个以上相机9拍摄图片来收集地理数据。而且，可将一个或一个以上激光扫描仪(图1中未图示)安装到交通工具1。相机(以及任选的激光扫描仪)连接到微处理器μP。相机图片以及可能其它传感器数据可连同所有DMI、IMU和GPS数据一起被收集，以供将来离线处理。图1的系统的另一应用可在交通工具导航系统的领域中。现今，在交通工具导航系统中，显示信号通常涉及二维(2D)地图上的位置。

图2a展示从图1所示的三个测量单元GPS、DMI和IMU可获得哪些位置信号。图2a展示微处理器μP经布置以计算6个不同的参数：相对于原点的3个距离参数x、y、z以及3个参数ω_x、ω_y和ω_z。请注意，坐标系的原点不是固定的，而是相对于交通工具(例如，相对于IMU系统)而界定的。3个参数ω_x、ω_y和ω_z分别表示分别围绕x轴、y轴和z轴的角度改变速度(或旋转速度)。z方向与局部重力向量的方向重合。在许多情形中，轨迹相对于重力向量将具有斜率。因此，x方向不垂直于局部重力向量。此坐标系和测量还可结合离线使用的计算系统而使用。请注意，交通工具围绕y轴的定向通常被称为纵摇，而围绕x轴的定向通常被称为横摇，且航向是围绕z轴的定向。因此，以下等式成立：

纵摇＝∫ω_x·dt

横摇＝∫ω_y·dt

航向＝∫ω_z·dt

图2b中展示所使用的坐标系。下文中将阐释处理器11可如何在地理坐标中确定交通工具1的纬度、经度和海拔以及自由角度。地理坐标系(GEO)被界定为使得其X轴在地球的赤道平面内，但随着地球的旋转而固定，使得其穿过格林威治子午线(0°经线)。所述坐标系的Z轴平行于地球的旋转轴，且所述坐标系的Y轴完成右旋正交集(Y＝Z×X)。为了这样做，将阐释如何确定交通工具的位置和定向，其中所述交通工具的测量仪器GPS、IMU和DMI在如由局部重力向量界定的局部移动坐标系中。

为此，将交通工具1布置在具有某一纬度/经度的点处，其中其x轴平行于交通工具1的航向，且z轴平行于在地球上所述点中的局部重力向量。

一般来说，本申请案可应用于如图1所示的交通工具。然而，本申请案可同样应用于任何其它具备车轮以沿着地球边界轨迹移动的交通工具，例如公共汽车、卡车、火车或有轨电车等。而且，如下文所阐释的方法的实施例(针对从存储受非高斯噪声影响的GPS样本的数据库去除不准确的GPS样本(其可能由于多路径或其它原因而从真实位置偏移，即，非经模型化的对流层传播延迟))同样适用于基于水的系统(例如船)和航空系统(直升机和飞机)，只要所述系统配备有GPS和惯性定位系统。

本发明可应用的领域之一涉及产生城市街道中的建筑物的3D图像。为了产生此些图像，可使用移动测绘系统(MMS)，其由驾驶员驾驶穿过所关注的街道。图1展示此MMS系统，如上文已阐释。微处理器μP经布置以控制相机9，使得其以规则的时间或距离间隔或以特定的航向改变来拍摄图片。对于一些应用来说，相机9是以其可拍摄城市中的建筑块的立面的图片的方式而对准环境的。这些立面图片在稍后的时间点处(因此，离线)由单独的计算机布置处理。图3展示此离线计算机布置10。

观察到，所述图片可能替代地已通过航空交通工具载有的一个或一个以上相机来拍摄。

而且，观察到，MMS系统(或航空系统)还可包括一个或一个以上激光扫描仪，其收集(例如)建筑物的激光样本，在将图片映射到建筑物立面的过程或识别道路标志的过程中使用所述激光样本。

在图3中，给出计算机布置10的总览，所述计算机布置10可用于实行根据本发明的离线计算。计算机布置10包括用于实行算术运算的处理器11。

处理器11连接到多个存储器组件，包含硬盘12、只读存储器(ROM)13、电可擦除可编程只读存储器(EEPROM)14以及随机存取存储器(RAM)15。未必需要提供所有这些存储器类型。而且，这些存储器组件无需物理上位于靠近处理器11处，而是可位于远离处理器11处。

处理器11还连接到用于通过用户来输入指令、数据等的构件，如键盘16和鼠标17。也可提供所属领域的技术人员已知的例如触摸屏幕、跟踪球和/或语音转换器等其它输入构件。

提供连接到处理器11的读取单元19。读取单元19经布置以从如软盘20或CDROM21等数据载体读取数据且可能将数据写入在所述数据载体上。其它数据载体可为磁带、DVD、CD-R、DVD-R、记忆棒等，如所属领域的技术人员已知。

处现器11还连接到用于将输出数据印刷在纸上的打印机23，以及连接到显示器18，例如监视器或液晶显示器(LCD)屏幕或所属领域的技术人员已知的任何其它类型的显示器。处理器11可连接到扬声器29。

处理器11可借助于I/O构件25连接到通信网络27，例如公共交换电话网络(PSTN)、局域网(LAN)、广域网(WAN)、因特网等。处理器11可经布置以通过网络27与其它通信布置通信。

数据载体20、21可包括呈数据和指令形式的计算机程序产品，所述数据和指令经布置以向处理器提供执行根据本发明的方法的能力。然而，此计算机程序产品可替代地经由电信网络27而下载。

处理器11可实施为独立的系统；或实施为多个并行操作的处理器，其每一者经布置以实行较大计算机程序的子任务；或实施为具有若干子处理器的一个或一个以上主处理器。本发明的功能性的部分可甚至由通过网络27与处理器11通信的远程处理器来实行。

观察到，图3所示的布置还可用于交通工具1中的微处理器，尽管在交通工具1中并不需要应用如图示的所有组件，如扬声器29和打印机23。

在一实施例中，处理器11经布置以接收如由相机9拍摄的图片，且将所述图片存储在其存储器中的一者(例如，硬盘12)中。硬盘12还存储从其拍摄立面图片的建筑块的所谓的“占地面积”。这些占地面积包括关于建筑块在地球上的位置的2D数据。存储器12到15中的一者存储一程序，所述程序可由处理器11运行且指令处理器将立面图片与占地面积组合，使得正确的立面图片与正确的建筑块相关联。如此获得的数据被存储以供稍后用于(例如)交通工具导航系统中，以向驾驶员展示他/她正驾驶在的街道的3D视图。为此，此数据可存储在DVD上。

本发明并不是针对可由处理器11运行以使立面图片与占地面积相关联的计算机程序。任何现有技术(或仍待开发的)的计算机程序均可在本发明的情形中用来进行此操作，或其可在所属领域的技术人员的辅助下完成。

本发明涉及与(例如)立面图片相关联的位置和定向数据。更一般地说，本发明涉及由某一(某些)测量单元收集到的数据，其中所述数据与在地球上的位置和定向相关联，且此位置和定向是在测量其它数据的同时测量到的。测得的数据可(例如)涉及完全不同于立面图片的某物，如用于数字地图的道路侧标志或用于搜索或探测土壤中的自然能源(石油、天然气等)的土壤条件，或手机或其它RF信号强度测量。因此，本发明的应用不限于交通工具1上用于收集在地球上某一位置处相关的数据的传感器类型。然而，为了简单起见，下文在阐释本发明时有时将使用收集立面图片的实例。

如所属领域的技术人员将明了，涉及每一立面图片的3D位置数据必须由MMS系统尽可能准确地测量，以允许处理器11将立面图片数据正确地对准到其它可用数据，例如建筑物占地面积数据。立面图片的这些位置数据直接与在相机9拍摄每一图片的时刻MMS系统的位置和定向数据有关。因此，应尽可能准确地知晓在拍摄立面图片时MMS系统的位置和定向。在拍摄这些图片时，MMS系统应(例如)具有关于其在x、y和z上的位置的1到1.5米或更好的准确度，以及关于其角度定向的航向、纵摇和横摇的0.1°或更好的准确度。

如所属领域的技术人员已知，现有技术系统使用图1所示的系统的GPS部分来导出位置数据x、y且可能还有z。然而，所属领域的技术人员还知道，GPS系统数据的准确度因为如多路径误差等误差而降级。图4以示意方式展示多路径问题。

图4展示MMS在两个建筑块BB1与BB2之间的街道上驾驶。卫星(未图示)向地球传输位置信号。如果MMS系统与卫星之间的直接且不中断的传输路径将可用，那么MMS系统将经由沿MMS系统与卫星之间的直线的信号路径SP1接收位置信号。然而，建筑块BB1阻挡了传输路径SP1。然而如图所示，MMS系统确实从卫星接收到如由另一建筑块BB2反射的位置信号。然而，所述位置信号已沿另一路径SP2行进，从而导致如取决于所关注卫星的不准确的位置测量。

在真实情形中，来自卫星的位置信号将常由MMS系统经由多个路径接收。在许多情况下，存在直接路径SP1和经由对着建筑块和树木等的反射的多个路径。一个此反射源(例如)由从MMS系统旁边经过的大卡车形成。例如以卡尔曼滤波器、合适的统计计算、随着时间的过去的求平均等的形式提供现有技术解决方案以解决多路径问题。其它多路径误差源可涉及雷暴云或其它电离层反射。这里提到的多路径误差的实例无意是详尽的。然而，尤其在基于陆地的应用中，在公共道路卡车、周围建筑物、标志以及移动的交通工具上产生复杂且快速改变的局部配置。在此情形中，基于较长时间观察的多路径确定的标准方法很可能失败。

现在将阐释如由两个其它系统IMU和DMI(图1)中的至少一者收集到的位置和定向测量数据可如何用于判定哪些GPS测量值很可能是准确的，且哪些GPS测量值很可能不准确(相对于所需的准确度)且不应在任何稍后的位置计算(例如由处理器11(图3)执行)中被考虑。在下文中，将首先参考2D情形(即，不考虑高度z且不考虑纵摇和横摇，而仅考虑x、y和航向的情形)来阐释完成此判定的方式。只有在其之后才将给出3D实例。

计算2D位置和定向

图5以示意方式展示四个相继区域A1、A2、A3、A4，其中MMS系统已沿着道路行进。图5是道路表面的示意性俯视图(未按比例绘制)，其指示交通工具1将如何根据不同的测量源而行进。图5展示MMS系统已在如用箭头AR指示的方向上行进。图5中的虚线指示MMS系统所行进的真实轨迹。

在所有四个区域A1、A2、A3、A4中，MMS系统已收集GPS样本以便在连续立面图片的拍摄期间测量MMS系统的2D位置和定向。用G(i)(i＝1、2、3、…、I)来指示所行进的根据GPS样本的轨迹。每秒可存在5个或10个或甚至更多的GPS样本，然而本发明不限于此数字。

图5以非常示意性的方式展示GPS样本。即，将GPS样本展示为连续的步骤。实际上，每一步骤是一个实例，其中相继样本之间存在取样时间。

而且，IMU系统已测量MMS系统所行进的轨迹。IMU系统所计算的轨迹以实线指示。回想IMU是相对位置和定向测量装置，且需要一绝对参考来使其相对于轨迹而放置。在此实例中，在轨迹的开始(左侧)处，将纯IMU位置任意地放置在真实轨迹附近。指示此实线在MMS系统随着时间的过去的移动期间越来越偏离真实轨迹。这是由于IMU系统所做出的计算中的漂移或其它未校准的移位而导致的。此移位/漂移是由IMU仅提供相对测量而不提供绝对测量的事实引起的。由漂移引起的误差可阐释如下。

如上文所指示，IMU系统提供可从中推断以下参数的数据：

·围绕x轴、y轴和z轴的旋转速度，即dω_x/dt、dω_y/dt、dω_z/dt(图2a)；

·a_x、a_y和a_z，即分别在x、y和z方向上的加速度。

为了计算在2D空间中的位置和定向，使用在x和y方向上的加速度a_x和a_y以及围绕z轴的旋转速度，即ω_z。从这些加速度，可如下计算位置x和y以及航向：

x＝∫∫a_x·dt²

y＝∫∫a_y·dt²

航向＝∫dω_z/dt

通常，从这些加速度计算x、y位置和航向随着时间的过去是不够准确的，这是由于输出信号中表示误差的漂移所导致的，所述误差随后根据双积分而累积。每行进1km此漂移通常可大约为1m。在下文中，将使用术语“漂移”来指代如以这些等式阐释的所述种类的漂移以及由于IMU系统的输出信号中的累积误差而导致的其它种类的移位。

最后，图5中的点划曲线展示基于GPS真实航向测量样本G(i)和时间上接近的GPS探测器之间的局部距离以及IMU测量和DMI测量而估计的MMS系统所行进的轨迹。

如从图5可见，在区域A4中，GPS系统展示估计轨迹与GPS输出之间的逐渐增加的偏差。这可能是由如所属领域的技术人员将显而易见的所有种类的误差引起的。

而且，图5展示GPS测量值在区域A1与A2之间以及A2与A3之间的过渡处展示其输出中的突然移位(请注意，图5仅展示一些GPS样本。实际上，区域(如区域A2)中的GPS样本的数目可能大得多)。此些突然移位可为多路径误差的指示。然而，不能简单地宣称区域A2和A4中的GPS测量值由于多路径误差而不准确。同样可能的情况是区域A1和A3中的GPS样本由于多路径误差而不准确。另外，不能以推理方式依赖于在这些区域中如由IMU系统测量到的x和y的绝对值，因为所述绝对值展示如上文所阐释的未知漂移，且需要某一绝对参考。然而，可使用IMU输出数据来找出哪些GPS样本不准确且哪些GPS样本是准确的。接着，可去除不准确的GPS样本，使得一组准确的GPS样本保留下来。这组可靠的GPS样本随后又可用于为IMU建立绝对位置且校准IMU输出数据，即估计IMU输出数据中的漂移且从IMU测量值中去除所述漂移，从而得出总体准确的位置确定。这将在下文参看图6、图7a到图7d以及图8a到图8c来详细阐释。

图6展示可根据本发明而执行的动作的流程图。这些动作由处理器11执行，且针对解决区域A2中的多路径问题以及区域A4中所示的偏差两者。

在动作62中，处理器11获得如由GPS系统测得的交通工具1的随时间而变的航向。在这里将所述航向称为“GPS航向”。图8a展示此GPS航向在周期t0到t3期间的实例。请注意，GPS测量受到噪声的影响，如曲线中较小的高频变化所指示。

处理器11从GPS系统获得的其它数据包含交通工具1的x、y位置，如动作88中所指示。从这些x、y位置可推导出交通工具1的速度，如动作78中所指示。如从现有技术已知，所有这些测量数据在统计上不相关。

在动作64中，处理器11获得如由IMU系统沿MMS系统所行进的轨迹测得的旋转速度ω_z，且提供如通过上文所阐释的积分计算出的交通工具1的航向。

在动作72中，处理器11获得如由DMI系统沿MMS系统所行进的轨迹测得的MMS系统的车轮角度随时间而变的值。

为了计算在2D平面内的位置和定向，将在大多数情况下具有大于1000kg的质量的交通工具1视为相当于低通滤波器。因此，假定在交通工具1正在移动时将不会观察到交通工具1所行进的轨迹中的快速改变。而且，假定交通工具1相当于固定框架。

DMI传感器(例如)安装到交通工具1的后车轮。因此，DMI在2D平面内相对于交通工具1的质心的位置波动最小，且可将其输出中的波动视为幅值比GPS信号中的波动小得多的白噪声。因此，在计算中可忽略DMI输出中的波动。

根据交通工具动力学模拟，众所周知的是交通工具主体相对于局部重力向量的角度与交通工具的加速度成比例。这在“正常”条件下是真实的，所述“正常”条件即(例如)交通工具没有被卷入事故，驾驶员在向前方向上以非常平稳的方式(非强有力的驾驶风格)驾驶交通工具(以同一比例因数刹车、加速)。

在这些假定下，以下等式有效：

“MC轨迹”＝“V参考轨迹”+“MC振荡”(1)

“MC轨迹”＝“V参考轨迹”+“MC振荡”(1)

其中：

·MC轨迹＝交通工具1的质心所行进的轨迹；

·V参考轨迹＝如从GPS系统导出的交通工具1所行进的参考轨迹；

·MC振荡＝交通工具1所行进的参考轨迹与交通工具1的质心之间的局部差异；MC振荡的幅值预期在0.1m的范围内，且可被视为针对轨迹确定的白噪声。

由于交通工具主体可被视为固定框架，因此交通工具主体的所有点共享与交通工具1的质心相同的定向(即，具有相同的横摇、纵摇和航向)。因此，可将交通工具1的位置确定为交通工具1的质心的位置的3D位移向量。

在一实施例中，不是使用交通工具1的中心作为局部坐标系的中心，而是将GPS天线8的位置用作局部坐标系中的中心。此做法的优点在于无需确定质心，其使计算便利。

现在，处理器11执行以下动作。

在动作70中，处理器11对如由IMU系统导出的交通工具1的航向执行局部线性回归，以便获得沿着轨迹的航向的经平滑的局部平均值。在此过程中，处理器11使用多个GPS样本(在所述GPS样本中的非高斯噪声去除之后)。这提供交通工具1的具有可能低于0.1度的精度的准确“真实”航向。动作70的输出是交通工具1的随时间而变的航向。

在动作74中，处理器11执行对如由DMI提供的车轮角度旋转的解校准。这再现所述车轮所行进的距离。动作74的结果是基于DMI测量的MMS系统所行进的随时间而变距离的估计。所述距离在这里被称为“DMI距离”。

在动作80中，如由GPS系统测得的速度由处理器11用来动态地校准此DMI距离。因此在动作80中，处理器11产生随时间而变的估计“真实”行进距离。

观察到，用GPS速度测量值动态地校准DMI测量值在确定“真实”行进距离方面通常比从IMU系统获得所述值更精确，因为DMI系统受惯性失真的影响较少。

将动作80的输出和动作70的输出两者输入到被称为“曲线计算”84的下一动作。在此曲线计算动作84中，使用以下等式：

INS_Trajectory2D＝Traj_true2D+IMU_drift2D+IMU_noise    (2)

其中：

·INS_Trajectory2D＝从惯性导航系统INS所做的测量计算出的交通工具1所行进的曲线，所述INS包含IMU、DMI和GPS单元。这是动作84的输出；

·Traj_true2D＝2D平面内的交通工具参考轨迹，即，交通工具1真实行进的轨迹。

·IMU_Drift2D＝IMU系统测量引起的漂移。在实践中，“IMU_Drift-2D”将是2D的随着时间的过去而缓慢改变的函数；

·IMU_noise＝IMU系统测量中的噪声。

等式(2)中的INS_Trajectory2D对应于图5中的“估计轨迹”。

在较小的距离上，以下等式有效：

INS_Trajectory2D＝Traj_true2D+IMU_drift2D+IMU_noise(小)(3)

Traj_true-2D＝GPS_meas2D-GPS_noise-GPS_multipath  (4)

其中：

·GPS_meas2D＝如从GPS系统导出的测得位置信号

·GPS_noise＝来自GPS系统的GPS_meas中如在没有多路径误差的情况下将存在的噪声。请注意，可从GPS样本将任何高斯噪声减到最小。可使用任何进行此做法的已知方法。然而，除高斯噪声以外，可能仍留有其它噪声。

·GPS_multipath＝GPS_meas中由于多路径问题而导致的误差。

现在，从等式(3)和(4)可导出：

IMU_drift2D＝INS_Trajectory2D-IMU_noise-

[GPS_meas2D-GPS_noise-GPS_mulipath](5)

现在，以如下形式来改写等式(2)：

Traj_ture2D_approx＝INS_Trajectory2D-IMU_druft2D_approx  (6)

其中所有参数具有与在等式(2)中相同的意义，且添加的“approx”指代具有近似值的参数。等式(6)是与动作86相关联的等式。而且，等式(5)可用于对由IMU系统引起的漂移(IMU_drift2D)做出第一估计。即，估计IMU_drift2D大致等于：

IMU_drift2D≈[IMU_meas2D-GPS_meas-2D](5a)

其中：

IMU_meas2D＝如由IMU系统测得的交通工具的航向。

这可如下参看图8a到图8c来阐释。图8a展示如由GPS系统测得的交通工具1的航向。图8a中所示的信号上可能仍具有(高斯)噪声。图8b展示相同的航向，但是从IMU系统导出。从IMU系统导出的航向相对于真实航向而具有漂移。

图8c展示从图8b的曲线减去图8a的曲线得到的曲线。所得曲线是相对于原点具有偏移的大体直线。所述曲线的角度对应于IMU系统中随时间而变的漂移的一阶且主要分量。所述曲线上仍展示由GPS系统引起的某一(高斯)噪声。然而，所述噪声可容易通过从现有技术已知的任何方法来滤出。其余的误差将是由于如下文所阐释的所识别出的多路径而导致的。

当然，当测得的GPS信号由于多路径问题和非高斯噪声而仍含有误差时，这些误差将也在图8c的曲线中可见。因此，只有GPS信号中由于多路径和非高斯噪声而导致的所有误差均已被去除，如参看图8a到图8c而阐释的方法才会再现对IMU系统中的漂移的适当估计。因此，当在动作86中使用时，结果仍非常不准确。而且，一旦越来越多的不准确GPS样本被从GPS测量值去除，就可以迭代方式使用所述方法。这将在下文阐释。

将等式(5)与(6)进行组合得到：

Traj_true2D_approx＝Traj_meas2D-EQ    (7)

[GPS_meas2D-GPS_noise-GPS_multipath-IMU_meas2D]

其中EQ[...]指代参数的时间数列均衡，即，在[...]之间确定预定数目(例如100个)样本上的移动平均值(或任何其它低通滤波器)。

观察到，可将所有这些2D信号视为在x和y方向两者上具有分量的参数时间数列。因此，将定位问题分解成一系列1维时间数列均衡问题。

如从等式(7)可见，现在的问题是为了计算EQ[...]，需要知道GPS_noise和GPS_multipath-IMU_meas2D。如果可从GPS测量值中滤出这两个参数，那么等式将较好地起作用。这可在如下的迭代过程中完成。

首先，如动作92中指示，处理器11应用局部方差滤波器，使得其测试是否：

|GPS_meas2D-Traj_true2D_approx|＞Threshold  (8)

其中GPS_meas2D由所有GPS样本形成。

此“Threshold”(例如)等于对GPS集合GPS的方差测量的平均方差值。可针对若干(例如20个到10个)GPS样本的范围而计算局部方差。

图7a到图7d展示此方差滤波器在应用于图5所示的实例时可如何工作。

图7a展示对应于图5所示的曲线(假如无实际轨迹)的若干曲线。第一曲线展示块形信号，其表示从俯视图看到的交通工具1所行进的根据多个连续GPS样本G(1)、G(2)、G(3)、…的轨迹。如从图7a可见，在GPS样本G(i)与G(i+1)之间、G(i+5)与G(i+6)之间以及G(j+6)与G(j+7)之间存在较大过渡。而且，存在开始于G(j+1)处的逐渐改变的过渡。较大的过渡可能是由于多路径误差而导致的。然而，仅基于GPS样本，无法判定哪些GPS样本与由于噪声/多路径问题而导致的错误测量有关，且哪些样本是正确的。

图7a中的第二曲线是实线，其指代如从IMU测量值a_x和a_y计算出的轨迹。此IMU线具有漂移(通常每1km为1m)，但确切的漂移值是未知的。

图7a中的第三曲线是虚线，其是动作86(即，上文的等式(6))的新计算出的曲线。

现在，在动作92中，处理器11执行方差滤波器，即，处理器11以如参看等式(8)所阐释的方式将GPS线与图7a的虚线进行比较。如可从图7a显而易见，所述比较再现了具有较高方差的区域。请注意，在本描述中，术语“方差”指代两个不同信号之间的差而不是同一信号中的连续样本之间的差。这些区域在图7a中以点划圈指示。因此，首先，这传达了样本G(j+1)到G(j+5)具有如此高的方差，以致其不应再在对交通工具1所行进的轨迹的计算中使用。其次，这传达了G(i)与G(i+1)之间的过渡处的样本具有过高的方差。因此，也不应再使用样本G(i+1)。对于样本G(i+5)也是如此。因此，应用方差滤波器的结果是不再使用与噪声有关的样本(即，G(j+1)到G(j+5))，以及由于多路径误差而导致的过渡处的GPS样本(即，G(i+1)和G(i+5))。

这里，假定与近似轨迹线Traj_true-2D_approx的比较是公平比较，其没有过多地受IMU系统引起的任何漂移问题影响，因为IMU信号中的漂移的影响足够小而可以在例如几个邻近GPS样本的较小时间周期中被忽略。

在动作90中，处理器11现在从所述组GPS样本中去除方差滤波器所发现的GPS样本(其可保存在存储器中但不再被考虑)。这在图7b中以十字指示。这再现了一组新的GPS样本。

在一实施例中观察到，在时间上稍后的动作处，处理器11可寻找哪些是被去除的GPS样本中最佳的样本，且将其再次添加到所述组准确的GPS样本中。这可能要花费处理器11一些时间，但也可改进轨迹的近似。尤其在已去除了许多GPS样本之后，将此些GPS样本以回归方式再次添加到所述组可显著减少没有任何GPS样本的空间。这可使所述方法更准确。

在动作94中，这组新的GPS样本用于对交通工具1所行进的轨迹Traj_ture2D_approx做出新的估计。新估计的轨迹在图7b中以点划线指示。而且，再次根据等式(5a)来估计IMU数据引起的漂移。

在动作96中，处理器11应用“形状滤波器”来检测在动作92之后所述组剩余GPS样本中的GPS样本，且将其从计算轨迹Traj_true-2D_approx中排除。在此“形状滤波器”中，再次应用等式(8)，但随后将等式(8)应用于来自动作94的新计算出的轨迹和更敏感的阈值。

此动作将再现展示较高方差的新GPS样本：在图7b中，这将标识应除去GPS样本G(i+2)和G(i+4)。处理器11从在动作94之后所述组剩余的GPS样本中去除这些GPS样本G(i+2)和G(i+4)。

如图7c中所示，在动作98中，处理器11检查在动作96中是否找到此些具有较高方差的GPS样本。如果找到，那么处理器以动作100继续。如果未找到，那么处理器11以动作102继续。

在动作100中，处理器11再次使用剩余的GPS样本来计算新估计的轨迹Traj_true2D_approx(在图7c中以点划线指示)。通过使用此新估计的轨迹Traj_true2D_approx，再次根据等式(5a)来计算如由IMU系统引起的漂移。

因此，由于动作98中的测试，只要在以迭代方式新估计的轨迹Traj_true2D_approx中找到具有较高方差的GPS样本，就去除具有此较高方差的GPS样本。如果如动作98中所检查，没有再找到此些具有较高方差的GPS样本，那么处理器以动作102继续。在动作102中，处理器11将所得的估计轨迹Traj_true2D_approx与交通工具1的随时间而变(且因此随在轨迹上的位置而变)的航向结合起来。

因此，在处理器11已执行图6所示的流程图的动作之后，知道MMS系统在2D平面内所行进的轨迹。即，处理器11已计算出随时间而变的x、y和航向。

请注意，本发明的过程利用了在找到轨迹的最佳估计之前的测量序列。在实时应用中，测量序列仅须考虑过去的测量值。虽然本发明可经合适地修改以应用于实时应用中，但充分益处主要是在迭代允许对过去的点以及参考任何特定点的未来的点进行处理时实现。出于此原因，本发明被视为主要针对离线应用。

备注和替代方案

做出以下观察。

如参看图8a到图8c而阐释的估计由于IMU系统的输出信号中的漂移而导致的估计轨迹中的漂移的方法还可用于直接估计IMU系统的在动作66之后可用的定向输出信号中的漂移。为此，从IMU系统的定向信号减去在已去除所有不准确的GPS样本之后从GPS系统可用的随时间而变的定向信号。这将再现类似于图8c中所示信号的信号，其展示IMU信号中的随时间而变的漂移。

在一些情形下，多路径问题可能涉及大量的GPS样本。随后，在多路径的区域外的GPS样本也可能受多路径影响。因此，处理器11还可去除在多路径区域外的一些GPS测量值，例如在所述区域前和所述区域后的20个GPS样本。

为了适当地检测GPS曲线中的“点”周围的形状差异，“形状滤波器”必须在以下折衷要求之间寻求妥协：

1.滤波器应考虑尽可能多的点(在行进距离中)

2.滤波器还应检测GPS曲线中的短形状异常。

从现有技术已知的多种方法可用于建立此滤波器。

用于方差滤波器和形状滤波器两者的窗样本大小可以是可变的，且借助于根据DMI测量的所行进距离(例如在两个时间方向上不小于100m(或任何其它值))来计算以去除交通工具停止的影响。

如动作84、动作86、动作94和动作100中所指示，处理器11计算MMS系统所行进的轨迹的估计曲线。这可根据从现有技术已知的任何方法来完成。然而，在一实施例中，使用如参看图9a和图9b而阐释的方法。

图9a展示MMS系统所行进的轨迹Tture。然而，提供随时间而变的所计算航向的动作70的输出仅呈有限数目个样本的形式(IMU系统每秒提供(例如)200个样本，且GPS系统每秒提供5个样本)。因此，应从随时间而变的这些定向样本且从作为来自动作80的输出的所计算距离来近似表示MMS系统所行进的真实轨迹。此项技术中已知的计算此近似轨迹的非常常见的方法以箭头Tappr1展示。箭头Tappr1指示基于轨迹上的相继所计算点P之间的线性内插的计算。然而，如从图9a可见，这仅提供一阶近似，其在MMS系统在曲线中驾驶时可能不是非常准确。

计算相继点P之间的近似轨迹的替代方法在图9a中以轨迹Tappr2指示。轨迹Tappr2本身是弯曲的，且以基于回旋曲线部分的所计算轨迹上的相继点之间的内插为基础。图9b展示回旋曲线形状的定义。即，当曲线具有到固定点R0的距离R且在沿曲线朝固定点R0行进距离D时所述距离R以对应于1/R的量减小时，将所述曲线称为回旋形状。因此，在图9b中，距离R1在已行进了距离D之后减小到R2＝R1-a/R，且在已行进了又一距离D之后减小到R3＝R2-a/R2。两个相继点P之间所使用的回旋部分的确切曲率取决于针对点P之前的轨迹计算出的轨迹的曲率的量。

根据图9a和图9b的解决方案是基于以下理解的：当驾驶员转动其方向盘以将交通工具驾驶到曲线中或从一转弯处出来时，进行此做法的最自然的方式是以交通工具遵循回旋形状曲线的方式来进行此做法。而且，道路中的曲线现今在法律上被要求以回旋形状设计，因为此曲线形状允许驾驶员以最高可能速度驾驶通过所述曲线。如果不是这样，那么驾驶员可能仅以有限的速度驾驶通过曲线以避免危险情况。

对计算的输入还是航向和距离。逐个样本进行所述计算。通过使用参看图9a和图9b而阐释的方法，实现了比现有技术方法可能实现的估计更准确的对MMS系统所行进的真实轨迹的估计。

结果是基于多个曲线Tappr2的串接的估计轨迹。

计算围绕x轴和y轴的定向、斜率、局部重力和z水平

上文已阐释了可如何应用“形状滤波器”来从一系列GPS样本(包含由于多路径误差而导致的不准确的GPS样本)去除不准确的GPS样本。而且，通过以迭代方式对近似轨迹与从GPS样本确定的轨迹进行比较，可消除由于IMU系统而导致的漂移。通过这样做，已展示可确定在2D世界中的轨迹的准确近似。然而，真实世界是3D的，且基于(例如)图1所示的MMS系统的测量的大多数应用同样需要3D数据。此3D数据包含沿所行进道路的MMS系统的围绕x轴和y轴的定向以及z水平。然而，另一重要参数可为相对于局部重力向量的斜坡角。请注意，局部重力向量无需与指向地球中心的向量重合。这在山脉情况下尤其如此，其中山脉的质量致使重力向量从指向地球中心的向量局部偏差。差异可高达2％。对于(例如)卡车，地球中心向量如何指向并不如此重要，但局部重力向量如何指向是重要的。仅局部重力向量决定施加于卡车的重力。这在卡车行业中逐渐受到关注。

GPS传感器不提供局部重力向量。GPS传感器仅提供关于指向地球中心的向量的数据。

原则上，IMU传感器可在静止情形下测量局部重力向量。在动态情形下，IMU测量重力和惯性力的叠加。因此在动态情形下，当尝试测量重力向量时，需要从IMU读数中去除所有动态力。

基本上，此处所提出的方法如下；

·IMU系统在动态情形下提供关于重力加上所有移动加速度的测量值，因为MMS系统在收集数据的同时正在行驶；

·然而，DMI系统测量速度，且因此处理器11可导出MMS系统在x方向上的移动加速度(请注意，x方向不是固定的x方向，而是相对于安装在MMS系统中的IMU系统沿道路的移动的方向；类似地，y方向局部界定为垂直于x方向且在道路表面内)；

·因此，处理器11可从IMU系统所获得的测量值中去除从DMI系统导出的移动加速度，从而经由MMS系统的随时间而变的纵摇和横摇而再现重力向量。

现在将以数学方式对此进行阐释。

在建立精确的航向数据且校准DMI系统之后，处理器11可导出影响加速度计读数的非常精确的动态加速度。这些加速度可由以下等式描述：

${\stackrel{\→}{a}}_x=d{\stackrel{\→}{v}}_x/\mathrm{dt}---\left(9\right)$

${\stackrel{\→}{a}}_y={\stackrel{\→}{\mathrm{\ω}}}_z\×{\stackrel{\→}{v}}_x---\left(10\right)$

其中：

等于MMS系统的行进方向上的加速度；

是从DMI系统导出的MMS系统在x方向上的速度；

等于在垂直于MMS系统的行进方向且垂直于局部重力向量的方向上测得的加速度；

是从上文所述的2D方法导出的MMS的航向改变速度；

去除水平加速度，且另外，可从以下等式导出垂直加速度：

${\stackrel{\→}{a}}_z={\stackrel{\→}{\mathrm{\ω}}}_y\×{\stackrel{\→}{v}}_x---\left(11\right)$

其中：

等于与局部重力向量相反的方向上的加速度；

等于MMS系统相对于局部重力向量的纵摇。

这给予处理器11去除动态加速度的主要分量的机会。以此方式产生“类似于静止的”读数：

${\stackrel{\→}{a}}_{\mathrm{STx}}={\stackrel{\→}{a}}_{\mathrm{IMUX}}-{\stackrel{\→}{a}}_x---\left(12\right)$

${\stackrel{\→}{a}}_{\mathrm{STy}}={\stackrel{\→}{a}}_{\mathrm{IMUy}}-{\stackrel{\→}{a}}_y---\left(13\right)$

${\stackrel{\→}{a}}_{\mathrm{STz}}={\stackrel{\→}{a}}_{\mathrm{IMUz}}-{\stackrel{\→}{a}}_z---\left(14\right)$

其中：

等于IMU系统在x轴上测得的加速度

等于x轴上的静止(动态已去除的)加速度

等于IMU系统在y轴上测得的加速度

等于y轴上的静止(动态已去除的)加速度

等于IMU系统在z轴上测得的加速度

等于z轴上的静止(动态已去除的)加速度

这些“类似于静止的”值将是局部重力读数的无偏估计，然而仍具有一些由MMS系统在沿道路行进时的振动引起的白噪声。随着时间的过去，此噪声分量的平均值将等于零。因此，可使用求平均来从信号中消除此白噪声。

现在，可从以下等式获得纵摇和横摇：

$\sin \left({\mathrm{pitch}}_{\mathrm{acc}}\right)=\frac{a_{\mathrm{STx}}}{\sqrt{a_{\mathrm{STx}}^2+a_{\mathrm{STy}}^2+a_{\mathrm{STz}}^2}}---\left(15\right)$

$\sin \left({\mathrm{roll}}_{\mathrm{acc}}\right)=\frac{a_{\mathrm{STy}}}{\sqrt{a_{\mathrm{STx}}^2+a_{\mathrm{STy}}^2+a_{\mathrm{STz}}^2}}---\left(16\right)$

从等式(15)和等式(16)导出的pitch_acc和roll_acc的值是基于加速度测量值的IMU系统的定向的纵摇和横摇的无偏近似值。然而，由于所述值是从具有噪声的数据导出的，所以所述值将也是具有噪声的数据。

为了估计IMU定向的纵摇和横摇的真实值，处理器11可以类似方式使用如上文在2D部分中所描述的方法。上文已阐释了可如何在交通工具航向

的估计中去除漂移。现在，处理器11分别使用pitch_acc和roll_acc作为参考数据，且分别计算IMU参数

和

由的漂移。

图10a展示针对时间窗t0到t3的所得近似纵摇值pitch_acc。在实例中，pitch_acc在时刻t1处上升，且在时刻t2处减小。所展示的信号具有由于在道路上行进期间的MMS系统振动而导致的噪声。

现在，处理器11可导出IMU系统相对于重力向量的纵摇和横摇定向的真实值。在下文的描述中，这些值将分别被称为pitch_true和roll_true。

处理器11通过IMU系统所做出的加速度测量来获得围绕y轴的纵摇值。通过双积分，处理器11从此加速度测量导出所计算的纵摇pitch_IMU。图10b中展示结果。如图所示，计算出的纵摇pitch_IMU不具有噪声，但其具有由双积分引起的漂移。

接着，处理器11从图10b所示的曲线减去图10a所示的曲线。这产生图10c所示的曲线。图10c所示的曲线应展示IMU系统所测得的纵摇中的漂移具有添加到其的一些噪声。因此，(例如)通过使用求平均技术或局部线性回归，处理器11从图10c所示的信号中去除噪声，从而再现从IMU测量得出的纵摇中的漂移。现在，处理器11可补偿来自从IMU系统的测量得出的纵摇的漂移。这提供准确的纵摇pitch_true。

现在，处理器11也可以如相对于纵摇pitch_true(对照图10a到图10c)而阐释的方式相同的方式来计算准确的横摇roll_true。

计算斜率

原则上，静止IMU传感器可测量局部重力向量，也就是说，一旦知道相对于局部重力向量的纵摇和横摇，就知道局部重力向量。在上文的部分中呈现了关于如何在动态情形下测量局部重力向量的方法。为了进一步从这些读数计算斜率，需要考虑交通工具的额外动态特性。举例来说，如果小汽车具有相对较短的长度，或IMU系统没有精确地安装到交通工具主体，那么测得的局部重力向量可能是准确的，但斜坡的角度可能仍非常不准确。这当然可通过用非常长的小汽车(或卡车)进行测量，且将IMU系统精确地安装到交通工具来避免，但那是不切实际的。而且，这可通过在小汽车正在行驶时用IMU传感器进行测量来避免，但测量可能遭受IMU输出中的漂移的过多影响。

图11展示用于计算相对于重力的斜率的一些相关参数。假定MMS系统处于斜坡上的点P处。MMS系统以速度

行驶，所述速度由DMI系统测得(请记住，x方向是相对于由MMS本身界定的移动坐标系的，因此x方向是沿着斜坡本身的)。出于阐释的目的，以从指向地球中心的方向的相对较大的偏差来展示局部重力向量

在大多数情况下，此偏差将不大于2％。行驶速度

可被分解为在局部重力向量

的方向上的“垂直”速度

和垂直于此垂直速度

的速度(此后一个垂直速度在图11中未图示)。因此，以下等式成立：

slope_angle＝arcsin(v_z/v_x)(17)

垂直速度v_z也可从IMU静止系统所做出的测量中导出，所述IMU静止系统测量局部重力向量的方向上的垂直静止加速度

以下等式成立：

${\stackrel{\→}{v}}_z={\∫\stackrel{\→}{a}}_{\mathrm{STz}}\·\mathrm{dt}---\left(18\right)$

此等式的结果展示某一漂移，但可良好地在小距离上使用。可将从等式(18)导出的垂直速度v_z的值代入等式(17)中。而且，可将从DMI系统已知的行驶速度

代入等式(17)中。

换句话说，通过从DMI测量值获得行驶速度

从关于垂直加速度

的IMU静止测量值计算垂直速度v_z，且应用等式(17)，处理器11计算出相对于局部重力向量

的斜坡角。

在第一替代方案中，处理器11使用以下等式来计算斜坡角：

v_z＝v_x·sin(pitch_true-k·a_x)(19)

其中：pitch_true＝纵摇，其可从上文所阐释的计算中获得；

a_x＝x方向上的加速度，其可从DMI系统获得；

k＝常数。k的值可通过测试找到，且取决于MMS系统的质量弹簧特性。

观察到，等式(19)与从现有技术中已知的一样。然而，新的内容是理解此等式确定相对于局部重力向量

的斜坡角，且处理器11因此可容易计算此斜率。

在第二替代方案中，处理器11使用等式(17)和等式(19)两者来计算斜率，且不使用IMU测量来获得垂直速度v_z。使用等式(19)来从通过等式(18)获得的值中去除漂移(或其它误差)，且再现经误差补偿的垂直速度值v_z，err-comp。因此，可避免从IMU系统确定垂直速度v_z中的漂移。因此，此方法向处理器11提供了计算相对于局部重力向量

的垂直速度v_z的另一方式，即，以v_z，err-comp的形式。一旦以此方式计算出经误差补偿的垂直速度值v_z，err-comp，处理器11就使用等式(17)来计算斜坡角，其中在等式中使用v_z，err-comp代替v_z。

尽管此处将斜率计算呈现为基于等式(17)，但一般来说，相对于局部重力向量的斜率的计算可基于其中使用DMI系统和IMU系统的测量值且其中认识到IMU系统的测量是相对于局部重力向量而做出的任何等效等式。

而且，观察到，将计算相对于局部重力向量的斜率视为单独的发明，其不同于2D平面内的任何发明，例如去除不准确的GPS样本。一旦具有一组相当准确的GPS样本，就可以上文所描述的方式来计算斜率。这组GPS样本无需以本文献中所阐释的方式获得。

计算z水平

现在，处理器11可使用以下等式来容易地计算MMS系统的随时间而变的相对z水平：

z(t)＝∫v_x(t)·sin(slope_angle(t))(20)

其中：z(t)是在局部重力向量

的方向上的相对z水平。

此相对z水平可由处理器11用来计算绝对z水平，即在与地球的中心方向相反的z方向上测得的高于海平面的z水平。为此，处理器11可经布置以导出移位向量，其用于使每一相对z水平值移位到绝对海平面值。此移位向量可(例如)由处理器11通过以下方式获得：对上文所阐释的2D方法已识别为良好的所有GPS z_GPS(t)读数进行求平均，从而得到平均值，且从此平均值减去对应相对z(t)值的平均值。

Claims (31)

1.一种计算机布置，其用于确定一组全球定位系统样本中的不准确的全球定位系统样本，所述计算机布置包括处理器(11)，所述处理器(11)经布置以与存储器(12；13；14；15)通信，所述存储器(12；13；14；15)存储可由所述处理器(11)运行的包括指令和数据的计算机程序，所述处理器经布置以在其运行所述计算机程序时执行以下动作：

a)获得如由基于陆地的交通工具载有的全球定位系统在沿轨迹行进时所取得的全球定位系统样本；

b)基于所述全球定位系统样本而获得所述轨迹的第一估计；

c)至少基于由所述基于陆地的交通工具载有的惯性测量单元在沿所述轨迹行进时做出的测量而获得所述轨迹的第二估计；

d)将所述第一估计与第二估计进行比较；

e)建立其中所述第一估计与所述第二估计相比展现高于预定阈值的变化的位置；

f)如果不能建立此些位置，那么以动作j)继续，否则以动作g)继续；

g)将与具有较高变化的所述位置相关联的全球定位系统样本作为不准确的全球定位系统样本而去除，从而形成一组剩余的全球定位系统样本；

h)基于所述剩余的全球定位系统样本再计算所述轨迹的所述第一估计，且再计算所述第二估计；

i)重复动作d)到h)；

j)结束所述动作。

2.根据权利要求1所述的计算机布置，其中所述计算机布置经布置以在动作j)中在结束所述动作之前测试是否满足预定标准，且如果未满足所述预定标准，那么重复动作d)到h)，所述预定标准包含以下各项中的至少一者：所述交通工具所行进的最小距离和所述交通工具的行进的最小时间周期。

3.根据权利要求2所述的计算机布置，其中处理器(11)经布置以基于所述交通工具载有的距离测量仪器做出的距离测量来检查所述交通工具所行进的最小距离的所述预定标准。

4.根据权利要求2或3所述的计算机布置，其中所述最小距离是100m。

5.根据前述权利要求中任一权利要求所述的计算机布置，其中所述轨迹的所述第二估计还基于所述基于陆地的交通工具载有的距离测量仪器在沿所述轨迹行进时做出的测量。

6.根据前述权利要求中任一权利要求所述的计算机布置，其中所述轨迹的所述第二估计还基于所述基于陆地的交通工具载有的所述全球定位系统在沿所述轨迹行进时测得的关于所述交通工具的行进方向的测量值。

7.根据前述权利要求中任一权利要求所述的计算机布置，其中所述轨迹的所述第二估计还基于所述基于陆地的交通工具载有的所述全球定位系统在沿所述轨迹行进时测得的关于所述交通工具的行进速度的测量值。

8.根据前述权利要求中任一权利要求所述的计算机布置，其中所述处理器(11)经布置以在其运行所述计算机程序时在其执行动作d)之前的任何时间执行以下动作：获得与所述交通工具所行进的所述轨迹的一部分相关联的随时间而变的第一参数，所述第一参数是包括以下各项的参数群组中的一者：如在xyz系统中界定的x值、y值、z值、在xy平面内的定向、横摇和纵摇，所述第一参数是从所述交通工具载有的第一测量系统在沿所述轨迹的所述部分行进时获得的；

获得与所述交通工具所行进的所述轨迹的所述部分相关联的随时间而变的第二参数，所述第二参数是所述第一参数的等效物，但是从所述惯性测量单元做出的所述测量获得的；

将随时间而变的所述第一参数与第二参数进行比较，且从所述比较推断出所述第二参数中的在所述轨迹的所述部分中的测量误差；

补偿由于所述第二参数中的所述测量误差而导致的所述轨迹的所述第二估计中的误差。

9.根据权利要求8所述的计算机布置，其中所述第一测量系统是所述全球定位系统。

10.根据权利要求8或9所述的计算机布置，其中所述测量误差涉及所述惯性测量单元所做出的所述测量中的漂移和移位中的至少一者。

11.根据前述权利要求中任一权利要求所述的计算机布置，其中所述处理器(11)经布置以在获得所述轨迹的所述第二估计的所述动作中，计算所述轨迹的相继位置，且在使用所述位置之间的回旋形近似时在所述位置之间进行内插。

12.根据前述权利要求中任一权利要求所述的计算机布置，其中所述处理器(11)经布置以在执行以下动作的同时，计算所述交通工具沿所述轨迹行进时在xy平面内的随时间而变的定向，所述xy平面由x轴和y轴局部界定，所述x轴沿所述轨迹，且所述y轴垂直于所述x轴且垂直于局部重力向量：

基于所述剩余的全球定位系统样本而估计随时间而变的第一定向信号；

基于所述惯性测量单元所做出的定向测量而估计随时间而变的第二定向信号；

将随时间而变的所述第一定向信号与所述第二定向信号进行比较；

基于所述比较动作而导出随时间而变的所述第二定向信号中的误差；

补偿所述第二定向信号中的所述误差，从而再现经误差补偿的第二定向信号；

基于所述第一定向信号和所述经误差补偿的第二定向信号而计算所述交通工具沿所述行进轨迹的随时间而变的所述定向。

13.根据权利要求12到14中任所述的计算机布置，其中所述处理器(11)经布置以根据以下动作来计算所述交通工具沿所述轨迹行进时随时间而变的纵摇和横摇中的至少一者的第一估计：

从所述惯性测量单元获得关于移动加速度的测量数据；

从所述交通工具载有的距离测量单元获得所述x方向上的速度测量数据；

导出所述交通工具在所述x方向上的移动加速度；

从所述IMU系统获得的测量值中去除所述移动加速度，从而再现所述加速度。

14.根据权利要求13所述的计算机布置，其中所述处理器(11)经布置以在执行以下动作的同时，计算所述交通工具沿所述轨迹行进时随时间而变的纵摇和横摇中的至少一者的近似值：

计算：

${\stackrel{\→}{a}}_x=d{\stackrel{\→}{v}}_x/\mathrm{dt}---\left(9\right)$

${\stackrel{\→}{a}}_y={\stackrel{\→}{\mathrm{\ω}}}_z\×{\stackrel{\→}{v}}_x---\left(10\right)$

其中：

计算：

${\stackrel{\→}{a}}_z={\stackrel{\→}{\mathrm{\ω}}}_y\×{\stackrel{\→}{v}}_x---\left(11\right)$

其中：

等于在与所述局部重力向量相反的方向上的加速度；

等于所述交通工具相对于所述局部重力向量的纵摇；

计算：

${\stackrel{\→}{a}}_{\mathrm{STx}}={\stackrel{\→}{a}}_{\mathrm{IMUx}}-{\stackrel{\→}{a}}_x---\left(12\right)$

${\stackrel{\→}{a}}_{\mathrm{STy}}={\stackrel{\→}{a}}_{\mathrm{IMUy}}-{\stackrel{\→}{a}}_y---\left(13\right)$

${\stackrel{\→}{a}}_{\mathrm{STz}}={\stackrel{\→}{a}}_{\mathrm{IMUz}}-{\stackrel{\→}{a}}_z---\left(14\right)$

其中：

计算以下各项中的至少一者：

$\sin \left({\mathrm{pitch}}_{\mathrm{app}}\right)=\frac{a_{\mathrm{STx}}}{\sqrt{a_{\mathrm{STx}}^2+a_{\mathrm{STy}}^2+a_{\mathrm{STz}}^2}}---\left(15\right)$

$\sin \left({\mathrm{roll}}_{\mathrm{app}}\right)=\frac{a_{\mathrm{STy}}}{\sqrt{a_{\mathrm{STx}}^2+a_{\mathrm{STy}}^2+a_{\mathrm{STz}}^2}}---\left(16\right)$

其中：

pitch_app＝纵摇的近似值

roll_app＝横摇的近似值。

15.根据权利要求14所述的计算机布置，其中所述处理器(11)经布置以从所述惯性测量单元做出的所述测量中计算纵摇和横摇中的所述至少一者的估计，将纵摇和横摇中的所述至少一者的所述近似值与所述纵摇和横摇中的所述至少一者的所述估计进行比较，且基于所述比较而补偿纵摇和横摇中的所述至少一者的所述估计中的误差，以便再现被界定为经误差补偿的纵摇的真实纵摇以及被界定为经误差补偿的横摇的真实横摇中的至少一者。

16. 根据权利要求14或15所述的计算机布置，其中所述处理器(11)经布置以根据以下等式来计算所述交通工具沿所述轨迹行进时相对于局部重力向量

的斜坡角slope_angle：

slope_angle＝arcsin(v_z/v_x)    (17)

其中：

v_z＝与所述局部重力向量相反的方向上的速度，其是从以下等式计算：

${\stackrel{\→}{v}}_z={\∫\stackrel{\→}{a}}_{\mathrm{STz}}\·\mathrm{dt}---\left(18\right).$

17.根据权利要求15所述的计算机布置，其中所述处理器(11)经布置以根据以下等式来计算所述交通工具沿所述轨迹行进时相对于局部重力向量

的斜坡角

slope_angle：

slope_angle＝arcsin(v_z/v_x)    (17)

v_z＝v_x·sin(pitch_true-k·a_x)    (19)

其中：

v_x＝与所述局部重力向量相反的方向上的速度；

pitch_ture＝真实纵摇；

a_x＝从所述距离测量单元导出的x方向上的加速度；

k＝常数。

18.根据权利要求16所述的计算机布置，其中所述处理器(11)经布置以：

计算：

v_z＝v_x·sin(pitch_true-k·a_x)    (19)

其中：

pitch_true＝真实纵摇；

a_x＝从所述距离测量单元导出的x方向上的加速度；

k＝常数；

使用等式(19)从通过等式(18)获得的v_z中去除误差，从而再现经误差补偿

的垂直速度值v_{z，err_comp}；

用所述经误差补偿的垂直速度v_{z，err_comp}代替等式(17)中的v_z。

19.根据权利要求16或17所述的计算机布置，其中所述处理器(11)经布置以从以下等式计算与所述局部重力向量

相反的方向上的z水平z(t)：

z(t)＝∫v_x(t).sin(slope_angle(t))    (20)。

20.一种确定一组全球定位系统样本中的不准确的全球定位系统样本的方法，所述方法包括以下动作：

a)获得如由交通工具载有的全球定位系统在沿轨迹行进时所取得的全球定位系统样本；

b)基于所述全球定位系统样本而获得所述轨迹的第一估计；

c)至少基于由所述交通工具载有的惯性测量单元在沿所述轨迹行进时做出的测量而获得所述轨迹的第二估计；

d)将所述第一估计与第二估计进行比较；

e)建立其中所述第一估计与所述第二估计相比展现高于预定阈值的变化的位置；

f)如果不能建立此些位置，那么以动作j)继续，否则以动作g)继续；

g)将与具有较高变化的所述位置相关联的全球定位系统样本作为不准确的全球定位系统样本而去除，从而形成一组剩余的全球定位系统样本；

h)基于所述剩余的全球定位系统样本再计算所述轨迹的所述第一估计，且再计算所述第二估计；

i)重复动作d)到h)；

j)结束所述动作。

21.一种计算机程序产品，其包括可由计算机布置加载以允许所述计算机布置执行根据权利要求20所述的方法的指令和数据。

22.一种数据载体，其包括根据权利要求21所述的计算机程序产品。

23.一种计算机布置，其用于计算交通工具沿轨迹行进时相对于局部重力向量

的斜坡角slope_angle，所述计算机布置包括处理器(11)，所述处理器(11)经布置以与存储器(12；13；14；15)通信，所述存储器(12；13；14；15)存储可由所述处理器(11)运行的包括指令和数据的计算机程序，所述交通工具包括距离测量单元(DMI)和惯性测量单元(IMU)，xy平面由x轴和y轴局部界定，所述x轴沿所述轨迹，且所述y轴垂直于所述x轴且垂直于所述局部重力向量，所述处理器(11)经布置以从所述距离测量单元(DMI)和所述惯性测量单元(IMU)两者做出的测量计算所述斜坡角。

24.根据权利要求23所述的计算机布置，其中所述处理器经布置以根据以下等式来计算所述斜坡角slope_angle：

slope_angle＝arcsin(v_z/v_x)    (17)

其中：

v_z＝与所述局部重力向量相反的方向上的速度；

25.根据权利要求24所述的计算机布置，其中所述处理器经布置以从以下等式计算v_z：

${\stackrel{\→}{v}}_z={\∫\stackrel{\→}{a}}_{\mathrm{STz}}\·\mathrm{dt}---\left(18\right)$

其中： ${\stackrel{\→}{a}}_{\mathrm{STz}}={\stackrel{\→}{a}}_{\mathrm{IMUz}}-{\stackrel{\→}{a}}_z---\left(14\right)$

其中：

${\stackrel{\→}{a}}_z={\stackrel{\→}{\mathrm{\ω}}}_y\×{\stackrel{\→}{v}}_x---\left(11\right)$

其中：

等于所述交通工具相对于所述局部重力向量的纵摇。

26.根据权利要求24所述的计算机布置，其中所述处理器经布置以从以下等式计算v_z：

v_z＝v_x·sin(pitch_true-k·a_x)    (19)

其中：

pitch_true＝所述交通工具相对于所述局部重力向量的真实纵摇；

a_x＝从所述距离测量单元导出的x方向上的加速度；

k＝常数。

27.根据权利要求25所述的计算机布置，其中所述处理器(11)经布置以：

计算：

v_z＝v_x·sin(pitch_true-k·a_x)    (19)

其中：

pitch_true＝所述交通工具相对于所述局部重力向量的真实纵摇；

a_x＝从所述距离测量单元导出的x方向上的加速度；

k＝常数；

使用等式(19)从通过等式(18)获得的v_z中去除误差，从而再现经误差补偿的垂直速度值v_{z，err_comp}；

用所述经误差补偿的垂直速度v_{z，err_comp}代替等式(17)中的v_z。

28.根据权利要求24到27中任一所述的计算机布置，其中所述处理器(11)经布置以从以下等式计算与所述局部重力向量

相反的方向上的z水平z(t)：

z(t)＝∫v_x(t)·sin(slope_angle(t))    (20)。

29.一种计算交通工具沿轨迹行进时相对于局部重力向量

的斜坡角slope_angle的方法，所述交通工具包括距离测量单元(DMI)和惯性测量单元(IMU)，xy平面由x轴和y轴局部界定，所述x轴沿所述轨迹，且所述y轴垂直于所述x轴且垂直于所述局部重力向量，所述方法包括从所述距离测量单元(DMI)和所述惯性测量单元(IMU)两者做出的测量计算所述斜坡角。

30.一种计算机程序产品，其包括可由计算机布置加载以允许所述计算机布置执行根据权利要求29所述的方法的指令和数据。

31.一种数据载体，其包括根据权利要求30所述的计算机程序产品。

Images