CN102472609B - 位置和姿势校准方法及设备 - Google Patents

Abstract

一种位置和姿势测量设备，计算物体的二维图像的图像特征与所存储的物体的位置和姿势中的三维模型投影在所述二维图像上的投影图像之间的差。所述位置和姿势测量设备还计算三维坐标信息与所存储的物体的位置和姿势中的三维模型之间的差。所述位置和姿势测量设备然后转换第一差和/或第二差的尺度以使得所述第一差和所述第二差具有同等尺度并校正所存储的位置和姿势。

Description

位置和姿势校准方法及设备

技术领域

本发明涉及一种用于测量已知三维形状的物体的位置和姿势的技术。

背景技术

近年来，随着机器人技术的发展，机器人开始进行诸如工业产品的装配等的过去利用人工进行的复杂任务。在这类机器人使用包括手的末端执行器来保持和装配零部件时，需要测量要保持的零部件和机器人(手)之间的相对位置和姿势。

可以通过采用模型拟合来测量物体的位置和姿势，其中，在模型拟合中，使物体的三维模型与从二维图像或者距离图像检测到的特征拟合。当相对于二维图像进行模型拟合时，估计位置和姿势，以使得在基于物体的位置和姿势将三维模型投影在图像上时所获得的投影图像与所检测到的特征相匹配。当进行对于距离图像的模型拟合时，将距离图像中的各点转换成具有三维坐标的三维点群。然后估计位置和姿势，以使得在三维空间中三维模型拟合三维点群。

然而，二维图像中所检测到的特征的位置或点群的三维坐标包含误差。这类误差是由像素的量化误差、模糊、特征检测算法的精度和照相机之间的对应关系所导致的。因此进行诸如平均多个测量信息(即图像的特征和点群)所包括的测量误差的影响等的处理，以提高位置和姿势的测量精度。

在无需明确进行特征检测的情况下，通过使用亮度图像和距离图像的梯度(gradient)估计位置和姿势，可以高精度地测量物体的位置和姿势(Hiura，Yamaguchi，S ato，Ikenouchi，“Real-Time Tracking of Free-Form Objects by Range andIntensity Image Fusion”，Denshi Joho Tsushin GakkaiRonbunshi(电子情报通信学会论文集)，Vol.J80-D-II，No.11，November 1997，pp.2904-2911)。在这一方法中，假定在物体移动时，亮度和距离平滑变化。然后基于梯度算法，根据亮度图像的亮度变化和距离图像的距离变化计算物体的姿势参数。然而，由于在二维亮度图像和三维距离图像之间尺度不同，所以难以有效组合这两个图像。因此需要进行人工调节(manualtuning)来计算姿势参数。

发明内容

本发明涉及一种能够精确测量各种物体的位置和姿势的位置和姿势校准方法。通过有效合并从二维图像所获得的测量信息和从距离图像所获得的测量信息来估计位置和姿势，实现该位置和姿势校准方法。

根据本发明的一个方面，一种位置和姿势校准方法，其用于重复校正所存储的物体的位置和姿势，所述位置和姿势校准方法包括以下步骤：输入所述物体的二维图像；从所述二维图像检测图像特征；输入所述物体的表面的三维坐标信息；计算所检测到的图像特征与在基于所存储的物体的位置和姿势将所存储的三维模型投影至所述二维图像上时所获得的投影图像的投影特征之间的第一差异；计算所述三维坐标信息的三维特征与所存储的位置和姿势时的所述三维模型的模型特征之间的第二差异；转换所述第一差异和/或所述第二差异的尺度以使得所述第一差异和所述第二差异具有同等的尺度；以及基于转换了所述第一差异和所述第二差异中的至少一个的尺度之后的所述第一差异和所述第二差异，校正所存储的位置和姿势。

通过以下参考附图对典型实施例的详细说明，本发明的其它特征和方面将显而易见。

附图说明

包含在说明书中并构成说明书的一部分的附图，示出本发明的典型实施例、特征和方面，并与说明书一起用来解释本发明的原理。

图1示出根据本发明典型实施例的位置和姿势测量设备的结构。

图2A示出根据本发明典型实施例的三维模型。

图2B示出三维模型。

图2C示出三维模型。

图2D示出三维模型。

图3是示出根据本发明第一典型实施例的位置和姿势校准处理的流程图。

图4A示出从图像的边缘检测。

图4B示出从图像的边缘检测。

图5示出根据本发明第一典型实施例的位置和姿势计算单元的结构。

图6是示出根据本发明第一典型实施例的位置和姿势计算处理的流程图。

图7示出线段的投影图像和所检测到的边缘之间的关系。

图8示出用于将图像中的误差近似为三维空间中的误差的方法。

图9是详细示出根据本发明第三典型实施例的位置和姿势校准处理的流程图。

具体实施方式

下面将参考附图详细说明本发明的各种典型实施例、特征和方面。

根据本发明的第一典型实施例，通过进行模型拟合测量物体的位置和姿势。模型拟合同时使用从二维图像获取的测量信息(即图像特征)和从距离图像获取的测量信息(即三维点群)。上述使用二维图像和距离图像的两个方法都设立包括位置和姿势的校正值作为未知变量的线性方程。设立该方程以通过校正位置和姿势来抵消各测量信息在图像和三维空间中的误差。然后通过设立二维和三维测量信息各自的线性方程、并作为一组联立方程进行求解，可以同时使用这两个测量信息来估计位置和姿势。然而，由于对于图像中的误差和三维空间中的误差，评价尺度不同，所以测量信息中的一个的影响变大。因而同时使用测量信息的优势减少。为解决这一问题，本典型实施例统一评价尺度，以使得二维图像中的误差与三维空间中的误差相当。

图1示出根据本典型实施例的位置和姿势测量设备1的结构。参考图1，位置和姿势测量设备1包括三维模型存储单元110、近似位置和姿势输入单元120、二维图像输入单元130、图像特征检测单元140、三维坐标信息输入单元150以及位置和姿势计算单元160。此外，位置和姿势测量设备1与二维图像拍摄单元100和三维坐标信息测量单元105连接。下面将说明位置和姿势测量设备1的各组件。

