CN101331381B - 三维形状数据的位置对准方法和装置 - Google Patents

Abstract

本发明实施下述步骤：数据输入步骤(S1)，将三维形状上的坐标值输入到计算机中；模型构筑步骤(S4)，构筑环境模型，该环境模型将三维形状所存在的空间区域分割为由边界表面相互正交的立方体构成的多个体素，并存储各体素位置；以及匹配步骤(S5)，在对应于坐标值的体素的内部设定代表点及其误差分布并存储起来。还实施：精密对准步骤(S7)，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，或者使针对新的计测位置的环境模型旋转及平移，并以邻近的误差分布间距离的总和成为最小的方式，进行位置对准。并将所述体素位置、代表点以及误差分布的至少任一个输出到输出装置中。

Description

三维形状数据的位置对准方法和装置

技术领域

本发明涉及一种用于从多个计测位置来计测静止的三维形状并综合其距离数据来进行位置对准的三维形状数据的位置对准(positionmatching；aligning)方法和装置。

背景技术

在本发明中将对从多视点计测静态三维形状的多个距离图像的几何学位置关系进行推定的过程称为“位置对准”。

三维形状的计测技术已普及，三维形状数据用于各种各样的应用领域，例如机械零件/工件等的形状计测、移动机器人中的自身位置的辨认、地形/结构物的计测等。

在进行某个物体的三维计测来复原三维形状的情况下，从一个视点(计测位置)的计测能够获得面对该视点的表面信息，但不能计测物体的背面。因此，为了复原三维形状，需要将来自多个视点的计测及其计测结果精度良好地进行位置对准。

为了对从多个视点取得的数据进行位置对准，视点和计测对象的位置关系必须是明显的。作为取得该视点和计测对象的位置关系的方法，存在将物体配置在台子上并使其旋转的方法，或者使具有连杆机构(link mechanism)的臂状物等带有视点的方法，但均对位置关系的精度有极限。此外，假设即使能够得到精度，也会发生对计测物体的大小的限制。

因此作为不受位置关系的精度的影响，并且对计测物体的大小也没有限制的办法，有多种多样的一边比较先得到的计测数据和之后得到的计测数据一边进行位置对准的办法被提出。

其中，在对应点是已知的情况下，作为通过旋转和平移(translate)的平方误差的最小化问题，能够容易地求得。但是，一般对应点并不是已知的。

作为对应点不是已知的情况下的位置对准办法，根据不变特征量的匹配(matching)、直接法以及ICP算法已经被提出(例如，非专利文献1、2)。

“根据不变特征量的匹配”是对形状利用不变的局部特征点(微分特征等)来进行两个数据的匹配。

“直接法”是关于三维图像求出二维图像的光流(optical flow)。

“ICP(Iterative Closest Points：迭代最近点)算法”是求出相对于先前的计测数据的其后的计测数据的最邻近的点，一边进行旋转/平移一边以其距离的总和为最小的状态变成一致状态的方式来求解。

此外，作为与本发明有关的技术，公开有非专利文献3。

非专利文献1：增田健等、“来自多个距离图像的形状模型生成技术”

非专利文献2：Paul J.Besl，“A Method for Registration of 3-D Shapes”，IEEE Transactions of Pattern Analysis and Mechanical Intelligence，Vol.14，No.2，February 1992

非专利文献3：关本清英等、“三维激光雷达的开发”，石川岛播磨技报Vol.43 No.4(2003-7)

发明内容

发明要解决的问题

在使用三维激光雷达那样的距离传感器的情况下，被计测的三维形状上的被计测点成为在横向以及垂直方向上离散的点群。该点群的间隔，在距计测点的距离例如是50m的情况下，被计测点的间隔例如达到在横向是大约315mm，在垂直方向是大约525mm。

此外，在从多个计测位置来计测静止的三维形状的情况下，在三维激光雷达那样的距离传感器，每个计测位置，通常被计测点的位置不同。

进而，这样的距离传感器一般在测定距离上具有大约20cm前后的误差。

因此，在使用三维激光雷达那样的距离传感器的情况下，有以下的限制条件A～C。

条件A：计测数据中点数少(例如，1帧＝166×50点)

条件B：计测数据中包含误差(例如，在测定距离中是大约20cm前后)

条件C：计测数据未必测量相同的计测点。

即，得到的距离数据是在横向以及垂直方向上离散的点群，由于每一计测的位置不同，所以没有对应点，在测定距离中包含比较大的误差。

在以上述的“根据不变特征量的匹配”或“直接法”来处理这样的距离数据的情况下，由于是离散的点群，计测点少，没有对应点，计测误差大，所以正确的位置对准几乎不可能。

此外，在使用ICP算法的情况下，点群的位置对准在原理上是可能的，但有以下问题。

(1)误差的累积

ICP算法是两个距离数据的重合办法，反复先前的数据和其后的数据的比较，即使积分其差分，由于几乎没有对应点，所以误差累积。

(2)计算量多

ICP算法由于是反复计算，所以计算量变得庞大。即，ICP算法由于需要探测对应于计测数据的各数据点的模型数据，所以当模型数据点数以及计测数据点数增加时计算量增大。具体是，在将模型数据的点数作为M，计测数据的点数作为N的情况下，例如全探测时的计算量级(order)成为O(M×N)。

(3)不能处理计测点少的情况

由于ICP算法以密集的距离数据为对象，所以在是离散的点群，在空间上稀疏的情况下，会收敛到错误的结果。

发明概要

本发明是为了解决上述问题而做出的。即，本发明的目的是提供一种三维形状数据的位置对准方法和装置，其中，即使在从多个计测位置来计测静止的三维形状的距离数据是空间上离散的点群，每一计测的位置不同且没有对应点，包含比较大的误差的情况下，也没有误差的累积，并以较少的计算量就能够进行正确的位置对准。

根据本发明，提供一种三维形状数据的位置对准方法，用于从多个计测位置来计测静止的三维形状，综合其距离数据以进行位置对准，其特征在于，实施下述步骤：数据输入步骤，在新的计测位置处将三维形状上的坐标值输入到计算机中；模型构筑步骤，构筑环境模型，该环境模型将所述三维形状所存在的空间区域分割为边界表面相互正交的长方体构成的多个体素(voxel)，存储各体素位置；以及匹配步骤，在对应于所述坐标值的体素的内部设定代表点及其误差分布并存储起来，还具有下述步骤：精密对准步骤，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，或者使针对新的计测位置的环境模型旋转及平移，并以与邻近的误差分布间距离有关的评价值(例如，该距离的总和)成为最小的方式，进行位置对准；以及输出步骤，将所述体素位置、代表点以及误差分布输出到输出装置中。

再有，上述与距离有关的评价值，代替该距离的总和，是该距离的平均值、该距离的二次方的总和、或该距离的最大值也可，是其他的适合的评价值也可。

根据本发明的优选实施方式，在所述匹配步骤中，在体素内部除了代表点及其误差分布之外，还设定并存储表示在体素内物体存在概率的概率值。

此外，在所述精密对准步骤之前具有：粗略对准步骤，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，并以与具有代表点的体素间距离有关的评价值(例如，该距离的总和)成为最小的方式，进行位置对准，其中该代表点是与计测数据以及误差分布邻近的代表点，或者使针对新的计测位置的环境模型旋转及平移，并以与具有代表点的体素间距离有关的评价值(例如，该距离的总和)成为最小的方式，进行位置对准。

再有，与具有和计测数据以及误差分布邻近的代表点的体素间距离有关的上述评价值，代替该距离的总和，是该距离的平均值、该距离的二次方的总和、或该距离的最大值也可，是其他的适合的评价值也可。

此外，与具有代表点的体素间距离有关的上述评价值，代替该距离的总和，是该距离的平均值、该距离的二次方的总和、或该距离的最大值也可，是其他的适合的评价值也可。

或者，在所述精密对准步骤之前具有：粗略对准步骤，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，并以与具有代表点的体素的概率值有关的评价值(例如，该概率值的总和)成为最大的方式，进行位置对准，其中该代表点是与计测数据以及误差分布邻近的代表点，或者使针对新的计测位置的环境模型旋转及平移，并以与邻近的体素所具有的概率值的差有关的评价值(例如，该概率值的差的总和)成为最小的方式，进行位置对准。

再有，与具有和计测数据以及误差分布邻近的代表点的体素的概率值有关的上述评价值，代替该概率值的总和，是该概率值的平均值、该概率值的二次方的总和、或该概率值的最小值也可，是其他的适合的评价值也可。

此外，与邻近的体素所具有的概率值的差有关的上述评价值，代替该概率值的差的总和，是该概率值的差的平均值、该概率值的差的二次方的总和、或该概率值的差的最大值也可，是其他的适合的评价值也可。

此外，在所述数据输入步骤之后具有：探测范围限定步骤，根据过去的计测位置的变化推定当前的计测位置，或通过能够取得当前的计测位置的传感器来取得，或不仅利用计测数据的距离值还利用反射强度值，限定核对的范围。

此外，在所述模型构筑步骤中，在将最大的体素设定为相当于必要最小限度的分辨能力的大小，而且单一的体素内存在多个被计测点的情况下，以在单一的体素内仅存在单一的被计测点的方式，将该体素进一步分割并分级地分割成多个体素。

