CN1136841A - 移动体的位置测量装置 - Google Patents

Abstract

本发明的第1个目的是根据航位绘算航法修正导航行驶的移动体环绕前后的偏移、进行高精确度的导航行驶。第2个目的是根据航位绘算航法简易地求得导航行驶的初始位置和初始方向。

当移动体1在行驶路线L3上行驶时，紧邻在行驶路线L3的起始地点P3之后的2个基准点R11、R12的位置作为坐标轴X-Y上的位置被运算。另外，当移动体1环绕行驶路线L3回到起始地点之后再继续行驶、上述2个基准点的位置R11、R12作为R′11、R′12再次被运算后作为坐标轴X-Y上的位置。并且，根据这些被运算的2个基准点的位置和移动体1环绕行驶路线L3又回到起始地点PS的2个推断位置及推断方向Pe(Xe、Ye、θe)来修正移动体1环绕了行驶路线L3时产生的位置和行驶方向的偏移。

在移动体1通过预定行驶路线上的第1基准点R01时根据由推断运算装置输出的推断位置(Xte01、Yte01)和对于由相对位置检测装置输出的第1基准点的移动体1的相对位置来运算第1基准点R01的位置(Xr01、Yr01)。另外，在移动体1通过了预定行驶路线上的第2基准点R02时根据由推断运算装置输出的推断装置(Xte02、Yte02)和对于由相对位置检测装置输出的第2基准点R02的移动体1的相对位置来运算第2基准点R02的位置(Xr02、Yr02)。并且，根据既定坐标轴Xte-Yte上的第1和第2基准点的位置(Xr01、Yr01)、(Xr02、Yr02)，设定与既定坐标轴不同的新坐标轴X-Y。既定坐标轴Xte-Yte上的位置和行驶方向(Xte、Yte、θte)被变换为新设定的坐标轴X-Y上的位置和行驶方向(X、Y、θ)，并认为是坐标轴X-Y上的初始位置和初始方向。

Description

移动体的位置测量装置

本发明涉及用航位绘算航法进行制导行驶的移动体的位置测量装置。

以往，作为沿着既定的预定移动路线使移动体导航行驶的方法，一般有叫做航位绘算航法的导航方法，这是根据移动距离传感器和方位传感器的输出推断运算移动体的位置和行驶方向、在该推断运算结果的基础上掌舵控制移动体的行驶，使移动体应通过予先示教了的预定行驶路线上的通过予定地点。

在这种情况下，作为上述通过预定地点的数据(航线数据)的测量方法有以下所列举的绝对位置测量方式(a)和相对位置测量方式(b)。

(a)在绝对位置测量方式中，通过测量移动体应行驶的预定行驶路线上的各位置，来求绝对位置(例如以纬度、经度为基准)，把其作为导航行驶控制时的航线数据给出。

(b)在相对位置测量方式中，在示教时，使用与再现(重演)时相同的位置测量装置沿着预定行驶路线使移动体移动，取得航线数据。

再者，上述(b)的相对位置测量方式与本申请人的申请有关，这已在众所周知的以前申请(特愿平3-148921号)中明确指出。

在比较上述各方式(a)、(b)时，(b)的相对位置测量方式与(a)的绝对位置测量方式相比，有以下优点。

1)航线数据输入容易。

2)由于在航线数据测量(示教时)和导航时(重演时)可以使用同一位置测量装置，因而共同的误差因素(偏差)被抵消，能获得更精确的高导航精度。

3)即使是曲折行驶和急转弯等复杂的航线，在不改变通常的航线情况下很容易取得航线数据。

4)由于实际上一边使移动体移动一边测量，所以能够取得适合于移动体的运动性能的行驶路线的数据。

5)假定，即使位置测量装置是二维测量用的，由于在航线数据测量时(示教时)和导航时(重演时)有再现性，实际上移动即使达到三维，也能进行高精确的导航。

即使是有这么多优点的相对位置测量方式，但在重复闭环环绕航线进行环绕移动的情况下，环绕前的起始地点和环绕后的同一起始地点，由于在位置和方向上产生偏移，因此，必须对其进行修正。

作为这一修正方法，在上述的特愿平3-148921号(公报)中公布了在转圈前和转圈后2次测量紧邻起始地点的第1和第2这2个基点(反射镜)的位置，并根据这2次测量的2个基准点位置的偏移，修正转圈(环绕)后又回到起始地点时的推断位置和推断方向的偏移的技术。

此处，为记述移动逐次的推断位置和推断方向，通过座标变换运算装置，根据航线上第1和第2这2个基准点的位置来设定座标轴是很方便的。

因此，当移动体反复行驶在包含上述第1和第2基准点的预定行驶路线上时，由于所述的第1和第2基准点位置通过上述座标变换运算装置作为新设定的座标轴上的位置已被存贮起来，因此，通过在示教中从起始点到起始点行驶1圈连续2次测量第1和第2的基准点，就能够修正所述的偏移量。

然而，由于航线设计方案不同，也有移动体行驶在不包含上述第1和第2基准点的其他行驶路线上的情况。

即使是这种情况，也有必要根据基于上述第1和第2基准点的同一座标轴来记述推测位置和推测方向。但是，由于在这一行驶路线上不存在第1和第2基准点，如果只是从这一行驶路线的起点又回到起点行驶一圈的示教(训练)，就不能在2次测量同一基准点，因而不能修正上述偏移。

