CN100410622C - 用于获得目标物体的位置和方位的信息处理方法和设备 - Google Patents

Abstract

在一种信息处理方法中，方位传感器被安装到要测量的目标物体上，并且固定地安装用于捕捉目标物体图像的俯瞰视角照相摄像机。标志检测单元从俯瞰视角照相摄像机捕捉的图像，检测安装于方位传感器上的标志。方位传感器测量出的方位值被输入到方位预测单元，方位预测单元根据方位角-漂移-误差校正值预测目标物体的当前方位。位置-方位计算单元使用检测到的标志的图像坐标计算作为未知参数的摄像装置的位置和方位角-漂移-误差校正值的更新值。位置-方位计算单元从得到的参数获得并输出目标物体的位置和方位。

Description

用于获得目标物体的位置和方位的信息处理方法和设备

技术领域

本发明涉及一种用于测量物体的位置和方位的设备和方法.

背景技术

近年来，进行了大量的关于混合现实的研究活动，混合现实的目的是在现实空间和虚拟空间之间进行无缝连接。一种用于显示混合现实的图像显示设备通过所谓的“视频透视方法”得以实现，其中，根据例如摄像机的摄像装置的位置和方位生成的虚拟空间图像(例如用计算机图形、文字信息等绘制的虚拟物体)被绘制以便将其叠加到由摄像装置捕捉的现实空间图像上，由此显示叠加图像。

另外，图像显示设备也可以通过所谓的“光学透视法”实现，其中，根据观察者视点的位置和方位生成的虚拟空间图像被显示在架设在观察者头部的光学透视显示器上。

期望将与相关技术的虚拟现实不同的新领域，例如用于将患者身体内的状态以叠加形式显示到患者身体表面的外科辅助工具，以及其中一个在现实空间中移动的游戏者与虚拟空间内的虚拟敌人作战的混合现实游戏，作为这种图像显示设备的应用。

这些应用共同要求的是现实空间和虚拟空间之间进行的对齐的准确性。已经进行了很多这样的尝试。在使用视频透视法的情况下，混合现实中的对齐问题相当于在场景中(即，在世界坐标系统中)得到摄像装置的位置和方位的问题。类似的，在使用光学透视法的情况下，混合现实感觉中的对齐问题归结为得到显示器或观察者的视点在场景中的位置和方位的问题。

作力解决前一问题的方法，普遍采用的方法是通过在场景中排放或设置许多标志，并且检测由一摄像装置拍摄的一图像中的标志的投影图像的坐标，从而得到摄像装置在该场景中的位置和方位。另外，有许多试图想通过使用安装在摄像装置上的惯性传感器获得比仅使用图像信息的情况更稳定的对齐。更具体地，根据惯性传感器测量的值估计出的摄像装置的位置和方位被用于对标志进行检测。估计出的位置和方位也被用作根据图像来计算的位置和方位的初始值，或者即使在没有发现标志的情况下作为粗略的位置和方位(例如，HirofumiFUJII，Masayuki KANBARA，Hidehiko IWASA，Haruo TAKEMURA，Naokazu  YOKOYA，″Kakuchogenjitsu-notameno Jairosensa-woHeiyoshita Sutereokamera-niyoru Ichiawase(registration with a stereocamera by jointly using a gyro sensor for augmented reality)″，DenshiJoho Tsushin Gakkai(Institute of Electronics，Information andCommunication Engineers)Gijutsu Hokoku(Technical report)PRMU99-192(Shingaku Giho(Technical Report of IEICE)，vol.99，no.574，pp.1-8)″)

作为解决后一问题的方法，所普遍采用的方法是，通过将摄像装置(和惯性传感器)架设在要测量的目标物体(即观察者的头部或显示器)上以与前面情况中类似的方式获得摄像装置的位置和方位，从摄像装置和目标物体之间的已知的相对位置和方位关系中获得目标物体的位置和方位。

然而，在相关技术的上述方法中，在主观的视点图像没有包括足以实现稳定对齐的图像信息的情况下，例如，当局部地存在于图像的一部分中的标志被观察到时，并且当仅有三个标志被观察到并且标志检测中包括误差时，获得的解的准确性和稳定性可能会不够。另外，当观察的标志的数量不比2大时，不能得到解。为了避免这些问题，需要在场景中均匀地设置大量的标志。这导致了这样的问题，即识别标志比较困难并且现实空间图像会变形。另外，存在一个问题，即在用观察者的手盖住主观视点图像上的标志图像的情况下，对齐完全不可能。

发明内容

根据本发明的一个方面，用于计算物体的位置和方位的信息处理方法包括以下步骤：从摄像装置输入捕捉图像，所述摄像装置用于从关于物体的俯瞰视角位置捕捉物体图像，从方位传感器输入测量出的方位值，所述方位传感器用于测量关于物体的方位信息，从捕捉的图像，检测与安置在物体上的标志的图像坐标相关的特征值，通过使用检测出的关于标志图像坐标的特征值，获得关于物体方位角的参数和关于物体位置的参数，所述参数视为至少为未知参数，通过使用获得的参数，计算物体的位置和方位。

根据本发明的另一方面，提供了一种用于计算摄像装置的位置和方位的信息处理方法，所述摄像装置用于捕捉场景的图像。该信息处理方法包括：第一幅图像输入步骤，输入由摄像装置捕捉的第一幅图像；第二幅图像输入步骤，输入由俯瞰视角摄像单元捕捉的第二幅图像，所述俯瞰视角摄像单元用于从摄像装置的俯瞰视角位置捕捉图像；方位输入步骤，从方位传感器输入所测量出的方位值，所述方位传感器用于测量关于摄像装置的方位的信息；第一检测步骤，从在第一幅图像输入步骤中输入的第一幅图像中检测关于安置于场景中的第一标志的图像坐标的第一-标志图像-坐标特征值；第二检测步骤，从在第二幅图像输入步骤中输入的第二幅图像中检测关于安置于场景中的第二标志的图像坐标的第二-标志图像-坐标特征；位置-方位计算步骤，通过使用在第一检测步骤中检测的第一-标志图像-坐标特征值、在第二检测步骤中检测的第二-标志图像-坐标特征值和在方位输入步骤中输入的测量方位值，计算摄像装置的位置和方位。

根据本发明的另一方面，本发明提供了一种用于计算摄像装置的位置和方位的信息处理方法，所述摄像装置用于捕捉场景图像。所述信息处理方法包括：第一幅图像输入步骤，输入摄像装置捕捉的第一幅图像；第二幅图像输入步骤，输入由俯瞰视角摄像单元捕捉的第二幅图像，所述俯瞰视角摄像单元用于从摄像装置上的视点位置捕捉场景的图像，方位输入步骤，从方位传感器输入测量出的方位值，所述方位传感器用于测量关于摄像装置的方位的信息，第一检测步骤，从在第一幅输入步骤中输入的第一幅图像检测关于置于场景中的第一标志的图像坐标的第一-标志图像-坐标特征值，第二检测步骤，检测关于安置于摄像装置上的第二标志的图像坐标的第二-标志图像-坐标特征值，位置和方位计算步骤，通过使用在第一检测步骤中检测的第一-标志图像-坐标、在第二检测步骤中检测的第二-标志图像-坐标和在方位输入步骤中输入的测量方位值，计算摄像装置的位置和方位。

根据本发明的另一个方面，提供一种用于计算物体的位置和方位的信息处理设备。该信息处理设备包括：捕捉图像输入单元，从摄像装置输入捕捉的图像，所述摄像装置从关于物体的俯瞰视角位置捕捉物体的图像；测量方位值输入单元，从方位传感器输入测量出的方位值，所述方位传感器用于测量关于物体方位的信息；检测单元，从捕捉的图像检测关于安置于物体上的标志的图像坐标的特征值；和位置-方位计算单元，通过使用检测出的关于标志图像坐标的特征值获得关于物体方位角的参数和关于物体位置的参数，所述参数视为至少为未知参数，并且通过使用获得的参数计算物体的位置和方位。

根据本发明的另一个方面，提供一种用于计算摄像装置的位置和方位的信息处理设备，所述摄像装置用于捕捉场景的图像。该信息处理设备包括：第一幅图像输入单元，输入由摄像装置捕捉的第一幅图像；第二幅图像输入单元，输入由俯瞰视角摄像单元捕捉的第二幅图像，所述俯瞰视角摄像单元用于从摄像装置上的视点位置捕捉图像；方位输入单元，从用于测量关于摄像装置的方位的信息的方位传感器输入测量的方位值；第一检测单元，从在第一幅图像输入步骤中输入的第一幅图像中检测关于安置于场景中的第一标志的图像坐标的第一-标志图像-坐标特征值；第二检测单元，从在第二幅图像输入步骤中输入的第二幅图像中检测关于安置于摄像装置上的第二标志的图像坐标的第二-标志图像-坐标特征值；位置-方位计算单元，通过使用由第一检测单元检测到的第一-标志图像-坐标特征值、由第二检测单元检测到的第二-标志图像-坐标特征值和由方位输入单元输入的检测方位值，计算摄像装置的位置和方位。

根据本发明的另一方面，提供一种用于计算用于捕捉场景图像的摄像装置的位置和方位的信息处理设备。该信息处理设备包括：第一幅图像输入单元，输入由摄像装置捕捉的第一幅图像；第二幅图像输入单元，输入由用于从摄像装置上的视点位置捕捉图像的俯瞰视角摄像单元捕捉的第二幅图像；方位输入单元，从用于测量关于摄像装置的方位的信息的方位传感器输入测量的方位值；第一检测单元，从由第一幅图像输入单元输入的第一幅图像中检测关于安置于场景中的第一标志的图像坐标的第一-标志图像-坐标特征值；第二检测单元，从由第二幅图像输入单元输入的第二幅图像中检测关于安置于摄像装置上的第二标志的图像坐标的第二-标志图像-坐标特征值；位置-方位计算单元，通过使用由第一检测单元检测到的第一-标志图-坐标特征值、由第二检测单元检测到的第二-标志图像-坐标特征值和由方位输入单元输入的测量的方位值，计算摄像装置的位置和方位。

本发明的其它的特征和优点将从通过下面结合附图进行的说明中变得明显，在整个附图中同样的参考符号表示同样或类似的部分。

附图说明

图1表示根据本发明的第一实施例的位置-方位测量设备的方框图。

图2表示计算机的基本构造的方框图。

图3为计算表示目标物体的位置和方位的参数的处理的流程图，该处理被执行使得图1中所示的方位预测单元的软件程序被图2中所示的CPU执行。

图4表示计算图1中所示的目标物体的过程的流程图，该处理被执行使得图1中所示的位置-方位计算单元的软件程序被图2中所示的CPU执行。

图5为计算代表图1中所示的目标物体的位置和方位的参数的过程的流程图，该处理被执行使得图1中所示的位置-方位检测单元的软件程序被图2中所示的CPU执行。

图6表示根据本发明的第二实施例的位置-方位测量设备的方框图。

图7表示计算图6中所示的摄像装置的过程的流程图，该过程的执行使得图6中所示的方位预测单元的软件程序被图2中所示的CPU执行。

图8为计算代表图6中所示的摄像装置的位置和方位的参数的过程的流程图，该过程被执行使得图6中所示的位置-方位计算单元的软件程序被图2中所示的CPU执行。

图9表示根据本发明的第二实施例的第一修改方案的位置-方位测量设备的方框图。

图10为计算代表摄像装置的位置和方位的参数的过程的流程图，该过程被执行使得位置-方位计算单元的软件程序被CPU执行。

图11表示根据本发明的第二个实施例的第四修改方案的位置-方位测量设备的方框图。

具体实施方式

下面将根据附图对本发明的优选实施例进行详细的说明。

第一实施例

根据本发明的第一实施例的位置-方位测量设备对要测量的任意的目标物体的位置和方位进行测量。下面说明根据本实施例的位置-方位测量设备及其位置-方位测量方法。

图1表示根据第一实施例的位置-方位测量设备10的结构。如图1所示，位置-方位测量设备10包括：俯瞰视角照相摄像机18a，18b，18c和18d、图像输入单元16、数据存储单元17、标志检测单元11、方位传感器14、方位预测单元15和位置-方位计算单元12。位置-方位测量设备10被连接到要测量的目标物体13上。

在方位传感器14和/或目标物体13上的多个位置上，在物体坐标系统中的位置x_c ^Pk为已知的标志P_k(k＝1，…，K)(后面称为“俯瞰视角标志”或简单为“标志”)被设置以便被俯瞰视角照相摄像机18a，18b，18c和18d观察。这里物体坐标系统由目标物体13上的一个点和三个垂直相交的轴定义。

优选地，这些标志被设置以便，当目标物体13被放置在每个在测量位置和方位的测量范围内的点上时，由俯瞰视角照相摄像机18a，18b，18c和18d获得的在俯瞰视角图像上观察到的(物理)标志的总数至少为2。图1中所示的例子表示以下情况：设置两个标志P₁和P₂，标志P₁包含在俯瞰视角照相摄像机18c的视野之内，标志P₂包含在俯瞰视角照相摄像机18c和18d的视野之内。

标志P_k可以由例如带有不同颜色的球形或圆形标记构成，或者可以由例如具有不同结构特征的自然特征的特征点构成。如果在捕捉的图像中投影图像的坐标可以被检测并且每个标志可以被识别，则标志P_k可以具有任何的标志形式。另外，标志P_k可以有意地设置或者可以具有形成的自然形状而不用被有意地设置。

俯瞰视角照相摄像机18a，18b，18c和18d被固定放置在这样的位置使得，当目标物体13被置于测量范围内时，俯瞰视角照相摄像机18a，18b，18c和18d中的一个照相摄像机可以捕捉目标物体13的图像。下面，我们使用术语“俯瞰视角照相摄像机”表示从第三人的视点观察目标物体13的照相摄像机；照相摄像机的位置并不限于“俯瞰”位置。俯瞰视角照相摄像机18a，18b，18c和18d在世界坐标系统中的位置和方位应该作为已知值预先存储在数据存储单元17中。由俯瞰视角照相摄像机18a，18b，18c和18d输出的图像(后面称为“俯瞰视角图像”)被输入到图像输入单元16中。

