CN101331379B - 自身位置辨认方法和装置以及三维形状的计测方法和装置 - Google Patents

Abstract

本发明实施下述步骤：步骤(S1)，在新的计测位置处将三维形状上的坐标值输入到计算机中；步骤(S4)，构筑环境模型，该环境模型将三维形状所存在的空间区域分割为由立方体构成的多个体素，并存储各位置；以及步骤(S5)，在对应于坐标值的体素的内部设定代表点及其误差分布并存储起来。接着，在先前的计测位置的数据存在的情况下，实施：精密对准步骤(S7)，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，或者使针对新的计测位置的环境模型旋转及平移，并以与邻近的误差分布间距离有关的评价值成为最小的方式，进行位置对准。根据精密对准步骤中的旋转量以及平移量来辨认自身位置。

Description

自身位置辨认方法和装置以及三维形状的计测方法和装置

技术领域

本发明涉及一种引入外界的三维信息来辨认自身位置的自身位置辨认方法和装置以及三维形状的计测方法和装置。

背景技术

作为直接辨认自身位置的办法，已知有GPS、DGPS、磁道钉(magnetic marker)、磁方式、无线方式、图像标记(image marker)方式、光学式/电磁式标记方式等。但是这些办法虽然有能进行稳定的位置辨认(identify)的优点，但存在需要基础设施、能进行位置检测的范围有限、需要用于设置的工时等问题。

对此，将不使用基础设施而通过传感器来计测周围环境获得的环境信息、和新获得的计测数据一边比较一边计算自身位置的自身位置辨认办法被各种各样地提出。

作为适合该目的的三维传感器，例如可以举出超声波传感器、图像传感器、距离传感器。但是，超声波传感器有计测范围狭窄精度低的问题，图像传感器有容易受到照明条件的影响的问题。

与此相比，由于距离传感器比超声波传感器能够精度良好地计测宽范围的距离数据，而且，不像图像处理那样会受照明的变化的影响，所以，有能够不受外扰光影响地进行稳定的计测的优点。

在非专利文献1、专利文献1等中提出了使用距离传感器的位置辨认的办法。

非专利文献1的办法是利用ICP算法一边将使用三维扫描式激光传感器通过周围环境计测而获得的环境模型、和逐次得到的计测数据进行比较一边进行位置对准(position matching；aligning)。

ICP算法是对应点非已知情况的位置对准办法之一，求出相对于先前的计测数据的其后的计测数据的最邻近的点，一边进行旋转/平移(translate)一边以其距离的总和为最小的状态变成一致状态的方式来求解(非专利文献2)。

专利文献1的“自身位置辨认装置”是专门无轨道且在外界没有设置地面标记等设备的装置，是通过预先以手动使移动体沿作为目的的轨道动作(教导)，使外部环境的地图自动记忆的装置。

在该专利文献1中，如图1所示那样，使用外部环境地图，该外部环境地图是将在平面内移动的移动体101的、与该平面内的周围外部环境物体102的位置关系作为离散化的数据保持在存储器上。所述数据是以所述移动体上固定的点为原点103，包括：离散化为规定间隔的来自所述移动体的方向数据104、和在所述方向上的离散化为规定间隔的到所述周围外部环境物体的为止的距离数据105的组。

此外，作为与本发明有关的技术，公开有非专利文献3。非专利文献1：Andreas Nuchter，et al.，“6D SLAM with an Applicationin Autonomous Mine Mapping”，IEEE 2004 International ConferenceRobotics and Automation(ICRA’04)，April 2004

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

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

专利文献1：日本专利申请公开2003-15739号公报，“外部环境地图以及自身位置辨认装置以及诱导控制装置”

发明内容

发明要解决的问题

在使用三维激光雷达那样的距离传感器的情况下，被计测的三维形状上的被计测点成为离散到横向以及垂直方向上的点群。该点群的间隔，在距计测点的距离例如是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算法以密集的距离数据为对象，所以在是离散的点群，在空间上稀疏的情况下，会收敛到错误的结果。