如果是以往技术，由于没有解决这个问题的方法，不得已只好使用绝对位置测量方式。

本发明的第1发明就是鉴于这样的实际情况而形成的，旨在提供即使是在没有共同的2个基准点的不同的多个行驶路线上行驶情况下，也能修正在行驶路线上转圈前后的位置和行驶方向的偏移，从而通过使用相对位置测量方式能够对移动体进行高精度的导航行驶的装置。

在根据航位绘算航法的导航方法中，假定移动距离传感器在时刻t的输出为d1(t)，方位传感器在时刻t的输出为θ(t)，若时刻t0的初期位置为(X0，Y0)，则在移动体的时刻t的2维推断位置(X(t)、Y(t))像下式(8)、(9)那样表示。 $x\left(t\right)={\∫}_{t0}^t\cos \mathrm{\θ}\left(t\right)\mathrm{dl}\left(t\right)+X0---\left(8\right)$ $y\left(t\right)={\∫}_{t0}^t\sin \mathrm{\θ}\left(t\right)\mathrm{dl}\left(t\right)+Y0---\left(9\right)$

式中，作为方位传感器例如若使用角速度传感器，则它的输出为角速度ω(t)，若初始方位为Θ0，则方位θ(t)作为对角速度ω(t)积分后的值表示如下。 $\mathrm{\θ}\left(t\right)={\∫}_{t0}^t\mathrm{\ω}\left(t\right)\mathrm{dt}+\mathrm{\Θ}0---\left(10\right)$

如从上述(8)-(10)式所知，为通过运算求出移动体的位置和方向，需要初始值X0，Y0，Θ0。而且，为给出这样的初始值，必须设定既定的座标，即座标轴，记述初始值。

作为设定这样的座标系，在以住有在以下(c)-(e)中列举的方法。

(c)用三角测量来测量初始位置的方法。

亦即，首先在移动体移动场地规定任意的座标轴。而且，以该座标系内的基准点为基准，利用三角测量原理，使用光学测量装置等来测量移动体的初始位置。

但是，这种技术需要大规模的初始值测量专用系统，在花费成本的同时，需要人力，操作麻烦。

并且，根据该技术，要正确地求出上述(10)式所表示的初始方位Θ0是困难的。而且，若初始方位Θ0的测量精度不高，则如(8)、(9)式所示，就会对位置测量精度产生影响，因而，位置测量精确度也会变差。

(d)另外，还有通过上述(c)的三角测量使移动体在预先测量的位置上移动的方法。

但是，随着移动体不同，有时难以正确地使其停止在指定的位置。正确地停止在指定的方向就更难。因此，该方法也会引起精确度变坏。

(e)此外，还有利用GPS(全球定位系统)的方法。

但是，为要进行高精确度的测量，就需要昂贵的GPS系统。而且，座标系受GPS的座标系的限制，也会产生受到装置设计方面制约的不方便的情况。另外，还存在即使能求出初始位置，但不能求出初始方位Θ0的问题。

本发明的第2发明就是鉴于这样的实际情况而形成的，旨在提供能够简易而且低成本地设计移动体的座标轴、不仅能正确地记述初始位置而且能正确地记述初始方向的装置。

因此，如果是本发明的第1发明，则移动体的位置测量装置具有在既定的座标轴上推断运算移动体的位置和行驶方向的推断运算装置，同时，具有检测对沿着上述移动体的预定行驶路线间断配设的对于基准点的上述移动体的相对位置的相对位置检测装置，并根据上述相对位置检测装置的输出和上述基准点的位置间断地修正通过上述推断运算装置所推断运算的移动体的位置和行驶方向，根据该被修正的位置和行驶方向沿着所述的预定行驶路线使上述的移动体行驶。在该装置中，上述的预定行驶路线包含具有紧邻起始地点之前的第1和第2基准点的闭环第1行驶路线和不具有所述的第1和第2基准点的闭环第2行驶路线，并具有基准点位置运算装置、座标变换运算装置、第1偏移运算装置、第1修正装置、第2偏移运算装置以及第2修正装置。其中基准点位置运算装置的功能是：在上述的移动体通过第2行驶路线上的第1基准点时根据由上述推断运算装置输出的推断位置，以及相对于由上述的相对位置检测检测装置输出的上述第1基准点的上述移动体的相对位置来运算上述的第1基准点位置，同时在上述的移动体通过预定的行驶路线上的第2基准点时根据由上述推断运算装置输出的推断位置和相对于从上述相对位置检测装置输出的上述第2基准点的上述移动体的相对位置来运算上述第2基准点位置；座标变换运算装置的功能是：根据用上述基准点位置运算装置所运算的、在既定座标轴上的第1和第2基准点位置设定与上述既定座标轴不同的新座标轴，进行将从上述推断运算装置输出的上述既定座标轴上的位置和行驶方向变换为上述新设定的座标轴上的位置和行驶方向的运算，并将此变换运算的位置和行驶方向设定为由上述推断运算装置进行的推断运算的初始值；第1偏移运算装置的功能是：在所述的座标变换运算装置中，将上述的第1和第2基准点位置作为上述的新设定的座标轴上的位置存贮起来，同时当上述移动体在第1行驶路线上环绕一周后即将回到上述起始地点之前通过上述基准点位置运算装置再次运算上述第1和第2基准点位置，根据这些在上述座标变换运算装置中所存贮的第1和第2基准点位置以及上述移动体沿着第1行驶路线环绕一周后在即将回到起始地点之前再次运算的第1和第2基准点位置，运算上述移动体沿第1行驶路线环绕一周后的时候所产生的基准点的偏移；第1修正装置的功能是：根据用上述第1偏移运算装置运算的偏移，每当上述移动体沿第1行驶路线环绕一周后回到起始地点时，就修正上述移动体的位置和行驶方向；第2偏移运算装置的功能是：当上述移动体在第2行驶路线上行驶时，将紧邻在第2行驶路线的上述起始地点之后2个基准点位置通过上述基准点位置运算装置作为上述第1行驶路线上在上述座标变换运算装置中所设定的座标轴上的位置进行运算，同时，上述移动体在第2行驶路线上环绕又回到上述起始地点后，再继续行驶通过上述基准点位置运算装置对上述2个基准点位置再次进行运算，根据这些刚刚在第2行驶路线上开始环绕之后所运算的上述2个基准点位置和上述移动体在第2行驶路线上环绕又回到上述起始地点后再次被运算的上述2个基准点位置，运算上述移动体环绕第2行驶路线时所产生的基准点偏移；第2修正装置的功能是：根据由上述第2偏移运算装置所运算的偏移，每当上述移动体环绕第2行驶路线又回到上述起始地点时，就对上述移动体的位置和行驶方向进行修正。