输入的图像被图像输入单元16变换为数字数据，并存储在数据存储单元17中。

方位传感器14被架设在目标物体13上。方位传感器14测量其当前的方位并将测量的值输出到方位预测单元15中。方位传感器14为基于例如陀螺仪的角速率传感器的传感器单元，并且由例如由日本的Tokimec公司制造的TISS-5-40或者由美国的InterSense公司制造的InertiaCube2组成。由上述传感器获得的测量方位值是具有误差的方位，与实际的方位不同。上述的方位传感器具有用于观察地球的重力方位的加速传感器作为其部件，并具有消除在倾斜角方位上的漂移误差积累的功能。这样，上述的方位传感器具有在倾斜角方位上(俯仰角和摆动角)不产生漂移误差的特性。换句话说，上述的传感器具有随着时间积累的关于围绕重力轴的方位角，也即是方位(偏航)角的漂移误差。

方位预测单元15从数据存储单元17接收到方位-漂移-误差校正值φ，通过校正从方位传感器14输出的测量方位值预测目标物体13的方位，并将预测的方位输出到数据存储单元17中。

俯瞰视角图像从数据存储单元17输入到标志检测单元11中，并且检测输入图像中标志的图像坐标。例如，当标志由具有不同颜色的标记组成时，从俯瞰视角图像中检测对应于标记颜色的区域，它们的重心位置用作标志的检测坐标。当标志由具有不同结构特征的特征点组成时，标志的位置通过在俯瞰视角图像上基于作为已知信息预先存储的标志的模板图像执行模板匹配进行检测。根据由位置方位计算单元12输出且存储在数据存储单元17中的的目标物体13的位置的计算值和由方位预测单元15输出且存储在数据单元17中的目标物体13的方位的预测值，通过对图像中的每个标志的位置进行预测可以减小搜索范围。根据这个选择，可以减小标志检测所需的计算量和标志的错误检测及错误识别。

标志检测单元11将检测的标志的图像坐标及其标识符输出到数据存储单元17中。通过使用照相摄像机标识符x(x＝a，b，c，d)和标识符m(m＝1，…，M_x)，其中M_x表示在每个俯瞰视角图像中检测到的标志的数量，下面由标志检测单元11检测到的俯瞰视角照相摄像机18a，18b，18c和18d捕捉的图像的标志用P_kxm表示。另外，依据俯瞰视角照相摄像机18a，18b，18c和18d的标识符，在图像上检测到的标志P_kxm的坐标分别用u_a ^Pkam，u_b ^Pkbm，u_c ^Pkcm，u_d ^Pkdm表示。M表示在每个图像中检测到的标志的总数。例如，在图1的情况下，M_a＝0，M_b＝0，M_c＝2，M_d＝1，M＝3。因此，标志标识符k_c1＝1，k_c2＝2，k_d1＝2，拍摄这些标志标识符的俯瞰视角照相摄像机的标识符和对应于这些标识符的图像坐标u_c ^Pkc1，u_c ^Pkc2和u_d ^Pkd1被输出。

目标物体13的方位预测值、由标志检测单元11检测到的标记的图像坐标u_a ^Pkam，u_b ^Pkbm，u_c ^Pkcm，u_d ^Pkdm和相应的物体坐标(在物体坐标系统中的坐标值)x_C ^Pkam，x_C ^Pkbm，x_C ^Pkcm，x_C ^Pkdm组成的数据集从数据存储单元17被输入到位置-方位计算单元12中。位置-方位计算单元12根据上述信息计算目标物体13的位置和方位，并且通过接口(未示出)将计算出的位置和方位输出到外部。另外，位置-方位计算单元12将计算出的目标物体13的位置输出到数据存储单元17中，并使用在计算目标物体13的位置和方位的过程中产生的方位传感器14的方位角-漂移-误差校正值的更新值对存储在数据存储单元17中的方位角-漂移-误差校正值进行更新。

数据存储单元17存储方位角-漂移-误差校正值、从图像输入单元16输入的图像、从方位预测单元15输入的方位预测值、从位置方位计算单元12输入的位置计算值、从标志检测单元11输入的标志的图像坐标和标识符、以及例如标志的物体坐标(在物体坐标系统中的坐标值)的数据，和作为已知值的俯瞰视角照相摄像机18a，18b，18c和18d的照相摄像机参数。如果需要，则数据存储单元17对存储的数据进行输入和输出处理。

图1中所示的图像输入单元16、数据存储单元17、标志检测单元11、方位预测单元15和位置-方位计算单元12可以视为独立的设备。另外，图1中所示的图像输入单元16、数据存储单元17、标志检测单元11、方位预测单元15和位置-方位计算单元12的功能可以通过将软件安装到一台或多台电脑中并允许每台电脑的中央处理单元(CPU)对安装的软件进行执行而得以实现。在第一实施例中，图1中所示的图像输入单元16数据存储单元17、标志检测单元11、方位预测单元15和位置方位计算单元12中的每个被视为由单台电脑执行的软件。

图2表示执行作为软件的图1中所示的图像输入单元16、数据存储单元17、标志检测单元11、方位预测单元15和位置方位计算单元12的计算机的基木结构方框图。

CPU1001使用存储在随机存取存储器(RAM)1002和只读存储器(ROM)1003中的程序和数据对整台计算机进行控制。通过控制对应于图像输入单元16、标志检测单元11、方位预测单元15和位置方位计算单元12的软件的执行，CPU1001实现每个单元的功能。

RAM1002包括用于暂时存储从外部存储设备1007或存储介质驱动器1008装载的程序和数据的区域，也包括CPU1001执行各种处理需要的工作区域。数据存储单元17的功能由RAM1002实现。

通常，ROM1003存储为计算机而存储的程序和设置数据。键盘1004和鼠标1005被操作员用于输入各种指示到CPU1001中。

显示单元1006由阴极射线管、液晶显示屏或类似设备组成，可以显示为了目标物体13的位置和方位测量而显示的例如消息等。

外部存储设备1007用作大容量信息存储设备，并存储操作系统、软件、程序等。在第一实施例的描述中，作为已知信息描述的信息存储在外部存储设备1007中，而且需要时被装载进RAM1002中。

存储介质驱动器1008根据CPU1001的指示读取存储在例如CD-ROM或DVD-ROM的记录介质中的程序或数据，并将读取的程序或数据输出到RAM 1002或外部存储设备1007中。

接口1009包括：例如IEEE1394标准的模拟视频端口或数字视频端口，用于连接俯瞰视角照相摄像机18；RS-233C或USB串行端口，用于连接方位传感器14；以太网端口，用于将目标物体13的位置和方位输出到外部。输入数据通过接口1009被装载进RAM 1002。图像输入单元16的部分功能由接口1009实现。

总线1010用于连接CPU101、RAM1002、ROM1003、键盘1004、鼠标1005、显示单元1006、外部存储设备1007、存储介质驱动器1008和接口1009。

图3表示方位预测单元15的处理的流程图。执行处理使得CPU 1001执行方位预测单元15的软件程序。在执行下面处理之前的阶段中，与该流程图相对应的程序代码应该预先装载到RAM1002中。

虽然已经有各种表示方位的方法，但是在本实施例中，用3×3旋转矩阵R表示方位。

在步骤S300，测量的方位值R^#(^#是表示传感器-测量值的符号)从方位传感器14被输入到方位预测单元15中。

在步骤S301，方位角-漂移-误差校正值φ^*从数据存储单元17被输入到方位预测单元15中。

在步骤S302，通过将测量的方位值R^#(表示方位传感器14的方位)代入到从方位传感器14的方位到目标物体13的方位的变换中，并且根据方位角-漂移-误差校正值φ^*对漂移误差进行校正，方位预测单元15计算目标物体13的方位作为预测的方位值R^*。

R^*＝ΔR(φ^*)·R^#·R_SC(1)

其中ΔR(φ)表示在方位角方向上增加旋转φ的旋转矩阵，并通过下面的表达式被定义为φ的函数。

$\mathrm{\ΔR}\left(\mathrm{\φ}\right)=\left[\begin{array}{ccc}{l}_1{l}_1(1-\cos \mathrm{\φ})+\cos \mathrm{\φ}& l_2{l}_1(1-\cos \mathrm{\φ})-l_3\sin \mathrm{\φ}& l_3{l}_1(1-\cos \mathrm{\φ})+l_2\sin \mathrm{\φ}\\ l_1{l}_2(1-\cos \mathrm{\φ})+l_3\sin \mathrm{\φ}& l_2{l}_2(1-\cos \mathrm{\φ})+\cos \mathrm{\φ}& l_3{l}_2(1-\cos \mathrm{\φ})-l_1\sin \mathrm{\φ}\\ l_1{l}_3(1-\cos \mathrm{\φ})-l_2\sin \mathrm{\φ}& l_2{l}_3(1-\cos \mathrm{\φ})+l_1\sin \mathrm{\φ}& l_3{l}_3(1-\cos \mathrm{\φ})+\cos \mathrm{\φ}\end{array}\right]---\left(2\right)$

其中″l＝(l₁，l₂，l₃)″表示指示在世界坐标系统中的垂直方位(与地球重力方位相反)的已知向量，R_sc表示用于将物体坐标系统(表示目标物体13的位置和方位的坐标系统)中的方位变换到传感器坐标系统(表示方位传感器14的位置和方位的坐标系统)中的方位的3×3矩阵，并且根据方位传感器14和目标物体13之间的相对方位的固定值被预先设置为已知值。

在步骤S303，方位预测单元15将预测的方位值R输出到数据存储单元17中。

在步骤S304，方位预测单元15判断是否终止处理。如果方位预测单元15的判断结果为不终止处理，则返回到步骤S300。

图4是对表示目标物体13的位置和方位的参数进行计算的处理的流程图。执行该处理使得CPU1001执行对应于位置-方位计算单元12的软件的程序。在执行下面的处理之前的阶段，根据图4中所示的流程图的程序代码应该预先装载到RAM1002中。

在位置-方位计算单元12中，总共四个参数，即目标物体13的位置″t＝[x y z]^T″和方位传感器14的方位角-漂移-误差校正值的更新φ值被视为要计算的未知参数。换句话说，在第一实施例中，不是所有的表示方位的元素都视为未知。假定预测方位值R仅包括方位角方位上的漂移误差。因此，使用一种模型，其中目标物体13的方位可以通过仅确定方位角-漂移-误差校正值的更新值φ而获得。下面，要得到的未知参数由4值状态向量s＝[x y zφ]^T描述。

在步骤S400，目标物体13的预测方位值R(方位预测单元15的输出)从数据存储单元17被输入到位置-方位计算单元12中。

在步骤S401，位置方位计算单元12将s＝[x_τ-1 y_τ-1 z_τ-1 0]^T设置为状态向量s的初始值。在这个表达式中，x_τ-1，y_τ-1和z_τ-1表示在前一个循环中(在τ-1时间)在步骤S411中计算的目标物体13的位置。

在步骤S402，由标志检测单元11检测到的标志的图像坐标和其物体坐标(在物体坐标系统中的坐标值)组从数据存储单元17被输入到位置-方位计算单元12中。例如，在图1的情况下，输入图像坐标u_c ^P1，u_c ^P2和u_d ^P2以及对应的物体坐标x_C ^P1和x_C ^P2。

在步骤S403，位置-方位计算单元12判断输入的标志信息是否包括足以估计位置和方位的信息。位置-方位计算单元12的判断结果允许处理分支。特别地，如果其图像被输入的物理标志的总数不小于2，则位置-方位计算单元12进行到步骤S404。如果其图像被输入的标志的总数小于2，则位置-方位计算单元12进行到步骤S410。例如，在图1中所示的情况下，处理进行到步骤S404，因为已经检测到两个标志(虽然投影图像的数量为3，但是物理标志的数量为2)。

在步骤S404，每个标志P_km的图像坐标的估计值m由位置-方位计算单元12计算。

的计算基于预先作为已知信息存储的每个标志P_km的物体坐标(在物体坐标系统中的坐标值)x_C ^Pkm和当前状态向量s的函数，该函数表示为：

$u^{{P_{k_m}}^{*}}=F_B(x_C^{P_{k_m}},s)---\left(3\right)$

具体地，函数F_B()包括表达式：

$x_W^{P_{k_m}}=\left[\begin{array}{c}{x}_W^{P_{k_m}}\\ y_W^{P_{k_m}}\\ z_W^{P_{k_m}}\end{array}\right]=\mathrm{\ΔR}\left(\mathrm{\φ}\right)\·R^{*}\·x_C^{P_{k_m}}+\left[\begin{array}{c}x\\ y\\ z\end{array}\right]---\left(4\right)$

该表达式从x_C ^Pkm和s获得标志在世界坐标系统中的坐标x_W ^Pkm；下面的表达式，表示为：

$x_B^{P_{k_m}}=\left[\begin{array}{c}{x}_B^{P_{k_m}}\\ y_B^{P_{k_m}}\\ z_B^{P_{k_m}}\end{array}\right]=R_{\mathrm{WB}}^{-1}(x_W^{P_{k_m}}-t_{\mathrm{WB}})---\left(5\right)$

该表达式根据世界坐标系统x_W ^Pkm获得标志在俯瞰视角坐标中的坐标x_B ^Pkm(俯瞰视角照相摄像机18a，18b，18c和18d的坐标系统，其中一个原点和三个垂直相交的轴被定义在每个俯瞰视角照相摄像机上)，以及下面的表达式，表示为

$u^{{P_{k_m}}^{*}}={\left[\begin{array}{cc}{u}_x^{{P_{k_m}}^{*}}& u_y^{{P_{k_m}}^{*}}\end{array}\right]}^T={\left[\begin{array}{cc}-f_x^B\frac{x_B^{P_{k_m}}}{z_B^{P_{k_m}}}& -f_y^B\frac{y_B^{P_{k_m}}}{z_B^{P_{k_m}}}\end{array}\right]}^T---\left(6\right)$

该表达式从俯瞰视角坐标x_B ^Pkm获得图像坐标

