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

专利名称：用于二维和三维精确位置和定向确定的布置和方法
技术领域：
本发明涉及与(例如)立面图片相关联的位置和定向数据。更一般地说，本发明涉及由某一(某些)测量单元收集到的数据，其中所述数据与在地球上的位置和定向相关联，且此位置和定向是在测量其它数据的同时测量到的。测得的数据可(例如)涉及完全不同于立面图片的某物，如用于数字地图的道路侧标志或用于搜索或探测土壤中的自然能源(石油、天然气等)的土壤条件，或手机或其它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)； ·ax、ay和az，即分别在x、y和z方向上的加速度。
为了计算在2D空间中的位置和定向，使用在x和y方向上的加速度ax和ay以及围绕z轴的旋转速度，即ωz。从这些加速度，可如下计算位置x和y以及航向 x＝∫∫ax·dt2 y＝∫∫ay·dt2 航向＝∫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测量值ax和ay计算出的轨迹。此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可导出影响加速度计读数的非常精确的动态加速度。这些加速度可由以下等式描述 其中

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

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

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

是从上文所述的2D方法导出的MMS的航向改变速度； 去除水平加速度，且另外，可从以下等式导出垂直加速度 其中

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

等于MMS系统相对于局部重力向量的纵摇。
这给予处理器11去除动态加速度的主要分量的机会。以此方式产生“类似于静止的”读数 其中

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

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

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

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

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

等于z轴上的静止(动态已去除的)加速度 这些“类似于静止的”值将是局部重力读数的无偏估计，然而仍具有一些由MMS系统在沿道路行进时的振动引起的白噪声。随着时间的过去，此噪声分量的平均值将等于零。因此，可使用求平均来从信号中消除此白噪声。
现在，可从以下等式获得纵摇和横摇 从等式(15)和等式(16)导出的pitchacc和rollacc的值是基于加速度测量值的IMU系统的定向的纵摇和横摇的无偏近似值。然而，由于所述值是从具有噪声的数据导出的，所以所述值将也是具有噪声的数据。
为了估计IMU定向的纵摇和横摇的真实值，处理器11可以类似方式使用如上文在2D部分中所描述的方法。上文已阐释了可如何在交通工具航向

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

和

由的漂移。
图10a展示针对时间窗t0到t3的所得近似纵摇值pitchacc。在实例中，pitchacc在时刻t1处上升，且在时刻t2处减小。所展示的信号具有由于在道路上行进期间的MMS系统振动而导致的噪声。
现在，处理器11可导出IMU系统相对于重力向量的纵摇和横摇定向的真实值。在下文的描述中，这些值将分别被称为pitchtrue和rolltrue。
处理器11通过IMU系统所做出的加速度测量来获得围绕y轴的纵摇值。通过双积分，处理器11从此加速度测量导出所计算的纵摇pitchIMU。图10b中展示结果。如图所示，计算出的纵摇pitchIMU不具有噪声，但其具有由双积分引起的漂移。
接着，处理器11从图10b所示的曲线减去图10a所示的曲线。这产生图10c所示的曲线。图10c所示的曲线应展示IMU系统所测得的纵摇中的漂移具有添加到其的一些噪声。因此，(例如)通过使用求平均技术或局部线性回归，处理器11从图10c所示的信号中去除噪声，从而再现从IMU测量得出的纵摇中的漂移。现在，处理器11可补偿来自从IMU系统的测量得出的纵摇的漂移。这提供准确的纵摇pitchtrue。
现在，处理器11也可以如相对于纵摇pitchtrue(对照图10a到图10c)而阐释的方式相同的方式来计算准确的横摇rolltrue。
计算斜率 原则上，静止IMU传感器可测量局部重力向量，也就是说，一旦知道相对于局部重力向量的纵摇和横摇，就知道局部重力向量。在上文的部分中呈现了关于如何在动态情形下测量局部重力向量的方法。为了进一步从这些读数计算斜率，需要考虑交通工具的额外动态特性。举例来说，如果小汽车具有相对较短的长度，或IMU系统没有精确地安装到交通工具主体，那么测得的局部重力向量可能是准确的，但斜坡的角度可能仍非常不准确。这当然可通过用非常长的小汽车(或卡车)进行测量，且将IMU系统精确地安装到交通工具来避免，但那是不切实际的。而且，这可通过在小汽车正在行驶时用IMU传感器进行测量来避免，但测量可能遭受IMU输出中的漂移的过多影响。
图11展示用于计算相对于重力的斜率的一些相关参数。假定MMS系统处于斜坡上的点P处。MMS系统以速度

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

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

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