根据这样的第1发明的构成，预定行驶路线包含具有为设定在起始地点附近的座标轴而在座标变换运算装置中使用的第1和第2基准点的闭环第1行驶路线和不具有上述第1和第2基准点的闭环第2行驶路线。

当移动体在第1行驶路线上行驶时，情况如下。首先，在移动体通过第1行驶路线上的第1基准点时，根据从推断运算装置输出的推断位置和相对于从相对位置检测装置输出的第1基准点的移动体的相对位置，通过基准点位置运算装置运算第1基准点位置。另外，当移动体通过预定行驶路线上的第2基准点时，根据从推断运算装置输出的推断位置和相对于从相对位置检测装置输出的第2基准点的移动体的相对位置通过基准点位置运算装置运算第2基准点位置。

并且，根据通过基准点位置运算装置所运算的在既定座标轴上的第1和第2基准点位置设定与既定座标轴不同的新座标轴，进行将自推断运算装置输出的、在既定座标轴上的位置和行驶方向变换为在新设定的座标轴上的位置和行驶方向的运算，并将它们作为初始值进行推断运算。

然后，在座标变换运算装置中，第1和第2基准点位置作为新设定的座标轴上的位置被存贮起来。另外，通过基准点位置运算装置，在移动体环绕第1行驶路线后即将返回到上述起始地点之前再次对第1和第2基准点位置进行运算。并且根据这些被运算的第1和第2基准点位置和移动体环绕第1行驶路线后返回到起始地点时再次被运算的第1和第2基准点位置，通过第1偏移运算装置对移动体环绕第1行驶路线时产生的基准点的偏移进行运算。

根据这样被运算的偏移，每当移动体环绕第1行驶路线后返回起始地点时，移动体的位置和行驶方向就被修正。

另外，移动体在第2行驶路线上行驶时的情况如下。

首先，当移动体在第2行驶路线上行驶时，使用与在第1行驶路线中相同的座标轴，通过基准点位置运算装置，紧邻在第2行驶路线起始地点之后的另外2个基准点位置作为上述座标轴上的位置被运算。另外，通过基准点位置运算装置，在移动体环绕第2行驶路线后又返回到起动地点后再继续行驶，上述2个基准点位置作为上述座标轴上的位置再次被运算。然后，根据这些在紧邻起始(地点)之后运算的2个基准点位置和移动体环绕第2行驶路线并回到起始地点后再次运算的上述2个基准点位置运算移动体在环绕第2行驶路线时产生的基准点的偏移。

根据这样被运算的偏移，每当移动体环绕第2行驶路线并返回到起始地点时，移动体的位置和行驶方向就被修正。

另外，如果是本发明的第2发明，移动体的位置测量装置具有推断运算在既定座标轴上移动体的位置和行驶方向的推断运算置，同时具检测相对于沿着上述移动体的预定行驶路线间断地配设的基准点的上述移动体的相对位置的相对位置检测装置，该装置还具有基准点位置运算装置和座标变换运算装置，能做到用上述推断运算装置将由上述座标变换运算装置变换的位置和行驶方向作为初始值进行推断运算。其中，基准点位置运算装置的功能是：在移动体通过预定行驶路线上的第1基准点时根据自上述推断运算装置输出的推断位置和相对于自上述相对位置检测装置输出的所述第1基准点的上述移动体的相对位置运算上述第1基准点的位置，同时在上述移动体通过预定行驶路线上的第2基准点时根据自上述推断运算装置输出的推断位置和相对于自上述相对位置检测装置输出的上述第2基准点的上述移动体的相对位置运算上述第2基准点的位置；座标变换运算装置的功能是：根据由上述基准点位置运算装置运算的、在既定座标轴上的第1和第2基准点位置，设定不同用于上述既定座标轴的新座标轴，进行将自上述推断运算装置输出的、上述既定座标轴上的位置和行驶方向变换为上述新设定的座标轴上的位置和行驶方向的运算。

