CN102192737A

CN102192737A - 位置推定装置

Info

Publication number: CN102192737A
Application number: CN2011100369690A
Authority: CN
Inventors: 板东干雄; 田中克明; 川股幸博; 青木利幸
Original assignee: Hitachi Ltd
Current assignee: Hitachi Ltd
Priority date: 2010-02-25
Filing date: 2011-02-10
Publication date: 2011-09-21
Anticipated expiration: 2031-02-10
Also published as: JP2011174829A; US8260325B2; CN102192737B; JP5114514B2; EP2363731B1; US20110244881A1; EP2363731A1

Abstract

本发明提供一种位置推定装置。本发明的课题是，不设置成为用于提高GPS定位精度的参照的基站，而提高在基于三角测量的位置确定中使用的距离测量不稳定的场所中的定位精度。在采用GPS或其他三角测量对移动体的位置进行推定的装置中，与其他移动体和中心等，通过通信单元获取从自身还未得到的距离测量信息。此外，通过距离测量单元测量到其他移动体的相对位置。通过对还未得到的距离测量信息进行适当变换，并给出测量出相对位置的移动体间的相对距离，从而求解与能得到来自定位计算最低限所需3点的距离信息同等的联立方程式，由此即使在距离测量不稳定的场所也能够进行定位。

Description

位置推定装置

技术领域

本发明涉及在室内外推定移动体的位置的系统。

背景技术

以往，为了精度良好地求取多个移动体的位置，提案有采用多个移动体的信息的方法。例如，在JP特开2003-337029号公报中提案有如下装置，即，通过对其他移动体的GPS(Global Positioning System)和本移动体的GPS信号进行比较，并采用公共的GPS卫星信号，从而正确计算相对位置，并在地图上进行绘制(mapping)的装置。此外，在JP特开2002-340589号公报中公开了如下方法，即，在正确的位置设置已知的基站，从GPS终端观测到的虚拟距离中减去以由基站测量出的虚拟距离为基础计算出的误差，并在网络上发布修正信息，从而对位置进行修正。此外，在JP特开2009-150722号公报中公开了如下方法，即，计算移动体的绝对位置的概率分布，确定3个以上的移动体的绝对位置。此外，在JP特开2008-312054号公报中公开了采用某阈值来判别信号的周期的方法，采用这种方法能够减少GPS信号的误检测。

【专利文献】

专利文献1：JP特开2003-337029号公报

专利文献2：JP特开2002-340589号公报

专利文献3：JP特开2009-150722号公报

专利文献4：JP特开2008-312054号公报

但是，成为用于提高GPS定位(positioning)精度的参照(reference)的基站的设置存在必须考虑其设置条件和成本变高的课题。并且，为了降低用于基站设置的成本，即使在如专利文献2记载的发明那样在网络上发布修正信息并对位置进行修正的情况下、和在如专利文献3记载的发明那样以由多个移动体得到的信息为基础对位置进行修正的情况下，在实际观测场所中，在各终端不能观测4个以上GPS卫星就不能进行位置推定等、采用三角测量(triangular surveying)的方式对位置进行推定的情况下，距离测量不稳定的场所中的定位精度就成为课题。

发明内容

在本发明中，课题是，不设置成为用于提高GPS定位精度的参照的基站，就提高有时能基于三角测量进行位置确定、有时又不能基于三角测量进行位置确定这样的在距离测量不稳定的场所中的定位精度。

在采用GPS或其他三角测量来推定移动体的位置的装置中，与其他移动体和中心等，通过通信单元，获取在本装置中还未获得的距离测量信息。并且，通过距离测量单元来测量到获取了距离测量信息的移动体为止的相对距离。通过对获取到的距离测量信息进行适当变换，并给出该进行了测量的移动体间的相对距离，从而建立与能得到来自3点的距离信息同等的联立方程式，求解该方程式得到定位结果。

在现有的采用三角测量原理的位置推定方法中，条件是相对一个移动体必须能够测量距3点以上场所的距离，但是通过采用多个移动体的信息，并且反映与多个移动体之间的关系性，并设包含多个移动体在内而能够进行观测的点数合计为3点以上，从而能够测量各自的位置，能够保持定位精度。此外，由于即使在能够测量从3点以上的场所到自身位置的距离的情况下，通过获取距离的差分，按照使该误差最小的方式进行最大似然推定，也能够减少在测量距离3点的距离时产生的误差，所以能够比单一移动体的位置推定精度更高精度地对位置进行推定。

附图说明

图1是位置推定系统的结构图。

图2是移动体的处理流程图。

图3是中心中的处理流程图。

图4是关系性判定单元的结构图。

图5是关系性判定单元的处理流程图。

图6是基线向量计算单元的处理流程图。

图7是连锁判定单元的处理流程图。

图8是信号独立性判定单元的处理流程图。

图9是移动体携带有关系性判定单元的位置推定系统的结构图。

图10是移动体的处理流程图。

图11是本发明的位置推定系统的适用例。

符号说明：

101、111、1101移动体

102、1102信号接收单元

103、1103位置姿势计算单元

104、1104距离测量单元

105、125、1105通信单元

106位置修正信息存储单元

107、1109特征识别单元

108、1108内部存储单元

121中心

122、1107关系性判定单元

123移动体分类存储单元

124、1106位置修正单元

126移动体位置修正信息存储单元

127移动体信息存储单元

402连锁判定单元

403信号独立性判定单元

404基线向量计算单元

具体实施方式

以下，采用附图，说明采用了本发明的位置推定系统的实施例。

【实施例1】

图1示出采用了本发明的位置推定系统的第一实施例的结构图。第一实施例是在如矿山等这样的、确定的移动体在确定的行驶路径上进行移动的情况下，在移动体和中心或移动体彼此之间能够频繁进行通信的环境下，提高定位位置的精度。

