CN1847789A - 测量位置及姿态的方法和装置 - Google Patents

Abstract

不执行迭代运算而获得其上安装了倾斜传感器的图像捕捉设备或目标的位置及姿态。一种位置及姿态测量方法，包括以下步骤：输入倾斜传感器的测量倾斜值，该倾斜传感器安装在目标和图像捕捉设备的至少一个上；输入来自图像捕捉设备的被捕捉图像；根据被捕捉图像检测目标上的标记；以及基于测量倾斜值、被检测出的标记的图像坐标和关于被检测出的标记的已知的位置信息，计算其上安装了倾斜传感器的目标和图像捕捉设备其中之一的位置及姿态，而不执行迭代运算。

Description

测量位置及姿态的方法和装置

技术领域

本发明涉及一种根据倾斜传感器的测量值直接测量图像捕捉设备或目标的位置及姿态的技术。

背景技术

近年来，对于混合现实(mixed reality)(MR)技术的研究已经不断取得进步。MR技术是指无缝地整合物理空间与计算机产生的虚拟空间的技术。在MR技术中，用于在物理空间上叠加虚拟空间的增强现实(augmented reality)(AR)技术引起了特别的关注。

通过视频透明(see-through)方法或光学透明方法来实现使用AR技术的图像显示设备。按照视频透明方法，显示了通过在物理空间的图像上叠加虚拟空间的图像而形成的合成图像，其中物理空间的图像是由例如视频照相机的图像捕捉设备捕捉的，虚拟空间的图像(例如通过计算机图形或文本信息绘制的虚拟目标/对象)是根据图像捕捉设备的位置及姿态产生的。同时，按照光学透明方法，在安装到观察者头部的光学透明显示器上显示了根据观察者眼睛的位置以及观察者头部的姿态而产生的虚拟空间的图像。

现在期望将AR技术应用在各个领域，例如用于在身体表面叠加并显示患者体内状况的外科协助，用于在空的建筑地点叠加并显示虚拟建筑物的建筑模拟，以及用于叠加并显示装配工作期间的操作流程的装配协助。

在AR技术中一个最重要的问题是如何在物理空间与虚拟空间之间执行精确的对准(重合)，迄今为止已经提出了大量的方法。在使用视频透明方法的情况下，AR技术中对准的问题本质上是获得图像捕捉设备在现场(更具体地，在现场中定义的世界坐标系中)中的位置及姿态的问题。类似地，在使用光学透明方法的情况下，AR技术中对准的问题导致了获得观察者眼睛的位置和观察者头部的姿态或者显示器在现场中的位置及姿态的问题。

作为解决前面问题的一种方法，在现场排列或设置多个标记(index)，并检测这些标记在图像捕捉设备捕捉到的图像上的投影位置，由此获得图像捕捉设备在现场中的位置及姿态。作为解决后面问题的一种方法，将图像捕捉设备安装到测量对象(即，观察者的头部或者显示器)，按照与前面方面类似的方式获得图像捕捉设备的位置及姿态，然后基于图像捕捉设备的位置及姿态来获得测量对象的位置及姿态。

基于标记在被捕捉图像上的投影位置与标记的三维位置之间的对应关系来获得图像捕捉设备的位置及姿态的方法已经在摄影测量学领域中提出多年。此外，Kato、M.Billinghurst、Asano和Tachibana的论文：“Augmented reality system on the basis of marker trackingand calibration therefor”，日本虚拟现实协会的刊物，vol.4、no.4、pp.607-616(1999)，公开了一种将基于标记在被捕捉图像上的投影位置而获得的图像捕捉设备的位置及姿态设为初始值的过程。之后，进行迭代运算以使标记在被捕捉图像上的实际观察位置与投影标记的计算位置(即，根据标记的三维位置计算出的任何位置与图像捕捉设备的位置及姿态)之间的任何位置误差最小化，来优化图像捕捉设备的位置及姿态。

或者，取代仅使用由图像捕捉设备捕捉的图像，执行一种过程使得陀螺仪传感器(确切的说，通过组合用于在三轴方向上测量角速度的多个陀螺仪传感器和在三轴方向上的多个加速度传感器来构成三轴姿态传感器；但是此处为了方便将其称作陀螺仪传感器)安装在图像捕捉设备上，以便在使用图像信息和陀螺仪传感器信息的同时获得图像捕捉设备的位置及姿态。

美国专利申请No.2004-0090444公开了一种方法，其使用从陀螺仪传感器输出的姿态的测量值作为图像捕捉设备的姿态，并基于图像捕捉设备的姿态以及标记在被捕捉图像上的投影标记与标记的三维位置之间的对应关系来求解线性联立方程以获得图像捕捉设备的位置。此外，K.Satoh、S.Uchiyama和H.Yamamoto的论文：“A headtracking method using bird’s-eye view camera and gyroscope”，Proc.3rd IEEE/ACM Int’l Symp.on Mixed and Augmented Reality(ISMAR 2004)，pp.89-98，2004，建议一种过程，其使用从陀螺仪传感器输出的测量值的倾斜分量作为图像捕捉设备的姿态的倾斜分量。之后，进行迭代运算，用于优化图像捕捉设备的位置以及姿态的方位角分量，以使计算出的位置与在上述被捕捉图像上的投影标记的观察位置之间的位置误差最小化。

按照上述的仅使用标记在由图像捕捉设备所捕捉的图像上的投影标记的方法，因为出现标记在被捕捉图像上的检测误差或者因为图像的分辨率有限，所以由此获得的图像捕捉设备的位置及姿态可能是不准确的，或者可能具有抖动。

然后，陀螺仪传感器涉及漂移误差的产生。因此，随着时间流逝，在方位角方向上逐渐产生测量值的误差。由于此原因，当把陀螺仪传感器输出的测量姿态值设置为图像捕捉设备的实际姿态时，在某些情况下可能获得图像捕捉设备的不准确的位置。

而且，为了通过迭代运算获得图像捕捉设备的位置及姿态或者图像捕捉设备的位置以及姿态的方位角分量，需要初始值。当前一帧(frame)的结果用作初始值时，如果在前一帧中没有获得图像捕捉设备的位置及姿态(例如，在前一帧中没有捕捉标记的情况下)，则不能够获得初始值。当仅把根据被捕捉图像计算的图像捕捉设备的位置及姿态用作初始值时，根据以上描述从一开始初始值可能就是不准确的。因此，即使之后进行迭代运算，也不能偶尔地实现最佳化。

发明内容

考虑到上述问题而提出本发明。按照本发明的第一方面，提供一种不执行迭代运算而获得其上安装了倾斜传感器的图像捕捉设备或目标的位置及姿态的方法。

按照本发明的第二方面，提供一种不执行迭代运算而获得其上安装了倾斜传感器的图像捕捉设备与其上安装了另一个倾斜传感器的目标之间的相对位置及相对姿态的方法。

按照本发明的第三方面，本发明提供一种采用下述特征的位置及姿态测量装置。

按照本发明的第四方面，本发明提供一种采用下述特征的程序。

按照第一方面，本发明提供一种位置及姿态测量方法，包括以下步骤：输入倾斜传感器的测量倾斜值，该倾斜传感器安装在目标和图像捕捉设备的至少一个上；输入来自所述图像捕捉设备的被捕捉图像；根据所述被捕捉图像检测所述目标上的标记；以及基于所述测量倾斜值、被检测出的标记的图像坐标和关于被检测出的标记的已知的位置信息，计算其上安装了所述倾斜传感器的目标和图像捕捉设备其中之一的位置及姿态，而不执行迭代运算。

按照第二方面，本发明提供一种位置及姿态测量方法，包括以下步骤：输入安装到目标上的第一倾斜传感器的第一测量倾斜值；输入安装到图像捕捉设备上的第二倾斜传感器的第二测量倾斜值；输入来自所述图像捕捉设备的被捕捉图像；根据所述被捕捉图像检测所述目标上的标记；以及基于第一测量倾斜值、第二测量倾斜值、被检测出的标记的图像坐标以及关于被检测出的标记的已知的位置信息，计算所述目标与所述图像捕捉设备的相对位置和相对姿态，而不执行迭代运算。

根据结合附图的下面的说明，本发明其它的特征及优点将显而易见，其中在全部附图中类似的参考标记表示相同或相似的部分。

附图说明

附图并入和构成说明书的一部分，其与说明书一起给出了本发明的实施例，附图用于说明本发明的原理：

图1显示了按照本发明第一实施例的位置及姿态测量装置；

图2显示了按照第一实施例的坐标系之间的关系；

图3是按照第一实施例的方法的流程图；

图4显示了按照本发明第二实施例的位置及姿态测量装置；

图5显示了按照第二实施例的坐标系之间的关系；

图6是按照第二实施例的方法的流程图；

图7显示了按照本发明第三实施例的位置及姿态测量装置；

图8显示了按照第三实施例的坐标系之间的关系；

图9是按照第三实施例的方法的流程图。

具体实施方式

下面将结合附图详细说明本发明的优选实施例。

第一实施例