R表示在步骤S400输入的预测方位值，ΔR(φ)表示在方位角方位上增加旋转角φ的旋转矩阵，f^B _x和f^B _y分别表示每个俯瞰视角照相摄像机18a，18b，18c和18d在X轴和Y轴方位上的焦距，R_WB表示代表每个俯瞰视角照相摄像机18a，18b，18c和18d的方位的3×3矩阵，t_WB表示描述每个俯瞰视角照相摄像机18a，18b，18c和18d在世界坐标系统中的位置的三维向量，并且作为每个俯瞰视角照相摄像机18a，18b，18c和18d的已知值被预先存储。

在步骤S405，位置-方位计算单元12根据下列表达式计算每个标志P_km图像坐标的估计值

和实际测量值u^Pkm之间的误差Δu^Pkm。

$\mathrm{\Δ}{u}^{P_{k_m}}=u^{P_{k_m}}-u^{{P_{k_m}}^{*}}---\left(7\right)$

在步骤S406，位置-方位计算单元12计算每个标志P_km关于状态向量s的图像雅可比行列式 ${J_{\mathrm{us}}}^{\mathrm{Pkm}}(=\∂u/\∂s).$ 换句话说，图像雅可比行列式为2×4雅可比矩阵，具有作为元素的，将函数F_B(用表达式(3)表示)对于状态向量S的每个元素进行偏微分而获得的解。特别地，位置-方位计算单元12计算2×3雅可比矩阵 ${J_{\mathrm{uxB}}}^{\mathrm{Pkm}}(=\∂u/\∂x_B),$ 具有做为元素的，将表达式(6)的右边对于俯瞰视角照相摄像机坐标x_B ^Pkm的每个元素进行偏微分而获得的解，3×3雅可比矩阵 ${J_{\mathrm{xBxW}}}^{\mathrm{Pkm}}(={\∂x}_B/{\∂x}_W),$ 具有作为元素的，将表达式(5)的右边对于世界坐标x_W ^Pkm的每个元素进行偏微分而获得的解，3×4雅可比矩阵 ${J_{\mathrm{xWS}}}^{\mathrm{Pkm}}(={\∂x}_W/\∂s),$ 具有做为元素的，将表达式(4)的右侧对于状态向量s的每个元素进行偏微分而获得的解。位置-方位计算单元12通过下面的表达式计算J_us ^Pkm。

$J_{\mathrm{us}}^{P_{k_m}}=J_{{\mathrm{ux}}_B}^{P_{k_m}}\·J_{x_B{x}_W}^{P_{k_m}}\·J_{x_{W}s}^{P_{k_m}}---\left(8\right)$

在步骤S407，位置-方位计算单元12根据在步骤S405和S406中计算的误差Δu^Pkm和图像雅可比行列式J_us ^Pkm计算状态向量s的校正值Δs。具体地，位置-方位计算单元12生成其中垂直排列有所获得的标志P_km的误差Δu^Pkm的2M维误差向量U、垂直排列有所获得的标志P_km的图像雅可比行列式J_us ^Pkm的2M×4矩阵Θ，并且使用2M×4矩阵Θ的伪逆矩阵Θ′计算由下式表示的值。

Δs＝Θ′U(9)

在图1所示的情况下，因为M＝3，所以U为六维向量，Θ为6×4矩阵。

在步骤S408，位置-方位计算单元12通过在步骤S407中计算出的校正值Δs根据下述表达式更新状态向量s。

s+Δs→s (10)

在步骤S409，位置-方位计算单元12判断计算是否收敛，通过使用一个标准例如判断误差向量U是否小于预定的阈值，或者判断校正值Δs是否小于预定的阈值。如果计算不收敛，则位置-方位计算单元12通过使用校正后的状态向量s重新执行步骤S404及其后面的步骤。

如果，在步骤S409中判断结果为计算收敛，则在步骤S410，位置-方位计算单元12从获得的状态向量s计算目标物体13的方位。具体地，从在前面的步骤中获得的状态向量s，位置-方位计算单元12获得方位角-漂移-误差校正值的更新值φ并通过下述表达式计算目标物体13的方位R：

R＝ΔR(φ)·R^*(11)

这样，目标物体13的方位R得以计算。

在步骤S411，位置-方位计算单元12将获得的目标物体13的位置和方位的信息通过接口1009输出到外部。位置-方位计算单元12还将目标物体13的位置t输出到数据存储单元17中。位置和方位的输出格式可以为一套表示方位的3×3矩阵R和表示位置的三维向量t，通过对方位元素进行变换获得的Euler角、从位置和方位计算出的模型变换矩阵，或任何用于描述位置和方位的其他方法。

在步骤S412，通过使用在上述计算步骤中获得的方位角-漂移-误差校正值的更新值φ，位置方位计算单元12根据下述表达式更新存储在数据存储单元17中的方位角-漂移-误差校正值φ^*：

φ^*+φ→φ^*(12)

在步骤S413，位置-方位计算单元12判断是否终止处理。如果位置-方位计算单元12的判断结果为不终止处理，则进行到步骤S400，并且在下个帧及其后的帧中对输入的数据执行类似的处理。

上述处理测量目标物体13的位置和方位。

虽然上述实施例使用多个俯瞰视角照相摄像机18a，18b，18c和18d，但是并不总是必须使用多个俯瞰视角照相摄像机18a，18b，18c和18d，很明显，即使仅使用了一个俯瞰视角照相摄像机，也可以获得与上述实施例类似的优点。根据上述实施例，即使由图像上的标志组成的凸包很小，物体的位置和方位可以被稳定地高准确性地测量。换句话说，如果使用类似安排的标志，则可以获得稳定的位置和方位。另外，在标志安排上较小的限制使得可以测量多种类型的物体。另外，可以使用具有广视角的用于覆盖更宽范围的俯瞰视角照相摄像机，从而维持一个宽的移动测量范围。

第一实施例的第一修改方案

在上述的实施例中，方位传感器的方位角-漂移-误差校正值的更新值φ作为未知值被获得。然而，当方位传感器的准确性好，且使用时间短的时候，或者当方位角-漂移-误差校正值的更新值φ可以手工输入的时候，由位置-方位计算单元12获得的参数可以限于目标物体13的位置。根据本实施例的第一修改方案的位置-方位测量设备意在测量有待测量的任意目标物体的位置和方位。该位置-方位测量设备设计成改变根据第一实施例的位置-方位测量设备中的位置-方位计算单元12的功能。对根据第一修改方案的位置-方位测量设备及其位置-方位测量方法将在下面进行描述。

在第一修改方案中，第一实施例中的所有更新值φ都被设为0。换句话说，在第一修改方案中的位置-方位计算单元12，通过3值状态向量s′＝[x y z]^T描述有待获得的未知参数。在第一修改方案中的位置-方位计算单元12可以使用通过将与更新值φ相关的项从位置-方位计算单元12的处理步骤(例如雅可比矩阵和表达式(4))中移除所得到的。例如，表达式(4)变为下述的表达式：

$x_W^{P_{k_m}}=\left[\begin{array}{c}{x}_W^{P_{k_m}}\\ y_W^{P_{k_m}}\\ z_W^{P_{k_m}}\end{array}\right]=R^{*}\·x_C^{P_{k_m}}+\left[\begin{array}{c}x\\ y\\ z\end{array}\right]---\left(13\right)$

根据第一修改方案的位置-方位测量设备，未知参数的数量减少了。这样，就可能期望所获得的解(目标物体13的位置和方位)的稳定性进一步提高。

为了手工输入方位角-漂移-误差校正值的更新值φ，例如，可以进一步将一校正-值-更新单元添加到图1所示的结构中。该校正-值-更新单元通过操作员的输入获得方位角-漂移-误差校正值的更新值φ，并且根据表达式(12)对存储在数据存储单元17中的方位角-漂移-误差校正值φ^*进行更新。该校正-值-更新单元可以使用键盘1004的特定的键作为接口。例如，可以设置校正-值-更新单元，从而使得正符号“+”键被用来设置+0.1的更新值，“-”键被用来设置-0.1的更新值。甚至在如第一实施例中的格式中，其中根据图像信息得到方位角-漂移-误差校正值的更新值φ，明显地，可以共同使用一个手工-输入校正-值-更新单元。

第一实施例的第二修改方案

在第一实施例及其第一修改方案中，作为未知值获得的参数限于方位角-漂移-误差校正值的更新值φ或仅为位置。然而，视为未知的参数并不总是需要固定的。如果有必要，依据参数的特性，通过改变视作未知参数的参数，可以得到更好的位置和方位的估计。根据第二修改方案的位置-方位测量设备意在测量有待测量的任意目标物体的位置和方位。根据第二修改方案的位置-方位测量设备设计成改变根据第一实施例的位置-方位测量设备中的位置-方位计算单元12的功能。根据第二修改方案的位置-方位测量设备和其位置-方位测量方法将在下面进行描述。

第二修改方案中的位置-方位计算单元具有第一实施例中的位置-方位计算单元和第一实施例的第一修改方案中的位置-方位计算单元的合并功能。根据第二修改方案的位置-方位测量设备通常执行在第二修改方案中位置-方位计算单元的处理，其中仅使用位置作为未知参数。另外，以规则的时间间隔(例如，每10秒钟一次(300帧))，根据第二修改方案的位置-方位测量设备执行第一实施例中的位置-方位计算单元12的处理，其中使用方位角-漂移-误差校正值的更新值φ作和位置作为未知参数。优选地，更新方位角-漂移-误差校正值的时间间隔根据位置-方位计算单元12的漂移特性被设置，并且优选地，时间间隔可以由操作员的交互式的操作来设置。

根据第二修改方案的位置-方位测量设备，当使用了一个方位传感器其可以在短时间内获得可以忽略方位角-漂移-误差的准确度来作为方位传感器14时，可以期望在校正方位角漂移误差时所得到的解的稳定性被提高。

第一实施例的第三修改方案

在第一实施例和上述的修改方案中，根据某个时间的图像信息获得方位角-漂移-误差校正值的更新值φ。帧之间的方位角-漂移-误差值具有高度相关性。这样，通过使用多个帧的信息，可以以较高的准确度获得方位角-漂移-误差值。根据第三修改方案的位置-方位测量设备意在测量有待测量的任意目标物体的位置和方位。根据第三修改方案的位置-方位测量设备设计成改变在第一实施例中的位置-方位计算单元12的功能。根据第三修改方案的位置-方位测量设备及其位置-方位测量方法在下面进行描述。

在第三修改方案中的位置-方位计算单元12具有第一实施例中的位置-方位计算单元12和第二修改方案中的位置-方位计算单元的合并功能，并且执行第一实施例和第二修改方案中的参数估计处理。图5表示计算表示目标物体13的位置和方位的参数的处理的流程图。在执行下面步骤之前的阶段中，根据流程图的程序代码应当预先装载到RAM1002中。

在步骤S500，与第一实施例中的步骤S400相似，目标物体13的预测方位值R，即方位预测单元15的输出，从数据存储单元17被输入到位置-方位计算单元12中。

在步骤S501，与步骤S402相似，由标志检测单元11检测到的标志的一套图像坐标和世界坐标从数据存储单元17被输入到位置-方位计算单元12中。

在步骤S502，通过使用作为未知参数的目标物体13的位置t＝[xy z]^T和方位角-漂移-误差校正值的更新值φ，位置-方位计算单元12在类似步骤S401和S403到S409的处理中估计位置T和更新值φ。

在步骤S503，位置-方位计算单元12将在步骤S502中计算出的方位角-漂移-误差校正值的更新值φ求和，得到和φ_SUM。

在步骤S504，位置-方位计算单元12判断是否已经完成在预定数量的帧(例如30帧)上的求和。如果已经完成求和，则位置-方位计算单元12进行到步骤S505。如果没有，则位置-方位计算单元12进行到步骤S508。

在步骤S505，在位置-方位计算单元12中，通过将在步骤S503中获得的和φ_SUM除以帧数量，计算出方位角-漂移-误差校正值的更新值的平均值并且该平均值用作方位角-漂移-误差校正值的新的更新值φ。然后，和φ_SUM被清零。

在步骤S506，在位置-方位计算单元12中，与第一实施例中的步骤S412相似，通过使用在步骤S505中获得的方位角-漂移-误差校正值的更新值φ，根据表达式(12)对存储在数据存储单元17中的方位角-漂移-误差校正值φ^*进行更新。

在步骤S507，在位置-方位计算单元12中，通过使用在步骤S505中获得的方位角-漂移-误差校正值的更新值φ，根据表达式(11)计算出目标物体13的方位并且所计算出的方位被用作新的预测方位值。

在步骤S508，在位置-方位计算单元12中，目标物体13的位置t＝[x y z]^T被用作未知参数，并且通过类似于第一修改方案的处理被估计。

在步骤S509，与第一实施例中的步骤S411相似，位置-方位计算单元12输出目标物体13的位置和方位的信息。

在步骤S510，位置-方位计算单元12判断是否终止处理。如果位置-方位计算单元12已经确定不终止处理，则回到步骤S500并且对下一个帧之后(τ+1时间)输入的数据执行类似的处理。

在上述的处理中，多帧信息被用于实现方位角-漂移-误差的更新值的准确度的提高。虽然，在第三修改方案中，使用了在帧中获得的更新值的平均值，但是在帧中的更新值的中间值也可以使用，并且任何其它的低通滤波器都可以使用。

第二实施例

根据本发明的第二实施例的位置-方位测量设备测量摄像装置的位置和方位。根据第二实施例的位置-方位测量设备及其位置-方位测量方法在下面进行描述。

图6表示根据第二实施例的位置-方位测量设备的结构。如图6所示，位置-方位测量设备100包括俯瞰视角照相摄像机180a，180b，180c和180d、图像输入单元160、数据存储单元170、标志检测单元110、方位传感器140、方位预测单元150和位置-方位计算单元120。位置-方位测量设备100被连接到摄像装置130。