此外，在所述精密对准步骤之后具有更新所述环境模型的模型更新步骤，在该模型更新步骤中，探测对应于新输入的被计测点的坐标值的体素，作为在原点和被计测点之间不存在物体的，再设定或擦除位于其间的体素内的代表点和误差分布。

此外，在所述精密对准步骤之后具有更新所述环境模型的模型更新步骤，在该模型更新步骤中，探测对应于新输入的被计测点的坐标值的体素，在该体素内没有代表点的情况下，将所述坐标值和误差分布设定为代表点的坐标值和误差分布。

此外，在所述精密对准步骤之后具有更新所述环境模型的模型更新步骤，在该模型更新步骤中，探测对应于新输入的被计测点的坐标值的体素，在所述体素内有已经设定了的代表点的情况下，比较新取得的误差分布和已经设定了的体素内的误差分布，在误差分布相互重叠的情况下，根据两误差分布、或根据两误差分布和体素内已经设定了的代表点和新输入的被计测点的坐标值，再设定新的误差分布和新的代表点，在误差分布不相互重叠的情况下，以在单一的体素内仅存在单一的代表点的方式，将该体素进一步分割并分级地分割为多个体素。体素的分割例如使用八叉树或K-D树。

在所述模型更新步骤中，比较所述新取得的误差分布和所述已经设定了的体素内的误差分布，在误差分布相互重叠的情况下，根据两误差分布再设定新的误差分布和新的代表点，其结果是新的代表点移动到其他体素内，此时，在该其他体素内没有代表点的情况下，在该其他体素的内部设定该新的误差分布和该新的代表点，在该其他体素内有已经设定了的代表点的情况下，比较该新的误差分布和已经设定了的该其他体素内的误差分布，(A)在误差分布相互重叠的情况下，根据两误差分布、或根据两误差分布和体素内已经设定了的代表点和新输入的被计测点的坐标值，再设定新的误差分布和新的代表点，(B)在误差分布不相互重叠的情况下，以在单一的体素内仅存在单一的代表点的方式，将该体素进一步分割并分级地分割为多个体素。

根据本发明的另外的实施方式，在所述精密对准步骤之后具有更新所述环境模型的模型更新步骤，在该模型更新步骤中，根据新输入的被计测点的坐标值及其误差分布、和已经设定了的体素内的代表点及其误差分布，通过卡尔曼滤波器取得新的代表点和误差分布并进行再设定。

在所述精密对准步骤中，代替以与所述邻近的误差分布间距离有关的评价值成为最小的方式进行位置对准，而是以与一致度有关的评价值(例如，该一致度的总乘积)成为最大的方式，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，或者使针对新的计测位置的环境模型旋转及平移，从而进行位置对准，其中该一致度是通过基于所述邻近的误差分布间的极大似然推定值而确定的。

所述与一致度有关的评价值的计算式以下面的[算式1]表示，

[算式1]

$\mathrm{EM}=\underset{j=1}{\overset{N}{\mathrm{\Π}}}\left\{\mathrm{\ω}\left(j\right)\mathrm{EM}(i,j)\right\}$

在该式中，计测点j和环境模型上的代表点i对应起来，能得到该计测点j的计测数据的概率为EM(i，j)，ω(j)在环境模型中与计测点j对应起来的代表点存在的情况下是1，在除此之外的情况下为0。

此外，根据本发明，提供一种三维形状数据的位置对准装置，用于从多个计测位置来计测静止的三维形状，综合其距离数据以进行位置对准，其特征在于，具备：数据输入装置，将三维形状上的坐标值输入到计算机中；模型构筑装置，构筑环境模型，该环境模型将所述三维形状所存在的空间区域分割为由边界表面相互正交的长方体构成的多个体素，并存储各体素位置；匹配装置，在对应于所述坐标值的体素的内部设定代表点及其误差分布并存储起来；以及数据传输装置，将所述体素位置、代表点以及误差分布输出到输出装置中，对于针对先前的计测位置的环境模型，使针对新的计测位置的环境模型旋转及平移，并以与邻近的误差分布间距离有关的评价值(例如，该距离的总和)成为最小的方式，进行位置对准。

所述匹配装置在体素的内部除了代表点及其误差分布之外，还设定并存储表示在体素内物体存在概率的概率值。

所述三维形状数据的位置对准装置，在所述位置对准(所述精密对准步骤)之前，进行：粗略对准步骤，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，以与具有代表点的体素间距离有关的评价值(例如，该距离的总和)成为最小的方式，进行位置对准，其中该代表点是与计测数据以及误差分布邻近的代表点，或者使针对新的计测位置的环境模型旋转及平移，以与具有代表点的体素间距离有关的评价值(例如，该距离的总和)成为最小的方式，进行位置对准。

所述三维形状数据的位置对准装置，在所述精密对准步骤之前，进行：粗略对准步骤，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，并以与具有代表点的体素的概率值有关的评价值(例如，该概率值的总和)成为最大的方式，进行位置对准，其中该代表点是与计测数据以及误差分布邻近的代表点，或者使针对新的计测位置的环境模型旋转及平移，并以与邻近的体素所具有的概率值的差有关的评价值(例如，该概率值的差的总和)成为最小的方式，进行位置对准。

所述三维形状数据的位置对准装置，在所述数据输入步骤之后，进行：探测范围限定步骤，根据过去的计测位置的变化推定当前的计测位置，或通过能够取得当前的计测位置的传感器来取得，或不仅利用计测数据的距离值还利用反射强度值，限定核对的范围。

所述三维形状数据的位置对准装置，在所述精密对准步骤中，将误差分布交叉的情况作为同一计测点，将根据分布的一致度求得的加权(weight)乘以该情况的距离值，计算误差分布间的距离。

所述模型构筑装置，在将最大的体素设定为相当于必要最小限度的分辨能力的大小，而且单一的体素内存在多个被计测点的情况下，以在单一的体素内仅存在单一的被计测点的方式，将该体素进一步分割并分级地分割成多个体素。

所述三维形状数据的位置对准装置，在所述精密对准步骤之后，进行更新所述环境模型的模型更新步骤，在该模型更新步骤中，探测对应于新输入的被计测点的坐标值的体素，作为在原点和被计测点之间不存在物体的，再设定或擦除位于其间的体素内的代表点和误差分布。

所述三维形状数据的位置对准装置，在所述精密对准步骤之后具有更新所述环境模型的模型更新装置，该模型更新装置探测对应于新输入的被计测点的坐标值的体素，在该体素内没有代表点的情况下，将所述坐标值和误差分布设定为代表点的坐标值和误差分布。

所述三维形状数据的位置对准装置，在所述精密对准步骤之后具有更新所述环境模型的模型更新装置，该模型更新装置探测对应于新输入的被计测点的坐标值的体素，在所述体素内有已经设定了的代表点的情况下，比较新取得的误差分布和已经设定了的体素内的误差分布，在误差分布相互重叠的情况下，根据两误差分布、或根据两误差分布和体素内已经设定了的代表点和新输入的被计测点的坐标值，再设定新的误差分布和新的代表点，在误差分布不相互重叠的情况下，以在单一的体素内仅存在单一的代表点的方式，将该体素进一步分割并分级地分割为多个体素。

所述模型更新装置比较所述新取得的误差分布和所述已经设定了的体素内的误差分布，在误差分布相互重叠的情况下，根据两误差分布再设定新的误差分布和新的代表点，其结果是新的代表点移动到其他体素内，此时，在该其他体素内没有代表点的情况下，在该其他体素的内部设定该新的误差分布和该新的代表点，在该其他体素内有已经设定了的代表点的情况下，比较该新的误差分布和已经设定了的该其他体素内的误差分布，(A)在误差分布相互重叠的情况下，根据两误差分布、或根据两误差分布和体素内已经设定了的代表点和新输入的被计测点的坐标值，再设定新的误差分布和新的代表点，(B)在误差分布不相互重叠的情况下，以在单一的体素内仅存在单一的代表点的方式，将该体素进一步分割并分级地分割为多个体素。

根据本发明的另外的实施方式，所述三维形状数据的位置对准装置，在所述精密对准步骤之后具有更新所述环境模型的模型更新装置，该模型更新装置根据新输入的被计测点的坐标值及其误差分布、和已经设定了的体素内的代表点及其误差分布，通过卡尔曼滤波器取得新的代表点和误差分布并进行再设定。

所述三维形状数据的位置对准装置，在所述精密对准步骤中，代替以与所述邻近的误差分布间距离有关的评价值(例如，该一致度的总乘积)成为最小的方式进行位置对准，而是以与一致度有关的评价值(例如，该一致度的总乘积)成为最大的方式，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，或者使针对新的计测位置的环境模型旋转及平移，从而进行位置对准，其中该一致度是通过基于所述邻近的误差分布间的极大似然推定值而确定的。在该情况下，所述与一致度有关的评价值的计算式以上述[算式1]表示。

发明的效果

根据所述本发明的方法和装置，由于将三维形状所存在的空间区域分割为多个体素，并存储各体素位置，所以即使是计测对象物很大的情况下，也能够将数据量抑制在与体素数成比例的小的数据尺寸。

此外，因为在对应于坐标值的体素的内部设定并储存代表点及其误差分布，所以能够表现体素的分辨能力以上的信息。