该位置推定系统通过被组装入移动体的专用装置或移动体中来实现，且由多个移动体101、111和中心121构成。此外，设移动体101、111以及中心121的管理时刻在共同的时刻取得同步。在图1中，作为简单的例子，示出由移动体101和移动体111以及中心121构成的情况，但是移动体的数目也可以更多。移动体101包括：信号接收单元102，其获取定位信号等传感器信息；位置姿势计算单元103，其基于接收到的定位信号等传感器信息来计算位置；距离测量单元104，其测量与其他移动体的相对距离；通信单元105，其将搭载于移动体上的传感器的信息和由位置姿势计算单元103计算出的位置的信息与外部进行通信；位置修正信息存储单元106，其预先保存从中心121递送的位置修正信息；特征识别单元107，其识别由距离测量单元104测量相对距离的对方移动体的特征；和内部存储单元108，其具有存储接收到的定位信号等传感器信息和相对距离的测量结果以及此时的时刻的测量信息存储区域、以及存储发送中途的值的中断信息存储区域。距离测量单元104和特征识别单元107也可以采用如立体照相机这样具有由一个设备获取到对象物体的距离和特征的两重效果的机器。此外，移动体111也和移动体101是相同的结构。

中心121由以下构成：关系性判定单元122，其根据包含从移动体101和移动体111等多个移动体得到的距离能否测量在内的移动体相互的相对距离的测量结果，来判定与其他移动体的相对位置等联系；移动体分类存储单元123，其存储由关系性判定单元122判定的信息；位置修正单元124，其计算位置修正信息并对来自掌握当前位置的各移动体的当前位置进行修正；通信单元125，其将位置修正信息向移动体进行通信；移动体位置修正信息存储单元126，其存储由位置修正单元124计算出的位置修正信息；和移动体信息存储单元127，其存储从多个移动体得到的信息。

下面，在图2中示出位置推定系统的移动体的处理流程，并且在图3中示出位置推定系统的中心的处理流程。该位置推定系统大致区分为终端的处理和中心的处理，通过采用双方的信息进行处理来进行整体的处理。

首先，从移动体的处理开始说明。在该说明中，说明移动体101的动作。在移动体101中，通过信号接收单元102按规定的周期来获取由传感器等得到的定位信号等传感器信息。此外，特征识别单元107扫描移动体101的周围，针对事前识别的对象来检索存储在内部存储单元108中的其他移动体的特征。这里，所谓移动体的特征是指各个移动体携带的激光反射板或特征性的图案/标记或移动体的形状等、预先与各移动体建立对应并进行保存的指标。在第一实施例中，作为其他移动体，保存移动体111的特征。将各个特征按照事前附加了规则的转换方式转换为值，并配合识别出特征的方向，作为特征量。并且，通过距离测量单元104来测量到识别出的特征的相对距离(步骤201)。

下面，在步骤202中，将在步骤201中获取到的传感器信息和该获取时刻以及测量出的相对距离和识别出的特征量作为测量信息存储至内部存储单元108的测量信息存储区域中。此时，即使在测量信息存储区域中留有上次存储的信息，也将其覆盖。

下面，在步骤203中，确认在位置修正信息存储单元106中是否存在位置修正信息，如果存在，则转移至步骤204采用该位置修正信息来修正当前的位置。如果不存在位置修正信息，则转移至步骤205。在步骤205中，判断是否能够采用在步骤202中存储的测量信息来计算位置/姿势。如果能够计算，则在步骤206中计算位置/姿势。如果不能计算，则转移至步骤207。

在步骤207中，判断是否能够与中心121进行通信。在不能与中心121进行通信的情况下，转移至步骤213，待机直到下一次的测量信息获取。如果能够与中心121进行通信，则在步骤208中，判断在后述的内部存储单元108的中断信息存储区域中是否存在从上次的步骤209至步骤211的处理期间在中途保留的处理信息。不存在的情况下，在步骤209中，将位置修正信息的接收请求与在步骤202中存储的传感器等的信息以及传感器信息的获取时刻、本移动体的移动体ID、识别出的与其他移动体的相对距离和特征量一起，向中心发送。在步骤210中，接收与送出接收请求的移动体的ID对应的位置修正信息，其中，该接收请求作为来自中心121的针对在步骤209中向中心送出的位置修正信息的请求的应答而送来。在步骤211中，在位置修正信息存储单元106中存储在步骤210中接收到的信息，在步骤213中待机直到获取测量信息。

其中，步骤207以后，如果在该处理中下一次的测量信息获取的机会来临，则为了随时保留处理并进行步骤201的测量信息获取的处理和步骤202的测量信息的存储处理，而在内部存储单元108的中断信息存储区域中存储保留处理后留下的信息，并返回步骤201。然后，在以后的处理中，如果在步骤208的处理中判断为在内部存储单元108的中断信息存储区域中有保留信息，则从在内部存储单元108的中断信息存储区域中存储的保留的地方开始重新开始处理。同样的处理也可以在移动体111的位置推定系统中进行。

图3示出中心121的处理流程。首先，获取来自移动体的在图2所示的处理流程中的步骤209中发送的信息(步骤301)。下面，在步骤302中，将从移动体接收到的传感器信息以及该获取时刻、其他移动体的相对距离和该特征量的信息存储至移动体信息存储单元127。此外，在该时间点，删除在移动体信息存储单元127中存储的来自移动体的信息之内、经过某固定时间以上的信息。此外，在存在具有同一移动体ID的信息的情况下，覆盖该信息。

下面，在步骤303中，判断是否在移动体信息存储单元127中存储有来自多个移动体的信息。在只有单个移动体的信息的情况下，转移至步骤308，针对作为移动体位置修正信息存储单元126的预先管理对象而进行登记的所有的移动体ID，存储为“无修正信息”。在存在来自多个移动体的信息的情况下，转移至步骤304，通过关系性判定单元122判定多个移动体的关系性，将具有关系性的移动体判断为汇总的集团，在下一步骤305中，将来自步骤304的输出与各移动体ID一起存储在移动体分类存储单元123中。

然后，在步骤306中，基于在移动体分类存储单元123中存储的信息，判断是否能够计算位置修正信息。此时，求取接收到的传感器信息中的定位信号之内、独立的定位信号的数目和各移动体的应当求取的位置的未知变量的数目。这里，对求出的独立的定位信号的数目和各移动体的应当求取的位置的未知变量的数目进行比较，在独立的定位信号的数目比各移动体的应当求取的位置的未知变量的数目少的情况下，判断为不能计算位置修正信息，并转移至步骤308，针对移动体位置修正信息存储单元126的所有的移动体ID，存储为“无修正信息”。

此外，对求出的独立的定位信号的数目和各移动体的应当求取的位置的未知变量的数目进行比较，在独立的定位信号的数目比各移动体的应当求取的位置的未知变量的数目多或者是相同的数目的情况下，如果判断为能够计算位置修正信息，则转移至步骤307，计算位置修正信息。如果在步骤307中计算出位置修正信息，则在步骤308中，按每个移动体ID在移动体位置修正信息存储单元126中存储位置修正信息。

