JP2023159484A - 情報処理装置、情報処理システム、情報処理装置の制御方法およびプログラム - Google Patents

Abstract

【課題】移動体の位置姿勢計測に用いる地図の精度を向上させる。
【解決手段】情報処理装置１００は、移動体の位置姿勢を計測するための地図を取得する地図取得部１０１と、前記地図に対応する前記移動体が移動する実空間または前記実空間内の物体に対応する図面情報から視覚的指標を検出する検出部１０４と、前記地図と前記図面情報の座標系の位置合わせを行う位置合わせ部１０３と、前記位置合わせ後の前記図面情報に含まれる前記視覚的指標と前記地図に含まれる構成要素の対応付けを行う対応付け部１０５と、前記対応付けの結果に基づいて前記地図を補正する補正部１０６と、を有する。
【選択図】図１

Description

本発明は、移動体の移動制御に用いる３次元地図に関する。

無人搬送車（ＡＧＶ（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｖｅｈｉｃｌｅ））などの工場や物流倉庫といった環境内において自動で移動させる移動体が普及している。移動体は、実空間の地図作成と実空間内での自己位置姿勢計測を行いながら自動で移動（自律走行）する。地図作成および自己位置姿勢計測の方法としては、例えばＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）法が知られている。

また、移動体を自律走行させるためには、事前に移動体が走行する経路を設定する必要がある。経路設定方法としては、現場の環境を示すＣＡＤデータを用いて、ＣＡＤデータ上で移動体の走行経路の経由点を設定する方法がある。そして、ＣＡＤデータ上の経由点をＳＬＡＭで作成した地図上の経由点へと座標変換し、移動体に走行経路の経由点としてＳＬＡＭ地図上の座標を渡す。しかし、ＳＬＡＭで作成する地図には地図作成時の蓄積誤差が含まれるため、ＳＬＡＭで作成する地図とＣＡＤデータは完全には一致しない。位置ずれを修正するために、特許文献１は、図面から作成したＳＬＡＭ地図とデプスセンサで作成した実環境データからスケール調整パラメータを推定し、該パラメータを用いてＳＬＡＭ地図のスケールを調整することで実空間との整合をとる技術を開示している。

特許第６８９３２００号公報

しかしながら、特許文献１に示されるスケール調整を行うだけでは実空間を示すＣＡＤデータと移動体の位置姿勢計測に用いるＳＬＡＭ地図全体の整合を取ることは難しく、実空間との誤差が残ってしまう。

本発明は、移動体の位置姿勢計測に用いる地図の精度を向上させることを目的とする。

上記課題を解決するために、本発明の情報処理装置は、移動体の位置姿勢を計測するための地図を取得する地図取得手段と、前記地図に対応する前記移動体が移動する実空間または前記実空間内の物体に対応する図面情報から視覚的指標を検出する検出手段と、前記地図と前記図面情報の座標系の位置合わせを行う位置合わせ手段と、前記位置合わせ後の前記図面情報に含まれる前記視覚的指標と前記地図に含まれる構成要素の対応付けを行う対応付け手段と、前記対応付けの結果に基づいて前記地図を補正する補正手段と、を有する。

本発明によれば、移動体の位置姿勢計測に用いる地図の精度を向上させることができる。

情報処理装置の機能構成例を示す図である。 情報処理装置のハードウェア構成の一例を示す図である。 地図の補正処理を示すフローチャートである。 座標系の位置合わせを説明する図である。 視覚的指標の検出を説明する図である。 視覚的指標の検出を説明する図である。 ＣＡＤデータの視覚的指標とＳＬＡＭ地図の特徴点の対応付けを説明する図である。 地図の補正処理におけるＧＵＩの一例を示す図である。 情報処理装置が実行する処理を示すフローチャートである。

（第１実施形態）
図１は、本実施形態における情報処理装置１００を備える情報処理システム１０の構成を示す図である。情報処理システム１０は、ＡＧＶなどの移動体の走行を制御する移動体システムである。なお、移動体は車両に限られず、自動２輪車、電動カート、ロボット（自律移動ロボット（Ａｕｔｏｎｏｍｏｕｓ Ｍｏｂｉｌｅ Ｒｏｂｏｔ））、ドローンなどの移動をする機器であればどのようなものであっても良い。情報処理システム１０は、不図示の位置姿勢計測用のカメラで撮影した周辺環境の撮影画像に基づいて位置姿勢計測のための地図を生成し、該地図に基づいて移動体の移動（自律走行）を制御する。本実施形態では、情報処理システム１０がＶｉｓｕａｌ ＳＬＡＭで移動体の移動を制御する例を説明する。

情報処理システム１０は、情報処理装置１００、表示データ生成部１０７、表示部１０８を有する。情報処理装置１００は、移動体の位置姿勢計測のための地図を生成する。本実施形態では、Ｖｉｓｕａｌ ＳＬＡＭで移動体の位置姿勢計測を行うために利用する地図のことをＳＬＡＭ地図と呼ぶ。ＳＬＡＭ地図は、移動体の位置姿勢計測に用いられる３次元地図であり、移動体が備えるカメラなどのセンサにより計測された計測結果に基づいて生成される。ＳＬＡＭ地図には複数のキーフレーム情報が含まれる。キーフレームは地図作成時のカメラの撮影画像を含み、カメラの位置および姿勢が対応付けられている。さらに、ＳＬＡＭ地図はキーフレームの他に、撮影画像から検出した特徴点の３次元座標のデータも含まれる。

