具体实施方式

下面将描述示例性实施例，该示例性实施例用于防止生成由用于在进行闭环时校正地图数据的较长处理时间而导致的冗余测量点。

下面将参照附图详细描述示例性实施例。以下示例性实施例并不意图限制本公开的权利要求的范围，并且并非以下示例性实施例中描述的特征的所有组合均对本公开必不可少。

下面将参照附图详细描述根据第一示例性实施例的移动体系统、环境地图创建系统、信息处理装置、信息处理方法和计算机程序。

第一示例性实施例示出了如下示例，其中，安装有传感器的移动体由用户从外部操作以在环境中移动，并且生成可用于移动体的位置/朝向估计和自主行进的环境地图数据。尽管本示例性实施例示出了将灰度相机用作传感器的示例，但是本示例性实施例不限于该示例。例如，复眼相机也可以用作传感器，并且深度可以用作传感器信息。激光测距扫描器、激光测距仪、激光成像检测和测距(LIDAR，Laser Imaging Detection and Ranging)等也可以用作传感器。

在本示例性实施例中使用的术语“环境”是指包括传感器在其中移动的区域和周围区域的三维空间。在本示例性实施例中使用的“位置和朝向”是作为三维位置信息和具有三个自由度的朝向信息的组合的具有六个自由度的值。在本示例性实施例中，三维空间中的位置和朝向可以由4×4的仿射矩阵表示，并且仅使用仿射变换的性质中的旋转和平行移动。如果存在分别表示位置和朝向的两个仿射矩阵A和B，则可以通过将仿射矩阵B与仿射矩阵A的逆矩阵相加来获得表示仿射矩阵A与仿射矩阵B之间的相对位置和朝向(位置/朝向差)的仿射矩阵d。类似地，可以通过将相对位置和朝向d与位置/朝向A相加来获得位置/朝向B。

(环境地图数据)

根据本示例性实施例的在计算位置和朝向中使用的环境地图数据包括一个或更多个测量点和姿态图。各测量点包括传感器信息(传感器拍摄的图像数据)和位置/朝向信息，作为用于位置/朝向测量的信息和用于环路检测的信息。

姿态图是简单图，其中测量点被示出为节点，并且两个测量点之间的相对位置和朝向被示出为边缘。在姿态图中，各测量点通过边缘连接到一个或更多个其他测量点。环境地图数据上的所有测量点都连接在姿态图上。

(漂移和闭环)

现在将描述当生成环境地图数据时发生的位置和朝向的误差(漂移)与闭环处理之间的关系。通过重复生成测量点并使用该测量点估计当前位置和朝向来生成环境地图数据。因此，当生成测量点时发生的位置和朝向的误差导致在随后的使用该测量点的位置/朝向估计中的误差，使得每次生成测量点时累积误差。误差的累积称为“漂移”。

图1是示意性示出在环境地图数据中生成的测量点与移动体在平面上的真实移动路径之间的关系的图。在图1中，实线表示通过估计结果获得的移动体的估计位置和朝向，而虚线表示移动体的真实位置和朝向路径。在图1中，各白色圆圈表示测量点。移动体在环境中以测量点A为起点顺时针移动的同时生成测量点B、C、…、E、F、G和H，然后返回到测量点A附近的位置。测量点H'表示当生成测量点H时的真实位置和朝向。累积在各测量点处发生的位置和朝向的误差并且位置和朝向根据移动量逐渐偏离真实位置和朝向的现象被称为漂移。

为了校正漂移，如下进行被称为“闭环”的处理。即，当移动体重复到达(循环)环境地图数据上的某个点时，校正移动体和环境地图数据上的元素的位置和朝向。在闭环处理中，当传感器检测到移动体已经返回到已经映射的区域时(环路检测)，对通过搜索而累积的误差进行校正(环路校正)。在图1所示的示例中，校正从测量点A到测量点H的路径上的各测量点处的位置和朝向(姿态图优化)，从而校正在测量点H处的位置和朝向，以基本上对应于测量点H'并且满足校正后的测量点与其他测量点之间的相对位置/朝向关系。

然而，在姿态图优化中，将环境地图数据上的大量测量点处的位置和朝向用作参数。因此，需要花费取决于测量点的数量的较长的时间。因此，如果在检测到环路之后进行姿态图优化时移动体连续移动，则可以在姿态图优化完成之前生成新的测量点。

图2是示出上述情况下的环境地图数据的状态的图。在生成测量点H之后，在测量点A与H之间检测到环路。移动体连续行进通过环境地图数据上远离测量点A和B的位置，使得在姿态图优化处理完成之前生成新的测量点I和J。在姿态图优化处理完成之后，将测量点I和J处的位置和朝向分别校正为测量点I'和J'。然而，在姿态图上，测量点I'和J'没有连接到存在于校正后的测量点I'和J'附近的测量点A'、B'和C'。因此，如果移动体从测量点J'进一步向前移动，则可以在测量点C'附近生成其他新的测量点。如果移动体再次在这些测量点附近行进并计算位置和朝向，则位置/朝向估计结果可能根据是参照测量点B'和C'计算位置和朝向、还是参照测量点I'和J'计算位置和朝向而改变。

为了避免该问题，在本示例性实施例中，在测量点A与H之间检测到环路的定时，对用于测量移动体的位置和朝向的测量点H进行可以在短时间段内完成的局部校正处理。图3是示出当局部校正处理结束时的环境地图数据的状态的图。将测量点H处的位置和朝向校正为测量点H'。在本示例性实施例中，移动体在此时仍位于测量点H'附近，并且不生成新的测量点。