在现实空间的多个位置上，在世界坐标系统中的位置x_W ^Qk为已知的多个标志Q_k(k＝1，…，K_Q)(下面称为“主观视点标志”)被作为由摄像装置130观察的标志被安置。这里，世界坐标系统通过在场景中的一个原点和三个垂直相交的轴定义。在方位传感器140和/或摄像装置130上的多个位置上，在物体坐标系统中的位置x_C ^Pk为已知的标志P_k(k＝1，…，K_P)(后面称为“俯瞰视角标志”)被设置以便由俯瞰视角照相摄像机180a，180b，180c和180d观察。这里，物体坐标系统由摄像装置130上的一个原点和三个垂直相交的轴定义。

优选地，安置这些标志以便，当摄像装置被置于在测量位置和方位的测量范围内的每个点上，由摄像装置130获得的在主观视点图像中观察到的主观视点标志和由俯瞰视角照相摄像机180a，180b，180c和180d获得的在俯瞰视角图像中观察到的(物理的)俯瞰视角标志的总数至少等于2。在图6中所示的情况中，设置有三个主观视点Q₁、Q₂和Q₃以及两个俯瞰视角标志P₁和P₂，并且三个主观视点标志Q₁、Q₂和Q₃中，两个主观视点标志Q₁和Q₃包括在摄像装置130的视野范围内。俯瞰视角标志P₁包括在俯瞰视角照相摄像机180c的视野范围内，俯瞰视角标志P₂包括在俯瞰视角照相摄像机180c和180d的视野范围内。

主观视点标志和Q^k和俯瞰视角标志P^k可以由，例如具有不同颜色的球形或圆形标记组成，或者由例如具有不同结构特征的自然特征的特征点组成。主观视点标志Q^k和俯瞰视角标志P^k可以具有任何形式，只要可以检测到捕捉的图像上的投影图像的图像坐标，并且每个标志可以以某种方式被识别即可。主观视点标志Q^k和俯瞰视角标志P^k可以有意地设置或者可以具有不是有意设置的自然的形状。

由摄像装置130输出的图像(后面称为“主观视点图像”)被输入到位置-方位测量设备100中。

俯瞰视角照相摄像机180a，180b，180c和180d被固定地放置在某一位置以便，当摄像装置130位于测量范围内时，俯瞰视角照相摄像机180a，180b，180c和180d中的一个照相摄像机可以捕捉摄像装置130的图像。每个俯瞰视角照相摄像机180a，180b，180c和180d在世界坐标系统中的位置和方位应该预先作为已知值存储在数据存储单元170中。由俯瞰视角照相摄像机180a，180b，180c和180d输出的图像(后面称为“俯瞰视角图像”)被输入到图像输入单元160中。

输入到位置-方位测量设备100中的主观视点图像和俯瞰视角图像由图像输入单元160变换为数字数据。图像输入单元160将数字数据存储到数据存储单元170中。

方位传感器140被架设在摄像装置130上。方位传感器140测量其当前的方位并将测量的方位输出到方位预测单元150中。方位传感器140是基于例如陀螺仪的角速率传感器的传感器单元，并且由例如由日本的Tokimec公司制造的TISS-5-40或者由美国的InterSense公司制造的InertiaCube2组成。由上述传感器获得的测量方位值不同于实际方位且具有误差。上述方位传感器具有作为其部件的一些加速传感器，用于观察地球的重力方方向，并且具有在倾斜角方位上消除漂移误差的积累的功能。这样，上述方位传感器具有在倾斜角方向上(俯仰角和摆动角)不产生漂移误差的特性。换句话说，上述传感器具有随着时间积累的关于围绕重力轴的方向角(偏航角)的漂移误差。

方位预测单元150从数据存储单元170接收方位角-漂移-误差校正值φ，通过校正从方位传感器140输入的测量方位值对摄像装置130的方位进行预测，并且将预测的方位输出到数据存储单元170中。

从数据存储单元170输入到标志检测单元110的主观视点图像和俯瞰视角图像，和在输入的图像中拍摄的标志的图像坐标被检测。例如，当标志由具有不同颜色的标记组成时，从俯瞰视角图像检测对应于标记颜色的区域，并且它们的重心位置被用作检测到的标志坐标。当标志由具有不同结构特征的特征点组成时，标志的位置通过基于作为已知信息预先存储的标志的模板图像执行模板匹配进行检测。根据由位置-方位计算单元120输出且存储在数据存储单元170中的摄像装置130的位置的计算值和由方位预测单元150输出且存储在数据单元170中的摄像装置130的方位的预测值，通过预测在图像中的每个标志的位置可以缩小搜索范围。根据这个选择，可以减小标志检测所需的计算量和标志的错误检测及错误识别。

标志检测单元110将检测到的标志的图像坐标及其标识符输出到数据存储单元170。在下面的描述中，通过使用分配给每个检测到的标志的标识符n(n＝1，…，N)，用Q_kn表示在主观视点中检测到的标志。另外，通过使用照相摄像机标识符x(x＝a，b，c和d)和分配给每个检测到的标志的标识符m(m＝1，…，M_x)，用P_kxm表示在俯瞰视角图像中检测到的标志。字母N表示在主观视点图像中检测到的标志的数量，字母M_x表示在每个俯瞰视角图像中检测到的标志的数量。字母M表示在俯瞰视角图像中检测到的标志的总数。另外，检测到的标志Q_kn的图像坐标用u^Qkn表示，检测到的标志P_kxm的图像坐标根据捕捉图像的俯瞰视角照相摄像机的标识符用u_a ^Pkam，u_b ^Pkbm，u_c ^Pkcm和u_d ^Pkdm表示。例如，在图6所示的情况中，N＝2，M_a＝0，M_b＝0，M_c＝2，M_d＝1，M＝3。因此，标志标识符(k₁＝1，k₂＝3，k_c1＝1，k_c2＝2，k_d1＝2)、捕捉标志图像的俯瞰视角照相摄像机的标识符和对应的图像坐标(u^Qk1，u^Qk2，u_c ^Pkc1，u_c ^Pkc2和u_d ^Pkd1)被输出。

摄像装置130的方位的预测值、由标志检测单元110检测到的主观视点标志的图像坐标u^Qkn和世界坐标x_W ^Qkn，以及俯瞰视角照相摄像机的图像坐标u^Pkam，u^Pkbm，u^Pkcm，u^Pkdm和物体坐标(在物体坐标系统中的坐标值)x_C ^Pkam，x_C ^Pkbm，x_C ^Pkcm，x_C ^Pkdm从数据存储单元170被输入到位置-方位计算单元120。位置-方位计算单元120根据上述信息计算摄像装置130的位置和方位并且将计算的位置和方位通过接口(未示出)输出。另外，位置-方位计算单元120将计算的位置输出到数据存储单元170中，并且通过使用方位传感器140的方位角-漂移-误差校正值的更新值更新存储在数据存储单元170中的方位角-漂移-误差校正值，该更新值来自计算摄像装置130的位置和方位的处理。

数据存储单元170存储各种类型的数据，例如方位角-漂移-误差校正值.从图像输入单元160输入的图像、从方位预测单元150输入的方位的预测值、从位置-方位计算单元120输入的计算位置值、从标志检测单元110输入的标志的图像坐标及其标识符、作为已知值的主观视点标志的世界坐标、俯瞰视角标志的物体坐标(在物体坐标系统中的坐标值)和俯瞰视角照相摄像机180a，180b，180c和180d的照相摄像机参数。如果有必要，则将各种类型的数据从数据存储单元170输入或输出。

在图6中的图像输入单元160、数据存储单元170、标志检测单元110、方位预测单元150和位置-方位计算单元120中的每一个可以视为单独的设备。做为选择地，通过将作为软件的每个单元安装到一台或多台计算机上并且使用每个计算机的CPU执行该软件，可以实现该单元的功能。在第二实施例中，图像输入单元160、数据存储单元170、标志检测单元110、方位预测单元150和位置-方位计算单元120作为由单台计算机执行的软件。图1方框图表示执行作为软件的图像输入单元150、数据存储单元170、标志检测单元110、方位预测单元150和位置-方位计算单元120的每一个的计算机的基本结构。

图7表示方位预测单元150的处理的流程图。执行这个处理使得CPU1001执行方位预测单元150的软件程序。在执行接下来的处理之前的阶段，根据流程图的程序代码应当预先装载到RAM1002中。

有各种表示方位的方法。在第二实施例中，方位用3×3旋转矩阵R表示。

在步骤S3000，测量的方位值R^#(^#表示传感器-测量值)从方位传感器140被输入到方位预测单元150中。

在步骤S3010，方位角-漂移-误差校正值φ^*从数据存储单元170被输入到方位预测单元150中。

在步骤S3020，通过将测量的方位值R^#(表示方位传感器140的方位)代入从方位传感器140的方位到摄像装置130的方位的变换，并且根据方位角-漂移-误差校正值φ^*对漂移误差进行校正，方位预测单元150计算摄像装置130的方位作为预测的方位值R^*。

R^*＝ΔR(φ^*)·R^#·R_SC(14)

其中ΔR(φ)表示在方位角方向上增加旋转φ的旋转矩阵，并通过下面的表达式被定义为φ的函数。

$\mathrm{\ΔR}\left(\mathrm{\φ}\right)=\left[\begin{array}{ccc}{l}_1{l}_1(1-\cos \mathrm{\φ})+\cos \mathrm{\φ}& l_2{l}_1(1-\cos \mathrm{\φ})-l_3\sin \mathrm{\φ}& l_3{l}_1(1-\cos \mathrm{\φ})+l_2\sin \mathrm{\φ}\\ l_1{l}_2(1-\cos \mathrm{\φ})+l_3\sin \mathrm{\φ}& l_2{l}_2(1-\cos \mathrm{\φ})+\cos \mathrm{\φ}& l_3{l}_2(1-\cos \mathrm{\φ})-l_1\sin \mathrm{\φ}\\ l_1{l}_3(1-\cos \mathrm{\φ})-l_2\sin \mathrm{\φ}& l_2{l}_3(1-\cos \mathrm{\φ})+l_1\sin \mathrm{\φ}& l_3{l}_3(1-\cos \mathrm{\φ})+\cos \mathrm{\φ}\end{array}\right]---\left(15\right)$

其中″l＝(l₁，l₂，l₃)″表示用于指示在世界坐标系统中的垂直方位(与地球重力方位相反)，R_SC表示用于将方位从物体坐标系统(表示摄像装置130的位置和方位的坐标系统)变换到传感器坐标系统(表示方位传感器140的位置和方位的坐标系统)的3×3矩阵，并且根据方位传感器140和摄像装置130之间相对方位的固定值预先设置为已知值。

在步骤S3030，方位预测单元150将预测的方位值R^*输出到数据存储单元170中。

在步骤S3040，方位预测单元150判断是否终止处理。如果方位预测单元150的判断结果为不终止处理，则返回到步骤S3000。

图8是表示对摄像装置130的位置和方位的参数进行计算的过程的流程图。执行该处理以便CPU1001执行对应于位置方位计算单元120的软件程序。在下面的处理之前的阶段，根据上述流程图的程序代码应该事先被装载到RAM1002中。

在位置方位计算单元120中，总共四个参数，即摄像装置130的位置″t＝[x y z]^T″和方位传感器140的方位角漂移误差校正值的更新值φ视为要计算的未知参数。换句话说，在第二实施例中，不是所有的表示方位的元素都视为未知。假定预测方位值R仅包括方位角方位上的漂移误差。因此，使用一种模型，其中摄像装置130的方位可以通过仅确定方位角漂移误差校正值的更新值φ而获得。下面，要得到的未知参数由4值状态向量s＝[x y zφ]^T描述。

在步骤S4000，摄像装置130的预测方位值R(方位预测单元150的输出)从数据存储单元170被输入到位置方位计算单元120中。

在步骤S4010，位置方位计算单元120将s＝[x_τ-1 y_τ-1 z_τ-1 0]^T设置为状态向量s的初始值。在这个表达式中，x_τ-1，y_τ-1和z_τ-1表示在前一个循环中(在τ-1时间)在步骤S4110中计算的摄像装置130的位置。

在步骤S4020，由标志检测单元110检测到的主观视点标志的图像坐标和物体坐标以及由标志检测单元110检测到的俯瞰视角标志的图像坐标和其物体坐标(在物体坐标系统中的坐标值)从数据存储单元170被输入到位置方位计算单元120中。例如，在图6的情况下，主观视点标志的图像坐标u^Q1和u^Q3和相应的世界坐标x_W ^Q1 andx_W ^Q3以及图像坐标u_c ^P1，u_c ^P2和u_d ^P2和相应的物体坐标x_C ^P1和x_C ^P2被输入。

在步骤S4030，位置方位计算单元120判断输入标志信息是否包括足够估计位置和方位的信息。位置方位计算单元120的判断结果允许处理分支。具体而言，如果其图像被输入的物理标志的总数不小于2，则位置方位计算单元120前进到步骤S4040。如果其图像被输入的标志的总数小于2，则位置方位计算单元120前进到步骤S4100。例如，在图8中所示的情况下，两个主观视点标志和两个俯瞰视角标志(虽然投影图像的数量为3，但是物理标志的数量为2)被检测，所以总数为4。因此，处理前进到步骤S4040。

在步骤S4040，根据当前的状态向量s，位置方位计算单元120计算在步骤S4020中输入的每个主观视点标志Q_kn和俯瞰视角标志P_km的图像坐标的估计值。

每个主观视点标志Q_kn的图像坐标的估计值

通过使用每个Q_kn的世界坐标x_W ^Qkn和当前的状态向量s的下述函数计算：

$u^{{Q_{k_n}}^{*}}=F_C(x_W^{Q_{k_n}},s)---\left(16\right)$

具体地，函数F_C()包括下述表达式：

$x_C^{Q_{k_n}}=\left[\begin{array}{c}{x}_C^{Q_{k_n}}\\ y_C^{Q_{k_n}}\\ z_C^{Q_{k_n}}\end{array}\right]={(\mathrm{\ΔR}\left(\mathrm{\φ}\right)\·R^{*})}^{-1}(x_W^{Q_{k_n}}-\left[\begin{array}{c}x\\ y\\ z\end{array}\right])---\left(17\right)$