移動体の経路設定時には、移動体が移動する実空間である現場環境を示すＣＡＤデータ等の図面情報上で座標を指定する。そして、ＣＡＤデータの座標をＳＬＡＭ地図の座標へと変換して、走行経路の経由点を設定する。しかし、Ｖｉｓｕａｌ ＳＬＡＭで作成したＳＬＡＭ地図は作成時のスケール誤差やループクローズ時の歪みなどによる蓄積誤差により局所的に歪む場合がある。そのため、ＣＡＤデータとＳＬＡＭ地図には誤差が生じ、ＣＡＤデータ上の座標から相似変換のような単純な座標変換をしてもＳＬＡＭ地図上の座標とは完全には一致しない。そこで、本実施形態ではＣＡＤデータから視覚的指標を検出し、ＳＬＡＭ地図に含まれる特徴点の３次元座標を視覚的指標の３次元座標に基づき修正する。視覚的指標となるのは、例えば、配管、ダクト、スプリンクラー、空調器具、照明器具などカメラで計測可能な構造である。本実施形態では図面情報として、現場環境において配管、ダクト、照明器具といった設備のレイアウト設計に用いられる３次元のＣＡＤデータを用いる例について説明する。

情報処理装置１００は、地図取得部１０１、視覚的指標情報取得部１０２、位置合わせ部１０３、検出部１０４、対応付け部１０５、補正部１０６を有する。地図取得部１０１は、Ｖｉｓｕａｌ ＳＬＡＭで作成したＳＬＡＭ地図を取得する。そして、地図取得部１０１は、取得したＳＬＡＭ地図を位置合わせ部１０３、検出部１０４、対応付け部１０５および補正部１０６に出力する。視覚的指標情報取得部１０２は、視覚的指標の検出対象としてＣＡＤデータを取得し、位置合わせ部１０３に出力する。

位置合わせ部１０３は、視覚的指標情報取得部１０２が取得したＣＡＤデータ上の座標系と地図取得部１０１が取得したＳＬＡＭ地図上の座標系との間の相対位置関係を算出する。また位置合わせ部１０３は、相対位置関係に基づきＣＡＤデータの基準とする座標系をＳＬＡＭ地図の座標系へと変換して検出部１０４に出力する。なお、本実施形態においては、ＣＡＤデータとＳＬＡＭ地図の座標系をＳＬＡＭ地図の座標系に統一しているが、ＣＡＤデータの座標系に統一して以降の処理を行ってもよい。

検出部１０４は、地図取得部１０１が出力したＳＬＡＭ地図に基づいて、位置合わせ部１０３が出力した座標系変換済みのＣＡＤデータから視覚的指標を検出し、検出結果を対応付け部１０５に出力する。対応付け部１０５は、検出部１０４が出力した視覚的指標を地図取得部１０１が取得したＳＬＡＭ地図に含まれる特徴点と関連付け、３次元座標のペアを作成する。そして、対応付け部１０５は、３次元座標のペアを対応付け結果として補正部１０６に出力する。補正部１０６は、対応付け部１０５によってＳＬＡＭ地図上の特徴点と関連付けられた視覚的指標の３次元座標に基づいて、地図取得部１０１が取得したＳＬＡＭ地図に含まれる特徴点の３次元座標を補正する。そして、補正部１０６は、特徴点の３次元座標が補正されたＳＬＡＭ地図を表示データ生成部１０７に出力する。

表示データ生成部１０７および表示部１０８は、地図を補正するための一連の処理（位置合わせ部１０３による位置合わせ、検出部１０４および対応付け部１０５による対応付け、補正部１０６による補正）の過程および結果の表示を制御する。表示データ生成部１０７は、位置合わせ部１０３および対応付け部１０５、補正部１０６の出力情報を可視化するためのデータを生成し、生成したデータを表示部１０８に出力する。表示部１０８は、表示データ生成部１０７により生成されたデータを受け取り、表示装置に出力する。

図２は、情報処理装置１００のハードウェア構成を示す図である。情報処理装置１００は、ＣＰＵ２１１、ＲＯＭ２１２、ＲＡＭ２１３、外部メモリ２１４、入力部２１５、表示制御部２１６、通信Ｉ／Ｆ２１７と、これらを互いに接続するシステムバス２２０を有する。ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）２１１は、システムバス２２０に接続された各種デバイスの制御を行う。ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）２１２は、ＢＩＯＳ（Ｂａｓｉｃ Ｉｎｐｕｔ／Ｏｕｔｐｕｔ Ｓｙｓｔｅｍ）のプログラムやブートプログラムなど情報処理装置１００を制御するためのプログラムを記憶する。外部メモリ２１４は、情報処理装置１００が処理するアプリケーションやプログラム、各種データやファイル等を格納する。外部メモリ２１４は、例えば、ハードディスク（ＨＤ）やソリッドステートドライブ（ＳＳＤ）等のメモリである。ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）２１３は、ＣＰＵ２１１の主記憶装置として使用される。また、ＲＡＭ２１３は、ワークエリアとしても機能する。ＣＰＵ２１１は、ＲＯＭ２１２または外部メモリ２１４に記憶されたプログラムをＲＡＭ２１３にロードして実行し、システムバス２２０に接続された各ユニットを総括的に制御する。

