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

专利名称：用于二维和三维精确位置和定向确定的布置和方法
技术领域：
本发明涉及二维和三维精确位置和定向确定的布置和方法。
背景技术：
位置确定装置是现有技术中已知的。现今位置确定装置越来越多地应用于(例如)交通工具，例如车辆、轮船和飞机。为此目的，此交通工具可包括不同的测量单元，如全球定位系统(GPS)、惯性测量单元(IMU)和距离测量仪器(DMI)。在行进时，此些测量单元的输出数据由处理器用来计算交通工具的位置和/或定向。取决于应用，在线或离线使用这些测量单元做出的测量。一般需要从3个测量单元GPS、IMU和DMI提供尽可能准确的位置和定向测量。 为此目的应解决许多问题，例如多路径问题、测量信号中的噪声以及IMU的输出信号中的漂移(或其它移位)。在当前技术水平下，卡尔曼(Kalman)滤波器广泛用于补偿来自IMU 的输出信号中的漂移以及补偿其它效应。而且，其它统计方法，如移动平均技术和高斯 (Gaussian)白噪声滤波，可用于从信号去除许多噪声以使信号变得纯净。然而，有效地去除高斯噪声的一些滤波技术和平均技术对经偏移信号将低效地起作用，所述信号展示对测量系列(如在GPS测量中)的较长时间常数移位。第5，311，195号和第5，948，043号美国专利揭示具有其它传感器和用于识别不应考虑的不准确GPS测量的非卡尔曼构件的GPS系统。

发明内容
本发明的目标是提供一种位置和定向确定系统以及位置和定向确定方法，以改进基于在沿着轨迹移动的交通工具上执行的测量的位置和定向计算中的至少一者。为此，本发明提供一种如独立设备技术方案中所主张的计算机布置。而且，本发明涉及一种如独立方法技术方案中所主张的方法。本发明提供非常准确且可靠的方法来从在交通工具沿着其轨迹移动时收集到的一组GPS样本中去除不准确的GPS样本。通过使用所主张的设备或方法，不准确的GPS样本以非线性方式被消除。在一实施例中，关于位置和定向的计算可离线进行，S卩，在已收集到来自轨迹的所有测量数据之后进行。这(例如)在所谓的移动测绘系统(MMS系统)中是真实的，其中通过沿着道路网行进的交通工具来收集位置和定向数据，所述位置和定向数据稍后用于产生 2D和/或3D道路地图，或捕捉可在一区域(如城市中的道路)的3D显示中使用的地理数
4据，所述3D显示还展示沿着所述道路的建筑物的立面。可应用本发明的其它领域是在道路资产细目数据库创建中，其中MMS系统可提供重要的支持。同样可使用其它传感器，如用于路面管理的作为地理雷达的传感器、基于激光的粗糙度系数测量单元、广泛用于裂纹检测应用的频闪观测器增强高速垂直相机，以及用于对象确定、3D视图应用等的激光扫描仪。在此实施例中测得的位置和定向数据无需实时使用而是仅在后来使用的事实向离线处理器提供了执行不同于在实时情况下是可能的校正机制的校正机制的能力。本发明的离线方法提供检查如在沿着道路网行进期间收集到的所有GPS样本的甚至更准确的选择，从而识别不准确的GPS样本，且不再考虑这些不准确的GPS样本来计算所行进的轨迹。 如以此方式计算出的轨迹具有较高的准确度，因为其不再受经偏移GPS信号影响。因此，本发明提供准确的结果，其可非常成本有效地实施且可在离线环境中使用， 在所述离线环境中，位置/定向测量是从用例如已以平稳方式(因此，没有例如由比赛车辆做出的突然移动)沿着道路行驶的交通工具收集到的数据计算的。在离线环境中，本发明可用于产生轨迹的更敏感确定，且因此最终产生略微更精确的最终结果。这是因为在离线实施例中，可应用形状滤波器，所述形状滤波器为全局的、 回归的且自适应的，且可将GPS样本分类为准确的或不准确的。在一实施例中，本发明提供对用于计算交通工具的随着时间而变的位置和定向的惯性测量单元(IMU)的输出信号中的漂移(或其它移位)进行计算的选择。这是通过使用 GPS样本来完成的，且随后校正IMU信号的以此方式确定的漂移。随后将经漂移校正的IMU 信号用作用于计算位置和定向的主要基础，而GPS样本主要用于漂移校正。用于此漂移补偿的GPS样本的数目可高达25%。


