JP7483414B2

JP7483414B2 - 情報処理装置、情報処理方法及びプログラム

Info

Publication number: JP7483414B2
Application number: JP2020038558A
Authority: JP
Inventors: 久義降籏
Original assignee: Canon Inc
Current assignee: Canon Inc
Priority date: 2020-03-06
Filing date: 2020-03-06
Publication date: 2024-05-15
Anticipated expiration: 2040-03-06
Also published as: JP2021139785A

Description

本発明は、センサの位置姿勢を推定する技術に関する。

カメラや距離センサ等のセンサで取得した情報から、センサの位置姿勢や周辺のマップ情報を推定する技術としてＳＬＡＭ（ＳｉｍｕｌｔａｎｅｏｕｓＬｏｃａｌｉｚａｔｉｏｎＡｎｄＭａｐｐｉｎｇ）がある。非特許文献１では、ループクローズと呼ばれる機能についての開示がある。ループクローズでは、例えば、センサを周回的に移動させて閉経路を構成し、経路の始点と終点の位置姿勢を一致させるように最適化を行う。具体的には、センサを有する移動体が所定の経路を走行するとき、センサによって周囲の形状を計測した計測情報のうち、走行中に同じ視点で計測されたと判断される情報を用いて、センサの位置推定結果の誤差を修正する。

Ｍ．Ａ．Ｒａuｌ，ＯＲＢ－ＳＬＡＭ：ＡＶｅｒｓａｔｉｌｅａｎｄＡｃｃｕｒａｔｅＭｏｎｏｃｕｌａｒＳＬＡＭＳｙｓｔｅｍ，２０１５．

しかしながら、センサを利用する環境において、幾何的なパターンを持つ構造物や似たようなシーンが多い場合、異なる視点を同一視点として対応付けてしまう可能性がある。異なる視点を同一視点と見なしてループクローズ処理を行うと、センサの位置として誤った位置を推定してしまう。つまり、非特許文献１の方法では、所定の視点で計測した計測情報と、複数の視点で類似した計測情報と、の対応付けの誤りによってセンサの位置が正しく修正されない。本発明は、以上の課題を鑑みたものであり、センサの位置を誤って修正することを抑制することを目的とする。

上記課題を解決する本発明にかかる情報処理装置は、移動体が有するセンサによって計測した計測情報に基づいて該移動体の位置を推定する情報処理装置であって、
前記移動体が走行する経路において、前記センサが取得する画像と前記移動体の位置姿勢の情報とを含む計測情報を取得する取得手段と、
第１の時点で取得した前記計測情報である第１の計測情報と類似する計測情報を持つ、第１の時点より前の第２の時点で取得した前記計測情報である第２の計測情報を出力する出力手段と、
前記第１の時点における前記移動体の位置が前記第２の計測情報が示す位置と同じ位置か否かをユーザの入力によって決定する決定手段と、を有する。

本発明によれば、センサの位置を誤って修正することを抑制できる。

移動ロボットの走行ルートを示す俯瞰図移動ロボットを操作する様子を示す図情報処理装置のハードウェア構成例を示す図情報処理装置の機能構成の一例を示すブロック図情報処理装置の処理を示すフローチャートＧＵＩの一例を示す図移動ロボットの装置構成を示す図

＜第１の実施形態＞
ここでは、ＳＬＡＭを自律的に走行可能な移動ロボットに適用した場合について説明する。一般的にＳＬＡＭでは、センサから連続的に画像や３次元点群のデータを取得し、現在のデータとその少し過去のデータとに基づいてセンサの位置姿勢を推定する。ここで、ＳＬＡＭの課題として、センサの移動量に応じて、位置姿勢結果に含まれる誤差が増える現象が知られている（以降、誤差が増える現象をドリフトと呼ぶ）。この現象により、例えば、ある地点からＳＬＡＭによる位置姿勢の推定を開始し、しばらく移動しながらセンサの位置姿勢の推定を続けて、その後に最初の地点に戻ってきたとしても、ドリフトによる誤差分のズレを含む位置姿勢推定結果が出力される。