入力部２１５は、キーボードやポインティングデバイス、ロボットコントローラ等の入力装置であり、ユーザからの入力を受け付ける。表示制御部２１６は、液晶ディスプレイ等の表示装置を有し、ＣＰＵ２１１からの指令にしたがって情報処理装置１００の処理結果を表示装置に出力する。なお、表示装置は液晶ディスプレイやプロジェクタ、ＬＥＤインジケーター、ＶＲ（仮想現実）表示が可能なヘッドマウントディスプレイなどであってもよい。また、入力部２１５および表示制御部２１６は、タッチパネルで構成されていてもよい。タッチパネルにおける入力座標と表示座標を対応付けることで、あたかもユーザがタッチパネルに表示された画面を直接的に操作可能であるかのようなＧＵＩを構成することができる。通信Ｉ／Ｆ２１７は、ネットワークを介して外部装置と情報通信を行う。通信Ｉ／Ｆ２１７は、イーサネット、ＵＳＢ、シリアル通信、無線通信等種類は問わない。ネットワークは、例えば、ＬＡＮ、ＷＡＮなどの通信ネットワーク、セルラネットワーク（例えば、ＬＴＥや５Ｇなど）、無線ネットワークのいずれか、またはこれらの組み合わせで構成されてもよい。すなわち、ネットワークはデータの送受信が可能に構成されていればよく、物理レイヤの通信方式はいずれの方式を採用してもよい。

図３は、本実施形態における地図の補正処理を示すフローチャートである。図３のフローチャートに示される地図を補正するための各処理は、情報処理装置１００のＣＰＵ２１１が、ＲＯＭ２１２または外部メモリ２１４に格納されているプログラムをＲＡＭ２１３に読み出して実行することにより実現される。

ステップＳ３０１では、情報処理装置１００の初期化を行う。情報処理装置１００の初期化では、ＲＯＭ２１２または外部メモリ２１４からプログラムを読み込み、情報処理装置１００を動作可能な状態にする。また、ＳＬＡＭ地図作成時に用いたカメラと紐づいたカメラパラメータを外部メモリ２１４から読み込み、ＲＡＭ２０３に記憶する。

ステップＳ３０２では、地図取得部１０１がＶｉｓｕａｌ ＳＬＡＭで作成したＳＬＡＭ地図を取得する。そして、地図取得部１０１は取得したＳＬＡＭ地図を、位置合わせ部１０３、検出部１０４、対応付け部１０５、補正部１０６に出力する。ステップＳ３０３では、視覚的指標情報取得部１０２が、ステップＳ３０１で取得したＳＬＡＭ地図の領域に対応する視覚的指標情報を取得する。本実施形態では視覚的指標情報として、視覚的指標を検出する対象データとなるＣＡＤデータを取得する。すなわち、視覚的指標情報取得部１０２は、ＳＬＡＭ地図を作成した現場環境全体のレイアウトを示すＣＡＤデータを取得する。そして、視覚的指標情報取得部１０２は、取得したＣＡＤデータは位置合わせ部１０３に出力する。

ステップＳ３０４では、位置合わせ部１０３が、地図取得部１０１により取得されたＳＬＡＭ地図の座標系と、視覚的指標情報取得部１０２により取得された視覚的指標情報であるＣＡＤデータの座標系との位置合わせを行う。位置合わせ部１０３は、位置合わせとして、ＣＡＤデータの座標系をＳＬＡＭ地図の座標系に変換する処理を行う。座標系の位置合わせの方法として、例えば、実空間に物理的に設置したマーカーを利用して相対位置関係を求め、座標変換する方法がある。ここでマーカーとは、点対称・線対象でない平面図形のような位置および姿勢を規定できるものである。

マーカーを利用して座標系の位置合わせをする方法について図４を用いて説明する。図４は、座標系の位置合わせを説明する図である。図４（Ａ）は、マーカー４００の配置をＳＬＡＭ地図の座標空間上で示す図である。図４（Ａ）において、（ｘ１，ｙ１，ｚ１）がマーカー４００の座標である。図４（Ｂ）は、マーカー４００の配置をＣＡＤデータの座標空間上で示す図である。図４（Ｂ）において、（ｘ２，ｙ２，ｚ２）がマーカー４００の座標である。ＣＡＤデータ上においては３次元座標が既知であるとし、マーカーの位置および姿勢が４×４行列であるＭｃで表される。また、ＳＬＡＭ地図上の３次元座標は、例えば特許第４５２２１４０号公報に開示されるマーカーの配置情報を推定する方法を利用することで求めることができ、ＳＬＡＭ地図上のマーカーの３次元の位置および姿勢を４×４行列であるＭｓとする。