在此实施例中，将对使用按照本发明的位置及姿态测量装置及其方法来测量安装了陀螺仪传感器的图像捕捉设备的位置及姿态的情况进行说明。应当注意，此实施例的陀螺仪传感器是指如上所述的通过组合用于在三轴方向上测量角速度的多个陀螺仪传感器和用于在三轴方向上测量加速度的多个加速度传感器构成的三轴姿态传感器。倾斜分量的测量值以某种程度的可靠性从陀螺仪传感器输出。但是，方位角分量的测量值含有漂移误差。基于此原因，此实施例不仅可适用于陀螺仪传感器，还可适用于任何能够测量姿态的倾斜分量的倾斜传感器。

图1显示了此实施例的位置及姿态测量装置的结构。如图1所示，此实施例的位置及姿态测量装置100包括标记检测部分110、姿态输入部分120和位置及姿态计算部分130。位置及姿态测量装置100连接到图像捕捉设备140。陀螺仪传感器150固定在图像捕捉设备140上，并连接到姿态输入部分120。

图2显示了按照第一实施例的坐标系(统)之间的关系。在世界坐标系中，排列或设置每个已知位置的标记。按照此实施例的位置及姿态测量装置测量图像捕捉设备相对于世界坐标系的位置及姿态。在此实施例中，假设世界坐标系中的y_w轴处于垂直向上的方向。

陀螺仪传感器150输出图像捕捉设备相对于世界坐标系的姿态。将指示图像捕捉设备相对于世界坐标系的姿态的3×3旋转矩阵表示为R_wc。将指示图像捕捉设备相对于世界坐标系的位置的三维位置向量表示为t_wc。能够通过以规定的顺序组合(下述的)旋转变换来表示R_wc：下面给出了以世界坐标系的z_w轴为中心的旋转变换，

R_wc ^zw                ……(1)

下面给出了围绕世界坐标系的X_w轴的旋转变换，

R_wc ^xw               ……(2)

下面给出了围绕世界坐标系的Y_w轴的旋转变换，

R_wc ^yw               ……(3)

因此，R_wc能够由下面所示的表达式(1)表示，

$R_{\mathrm{wc}}=R_{\mathrm{wc}}^{y_w}\·R_{\mathrm{wc}}^{x_w}\·R_{\mathrm{wc}}^{\mathrm{zw}}---\left(4\right)$

表达式(1)

在表达式(1)中，R_wc的倾斜分量被如下表示，

R_wc ^xw，R_wc ^zw        ……(5)

R_wc的方位角分量被表示如下，

R_wc ^yw               ……(6)

然后，进行下面的赋值。

$R_{\mathrm{wc}}^i=R_{\mathrm{wc}}^{x_w}\·R_{\mathrm{wc}}^{z_w},R_{\mathrm{wc}}^a=R_{\mathrm{wc}}^{y_w}---\left(7\right)$

表达式(1)能够表示为表达式(2)(此处i表示倾斜，a表示方位角)。

$R_{\mathrm{wc}}=R_{\mathrm{wc}}^a\·R_{\mathrm{wc}}^i---\left(8\right)$

表达式(2)

因此，能够以下面的表达式(3)表示从世界坐标系到图像捕捉设备的坐标系(以后称作图像捕捉设备坐标系)的坐标变换。此时，使用下列各项：

R_wc ⁱ，R_wc ^a         ……(9)

此外，使用世界坐标系原点在图像捕捉设备坐标系中的位置t_cw。

$x_c={\left(R_{\mathrm{wc}}^i\right)}^{-1}\·{\left(R_{\mathrm{wc}}^a\right)}^{-1}\·x_w+t_{\mathrm{cw}}---\left(10\right)$

表达式(3)

在表达式(3)中，x_w和x_c分别表示在世界坐标系和图像捕捉设备中的点(point)的坐标。

此处，说明下面的(11)，

(R_wc ⁱ)^-1             ……(11)

将上面的(11)表示为表达式(4)，

${\left(R_{\mathrm{wc}}^i\right)}^{-1}=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]---\left(12\right)$

表达式(4)

由θ表示方位角，

(R_wc ^a)^-1                ……(13)

将上面的(13)表示为表达式(5)，

${\left(R_{\mathrm{wc}}^a\right)}^{-1}=\left[\begin{array}{ccc}\cos \mathrm{\θ}& 0& -\sin \\ 0& 1& 0\\ \sin \mathrm{\θ}& 0& \cos \mathrm{\θ}\end{array}\right]---\left(14\right)$

表达式(5)

将标记在被捕捉图像上的投影位置(u_x，u_y)表示为表达式(6)，其中标记在世界坐标系中的位置是x_w＝[x_wy_wz_w]^t，在表达式(6)中用f表示焦距，α＝cosθ，β＝sinθ，x_c＝[x_cy_cz_c]^t，t_cw＝[t_xt_yt_z]^t。

$u_x=-f\frac{x_c}{z_c}=-f\frac{(r_{11}{x}_w+r_{13}{z}_w)\mathrm{\α}+(r_{13}{x}_w-r_{11}{z}_w)\mathrm{\β}+t_x+r_{12}{y}_w}{(r_{31}{x}_w+r_{33}{z}_w)\mathrm{\α}+(r_{33}{x}_w-r_{31}{z}_w)\mathrm{\β}+t_z+r_{32}{y}_w}$

$u_y=-f\frac{y_c}{z_c}=-f\frac{(r_{21}{x}_w+r_{23}{z}_w)\mathrm{\α}+(r_{23}{x}_w-r_{21}{z}_w)\mathrm{\β}+t_v+r_{22}{y}_w}{(r_{31}{x}_w+r_{33}{z}_w)\mathrm{\α}+(r_{33}{x}_w-r_{31}{z}_w)\mathrm{\β}+t_z+r_{32}{y}_w}---\left(15\right)$

表达式(6)

接下来，将说明通过使用在图像捕捉设备所捕捉的图像上检测到的标记和陀螺仪传感器输出的姿态的倾斜分量来计算图像捕捉设备的位置及姿态的方法。图3是按照第一实施例的方法的流程图。

首先，在步骤S1010中，标记检测部分110输入来自图像捕捉设备140的图像，姿态输入部分120输入图像捕捉期间陀螺仪传感器的测量值。姿态输入部分120将陀螺仪传感器的测量值R_wc分解成由下面(16)表示的倾斜分量以及由下面(17)表示的方位角分量，

R_wc ⁱ                    ……(16)

R_wc ^a                    ……(17)

根据表达式(4)，获得r₁₁、r₁₂、r₁₃、r₂₁、r₂₂、r₂₃、r₃₁、r₃₂和r₃₃并将其输出到位置及姿态计算部分130。然后，在步骤S1020中，标记检测部分110在被捕捉图像上检测标记，并将检测到的标记输出到位置及姿态计算部分130。此处，将在图像上检测到的标记的数量设为n，

(u_xi，u_yi)              ……(18)

上面的(18)说明被检测出的标记i(i＝1，2，...，n)在图像上的检测位置。

随后，在步骤S1030中，位置及姿态计算部分130判断是否所有的被检测出的标记都处于同一水平面上(通过yw是常数的形式来表示此实施例的水平面)。例如，以下面的方式来进行是否所有的被检测出的标记都处于同一水平面上的判断。被检测出的标记i(i＝1，2，...，n)在世界坐标系中的位置被表示如下，

$x_{w_i}={\left[\begin{array}{ccc}{x}_{w_I}& y_{w_i}& z_{w_i}\end{array}\right]}^t---\left(19\right)$

从表达式(7)获得被检测出的标记的y_w坐标的方差σ²。

${\mathrm{\σ}}^2=\frac{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{(y_{w_i}-{\stackrel{\‾}{y}}_w)}^2}{n}---\left(20\right)$

表达式(7)

在表达式(7)中，标记的y_w坐标的平均值被表示如下，

y_w            ……(21)

从表达式(8)获得该平均值，

${\stackrel{\‾}{y}}_w=\frac{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{y}_{w_i}}{n}---\left(22\right)$

表达式(8)

当方差σ²等于或小于预定值时，则判断出所有的被检测出的标记都位于所述同一水平面。在步骤S1030中，当判断出被检测出的标记都位于同一水平面时，流程前进到步骤S1050，当判断出被检测出的标记没有位于同一水平面时，流程前进到步骤S1040。

在步骤S1040中，位置及姿态计算部分130判断被检测出的标记的数量是否为3或更高。在步骤S1040中，当判断出被检测出的标记的数量为3或更高时，流程前进到步骤S1060，当判断出被检测出的标记的数量小于3时，流程结束。在步骤S1050中，判断被检测出的标记的数量是否为2或更高。在步骤S1050中，当判断出被检测出的标记的数量为2或更高时，流程前进到步骤S1070，当判断出被检测出的标记的数量小于2时，流程结束。

在步骤S1060中，位置及姿态计算部分130采用过程(1A)来计算图像捕捉设备的位置及姿态，并输出该位置及姿态。现在描述过程(1A)。

通过对分数的分母和分子的除法处理来简化表达式(6)。然后进行下面的赋值。

u′_x＝-u_x/f，u′_y＝-u_y/f    ……(23)

结果，获得了表达式(9)中所示的α、β、t_x、t_y和t_z的线性方程，

(u′_x(r₃₁x_w+r₃₃z_w)-(r₁₁x_w+r₁₃z_w))α+(u′_x(r₃₃x_w-r₃₁z_w)-(r₁₃x_w-r₁₁z_w))β-t_x+u′_xt_z＝r₁₂y_w-u′_xr₃₂y_w