二维图像拍摄单元100是用于拍摄正常二维图像的照相机。所拍摄的二维图像可以是亮度图像或彩色图像。

二维图像输入单元130向位置和姿势测量设备1输入通过二维图像拍摄单元100拍摄的图像。可以预先校准诸如焦距、主点和镜头失真参数等的内部参数(R.Y.Tsai，“A versatile cameracalibration technique for high-accuracy 3D machine visionmetrology using off-the-shelf TV cameras and lenses”，IEEEJournal of Robotics and Automation，vol.RA-3，no.4，1987)。

三维坐标信息测量单元105测量要测量的物体的表面上的点的三维信息。根据本典型实施例，使用输出距离图像的距离传感器作为三维坐标信息测量单元105。距离图像是各像素具有深度信息的图像。根据本典型实施例的距离传感器是照相机拍摄照射至对象的激光束的反射光、并且利用三角测量来测量距离的主动式距离传感器。然而，距离传感器不局限于上述距离传感器，并且可以是使用光的飞行时间的飞行时间传感器。在对象物体的表面具有较少纹理时，适于使用这类主动式传感器。此外，可以使用根据通过立体照相机所拍摄的图像利用三角测量计算各像素的深度的被动式距离传感器。当对象物体具有足够的表面纹理时，适于使用被动式距离传感器。根据本发明，可以使用测量距离图像的任何传感器。

三维坐标信息输入单元150获取通过三维坐标信息测量单元105所测量的三维信息。三维坐标信息输入单元150然后基于距离传感器和照相机的已知相对位置和姿势，将距离图像中的各像素转换成照相机坐标系中的点群数据、即三维坐标信息。三维坐标信息输入单元150将转换后的点群数据输入给位置和姿势测量设备1。假定距离传感器和照相机相对于彼此的位置固定，并且它们的相对位置和姿势不变。因而可以预先校准相对位置和姿势。例如，从各个方向观察其三维形状已知的校准物体。然后根据基于二维图像的校准物体的位置和姿势与基于距离图像的校准物体的位置和姿势之间的差获取相对位置和姿势。

假定在距离传感器测量距离的同时，照相机拍摄图像。然而，如果位置和姿势测量设备1以及对象物体的位置和姿势不变，诸如在对象物体静止时等，则不必同时拍摄图像和测量距离。

三维模型存储单元110存储要测量其位置和姿势的物体的三维模型。根据本典型实施例，将物体描述为由线段和平面构成的三维模型。

图2A、2B、2C和2D示出根据本发明的本典型实施例的三维模型。将三维模型定义为点的集合和连接这些点的线段的集合。参考图2A，观察对象物体201的三维模型包括14个点，即点P1～P14。此外，参考图2B，观察对象物体201的三维模型包括16个线段，即线段L1～L16。参考图2C，以三维坐标值表示各点P1～P14。此外，以由构成线段的点所构成的标识符(ID)表示各线段L1～L16。此外，三维几何模型存储与平面有关的信息。以构成平面的点表示各平面。图2A、2B、2C和2D所示的三维模型存储与构成立方体的六个平面有关的信息。在位置和姿势计算单元160计算物体的位置和姿势时，使用该三维模型。

近似位置和姿势输入单元120输入物体相对于位置和姿势测量设备1的位置和姿势的近似值。物体相对于位置和姿势测量设备1的位置和姿势表示物体在照相机坐标系中的位置和姿势。然而，如果相对于照相机坐标系的相对位置已知且不改变，则位置和姿势可以以位置和姿势测量设备1的任何部分为基准。

根据本典型实施例，假定位置和姿势测量设备1在时间轴方向上连续测量位置和姿势。因而使用前一测量值(即前一次所测量的值)作为近似位置和姿势。然而，用于输入位置和姿势的近似值的方法不局限于上述方法。例如，可以基于过去测量的位置和姿势，使用时间序列滤波器估计物体的速度或角速度。然后根据过去的位置和姿势以及所估计的速度和加速度来预测当前位置和姿势。

此外，如果存在能够测量物体的位置和姿势的其它传感器，则可以使用这些传感器的输出值作为位置和姿势的近似值。该传感器可以是使用安装至物体的接收器检测通过发射器所生成的磁场来测量位置和姿势的磁传感器。此外，该传感器可以是使用固定至场景的照相机拍摄设置在物体上的标记来测量位置和姿势的光学传感器。此外，可以使用测量6个自由度的位置和姿势的任何传感器。此外，如果预先已知物体的近似位置和姿势，则可以使用这样的值作为近似值。

图像特征检测单元140从二维图像输入单元130输入的二维图像检测图像特征。根据本典型实施例，图像特征检测单元140检测边缘作为图像特征。

位置和姿势计算单元160将存储在三维模型存储单元110中的三维模型与通过图像特征检测单元140检测到的图像特征进行拟合。位置和姿势计算单元160还将三维模型与通过三维坐标信息输入单元150输入的三维点群进行拟合。从而通过这类拟合处理来测量物体的位置和姿势。

图3是示出根据本发明第一典型实施例用于测量位置和姿势的处理的流程图。

在图3所示的步骤S301，操作者使用近似位置和姿势输入单元120向位置和姿势测量设备1输入物体相对于位置和姿势测量设备1(即照相机)的位置和姿势的近似值。如上所述，根据本典型实施例，使用前一次测量的位置和姿势作为近似值。

在步骤S302，位置和姿势测量设备1获取用于通过进行模型拟合来计算物体的位置和姿势的测量信息。更具体地，位置和姿势测量设备1获取对象物体的二维图像和三维信息。