该表达式根据x_W ^Qkn和状态向量s获得在物体坐标系统中(由摄像装置130定义的物体坐标系统)的坐标x_C ^Qkn；下面的表达式，表示为：

$u^{{Q_{k_n}}^{*}}={\left[\begin{array}{cc}{u}_x^{{Q_{k_n}}^{*}}& u_y^{{Q_{k_n}}^{*}}\end{array}\right]}^T={\left[\begin{array}{cc}-f_x^C\frac{x_C^{Q_{k_n}}}{z_C^{Q_{k_n}}}& -f_y^C\frac{y_C^{Q_{k_n}}}{z_C^{Q_{k_n}}}\end{array}\right]}^T---\left(18\right)$

该表达式从x_C ^Qkn获得图像坐标和其中R表示在步骤S4000输入的预测方位值，ΔR(φ)表示在方位角方位上增加旋转φ的旋转矩阵，f^C _x和f^C _y分别表示在摄像装置130的X轴和Y轴方位上的焦距，其预先作为已知值存储。

每个俯瞰视角标志P_km的世界坐标的估计值

通过使用每个P_km的物体坐标x_C ^Pkm(在物体坐标系统中的坐标值)和状态向量s的下述函数计算。

$u^{{P_{k_m}}^{*}}=F_B(x_C^{P_{k_m}},s)---\left(19\right)$

具体地，函数F_B()包括下述表达式：

$x_W^{P_{k_m}}=\left[\begin{array}{c}{x}_W^{P_{k_m}}\\ y_W^{P_{k_m}}\\ z_W^{P_{k_m}}\end{array}\right]=\mathrm{\ΔR}\left(\mathrm{\φ}\right)\·R^{*}\·x_C^{P_{k_m}}+\left[\begin{array}{c}x\\ y\\ z\end{array}\right]---\left(20\right)$

该表达式从x_C ^Pkm和状态向量s获得标志在世界坐标的坐标x_W ^Pkm；下面的表达式，表示为：

$x_B^{P_{k_m}}=\left[\begin{array}{c}{x}_B^{P_{k_m}}\\ y_B^{P_{k_m}}\\ z_B^{P_{k_m}}\end{array}\right]=R_{\mathrm{WB}}^{-1}(x_W^{P_{k_m}}-t_{\mathrm{WB}})---\left(21\right)$

该表达式从世界坐标系统x_W ^Pkm获得标志在俯瞰视角照相摄像机坐标(俯瞰视角照相摄像机180a，180b，180c和180d的坐标系统，其中一个原点和三个垂直相交的轴被定义在每个俯瞰视角照相摄像机上)中的坐标x_B ^Pkm；下面的表达式：

$u^{{P_{k_m}}^{*}}={\left[\begin{array}{cc}{u}_x^{{P_{k_m}}^{*}}& u_y^{{P_{k_m}}^{*}}\end{array}\right]}^T={\left[\begin{array}{cc}-f_x^B\frac{x_B^{P_{k_m}}}{z_B^{P_{k_m}}}& -f_y^B\frac{y_B^{P_{k_m}}}{z_B^{P_{k_m}}}\end{array}\right]}^T---\left(22\right)$

该表达式从x_B ^Pkm获得图像坐标其中f^B _x和f^B _y分别表示在俯瞰视角照相摄像机180a，180b，180c和180d的X轴和Y轴方位上的焦距，R_WB表示描述每个俯瞰视角照相摄像机180a，180b，180c和180d的方位的3×3矩阵，t_WB表示描述每个俯瞰视角照相摄像机180a，180b，180c和180d在世界坐标系统中的位置的三维向量，预先作为每个俯瞰视角照相摄像机180a，180b，180c和180d的已知值存储。

在步骤S4050，对于每个标志(主观视点标志和俯瞰视角标志)，位置方位计算单元120通过使用下面的表达式计算估计值和实际测量值之间的误差Δu^Qkn和Δu^Pkm：

$\mathrm{\Δ}{u}^{Q_{k_n}}=u^{Q_{k_n}}-u^{{Q_{k_n}}^{*}}---\left(23\right)$

$\mathrm{\Δ}{u}^{P_{k_m}}=u^{P_{k_m}}-u^{{P_{k_m}}^{*}}---\left(24\right)$

在步骤S4060，对于每个标志(主观视点标志和俯瞰视角视点标志)，位置方位计算单元120计算关于状态向量s的图像雅可比行列式。主观视点标志Q_kn的图像雅可比行列式为2×4雅可比矩阵 ${J_{\mathrm{us}}}^{\mathrm{Qkn}}(=\∂u/\∂s),$ 具有通过关于状态向量s的每个元素的偏导函数f_C()获得的作为其元素的解。具体而言，在获得具有通过对关于x_C ^Qkn的每个元素的表达式(18)的右侧求偏导获得的作为其元素的解的2×3雅可比矩阵 ${J_{\mathrm{ux}}}^{\mathrm{Qkn}}(=\∂u/\∂x)$ 和具有通过对关于状态向量s的每个元素的表达式(17)的右侧求偏导获得的作为其元素的解的3×4雅可比矩阵 ${J_{\mathrm{xs}}}^{\mathrm{Qkn}}(=\∂x/\∂s)$ 之后，主观视点标志Q_kn的图像雅可比行列式根据下面的表达式通过使用上述的矩阵计算：

$J_{\mathrm{us}}^{Q_{k_n}}=J_{\mathrm{ux}}^{Q_{k_n}}\·J_{\mathrm{xs}}^{Q_{k_n}}---\left(25\right)$

俯瞰视角标志的图像雅可比行列式为2×4雅可比矩阵 ${J_{\mathrm{us}}}^{\mathrm{Pkm}}(=\∂u/\∂s),$ 具有通过对关于状态向量s的每个元素的表达式(19)中的函数F_B()求偏导获得的作为其元素的解。在获得具有通过对关于俯瞰视角照相摄像机坐标x_B ^Pkm的每个元素的的表达式(22)的右侧求偏导获得的作为其元素的解的2×3雅可比矩阵 ${J_{\mathrm{uxB}}}^{\mathrm{Pkm}}(=\∂u/\∂x_B),$ 具有通过对关于世界坐标x_W ^Pkm的每个元素的表达式(21)的右侧求偏导获得的作为其元素的解的3×3雅可比矩阵 ${J_{\mathrm{xBxW}}}^{\mathrm{Pkm}}(={\∂x}_B/{\∂x}_W)$ 和具有通过对关于状态向量s的每个元素的表达式(20)的右侧求偏导获得的作为其元素的解的3×4雅可比矩阵 ${J_{\mathrm{xWs}}}^{\mathrm{Pkm}}(={\∂x}_W/\∂s)$ 之后，俯瞰视角标志P_km的图像雅可比行列式根据下面的表达式通过使用上述的矩阵计算：

$J_{\mathrm{us}}^{P_{k_m}}=J_{{\mathrm{ux}}_B}^{P_{k_m}}\·J_{x_B{x}_W}^{P_{k_m}}\·J_{x_{W}s}^{P_{k_m}}---\left(26\right)$

在步骤S4070，位置-方位计算单元120根据在步骤S4050和S4060中关于每个标志计算出的误差Δu和图像雅可比行列式J_us，根据下面的表达式计算状态向量s的校正值Δs：

Δs＝Θ′U(27)

其中，U表示2(N+M)维误差向量，表示为：

其中关于标志(主观视点标志和俯瞰视角标志)获得的误差Δu垂直排列，Θ表示2(N+M)×4矩阵，表示为

其中，关于标志(主观视点标志和俯瞰视角标志)获得的图像雅可比行列式垂直排列。这里Θ′表示Θ的伪逆矩阵。在图6所示的情况中，N＝2，M＝3。这样，U为10维向量，Θ为10×4向量。(矩阵)？

在步骤S4080，位置-方位计算单元120通过使用在步骤S4070中计算出的校正值Δs根据下述表达式校正状态向量s，并将获得的值作为新的估计值。

s+Δs→s(30)

在步骤S4090，位置-方位计算单元120判断计算是否收敛，该判断通过使用一个标准完成，例如判断误差向量U是否小于预定的阈值，或者判断校正值Δs是否小于预定的阈值。如果计算不收敛，则位置位计算单元120使用校正后的状态向量s重新执行步骤S4040及其后面的步骤。

如果，在步骤S4090中判断结果为计算收敛，则在步骤S4100，位置位计算单元120从获得的状态向量s计算摄像装置130的方位。具体地，从在前面的步骤中获得的状态向量s，位置-方位计算单元120获得方位角-漂移-误差校正值的更新值φ以通过下述表达式来计算摄像装置130的方位R：

R＝ΔR(φ)·R^*(31)

在步骤S4110，位置-方位计算单元120将获得的摄像装置130的位置和方位的信息通过接口1009输出到外部。位置-方位计算单元120也将摄像装置130的位置t输出到数据存储单元170中。位置和方位的输出格式可以为一套表示方位的3×3矩阵R和表示位置的三维向量t、通过对方位元素进行变换获得的Euler角、从位置和方位计算出的观察变换矩阵，或任何其它用于描述位置和方位的方法。

在步骤S4120，通过使用在上述计算步骤中获得的方位角-漂移-误差校正值的更新值φ，位置-方位计算单元120根据下述表达式更新存储在数据存储单元170中的方位角-漂移-误差校正值φ^*：

φ^*+φ→φ^*(32)

在步骤S4130，位置-方位计算单元120判断是否终止处理。如果位置-方位计算单元120的判断结果为不终止处理，则进行到步骤S4000，并且在下个帧及其后的帧中对于输入的数据执行类似的处理。

上述处理测量摄像装置130的位置和方位。

根据第二实施例的位置-方位测量设备，如果在主观视点图像上观察到的标志的数量和在俯瞰视角图像上观察到的标志的数量的总和至少为2，则摄像装置130的位置和方位可以得到测量。因此，即使主观视点图像被遮住了(用手或类似方法)，也可以根据俯瞰视角图像信息(至少对两个俯瞰视角标志的观察)对摄像装置130的位置和方位进行测量。相反，即使所有的俯瞰视角标志被挡住了，也可以根据主观视点图像信息(至少对两个主观视点标志的观察)对摄像装置130的位置和方位进行测量。

虽然，在第二实施例中，使用了多个俯瞰视角照相摄像机180a，180b，180c和180d，但是它们并不总是必须的。即使在使用单个俯瞰视角照相摄像机的情况下，明显地，也可以获得与第二实施例中类似的优点。

第二实施例的第一修改方案

第二实施例意在测量在空间中移动的摄像装置的位置和方位。与第二实施例不同，根据第二实施例的第一修改方案的位置-方位测量设备意在测量任意的目标物体的位置和方位。该位置-方位测量设备设计成在根据第二实施例的位置-方位测量设备上增加一主观视点照相摄像机。所述的根据第二实施例的第一修改方案的位置-方位测量设备及其方法将在下面进行描述。

图9表示根据第二实施例的第一修改方案的位置-方位测量设备(用标号500表示)的结构的方框图。如图9所示，位置-方位测量设备500包括俯瞰视角照相摄像机180a，180b，180c和180d、图像输入单元160、数据存储单元170、标志检测单元110、方位传感器140、方位预测单元150、位置-方位检测单元520和主观视点照相摄像机530。

位置-方位测量设备500中与第二实施例中功能相同的部分用与图6中相同的标号进行标注。因此，省去了对它们的描述。位置-方位测量设备500与第二实施例的不同之处在于，由主观视点照相摄像机530捕捉的图像被作为主观视点图像输入到图像输入单元160中，由方位预测单元150获得的预测方位值为主观视点照相摄像机530的方位，且方位传感器140架设在主观视点照相摄像机530上。

主观视点照相摄像机530固定地架设在目标物体580上。目标物体580在主观视点照相摄像机坐标系统中的位置和方位应当为已知。

主观视点照相摄像机530的预测方位值R、由标志检测单元110检测到的每个主观视点标志的图像坐标和世界坐标以及每个俯瞰视角的图像坐标及其对应的主观视点照相摄像机坐标从数据存储单元170被输入到位置位检测单元520。根据上述的信息，位置-方位检测单元520通过执行与第二实施例中的位置-方位计算单元120类似的处理计算主观视点照相摄像机530的位置和方位。另外，位置方-位检测单元520将计算的位置输出到数据存储单元170中，并使用来自位置和方位的计算处理的方位传感器140的方位角-漂移-误差校值的更新值φ对存储在数据存储单元170中的方位角-漂移-误差校正值进行更新。

根据所述计算出的主观视点照相摄像机530的位置和方位(在世界坐标系统中)以及已知的目标物体580在照相摄像机坐标系统中的位置和方位，位置-方位检测单元520计算目标物体580的位置和方位。计算出的位置和方位通过接口1009被输出到外部。

上述方式实现了对任意目标物体的位置和方位的测量。

在第二实施例的第一修改方案中，在临时地获得主观视点照相摄像机530的位置和方位之后，位置-方位检测单元520获得目标物体580的位置和方位。然而，位置-方位检测单元520可以直接获得目标物体580的位置和方位。在这种情况下，方位-预测单元150设计成估计目标物体580的位置和方位(在表达式(14)中的R_SC设置为用于将方位从目标物体的坐标系统变换到传感器坐标系统中的3×3矩阵)。另外，目标物体580的位置设置为状态向量s的一元素，用于从状态向量s获得主观视点的主观视点照相摄像机坐标x_C ^Qkn的表达式(17)用以下表达式替代：

$x_C^{Q_{k_n}}=\left[\begin{array}{c}{x}_C^{Q_{k_n}}\\ y_C^{Q_{k_n}}\\ z_C^{Q_{k_n}}\end{array}\right]=R_{\mathrm{CO}}\·{(\mathrm{\ΔR}\left(\mathrm{\φ}\right)\·R_{\mathrm{\τ}}^{*})}^{-1}(x_W^{Q_{k_n}}-\left[\begin{array}{c}x\\ y\\ z\end{array}\right])+t_{\mathrm{CO}}---\left(33\right)$