(u′_y(r₃₁x_w+r₃₃z_w)-(r₂₁x_w+r₂₃z_w))α+(u′_y(r₃₃x_w-r₃₁z_w)-(r₂₃x_w-r₂₁z_w))β-t_y+u′_yt_z＝r₂₂y_w-u′_yr₃₂y_w

                                                                    ……(24)

表达式(9)

各个输入标记(n个点)被表示如下，

$u_{x_i}^{\′}=-u_{x_i}/f,u_{y_i}^{\′}=-u_{y_i}/f---\left(25\right)$

然后，为各个标记建立表达式(9)的线性方程，获得表达式(10)所示的联立方程，

表达式(10)

通过求解表达式(10)，获得了α、β、t_x、t_y、和t_z。在这些联立方程中，未知数是5个，因此至少需要3个被检测出的标记。如表达式(11)所示的那样来简化并表示表达式(10)。

A·x＝b    表达式(11)

其中x＝[αβt_xt_yt_z]^t。A是行数为(2×n)列数为5的矩阵，b是(2×n)维向量。通过最小平方法，获得如表达式(12)表示的x，

x＝(A^t·A)^-1·A^t·b    表达式(12)

应当注意，在表达式(12)中将A的广义逆矩阵设置为(A^t·A)^-1，但是在不损害本发明本质的情况下也可以使用其它的广义逆矩阵。此外，通过表达式(13)利用从表达式(12)获得的α和β来计算方位角θ，