根据本典型实施例，三维坐标信息测量单元105输出距离图像作为三维信息。不同于在各像素中记录亮度值和颜色值的二维图像，在距离图像的各像素中记录从视点位置所测量的深度值。经由二维图像输入单元130将通过二维图像拍摄单元100拍摄的二维图像输入给位置和姿势测量设备1。此外，经由三维坐标信息输入单元150将从三维坐标信息测量单元105输出的距离图像输入给位置和姿势测量设备1。如上所述，将距离图像转换成作为照相机坐标系中的三维坐标信息的三维点群数据，然后将其输入给位置和姿势测量设备1。通过针对距离图像中的各像素将与像素位置相对应的视线矢量乘以深度值，将距离图像转换成三维点群。

在步骤S303，位置和姿势测量设备1从在步骤S302输入的二维图像检测图像特征。根据本典型实施例，位置和姿势测量设备1检测边缘作为图像特征。边缘是浓度梯度的极值。

图4A和4B示出根据本典型实施例的边缘检测。位置和姿势测量设备1使用在步骤S301输入的要测量的物体的近似位置和姿势以及二维图像拍摄单元100的校正后的内部参数，计算构成三维模型的各线段在图像上的投影图像。

参考图4A和4B，位置和姿势测量设备1然后在投影至图像上的线段401上以相等间隔设置控制点402。位置和姿势测量设备1对于各控制点402，在投影线段401的法线方向403上检测一维边缘404。由于作为像素值的浓度梯度405的极值来检测边缘，因而当在附近存在边缘时，可能检测到多个边缘406。根据本典型实施例，假定存储所有检测到的边缘(L.Vacchetti，V.Lepetit，and P.Fua，“Combining edge and texture information forreal-time accurate 3D camera tracking”，Proc.3rd IEEE/ACMInternational Symposium on Mixed and Augmentedreality(ISMAR’04)，pp.48-57，2004)。

图5示出位置和姿势计算单元160的结构。

在步骤S 304，位置和姿势计算单元160将三维模型与在步骤S303检测到的边缘和在步骤S302输入的三维点群进行拟合，以计算要测量的物体的位置和姿势。

参考图5，二维图像位移计算单元501计算图像特征和投影图像之间的距离，图像特征即所检测到的图像上的边缘，投影图像即基于估计的位置和姿势被投影至图像上的线段。

三维空间位移计算单元502计算三维特征和模型特征之间的距离，三维特征即构成点群数据的各点，模型特征即基于位置和姿势被转换成三维坐标信息输入单元150的坐标系的平面。

同等尺度转换单元503基于计算出的距离来优化位置和姿势。更具体地，同等尺度转换单元503计算二维图像中的点和线之间的带符号距离以及三维空间中的点和平面之间的带符号距离。然后同等尺度转换单元503对这两个带符号距离作为物体的位置和姿势的函数进行线性近似。同等尺度转换单元503设立在带符号距离为0时对于各测量信息都成立的线性方程。

位置和姿势校正单元504作为联立方程组求解线性方程以获取物体的位置和姿势的微小变化，并且校正位置和姿势。从而通过重复上述处理计算最终的位置和姿势。

如上所述，由于图像和三维空间中的距离的尺度不同，所以，如果简单求解该联立方程，则贡献率变得偏向于多个测量信息中的一个。在这种情况下，使用两个类型的测量信息的优势减少，并且不能期望精度的提高。根据本典型实施例，通过将二维图像中的距离转换成三维空间中的距离来统一尺度，从而防止贡献率变得偏颇。下面将说明用于计算位置和姿势的处理。

图6是详细示出在图3所示的步骤S304中进行的用于计算物体的位置和姿势的处理的流程图。

在该处理中，位置和姿势计算单元160通过迭代运算重复校正要测量的物体的位置和姿势的近似值(以下称为六维矢量s)。位置和姿势计算单元160使用作为非线性优化方法的高斯-牛顿方法进行这一处理。然而，用于计算要测量的物体的位置和姿势的方法不局限于高斯-牛顿方法。例如，可以使用计算更鲁棒的Levenberg-Marquardt方法，或者可以使用作为更简单的方法的最速下降方法。此外，可以使用诸如共轭梯度方法和不完全乔类斯基共轭梯度方法(ICCG)等的非线性优化计算方法。

在图6所示的步骤S601，位置和姿势计算单元160进行初始化。换句话说，位置和姿势计算单元160输入在步骤S301获取的要测量的物体的近似位置和姿势，作为位置和姿势计算的近似值。

在步骤S602，位置和姿势计算单元160将三维模型与测量信息相关联。

更具体地，位置和姿势计算单元160将三维模型与图像特征相关联。在图3所示的步骤S303，假定对于控制点检测到了多个边缘。在步骤S602，位置和姿势计算单元160将在图像中所检测到的多个边缘中与基于位置和姿势的近似值所投影的线段最靠近的边缘与控制点相关联。

位置和姿势计算单元160然后通过基于位置和姿势的近似值对三维模型或点群数据进行坐标转换，将三维模型与点群数据相关联。位置和姿势计算单元160然后针对点群数据中的各点搜索在三维空间中最靠近的平面，并且将该平面与各点相关联。

在步骤S603，位置和姿势计算单元160计算用于计算物体的位置和姿势的系数矩阵和误差矢量。对于边缘的系数矩阵中的各元素是在将图像中的点和线之间的距离定义为位置和姿势的函数时的物体的位置和姿势的各元素的线性偏微分系数。此外，对于点群数据的系数矩阵中的各元素是在将三维空间中的点和平面之间的距离定义为位置和姿势的函数时的位置和姿势的各元素的线性偏微分系数。对于边缘的误差矢量是投影线段和所检测到的边缘之间在图像中的带符号距离。对于点群数据的误差矢量是点和模型的平面之间在三维空间中的带符号距离。

下面说明系数矩阵的导出。

图7示出线段的投影图像和所检测到的边缘之间的关系。参考图，u轴701表示图像的水平方向，并且v轴702表示图像的垂直方向。将控制点703(在图像中以相等间隔分割各投影线段的点)在图像中的坐标704表示为(u0，v0)。将图像中包括控制点的线段相对于u轴701的倾斜度表示为θ705。计算倾斜度θ705，作为在根据s将线段706的两端的三维坐标投影至图像时连接图像中的两端的坐标的线的倾斜度。图像中的线段706的法向矢量变成(sinθ，-cosθ)。此外，控制点703的对应点707在图像中的坐标708为(u’，v’)。可以将通过对应点707的坐标708(u’，v’)并且倾斜度为θ705的线(图7中以虚线表示)上的点(u，v)表示为：