ＣＡＤデータの座標系およびＳＬＡＭ地図の座標系におけるマーカーの位置および姿勢は実空間上では一致していると仮定できるため、ＣＡＤデータ上の任意の座標をＳＬＡＭ地図上の座標へと変換する変換行列をＭｓｃとすると、下記の式（１）で表される。
Ｍｓｃ＝ＭｓＭｃ－１・・・（１）
ここで、Ｍｃ－１はＭｃの逆行列である。位置合わせ部１０３は、変換行列Ｍｓｃを用いてＣＡＤデータの座標系をＳＬＡＭ地図の座標系へと変換することで座標系の位置合わせを行う。図４（Ｃ）は、変換行列ＭｓｃによりＣＡＤデータの座標系をＳＬＡＭ地図の座標系に変換した図である。座標変換後のＣＡＤデータの座標軸は、図４（Ｃ）のＸ’Ｙ’Ｚ’となる。このように、２つの座標系上のマーカーの３次元座標に基づき変換行列Ｍｓｃを求め、変換行列ＭｓｃによりＣＡＤデータの座標系をＳＬＡＭ地図の座標系に変換することができる。位置合わせ部１０３は、座標変換したＣＡＤデータを検出部１０４および表示データ生成部１０７に出力する。なお、ＳＬＡＭ地図とＣＡＤデータの座標系の位置合わせの方法はマーカーを利用した方法に限られるものではなく、他の座標変換の方法を用いてもよい。

ステップＳ３０５では、検出部１０４が位置合わせ部１０３により座標軸を変換済みのＣＡＤデータから視覚的指標を検出する。本実施形態においては、視覚的指標として特徴点を使用する例について説明する。ここで特徴点とは、画像上のエッジの交差やコーナー点のように輝度勾配が閾値よりも大きい画素のことである。ＣＡＤデータから検出される特徴点は、地図取得部１０１が取得するＳＬＡＭ地図の構成要素である特徴点と同じである。

検出部１０４は、例えば、ＳＬＡＭ地図に含まれる任意のキーフレームと同じ視点でＣＡＤデータをＣＧ描画した画像を生成した後に、閾値よりも輝度勾配が大きい画素を検出するための画像処理を行うことで、ＣＡＤデータから特徴点を検出する。具体的には、ＳＬＡＭ地図に含まれる任意のキーフレームを選択し、キーフレームを撮影したカメラの位置および姿勢に基づいてＣＡＤデータをＣＧ描画する。ここで、ステップＳ３０４においてＣＡＤデータの座標系をＳＬＡＭ地図の座標系に合わせているため、ＳＬＡＭ地図上の３次元座標はＣＡＤデータ上でも同一の座標として扱うことができる。そして、ＣＧ描画して得られた２次元画像に対して画像処理により視覚的指標となる特徴点を検出する。

図５および図６は、視覚的指標の検出を説明する図である。図５（Ａ）は、ＣＡＤデータを上から見た図である。ＣＡＤデータ５００には、視覚的指標となる照明器具５０１および配管５０２が配置されている。図５（Ｂ）は、キーフレームを撮影したカメラの位置および姿勢と特徴点をＣＡＤデータ上で説明する図である。カメラ５０３は、ＳＬＡＭ地図生成時にキーフレームを撮影したカメラの位置および姿勢を示している。カメラ５０３の位置および姿勢でＣＡＤデータをＣＧ描画すると、図６（Ａ）に示される２次元画像６００が生成される。図６（Ａ）は、視覚的指標の検出時のカメラ視点画像の一例を示す図である。検出部１０４は、２次元画像６００から視覚的指標となる特徴点を検出する。２次元画像６００では、例えば、黒点で示される位置で特徴点が検出される。図６（Ａ）に示される特徴点６０１の位置は、図５（Ｂ）に示されるＣＡＤデータの点５０４の位置と対応している。図６（Ｂ）は、検出したすべての特徴点をＣＡＤデータの座標空間上に示す図である。検出部１０４は、ＳＬＡＭ地図に含まれるキーフレームの数だけ繰り返し特徴点の検出を行う。すべてのキーフレームに対する特徴点の検出が完了すると、検出部１０４は検出したすべての特徴点の３次元座標の配列を視覚的指標として対応付け部１０５に出力する。

ステップＳ３０６では、対応付け部１０５がＣＡＤデータ上の視覚的指標とＳＬＡＭ地図上の構成要素とを対応（関連）付ける。本実施形態においてＣＡＤデータ上の視覚的指標はＳＬＡＭ地図に含まれる構成要素の特徴点と同じであるため、ＳＬＡＭ地図の特徴点に対してＣＡＤデータ上の視覚的指標を関連付ける。具体的な関連付けの方法としては、例えば、ＣＡＤデータ上の視覚的指標に対してＳＬＡＭ地図上で最近傍の特徴点を選んで対応付ける。本実施形態では、ステップＳ３０４においてＣＡＤデータの座標系をＳＬＡＭ地図の座標系に変換したため、ＣＡＤデータ上の３次元座標をＳＬＡＭ地図上の３次元座標と同じ座標空間上の座標として扱うことができる。そのため対応付け部１０５は、ＣＡＤデータ上の視覚的指標とＳＬＡＭ地図上の特徴点との２点間のユークリッド距離が最小となる特徴点を選んで対応付ければよい。対応付け部１０５は、対応付けをすべての視覚的指標に対して行う。