此外，通过设定、保存表示在体素的内部物体存在概率的概率值，从而即使在误差分布比代表点所属的体素扩展的情况下，也不用找到代表点所属的体素，从其误差分布再计算各体素中的物体的存在有无，而是仅以该体素的概率值就能容易判断，所以能够抑制检索时间。

此外，由于根据过去的计测位置的变化推定当前的计测位置，或通过能够取得当前的计测位置的传感器来取得，或不仅利用计测数据的距离值还利用反射强度值，限定核对的范围，所以能够抑制检索时间。

此外，由于在粗略对准步骤中，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，并以与具有代表点的体素间距离有关的评价值(例如，该距离的总和)成为最小的方式，进行位置对准，其中该代表点是与计测数据以及误差分布邻近的代表点，或者使针对新的计测位置的环境模型旋转及平移，并以与具有代表点的体素间距离有关的评价值(例如，该距离的总和)成为最小的方式，进行位置对准，或者，使新的计测数据以及误差分布旋转及平移，并以与具有代表点的体素的概率值有关的评价值(例如，该概率值的总和)成为最大的方式，进行位置对准，其中该代表点是与计测数据以及误差分布邻近的代表点，或者使针对新的计测位置的环境模型旋转及平移，并以与邻近的体素所具有的概率值的差有关的评价值(例如，该概率值的差的总和)成为最小的方式，进行位置对准，所以能够一边防止误差的累积，一边短时间地进行具有代表点的体素彼此的位置对准。

此外，由于对于针对先前的计测位置的环境模型，使针对新的计测位置的环境模型旋转及平移，并以与邻近的体素所具有的概率值的差有关的评价值(例如，该概率差的总和)成为最小的方式，进行位置对准，在该情况下，还附加物体不存在的信息进行位置对准，所以能够谋求精度的提高。

接着，由于在精密对准步骤中，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，或者使针对新的计测位置的环境模型旋转及平移，并以邻近的误差分布间距离的总和成为最小的方式，进行位置对准，所以能够短时间地进行误差分布彼此的精密的位置对准。

因此，可利用通过将逐次得到的计测数据综合到本发明的环境模型而得到的模型数据，来求出计测数据最一致的位置姿势，而且能够防止误差的累积。

此外，由于当使用本发明的环境模型时，通过在体素内具有一个代表点从而点数减少，因此能够使计算量降低。即，由于在本发明提出的数据结构是在体素内保存一个代表点，由于能够使探测对应于计测点的模型点的计算量级为1，所以能够将整体的计算量级减少为O(N)。

此外，虽然点数减少，但本发明的环境模型通过进行代表点的误差分布和体素的分级化，能够保持精度。

进而，原来的ICP算法对于稀疏数据输出错误的结果，但本发明的环境模型由于在体素内具有代表点和误差分布，所以对应于稀疏数据的位置对准成为可能。

因此，根据本发明的方法和装置，在使用多个视点的计测数据的三维形状信息复原中，即使是稀疏数据也能防止误差的累积并同时能取得高精度的形状。此外，由于成为重合时的比较对象的数据量减少，所以能够减少计算量。

进而，由于在所述模型更新步骤中，根据新输入的被计测点的坐标值及其误差分布、和已经设定了的体素内的代表点及其误差分布，通过卡尔曼滤波器取得新的代表点和误差分布并进行再设定，所以能够得到更接近真值的形状。

特别是，通过反复使用卡尔曼滤波器的模型更新步骤，从而即使是包含误差的数据也能通过卡尔曼滤波器的效果得到收敛到真值的高精度的形状。

此外，通过在所述精密对准步骤中，代替以与所述邻近的误差分布间距离有关的评价值成为最小的方式进行位置对准，而是以与一致度有关的评价值(例如，该一致度的总乘积)成为最大的方式，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，或者使针对新的计测位置的环境模型旋转及平移来进行位置对准，其中该一致度是通过基于所述邻近的误差分布间的极大似然推定值而确定的，从而，能够考虑环境模型、计测数据双方的误差来进行位置对准。

本发明的其他目的以及有利的特征可从参照附图的以下说明而明确。

附图说明

图1是在非专利文献2中公开的三维激光雷达的结构图。

图2A是表示以距离传感器计测的极坐标数据和误差的关系的图。

图2B表示将误差分布作为包含在长方体中的椭圆体来近似的情况。

图3是用于执行本发明的方法的装置结构图。

图4是表示本发明的方法的流程图。

图5是模型构筑步骤的示意图。

图6是构筑的环境模式的示意图。

图7A是表示本发明的体素数据的数据构造的图，表示了各体素数据的存储器布局例。

图7B是表示本发明的体素数据的数据构造的图，表示了等级(level)2(1，1，0)的体素具有代表点的情况的例子。

图8是粗略对准步骤S6和精密对准步骤S7的数据处理流程图。

图9是粗略对准步骤S6的示意图。

图10是精密对准步骤S7的示意图。

图11是模型更新步骤中的数据处理流程图。

图12是在相应的体素内有已经设定了的代表点情况的示意图。

图13表示在误差分布相互重叠的情况下，根据两误差分布再设定新的误差分布和新的误差分布的中心，其结果是新的代表点移动到其他体素内的情况。

图14是误差分布相互重叠的情况的示意图。

图15表示通过使用了卡尔曼滤波器的模型更新步骤得到的结果。

图16是图15的局部放大图。

图17表示考虑了误差的对应。

具体实施方式

以下参照附图对本发明的优选实施方式进行说明。再有，在各图中，对共通的部分赋予同一附图标记，省略重复的说明。

图1是作为距离传感器的一例的三维激光雷达的结构图。三维激光雷达例如是在非专利文献3中公开的。

如该图所示，三维激光雷达10包括雷达头(radar head)12和控制器20。从激光二极管13激发的脉冲激光1在投射透镜14被整形为平行光2，用反射镜18a、18b和旋转/摇动的多角镜15在二维方向上进行扫描，照射到测定对象物。从测定对象物反射的脉冲激光3，经由多角镜15在受光透镜16会聚，在光检测器17转换为电信号。

控制器20内的时间间隔计数器21，计测与激光二极管13的脉冲振荡定时同步的起动脉冲4、和从光检测器17输出的停止脉冲5的时间间隔。信号处理板22将检测出反射光时刻的时间间隔t、多角镜的旋转角度θ、摇动角度

作为极坐标数据(r，θ，

)输出。

r是以计测位置(雷达头设置位置)为原点的距离，以r＝c×t/2的式子求出。在这里c是光速。

判定处理单元23将来自信号处理板的极坐标数据，向以雷达头设置位置为原点的三维空间数据(x，y，z)转换，进行检测处理。再有在该图中24是驱动单元。

所述三维激光雷达10的计测范围，例如是水平视场角60°、垂直视场角30°、最大测定距离50m。此外，位置检测精度例如是大约20cm。

此外，在将计测数据以对各像素具有进深方向的距离值的距离图像来表示的情况下，一帧的计测点数为横向166点，扫描方向是50点时，1帧表示166×50＝8300点。这种情况下帧频例如大约是2帧/秒。

在该三维激光雷达10计测的三维形状上的计测点，是横向上Δθ×r、垂直方向上

互相离散的点群。例如，在Δθ＝60/166×π/180＝6.3×10^-3弧度、

r＝50m的情况下，即使是最接近的情况，被计测点的间隔也是在横向大约是315mm，在垂直方向大约是525mm。

在本发明中，作为距离传感器，例如使用上述的三维激光雷达10。但是，距离传感器并不限定于此，也可以使用利用视差的距离传感器等其他公知的距离传感器。

图2A、图2B是表示在距离传感器计测的极坐标数据和误差的关系的图。

如图2A所示，将以任意的计测位置为原点的极坐标值(r，θ，

)作为计测结果进行计测。根据距离传感器的计测结果中，通常存在图示那样的误差分布。

该误差分布在误差分布的在r_s、θ_s、的存在概率为P(r_s，θ_s，

)的情况下，误差分布是在计测的轴r、θ、

方向上正态分布，例如能够以式(1)表示。这里，r、θ、

是来自传感器的计测值，σ_r、σ_θ、

是标准偏差，A是归一化常数。

如图2B所示，误差分布是通常内包于r方向上的长切头圆锥形(左图)的分布，但在远方a和b的差小。因此，能够将该误差分布作为包含在长方体中的椭圆体而近似于安全侧。

[算式2]

图3是用于执行本发明的方法的装置结构图。如该图所示，该装置具备：数据输入装置32、外部存储装置33、内部存储装置34、中央处理装置35以及输出装置36。

数据输入装置32具有上述的距离传感器，对存储装置输入三维形状上的坐标值。此外，例如并用测角仪、测距器(odometer)等，还输入距离传感器的位置/姿势或移动距离较好。再有，数据输入装置32还具有键盘等通常的输入单元较好。

外部存储装置33是硬盘、软(floppy(注册商标))盘、磁带、高密度磁盘等。外部存储装置33在环境模型的大小较大，不能保持输入到后述的内部存储装置34的三维形状上的坐标值、体素位置以及代表点及其误差分布的整体的情况下，存储相对于环境模型的一部分范围或整体范围的输入的三维形状上的坐标值、体素位置以及代表点及其误差分布的一部分或全部，而且存储用于运行本发明的方法的程序。

内部存储装置34例如是RAM、ROM等，保管相对于环境模型的一部分范围或整体范围的、输入的三维形状上的坐标值、体素位置以及代表点及其误差分布的一部分或全部，而且保管运算信息。