u sinθ-v cosθ＝d    …(1)

(其中，θ是常数)。在方程(1)中，

d＝u’sinθ-v’cosθ

(其中，d是常数)。

图像中控制点703的位置根据要测量的物体的位置和姿势而改变。此外，要测量的物体的位置和姿势的自由度是6个自由度。换句话说，s是包括表示要测量的物体的位置的三个元素和表示其姿势的三个元素的六维矢量。将表示姿势的这三个元素表示为欧拉角或者三维矢量，在该三维矢量中，方向表示通过原点的转动轴，并且范数表示转动角。通过在坐标704(u0，v0)附近进行线性泰勒展开，如方程(2)一样，可以近似根据位置和姿势而改变的点在图像中的坐标(u，v)。在方程(2)中，Δsi(i＝1、2、......6)表示s的各成分的微小变化。

[数学公式1]

$u\≈u_0+\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂u}{{\∂s}_i}\mathrm{\Δ}{s}_i$

           …(2)

$v\≈v_0+\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂v}{{\∂s}_i}\mathrm{\Δ}{s}_i$

如果假定在物体的位置和姿势的近似值与实际位置和姿势之间存在的差小，则可以假定根据正确的s可获取的控制点在图像中的位置是在以方程(1)表示的线上。因而通过将利用方程(2)近似的u和v代入方程(1)获取方程(3)。

[数学公式2]

$\sin \mathrm{\θ}\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂u}{{\∂s}_i}{\mathrm{\Δs}}_i-\cos \mathrm{\θ}\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂v}{{\∂s}_i}{\mathrm{\Δs}}_i=d-r\·\·\·\left(3\right)$

在方程(3)中，

r＝u 0 sinθ-v 0 cosθ

(其中，r是常数)。可以对于在步骤S602与三维模型相关联的所有边缘设立方程(3)。

使用要测量的对象的位置和姿势s，将以三维坐标信息输入单元150的坐标系(即照相机坐标系)所表示的点群的三维坐标转换成要测量的物体的坐标系的三维坐标(x，y，z)。假定基于近似位置和姿势，将点群数据中的点转换成要测量的物体的坐标(x0，y0，z0)。三维坐标(x，y，z)根据要测量的物体的位置和坐标而改变，并且通过在(x0，y0，z0)附近进行线性泰勒展开，可以将三维坐标(x，y，z)近似为方程(4)。

[数学公式3]

$x\≈x_0+\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂x}{{\∂s}_i}{\mathrm{\Δs}}_i$

$y\≈y_0+\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂y}{{\∂s}_i}{\mathrm{\Δs}}_i\·\·\·\left(4\right)$

$z\≈z_0+\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂z}{{\∂s}_i}{\mathrm{\Δs}}_i$

将在步骤S602与点群数据中的点相关联的三维几何模型的平面在要测量的物体的坐标系中的方程表示为ax+by+cz＝e(其中，a2+b2+c2＝1，并且a、b、c和e是常数)。假定根据正确的s转换后的(x，y，z)满足平面的方程ax+by+cz＝e。因而通过将方程(4)代入平面的方程获得方程(5)。

[数学公式4]

$a\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂x}{{\∂s}_i}{\mathrm{\Δs}}_i+b\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂y}{{\∂s}_i}{\mathrm{\Δs}}_i+c\underset{i=1}{\overset{6}{\mathrm{\Σ}}}{\mathrm{\Δs}}_i=e-q\·\·\·\left(5\right)$

在方程(5)中，

q＝a x 0+b y 0+c z 0

(其中，q是常数)。对于在步骤S602与三维模型相关联的所有点群数据，都可以设立方程(5)。

由于方程(3)和方程(5)是包括s的各成分的微小变化Δsi(i＝1、2、......6)的方程，所以可以设立诸如方程(6)等的关于Δsi的线性联立方程。

[数学公式5]

如上所述，由于方程(6)右边的误差矢量是对于边缘的图像中的带符号距离和对于点群的三维空间中的带符号距离，所以尺度不一致。因此，通过将图像中的误差乘以边缘的深度将图像中的误差近似成三维空间中的误差。结果，将尺度统一成三维空间中的距离。由于从二维图像不能获取深度信息，所以需要通过进行近似方法来获取边缘的深度。

图8示出根据本典型实施例用于将图像中的误差801近似成三维空间中的误差802的方法。参考图8，将图像中的误差801乘以基于位置和姿势的近似值计算出的各控制点803的从视点804所测量的深度805。从而将图像中的误差801转换成三维空间中的误差802。此外，代替深度805，可以将图像中的误差801乘以缩放系数。缩放系数是对于通过三维空间中的控制点803的视线矢量所绘制的到图像平面806的边缘的垂直线的长度。要求解的联立方程(7)变成下面的方程。

[数学公式6]

在方程(7)中，z1、z2、......表示各边缘的深度。还可以将方程(7)表示为方程(8)。

JΔs＝E    …(8)

然后计算用于计算线性联立方程的系数矩阵J的偏微分系数。

在步骤S604，位置和姿势计算单元160基于方程(8)并且使用矩阵J的广义逆矩阵，即(J^T·J)^-1·J^T，根据最小二乘准则获取位置和姿势的校正量Δs。然而，由于因错误检测、在边缘或点群数据中常常存在离群值，所以采用下述的鲁棒估计方法。通常，在作为离群值的边缘或点群数据中方程(7)右边的误差矢量变大。因此，对误差的绝对值大的信息应用小的权重，并且对误差的绝对值小的信息应用大的权重。例如，使用如方程(9)所示的Tukey函数来应用权重。

[数学公式7]