根据这样的第2发明的构成，在移动体通过预定行驶路线上的第1基准点时根据自推断运算装置输出的推断位置和相对于自相对位置检测装置输出的第1基准点的移动体的相对位置运算第1基准点位置。另外，在移动体通过预定行驶路线上的第2基准点时根据自推断运算装置输出的推断位置和相对于位置检测装置输出的第2基准点的移动体的相对位置运算第2基准点的位置。

即，原来根据通过相对位置检测装置所得到的相对位置和已知的基准点位置求出移动体的正确位置，由此，推断位置应该被修正，但在此处，可以说是以对原来运算进行逆运算的形式运算基准点的位置。

而且，根据通过基准点位置运算装置运算的既定座标轴上的第1和第2基准点位置设定不同于既定座标轴的新座标轴。例如，假定以第2基准点位置为原点，使连接第1基准点和第2基准点的线段为X轴。而且，自推断运算装置输出的、既定座标轴上的位置和行驶方向被变换为新设定的座标轴上的位置和行驶方向，初始位置和初始方向就能通过新设定的座标轴正确地被记述。

在这种场合，不费人力、低成本而且简易地就能取得初始值。

图1是俯视而获得的有关本发明移动体的位置测量装置的第1实施例中移动体行驶状态的平面图。

图2是表示在第1实施例中处理程序的流程图。

图3是俯视而获得的在第1实施例和第2实施例中移动体行驶状态的平面图。

图4是表示实施例装置的构成的方框图。

图5是用X-Y座标系表示根据航位绘算航法的移动体行驶状态的示意图。

以下，参照平面图说明有关本发明移动体的位置测量装置的实施例。

第1实施例

图1是俯视而获得的移动体1沿预定行驶路线移动状态的二维平面图，移动体1有选择性地在闭环第1行驶路线L1和相同的闭环第2行驶路线L2以及相同的闭环第3行驶路线L3的3条预定行驶路线上行驶。

移动体1是根据航位绘算航法在预定行驶路线L1-L3上行驶的，根据与沿着预定行驶路线间断配设的基准点(反射镜)的相对位置不断抵消伴随航位绘算航法的积累误差继续行驶。

此处，航线设计方案是预定行驶路线L1，L2，L3具有共同的起始地点Ps，但是在预定行驶路线L1，L2的起始地点Ps跟前2个相邻基准点R01，R02不包含在行驶路线L3中。

为了以上述的特愿平3-148921号那样的示教并再现的方式使移动体1行驶，首先，必须设定记述在原来导航行驶时(再现时)和再现之前所进行的示教行驶时其行驶位置和行驶方向的座标轴。

下面，参照图2所示的流程图进行说明。

用于决定座标系的测量行驶(步骤101)

首先就有关在示教和再现之前所进行的座标轴的设定处理进行说明。

图4是表示构成实施例装置的方框图。

首先，如图3所示，例如移动体1停止在行驶路线L1，L2的起始地点Ps跟前距离相邻2个基准点R01，R02更近的既定位置。并且，假定将这个停止位置确定为(Xote，Yote，Θote)。换言之这与规定临时座标轴Xte-Yte是相同的。

因此，使移动体1从这个位置(Xote，Yote，Θote)开始移动。于是，在推断运算部分2将位置(Xote，Yote，Θote)作为初始值，并根据航位绘算航法像下述(1)-(3)式那样推断运算移动体1的位置和行驶方向。 $\mathrm{xte}\left(t\right)={\∫}_{t0}^t\cos \mathrm{\θ}\left(t\right)\mathrm{dl}\left(t\right)+X0\mathrm{te}---\left(1\right)$ $\mathrm{yte}\left(t\right)={\∫}_{t0}^t\sin \mathrm{\θ}\left(t\right)\mathrm{dl}\left(t\right)+Y0\mathrm{te}---\left(2\right)$ $\mathrm{\θte}\left(t\right)={\∫}_{t0}^t\mathrm{\ω}\left(t\right)\mathrm{dt}+\mathrm{\Θ}0\mathrm{te}---\left(3\right)$

然后，假定现在移动体1通过了起始地点跟前2个基准点中的第1基准点R01附近，则在相对位置检测部分3移动体1对于第1基准点R01的相对位置(dX01，dY01)被检测、运算后将它输入到基准点位置运算部分4中。再者，作为相对位置的检测方法，可以考虑这样的方法，该方法是，如前面所出现的特愿平3-148921号中所示的那样，根据从2个投射角度不同的投/受光器中的一个投光器所投射的光被沿着预定行驶路线配设的基准点即反射镜所反射后用一个受光器受光到由另一个投光器所投射的光、被同样的反射镜反射后被另一受光器受光为止的移动距离进行运算。但是作为本发明中检测相对位置的方法，不限于上述的以前申请中所指出的方法，也可以是任何其它方法。