中央处理装置35(CPU)作为模型构筑装置、匹配装置、粗略对准和精密对准的位置对准装置、模型更新装置、数据传输装置而发挥功能，集中地处理运算和输入输出等，与内部存储装置34一起执行程序。模型构筑装置是进行后述的模型构筑步骤的装置，匹配装置是进行后述的匹配步骤的装置，位置对准装置是进行后述的粗略对准步骤和精密对准步骤的装置，模型更新装置是进行后述的模型更新步骤的装置，数据传输装置是向输出装置36输出数据的装置。

输出装置36例如是显示装置、打印机、外部存储装置等，对内部存储装置34以及外部存储装置33的至少任一个输出存储的数据以及程序的执行结果。与外部装置的接口是LAN、USB、IEEE1394等，对应于要求而输出对输入的三维形状上的坐标值附加了相应的体素内的代表点、误差分布、体素位置等的结果，或环境模型整体或环境模型的一部分。

上述本发明的装置是将所述距离传感器和通常的PC(计算机)组合在一起的也可，或是将整体一体化的装置也可。此外，一体地装入能够自动推进的装置内也可

图4是表示本发明的方法的流程图。

本发明的方法是用于根据三维形状上的被计测点的坐标值来复原三维形状的三维形状数据的位置对准方法，具有：数据输入步骤S1、数据校正步骤S2、探测范围限定步骤S3、模型构筑步骤S4、匹配步骤S5、粗略对准步骤S6、精密对准步骤S7、模型更新步骤S8以及输出步骤S9。

再有，在该一系列的处理中，S1、S2、S3、S5～S9每当得到计测数据时实施，S4仅在初次得到计测数据时实施。

在数据输入步骤S1，使用距离传感器，对计算机的存储装置输入三维形状上的坐标值。此外，例如并用测角仪、测距器等还输入距离传感器的位置/姿势或移动距离较好。

再有，在该数据输入步骤S1中，使用三维激光雷达10，将三维形状上的坐标值作为以任意的计测位置为原点的距离数据，一边移动原点一边依次取得较好。

在作为距离传感器使用三维激光雷达10的情况下，三维形状上的坐标值是以任意的计测位置为原点的距离数据，以极坐标值(r，θ，

)表示。此外，各坐标值的误差分布根据极坐标值(r，θ，

)以运算求得，或预先以别的输入单元(例如键盘)输入。

在数据校正步骤S2，进行距离数据的校正处理，提高距离数据的精度。此外，根据极坐标数据和测距器的数据，转换为以任意的固定位置为原点的三维空间数据(x，y，z)也可。

在距离数据的校正处理，进行孤立点的去除、统计上的处理等。孤立点是从周围的点孤立而存在的点，由于计测数据是以多个接近的点构成的，所以能够假定孤立点是错误计测而去除。统计上的处理是考虑计测数据包含的误差分布，通过统计处理(例如平均值等)多次的计测，进行距离的校正。

进而，成为对象的三维形状在能够直线近似或平面近似的情况下进行这些也可。

在探测范围限定步骤S3，限定距离传感器的探测范围。

当不限定探测范围而对环境模型进行计测数据的匹配处理时，有得到多个解(被计测点)的可能性。因此，实施：(1)根据过去的传感器位置的变化推定当前的传感器位置，探测传感器位置推定结果的邻近，(2)使用测距器推定传感器位置，限定探测范围，(3)距离数据中，不仅是距离值，也利用反射强度值来缩小探测结果等等。

图5是对体素的分割使用八叉树的情况下的模型构筑步骤的示意图。

在模型构筑步骤S4，如该图所示那样，构筑环境模型，该环境模型是将三维形状所存在的区域，分割为由分界表面相互正交的长方体构成的多个体素6，存储各体素位置。

体素6的形状是各边的长度相等的立方体，或各边的长度不同的长方体均可。

此外，体素6的各边的长度，将最大的体素6设定为相当于必要最小限度的分辨能力的大小较好。以下，将最大的体素6称为等级1的体素。

此外，在单一的体素内存在多个被计测点的情况下，以单一的体素内仅存在单一的被计测点的方式，例如在选择八叉树的情况下，进一步八分割体素，分级地分割为多个体素。以下，将实施了一次最大的体素6的八分割的空间区域称为等级2的体素，将实施了k次的空间区域称为等级k+1的体素。

图6是构筑的环境模型的示意图。

在匹配步骤S5，如该图所示，在对应于三维形状上的坐标值的体素6内部设定并存储代表点7与其误差分布8。末端的体素能够仅具有一个计测值的代表点。各体素通过具有计测值的代表点及其误差分布，从而表示物体的形状。此外，也能使体素具有表示物体存在概率的概率值。

在匹配步骤S5，通过[算式3]的式(2)给出代表点的绝对位置。这里，(x，y，z)是代表点在体素的相对坐标，S_x、S_y、S_z是在等级1的体素的一边的大小，n_x(k)、n_y(k)、n_z(k)是在等级k的体素的地址号码，L是所要求的代表点存在的等级。

[算式3]

$(x,y,z)+\underset{k=1}{\overset{L}{\mathrm{\Σ}}}(\frac{S_x}{2^{k-1}}{n}_x\left(k\right),\frac{S_y}{2^{k-1}}{n}_y\left(k\right),\frac{S_z}{2^{k-1}}{n}_z\left(k\right))...\left(2\right)$

图7A、图7B是表示本发明的体素数据的数据结构的图。

在该图中，图7A是各体素数据的存储器布局的例子。在该图中，箭头表示对数据的链接，将对数据的指针作为值而保持。

图7B表示等级2(1，1，0)的体素具有代表点的情况的例子。再有在该图中，null表示空集合。

所述数据结构的环境模型，具有以下特征。

(1)内容：以小长方体分割空间将计测点的代表点和误差分布保持到各体素。

(2)精度：相当于每个体素具有的计测点的代表值。

(3)存在：能够表现物体存在的有无。

(4)数据量：与体素的个数成比例地需要存储器，但大小是固定的。

(5)来自点群的转换：适应，计算量少。

(6)访问速度：由于是简单的结构，所以对要素的访问是高速的。

此外根据该特征，所述环境模型满足以下全部效果A～C。

效果A：考虑了误差的表现是可能的。

效果B：需要的存储量和计算量在一定量以下。

效果C：不仅能表示物体的存在，也能表示物体不存在。

此外在图4中，粗略对准步骤S6和精密对准步骤S7在匹配步骤S5之后实施。

图8是粗略对准步骤S6和精密对准步骤S7的数据处理流程图，图9是粗略对准步骤S6的示意图，图10是精密对准步骤S7的示意图。

在图8中，在粗略对准步骤S6，如图9所示，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，并以与具有代表点的体素间距离有关的评价值(例如，该距离的总和)成为最小的方式，进行位置对准，其中该代表点是与计测数据以及误差分布邻近的代表点，或者使针对新的计测位置的环境模型旋转及平移，并以与具有代表点的体素间距离有关的评价值(例如，该距离的总和)成为最小的方式，进行位置对准，或者，使新的计测数据以及误差分布旋转及平移，并以与具有代表点的体素的概率值有关的评价值(例如，该概率值的总和)成为最大的方式，进行位置对准，其中该代表点是与计测数据以及误差分布邻近的代表点，或者使针对新的计测位置的环境模型旋转及平移，并以与邻近的体素所具有的概率值的差有关的评价值(例如，该概率值的差的总和)成为最小的方式，进行位置对准。

粗略对准步骤S6中的位置对准，是通过在体素空间上表现环境模型和计测数据，或环境模型在体素空间上、计测数据表现代表点和误差分布，从而实施的。设当前的计测数据是在位置(x，y，z)、姿势(θ，

ψ)的计测，将计测数据转换为世界坐标来计算与环境模型的一致度。

一致度的计算例如能够使用最短距离法。在使用最短距离法的情况下的体素间距离，当两个体素空间为x⁽¹⁾、x⁽²⁾，体素的总数为I，体素的值为x_i ⁽ⁿ⁾时，能够以[算式4]的式(3)定义。

计测数据的最合适的位置/姿势，能够利用通过使位置(x，y，z)、姿势(θ，

ψ)变化而使ε为最小的最小二乘法来计算。

此外，作为一致度，例如在环境模型和计测数据的两体素中，能够使用与邻近的两体素的具有的概率值的差有关的评价值(例如，该概率值的差的总和)。在这种情况下以一致度为最小的方式，使计测数据的最适合的位置/姿势变化。

此外，在环境模型在体素空间上、计测数据表现代表值和误差分布的情况下，能够使用与计测数据的代表值以及误差分布邻近的环境模型的体素的概率值有关的评价值(例如，该概率值的总和)。在该情况下以一致度为最大的方式，使计测数据的最合适的位置/姿势变化。

[算式4]

$\mathrm{\ϵ}=\left|\right|x^{\left(1\right)}-x^{\left(2\right)}\left|\right|={\left\{\underset{i}{\overset{I}{\mathrm{\Σ}}}{({x_i}^{\left(1\right)}-{x_i}^{\left(2\right)})}^2\right\}}^{\frac{1}{2}}...\left(3\right)$