图4是示出如下状态的图，其中，在局部校正处理结束之后、姿态图优化处理完成之前，移动体在沿着测量点A和B的路线上连续向前移动。此时，在本示例性实施例中，可以参照测量点A和B来计算移动体的位置和朝向，使得因此不生成与测量点I和J相对应的新的测量点。

此时，由于未校正测量点B处的位置和朝向的漂移，因此在检测到环路之后通过位置/朝向估计结果获得的移动体的路径由于测量点B而在图4的左侧方向上展示出误差。然而，该误差的量小于由测量点H处的漂移引起的误差的量。此外，此时维持当生成测量点B时的路径与在检测到环路之后的路径之间的相对位置/朝向关系。

图5是示出当姿态图优化处理完成时的状态的图。姿态图优化处理使得能够将测量点B至G处的位置和朝向分别校正为位于真实运动路径附近的测量点B'至G'。此时，还基于测量点B校正移动体的位置和朝向。

因此，在本示例性实施例中，在姿态图优化之前进行在短时间段内完成的局部校正处理。因此，即使花费很长时间进行姿态图优化，也可以防止生成冗余测量点并防止在使用环境地图数据时位置/朝向估计的精度下降。根据本示例性实施例，在同时定位和地图构建(SLAM)中，当检测到环路时，在进行姿态图优化之前以及在生成新的测量点之前，仅对环路源等进行局部位置/朝向校正处理，与姿态图优化处理相比，局部位置/朝向校正处理以更小的处理量在短时间段内完成。因此，可以防止生成不应基于环境地图数据中的误差而生成的测量点，并且可以防止由于基于该误差生成的测量点而导致的位置/朝向估计结果的精度下降。

(移动体系统、环境地图创建系统和信息处理装置的构造)

接下来，将参照图6描述根据本示例性实施例的移动体系统601的构造示例。移动体系统601包括环境地图创建系统602、通信单元605、控制装置606和移动单元607，环境地图创建系统602包括传感器603和信息处理装置604。

传感器603输出通过测量周围环境获得的传感器信息。传感器603是相机，其固定在移动体系统601的前方并且被构造为连续获取灰度亮度图像。为了简化说明，假设已知相机的内部参数(诸如焦距和视角)，并且在图像没有畸变或校正了图像中的畸变的状态下输出图像。传感器603每秒拍摄图像30次，但是可以以其他帧速率拍摄图像。

信息处理装置604基于从传感器603输入的信息来估计移动体系统601的位置和朝向，并生成环境地图数据。在移动体系统601的自主行进期间，信息处理装置604向控制装置606发出移动指令。下面将详细描述信息处理装置604的构造。

通信单元605从用户接收指令，诸如移动体系统601的移动或旋转的指令以及用于开始或结束要由信息处理装置604进行的环境地图创建处理的指令。通信单元605例如是用于基于电气和电子工程师协会(IEEE)802.11系列建立通信的芯片或天线。通信单元605的通信方法不限于IEEE 802.11系列。也可以使用其他通信方法，诸如或有线通信。

控制装置606控制移动单元607的驱动。控制装置606基于来自信息处理装置604和通信单元605的指令来驱动移动单元607，以使移动体系统601移动或旋转。移动单元607是部分或全部利用动力操作的多个轮胎。

(信息处理装置的构造)

信息处理装置604包括一般的内置个人计算机(PC)装置的功能。信息处理装置604包括中央处理单元(CPU)611、只读存储器(ROM)612、随机存取存储器(RAM)613、存储单元614(诸如硬盘驱动器(HDD)或固态驱动器(SSD))、通用接口(I/F)615(诸如通用串行总线(USB))和系统总线616。

CPU 611使用RAM 613作为工作存储器来执行存储在ROM 612和存储单元614中的操作系统(OS)和各种计算机程序，从而计算或处理信息并经由系统总线616控制各单元。例如，要由CPU 611执行的程序包括用于执行下述处理的程序。

传感器603、通信单元605和控制装置606各自经由通用I/F 615连接至信息处理装置604。

图6示出了将信息处理装置604并入移动体系统601中的示例。然而，信息处理装置604的构造不限于该示例。信息处理装置604可以被构造为与移动体系统601分离的装置。在这种情况下，通用I/F 615可以用作无线或有线通信接口，并且可以被构造为与通信单元605通信，并与传感器603通信以进行数据交换。信息处理装置604可以构成整个移动体系统601。

(信息处理装置的逻辑构造)

下面将描述根据本示例性实施例的信息处理装置604的逻辑构造。以将计算机程序从ROM 612等加载到RAM 613中并由CPU 611执行加载的程序的方式，将以下描述的各单元的处理作为软件来执行。图7是示出环境地图创建系统602和信息处理装置604的逻辑构造的框图。图7所示的各逻辑构造可以被构造为专用集成电路(ASIC)、现场可编程门阵列(FPGA)等。

环境地图数据701是存储在RAM 613中并且当前被创建的环境地图数据。

传感器信息获取单元702获取来自传感器603的输出。具体地，传感器信息获取单元702获取由传感器603拍摄的图像。

位置/朝向计算单元703从环境地图数据701中包括的测量点中选择位于传感器603附近的测量点，并基于由传感器603拍摄的图像和选择的测量点来估计传感器603的位置和朝向。

测量点生成单元704根据需要基于由传感器603拍摄的图像和由位置/朝向计算单元703估计的位置/朝向信息，将测量点添加到环境地图数据701。例如，当环境地图中的传感器603的位置与环境地图中的现有测量点之间的距离大于或等于预定值时，以及当确定出难以基于现有的测量点来估计位置和朝向时，最好添加测量点。此外，测量点生成单元704将所生成的测量点与当生成测量点时用于位置/朝向估计的测量点之间的相对位置和朝向，添加到环境地图数据701上的姿态图。