而且，如果通过第1基准点R01时的推断位置为(Xte01，Yte01)，则在推断运算部分2被运算后将其输入到基准点位置运算部分4。不久，移动体1通过了第2基准点R02时，来自在推断运算部分2和相对检测部分3的同样的输出也输入到基准点位置运算部分4。

根据下述(4)式，第1基准点R01的位置(Xr01，Yr01)和第2基准点R02的位置(Xr02，Yr02)将在基准点位置运算部分4被运算。

Xr0i＝xte0i-dX0i

Yr0i＝yte0i-dY0i(i＝1、2)…(4)

基准点位置运算部分4的运算值被加到座标变换运算部分5中。

在座标变换运算部分5中，根据被加入的2个基准点R01，R02的位置，设定座标轴X-Y。这是因为如果知道成为2个基准的位置，则方向和位置被唯一地决定，由此能决定这二维座标轴。例如，假定将第2基准点R02的位置(Xr02，Yr02)作为原点，将第1基准点R01的位置(Xr01，Yr01)和第2基准点R02的位置(Xr02，Yr02)连接起来的线段作为X轴，就可以设定座标轴X-Y。

接下来，将临时设定的座标轴Xte-Yte上的移动体1的位置和行驶方向(Xte，Xte，θte)从推断运算部分2加到座标变换运算部分5后，如下述(5)式那样座标变换到上述新设定的座标轴X-Y上的位置和行驶方向(X、Y、θ)。

dθ0＝tan-1{(Yr02-Yr01)/(Xr02-Yr01)}

x＝(xte-Xr02)cos(dθ0)+(yte-Yr02)sin(dθ0)

y＝-(xte-Xr02)sin(dθ0)+(yte-Yr02)cos(dθ0)

θ＝θte-dθ0

…(5)

示教/再现行驶(步骤102)

如果像以上那样，设定新座标轴X-Y，移动体1的导航行驶的示教和再现就开始了。即，将座标轴Xte-Yte上的位置和方向变换成座标轴X-Y上的位置和方向作为初始值，以后，可以正确地求出移动体1的任意位置X，Y和行驶方向θ作为座标轴X-Y上的值。从而，使移动体1能够高精确度地沿预定行驶路线行驶。此处，如果使基准点R01和R02的间隔取得足够大，初始方位就能正确地求出。

再者，虽然以第2基准点R02的位置为原点，将连接第1基准点R01和第2基准点R02的线段作为X轴，但也可以以第1基准点R01位置为原点，将连接第1基准点R01和第2基准点R02的线段设定在Y轴。

图2中用虚线表示的流程是表示示教处理，而用点划线表示的流程是表示再现处理。

在行驶路线L1，L2上示教行驶的情况(步骤103、104)

在移动体1到达了起始地点Ps时以后，在新获得的座标系中，按照既定的推测运算式顺序地求出行驶沿线L1上的推测位置P’1，P’2……，并将这些位置数值作为预定行驶路线L1的目标通过地点存贮在既定的存贮器中。

若移动体1环绕行驶路线L1后再次到达第1基准点R01附近，就使用与上述相同的方法，再次运算座标轴X-Y上的第1基准点R01的位置(X”r01，Y”r01)。另外，若移动体1再次到达第2基准点R02，同样，若移动体1再次到达第2基准点R02，同样，也再次运算座标轴X-Y上的第2基准点R02的位置(X”r02，Y”r02)。

另一方面，由于是按照该第1和第2基准点Rr01，Rr02来决定座标轴X，Y，所以在环绕前的第1和第2基准点Rr01，Rr02的座标轴X-Y上的位置(X’r01，Y’r01)、(X’r02，Y’r02)为(X’r01、0)(0、0)时测量已经结束。

而且，在移动体1结束环绕的终点地点又是到达起始地点P’e(Xe，Ye，θe)的时刻，为使通过2次测量的R01位置一致，2次测量的R01，R02的方向一致而用下述(6)式把移动体1的位置修正在座标轴X-Y上Ps的位置和方向(Xs，Ys，θs)。

ΔθL1＝tan-1{(Y″r02-Y″r01)/

             (X″r02-X″r01)}

xs＝(xe-X′r02)cosΔθL1+

    (ye-Y″r02)sinΔθL1

ys＝-(xe-X″r02)sinΔθL1+

     (ye-Y″r02)cosΔθL1

θs＝θe-ΔθL1

…(6)

另外，这2次测量R01的位置偏移和2次测量的R01，R02的方向偏移与示教数据同时被存贮起来。

再者，行驶路线L2上的偏移修正也能够用相同方法求得。

在行驶路线L1，L2上再现行驶的情况(步骤103，104)。

根据关于行驶路线L1的上述示教数据P’1、P’2……，掌舵控制移动体1使其通过这些预定通过地点。

如果能判断移动体1返回到终点，即环绕一周后返回到了原来的起始地点Ps，则根据示教时所测量的基准点和偏移以及上述(6)式就能对移动体的位置和方向进行修正。

再者，移动体1行驶在行驶路线L2上的时候也同样能进行修正。

在行驶路线L3上进行示教行驶的情况(步骤105)

如果移动体1的行驶路线切换到L3，在移动体1到达了起始地点Ps时刻以后，按照推测运算式，就能顺序地求得行驶路线L3上的推测位置P”1、P”2，并将这些位置数值作为预定行驶路线L3的目标通过地点存贮在既定的存贮器中。