このドリフトの影響を抑えために、ループクローズと呼ばれる機能がある。例えば、ある環境を周回する移動ロボットを考える。ループクローズでは、例えば、センサを周回的に移動させて閉経路を構成し、経路の始点と終点の位置姿勢を一致させるように最適化を行う。つまり、物理的に近い視点の対応を認識し、その対応する視点におけるセンサの位置姿勢の計測結果の差を小さくするように、センサの位置姿勢の推定結果を更新する機能である。この処理により、センサの位置姿勢の精度を高めることができる。例えば、移動体が部屋を周回する経路、八の字に周回するような経路や同じ道を往復するような経路の場合に適用できる。なお、ループクローズが適用できる条件とは、閉経路の始点における撮影画像と終点における撮影画像において、両者の撮影領域内に共通の特徴が検出される程度の重なりが存在し、二画像間の相対的な位置姿勢が算出可能であることである。二画像間で共通領域が多いということは、計測情報から推定したセンサの位置姿勢が両者で似通っており、連続したものと見なせることを意味する。ループクローズの対象となる視点は、閉経路の始点と終点には限らず、経路の途中であっても同じ地点を通過する場合はループクローズ可能である。そのため、本実施形態においては、ＢａｇｏｆＷｏｒｄｓ（ＢｏＷ）を適用して、ループクローズが適用できるか位置にセンサが戻って来たか否かを検知する。ＢｏＷは画像中に含まれる特徴の出現頻度によって複数の画像間での類似度を算出するものである。なお、移動体が直線の経路を走行する場合でも、本実施形態における類似する計測情報の通知は有効である。例えば、経路の始点と終点が離れた地点であって、かつ始点と終点のそれぞれを視点とした計測情報が類似する場合は、ループクローズの候補地点として各地点からの計測情報が検出される。このとき始点と終点は異なる地点であることからループクローズの対象ではない。このような場合はユーザが類似する計測情報として検出されて２つの地点は異なる地点であることを装置に入力することで、ループクローズを実施しないようにする。このようにすることによって、異なる地点同士をループクローズせず、位置姿勢推定が失敗することを防ぐことができる。

非特許文献１では、物理的に近い視点の対応を認識する具体的な方法として、センサで取得した画像の類似度に基づく方法が記載されている。しかし、物理的に離れた視点であっても似た画像が撮影されうる場合（例えば、同じような物体が並んでいて、別の視点から見ても同じような画像が撮影される場合、など）では、画像の類似度に基づいた認識が誤る場合がある。また、レイアウト変更等で空間内にものが増減したことにより元々目印となっていた空間特徴が消えることや隠れてしまった場合、元の位置に移動体が戻ってきてもループクローズを実行できなくなる可能性がある。その場合、ループクローズの処理を正常に実行できず、センサの位置姿勢の精度を向上できない。

具体例として、図１に示す走行ルートを考える。図１は移動ロボットを平面的に動かしたときの状況を表す俯瞰図である。図１の実線（１００）は、ロボットが実際に走行したルートであり、破線（１１０）はＳＬＡＭにより推定したセンサの位置姿勢の軌跡である。ここで、ＳＬＡＭで推定した位置姿勢である破線（１１０）はドリフトに基づく誤差を含むため、実線（１００）と比べてズレが発生している。図１の２００は障害物（棚などが複数整列した状態）を示す。ここで、破線１１０において走行ルートの始点付近で推定した位置姿勢をＡ（１１１）、現在の位置姿勢をＢ（１１２）とする。実際には移動ロボットは障害物の間のＡの付近に戻ってきているが、ドリフトの影響を受けて誤差を含んだＢの位置姿勢が推定されてしまう。

この問題に対して、本実施形態では、ループクローズ処理が可能な位置にセンサが戻ってきたことを検知することで、より確実にセンサの位置姿勢推定の精度を向上できるようにする。以下に、その具体的な構成や手順を説明する。

本実施形態では、図２示すようにユーザ（３００）が端末（３１０）を使って移動ロボット（４００）を操縦する最中に、ＳＬＡＭを動作させる場合を考える。移動ロボット（４００）にはカメラ（４１０）が搭載されていて、そこで撮影する動画像に基づいてＳＬＡＭによる位置姿勢の推定やマップ情報の推定を行う。なお、以下の実施形態では、センサを有する移動ロボットを移動体と呼ぶこともある。また、センサの位置（姿勢）は移動体の位置（姿勢）と読み替えても良い。これは、事前にセンサを取り付けた位置姿勢と移動体との相対的な位置関係をキャリブレーションしておくことで２つの装置の絶対位置姿勢を互いに変換できるためである。また、センサによって取得される計測情報は、基本的にはセンサから周囲の環境における物体までの相対的な位置関係（距離）を示す情報である。センサまたは移動体の絶対位置姿勢は、計測情報と予め用意された環境の三次元位置情報を示す地図情報とから推定可能であるものとする。つまり、計測情報が示すセンサ（移動体）の位置と、計測情報から推定されるセンサ（移動体）の位置は基本的には同じ位置を示すものとする。