闭环处理单元705进行闭环处理。当传感器检测到移动体已经返回到已经映射的区域时(环路检测)，闭环处理单元705对由于搜索而累积的误差进行校正(环路校正)。具体地，当闭环处理单元705确定新生成的测量点位于先前生成的测量点附近时，闭环处理单元705计算两个测量点的相对位置，并在姿态图上将测量点相关联。然后，闭环处理单元705进行用于校正环境地图上的至少构成环路的区域的形状的处理。下面将详细描述闭环处理单元705的构造。

(闭环处理单元的逻辑构造)

图8是示出闭环处理单元705的详细逻辑构造的框图。

环路检测单元801检查环境地图数据701上的新生成的测量点，并确定是否发生了测量点的环路。在此使用的术语“环路”是指新生成的测量点和在姿态图上未关联的先前创建的测量点在真实空间中彼此靠近的状态。具体地，首先，通过使用已知的词袋(Bag-of-Words，BoW)模型来计算在新生成的测量点处拍摄的图像与已经创建的各测量点处拍摄的图像之间的相似度，并且提取具有大于或等于一定水平的相似度的测量点。

之后，环路检测单元801按照相似度的降序，计算新生成的测量点与各创建的测量点之间的相对位置和朝向。具体地，从关于两个测量点的传感器信息中提取图像特征点，并且基于特征点的分布来估计图像之间的相对位置和朝向。如果在任一测量点处的相对位置和朝向的计算成功，则环路检测单元801确定在两个测量点之间已经发生环路。在下文中，新生成的测量点被称为环路源，而先前创建的测量点被称为环路目的地。在检测到环路之前，姿态图上的环路源与环路目的地之间的路径被称为环路范围。

第一校正单元802将由位置/朝向计算单元703参照的关于环境地图数据701中的元素的位置/朝向信息，校正为基于环路目的地测量点的位置和朝向。此外，第一校正单元802将环路源测量点与环路目的地测量点之间的相对位置和朝向添加到环境地图数据701上的姿态图。

第二校正单元803对环境地图数据701上的姿态图中示出的各测量点的位置/朝向信息进行优化处理(姿态图优化)。第二校正单元803获取测量点之间的相对位置和朝向，并进行姿态图优化，以最小化姿态图中示出的测量点之间的相对位置/朝向信息与基于测量点处的位置和朝向计算出的相对位置/朝向信息之间的误差。

(环境地图数据创建处理)

接下来，将参照图9至图11描述根据本示例性实施例的与使用信息处理装置604的信息处理方法有关的环境地图数据的创建方法。图9是示出根据本示例性实施例的环境地图数据校正处理流程的流程图。通过CPU 611加载存储在ROM 612或存储单元614中的程序并执行该程序以计算或处理信息并控制各硬件模块，来实现图9至图11的流程图所示的处理。各流程图中示出的一些或全部步骤可以由诸如ASIC或FPGA的硬件模块来实现。

如图9所示，在步骤S901中，信息处理装置604进行初始化。信息处理装置604构建空的环境地图数据701，并在环境地图数据701上生成第一测量点(图1所示的测量点A)。将第一测量点的位置设置为环境中的原点，并且将朝向设置在预定方向(例如，Y轴正方向)上。信息处理装置604利用在第一测量点处的位置和朝向来初始化由位置/朝向计算单元703估计的当前位置/朝向信息。

随后的处理在三个线程(即，位置/朝向计算线程、地图创建线程和地图校正线程)中并行执行。步骤S902至S904对应于地图创建线程中的处理。

在步骤S902中，位置/朝向计算单元703从传感器603获取图像，并基于图像和当前参照的测量点更新当前位置/朝向信息。具体地，信息处理装置604使用通过将与测量点相关联的图像与在当前位置拍摄的最新图像之间的位置/朝向差应用于与当前参照的测量点相关联的位置/朝向信息而获得的位置/朝向信息，作为当前位置/朝向信息。根据各图像上的特征点的分布来估计图像之间的位置/朝向差。为了维持图像之间的特征点的对应关系，在最新图像与先前图像之间跟踪特征点。

代替在各图像中计算与测量点相关联的图像的位置/朝向差，可以计算最新图像与先前图像之间的位置/朝向差，并且可以重复地将计算出的位置/朝向差应用到当前位置/朝向信息。

对于图像上的特征点的上述提取，在本示例性实施例中使用了加速分段测试提取特征(FAST，Features from Accelerated Segment Test)算法。为了估计图像之间的位置/朝向差，在各特征点的位置固定的情况下，使用束调整(bundle adjustment)处理，以仅优化传感器603的位置和朝向。束调整处理是用于解决基于在多个图像之间提取的图像特征点的对应关系来估计几何模型的参数的问题的方法，并且还用于对非线性优化问题进行数值求解。在束调整处理中，基于输入图像之间的特征点的对应关系来计算各特征点的三维坐标。将计算出的各特征点的三维坐标重新投影到像平面上，并且重复地重新估计作为重新投影点与各特征点之间的距离而计算出的重新投影误差，从而估计出各特征点的三维坐标更准确的值。

已知的Kanade-Lucas-Tomasi(KLT)算法用于跟踪图像之间的特征点。上述用于特征点提取和特征点跟踪的算法可以被其他算法代替。

