ホーム > 特許ランキング > 国立大学法人東北大学
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-10-07
(45)【発行日】2024-10-16
(54)【発明の名称】情報処理装置、移動体、情報処理方法、及びプログラム
(51)【国際特許分類】
G05D 1/00 20240101AFI20241008BHJP
G05D 1/46 20240101ALN20241008BHJP
G05D 1/43 20240101ALN20241008BHJP
【ＦＩ】
G05D1/00
G05D1/46
G05D1/43
【請求項の数】
8
(21)【出願番号】P 2023525240
(86)(22)【出願日】2021-06-02
(86)【国際出願番号】 JP2021021001
(87)【国際公開番号】W WO2022254609
(87)【国際公開日】2022-12-08
【審査請求日】2023-08-31
(73)【特許権者】
【識別番号】504157024
【氏名又は名称】国立大学法人東北大学
(74)【代理人】
【識別番号】100165179
【弁理士】
【氏名又は名称】田▲崎▼ 聡
(74)【代理人】
【識別番号】100188558
【弁理士】
【氏名又は名称】飯田 雅人
(74)【代理人】
【識別番号】100175824
【弁理士】
【氏名又は名称】小林 淳一
(74)【代理人】
【識別番号】100152272
【弁理士】
【氏名又は名称】川越 雄一郎
(74)【代理人】
【識別番号】100181722
【弁理士】
【氏名又は名称】春田 洋孝
(72)【発明者】
【氏名】大野 和則
(72)【発明者】
【氏名】岡田 佳都
(72)【発明者】
【氏名】小島 匠太郎
(72)【発明者】
【氏名】昆陽 雅司
(72)【発明者】
【氏名】田所 諭
(72)【発明者】
【氏名】軍司 健太
【審査官】田中 成彦
(56)【参考文献】
【文献】米国特許出願公開第２０１９／０１８８４７３（ＵＳ，Ａ１）
【文献】特開２００４－２７６１６８（ＪＰ，Ａ）
【文献】韓国公開特許第１０－２００９－０１０９８３０（ＫＲ，Ａ）
【文献】米国特許出願公開第２０２０／０３００６３９（ＵＳ，Ａ１）
【文献】米国特許出願公開第２０２０／０３３４８８７（ＵＳ，Ａ１）
【文献】国際公開第２０２１／０２４６６５（ＷＯ，Ａ１）
【文献】国際公開第２０１９／２１１９３２（ＷＯ，Ａ１）
【文献】特開２０２０－１１８５８６（ＪＰ，Ａ）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０５Ｄ １／００ － １／８７
Ｂ２５Ｊ １／００ － ２１／０２
(57)【特許請求の範囲】
【請求項1】
制御対象となる制御対象装置を制御する情報処理装置であって、前記制御対象装置が位置する空間内に配置された複数の物体と、前記複数の物体のうちの一部からなり、用途に応じてまとめられた一以上のグループと、前記グループを構成する前記物体の相対的な配置及び/又は姿勢を拘束するレイアウト拘束条件情報とを記憶する記憶部と、
前記空間内における一部の前記物体と前記制御対象装置との相対的な位置及び/又は姿勢と、前記記憶部に記憶された前記レイアウト拘束条件情報と、に基づいて、前記空間内の地図情報を生成する制御部と、
を備える情報処理装置。
【請求項2】
前記レイアウト拘束条件情報は、前記グループを構成する前記物体同士の間で許容される相対的な位置及び/又は姿勢についての誤差の値を含む請求項１に記載の情報処理装置。
【請求項3】
前記レイアウト拘束条件情報は、ベクトルで表され、前記グループを構成する2つの前記物体の間の相対位置及び相対姿勢に対する前記誤差の値の第１要素と、2つの前記物体の間の相対位置及び相対姿勢の第２要素とを含み、前記第２要素は、2つの前記物体の間の相対位置を示すｘ座標、ｙ座標と、前記物体の間の相対位置を示す方位角とを含む
請求項２に記載の情報処理装置。
【請求項4】
前記制御部は、前記制御対象装置が第１時間において取得した、前記グループを構成する少なくとも一つの前記物体の画像から算出した、前記制御対象装置と前記物体との相対位置及び/又は相対姿勢の物体位置姿勢のベクトルと、前記レイアウト拘束条件情報のベクトルとから、前記制御対象装置が取得していない前記グループを構成する他の前記物体の位置及び/又は姿勢のベクトルを算出する
請求項３に記載の情報処理装置。
【請求項5】
前記制御部は、前記物体位置姿勢のベクトルと前記レイアウト拘束条件情報のベクトルとから、前記グループを構成する前記物体同士の間に関係する情報行列及び情報ベクトルを、前記制御対象装置が各時間において取得し、算出した、前記物体位置姿勢のベクトルに基づき要素の足し上げを行うことで生成する
請求項４に記載の情報処理装置。
【請求項6】
前記制御部は、前記物体位置姿勢のベクトルから算出された位置及び姿勢である第１推定位置姿勢ベクトルと、前記制御対象装置が取得していない前記グループを構成する他の前記物体の位置及び/又は姿勢のベクトルから算出された位置及び/又は姿勢である第２推定位置姿勢ベクトルとの差分ベクトルを求め、前記差分ベクトルの内積が、所定の閾値を超えた場合、前記グループを構成する2つの前記物体の間の前記第２要素を削除する
請求項４又は５に記載の情報処理装置。
【請求項7】
制御対象装置が位置する空間内に配置された複数の物体と、前記複数の物体のうちの一部からなり、用途に応じてまとめられた一以上のグループと、前記グループを構成する前記物体の相対的な配置及び/又は姿勢を拘束するレイアウト拘束条件情報とを記憶する記憶部から前記レイアウト拘束条件情報を読み出す読出ステップと、
前記空間内における一部の前記物体と前記制御対象装置との相対的な位置及び/又は姿勢と、前記記憶部に記憶された前記レイアウト拘束条件情報と、に基づいて、前記空間内の地図情報を生成する生成ステップと、
を有する情報処理方法。
【請求項8】
コンピュータに、
制御対象装置が位置する空間内に配置された複数の物体と、前記複数の物体のうちの一部からなり、用途に応じてまとめられた一以上のグループと、前記グループを構成する前記物体の相対的な配置及び/又は姿勢を拘束するレイアウト拘束条件情報とを記憶する記憶部から前記レイアウト拘束条件情報を読み出す読出ステップと、
前記空間内における一部の前記物体と前記制御対象装置との相対的な位置及び/又は姿勢と、前記記憶部に記憶された前記レイアウト拘束条件情報と、に基づいて、前記空間内の地図情報を生成する生成ステップと、
を実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、情報処理装置、移動体、情報処理方法、及びプログラムに関する。
【背景技術】
【0002】
ロボット、ドローン等のような制御対象となる装置を制御する情報処理装置についての研究、開発が行われている。
【0003】
これに関し、ロボットを制御する情報処理装置であって、ロボットが位置する空間内において、当該空間内における物体とロボットとの相対的な位置及び姿勢を検出し、検出した位置及び姿勢に基づいて、当該空間内の地図を示す地図情報を生成する情報処理装置が知られている（非特許文献１参照）。
【先行技術文献】
【非特許文献】
【0004】
【文献】「環境とのインタラクションによる空間構造の獲得」、友納正裕、ＪＳＴプロジェクトデータベース、２００５年度終了報告書、https://projectdb.jst.go.jp/report/JST-PROJECT-7700000694/2913/
【発明の概要】
【発明が解決しようとする課題】
【0005】
ここで、ある空間内における物体とロボットとの相対的な位置及び姿勢を検出し、検出した位置及び姿勢と、当該空間内に位置する複数の物体の初期配置とに基づいて、当該空間内の地図を示す地図情報を生成する方法も知られている。しかしながら、当該空間内における複数の物体の配置は、初期配置から変更されてしまうことも少なくない。しかしながら、当該方法では、当該空間内に位置する複数の物体の初期配置が変更された場合、精度の悪い地図を示す地図情報を生成してしまうことがあった。
【0006】
本発明は、このような事情を考慮してなされたもので、制御対象装置が位置する空間内における複数の物体の配置が初期配置から変更されていたとしても、当該空間内の地図を示す地図情報を精度よく生成することができる情報処理装置、移動体、情報処理方法、及びプログラムを提供することを課題とする。
【課題を解決するための手段】
【0007】
本発明は以下の態様を含む。
［１］制御対象となる制御対象装置を制御する情報処理装置であって、前記制御対象装置が位置する空間内に配置された第１物体と、前記空間内に配置された前記第１物体と異なる第２物体との相対的な位置を示す情報を含むレイアウト拘束条件情報を記憶する記憶部と、前記制御対象装置の位置と、前記第１物体と前記制御対象装置との相対的な位置を示す第１物体装置間相対位置情報と、前記第２物体と前記制御対象装置との相対的な位置を示す第２物体装置間相対位置情報との少なくとも一方と、前記記憶部に記憶された前記レイアウト拘束条件情報とに基づいて、前記空間内の地図を示す地図情報を生成する制御部と、を備える情報処理装置。
［２］上記に記載の情報処理装置では、前記制御部は、予め決められた初期値に基づいて前記制御対象装置の位置を推定する第１推定部と、前記第１物体装置間相対位置情報と、前記第２物体装置間相対位置情報との少なくとも一方を物体装置間相対位置情報として取得する取得部と、前記記憶部に記憶された前記レイアウト拘束条件情報と、前記取得部により取得された前記物体装置間相対位置情報と、前記第１推定部により推定された前記制御対象装置の位置とに基づいて、前記空間内の地図を示す地図情報を生成する生成部と、を備える。
［３］上記に記載の情報処理装置では、前記制御部は、前記第１物体と前記第２物体との少なくとも一方を検出する検出部からの出力に基づいて、前記第１物体と前記制御対象装置との相対的な位置と、前記第２物体と前記制御対象装置との相対的な位置との少なくとも一方を推定する第２推定部を更に備え、前記取得部は、前記第１物体と前記制御対象装置との相対的な位置と、前記第２物体と前記制御対象装置との相対的な位置との少なくとも一方を示す情報を、前記物体装置間相対位置情報として前記第２推定部から取得する。
［４］上記に記載の情報処理装置では、前記生成部は、前記取得部により取得された前記物体装置間相対位置情報と、前記記憶部により記憶された前記レイアウト拘束条件情報と、前記第１推定部により推定された前記制御対象装置の位置とに基づいて、Ｇｒａｐｈ－ＳＬＡＭ（Simultaneous Localization and Mapping）における情報行列及び情報ベクトルを生成し、生成した前記情報行列及び前記情報ベクトルに基づく評価関数の最適化を行うことにより、前記地図情報を生成する。
［５］上記に記載の情報処理装置では、前記生成部は、前記取得部により取得された前記物体装置間相対位置情報と、前記記憶部により記憶された前記レイアウト拘束条件情報と、前記第１推定部により推定された前記制御対象装置の位置とに基づいて、前記第１物体及び前記第２物体それぞれの位置を推定し、推定した前記第１物体及び前記第２物体それぞれの位置に基づいて、前記情報行列及び前記情報ベクトルを生成する。
［６］上記に記載の情報処理装置では、前記生成部は、前記第１物体装置間相対位置情報と前記第２物体装置間相対位置情報との両方が前記物体装置間相対位置情報として前記取得部により取得された場合、前記第１推定部により推定された前記制御対象装置の位置と、前記取得部により取得された前記物体装置間相対位置情報と、前記記憶部により記憶された前記レイアウト拘束条件情報とに基づいて、前記第１物体の位置を第１推定位置として推定するとともに前記第２物体の位置を第２推定位置として推定し、且つ、前記第１推定部により推定された前記制御対象装置の位置と、前記取得部により取得された前記物体装置間相対位置情報とに基づいて、前記第１物体の位置を第３推定位置として推定するとともに前記第２物体の位置を第４推定位置として推定し、前記第１推定位置を示すベクトルと前記第３推定位置を示すベクトルとの差を第１差分として算出するとともに、前記第２推定位置を示すベクトルと前記第４推定位置を示すベクトルとの差を第２差分として算出し、前記第１差分と、前記第１物体の位置についての推定誤差と、前記第２差分と、前記第２物体の位置についての推定誤差とに基づいて、前記情報行列が有する要素のうち前記レイアウト拘束条件情報に応じて決められた要素の繋がりを削除する。
［７］上記に記載の情報処理装置では、前記制御対象装置は、前記制御対象装置の位置及び姿勢の少なくとも一方を変化させることができる移動体である。
［８］前記制御対象装置として、上記に記載の情報処理装置を備える、移動体。
［９］制御対象となる制御対象装置が位置する空間内に配置された第１物体と、前記空間内に配置された前記第１物体と異なる第２物体との相対的な位置を示す情報を含むレイアウト拘束条件情報を記憶する記憶部から前記レイアウト拘束条件情報を読み出す読出ステップと、前記制御対象装置の位置と、前記第１物体と前記制御対象装置との相対的な位置を示す第１物体装置間相対位置情報と、前記第２物体と前記制御対象装置との相対的な位置を示す第２物体装置間相対位置情報との少なくとも一方と、前記読出ステップにより読み出された前記レイアウト拘束条件情報とに基づいて、前記空間内の地図を示す地図情報を生成する生成ステップと、を有する情報処理方法。
［１０］コンピュータに、制御対象となる制御対象装置が位置する空間内に配置された第１物体と、前記空間内に配置された前記第１物体と異なる第２物体との相対的な位置を示す情報を含むレイアウト拘束条件情報を記憶する記憶部から前記レイアウト拘束条件情報を読み出す読出ステップと、前記制御対象装置の位置と、前記第１物体と前記制御対象装置との相対的な位置を示す第１物体装置間相対位置情報と、前記第２物体と前記制御対象装置との相対的な位置を示す第２物体装置間相対位置情報との少なくとも一方と、前記読出ステップにより読み出された前記レイアウト拘束条件情報とに基づいて、前記空間内の地図を示す地図情報を生成する生成ステップと、を実行させるためのプログラム。
［１１］制御対象となる制御対象装置を制御する情報処理装置であって、前記制御対象装置が位置する空間内に配置された第１物体と、前記空間内に配置された前記第１物体と異なる第２物体との相対的な姿勢を示す情報を含むレイアウト拘束条件情報を記憶する記憶部と、前記制御対象装置の姿勢と、前記第１物体と前記制御対象装置との相対的な姿勢を示す第１物体装置間相対姿勢情報と、前記第２物体と前記制御対象装置との相対的な姿勢を示す第２物体装置間相対姿勢情報との少なくとも一方と、前記記憶部に記憶された前記レイアウト拘束条件情報とに基づいて、前記空間内の地図を示す地図情報を生成する制御部と、を備える情報処理装置。
［１２］前記制御対象装置として、上記に記載の情報処理装置を備える、移動体。
［１３］制御対象となる制御対象装置が位置する空間内に配置された第１物体と、前記空間内に配置された前記第１物体と異なる第２物体との相対的な姿勢を示す情報を含むレイアウト拘束条件情報を記憶する記憶部から前記レイアウト拘束条件情報を読み出す読出ステップと、前記制御対象装置の姿勢と、前記第１物体と前記制御対象装置との相対的な姿勢を示す第１物体装置間相対姿勢情報と、前記第２物体と前記制御対象装置との相対的な姿勢を示す第２物体装置間相対姿勢情報との少なくとも一方と、前記読出ステップにより読み出された前記レイアウト拘束条件情報とに基づいて、前記空間内の地図を示す地図情報を生成する生成ステップと、を有する情報処理方法。
［１４］コンピュータに、制御対象となる制御対象装置が位置する空間内に配置された第１物体と、前記空間内に配置された前記第１物体と異なる第２物体との相対的な姿勢を示す情報を含むレイアウト拘束条件情報を記憶する記憶部から前記レイアウト拘束条件情報を読み出す読出ステップと、前記制御対象装置の姿勢と、前記第１物体と前記制御対象装置との相対的な姿勢を示す第１物体装置間相対姿勢情報と、前記第２物体と前記制御対象装置との相対的な姿勢を示す第２物体装置間相対姿勢情報との少なくとも一方と、前記読出ステップにより読み出された前記レイアウト拘束条件情報とに基づいて、前記空間内の地図を示す地図情報を生成する生成ステップと、を実行させるためのプログラム。
［１］制御対象となる制御対象装置を制御する情報処理装置であって、前記制御対象装置が位置する空間内に配置された第１物体と、前記空間内に配置された前記第１物体と異なる第２物体との相対的な位置を示す情報を含むレイアウト拘束条件情報を記憶する記憶部と、前記制御対象装置の位置と、前記第１物体と前記制御対象装置との相対的な位置を示す第１物体装置間相対位置情報と、前記第２物体と前記制御対象装置との相対的な位置を示す第２物体装置間相対位置情報との少なくとも一方と、前記記憶部に記憶された前記レイアウト拘束条件情報とに基づいて、前記空間内の地図を示す地図情報を生成する制御部と、を備える情報処理装置。
［２］上記に記載の情報処理装置では、前記制御部は、予め決められた初期値に基づいて前記制御対象装置の位置を推定する第１推定部と、前記第１物体装置間相対位置情報と、前記第２物体装置間相対位置情報との少なくとも一方を物体装置間相対位置情報として取得する取得部と、前記記憶部に記憶された前記レイアウト拘束条件情報と、前記取得部により取得された前記物体装置間相対位置情報と、前記第１推定部により推定された前記制御対象装置の位置とに基づいて、前記空間内の地図を示す地図情報を生成する生成部と、を備える。
［３］上記に記載の情報処理装置では、前記制御部は、前記第１物体と前記第２物体との少なくとも一方を検出する検出部からの出力に基づいて、前記第１物体と前記制御対象装置との相対的な位置と、前記第２物体と前記制御対象装置との相対的な位置との少なくとも一方を推定する第２推定部を更に備え、前記取得部は、前記第１物体と前記制御対象装置との相対的な位置と、前記第２物体と前記制御対象装置との相対的な位置との少なくとも一方を示す情報を、前記物体装置間相対位置情報として前記第２推定部から取得する。
［４］上記に記載の情報処理装置では、前記生成部は、前記取得部により取得された前記物体装置間相対位置情報と、前記記憶部により記憶された前記レイアウト拘束条件情報と、前記第１推定部により推定された前記制御対象装置の位置とに基づいて、Ｇｒａｐｈ－ＳＬＡＭ（Simultaneous Localization and Mapping）における情報行列及び情報ベクトルを生成し、生成した前記情報行列及び前記情報ベクトルに基づく評価関数の最適化を行うことにより、前記地図情報を生成する。
［５］上記に記載の情報処理装置では、前記生成部は、前記取得部により取得された前記物体装置間相対位置情報と、前記記憶部により記憶された前記レイアウト拘束条件情報と、前記第１推定部により推定された前記制御対象装置の位置とに基づいて、前記第１物体及び前記第２物体それぞれの位置を推定し、推定した前記第１物体及び前記第２物体それぞれの位置に基づいて、前記情報行列及び前記情報ベクトルを生成する。
［６］上記に記載の情報処理装置では、前記生成部は、前記第１物体装置間相対位置情報と前記第２物体装置間相対位置情報との両方が前記物体装置間相対位置情報として前記取得部により取得された場合、前記第１推定部により推定された前記制御対象装置の位置と、前記取得部により取得された前記物体装置間相対位置情報と、前記記憶部により記憶された前記レイアウト拘束条件情報とに基づいて、前記第１物体の位置を第１推定位置として推定するとともに前記第２物体の位置を第２推定位置として推定し、且つ、前記第１推定部により推定された前記制御対象装置の位置と、前記取得部により取得された前記物体装置間相対位置情報とに基づいて、前記第１物体の位置を第３推定位置として推定するとともに前記第２物体の位置を第４推定位置として推定し、前記第１推定位置を示すベクトルと前記第３推定位置を示すベクトルとの差を第１差分として算出するとともに、前記第２推定位置を示すベクトルと前記第４推定位置を示すベクトルとの差を第２差分として算出し、前記第１差分と、前記第１物体の位置についての推定誤差と、前記第２差分と、前記第２物体の位置についての推定誤差とに基づいて、前記情報行列が有する要素のうち前記レイアウト拘束条件情報に応じて決められた要素の繋がりを削除する。
［７］上記に記載の情報処理装置では、前記制御対象装置は、前記制御対象装置の位置及び姿勢の少なくとも一方を変化させることができる移動体である。
［８］前記制御対象装置として、上記に記載の情報処理装置を備える、移動体。
［９］制御対象となる制御対象装置が位置する空間内に配置された第１物体と、前記空間内に配置された前記第１物体と異なる第２物体との相対的な位置を示す情報を含むレイアウト拘束条件情報を記憶する記憶部から前記レイアウト拘束条件情報を読み出す読出ステップと、前記制御対象装置の位置と、前記第１物体と前記制御対象装置との相対的な位置を示す第１物体装置間相対位置情報と、前記第２物体と前記制御対象装置との相対的な位置を示す第２物体装置間相対位置情報との少なくとも一方と、前記読出ステップにより読み出された前記レイアウト拘束条件情報とに基づいて、前記空間内の地図を示す地図情報を生成する生成ステップと、を有する情報処理方法。
［１０］コンピュータに、制御対象となる制御対象装置が位置する空間内に配置された第１物体と、前記空間内に配置された前記第１物体と異なる第２物体との相対的な位置を示す情報を含むレイアウト拘束条件情報を記憶する記憶部から前記レイアウト拘束条件情報を読み出す読出ステップと、前記制御対象装置の位置と、前記第１物体と前記制御対象装置との相対的な位置を示す第１物体装置間相対位置情報と、前記第２物体と前記制御対象装置との相対的な位置を示す第２物体装置間相対位置情報との少なくとも一方と、前記読出ステップにより読み出された前記レイアウト拘束条件情報とに基づいて、前記空間内の地図を示す地図情報を生成する生成ステップと、を実行させるためのプログラム。
［１１］制御対象となる制御対象装置を制御する情報処理装置であって、前記制御対象装置が位置する空間内に配置された第１物体と、前記空間内に配置された前記第１物体と異なる第２物体との相対的な姿勢を示す情報を含むレイアウト拘束条件情報を記憶する記憶部と、前記制御対象装置の姿勢と、前記第１物体と前記制御対象装置との相対的な姿勢を示す第１物体装置間相対姿勢情報と、前記第２物体と前記制御対象装置との相対的な姿勢を示す第２物体装置間相対姿勢情報との少なくとも一方と、前記記憶部に記憶された前記レイアウト拘束条件情報とに基づいて、前記空間内の地図を示す地図情報を生成する制御部と、を備える情報処理装置。
［１２］前記制御対象装置として、上記に記載の情報処理装置を備える、移動体。
［１３］制御対象となる制御対象装置が位置する空間内に配置された第１物体と、前記空間内に配置された前記第１物体と異なる第２物体との相対的な姿勢を示す情報を含むレイアウト拘束条件情報を記憶する記憶部から前記レイアウト拘束条件情報を読み出す読出ステップと、前記制御対象装置の姿勢と、前記第１物体と前記制御対象装置との相対的な姿勢を示す第１物体装置間相対姿勢情報と、前記第２物体と前記制御対象装置との相対的な姿勢を示す第２物体装置間相対姿勢情報との少なくとも一方と、前記読出ステップにより読み出された前記レイアウト拘束条件情報とに基づいて、前記空間内の地図を示す地図情報を生成する生成ステップと、を有する情報処理方法。
［１４］コンピュータに、制御対象となる制御対象装置が位置する空間内に配置された第１物体と、前記空間内に配置された前記第１物体と異なる第２物体との相対的な姿勢を示す情報を含むレイアウト拘束条件情報を記憶する記憶部から前記レイアウト拘束条件情報を読み出す読出ステップと、前記制御対象装置の姿勢と、前記第１物体と前記制御対象装置との相対的な姿勢を示す第１物体装置間相対姿勢情報と、前記第２物体と前記制御対象装置との相対的な姿勢を示す第２物体装置間相対姿勢情報との少なくとも一方と、前記読出ステップにより読み出された前記レイアウト拘束条件情報とに基づいて、前記空間内の地図を示す地図情報を生成する生成ステップと、を実行させるためのプログラム。
【発明の効果】
【0008】
本発明によれば、制御対象装置が位置する空間内における複数の物体の配置が初期配置から変更されていたとしても、当該空間内の地図を示す地図情報を精度よく生成することができる。
【図面の簡単な説明】
【0009】
【図1】実施形態に係る制御システム１の構成の一例を示す図である。
【図2】情報処理装置３０のハードウェア構成の一例を示す図である。
【図3】情報処理装置３０の機能構成の一例を示す図である。
【図4】式（４）に示したベクトルのイメージ図である。
【図5】情報処理装置３０が情報行列Ω及び情報ベクトルξを生成する処理の流れの一例を示す図である。
【図6】情報処理装置３０が地図情報を生成する処理の流れの一例を示す図である。
【図7】情報処理装置３０により生成された地図情報の一例を示す図である。
【図8】情報処理装置３０により生成された地図情報の他の例を示す図である。
【図9】情報処理装置３０により生成された地図情報の更に他の例を示す図である。
【図10】情報処理装置３０により生成された地図情報の更に更に他の例を示す図である。
【図11】実施形態に係る制御システム１の構成の他の例を示す図である。
【図12】３つのマーカーが設けられた物体Ｍ１の一例を示す図である。
【発明を実施するための形態】
【0010】
＜実施形態＞
以下、本発明の実施形態について、図面を参照して説明する。
以下、本発明の実施形態について、図面を参照して説明する。
【0011】
【0012】
制御システム１は、制御対象となる制御対象装置１０と、制御対象装置１０に設けられた検出部２０と、情報処理装置３０を備える。図１に示した例では、制御システム１では、制御対象装置１０と、検出部２０と、情報処理装置３０とは、別体に構成されている。しかしながら、制御システム１では、制御対象装置１０と、検出部２０と、情報処理装置３０とのうちの一部または全部は、一体に構成されてもよい。また、制御システム１では、図１に示したように、制御対象装置１０及び検出部２０の両方と、情報処理装置３０とが別体である場合、情報処理装置３０は、ＬＡＮ（Local Area Network）、ＷＡＮ（Wide Area Network）、専用通信網等のネットワークを介して制御対象装置１０及び検出部２０のそれぞれと通信可能に接続されてもよく、有線又は無線により制御対象装置１０及び検出部２０のそれぞれと通信可能に接続される構成であってもよい。以下では、一例として、情報処理装置３０が、無線により制御対象装置１０及び検出部２０のそれぞれと通信可能に接続される場合について説明する。
【0013】
制御システム１は、制御対象装置１０が位置する空間内の地図を示す地図情報を生成する。以下では、説明の便宜上、制御対象装置１０が位置する空間を、対象空間Ｒと称して説明する。対象空間Ｒは、例えば、制御対象装置１０が位置する部屋の中の空間等であるが、これに限られるわけではない。なお、制御対象装置１０が位置する部屋の中の空間以外に対象空間Ｒとなり得る空間は、例えば、水中、空中、宇宙空間等である。
【0014】
本実施形態において、対象空間Ｒは、複数の物体が配置された空間である。そして、対象空間Ｒに配置された複数の物体の少なくとも一部は、用途に応じて、１つ以上のグループにまとめられている。なお、これら１つ以上のグループは、２つ以上の物体がまとまることによって構成される。このため、対象空間Ｒに配置された複数の物体には、グループとしてまとめられていない物体が１つ以上含まれていてもよい。以下では、一例として、対象空間Ｒに配置された複数の物体が、図１に示した物体Ｍ１～物体Ｍ４の４つの物体である場合について説明する。また、以下では、一例として、図１に示したように、物体Ｍ１と物体Ｍ２とがグループＧ１にまとめられており、物体Ｍ３と物体Ｍ４とがグループＧ２にまとめられている場合について説明する。すなわち、図１に示した例では、対象空間Ｒ内に配置された４つの物体は、２つのグループにまとめられている。このため、この一例では、対象空間Ｒに配置された４つの物体には、グループとしてまとめられていない物体が１つも含まれていない。
【0015】
対象空間Ｒ内における４つの物体が２つのグループにまとめられている理由は、これら４つの物体の配置が初期配置と異なる配置に変更された場合であっても、各グループ内での物体同士の相対的な位置及び姿勢が保持されていることが多いためである。例えば、グループＧ１にまとめられている物体Ｍ１及び物体Ｍ２が、対象空間Ｒ内において横並びに連結されている場合、物体Ｍ１及び物体Ｍ２の配置は、初期配置と異なる配置に変更されるとしても、物体Ｍ１及び物体Ｍ２の用途が変更されない限り、物体Ｍ１と物体Ｍ２との相対的な位置及び姿勢が保持されたまま、初期配置と異なる配置に変更される。これは、物体Ｍ１及び物体Ｍ２の状態が横並びに連結されている状態であることが、物体Ｍ１及び物体Ｍ２の用途に適した状態であるためである。このようにして使用される物体としては、例えば、作業机、棚等が挙げられる。このような事情から、各グループ内において、物体同士の配置、すなわち、物体同士のレイアウトは、ほぼ変化しない又は変化しないように拘束されていると考えることができる。このため、各グループ内において、物体同士の相対的な位置及び姿勢は、対象空間Ｒ内の地図を示す地図情報の生成において、物体同士の配置を変化させないように拘束させる拘束条件として用いることができる。
【0016】
そこで、制御システム１は、対象空間Ｒ内におけるグループ毎のレイアウト拘束条件情報に基づいて、対象空間Ｒ内の地図を示す地図情報を生成する。すなわち、制御システム１は、グループＧ１のレイアウト拘束条件情報と、グループＧ２のレイアウト拘束条件情報とに基づいて、当該地図情報を生成する。なお、以下では、説明の便宜上、グループＧ１のレイアウト拘束条件情報と、グループＧ２のレイアウト拘束条件情報とを区別する必要がない限り、まとめてレイアウト拘束条件情報と称して説明する。なお、以下では、説明の便宜上、上記のような拘束を、レイアウト拘束と称して説明する場合がある。また、以下では、上記のような拘束条件を、レイアウト拘束条件と称して説明する場合がある。
【0017】
ここで、対象空間Ｒ内におけるあるグループのレイアウト拘束条件情報は、当該グループにまとめられている２つ以上の物体同士の配置を変化させないように拘束させる拘束条件を示す情報である。より具体的には、当該レイアウト拘束条件情報は、当該グループにまとめられている２つ以上の物体同士の相対的な位置及び姿勢を示す情報を、当該拘束条件として含む情報のことである。すなわち、対象空間Ｒ内におけるグループＧ１のレイアウト拘束条件情報は、物体Ｍ１と物体Ｍ２との相対的な位置及び姿勢を示す情報を、物体Ｍ１と物体Ｍ２との配置を変化させないように拘束させる拘束条件として含む情報である。以下では、一例として、当該レイアウト拘束条件情報には、物体Ｍ１と物体Ｍ２との相対的な位置及び姿勢を示す情報に加えて、物体Ｍ１と物体Ｍ２との相対的な位置及び姿勢について許容される誤差の値を示す情報が含まれている場合について説明する。また、対象空間Ｒ内におけるグループＧ２のレイアウト拘束条件情報は、物体Ｍ３と物体Ｍ４との相対的な位置及び姿勢を示す情報を、物体Ｍ３と物体Ｍ４との配置を変化させないように拘束させる拘束条件として含む情報である。以下では、一例として、当該レイアウト拘束条件情報には、物体Ｍ３と物体Ｍ４との相対的な位置及び姿勢を示す情報に加えて、物体Ｍ３と物体Ｍ４との相対的な位置及び姿勢について許容される誤差の値を示す情報が含まれている場合について説明する。
【0018】
このようなレイアウト拘束条件情報を用いることにより、制御システム１は、対象空間Ｒ内における複数の物体の配置が初期配置から変更されていたとしても、対象空間Ｒ内の地図を示す地図情報を精度よく生成することができる。これは、初期配置から変更された後の当該複数の物体の配置の少なくとも一部を、レイアウト拘束条件情報によって推定することができるためである。以下では、一例として、制御システム１が、グループ毎のレイアウト拘束条件情報を用いたＧｒａｐｈ－ＳＬＡＭ（Simultaneous Localization and Mapping）アルゴリズムに基づいて、対象空間Ｒ内の地図を示す地図情報を生成する場合について説明する。ただし、本実施形態において、従来のＧｒａｐｈ－ＳＬＡＭアルゴリズムについては、既によく知られているアルゴリズムであるため、必要以上の詳細な説明を省略する。すなわち、本実施形態では、グループ毎のレイアウト拘束条件情報を用いたＧｒａｐｈ－ＳＬＡＭアルゴリズムと、従来のＧｒａｐｈ－ＳＬＡＭアルゴリズムとの差分について説明する。従来のＧｒａｐｈ－ＳＬＡＭアルゴリズムについては、例えば、「「確率ロボティクス」、出版社：毎日コミュニケーションズ、著者：Sebastian Thrun、Wolfram Burgard、Dieter Fox、翻訳者：上田 隆一」等に詳しい記載がある。なお、制御システム１は、グループ毎のレイアウト拘束条件情報を用いた他の種類のＳＬＡＭに基づいて当該地図情報を生成する構成であってもよい。また、制御システム１は、レイアウト拘束条件情報を用いるアルゴリズムとして、ＳＬＡＭアルゴリズム以外のアルゴリズムに基づいて当該地図情報を生成する構成であってもよい。ここで、当該アルゴリズムは、地図情報を生成可能なアルゴリズムであれば、既知のアルゴリズムであってもよく、これから開発されるアルゴリズムであってもよい。以下では、説明の便宜上、グループ毎のレイアウト拘束条件情報を用いたＧｒａｐｈ－ＳＬＡＭアルゴリズムを、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭと称して説明する。
【0019】
ここで、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭの特徴のうちの１つについて、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭと異なるＧｒａｐｈ－ＳＬＡＭアルゴリズム（例えば、従来のＧｒａｐｈ－ＳＬＡＭアルゴリズム等）と比較しながら紹介する。
【0020】
レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭと異なるＧｒａｐｈ－ＳＬＡＭアルゴリズムに基づいて地図情報を生成する場合、制御システム１は、対象空間Ｒ内における全部の物体を検出部２０によって検出する必要がある。これにより、制御システム１は、対象空間Ｒ内における全部の物体について、物体と制御対象装置１０との相対的な位置及び姿勢を推定し、当該Ｇｒａｐｈ－ＳＬＡＭアルゴリズムにおける情報行列の各要素の足し上げ（更新）と、当該Ｇｒａｐｈ－ＳＬＡＭアルゴリズムにおける情報ベクトルの各要素の足し上げ（更新）とを行うことができる。
【0021】
一方、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭに基づいて地図情報を生成する場合、制御システム１は、対象空間Ｒ内における一部の物体を検出部２０によって検出することにより、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける情報行列の各要素の足し上げを行うことができる。すなわち、当該場合、制御システム１は、物体Ｍ１～物体Ｍ４の４つの物体のうち、物体Ｍ１と物体Ｍ２との少なくとも一方と、物体Ｍ３と物体Ｍ４との少なくとも一方とを検出部２０によって検出することにより、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける情報行列の各要素の足し上げと、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける情報ベクトルの各要素の足し上げとを行うことができる。これは、グループＧ１のレイアウト拘束条件情報を用いると、物体Ｍ１と物体Ｍ２とのうちの一方と制御対象装置１０との相対的な位置及び姿勢を推定することにより、物体Ｍ１と物体Ｍ２とのうちの他方と制御対象装置１０との相対的な位置及び姿勢も推定することができ、且つ、グループＧ２のレイアウト拘束条件情報を用いると、物体Ｍ３と物体Ｍ４とのうちの一方と制御対象装置１０との相対的な位置及び姿勢を推定することにより、物体Ｍ３と物体Ｍ４とのうちの他方と制御対象装置１０との相対的な位置及び姿勢も推定することができるためである。このため、制御システム１は、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭに基づいて対象空間Ｒ内の地図を示す地図情報を生成する場合、対象空間Ｒ内において１つのグループとしてまとまっている複数の物体のうちの一部の物体が、検出部２０により検出不可能な位置（例えば、光学カメラにおけるオクルージョン領域内の位置）に位置していたとしても、当該地図情報を精度よく生成することができる。
【0022】
次に、制御システム１が備える制御対象装置１０、検出部２０、情報処理装置３０それぞれの構成について詳しく説明する。
【0023】
制御対象装置１０は、情報処理装置３０により制御される。制御対象装置１０は、例えば、ドローン、移動可能なロボット、ＡＧＶ（Automatic Guided Vehicle）等の移動体であるが、情報処理装置３０により制御される移動不可能な装置であってもよい。また、例えば、制御対象装置１０は、人、犬等の動物によって運搬される装置であってもよい（すなわち、自走不可能な装置であってもよい）。以下では、一例として、制御対象装置１０が、図１に示したようにドローンである場合について説明する。なお、制御対象装置１０がロボットである場合、制御対象装置１０は、プロペラ、足、車輪、キャタピラ等で移動可能なロボットであってもよく、蛇型ロボットのように筐体の動きによって移動可能なロボットであってもよく、移動可能な他の種類のロボットであってもよく、移動不可能なロボットであってもよく、後述する検出部２０を備えた移動不可能な他の装置、他の部材等であってもよく、検出部２０そのものであってもよい。ただし、制御対象装置１０が移動不可能な装置であった場合であっても、制御対象装置１０は、人が運んだ際にその移動量を検出可能な装置である。この移動量の検出は、検出部２０により行われてもよく、検出部２０と異なるセンサによって行われてもよい。なお、制御対象装置１０は、情報処理装置３０を備える構成であってもよい。以下では、一例として、制御対象装置１０が、情報処理装置３０を備えない場合について説明する。
【0024】
検出部２０は、対象空間Ｒ内の物体を検出可能な装置であれば、如何なる装置であってもよい。以下では、一例として、検出部２０が、集光された光を電気信号に変換する撮像素子として、ＣＣＤ（Charge Coupled Device）、ＣＭＯＳ（Complementary Metal Oxide Semiconductor）等を備えた撮像装置（例えば、カメラ等）である場合について説明する。すなわち、この一例における検出部２０は、対象空間Ｒ内の物体を、撮像することにより検出する。検出部２０は、情報処理装置３０からの制御に応じて、撮像可能な範囲を撮像する。検出部２０は、撮像した撮像画像を、情報処理装置３０に出力する。なお、検出部２０は、物体を検出可能な装置であれば、ＬＩＤＥＲ（Light Detection and Ranging、Laser Imaging Detection and Ranging）、ＴｏＦ（Time of Flight）等の他の装置であってもよい。ただし、この場合、検出部２０は、何らかの方法により、個々の物体を識別する必要がある。このような方法としては、既知の方法が用いられてもよく、これから開発される方法が用いられてもよい。
【0025】
ここで、以下では、一例として、検出部２０により撮像される（すなわち、検出部２０により検出される）物体Ｍ１～物体Ｍ４のそれぞれには、図１に示したように、マーカーが設けられている場合について説明する。
【0026】
より具体的には、物体Ｍ１には、物体Ｍ１を識別する識別情報が第１符号化情報として符号化された第１マーカーと、物体Ｍ１を検出する検出部２０と物体Ｍ１との相対的な位置及び姿勢を示す情報が第２符号化情報として符号化された第２マーカーとを含むマーカーＭＫＲ１が設けられている。なお、マーカーＭＫＲ１は、既知のマーカーであってもよく、これから開発されるマーカーであってもよい。
【0027】
また、物体Ｍ２には、物体Ｍ２を識別する識別情報が第１符号化情報として符号化された第１マーカーと、物体Ｍ２を検出する検出部２０と物体Ｍ２との相対的な位置及び姿勢を示す情報が第２符号化情報として符号化された第２マーカーとを含むマーカーＭＫＲ２が設けられている。なお、マーカーＭＫＲ２は、既知のマーカーであってもよく、これから開発されるマーカーであってもよい。
【0028】
また、物体Ｍ３には、物体Ｍ３を識別する識別情報が第１符号化情報として符号化された第１マーカーと、物体Ｍ３を検出する検出部２０と物体Ｍ２との相対的な位置及び姿勢を示す情報が第２符号化情報として符号化された第２マーカーとを含むマーカーＭＫＲ３が設けられている。なお、マーカーＭＫＲ３は、既知のマーカーであってもよく、これから開発されるマーカーであってもよい。
【0029】
また、物体Ｍ４には、物体Ｍ４を識別する識別情報が第１符号化情報として符号化された第１マーカーと、物体Ｍ４を検出する検出部２０と物体Ｍ２との相対的な位置及び姿勢を示す情報が第２符号化情報として符号化された第２マーカーとを含むマーカーＭＫＲ４が設けられている。なお、マーカーＭＫＲ４は、既知のマーカーであってもよく、これから開発されるマーカーであってもよい。
【0030】
なお、以下では、説明の便宜上、マーカーＭＫＲ１～マーカーＭＫＲ４のそれぞれを区別する必要がない限り、まとめてマーカーＭＫＲと称して説明する。
【0031】
検出部２０は、前述した通り、この一例において、制御対象装置１０に設けられる。このため、検出部２０が撮像可能な範囲は、制御対象装置１０の移動とともに変化する。すなわち、検出部２０は、制御対象装置１０の位置及び姿勢に応じた範囲を撮像可能である。なお、検出部２０は、制御対象装置１０に設けられる構成に代えて、対象空間Ｒ内の少なくとも一部を撮像可能なように対象空間Ｒ内に設けられる構成であってもよい。
【0032】
検出部２０は、例えば、静止画像を撮像する。なお、検出部２０は、動画像を撮像する構成であってもよい。この場合、本実施形態において説明する撮像画像は、検出部２０により撮像された動画像を構成する各フレームによって代替可能である。
【0033】
情報処理装置３０は、例えば、多機能携帯電話端末（スマートフォン）である。なお、情報処理装置３０は、多機能携帯電話端末に代えて、タブレットＰＣ（Personal Computer）、ノートＰＣ、ＰＤＡ（Personal Digital Assistant）、携帯電話端末、デスクトップＰＣ、ワークステーション等の他の情報処理装置であってもよい。
【0034】
情報処理装置３０は、制御対象装置１０を制御する。例えば、情報処理装置３０は、予め記憶されたプログラムに基づいて、所定の軌道に沿って制御対象装置１０を移動させる。また、例えば、情報処理装置３０は、受け付けた操作に応じて、制御対象装置１０を移動させる。
【0035】
また、情報処理装置３０は、所定の軌道に沿って制御対象装置１０を移動させながら検出部２０を制御し、所定のサンプリング周期が経過する毎に、検出部２０が撮像可能な範囲を検出部２０に撮像させる。情報処理装置３０は、検出部２０が撮像した撮像画像を、検出部２０から取得する。ここで、検出部２０により撮像された個々の撮像画像には、その撮像画像が撮像された時刻を示す時刻情報が対応付けられている。情報処理装置３０は、制御対象装置１０が所定の軌道に沿って移動している間に検出部２０により撮像された複数の撮像画像と、予め記憶されたグループ毎のレイアウト拘束条件情報とに基づいて、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける情報行列の各要素の足し上げと、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける情報ベクトルの各要素の足し上げとを行う。これにより、情報処理装置３０は、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける評価関数の最適化を行い、対象空間Ｒ内における４つの物体それぞれについてのワールド座標系における位置及び姿勢を推定する。そして、情報処理装置３０は、対象空間Ｒ内における４つの物体それぞれについてのワールド座標系における位置及び姿勢の推定結果に基づいて、対象空間Ｒ内の地図を示す地図情報を生成することができる。なお、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける地図情報の生成方法は、従来のＧｒａｐｈ－ＳＬＡＭアルゴリズムにおける地図情報の生成方法と同様の方法であってもよく、これから開発される生成方法であってもよい。このように、情報処理装置３０は、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭに基づいて、対象空間Ｒ内の地図を示す地図情報を生成する。これにより、情報処理装置３０は、対象空間Ｒ内における４つの物体の配置が初期配置から変更されていたとしても、対象空間Ｒ内の地図を示す地図情報を精度よく生成することができる。その結果、情報処理装置３０は、例えば、制御対象装置１０に精度の高い作業を行わせることができる。ここで、ワールド座標系は、現実の対象空間Ｒ内における位置及び姿勢を示すための三次元直交座標系であり、例えば、対象空間Ｒに対応付けられた三次元直交座標系である。
【0036】
ここで、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける情報行列には、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭと異なるＧｒａｐｈ－ＳＬＡＭアルゴリズムにおける情報行列において足し上げられることのない新たな要素が足し上げられる。レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭでは、この新たな要素として、対象空間Ｒ内における各グループの物体間の要素が足し上げられる。具体的には、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける情報行列には、物体Ｍ１と物体Ｍ２との配置を変化させないように拘束させる拘束条件によって物体Ｍ１と物体Ｍ２との配置を拘束する強さを示す値と、物体Ｍ３と物体Ｍ４との配置を変化させないように拘束させる拘束条件によって物体Ｍ３と物体Ｍ４との配置を拘束する強さを示す値とが、対象空間Ｒ内における各グループの物体間の要素として足し上げられる。ここで、物体Ｍ１と物体Ｍ２との配置を変化させないように拘束させる拘束条件によって物体Ｍ１と物体Ｍ２との配置を拘束する強さを示す値は、グループＧ１のレイアウト拘束条件情報に含まれている情報が示す誤差の値の逆数である。すなわち、物体Ｍ１と物体Ｍ２との配置を変化させないように拘束させる拘束条件によって物体Ｍ１と物体Ｍ２との配置を拘束する強さを示す値は、当該誤差の値が大きいほど小さくなり、当該誤差の値が小さいほど大きくなる。そして、物体Ｍ１と物体Ｍ２との配置を変化させないように拘束させる拘束条件によって物体Ｍ１と物体Ｍ２との配置を拘束する強さを示す値が大きいほど、物体Ｍ１と物体Ｍ２との相対的な位置及び姿勢は、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける地図情報の生成において、変化しにくい。また、物体Ｍ３と物体Ｍ４との配置を変化させないように拘束させる拘束条件によって物体Ｍ３と物体Ｍ４との配置を拘束する強さを示す値は、グループＧ２のレイアウト拘束条件情報に含まれている情報が示す誤差の値の逆数である。すなわち、物体Ｍ３と物体Ｍ４との配置を変化させないように拘束させる拘束条件によって物体Ｍ３と物体Ｍ４との配置を拘束する強さを示す値は、当該誤差の値が大きいほど小さくなり、当該誤差の値が小さいほど大きくなる。そして、物体Ｍ３と物体Ｍ４との配置を変化させないように拘束させる拘束条件によって物体Ｍ３と物体Ｍ４との配置を拘束する強さを示す値が大きいほど、物体Ｍ３と物体Ｍ４との相対的な位置及び姿勢は、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける地図情報の生成において、変化しにくい。
【0037】
一方、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける情報ベクトルの各要素の足し上げについては、従来のＧｒａｐｈ－ＳＬＡＭアルゴリズムにおける情報ベクトルの各要素の足し上げと変わりない。このため、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける情報ベクトルの各要素の足し上げについては、詳細な説明を省略する。
【0038】
なお、以下では、説明の便宜上、ある物体と制御対象装置１０との相対的な位置及び姿勢を、当該物体についての物体装置間相対位置姿勢と称して説明する。また、以下では、説明の便宜上、ある２つの物体同士の相対的な位置及び姿勢を、当該２つの物体についての物体間相対位置姿勢と称して説明する。
【0039】
【0040】
情報処理装置３０は、例えば、ＣＰＵ（Central Processing Unit）３１と、記憶部３２と、入力受付部３３と、通信部３４と、表示部３５を備える。これらの構成要素は、バスを介して相互に通信可能に接続されている。また、情報処理装置３０は、通信部３４を介して制御対象装置１０、検出部２０のそれぞれと通信を行う。
【0041】
ＣＰＵ３１は、例えば、情報処理装置３０の全体を制御するプロセッサーである。なお、ＣＰＵ３１は、ＦＰＧＡ（Field Programmable Gate Array）等の他のプロセッサーであってもよい。ＣＰＵ３１は、記憶部３２に格納された各種のプログラムを実行する。
【0042】
記憶部３２は、例えば、ＨＤＤ（Hard Disk Drive）、ＳＳＤ（Solid State Drive）、ＥＥＰＲＯＭ（Electrically Erasable Programmable Read-Only Memory）、ＲＯＭ（Read-Only Memory）、ＲＡＭ（Random Access Memory）等を含む。なお、記憶部３２は、情報処理装置３０に内蔵されるものに代えて、ＵＳＢ（Universal Serial Bus）等のデジタル入出力ポート等によって接続された外付け型の記憶装置であってもよい。記憶部３２は、情報処理装置３０が処理する各種の情報、各種の画像、各種のプログラム等を記憶する。例えば、記憶部３２には、前述のグループ毎のレイアウト拘束条件情報が記憶されている。
【0043】
入力受付部３３は、キーボード、マウス、タッチパッド等の入力装置である。なお、入力受付部３３は、表示部３５と一体に構成されたタッチパネルであってもよい。
【0044】
通信部３４は、例えば、アンテナ、ＵＳＢ等のデジタル入出力ポート、イーサネット（登録商標）ポート等を含んで構成される。
【0045】
表示部３５は、例えば、液晶ディスプレイパネル、あるいは、有機ＥＬ（Electro Luminescence）ディスプレイパネル等を含む表示装置である。
【0046】
【0047】
情報処理装置３０は、記憶部３２と、入力受付部３３と、通信部３４と、表示部３５と、制御部３６を備える。
【0048】
制御部３６は、情報処理装置３０の全体を制御する。制御部３６は、例えば、撮像制御部３６１と、画像取得部３６２と、第１推定部３６３と、第２推定部３６４と、取得部３６５と、生成部３６６と、移動体制御部３６７を備える。制御部３６が備えるこれらの機能部は、例えば、ＣＰＵ３１が、記憶部３２に記憶された各種のプログラムを実行することにより実現される。また、当該機能部のうちの一部又は全部は、ＬＳＩ（Large Scale Integration）、ＡＳＩＣ（Application Specific Integrated Circuit）等のハードウェア機能部であってもよい。また、撮像制御部３６１と、画像取得部３６２と、第１推定部３６３と、第２推定部３６４と、取得部３６５と、生成部３６６と、移動体制御部３６７とのうちの一部又は全部は、一体に構成されてもよい。撮像制御部３６１と、画像取得部３６２と、第１推定部３６３と、第２推定部３６４と、取得部３６５と、生成部３６６と、移動体制御部３６７とのうちの一部又は全部のそれぞれは、２つ以上の機能部に分けられてもよい。
【0049】
撮像制御部３６１は、検出部２０が撮像可能な範囲を検出部２０に撮像させる。
【0050】
画像取得部３６２は、検出部２０が撮像した撮像画像を検出部２０から取得する。
【0051】
第１推定部３６３は、検出部２０により撮像が行われた時刻毎の制御対象装置１０のワールド座標系における位置及び姿勢を推定する。以下では、説明の便宜上、制御対象装置１０のワールド座標系における位置及び姿勢を、制御対象装置位置姿勢と称して説明する。
【0052】
第２推定部３６４は、画像取得部３６２により取得された撮像画像に基づいて、対象空間Ｒ内における４つの物体のうち、当該撮像画像に撮像されている１つ以上の物体のそれぞれについての物体装置間相対位置姿勢を推定する。
【0053】
取得部３６５は、第２推定部３６４により推定された１つ以上の物体装置間相対位置姿勢のそれぞれを示す物体装置間相対位置姿勢情報を、第２推定部３６４から取得する。
【0054】
生成部３６６は、記憶部３２に予め記憶されたレイアウト拘束条件情報と、取得部３６５により取得された物体装置間相対位置姿勢情報と、第１推定部３６３により推定された制御対象装置位置姿勢とに基づいて、情報行列の各要素の足し上げと、情報ベクトルの各要素の足し上げとを行う。そして、生成部３６６は、各要素の足し上げが行われた後の情報行列及び情報ベクトルに基づく評価関数の最適化を行い、対象空間Ｒ内における４つの物体それぞれのワールド座標系における位置及び姿勢を推定する。そして、生成部３６６は、対象空間Ｒ内における４つの物体それぞれのワールド座標系における位置及び姿勢の推定結果に基づいて、対象空間Ｒ内の地図を示す地図情報を生成する。
【0055】
移動体制御部３６７は、制御対象装置１０を制御する。例えば、移動体制御部３６７は、記憶部３２に予め記憶された動作プログラムに基づいて、所定の軌跡に沿って制御対象装置１０を移動させる。また、例えば、移動体制御部３６７は、受け付けた操作に応じて、制御対象装置１０を移動させる。
【0056】
＜レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける情報行列の各要素の足し上げ方法＞
以下、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける情報行列の各要素の足し上げ方法について説明する。ただし、以下では、対象空間Ｒ内における各グループの物体間に関係する要素の当該情報行列への足し上げ方法について説明し、この要素以外の要素の足し上げ方法については、従来のＧｒａｐｈ－ＳＬＡＭアルゴリズムにおける足し上げ方法と同様であるため、説明を省略する。
以下、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける情報行列の各要素の足し上げ方法について説明する。ただし、以下では、対象空間Ｒ内における各グループの物体間に関係する要素の当該情報行列への足し上げ方法について説明し、この要素以外の要素の足し上げ方法については、従来のＧｒａｐｈ－ＳＬＡＭアルゴリズムにおける足し上げ方法と同様であるため、説明を省略する。
【0057】
なお、以下では、説明の便宜上、対象空間Ｒ内における４つの物体のそれぞれを、変数ｋの値によって識別する。ｋの値は、１～４のいずれかである。ｋ＝１である場合、ｋは、物体Ｍ１を示す。ｋ＝２である場合、ｋは、物体Ｍ２を示す。ｋ＝３である場合、ｋは、物体Ｍ３を示す。ｋ＝４である場合、ｋは、物体Ｍ４を示す。また、以下では、説明の便宜上、対象空間Ｒ内における４つの物体のうち、ｋの値によって識別される物体との相対的な位置及び姿勢がレイアウト拘束条件情報によって拘束されている他の物体を、変数ｌの値によって識別する。ｌの値は、１～４のうち、ｋの値以外の値のいずれかである。また、以下では、一例として、時刻を、ｔによって表す。ただし、実施形態では、ある基準となる時刻をｔ＝０とし、ｔ＝０からの時間の経過を、ｔへの整数の加算によって表す。すなわち、実施形態では、時刻ｔは、離散化されている。また、実施形態では、ｘ＾ｙと記載した場合、ｘに上付添字としてｙが付随していること、すなわち、ｘｙを意味する。また、実施形態では、ｘ＿ｙと記載した場合、ｘに下付添字としてｙが付随していること、すなわち、ｘｙを意味する。また、実施形態では、ｘ＾ｙ＿ｚと記載した場合、ｘに上付添字としてｙが付随しているとともに、ｘに下付添字としてｚが付随していることを意味する。ここで、これらｘ、ｙ、ｚのそれぞれは、如何なるアルファベットであってもよく、中括弧で括られた２つ以上のアルファベットの連なりであってもよい。例えば、ｘｙｚ＾｛ｊｋｌ｝は、ｘｙｚｊｋｌを意味する。また、例えば、ｘｙｚ＿｛ｊｋｌ｝は、ｘｙｚｊｋｌを意味する。また、例えば、ｘ＾｛ｙ＿ｚ｝と記載した場合、ｘに上付添字としてｙ＿ｚが付随していることを意味する。また、例えば、ｘ＿｛ｙ＿ｚ｝と記載した場合、ｘに下付添字としてｙ＿ｚが付随していることを意味する。また、以下では、一例として、ワールド座標系における位置が、ワールド座標系におけるｘｙ平面上の座標によって表される場合について説明する。また、以下では、一例として、ワールド座標系における姿勢が、ワールド座標系における方位角によって表される場合について説明する。なお、ワールド座標系における位置は、ワールド座標系における３次元の座標によって表される構成であってもよい。また、ワールド座標系における姿勢は、ワールド座標系におけるオイラー角によって表される構成であってもよい。
【0058】
例えば、ある時刻ｔにおいて検出部２０により撮像された撮像画像に、ｋのある値によって識別される物体が撮像されていた場合、すなわち、検出部２０により当該物体が検出された場合について考える。この場合、当該物体のワールド座標系における位置及び姿勢は、時刻ｔでの制御対象装置１０のワールド座標系における位置及び姿勢と、時刻ｔでの当該物体についての物体装置間相対位置姿勢を用いて、以下の式（１）、式（２）に基づいて算出することができる。なお、以下では、説明の便宜上、ある物体のワールド座標系における位置及び姿勢を、当該物体についての物体位置姿勢と称して説明する。
【0059】
【数1】
【0060】
【数2】
【0061】
【数3】
【0062】
ここで、ｍ＿ｋは、ｋによって識別される物体についての物体位置姿勢を示す３つの値を要素として有するベクトルである。ｒ＿ｔは、時刻ｔでの制御対象装置位置姿勢を示すベクトルである。ｒ＿ｔは、上記の式（２）によって表される。ｘ＿ｒｔは、時刻ｔでの制御対象装置のワールド座標系における位置を示すｘ座標である。ｙ＿ｒｔは、時刻ｔでの制御対象装置１０のワールド座標系における位置を示すｙ座標である。θ＿ｒｔは、時刻ｔでの制御対象装置１０のワールド座標系における姿勢を示す方位角である。Ｚ＿ｋ＿ｔは、当該物体についての観測値を表すベクトルである。より具体的には、Ｚ＿ｋ＿ｔは、時刻ｔにおいて検出部２０により撮像された撮像画像に基づいて推定された当該物体についての物体装置間相対位置姿勢を表すベクトルである。Ｚ＿ｋ＿ｔは、上記の式（３）によって明示的に表される。式（３）の右辺の第１要素であるｘ＾｛ｒ＿ｔ｝＿｛ｍ＿ｋ｝は、当該物体と、時刻ｔにおける制御対象装置１０との相対的な位置を示すｘ座標である。換言すると、ｘ＾｛ｒ＿ｔ｝＿｛ｍ＿ｋ｝は、「時刻ｔの制御対象装置位置姿勢を示すｒ＿ｔ」から見た「ｋによって識別される物体についての物体位置姿勢を示すｍ＿ｋ」の相対的な位置を示すｘ座標である。式（２）の右辺の第２要素であるｙ＾｛ｒ＿ｔ｝＿｛ｍ＿ｋ｝は、当該物体と、時刻ｔにおける制御対象装置１０との相対的な位置を示すｙ座標である。換言すると、ｙ＾｛ｒ＿ｔ｝＿｛ｍ＿ｋ｝は、「時刻ｔの制御対象装置位置姿勢を示すｒ＿ｔ」から見た「ｋによって識別される物体についての物体位置姿勢を示すｍ＿ｋ」の相対的な位置を示すｙ座標である。また、式（２）の右辺の第３要素であるθ＾｛ｒ＿ｔ｝＿｛ｍ＿ｋ｝は、当該物体と、時刻ｔにおける制御対象装置１０との相対的な姿勢を示す方位角である。換言すると、θ＾｛ｒ＿ｔ｝＿｛ｍ＿ｋ｝は、「時刻ｔの制御対象装置位置姿勢を示すｒ＿ｔ」から見た「ｋによって識別される物体についての物体位置姿勢を示すｍ＿ｋ」の相対的な姿勢を示す方位角である。
【0063】
一方、ｋの値によって識別される物体と、ｌの値によって識別される物体との物体間相対位置姿勢は、記憶部３２に予め記憶されたグループ毎のレイアウト拘束条件情報により特定される。そして、当該物体間相対位置姿勢情報を含むレイアウト拘束条件情報は、以下の式（４）及び式（５）のように表わすことができる。
【0064】
【数4】
【0065】
【数5】
【0066】
ここで、式（４）の左辺であるｐ＾ｋ＿ｌは、ｋの値によって識別される物体と、ｌの値によって識別される物体との物体間相対位置姿勢を示す情報を、これら２つの物体の配置を変化させないように拘束させる拘束条件として含むレイアウト拘束条件情報を示すベクトルである。より具体的には、ｌの値によって識別される物体の位置及び姿勢に対する、ｋの値によって識別される物体の相対的な位置及び姿勢を示す情報を、当該拘束条件として含むレイアウト拘束条件情報を示すベクトルである。図４は、式（４）に示したベクトルのイメージ図である。図４に示した物体ＭＫは、ｋの値によって識別される物体の一例を示す。図４に示した物体ＭＬは、ｌの値によって識別される物体の一例を示す。そして、図４に示した矢印は、当該ベクトルの一例を示す。すなわち、当該ベクトルは、物体ＭＫの位置から物体ＭＬの位置を示すベクトルとして表される。なお、図４では、物体ＭＫの位置は、一例として、物体ＭＫの図心の位置によって表している。また、図４では、物体ＭＫの位置は、一例として、物体ＭＫの図心の位置によって表している。なお、このベクトルは、実際には、物体ＭＫと物体ＭＬとの配置を変化させないように拘束させる拘束条件によって物体ＭＫと物体ＭＬとの配置を拘束する強さを示す値を要素として有しているため、図４においてイメージとして矢印によって示されているに過ぎない。式（４）の右辺の第１要素であるσ＾ｋ＿ｌは、ｌの値によって識別される物体の位置及び姿勢に対する、ｋの値によって識別される物体の相対的な位置及び姿勢について許容される誤差の値（この値は、単一の値であってもよく、ベクトルであってもよく、行列であってもよい）を示す。式（４）の右辺の第２要素であるｍ＾ｋ＿ｌは、ｌの値によって識別される物体の位置及び姿勢に対する、ｋの値によって識別される物体の相対的な位置及び姿勢を示すベクトルである。すなわち、当該第２要素は、これら２つの物体についての物体間相対位置姿勢を示すベクトルである。式（４）の右辺の第２要素は、式（５）のように表される。式（５）の右辺の第１要素であるｘ＾｛ｍ＿ｋ｝＿｛ｍ＿ｌ｝は、ｌの値によって識別される物体の位置に対する、ｋの値によって識別される物体の相対的な位置を示すｘ座標である。式（５）の右辺の第２要素であるｙ＾｛ｍ＿ｋ｝＿｛ｍ＿ｌ｝は、ｌの値によって識別される物体の位置に対する、ｋの値によって識別される物体の相対的な位置を示すｙ座標である。式（５）の右辺の第３要素であるθ＾｛ｍ＿ｋ｝＿｛ｍ＿ｌ｝は、ｌの値によって識別される物体の姿勢に対する、ｋの値によって識別される物体の相対的な姿勢を示す方位角である。
【0067】
式（４）及び式（５）のように表されたレイアウト拘束条件情報を用いると、ｌの値によって識別される物体が時刻ｔにおいて検出部２０により検出されていない場合であっても、当該物体のワールド座標系における位置及び姿勢は、以下の式（６）に基づいて算出することができる。
【0068】
【数6】
【0069】
ここで、ｋのある値によって識別される物体についての物体位置姿勢と、式（４）により表されたレイアウト拘束条件情報と、ｌの値によって識別される物体についての物体位置姿勢とを算出することができた場合、以下の式（７）のように、これら２つの物体間に関係する要素を情報行列Ω及び情報ベクトルξへ足し上げることができる。
【0070】
【数7】
【0071】
上記の式（７）の情報行列Ωにおいて明示的に示されている要素は、情報行列Ωにおけるｍ＿ｌとｍ＿ｋとに関係する要素、すなわち、ｋのある値によって識別される物体とｌの値によって識別される物体との間に関係する要素である。そして、この要素は、レイアウト拘束条件情報を表すベクトルの第１要素の逆数である。すなわち、この要素は、ｋの値によって識別される物体と、ｌの値によって識別される物体との配置を変化させないように拘束させる拘束条件によってこれら２つの物体の配置を拘束する強さを示す値である。また、上記の式（７）の情報ベクトルξにおいて明示的に示されている要素は、情報ベクトルξにおけるｍ＿ｌとｍ＿ｋとに関係する要素であり、ｍ＿ｌとｍ＿ｋとのそれぞれそのものである。
【0072】
このように、情報処理装置３０は、上記の式（１）～式（７）と、時刻ｔにおいて検出部２０により撮像された撮像画像とに基づいて、情報行列Ωの各要素の足し上げと情報ベクトルξの各要素との足し上げとを行うことができる。情報処理装置３０は、このような要素の足し上げを、各時刻において検出部２０により撮像された撮像画像に基づいて行う。これにより、情報処理装置３０は、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける情報行列Ω及び情報ベクトルξを生成することができる。
【0073】
ここで、ｋの値によって識別される物体のワールド座標系における位置及び姿勢とともに、ｌの値によって識別される物体の物体位置姿勢が、上記の式（１）及び式（２）に基づいて算出されることもある。以下では、説明の便宜上、ｌの値によって識別される物体の物体位置姿勢のうち、式（１）及び式（２）に基づいて算出された位置及び姿勢を、第１推定位置姿勢と称して説明する。また、以下では、説明の便宜上、ｌの値によって識別される物体の物体位置姿勢のうち、式（６）に基づいて算出された位置及び姿勢を第２物体位値姿勢と称して説明する。第１推定位置姿勢を表すベクトルと第２物体位値姿勢を表すベクトルとの差分ベクトルは、第１推定位置姿勢と第２推定位置姿勢とのずれの大きさを表す量の算出に用いることができる。このような量は、第１推定位置姿勢と第２推定位置姿勢との間の距離として算出可能な量であり、例えば、ユークリッド空間における内積、多変量の空間におけるマハラノビス距離、確率空間におけるＫＬ（Kullback Leibler）情報量等である。以下では、一例として、第１推定位置姿勢と第２推定位置姿勢とのずれの大きさを表す量が、当該差分ベクトルの内積である場合について説明する。この場合、この内積が所定の第１閾値を超えると、ｌの値によって識別される物体と、ｋの値によって識別される物体との配置は、初期配置から変更されていると考えられる。これら２つの物体の配置が初期配置から変更されている場合、上記の情報行列Ωにおいて、ｍ＿ｌとｍ＿ｋとに関係する要素の繋がりは、削除されるべきである。当該要素の繋がりの削除は、当該要素を０に置き換えることを意味する。以下の式（８）及び式（９）は、当該差分ベクトルの内積を表している。
【0074】
【数8】
【0075】
【数9】
【0076】
上記の式（８）の左辺は、前述の内積を示す。式（８）の右辺の２つの括弧内それぞれの第１項は、第２推定位置姿勢を表すベクトルである。当該第１項は、式（９）に基づいて算出される。そして、式（８）の右辺の２つの括弧内それぞれの第２項、すなわち、ｍ＿ｌは、第１推定位置姿勢を表すベクトルである。
【0077】
ここで、前述の第１閾値は、例えば、第２推定位置姿勢についての推定誤差である。この推定誤差は、如何なる方法で推定されてもよい。なお、第１閾値は、当該推定誤差に代えて、他の値であってもよい。また、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける情報行列Ω及び情報ベクトルξそれぞれの各要素の足し上げに係る繰り返し処理において、前述の内積は、繰り返し算出される。このため、内積が第１閾値を超える回数が所定の第２閾値を超えた場合、上記の情報行列Ωにおいて、ｍ＿ｌとｍ＿ｋとの間に関係する要素を、０に置き換えられるようにすることが望ましい。なお、第２閾値は、０．０～１．０の間の数である。第２閾値が１．０である場合、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭでは、物体間のレイアウトの変更が行われても、物体間のレイアウトの変更が結果に反映されなくなる。一方、第２閾値が０．０に近づくほど、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭでは、物体間のレイアウトの変更が行われた場合、物体間のレイアウトの変更が結果に反映されやすくなる。以下では、説明の便宜上、このように２つの物体間に関係する要素を０に置き換える処理を、レイアウト解除処理と称して説明する。
【0078】
＜情報処理装置が情報行列及び情報ベクトルを生成する処理＞
以下、図５を参照し、情報処理装置３０が情報行列Ω及び情報ベクトルξを生成する処理について説明する。図５は、情報処理装置３０が情報行列Ω及び情報ベクトルξを生成する処理の流れの一例を示す図である。なお、以下では、一例として、図５に示したステップＳ１１０の処理が行われるよりも前のタイミングにおいて、制御対象装置１０が所定の軌道を移動している期間において検出部２０により撮像された複数の撮像画像を、情報処理装置３０が検出部２０から取得している場合について説明する。また、以下では、一例として、当該タイミングにおいて、情報処理装置３０が当該処理を開始する操作を予め受け付けている場合について説明する。
以下、図５を参照し、情報処理装置３０が情報行列Ω及び情報ベクトルξを生成する処理について説明する。図５は、情報処理装置３０が情報行列Ω及び情報ベクトルξを生成する処理の流れの一例を示す図である。なお、以下では、一例として、図５に示したステップＳ１１０の処理が行われるよりも前のタイミングにおいて、制御対象装置１０が所定の軌道を移動している期間において検出部２０により撮像された複数の撮像画像を、情報処理装置３０が検出部２０から取得している場合について説明する。また、以下では、一例として、当該タイミングにおいて、情報処理装置３０が当該処理を開始する操作を予め受け付けている場合について説明する。
【0079】
第２推定部３６４は、記憶部３２に予め記憶されたグループ毎のレイアウト拘束条件情報を記憶部３２から読み出す（ステップＳ１１０）。
【0080】
次に、制御部３６は、検出部２０から予め取得した複数の撮像画像から、撮像された時刻が早い順に１つずつ撮像画像を対象撮像画像として選択し、選択した対象撮像画像毎に、ステップＳ１３０～ステップＳ１７０の処理を繰り返し行う（ステップＳ１２０）。
【0081】
ステップＳ１２０において対象撮像画像が選択された後、第２推定部３６４は、選択された対象撮像画像に、対象空間Ｒ内における４つの物体のうちの少なくとも１つが撮像されているか否かを判定する（ステップＳ１３０）。すなわち、ステップＳ１３０において、第２推定部３６４は、選択された対象撮像画像に基づいて、対象空間Ｒ内における４つの物体のうちの少なくとも１つが検出されているか否かを判定する。図５では、ステップＳ１３０の処理を、「物体検出？」によって示している。
【0082】
第２推定部３６４は、選択された対象撮像画像に、対象空間Ｒ内における４つの物体のうちのいずれもが撮像されていないと判定した場合（ステップＳ１３０－ＮＯ）、ステップＳ１２０に遷移し、次の対象撮像画像を選択する。なお、ステップＳ１２０において次の対象撮像画像として選択可能な撮像画像が存在しない場合、制御部３６は、ステップＳ１２０～ステップＳ１７０の繰り返し処理を終了させ、図５に示したフローチャートの処理を終了する。
【0083】
一方、第１推定部３６３は、選択された対象撮像画像に、対象空間Ｒ内における４つの物体のうちの少なくとも１つが撮像されていると第２推定部３６４が判定した場合（ステップＳ１３０－ＹＥＳ）、対象撮像画像に対応付けられた時刻情報に基づいて、対象撮像画像が撮像された時刻を特定する。そして、第１推定部３６３は、予め決められた初期値と、所定の軌道の移動中における制御対象装置１０の速度の履歴とに基づいて、特定した時刻毎の制御対象装置位置姿勢を推定する（ステップＳ１４０）。ここで、予め決められた初期値は、制御対象装置１０のワールド座標系における位置及び姿勢を表すベクトルが有する３つの要素それぞれについての初期値のことである。これら３つの初期値は、如何なる値であってもよい。
【0084】
次に、第２推定部３６４は、対象撮像画像に基づいて、対象空間Ｒ内における４つの物体のうち対象撮像画像に撮像されている１つ以上の物体についての物体装置間相対位置姿勢を推定する（ステップＳ１５０）。ここで、例えば、第２推定部３６４は、対象撮像画像に、当該４つの物体のうちのある物体が撮像されていた場合、当該物体のマーカーＭＫＲから、第１符号化情報及び第２符号化情報を読み出す。これにより、第２推定部３６４は、当該物体が当該４つの物体のうちのいずれであるかを識別することができ、且つ、ステップＳ１４０において特定された時刻における当該物体についての物体装置間相対位置姿勢情報を推定することができる。第２推定部３６４は、ステップ１５０において、このような推定を、対象撮像画像に撮像されている１つ以上の物体のそれぞれについて行う。
【0085】
次に、取得部３６５は、ステップＳ１５０において第２推定部３６４が推定した推定結果を取得する。そして、生成部３６６は、この推定結果と、ステップＳ１１０において記憶部３２から読み出されたレイアウト拘束条件情報と、ステップＳ１４０において第１推定部３６３により推定された推定結果とに基づいて、対象空間Ｒ内における４つの物体のうち対象撮像画像に撮像されている１つ以上の物体と、当該１つ以上の物体のそれぞれとグループを構成している１つ以上の物体とのそれぞれについての物体位置姿勢を推定する（ステップＳ１６０）。なお、ステップＳ１６０の推定方法については、＜レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける情報行列の各要素の足し上げ方法＞において説明済であるため、ここでの詳細な説明を省略する。また、ステップＳ１６０において、生成部３６６は、対象撮像画像に撮像されている１つ以上の物体のそれぞれについて、前述の第１推定位置姿勢を表すベクトルと第２推定位置姿勢を表すベクトルとの差分ベクトルの内積を算出し、算出した当該内積が第１閾値を超えるか否かを判定する構成であってもよい。この場合、例えば、生成部３６６は、対象撮像画像に物体Ｍ１が撮像されていた場合、ステップＳ１６０において、物体Ｍ１についての第１推定位置姿勢を表すベクトルと、物体Ｍ１についての第２推定位置姿勢を表すベクトルとの差分ベクトルの内積を算出し、算出した当該内積が第１閾値を超えるか否かを判定する。そして、生成部３６６は、ステップＳ１２０～ステップＳ１７０の繰り返し処理において、物体Ｍ１について算出された内積が第１閾値を超えていると現在までに判定した回数が第２閾値を超えた場合、物体Ｍ１と物体Ｍ２との間に関係する要素を０に置き換えるレイアウト解除処理を行う。
【0086】
次に、生成部３６６は、ステップＳ１６０における推定結果と、ステップＳ１１０において読み出されたレイアウト拘束条件情報とに基づいて、情報行列Ωの各要素の足し上げと、情報ベクトルξの各要素の足し上げとを行う（ステップＳ１７０）。なお、ステップＳ１７０の足し上げ方法については、＜レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける情報行列の各要素の足し上げ方法＞において説明済であるため、ここでの詳細な説明を省略する。
【0087】
ステップＳ１７０の処理が行われた後、制御部３６は、ステップＳ１２０に遷移し、次の対象撮像画像を選択する。なお、ステップＳ１２０において次の対象撮像画像として選択可能な撮像画像が存在しない場合、制御部３６は、ステップＳ１２０～ステップＳ１７０の繰り返し処理を終了させ、図５に示したフローチャートの処理を終了する。
【0088】
なお、取得部３６５は、ステップＳ１４０において第１推定部３６３により推定される推定結果と、ステップＳ１５０において推定される推定結果とを、他の装置から取得する構成であってもよい。この場合、生成部３６６は、取得部３６５により取得されたこれらの推定結果に基づいて、ステップＳ１６０の処理及びステップＳ１７０の処理を行う。これにより、情報処理装置３０は、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける情報行列Ω及び情報ベクトルξを生成することができる。
【0089】
＜情報処理装置が地図情報を生成する処理＞
以下、図６を参照し、情報処理装置３０が地図情報を生成する処理について説明する。図６は、情報処理装置３０が地図情報を生成する処理の流れの一例を示す図である。なお、以下では、一例として、図６に示したステップＳ２１０の処理が行われるよりも前のタイミングにおいて、図５に示したフローチャートの処理によって情報処理装置３０が情報行列Ω及び情報ベクトルξを生成している場合について説明する。また、以下では、一例として、当該タイミングにおいて、情報処理装置３０が当該処理を開始する操作を予め受け付けている場合について説明する。
以下、図６を参照し、情報処理装置３０が地図情報を生成する処理について説明する。図６は、情報処理装置３０が地図情報を生成する処理の流れの一例を示す図である。なお、以下では、一例として、図６に示したステップＳ２１０の処理が行われるよりも前のタイミングにおいて、図５に示したフローチャートの処理によって情報処理装置３０が情報行列Ω及び情報ベクトルξを生成している場合について説明する。また、以下では、一例として、当該タイミングにおいて、情報処理装置３０が当該処理を開始する操作を予め受け付けている場合について説明する。
【0090】
生成部３６６は、予め生成している情報行列Ω及び情報ベクトルξに基づいて、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭにおける評価関数を生成する（ステップＳ２１０）。なお、この評価関数を生成する方法については、従来のＧｒａｐｈ－ＳＬＡＭアルゴリズムにおける方法であってもよく、これから開発される方法であってもよい。
【0091】
次に、生成部３６６は、ステップＳ２１０において生成された評価関数に基づく最適化処理を行う（ステップＳ２２０）。なお、ステップＳ２２０における最適化処理において用いられる最適化の手法については、既知の手法であってもよく、これから開発される手法であってもよい。これにより、生成部３６６は、対象空間Ｒ内における４つの物体それぞれについてのワールド座標系における位置及び姿勢を推定することができる。
【0092】
次に、生成部３６６は、ステップＳ２２０における推定結果に基づいて、対象空間Ｒ内の地図を示す地図情報を生成し（ステップＳ２３０）、図６に示したフローチャートの処理を終了する。
【0093】
以上のように、情報処理装置３０は、予め決められた初期値に基づいて制御対象装置位置姿勢を推定し、対象空間Ｒ内における４つの物体のうちの少なくとも１つの物体についての物体装置間相対位置姿勢情報を取得し、記憶部３２に記憶されたレイアウト拘束条件情報と、取得した当該少なくとも１つの物体についての物体装置間相対位置姿勢情報と、推定した制御対象装置位置姿勢とに基づいて、対象空間Ｒ内の地図を示す地図情報を生成する。これにより、情報処理装置３０は、対象空間Ｒ内における４つの物体の配置が初期配置から変更されていたとしても、対象空間Ｒ内の地図を示す地図情報を精度よく生成することができる。
【0094】
＜シミュレーターによる情報処理装置の性能試験結果＞
以下、図を参照し、シミュレーターによる情報処理装置３０の性能試験結果について説明する。図７は、情報処理装置３０により生成された地図情報の一例を示す図である。ただし、図７に示した地図情報の生成において、情報処理装置３０は、レイアウト拘束条件情報を用いない従来のＧｒａｐｈ－ＳＬＡＭアルゴリズムに基づいて、制御対象装置１０が位置する空間内の地図を示す地図情報を生成した。図７に示した地図情報が示す地図の空間は、レイアウト拘束によって互いの相対的な位置及び姿勢が保持されている８つの物体が配置された空間である。また、この空間内には、当該Ｇｒａｐｈ－ＳＬＡＭアルゴリズムにおける繰り返し処理の収束性を向上させるため、ワールド座標系における位置及び姿勢が動かないように固定されている４つのランドマークＬＭ１～ＬＭ４が配置されている。その結果、地図全体の精度は、従来手法よりも改善される。ただし、これら４つのランドマークＬＭ１～ＬＭ４は、図７に示した地図情報の物体間の相対的な位置関係の精度には、ほぼ影響を与えない。このため、この空間内には、これら４つのランドマークＬＭ１～ＬＭ４のうちの一部又は全部は、配置されていなくてもよい。図７では、この空間内に配置されている当該８つの物体が実際に位置している位置を、８つの点線の四角形Ｍｓによって示している。また、図７では、当該８つの物体の実際の姿勢を、８つの点線の四角形Ｍｓのそれぞれに付随した矢印によって示している。また、図７において点線で示した経路Ｖ１は、制御対象装置１０により推定された制御対象装置１０の移動経路を示す。一方、図７において点線で示した経路Ｖ２は、実際の制御対象装置１０の移動経路を示す。すなわち、図７に示した例では、情報処理装置３０は、検出部２０に撮像を行わせながら、所定の軌道として経路Ｖ２に沿った移動を制御対象装置１０に行わせている。そして、図７に実線で示した８つの四角形ＶＭｓのそれぞれは、制御対象装置１０によって推定された当該８つの物体それぞれの位置を示している。また、図７に実線で示した８つの四角形ＶＭｓのそれぞれに付随する矢印は、制御対象装置１０によって推定された当該８つの物体それぞれの姿勢を示している。図７に示したように、レイアウト拘束条件情報を用いない従来のＧｒａｐｈ－ＳＬＡＭアルゴリズムを用いた場合、情報処理装置３０は、当該８つの物体それぞれの位置及び姿勢を、精度よく推定できていないことが分かる。これは、８つの四角形Ｍｓと８つの四角形ＶＭｓとの一致度合いの低さと、これら４角形のそれぞれに付随する矢印同士の一致度合いの低さとのそれぞれから明らかである。
以下、図を参照し、シミュレーターによる情報処理装置３０の性能試験結果について説明する。図７は、情報処理装置３０により生成された地図情報の一例を示す図である。ただし、図７に示した地図情報の生成において、情報処理装置３０は、レイアウト拘束条件情報を用いない従来のＧｒａｐｈ－ＳＬＡＭアルゴリズムに基づいて、制御対象装置１０が位置する空間内の地図を示す地図情報を生成した。図７に示した地図情報が示す地図の空間は、レイアウト拘束によって互いの相対的な位置及び姿勢が保持されている８つの物体が配置された空間である。また、この空間内には、当該Ｇｒａｐｈ－ＳＬＡＭアルゴリズムにおける繰り返し処理の収束性を向上させるため、ワールド座標系における位置及び姿勢が動かないように固定されている４つのランドマークＬＭ１～ＬＭ４が配置されている。その結果、地図全体の精度は、従来手法よりも改善される。ただし、これら４つのランドマークＬＭ１～ＬＭ４は、図７に示した地図情報の物体間の相対的な位置関係の精度には、ほぼ影響を与えない。このため、この空間内には、これら４つのランドマークＬＭ１～ＬＭ４のうちの一部又は全部は、配置されていなくてもよい。図７では、この空間内に配置されている当該８つの物体が実際に位置している位置を、８つの点線の四角形Ｍｓによって示している。また、図７では、当該８つの物体の実際の姿勢を、８つの点線の四角形Ｍｓのそれぞれに付随した矢印によって示している。また、図７において点線で示した経路Ｖ１は、制御対象装置１０により推定された制御対象装置１０の移動経路を示す。一方、図７において点線で示した経路Ｖ２は、実際の制御対象装置１０の移動経路を示す。すなわち、図７に示した例では、情報処理装置３０は、検出部２０に撮像を行わせながら、所定の軌道として経路Ｖ２に沿った移動を制御対象装置１０に行わせている。そして、図７に実線で示した８つの四角形ＶＭｓのそれぞれは、制御対象装置１０によって推定された当該８つの物体それぞれの位置を示している。また、図７に実線で示した８つの四角形ＶＭｓのそれぞれに付随する矢印は、制御対象装置１０によって推定された当該８つの物体それぞれの姿勢を示している。図７に示したように、レイアウト拘束条件情報を用いない従来のＧｒａｐｈ－ＳＬＡＭアルゴリズムを用いた場合、情報処理装置３０は、当該８つの物体それぞれの位置及び姿勢を、精度よく推定できていないことが分かる。これは、８つの四角形Ｍｓと８つの四角形ＶＭｓとの一致度合いの低さと、これら４角形のそれぞれに付随する矢印同士の一致度合いの低さとのそれぞれから明らかである。
【0095】
一方、図８は、情報処理装置３０により生成された地図情報の他の例を示す図である。ただし、図８に示した地図情報の生成において、情報処理装置３０は、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭに基づいて、図７に示した空間と同じ空間内の地図を示す地図情報を生成した。図８に示したように、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭを用いた場合、情報処理装置３０は、レイアウト拘束条件情報を用いない従来のＧｒａｐｈ－ＳＬＡＭアルゴリズムを用いた図７の結果と比較して、８つの物体それぞれの位置及び姿勢を、精度よく推定できていることが分かる。これは、図７における８つの四角形Ｍｓと８つの四角形ＶＭｓとの一致度合いよりも、図８における８つの四角形Ｍｓと８つの四角形ＶＭｓとの一致度合いの方が高いことからも明らかである。また、これは、図７における８つの四角形Ｍｓと８つの四角形ＶＭｓとのそれぞれに付随した矢印の一致度合いよりも、図８における８つの四角形Ｍｓと８つの四角形ＶＭｓとのそれぞれに付随した矢印の一致度合いの方が高いことからも明らかである。すなわち、情報処理装置３０は、制御対象装置１０が位置する空間内における複数の物体の配置が初期配置から変更されていたとしても、当該空間内の地図を示す地図情報を精度よく生成することができる。なお、図８に示した結果を得るために用いたレイアウト拘束Ｇｒａｐｈ－ＳＬＡＭは、前述のレイアウト解除処理を行う構成であってもよく、レイアウト解除処理を行わない構成であってもよい。
【0096】
次に、図９は、情報処理装置３０により生成された地図情報の更に他の例を示す図である。ただし、図９に示した地図情報の生成において、情報処理装置３０は、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭに基づいて、制御対象装置１０が位置する空間内の地図を示す地図情報を生成した。図９に示した地図情報が示す地図の空間は、一部を除いて、図７に示した空間と同じ空間である。図９に示した空間では、レイアウト拘束によって互いの相対的な位置及び姿勢が保持されていた８つの物体のうちの１つの物体が、残りの７つの物体から離れた位置に移動させられている。このような１つの物体の移動が、図７に示した空間と、図９に示した空間との違いである。図９では、当該７つの物体が実際に位置している位置を、７つの点線の四角形Ｍｓｓによって示している。また、図９では、当該７つの物体の実際の姿勢を、７つの点線の四角形Ｍｓｓのそれぞれに付随した矢印によって示している。また、図９では、レイアウト拘束を解かれている当該１つの物体が実際に位置している位置を、点線の四角形Ｍｓ１によって示している。また、図９では、当該１つの物体の実際の姿勢を、点線の四角形Ｍｓ１に付随した矢印によって示している。そして、図９に実線で示した７つの四角形ＶＭｓｓのそれぞれは、制御対象装置１０によって推定された当該７つの物体それぞれの位置を示している。また、図９に実線で示した７つの四角形ＶＭｓｓのそれぞれに付随する矢印は、制御対象装置１０によって推定された当該７つの物体それぞれの姿勢を示している。また、図９に実線で示した四角形ＶＭｓ１は、制御対象装置１０によって推定された当該１つの物体の位置を示している。また、図９に実線で示した四角形ＶＭｓ１に付随する矢印は、制御対象装置１０によって推定された当該１つの物体の姿勢を示している。ここで、図９は、情報処理装置３０は、レイアウト解除処理を行わないレイアウト拘束Ｇｒａｐｈ－ＳＬＡＭに基づいて、この空間内の地図を示す地図情報を生成した結果である。図９に示したように、この場合、情報処理装置３０は、８つの物体それぞれの位置及び姿勢を、精度よく推定できていないことが分かる。これは、７つの四角形Ｍｓｓと７つの四角形ＶＭｓｓとの一致度合いの低さから明らかであるが、四角形Ｍｓ１と四角形ＶＭｓ１とが離れていることからも更に明らかである。
【0097】
一方、図１０は、情報処理装置３０により生成された地図情報の更に更に他の例を示す図である。ただし、図１０に示した地図情報の生成において、情報処理装置３０は、レイアウト解除処理を行うレイアウト拘束Ｇｒａｐｈ－ＳＬＡＭに基づいて、図９に示した空間と同じ空間内の地図を示す地図情報を生成した。図１０に示したように、レイアウト解除処理を行うレイアウト拘束Ｇｒａｐｈ－ＳＬＡＭを用いた場合、情報処理装置３０は、図９の結果と比較して、７つの物体それぞれの位置及び姿勢と、１つの物体の位置及び姿勢とを、精度よく推定できていることが分かる。すなわち、情報処理装置３０は、レイアウト解除処理を行うレイアウト拘束Ｇｒａｐｈ－ＳＬＡＭを用いることにより、制御対象装置１０が位置する空間内における複数の物体の配置が初期配置から変更されていたとしても、当該空間内の地図を示す地図情報を、より確実に精度よく生成することができる。
【0098】
なお、上記において説明した制御対象装置１０は、前述した通り、ドローンに代えて、車輪を有する移動可能なロボットであってもよい。図１１は、実施形態に係る制御システム１の構成の他の例を示す図である。この場合であっても、この場合であっても、情報処理装置３０は、レイアウト拘束Ｇｒａｐｈ－ＳＬＡＭに基づいて、対象空間Ｒ内における４つの物体それぞれについてのワールド座標系における位置及び姿勢の推定を行い、対象空間Ｒ内の地図を示す地図情報を生成することができる。
【0099】
また、上記において説明した物体Ｍ１～物体Ｍ４のうちの少なくとも１つの物体には、複数のマーカーが設けられる構成であってもよい。図１２は、３つのマーカーが設けられた物体Ｍ１の一例を示す図である。図１２に示した物体Ｍ１には、マーカーＭＫＲ１－１、マーカーＭＫＲ１－２、マーカーＭＫＲ１－３の３つのマーカーが設けられている。これにより、検出部２０により撮像される撮像画像には、物体Ｍ１に設けられたマーカーが撮像されやすくなる。また、情報処理装置３０がこれら３つのマーカーのそれぞれを識別可能な場合、情報処理装置３０は、物体Ｍ１が撮像された撮像画像に基づいて、物体Ｍ１の姿勢をより精度よく検出することができる。これら３つのマーカーを識別可能なマーカーとする方法としては、ＱＲコード（登録商標）とＡＲコードとを組み合わせたＨｕｅｃｏｄｅを用いる方法を採用することが望ましい。Ｈｕｅｃｏｄｅについては、既に公知であるため、詳細な説明を省略する。Ｈｕｅｃｏｄｅについては、例えば、特願２０２０－１６９１１５号の特許文献等に詳細な説明が記載されている。
【0100】
なお、上記において説明した位置及び姿勢は、位置のみであってもよい。この場合、上記において説明した姿勢については、各種の処理に使用されず、位置のみを用いて各種の処理等が実行される。この場合、例えば、位置及び姿勢を示す各種の情報は、位置を示す情報となる。
また、上記において説明した位置及び姿勢は、姿勢のみであってもよい。この場合、上記において説明した位置については、各種の処理に使用されず、姿勢のみを用いて各種の処理等が実行される。この場合、例えば、位置及び姿勢を示す各種の情報は、姿勢を示す情報となる。
また、上記において説明した位置及び姿勢は、姿勢のみであってもよい。この場合、上記において説明した位置については、各種の処理に使用されず、姿勢のみを用いて各種の処理等が実行される。この場合、例えば、位置及び姿勢を示す各種の情報は、姿勢を示す情報となる。
【0101】
以上のように、実施形態に係る情報処理装置（上記において説明した例では、情報処理装置３０）は、制御対象となる制御対象装置（上記において説明した例では、制御対象装置１０）を制御する情報処理装置であって、制御対象装置が位置する空間（上記において説明した例では、対象空間Ｒ）内に配置された第１物体（上記において説明した例では、例えば、物体Ｍ１）と、空間内に配置された第１物体と異なる第２物体（上記において説明した例では、例えば、物体Ｍ２）との相対的な位置及び姿勢を示す情報を含むレイアウト拘束条件情報を記憶する記憶部（上記において説明した例では、記憶部３２）と、制御対象装置の位置及び姿勢と、第１物体と制御対象装置との相対的な位置及び姿勢を示す第１物体装置間相対位置姿勢情報と、第２物体と制御対象装置との相対的な位置及び姿勢を示す第２物体装置間相対位置姿勢情報との少なくとも一方と、記憶部に記憶されたレイアウト拘束条件情報とに基づいて、空間内の地図を示す地図情報を生成する制御部（上記において説明した例では、制御部３６）と、を備える。これにより、情報処理装置は、制御対象装置が位置する空間内における複数の物体の配置が初期配置から変更されていたとしても、当該空間内の地図を示す地図情報を精度よく生成することができる。
【0102】
また、情報処理装置において、制御部は、予め決められた初期値に基づいて制御対象装置の位置及び姿勢を推定する第１推定部（上記において説明した例では、第１推定部３６３）と、第１物体装置間相対位置姿勢情報と、第２物体装置間相対位置姿勢情報との少なくとも一方を物体装置間相対位置姿勢情報として取得する取得部（上記において説明した例では、取得部３６５）と、記憶部に記憶されたレイアウト拘束条件情報と、取得部により取得された物体装置間相対位置姿勢情報と、第１推定部により推定された制御対象装置の位置及び姿勢とに基づいて、空間内の地図を示す地図情報を生成する生成部（上記において説明した例では、生成部３６６）と、を備える、構成が用いられてもよい。
【0103】
また、情報処理装置は、制御部は、第１物体と第２物体との少なくとも一方を検出する検出部（上記において説明した例では、検出部２０）からの出力に基づいて、第１物体と制御対象装置との相対的な位置及び姿勢と、第２物体と制御対象装置との相対的な位置及び姿勢との少なくとも一方を推定する第２推定部（上記において説明した例では、第２推定部３６４）を更に備え、取得部は、第１物体と制御対象装置との相対的な位置及び姿勢と、第２物体と制御対象装置との相対的な位置及び姿勢との少なくとも一方を示す情報を、物体装置間相対位置姿勢情報として第２推定部から取得する、構成が用いられてもよい。
【0104】
また、情報処理装置では、生成部は、Ｇｒａｐｈ－ＳＬＡＭに基づいて、地図情報を生成する、構成が用いられてもよい。
【0105】
また、情報処理装置では、生成部は、取得部により取得された物体装置間相対位置姿勢情報と、記憶部により記憶されたレイアウト拘束条件情報と、第１推定部により推定された制御対象装置の位置及び姿勢とに基づいて、Ｇｒａｐｈ－ＳＬＡＭにおける情報行列及び情報ベクトルを生成し、生成した情報行列及び情報ベクトルに基づく評価関数の最適化を行うことにより、地図情報を生成する、構成が用いられてもよい。
【0106】
また、情報処理装置では、生成部は、取得部により取得された物体装置間相対位置姿勢情報と、記憶部により記憶されたレイアウト拘束条件情報と、第１推定部により推定された制御対象装置の位置及び姿勢とに基づいて、第１物体及び第２物体それぞれの位置及び姿勢を推定し、推定した第１物体及び第２物体それぞれの位置及び姿勢に基づいて、情報行列及び情報ベクトルを生成する、構成が用いられてもよい。
【0107】
また、情報処理装置では、生成部は、第１物体装置間相対位置姿勢情報と第２物体装置間相対位置姿勢情報との両方が物体装置間相対位置姿勢情報として取得部により取得された場合、第１推定部により推定された制御対象装置の位置及び姿勢と、取得部により取得された物体装置間相対位置姿勢情報と、記憶部により記憶されたレイアウト拘束条件情報とに基づいて、第１物体の位置及び姿勢を第１推定位置姿勢として推定するとともに第２物体の位置及び姿勢を第２推定位置姿勢として推定し、且つ、第１推定部により推定された制御対象装置の位置及び姿勢と、取得部により取得された物体装置間相対位置姿勢情報とに基づいて、第１物体の位置及び姿勢を第３推定位置姿勢として推定するとともに第２物体の位置及び姿勢を第４推定位置姿勢として推定し、第１推定位置姿勢を示すベクトルと第３推定位置姿勢を示すベクトルとの差を第１差分として算出するとともに、第２推定位置姿勢を示すベクトルと第４推定位置姿勢を示すベクトルとの差を第２差分として算出し、第１差分と、第１物体の位置及び姿勢についての推定誤差と、第２差分と、第２物体の位置及び姿勢についての推定誤差とに基づいて、情報行列が有する要素のうちレイアウト拘束条件情報に応じて決められた要素の繋がりを削除する、構成が用いられてもよい。
【0108】
また、情報処理装置では、制御対象装置は、制御対象装置の位置及び姿勢を変化させることができる移動体である、構成が用いられてもよい。
【0109】
また、以上のように、実施形態に係る情報処理装置（上記において説明した例では、情報処理装置３０）は、制御対象となる制御対象装置（上記において説明した例では、制御対象装置１０）を制御する情報処理装置であって、制御対象装置が位置する空間（上記において説明した例では、対象空間Ｒ）内に配置された第１物体（上記において説明した例では、例えば、物体Ｍ１）と、空間内に配置された第１物体と異なる第２物体（上記において説明した例では、例えば、物体Ｍ２）との相対的な位置を示す情報を含むレイアウト拘束条件情報を記憶する記憶部（上記において説明した例では、記憶部３２）と、制御対象装置の位置と、第１物体と制御対象装置との相対的な位置を示す第１物体装置間相対位置情報と、第２物体と制御対象装置との相対的な位置を示す第２物体装置間相対位置情報との少なくとも一方と、記憶部に記憶されたレイアウト拘束条件情報とに基づいて、空間内の地図を示す地図情報を生成する制御部（上記において説明した例では、制御部３６）と、を備える。これにより、情報処理装置は、制御対象装置が位置する空間内における複数の物体の配置が初期配置から変更されていたとしても、当該空間内の地図を示す地図情報を精度よく生成することができる。
【0110】
また、情報処理装置において、制御部は、予め決められた初期値に基づいて制御対象装置の位置を推定する第１推定部（上記において説明した例では、第１推定部３６３）と、第１物体装置間相対位置情報と、第２物体装置間相対位置情報との少なくとも一方を物体装置間相対位置情報として取得する取得部（上記において説明した例では、取得部３６５）と、記憶部に記憶されたレイアウト拘束条件情報と、取得部により取得された物体装置間相対位置情報と、第１推定部により推定された制御対象装置の位置とに基づいて、空間内の地図を示す地図情報を生成する生成部（上記において説明した例では、生成部３６６）と、を備える、構成が用いられてもよい。
【0111】
また、情報処理装置は、制御部は、第１物体と第２物体との少なくとも一方を検出する検出部（上記において説明した例では、検出部２０）からの出力に基づいて、第１物体と制御対象装置との相対的な位置と、第２物体と制御対象装置との相対的な位置との少なくとも一方を推定する第２推定部（上記において説明した例では、第２推定部３６４）を更に備え、取得部は、第１物体と制御対象装置との相対的な位置と、第２物体と制御対象装置との相対的な位置との少なくとも一方を示す情報を、物体装置間相対位置情報として第２推定部から取得する、構成が用いられてもよい。
【0112】
また、情報処理装置では、生成部は、Ｇｒａｐｈ－ＳＬＡＭに基づいて、地図情報を生成する、構成が用いられてもよい。
【0113】
また、情報処理装置では、生成部は、取得部により取得された物体装置間相対位置情報と、記憶部により記憶されたレイアウト拘束条件情報と、第１推定部により推定された制御対象装置の位置とに基づいて、Ｇｒａｐｈ－ＳＬＡＭにおける情報行列及び情報ベクトルを生成し、生成した情報行列及び情報ベクトルに基づく評価関数の最適化を行うことにより、地図情報を生成する、構成が用いられてもよい。
【0114】
また、情報処理装置では、生成部は、取得部により取得された物体装置間相対位置情報と、記憶部により記憶されたレイアウト拘束条件情報と、第１推定部により推定された制御対象装置の位置とに基づいて、第１物体及び第２物体それぞれの位置を推定し、推定した第１物体及び第２物体それぞれの位置に基づいて、情報行列及び情報ベクトルを生成する、構成が用いられてもよい。
【0115】
また、情報処理装置では、生成部は、第１物体装置間相対位置情報と第２物体装置間相対位置情報との両方が物体装置間相対位置情報として取得部により取得された場合、第１推定部により推定された制御対象装置の位置と、取得部により取得された物体装置間相対位置情報と、記憶部により記憶されたレイアウト拘束条件情報とに基づいて、第１物体の位置を第１推定位置として推定するとともに第２物体の位置を第２推定位置として推定し、且つ、第１推定部により推定された制御対象装置の位置と、取得部により取得された物体装置間相対位置情報とに基づいて、第１物体の位置を第３推定位置として推定するとともに第２物体の位置を第４推定位置として推定し、第１推定位置を示すベクトルと第３推定位置を示すベクトルとの差を第１差分として算出するとともに、第２推定位置を示すベクトルと第４推定位置を示すベクトルとの差を第２差分として算出し、第１差分と、第１物体の位置についての推定誤差と、第２差分と、第２物体の位置についての推定誤差とに基づいて、情報行列が有する要素のうちレイアウト拘束条件情報に応じて決められた要素の繋がりを削除する、構成が用いられてもよい。
【0116】
また、情報処理装置では、制御対象装置は、制御対象装置の位置及び姿勢の少なくとも一方を変化させることができる移動体である、構成が用いられてもよい。
【0117】
また、以上のように、実施形態に係る情報処理装置（上記において説明した例では、情報処理装置３０）は、制御対象となる制御対象装置（上記において説明した例では、制御対象装置１０）を制御する情報処理装置であって、制御対象装置が位置する空間（上記において説明した例では、対象空間Ｒ）内に配置された第１物体（上記において説明した例では、例えば、物体Ｍ１）と、空間内に配置された第１物体と異なる第２物体（上記において説明した例では、例えば、物体Ｍ２）との相対的な姿勢を示す情報を含むレイアウト拘束条件情報を記憶する記憶部（上記において説明した例では、記憶部３２）と、制御対象装置の姿勢と、第１物体と制御対象装置との相対的な姿勢を示す第１物体装置間相対姿勢情報と、第２物体と制御対象装置との相対的な姿勢を示す第２物体装置間相対姿勢情報との少なくとも一方と、記憶部に記憶されたレイアウト拘束条件情報とに基づいて、空間内の地図を示す地図情報を生成する制御部（上記において説明した例では、制御部３６）と、を備える。これにより、情報処理装置は、制御対象装置が位置する空間内における複数の物体の配置が初期配置から変更されていたとしても、当該空間内の地図を示す地図情報を精度よく生成することができる。
【0118】
また、情報処理装置において、制御部は、予め決められた初期値に基づいて制御対象装置の姿勢を推定する第１推定部（上記において説明した例では、第１推定部３６３）と、第１物体装置間相対姿勢情報と、第２物体装置間相対姿勢情報との少なくとも一方を物体装置間相対姿勢情報として取得する取得部（上記において説明した例では、取得部３６５）と、記憶部に記憶されたレイアウト拘束条件情報と、取得部により取得された物体装置間相対姿勢情報と、第１推定部により推定された制御対象装置の姿勢とに基づいて、空間内の地図を示す地図情報を生成する生成部（上記において説明した例では、生成部３６６）と、を備える、構成が用いられてもよい。
【0119】
また、情報処理装置は、制御部は、第１物体と第２物体との少なくとも一方を検出する検出部（上記において説明した例では、検出部２０）からの出力に基づいて、第１物体と制御対象装置との相対的な姿勢と、第２物体と制御対象装置との相対的な姿勢との少なくとも一方を推定する第２推定部（上記において説明した例では、第２推定部３６４）を更に備え、取得部は、第１物体と制御対象装置との相対的な姿勢と、第２物体と制御対象装置との相対的な姿勢との少なくとも一方を示す情報を、物体装置間相対姿勢情報として第２推定部から取得する、構成が用いられてもよい。
【0120】
また、情報処理装置では、生成部は、Ｇｒａｐｈ－ＳＬＡＭに基づいて、地図情報を生成する、構成が用いられてもよい。
【0121】
また、情報処理装置では、生成部は、取得部により取得された物体装置間相対姿勢情報と、記憶部により記憶されたレイアウト拘束条件情報と、第１推定部により推定された制御対象装置の姿勢とに基づいて、Ｇｒａｐｈ－ＳＬＡＭにおける情報行列及び情報ベクトルを生成し、生成した情報行列及び情報ベクトルに基づく評価関数の最適化を行うことにより、地図情報を生成する、構成が用いられてもよい。
【0122】
また、情報処理装置では、生成部は、取得部により取得された物体装置間相対姿勢情報と、記憶部により記憶されたレイアウト拘束条件情報と、第１推定部により推定された制御対象装置の姿勢とに基づいて、第１物体及び第２物体それぞれの姿勢を推定し、推定した第１物体及び第２物体それぞれの姿勢に基づいて、情報行列及び情報ベクトルを生成する、構成が用いられてもよい。
【0123】
また、情報処理装置では、生成部は、第１物体装置間相対姿勢情報と第２物体装置間相対姿勢情報との両方が物体装置間相対姿勢情報として取得部により取得された場合、第１推定部により推定された制御対象装置の姿勢と、取得部により取得された物体装置間相対姿勢情報と、記憶部により記憶されたレイアウト拘束条件情報とに基づいて、第１物体の姿勢を第１推定姿勢として推定するとともに第２物体の姿勢を第２推定姿勢として推定し、且つ、第１推定部により推定された制御対象装置の姿勢と、取得部により取得された物体装置間相対姿勢情報とに基づいて、第１物体の姿勢を第３推定姿勢として推定するとともに第２物体の姿勢を第４推定姿勢として推定し、第１推定姿勢を示すベクトルと第３推定姿勢を示すベクトルとの差を第１差分として算出するとともに、第２推定姿勢を示すベクトルと第４推定姿勢を示すベクトルとの差を第２差分として算出し、第１差分と、第１物体の姿勢についての推定誤差と、第２差分と、第２物体の姿勢についての推定誤差とに基づいて、情報行列が有する要素のうちレイアウト拘束条件情報に応じて決められた要素の繋がりを削除する、構成が用いられてもよい。
【0124】
また、情報処理装置では、制御対象装置は、制御対象装置の位置及び姿勢の少なくとも一方を変化させることができる移動体である、構成が用いられてもよい。
【0125】
以上、この発明の実施形態を、図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、この発明の要旨を逸脱しない限り、変更、置換、削除等されてもよい。
【0126】
また、以上に説明した装置（例えば、制御対象装置１０、検出部２０、情報処理装置３０等）における任意の構成部の機能を実現するためのプログラムを、コンピュータ読み取り可能な記録媒体に記録し、そのプログラムをコンピュータシステムに読み込ませて実行するようにしてもよい。なお、ここでいう「コンピュータシステム」とは、ＯＳ（Operating System）や周辺機器等のハードウェアを含むものとする。また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ（Read Only Memory）、ＣＤ（Compact Disk）－ＲＯＭ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピュータ読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムが送信された場合のサーバーやクライアントとなるコンピュータシステム内部の揮発性メモリー（ＲＡＭ（Random Access Memory））のように、一定時間プログラムを保持しているものも含むものとする。
【0127】
また、上記のプログラムは、このプログラムを記憶装置等に格納したコンピュータシステムから、伝送媒体を介して、あるいは、伝送媒体中の伝送波により他のコンピュータシステムに伝送されてもよい。ここで、プログラムを伝送する「伝送媒体」は、インターネット等のネットワーク（通信網）や電話回線等の通信回線（通信線）のように情報を伝送する機能を有する媒体のことをいう。
また、上記のプログラムは、前述した機能の一部を実現するためのものであってもよい。さらに、上記のプログラムは、前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であってもよい。
また、上記のプログラムは、前述した機能の一部を実現するためのものであってもよい。さらに、上記のプログラムは、前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であってもよい。
【符号の説明】
【0128】
１…制御システム、１０…制御対象装置、２０…検出部、３０…情報処理装置、３１…ＣＰＵ、３２…記憶部、３３…入力受付部、３４…通信部、３５…表示部、３６…制御部、３６１…撮像制御部、３６２…画像取得部、３６３…第１推定部、３６４…第２推定部、３６５…取得部、３６６…生成部、３６７…移動体制御部、ＭＫＲ、ＭＫＲ１、ＭＫＲ２、ＭＫＲ３、ＭＫＲ４…マーカー、Ｒ…対象空間
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】