図７は、ＣＡＤデータの視覚的指標とＳＬＡＭ地図の特徴点の対応付けを説明する図である。図７では、ＣＡＤデータの視覚的指標とＳＬＡＭ地図上の特徴点が同じ座標空間上に重畳して示される。白色の点がＣＡＤデータの視覚的指標、灰色の点がＳＬＡＭ地図の特徴点を示している。図７（Ａ）は、対応付けを行う前の視覚的指標および特徴点を示す図である。図７（Ｂ）は、対応付けを行った後の視覚的指標および特徴点を示す図である。図７（Ｂ）では、視覚的指標および各視覚的指標の近傍にあり視覚的指標と対応付けられた特徴点だけが残り、視覚的指標に対応付けられなかったＳＬＡＭ地図の特徴点が排除されている。ＣＡＤデータの視覚的指標とＳＬＡＭ地図の特徴点が一対一対応し、個数が一致した状態となる。対応付け部１０５は視覚的指標の特徴点の３次元座標と、対応付けたＳＬＡＭ地図の特徴点の３次元座標のペアの配列を生成し、補正部１０６に出力する。

ステップＳ３０７では、補正部１０６がＳＬＡＭ地図上の特徴点と対応付けられた視覚的指標の３次元座標に基づいて、地図取得部１０１が取得したＳＬＡＭ地図に含まれる特徴点の３次元座標を補正する。補正部１０６は、まず、視覚的指標を真値とするために対応付け部１０５が出力した特徴点の３次元座標ペアに基づいて、ＳＬＡＭ地図の特徴点の３次元座標を視覚的指標の３次元座標に置き換える。次に、３次元座標を置き換えた特徴点群を固定してＳＬＡＭ地図を最適化する。最適化する手法としては、例えば、バンドル調整を用いる。ただし、一般にバンドル調整は再投影誤差を最小化する方法である。そのため、再投影誤差とキーフレーム間の位置および姿勢の差分の和が最小となるように位置および姿勢を推定するＳｋｒｙｐｎｙｋらの方法（Ｉ． Ｓｋｒｙｐｎｙｋ ａｎｄ Ｄ． Ｇ． Ｌｏｗｅ， “Ｓｃｅｎｅ ｍｏｄｅｌｌｉｎｇ， ｒｅｃｏｇｎｉｔｉｏｎ ａｎｄ ｔｒａｃｋｉｎｇ ｗｉｔｈ ｉｎｖａｒｉａｎｔ ｉｍａｇｅ ｆｅａｔｕｒｅｓ，” Ｐｒｏｃ． ３ｒｄ ＩＥＥＥ ａｎｄ ＡＣＭ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ， ｐｐ．１１０－１１９， ２００４．）と組み合わせて推定を行う。

以上説明したように、本実施形態によれば、Ｖｉｓｕａｌ ＳＬＡＭで自律走行する移動体のシステムにおいて移動体の位置姿勢計測に用いる３次元の地図（ＳＬＡＭ地図）と実空間を示す地図（ＣＡＤデータ）との誤差を低減できる。これにより、移動体の位置姿勢計測に用いる地図（ＳＬＡＭ地図）に含まれる誤差が低減され、地図の精度を向上させることができる。

（変形例１－１）
第１実施形態において、移動体の位置姿勢計測手法としてＶｉｓｕａｌ ＳＬＡＭを利用する例について説明したが、これに限られるものでなく、移動体の位置姿勢を地図に基づいて計測可能な手法であればどんな手法であってもよい。例えば、ＬｉＤＡＲ ＳＬＡＭやデプスカメラなどのセンサを用いたＳＬＡＭにより移動体の位置姿勢を計測してもよい。２Ｄ ＬｉＤＡＲ ＳＬＡＭのように位置姿勢計測に用いる地図が物体までの距離値から生成した２次元地図である場合は、ＣＡＤデータに含まれる頂点情報から推定したエッジを視覚的指標として用いてもよい。ＣＡＤデータから取得したエッジをＳＬＡＭ地図上のエッジとマッチングして、一致するエッジを固定して、ＳＬＡＭ地図上の他のエッジの位置や向きを調整して地図を補正することができる。また、３Ｄ ＬｉＤＡＲやデプスカメラのように位置姿勢計測に用いる地図が３次元点群の集合体となるような場合は、視覚的指標として面の情報を用いてもよい。ＣＡＤデータとＳＬＡＭ地図のそれぞれからエッジで区分された面を抽出し、ＣＡＤの面と３次元点群から抽出した面とをマッチングして、マッチした３次元点群の座標をＣＡＤの面に合わせて補正する。そして、補正後の面を示す点群の座標を固定して、ＳＬＡＭ地図に含まれるすべての特徴点を最適化処理により補正することができる。このように、移動体の位置姿勢計測の手法は、画像を撮影しなくても距離情報等から特徴点を抽出可能であればよい。また、視覚的指標は、照明や配管には限られず、エッジが検出しやすいものや、エッジで区分される面であってもよい。

（変形例１－２）
第１実施形態において、検出部１０４はＳＬＡＭ地図に含まれるキーフレームと同じ視点でＣＡＤデータをＣＧ描画した後に画像処理することで視覚的指標を検出していたが、これに限られるものでない。また、第１実施形態においては、視覚的指標として輝度勾配が閾値よりも大きい画素である特徴点を用いていたがこれに限られるものではない。視覚的指標の検出方法は、ＣＡＤデータから視覚的指標と定義した対象物を検出できればどんな検出方法であってもよい。例えば、ＣＡＤデータに含まれるオブジェクトの頂点情報から線の交点となる点を求めて視覚的指標としてもよい。また、ＣＡＤデータの頂点情報から直線を求め、閾値以上の長さの直線をエッジとして検出し、そのエッジを視覚的指標とする、あるいはエッジの端点を視覚的指標としてもよい。さらに、ＣＡＤデータからエッジで区分された面を抽出し、２つの面がなす角が閾値以内の場合の接合部分のエッジを視覚的指標としてもよい。その他にも、ＣＡＤデータを正射影変換などにより２次元画像に変換した後に画像処理により特徴点やエッジ等を検出し、それらを視覚的指標としてもよい。