用于获得俯瞰视角标志的世界坐标x_W ^Pkm的表达式(20)用以下表达式替代：

$x_W^{P_{k_m}}=\left[\begin{array}{c}{x}_W^{P_{k_m}}\\ y_W^{P_{k_m}}\\ z_W^{P_{k_m}}\end{array}\right]=\mathrm{\ΔR}\left(\mathrm{\φ}\right)\·R^{*}\·R_{\mathrm{CO}}^{-1}\·(x_C^{P_{k_m}}-t_{\mathrm{CO}})+\left[\begin{array}{c}x\\ y\\ z\end{array}\right]---\left(34\right)$

其中R_CO表示用于将方位从目标物体坐标系统(其中，目标物体580上的一个点被定义为原点，三个垂直相交的轴被定义为X轴、Y轴和Z轴)变换到主观视点照相摄像机坐标系统中的矩阵，t_CO表示将位置在相同坐标系统间进行变换的向量，并且应当根据作为已知值存储的目标物体580在主观视点照相摄像机坐标系统中的位置和方位而提前被计算。

在第二实施例的第一修改方案中，目标物体580可以为用于捕捉场景图像的摄像装置。另外，主观视点照相摄像机530可以例如向上放置，以便与用于捕捉场景图像的摄像装置的视野具有不同的视野范围，主观视点标志Q_k可以相应地放置在主观视点照相摄像机530的视野范围之内，例如天花板上。这有利于减轻图像变形等问题，因为主观视点标志Q_k不包括在用于捕捉场景图像的摄像装置的视野范围内。在第二实施例的第一修改方案中，通过将多个主观视点照相摄像机(与主观视点照相摄像机530相同)架设在目标物体580上，测量出的目标物体580的位置和方位在位置和方位方面都具有高准确性。

在第二实施例的第二修改方案中

在第二实施例及其第一修改方案中，在每个位置-方位计算单元120和位置-方位检测单元520中，表示方位角-漂移-误差校正值的4值状态向量s和位置被用作未知值，并且获得使得主观视点标志和俯瞰视角标志的检测到的坐标(实际测量值)和计算出的值之间的误差最小的状态向量s。在第二实施例的第二修改方案中，使用了几何标志限制条件。根据第二实施例的第二修改方案的位置-方位测量设备的特征在于，使用不同于总误差最小化技术的技术的位置-方位检测单元作为部件包括在其中。

根据第二实施例的第二修改方案的位置-方位测量设备基本上与根据第二实施例的位置-方位测量设备的结构类似。然而，在该第二修改方案中，在第二实施例中的位置-方位计算单元120用与之不同的位置-方位计算单元120′(未示出)替代。换句话说，位置-方位计算单元120′的处理与在第二实施例中位置-方位计算单元120的处理不同。根据第二实施例的第二修改方案的位置-方位测量设备及其位置-方位测量方法在下面进行描述。

在第二实施例的第二修改方案中，功能单元(图像输入单元160、数据存储单元170、标志检测单元110、方位预测单元150和位置-方位计算单元120′)作为在单台计算机中执行的软件。该计算机的基本结构如图2所示。

图10为计算表示摄像装置130的位置和方位的参数的过程和使得CPU1001执行位置-方位计算单元120′的软件程序的过程的流程图。在执行后面处理之前的阶段.根据上述流程图的程序代码应该预先装载到RAM1002中。

在步骤S6000，摄像装置130的方位的预测方位值R(方位预测单元150的输出)从数据存储单元170被输入到位置-方位计算单元120′。

在步骤S6003，由标志检测单元检测到的俯瞰视角标志的图像坐标和照相摄像机坐标被输入到位置-方位计算单元120′。

当多个俯瞰视角标志被设置或者多个俯瞰视角照相摄像机被安装时，俯瞰视角标志的投影图像被检测，从而出现输入每个图像的图像坐标的情况。在第二实施例的第二修改方案中，即使在上述情况中，用于后续处理的俯瞰视角标志的数量视为1，并且位置-方位计算单元120′选出一个合适的点作为俯瞰视角标记P的图像坐标up。

在步骤S6006，根据图像坐标up，位置-方位计算单元120′计算表示限制世界坐标系统中的俯瞰视角标志P的位置的直线的参数。首先，根据图像坐标up，在世界坐标系统中的直线斜度(方位向量)h_x，h_y和h_z通过使用下面的表达式计算：

$\left[\begin{array}{c}{h}_x\\ h_y\\ h_z\end{array}\right]=R_{\mathrm{WB}}\·\left[\begin{array}{c}{u}_x^P/f_x^B\\ u_y^P/f_y^B\\ 1\end{array}\right]---\left(35\right)$

其中R_WB为用来表示检测俯瞰视角标志P的俯瞰视角照相摄像机180a，180b，180c和180d在世界坐标系统中的方位的3×3矩阵，f^B _x和f^B _y分别表示在X轴和Y轴方位上的焦距，作为已知值预先存储在外部存储设备1007中。在这种情况下，在世界坐标系统中直线上的点在下面的表达式中可以作为参数τ的函数表示：

$l_W\left(\mathrm{\τ}\right)=\left[\begin{array}{c}{h}_x\\ h_y\\ h_z\end{array}\right]\mathrm{\τ}+t_{\mathrm{WB}}---\left(36\right)$

其中t_WB表示俯瞰视角照相摄像机180a，180b，180c和180d中的一个照相摄像机在世界坐标系统中的位置，并作为已知值预先存在外部存储设备1007中。用表达式(36)表示的直线通过俯瞰视角照相摄像机180a，180b，180c和180d中的一个照相摄像机在世界坐标系统中的位置和俯瞰视角标志P的位置，并且俯瞰视角标志P的位置被获得使得参数τ取一个合适的值。

共有两个参数，即设置俯瞰视角标志P在世界坐标系统中的位置的参数τ和方位传感器140的方位角-漂移-误差校正值的更新值φ，在下面的说明中被视为要计算的未知参数。在下面的描述中，要计算的未知参数用2值状态向量s′＝[τφ]^T描述。

在步骤S6010，位置-方位计算单元120′将初始值s′＝[τ_-1 0]^T设为状态向量s′。在该设置中，例如，代表着位于一个接近于从前次处理中获得的摄像装置130的位置处获得的俯瞰标志P的世界坐标的位置的直线上的位置τ被设置为τ_-1。

在步骤S6020，在标志检测单元110中检测到的每个主观视点标志的图像坐标和世界坐标被输入到位置-方位计算单元120′。

在步骤S6030，位置-方位计算单元120′判断输入的主观视点标志的数量N是否至少为1。如果输入的主观视点标志的数量N小于1，则处理进行到步骤S6100而不用执行步骤S6040到S6090中执行的s′的更新。

在步骤S6040，对于每个主观视点标志Q^kn，根据下面的表达式和表达式(18)计算出图像坐标的估计值该表达式从世界坐标x_W ^Qkn和s′获得主观视点照相摄像机坐标(在主观视点照相摄像机坐标中的坐标)x_C ^Qkn：

$x_C^{Q_{k_n}}=\left[\begin{array}{c}{x}_C^{Q_{k_n}}\\ y_C^{Q_{k_n}}\\ z_C^{Q_{k_n}}\end{array}\right]={(\mathrm{\ΔR}\left(\mathrm{\φ}\right)\·R^{*})}^{-1}(x_W^{Q_{k_n}}-l_W\left(\mathrm{\τ}\right))+x_C^P---\left(37\right)$

且表达式(18)从照相摄像机坐标x_C ^Qkn获得图像坐标

其中x_C ^P表示标志P在主观视点照相摄像机坐标系统中的坐标值，并作为已知信息预先存储在外部存储设备1007中。

换句话说，假定摄像装置130的位置和方位服从先前获得的状态向量s′，则根据摄像装置130和主观视点标志之间的位置和方位关系获得每个主观视点标志的估计图像坐标值。

在步骤S6050，对于主观视点标志Q^kn，位置-方位计算单元120′根据表达式(23)计算图像坐标的估计值

和实际测量值u^Qkn之间的误差Δu^Qkn。

在步骤S6060，对于主观视点标志Q^kn，位置-方位计算单元120′计算2×2雅可比矩阵 ${J_{{\mathrm{us}}^{\′}}}^{\mathrm{Qkn}}(=\∂u/\∂s^{\′}),$ 该矩阵具有作为元素的，关于状态向量s′的图像雅可比行列式(即，表达式(14)中的函数f_c())(由第二实施例中的第二修改方案中的表达式(37)和(18)组成，状态向量s用作s′)对于s′的每个元素进行偏微分而获得的解。具体地讲，在计算具有作为元素的，表达式(18)的右侧对于照相摄像机坐标x_C ^Qkn的每个元素进行偏微分而获得的解的2×3雅可比矩阵 ${J_{\mathrm{ux}}}^{\mathrm{Qkn}}(=\∂u/\∂x),$ 和具有作为元素的，表达式(37)的右侧对于状态向量s′的每个元素进行偏微分而获得的解的3×2雅可比矩阵 ${J_{{\mathrm{xs}}^{\′}}}^{\mathrm{Qkn}}(=\∂x/\∂s^{\′})$ 之后，通过使用计算出的矩阵根据表达式(25)(但是用s′替代s)计算出2×2雅可比矩阵 ${J_{{\mathrm{us}}^{\′}}}^{\mathrm{Qkn}}(=\∂u/\∂s^{\′}).$

在步骤S6070，位置-方位计算单元120′通过使用表达式(27)(用s′替代s)计算校正值Δs′。在第二实施例的第二修改方案中，U表示2N维误差向量，其中对于每个主观视点标志的误差Δu^Qkn垂直排列，以及Θ表示2N×2矩阵，其中对于主观视点标志获得的图像雅可比行列式J_us′ ^Qkn垂直排列。

在步骤S6080，通过使用在步骤S6070中计算出的校正值Δs′，位置-方位计算单元120′根据表达式(30)(用s′替代s)校正状态向量s′，并且使用获得的任作为新的估计值。

在步骤S6090，位置-方位计算单元120′通过使用例如误差向量U是否小于预定的阈值或者校正值Δs′是否小于预定的阈值的标准，判断计算是否收敛。如果计算不收敛，则位置-方位计算单元120′使用校正后的状态向量s′重新执行步骤S6040和随后的步骤。

如果，在步骤S6090中，判断结果为计算收敛，则在步骤S6100，位置-方位计算单元120′从获得的状态向量s′计算摄像装置130的方位。方位R的计算通过使用直到前一步骤获得的方位角-漂移-误差校正值的更新值φ并基于表达式(31)实现。另外，位置t的计算通过使用直到前一步骤获得的参数τ和方位R并基于下面的表达式实现。

$t=l_W\left(\mathrm{\τ}\right)-R\·x_C^P---\left(38\right)$

在步骤S6110，位置-方位计算单元120′将摄像装置130的位置和方位通过接口1009输出到外部。位置-方位计算单元120′也将摄像装置130的位置t输出到数据存储单元170。位置和方位的输出格式可以是一套表示方位的3×3矩阵R和表示位置的三维向量t、由变换方位元素获得的Euler角、从位置和方位计算出的观察变换矩阵，或任何其它的位置和方位描述方法。

在步骤S6120，通过使用在上述计算步骤中获得的方位角-漂移-误差校正值的更新值φ，位置-方位计算单元120′根据表达式(32)对存储在数据存储单元170中的方位角-漂移-误差校正值φ^*进行更新。

在步骤S6130，位置-方位计算单元120′判断是否终止处理。如果位置方位计算单元120′的判断结果为不终止处理，则进行到步骤S6000，并且在下一帧及其随后的帧中对输入数据执行类似的处理。

在上述的处理中，其上存在有从俯瞰视角照相摄像机180a，180b，180c和180d获得的俯瞰视角标志的直线被用作限制条件，在该限制条件下.可以获得摄像装置130的位置和方位以使得在主观视点图像上的主观视点中的误差最小。

与在第二实施例中的位置方位的测量结果相比较，在第二实施例的第二修改方案中的位置和方位的测量结果更加倾向依赖于从俯瞰视角照相摄像机180a，180b，180c和180d获得的信息。因此，例如，在从俯瞰视角照相摄像机180a，180b，180c和180d获得的信息的可靠性比从摄像装置130获得的信息的可靠性高的情况下，当存在高分辨率的俯瞰视角照相摄像机且当仅有一具有高检测准确度的标记存在时，根据第二实施例的第二修改的位置-方位测量设备与第二实施例中的相比可以更有效地运行。

第二实施例的第三修改方案

在第二实施例及其修改方案中，位置-计算单元120、520和120′中的每个获得作为未知值的方位传感器140的方位角-漂移-误差校正值的更新值。然而，方位传感器140在三个轴的方位上的误差的校正值可以被获得而不需要将方位的校正项仅限于方位角方向。根据第二实施例的第三修改方案的位置-方位测量设备与根据第二实施例中的位置-方位测量设备的结构几乎一样。因此，下面对其与第二实施例中的不同部分进行描述。

在第二实施例的第三修改方案中，数据存储单元170存储方位传感器140的旋转-误差校正矩阵ΔR^*而不是方位传感器140的方位角-漂移-误差校正值φ^*。

在第二实施例的第三修改方案中，代替方位角-漂移-误差校正值φ^*，方位传感器140的旋转-误差校正矩阵ΔR^*被输入到方位预测单元150(步骤S3010)。该方位预测单元150根据下面的表达式而不是表达式(14)计算预测方位值R(步骤S3020)：

R^*＝ΔR^*·R^#·R_SC(39)

在第二实施例的第三修改方案的位置-方位计算单元120中，摄像装置130的位置t＝[x y z]^T和摄像装置130的方位的3值表达式ω＝[ξψζ]^T，即总共有六个参数被视为要计算的未知参数。下面将要计算的未知参数写为6值状态向量s″＝[x y zξψζ]^T。

虽然有各种通过使用3值表示方位(旋转矩阵)的方法，但是在这个修改方案中，以这样的3值向量表示方位，即向量幅度用旋转角定义，向量方位定义旋转轴的方位。方位ω可以用