因此，使用了这样的距离传感器的位置辨认，需要满足以下的要求。

(1)存储器效率良好的用于周围环境的数据结构

将逐次得到的计测数据全部保存的方法中需要无穷尽的存储器。因此，需要将计测的周围环境结果高效存放的数据结构。

(2)对计测数据的点数不足以及误差的稳定性

即使在计测数据的点数少的情况下以及包含误差的情况下，也需要尽量不使位置辨认精度下降。

(3)用于位置辨认的计算的高效化

通过将计测周围环境而获得的环境信息和计测数据比较，从而进行自身位置辨认，但该比较处理需要很多的计算。

发明的概要

本发明是为了解决上述问题而做出的。即，本发明的目的是提供一种自身位置辨认方法和装置以及三维形状的计测方法和装置，其中具有存储器效率良好的用于周围环境的数据结构，对于计测数据的点数不足以及误差有稳定性，用于位置辨认的计算能够高效进行，由此，即使在外界的三维信息是空间离散的点群，每个计测的位置不同，没有对应点，包含比较大的误差的情况下，也能够没有误差的累积，并能以少的计算量进行正确的位置对准。

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

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

再有，在所述自身位置辨认方法或三维形状计测方法中，关于上述距离的评价值，替代该距离的总和，是该距离的平均值、该距离的平方的总和、或该距离的最大值也可，是其他的适合的评价值也可。

所述自身位置辨认方法或三维形状的计测方法，包含以下的内容也可。

根据本发明的优选实施方式，在所述自身位置辨认步骤中，根据先前的计测位置的位置以及姿势来辨认新的计测位置的六自由度位置。

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

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

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

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

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

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

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

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

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

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

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

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

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

在所述输出步骤中，将所述体素位置、代表点位置以及误差分布位置的至少任一个作为三维形状的计测值输出到输出装置中，并且将表示该计测值的可靠性或精度的指标，基于该体素内部的误差分布的大小，输出到输出装置中。

在所述输出步骤中，将所述体素位置、代表点位置以及误差分布位置的至少任一个作为三维形状的计测值输出到输出装置中时，在该体素内部的误差分布的大小比规定的基准值大的情况下，该计测值的可靠性或精度定为比规定的基准低，不将该体素的所述计测值输出到输出装置中。

在所述匹配步骤之后具有更新所述环境模型的模型更新步骤，在该模型更新步骤中，探测对应于新输入的被计测点的坐标值的体素，在所述体素内的代表点以及误差分布的至少任一个被新设定、或被再设定、或将该体素进一步分割并分级地分割为多个体素的情况下，在所述输出步骤中，将该体素的代表点位置作为三维形状的计测值输出到输出装置中。

在所述输出步骤中，将从距离传感器的位置出发距离传感器能进行位置计测的范围的环境模型内的体素的代表点位置，作为三维形状的计测值输出到输出装置中。

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

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

所述自身位置辨认装置或三维形状的计测装置，包含以下的内容也可。

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

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

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

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

所述自身位置辨认装置或三维形状的计测装置，据先前的计测位置的位置以及姿势来辨认新的计测位置的六自由度位置。

所述自身位置辨认装置或三维形状的计测装置，将误差分布交叉的情况作为同一计测点，将根据分布的一致度求得的加权乘以该情况的距离值，计算误差分布间的距离。

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

所述自身位置辨认装置或三维形状的计测装置，在所述自身位置的辨认(自身位置辨认步骤)之后具有更新所述环境模型的模型更新装置，该模型更新装置探测对应于新输入的被计测点的坐标值的体素，作为在原点和被计测点之间不存在物体的，再设定或擦除位于其间的体素内的代表点和误差分布。

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

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

所述自身位置辨认装置或三维形状的计测装置，在所述自身位置辨认步骤中，在自身位置辨认的同时，辨认自身位置的误差分布，在所述输出步骤之前，根据当前的自身位置和误差分布以及辨认了的自身位置和误差分布，利用卡尔曼滤波器，校正自身位置和误差分布，所述数据传输装置将自身位置和误差分布输出到输出装置中。

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