以降、本実施形態の詳しい説明に移る。図３は、情報処理装置５００のハードウェア構成を示す図である。Ｈ１１はＣＰＵであり、システムバスＨ２０に接続された各種デバイスの制御を行う。Ｈ１２はＲＯＭであり、ＢＩＯＳのプログラムやブートプログラムを記憶する。Ｈ１３はＲＡＭであり、ＣＰＵであるＨ１１の主記憶装置として使用される。Ｈ１４は外部メモリであり、情報処理装置５００が処理するプログラムを格納する。入力部Ｈ１５は、例えば、キーボードやマウス、タブレット、ロボットコントローラーであり、情報等の入力に係る処理を行う。表示部Ｈ１６は、Ｈ１１からの指示に従って情報処理装置５００の演算結果を表示装置に出力する。なお、表示装置は液晶表示装置やプロジェクタ、ＬＥＤインジケーターなど、種類は問わない。Ｈ１７は通信インターフェイスであり、ネットワークを介して情報通信を行うものであり、通信インターフェイスはイーサネット（登録商標）でもよく、ＵＳＢやシリアル通信、無線通信等種類は問わない。なお、情報処理装置５００は、図２の端末（３１０）とは無線または優先で通信する。Ｈ１７はＩ／Ｏであり、センサＨ１８から画像を入力する。なお、情報処理装置５００は、移動ロボット（４００）に搭載された装置であってもうよいし、ネットワークを介したサーバとして移動ロボットを遠隔制御する装置であってもよい。

次に、本実施形態のモジュール構成について説明する。図４において、３２０は情報を出力する出力装置、３３０は入力装置、２１０は２次元画像や３次元点群の情報を取得するセンサを示す。情報処理装置５００は、計測情報取得部５１０、計測情報保持部５２０、計測情報検索部５３０、出力部５４０、入力受付部５５０、対応決定部５６０、位置姿勢補正部５７０からなる。

センサ２１０は、受光センサを有する計測装置であって、ＳＬＡＭ等の三次元計測に用いる計測情報として環境の形状を計測するセンサである。具体的には撮像装置（カメラ）である。センサ２１０は計測情報としてセンサの位置及び姿勢に関する情報を計測できる。ここでは、移動ロボットに搭載された撮像装置が、周囲の物体を撮像することで画像情報を取得し、情報処理装置５００に出力する。ただし、計測情報は、ＲＧＢカラー画像、グレイスケール画像、白黒画像、奥行き画像など、いかなる種類の画像でもよい。ただし、センサ２１０は、カラーカメラのほかモノクロカメラでも良い。また、ＬｉｄａｒやＴｏＦを代表とするアクティブ距離センサ、グレースケールカメラや、赤外線カメラや、広角レンズカメラや、パノラマカメラであっても良い。パン、チルト、ズームが可能なカメラであっても良い。センサ２１０は、具体的にここでは２台の撮像装置によって撮像されたステレオ画像によって周囲の環境の距離情報（３次元位置情報）を計測する。なお、２つのカメラの位置関係はキャリブレーション済みであるものとする。

出力装置３２０は、例えば、情報を表示する表示装置（画面に２次元情報を映し出すモニタ）である。出力装置３２０は、情報処理装置５００の使用者に振動や触覚などを提示するようにしてもよい。例えば、標識等の画像、計測情報や環境のマップを投影する投影装置（プロジェクタ）や、所定の音によってユーザに通知する音声出力装置（スピーカー）等でも良い。つまり、情報処理装置５００によって出力されたループクローズの対象となる計測情報の候補をユーザに所定の方法で通知することによって、ユーザは情報処理装置５００がループクローズ処理を行うことを認識しやすい。

入力装置３３０は、ユーザによって所定の情報を入力するための装置である。例えば、図２における端末（３１０）であり、モニタの画面に備わったタッチパネルを有する装置である。ユーザは入力装置３３０を用いて、出力装置３２０に出力された第２の計測情報のうち、第１の計測情報と対応付けてループクローズ処理を行う対象としてふさわしい注目計測情報を選択できる。ユーザによって選択された第２の計測情報は、入力装置３３０にユーザ入力情報として受け付けられ、情報処理装置５００に入力される。なお、入力装置５００は、ユーザによって計測情報を指定できるものであればなんでも良い。例えば、音声入力装置（ボイスレコーダー等）が「〇〇番目の計測情報にする」といった音声による指示をユーザ入力情報として受け付けても良い。

計測情報取得部５１０は、移動体が走行する閉経路において、第１の時点にセンサによって計測された第１の計測情報を取得する。すなわち、センサ２１０によって環境の形状を計測された計測情報を取得する。計測情報とは、カメラで取得した２次元の画像情報と、センサの位置姿勢の数値情報（位置を表す数値と姿勢を表す数値）との両方を含む。センサの位置姿勢は画像からＳＬＡＭにより推定するものとする。具体的なＳＬＡＭの計算方法は例えば非特許文献１に記載の方法が利用できる。