$R\left(\mathrm{\ω}\right)=\left[\begin{array}{ccc}\frac{{\mathrm{\ξ}}^2}{{\mathrm{\θ}}^2}(1-\cos \mathrm{\θ})+\cos \mathrm{\θ}& \frac{\mathrm{\ξ\ψ}}{{\mathrm{\θ}}^2}(1-\cos \mathrm{\θ})-\frac{\mathrm{\ζ}}{\mathrm{\θ}}\sin \mathrm{\θ}& \frac{\mathrm{\ξ\ζ}}{{\mathrm{\θ}}^2}(1-\cos \mathrm{\θ})+\frac{\mathrm{\ψ}}{\mathrm{\θ}}\sin \mathrm{\θ}\\ \frac{\mathrm{\ψ\ξ}}{{\mathrm{\θ}}^2}(1-\cos \mathrm{\θ})+\frac{\mathrm{\ζ}}{\mathrm{\θ}}\sin \mathrm{\θ}& \frac{{\mathrm{\ψ}}^2}{{\mathrm{\θ}}^2}(1-\cos \mathrm{\θ})+\cos \mathrm{\θ}& \frac{\mathrm{\ψ\ζ}}{{\mathrm{\θ}}^2}(1-\cos \mathrm{\θ})-\frac{\mathrm{\ξ}}{\mathrm{\θ}}\sin \mathrm{\θ}\\ \frac{\mathrm{\ζ\ξ}}{{\mathrm{\θ}}^2}(1-\cos \mathrm{\θ})-\frac{\mathrm{\ψ}}{\mathrm{\θ}}\sin \mathrm{\θ}& \frac{\mathrm{\ζ\ψ}}{{\mathrm{\θ}}^2}(1-\cos \mathrm{\θ})+\frac{\mathrm{\ξ}}{\mathrm{\θ}}\sin \mathrm{\θ}& \frac{{\mathrm{\ζ}}^2}{{\mathrm{\θ}}^2}(1-\cos \mathrm{\θ})+\cos \mathrm{\θ}\end{array}\right]---\left(40\right)$

表示，其中，

$\mathrm{\θ}=\sqrt{{\mathrm{\ξ}}^2+{\mathrm{\ψ}}^2+{\mathrm{\ζ}}^2}$

这样，方位ω可以用3×3旋转矩阵R表示。因此，ω和R可以相互进行唯一的变换。在这里省略将R变换到ω的详细描述，因为该变换为已知技术。

在第二实施例的第三修改方案中，位置-方位计算单元120将s″＝[x_-1 y_-1 z_-1 ξψζ]^T设为状态向量s″的初始值(步骤S4010)。在这个表达式中，x_-1，y_-1和z_-1表示在上次处理中计算出的摄像装置130的位置，ξ，ψ和ζ表示从预测方位值R获得的3值表达。

在第二实施例的第三修改方案中，位置-方位计算单元120根据输入标志的数量通过判断输入标志的总数是否至少为3进行分支(步骤S4030)。

在第二实施例的第三修改方案的位置-方位计算单元120中，从标志的世界坐标x_W ^Qkn和s(在该第三修改方案中为s″)获得主观视点标志Q_kn的主观视点照相摄像机坐标x_C ^Qkn的表达式和从标志的主观视点照相摄像机的坐标x_C ^pkm和s(在该第三修改方案中为s″)获得俯瞰视角标志P_km的世界坐标x_W ^Pkm的表达式，从第二实施例中的表达式(17)和(20)改变为下述的表达式：

$x_C^{Q_{k_n}}=\left[\begin{array}{c}{x}_C^{Q_{k_n}}\\ y_C^{Q_{k_n}}\\ z_C^{Q_{k_n}}\end{array}\right]=R{\left(\mathrm{\ω}\right)}^{-1}(x_W^{Q_{k_n}}-\left[\begin{array}{c}x\\ y\\ z\end{array}\right])---\left(41\right)$

$x_W^{P_{k_m}}=\left[\begin{array}{c}{x}_W^{P_{k_m}}\\ y_W^{P_{k_m}}\\ z_W^{P_{k_m}}\end{array}\right]=R\left(\mathrm{\ω}\right)\·x_C^{P_{k_m}}+\left[\begin{array}{c}x\\ y\\ z\end{array}\right]---\left(42\right)$

因此，针对每个标志的图像雅可比行列式为2×6雅可比矩阵 ${J_{{\mathrm{us}}^{\′\′}}}^{\mathrm{Qkn}}(=\∂u/\∂s^{\′\′}).$

在第二实施例的第三修改方案中，位置-方位计算单元120使用获得的状态向量s″基于表达式(40)计算摄像装置130的方位R。

在该第三修改方案中，在步骤S4120，位置-方位计算单元120使用在上述计算步骤中获得的摄像装置130的方位R基于下面的表达式计算方位传感器140的旋转-误差校正矩阵ΔR^*：

R·R^*-1·ΔR^*→ΔR^*(43)

并对存储在数据存储单元170中的数值进行更新。

上述的处理对摄像装置130的位置和方位进行测量。

第二实施例的第四修改方案

在第二实施例及其修改方案中，通过使用固定到世界坐标系统的俯瞰视角照相摄像机180a，180b，180c和180d，安置于(方位传感器架设在其上)摄像装置130上的俯瞰视角标志P_k的图像被捕捉。然而，获取摄像装置130的位置和方位的结构并不限于第二实施例及其修改方案。根据第二实施例的第四修改方案的位置方位测量设备的特征在于，具有一结构，其中固定在摄像装置130上的高视点照相摄像机180(不同与摄像装置130)捕捉置于世界坐标系统中的高视点标志P_k的图像，而不是使用俯瞰视角照相摄像机。根据第二实施例的第四修改方案的位置-方位测量设备及其位置-方位测量方法在下面进行描述。

图11表示根据第二实施例的第四修改方案的位置-方位测量设备的结构。如图11中所示，根据第二实施例的第四修改方案的位置-方位测量设备(用标号700表示)包括高视点照相摄像机180a和180b、图像输入单元160、数据存储单元770、标志检测单元110、方位传感器140、方位预测单元150和位置-方位计算单元720。位置-方位测量设备700连接到摄像装置130。具有与在第二实施例中相同功能的部分用与图6中相同的标记标注。因此，省去了这部分的描述。

在主观视点照相摄像机坐标系统中的位置和方位为已知的高视点照相摄像机180a和180b，被固定地安置在摄像装置130上。以后，我们使用术语“高视点照相摄像机”表示置于摄像装置130上的具有不同于摄像装置130的视野范围的照相摄像机；该照相摄像机的方位不限于“高”方位。在现实空间内的多个位置上，除了由摄像装置130捕捉图像的主观视点标志Q_k，在世界坐标系统中位置x_W ^Pk为已知的多个高视点标志Pk(k＝1，。。。，kp)作为由高视点照相摄像机180a和180b进行捕捉的图像中的标志安置。在图11所示的情况下，设置了三个主观视点标志Q₁，Q₂和Q₃以及两个高视点标志P₁和P₂，其中，两个主观视点标志Q₁和Q₃位于摄像装置130的视野范围内，高视点标志P₁位于高视点照相摄像机180a和180b的视野范围内，高视点标志P₂位于高视点照相摄像机180b的视野范围内。在图11所示的情况下，关于在主观视点图像中检测到的标志的数量以及在每个高视点图像中检测到的标志的数量，N＝2，M_a＝1，M_b＝2。标记检测单元110输出标志的标识符(k₁＝1，k₂＝3，k_a1＝1，k_b1＝1，k_b2＝2)、捕捉标志图像的照相摄像机的标识符以及相应的图像坐标u^Qk1，u^Qk2，u_a ^Pka1，u_b ^Pkb1和u_b ^Pkb2。高视点照相摄像机180a和180b被放置以便，当摄像装置130位于测量范围内时，高视点照相摄机180a和180b中的一个照相摄像机可以捕捉高视点标志P_k的图像。高视点照相摄像机180a和180b的位置和方位应该预先作为已知值存储在数据存储单元770中。摄像装置130的方位的预测值、一套由标志检测单元110检测到的每个主观视点标志的图像坐标u^Qkn和世界坐标x_W ^Qkn以及每个高视点标志的图像坐标u^Pkm和相应的世界坐标x_W ^Pkm从数据存储单元770被输入到位置-方位计算单元720。根据这些信息，位置-方位计算单元720计算摄像装置130的位置和方位并且将计算出的位置和方位通过接口(未示出)输出到外部。另外，位置-方位计算单元720将计算出的摄像装置130的位置输出到数据存储单元770中。另外，位置-方位计算单元720通过使用在计算位置和方位的过程中产生的更新的方位传感器140的方位角-漂移-误差校正值对存储在数据存储单元770中的方位角-漂移-误差校正值进行更新。数据存储单元770存储各种类型的数据，例如，方位角-漂移-误差校正值、从图像输入单元160输入的图像、从方位预测单元150输入的方位的预测值、从位置-方位计算单元150输入的计算出的位置值、从标志检测单元110输入的标志的图像坐标和标志标识符、为已知值的主观视点标志的世界坐标、高视点标志的世界坐标和高视点照相摄像机180a和180b在主观视点照相摄像机坐标系统中的位置和方位。如果有必要，各种类型的数据从数据存储单元770被输入或被输出。

在第二实施例的第四修改方案中计算表示摄像装置130的位置和方位的参数的过程的流程图与在第二实施例中的流程图(图8)几乎相同。在下面，仅描述与在第二实施例中不同的部分。

在步骤S4020，代替俯瞰视角标志的图像坐标和主观视点照相摄像机坐标(在主观视点照相摄像机坐标系统中的坐标值)，高视点标志的图像坐标和世界坐标被输入到位置-方位计算单元720。

在步骤S4040，每个高视点标志P_km的估计图像坐标值

的计算通过使用高视点标志的世界坐标x_W ^Pkm和当前状态向量s的下述函数完成：

$u^{{P_{k_m}}^{*}}=F_D(x_W^{P_{k_m}},s)---\left(44\right)$

具体地，函数F_D()包括下述表达式：

$x_C^{P_{k_m}}=\left[\begin{array}{c}{x}_C^{P_{k_m}}\\ y_C^{P_{k_m}}\\ z_C^{P_{k_m}}\end{array}\right]={(\mathrm{\ΔR}\left(\mathrm{\φ}\right)\·R^{*})}^{-1}(x_W^{P_{k_m}}-\left[\begin{array}{c}x\\ y\\ z\end{array}\right])---\left(45\right)$

用于从世界坐标x_W ^Pkm和状态向量s获得照相摄像机坐标x_C ^Pkm(在主观视点照相摄像机坐标系统中的坐标)，下面表达式：

$x_B^{P_{k_m}}=\left[\begin{array}{c}{x}_B^{P_{k_m}}\\ y_B^{P_{k_m}}\\ z_B^{P_{k_m}}\end{array}\right]=R_{\mathrm{CB}}^{-1}(x_C^{P_{k_m}}-t_{\mathrm{CB}})---\left(46\right)$

用于从主观视点照相摄像机坐标x_C ^Pkm获得高视点照相摄像机坐标x_B ^Pkm(标志在高视点照相摄像机坐标系统中的坐标，在该高视点照相摄像机坐标系统中，在高视点照相摄像机180a和180b中的一个照相摄像机上的一个点被定义为原点，三个垂直相交的轴被定义为X轴、Y轴和Z轴)，和表达式(22)，用于从高视点照相摄像机坐标x_B ^Pkm获得图像坐标

其中f^B _x和f^B _y分别表示每个高视点照相摄像机180a和180b的X轴和Y轴方位上的焦距，R_CB表示描述每个高视点照相摄像机180a和180b的方位的3×3矩阵，t_CB表示描述每个高视点照相摄像机180a和180b的位置的三维向量，作为关于每个高视点照相摄像机180a和180b的已知值存储。如上面所述的，摄像装置130的位置和方位。

虽然第二实施例的第四修改方案使用多个高视点照相摄像机180a和180b，但是并不总是必须使用多个高视点照相摄像机180a和180b，明显地，即使使用单个高视点照相摄像机，也可以获得与第二实施例的第四修改方案中类似的优点。

与在第二实施例的第一修改方案到第三修改方案中使用俯瞰视角照相摄像机不同，在第二实施例的第四修改方案中描述的机构是可应用的，其中固定在摄像装置130的高视点照相摄像机180a和180b可以捕捉在世界坐标系统中安置的高视点标志P_k的图像。

其它的修改方案

虽然每个上述实施例及其修改方案使用了用表达式(9)或(27)表示的高斯-牛顿方法，用于根据误差向量U和矩阵Θ计算Δs，但是校正值Δs的计算并不总是必须使用高斯-牛顿方法实现。例如可以通过使用Levenberg-Marquardt(LM)方法进行计算，该方法为已知的求解非线性方程的迭代方法。另外，可以结合使用作为一种强估计技术的例如M估计的统计方法，并且任何其它的数值计算技术都可以使用。

每个上述实施例及其修改方案针对每个输入图像使用非线性优化技术以获得最优解(误差最小化)。然而，根据图像上的标志的误差消除误差的技术，并不限于非线性优化技术。即使使用了其它的技术，该技术的使用不会削弱本发明的性质，在本发明中，根据第二实施例中的俯瞰视角照相摄像机获得的图像信息和从从主观照相摄像机获得的图像信息，通过计算要测量的目标物体的位置和方位传感器的方位角-漂移-误差校正值，可以高准确度且稳定地获得目标物体的位置和方位。在使用例如作为已知技术的扩展Kalman滤波器和迭代扩展Kalman滤波器的情况下，根据图像上的标志的误差获得消除误差的解，在J.Park，B.Jiang，和U.Neumann，″Vision-based posecomputation：robust and accurate augmented reality tracking″，Proc.Second International Workshop on Augmented Reality(IWAR′99)，pp.3-12，1999中对其进行了详细的描述，通过将s定义为每个上述实施例和修改方案中的状态向量，将表达式(3)或(16)和(19)定义为观察方程，可能构成具有每个上述实施例的优点的滤波器。