$w\left(z(d-r)\right)=\left\{\begin{array}{cc}{(1-{(z(d-r)/c_1)}^2)}^2& \left|z(d-r)\right|\≤c_1\\ 0& \left|z(d-r)\right|>c_1\end{array}\right.$

$w(e-q)=\left\{\begin{array}{cc}{(1-{((e-q)/c_2)}^2)}^2& |e-q|\≤c_2\\ 0& |e-q|>c_2\end{array}\right.\·\·\·\left(9\right)$

在方程(9)中，c1和c2是常数。对于应用权重不一定使用Tukey函数，并且可以使用向误差大的信息应用小的权重、并且向误差小的信息应用大的权重的任何函数。这类函数的例子有Huber函数。将各测量信息(边缘或点群数据)表示为wi。因而如方程(10)一样定义权重矩阵W。

[数学公式8]

权重矩阵W是矩形矩阵，除对角成分以外，该矩形矩阵的其它成分都是0，并且在该矩形矩阵中，在对角成分中输入权重wi。然后使用权重矩阵W将方程(8)变换成方程(11)。

W JΔs＝W E    …(11)

从而通过如方程(12)那样求解方程(11)获得校正值Δs。

[数学公式9]

Δs＝(J^TWJ)^-1J^TWE…(12)

在步骤S605，位置和姿势计算单元160接着使用在步骤S604计算出的位置和姿势的校正值Δs，校正物体的位置和姿势的近似值。

s←s+Δs

在步骤S606，位置和姿势计算单元160进行收敛判断。如果位置和姿势计算单元160判断为校正值Δs收敛(步骤S606为“是”)，则结束该处理。如果位置和姿势计算单元160判断为校正值Δs未收敛(步骤S606为“否”)，则处理返回到步骤S602。如果校正值Δs接近0，或者如果误差矢量的平方和在校正前后几乎没有变化，则确定收敛。因而通过重复上述处理直到收敛为止，可以计算位置和姿势。

如上所述，根据第一典型实施例，将二维图像中的误差近似地转换成三维空间中的误差。因而统一了二维图像和点群数据中的误差的尺度以被视作同等尺度，并且同时用于测量物体的位置和姿势。

根据第一典型实施例，在将二维图像中的误差转换成三维空间中的误差时，使用与图像的边缘相对应的控制点的根据位置和姿势的近似值所计算出的深度作为边缘的深度。然而，可以利用其它方法获取边缘深度。例如，如果距离传感器可以测量密集距离信息，并且可以从距离传感器获取与二维图像中的各像素相对应的距离信息，则可以使用该距离传感器所测量的距离信息作为边缘深度。

此外，根据第一典型实施例，分别计算图像中所检测到的边缘的深度。然而，当要测量的物体与位置和姿势测量设备充分分离，并且可以将整个物体表示为一个深度时，可以使用平均深度。可以根据各控制点的深度或者距离图像来获取平均深度。因而可以减小由错误对应关系或者测量距离中的误差所导致的离群值的影响。

此外，根据第一典型实施例，当进行用于减小离群值的影响的鲁棒估计时，基于三维空间中的误差对测量信息进行加权。然而，加权方法不局限于上述方法，并且可以基于二维图像中的误差来进行加权。因而可以如方程(13)所表示的一样来对误差进行加权。

[数学公式10]

$w(d-r)=\left\{\begin{array}{cc}{(1-{((d-r)/c_1)}^2)}^2& |d-r|\≤c_3\\ 0& |d-r|>c_3\end{array}\right.\·\·\·\left(13\right)$

在方程(13)中，c3是常数。

此外，根据第一典型实施例，基于输入的近似位置和姿势，通过进行边缘检测来检测多个假设。然后选择最接近根据位置和姿势的近似值所投影的线段的假设作为与重复循环中的控制点相对应的边缘。换句话说，对于一次测量仅进行一次边缘检测。然而，如果有足够的计算时间，代替选择对应点，在重复循环中可以包括边缘检测。然后每当校正位置和姿势的近似值时，都进行边缘检测。结果，即使在初始输入的近似位置和姿势与实际位置和姿势之间的差大时，随着重复的数量增加，也可以检测到正确的边缘。因而可以高精度地测量位置和姿势。

根据第一典型实施例，转换图像中的误差以与三维空间中的误差相当。然后在同一评价尺度下，同时使用二维图像和距离图像来估计位置和姿势。根据本发明第二典型实施例，使用最大似然估计将三维几何模型与测量信息拟合，并且采用似然性作为共同评价尺度。由于根据第二典型实施例的位置和姿势测量设备的结构以及用于测量位置和姿势的处理与根据第一典型实施例的相同，所以省略了说明。

根据本典型实施例，似然性表示在基于给定的物体的位置和姿势所计算出的值(即预测值)与实际测量值之间发生误差的可能性。还假定：仅在从图像所检测到的边缘的搜索线的方向上存在模糊，并且边缘的检测误差服从平均值为0且标准差为σ_2D的一维高斯分布。难以估计实际图像中的标准差σ_2D，因而通过假定边缘的检测误差是由图像的量化误差所导致的，将σ_2D设置成1个像素。如果预测值和测量值之间的误差是如“边缘和投影线段之间的距离”一样的err_2D(即标量值)，则将似然性表示为方程(14)。

[数学公式11]

$p\left({\mathrm{err}}_{2D}\right)=\frac{1}{\sqrt{2\mathrm{\π}}{\mathrm{\σ}}_{2D}}\exp (-\frac{1}{2}{\left(\frac{{\mathrm{err}}_{2D}}{{\mathrm{\σ}}_{2D}}\right)}^2)\·\·\·\left(14\right)$

此外，假定通过距离传感器测量的三维点群的测量误差服从平均值为0并且协方差矩阵为∑的三维高斯分布。协方差矩阵∑是3×3矩阵，并且将轴内方差设置成对角成分，并且将轴之间的互协方差设置成非对角成分。可以基于根据距离传感器的规格所发布的测量精度来计算∑的估计值。

在根据第一实施例的方法中(即用于最小化三维点和相应平面之间的距离的方法)，仅该平面的法线方向上的模糊有助于物体的位置和姿势的计算。因此根据点群的测量误差的协方差矩阵计算该平面的法线方向上测量误差的标准差σ_3D。更具体地，将该平面的坐标系和照相机坐标系之间的旋转矩阵表示为R。然后对协方差矩阵∑进行R^T∑R变换以将其变换成照相机坐标系中的协方差矩阵，并且提取法向矢量方向上的标准差。当物体的位置和姿势的预测值和实际测量值之间的误差(即三维点和平面之间的距离)为err_3D时，将似然性表示为方程(15)。

[数学公式12]

$p\left({\mathrm{err}}_{3D}\right)=\frac{1}{\sqrt{2\mathrm{\π}}{\mathrm{\σ}}_{3D}}\exp (-\frac{1}{2}{\left(\frac{{\mathrm{err}}_{3D}}{{\mathrm{\σ}}_{3D}}\right)}^2)\·\·\·\left(15\right)$

最大似然估计估计未知参数(即物体的位置和姿势)，以使得利用下面的方程所计算出的各测量信息的似然性的积成为最大值。

[数学公式13]

$L=\underset{i=1}{\overset{M}{\mathrm{\Π}}}p\left({{\mathrm{err}}_{2D}}^i\right)\underset{j=1}{\overset{N}{\mathrm{\Π}}}p\left({{\mathrm{err}}_{3D}}^j\right)=$

${\left(\frac{1}{\sqrt{2\mathrm{\π}}{\mathrm{\σ}}_{2D}}\right)}^M\exp (-\frac{1}{2}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{\left(\frac{{{\mathrm{err}}_{2D}}^i}{{\mathrm{\σ}}_{2D}}\right)}^2){\left(\frac{1}{\sqrt{2\mathrm{\π}}{\mathrm{\σ}}_{3D}}\right)}^N\exp (-\frac{1}{2}\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\left(\frac{{{\mathrm{err}}_{3D}}^j}{{\mathrm{\σ}}_{3D}}\right)}^2)$

更具体地，估计未知参数，从而使得对上述的似然性的积的对数进行符号反转而得到的值成为最小值。

[数学公式14]

$-\log \left(L\right)=-\log {\left(\frac{1}{\sqrt{2\mathrm{\π}}{\mathrm{\σ}}_{2D}}\right)}^M+\frac{1}{2}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{\left(\frac{{{\mathrm{err}}_{2D}}^i}{{\mathrm{\σ}}_{2D}}\right)}^2-\log {\left(\frac{1}{\sqrt{2\mathrm{\π}}{\mathrm{\σ}}_{3D}}\right)}^N+\frac{1}{2}\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\left(\frac{{{\mathrm{err}}_{3D}}^j}{{\mathrm{\σ}}_{3D}}\right)}^2$

由于上述方程中的第一项和第三项是不依赖于位置和姿势的常数，所以估计未知参数以使方程(16)最小化。

[数学公式15]

$\frac{1}{2}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{\left(\frac{{{\mathrm{err}}_{2D}}^i}{{\mathrm{\σ}}_{2D}}\right)}^2+\frac{1}{2}\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\left(\frac{{{\mathrm{err}}_{3D}}^j}{{\mathrm{\σ}}_{3D}}\right)}^2\·\·\·\left(15\right)$