所述自身位置辨认装置或三维形状的计测装置，在所述自身位置辨认步骤之后具有更新所述环境模型的模型更新装置，该模型更新装置根据新输入的被计测点的坐标值、和已经设定了的体素内的代表点及其误差分布，通过卡尔曼滤波器取得新的代表点和误差分布并进行再设定。

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

所述数据传输装置将所述体素位置、代表点位置以及误差分布位置的至少任一个作为三维形状的计测值输出到输出装置中，并且将表示该计测值的可靠性或精度的指标，基于该体素内部的误差分布的大小，输出到输出装置中。

所述数据传输装置将所述体素位置、代表点位置以及误差分布位置的至少任一个作为三维形状的计测值输出到输出装置中时，在该体素内部的误差分布的大小比规定的基准值大的情况下，该计测值的可靠性或精度定为比规定的基准低，不将该体素的所述计测值输出到输出装置中。

所述数据输入装置使用距离传感器将三维形状上的坐标值作为以任意计测位置为原点的距离数据，一边移动原点一边依次取得。

在所述匹配装置之后具有更新所述环境模型的模型更新装置，在该模型更新装置中，探测对应于新输入的被计测点的坐标值的体素，在所述体素内的代表点以及误差分布的至少任一个被新设定、或被再设定、或将该体素进一步分割并分级地分割为多个体素的情况下，所述输出装置，将该体素的代表点位置作为三维形状的计测值输出到输出装置中。

所述数据传输装置将从距离传感器的位置出发距离传感器能进行位置计测的范围的环境模型内的体素的代表点位置，作为三维形状的计测值输出到输出装置中。

发明的效果

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

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

因此，通过本发明的数据结构，能够将多个视点的计测数据综合为固定大小。

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

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

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

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

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

因此，通过根据本发明的来自多个视点的数据的综合处理，能够在防止误差的累积的同时，取得高精度的形状。

此外，本发明的数据结构，由于是将体素结构扩展了的结构，所以数据尺寸能够比点群小而完成。即，由于在本发明提出的数据结构是在体素内保存一个代表点，由于能够使探测对应于计测点的模型点的计算量级(order)为1，所以能够将整体的计算量级减少为O(N)。

因此，在进行周围环境和计测数据的位置对准(ICP算法)时，由于成为探测对象的数据量减少，所以能够计算高效化。

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

此外，通过根据当前的自身位置和误差分布以及辨认了的自身位置和误差分布，并利用卡尔曼滤波器，校正自身位置和误差分布，从而能进一步提高自身位置的精度。

因此，根据本发明的方法和装置，具有将包含误差的数据校正为正确的信息的功能，并且对长时间的计测收敛到高精度。而且，位置辨认处理由于是将体素的对应于各体素的代表点及其误差分布以计测点来更新的处理，所以计算量小，运算不对周围体素施加影响而封闭在体素内。因此，所以能够高速处理。此外，计测数据能够逐次综合到体素，其结果是得到的周围环境信息的存储器大小不会超过固定大小。

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

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

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

此外，由于在所述输出步骤中，在将所述体素的代表点的位置作为三维形状的计测值输出到输出装置中时，由于将表示该计测值的可靠性或精度的指标，对该体素内部的误差分布的大小输出装置输出，所以在使用计测装置时，对应于应用的内容使用者能够取舍选择可靠性低的计测值。

此外，在所述输出步骤中，在将所述体素的代表点的位置作为三维形状的计测值输出到输出装置中时，在该体素内部的误差分布大小比规定的基准值大的情况下，该计测值的可靠性或精度定为比规定的基准低，不输出到输出装置中该体素的所述计测值，由此，在使用计测装置时，由于能够最初就仅使用可靠性高的计测值，所以能够削减处理的数据量，带来可靠性的提高。