如果移动体1到达紧邻起始地点Ps之后的基准点R11，则使用与上述相同的方法，运算座标轴X-X上的基准点R11的位置(X11，Y11)。若移动体1到达下一个基准点R12，则以同样的方法运算座标轴X-Y上的基准点R12的位置(X12，Y12)。

如果移动体1环绕行驶路线L3再次回到起始地点，其位置和方向将被运算作为座标轴X-Y上的位置和方向Pe(Xe、Ye、θe)

此处，由于当前正在行驶的行驶路线L3是不具有在步骤101的座标轴测量中所使用的第1基准点R01、第2基准点R02的行驶路线，因此步骤106的判断为否(NO)，程序转移到步骤107。

行驶路线L3中环绕前后的偏移测量(步骤107)。

这样一来，在行驶路线L3中的示教虽然结束，但移动体1再次在行驶路线L3上继续行驶，通过上述的基准点位置运算装置再次运算紧邻起始地点之后的上述基准点R11的位置作为位置R’11(X’11、Y’11)。移动体1再行驶到下一个基准点R12，用同样的方法再次运算基准点R12的位置作为位置R’12(X’12、Y’12)。

根据这样得到的环绕后的终止地点的位置和行驶方向(Xe、Ye、θe)，行驶路线L3的示教时的2个基准点位置(X11，Y11)、(X12、Y12)以及示教后的2个基准点位置(X’11、Y’11)、(X’12、Y’12)终止地点的推测位置和推测方向Pe(Xe、Ye、θe)将如下述(7)式那样修正到正确的位置和方向Ps(Xs，Ys，θs)以便使得基准点位置R11和R’11的位置一致，R11、R12和R’11、R’12的方向一致。

ΔθL3＝tan-1{(Y′12-Y′11)/(X′12-X′11)}

        -tan-1{(Y12-Y11)/(X12-X11)}

xs＝(xe-X′11)cos(ΔθL3)+

    (ye-Y′11)sin(ΔθL3)+X11

ys＝-(xe-X′11)sin(ΔθL3)+

    (ye-Y′11)cos(ΔθL3)+X11

θs＝θe-ΔθL3

…(7)

另外，将这2次测量的R11和R’11的位置偏移和2次测量的R11、R12和R’11、R’12的方向偏移与示教数据同时存贮起来。

在行驶路线L3上再现(重演)行驶的情况(步骤105)。

根据在行驶路线L3上的上述示教数据P”1、P”2……掌舵控制移动体1使其通过这些预定通过地点。

如果判断移动体1回到终止点，即环绕1周后回到原来的起始地点Ps时，根据上述(7)式和示教时所测量的基准点的偏移，被修正到正确的位置和方向(Xs、Ys、θs)。

如上面所说明的那样，根据第1实施例，即使是在没有共同的2个基准点的不同的多个预定行驶路线上行驶情况，也能通过相对位置测量方式测量在行驶路线上环绕前后的位置和行驶方向的偏移，能使移动体高精确度地导航行驶。

第2实施例

接着，就有关本发明的移动体的位置测量装置的第2实施例进行说明。

图3、图4与第1实施例通用。

如图4所示，在移动体1中装有推断在既定座标轴X-Y上的该移动体1在时刻t的位置(X(t)、Y(t))和行驶方向θ(t)的推断运算部分2。

即，假定移动体1装载的未图示的距离传感器的输出为d1(t)，角速度传感器的输出为ω(t)，初始位置和初始方向为(X0，Y0，θ0)，则与上述(8)-(10)式一样，在推断运算部分2进行以下的推断运算。 $x\left(t\right)={\∫}_{t0}^t\cos \mathrm{\θ}\left(t\right)\mathrm{dl}\left(t\right)+X0---\left(11\right)$ $y\left(t\right)={\∫}_{t0}^t\sin \mathrm{\θ}\left(t\right)\mathrm{dl}\left(t\right)+Y0---\left(12\right)$ $\mathrm{\θ}\left(t\right)={\∫}_{t0}^t\mathrm{\ω}\left(t\right)\mathrm{dt}+\mathrm{\Θ}0---\left(13\right)$

可是，如上述(11)-(13)式所示，为了通过运算求出移动体1的位置和方向，就需要这些初始值X0、Y0、Θ0。并且，为给出这样的初始值，必须设定既定座标系，即座标轴X-Y来记述初始值。

在基准点位置运算部分4，运算为设定上述座标轴的基准点R01、R02的位置。此处，基准点R01、R02是在预定行驶路线的起始地点附近的2个基准点。在移动体1中装有照射角度不同的2个投/受光器，根据从由一个投光器投射的光经位于基准点的反射镜反射后用一个受光器受光，到从另一投光器投射的光经同样的反射镜反射后用另一受光器受光为止的移动距离，在相对位置检测部分3运算、检测对于基准点的移动体的相对位置。而且，由这个相对位置和移动体的当前位置运算基准点位置。这样的运算方法，在特愿平3-148921号中指出，已众所周知。再说，作为检测本发明中的相对位置的方法，不限于上述以前的申请中所指出的方法，无论什么方法都行。