第一典型实施例和第二典型实施例之间的不同是位置和姿势计算算法中的系数矩阵和误差矢量的计算。

下面说明用于计算位置和姿势以最小化方程(16)的方法。将边缘检测误差的标准差σ_2D的倒数乘以对于边缘所获得的方程，即

[数学公式16]

$\sin \mathrm{\θ}\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂u}{{\∂s}_i}{\mathrm{\Δs}}_i-\cos \underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂v}{{\∂s}_i}{\mathrm{\Δs}}_i=d-r$

此外，将平面的法线方向的测量误差的标准差σ_3D的倒数乘以对于点群所获得的方程，即

[数学公式17]

$a\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂x}{{\∂s}_i}{\mathrm{\Δs}}_i+b\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂y}{{\∂s}_i}{\mathrm{\Δs}}_i+c\underset{i=1}{\overset{6}{\mathrm{\Σ}}}{\mathrm{\Δs}}_i=e-q$

结果，获得如方程(17)一样的线性联立方程。

[数学公式18]

基于方程(17)计算物体的位置和姿势的校正值。由于其它处理与第一典型实施例中所述的相同，所以省略了说明。

如上所述，根据第二典型实施例，通过采用测量信息的似然性作为统一尺度，同时使用二维图像和距离图像来测量位置和姿势。

根据第二典型实施例，对于点群数据中的各点使用相同的协方差矩阵∑。然而，对于所有点信息，协方差矩阵不一定相同。如果距离传感器能够以像素和点为单位输出测量的可靠度，则可以基于该可靠度对各点计算协方差矩阵，并且使用计算出的协方差矩阵。

此外，根据第二典型实施例，边缘检测误差的分布服从相同的标准差。然而，本发明不局限于此。换句话说，如果对于图像中检测到的各边缘可以估计检测的模糊，则可以基于模糊计算标准差，并且可以对于各边缘改变标准差。边缘检测的模糊可以使用在边缘检测中所使用的核大小。结果，对模糊边缘(ambiguous edge)应用小的权重，并且对以高精度检测到的边缘应用大的权重，因而可以以更高精度计算位置和姿势。

根据第一和第二典型实施例，同时使用二维图像和三维测量信息计算物体的位置和姿势。根据本发明的第三典型实施例，代替同时使用二维图像和三维点群，通过分开使用它们来计算物体的位置和姿势。然后将这两个结果合并。由于位置和姿势测量设备的结构和用于测量位置和姿势的处理与第一典型实施例的相同，所以省略了说明。

图9是详细示出根据第三典型实施例用于计算位置和姿势的处理的流程图。该处理对应于在图3的流程图所示的步骤S304所进行的处理。