（変形例１－３）
第１実施形態において、ステップＳ３０４の位置合わせ部１０３によるＣＡＤデータとＳＬＡＭ地図の座標系の位置合わせ方法として、ＣＡＤデータ上の座標が既知であるマーカーを用いる方法を説明したが、これに限られるものではない。位置合わせ方法は、ＣＡＤデータとＳＬＡＭ地図の座標系の位置合わせを行うことができればどんな位置合わせ方法であってもよい。例えば、マーカーの代わりにカメラを搭載した移動体を用いて、移動体がＣＡＤデータ上で既知の座標に停止している時に、カメラの位置姿勢を推定して、その位置姿勢情報に基づきＣＡＤデータとＳＬＡＭ地図の座標系間の相対位置関係を求めてもよい。また、位置姿勢計測に用いる地図が２Ｄ ＬｉＤＡＲ ＳＬＡＭのようなエッジ情報を持つ地図である場合は、ＣＡＤデータの壁や柱とＳＬＡＭ地図の壁や柱に相当する部分とをマッチングして相対位置関係を求めてもよい。

（変形例１－４）
第１実施形態において、ステップＳ３０６のＣＡＤデータの視覚的指標とＳＬＡＭ地図に含まれる構成要素との対応付けでは、視覚的指標がＳＬＡＭ地図に含まれる特徴点と同一であるため直接関連付けたが、これに限られるものではない。対応付けの方法は、ＣＡＤデータの視覚的指標とＳＬＡＭ地図に含まれる構成要素との対応付けが可能であればどんな方法であってもよい。例えば、対応付け部１０５は、ＣＡＤデータから抽出したエッジの端点とＳＬＡＭ地図に含まれる構成要素とを関連付けるなど異なる種類の特徴を対応付けしてもよい。

（変形例１－５）
第１実施形態において、情報処理装置１００がＳＬＡＭ地図を補正するまでの設定や実行について、表示部１０８に表示されるＧＵＩを用いてユーザが指示を行えるようにしてもよい。図８は、地図の補正処理におけるＧＵＩの一例を示す図である。地図の補正処理のＧＵＩは、地図の補正処理を実行する際に表示データ生成部１０７により生成され、表示部１０８に表示される。

図８（Ａ）は、ステップＳ３０４の位置合わせを行うためのＧＵＩを示す図である。ＧＵＩ８１０には、ＳＬＡＭ地図８１１とＣＡＤデータ８１２が表示される。図８に示される例では、上から見た（同じ方向から見た）ＳＬＡＭ地図８１１とＣＡＤデータ８１２が表示されている。ユーザは、例えば、ＣＡＤデータ８１２上に重畳表示されるＳＬＡＭ地図８１１を、並進、回転、拡大縮小させて座標系が一致するように調整し、位置決定ボタン８２０を押下する。情報処理装置１００において位置決定ボタン８２０が押下されたことを検出すると、位置合わせ部１０３はユーザによる位置合わせの結果を入力部２１５を介して取得し、検出部１０４および表示データ生成部１０７に出力する。

図８（Ｂ）は、ステップＳ３０６のＣＡＤデータの視覚的指標とＳＬＡＭ地図の特徴点との対応付けを行うためのＧＵＩを示す図である。ＧＵＩ８３０は、ＣＡＤデータ上の視覚的指標を座標空間上にマッピングして示している。ＧＵＩ８４０は、ＳＬＡＭ地図上の視覚的指標を座標空間上にマッピングして示している。ＧＵＩ８３０のＣＡＤデータ上の座標系とＧＵＩ８４０のＳＬＡＭ地図の座標系は統一されるようにステップＳ３０４で変換済みである。図８（Ｂ）では、ＣＡＤデータおよびＳＬＡＭ地図のＸＹ平面をＺ軸の正方向から見た例を示している。ユーザは、まずＧＵＩ８３０においてＣＡＤデータ上の視覚的指標を選択する。次にユーザは、ＧＵＩ８４０のＳＬＡＭ地図上で、ＧＵＩ８３０で選択した視覚的指標に対応する視覚的指標を選択する。図８（Ｂ）の例では、ＧＵＩ８３０において視覚的指標８３１が選択され、対応する視覚的指標としてＧＵＩ８４０において視覚的指標８４１が選択されている。ＧＵＩ８３０に表示されるＣＡＤデータ上の全ての視覚的指標それぞれに、ＧＵＩ８４０に表示されるＳＬＡＭ地図上の視覚的指標を対応付けると、ユーザは完了ボタン８５０を押下する。情報処理装置１００において完了ボタン８５０が押下されたことを検出すると、対応付け部１０５はユーザによる対応付けの結果を入力部２１５を介して取得し、補正部１０６および表示データ生成部１０７に出力する。