計測情報保持部５２０は、センサを動かして環境の形状を計測した計測情報を保持する。つまり、移動ロボットが走行中の任意の時点（第１の時点）より前の取得された計測情報を保持する。計測情報検索部５３０は、計測情報取得部５１０で取得したある視点の計測情報について、類似する計測情報を計測情報保持部５２０で保持する計測情報の中から検索する。すなわち、第１の時点より前の取得された計測情報から第１の計測情報と類似する第２の計測情報を検索する。出力部５４０は、計測情報検索部５３０で検索した計測情報を、出力装置３２０に出力する。すなわち、第１の時点より前の取得された計測情報から第１の計測情報と類似する第２の計測情報を出力する。入力受付部５５０は、入力装置３３０を使って入力された情報を受け付ける。対応決定部５６０は、入力受付部５６０が受け付けた情報に従って、位置と姿勢が近接する２つの計測情報の対応を決定する。すなわち、ユーザによって選択された第２の計測情報を、ループクローズ処理に用いる注目計測情報として決定する。位置姿勢補正部５７０は、対応決定部５６０で決定した計測情報の対応に基づいて、センサの位置姿勢を補正する。すなわち、決定された注目計測情報と第１の計測情報とに基づいて、それまでの計測装置から推定されたセンサまたは移動体の位置のうち少なくとも一部を更新する。

次に、本実施形態の処理手順について説明する。図５は、処理手順を示すフローチャートである。図５のフローチャートに示した処理は、コンピュータである図３のＣＰＵＨ１１により外部メモリＨ１４に格納されているコンピュータプログラムに従って実行される。以下の説明では、各工程（ステップ）について先頭にＳを付けて表記することで、工程（ステップ）の表記を省略する。ただし、情報処理装置５００はこのフローチャートで説明するすべてのステップを必ずしも行わなくても良い。例えば、一部の工程を外部装置が行っても良い。

（Ｓ６００）情報処理装置５００が、プログラムを起動し、必要なパラメータを読み込む等の初期化を行う。ここで、現在時点までにセンサを動かしながら取得した計測情報は、計測情報保持部５２０にロードされているものとする。

（Ｓ６１０）計測情報取得部５１０が、第１の時点（現在）にセンサによって計測された第１の計測情報を取得する。具体的には、第１の時点にカメラで取得した２次元の画像情報を計測情報とする。または、センサの相対的な位置姿勢の数値情報を計測情報として取得する。カメラとセンサはそれぞれ環境の形状を計測する。なお、ここでは、カメラまたはセンサが周囲環境を計測した最新の時点を第１の時点とする。

（Ｓ６２０）計測情報検索部５３０が、第１の時点より前に取得された計測情報から第１の計測情報と類似する第２の計測情報を出力する。具体的には、現在より前に計測された撮像画像と、第１の時点で取得された第１の計測情報（画像）と、の類似度が所定閾値以上である場合に、その撮像画像を第２の計測情報とする。現在より前の時点に撮像した画像は計測情報保持部５２０に保持されている。つまり、現在（第１の時点）の視点（例として、図６の位置姿勢Ｂの視点）において計測情報取得部５１０で取得した計測情報Ｘについて、類似する計測情報を、計測情報保持部５２０で保持する計測情報の中から検索する。つまり、ループクローズ処理を実施する対象となる注目計測情報の候補を検索する。

本実施形態では、画像の類似度が高い計測情報を検索する。画像の類似度を計算する方法として、例えば、ＢｏＷ（ＢａｇｏｆＷｏｒｄｓ）に基づく手法が利用できる。ＢｏＷでは画像から特徴ベクトルを抽出し、その特徴ベクトルの類似度を、画像の類似度として計算する。具体的に、Ｂの視点における計測情報に含まれる画像と、計測情報保持部５２０で保持する計測情報に含まれる画像のそれぞれから特徴ベクトルを抽出する。そして、そのベクトルの類似度が所定値以上となる計測情報を第２の計測情報として取得する。なお、ＢｏＷについての詳細は非特許文献１で公知である。特徴ベクトルの類似度は、ベクトルの内積により計算できる。

（Ｓ６３０）計測情報検索部５３０が、第１の計測情報との類似度が所定値以上である第２の計測情報が少なくとも１つ以上出力された場合は、Ｓ６４０に進む。第１の計測情報との類似度が所定値以上である第２の計測情報を出力しなかった場合は、第１の計測情報を保持部に記憶して、Ｓ６１０に戻る。なお、本処理結果である第２の計測情報群をＹ、各計測情報をＹｉ（ｉ＝１～Ｎ）とする。Ｎは検索して見つかった第２の計測情報の個数である。ｉは検索して見つかった計測情報の番号である。

（Ｓ６４０）出力部５４０が、第１の時点より前に取得された計測情報から第１の計測情報と類似する第２の計測情報を所定の方法によって出力する。ここでは、計測情報検索部５３０で検索した第２の計測情報Ｙｉ（ｉ＝１～Ｎ）を、出力装置３２０を使って出力する。具体的には、各計測情報Ｙｉに属する画像Ｉｉを並べてモニタ上に表示して、ユーザはそれを見て同一の視点かどうかの判断ができるようにする。なお、計測情報は、無線通信等を利用して出力装置３２０に出力できるものとする。また、ユーザが用いる端末３１０に出力して、ＧＵＩで表示してもよい。