的方向上的“垂直”速度

和垂直于此垂直速度

的速度(此后一个垂直速度在图11中未图示)。因此，以下等式成立 slope_angle＝arcsin(vz/vx)(17) 垂直速度vz也可从IMU静止系统所做出的测量中导出，所述IMU静止系统测量局部重力向量

的方向上的垂直静止加速度

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

代入等式(17)中。
换句话说，通过从DMI测量值获得行驶速度

从关于垂直加速度

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

的斜坡角。
在第一替代方案中，处理器11使用以下等式来计算斜坡角 vz＝vx·sin(pitchtrue-k·ax)(19) 其中pitchtrue＝纵摇，其可从上文所阐释的计算中获得； ax＝x方向上的加速度，其可从DMI系统获得； k＝常数。k的值可通过测试找到，且取决于MMS系统的质量弹簧特性。
观察到，等式(19)与从现有技术中已知的一样。然而，新的内容是理解此等式确定相对于局部重力向量

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

的垂直速度vz的另一方式，即，以vz，err-comp的形式。一旦以此方式计算出经误差补偿的垂直速度值vz，err-comp，处理器11就使用等式(17)来计算斜坡角，其中在等式中使用vz，err-comp代替vz。
尽管此处将斜率计算呈现为基于等式(17)，但一般来说，相对于局部重力向量的斜率的计算可基于其中使用DMI系统和IMU系统的测量值且其中认识到IMU系统的测量是相对于局部重力向量而做出的任何等效等式。
而且，观察到，将计算相对于局部重力向量的斜率视为单独的发明，其不同于2D平面内的任何发明，例如去除不准确的GPS样本。一旦具有一组相当准确的GPS样本，就可以上文所描述的方式来计算斜率。这组GPS样本无需以本文献中所阐释的方式获得。
计算z水平 现在，处理器11可使用以下等式来容易地计算MMS系统的随时间而变的相对z水平 z(t)＝∫vx(t)·sin(slope_angle(t))(20) 其中z(t)是在局部重力向量