図８（Ｃ）は、ステップＳ３０７の補正処理で利用する視覚的指標を選択するためのＧＵＩを示す図である。ＧＵＩ８６０にはＣＡＤデータ上の視覚的指標と、対応付けられたＳＬＡＭ地図上の視覚的指標が重畳表示されている。図８（Ｃ）の例では、ＧＵＩ８６０を、ＸＹ平面をＺ軸正方向から見た平面図で示している。ユーザは、ＧＵＩ８６０に表示される各視覚的指標について、補正処理で利用するか否かをチェックボックス８７０により設定する。例えば、ユーザはＧＵＩ８６０においてＣＡＤデータ上の視覚的指標を選択し、選択中の視覚的指標を補正処理で利用する場合にはチェックボックス８７０にチェックを入れる。一方、選択中の視覚的指標を補正処理で利用しない場合にはチェックボックス８７０を空にする。ＧＵＩ８６０に表示されるすべてのＣＡＤデータ上の視覚的指標について補正処理で利用するか否かの設定を完了すると、ユーザは補正実行ボタン８８０を押下する。情報処理装置１００において補正実行ボタン８８０が押下されたことを検出すると、補正部１０６はユーザにより選択されたＣＡＤデータ上の視覚的指標とこれに対応するＳＬＡＭ地図上の視覚的指標を用いて、ステップＳ３０７の補正処理を実行する。なお、ここではＧＵＩ８６０においてＣＡＤデータ上の視覚的指標を選択する例について説明したが、選択対象がＳＬＡＭ地図上の視覚的指標であってもよい。また、選択中の視覚的指標を利用するか否かをチェックボックス８７０で設定するのではなく、補正処理に利用する視覚的指標のみをＧＵＩ８６０で選択して補正実行ボタン８８０を押下するようにしてもよい。また、視覚的指標の内、ＳＬＡＭ地図の補正に利用したくない視覚的指標を選択して除外するようにしてもよい。図８（Ａ）～図８（Ｃ）に示したＧＵＩによって、ユーザはＳＬＡＭ地図の補正に至るまでの設定や途中結果を確認しながらＳＬＡＭ地図を補正することができる。

（第２実施形態）
第１実施形態では移動体が移動する現場環境の実空間に対応するＣＡＤデータから検出した視覚的指標を用いてＳＬＡＭ地図を補正した。第２実施形態では、環境内に後から設置した機械などを視覚的指標として用いて、ＳＬＡＭ地図を補正する例について説明する。第１実施形態では現場環境のＣＡＤデータから視覚的指標を検出したのに対し、第２実施形態では機械ごとのＣＡＤデータから視覚的指標を検出する。なお、増設された機械の位置および姿勢は現場環境のＣＡＤデータ上の位置および姿勢として既知であるとする。本実施形態における情報処理装置１００の構成は、第１実施形態と同様のためその説明を省略する。

図９は、第２実施形態における地図の補正処理を示すフローチャートである。図９のフローチャートに示される地図を補正するための各処理は、情報処理装置１００のＣＰＵ２１１が、ＲＯＭ２１２または外部メモリ２１４に格納されているプログラムをＲＡＭ２１３に読み出して実行することにより実現される。以下では、第１実施形態との差異について説明し、第１実施形態と同様の処理については同じ符号を付すことでその説明を省略する。

ステップＳ３０４におけるＣＡＤデータとＳＬＡＭ地図の座標系の位置合わせが完了すると、ステップＳ９０１の処理を行う。複数の実空間内に設置された機械のＣＡＤデータがある場合には、１つのＣＡＤデータについてステップＳ９０１～ステップＳ９０３の処理を行い、残りの機械のＣＡＤデータについて同様の処理を繰り返す。ステップＳ９０１で、検出部１０４は、設置された機械のＣＡＤデータから視覚的指標を検出する。検出対象が異なるが、視覚的指標の検出方法は第１実施形態と同じである。また、設置機械のＣＡＤデータと現場環境のＣＡＤデータとでは座標系が異なるため、検出部１０４は、現場環境のＣＡＤデータにおける設置機械の位置情報を用いて検出した視覚的指標の３次元座標を現場環境のＣＡＤデータ上の座標系に変換する。ステップＳ９０１の処理が完了すると、次にステップＳ３０６の処理が行われる。そして、ステップＳ３０６の処理が完了すると、次にステップＳ９０２の処理が行われる。

ステップＳ９０２で、補正部１０６は、ＳＬＡＭ地図の特徴点と対応付けられたＣＡＤデータの視覚的指標の３次元座標に基づいてＳＬＡＭ地図上の特徴点の３次元座標を補正する。補正部１０６は、第１実施形態と同様のバンドル調整を行ってＳＬＡＭ地図を最適化することで補正する。ただし、既に他の設置機械のＣＡＤデータに基づいてＳＬＡＭ地図の補正処理を行っている場合は、補正済みの機械の設置位置の近傍に存在する特徴点は最適化済みであるとして固定し、最適化済みでない特徴点についてバンドル調整を行う。ステップＳ９０２の処理が完了すると、次にステップＳ９０３の処理が行われる。

ステップＳ９０３で、補正部１０６は、すべての機械のＣＡＤデータを用いてＳＬＡＭ地図の補正処理を行ったか判定する。すべての設置機械に対してステップＳ９０１からステップＳ９０２の処理を完了した場合は、本処理を終了する。一方、すべての設置機械に対してステップＳ９０１からステップＳ９０２の処理を完了していない場合は、ステップＳ９０１に戻り、処理が完了していない設置機械に対してステップＳ９０１からステップＳ９０２の処理を継続する。なお、本実施形態では機械のＣＡＤデータを用いる例について説明したが機械に限られるものではなく、現場環境に設置された物体のＣＡＤデータであればよい。