在步骤S903中，位置/朝向计算单元703选择在随后的位置/朝向计算中要参照的测量点。具体地，信息处理装置604从环境地图数据701上位于当前位置和朝向附近的测量点中，选择相对于最新图像具有最高特征点对应量的测量点。

位置/朝向计算单元703确定是否要添加新的测量点。如果要添加新的测量点，则位置/朝向计算单元703向测量点生成单元704发出生成测量点的指令。具体地，如果在步骤S903中选择的测量点与最新图像之间的特征点的对应量小于或等于阈值，或者如果测量点处的位置和朝向与当前位置和朝向之间的差大于或等于阈值，则信息处理装置604确定为要添加新的测量点。如果确定要添加新的测量点并且测量点生成单元704不具有任何未处理的测量点生成指令，则信息处理装置604发出在地图创建线程中生成新的测量点的指令。在生成测量点的指令中，位置/朝向计算单元703将关于选择的测量点和最新图像的信息传递给测量点生成单元704。

在步骤S904中，确定是否结束位置/朝向计算线程中的处理。如果响应于来自用户的指令结束环境地图数据创建处理(步骤S904中为“是”)，则线程结束。如果环境地图数据创建处理未结束(步骤S904中为“否”)，则处理返回至步骤S902以重复该处理。

步骤S905和S906是地图创建线程中的处理。

在步骤S905中，测量点生成单元704基于在步骤S903中从位置/朝向计算单元703发出的指令来生成新的测量点。如果测量点生成单元704没有未处理的指令，则测量点生成单元704等待接收新的指令。新的测量点包括指示在步骤S902中获取的图像以及位置和朝向的信息。测量点生成单元704从传感器603获取拍摄图像，并基于拍摄图像计算位置和朝向。然后，测量点生成单元704生成包括所获取的拍摄图像以及计算出的位置和朝向的测量点。

此外，测量点生成单元704将新的测量点与在步骤S903中选择的测量点之间的相对位置/朝向信息添加到姿态图。

在步骤S906中，确定是否结束地图创建线程中的处理。如果位置/朝向计算线程已经结束(步骤S906中为“是”)，则地图创建线程终止。如果位置/朝向计算线程未结束(步骤S906中为“否”)，则处理返回至步骤S905以重复该处理。

步骤S907和S908是地图校正线程中的处理。

在步骤S907中，在检测到在更新了地图创建线程的环境地图数据上的环路时，信息处理装置604进行闭环处理。下面将详细描述闭环处理。

在步骤S908中，确定是否结束地图校正线程。如果位置/朝向计算线程和地图创建线程已经结束(步骤S908中为“是”)，则地图校正线程终止。如果位置/朝向计算线程和地图创建线程未结束(步骤S908中为“否”)，则处理返回至步骤S907以重复该处理。

(闭环处理的细节)

接下来，将详细描述步骤S907所示的闭环处理。图10是示出根据本示例性实施例的闭环处理流程的流程图。

如图10所示，在步骤S1001中，环路检测单元801检测环路，并计算环路源测量点与环路目的地测量点之间的相对位置和朝向。在步骤S1002中，确定是否检测到环路。如果检测到环路(步骤S1002中为“是”)，则在将关于环路源与环路目的地之间的相对位置和朝向的信息添加到姿态图之后，处理进入S1003。如果没有检测到环路(步骤S1002中为“否”)，则闭环处理终止。

在步骤S1003中，第一校正单元802进行第一校正处理，以将位置/朝向计算单元703当前参照的测量点的位置/朝向信息校正为基于环路目的地测量点的坐标。第一校正处理在比用于在地图创建线程中生成测量点的间隔更短的时间段内完成。换句话说，信息处理装置604在从第一校正处理开始到第一校正处理完成的时段期间，不生成任何新的测量点。下面将详细描述第一校正处理。

在步骤S1004中，第二校正单元803对环境地图数据701上的环路范围和在环路范围之后生成的测量点进行姿态图优化处理(第二校正处理)。在步骤S1003中将关于在步骤S1001中计算出的环路源与环路目的地之间的相对位置/朝向的信息添加到姿态图，并且通过姿态图优化处理来校正在各测量点处发生的漂移。因此，可以将各测量点处的位置和朝向校正为接近传感器603的真实位置/朝向路径的位置。如果通过姿态图优化处理来校正由位置/朝向计算单元703当前参照的测量点处的位置和朝向，则还校正关于传感器603的当前位置/朝向信息。

(第一校正处理的细节)

接下来，将参照图11所示的流程图详细描述在步骤S1003中示出的第一校正处理。在这种情况下，通过使用位置/朝向差的刚性变换来校正各测量点处的位置和朝向。

首先，考虑如下情况：如图1所示，传感器603在环境中以测量点A为起点顺时针移动的同时生成测量点B、C、…、E、F、G和H，然后传感器603返回到测量点A附近的位置。在步骤S1001中，检测将测量点H设置为环路源并且将测量点A设置为环路目的地的环路，并计算测量点之间的相对位置和朝向。

如图11所示，在步骤S1101中，确定是否进行使用刚性变换的校正处理。刚性变换指示仅平行移动和旋转的变换。

如上所述，第一校正处理是用于将位置/朝向计算单元703当前参照的测量点的位置/朝向信息校正为基于环路目的地测量点的坐标的处理。因此，如果位置/朝向计算单元703已经参照姿态图上的环路目的地测量点或环路目的地测量点附近的测量点，则不能有效操作使用刚性变换对测量点处的位置和朝向的校正。在本示例性实施例中，将位置/朝向计算单元703当前参照的测量点与姿态图上的环路源测量点和环路目的测量点之间的距离相比较。仅当环路源测量点更靠近测量点时，才执行步骤S1102至S1104的以下处理。