此外，在所述数据输入步骤中，使用距离传感器，将三维形状上的坐标值作为以任意的计测位置为原点的距离数据，一边移动原点一边依次取得，特别是通过从不同方向得到距离数据，能够综合分布形状不同的误差分布的距离数据，能够提高精度。

此外，在所述匹配步骤之后具有更新所述环境模型的模型更新步骤，在该模型更新步骤中，探测对应于新输入的被计测点的坐标值的体素，仅在所述体素内的代表点以及误差分布的至少任一个被新设定、或被再设定、或将该体素进一步分割并分级地分割为多个体素的情况下，在所述输出步骤中，通过使该体素的代表点的位置、误差分布以及体素的位置的至少任一个作为三维形状的计测值输出到输出装置中，从而输出通过从距离传感器新得到的被计测点受到影响的体素内的代表点等的值。因此，设想使用者与以往同样的运用，以距离传感器的得到的原本的计测值就像置换为更高精度的计测值那样，变得能够利用。这样，能够进行精度更高的三维形状计测。

此外，在所述输出步骤中，将从距离传感器的位置出发距离传感器可以计测的位置范围的环境模型内的体素代表点的位置，作为三维形状的计测值输出到输出装置中，由此，即使在现有的距离传感器的计测值的分辨能力粗糙的情况下，也能像精度优良且分辨能力高的距离传感器那样使用，这样，精度更高的三维形状计测成为可能。

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

附图说明

图1是专利文献1的“自身位置辨认装置”的示意图。

图2是本发明的自身位置辨认装置的整体结构图。

图3是非专利文献2公开的三维激光雷达的结构图

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

图17是表示通过综合以多个计测位置为原点的距离数据从而代表点的误差分布缩小、代表点的精度提高的样子的示意图

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

图19是图18的局部放大图。

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

具体实施方式

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

图2是本发明的自身位置辨认装置的整体结构图。在该图中，本发明的自身位置辨认装置是六自由度的辨认单元，是从距离传感器、测距器、照相机、GPS、姿势传感器引入外界的三维信息来辨认自身位置的装置。再有，在本发明中，自身位置意味着自身位置辨认装置本身在外界的六自由度的位置和姿势。

再有，在本发明中距离传感器以外的测距器、照相机、GPS、姿势传感器不是不可缺的，而是对应于需要作为选项而使用。以下，说明使用距离传感器的例子。

图3是作为距离传感器的一个例子的三维激光雷达的结构图。例如非专利文献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。但是，距离传感器并不限定于此，也可以使用利用视差的距离传感器等其他公知的距离传感器。

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

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

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

该误差分布在误差分布的在r_s、θ_s、

的存在概率为P(r_s，θ_s，)的情况下，误差分布是在计测的轴r、θ、

方向上正态分布，例如能够以式(1)表示。这里，r、θ、是来自传感器的计测值，σ_r、σ_θ、

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

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

[算式2]

图5是用于执行本发明的方法的装置结构图。如该图所示，该装置具备：数据输入装置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(计算机)组合在一起的也可，或是将整体一体化的装置也可。此外，一体地装入能够自动推进的装置内也可

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

本发明的方法是引入外界的三维形状来辨认自身位置的自身位置辨认方法以及三维形状的计测方法，具有：数据输入步骤S1、数据校正步骤S2、探测范围限定步骤S3、模型构筑步骤S4、匹配步骤S5、自身位置辨认步骤S7、S10、粗略对准步骤S8、精密对准步骤S9、模型更新步骤S11以及输出步骤S12。

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

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

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

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

)表示。此外，各坐标值的误差分布根据极坐标值(r，θ，)以运算求得，或预先以别的输入单元(例如键盘)输入。

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

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

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

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

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

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

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

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

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

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

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

在匹配步骤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)$

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

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

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

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

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

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

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

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

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

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

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

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

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

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

在图6中，在匹配步骤S5之后，在步骤S6检查在先前的计测位置的同一三维形状的数据的有无。该检测在先前的计测的同一三维形状数据不存在的情况下，将新的计测位置辨认为自身位置。