本実施形態によれば、既に補正に使用済みの機械の周辺の特徴点を固定することで、すべての機械の周辺における位置姿勢計測の精度を確保することができる。また、環境内に設備の据付けの後に増設された複数の機械のＣＡＤデータを用いて、移動体が位置姿勢計測に用いる３次元地図に含まれる誤差を低減できる。これにより、現場環境のＣＡＤデータに基づいてＳＬＡＭ地図の補正を行った後に、各機械のＣＡＤデータから検出した視覚的指標を利用してＳＬＡＭ地図の補正を行うことで、ＳＬＡＭ地図の精度を向上させることができる。

本実施形態の開示は、以下の情報処理装置の構成を含む。
（構成１）移動体の位置姿勢を計測するための地図を取得する地図取得手段と、前記地図に対応する前記移動体が移動する実空間または前記実空間内の物体に対応する図面情報から視覚的指標を検出する検出手段と、前記地図と前記図面情報の座標系の位置合わせを行う位置合わせ手段と、前記位置合わせ後の前記図面情報に含まれる前記視覚的指標と前記地図に含まれる構成要素の対応付けを行う対応付け手段と、前記対応付けの結果に基づいて前記地図を補正する補正手段と、を有することを特徴とする情報処理装置。
（構成２）前記視覚的指標は、特徴点またはエッジであることを特徴とする構成１に記載の情報処理装置。
（構成３）前記検出手段は、前記図面情報に基づいて前記地図の任意のキーフレームに対応する画像を生成し、該画像から前記視覚的指標を検出することを特徴とする構成１または２に記載の情報処理装置。
（構成４）前記図面情報は、ＣＡＤデータであることを特徴とする構成１乃至３のいずれか１構成に記載の情報処理装置。
（構成５）前記地図は、前記移動体が備えるセンサにより前記移動体の周辺環境を計測して生成されることを特徴とする構成１乃至４のいずれか１構成に記載の情報処理装置。
（構成６）前記地図は移動体の位置姿勢を計測するためのＳＬＡＭで用いる３次元の地図であることを特徴とする構成１乃至５のいずれか１構成に記載の情報処理装置。
（構成７）構成１から６のいずれか１構成に記載の情報処理装置と、前記位置合わせ、前記対応付け、前記地図の補正の過程および結果の表示を制御する表示手段と、を有することを特徴とする情報処理システム。

（その他の実施形態）
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。

以上、本発明の好ましい実施形態について説明したが、本発明は、これらの実施形態に限定されず、その要旨の範囲内で種々の変形および変更が可能である。

１００ 情報処理装置
１０１ 地図取得部
１０２ 視覚的指標情報取得部
１０３ 位置合わせ部
１０４ 検出部
１０５ 対応付け部
１０６ 補正部

Claims (9)

1. 移動体の位置姿勢を計測するための地図を取得する地図取得手段と、
   前記地図に対応する前記移動体が移動する実空間または前記実空間内の物体に対応する図面情報から視覚的指標を検出する検出手段と、
   前記地図と前記図面情報の座標系の位置合わせを行う位置合わせ手段と、
   前記位置合わせ後の前記図面情報に含まれる前記視覚的指標と前記地図に含まれる構成要素の対応付けを行う対応付け手段と、
   前記対応付けの結果に基づいて前記地図を補正する補正手段と、を有することを特徴とする情報処理装置。
2. 前記視覚的指標は、特徴点またはエッジであることを特徴とする請求項１に記載の情報処理装置。
3. 前記検出手段は、前記図面情報に基づいて前記地図の任意のキーフレームに対応する画像を生成し、該画像から前記視覚的指標を検出することを特徴とする請求項１に記載の情報処理装置。
4. 前記図面情報は、ＣＡＤデータであることを特徴とする請求項１に記載の情報処理装置。
5. 前記地図は、前記移動体が備えるセンサにより前記移動体の周辺環境を計測して生成されることを特徴とする請求項１に記載の情報処理装置。
6. 前記地図は移動体の位置姿勢を計測するためのＳＬＡＭで用いる３次元の地図であることを特徴とする請求項１に記載の情報処理装置。
7. 請求項１から６のいずれか１項に記載の情報処理装置と、
   前記位置合わせ、前記対応付け、前記地図の補正の過程および結果の表示を制御する表示手段と、を有することを特徴とする情報処理システム。
8. 移動体の位置姿勢を計測するための地図を補正する情報処理装置の制御方法であって、
   前記地図を取得する工程と、
   前記地図に対応する前記移動体が移動する実空間または前記実空間内の物体に対応する図面情報から視覚的指標を検出する工程と、
   前記地図と前記図面情報の座標系の位置合わせを行う工程と、
   前記位置合わせ後の前記図面情報に含まれる前記視覚的指標と前記地図に含まれる構成要素の対応付けを行う工程と、
   前記対応付けの結果に基づいて前記地図を補正する工程と、を有することを特徴とする情報処理装置の制御方法。
9. 請求項８に記載の各工程を情報処理装置のコンピュータに実行させるプログラム。

Images