在步骤S1102中，第一校正单元802首先计算相对位置和朝向。此后，基于环路目的地测量点处的位置和朝向以及计算出的相对位置和朝向来计算第一校正之后的测量点H'处的位置和朝向。此外，基于校正后的测量点H'处的位置和朝向以及校正前的测量点H处的位置和朝向来计算在第一校正中使用的位置/朝向差dH。

在步骤S1103中，选择进行了使用刚性变换的校正处理的环境地图数据上的元素。这里假设从发出生成测量点H的指令(步骤S903)到检测到环路(步骤S1001)的时段足够短，并且传感器603位于测量点H附近。因此，位置/朝向计算单元703仍然参照测量点H。在这种情况下，选择测量点H作为要校正的测量点。

在步骤S1104中，第一校正单元802将在步骤S1102中计算出的位置/朝向差dH添加到在步骤S1103中选择的校正对象位置和朝向，并更新位置和朝向。关于传感器603的当前位置/朝向信息也以相同的方式被更新。

该处理可以通过整合4×4的简单矩阵来实现，并且可以在比步骤S905中的测量点生成处理或包括姿态图优化处理的步骤S1004的处理短得多的时间段内完成。因此，在闭环处理单元705检测到环路之后，在需要在位置/朝向计算线程中生成新的测量点之前，完成校正处理，然后可以进行参照测量点A的位置/朝向计算处理。因此，可以防止冗余测量点I和J的生成。

因此，当检测到环路时，信息处理装置604在姿态图优化处理之前执行局部校正处理，姿态图优化处理用于校正与环境地图数据中包括的多个测量点相关联的位置和朝向。作为局部校正处理进行的第一校正处理校正比在姿态图优化处理中校正的测量点的数量少的数量的测量点处的位置和朝向。换句话说，当检测到环路时，信息处理装置604执行第一校正处理以校正一些测量点处的位置和朝向。在完成第一校正处理之后，信息处理装置604完成对比在第一校正处理中校正的测量点的数量多的数量的测量点的第二校正处理。因此，可以防止生成冗余测量点。

本示例性实施例示出了第一校正单元802进行使用刚性变换的校正处理的示例。然而，校正处理不限于该示例。由第一校正单元802进行的校正处理可以是其他校正处理，只要其处理量小于由第二校正单元803进行的校正处理(姿态图优化处理)中的处理量即可。此外，由第一校正单元802进行的校正处理可以是其他校正处理，只要即使在检测到环路之后传感器603连续移动时，也可以在生成新的测量点之前完成校正处理、并且可以校正环路源测量点处或环路源测量点附近的测量点处的位置和朝向即可。

(本示例性实施例的有益效果)

如上所述，根据本示例性实施例，即使当在环境地图数据的创建期间进行环境地图校正处理时，也可以生成可以高精度地估计位置和朝向的环境地图数据。此外，根据本示例性实施例，第一校正处理在第二校正处理之前完成，从而防止了冗余测量点的生成，该冗余测量点是由于用于在闭环处理期间校正地图数据的时间增加而导致的。

移动体系统601移动以循环通过多个坐标，同时基于由信息处理装置604生成的环境地图数据和由传感器603拍摄的图像进行位置/朝向估计处理，从而高精确度地自主行进。

下面将描述第二示例性实施例。上述第一示例性实施例示出了位置/朝向计算单元703参照环境地图数据701中的单个测量点来进行位置/朝向测量处理的示例。本示例性实施例示出了用于在将关于环境中的各特征点的三维位置信息保持为环境地图数据并且移动体系统601基于三维特征点分布和传感器信息来估计位置/朝向信息的情况下校正环境地图数据的方法。在创建这样的环境地图数据的情况下，根据需要，通过称为束调整的方法来校正测量点之间的相对位置和朝向，从而使得能够以更高的精度生成环境地图数据。

(环境地图数据)

在本示例性实施例中用于位置和朝向的计算的环境地图数据包括环境中的多个特征点、一个或更多个测量点以及姿态图。

环境中的各特征点包括三维位置信息。各测量点不仅保持传感器信息(图像)和位置/朝向信息作为用于环境中的特征点检测的信息和用于环路检测的信息，而且保持关于环境中的各特征点的观察信息。观察信息是传感器信息中包括的图像的环境中的特征点和图像上的二维坐标的组合的列表。图像上的二维坐标是与从传感器观看时环境中的各特征点的方位相对应的信息。

(移动体系统、环境地图创建系统和信息处理装置的构造)

根据本示例性实施例的移动体系统601的物理和逻辑构造与第一示例性实施例的相似，因此省略其描述。

(环境地图数据创建处理)

将参照图12至图16描述根据本示例性实施例的用于创建环境地图数据的方法。图12是示出根据本示例性实施例的环境地图数据创建处理流程的流程图。与图9所示的流程图中的步骤相似的步骤在图12所示的流程图中由相同的数字标示，并且省略其详细描述。具体地，步骤S903、S904、S906和S908的处理与第一示例性实施例的处理类似，因此省略其描述。

如图12所示，在步骤S1201中，信息处理装置604进行初始化。信息处理装置604构建空的环境地图数据701，并在环境地图数据701上生成第一测量点(图1所示的测量点A)和从第一测量点观察到的环境中的特征点组。信息处理装置604利用第一测量点处的位置和朝向来初始化由位置/朝向计算单元703估计的当前位置/朝向信息。与第一示例性实施例中一样，将第一测量点的位置设置为环境中的原点，并且将朝向设置在预定方向(例如，Y轴正方向)上。