在座标变换运算部分5中，根据在基准点位置运算部分4中所运算的基准点R01、R02的位置设定新座标轴X-Y，并将临时设定的座标轴Xte-Yte上的位置和行驶方向座标变换为上述新设定的座标轴X-Y上的位置和行驶方向。

以下，按照顺序，就上述基准点位置运算部分4和座标变换运算部分5中所进行的运算进行说明。

还有，由于座标轴X-Y是记述在图5所示的原来测量时的行驶位置和行驶方向的座标轴，所以以下的处理是在原来测量之前进行的。

首先，如图3所示，例如，移动体1停止在位于预定行驶路线起始地点跟前的相邻2个基准点R01、R02的更近的既定位置。并且将这个停止位置暂且规定为(Xote、Yote、Θote)。换言之，这与规定临时的座标轴Xte-Yte相同。

因此使移动体从这个位置(Xote、Yote、Θote)移动。于是，在推断运算部分2，将位置(Xote、Yote、Θote)作为初始值按照航位绘算航法像下述(14)-(16)式那样推断运算移动体1的位置和行驶方向。 $\mathrm{xte}\left(t\right)={\∫}_{t0}^t\cos \mathrm{\θ}\left(t\right)\mathrm{dl}\left(t\right)+X0\mathrm{te}---\left(14\right)$ $\mathrm{yte}\left(t\right)={\∫}_{t0}^t\sin \mathrm{\θ}\left(t\right)\mathrm{dl}\left(t\right)+Y0\mathrm{te}---\left(15\right)$ $\mathrm{\θte}\left(t\right)={\∫}_{t0}^t\mathrm{\ω}\left(t\right)\mathrm{dt}+\mathrm{\Θ}0\mathrm{te}---\left(16\right)$

然后，如果当前移动体1通过了起始地点跟前的2个基准点的第1基准点R01附近，则如上述那样，在相对位置检测部分3检测、运算对于第1基准点R01的移动体1的相对位置(dX01、dY01)，并将它们到输入基准点位置运算部分4。并且，如果通过了第1基准点R01时的推断位置为(Xte01、Yte01)，则在推断运算部分2被运算后将它输入到基准点位置运算部分4。不久，移动体1通过了第2基准点R02时，从推断运算部分2和相对位置检测部分3的相同输出也输入到基准点位置运算部分4。

在基准点位置运算部分4中，根据下述(17)式，运算第1和第2基准点R01的位置(Xr01、Yr01)、R02的位置(Xr02、Yr02)

Xr0i＝xte0i-dX0i

Yr0i＝yte0i-dY0i(i＝1、2)…(17)

基准点位置运算部分4的运算值加到座标变换运算部分5。

在座标变换运算部分5中，根据所加的2个基准点R01、R02的位置，设定座标轴X-Y。因为若知道作为2个基准的位置，方向和位置就唯一地确定了，由此根据这2个基准位置能决定二维座标轴。例如，假定以第2基准点R02的位置(Xr02、Yr02)为原点，以连接第1基准点R01的位置(Xr01、Yr01)和第2基准点R02的位置(Xr02、Yr02)的线段为X轴就能设定座标轴X-Y。

接着，将临时设定的座标轴的Xte-Yte上的移动体1的位置和行驶方向(Xte、Yte、θte)从推断运算部分2加到座标变换变换运算部分5后，如下述(18)式那样，座标变换为上述新设定的座标轴X-Y上的位置和行驶方向(X、Y、θ)。

dθ0＝tan-1{(Yr02-Yr01)/(Xr02-Yr01)}

x＝(xte-Xr02)cos(dθ0)+(yte-Yr02)sin(dθ0)

y＝-(xte-Xr02)sin(dθ0)+(yte-Yr02)cos(dθ0)

θ＝θte-dθ0

…(18)

如以上那样，若设定新座标轴X-Y，如图5所示开始基于原来的推断运算的移动体1的位置测量。即，将座标轴Xte-Yte上的位置和方向(Xte、Yte、θte)变换为座标轴X-Y上的位置和方向(X、Y、θ)。

X0＝X

Y0＝Y

θ0＝θ

若把这些值作为上述(11)-(13)式中的初始值，以后根据这些(11)-(13)式就能作为座标轴X-Y上的值正确地求出移动体1的任意位置X、Y和行驶方向θ。从而，能高精确度地测量移动体1的位置。此处，如果基准点R01和R02的间隔足够大，则初始方向就能正确地求出。

再者，在该第2实施例中，是以第2基准点R02的位置为原点、连接第1基准点R01和第2基准点R02的线段为X轴，但也可以以第1基准点R01的位置为原点，将连接第1基准点R01和第2基准点R02的线段设定为Y轴。

如以上所说明的那样，根据该第2实施例，由于是根据相对于基准点的移动体的相对位置设定移动体的座标轴，所以能简易而低成本地进行座标轴的设定。还有，因为是以基准点的2个位置作为基准求得座标轴，因此，不仅是初始位置，就是初始方位也能正确地记述。

根据本发明，即使是在没有共同的2个基准点的不同的多条预定行驶路线上行驶的情况，行驶路线环绕前后的位置和行驶方向的偏移也可以通过相对位置测量方式来测量，从而使移动体能够高精确度地导航行驶。