在图8中，在精密对准步骤S7，如图10所示那样，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，或者使针对新的计测位置的环境模型旋转及平移，并以与邻近的误差分布间距离有关的评价值(例如，该距离的总和)成为最小的方式，进行位置对准。

在精密对准步骤S7中的环境模型和计测数据的位置对准中，利用对能够使点群和点群的位置对准的ICP算法考虑了误差分布的手法。位置对准的初始值利用通过粗略位置对准所得到的位置/姿势。

对ICP算法利用的误差分布间的距离的计算，例如将误差分布交叉的情况考虑为同一计测点，将根据分布的一致度求得的加权(weight)乘以该情况的距离值来计算。分布的一致例如能够使用马哈拉诺比斯距离那样的距离尺度。

该情况的环境模型和计测数据的距离，当环境模型数据为p_Mi，环境模型数据的误差分布为∑_Mi，计测数据为P_Di，计测数据的误差分布为∑_Di，误差分布的合成函数为w，对应于计测数据的环境模型数据的个数为N时，能以[算式5]的式(4)定义。这里，T表示转置。

计测数据的最合适的位置/姿势，能够利用通过使计测了计测数据的位置(x，y，z)、姿势(θ，ψ)变化并移动P_Di而使ε为最小的最小二乘法来计算。

[算式5]

$\mathrm{\ϵ}=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{(p_{M_i}-p_{D_i})}^{T}w({\mathrm{\Σ}}_{M_i},{\mathrm{\Σ}}_{D_i})(p_{M_i}-p_{D_i})...\left(4\right)$

进而在图4中，模型更新步骤S8在精密对准步骤S7之后实施，更新在模型构筑步骤S4构筑的环境模型。

图11是模型更新步骤S8中的数据处理流程图。如该图所示，在步骤ST1探测对应于新输入的被计测点的坐标值的体素，在步骤ST2相应体素内没有代表点(体素是空的)的情况下，将在步骤ST3新输入的被计测点的坐标值和误差分布作为代表点的坐标值和误差分布而设定(新注册)。

此外，在该步骤ST3中，新的计测位置(原点)和被计测点之间，原理上物体不应该存在。因此再设定或擦除位于新的计测位置(原点)和被计测点之间的体素内的代表点和误差分布。

图12是在相应体素内有已经设定了的代表点的情况的示意图。

在图11的步骤ST2相应体素内有已经设定了的代表点的情况下，在步骤ST4比较新取得的误差分布和已经设定了的体素内的误差分布(即判断是不同点还是相同点)。

在该比较，在误差分布相互重叠的情况下(图12的(A))，在步骤ST5根据两误差分布、或根据两误差分布和体素内已经设定了的代表点和新输入的被计测点的坐标值，再设定新的误差分布和新的代表点(即合成误差分布)。

此外在该比较，在误差分布不相互重叠的情况下(图12的(B))，在步骤ST6、ST7以在单一的体素内仅存在单一的代表点的方式，进一步八分割该体素并分级地分割为多个体素并新注册。

分割和合成的基准，例如根据误差分布的一致度来判断。误差分布的一致度例如能够利用马哈拉诺比斯距离那样的距离尺度。此外，基于两个误差分布，通过统计上的检验来判定两者是不是表示同一点也可。

在步骤ST5根据两误差分布再设定新的误差分布和新的误差分布的中心，其结果是新的代表点向其他体素内移动时(即，在步骤ST8，“是”)，返回步骤ST2，反复进行上述处理。

再有，图13表示：在步骤ST5根据两误差分布、或根据两误差分布和体素内已经设定了的代表点和新输入的被计测点的坐标值，再设定新的误差分布和新的误差分布的中心，其结果是新的代表点向其他体素内移动的情况。

在设定表示体素内物体的存在概率的概率值的情况下，在模型更新步骤S8中，对应于体素内的代表点和误差分布的新注册、或再设定、或擦除、或在分割后的新注册的处理，体素内的概率值也通过统计处理，进行新注册、或再设定、或擦除、或在分割后的新注册。

图14是误差分布相互重叠的情况(图12的(A))的其他的示意图。在步骤ST5中，作为合成两个代表点和误差分布并设定新的代表点和误差分布的单元，能够使用卡尔曼滤波器。例如在二维的情况下，如该图所示那样，当两个代表点分别是x(1)、x’(2)，两个误差分布分别是∑(1)、∑’(2)，将其合成的代表点是x(2)、误差分布是∑(2)时，计算代表点x(2)和误差分布∑(2)的示意图如图14。

在图4中，在输出步骤S9，对输出装置36输出体素位置以及代表点及其误差分布。输出装置36是显示装置(例如CRT)的情况下，优选在三维图像上立体显示。此外，将这些数据转送到其他的装置(例如控制装置、计算机)也可，在打印机输出也可。

在输出步骤S9，与所述自身位置一起将基于所述自身位置的所述体素位置、代表点以及误差分布输出到输出装置中。此外，在输出步骤S9，将体素的代表点的位置作为三维形状的计测值对输出装置36输出，并且将表示该计测值的可靠性或精度的指标(例如，数值)，基于该体素内部的误差分布的大小，对输出装置36输出也可。进而，在输出步骤S9中，将体素的代表点的位置作为三维形状的计测值对输出装置36输出时，在该体素的内部的误差分布的大小(扩展)比规定的基准值大的情况下，该计测值的可靠性或精度定为比规定的基准低，该体素的所述计测值(即，该体素的代表点的位置)不对输出装置36输出也可。

如图4所示的处理的次序，在新的计测位置处，每当得到新的计测数据时，反复进行处理，在内部存储装置34以及外部存储装置33的至少任一个中存放结果。为了使处理高速化，优选在容量允许的限度内尽可能的将结果存放在内部存储装置34。

根据所述的本发明的方法和装置，由于将三维形状所存在的空间区域分割为多个体素6，并将各体素位置存储在外部存储装置33，所以即使在计测对象物大的情况下，也能将数据量抑制为与体素数成比例的小的数据尺寸。

此外，由于在对应于坐标值的体素6的内部设定并存储代表点7及其误差分布8，所以能够表现体素的分辨能力以上的信息。

此外，由于通过设定并保存表示在体素的内部物体的存在概率的概率值，从而即使在误差分布比代表点所属的体素扩展的情况下，也不用找到代表点所属的体素，从其误差分布再计算各体素中的物体的存在有无，而是仅以该体素的概率值就能容易判断，所以能够抑制检索时间。

此外，由于根据过去的计测位置的变化推定当前的计测位置，或通过能够取得当前的计测位置的传感器来取得，或不仅利用计测数据的距离值还利用反射强度值，限定核对的范围，所以能够抑制检索时间。

此外，由于在粗略对准步骤S6中，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，并以与具有代表点的体素间距离有关的评价值(例如，该距离的总和)成为最小的方式，进行位置对准，其中该代表点是与计测数据以及误差分布邻近的代表点，或者使针对新的计测位置的环境模型旋转及平移，并以与具有代表点的体素间距离有关的评价值(例如，该距离的总和)成为最小的方式，进行位置对准，或者，使新的计测数据以及误差分布旋转及平移，并以与具有代表点的体素的概率值有关的评价值(例如，该概率值的总和)成为最大的方式，进行位置对准，其中该代表点是与计测数据以及误差分布邻近的代表点，或者使针对新的计测位置的环境模型旋转及平移，并以与邻近的体素所具有的概率值的差有关的评价值(例如，该概率值的差的总和)成为最小的方式，进行位置对准，所以能够一边防止误差的累积，一边短时间地进行具有代表点的体素彼此的位置对准。

接着，由于在精密对准步骤S7中，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，或者使针对新的计测位置的环境模型旋转及平移，并以与邻近的误差分布间距离有关的评价值(例如，该距离的总和)成为最小的方式，进行位置对准，所以能够短时间地进行误差分布彼此的精密的位置对准。

因此，能够利用通过将逐次得到的计测数据综合到本发明的环境模型而得到的模型数据，能够求出计测数据最一致的位置姿势，而且能够防止误差的累积。

此外，由于当使用本发明的环境模型时，通过在体素内具有一个代表点从而点数减少，因此能够使计算量降低。此外，虽然点数减少，但本发明的环境模型通过进行代表点的误差分布和体素的分级化，能够保持精度。

进而，原来的ICP算法对于稀疏数据输出错误的结果，但本发明的环境模型由于在体素内具有代表点和误差分布，所以对应于稀疏数据的位置对准成为可能。

因此，根据本发明的方法和装置，在使用多个视点的计测数据的三维形状信息复原中，即使是稀疏数据也能防止误差的累积并同时取得高精度的形状。此外，由于成为重合时的比较对象的数据量减少，所以能够减少计算量。

此外，由于在模型构筑步骤S4中，在将最大的体素9设定为相当于必要最小限度的分辨能力的大小，而且单一的体素9内存在多个被计测点的情况下，以在单一的体素内仅存在单一的被计测点的方式，进一步将该体素八分割，分级地分割成多个体素，所以能够将数据量抑制为小的数据尺寸的同时，能够使用分割后的体素和代表点进一步提高分辨率。

特别是，将三维形状上的多个坐标值作为以多个计测位置为原点的距离数据而取得，通过将该距离数据的坐标值作为所述代表点的坐标值，将距离数据的坐标值的计测误差作为代表点的误差分布，从而能够使用正确的坐标值和误差分布在统计上综合多次的计测，能够愈发提高精度。