另外，在上述实施例及其修改方案中，使用了每个标志指示着一套坐标的标志(后面称为“点标志”)。然而，标志并不限于标志类型。其它类型的标志也可以使用。

例如，每个上述的实施例及其修改方案都可以使用这样一种用在已知位置-方位测量设备中的特殊几何形状的标记作为主观视点标志和/或俯瞰视角标志(参见，例如，Takahashi，lshii，Makino，Nakashizu，″VR Intafesu-notameno Tangan-niyoru Chohokei Maka Ichi-shisei-noKoseido Jitsujikan Suiteiho(Method for Real-Time Estimation of thePosition And Orientation of Rectangular Marker through Single View forVR interface)″，Sanjigen Konfarensu 96 Koen Ronbunshu(Three-dimensional Conference′96 Collected Papers)，pp.167-172，1996)。例如，当四边形标记用作标志时，通过存储作为已知值的四边形标志的顶点的世界坐标或者从标记的位置、方位和大小计算这些值，并且从图像中检测顶点的图像坐标，可以获得与在第一实施例及其修改方案中使用四个标记的情况下获得的优点相类似的优点。特别地，可以说在要测量的目标物体上具有一个四边形标记(具有ID信息)的结构设计或者在方位传感器上具有一个四边形标记的结构设计是一种特别合适的形式，因为这种结构设计期望会有从图像进行检测标记的好的准确度和识别性。关于四边形标记，参见，例如，Kato，M.BillingHurst，Asano，Tachibana，″Maka Tsuiseki-nimotozukuKakuchogenjitsukan Shisutemu-to Sono Kyariburehshon(AugmentedReality System based on Marker Tracking and Calibration Thereof)″，Nippon Bacharu Riarithi Gakkai Ronbunshi(The Virtual Reality Societyof Japan，Collected Papers)，vol.4，no.4，pp.607-616，1999。

另外，上述的实施例及其修改方案可以使用这样的线性特征标志(后面称为“线型标志”)，如在另外一种已知的位置-方位测量设备中使用的标志(参见，例如，D.G.Low e″Fitting parameterized three-dimensional models to images″，IEEE Transactions on PAMI，vol.13，no.5.pp.441-450，1991)。例如，通过根据从图像测量值d计算出的误差Δd和状态向量s构成作为用于估计到直线原点的距离的参考的误差向量U，和通过用具有作为元素的，用于计算d^*的表达式对于状态向量s的每个元素进行偏微分而获得的1×4雅可比矩阵 $J_{\mathrm{ds}}(=\∂d/\∂s)$ 来构成矩阵Θ，就可以用一种与第一种实施例及其修改方案中类似的机制来实现对位置和方位的测量。另外，通过将从线性标志、点标志和其它标志获得的的误差和图像雅可比行列式求和，可以共同使用它们的特征。特别地，在第二实施例及其第一、第二和第三修改方案中，可能使用不同类型的标志作为主观视点标志和俯瞰视角标志。一个优选的例子是使用自然线型标志作为主观视点标志，使用着色的球形标记作为俯瞰视角标志。

在第二实施例及其修改方案中，主观视点照相摄像机530的数量为1。然而，多个主观视点照相摄像机可以架设在目标物体580上用于位置和方位的测量。在这种情况下，方位预测单元150和标志检测单元110对每个照相摄像机输出的图像进行处理。另外，方位预测单元150和位置-方位检测单元520通过使用目标物体580的位置和方位作为参考执行运算操作。位置-方位检测单元520使用一种与每个实施例中类似的机制对位置和方位进行估计，即，根据目标物体580的位置和方位角-漂移-误差校正值构成状态向量s，根据表达式(33)(对于每个照相摄像机R_CO和t_CO不同)从来自每个图像的标志信息获得每个标志的误差和图像雅可比行列式，并且累积获得的值构成误差向量U和矩阵Θ。

另外，在第二实施例的第一到第四修改方案中，摄像装置130的数量为1个。然而，即使当测量两个摄像装置时，如在立体视频透视头戴式显示器的情况中，一个摄像装置(例如，用于左眼的摄像装置)用作参考，在一种类似的技术中，可以实现位置和方位的测量。

虽然上述实施例及其修改方案使用了一种具有方位角漂移误差的方位传感器，但是任何仅在方位角方位上具有明显误差的方位传感器都可以用作方位传感器。通过使用，例如一种用加速传感器测量倾斜方向上的角度和用地磁传感器测量方位角方向的角度的方位传感器，目标物体的位置和方位可以通过与上述实施例及其修改方案相类似的处理并使用作为未知参数使用的位置和方位角-漂移-误差校正值的更新值进行测量。然而，因为在这种情况下误差特性不同于方位角漂移误差的特性，所以这种情况不适用于第一实施例的第三或第四修改方案。另外，即使使用了仅用于倾斜方向上的测量的方位传感器，通过假定该方位传感器为在方位角方位上的测量值总是为0的三轴方位传感器，目标物体的位置和方位可以通过类似的处理进行测量。

通过具有不同与可见光的波长的光捕捉图像的照相摄像机可以用作在每个上述实施例及其修改方案中的俯瞰视角照相摄像机。举例来说，通过红外线捕捉图像的照相摄像机可以用作每个俯瞰视角照相摄像机，发射或反射红外线的标志可以用作每个俯瞰视角标志。这种情况在第二实施例中具有的优点是，因为主观视点标志的图像不是由俯瞰视角照相摄像机捕捉的，所以可以消除俯瞰视角图像上的主观视点标志的错误检测。

在这种情况下，通过使用不同时刻发出红外线的标记作为每个俯瞰视角标志，可以对标志进行识别。换句话说，在标志检测单元110从俯瞰视角图像中提取出对应于标记的区域之后，该区域的重心位置可以用作以与俯瞰视角照相摄像机捕捉图像相同的定时而发出红外线的标志的检测坐标。明显地，当俯瞰视角标志的数量为1时，不需要对发光进行定时控制。

另外，在第二实施例的第一和第四修改方案中，用于通过红外线进行图像捕捉的照相摄像机可用作主观视点照相摄像机和俯瞰视角照相摄像机，并且发射或反射红外线的标志可以用作主观视点标志和俯瞰视角标志。

另外，用于通过具有与可见光不同波长的光的图像捕捉的照相摄像机并不限于通过红外线捕捉图像的照相摄像机，通过紫外线或类似光进行图像捕捉的照相摄像机也可以使用。另外，同时通过具有与可见光不同波长的光和通过可见光进行图像捕捉的照相摄像机也可以使用。

其它实施方式

明显地，在本发明中，包括实现上述实施例和修改方案的功能的软件程序代码的存储介质(或记录介质)被提供到系统或设备上，并且该系统或设备的计算机(或CPU/MPU)读取并执行存储介质上的程序代码。在这种情况下，从存储介质读取出的程序代码自身实现上述实施例和修改方案的功能。因此，包含程序代码的存储介质包括在本发明中。另外，明显地，本发明包括这样一种情况，即计算机执行读出的程序代码，从而，除了实现上述实施例和修改方案的功能，程序代码指示在计算机上运行的操作系统或类似系统执行全部或部分实际的处理，该处理执行上述实施例和修改方案的功能。

另外，明显地，本发明包括这样一种情况，即，在从存储介质读取出的程序代码被写入装载到计算机中的内插卡中的存储器中或者连接到计算机的内插单元中，程序代码指示在内插卡或内插单元中的CPU执行全部或部分实际处理，该处理执行上述实施例和修改方案的功能。

当本发明中使用上述存储介质时，对应于上述流程图的上述程序存储在存储介质中。

因为本发明可以有许多个具有很大差异但是不离开本发明的精神和范围的实施例，所以应该理解为本发明不限于其中的特定的实施例，而是应该由所附的权利要求书定义。

虽然本发明参照示意的实施例进行了说明，但是应该理解为，本发明并不限于所披露的实施例。相反，本发明意在覆盖包括在所附权利要求书的精神和范围内的各种修改方案和等同方案。下面的权利要求的范围应进行最宽的解释以便包括所有的这样的修改方案和等同的结构和功能。

本申请请求优先权的有：于2004年5月14日提交的日本专利申请No.2004-144893，于2004年5月14日提交的日本专利申请No.2004-144894，于2005年2月28日提交的日本专利申请No.2005-053441，这里都被引入作为参考。

Claims (7)

1. 一种用于计算摄像装置的位置和方位的信息处理方法，所述摄像装置捕捉现实空间的图像，该信息处理方法包括：

第一幅图像输入步骤，输入由摄像装置捕捉的第一幅图像；

第二幅图像输入步骤，输入由俯瞰视角摄像装置捕捉的第二幅图像，该俯瞰视角摄像装置从俯瞰视角位置捕捉摄像装置的图像；

方位输入步骤，输入从方位传感器输出的测量的方位值，该方位传感器测量关于摄像装置的方位的信息；

第一检测步骤，从在第一幅图像输入步骤中输入的第一幅图像检测关于置于现实空间中的第一标志的图像坐标的第一-标志图像-坐标特征值；

第二检测步骤，从在第二幅图像输入步骤中输入的第二幅图像检测关于置于摄像装置上的第二标志的图像坐标的第二-标志图像-坐标特征值；和

位置-方位计算步骤，通过使用在第一检测步骤中检测到的第一标志图像坐标特征值、在第二检测步骤中检测到的第二标志图像坐标特征值和在方位输入步骤中输入的测量出的方位值，计算摄像装置的位置和方位。

2. 根据权利要求1的信息处理方法，其中，在位置-方位计算步骤，通过使用在第一检测步骤中检测出的第一-标志图像-坐标特征值和在第二检测步骤中检测到的第二-标志图像-坐标特征值，获得关于校正方位传感器的方位角误差的校正值的参数和表示摄像装置位置的参数，通过使用所获得的参数对摄像装置的位置和方位进行计算。

3. 根据权利要求1的信息处理方法，其中：

在第二检测步骤中检测到的第二-标志图像-坐标特征值表示第二标志的图像坐标；

在位置-方位计算步骤中，根据由第二-标志图像-坐标特征值表示的第二标志的图像坐标和俯瞰视角摄像装置的位置和方位，获得在三维空间中限制第二标志位置的直线，并且，在第二标志位于直线上这个限制条件下，通过使用在第一检测步骤中检测到的第一-标志图像-坐标特征值和在方位输入步骤中输入的测量出的方位值，计算摄像装置的位置和方位。

4. 根据权利要求3的信息处理方法，其中，在位置-方位计算步骤中，通过使用在第一检测步骤中检测到的第一-标志图像-坐标特征值，获得表示直线上第二标志的位置的参数和关于校正方位传感器的方位角误差的校正值的参数，并且通过使用所获得的参数对摄像装置的位置和方位进行计算。

5. 一种用于计算摄像装置的位置和方位的信息处理方法，所述摄像装置捕捉现实空间的图像，所述信息处理方法包括：

第一幅图像输入步骤，输入由摄像装置捕捉的第一幅图像；

第二幅图像输入步骤，输入由第二摄像装置捕捉的第二幅图像，所述第二摄像装置从一个视点位置捕捉现实空间的图像，所述第二摄像装置布置在所述摄像装置上面；

方位输入步骤，输入从方位传感器输出的测量的方位值，所述方位传感器测量关于摄像装置方位的信息；

第一检测步骤，从在第一幅图像输入步骤中输入的第一幅图像，检测关于置于现实空间中的第一标志的图像坐标的第一-标志图像-坐标特征值；

第二检测步骤，从在第二幅图像输入步骤中输入的第二幅图像，检测关于置于现实空间中的第二标志的图像坐标的第二-标志图像-坐标特征值；

位置和方位计算步骤，通过使用在第一检测步骤中检测到的第一-标志图像-坐标特征值、在第二检测步骤中检测到的第二-标志图像-坐标特征值和在方位输入步骤中输入的测量出的方位值，计算摄像装置的位置和方位。

6. 一种用于计算摄像装置的位置和方位的信息处理设备，所述摄像装置捕捉现实空间的图像，所述信息处理设备包括：

第一幅图像输入单元，输入由摄像装置捕捉的第一幅图像；

第二幅图像输入单元，输入由俯瞰视角摄像装置捕捉的第二幅图像，所述俯瞰视角摄像装置从俯瞰视角位置捕捉该摄像装置的图像；

方位输入单元，输入从方位传感器输出的测量的方位值，所述方位传感器测量关于摄像装置方位的信息；

第一检测单元，从由第一幅图像输入单元输入的第一幅图像，检测关于置于现实空间中的第一标志的图像坐标的第一-标志图像-坐标特征值；

第二检测单元，从由第二幅图像输入单元输入的第二幅图像，检测关于置于该摄像装置上的第二标志的图像坐标的第二-标志图像-坐标特征值；

位置-方位计算单元，通过使用由第一检测单元检测到的第一-标志图像-坐标特征值、由第二检测单元检测到的第二-标志图像-坐标特征值和由方位输入单元输入的测量出的方位值，计算摄像装置的位置和方位。

7. 一种用于计算摄像装置位置和方位的信息处理设备，所述摄像装置捕捉现实空间的图像，所述信息处理设备包括：

第一幅图像输入单元，输入由摄像装置捕捉的第一幅图像；

第二幅图像输入单元，输入由第二摄像装置捕捉的第二幅图像，所述第二摄像装置从一个视点位置捕捉现实空间的图像，所述第二摄像装置布置在所述摄像装置上面；

方位输入单元，输入从方位传感器输出的测量的方位值，所述方位传感器测量关于摄像装置方位的信息；

第一检测单元，从由第一幅图像输入单元输入的第一幅图像，检测关于置于现实空间中的第一标志的图像坐标的第一-标志图像-坐标特征值；

第二检测单元，从由第二幅图像输入单元输入的第二幅图像，检测关于置于现实空间中的第二标志的图像坐标的第二-标志图像-坐标特征值；和

位置和方位计算单元，通过使用由第一检测单元检测到的第一-标志图像-坐标特征值、由第二检测单元检测到的第二-标志图像-坐标特征值和由方位输入单元输入的测量出的方位值，计算摄像装置的位置和方位。

Images