的方向上的相对z水平。
此相对z水平可由处理器11用来计算绝对z水平，即在与地球的中心方向相反的z方向上测得的高于海平面的z水平。为此，处理器11可经布置以导出移位向量，其用于使每一相对z水平值移位到绝对海平面值。此移位向量可(例如)由处理器11通过以下方式获得对上文所阐释的2D方法已识别为良好的所有GPS zGPS(t)读数进行求平均，从而得到平均值，且从此平均值减去对应相对z(t)值的平均值。
权利要求
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)经布置以在执行以下动作的同时，计算所述交通工具沿所述轨迹行进时随时间而变的纵摇和横摇中的至少一者的近似值
计算
其中
计算
其中
等于在与所述局部重力向量相反的方向上的加速度；
等于所述交通工具相对于所述局部重力向量的纵摇；
计算
其中
计算以下各项中的至少一者
其中
pitchapp＝纵摇的近似值
rollapp＝横摇的近似值。
15.根据权利要求14所述的计算机布置，其中所述处理器(11)经布置以从所述惯性测量单元做出的所述测量中计算纵摇和横摇中的所述至少一者的估计，将纵摇和横摇中的所述至少一者的所述近似值与所述纵摇和横摇中的所述至少一者的所述估计进行比较，且基于所述比较而补偿纵摇和横摇中的所述至少一者的所述估计中的误差，以便再现被界定为经误差补偿的纵摇的真实纵摇以及被界定为经误差补偿的横摇的真实横摇中的至少一者。
16. 根据权利要求14或15所述的计算机布置，其中所述处理器(11)经布置以根据以下等式来计算所述交通工具沿所述轨迹行进时相对于局部重力向量
的斜坡角slope_angle
slope_angle＝arcsin(vz/vx)(17)
其中
vz＝与所述局部重力向量相反的方向上的速度，其是从以下等式计算
17.根据权利要求15所述的计算机布置，其中所述处理器(11)经布置以根据以下等式来计算所述交通工具沿所述轨迹行进时相对于局部重力向量
的斜坡角
slope_angle
slope_angle＝arcsin(vz/vx)(17)
vz＝vx·sin(pitchtrue-k·ax)(19)
其中
vx＝与所述局部重力向量相反的方向上的速度；
pitchture＝真实纵摇；
ax＝从所述距离测量单元导出的x方向上的加速度；
k＝常数。
18.根据权利要求16所述的计算机布置，其中所述处理器(11)经布置以
计算
vz＝vx·sin(pitchtrue-k·ax)(19)
其中
pitchtrue＝真实纵摇；
ax＝从所述距离测量单元导出的x方向上的加速度；
k＝常数；
使用等式(19)从通过等式(18)获得的vz中去除误差，从而再现经误差补偿
的垂直速度值vz，err_comp；
用所述经误差补偿的垂直速度vz，err_comp代替等式(17)中的vz。
19.根据权利要求16或17所述的计算机布置，其中所述处理器(11)经布置以从以下等式计算与所述局部重力向量
相反的方向上的z水平z(t)
z(t)＝∫vx(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(vz/vx)(17)
其中
vz＝与所述局部重力向量相反的方向上的速度；
25.根据权利要求24所述的计算机布置，其中所述处理器经布置以从以下等式计算vz
其中
其中
其中
等于所述交通工具相对于所述局部重力向量的纵摇。
26.根据权利要求24所述的计算机布置，其中所述处理器经布置以从以下等式计算vz
vz＝vx·sin(pitchtrue-k·ax)(19)
其中
pitchtrue＝所述交通工具相对于所述局部重力向量的真实纵摇；
ax＝从所述距离测量单元导出的x方向上的加速度；
k＝常数。
27.根据权利要求25所述的计算机布置，其中所述处理器(11)经布置以
计算
vz＝vx·sin(pitchtrue-k·ax)(19)
其中
pitchtrue＝所述交通工具相对于所述局部重力向量的真实纵摇；
ax＝从所述距离测量单元导出的x方向上的加速度；
k＝常数；
使用等式(19)从通过等式(18)获得的vz中去除误差，从而再现经误差补偿的垂直速度值vz，err_comp；
用所述经误差补偿的垂直速度vz，err_comp代替等式(17)中的vz。
28.根据权利要求24到27中任一所述的计算机布置，其中所述处理器(11)经布置以从以下等式计算与所述局部重力向量
相反的方向上的z水平z(t)
z(t)＝∫vx(t)·sin(slope_angle(t))(20)。
29.一种计算交通工具沿轨迹行进时相对于局部重力向量
的斜坡角slope_angle的方法，所述交通工具包括距离测量单元(DMI)和惯性测量单元(IMU)，xy平面由x轴和y轴局部界定，所述x轴沿所述轨迹，且所述y轴垂直于所述x轴且垂直于所述局部重力向量，所述方法包括从所述距离测量单元(DMI)和所述惯性测量单元(IMU)两者做出的测量计算所述斜坡角。
30.一种计算机程序产品，其包括可由计算机布置加载以允许所述计算机布置执行根据权利要求29所述的方法的指令和数据。
31.一种数据载体，其包括根据权利要求30所述的计算机程序产品。
全文摘要
用于根据以下动作来确定一组GPS样本中不准确的GPS样本的方法和设备a)获得由交通工具载有的全球定位系统在沿着轨迹行进时取得的GPS样本；b)基于所述GPS样本，获得所述轨迹的第一估计；c)至少基于由交通工具载有的惯性测量单元在沿着所述轨迹行进时做出的测量而获得所述轨迹的第二估计；d)将所述第一估计与第二估计进行比较；e)建立其中所述第一估计与所述第二估计相比展现高于预定阈值的变化的位置；f)如果不能建立此些位置，那么以动作j)继续，否则以动作g)继续；g)将与具有高变化的位置相关联的GPS样本作为不准确的GPS样本而去除，从而形成一组剩余的GPS样本；h)基于所述剩余的GPS样本再计算所述轨迹的所述第一估计，且再计算所述第二估计；i)重复动作d)到h)；j)结束所述动作。
文档编号G01S5/14GK101563625SQ200680056664
公开日2009年10月21日 申请日期2006年11月6日 优先权日2006年11月6日
发明者马尔钦·米夏尔·克米奇克, 克日什托夫·米克萨 申请人:电子地图有限公司