在图9所示的步骤S901，位置和姿势计算单元160通过仅使用三维点群，计算物体的位置和姿势。用于计算位置和姿势的处理大体上与第一典型实施例所述的方法相同，并且不同仅在于使用三维点群数据。将基于三维点群数据计算出的物体的位置和姿势表示为六维矢量s_3D。位置和姿势计算单元160同时计算表示计算出的位置和姿势的模糊的6×6协方差矩阵∑_3D。位置和姿势计算单元160使用协方差矩阵∑_3D以在后面合并计算出的位置和姿势与要基于二维图像计算的位置和姿势。位置和姿势计算单元160如下所述计算位置和姿势的协方差矩阵。位置和姿势计算单元160因而使用2D-2D对应关系和3D-3D对应关系计算位置和姿势(W.Hoff and T.Vincent，“Analysis of headorientation accuracy in augmented reality”，IEEE Transactions onVisualization and Computer Graphics，vol.6，no4，pp.319-334，2000)。

根据本典型实施例，位置和姿势计算单元160基于三维空间中的点和平面之间的对应关系和二维图像中的点和线之间的对应关系计算协方差矩阵。将平面的法线方向上的点群数据的测量误差的成分表示为Δp，并且将其标准差表示为σ_3D。通过与第二典型实施例所述相同的方法计算标准差σ_3D。如果假定Δp对应于微小变化Δs_3D，则通过进行线性近似获得方程(18)(对于符号的定义，请参考：Hiura，Yamaguchi，Sato，Ikenouchi，“Real-time Tracking of Free-Form Objects by Range and IntensityImage Fusion”，Denshi Joho Tsushin Gakkai Ronbunshi，Vol.J80-D-II，No.11，November 1997，pp.2904-2911)。

[数学公式19]

然后通过对于三维点群数据中的所有点的方程(18)进行合并获得方程(19)。

[数学公式20]

其中，

[数学公式21]

因此可以如方程(20)一样表示方程(19)。

[数学公式22]

ΔP＝JΔs_3D    …(20)

基于方程(20)，然后使用最小二乘法如方程(21)一样计算Δs_3D。

[数学公式23]

Δs_3D＝(J^TJ)^-1J^TΔP    …(21)

Δs_3D的协方差矩阵∑_3D变成下面的协方差矩阵。E[...]表示...的期望值。

[数学公式24]

换句话说，根据点群数据的测量误差的标准差、三维平面参数和三维坐标中的位置和姿势的线性偏微分(雅可比矩阵)计算物体的位置和姿势的协方差矩阵∑_3D。

在步骤S902，位置和姿势计算单元160仅使用二维图像计算物体的位置和姿势。位置和姿势计算单元160以第一典型实施例所述的计算方法，通过仅使用边缘来计算位置和姿势。将所获得的位置和姿势表示为六维矢量s_2D。位置和姿势计算单元160计算表示计算出的位置和姿势的模糊的6×6协方差矩阵∑_2D，同时计算位置和姿势。位置和姿势计算单元160使用协方差矩阵以在后面合并计算出的位置和姿势与在步骤S901计算出的位置和姿势。位置和姿势计算单元160如下所述计算位置和姿势的协方差矩阵∑_2D。

将边缘的搜索方向的检测误差表示为Δd，并且将其标准差表示为σ_2D。通过根据第二典型实施例的相同方法计算标准差σ_2D。如果假定Δd对应于微小变化Δs_2D，则通过进行线性近似获得方程(23)(对于符号的定义，请参考第一典型实施例)。

[数学公式25]

然后通过对于所有边缘的方程(23)进行合并获得方程(24)。

[数学公式26]

其中，

[数学公式27]

因而可以如方程(25)一样表示方程(24)。

[数学公式28]

ΔD＝KΔs_1D    …(25)

然后如方程(26)一样，通过如计算∑_3D一样类似获得协方差矩阵∑_2D。

[数学公式29]

换句话说，根据边缘检测误差的标准差、投影至图像上的线段的方程和图像坐标中的位置和姿势的线性偏微分(雅可比矩阵)来计算协方差矩阵∑_2D。

在步骤S903，位置和姿势计算单元160合并基于三维测量信息所计算出的位置和姿势s_3D和基于二维图像所计算出的位置和姿势s_2D。更具体地，如果s_final是表示合并后的位置和姿势的六维矢量，则如方程(27)一样计算s_final。

[数学公式30]

s_final＝∑_2D(∑_2D+∑_3D)^-1s_3D+∑_3D(∑_2D+∑_3D)^-1s_2D  …(27)

通过进行上述计算，通过对方的测量结果相互补偿各位置和姿势的模糊。结果，可以高精度地测量位置和姿势。

如上所述，根据第三典型实施例，根据二维图像和三维点群数据分开计算物体的位置和姿势。然后使用计算出的位置和姿势的协方差矩阵合并作为结果的位置和姿势来测量物体的位置和姿势。

在上述典型实施例中，使用边缘作为二维图像的特征。然而，二维图像的特征不局限于边缘，并且可以使用其它特征。例如，可以将对象物体的三维模型表示为三维点群数据。然后基于作为图像特征所检测到的特征点和三维空间中的点之间的对应关系，计算位置和姿势。此外，代替仅使用一个特定特征，在位置和姿势的计算中可以使用多个特征(例如，特征点和边缘)。

此外，在上述典型实施例中，使用输出密集距离图像的距离传感器作为三维测量设备。然而，三维测量设备不局限于此，并且可以进行稀疏测量。例如，三维测量设备可以是使用点光源的测距设备。

还可以利用读出并执行记录在存储器装置上的程序以进行上述实施例的功能的***或设备的计算机(或者CPU或MPU等装置)和通过下面的方法实现本发明的各方面，其中，利用***或设备的计算机通过例如读出并执行记录在存储器装置上的程序以进行上述实施例的功能来进行上述方法的各步骤。为此，例如，通过网络或者通过用作存储器装置的各种类型的记录介质(例如，计算机可读介质)将该程序提供给计算机。

尽管参考典型实施例说明了本发明，但是应该理解，本发明不局限于所公开的典型实施例。所附权利要求书的范围符合最宽的解释，以包含所有这类修改、等同结构和功能。