此外，作为在原点和被计测点之间不存在物体的，通过再设定或擦除位于其间的体素内的代表点和误差分布，能够去除错误计测数据的影响。

此外，探测对应于新输入的被计测点的坐标值的体素，在该体素内没有代表点的情况下，通过将所述坐标值和误差分布设定为代表点的坐标值和误差分布，能够容易地设定代表点的坐标值和误差分布。

进而，在所述体素内有已经设定了的代表点的情况下，比较新取得的误差分布和已经设定了的体素内的误差分布，在误差分布相互重叠的情况下，根据两误差分布、或根据两误差分布和体素内已经设定了的代表点和新输入的被计测点的坐标值，再设定新的误差分布和新的代表点，在误差分布不相互重叠的情况下，以在单一的体素内仅存在单一的代表点的方式，将该体素进一步分割并分级地分割为多个体素。由此，能够回避误差的累积并同时收敛到高精度的形状。

因此，根据本发明的方法和装置，具有将包含误差的距离数据校正为正确的信息的功能，并且通过反复该功能，从而对于长时间的计测会收敛到高精度的形状。而且，本发明的方法，由于是以新的计测点来更新对应于各体素6的代表点7及其误差分布8的处理，所以计算量小。此外，由于运算不对周围的体素施加影响而封闭在体素内，所以能够高速处理。此外，计测数据能够逐次综合为最大体素具有必要最小限度的分辨能力的体素结构，存储量不会大幅超过固定量。

对使用了卡尔曼滤波器的模型更新步骤进行详细说明。

在使用了卡尔曼滤波器的模型更新步骤的情况下，根据新输入的被计测点的坐标值及其误差分布、和已经设定了的体素内的代表点及其误差分布，通过卡尔曼滤波器取得新的代表点和误差分布并进行再设定。

以各模型点群的位置m(i)为状态量，基于距离传感器的计测点的位置，以下式[算式6]表现模型。再有，在本实施例，m(i)是体素内部的代表点(以下相同)。

[算式6]

L(j)＝h_m(R_r，t_r，m(i))+v_L(j)

＝R_s ^-1(R_r ^-1(m(i)-t_r)-t_s)+v_L(j)  (j＝1，...，N)

在[算式6]中，

L(j)是根据距离传感器的计测位置。例如L(j)是在距离传感器的传感器坐标系中三维LRF(激光测距仪)的计测点j(j＝1，...，N)的位置L(j)＝(x_L(j)，y_L(j)，z_L(j))^t。这里，t表示转置矩阵(以下相同)。

h_m(R_r，t_r，m(i))是对L(j)的观测系模型。

R_r是表示装载了距离传感器的移动体(例如移动机器人)的相对于世界坐标系的姿势的旋转矩阵R_r＝R(θx，θy，θz)。再有，θx、θy、θz分别表示绕x轴、y轴、z轴的旋转角(以下相同)。

t_r是表示装载了距离传感器的移动体(例如移动机器人)的相对于世界坐标系的位置的平移矢量t_r＝(x，y，z)。

v_L(i)是加入到距离传感器的计测值L(j)的观测噪声。

R_s是传感器坐标系的对移动体坐标系的旋转矩阵R_s＝R(θx，θy，θz)。

t_s是表示传感器坐标系的对移动体坐标系的位置的平移矢量t_s＝(x，y，z)。

测定对象物是静止的，将测定对象物的位置t_r、姿势R_r对环境模型固定。

使根据距离传感器的计测点群和环境模型点群上的点i(即代表点)相对应。对进行该对应的模型点群上的点i通过下式(4)进行更新。再有，可仅对与根据距离传感器的计测点群进行了对应的模型点群上的代表点m(i)通过如下的[算式7]进行更新。

[算式7]

K_mk(i)＝∑_mk，k-1(i)H_mk(j)^t(H_mk(j)∑_mk，k-1(i)H_mk(j)^t+∑_Lk(j))^-1

m’_k(i)＝m_k，k-1(i)+K_mk(i)(L_k(j)-h_mk(R_rk，t_rk，m_k，k-1(i)))

∑’_mk(i)＝∑_mk，k-1(i)-K_mk(i)H_mk(j)∑_mk，k-1(i)

在[算式7]中，

下标k表示是在离散时刻k的值。

关于m_k(i)，m’_k(i)表示m_k(i)的更新值(事后推定值)，m_k，k-1(i)表示基于m’_k-1(i)的m_k(i)的预测值(事前推定值)。再有因为环境(测定对象物)静止，所以m_k，k-1(i)＝m’_k-1(i)。

∑_mk(i)是体素内部的代表点m_k(i)的误差协方差矩阵(即所述误差分布)。此外，关于∑_mk(i)，∑’_mk(i)表示∑_mk(i)的更新值(事后推定值)，∑_{mk， k-1}(i)表示基于∑’_mk-1(i)的∑_mk(i)的预测值(事前推定值)。将在传感器坐标系中将三维LRF的计测点j(j＝1，...，N)的位置以L(j)表示，将其误差协方差矩阵以∑_L(j)表示。在这里N是在三维LRF得到的计测点的总数。作为三维LRF的误差模型假定与计测距离没有关系的一定的正态分布。将对传感器坐标系的x轴方向照射激光的情况下的误差协方差矩阵作为∑_S。对应激光的照射方向误差分布也改变姿势。∑_L(j)是将对基准方向的激光照射方向使用旋转矩阵R_L(j)表示为∑_L(j)＝R_L(j)∑_SR_L ^t(j)。计测点j的在世界坐标系的位置z(j)、及其误差协方差矩阵∑_z(j)能够分别以z(j)＝R_r(R_sL(j)+t_s)+t_r、∑_z(j)＝R_rR_s∑_L(j)R_s ^tR_r ^t来表示。

K_mk(i)是对m_k(i)的卡尔曼增益。

h_mk(R_rk，t_rk，m_k，k-1(i))是对L_k(j)、i＝p_k(j)的观测系模型。i＝p_k(j)是对应于计测点j的环境地图(即，环境模型)上的点。

H_mk是对L_k(j)、i＝p_k(j)的观测系模型的雅可比矩阵，以下面的[算式8]表示。

[算式8]

$H_{\mathrm{mk}}\left(j\right)=\frac{\∂h_{\mathrm{mk}}(R_k,t_k,m_k\left(i\right))}{\∂m_k\left(i\right)}{|}_{m_k\left(i\right)=m_{k,k-1}\left(i\right)}$

根据卡尔曼滤波器的更新过程，在得到了环境地图的模型点群的各点(体素的代表点)的位置和误差协方差矩阵的更新值m’_k(i)、∑’_mk(i)的阶段，以下面的次序进行环境模型的更新。

(1)将这些更新值m’_k(i)、∑’_mk(i)作为新的代表点、误差分布再设定。

(2)所述(1)的结果是，代表点的位置移动到别的体素内的情况下，当移动目的地的体素没有保持代表点时，使移动目的地的体素保持移动后的代表点及其误差协方差矩阵，从移动起始地的体素去除代表点等。当移动目的地的体素已经保持有代表点时，在两个代表点中，判断其两误差分布是否重叠(与所述ST4中的判断相同)。之后的处理与图11的ST4以后的处理相同即可。

(3)关于没有进行与模型点群上的代表点m(i)对应的根据距离传感器的计测点，在包含该计测点的体素不具有代表点的情况下，将计测点及其误差分布作为该体素的代表点和误差分布追加并保持。如果体素内代表点已经存在的情况下，以包含位于体素内的没有进行对应的其他的多个计测点，既存的代表点和各计测点被包含在完全不同体素的方式，分割体素并在此基础上使代表点等继承到分割后的体素。

通过反复所述的使用了卡尔曼滤波器的模型更新步骤，从而渐渐地体素内的误差协方差矩阵(即误差分布)的范围变小，并且体素变得容易分割。通过分割体素，还能够表现初期体素的大小以下的变化。

图15表示通过使用了卡尔曼滤波器的模型更新步骤得到的结果。图16是图15的局部放大图。在这些图中，以初期的体素的一个边的长度为100cm，允许6分割以下的再分割数。在对象存在的区域，反复体素的再分割，其结果是精度良好地表现计测对象。可知在对象不存在的区域不进行体素的再分割，能够以所需的充分的数据量来表现环境。此外，各体素内的代表点的误差分布也小，能够以高精度表现环境地图。这样，即使是包含误差的数据，也能够通过卡尔曼滤波器的效果，得到收敛到真值的结果。进而，在该方法通过使计测数据数增加，从而标准偏差变小，能够期待精度的进一步提高。

此外，由于测定对象物的位置/姿势固定，所以能够与测定对象物的位置/姿势独立来进行更新。再有，通过仅对与根据距离传感器的计测点群进行了对应的模型点群上的代表点m(i)，进行根据所述的卡尔曼滤波器的更新，从而能够大幅削减计算成本。

在所述精密对准步骤中，也可代替以所述邻近的误差分布间距离的总和成为最小的方式进行位置对准，而是以与一致度有关的评价值(例如，该一致度的总乘积)成为最大的方式，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，或者使针对新的计测位置的环境模型旋转及平移，从而进行位置对准，其中该一致度是通过基于所述邻近的误差分布间的极大似然推定值而确定的。

对该情况进行详细地说明。