该辨认在逐次移动的移动体的初始位置中，优选以已知的六自由度位置(例如全局坐标系(global coordinate)的原点)实施。此外，在该辨认中，辨认包含新的计测位置的位置(三自由度)和姿势(三自由度)的六自由度位置较好。

此外，在步骤S6的检查，在先前的计测位置的同一三维形状的数据存在的情况下，在图6中，在匹配步骤S5之后实施粗略对准步骤S8和精密对准步骤S9。

图10是粗略对准步骤S8和精密对准步骤S9的数据处理流程图，图11是粗略对准步骤S8的示意图，图12是精密对准步骤S9的示意图。

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

粗略对准步骤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|={\{\underset{i}{\overset{I}{\mathrm{\Σ}}}{\left({x_i}^{\left(1\right)}{-x_i}^{\left(2\right)}\right)}^2\}}^{\frac{1}{2}}...\left(3\right)$

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

在精密对准步骤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)$

进而在图6中，模型更新步骤S11在自身位置辨认步骤S10之后实施，更新在模型构筑步骤S4构筑的环境模型。

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

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

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

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

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

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

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

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

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

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

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

在图6中，在输出步骤S12，将所述自身位置、基于所述自身位置的所述体素位置、代表点以及误差分布的至少任一个对输出装置36输出。输出装置36是显示装置(例如CRT)的情况下，优选在三维图像上立体显示自身位置。此外，将自身位置的数据转送到其他的装置(例如控制装置、计算机)也可，在打印机输出也可。

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

此外，在所述匹配步骤之后具有更新所述环境模型的模型更新步骤，在该模型更新步骤中，探测对应于新输入的被计测点的坐标值的体素，在所述体素内的代表点以及误差分布的至少任一个被新设定、或被再设定、或将该体素进一步分割并分级地分割为多个体素的情况下，在所述输出步骤中，将该体素的代表点的位置作为三维形状的计测值输出到输出装置中。

此外，在输出步骤S6中，能够得到距离传感器的位置/姿势的情况下，将从该位置能看到的范围的环境模型内的体素的代表点位置作为三维形状的计测值输出到输出装置中也可。从距离传感器的位置能看到的范围，指的是从距离传感器的位置出发距离传感器能进行位置计测的范围，可包含从距离传感器的位置出发距离传感器能进行位置计测的角度范围(视野)、和从距离传感器的位置出发距离传感器能进行位置计测的距离范围。

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

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

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

因此，根据本发明的数据结构，能够将多个视点的计测数据综合为固定大小。

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

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

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

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

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

因此，通过根据本发明的从多个视点的数据的综合处理，能够防止误差的累积的同时，取得高精度的形状。

此外，由于本发明的数据结构是将体素结构扩展了的结构，所以数据尺寸能够比点群小而完成。因此，在进行周围环境和计测数据的位置对准(ICP算法)时成为探测对象的数据量减少，能够计算高效化。

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

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

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

图17表示通过综合以多个计测位置为原点的距离数据，从而代表点的误差分布缩小，代表点的精度提高的样子。因为这样地以不同的计测位置(即作为距离传感器的三维计测器的位置)为原点而得到的距离数据其误差分布的朝向也不同，所以通过经由环境模型将这些距离数据逐次综合，从而代表点的误差分布缩小，代表点的位置精度提高。再有，在图17中，三维计测后的图是表示了杯子的二维截面的示意图，三维计测后的图的虚线表示杯子的实际表面。

此外，辨认自身位置的同时，辨认自身位置的误差分布，根据当前的自身位置和误差分布以及辨认了的自身位置和误差分布，利用卡尔曼滤波器校正自身位置和误差分布，能够愈发提高精度。

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

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

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

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

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

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

以各模型点群的位置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中的判断相同)。之后的处理与图13的ST4以后的处理相同即可。

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

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

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

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

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

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

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