（Ｓ６５０）入力受付部５５０が、出力された第２の計測情報からユーザ入力によって選択された計測情報を注目計測情報として受け付ける。ユーザは入力装置３３０を使って入力し、情報処理装置は入力された情報を受け付ける。具体的にはモニタに備わったタッチパネルの機能を利用し、ユーザが画面をタッチすることで、計測情報を選択できるようにし、その入力情報を読み取る。ここで選択した計測情報の番号をｋとする。移動ロボットが走行する環境には似たようなシーンが点在することがある。例えば、同じ形状の什器が並んでいる環境や、ブラインドや無地の壁がずっと続くような環境である。また、レイアウト変更によってある物体が違う位置に移動したが、移動した先の見え方が同じである場合は、ループクローズ処理に失敗する可能性がある。そのため、ループクローズ処理の対象となる計測情報をユーザによって対応付けることで、視点の対応付けを間違いやすい環境においてもループクローズ処理を失敗する可能性が抑制できる。

（Ｓ６６０）対応決定部５６０が、受け付けた注目計測情報と、第１の計測情報と、を対応付ける。つまり、第１の時点における移動体の位置が第２の計測情報（注目計測情報）が示す位置と同じ位置かどうかをユーザの入力によって決定する。具体的には、計測情報Ｘと、選択された注目計測情報Ｙｋとを対応として出力する。

（Ｓ６７０）位置姿勢補正部５７が、前記第１の時点より前に計測された計測情報から推定される前記移動体の位置の一部を更新する。言い換えれば、第１の計測情報Ｘと対応決定部５６０で決定した注目計測情報Ｙｋに基づいて各計測情報から推定された移動体の位置が整合する（位置の差分が所定の閾値以下になる）ようにセンサの位置（軌跡）を更新する。これによってセンサの位置姿勢を高精度に推定できる。具体的には。２つの計測情報に含まれる位置姿勢の差を小さくするように、計測情報Ｘと計測情報Ｙｋのそれぞれから推定されるセンサ（移動体）の位置姿勢を補正する。なお、対応付けた計測情報からそれまでのセンサの位置姿勢の推定結果を補正する処理はループクローズと呼ばれている。ループクローズの処理内容については、非特許文献１において公知である。また、ＸとＹｋの位置姿勢の補正に応じて、その間にある視点間の位置姿勢の整合性を保つように、各視点の位置姿勢推定結果に含まれる位置姿勢を補正してもよい。

なお、Ｓ６４０において、出力部５０４によって出力された計測情報をＧＵＩに表示する例について図６を使って説明する。出力部５４０における計測情報の出力方法としては、物理的に近い視点かどうかをユーザが判断しやすい出力方法が望ましい。その例として、図６に示すように、計測情報の画像とは別に、予め設定された地図情報（マップ）や位置姿勢の軌跡の情報、その軌跡上で各計測情報がどの位置に相当するかを表示する。図５の３１１は現在時点における計測情報Ｘに含まれる画像である。図６の３１２、３１３に示す複数の図は、検索した計測情報Ｙｉに含まれる画像である。３１４はマップの俯瞰図、３１５は検索した計測情報Ｙｉから推定されるセンサ（移動体）の位置姿勢をマップ上で示す矢印である。３１５の矢印を表示すると各計測情報の位置関係が分かるので、その位置関係を参考に正しい対応かどうかを判断しやすくなる。例えば図６では、現在の視点Ｂ（１１２）における計測情報の対応として２つ候補があるが、３１３の方はＢの視点から大きく離れた視点にあることが矢印の示す位置から分かる。そのため、それが正しい対応ではないことを判断しやすい。

これ以外の例として、計測情報に含まれる画像を表示する場合、計測情報の類似箇所或いは非類似箇所を表示することで、ユーザにとって視点の同一性を判断しやすくしても良い。具体的には、検索する基準である計測情報Ｘに含まれる画像と、検索した計測情報Ｙｉに含まれる画像との特徴の対応点を推定する。対応点が多い個所を類似箇所、対応が少ない箇所を非類似箇所として判定し、その程度に応じて画像の各個所に色を付けるなどの方法で可視化する。このように画像上で類似・非類似の個所が可視化されていると、ユーザが各画像を見て正誤を判断するための材料とすることができる。

また、出力した候補の中に正しい対応がない場合、いずれの候補とも対応付けないことを入力するため、図６の３１６に示すように「キャンセル」ボタンなどを画面に表示し、対応を決定しないことを明示的に入力できるようにしてもよい。このとき、移動体の位置の推定結果は更新されず、引き続き移動体は走行しながら環境の形状を計測する。もしくは、ループクローズできる地点が見付からなかった場合は、ユーザによって走行制御を指示してもよい。また、図６の２００は障害物の形状を表す。障害物の形状は、例えば、不図示のＳＬＡＭモジュールで推定したときのマップに含まれる３次元の情報から可視化できる。