下面，在步骤309中，判断是否存在来自移动体的位置修正信息的请求，在存在请求的情况下，在步骤310中发送与发出该接收请求的移动体的移动体ID对应的位置修正信息。在未接收到来自移动体的接收请求的情况下，在步骤311中，在接收下一次的信息之前转移至待机状态。另外，各移动体的处理流程和中心121的处理流程并行工作。

这样，由于中心在每次存在机会时都对移动体的位置推定结果进行修正，所以即使在基于GPS等三角测量的位置确定中使用的距离测量不稳定的场所中，也能够提高位置精度。

下面，说明移动体的各装置结构。在移动体中的信号接收单元102中，接收用于计算各个移动体的位置的信号。即，用于通过三角测量计算位置的信号和用于检测移动体的姿势以及速度的传感器的信号。用于通过三角测量计算位置的信号是来GPS卫星的导航电波、和来自陆上标志(landmark)的距离信息等。所谓用于检测移动体的姿势的传感器是陀螺传感器和加速度传感器等相对传感器、以及地磁传感器等表示绝对方位的传感器。此外，用于检测速度的传感器是轮胎的编码器、包含速度信息的GPS、多普勒速度计等。

在位置姿势计算单元103中，以由信号接收单元102接收到的来自传感器等的信息为基础来计算移动体的位置。例如，在基于GPS信号的三角测量中，接收来自N基卫星的信号，根据第i卫星的位置(Xi，Yi，Zi)、想要计算的移动体的位置(x，y，z)、距离第i卫星的距离ri及其误差s，通过对由以下(式1)表示的方程式进行联立求解，从而能够计算移动体的位置(x，y，z)。

【数1】