$\mathrm{\&theta;}=\left\{\begin{array}{c}\arctan \left(\frac{\mathrm{\&beta;}}{\mathrm{\&alpha;}}\right)(\mathrm{\&alpha;}\&NotEqual;0)\\ \frac{\mathrm{\&pi;}}{2}(\mathrm{\&alpha;}=0,\mathrm{\&beta;}>0)\\ -\frac{\mathrm{\&pi;}}{2}(\mathrm{\&alpha;}=0,\mathrm{\&beta;}<0)\end{array}\right.---\left(27\right)$

表达式(13)

为了计算图像捕捉设备相对于世界坐标系的姿态R_wc，使用由此获得的θ，并且也使用如下表示的传感器的测量值的倾斜分量，

R_wc ⁱ           ……(28)

通过表达式(14)进行该计算，

$R_{\mathrm{wc}}=R_{\mathrm{wc}}^a\&CenterDot;R_{\mathrm{wc}}^i$

$=\left[\begin{array}{ccc}\cos \mathrm{\&theta;}& 0& \sin \mathrm{\&theta;}\\ 0& 1& 0\\ -\sin \mathrm{\&theta;}& 0& \cos \mathrm{\&theta;}\end{array}\right]\&CenterDot;R_{\mathrm{wc}}^i---\left(29\right)$

表达式(14)

之后，通过表达式(15)来计算图像捕捉设备相对于世界坐标系的位置t_wc。

t_wc＝-R_wc·t_cw    表达式(15)

通过上述的方法，在步骤S1060中，计算并输出图像捕捉设备的位置及姿态。

在步骤S1070中，位置及姿态计算部分130采用过程(1B)来计算图像捕捉设备的位置及姿态，并输出该位置及姿态。现在说明过程(1B)。

将标记在世界坐标系中的y_w坐标设为y_avg。在表达式(6)中，y_w＝0并进行下面的赋值，

α′＝α/t_z，β′＝β/t_z，t′_x＝t_x/t_z，t′_y＝t_y/t_z    ……(30)

通过对分数的分母和分子的除法处理来进行分数的简化。此外，进行下面的赋值，

u′_x＝-u_x/f，u′_y＝-u_y/f    ……(31)

结果，获得了表达式(16)，

(u′_x(r₃₁x_w+r₃₃z_w)-r₁₁x_w-r₁₃z_w)α′+(u′_x(r₃₃x_w-r₃₁z_w)-r₁₃x_w+r₁₁z_w)β′-t′_x＝-u′_x

(u′_y(r₃₁x_w+r₃₃z_w)-r₂₁x_w-r₂₃z_w)α′+(u′_y(r₃₃x_w-r₃₁z_w)-r₂₃x_w+r₂₁z_w)β′-t′_y＝-u′_y

                                                                             ……(32)

表达式(16)

类似地，对于各个输入标记(n个点)，进行下面的赋值，

u′_xi＝-u_xi/f，u′_yi＝-u_yi/f    ……(33)

然后，针对各个标记建立表达式(16)的线性方程，并获得表达式(17)所示的联立方程，

表达式(17)

通过表达式(12)所示的方法求解表达式(17)，获得了α′、β′、t′_x和t′_y。在这些联立方程中，未知数的个数是4个，因此至少需要2个被检测出的标记。

基于通过求解表达式(17)所获得的α′和β′，使用表达式(18)来计算方位角θ，

$\mathrm{\&theta;}=\left\{\begin{array}{cc}\arctan \left(\frac{{\mathrm{\&beta;}}^{\&prime;}}{{\mathrm{\&alpha;}}^{\&prime;}}\right)& ({\mathrm{\&alpha;}}^{\&prime;}\&NotEqual;0)\\ \frac{\mathrm{\&pi;}}{2}& ({\mathrm{\&alpha;}}^{\&prime;}=0,{\mathrm{\&beta;}}^{\&prime;}>0)\\ -\frac{\mathrm{\&pi;}}{2}& ({\mathrm{\&alpha;}}^{\&prime;}=0,{\mathrm{\&beta;}}^{\&prime;}<0)\end{array}\right.---\left(35\right)$

表达式(18)

为了通过表达式(14)计算图像捕捉设备相对于世界坐标系的姿态R_wc，使用由此获得的θ，并且也使用如下表示的传感器的测量值的倾斜分量。

R_wc ⁱ                          ……(36)

此外，通过表达式(19)来获得t_cw＝[t_xt_yt_z]^t，

$t_z=\left\{\begin{array}{cc}\frac{\mathrm{\&alpha;}}{{\mathrm{\&alpha;}}^{\&prime;}}& ({\mathrm{\&alpha;}}^{\&prime;}\&NotEqual;0)\\ \frac{\mathrm{\&beta;}}{{\mathrm{\&beta;}}^{\&prime;}}& ({\mathrm{\&alpha;}}^{\&prime;}=0)\end{array}\right.$

t_x＝t′_x·t_z                  ……(37)

t_y＝t′_y·t_z

表达式(19)

此外，通过表达式(20)来计算图像捕捉设备相对于世界坐标系的位置t_wc，

$t_{\mathrm{wc}}=-R_{\mathrm{wc}}\&CenterDot;t_{\mathrm{wc}}+\left[\begin{array}{c}0\\ y_{\mathrm{avg}}\\ 0\end{array}\right]---\left(38\right)$

表达式(20)

通过上述的方法，在步骤S1070中，计算并输出图像捕捉设备的位置及姿态。

如上所述，按照此实施例，当陀螺仪传感器安装到图像捕捉设备时，能够通过其中迭代运算为非必要步骤的方法，根据传感器的测量值的倾斜分量以及图像信息直接计算图像捕捉设备的位置及姿态。

也就是，在陀螺仪传感器安装到图像捕捉设备之后，在不执行迭代运算的情况下(换言之，没有使用初始值)，通过使用该图像捕捉设备捕捉的图像以及仅使用陀螺仪传感器输出的测量姿态值的倾斜分量，能够直接获得图像捕捉设备的位置及姿态。结果，能够避免在进行迭代运算时产生的由于初始值引起的位置及姿态计算精度的下降。实际上，当在被捕捉图像上捕捉到标记时，能够高精度地计算位置及姿态。在此实施例中，已经说明了采用由一个照相机所产生的图像信息的位置及姿态测量方法。按照某种AR应用，可以使用立体显示器，因此存在将立体照相机安装到头佩式显示器(HMD)的情况。在安装了这样的HMD传感器的情况下，如果将一个倾斜传感器安装到构成立体配对的每个照相机，则每个照相机的倾斜如同使用一个照相机的情况一样被唯一地确定。因此，在此实施例所述的位置及姿态测量方法中，也可以使用从立体照相机的各个照相机获得的标记。此外，只要照相机之间的相对位置和相对姿态是已知的，则照相机的数量不限于两个，也可使用任何数量的照相机。

第二实施例

在第一实施例中，将陀螺仪传感器安装到图像捕捉设备以获得图像捕捉设备的位置及姿态。而在第二实施例中，将对使用位置及姿态测量装置及其方法来测量由固定在世界坐标系中的图像捕捉设备所捕捉的移动目标的位置及姿态的情况进行说明。

图4显示了按照第二实施例的位置及姿态测量装置的结构。如图4所示，按照第二实施例的位置及姿态测量装置200包括标记检测部分210、姿态输入部分220和位置及姿态计算部分230。固定在世界坐标系中的图像捕捉设备240连接到标记检测部分210。安装在移动目标260上的陀螺仪传感器250连接到姿态输入部分220。在第二实施例中，使用在由图像捕捉设备240捕捉的图像中检测到的移动目标上的标记以及安装在移动目标上的陀螺仪传感器的测量值的倾斜分量，来测量移动目标相对于世界坐标系的位置及姿态。

图5显示了按照第二实施例的坐标系之间的关系。将移动目标坐标系中每个已知位置的标记排列或设置在移动目标上。将图像捕捉设备固定在世界坐标系中，并且表示位置的三维位置向量t_wc以及表示图像捕捉设备在世界坐标系中的姿态的3×3旋转矩阵R_wc是已知的。在第二实施例中，获得移动目标相对于世界坐标系的姿态R_wo和位置t_wo。

将表示移动目标相对于世界坐标系的姿态的3×3旋转矩阵R_wo分解成如下表示的倾斜分量，

R_wo ⁱ            ……(39)

此外，将旋转矩阵分解成如下表示的方位角分量，

R_wo ^a            ……(40)

按照表达式(21)所示的进行该分解，

$R_{\mathrm{wo}}=R_{\mathrm{wo}}^a\&CenterDot;R_{\mathrm{wo}}^i---\left(41\right)$

表达式(21)

能够通过使用下列各项来表示从移动目标坐标系x_o到图像捕捉设备坐标系x_c的坐标变换，

R_wo ⁱ，R_wo ^a      ……(42)

此外，将移动目标在图像捕捉设备坐标系中的位置t_co用在表达式(22)中，

$x_c={\left(R_{\mathrm{wc}}\right)}^{-1}\&CenterDot;R_{\mathrm{wo}}^a\&CenterDot;R_{\mathrm{wo}}^i\&CenterDot;x_o+t_{\mathrm{co}}---\left(43\right)$

表达式(22)

此处，进行下面的过程，

(R_wc)^-1             ……(44)

将上面的(44)表示为表达式(23)，

${\left(R_{\mathrm{wc}}\right)}^{-1}=\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]---\left(45\right)$

表达式(23)

此外，由θ表示方位角。

R_wo ^a                    ……(46)

将上面的(46)表示为表达式(24)，

$R_{\mathrm{wo}}^a=\left[\begin{array}{ccc}\cos \mathrm{\&theta;}& 0& \sin \mathrm{\&theta;}\\ 0& 1& 0\\ -\sin \mathrm{\&theta;}& 0& \cos \mathrm{\&theta;}\end{array}\right]---\left(47\right)$

表达式(24)

然后，进行下面的赋值，

${X^{\&prime;}}_o={\left[{x^{\&prime;}}_o{y^{\&prime;}}_o{z^{\&prime;}}_o\right]}^t=R_{\mathrm{wo}}^i\&CenterDot;X_o---\left(48\right)$

在移动目标坐标系中位于x_o＝[x_oy_oz_o]^t的标记在被捕捉图像上具有位置(u_x，u_y)。当由f表示焦距并且α＝cosθ、β＝sinθ、x_c＝[x_cy_cz_c]^t，t_co＝[t_oxt_oyt_oz]^t时，将投影位置表示为表达式(25)，

$u_x=-f\frac{x_c}{z_c}=-f\frac{(R_{11}{x^{\&prime;}}_o+R_{13}{z^{\&prime;}}_o)\mathrm{\&alpha;}+(R_{11}{z^{\&prime;}}_c-R_{13}{x^{\&prime;}}_o)\mathrm{\&beta;}+t_{\mathrm{ox}}+R_{12}{y^{\&prime;}}_o}{(R_{31}{x^{\&prime;}}_o+R_{33}{z^{\&prime;}}_o)\mathrm{\&alpha;}+(R_{31}{z^{\&prime;}}_o-R_{33}{x^{\&prime;}}_o)\mathrm{\&beta;}+t_{\mathrm{oz}}+R_{32}{y^{\&prime;}}_o}$

$u_y=-f\frac{y_c}{z_c}=-f\frac{(R_{21}{x^{\&prime;}}_o+R_{23}{z^{\&prime;}}_o)\mathrm{\&alpha;}+(R_{21}{z^{\&prime;}}_c-R_{23}{x^{\&prime;}}_o)\mathrm{\&beta;}+t_{\mathrm{oy}}+R_{22}{y^{\&prime;}}_o}{(R_{31}{x^{\&prime;}}_o+R_{33}{z^{\&prime;}}_o)\mathrm{\&alpha;}+(R_{31}{z^{\&prime;}}_o-R_{33}{x^{\&prime;}}_o)\mathrm{\&beta;}+t_{\mathrm{oz}}+R_{32}{y^{\&prime;}}_o}---\left(49\right)$

表达式(25)

接下来，将说明通过使用在图像捕捉设备所捕捉的图像上检测到的标记和陀螺仪传感器输出的倾斜分量来计算移动目标的位置及姿态的方法。图6是按照第二实施例的方法的流程图。

首先，在步骤S2010中，标记检测部分210输入来自图像捕捉设备240的图像。姿态输入部分220输入在图像捕捉的同时安装在移动目标上的陀螺仪传感器的测量值R_wo，并将该测量值分解成如下表示的倾斜分量，

R_wo ⁱ         ……(50)

此外，将该测量值分解成如下所示的方位角分量，

R_wo ^a         ……(51)

从表达式(23)，获得已知的值R₁₁、R₁₂、R₁₃、R₂₁、R₂₂、R₂₃、R₃₁、R₃₂和R₃₃，并将其输出到位置及姿态计算部分230。接下来，在步骤S2020中，标记检测部分210根据被捕捉图像检测移动目标上的标记，以便将检测到的标记输出到位置及姿态计算部分230。此处，将被捕捉图像上检测到的标记的数量设为n，并且被检测出的标记i(i＝1，2，...，n)在被捕捉图像上的检测位置被表示如下，

(u_xi，u_yi)           ……(52)

接下来，在步骤S2030中，位置及姿态计算部分230判断是否所有的被检测出的标记都处于同一水平面上。例如，以下面的方式来进行是否所有的被检测出的标记都处于同一水平面上的判断。被检测出的标记i(i＝1，2，...，n)在移动目标坐标系中的位置被表示如下，

$x_{o_i}={\left[x_{o_i}{y}_{o_i}{z}_{o_i}\right]}^t,{x^{\&prime;}}_{o_i}={\left[{x^{\&prime;}}_{o_i}{y^{\&prime;}}_{o_i}{z^{\&prime;}}_{o_i}\right]}^t=R_{\mathrm{wo}}^i\&CenterDot;x_{o_i}---\left(53\right)$

从表达式(26)获得被检测出的标记的y_w坐标的方差σ²，

${\mathrm{\&sigma;}}^2=\frac{\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{({y^{\&prime;}}_{o_i}-{\stackrel{\&OverBar;}{y}}_w)}^2}{n}---\left(54\right)$

表达式(26)

在表达式(26)中，标记的y_w坐标的平均值被表示如下，

y_w                       ……(55)

从表达式(27)获得该平均值，

${\stackrel{\&OverBar;}{y}}_w=\frac{\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{y^{\&prime;}}_{o_i}}{n}---\left(56\right)$

表达式(27)

当方差σ²小于预定值时，则判断出所有的被检测出的标记都位于同一水平面。在步骤S2030中，当判断出被检测出的标记都位于同一水平面时，流程前进到步骤S2050；当判断出被检测出的标记没有位于同一水平面时，流程前进到步骤S2040。在步骤S2040中，位置及姿态计算部分230判断被检测出的标记的数量是否为3或更高。在步骤S2040中，当判断出被检测出的标记的数量为3或更高时，流程前进到步骤S2060；当判断出被检测出的标记的数量小于3时，流程结束。然后，在步骤S2050中，判断被检测出的标记的数量是否为2或更高。在步骤S2050中，当判断出被检测出的标记的数量为2或更高时，流程前进到步骤S2070，当判断出被检测出的标记的数量小于2时，流程结束。

在步骤S2060中，位置及姿态计算部分230采用过程(2A)来计算移动目标的位置及姿态，并输出该位置及姿态。现在说明过程(2A)。首先，通过对分数的分母和分子的除法处理来简化表达式(25)。为了获得α、β、t_ox、t_oy和t_oz，进行下面的赋值，

${u^{\&prime;}}_{x_i}=-u_{x_i}/f,{u^{\&prime;}}_{y_i}=-u_{y_i}/f---\left(57\right)$

然后，针对每个被检测出的标记建立由此获得的α、β、t_ox、t_oy和t_oz的线性方程。结果，获得了表达式(28)所示的联立方程，

表达式(28)

通过求解表达式(28)，获得了α、β、t_ox、t_oy、t_oz。在这些联立方程中，未知数是5个，因此至少3个被检测出的标记是必需的。根据α和β，使用表达式(13)来计算方位角θ。为了计算移动目标相对于世界坐标系的姿态R_wo，使用由此获得的θ，并且也使用如下表示的传感器的测量值的倾斜分量，

R_wo ⁱ            ……(59)

通过表达式(29)来进行该计算，

$R_{\mathrm{wo}}=R_{\mathrm{wo}}^a\&CenterDot;R_{\mathrm{wo}}^i$

$=\left[\begin{array}{ccc}\cos \mathrm{\&theta;}& 0& \sin \mathrm{\&theta;}\\ 0& 1& 0\\ -\sin \mathrm{\&theta;}& 0& \cos \mathrm{\&theta;}\end{array}\right]\&CenterDot;R_{\mathrm{wo}}^i---\left(60\right)$

表达式(29)

此外，通过表达式(30)来计算移动目标相对于世界坐标系的位置，

t_wo＝R_wc·t_co+t_wc             表达式(30)

通过上述的方法，在步骤S2060中，计算并输出移动目标的位置及姿态。

在步骤S2070中，位置及姿态计算部分230采用过程(2B)来计算并输出移动目标的位置及姿态。现在，说明过程(2B)。

将标记在世界坐标系中的y_w坐标设为y_avg。在表达式(25)中，进行下面的赋值，

y′_o＝0                 ……(61)

然后进行下面的赋值，

α′＝α/t_oz，β′＝β/t_oz，t′_ox＝t_ox/t_oz，t′_oy＝t_oy/t_oz

                            ……(62)

执行对分数的分母和分子的除法处理。然后，进行下面的赋值，

${u^{\&prime;}}_{x_i}=-u_{x_i}/f,{u^{\&prime;}}_{y_i}=-u_{y_i}/f---\left(63\right)$

因此，获得了α′、β′、t′_ox、t′_oy。针对每个被检测出的标记建立由此获得的α′、β′、t′_ox、t′_oy的线性方程。因此获得了表达式(31)所示的联立方程，

表达式(31)

在这些联立方程中，未知数是4个，因此至少2个被检测出的标记是必需的。根据通过求解表达式(31)所获得的α′和β′，通过表达式(18)来计算方位角θ。为了通过表达式(29)计算移动目标相对于世界坐标系的姿态R_wo，使用由此获得的θ，并且也使用如下表示的传感器的测量值的倾斜分量，

R_wo ⁱ                 ……(65)

然后，通过表达式(32)来计算t_co＝[t_oxt_oyt_oz]^t，

$t_{\mathrm{oz}}=\left\{\begin{array}{cc}\frac{\mathrm{\&alpha;}}{{\mathrm{\&alpha;}}^{\&prime;}}& ({\mathrm{\&alpha;}}^{\&prime;}\&NotEqual;0)\\ \frac{\mathrm{\&beta;}}{{\mathrm{\&beta;}}^{\&prime;}}& ({\mathrm{\&alpha;}}^{\&prime;}=0)\end{array}\right.$

t_ox＝t′_ox·t_oz         ……(66)

t_oy＝t′_oy·t_oz

表达式(32)

此外，通过表达式(33)来计算移动目标在世界坐标系中的位置t_wo，

$t_{\mathrm{wo}}=R_{\mathrm{wc}}\&CenterDot;t_{\mathrm{co}}+t_{\mathrm{wc}}-\left[\begin{array}{c}0\\ y_{\mathrm{avg}}\\ 0\end{array}\right]---\left(67\right)$

表达式(33)

通过上述的方法，在步骤S2070中，计算并输出移动目标的位置及姿态。

如上所述，当按照此实施例将陀螺仪传感器安装到移动目标时，通过上述方法，能够根据传感器的测量值的倾斜分量以及固定在世界坐标系中的图像捕捉设备上的图像信息来计算移动目标的位置及姿态，而不用执行迭代运算。

按照此实施例，将陀螺仪传感器安装到移动目标，仅使用图像捕捉设备捕捉到的移动目标的图像以及陀螺仪传感器的测量姿态值的倾斜分量，由此在不执行迭代运算的情况下(换言之，没有使用初始值)，能够直接计算移动目标的位置及姿态。结果，能够避免在进行迭代运算时产生的由于初始值引起的位置及姿态计算精度的下降。实际上，当在被捕捉图像上捕捉到标记时，能够高精度地计算位置及姿态。

在此实施例中，已经说明了利用一个照相机上的图像信息的位置及姿态测量方法。此实施例的照相机是其在世界坐标系中的位置及姿态已经已知的客观视点(objective viewpoint)照相机。当客观视点照相机在世界坐标系中的位置设置为t_wc(＝[x_wcy_wcz_wc]^t)以及表示照相机姿态的3×3旋转矩阵表示为R_wc时，将从移动目标坐标系x_o到图像捕捉设备坐标系x_c的坐标变换表示为表达式(34)，同时使用移动目标在世界坐标系中的位置t_wo(＝[x_woy_woz_wo]^t)，

$x_c={\left(R_{\mathrm{wc}}\right)}^{-1}\&CenterDot;(R_{\mathrm{wo}}^a\&CenterDot;R_{\mathrm{wo}}^i\&CenterDot;x_o+t_{\mathrm{wo}}-t_{\mathrm{wc}})---\left(68\right)$

表达式(34)

${x^{\&prime;}}_o={\left[{x^{\&prime;}}_o{y^{\&prime;}}_o{z^{\&prime;}}_o\right]}^t=R_{\mathrm{wo}}^i\&CenterDot;x_{o_i}---\left(69\right)$

使用表达式23、24和34将由如下(70)表示的点表示为下面的(71)，

x′_o              ……(70)

将所述点在被捕捉图像上的投影位置(u_x，u_y)表示为表达式(35)，

$u_x=-f\frac{(R_{11}{x^{\&prime;}}_o+R_{13}{z^{\&prime;}}_o)\mathrm{\&alpha;}+(R_{11}{z^{\&prime;}}_o-R_{13}{x^{\&prime;}}_o)\mathrm{\&beta;}+R_{11}{x}_{\mathrm{wo}}+R_{12}{y}_{\mathrm{wo}}+R_{13}{z}_{\mathrm{wo}}-(R_{11}{x}_{\mathrm{wc}}+R_{12}{y}_{\mathrm{wc}}+R_{13}{z}_{\mathrm{wc}})}{(R_{31}{x^{\&prime;}}_o+R_{33}{z^{\&prime;}}_o)\mathrm{\&alpha;}+(R_{31}{z^{\&prime;}}_o-R_{33}{x^{\&prime;}}_o)\mathrm{\&beta;}+R_{31}{x}_{\mathrm{wo}}+R_{32}{y}_{\mathrm{wo}}+R_{33}{z}_{\mathrm{wo}}-(R_{31}{x}_{\mathrm{wc}}+R_{32}{y}_{\mathrm{wc}}+R_{33}{z}_{\mathrm{wc}})}$

$u_y=-f\frac{(R_{21}{x^{\&prime;}}_o+R_{23}{z^{\&prime;}}_o)\mathrm{\&alpha;}+(R_{21}{z^{\&prime;}}_o-R_{23}{x^{\&prime;}}_o)\mathrm{\&beta;}+R_{21}{x}_{\mathrm{wo}}+R_{22}{y}_{\mathrm{wo}}+R_{23}{z}_{\mathrm{wo}}-(R_{21}{x}_{\mathrm{wc}}+R_{22}{y}_{\mathrm{wc}}+R_{23}{z}_{\mathrm{wc}})}{(R_{31}{x^{\&prime;}}_o+R_{33}{z^{\&prime;}}_o)\mathrm{\&alpha;}+(R_{31}{z^{\&prime;}}_o-R_{33}{x^{\&prime;}}_o)\mathrm{\&beta;}+R_{31}{x}_{\mathrm{wo}}+R_{32}{y}_{\mathrm{wo}}+R_{33}{z}_{\mathrm{wo}}-(R_{31}{x}_{\mathrm{wc}}+R_{32}{y}_{\mathrm{wc}}+R_{33}{z}_{\mathrm{wc}})}---\left(71\right)$

表达式(35)

表达式(35)是使用t_wo、α和β作为未知参数针对被捕捉图像上检测到的各个标记建立的观测方程。通过针对在多个照相机捕捉到的图像上检测到的标记建立此观测方程，如同此实施例中所述的方法，能够获得t_wo、α和β。也就是，此实施例不限于使用一个照相机的情况，而是能够利用其位置及姿态在世界坐标系中已经已知的多个照相机的图像信息。

第三实施例

在第二实施例中，将图像捕捉设备固定在世界坐标系中，并将陀螺仪传感器安装到移动目标上。而在第三实施例中，将给出对另一位置及姿态测量装置及其方法的说明，该方法和装置用在将陀螺仪传感器安装到图像捕捉设备和移动目标两者之上来测量移动目标相对于图像捕捉设备的位置及姿态的情况。

图7显示了按照第三实施例的位置及姿态测量装置的结构。如图7所示，按照第三实施例的位置及姿态测量装置300包括标记检测部分310、姿态输入部分320和位置及姿态计算部分330。图像捕捉设备340连接到标记检测部分310。陀螺仪传感器355安装到移动目标360，陀螺仪传感器350安装到图像捕捉设备340。陀螺仪传感器350和355连接到姿态输入部分320。在第三实施例中，使用在由图像捕捉设备340捕捉的图像中检测到的移动目标的标记以及分别安装到移动目标和图像捕捉设备上的陀螺仪传感器的测量值的倾斜分量，由此获得移动目标相对于图像捕捉设备的位置及姿态。图8显示了按照第三实施例的坐标系之间的关系。在第三实施例中，获得移动目标相对于图像捕捉设备坐标系的姿态R_co及位置t_co。表示移动目标在世界坐标系中的姿态的3×3旋转矩阵被表示为R_wo，表示图像捕捉设备的位置的3×3旋转矩阵被表示为R_wc。R_wo被分解成如下表示的倾斜分量，

R_wo ⁱ             ……(72)

R_wo还被分解成如下表示的方位角分量，

R_wo ^a             ……(73)

按照表达式(36)所示的进行该分解，

$R_{\mathrm{wo}}=R_{\mathrm{wo}}^a\&CenterDot;R_{\mathrm{wo}}^i---\left(74\right)$

表达式(36)

此外，R_wc被分解成如下表示的倾斜分量，

R_wc ⁱ             ……(75)

R_wc还被分解成如下表示的方位角分量，

R_wc ^a             ……(76)

按照表达式(37)所示的进行该分解，

$R_{\mathrm{wc}}=R_{\mathrm{wc}}^a\&CenterDot;R_{\mathrm{wc}}^i---\left(77\right)$

表达式(37)

使用下列各项来表示从移动目标坐标系x_o到图像捕捉设备坐标系x_c的变换，

R_wc ⁱ，R_wc ^a，R_wo ⁱ，R_wo ^a           ……(78)

此外，使用移动目标在图像捕捉设备坐标系中的位置t_co。由此能够将所述变换表示为表达式(38)，

$x_c={\left(R_{\mathrm{wc}}^i\right)}^{-1}\&CenterDot;{\left(R_{\mathrm{wc}}^a\right)}^{-1}\&CenterDot;R_{\mathrm{wo}}^a\&CenterDot;R_{\mathrm{wo}}^i\&CenterDot;x_o+t_{\mathrm{co}}---\left(79\right)$

表达式(38)

此处，进行下面的过程，

(R_wc ^a)^-1·R_wo ^a        ……(80)

上面的(80)表示方位角，并获得下面的表达式，

$R_a={\left(R_{\mathrm{wc}}^a\right)}^{-1}\&CenterDot;R_{\mathrm{wo}}^a---\left(81\right)$

获得上面的(81)作为组合的结果。

此处，将下面的(82)表示为表达式(39)，

(R_wc ⁱ)^-1              ……(82)

将R_a表示为表达式(40)，θ表示方位角。然后进行下面的赋值，

${\left(R_{\mathrm{wc}}^i\right)}^{-1}=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]---\left(83\right)$

表达式(39)

$R_a=\left[\begin{array}{ccc}\cos \mathrm{\&theta;}& 0& \sin \mathrm{\&theta;}\\ 0& 1& 0\\ -\sin \mathrm{\&theta;}& 0& \cos \mathrm{\&theta;}\end{array}\right]---\left(84\right)$

表达式(40)

${X^{\&prime;}}_o={\left[{x^{\&prime;}}_o{y^{\&prime;}}_o{z^{\&prime;}}_o\right]}^t=R_{\mathrm{wo}}^i\&CenterDot;X_o---\left(85\right)$

此时，当设焦距为f并且α＝cosθ、β＝sinθ、x_c＝[x_cy_cz_c]^t和t_co＝[t_oxt_oyt_oz]^t时，将移动目标坐标为x_o＝[x_oy_oz_o]^t的标记在被捕捉图像上的投影位置(u_x，u_y)表示为表达式(41)。

$u_x=-f\frac{x_c}{z_c}=-f\frac{(r_{11}{x^{\&prime;}}_o+r_{13}{z^{\&prime;}}_o)\mathrm{\&alpha;}+(r_{11}{z^{\&prime;}}_o-r_{13}{x^{\&prime;}}_o)\mathrm{\&beta;}+t_{\mathrm{ox}}+r_{12}{y^{\&prime;}}_o}{(r_{31}{x^{\&prime;}}_o+r_{33}{z^{\&prime;}}_o)\mathrm{\&alpha;}+(r_{31}{z^{\&prime;}}_o-r_{33}{x^{\&prime;}}_o)\mathrm{\&beta;}+t_{\mathrm{oz}}+r_{32}{y^{\&prime;}}_o}$

$u_y=-f\frac{y_c}{z_c}=-f\frac{(r_{21}{x^{\&prime;}}_o+r_{23}{z^{\&prime;}}_o)\mathrm{\&alpha;}+(r_{21}{z^{\&prime;}}_o-r_{23}{x^{\&prime;}}_o)\mathrm{\&beta;}+t_{\mathrm{oy}}+r_{22}{y^{\&prime;}}_o}{(r_{31}{x^{\&prime;}}_o+r_{33}{z^{\&prime;}}_o)\mathrm{\&alpha;}+(r_{31}{z^{\&prime;}}_o-r_{33}{x^{\&prime;}}_o)\mathrm{\&beta;}+t_{\mathrm{oz}}+r_{32}{y^{\&prime;}}_o}---\left(86\right)$

表达式(41)

接下来，将对计算移动目标相对于图像捕捉设备的位置及姿态的方法给出说明，其中使用了在图像捕捉设备所捕捉的被捕捉图像上检测到的标记以及陀螺仪传感器输出的姿态的倾斜分量。图9是按照第三实施例的方法的流程图。

首先，在步骤S3010中，标记检测部分310输入来自图像捕捉设备的图像。然后，在图像被捕捉的同时，姿态输入部分320输入安装在移动目标上的陀螺仪传感器的测量值R_wo、以及安装在图像捕捉设备上的陀螺仪传感器的测量值R_wc，并分别将上述测量值分解成如下表示的倾斜分量，

R_wo ⁱ，R_wc ⁱ             ……(87)

上述测量值也被分解成如下所示的方位角分量，

R_wo ^a，R_wc ^a             ……(88)

当进行分解时，采用已知的过程。然后，从表达式(39)获得已知的值r₁₁、r₁₂、r₁₃、r₂₁、r₂₂、r₂₃、r₃₁、r₃₂和r₃₃，并将其输出到位置及姿态计算部分330。随后，在步骤S3020中，标记检测部分310根据被捕捉图像检测移动目标上的标记，以便将检测到的标记输出到位置及姿态计算部分330。此处，将被捕捉图像中检测到的标记的数量设为n，并且被检测出的标记i(i＝1，2，...，n)在被捕捉图像上的检测位置被如下设定。

(u_xi，u_yi)              ……(89)

接下来，在步骤S3030中，位置及姿态计算部分330判断是否所有的被检测出的标记都处于同一水平面上。例如，以下面的方式来进行是否所有的被检测出的标记都处于同一水平面上的判断。被检测出的标记i(i＝1，2，...，n)在移动目标坐标系中的位置被表示如下，

$x_{o_i}={\left[x_{o_i}{y}_{o_i}{z}_{o_i}\right]}^t,{x^{\&prime;}}_{o_i}={\left[{x^{\&prime;}}_{o_i}{y^{\&prime;}}_{o_i}{z^{\&prime;}}_{o_i}\right]}^t=R_{\mathrm{wo}}^i\&CenterDot;x_{o_i}---\left(90\right)$

然后，从表达式(42)获得被检测出的标记的y_w坐标的方差σ²。

${\mathrm{\&sigma;}}^2=\frac{\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{({y^{\&prime;}}_{o_i}-{\stackrel{\&OverBar;}{y}}_w)}^2}{n}---\left(91\right)$

表达式(42)

在表达式(42)中，标记的y_w坐标的平均值被表示如下，

y_w                     ……(92)

从表达式(43)获得该平均值。

${\stackrel{\&OverBar;}{y}}_w=\frac{\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{y^{\&prime;}}_{o_i}}{n}---\left(93\right)$

表达式(43)

当方差σ²小于预定值时，则判断出所有的被检测出的标记都位于同一水平面。在步骤S3030中，当判断出被检测出的标记都位于同一水平面时，流程前进到步骤S3050，当判断出被检测出的标记没有位于同一水平面时，流程前进到步骤S3040。

在步骤S3040中，位置及姿态计算部分330判断被检测出的标记的数量是否为3或更高。在步骤S3040中，当判断出被检测出的标记的数量为3或更高时，流程前进到步骤S3060，当判断出被检测出的标记的数量小于3时，流程结束。而在步骤S3050中，判断被检测出的标记的数量是否为2或更高。在步骤S3050中，当判断出被检测出的标记的数量为2或更高时，流程前进到步骤S3070，当判断出被检测出的标记的数量小于2时，流程结束。

在步骤S3060中，位置及姿态计算部分330采用过程(3A)来计算并输出图像捕捉设备相对于移动目标的位置及姿态。现在说明过程(3A)。首先，通过对分数的分母和分子的除法处理来简化表达式(41)，并进行下面的赋值以获得α、β、t_ox、t_oy和t_oz，

${u^{\&prime;}}_{x_i}=-u_{x_i}/f,{u^{\&prime;}}_{y_i}=-u_{y_i}/f---\left(94\right)$

针对每个被检测出的标记建立由此获得的α、β、t_ox、t_oy和t_oz的线性方程，因此能够获得表达式(44)所示的联立方程，

表达式(44)

通过求解表达式(44)，则获得了α、β、t_ox、t_oy、t_oz。在这些联立方程中，未知数是5个，因此至少3个被检测出的标记是必需的。根据α和β，使用表达式(13)来计算方位角θ。通过表达式(45)来计算移动目标相对于图像捕捉设备的姿态R_co。

$R_{\mathrm{co}}={\left(R_{\mathrm{wc}}^i\right)}^{-1}\&CenterDot;R_a\&CenterDot;R_{\mathrm{wo}}^i$

$={\left(R_{\mathrm{wc}}^i\right)}^{-1}\&CenterDot;\left[\begin{array}{ccc}\cos \mathrm{\&theta;}& 0& \sin \mathrm{\&theta;}\\ 0& 1& 0\\ -\sin \mathrm{\&theta;}& 0& \cos \mathrm{\&theta;}\end{array}\right]\&CenterDot;R_{\mathrm{wo}}^i---\left(96\right)$

表达式(45)

通过上述的方法，在步骤S3060中，计算并输出移动目标相对于图像捕捉设备的位置及姿态。

在步骤S3070中，位置及姿态计算部分330采用过程(3B)来计算并输出移动目标相对于图像捕捉设备的位置及姿态。现在，说明过程(3B)。

将标记在世界坐标系中的y_w坐标设为y_avg。在表达式(41)中，进行下面的设定。

y′_o＝0            ……(97)

此外，进行下面的赋值，

α′＝α/t_oz，β′＝β/t_oz，t′_ox＝t_ox/t_oz，t′_oy＝t_oy/t_oz

                                               ……(98)

通过对分母和分子的除法处理来进行分数的简化。然后进行下面的赋值以获得α′、β′、t′_ox和t′_oy，

${u^{\&prime;}}_{x_i}=-u_{x_i}/f,{u^{\&prime;}}_{y_i}/f---\left(99\right)$

针对每个被检测出的标记建立由此获得的α′、β′、t′_ox、t′_oy的线性方程，因此能够获得表达式(46)所示的联立方程，

表达式(46)

在这些联立方程中，未知数是4个，因此至少需要2个被检测出的标记。根据通过求解表达式(46)所获得的α′和β′，通过表达式(18)来计算方位角θ。通过表达式(45)来计算移动目标相对于图像捕捉设备的姿态R_co。然后，通过表达式(32)来计算t_co＝[t_oxt_oyt_oz]^t。

通过上述的方法，在步骤S3070中，计算并输出移动目标相对于图像捕捉设备的位置及姿态。

如上所述，在此实施例中，当把陀螺仪传感器安装到移动目标和图像捕捉设备两者之上时，能够根据传感器的测量值的倾斜分量以及来自图像捕捉设备的图像信息来直接获得移动目标相对于图像捕捉设备的位置及姿态(即，图像捕捉设备与移动目标之间的相对位置及相对姿态)，而没有必要执行迭代运算。

按照此实施例，在将陀螺仪传感器安装到图像捕捉设备和移动目标两者之上之后，通过仅使用图像捕捉设备捕捉的移动目标的图像以及仅使用陀螺仪传感器输出的测量姿态值的倾斜分量，在不执行迭代运算的情况下(换言之，没有使用初始值)，能够直接获得图像捕捉设备与移动目标之间的相对位置及相对姿态。结果，能够避免在进行迭代运算时产生的由于初始值引起的位置及姿态计算精度的下降。实际上，当在被捕捉图像上捕捉(检测)到标记时，能够高精度地计算位置及姿态。

修改示例1

在上述的实施例中，陀螺仪传感器用作姿态传感器，但是不限于陀螺仪传感器用作姿态传感器。可以使用任何倾斜传感器，只要倾斜传感器能够测量姿态的倾斜分量即可。此外，倾斜不限于相对于垂直轴的倾斜。即使当倾斜与相对于任意轴的角度相对应，也可以使用任何的倾斜传感器，只要倾斜传感器能够测量相对于任意轴的倾斜即可。

修改示例2

在上述的实施例中，基于标记在被捕捉图像上的图像坐标、标记的三维位置以及由姿态传感器获得的姿态的倾斜分量，在不执行迭代运算的情况下，获得图像捕捉设备的位置及姿态(第一实施例)、移动目标的位置及姿态(第二实施例)以及图像捕捉设备与移动目标之间的相对位置及相对姿态(第三实施例)。通过将由此获得的位置及姿态设为初始值，可以通过其它的方法来优化所述位置及姿态。

例如，通过采用上述论文(Kato，M.Billinghurst，Asano，andTachibana：″Augmented reality system on the basis of markertracking and calibration therefor″，日本虚拟现实协会的刊物，vol.4，no.4，pp.607-616，1999)中公开的过程，根据图像坐标和标记的三维位置，可以优化六自由度的所有的位置及姿态。

此外，也能够使用上述论文(K.Satoh，S.Uchiyama，and H.Yamamoto：″A head tracking method using bird′s-eye view cameraand gyroscope，″Proc.3rd IEEE/ACM Int′l Symp.on Mixed andAugmented Reality(ISMAR 2004)，pp.89-98，2004)中公开的过程，根据图像坐标和标记的三维位置以及姿态传感器的测量值来优化位置和姿态的方位角分量。

此时，是否进行迭代运算不是问题。当把通过第一至第三实施例所述的方法获得的结果设为初始值用于涉及迭代运算的优化时，能够抑制由于不正确的值(例如，当不能在前一帧中获得位置及姿态时)导致的位置及姿态的计算精度的下降。

为了详细描述，能够针对每个帧独立地获得初始值，因此即使在前一帧中产生了不正确的处理结果，也能够获得适当的位置及姿态而不受不正确结果的影响。

修改示例3

在上述的实施例中，通过矩阵计算来获得图像捕捉设备与移动目标之间的相对位置及相对姿态。但是，上述实施例的要旨在于直接求解联立方程，而不执行迭代运算。因此，可以采用求解联立方程的其它方法，例如ICCG(不完全的楚列斯基共轭梯度)。

修改示例4

此外，在上述的实施例中，如表达式(10)所示，假设所有的被检测出的标记对于求解联立方程具有相同的可靠性程度。但是，可以通过改变每个被检测出的标记的可靠性程度以分配权重来求解联立方程。

修改示例5

而且，在上述的实施例中，通过判断是否所有的标记都在同一水平面上来切换计算过程。但是，当预先知道是否所有的标记都在同一水平面上时，则跳过判断步骤，可以直接求解表达式(10)或表达式(17)(或者表达式(28)或表达式(31)、或者表达式(44)或表达式(46))。

修改示例6

在上述的实施例中，基于标记在被捕捉图像上的图像坐标、标记的三维位置以及由姿态传感器获得的姿态的倾斜分量，在不进行迭代运算的情况下，获得图像捕捉设备的位置和方位角(第一实施例)、移动目标的位置和方位角(第二实施例)以及获得图像捕捉设备与移动目标之间的相对位置和方位角(第三实施例)。但是，当从姿态传感器所获得的姿态的倾斜分量足够精确并且从姿态传感器所获得的姿态的方位角分量具有相对较小的误差时，计算位置和方位角误差。

当转动角θ非常小时，通过使用线性近似如下地表示代表围绕y轴转动的旋转矩阵。

$R\left(\mathrm{\&theta;}\right)=\left[\begin{array}{ccc}1& 0& \mathrm{\&theta;}\\ 0& 1& 0\\ -\mathrm{\&theta;}& 0& 1\end{array}\right]---\left(101\right)$

在此修改示例中，假设从姿态传感器所获得的姿态的方位角误差足够小，并且其旋转矩阵能够通过表达式(101)线性地近似。真实的旋转矩阵R_TRUE与表示姿态传感器的输出的旋转矩阵R_SENSOR之间的相互关系被表示如下，

$R_{\mathrm{TRUE}}=R_{\mathrm{ERROR}}^{-1}\&CenterDot;R_{\mathrm{SENSOR}}---\left(102\right)$

此处，旋转矩阵R_ERROR表示姿态传感器所获得的姿态的方位角误差。R_SENSOR被表示为R_SENSOR＝R_CORRECT·R_MEASURED。R_MEASURED表示传感器原始输出的旋转矩阵。R_CORRECT表示修正了传感器的原始姿态的旋转矩阵。R_CORRECT最初被手动设定或被设定为单位矩阵，并且通过使用最新的R_ERROR被更新。

在与第一实施例相同的情况中，将姿态传感器固定到图像捕捉设备，并获得图像捕捉设备的位置及姿态，通过使用下列方程来表示世界坐标系x_w与照相机坐标系x_c之间的坐标变换，

$x_w=R_{\mathrm{ERROR}}^{-1}\&CenterDot;R_{\mathrm{SENSOR}}\&CenterDot;x_c+t_{\mathrm{wc}}---\left(103\right)$

表达式(103)能够写成下面的形式，

$x_c=R_{\mathrm{SENSOR}}^{-1}\&CenterDot;R_{\mathrm{ERROR}}\&CenterDot;x_w+t_{\mathrm{cw}}---\left(104\right)$

此处，t_cw(＝[t_xt_yt_z]^t)表示两种坐标系之间的变换。当θ表示方位角误差时，能够将R_ERROR表示为表达式(101)。将上面的R_SENSOR ^-1表示为表达式(105)，

$R_{\mathrm{SENSOR}}^{-1}=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]---\left(105\right)$

将位置是x_w＝[x_wy_wz_w]^t的标记的投影位置(u_x，u_y)表示为表达式(106)，

$u_x=-f\frac{x_c}{z_c}=-f\frac{(r_{11}{z}_w-r_{13}{x}_w)\mathrm{\&theta;}+t_x+r_{11}{x}_w+r_{12}{y}_w+r_{13}{z}_w}{(r_{31}{z}_w-r_{33}{x}_w)\mathrm{\&theta;}+t_z+r_{31}{x}_w+r_{32}{y}_w+r_{33}{z}_w}$

$u_y=-f\frac{y_c}{z_c}=-f\frac{(r_{21}{z}_w-r_{23}{x}_w)\mathrm{\&theta;}+t_y+r_{21}{x}_w+r_{22}{y}_w+r_{23}{z}_w}{(r_{31}{z}_w-r_{33}{x}_w)\mathrm{\&theta;}+t_z+r_{31}{x}_w+r_{32}{y}_w+r_{33}{z}_w}$ …(106)

通过赋值u_x′＝-u_x/f，u_y′＝-u_y/f，能够如下地获得针对θ、t_x、t_y和t_z的线性方程。

针对每个被检测出的标记获得表示为表达式(107)的线性方程。因此，通过按照与上述实施例中相同的方式求解由表达式(107)构成的联立方程，获得方位角误差θ和变换t_cw。由于未知数是4个，因此至少2个标记是计算所必需的。与第一实施例的区别点在于，在此修改示例中判断标记是否水平共面不是必需的。

在与第二实施例相同的情况中，能够采用类似的方法。当以表达式(102)的形式来表示代表目标相对世界坐标系旋转的旋转矩阵时，目标坐标系与照相机坐标系之间的坐标变换能够写成下面的形式。

$x_c=R_{\mathrm{wc}}^{-1}\&CenterDot;R_{\mathrm{ERROR}}^{-1}\&CenterDot;R_{\mathrm{SENSOR}}\&CenterDot;x_o+t_{\mathrm{co}}$

将R_SENSOR·x_o表示为x_o′，位置是x_o′＝[x_o′y_o′z_o′]的标记的投影位置(u_x，u_y)被表示为表达式(108)，

$u_x=-f\frac{x_c}{z_c}=-f\frac{(R_{13}{x}_w-R_{11}{z}_w)\mathrm{\&theta;}+t_{\mathrm{ox}}+R_{11}{x}_w+R_{12}{y}_w+R_{13}{z}_w}{(R_{33}{x}_w-R_{31}{x}_w)\mathrm{\&theta;}+t_{\mathrm{oz}}+R_{31}{x}_w+R_{32}{y}_w+R_{33}{z}_w}$

$u_y=-f\frac{y_c}{z_c}=-f\frac{(R_{23}{x}_w-R_{21}{z}_w)\mathrm{\&theta;}+t_{\mathrm{oy}}+R_{21}{x}_w+R_{22}{y}_w+R_{23}{z}_w}{(R_{33}{x}_w-R_{31}{x}_w)\mathrm{\&theta;}+t_{\mathrm{oz}}+R_{31}{x}_w+R_{32}{y}_w+R_{33}{z}_w}---\left(108\right)$

根据表达式(108)，获得表示为表达式(109)的线性方程。

通过求解使用针对每个被检测出的标记的表达式(109)所表示的联立方程，在此情况下能够获得方位角误差和变换。通过使用获得的方位角误差θ将R_CORRECT更新为R_CORRECT←R_ERROR ^-1·R_CORRECT。R_ERROR ^-1是根据表达式(101)计算的矩阵的逆矩阵。

在与第三实施例相同的情况中，能够通过使用表示为表达式(102)的分解来采用类似的方法。通过参考第三实施例和此修改示例的说明能够容易地推测详细的方式。

如上所述，在此修改示例中，在从姿态传感器所获得的姿态的方位角误差足够小时获得了方位角误差和位置，而不是方位角分量和位置。

其它的实施例

此外，下面的情况在本发明的范围内。为计算机提供用于实现上述实施例的功能的软件程序代码，计算机安装在设备中或者连接到各种设备的系统中用于实现上述实施例的功能并用于操作各种设备，通过按照存储的程序操作各种设备来实施该系统或设备的计算机(CPU或MPU)。

在此情况下，软件本身的程序代码实现了上述实施例的功能，因此程序代码本身和用于将程序代码提供给计算机的单元(例如，存储程序代码的记录介质)提供了本发明。

例如，能够使用软盘(注册商标为floppy)、硬盘、光盘、磁光盘、CD-ROM、磁带、非易失性存储卡或者ROM作为存储程序代码的记录介质。

当然，程序代码构成本发明的实施例不仅在于通过计算机运行所提供的程序代码以实现上述实施例的功能的情况，还在于程序代码用于与计算机上运行的操作系统(OS)或其它应用软件等相关联以实现上述实施例的功能的情况。

或者，提供的程序代码能够存储在计算机的功能扩展板上或者存储在连接到计算机的功能扩展单元的存储器中。在此情况下，功能扩展板或者功能扩展单元的CPU等按照程序代码的指令执行全部或部分的实际处理，以便通过该处理实现上述实施例的功能。

应当理解，本发明不限于上述的具体实施例，而是由下面的权利要求所限定，权利要求应当包含用于提供指定功能的所有的修改和等价手段。

Claims (18)

1.一种位置及姿态测量方法，包括以下步骤：

输入倾斜传感器的测量倾斜值，该倾斜传感器安装在目标和图像捕捉设备的至少一个上；

输入来自所述图像捕捉设备的被捕捉图像；

根据所述被捕捉图像检测所述目标上的标记；和

基于所述测量倾斜值、被检测出的标记的图像坐标和关于被检测出的标记的已知的位置信息，计算其上安装了所述倾斜传感器的目标和图像捕捉设备其中之一的位置及姿态，而不执行位置及姿态的修正值的迭代运算。

2.如权利要求1所述的位置及姿态测量方法，其中计算步骤准备多个位置及姿态计算过程，并且该方法包括按照被检测出的标记从所述多个位置及姿态计算过程中选择适当的位置及姿态计算过程。

3.如权利要求1所述的位置及姿态测量方法，其中计算步骤根据至少三个被检测出的标记来计算位置及姿态。

4.如权利要求1所述的位置及姿态测量方法，其中计算步骤在被检测出的标记位于垂直于预定轴的相同平面上的约束条件下计算位置及姿态。

5.如权利要求1所述的位置及姿态测量方法，其中计算步骤求解通过针对每个被检测出的标记建立线性方程而获得的联立方程。

6.如权利要求1所述的位置及姿态测量方法，其中进一步包括当计算出的位置和计算出的姿态被设为初始值时，根据被检测出的标记的图像坐标以及关于被检测出的标记的已知的位置信息来优化位置及姿态的步骤。

7.如权利要求6所述的位置及姿态测量方法，其中优化位置及姿态的步骤包括使用迭代运算的步骤。

8.一种位置及姿态测量方法，包括以下步骤：

输入安装到目标上的第一倾斜传感器的第一测量倾斜值；

输入安装到图像捕捉设备上的第二倾斜传感器的第二测量倾斜值；

输入来自所述图像捕捉设备的被捕捉图像；

根据所述被捕捉图像检测所述目标上的标记；和

基于第一测量倾斜值、第二测量倾斜值、被检测出的标记的图像坐标以及关于被检测出的标记的已知的位置信息，计算所述目标与图像捕捉设备的相对位置和相对姿态，而不执行位置及姿态的修正值的迭代运算。

9.如权利要求8所述的位置及姿态测量方法，其中计算步骤准备多个位置及姿态计算过程，并且该方法包括按照被检测出的标记从所述多个位置及姿态计算过程中选择适当的位置及姿态计算过程。

10.如权利要求8所述的位置及姿态测量方法，其中计算步骤根据至少三个被检测出的标记来计算相对位置及相对姿态。

11.如权利要求8所述的位置及姿态测量方法，其中计算步骤在被检测出的标记位于垂直于预定轴的相同平面上的约束条件下计算位置及姿态。

12.如权利要求8所述的位置及姿态测量方法，其中计算步骤求解通过针对每个被检测出的标记建立线性方程而获得的联立方程。

13.如权利要求8所述的位置及姿态测量方法，其中进一步包括当计算出的相对位置和计算出的相对姿态被设为初始值时，根据被检测出的标记的图像坐标以及关于被检测出的标记的已知的位置信息来优化相对位置及相对姿态的步骤。

14.如权利要求13所述的位置及姿态测量方法，其中优化步骤使用迭代运算。

15.一种位置及姿态测量装置，包括：

图像捕捉设备；

倾斜传感器，安装在目标和图像捕捉设备的至少一个上；

倾斜值输入单元，用于输入倾斜传感器的测量倾斜值；

图像输入单元，用于输入来自所述图像捕捉设备的被捕捉图像；

标记检测单元，用于根据所述被捕捉图像检测所述目标上的标记；和

位置及姿态计算器，用于基于所述测量倾斜值、被检测出的标记的图像坐标和关于被检测出的标记的已知的位置信息，计算其上安装了所述倾斜传感器的目标和图像捕捉设备其中之一的位置及姿态，而不执行位置及姿态的修正值的迭代运算。

16.一种位置及姿态测量装置，包括：

第一测量倾斜值输入单元，用于输入安装到目标上的第一倾斜传感器的第一测量倾斜值；

第二测量倾斜值输入单元，用于输入安装到图像捕捉设备上的第二倾斜传感器的第二测量倾斜值；

图像输入单元，用于输入来自所述图像捕捉设备的被捕捉图像；

标记检测单元，用于根据所述被捕捉图像检测所述目标上的标记；和

位置及姿态计算器，用于基于第一测量倾斜值、第二测量倾斜值、被检测出的标记的图像坐标以及关于被检测出的标记的已知的位置信息，计算所述目标与图像捕捉设备的相对位置和相对姿态，而不执行位置及姿态的修正值的迭代运算。

17.一种程序，当被载入计算机并运行时执行如权利要求1所述的位置及姿态测量方法。

18.一种程序，当被载入计算机并运行时执行如权利要求8所述的位置及姿态测量方法。

Images