以上述べたように、本実施形態では、ループクローズ処理が可能な位置にセンサが戻ってきたことを検知することで、より確実にセンサの位置姿勢の精度を向上できるようにする。

＜第２の実施形態＞
第１の実施形態の計測情報検索部５３０では、画像の類似度に基づいて、計測情報を検索する。この検索の処理において、さらに、計測情報に含まれる相対的な位置姿勢の情報を利用して検索の精度を高めることで、ユーザに通知する計測情報に間違った候補が含まれにくくなり、ユーザの負担を減らすことができる。以下に、その具体的な方法を説明する。なお、本実施形態において、計測情報はセンサまたは移動体の相対的な位置姿勢を示す情報である。センサまたは移動体の絶対的な位置姿勢（例えば、地図における三次元位置）についてはセンサまたは移動体の（絶対）位置と記載する。なお、移動体は移動ロボットのことである。センサの位置から移動体の位置を推定できるようにセンサと移動体との相対的な位置関係はキャリブレーション済みであるものとする。

第２の実施形態におけるハードウェア構成は、第１の実施形態と同様に図３に示されるハードウェアから構成される。第２の実施形態における情報処理装置５００の機能構成例は、第１の実施形態と同世に図４の機能構成を用いる。また、第２の実施形態において、情報処理装置が実行する処理は、第１の実施形態と同様に図５のフローチャートで説明されるが、第１の実施形態のＳ６２０とは処理が異なる。ここでは、第１の実施形態と同様の処理については説明を省略し、Ｓ６２０について詳細に説明する。

（Ｓ６２０）
計測情報検索部５３０で、ある計測情報Ｘについて物理的に近い（相対的な位置関係が近い）計測情報を検索する。まず、計測情報Ｘに含まれる移動体の相対的な位置姿勢をＰとする。ここで、移動体の位置姿勢Ｐはドリフトによる誤差を含むため、必ずしも正確な値ではない。ただし、そのドリフトの誤差がある範囲内に収まっていると考えれば、その範囲に基づいて、検索の候補を絞ることができる。

具体的には、計測情報Ｘの位置姿勢Ｐを中心に所定値Ｔより離れた位置姿勢を持つ計測情報は検索の対象から除外する。範囲を限定して検索を行うことで、検索の間違いを減らすことができる。そして、位置姿勢Ｐから所定値Ｔ以内にある計測情報の中から、第１の実施形態で述べたように画像の類似度に基づいて検索を行う。以降の処理は、第１の実施形態と同じである。なお、Ｔは位置と姿勢それぞれについて範囲を限定するベクトルである。例えば、位置１ｍ、姿勢１０度のような値を持ち、その範囲内で検索を行う。

ここで、範囲を限定するときに利用する値Ｔは、固定値でも良いし、以下に説明するようにドリフトによる誤差を予想して可変的に設定しても良い。ドリフトによる誤差はセンサの移動量に応じて増加する傾向がある。その傾向を利用し、センサの移動量をＬとしたときに、Ｔ＝ａ・Ｌとして閾値Ｔを調整できる。ここでａについては、あらかじめ設定する固定値とする。Ｌについては、計測情報Ｘの視点にセンサが移動するまでに推定した各時点の計測情報から計算した移動量である。

以上の処理により、計測情報に含まれる位置姿勢を利用して、検索範囲を限定することで、ユーザに通知する検索結果に間違った候補が含まれにくくなり、ユーザの負担を減らすことができる。

＜第１の変形例＞
第１の実施形態において、位置姿勢補正部５７０は、対応決定部５６０で決定した計測情報の対応に基づいてセンサの位置姿勢推定結果の一部を補正する。この補正処理はループクローズと呼ばれている。

これ以外にも、対応決定部５６０で決定した対応は、リローカライズと呼ばれる機能にも利用できる。リローカライズとは、例えばＳＬＡＭにおいて現在の位置姿勢が不明になったときの機能で、センサの位置姿勢を再度推定する処理である。そのためには、現在の計測情報（例えば画像）と、これまでに取得した計測情報とを照合し、同一の視点を対応付ける。これによって、その対応付けた視点の位置姿勢を参考に、現在の位置姿勢を計算することができる。リローカライズの詳しい計算は、非特許文献１において公知である。ここで、計測情報の対応を計算する処理として、本実施形態の手順が利用できる。

＜第２の変形例＞
計測情報検索部５３０にいて、計測情報を検索する方法は、類似する計測情報を検索できる方法であれば、どのような方法でも良い。第１の実施形態のように計測情報に含まれる画像の類似度に基づいて検索してもよいし、別途用意した３次元センサを利用して取得した距離画像や３次元点群のデータの類似度に基づいて検索してもよい。計測情報に含まれる位置姿勢の類似度に基づいて検索してもよい。例えば、センサの位置姿勢を利用する場合、位置姿勢の６自由度の成分の類似度に基づいて、視点の同一性を判断する。また、画像の類似度、位置姿勢の類似度、距離画像や３次元点群データの一致度を所定の比率で和を計算して類似度を計算しても良い。