为了生成环境中的特征点组，首先，在两个位置处(即，在用于生成第一测量点A的位置和朝向处以及在稍微远离第一测量点A的位置/朝向A'处)，从传感器603获取图像。之后，基于从图像中提取出的图像特征点，使用束调整处理来估计在图像之间匹配的图像特征点的三维坐标。最后，将成功进行三维坐标估计的图像特征点添加到环境地图数据中作为环境中的特征点，并添加与环境中的特征点相对应的测量点A的图像上的图像特征点的二维坐标，作为从测量点A观察时的观察信息。

将描述根据第二示例性实施例的位置/朝向计算线程。

在步骤S1202中，位置/朝向计算单元703从传感器603获取图像，并且基于图像和环境中的特征点来更新当前位置/朝向信息。位置/朝向计算单元703将从此时参照的测量点观察的环境中的特征点重新投影到图像上，并进行优化处理以计算位置和朝向，使得重新投影点与相应的图像特征点之间的重新投影误差最小。在这种情况下，在最新图像与先前图像之间跟踪特征点，以维持图像与环境中的特征点之间的对应关系。

此外，与第一示例性实施例中一样，执行步骤S903和S904。

接下来，将描述根据第二示例性实施例的地图创建线程。

在步骤S1203中，测量点生成单元704基于在步骤S903中从位置/朝向计算单元703发出的指令来生成环境中的特征点和测量点。

要生成的测量点包括在步骤S1202中获取的图像以及位置和朝向、由位置/朝向计算单元703跟踪的环境中的特征点组、以及下面将要描述的关于环境中的新生成的特征点组的观察信息。使用在步骤S1202中选择的测量点的图像和要生成的测量点处的拍摄图像，来生成环境中的特征点。

首先，信息处理装置604从两个图像中提取与环境中的现有特征点不相关联的图像特征点。之后，信息处理装置604基于图像之间的相对位置和朝向来估计图像之间的对应图像特征点的三维坐标。最后，信息处理装置604将成功进行了三维坐标估计的图像特征点添加到环境地图数据中作为环境中的特征点，并且添加与环境中的特征点相对应的各图像上的图像特征点的二维坐标，作为来自各测量点的观察信息。此外，测量点生成单元704将新的测量点与在步骤S903中选择的测量点之间的相对位置/朝向信息添加到姿态图。

在步骤S1204中，测量点生成单元704进行束调整处理，以校正在步骤S1202中生成的测量点以及将环境中的特征点共享的测量点组的位置和朝向。此外，测量点生成单元704将作为束调整的结果而获得的测量点之间的相对位置/朝向信息添加到或写在姿态图上。

接下来，将描述根据第二示例性实施例的地图校正线程。

在步骤S1205中，检测在地图创建线程中更新的环境地图数据上的环路，并进行闭环处理。除了第一校正处理(步骤S1003)的细节以外，闭环处理的细节与根据第一示例性实施例的图10所示的细节相似。因此，现在将仅描述第一校正处理的细节。

(第一校正处理的细节)

将参照图13至图16详细描述根据本示例性实施例的在步骤S1003中示出的第一校正处理。在本示例性实施例中，除了通过使用位置/朝向差的刚性变换来校正各测量点和环境中的特征点处的位置和朝向之外，还进行用于整合环境中的特征点的处理、以及使用环路源和环路目的地附近的测量点的束调整处理。

与第一示例性实施例中一样，第二示例性实施例还示出了如下示例，其中如图1所示，传感器603在环境中以测量点A为起点顺时针移动的同时生成测量点B、C、…、E、F、G和H，然后返回到测量点A附近的位置。

当检测到将测量点H设置为环路源并将测量点A设置为环路目的地的环路时，信息处理装置604计算测量点之间的相对位置和朝向(步骤S1001)。

图13是示出当检测到环路时传感器603附近的环境地图的状态的图。在图13中，黑点p、p'、q、r和s是环境中的特征点。从测量点A观察点p和q。从测量点H和G以及传感器603的最新图像观察点p'。从测量点H以及传感器603的最新图像观察点r。从测量点G观察点s。点p和p'此时作为环境中的不同特征点登记在环境地图数据701中，但是源自真实空间中的物体的相同特征点。尽管实际上在环境中存在大量的特征点用于位置/朝向估计，但是为了便于说明，在此仅示出了一些特征点。

图14是示出根据本示例性实施例的第一校正处理流程的流程图。如图14所示，在步骤S1401中，确定是否进行使用刚性变换的校正处理。在本示例性实施例中，信息处理装置604执行步骤S1102、S1402和S1403的处理，只要在由位置/朝向计算单元703参照的环境中的特征点中不包括从环路目的地测量点观察到的环境中的特征点即可。

在步骤S1401中，确定是否进行了使用刚性变换的校正处理。如果确定进行了使用刚性变换的校正处理(步骤S1401中为“是”)，则处理进入步骤S1102。然后，在步骤S1402中，信息处理装置604选择要通过刚性变换校正的环境地图数据上的元素。首先，提取由位置/朝向计算单元703跟踪的环境中的特征点。在图13所示的示例中，提取点p'和r。接下来，提取观察到任一特征点的测量点作为要校正的测量点。在图13所示的示例中，提取测量点H和G。最后，提取由要校正的任一测量点观察到的环境中的特征点作为要校正的环境中的特征点。在图13所示的示例中，点p'、r和s是要校正的环境中的特征点。关于传感器603的当前位置/朝向信息也被校正。因此，在这种情况下，校正测量点H和G中的各个处的位置和朝向、环境中的特征点p'、r和s中的各个的位置、以及传感器603的位置和朝向。

在步骤S1403中，第一校正单元802将在步骤S1102中计算出的位置/朝向差dH添加到在步骤S1102中选择的各校正对象，并更新位置和朝向或位置。该处理是刚性变换，并且维持要校正的元素之间的相对位置/朝向关系。

图15是示出在刚性变换处理之后获得的测量点之间的位置关系的图。在测量点H和G中的各个处的位置和朝向分别被校正为H'和G'。此时，测量点F与要校正的元素之间的相对位置/朝向关系被破坏。然而，测量点和位置/朝向计算单元703此时不共享环境中的特征点，因此相对位置/朝向关系的破坏对位置/朝向计算没有不利影响。

在步骤S1404中，第一校正单元802对在步骤S1403中校正了位置的环境中的特征点和在校正后的位置附近存在的环境中的特征点进行整合。首先，信息处理装置604从校正了位置的环境中的特征点中提取与环境地图中的各校正后的位置相距一定距离以下的其他点。之后，信息处理装置604基于环境中的提取的特征点与环境中的校正后的特征点之间的各观察信息，比较图像上的观察环境中的特征点的区域中的图像特征。使用补片匹配(patch matching)来比较图像特征。如果确定观察区域中的图像特征彼此足够相似，则确定图像特征指示参照实际空间中的相同物体，并且整合环境中的两个特征点。比较图像特征的方法不限于该示例。例如，可以使用定向FAST和旋转BRIEF(ORB)特征量。

图16是示出在特征点被整合之后传感器603附近的环境地图的状态的图。通过刚性变换校正点p'的位置，然后将点p'和p整合。整合后获得的点p包括关于整合前的点p'和p的观察信息。换句话说，从测量点A、H'和G'观察点p。

在步骤S1405中，第一校正单元802进行束调整处理，以校正在步骤S1404中整合的环境中的特征点组的位置、以及从中观察特征点组的任一位置的测量点组的位置和朝向。在图13所示的示例中，环境中的特征点p以及测量点A、H'和G'是进行了束调整的点。现实中，实际上对未示出的环境中的更多特征点进行整合处理和束调整，并且有足够的信息可用于通过束调整来优化位置和朝向。此外，第一校正单元802将作为束调整的结果而获得的测量点之间的相对位置/朝向信息添加到或写在姿态图上。

与使用刚性变换的校正相比，使用束调整来校正环路源和环路目的地附近的各测量点处的位置和朝向会花费更长的时间，但是与包括姿态图优化的步骤S1004的处理相比，可以在更短的时间段内完成。因此，在检测到环路之后，闭环处理单元705可以在短时间段内以高精度获得环路源和环路目的地附近的各测量点处的相对位置和朝向，这导致了位置/朝向计算单元703对位置和朝向的计算精度的提高。即使在第二校正处理完成之前生成新的测量点，位置/朝向计算的精度的提高也会导致位置/朝向信息的精度的提高。

(本示例性实施例的有益效果)

根据上述本示例性实施例，即使当通过在环境地图数据中包括环境中的特征点信息的位置/朝向计算方法在环境地图数据的创建期间执行环境地图校正处理时，也可以生成可以高精度地实现位置/朝向估计的环境地图数据。

[其他示例性实施例]

第一示例性实施例和第二示例性实施例示出了如下示例，其中，使用固定在移动体的前方并且被构造为获取灰度亮度图像的相机作为传感器603。然而，传感器603的类型、数量和固定方法不限于该示例。任何传感器都可以用作传感器603，只要该传感器可以连续地从移动体获取周围区域的亮度图像或深度图像作为数字数据即可。不仅可以使用灰度相机，还可以使用例如能够获取彩色图像的相机、深度相机、二维(2D)光检测和测距(LiDAR)相机或三维(3D)LiDAR相机。也可以使用立体相机，或者可以在移动体的各方向上布置多个相机。另外，每秒获取信息id的次数不限于30次。

例如，在将立体相机用作传感器603的情况下，在步骤S1201中，在用于生成环境中的特征点的处理中，信息处理装置604使用立体图像对来获取到各图像特征点的距离，而不是将传感器603移动到位置和朝向A'。信息处理装置604可以将距离转换为图像特征点的三维坐标。

另选地，在使用能够获得关于图像的各像素的距离信息的传感器(诸如深度相机或3D-LiDAR)作为传感器603的情况下，信息处理装置604基于传感器的位置和朝向以及视角、以及与各像素的距离来计算三维坐标。此外，信息处理装置604可以生成与各像素相对应的环境中的特征点。在该构造中，可以获得比第二示例性实施例中更密集的环境中的一组特征点。在这种情况下，使用已知的交互式最近点(ICP，Interactive Closest Point)算法计算测量点之间的相对位置和朝向。计算出的相对位置和朝向可以用于环路检测处理和第一校正处理。

上述第一和第二示例性实施例示出了如下示例，其中，测量三维空间中的位置和朝向并且创建用于位置/朝向测量的环境地图数据。然而，可以在沿着移动体进行移动的表面的二维平面上进行位置/朝向测量和环境地图数据创建。例如，在地面上行进的移动体系统中，可以将用于在水平方向上扫描数据的2D-LiDAR用作传感器603，并且可以在二维平面上创建如上所述具有更密集环境中的特征点组的环境地图数据。

在第一示例性实施例中，图像被保持为用于在各测量点处进行环路检测的信息。然而，例如，当在步骤S905中生成测量点时，可以基于BoW模型来计算特征值向量和图像特征点，并且可以保持特征值向量和图像特征点来代替图像。在这种情况下，可以基于由环路检测单元801预先计算出的特征值向量来计算图像相似度，并且可以基于预先计算出的特征点来计算相对位置和朝向。

在第一示例性实施例中，在步骤S901中，将第一测量点的位置设置为环境中的原点，并将朝向设置在预定方向(例如，Y轴正方向)上。然而，如果可以通过其他方法指定第一测量点的位置和朝向，则可以使用通过该方法获得的值。例如，可以使用可以由传感器603或其他单元检测到的标记来计算相对于传感器603的相对位置和朝向，并且可以基于计算出的相对位置和朝向来设置原点和坐标轴。另外，可以使用传感器603在移动体系统601上的安装位置和朝向作为补偿，来设置原点和坐标轴。

可以将与上述类似的标记用于环路检测单元801的环路检测。例如，通过检测从两个测量点观察到相同的标记，而不是计算图像相似度，可以基于各图像中传感器与标记之间的相对位置和朝向来计算图像之间相对位置和朝向。

上面描述的第二示例性实施例示出了如下示例，其中，通过确定位置/朝向计算单元703是否与环路目的地测量点共享环境中的特征点，来进行关于在步骤S1401中是否执行刚性变换处理的确定。然而，用于确定是否执行刚性变换处理的方法不限于该示例。例如，可以将在位置/朝向计算单元703与环路源测量点之间共享的环境中的特征点的数量与在位置/朝向计算单元703与环路目的地测量点之间共享的环境中的特征点的数量进行比较，然后，如果前者的数量大于后者，则确定执行刚性变换处理。另选地，如在第一示例性实施例中一样，基于距姿态图上的各测量点的距离来确定是否要执行刚性变换处理。

尽管第一示例性实施例示出了用户从外部操作移动体的示例，但是移动体系统601的构造不限于该示例。例如，可以使用用户可以乘坐并且可以直接操作的载人移动体。另选地，可以使用包括用于沿着预设路线自主行进的功能的移动体。在这种情况下，可以通过基于环境地图数据701和由位置/朝向计算单元703计算出的位置/朝向信息生成用于移动体系统601的控制信息、并通过控制装置606驱动移动单元607，来实现自主行进。此外，移动体系统601可以基于在自主行进期间从传感器603获取的传感器信息，根据第一或第二示例性实施例中描述的方法来更新环境地图数据701。

尽管示例性实施例示出了移动单元607是车轮的构造，但是也可以采用如下构造，其中在移动体系统601上安装了多个螺旋桨等，移动体系统601在空中飞翔，并且传感器603在地面的方向上观察。

本公开还可以通过以下处理来实现，在该处理中，经由网络或存储介质将用于实现根据上述示例性实施例的一个或更多个功能的程序提供给系统或装置，并且系统或装置的计算机中的一个或更多个处理器读出并执行该程序。本公开还可以通过用于实现根据上述示例性实施例的一个或更多个功能的电路(例如，ASIC)来实现。

可以使用通过机器学习获得的训练模型代替上述处理单元中包括的位置/朝向计算单元703和测量点生成单元704来执行处理。在这种情况下，例如，准备处理单元的输入数据和输出数据的多种组合作为学习数据，并且通过机器学习获取知识来生成训练模型，使得训练模型基于获取的知识输出对应于输入数据的输出数据，作为结果。可以使用例如神经网络模型来构造训练模型。训练模型与CPU或图形处理单元(GPU)协作地作为用于进行等同于处理单元的处理的程序来操作，从而进行与处理单元相对应的处理。可以根据需要在预定处理之后更新训练模型。

根据本公开的示例性实施例，可以防止由于在闭环处理期间校正地图数据的时间增加而引起的冗余测量点的生成。

其他实施例

还可以通过读出并执行记录在存储介质(也可更完整地称为“非临时性计算机可读存储介质”)上的计算机可执行指令(例如，一个或更多个程序)以执行上述实施例中的一个或更多个的功能、并且/或者包括用于执行上述实施例中的一个或更多个的功能的一个或更多个电路(例如，专用集成电路(ASIC))的系统或装置的计算机，来实现本公开的实施例，并且，可以利用通过由所述系统或装置的所述计算机例如读出并执行来自所述存储介质的所述计算机可执行指令以执行上述实施例中的一个或更多个的功能、并且/或者控制所述一个或更多个电路执行上述实施例中的一个或更多个的功能的方法，来实现本发明的实施例。所述计算机可以包括一个或更多个处理器(例如，中央处理单元(CPU)，微处理单元(MPU))，并且可以包括分开的计算机或分开的处理器的网络，以读出并执行所述计算机可执行指令。所述计算机可执行指令可以例如从网络或所述存储介质被提供给计算机。所述存储介质可以包括例如硬盘、随机存取存储器(RAM)、只读存储器(ROM)、分布式计算系统的存储器、光盘(诸如压缩光盘(CD)、数字通用光盘(DVD)或蓝光光盘(BD)^TM)、闪存设备以及存储卡等中的一个或更多个。

本发明的实施例还可以通过如下的方法来实现，即，通过网络或者各种存储介质将执行上述实施例的功能的软件(程序)提供给系统或装置，该系统或装置的计算机或是中央处理单元(CPU)、微处理单元(MPU)读出并执行程序的方法。

虽然参照示例性实施例对本公开进行了描述，但是应当对所附权利要求的范围给予最宽的解释，以使其涵盖所有这些变型例以及等同的结构和功能。