本申请要求2009年7月28日提交的日本2009-175387号专利申请的优先权，其全部内容通过引用包含于此。

Claims (16)

1.一种位置和姿势校准方法，用于校正所存储的物体的位置和姿势，所述位置和姿势校准方法包括以下步骤：

输入所述物体的二维图像；

从所述二维图像检测图像特征；

输入所述物体的表面的三维信息；

计算所检测到的图像特征与在基于所存储的物体的位置和姿势将所存储的三维模型投影至所述二维图像上时所获得的投影图像的投影特征之间的二维差异；

计算所述三维信息的三维特征和所存储的位置和姿势中的所述三维模型的模型特征之间的三维差异；

转换所述二维差异和/或所述三维差异的尺度以使得所述二维差异和所述三维差异具有同等的尺度；以及

基于转换了所述二维差异和所述三维差异中的至少一个的尺度之后的所述二维差异和所述三维差异，校正所存储的位置和姿势。

2.根据权利要求1所述的位置和姿势校准方法，其特征在于，还包括以下步骤：

输入和存储所述物体的近似位置和姿势，作为所述物体的位置和姿势；以及

基于所输入和存储的位置和姿势计算所述二维差异。

3.根据权利要求1所述的位置和姿势校准方法，其特征在于，所述二维图像包括所述物体的拍摄图像。

4.根据权利要求1所述的位置和姿势校准方法，其特征在于，所述三维信息包括从距离图像获取的所述物体的表面上的点群的三维坐标。

5.根据权利要求1所述的位置和姿势校准方法，其特征在于，所述图像特征包括特征点或边缘。

6.根据权利要求1所述的位置和姿势校准方法，其特征在于，还包括以下步骤：

计算多个二维差异作为所述二维差异；以及

基于所述多个二维差异中小于或等于预定阈值的二维差异，校正所存储的位置和姿势。

7.根据权利要求1所述的位置和姿势校准方法，其特征在于，还包括以下步骤：

计算多个三维差异作为所述三维差异；以及

基于所述多个三维差异中小于或等于预定阈值的三维差异，校正所存储的位置和姿势。

8.根据权利要求1所述的位置和姿势校准方法，其特征在于，还包括以下步骤：

将所述二维差异转换成与三维空间中的距离相当的尺度。

9.根据权利要求8所述的位置和姿势校准方法，其特征在于，还包括以下步骤：

通过将所述二维差异乘以从所述三维信息获取的深度值来转换所述二维差异的尺度。

10.根据权利要求1所述的位置和姿势校准方法，其特征在于，还包括以下步骤：

将所述二维差异和所述三维差异的尺度分别转换成所述二维差异和所述三维差异的似然性。

11.一种位置和姿势校准设备，用于校正所存储的物体的位置和姿势，所述位置和姿势校准设备包括：

图像输入单元，用于输入所述物体的二维图像；

特征检测单元，用于从所述二维图像检测图像特征；

三维信息输入单元，用于输入所述物体的表面的三维信息；

二维图像差异计算单元，用于计算所检测到的图像特征与在基于所存储的物体的位置和姿势将所存储的三维模型投影至所述二维图像上时所获得的投影图像的投影特征之间的二维差异；

三维空间差异计算单元，用于计算所述三维信息的三维特征和所存储的位置和姿势中的所述三维模型的模型特征之间的三维差异；

同等尺度转换单元，用于转换所述二维差异和/或所述三维差异的尺度以使得所述二维差异和所述三维差异具有同等的尺度；以及

位置和姿势校正单元，用于基于转换了所述二维差异和所述三维差异中的至少一个的尺度之后的所述二维差异和所述三维差异，校正所存储的位置和姿势。

12.一种位置和姿势校准方法，用于校正所存储的物体的位置和姿势，所述位置和姿势校准方法包括以下步骤：

输入所述物体的二维图像；

从所述二维图像检测图像特征；

输入所述物体的表面的三维信息；

计算所检测到的图像特征与在基于所存储的物体的位置和姿势将所存储的三维模型投影至所述二维图像上时所获得的投影图像的投影特征之间的二维差异；

计算所述三维信息的三维特征与所存储的位置和姿势中的所述三维模型的模型特征之间的三维差异；以及

使用包括所述二维差异和所述三维差异的评价函数来校正所存储的位置和姿势。

13.根据权利要求12所述的位置和姿势校准方法，其特征在于，还包括以下步骤：

转换所述二维差异和/或所述三维差异的尺度以使得所述二维差异和所述三维差异具有同等的尺度；以及

使用包括转换了所述二维差异和所述三维差异中的至少一个的尺度之后的所述二维差异和所述三维差异的评价函数来校正所存储的位置和姿势。

14.根据权利要求12所述的位置和姿势校准方法，其特征在于，还包括以下步骤：

使用包括所述二维差异的评价函数计算所述物体的位置和姿势的第一模糊值；

使用包括所述三维差异的评价函数计算所述物体的位置和姿势的第二模糊值；以及

使用包括所述第一模糊值和所述第二模糊值的评价函数来校正所存储的位置和姿势。

15.根据权利要求14所述的位置和姿势校准方法，其特征在于，

所述第一模糊值和所述第二模糊值分别是协方差矩阵。

16.一种位置和姿势校准设备，用于校正所存储的物体的位置和姿势，所述位置和姿势校准设备包括：

图像输入单元，用于输入所述物体的二维图像；

特征检测单元，用于从所述二维图像检测图像特征；

三维信息输入单元，用于输入所述物体的表面的三维信息；

二维图像差异计算单元，用于计算所检测到的图像特征与在基于所存储的物体的位置和姿势将所存储的三维模型投影至所述二维图像上时所获得的投影图像的投影特征之间的二维差异；

三维空间差异计算单元，用于计算所述三维信息的三维特征与所存储的位置和姿势中的所述三维模型的模型特征之间的三维差异；以及

位置和姿势校正单元，用于使用包括所述二维差异和所述三维差异的评价函数来校正所存储的位置和姿势。