また、一般的に動画像では、現在の時点の画像と、その時間的に近傍で取得した画像は、ほぼ同一視点といえるが、この視点間ではドリフトの程度も少ないためループクローズの効果は薄い。そのため、計測情報検索部５３０で検索する計測情報は、例えば計測を開始した時点から所定期間内で計測された計測情報は、検索対象から除外しても良い。逆に、最初にセンサで計測を開始してから所定時間後の計測情報から第２の計測情報を探索するようにしてもよい。また、移動ロボットの経路上に第２の計測情報の検索対象となる範囲を設定してもよい。さらに、センサ（移動ロボット）が閉経路を移動する場合、途中の経路はループクローズ処理の対象にはならない。そのため、計測を開始した地点の付近に戻ってきてから計測された計測情報を検索対象範囲とすることで効率的にループクローズ処理が行える。すなわち、計測を開始した位置から所定の範囲内にセンサが位置するときに計測された計測情報を検索対象範囲とする。この位置の条件と時間の条件とを組み合わせることによって、より限定的な範囲に検索対象を絞ることが出来、効率的にループクローズ処理が可能になる。つまり、一部の計測情報を対象として探索処理を行うため効率よくループクローズが可能な点を抽出できる。

＜第３の変形例＞
出力部５４０において、表示装置に複数の計測情報を出力する場合、優先度の高い順から表示することにより、ユーザが優先度順に確認しやすくしても良い。優先度の設定方法としては、計測情報検索部５３０で計測情報を検索した時に利用した情報の類似度、すなわち画像や位置姿勢の類似度に応じて、類似度の高い候補ほど順序を上げて表示する。また、さらに各計測情報について、類似度の数値の情報を表示してもよい。その場合、類似度の数値は画面上の各計測情報との対応が分かるように表示する。

＜第４の変形例＞
本実施形態において、計算した位置姿勢の情報は、移動ロボットを利用した自律移動のシステムに利用可能である。その場合、図６に示すように、制御装置７００はセンサを有する移動ロボットを所定の経路を走行するように制御する。図７の情報処理システムは、情報処理装置５００が出力する計測情報から推定される移動ロボットの位置姿勢を参照して、制御装置７００によって移動ロボットを制御する。

自律移動において移動ロボットは、予め走行ルート上で取得した計測情報に含まれる画像との特徴点の対応付けを行い、その画像を撮影したセンサに対しての相対的な位置姿勢を推定する。つまり、走行ルート上における計測情報から推定された移動ロボットの位置姿勢が基準となって、移動ロボットの位置姿勢が決まる。本実施形態において、走行ルート上の計測情報の位置姿勢の精度を向上できるため、それを基準として算出した移動ロボットの位置姿勢の精度についても向上させることができる。また、ループクローズ処理が可能な場合、すなわち第２の計測情報が出力された場合に、移動ロボットの移動を抑制し、ループクローズの対象となる計測情報をより安定的な状況で計測するようにしてもよい。このようにすることによって、ループクローズ処理に必要な情報を十分に計測し、センサの位置姿勢推定の精度が向上する。

＜その他の実施例＞
本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。また、そのプログラムをコンピュータ読み取り可能な記録媒体に記録して提供してもよい。

なお、上述した各処理部のうち、位置補正部等については、その代わりとして、機械学習された学習済みモデルを代わりに用いて処理しても良い。その場合には、例えば、その処理部への入力データと出力データとの組合せを学習データとして複数個準備し、それらから機械学習によって知識を獲得し、獲得した知識に基づいて入力データに対する出力データを結果として出力する学習済みモデルを生成する。学習済みモデルは、例えばニューラルネットワークモデルで構成可能である。そして、その学習済みモデルは、前記処理部と同等の処理をするためのプログラムとして、ＣＰＵあるいはＧＰＵなどと協働で動作することにより、前記処理部の処理を行う。なお、上記学習済みモデルは、必要に応じて一定の処理後に更新しても良い。

４００移動ロボット
４１０センサ
５００情報処理装置
７００制御装置

Claims