具体地，假定为与计测点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(j\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)的朝向与图20所示的不同，所以误差协方差矩阵∑_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(j\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 (31)

1.一种自身位置辨认方法，引入外界的三维形状来辨认自身位置，其特征在于，实施下述步骤：

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

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

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

在先前的计测位置的所述三维形状上的所述坐标值不存在的情况下，将新的计测位置辨认为自身位置，

在先前的计测位置的所述三维形状上的所述坐标值存在的情况下，实施下述步骤：精密对准步骤，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，或者使针对新的计测位置的环境模型旋转及平移，并以与邻近的误差分布间距离相关的评价值成为最小的方式，进行位置对准；以及自身位置辨认步骤，根据所述精密对准步骤中的旋转量以及平移量来辨认自身位置。

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

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

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

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

6.根据权利要求1所述的自身位置辨认方法，其特征在于，在所述自身位置辨认步骤中，根据先前的计测位置的位置以及姿势来辨认新的计测位置的六自由度位置。

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

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

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

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

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

在对应于新输入的所述被计测点的坐标值的该体素内没有代表点的情况下，将新输入的所述被计测点的所述坐标值和误差分布设定为代表点的坐标值和误差分布。

11.根据权利要求1所述的自身位置辨认方法，其特征在于，在所述自身位置辨认步骤之后具有更新所述环境模型的模型更新步骤，在该模型更新步骤中，探测对应于新输入的被计测点的坐标值的体素，

在对应于新输入的所述被计测点的所述坐标值的所述体素内有已经设定了的代表点的情况下，比较新取得的误差分布和已经设定了的体素内的误差分布，

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

在误差分布不相互重叠的情况下，以在单一的体素内仅存在单一的代表点的方式，将对应于新输入的所述被计测点的所述坐标值的该体素进一步分割并分级地分割为多个体素。

12.根据权利要求1所述的自身位置辨认方法，其特征在于，还实施将所述自身位置输出到输出装置的输出步骤，在所述自身位置辨认步骤中，将自身位置的误差分布与自身位置的辨认一起进行辨认，在所述输出步骤之前，根据当前的自身位置和误差分布以及辨认了的自身位置和误差分布，通过卡尔曼滤波器，来校正自身位置和误差分布，在所述输出步骤中，输出被校正的自身位置和误差分布。

13.根据权利要求11所述的自身位置辨认方法，其特征在于，在所述模型更新步骤中，比较所述新取得的误差分布和所述已经设定了的体素内的所述误差分布，在误差分布相互重叠的情况下，根据两误差分布再设定新的误差分布和新的代表点，其结果是新的代表点移动到其他体素内，此时，

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

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

14.根据权利要求1所述的自身位置辨认方法，其特征在于，在所述自身位置辨认步骤之后具有更新所述环境模型的模型更新步骤，

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

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

16.根据权利要求15所述的自身位置辨认方法，其特征在于，所述与一致度有关的评价值的计算式以下面的[算式16]表示，

[算式16]

$\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。

17.根据权利要求1所述的自身位置辨认方法，其特征在于，所述评价值是临近的误差分布间的距离的总和。

18.根据权利要求17所述的自身位置辨认方法，其特征在于，实施：输出步骤，将所述自身位置输出到输出装置中。

19.根据权利要求17所述的自身位置辨认方法，其特征在于，在所述匹配步骤中，体素内部除了代表点及其误差分布之外，还设定并存储表示在体素内物体存在概率的概率值。

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

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

22.一种三维形状的计测方法，根据外界的三维形状的被计测点的坐标值来复原三维形状，并输出三维形状数据，其特征在于，实施下述步骤：

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

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

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

在先前的计测位置的所述三维形状上的所述坐标值不存在的情况下，将新的计测位置辨认为自身位置，

在先前的计测位置的所述三维形状上的所述坐标值存在的情况下，实施下述步骤：精密对准步骤，对于针对先前的计测位置的环境模型，使新的计测数据以及误差分布旋转及平移，或者使针对新的计测位置的环境模型旋转及平移，并以与邻近的误差分布间距离有关的评价值成为最小的方式，进行位置对准；以及自身位置辨认步骤，根据所述精密对准步骤中的旋转量以及平移量来辨认自身位置，

所述方法还具有：输出步骤，将所述自身位置、基于所述自身位置的所述体素位置、代表点以及误差分布的至少任一个输出到输出装置中。

23.根据权利要求22所述的三维形状的计测方法，其特征在于，在所述输出步骤中，将所述体素位置、代表点位置以及误差分布位置的至少任一个作为三维形状的计测值输出到输出装置中，并且将表示该计测值的可靠性或精度的指标基于该体素内部的误差分布的大小，输出到输出装置中。

24.根据权利要求22所述的三维形状的计测方法，其特征在于，在所述输出步骤中，将所述体素位置、代表点位置以及误差分布位置的至少任一个作为三维形状的计测值输出到输出装置中时，在该体素内部的误差分布的大小比规定的基准值大的情况下，该计测值的可靠性或精度定为比规定的基准低，不将该体素的所述计测值输出到输出装置中。

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

在对应于新输入的被计测点的所述坐标值的该体素内没有代表点的情况下，将所述坐标值和误差分布设定为代表点的坐标值和误差分布。

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

在对应于新输入的被计测点的所述坐标值的所述体素内有已经设定了的代表点的情况下，比较新取得的误差分布和已经设定了的体素内的误差分布，

在误差分布相互重叠的情况下，根据两误差分布、或根据两误差分布和体素内已经设定了的代表点和新输入的被计测点的坐标值，再设定新的误差分布和新的代表点，

在误差分布不相互重叠的情况下，以在单一的体素内仅存在单一的代表点的方式，将对应于新输入的被计测点的所述坐标值的该体素进一步分割并分级地分割为多个体素。

27.根据权利要求22所述的三维形状的计测方法，其特征在于，在所述匹配步骤之后具有更新所述环境模型的模型更新步骤，在该模型更新步骤中，探测对应于新输入的被计测点的坐标值的体素，在对应于新输入的被计测点的所述坐标值的所述体素内的代表点以及误差分布的至少任一个被新设定、或被再设定、或将对应于新输入的被计测点的所述坐标值的该体素进一步分割并分级地分割为多个体素的情况下，在所述输出步骤中，将对应于新输入的被计测点的所述坐标值的该体素、或者被分级地分割了的所述多个体素的代表点位置作为三维形状的计测值输出到输出装置中。

28.根据权利要求22所述的三维形状的计测方法，其特征在于，在所述输出步骤中，将从距离传感器的位置出发距离传感器能进行位置计测的范围的环境模型内的体素的代表点位置，作为三维形状的计测值输出到输出装置中。

29.一种自身位置辨认装置，引入外界的三维形状来辨认自身位置，其特征在于，具备：

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

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

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

输出装置，将自身位置输出到输出装置中，

在新的计测位置处，在先前的计测位置的所述三维形状上的所述坐标值不存在的情况下，将新的计测位置辨认为自身位置，

在新的计测位置处，在先前的计测位置的所述三维形状上的所述坐标值存在的情况下，

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

根据所述位置对准中的旋转量以及平移量来辨认自身位置。

30.根据权利要求29所述的自身位置辨认方法，其特征在于，所述评价值是临近的误差分布间的距离的总和。

31.一种三维形状的计测装置，根据三维形状上的被计测点的坐标值来复原三维形状，并输出三维形状数据，其特征在于，具备：

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

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

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

数据传输装置，

其中，在该数据传输装置中，

在新的计测位置处，在先前的计测位置的所述三维形状上的所述坐标值不存在的情况下，将新的计测位置辨认为自身位置，

在新的计测位置处，在先前的计测位置的所述三维形状上的所作坐标值存在的情况下，

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

根据所述位置对准中的旋转量以及平移量来辨认自身位置，

进而将所述自身位置、基于所述自身位置的所述体素位置、代表点以及误差分布的至少任一个输出到输出装置中。

Images