另外，由于是根据相对于基准点的移动体的相对位置设定移动体的座标轴，因而座标轴的设定能够简易且低成本地进行。再因为是以基准点的2个位置作为基准来求得座标轴，因此，不仅是初始位置，就是初始方向也能正确地记述。

Claims (4)

1.移动体的位置测量装置，具有推断既定座标轴上的移动体的位置和行驶方向的推断运算装置，同时具有检测对于沿着所述移动体的预定行驶路线间断地配设的基准点的上述移动体的相对位置的相对位置检测装置，根据上述相对位置检测装置的输出和上述基准点的位置间断地修正由上述推断运算装置所推断运算的移动体的位置和行驶方向，并根据该修正了的位置和行驶方向使上述移动体沿着上述预定的行驶路线行驶，其特征是，在该移动体的位置测量装置中：

所述预定行驶路线包含具有紧邻起始地点之前的第1和第2基准点的闭环第1行驶路线和不具有上述第1和第2基准点的闭环第2行驶路线；

该装置具有基准点位置运算装置、座标变换运算装置、第1偏移运算装置、第1修正装置、第2偏移运算装置以及第2修正装置，

其中，基准点位置运算装置的功能是：在上述移动体通过第1行驶路线上的第1基准点时根据由上述推断运算装置输出的推断装置和对于由上述相对装置检测装置输出的上述第1基准点的上述移动体的相对位置运算上述第1基准点的位置，同时，在上述移动体通过了预定行驶路线上的第2基准点时根据由上述推断运算装置输出的推断位置和对于由上述相对位置检测装置输出的上述第2基准点的上述移动体的相对位置运算上述第2基点位置；

座标变换运算装置的功能是：根据由上述基准点位置运算装置运算的既定座标轴上的第1和第2基准点的位置设定不同于上述既定座标轴的新座标轴，进行将由上述推断运算装置输出的、上述既定座标轴上的位置和行驶方向变换为上述新设定的座标轴上的位置和行驶方向的运算，并将该变换运算的位置和方向设定为由上述推断装置进行的推断运算的初始值；

第1偏移运算装置的功能是：在上述座标变换运算装置中，将上述第1和第2基准点的位置作为上述新设定的座标轴上的位置存贮起来，同时，在上述移动体环绕第1行驶路线后即将回到上述起始地点之前由上述基准点位置运算装置再将运算上述第1和第2基准点的位置，根据这些在上述座标变换运算装置中所存贮的第1和第2基准点的位置，以及上述移动体环绕第1行驶路线后即将回到起始地点之前再次被运算的第1和第2基准点的位置，运算上述移动体环绕第1行驶路线时产生的基准点的偏移；

第1修正的功能是：根据由上述第1偏移运算装置运算的偏移，每当上述移动体环绕第1行驶路线并回到起始地点时，就修正上述移动体的位置和行驶方向；

第2偏移运算装置的功能是：当上述移动体在第2行驶路线上行驶时，由上述基准点位置运算装置将紧邻在第2行驶路线的上述起始地点之后的2个基准点的位置作为上述第1行驶路线上的在上述座标变换运算装置中设定的座标轴上的位置进行运算，同时，上述移动体环绕第2行驶路线又回到上述起始地点之后再继续行驶、并由上述基准点位置运算装置对上述2个基准点的位置再次进行运算，根据这些紧邻开始环绕第2行驶路线之后的被运算的上述2个基准点的位置和上述移动体环绕第2行驶路线回到起始地点之后再次被运算的上述2个基准点的位置，运算上述移动体环绕第2行驶路线时产生的基准点的偏移。

2.在权利要求1所记载的移动体的位置测量装置中，上述的新座标轴被设定为使第1基准点或第2基准点为原点，连接第1基准点和第2基准点的线段为X轴或Y轴。

3.在具有推断运算既定座标轴上移动体的位置和行驶方向的推断运算装置的同时，具有检测相对于沿着上述移动体的预定行驶路线间断地配设的基准点的上述移动体的相对位置的相对位置检测装置的移动体的位置测量装置中，具有基准点位置运算装置和座标变换运算装置，将由上述座标变换装置所变换的位置和行驶方向作为初始值实行由上述推断运算装置进行的推断运算，

基准点位置运算装置的功能是：当上述移动体通过预定行驶践线上第1基准点时，根据由上述推断运算装置输出的推断位置和相对于由上述相对位置检测装置输出的上述第1基准点的上述相对位置检测装置输出的上述第1基准点的上述移动体的相对位置对上述第1基准点的位置进行运算，同时，当上述移动体通过了预定行驶路线上的第2基准点时，根据由上述推断运算装置输出的推断位置和相对于由上述相对位置检测装置输出的上述第2基准点的上述移动体的相对位置对上述第2基准点的位置进行运算；

座标变换运算装置的功能是：根据由上述基准点位置运算装置运算的既定座标轴上的第1和第2基准点的位置设定不同于上述既定座标轴的新座标轴，并进行将由上述推断运算装置输出的、将上述既定座标轴上的位置和行驶方向变换为上述新设定的座标轴上的位置和行驶方向的运算。

4.在权利要求3所记载的移动体的位置测量装置中，上述新设定的座标轴是将连接第1基准点和第2基准点的线段作为X轴或Y轴，将第1基准点或第2基准点作为原点的座标轴。

Images