将参看一些图式来详细阐释本发明，所述图式既定用于说明本发明而不是限制其范围，本发明的范围由附加的权利要求书及其等效实施例界定。在图式中图1展示交通工具中的现有技术位置和定向测量系统；
图加展示局部交通工具坐标系；图2b展示所谓的Wgs坐标系；图3展示计算机布置的示意性实例；图4是用于阐释多路径问题的示意图；图5展示GPS系统和惯性测量单元(IMU)的输出信号的图；图6展示用于阐释本发明的流程图；图7a到图7d展示用于从一组GPS样本中去除不准确的GPS样本的方法中的相继动作；图8a、图8b和图8c展示用于阐明可如何估计IMU信号中的漂移(和其它移位) 的曲线；图9a和图9b展示可如何通过回旋曲线来近似表示道路交通工具所行进的轨迹；图10a、图IOb和图IOc展示用于阐明可如何估计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可以IOHz或IOHz以上的取样频率操作。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的系统的另一应用可在交通工具导航系统的领域中。现今，在交通工具导航系统中，显示信号通常涉及二维OD)地图上的位置。图加展示从图1所示的三个测量单元GPS、DMI和IMU可获得哪些位置信号。图加展示微处理器μ P经布置以计算6个不同的参数相对于原点的3个距离参数x、y、z以及3个参数ωχ、ωζ。请注意，坐标系的原点不是固定的，而是相对于交通工具(例如，相对于IMU系统)而界定的。3个参数ωχ、Coy和ωζ分别表示分别围绕χ轴、y轴和ζ 轴的角度改变速度(或旋转速度)。ζ方向与局部重力向量的方向重合。在许多情形中，轨迹相对于重力向量将具有斜率。因此，χ方向不垂直于局部重力向量。此坐标系和测量还可结合离线使用的计算系统而使用。请注意，交通工具围绕1轴的定向通常被称为纵摇，而围绕X轴的定向通常被称为横摇，且航向是围绕ζ轴的定向。因此，以下等式成立纵摇=/ ωχ . dt横摇=/Oy .dt航向=/ωζ · dt图2b中展示所使用的坐标系。下文中将阐释处理器11可如何在地理坐标中确定交通工具1的纬度、经度和海拔以及自由角度。地理坐标系(GEO)被界定为使得其X轴在地球的赤道平面内，但随着地球的旋转而固定，使得其穿过格林威治子午线(0°经线)。所述坐标系的Z轴平行于地球的旋转轴，且所述坐标系的Y轴完成右旋正交集(Y = ZXX)。为了这样做，将阐释如何确定交通工具的位置和定向，其中所述交通工具的测量仪器GPS、IMU 和DMI在如由局部重力向量界定的局部移动坐标系中。为此，将交通工具1布置在具有某一纬度/经度的点处，其中其χ轴平行于交通工具1的航向，且ζ轴平行于在地球上所述点中的局部重力向量。—般来说，本申请案可应用于如图1所示的交通工具。然而，本申请案可同样应用于任何其它具备车轮以沿着地球边界轨迹移动的交通工具，例如公共汽车、卡车、火车或有轨电车等。而且，如下文所阐释的方法的实施例(针对从存储受非高斯噪声影响的GPS样本的数据库去除不准确的GPS样本(其可能由于多路径或其它原因而从真实位置偏移，即， 非经模型化的对流层传播延迟))同样适用于基于水的系统(例如船)和航空系统(直升机和飞机)，只要所述系统配备有GPS和惯性定位系统。本发明可应用的领域之一涉及产生城市街道中的建筑物的3D图像。为了产生此些图像，可使用移动测绘系统(MMS)，其由驾驶员驾驶穿过所关注的街道。图1展示此匪S 系统，如上文已阐释。微处理器μ 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或CDR0M21等数据载体读取数据且可能将数据写入在所述数据载体上。其它数据载体可为磁带、DVD、⑶-R、DVD-R、记忆棒等，如所属领域的技术人员已知。处理器11还连接到用于将输出数据印刷在纸上的打印机23，以及连接到显示器 18，例如监视器或液晶显示器(LCD)屏幕或所属领域的技术人员已知的任何其它类型的显示器。处理器11可连接到扬声器四。处理器11可借助于I/O构件25连接到通信网络27，例如公共交换电话网络 (PSTN)、局域网(LAN)、广域网(WAN)、因特网等。处理器11可经布置以通过网络27与其它
通信布置通信。数据载体20、21可包括呈数据和指令形式的计算机程序产品，所述数据和指令经布置以向处理器提供执行根据本发明的方法的能力。然而，此计算机程序产品可替代地经由电信网络27而下载。处理器11可实施为独立的系统；或实施为多个并行操作的处理器，其每一者经布置以实行较大计算机程序的子任务；或实施为具有若干子处理器的一个或一个以上主处理器。本发明的功能性的部分可甚至由通过网络27与处理器11通信的远程处理器来实行。观察到，图3所示的布置还可用于交通工具1中的微处理器，尽管在交通工具1中并不需要应用如图示的所有组件，如扬声器四和打印机23。在一实施例中，处理器11经布置以接收如由相机9拍摄的图片，且将所述图片存储在其存储器中的一者(例如，硬盘12)中。硬盘12还存储从其拍摄立面图片的建筑块的所谓的“占地面积”。这些占地面积包括关于建筑块在地球上的位置的2D数据。存储器12 到15中的一者存储一程序，所述程序可由处理器11运行且指令处理器将立面图片与占地面积组合，使得正确的立面图片与正确的建筑块相关联。如此获得的数据被存储以供稍后用于(例如)交通工具导航系统中，以向驾驶员展示他/她正驾驶在的街道的3D视图。为此，此数据可存储在DVD上。本发明并不是针对可由处理器11运行以使立面图片与占地面积相关联的计算机程序。任何现有技术(或仍待开发的)的计算机程序均可在本发明的情形中用来进行此操作，或其可在所属领域的技术人员的辅助下完成。本发明涉及与(例如)立面图片相关联的位置和定向数据。更一般地说，本发明涉及由某一(某些)测量单元收集到的数据，其中所述数据与在地球上的位置和定向相关联，且此位置和定向是在测量其它数据的同时测量到的。测得的数据可(例如)涉及完全不同于立面图片的某物，如用于数字地图的道路侧标志或用于搜索或探测土壤中的自然能源(石油、天然气等)的土壤条件，或手机或其它RF信号强度测量。因此，本发明的应用不限于交通工具1上用于收集在地球上某一位置处相关的数据的传感器类型。然而，为了简单起见，下文在阐释本发明时有时将使用收集立面图片的实例。如所属领域的技术人员将明了，涉及每一立面图片的3D位置数据必须由匪S系统尽可能准确地测量，以允许处理器11将立面图片数据正确地对准到其它可用数据，例如建筑物占地面积数据。立面图片的这些位置数据直接与在相机9拍摄每一图片的时刻MMS 系统的位置和定向数据有关。因此，应尽可能准确地知晓在拍摄立面图片时MMS系统的位置和定向。在拍摄这些图片时，匪S系统应(例如)具有关于其在χ、y和ζ上的位置的1 到1. 5米或更好的准确度，以及关于其角度定向的航向、纵摇和横摇的0. Γ或更好的准确
8度。如所属领域的技术人员已知，现有技术系统使用图1所示的系统的GPS部分来导出位置数据X、y且可能还有Z。然而，所属领域的技术人员还知道，GPS系统数据的准确度因为如多路径误差等误差而降级。图4以示意方式展示多路径问题。图4展示匪S在两个建筑块BBl与BB2之间的街道上驾驶。卫星(未图示)向地球传输位置信号。如果MMS系统与卫星之间的直接且不中断的传输路径将可用，那么MMS 系统将经由沿MMS系统与卫星之间的直线的信号路径SPl接收位置信号。然而，建筑块BBl 阻挡了传输路径SP1。然而如图所示，MMS系统确实从卫星接收到如由另一建筑块BB2反射的位置信号。然而，所述位置信号已沿另一路径SP2行进，从而导致如取决于所关注卫星的不准确的位置测量。在真实情形中，来自卫星的位置信号将常由匪S系统经由多个路径接收。在许多情况下，存在直接路径SPl和经由对着建筑块和树木等的反射的多个路径。一个此反射源 (例如)由从MMS系统旁边经过的大卡车形成。例如以卡尔曼滤波器、合适的统计计算、随着时间的过去的求平均等的形式提供现有技术解决方案以解决多路径问题。其它多路径误差源可涉及雷暴云或其它电离层反射。这里提到的多路径误差的实例无意是详尽的。然而， 尤其在基于陆地的应用中，在公共道路卡车、周围建筑物、标志以及移动的交通工具上产生复杂且快速改变的局部配置。在此情形中，基于较长时间观察的多路径确定的标准方法很可能失败。现在将阐释如由两个其它系统IMU和DMI (图1)中的至少一者收集到的位置和定向测量数据可如何用于判定哪些GPS测量值很可能是准确的，且哪些GPS测量值很可能不准确(相对于所需的准确度)且不应在任何稍后的位置计算(例如由处理器11(图3)执行)中被考虑。在下文中，将首先参考2D情形(即，不考虑高度ζ且不考虑纵摇和横摇，而仅考虑χ、y和航向的情形)来阐释完成此判定的方式。只有在其之后才将给出3D实例。计算2D位置和定向图5以示意方式展示四个相继区域A1、A2、A3、A4，其中MMS系统已沿着道路行进。 图5是道路表面的示意性俯视图(未按比例绘制)，其指示交通工具1将如何根据不同的测量源而行进。图5展示匪S系统已在如用箭头AR指示的方向上行进。图5中的虚线指示 MMS系统所行进的真实轨迹。在所有四个区域A1、A2、A3、A4中，MMS系统已收集GPS样本以便在连续立面图片的拍摄期间测量MMS系统的2D位置和定向。用G(i)(i = l、2、3、…、I)来指示所行进的根据GPS样本的轨迹。每秒可存在5个或10个或甚至更多的GPS样本，然而本发明不限于此数字。图5以非常示意性的方式展示GPS样本。即，将GPS样本展示为连续的步骤。实际上，每一步骤是一个实例，其中相继样本之间存在取样时间。而且，IMU系统已测量MMS系统所行进的轨迹。IMU系统所计算的轨迹以实线指示。 回想IMU是相对位置和定向测量装置，且需要一绝对参考来使其相对于轨迹而放置。在此实例中，在轨迹的开始(左侧)处，将纯IMU位置任意地放置在真实轨迹附近。指示此实线在MMS系统随着时间的过去的移动期间越来越偏离真实轨迹。这是由于IMU系统所做出的计算中的漂移或其它未校准的移位而导致的。此移位/漂移是由IMU仅提供相对测量而不提供绝对测量的事实引起的。由漂移引起的误差可阐释如下。如上文所指示，IMU系统提供可从中推断以下参数的数据 围绕 χ 轴、y 轴和 ζ 轴的旋转速度，即 dcox/dt、dcoy/dt、dcoz/dt (图 2a)；· ax、ay和az，即分别在x、y和ζ方向上的加速度。为了计算在2D空间中的位置和定向，使用在χ和y方向上的加速度 和 以及围绕ζ轴的旋转速度，即ωζ。从这些加速度，可如下计算位置χ和y以及航向χ = / / ax · dt2y = / / ay · dt2航向=/dcoz/dt通常，从这些加速度计算χ、y位置和航向随着时间的过去是不够准确的，这是由于输出信号中表示误差的漂移所导致的，所述误差随后根据双积分而累积。每行进Ikm此漂移通常可大约为lm。在下文中，将使用术语“漂移”来指代如以这些等式阐释的所述种类的漂移以及由于IMU系统的输出信号中的累积误差而导致的其它种类的移位。最后，图5中的点划曲线展示基于GPS真实航向测量样本G(i)和时间上接近的 GPS探测器之间的局部距离以及IMU测量和DMI测量而估计的MMS系统所行进的轨迹。如从图5可见，在区域A4中，GPS系统展示估计轨迹与GPS输出之间的逐渐增加的偏差。这可能是由如所属领域的技术人员将显而易见的所有种类的误差引起的。而且，图5展示GPS测量值在区域Al与A2之间以及A2与A3之间的过渡处展示其输出中的突然移位(请注意，图5仅展示一些GPS样本。实际上，区域(如区域A2)中的 GPS样本的数目可能大得多)。此些突然移位可为多路径误差的指示。然而，不能简单地宣称区域A2和A4中的GPS测量值由于多路径误差而不准确。同样可能的情况是区域Al和 A3中的GPS样本由于多路径误差而不准确。另外，不能以推理方式依赖于在这些区域中如由IMU系统测量到的χ和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的χ、y位置，如动作88中所指示。从这些x、y位置可推导出交通工具1的速度，如动作78中所指示。如从现有技术已知，所有这些测量数据在统计上不相关。在动作64中，处理器11获得如由IMU系统沿匪S系统所行进的轨迹测得的旋转速度ω z，且提供如通过上文所阐释的积分计算出的交通工具1的航向。在动作72中，处理器11获得如由DMI系统沿匪S系统所行进的轨迹测得的匪S系统的车轮角度随时间而变的值。为了计算在2D平面内的位置和定向，将在大多数情况下具有大于IOOOkg的质量的交通工具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. Im的范围内，且可被视为针对轨迹确定的白噪声。由于交通工具主体可被视为固定框架，因此交通工具主体的所有点共享与交通工具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测量的匪S系统所行进的随时间而变距离的估计。所述距离在这里被称为“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系统测量中的噪声。等式O)中的INS_Trajectory2D对应于图5中的“估计轨迹”。在较小的距离上，以下等式有效INS_Trajectory2D = Traj_true2D+IMU_drift2D+IMU_noise ( /Jn ) (3)Traj_true-2D = GPS_meas2D-GPS_noise-GPS_multipath (4)其中· GPS_meas2D =如从GPS系统导出的测得位置信号· GPS_noise =来自GPS系统的GPSjiieas中如在没有多路径误差的情况下将存在的噪声。请注意，可从GPS样本将任何高斯噪声减到最小。可使用任何进行此做法的已知方法。然而，除高斯噪声以外，可能仍留有其它噪声。· GPS_multipath = GPS_meas中由于多路径问题而导致的误差。现在，从等式⑶和⑷可导出IMU_drift2D = INS_Trajectory2D-IMU_noise-[GPS_meas2D-GPS_noise-GPS_mulipath] (5)现在，以如下形式来改写等式O)Traj_ture2D_approx = INS_Trajectory2D-IMU_drifi2D_approx (6)其中所有参数具有与在等式O)中相同的意义，且添加的“approx”指代具有近似值的参数。等式(6)是与动作86相关联的等式。而且，等式(5)可用于对由IMU系统引起的漂移(IMU_drift2D)做出第一估计。即，估计IMU_drift2D大致等于IMU_drifi2D ^ [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信号中由于多路径和非高斯噪声而导致的所
12有误差均已被去除，如参看图8a到图8c而阐释的方法才会再现对IMU系统中的漂移的适当估计。因此，当在动作86中使用时，结果仍非常不准确。而且，一旦越来越多的不准确 GPS样本被从GPS测量值去除，就可以迭代方式使用所述方法。这将在下文阐释。将等式(5)与(6)进行组合得到Traj_true2D_approx = Traj_meas2D-EQ[GPS_meas2D-GPS_noise-GPS_multipath-IMU_meas2D] (7)其中EQ[...]指代参数的时间数列均衡，S卩，在[...]之间确定预定数目(例如 100个)样本上的移动平均值(或任何其它低通滤波器)。观察到，可将所有这些2D信号视为在χ和y方向两者上具有分量的参数时间数列。因此，将定位问题分解成一系列1维时间数列均衡问题。如从等式(7)可见，现在的问题是为了计算EQ[...]，需要知道GPS_noiSe和GPS_ multipath-IMU_meas2D0如果可从GPS测量值中滤出这两个参数，那么等式将较好地起作用。这可在如下的迭代过程中完成。首先，如动作92中指示，处理器11应用局部方差滤波器，使得其测试是否GPS_meas2D-Traj_true2D_approxI > Threshold (8)其中GPS_meas2D由所有GPS样本形成。此“Threshold” (例如)等于对GPS集合GPS的方差测量的平均方差值。可针对若干(例如20个到10个)GPS样本的范围而计算局部方差。图7a到图7d展示此方差滤波器在应用于图5所示的实例时可如何工作。图7a展示对应于图5所示的曲线(假如无实际轨迹)的若干曲线。第一曲线展示块形信号，其表示从俯视图看到的交通工具1所行进的根据多个连续GPS样本G(1)、GQ)、 G (3)、…的轨迹。如从图7a可见，在GPS样本G⑴与G(i+1)之间、G(i+5)与G(i+6)之间以及G(j+6)与G(j+7)之间存在较大过渡。而且，存在开始于G(j+1)处的逐渐改变的过渡。 较大的过渡可能是由于多路径误差而导致的。然而，仅基于GPS样本，无法判定哪些GPS样本与由于噪声/多路径问题而导致的错误测量有关，且哪些样本是正确的。图7a中的第二曲线是实线，其指代如从IMU测量值 和、计算出的轨迹。此IMU 线具有漂移(通常每Ikm为Im)，但确切的漂移值是未知的。图7a中的第三曲线是虚线，其是动作86( S卩，上文的等式(6))的新计算出的曲线。现在，在动作92中，处理器11执行方差滤波器，S卩，处理器11以如参看等式(8)所阐释的方式将GPS线与图7a的虚线进行比较。如可从图7a显而易见，所述比较再现了具有较高方差的区域。请注意，在本描述中，术语“方差”指代两个不同信号之间的差而不是同一信号中的连续样本之间的差。这些区域在图7a中以点划圈指示。因此，首先，这传达了样本G(j+1)到具有如此高的方差，以致其不应再在对交通工具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))。这里，假定与近似轨迹线"TrajjruejD.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_appr0X中排除。在此“形状滤波器” 中，再次应用等式(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中以点划线指示)。通过使用此新估计的轨迹Tra j_true2D_ approx，再次根据等式(5a)来计算如由IMU系统引起的漂移。因此，由于动作98中的测试，只要在以迭代方式新估计的轨迹Tra j_true2D_ approx中找到具有较高方差的GPS样本，就去除具有此较高方差的GPS样本。如果如动作 98中所检查，没有再找到此些具有较高方差的GPS样本，那么处理器以动作102继续。在动作102中，处理器11将所得的估计轨迹Traj_true2D_approX与交通工具1的随时间而变 (且因此随在轨迹上的位置而变)的航向结合起来。因此，在处理器11已执行图6所示的流程图的动作之后，知道匪S系统在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计算匪S系统所行进的轨迹的估计曲线。这可根据从现有技术已知的任何方法来完成。然而，在一实施例中，使用如参看图9a和图9b而阐释的方法。图9a展示MMS系统所行进的轨迹Tture。然而，提供随时间而变的所计算航向的动作70的输出仅呈有限数目个样本的形式(IMU系统每秒提供(例如)200个样本，且GPS 系统每秒提供5个样本)。因此，应从随时间而变的这些定向样本且从作为来自动作80的输出的所计算距离来近似表示MMS系统所行进的真实轨迹。此项技术中已知的计算此近似轨迹的非常常见的方法以箭头Tapprl展示。箭头Tapprl指示基于轨迹上的相继所计算点 P之间的线性内插的计算。然而，如从图9a可见，这仅提供一阶近似，其在MMS系统在曲线中驾驶时可能不是非常准确。计算相继点P之间的近似轨迹的替代方法在图9a中以轨迹Tappr2指示。轨迹 Tappr2本身是弯曲的，且以基于回旋曲线部分的所计算轨迹上的相继点之间的内插为基础。图9b展示回旋曲线形状的定义。即，当曲线具有到固定点RO的距离R且在沿曲线朝固定点RO行进距离D时所述距离R以对应于1/R的量减小时，将所述曲线称为回旋形状。 因此，在图9b中，距离Rl在已行进了距离D之后减小到R2 = Rl-a/R，且在已行进了又一距离D之后减小到R3 = R2-a/R2。两个相继点P之间所使用的回旋部分的确切曲率取决于针对点P之前的轨迹计算出的轨迹的曲率的量。根据图9a和图9b的解决方案是基于以下理解的当驾驶员转动其方向盘以将交通工具驾驶到曲线中或从一转弯处出来时，进行此做法的最自然的方式是以交通工具遵循回旋形状曲线的方式来进行此做法。而且，道路中的曲线现今在法律上被要求以回旋形状设计，因为此曲线形状允许驾驶员以最高可能速度驾驶通过所述曲线。如果不是这样，那么驾驶员可能仅以有限的速度驾驶通过曲线以避免危险情况。对计算的输入还是航向和距离。逐个样本进行所述计算。通过使用参看图9a和图9b而阐释的方法，实现了比现有技术方法可能实现的估计更准确的对MMS系统所行进的真实轨迹的估计。结果是基于多个曲线Tappr2的串接的估计轨迹。i十算_绕χ轴和γ轴的定向、斜率、局部重力和ζ水平上文已阐释了可如何应用“形状滤波器”来从一系列GPS样本(包含由于多路径误差而导致的不准确的GPS样本)去除不准确的GPS样本。而且，通过以迭代方式对近似轨迹与从GPS样本确定的轨迹进行比较，可消除由于IMU系统而导致的漂移。通过这样做， 已展示可确定在2D世界中的轨迹的准确近似。然而，真实世界是3D的，且基于(例如)图 1所示的MMS系统的测量的大多数应用同样需要3D数据。此3D数据包含沿所行进道路的 MMS系统的围绕χ轴和y轴的定向以及ζ水平。然而，另一重要参数可为相对于局部重力向量的斜坡角。请注意，局部重力向量无需与指向地球中心的向量重合。这在山脉情况下尤其如此，其中山脉的质量致使重力向量从指向地球中心的向量局部偏差。差异可高达2%。 对于(例如)卡车，地球中心向量如何指向并不如此重要，但局部重力向量如何指向是重要的。仅局部重力向量决定施加于卡车的重力。这在卡车行业中逐渐受到关注。GPS传感器不提供局部重力向量。GPS传感器仅提供关于指向地球中心的向量的数据。原则上，IMU传感器可在静止情形下测量局部重力向量。在动态情形下，IMU测量重力和惯性力的叠加。因此在动态情形下，当尝试测量重力向量时，需要从IMU读数中去除所有动态力。基本上，此处所提出的方法如下· IMU系统在动态情形下提供关于重力加上所有移动加速度的测量值，因为MMS 系统在收集数据的同时正在行驶； 然而，DMI系统测量速度，且因此处理器11可导出匪S系统在χ方向上的移动加速度(请注意，X方向不是固定的X方向，而是相对于安装在MMS系统中的IMU系统沿道路的移动的方向；类似地，y方向局部界定为垂直于χ方向且在道路表面内)； 因此，处理器11可从IMU系统所获得的测量值中去除从DMI系统导出的移动加速度，从而经由MMS系统的随时间而变的纵摇和横摇而再现重力向量。现在将以数学方式对此进行阐释。在建立精确的航向数据且校准DMI系统之后，处理器11可导出影响加速度计读数的非常精确的动态加速度。这些加速度可由以下等式描述ax=dvjdt (9) γ= ζ χ νχ (10)其中疋等于匪S系统的行进方向上的加速度；圮是从DMI系统导出的匪S系统在χ方向上的速度； 等于在垂直于MMS系统的行进方向且垂直于局部重力向量的方向上测得的加速度；黾是从上文所述的2D方法导出的匪S的航向改变速度；去除水平加速度，且另外，可从以下等式导出垂直加速度
1权利要求
1.一种计算机布置，其用于计算交通工具沿轨迹行进时相对于局部重力向量(gl(K )的斜坡角sl0pe_angle，所述计算机布置包括处理器(11)，所述处理器(11)经布置以与存储器(12;13;14;1幻通信，所述存储器(12;13;14;1幻存储可由所述处理器(11)运行的包括指令和数据的计算机程序，所述交通工具包括距离测量单元(DMI)和惯性测量单元 (IMU)，xy平面由χ轴和y轴局部界定，所述χ轴沿所述轨迹，且所述y轴垂直于所述χ轴且垂直于所述局部重力向量，所述处理器(11)经布置以从所述距离测量单元(DMI)和所述惯性测量单元(IMU)两者做出的测量计算所述斜坡角。
2.根据权利要求1所述的计算机布置，其中所述处理器经布置以根据以下等式来计算所述斜坡角slope_angle slope_angle = arcsin (vz/vx) (17) 其中Vz =与所述局部重力向量相反的方向上的速度； 圮=从所述距离测量单元(DMI)导出的所述χ方向上的速度。
3.根据权利要求2所述的计算机布置，其中所述处理器经布置以从以下等式计算Vz vz=\aSTz-dt (18)Ji Φ -Msrz ^aimh-Qz (14)其中减=所述惯性测量单元(IMU)测得的ζ方向加速度；3Ζ =通过以下等式计算出的与所述局部重力向量相反的ζ方向上的加速度az=a>yx vx (11)其中A等于所述交通工具相对于所述局部重力向量的纵摇。
4.根据权利要求2所述的计算机布置，其中所述处理器经布置以从以下等式计算Vz vz = vx · sin (pitchtrue-k · ax) (19)其中Pitchtrue =所述交通工具相对于所述局部重力向量的真实纵摇； ax =从所述距离测量单元导出的χ方向上的加速度； k=常数。
5.根据权利要求3所述的计算机布置，其中所述处理器(11)经布置以 计算vz = Vx · sin(pitchtrue-k · ax) (19) 其中Pitchtrue =所述交通工具相对于所述局部重力向量的真实纵摇； ax =从所述距离测量单元导出的χ方向上的加速度； k=常数；使用等式(19)从通过等式(18)获得的Vz中去除误差，从而再现经误差补偿的垂直速度值ν z, err_comp ‘用所述经误差补偿的垂直速度Vz,_—。。_代替等式(17)中的vz。
6.根据权利要求2-5中任一所述的计算机布置，其中所述处理器(11)经布置以从以下等式计算与所述局部重力向量(gklc )相反的方向上的ζ水平z(t) z(t) = / Vx (t) · sin(slope_angle(t)) (；20)。
7. 一种计算交通工具沿轨迹行进时相对于局部重力向量(ghc )的斜坡角sl0pe_angle 的方法，所述交通工具包括距离测量单元(DMI)和惯性测量单元(IMU)，xy平面由χ轴和 y轴局部界定，所述χ轴沿所述轨迹，且所述y轴垂直于所述χ轴且垂直于所述局部重力向量，所述方法包括从所述距离测量单元(DMI)和所述惯性测量单元(IMU)两者做出的测量计算所述斜坡角。
全文摘要
本发明涉及用于二维和三维精确位置和定向确定的布置和方法。用于根据以下动作来确定一组GPS样本中不准确的GPS样本的方法和设备a)获得由交通工具载有的全球定位系统在沿着轨迹行进时取得的GPS样本；b)基于所述GPS样本，获得所述轨迹的第一估计；c)至少基于由交通工具载有的惯性测量单元在沿着所述轨迹行进时做出的测量而获得所述轨迹的第二估计；d)将所述第一估计与第二估计进行比较；e)建立其中所述第一估计与所述第二估计相比展现高于预定阈值的变化的位置；f)如果不能建立此些位置，那么以动作j)继续，否则以动作g)继续；g)将与具有高变化的位置相关联的GPS样本作为不准确的GPS样本而去除，从而形成一组剩余的GPS样本；h)基于所述剩余的GPS样本再计算所述轨迹的所述第一估计，且再计算所述第二估计；i)重复动作d)到h)；j)结束所述动作。
文档编号G01S19/47GK102385060SQ20111022996
公开日2012年3月21日 申请日期2006年11月6日 优先权日2006年11月6日
发明者克日什托夫·米克萨, 马尔钦·米夏尔·克米奇克 申请人:电子地图有限公司