\{\begin{matrix} r_{1} = \sqrt{{(X_{1} - x)}^{2} + {(Y_{1} - y)}^{2} + {(Z_{1} - z)}^{2}} + s \\ r_{2} = \sqrt{{(X_{2} - x)}^{2} + {(Y_{2} - y)}^{2} + {(Z_{2} - z)}^{2}} + s \\ . \\ . \\ . \\ r_{N} = \sqrt{{(X_{N} - x)}^{2} + {(Y_{N} - y)}^{2} + {(Z_{N} - z)}^{2}} + s \end{matrix}

…(式1)

然后，考虑由(式1)计算出的位置的同时考虑其定位误差，在位置姿势计算中使用卡尔曼滤波器(Kalman filter：カルマンフイルタ)等概率模型滤波器，根据接收到的信号进行最大似然位置推定，并且按概率方法来估计其误差。

在移动体的距离测量单元104中，采用传感器等测量从本移动体到其他移动体之间的相对距离。作为用于测量相对距离的传感器，有立体照相机(stereo camera)、激光测距仪(laser range finder)、毫米波雷达(millimeterwave radar)等。

在移动体的特征识别单元107扫描本移动体的周围，检索作为事前识别的对象而进行存储的其他移动体的特征，与相对本移动体的方向一起进行测量。作为用于进行特征识别的传感器，列举照相机和3维激光测距仪等。

在移动体的通信单元105中，向中心121发送由信号接收单元102、位置姿势计算单元103、距离测量单元104得到的信息。此外，接收从中心121送来的信息。

在位置修正信息存储单元106中，存储由通信单元105接收到的来自中心121的位置修正信息。这里，存储的信息在进行位置修正后被消除。

下面，说明中心121的各装置结构。在关系性判定单元122中，通过判定多个移动体的关系性，将关联的移动体分类为汇总的集团，并且根据各自的位置坐标和各移动体间的相对距离等来计算各个移动体的位置。图4示出关系性判定单元122的详细结构，图5示出其处理流程。关系性判定单元122由以下构成：连锁判定单元402，其根据从移动体发送来的由距离测量单元104得到的信息，将知道移动体间的相对距离的移动体彼此之间判定为有联系，将不知道相对距离的移动体彼此之间判定为没有联系；信号独立性判定单元403，其判定由信号接收单元得到的传感器信息之内的各个定位信号是来自独立的发送源的信息、还是与其他定位信号的发送源相同；和基线向量计算单元404，其采用移动体的信号接收单元和距离测量单元中的至少任意一方的计算结果来计算各移动体彼此之间的相对高度。

关系性判定单元122，将确认到接收用于在步骤303中个别识别多个移动体的移动体ID以及从各个移动体得到的传感器信息以及相对距离/特征量的数据作为处理开始的前提。首先，在步骤502中，从移动体信息存储单元127获取传感器信息以及测量了相对距离的移动体的移动体ID。这里，设已经对移动体分配了移动体ID。下面，在步骤503中，根据获取到的相对距离以及特征量的方向，计算将测量了相对距离的移动体的位置作为基准地点、并将被测量的移动体作为未知地点时的到该未知地点为止的基线向量。

图6示出该基线向量计算的详细处理流程。在针对在步骤502中获取到的在移动体信息存储单元127中存储有来自移动体的信息的所有移动体的循环处理(步骤600)中，在步骤601中，选择一个还未进行基线向量计算的移动体，从移动体信息存储单元127中获取与所选择的移动体ID对应的特征量。在步骤602中，在来自移动体的信息中包含的测量信息之内、针对识别出的特征的方向的信息具有包含相对移动体的行进方向的高度方向以及水平方向的倾斜的信息，判断是否能够作为3维基线向量来进行计算。3维基线向量的计算如果是基于立体照相机或3维激光测距仪等的测量就是可能的。如果3维基线向量能够计算，则在步骤603中获取在来自移动体的信息中包含的测量信息之内、移动体的姿势的信息，配合该结果在步骤604中计算3维基线向量。将计算出的基线向量在步骤609中作为相对距离信息，与作为基准的移动体的移动体ID一起送出至连锁判定单元402。

此外，在步骤602中，在判断为3维基线向量不能计算的情况下，在步骤605中，针对识别出的特征的方向的信息具有相对移动体的行进方向的水平方向的倾斜的信息，判断是否能够进行2维基线向量的计算。2维基线向量的计算如果是基于激光测距仪等的测量就能够进行。如果能够计算2维基线向量，则在步骤606中，获取在来自移动体的信息中包含的测量信息之内、移动体的姿势的信息，并且在步骤607中，计算2维基线向量，在步骤609中，作为相对距离信息，与作为基准的移动体的移动体ID一起送出至连锁判定单元402。

此外，在步骤605中判断为不能计算2维基线向量的情况下，在步骤608中，放弃计算基线向量的2点间的方向，仅仅计算相对距离。在已经计算出相对距离的情况下，采用该相对距离。并且，在步骤609中，与以相对距离作为基准的移动体的移动体ID一起送出至连锁判定单元402。

以上处理，针对在步骤502中获取到的在移动体信息存储单元127中存储有来自移动体的信息的所有移动体，直到没有未处理的移动体为止都继续从步骤600开始的循环处理。

返回图5的说明，下面，在步骤504中，通过关系性判定单元122中的连锁判定单元402，做出如下判定，即，根据从移动体发送来的由距离测量单元104得到的信息，将知道移动体间的相对距离的移动体彼此之间判定为有联系，将不知道相对距离的移动体彼此之间判定为没有联系。图7示出该连锁判定单元402的连锁判定处理流程。

在步骤701中，如果接受在步骤503中求出的3维或2维基线向量或相对距离和成为基准的移动体的移动体ID，则从移动体信息存储单元127获取与关系性判定单元122在步骤502中获取的所有移动体ID对应的各移动体的位置/姿势以及各移动体测量出的相对距离和对象移动体的特征量(步骤702)，将其中进行了位置/姿势的计算的移动体映射至3维空间上(步骤703)。

下面，针对测量了3维或2维基线向量或相对距离等的其他移动体的相对位置的各移动体，重复将进行了测量的对象移动体建立关联的处理(步骤704)。然后，从在登记在移动体信息存储单元127中的移动体之内、未处理的移动体之中选择一个作为处理对象(步骤705)。

下面，在步骤706中，判断是否能够提取成为相对位置测量的对象的移动体的特征。这里，作为能够提取移动体的特征的情况有以下情况等：被测量移动体携带有激光反射板等，由激光测距仪测量其反射；各个移动体携带有具有特征性的图案的标记，并能够由立体照相机来识别该标记。如果能够提取特征，则转移至步骤707，对事前确定的各移动体的特征进行比较，并确定测量出相对位置的移动体，判定为作为移动体对象而选择出的移动体和该确定出的移动体有联系。

此外，如果不能提取特征(也包括不存在成为相对位置的测量对象的移动体的情况，即未测量出其他移动体的情况)，则转移至步骤708，从移动体信息存储单元127中提取存在于测量出的相对位置的误差范围内的移动体。此时，在未利用作为处理对象而选择出的移动体来计算位置的情况下，认为没有存在于误差范围内的移动体。下面，转移至步骤709，判断是否存在多个相对距离的误差范围内的移动体。在提取出的移动体为一个的情况或者一个也不存在的情况下，转移至步骤710，在提取出的移动体为一个的情况下，确定测量出该相对位置的移动体，判定为作为处理对象而选择出的移动体和该确定出的移动体有联系。另外，在提取出的移动体一个也不存在的情况下，结束针对作为处理对象而选择出的移动体的处理。

在提取出的移动体存在多个的情况下，转移至步骤711，判断测量出的相对位置的信息是否是2维以上的基线向量的信息。在具有2维以上的基线向量的情况下，在步骤712中将存在于与相对距离信息的水平方向的倾斜最接近的位置的移动体确定为测量出相对位置的移动体，判定为作为处理对象而选择出的移动体和该确定出的移动体有联系。如果相对位置的信息是1维，即仅仅是相对距离的信息，则由于不对来自成为基准的移动体的方向进行确定，所以转移至步骤713，存储具有不能确定的相对距离信息的移动体的移动体ID。然后，在步骤714中，对于所有移动体确认进行了上述判定，在未进行的情况下，重复从步骤705开始的处理。

在对于所有的移动体都判断为进行了步骤705～714的循环处理的情况下，转移至步骤715，提取判定为有联系的移动体的组。下面，转移至步骤716，即使是在步骤715中提取出的组的一个，也将共有的移动体的组集中作为一个集团，将所有移动体分组。下面，在步骤717中，将在步骤716中分组后的结果保存至移动体分类存储单元123。

说明对该移动体进行分组的处理的概念。设能够从移动体A测量移动体B的相对距离。此外，同样地，设能够从移动体B测量移动体C和移动体D804的相对距离，能够从移动体E测量与移动体F的相对距离。此时，在移动体A、移动体B、移动体C、移动体D之间，直接或间接地测量各个相对距离，判断为有联系，判定为一个集团。此外，同样地，移动体E和移动体F由于测量直接相对距离，所以判断为有联系，判定为一个集团。但是，在包含移动体A的集团和包含移动体E的集团的移动体之间，由于直接地和间接地都未测量相对距离，所以判定为其他集团。

再次返回图5的流程，在步骤505中，由信号独立性判定单元403判定由属于相同集团的各移动体的信号接收单元102得到的传感器等的信号之内的定位信号是否是来自独立的发送源的信息。其中，设从作为发送源的定位卫星发送的信号是独一无二的。图8中示出信号独立性判定单元403的处理流程。

在该处理中，通过连锁判定单元402按分组后的每个集团，针对所有集团进行处理(步骤900)。在各集团中，首先，在步骤901中，针对接收了进行三角测量所需的定位信号的所有移动体，读出该移动体ID和从具有该移动体ID的各移动体接收到的传感器信息之内的定位信号。下面，在步骤902中，判断在读出的这些定位信号中是否载有能够进行独立性判定的代码。所谓能够进行该独立性判定的代码在GPS中表示PN码这样的识别代码。PN码的辨别采用专利文献4中记载的方法。在确定这样的发送源的代码存在于定位信号中的情况下，为了能够判断直接发送源的独立性，转移至步骤903，按照在接收到的定位信号中载有的代码不重复的方式，即发送源不重复的方式选择定位信号。

在能够进行独立性判定的代码未载于接收到的定位信号中的情况下，转移至步骤904，针对从各移动体接收到的传感器信息来求取观测矩阵。例如，将求出的位置的变量(x，y，z)作为未知变量，通过在某点(x0，y0，z0)周围对(式1)进行泰勒展开至1次项，从而能够如以下(式2)这样表示接收到由(式1)表示的各定位信号时的观测矩阵。

【数2】

(\begin{matrix} r_{1} \\ r_{2} \\ . \\ . \\ . \\ r_{N} \end{matrix}) = (\begin{matrix} \sqrt{{(X_{1} - x_{0})}^{2} + {(Y_{1} - y_{0})}^{2} + {(Z_{0} - z_{0})}^{2}} + \frac{&PartialD; r_{1}}{&PartialD; (x_{1}, y_{1}, z_{1})} \\ \sqrt{{(X_{2} - x_{0})}^{2} + {(Y_{2} - y_{0})}^{2} + {(Z_{2} - z_{0})}^{2}} + \frac{&PartialD; r_{2}}{&PartialD; (x_{2}, y_{2}, z_{2})} \\ . \\ . \\ . \\ \sqrt{{(X_{N} - x_{0})}^{2} + {(Y_{N} - y_{0})}^{2} + {(Z_{N} - z_{0})}^{2}} + \frac{&PartialD; r_{N}}{&PartialD; (x_{N}, y_{N}, z_{N})} \end{matrix}) = G (\begin{matrix} x \\ y \\ z \end{matrix}) + g (x_{0}, y_{0}, z_{0})

…(式2)

将(式2)中的G称为观测矩阵。获取到的传感器信号的数目是行数，想要求取的未知变量的数目是列数。G的要素准备事前能够获取的所有行数份。G留下与获取到的传感器信号对应的要素，通过将0代入该其他要素，从而能够作成G。下面，转移至步骤905，求取观测矩阵的阶数(秩：rank)。观测矩阵的阶数能够通过对在步骤904中求出的观测矩阵G进行特异值分解或者QR分解来求取。这里，求出的阶数是独立的信号的数目。

然后，在步骤906中，消去重复的信号。在具有同一发送源的定位信号存在的情况下，即，在未知变量的数目比观测矩阵G的阶数更大的情况下，除算出同一发送源的定位信号，只留下各个相互独立的发送源的定位信号。例如，在观测矩阵G的要素中，从距离第1个定位信号的发送源的距离r1开始到距离第n个定位信号的发送源的距离信号rn为止，一个一个地，全部消去有关该定位信号的要素，计算矩阵的阶数，由于能够判断为阶数未变为n的定位信号是重复的发送源的定位信号，所以通过从观测矩阵G中将与该重复的发送源的定位信号对应的行全部消去，从而能够生成重复的发送源的定位信号不存在的观测矩阵G。

此外，在步骤907中，如果未知变量的数目即想要求取的位置的要素数目比独立的发送源的数目小，则设能够进行位置修正，在步骤908中，输出具有独立的发送源的定位信号的移动体的ID和该定位信号。这里，未知变量的数目是上述想要求取的位置的变量的数目，独立的发送源的定位信号的数目经过步骤906，是最终求出的G的列数。此外，如果未知变量的数目比独立发送源的定位信号的数目大，则设不能进行位置修正计算，在步骤909中输出该结果。

通过对所有的分组后的集团重复这些处理(步骤910)，从而针对属于各集团的移动体来判定是否能够计算位置修正信息、定位信号的独立性。

这样，作为通过图5所示的处理流程判定出的移动体间的关系性的结果，通过图3所示的处理流程的步骤305，将到此为止求出的各移动体属于的集团和独立的定位信号以及基线向量与移动体ID一起存储在移动体分类存储单元123中。

这样，在中心121中的移动体分类存储单元123中，按每个由关系性判定单元122判定出的集团，存储从移动体发送来的各移动体的位置/姿势、与其他移动体的相对位置信息、传感器信息中判定为是独立的发送源的定位信号的定位信号和与该定位信号对应的变量。

中心121的位置修正单元124，根据存储在移动体分类存储单元123中的信息，计算各移动体的位置，将与原本存储的移动体的位置之间的差分作为修正值，与其误差估计一起作为修正信息来进行计算，并存储在移动体位置修正信息存储单元126中。这里，在位置计算和误差估计中采用的变量设为存储在所有移动体分类存储单元123中的变量。作为该计算的例子，说明移动体-1以及移动体-2这两个移动体的位置计算时的例子。设移动体-1和移动体-2的位置分别为(x1，y1，z1)、(x2，y2，z2)。设移动体-1从第一信号发送源接收第一信号发送源和移动体-1间的距离信息r1，从第二信号发送源接收第二信号发送源和移动体-1间的距离信息r2。此外，同时，设移动体-2从第三信号发送源接收第三信号发送源和移动体-2间的距离信息r3，从第四信号发送源接收第四信号发送源和移动体-2间的距离信息r4。此外，设能在移动体-2测量到移动体-1为止的相对位置，移动体-2和移动体-1间的相对距离的值为L，移动体-1和移动体-2的基线向量为(dx，dy，dz)。此时，要求取移动体-1的位置(x1，y1，z1)以及移动体-2的位置(x2，y2，z2)，只要求解由下面(式3)表示的联立方程式即可。

【数3】

\{\begin{matrix} r_{1} = \sqrt{{(X_{1} - x_{1})}^{2} + {(Y_{1} - y_{1})}^{2} + {(Z_{1} - z_{1})}^{2}} \\ r_{2} = \sqrt{{(X_{2} - x_{1})}^{2} + {(Y_{2} - y_{1})}^{2} + {(Z_{2} - z_{1})}^{2}} \\ r_{3} = \sqrt{{(X_{3} - x_{2})}^{2} + {(Y_{3} - y_{2})}^{2} + {(Z_{3} - z_{2})}^{2}} \\ r_{4} = \sqrt{{(X_{4} - x_{2})}^{2} + {(Y_{4} - y_{2})}^{2} + {(Z_{4} - z_{2})}^{2}} \\ L = \sqrt{{(x_{2} - x_{1})}^{2} + {(y_{2} - y_{1})}^{2} + {(z_{2} - z_{1})}^{2}} \\ (\begin{matrix} x_{2} \\ y_{2} \\ z_{2} \end{matrix}) = (\begin{matrix} x_{1} \\ y_{1} \\ z_{1} \end{matrix}) + (\begin{matrix} dx \\ dy \\ dx \end{matrix}) \end{matrix}

…(式3)

此外，以某点(x0，y0，z0)为中心，通过将(式3)进行泰勒展开至1次项，从而能够如(式4)这样表示由(式3)计算出的位置的误差的估计。

【数4】

(\begin{matrix} r_{1} \\ r_{2} \\ r_{3} \\ r_{4} \\ L \\ dx \\ dy \\ dz \end{matrix}) = (\begin{matrix} \sqrt{{(X_{1} - x_{0})}^{2} + {(Y_{1} - y_{0})}^{2} + {(Z_{1} - z_{0})}^{2}} + \frac{&PartialD; r_{1}}{&PartialD; (x_{1}, y_{1}, z_{1})} \\ \sqrt{{(X_{2} - x_{0})}^{2} + {(Y_{2} - y_{0})}^{2} + {(Z_{2} - z_{0})}^{2}} + \frac{&PartialD; r_{2}}{&PartialD; (x_{1}, y_{1}, z_{1})} \\ \sqrt{{(X_{3} - x_{0})}^{2} + {(Y_{3} - y_{0})}^{2} + {(Z_{3} - z_{0})}^{2}} + \frac{&PartialD; r_{3}}{&PartialD; (x_{2}, y_{2}, z_{2})} \\ \sqrt{{(X_{4} - x_{0})}^{2} + {(Y_{4} - y_{0})}^{2} + {(Z_{4} - z_{0})}^{2}} + \frac{&PartialD; r_{4}}{&PartialD; (x_{2}, y_{2}, z_{2})} \\ \sqrt{{(x_{2} - x_{0})}^{2} + {(x_{2} - y_{0})}^{2} + {(x_{2} - z_{0})}^{2}} + \frac{&PartialD; L}{&PartialD; (x_{1}, x_{2}, x_{3})} \\ x_{2} - x_{0} + \frac{&PartialD; dx}{&PartialD; x_{1}} \\ y_{2} - y_{0} + \frac{&PartialD; dy}{&PartialD; x_{1}} \\ z_{2} - z_{0} + \frac{&PartialD; dz}{&PartialD; x_{1}} \end{matrix}) = G (\begin{matrix} x_{1} \\ y_{1} \\ z_{1} \\ x_{2} \\ y_{2} \\ z_{2} \end{matrix}) + g (x_{0}, y_{0}, z_{0})

…(式4)

称(式4)的G为观测矩阵，通过利用该观测矩阵G，按照σ＝(GG^T)-1这样来计算观测值的误差的方差σ，作为观测到的距离等的误差的方差。

通过在卡尔曼滤波器等概率模型滤波器中应用由该(式4)表示的观测矩阵G和上述误差的方差σ，从而能够求取由(式3)计算出的位置的误差的估计。

中心121的通信单元125向移动体递送由位置修正单元124计算并存储在移动体位置修正信息存储单元中的修正信息。中心121从通信单元125发送的修正信息，对于所有求出的移动体的位置的修正信息分配进行该修正的对象的移动体的移动体ID，在能够接收递送的各移动体中，根据分配给从中心121递送的修正信息的移动体ID，确定在位置修正中使用的修正信息，基于该修正信息，在移动体重新计算位置。

移动体将误差方差信息作为位置修正信息与修正值一起从中心接受，能够用于在移动体内部的位置推定时。这如例如专利文献3中记载的那样，在移动体的存在概率的计算中是必要的。

(实施例2)

在第一实施例中的位置推定系统中，将关系性判定单元设定在中心121中，但是作为第二实施例说明各移动体具有关系性判定单元的位置推定系统。在第二实施例中，在各移动体中具备关系性判定单元，能够由移动体局部地进行位置修正，所以在大城市等各种各样的车辆往来、并且与中心的通信也可能中断的情况下，能够提高位置精度。

第二实施例的位置推定系统与第一实施例同样地，通过组装入移动体的专用装置或者移动体中进行实现。与第一实施例的不同在于，由多个移动体构成，且不需要中心。图9示出移动体1101中的位置推定系统的结构。在移动体1101中，由以下构成：信号接收单元1102，其获取定位信号等传感器信息；位置姿势计算单元1103，其基于该传感器信息计算位置；距离测量单元1104，其测量与其他移动体的相对距离；通信单元1105，其将移动体的传感器信息和计算出的位置的信息进行通信；位置修正单元1106，其计算位置修正信息；关系性判定单元1107，其根据能否测量由移动体1101自身得到的、或者从其他移动体得到的相对距离来判定与其他移动体的联系等；内部存储单元1108，其设置存储由位置修正单元1106计算出的位置修正信息的位置修正信息存储区域、存储获取到的传感器信息和测量结果的测量信息存储区域、存储发送中途的值的中断信息存储区域、存储移动体的集团的移动体分类存储区域、以及存储其他移动体的信息的移动体信息存储区域；特征识别单元1109，其识别由距离测量单元1104进行了测量的移动体的特征。

其中，信号接收单元1102、位置姿势计算单元1103、距离测量单元1104、和特征识别单元1109由于分别与第一实施例的信号接收单元102、位置姿势计算单元103、距离测量单元104、特征识别单元107相同，所以省略说明。

此外，设各移动体的信号接收单元1102都具备接收GPS的定位信号的功能，由各个移动体管理的时刻通过GPS时刻而被同步。

下面，图10示出移动体的位置推定系统的处理流程。在以下的说明中，以移动体1101的动作来进行处理的说明。首先，在移动体1101中，由信号接收单元1102获取按规定的周期从传感器等得到的定位信号等传感器信息。此外，特征识别单元1109扫描移动体1101的周围，与第一实施例的情况相同，检索其他移动体的特征，与相对移动体1101的方向一起进行识别，求取特征量。然后，由距离测量单元1104测量到识别出的特征为止的相对距离(步骤1201)。

下面，在步骤1202中，将在步骤1201中获取到的传感器信息以及测量出的相对距离和识别出的特征量作为测量信息存储在内部存储单元1108的测量信息存储区域中。此时，在测量信息存储区域中，即使留有上次存储的信息，也将其覆盖。

下面，在步骤1203中，确认在内部存储单元1108的位置修正信息存储区域中是否存在有位置修正信息，如果存在，则转移至步骤1204，采用该位置修正信息来修正当前的位置。如果位置修正信息不存在，则转移至步骤1205。在步骤1205中，判断是否能够采用在步骤1202中存储的测量信息进行位置/姿势的计算。如果能够计算，则在步骤1206中计算位置/姿势。如果不能计算，则转移至步骤1207。

在步骤1207中判断是否能够与其他移动体进行通信。能否与其他移动体进行通信的判断是如下判断，即，在由通信单元1105仅输出应答请求的信号、并且在某固定时间内从其他移动体有应答的情况下，判断为能够进行通信，在没有应答的情况下，判断为不能进行通信。此外，在从其他移动体存在应答请求的通信的情况下，在该步骤中返回发给请求源的应答。在不能与其他移动体进行通信的情况下，转移至步骤1218，直到下一次测量信息获取为止都待机。

如果与其他移动体能够进行通信，则在步骤1208中，判断在内部存储单元1108的中断信息存储区域中是否存在有上次处理中途保留的信息。在不存在的情况下，在步骤1209中，向其他移动体发送在步骤1202中存储的传感器等的信息以及本移动体的移动体ID、与在识别其他移动体时识别出的其他移动体的相对距离和特征量以及获取它们的时刻。时刻如前所述采用GPS时刻。

在步骤1210中，接收其他移动体在步骤1209中送出的、传感器等的信息以及移动体ID、与识别出的其他移动体之间的相对距离和特征量以及获取他们的时刻。下面，在步骤1211中，在内部存储单元1108的移动体信息存储区域中存储从其他移动体接收到的传感器信息及其时刻、与在周围识别出的与移动体的相对距离及其特征量等信息。此外，在该时刻，删除经过某固定时间以上的其他移动体的信息。此外，在存在具有同一移动体ID的信息的情况下，将该信息覆盖。

下面，在步骤1212中，判断来自其他移动体的信息是否存储在内部存储单元1108的移动体信息存储区域中。在没有存储来自其他移动体的信息的情况下，即在固定时间期间不能接收来自其他移动体的信息、或在接收到的信息中未凑齐其他移动体的位置信息以及特征量双方的情况下，转移至步骤1216，在内部存储单元1108中的位置修正信息存储区域中，存储为“无位置修正信息”。

在存储了来自其他移动体的信息的情况下，转移至步骤1213，通过关系性判定单元1107判定与接收到信息的移动体之间的关系性，针对判断为有关联的移动体，求取在接收到的信息中包含的定位信号之内、独立的发送源的定位信号的数目和位置修正所需的未知变量的数目。然后，对求出的独立的发送源的定位信号的数目和位置的修正所需的未知变量的数目进行比较，在独立的发送源的定位信号的数目比为了移动体的位置修正所需的未知变量的数目少的情况下，判定为不能计算位置修正信息，此外，在求出的独立的发送源的定位信号的数目比为了移动体的位置修正所需的未知变量的数目多或者是相同数目的情况下，判定为能够进行位置修正信息的计算。

下面，在步骤1214中，判断是否能够根据步骤1213中的判定结果来计算位置修正信息，如果不能，则转移至步骤1216，在内部存储单元1108的位置修正信息存储区域中，存储为“无位置修正信息”。如果能够计算位置修正信息，则转移至步骤1215，计算位置修正信息。如果在步骤1215中计算出位置修正信息，则在步骤1216中在内部存储单元1108的位置修正信息存储区域中存储位置修正信息。然后，在步骤1218中直到下一次测量信息获取为止都待机，并重复从步骤1201开始的处理。

由于以这样保存的位置修正信息为基础，在下一次处理时在步骤1204的处理中修正位置，所以能够提高定位精度。其中，在步骤1207以后，如果下一次的测量信息获取的机会来临，则需要插入并随时保留该时刻的处理，将留下的处理的信息存储在内部存储单元1108的中断信息存储区域中，并返回步骤1201。然后，当再次将处理转移至步骤1208时，如果在内部存储单元1108的中断信息存储区域中具有保留信息，则在步骤1217中从最后保留了处理的地方再次开始处理，并返回在内部存储单1108的中断信息存储区域中存储的信息的处理。其他移动体的位置推定系统也与上述处理同样地进行工作。

下面，说明各单元。移动体1101中的信号接收单元1102、位置姿势计算单元1103、距离测量单元1104、特征识别单元1109如前所述，与第一实施例中的信号接收单元102、位置姿势计算单元103、距离测量单元104、特征识别单元107相同。

此外，内部存储单元1108与第一实施例的内部存储单元108对应，进一步地，与第一实施例中的设置在中心的移动体分类存储单元123和移动体位置修正信息存储单元126、移动体信息存储单元127对应，在内部存储单元1108中分别设置移动体分类存储区域、位置修正信息存储区域、移动体信息存储区域，以下的关系性判定单元1107的说明，与在第一实施例中的关系性判定单元122以及与此关联的连锁判定单元402、信号独立性判定单元403、基线向量计算单元404的动作的说明中，将移动体分类存储单元123、移动体位置修正信息存储单元126、移动体信息存储单元127分别替换为移动体分类存储区域、位置修正信息存储区域、移动体信息存储区域的说明相同。

在移动体1101中的通信单元1105中，向其他移动体发送由信号接收单元1102、位置姿势计算单元1103、距离测量单元1104得到的信息。此外，也能够接受从其他移动体送来的信息。

在移动体1101中的位置修正信息存储区域中，存储在上述步骤1215中求出的位置修正信息。这里存储的信息在步骤1204中进行位置的修正之后删除。

在移动体1101中的关系性判定单元1107中，通过判定多个移动体的关系性，从而将具有关联的移动体分类为汇总的集团。

关系性判定单元1107的结构与第一实施例中的关系性判定单元122相同，其处理流程也与图5所示的处理相同。其中，在连锁判定单元的处理中，在步骤717中，将在步骤716分组后得到的结果保存在内部存储单元1108中的移动体分类存储区域这一点与第一实施例不同。

此外，在步骤1213中的关系性判定单元1107的处理时计算出的各移动体的组和独立信号以及基线向量存储在内部存储单元1108中的移动体分类存储区域中。并且，内部存储单元1108中的移动体分类存储区域，按每个由关系性判定单元1107判定出的集团，存储判定为是由移动体发送的位置姿势、距离信息、传感器信息中的独立的信号的信号和表示该信号的未知变量。

位置修正单元1106根据存储在内部存储单元1108的移动体分类存储区域中的信息，计算各移动体的位置，将与原本存储的位置之间的差分作为修正值，与该误差估计一起进行计算。此时的计算方法与第一实施例相同。

下面，使用图11说明具体的例子。在该例子中，假设车辆为移动体，在信号接收单元1102中搭载知道GPS的接收时刻的GPS接收机，设发送源为GPS卫星，定位信号为GPS信号。现在，设在第一车辆1301中，在某时刻h1捕捉到了来自第一GPS卫星1302以及第二GPS卫星1303的GPS信号。此外，同样地，设在搭载了知道GPS的接收时刻的GPS接收机的第二车辆1311中，在某时刻h2捕捉到了来自第三GPS卫星1312以及第四GPS卫星1313的GPS信号。此外，设在第二车辆1311中，通过立体照相机在与其光轴1314偏离了某相对角度的线1315上的距离L的场所，观测第一车辆1301，识别该车号牌(number plate)的车体编号。设接收从第一GPS卫星1303到第一车辆1301的距离信息r1和从第二GPS卫星1303到第一车辆1301的距离信息r2。此外，设第二车辆1311接收从第三GPS卫星1312到第二车辆1311的距离信息r3和从第四GPS卫星1313到第二车辆1311的距离信息r4。

在该状态下，第一车辆1301和第二车辆1311不能分别独立推定位置。因此，通过通信单元1105的车车间通信，第一车辆1301和第二车辆1311相互将包含各个GPS接收时刻在内的GPS接收信息以及识别出的车体编号进行通信。然后，第一车辆1301以及第二车辆1311的关系性判定单元1107起动，首先，通过各自的基线向量计算单元，根据第二车辆1311通过立体照相机测量出的相对距离来计算基线向量。

下面，在第一车辆1301的关系性判定单元中，确认从第二车辆1311作为特征量接收到的识别对象车辆的车体编号和第一车辆1301作为数据具有的车体编号一致，识别为第二车辆1311属于相同的组。同样地，第二车辆1311的关系性判定单元确认由立体照相机进行了测量的车辆的车体编号和第一车辆1301的车体编号一致，识别为第一车辆1301是属于相同组的车辆。下面，根据由各个车辆接收到的GPS信号的PN码，能够判断从第一GPS卫星1302到第四GPS卫星1313全部都是独立的卫星。根据这些结果，通过求解以下(式5)的联立方程式，求取各个车辆的位置，将该值作为位置修正信息进行存储。

【数5】

\{\begin{matrix} r_{1} = \sqrt{{(X_{1} - x_{1})}^{2} + {(Y_{1} - y_{1})}^{2} + {(Z_{1} - z_{1})}^{2}} + s_{1} \\ r_{2} = \sqrt{{(X_{2} - x_{1})}^{2} + {(Y_{2} - y_{1})}^{2} + {(Z_{2} - z_{1})}^{2}} + s_{1} \\ r_{3} = \sqrt{{(X_{3} - x_{2})}^{2} + {(Y_{3} - y_{2})}^{2} + {(Z_{3} - z_{2})}^{2}} + s_{2} \\ r_{4} = \sqrt{{(X_{4} - x_{2})}^{2} + {(Y_{4} - y_{2})}^{2} + {(Z_{4} - z_{2})}^{2}} + s_{2} \\ L = \sqrt{{(x_{2} - x_{1})}^{2} + {(y_{2} - y_{1})}^{2} + {(z_{2} - z_{1})}^{2}} \\ (\begin{matrix} x_{2} \\ y_{2} \\ z_{2} \end{matrix}) = (\begin{matrix} x_{1} \\ y_{1} \\ z_{1} \end{matrix}) + (\begin{matrix} dx \\ dy \\ dz \end{matrix}) \\ dx = L \cos θ \\ dy = L \sin θ \\ dz = L \sin ψ \\ s_{1} - s_{2} = c (h_{1} - h_{2}) \end{matrix}

…(式5)

这里，设θ表示与立体照相机的光轴之间在水平方向的偏离角，ψ是与立体照相机的光轴之间在垂直方向的偏离角，c表示光速。

这样，即使在采用个体时GPS卫星不足而不能进行定位的情况下，也能够通过采用本装置来计算位置，能够增加定位精度。

Claims

1.一种位置推定系统，由多个移动体构成，

所述移动体包括：

定位单元，其接收来自多个发送源的定位信号，计算位置并一起计算其误差估计；以及

距离测量单元，其测量与其他移动体的距离，

所述移动体具有：

特征识别单元，其识别通过所述距离测量单元测量了距离的其他移动体的特征；

通信单元，其向其他移动体或中心发送接收到的定位信号以及计算出的位置信息，并接收位置修正信息；以及

位置修正信息存储单元，其存储接收到的位置修正信息，

在多个移动体之内的至少一台或所述中心中包括：

关系性判定单元，其判断测量了相对位置的移动体间的关系性，选择在由关联的其他移动体接收到的定位信号之中来自与由该移动体接收到的定位信号的发送源不同的发送源的定位信号；以及

位置修正单元，其基于由该移动体接收到的定位信号、和在所述关系性判定单元中选择出的由其他移动体接收到的定位信号，计算当前位置，并计算位置修正信息，以该位置修正信息为基础对推定位置进行修正。

2.根据权利要求1所述的位置推定系统，其特征在于，

所述关系性判定单元具有：

连锁判定单元，其判定是由所述距离测量单元对其他移动体进行了直接测量、还是自移动体从其他移动体进行了测量；

信号独立性判定单元，其判定与接收到的定位信号以及通过所述通信单元得到的其他移动体接收到的定位信号之间的发送源的独立性；以及

基线向量计算单元，其采用移动体的位置信息，根据通过所述距离测量单元测量出的与其他移动体的相对位置信息来计算基线向量。

3.根据权利要求2所述的位置推定系统，其特征在于，

所述连锁判定单元，将通过所述距离测量单元针对多个移动体测量了直接距离的多个移动体分类为一个集团。

4.根据权利要求2所述的位置推定系统，其特征在于，

所述信号独立性判定单元从通过所述信号接收单元接收到的定位信号、和在所述连锁判定单元中由分类为相同集团的各移动体接收到的定位信号中，判别定位信号的发送源独立的定位信号并进行选择。

5.根据权利要求4所述的位置推定系统，其特征在于，

所述基线向量计算单元在通过所述信号独立性判定单元选择出的独立的定位信号的数目为定位所需的数目以上的情况下，计算所述基线向量，从而计算移动体的位置。

6.根据权利要求4所述的位置推定系统，其特征在于，

所述信号独立性判定单元，在由权利要求3的连锁判定单元判断为一个集团的移动体的集团中，对于在集团整体中的未知变量，判定有在该集团整体的未知变量的个数以上的、独立的发送源的定位信号的观测。