移動体が有するセンサによって環境を計測した計測情報に基づいて該移動体の位置を推定する情報処理装置であって、
前記移動体が走行する経路において、前記センサが取得する画像と前記移動体の位置姿勢の情報とを含む計測情報を取得する取得手段と、
第１の時点で取得した前記計測情報である第１の計測情報と類似する計測情報を持つ、第１の時点より前の第２の時点で取得した前記計測情報である第２の計測情報を出力する出力手段と、
前記第１の時点における前記移動体の位置が前記第２の計測情報が示す位置と同じ位置か否かをユーザの入力によって決定する決定手段と、を有すること
を特徴とする情報処理装置。
前記出力手段は、前記第１の時点より前に取得された計測情報のうち、前記第１の計測情報に含まれる画像と位置姿勢の情報との類似度が所定の閾値以上である、前記第２の計測情報を出力すること
を特徴とする請求項１に記載の情報処理装置。
前記出力手段は、前記第２の計測情報によって推定された位置を表示装置に出力することを特徴とする請求項１または２に記載の情報処理装置。
前記出力手段は、前記第１の計測情報と前記第２の計測情報との類似度を前記表示装置に出力することを特徴とする請求項３に記載の情報処理装置。
前記出力手段は、前記第１の計測情報と前記第２の計測情報との類似度の大きさの順に前記第２の計測情報を並べて前記表示装置に出力することを特徴とする請求項４に記載の情報処理装置。
前記出力手段は、前記第２の計測情報を投影装置に出力することを特徴とする請求項１乃至５のいずれか１項に記載の情報処理装置。
前記出力手段は、前記第１の計測情報と前記第２の計測情報との類似度が所定の閾値より大きい場合に、所定の音を音声出力装置に出力することを特徴とする請求項１乃至６のいずれか１項に記載の情報処理装置。
前記出力手段は、前記第１の時点より前に取得された計測情報のうち、前記第１の計測情報とループクローズ処理を実施する対象となる計測情報から前記第２の計測情報を出力すること
を特徴とする請求項１乃至７のいずれか１項に記載の情報処理装置。
前記出力手段は、前記第１の時点から所定時間より前に計測された計測情報から前記第２の計測情報を出力することを特徴とする請求項１乃至８のいずれか１項に記載の情報処理装置。
前記出力手段は、前記第１の計測情報が計測された位置から所定の範囲内にある計測情報から前記第２の計測情報を出力することを特徴とする請求項１乃至９のいずれか１項に記載の情報処理装置。
前記出力手段は、前記第１の時点より前に取得された計測情報のうち、予め設定された第２の時点より前に計測された計測情報から前記第２の計測情報を出力することを特徴とする請求項１乃至１０のいずれか１項に記載の情報処理装置。
前記決定手段によって前記第１の時点における前記移動体の位置と前記第２の計測情報が示す位置とが同じ地点であると決定された場合、前記第１の計測情報と前記第２の計測情報とに基づいて、前記第１の時点より前に計測された計測情報から推定される前記移動体の位置の一部を更新する更新手段を更に有することを特徴とする請求項１乃至１１のいずれか１項に記載の情報処理装置。
前記決定手段によって前記第１の時点における前記移動体の位置と前記第２の計測情報が示す位置とが同じ地点ではないと決定された場合、前記更新手段は、前記第１の時点より前に計測された計測情報から推定される前記移動体の位置を更新しないこと
を特徴とする請求項１２に記載の情報処理装置。
前記出力手段によって出力された前記第２の計測情報が複数ある場合、前記決定手段は、前記出力手段によって出力された複数の前記第２の計測情報のうち、前記第１の時点における前記移動体の位置と同じ位置で計測された注目計測情報をユーザの入力によって決定し、
前記決定された前記注目計測情報と前記第１の計測情報とに基づいて、前記第１の時点より前に計測された計測情報から推定される前記移動体の位置の一部を更新する更新手段を更に有することを特徴とする請求項１乃至１１のいずれか１項に記載の情報処理装置。
前記移動体が走行する前記経路に対応付けた地図情報に基づいて、前記第２の時点における前記センサの位置を推定する推定手段をさらに有し、
前記出力手段は、前記推定手段によって推定された前記第２の時点における前記センサの位置を、前記地図情報の対応する位置に出力すること
を特徴とする請求項１乃至１４のいずれか１項に記載の情報処理装置。
前記センサを有する移動体が前記経路を走行するように制御する制御手段を更に有することを特徴とする請求項１乃至１５のいずれか１項に記載の情報処理装置。
前記制御手段は、前記出力手段によって前記第２の計測情報が少なくとも１つ出力された場合、前記移動体の移動を抑制することを特徴とする請求項１６に記載の情報処理装置。
コンピュータを、請求項１乃至１７のいずれか１項に記載の情報処理装置が有する各手段として機能させるためのプログラム。
移動体が有するセンサによって環境を計測した計測情報に基づいて該移動体の位置をＣＰＵが推定する情報処理方法であって、
前記移動体が走行する経路において、前記センサが取得する画像と前記移動体の位置姿勢の情報とを含む計測情報を取得する取得工程と、
第１の時点で取得した前記計測情報である第１の計測情報と類似する計測情報を持つ、第１の時点より前の第２の時点で取得した前記計測情報である第２の計測情報を出力する出力工程と、
前記第１の時点における前記移動体の位置が前記第２の計測情報が示す位置と同じ位置か否かをユーザの入力によって決定する決定工程と、を有すること
を特徴とする情報処理方法。