由于对作为环境地图(环境模型)的模型点群、以及传感器的计测点群双方考虑了误差模型，所以对所述与一致度有关的评价值(例如该一致度的总乘积)的计算式(以下叫做评价函数)也能够引入双方的误差。在本实施例的情况下，不是单使最近的点对应，而是通过引入似然的概念，从而对评价函数附加环境地图上的误差分布、以及计测点的误差分布，在当前时刻的环境地图中，确定在各计测点的出现概率成为最大的情况下评价函数也成为最大。

具体地，假定为与计测点j对应的环境地图上的点i＝p(j)的位置，符合平均值(代表点)m(i)、误差协方差矩阵∑_m(i)的正态分布，在此基础上在三维LRF计测的结果是，将得到L(j)的计测数据的概率值Pr(L(j)|m(i)，∑_m(i))作为点i和点j的评价函数EM(i，j)，以其总乘积为最大的方式以下面的[算式9]来确定评价函数。

[算式9]

$\mathrm{EM}=\underset{j=1}{\overset{N}{\mathrm{\Π}}}\left\{\mathrm{\ω}\left(j\right)\mathrm{EM}(i,j)\right\}$

$=\underset{j=1}{\overset{N}{\mathrm{\Π}}}\left\{\mathrm{\ω}\left(j\right)\mathrm{Pr}\left(L\left(j\right)\right|m\left(P\left(j\right)\right),{\mathrm{\Σ}}_m\left(P\left(j\right)\right))\right\}$

其中，ω(j)在模型点群中有与计测点j对应的点存在的情况下为1，在除此之外的情况下为0。

这里，将Pr(L(j)|q)作为表示在环境地图的点在q的位置的情况下能够得到L(j)的计测数据的概率值，将Pr(q|m(i)，∑_m(i))作为在假定符合平均值(代表点)m(i)、误差协方差矩阵∑_m(i)的正态分布之上表示环境地图的点在q的位置的概率值时，[算式10]成立。

[算式10]

Pr(L(j)|m(i)，∑_m(i))＝

∫{Pr(L(j)|q)·Pr(q|m(i)，∑_m(i))}dq

当假定Pr(q|m(i)，∑_m(i))为正态分布时，成为下面的[算式11]

[算式11]

$\mathrm{Pr}\left(q\right|m\left(i\right),{\mathrm{\Σ}}_m\left(i\right))=$

$\frac{1}{{\sqrt{2\mathrm{\π}}}^3\sqrt{\left|\mathrm{\Σm}\left(i\right)\right|}}\exp (-\frac{1}{2}(q-m{\left(i\right)}^t)\mathrm{\Σm}{\left(i\right)}^{-1}(q-m\left(i\right)))$

另一方面，以z(j)置换L(j)，Pr(L(j)|q)能够以下面的[算式12]近似。

[算式12]

$\mathrm{Pr}\left(L\left(j\right)\right|q)\≅$

$\frac{1}{{\sqrt{2\mathrm{\π}}}^3\sqrt{\left|\mathrm{\Σz}\left(i\right)\right|}}\exp (-\frac{1}{2}{(z\left(j\right)-q)}^t\mathrm{\Σz}{\left(j\right)}^{-1}(z\left(j\right)-q))$

这里，z_k(j)依赖于装载了距离传感器的移动体的位置t_r、姿势R_r。实际上，由于从三维LRF的传感器坐标系中心看到的q的朝向和计测点L(j)的朝向与图17所示的不同，所以误差协方差矩阵∑_z(j)也需要配合q的朝向旋转转换，但由于从对应的环境地图上的点i远离的q的存在概率低，所以能够以充分的精度近似。由此，Pr(L(j)|m(i)，∑_m(i))能够以下面的[算式13]表示。

[算式13]

$\mathrm{Pr}\left(L\left(j\right)\right|m\left(i\right),{\mathrm{\Σ}}_m\left(i\right))=$

$\frac{1}{{\sqrt{2\mathrm{\π}}}^6\sqrt{\left|{\mathrm{\Σ}}_z\left(j\right){\mathrm{\Σ}}_m\left(i\right)\right|}}\×$

$\∫\{\exp (-\frac{1}{2}{(z\left(j\right)-q)}^t\mathrm{\Σz}{\left(j\right)}^{-1}(z\left(j\right)-q))$

$\×\exp (-\frac{1}{2}{(m\left(i\right)-q)}^t\mathrm{\Σm}{\left(i\right)}^{-1}(m\left(i\right)-q))\}\mathrm{dq}$

通过简单的计算，得到下面的[算式14]

[算式14]

$\mathrm{Pr}\left(L\left(j\right)\right|m\left(i\right),{\mathrm{\Σ}}_m\left(i\right))=$

$=\frac{1}{{\sqrt{2\mathrm{\π}}}^6\sqrt{\left|{\mathrm{\Σ}}_z\left(j\right){\mathrm{\Σ}}_m\left(i\right)\right|}}\×\exp (-\frac{1}{2}\mathrm{\β}\left(j\right))\×$

$\∫\exp (-\frac{1}{2}{(q-\mathrm{\α}\left(j\right))}^t(\mathrm{\Σz}{\left(j\right)}^{-1}+\mathrm{\Σm}{\left(i\right)}^{-1})(q-\mathrm{\α}\left(j\right)))\mathrm{dq}$

$=\frac{{\sqrt{2\mathrm{\π}}}^3\sqrt{\left|{({\mathrm{\Σ}}_z{\left(i\right)}^{-1}+\mathrm{\Σm}{\left(i\right)}^{-1})}^{-1}\right|}}{{\sqrt{2\mathrm{\π}}}^6\sqrt{\left|\mathrm{\Σz}\left(j\right){\mathrm{\Σ}}_m\left(i\right)\right|}}\exp (-\frac{1}{2}\mathrm{\β}\left(j\right))$

$=\frac{1}{{\sqrt{2\mathrm{\π}}}^3\sqrt{|{\mathrm{\Σ}}_z\left(j\right)+{\mathrm{\Σ}}_m\left(i\right)|}}\exp (-\frac{1}{2}\mathrm{\β}\left(j\right))$

其中，α(j)、β(j)能够以下面的[算式15]来表示。

[算式15]

α(j)＝(∑_z(j)^-1+∑_m(i)^-1)^-1×

(∑_m(i)^-1m(i)+∑_z(j)^-1z(j))

β(j)＝(m(i)-z(j))^t(∑_z(j)+∑_m(i))^-1(m(i)-z(j))

因此，表示模型点群的点i和计测点群的点j的对应的一致度的评价函数EM(i，j)，在平均值m(i)、误差协方差矩阵∑_m(i)+∑_z(j)的正态分布中，能够近似为得到z(j)的概率值。通过使用该评价函数，能够进行考虑了环境地图、计测数据双方的对应。

关于计测点和环境地图(即，环境模型)的对应进行补充说明。在上述实施例，由于使用考虑了误差分布的统计上的评价函数，所以不求出评价函数的值就不能够确定对应点。因此，在环境地图上的模型点群中预先缩小对应的候补，从该候补中基于评价函数的值求出对应点。具体地，能够如以下方式确定。

(1)求出与作为对象的计测点j的误差协方差矩阵∑_L(j)的范围(例如标准偏差的三倍的范围)相交的最上位的体素、和邻接于该体素的最上位的体素，将还包含下层的体素的这些体素内存在的代表点作为对应点的候补。由于体素成为分级结构，所以该候补点的探测几乎不花费计算成本。这时，在没有成为候补的代表点的情况下，视为没有对应点。将邻接的体素也加入候补的理由是因为根据体素内的代表点的位置，误差协方差矩阵的范围有时会溢出至邻接的体素。

(2)使用成为候补的体素的代表点i和误差协方差矩阵，求出评价函数EM(i，j)的值。

(3)将评价函数EM(i，j)的值是最大的代表点i作为对应点。其中，在评价函数的值不足某个阈值的情况下，视为没有对应点。

在本实施例，作为对应的评价函数EM(i，j)，采用基于似然的算式，由于关于对应点的有无，有统计上的明确的判断尺度，所以不会有即使在认为对应点不存在的情况下还勉强地进行对应的情况。再有，在没有对应点的情况下，解释为成为对象的计测点是相当于至此未计测的部分的点，追加到环境地图。

作为实施方式，对三维形状数据的位置对准方法和装置进行了说明，但通过将二维形状作为三维形状的特殊情况来看，也能够作为二维形状数据的位置对准方法和装置的方式来实施。

此外，在所述输出步骤中，不全部输出所述体素位置、代表点以及误差分布也可，例如在全部没有它们也能把握三维形状的情况、或需要它们中的一个或两个的情况等情况下，可将所述体素位置、代表点以及误差分布的至少任一个输出到输出装置中。

再有，本发明并不局限于上述实施方式，当然能够在不脱离本发明的主旨的范围内进行各种变更。

Claims (19)

1.一种三维形状数据的位置对准方法，用于从多个计测位置来计测静止的三维形状，综合其距离数据以进行位置对准，其特征在于，实施下述步骤：

数据输入步骤，在新的计测位置处将三维形状上的坐标值输入到计算机中；

模型构筑步骤，构筑环境模型，该环境模型将所述三维形状所存在的空间区域分割为由边界表面相互正交的长方体构成的多个体素，并存储各体素位置；以及

匹配步骤，在对应于所述坐标值的体素的内部设定代表点及其误差分布并存储起来，

所述方法还具有下述步骤：

精密对准步骤，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，或者使针对新的计测位置的环境模型旋转及平移，并以邻近的误差分布间距离的总和成为最小的方式，进行位置对准；以及

输出步骤，将所述体素位置、代表点以及误差分布输出到输出装置中。

2.根据权利要求1所述的三维形状数据的位置对准方法，其特征在于，在所述匹配步骤中，在体素内部除了代表点及其误差分布之外，还设定并存储表示在体素内物体存在概率的概率值。

3.根据权利要求1所述的三维形状数据的位置对准方法，其特征在于，在所述精密对准步骤之前具有：粗略对准步骤，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，并以具有代表点的体素间距离的总和成为最小的方式，进行位置对准，其中该代表点是与计测数据以及误差分布邻近的代表点，或者使针对新的计测位置的环境模型旋转及平移，并以具有代表点的体素间距离的总和成为最小的方式，进行位置对准。

4.根据权利要求1所述的三维形状数据的位置对准方法，其特征在于，在所述精密对准步骤之前具有：粗略对准步骤，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，并以具有代表点的体素的概率值的总和成为最大的方式，进行位置对准，其中该代表点是与计测数据以及误差分布邻近的代表点，或者使针对新的计测位置的环境模型旋转及平移，并以邻近的体素所具有的概率值的差的总和成为最小的方式，进行位置对准。

5.根据权利要求1所述的三维形状数据的位置对准方法，其特征在于，在所述数据输入步骤之后具有：探测范围限定步骤，根据过去的计测位置的变化推定当前的计测位置，或通过能够取得当前的计测位置的传感器来取得，或不仅利用计测数据的距离值还利用反射强度值来限定核对的范围。

6.根据权利要求1所述的三维形状数据的位置对准方法，其特征在于，在所述精密对准步骤中，将误差分布交叉的情况作为同一计测点，将根据分布的一致度求得的加权乘以该情况的距离值，计算误差分布间的距离。

7.根据权利要求1所述的三维形状数据的位置对准方法，其特征在于，在所述模型构筑步骤中，在将最大的体素设定为相当于必要最小限度的分辨能力的大小，而且单一的体素内存在多个被计测点的情况下，以在单一的体素内仅存在单一的被计测点的方式，将该体素进一步分割并分级地分割成多个体素。

8.根据权利要求1所述的三维形状数据的位置对准方法，其特征在于，在所述精密对准步骤之后具有更新所述环境模型的模型更新步骤，在该模型更新步骤中，探测对应于新输入的被计测点的坐标值的体素，

作为在原点和被计测点之间不存在物体的，再设定或擦除位于其间的体素内的代表点和误差分布。

9.根据权利要求1所述的三维形状数据的位置对准方法，其特征在于，在所述精密对准步骤之后具有更新所述环境模型的模型更新步骤，在该模型更新步骤中，探测对应于新输入的被计测点的坐标值的体素，

在该体素内没有代表点的情况下，将所述坐标值和误差分布设定为代表点的坐标值和误差分布。

10.根据权利要求1所述的三维形状数据的位置对准方法，其特征在于，在所述精密对准步骤之后具有更新所述环境模型的模型更新步骤，在该模型更新步骤中，探测对应于新输入的被计测点的坐标值的体素，

在所述体素内有已经设定了的代表点的情况下，比较新取得的误差分布和已经设定了的体素内的误差分布，

在误差分布相互重叠的情况下，根据两误差分布再设定新的误差分布和新的代表点，

在误差分布不相互重叠的情况下，以在单一的体素内仅存在单一的代表点的方式，将该体素进一步分割并分级地分割为多个体素。

11.根据权利要求10所述的三维形状数据的位置对准方法，其特征在于，在所述模型更新步骤中，比较所述新取得的误差分布和所述已经设定了的体素内的误差分布，在误差分布相互重叠的情况下，根据两误差分布、或根据两误差分布和体素内已经设定了的代表点和新输入的被计测点的坐标值，再设定新的误差分布和新的代表点，其结果是新的代表点移动到其他体素内，此时，

在该其他体素内没有代表点的情况下，在该其他体素的内部设定该新的误差分布和该新的代表点，

在该其他体素内有已经设定了的代表点的情况下，比较该新的误差分布和已经设定了的该其他体素内的误差分布，(A)在误差分布相互重叠的情况下，根据两误差分布、或根据两误差分布和体素内已经设定了的代表点和新输入的被计测点的坐标值，再设定新的误差分布和新的代表点，(B)在误差分布不相互重叠的情况下，以在单一的体素内仅存在单一的代表点的方式，将该体素进一步分割并分级地分割为多个体素。

12.根据权利要求1所述的三维形状数据的位置对准方法，其特征在于，在所述精密对准步骤之后具有更新所述环境模型的模型更新步骤，

在该模型更新步骤中，根据新输入的被计测点的坐标值及误差分布、和已经设定了的体素内的代表点及其误差分布，通过卡尔曼滤波器取得新的代表点和误差分布并进行再设定。

13.根据权利要求1所述的三维形状数据的位置对准方法，其特征在于，在所述精密对准步骤中，代替以所述邻近的误差分布间距离的总和成为最小的方式进行位置对准，而是以与一致度有关的评价值成为最大的方式，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，或者使针对新的计测位置的环境模型旋转及平移，从而进行位置对准，其中该一致度是通过基于所述邻近的误差分布间的极大似然推定值而确定的。

14.根据权利要求13所述的三维形状数据的位置对准方法，其特征在于，所述与一致度有关的评价值以下述计算式表示，

$\mathrm{EM}=\underset{j=1}{\overset{N}{\mathrm{\Π}}}\left\{\mathrm{\ω}\left(j\right)\mathrm{EM}(i,j)\right\}$

在该式中，计测点j和环境模型上的代表点i对应起来，能得到该计测点j的计测数据的概率为EM(i，j)，ω(j)在环境模型中存在与计测点j对应起来的代表点的情况下是1，在除此之外的情况下为0。

15.一种三维形状数据的位置对准装置，用于从多个计测位置来计测静止的三维形状，综合其距离数据以进行位置对准，其特征在于，具备：

数据输入装置，将三维形状上的坐标值输入到计算机中；

模型构筑装置，构筑环境模型，该环境模型将所述三维形状所存在的空间区域分割为由边界表面相互正交的长方体构成的多个体素，并存储各体素位置；

匹配装置，在对应于所述坐标值的体素的内部设定代表点及其误差分布并存储起来；以及

数据传输装置，将所述体素位置、代表点以及误差分布输出到输出装置中，

对于针对先前的计测位置的环境模型，使针对新的计测位置的环境模型旋转及平移，并以邻近的误差分布间距离的总和成为最小的方式，进行位置对准。

16.一种三维形状数据的位置对准方法，用于从多个计测位置来计测静止的三维形状，综合其距离数据以进行位置对准，其特征在于，实施下述步骤：

数据输入步骤，在新的计测位置处将三维形状上的坐标值输入到计算机中；

模型构筑步骤，构筑环境模型，该环境模型将所述三维形状所存在的空间区域分割为由边界表面相互正交的长方体构成的多个体素，并存储各体素位置；以及

匹配步骤，在对应于所述坐标值的体素的内部设定代表点及其误差分布并存储起来，

所述方法还具有下述步骤：

精密对准步骤，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，或者使针对新的计测位置的环境模型旋转及平移，并以与邻近的误差分布间距离有关的评价值成为最小的方式，进行位置对准；以及

输出步骤，将所述体素位置、代表点以及误差分布的至少任一个输出到输出装置中。

17.根据权利要求16所述的三维形状数据的位置对准方法，其特征在于，在所述精密对准步骤之前具有：粗略对准步骤，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，并以与具有代表点的体素间距离有关的评价值成为最小的方式，进行位置对准，其中该代表点是与计测数据以及误差分布邻近的代表点，或者使针对新的计测位置的环境模型旋转及平移，并以与具有代表点的体素间距离有关的评价值成为最小的方式，进行位置对准。

18.根据权利要求16所述的三维形状数据的位置对准方法，其特征在于，在所述精密对准步骤之前具有：粗略对准步骤，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，并以与具有代表点的体素的概率值有关的评价值成为最大的方式，进行位置对准，其中该代表点是与计测数据以及误差分布邻近的代表点，或者使针对新的计测位置的环境模型旋转及平移，并以与邻近的体素所具有的概率值的差有关的评价值成为最小的方式，进行位置对准。

19.一种三维形状数据的位置对准装置，用于从多个计测位置来计测静止的三维形状，综合其距离数据以进行位置对准，其特征在于，具备：

数据输入装置，将三维形状上的坐标值输入到计算机中；

模型构筑装置，构筑环境模型，该环境模型将所述三维形状所存在的空间区域分割为由边界表面相互正交的长方体构成的多个体素，并存储各体素位置；

匹配装置，在对应于所述坐标值的体素的内部设定代表点及其误差分布并存储起来；以及

数据传输装置，将所述体素位置、代表点以及误差分布的至少任一个输出到输出装置中，

对于针对先前的计测位置的环境模型，使针对新的计测位置的环境模型旋转及平移，并以与邻近的误差分布间距离有关的评价值成为最小的方式，进行位置对准。

Images