JP6594008B2 - 移動体制御装置、ランドマーク、および、プログラム - Google Patents

Description

本発明は、ＳＬＡＭ(Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ)技術を用いて移動体を制御する技術に関する。

自律走行ロボット等の移動体が自律的に移動しながら、自己位置の推定と環境地図の作成とを同時に行うための技術として、ＳＬＡＭ(Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ)が知られている。

ＳＬＡＭ技術により自律走行するロボット（移動体）では、ロボットに搭載されたセンサーから得られる観測、および、ロボットに対する制御入力のみを使用して、ロボットの周囲の環境地図を作成しながら、作成した環境地図中でのロボットの自己の位置および姿勢を推定する。言い換えれば、この推定処理は、ロボットがシステムモデルｘ_ｔ〜g（ｘ_ｔ｜ｘ_ｔ−１，Ｕ_ｔ）（Ｕ_ｔ:制御入力）に従い、観測が観測モデルＺ_ｔ〜ｈ（Ｚ_ｔ｜ｘ_ｔ，ｍ）（ｍ：環境地図）に従う状態空間モデルに対して、制御入力系列Ｕ_１：ｔ＝｛Ｕ_１，Ｕ_２，・・・，Ｕ_ｔ｝、および、観測系列Ｚ_１：ｔ＝｛Ｚ_１，Ｚ_２，・・・，Ｚ_ｔ｝が与えられたときに、ロボットの自己位置および姿勢Ｘ_ｔ（あるいは、Ｘ_１：ｔ＝｛Ｘ_１，Ｘ_２，・・・，Ｘ_ｔ｝）と、環境地図ｍとを推定する処理である。

この推定処理において、パーティクルフィルタを用いて、ロボットの姿勢を高精度で推定する技術が開発されている（例えば、特許文献１）。

観測を取得するためのセンサーとしては、可視光用イメージセンサーを用いたＲＧＢカメラや、赤外線やレーザーを使用して距離情報を取得するレンジファインダー等がよく使用される。また、観測を取得するために、２種類以上のセンサーを組み合わせて用いられることもある。

ＳＬＡＭ技術により自律走行するロボット（移動体）において、取得される環境地図は、幾何学的な地図ではなく、トポロジカルな地図として、取得されることが多い。環境地図は、例えば、ランドマークに基づく情報により、取得される。この場合、環境地図は、ランドマークの情報の集合として表現される。それぞれのランドマークのパラメータは、例えば、（１）ランドマークの位置情報、および、（２）ランドマークの位置情報の不確実性を表す共分散行列である。

ＳＬＡＭ技術により自律走行するロボットにおいて、ＲＧＢカメラをセンサーとして使用する場合、例えば、何らかの特徴を持つ点や線等、あるいは、判別可能なマーカーを貼付した物体等をランドマークとして設定し、ＲＧＢカメラにより撮像された画像中において、ランドマークに相当する画像領域を検出（特定）することで、ランドマークの情報を取得する。この場合、ロボットの実際の位置と、ランドマークの実際の位置との相対関係を、ＲＧＢカメラで取得した画像から特定し、特定した結果に基づいて、現時刻における観測が取得される。

特開２００８−１２６４０１号公報

しかしながら、上記のように、ＲＧＢカメラで撮像した画像からランドマークの位置を取得する場合、ロボットの実際の位置と、ランドマークの実際の位置とを精度よく推定することが困難な場合がある。ＲＧＢカメラやレンジファインダーは指向性が高く観測できる範囲が制限されるため、ロボットの周囲に多くのランドマークが設置されている場合であっても、ＲＧＢカメラやレンジファインダーにより取得されたデータから、一度に発見することができるランドマークの数は少ない。そして、例えば、（１）ロボットから発見したランドマークまでの距離と、（２）ロボットが移動する環境を、例えば、上方から見たときのロボットから発見したランドマークまでの直線と所定の基準軸とのなす角度と、をランドマークの情報として取得する場合、取得したランドマークの情報からロボットの位置を精度良く推定することが困難である場合がある。これは、発見されたランドマークの位置を中心として、ロボットから発見したランドマークまでの距離を半径とする円周上の点の全てが、ロボットの位置の候補点となるためである。つまり、ロボットの位置の候補点が多いので、現時刻のロボットの位置を推定するための処理量が多くなり、ロボットの位置を精度良く推定することが難しい。このように、自律走行ロボットにおいて、ロボットの位置を精度良く推定できないと、実際のロボットの内部状態と推定結果との間の誤差が大きくなり、ロボット（移動体）の安定した自律走行が困難になる。

そこで、本発明は、上記問題点に鑑み、ランドマークの情報から、移動体（ロボット）の位置を精度良く推定し、高精度な状態推定処理を高速かつ適切に実行することで、移動体の制御を適切に行う移動体制御装置、プログラム、および、それらに用いられるランドマークを実現することを目的とする。

上記課題を解決するために、第１の発明は、２個以上のランドマークを１組として含むランドマークの組であるランドマークセットが設置されている環境内を移動する移動体を制御するために、ランドマークの情報を用いて表現される環境地図の作成処理と、移動体の自己の内部状態の推定処理とを実行する移動体制御装置であって、観測取得部と、ランドマーク検出部と、候補領域取得部と、状態推定部と、を備える。

観測取得部は、観測可能な事象から得られる観測データを取得する。

ランドマーク検出部は、観測取得部により取得された観測データに基づいて、（１）移動体とランドマークセットに含まれる２個以上のランドマークのそれぞれとの距離を、ランドマーク距離情報として取得し、（２）移動体とランドマークセットに含まれる２個以上のランドマークのそれぞれとを結ぶ直線と所定の軸とのなす角度をランドマーク角度情報として取得する。

候補領域取得部は、ランドマーク検出部により取得されたランドマーク距離情報に基づいて、移動体の自己位置の候補領域を決定し、決定した候補領域についての情報を候補領域情報として取得する。

状態推定部は、観測取得部により取得された観測データと、ランドマーク検出部により生成されたランドマーク距離情報およびランドマーク角度情報と、候補領域取得部により取得された候補領域情報とに基づいて、移動体の内部状態を推定することで、自己内部状態推定データを取得するとともに、候補領域情報と、ランドマーク距離情報と、ランドマーク角度情報とに基づいて、環境地図を推定することで、環境地図データを取得する。

この移動体制御装置では、２つ以上のランドマークを１組とするランドマークセットを用いて、移動体から２つ以上のランドマークまでの距離を測定することで、移動体の位置を少ない候補点に絞り、高速かつ高精度に、移動体の位置情報を含む内部状態データを推定することができる。つまり、この移動体制御装置では、ランドマークの情報から、移動体の位置を精度良く推定し、高精度な状態推定処理を高速かつ適切に実行することができる。その結果、この移動体制御装置では、移動体の制御を適切に行うことができる。

さらに、この移動体制御装置では、上記の通り、１つのランドマークセットに含まれる２つ以上のランドマークと、移動体との距離を取得するだけで、移動体の位置を高速かつ高精度に推定することができる。このため、この移動体制御装置では、多くの内部状態変数を用いた複雑な処理をすることなく、少ない演算量で推定処理を実現することができる。

第２の発明は、３次元形状を有するランドマークであって、ランドマークの表面の一部の領域である第１表面領域は、第１パターンを有し、ランドマークの表面の一部の領域であって、第１表面領域以外の領域である第２表面領域は、第１パターンとは異なる第２パターンを有しているランドマークが設置されている環境内を移動する移動体を制御するために、ランドマークの情報を用いて表現される環境地図の作成処理と、移動体の自己の内部状態の推定処理とを実行する移動体制御装置である。移動体制御装置は、観測取得部と、ランドマーク検出部と、候補領域取得部と、状態推定部と、を備える。

観測取得部は、観測可能な事象から得られる観測データを取得し、ランドマークを撮像した画像をランドマーク撮像画像として、観測データに含めて取得する。

ランドマーク検出部は、観測取得部により取得された観測データに基づいて、（１）移動体とランドマークとの距離を、ランドマーク距離情報として取得し、（２）移動体とランドマークとを結ぶ直線と所定の軸とのなす角度をランドマーク角度情報として取得する。

候補領域取得部は、ランドマーク検出部により取得されたランドマーク距離情報と、ランドマーク撮像画像上でのランドマークの第１パターンと第２パターンとの視認状態とに基づいて、移動体の自己位置の候補領域を決定し、決定した候補領域についての情報を候補領域情報として取得する。

状態推定部は、観測取得部により取得された観測データと、ランドマーク検出部により生成されたランドマーク距離情報およびランドマーク角度情報と、候補領域取得部により取得された候補領域情報とに基づいて、移動体の内部状態を推定することで、自己内部状態推定データを取得するとともに、候補領域情報と、ランドマーク距離情報と、ランドマーク角度情報とに基づいて、環境地図を推定することで、環境地図データを取得する。

この移動体制御装置では、所定のパターンを有するランドマークを撮像した画像を実際の観測データとして取得し、撮像画像上のランドマークの見え方を解析することで、移動体の位置を精度良く推定し、高精度な状態推定処理を高速かつ適切に実行することができる。その結果、この移動体制御装置では、移動体の制御を適切に行うことができる。

さらに、この移動体制御装置では、ランドマークと、移動体との距離を取得するだけで、移動体の位置を高速かつ高精度に推定することができる。このため、この移動体制御装置では、多くの内部状態変数を用いた複雑な処理をすることなく、少ない演算量で推定処理を実現することができる。

なお、ランドマークの第１パターンと第２パターンとの「視認状態」とは、例えば、撮像画像上における、ランドマークの第１パターンに相当する画像領域、および、第２パターンに相当する画像領域の（１）位置、（２）形状、（３）面積等で特定される状態のことをいう。

第３の発明は、第２の発明であって、候補領域取得部は、ランドマーク距離情報と、ランドマーク撮像画像上でのランドマークの第１パターンと第２パターンとの占有比率とに基づいて、移動体の自己位置の候補領域を決定し、決定した候補領域についての情報を候補領域情報として取得する。

これにより、この移動体制御装置では、ランドマーク撮像画像上でのランドマークの第１パターンと第２パターンとの占有比率とに基づいて、移動体の自己位置の候補領域を決定することができる。

第４の発明は、第２または第３の発明である移動体制御装置とともに用いられる３次元形状を有するランドマークである。ランドマークは、ランドマークの表面の一部の領域である第１表面領域は、第１パターンを有し、ランドマークの表面の一部の領域であって、第１表面領域以外の領域である第２表面領域は、第１パターンとは異なる第２パターンを有している。

これにより、第２または第３の発明である移動体制御装置により効率良く移動体の内部状態データの推定処理、環境地図の取得処理を実現するためのランドマークを実現することができる。

第５の発明は、２個以上のランドマークを１組として含むランドマークの組であるランドマークセットが設置されている環境内を移動する移動体を制御するために、ランドマークの情報を用いて表現される環境地図の作成処理と、移動体の自己の内部状態の推定処理とを実行する移動体制御方法をコンピュータで実行させるためのプログラムである。移動体制御方法は、観測取得ステップと、ランドマーク検出ステップと、候補領域取得ステップと、状態推定ステップと、を備える。

観測取得ステップは、観測可能な事象から得られる観測データを取得する。

ランドマーク検出ステップは、観測取得ステップにより取得された観測データに基づいて、（１）移動体とランドマークセットに含まれる２個以上のランドマークのそれぞれとの距離を、ランドマーク距離情報として取得し、（２）移動体とランドマークセットに含まれる２個以上のランドマークのそれぞれとを結ぶ直線と所定の軸とのなす角度をランドマーク角度情報として取得する。

候補領域取得ステップは、ランドマーク検出ステップにより取得されたランドマーク距離情報に基づいて、移動体の自己位置の候補領域を決定し、決定した候補領域についての情報を候補領域情報として取得する。

状態推定ステップは、観測取得ステップにより取得された観測データと、ランドマーク検出ステップにより生成されたランドマーク距離情報およびランドマーク角度情報と、候補領域取得ステップにより取得された候補領域情報とに基づいて、移動体の内部状態を推定することで、自己内部状態推定データを取得するとともに、候補領域情報と、ランドマーク距離情報と、ランドマーク角度情報とに基づいて、環境地図を推定することで、環境地図データを取得する。

これにより、第１の発明と同様の効果を奏する移動体制御方法をコンピュータで実行させるためのプログラムを実現することができる。

第６の発明は、３次元形状を有するランドマークであって、ランドマークの表面の一部の領域である第１表面領域は、第１パターンを有し、ランドマークの表面の一部の領域であって、第１表面領域以外の領域である第２表面領域は、第１パターンとは異なる第２パターンを有しているランドマークが設置されている環境内を移動する移動体を制御するために、ランドマークの情報を用いて表現される環境地図の作成処理と、移動体の自己の内部状態の推定処理とを実行する移動体制御方法をコンピュータで実行させるためのプログラムである。

移動体制御方法は、観測取得ステップと、ランドマーク検出ステップと、候補領域取得ステップと、状態推定ステップと、を備える。

観測取得ステップは、観測可能な事象から得られる観測データを取得する観測取得ステップであって、ランドマークを撮像した画像をランドマーク撮像画像として、観測データに含めて取得する。

ランドマーク検出ステップは、観測取得ステップにより取得された観測データに基づいて、（１）移動体とランドマークとの距離を、ランドマーク距離情報として取得し、（２）移動体とランドマークとを結ぶ直線と所定の軸とのなす角度をランドマーク角度情報として取得する。

候補領域取得ステップは、ランドマーク検出ステップにより取得されたランドマーク距離情報と、ランドマーク撮像画像上でのランドマークの第１パターンと第２パターンとの視認状態とに基づいて、移動体の自己位置の候補領域を決定し、決定した候補領域についての情報を候補領域情報として取得する。

状態推定ステップは、観測取得ステップにより取得された観測データと、ランドマーク検出ステップにより生成されたランドマーク距離情報およびランドマーク角度情報と、候補領域取得ステップにより取得された候補領域情報とに基づいて、移動体の内部状態を推定することで、自己内部状態推定データを取得するとともに、候補領域情報と、ランドマーク距離情報と、ランドマーク角度情報とに基づいて、環境地図を推定することで、環境地図データを取得する。

これにより、第２の発明と同様の効果を奏する移動体制御方法をコンピュータで実行させるためのプログラムを実現することができる。

本発明によれば、ランドマークの情報から、移動体（ロボット）の位置を精度良く推定し、高精度な状態推定処理を高速かつ適切に実行することで、移動体の制御を適切に行う移動体制御装置、プログラム、および、それらに用いられるランドマークを実現することができる。

第１実施形態に係る移動体制御装置１０００の概略構成図。 時刻ｔにおけるロボットＲｂｔ１の位置の候補となる点Ｃ１_ｔ、Ｃ２_ｔと、１組のランドマークＬＭ１ＡおよびランドマークＬＭ１Ｂとの位置関係を示す図（上方から見た図）。 時刻ｔ＋１におけるロボットＲｂｔ１の位置の候補となる点Ｃ１_ｔ＋１、Ｃ２_{ｔ ＋１}と、１組のランドマークＬＭ１ＡおよびランドマークＬＭ１Ｂとの位置関係を示す図（上方から見た図）。 第２実施形態に係る移動体制御装置２０００の概略構成図。 時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ１と、第２実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ１とを模式的に示した図。 時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ２と、第２実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ２とを模式的に示した図。 時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ３と、第２実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ３とを模式的に示した図。 時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ４と、第２実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ４とを模式的に示した図。 は、時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ５と、第２実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ５とを模式的に示した図。 ランドマークＬＭ３を模式的に示す図（上方から見た図）。

［第１実施形態］
第１実施形態について、図面を参照しながら、以下、説明する。

＜１．１：移動体制御装置の構成＞
図１は、第１実施形態に係る移動体制御装置１０００の概略構成図である。

移動体制御装置１０００は、移動体（例えば、自律走行ロボットＲｂｔ１）（不図示）に搭載される装置である。

移動体制御装置１０００は、図１に示すように、観測取得部１と、ランドマーク検出部２と、候補領域取得部３と、状態推定部４と、記憶部５とを備える。

なお、説明便宜のため、観測取得部１により取得される観測データＤ１は、移動体（ロボットＲｂｔ１）に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）と、距離センサー付き撮像装置により取得された距離画像（距離データ）であるものとして、以下、説明する。

観測取得部１は、移動体（ロボットＲｂｔ１）に搭載された距離センサー付き撮像装置（不図示）により取得されたデータを入力データＤｉｎとして入力とする。観測取得部１は、入力データＤｉｎから観測データＤ１、すなわち、画像（画像データ）Ｄ１＿ｉｍｇと距離画像（距離データ）Ｄ１＿ｄとを取得し、取得した観測データＤ１をランドマーク検出部２と、状態推定部４の状態更新部４１とに出力する。
ランドマーク検出部２は、観測取得部１から出力される観測データＤ１を入力する。ランドマーク検出部２は、観測データＤ１（画像データＤ１＿ｉｍｇおよび距離画像Ｄ１＿ｄ）から、ロボットＲｂｔ１からランドマークまでの距離および方向（角度）に関する情報を取得し、その取得した情報を含む信号を、ランドマーク検出信号ＬＭ_ｔとして、候補領域取得部３と、状態推定部４の状態更新部４１と、環境地図更新部４２とに出力する。

候補領域取得部３は、ランドマーク検出部２から出力されるランドマーク検出信号ＬＭ_ｔを入力する。候補領域取得部３は、ランドマーク検出信号ＬＭ_ｔに基づいて、ロボットＲｂｔ１の現時刻の位置の候補位置を推定するための情報である自己位置候補情報Ｃｄｔ_ｔを取得する。そして、候補領域取得部３は、取得した自己位置候補情報Ｃｄｔ_ｔを状態推定部４の状態更新部４１に出力する。

状態推定部４は、図１に示すように、状態更新部４１と、環境地図更新部４２とを備える。

状態更新部４１は、観測取得部１から出力される観測データＤ１と、ランドマーク検出部２から出力されるランドマーク検出信号ＬＭ_ｔと、候補領域取得部３から出力される自己位置候補情報Ｃｄｔ_ｔと、移動体（ロボットＲｂｔ１）に対する制御入力データＵ_ｔと、記憶部５から読み出した移動体（ロボットＲｂｔ１）の時刻ｔ−１の内部状態データＸ_ｔ−１とを入力する。状態更新部４１は、観測データＤ１、ランドマーク検出信号ＬＭ_ｔ、自己位置候補情報Ｃｄｔ_ｔ、制御入力データＵ_ｔ、および、ロボットＲｂｔ１の時刻ｔ−１の内部状態データＸ_ｔ−１基づいて、ロボットＲｂｔ１の内部状態を推定し、推定結果を移動体の現時刻ｔの内部状態データＸ_ｔとして、取得する（内部状態データを更新する）。そして、状態更新部４１は、取得した移動体の現時刻ｔの内部状態データＸ_ｔを、状態推定部４から外部に出力するとともに、記憶部５に出力する。なお、状態推定部４から外部に出力された内部状態データＸ_ｔは、移動体（自動走行ロボットＲｂｔ１）を制御するためのデータとして、例えば、移動体（自動走行ロボットＲｂｔ１）の全体を制御する機能部（全体制御部）（不図示）等により使用される。

また、状態更新部４１は、現時刻ｔのロボットＲｂｔ１の位置に関する内部状態データＸ_ｔを、環境地図更新部４２に出力する。

また、状態更新部４１は、現時刻ｔのロボットＲｂｔ１に対する制御入力データＵ_ｔを記憶部５に出力する。

環境地図更新部４２は、ランドマーク検出部２から出力されるランドマーク検出信号ＬＭ_ｔと、記憶部５から読み出した時刻ｔ−１の環境地図データｍ_ｔ−１と、状態更新部４１から出力される現時刻ｔのロボットＲｂｔ１の内部状態データＸ_ｔとを入力する。環境地図更新部４２は、ランドマーク検出信号ＬＭ_ｔから、検出されたランドマークについての情報を取得し、取得したランドマークの情報と、時刻ｔ−１の環境地図データｍ_ｔ−１と、現時刻ｔのロボットＲｂｔ１の内部状態データＸ_ｔとに基づいて、環境地図データを更新し、現時刻ｔの環境地図データｍ_ｔを取得する。そして、環境地図更新部４２は、更新した環境地図データｍ_ｔを状態推定部４から外部に出力するとともに、記憶部５に出力する。なお、状態推定部４から外部に出力された環境地図データｍ_ｔは、移動体（自動走行ロボットＲｂｔ１）を制御するためのデータとして、例えば、移動体（自動走行ロボットＲｂｔ１）の全体を制御する機能部（全体制御部）（不図示）等により使用される。

記憶部５は、状態推定部４の状態更新部４１から出力される移動体の現時刻ｔの内部状態データＸ_ｔと、環境地図データｍ_ｔと、状態更新部４１から出力される制御入力データＵ_ｔとを入力し、記憶する。記憶部５に記憶されたデータは、状態推定部４により、所定のタイミングで読み出される。なお、記憶部５は、複数の時刻の内部状態データと、環境地図データとを記憶することができる。

なお、現時刻ｔのデータとは、移動体制御装置１０００において、処理対象となっている観測データＤ１から取得されたデータ（移動体の内部状態データＸ_ｔ、および、環境地図データｍ_ｔ）のことをいい、現時刻ｔの１つ前の時刻ｔ−１のデータとは、現時刻ｔのデータを取得する１つ前に、移動体制御装置１０００において取得されたデータ（移動体の内部状態データＸ_ｔ−１、および、環境地図データｍ_ｔ−１）のことをいう。つまり、現時刻ｔのｋ個前の時刻ｔ−ｋ（ｋ：自然数）のデータとは、現時刻ｔのデータを取得するｋ個前に、移動体制御装置１０００において取得されたデータ（移動体の内部状態データＸ_ｔ−ｋ、および、環境地図データｍ_ｔ−ｋ）のことをいう。

また、上記データは、同期タイミングにより取得されるものであってもよいし、非同期タイミングにより取得されるものであってもよい。

＜１．２：移動体制御装置の動作＞
以上のように構成された移動体制御装置１０００の動作について、以下、説明する。

なお、以下では、自律走行する移動体をロボットＲｂｔ１とし、ロボットＲｂｔ１に搭載された可視光用イメージセンサーと距離センサーとを備える撮像装置（不図示）により撮像された画像（撮像画像）と、距離画像（距離データ）とを入力データＤｉｎとし、ランドマークの情報により環境地図が生成される場合について、説明する。

また、移動体制御装置１０００により環境地図を生成するために、２つのランドマークを１組のセットとして、ロボットＲｂｔ１が移動する環境に設置されているものとする。また、１組のセットを構成するランドマークは、他の組のランドマークと識別できるように、例えば、カラーコードや特定の模様が付されている。これにより、発見されたランドマークがどの組（どのランドマーク・セット）に属するランドマークであるかを特定することができる。

図２は、時刻ｔにおけるロボットＲｂｔ１の位置の候補となる点Ｃ１_ｔ、Ｃ２_ｔと、１組のランドマークＬＭ１ＡおよびランドマークＬＭ１Ｂとの位置関係を示す図（上方から見た図）である。なお、図２において、図２に示すようにｘ軸、ｙ軸をとるものとし、図２に「０」で示した位置を原点とする。なお、以下では、図２に示すように、説明便宜のため、２次元平面（ｘｙ平面）におけるロボットＲｂｔ１、ランドマークＬＭ１Ａ、ＬＭ１Ｂの位置関係に基づいて、移動体制御装置の処理について説明するが、これに限定されることはなく、移動体制御装置の処理については、３次元空間におけるロボットＲｂｔ１、ランドマークＬＭ１Ａ、ＬＭ１Ｂの位置関係に基づいて、実行されるものであってもよい。

図３は、時刻ｔ＋１におけるロボットＲｂｔ１の位置の候補となる点Ｃ１_ｔ＋１、Ｃ２_ｔ＋１と、１組のランドマークＬＭ１ＡおよびランドマークＬＭ１Ｂとの位置関係を示す図（上方から見た図）である。なお、図３においても、図２と同様に、図３に示すようにｘ軸、ｙ軸をとるものとし、図３に「０」で示した位置を原点とする。

以下では、時刻ｔにおいて、図２に点Ｃ１_ｔで示す位置に存在するロボットＲｂｔ１に対して、図３に示す矢印Ｄｉｒ１方向に移動させるための制御データが入力され、時刻ｔ＋１において、図３に点Ｃ１_ｔ＋１で示す位置にロボットＲｂｔ１が移動する場合を一例として、説明する。

ここで、移動体制御装置１０００において使用される、（１）ロボットＲｂｔ１の内部状態を示すデータ、および、（２）環境地図に関するデータについて、説明する。

（１．２．１：ロボットＲｂｔ１の内部状態データ）
移動体制御装置１０００では、ロボットＲｂｔ１の内部状態を示すデータＸ_ｔとして、時刻ｔにおけるロボットＲｂｔ１の位置および角度に関する内部状態データＸ_ｔが設定される。

時刻ｔにおけるロボットＲｂｔ１の位置および角度に関する内部状態データＸ_ｔは、３次元の状態ベクトルであり、
Ｘ_ｔ＝（ｘ０_ｔ，ｙ０_ｔ，θ_ｔ）
ｘ０_ｔ：時刻ｔにおけるロボットＲｂｔ１のｘ座標値の期待値
ｙ０_ｔ：時刻ｔにおけるロボットＲｂｔ１のｙ座標値の期待値
θ_ｔ：時刻ｔにおけるロボットＲｂｔ１のｘ軸正方向とのなす角度（ロボットＲｂｔ１の向き）の期待値
として表現される。

移動体制御装置１０００では、ロボットＲｂｔ１の内部状態を推定するために、パーティクルフィルタ（モンテカルロ近似）を用いるものとする。

つまり、パーティクルフィルタ処理では、時刻ｔ−１の状態ベクトルＸ_ｔ−１の事後確率分布を現時刻ｔの状態ベクトルＸ_ｔの事前確率分布とし、現時刻ｔの状態ベクトルＸ_ｔの事前確率分布から予測処理により現時刻ｔの状態ベクトルＸ_ｔの予測確率分布を取得する。そして、取得した予測確率分布と実際の観測データ（現時刻ｔの観測データ）とに基づいて、尤度を算出する。そして、算出した尤度に比例する割合で、粒子（パーティクル）を復元抽出する。このように処理することで、ロボットＲｂｔ１の内部状態を推定することができる。

以上のように、移動体制御装置１０００では、ロボットＲｂｔ１の内部状態を示すデータＸ_ｔが設定される。

（１．２．２：環境地図に関するデータ）
移動体制御装置１０００では、環境地図に関するデータとして、時刻ｔにおける環境地図データｍ_ｔが設定される。

環境地図データｍ_ｔは、ランドマークの情報により構成される。ロボットＲｂｔ１によりランドマークが発見された場合、発見されたランドマークの情報は、環境地図データｍ_ｔに追加される。

時刻ｔにおける環境地図データｍ_ｔは、移動体制御装置１０００において、以下のように、設定される。
ｍ_ｔ＝［ＬＭ１ＡＰ_ｔ，ＬＭ１ＢＰ_ｔ，．．．．，ＬＭｋＡＰ_ｔ，ＬＭｋＢＰ_ｔ］
ｋ：自然数
ＬＭｋＡＰ_ｔは、時刻ｔにおけるｋ組目のランドマークセットを構成する一方のランドマークの位置に関する内部状態データであり、ＬＭｋＢＰ_ｔは、時刻ｔにおけるｋ組目のランドマークセットを構成する他方のランドマークの位置に関する内部状態データである。なお、１組のランドマークセットは、２つのランドマークから構成される。

つまり、移動体制御装置１０００では、ｋ組のランドマークセットに含まれる２×ｋ個のランドマークについての内部状態データの集合が、時刻ｔにおける環境地図データｍ_ｔとして、設定される。

図２の場合において、環境地図データｍ_ｔは、１組のランドマークセットを構成する２つのランドマークＬＭ１Ａ、ＬＭ１Ｂの情報により構成される。図２の場合、環境地図データｍ_ｔは、
ｍ_ｔ＝［ＬＭ１ＡＰ_ｔ，ＬＭ１ＢＰ_ｔ］
である。

なお、ランドマークＬＭ１Ａ、ＬＭ１Ｂの情報（位置に関する内部状態データ）ＬＭ１ＡＰ_ｔ、ＬＭ１ＢＰ_ｔは、例えば、以下のように設定される。
ランドマークＬＭ１Ａ：
ＬＭ１ＡＰ_ｔ＝（ｄ１Ａ_ｔ，θ１Ａ_ｔ）
ｄ１Ａ_ｔ：ロボットＲｂｔ１からランドマークＬＭ１Ａまでの距離
θ１Ａ_ｔ：平面視において、ロボットＲｂｔ１からランドマークＬＭ１Ａへ引いた直線とｘ軸とのなす角度
ランドマークＬＭ１Ｂ：
ＬＭ１ＢＰ_ｔ＝（ｄ１Ｂ_ｔ，θ１Ｂ_ｔ）
ｄ１Ｂ_ｔ：ロボットＲｂｔ１からランドマークＬＭ１Ｂまでの距離
θ１Ｂ_ｔ：平面視において、ロボットＲｂｔ１からランドマークＬＭ１Ｂへ引いた直線とｘ軸とのなす角度
なお、以下では、説明便宜のため、図２、図３に示すように、移動体制御装置１０００を搭載したロボットＲｂｔ１が１つのランドマークセットに含まれる２つのランドマークＬＭＡ１、ＬＭＢ１により、ロボットＲｂｔ１の内部状態データ、および、環境地図データが取得される場合について、説明する。

（１．２．３：具体的処理）
次に、移動体制御装置１０００の具体的処理について、説明する。

（時刻ｔの処理）：
時刻ｔにおいて、ロボットＲｂｔ１、ランドマークＬＭ１Ａ、ＬＭ１Ｂの状態は、図２に示す状態である。なお、移動体制御装置１０００に搭載された距離センサー付き撮像装置では、図２に示す領域ＡＲｃｐの撮像画像（画像データ）と、距離画像（距離データ）とが取得される。

観測取得部１は、移動体制御装置１０００に搭載された距離センサー付き撮像装置からの入力データＤｉｎから観測データＤ１、すなわち、領域ＡＲｃｐの画像（画像データ）Ｄ１＿ｉｍｇと距離画像（距離データ）Ｄ１＿ｄとを取得し、取得した観測データＤ１をランドマーク検出部２と、状態推定部４の状態更新部４１とに出力する。

ランドマーク検出部２では、観測データＤ１（画像データＤ１＿ｉｍｇおよび距離画像Ｄ１＿ｄ）から、ロボットＲｂｔ１からランドマークＬＭ１Ａ、ＬＭ１Ｂまでの距離および方向（角度）に関する情報が取得される。具体的には、以下のデータＬＭ１ＡＰ_ｔ、ＬＭ１ＢＰ_ｔが、ランドマーク検出部２により取得される。
ランドマークＬＭ１Ａ：
ＬＭ１ＡＰ_ｔ＝（ｄ１Ａ_ｔ，θ１Ａ_ｔ）
ｄ１Ａ_ｔ：ロボットＲｂｔ１からランドマークＬＭ１Ａまでの距離
θ１Ａ_ｔ：平面視において、ロボットＲｂｔ１からランドマークＬＭ１Ａへ引いた直線とｘ軸とのなす角度
ランドマークＬＭ１Ｂ：
ＬＭ１ＢＰ_ｔ＝（ｄ１Ｂ_ｔ，θ１Ｂ_ｔ）
ｄ１Ｂ_ｔ：ロボットＲｂｔ１からランドマークＬＭ１Ｂまでの距離
θ１Ｂ_ｔ：平面視において、ロボットＲｂｔ１からランドマークＬＭ１Ｂへ引いた直線とｘ軸とのなす角度
上記により取得されたランドマークＬＭ１Ａ、ＬＭ１ＢについてのデータＬＭ１ＡＰ_ｔ、ＬＭ１ＢＰ_ｔを含めた信号が、ランドマーク検出信号ＬＭ_ｔとして、ランドマーク検出部２から、候補領域取得部３、状態推定部４の状態更新部４１、および、環境地図更新部４２に出力される。

候補領域取得部３では、ランドマーク検出信号ＬＭ_ｔに基づいて、ロボットＲｂｔ１の現時刻ｔの位置の候補位置を推定するための情報が取得される。

具体的には、候補領域取得部３は、図２に示すように、（１）点ＬＭ１Ａを中心とし、半径をロボットＲｂｔ１からランドマークＬＭ１Ａまでの距離ｄ１Ａ_ｔとする円と、（２）点ＬＭ１Ｂを中心とし、半径をロボットＲｂｔ１からランドマークＬＭ１Ｂまでの距離ｄ１Ｂ_ｔとする円との２つの交点Ｃ１_ｔ、Ｃ２_ｔとを求める。そして、候補領域取得部３は、求めた２つの交点Ｃ１_ｔ、Ｃ２_ｔに関する情報（交点Ｃ１_ｔ、Ｃ２_ｔを特定するための情報）を自己位置候補情報Ｃｄｔ_ｔとして、状態推定部４の状態更新部４１に出力する。

なお、候補領域取得部３での処理において、誤差やランドマーク位置の推定精度（ランドマークについての内部状態データの精度）を考慮して、候補領域取得部３は、求めた２つの交点Ｃ１_ｔ、Ｃ２_ｔを含む所定の面積を有する領域に関する情報を、自己位置候補情報Ｃｄｔ_ｔとして、状態推定部４の状態更新部４１に出力するようにしてもよい。

状態推定部４の状態更新部４１は、観測データＤ１、ランドマーク検出信号ＬＭ_ｔ、自己位置候補情報Ｃｄｔ_ｔ、制御入力データＵ_ｔ、および、ロボットＲｂｔ１の時刻ｔ−１の内部状態データＸ_ｔ−１基づいて、ロボットＲｂｔ１の内部状態を推定し、推定結果をロボットＲｂｔ１の現時刻ｔの内部状態データＸ_ｔとして、取得する（内部状態データを更新する）。

具体的には、自己位置候補情報Ｃｄｔ_ｔから、ロボットＲｂｔ１の自己位置の２つの候補点である点Ｃ１_ｔ、Ｃ２_ｔのうちのどちらか１つの点を、時刻ｔのロボットＲｂｔ１の位置であると推定する。ここでは、状態更新部４１により、点Ｃ１_ｔが、時刻ｔのロボットＲｂｔ１の位置であると推定されたものとして、以下、説明する。

状態更新部４１により取得された時刻ｔのロボットＲｂｔ１の内部状態データＸ_ｔは、状態更新部４１から記憶部５に出力され、記憶部５で記憶される。

環境地図更新部４２は、ランドマーク検出部２から出力されるランドマーク検出信号ＬＭ_ｔから、検出されたランドマークについての情報を取得し、取得したランドマークの情報と、時刻ｔ−１の環境地図データｍ_ｔ−１と、現時刻ｔのロボットＲｂｔ１の内部状態データＸ_ｔとに基づいて、環境地図データを更新し、現時刻ｔの環境地図データｍ_ｔを取得する。

具体的には、時刻ｔにおけるランドマークＬＭ１Ａのｘｙ平面上の位置座標を（ｘ１Ａ_ｔ，ｙ１Ａ_ｔ）とし、時刻ｔにおけるランドマークＬＭ１Ｂのｘｙ平面上の位置座標を（ｘ１Ｂ_ｔ，ｙ１Ｂ_ｔ）とすると、環境地図更新部４２は、
ｘ１Ａ_ｔ＝ｘ０_ｔ＋ｄ１Ａ_ｔ×ｃｏｓ（θ１Ａ_ｔ）
ｙ１Ａ_ｔ＝ｙ０_ｔ＋ｄ１Ａ_ｔ×ｓｉｎ（θ１Ａ_ｔ）
ｘ１Ｂ_ｔ＝ｘ０_ｔ＋ｄ１Ｂ_ｔ×ｃｏｓ（θ１Ｂ_ｔ）
ｙ１Ｂ_ｔ＝ｙ０_ｔ＋ｄ１Ｂ_ｔ×ｓｉｎ（θ１Ｂ_ｔ）
により、時刻ｔにおけるランドマークＬＭ１Ａ、ＬＭ１Ｂのｘｙ平面上の位置座標（位置情報）を取得する。

そして、環境地図更新部４２は、上記により取得したデータにより、環境地図データｍ_ｔを更新する。

また、時刻ｔにおいて、ロボットＲｂｔ１には、制御入力Ｕ_ｔが入力される。なお、制御入力Ｕ_ｔは、図３に矢印Ｄｉｒ１で示す方向にロボットＲｂｔ１を移動させるための制御入力データであるとして、以下、説明する。

（時刻ｔ＋１の処理）：
時刻ｔ＋１において、ロボットＲｂｔ１は、時刻ｔの制御入力データＵ_ｔにより、図３に点Ｃ１_ｔ＋１で示す位置、あるいは、図３に点Ｃ２＿ＮＧ_ｔ＋１で示す位置に移動していると予測される。

観測取得部１は、移動体制御装置１０００に搭載された距離センサー付き撮像装置からの入力データＤｉｎから観測データＤ１、すなわち、領域ＡＲｃｐの画像（画像データ）Ｄ１＿ｉｍｇと距離画像（距離データ）Ｄ１＿ｄとを取得し、取得した観測データＤ１をランドマーク検出部２と、状態推定部４の状態更新部４１とに出力する。

ランドマーク検出部２では、観測データＤ１（画像データＤ１＿ｉｍｇおよび距離画像Ｄ１＿ｄ）から、ロボットＲｂｔ１からランドマークＬＭ１Ａ、ＬＭ１Ｂまでの距離および方向（角度）に関する情報が取得される。具体的には、以下のデータＬＭ１ＡＰ_ｔ、ＬＭ１ＢＰ_ｔが、ランドマーク検出部２により取得される。
ランドマークＬＭ１Ａ：
ＬＭ１ＡＰ_ｔ＋１＝（ｄ１Ａ_ｔ＋１，θ１Ａ_ｔ＋１）
ｄ１Ａ_ｔ＋１：ロボットＲｂｔ１からランドマークＬＭ１Ａまでの距離
θ１Ａ_ｔ＋１：平面視において、ロボットＲｂｔ１からランドマークＬＭ１Ａへ引いた直線とｘ軸とのなす角度
ランドマークＬＭ１Ｂ：
ＬＭ１ＢＰ_ｔ＋１＝（ｄ１Ｂ_ｔ＋１，θ１Ｂ_ｔ＋１）
ｄ１Ｂ_ｔ＋１：ロボットＲｂｔ１からランドマークＬＭ１Ｂまでの距離
θ１Ｂ_ｔ＋１：平面視において、ロボットＲｂｔ１からランドマークＬＭ１Ｂへ引いた直線とｘ軸とのなす角度
上記により取得されたランドマークＬＭ１Ａ、ＬＭ１ＢについてのデータＬＭ１ＡＰ_ｔ＋１、ＬＭ１ＢＰ_ｔ＋１を含めた信号が、ランドマーク検出信号ＬＭ_ｔ＋１として、ランドマーク検出部２から、候補領域取得部３、状態推定部４の状態更新部４１、および、環境地図更新部４２に出力される。

候補領域取得部３では、ランドマーク検出信号ＬＭ_ｔ＋１に基づいて、ロボットＲｂｔ１の現時刻ｔの位置の候補位置を推定するための情報が取得される。

具体的には、候補領域取得部３は、図３に示すように、（１）点ＬＭ１Ａを中心とし、半径をロボットＲｂｔ１からランドマークＬＭ１Ａまでの距離ｄ１Ａ_ｔ＋１とする円と、（２）点ＬＭ１Ｂを中心とし、半径をロボットＲｂｔ１からランドマークＬＭ１Ｂまでの距離ｄ１Ｂ_ｔ＋１とする円との２つの交点Ｃ１_ｔ＋１、Ｃ２_ｔ＋１とを求める。そして、候補領域取得部３は、求めた２つの交点Ｃ１_ｔ＋１、Ｃ２_ｔ＋１に関する情報（交点Ｃ１_ｔ＋１、Ｃ２_ｔ＋１を特定するための情報）を自己位置候補情報Ｃｄｔ_ｔ＋１として、状態推定部４の状態更新部４１に出力する。

なお、候補領域取得部３での処理において、誤差やランドマーク位置の推定精度（ランドマークについての内部状態データの精度）を考慮して、候補領域取得部３は、求めた２つの交点Ｃ１_ｔ＋１、Ｃ２_ｔ＋１を含む所定の面積を有する領域に関する情報を、自己位置候補情報Ｃｄｔ_ｔ＋１として、状態推定部４の状態更新部４１に出力するようにしてもよい。

状態推定部４の状態更新部４１は、観測データＤ１、ランドマーク検出信号ＬＭ_ｔ＋１、自己位置候補情報Ｃｄｔ_ｔ＋１、制御入力データＵ_ｔ＋１、および、ロボットＲｂｔ１の時刻ｔの内部状態データＸ_ｔ基づいて、ロボットＲｂｔ１の内部状態を推定し、推定結果をロボットＲｂｔ１の現時刻ｔ＋１の内部状態データＸ_ｔ＋１として、取得する（内部状態データを更新する）。

具体的には、自己位置候補情報Ｃｄｔ_ｔ＋１から、ロボットＲｂｔ１の自己位置の２つの候補点である点Ｃ１_ｔ＋１、Ｃ２_ｔ＋１のうちのどちらか１つの点を、時刻ｔのロボットＲｂｔ１の位置であると推定する。

移動体制御装置１０００は、（１）時刻ｔのロボットＲｂｔ１の位置の２つの候補点として、点Ｃ１_ｔ、点Ｃ２_ｔを取得し、（２）時刻ｔ＋１のロボットＲｂｔ１の位置の２つの候補点として、点Ｃ１_ｔ＋１、点Ｃ２_ｔ＋１を取得している。そして、時刻ｔのロボットＲｂｔ１への制御入力Ｕ_ｔが、ロボットＲｂｔ１を図３の矢印Ｄｉｒ１への方向への移動をするためのデータである。

状態更新部４１は、時刻ｔのロボットＲｂｔ１への制御入力Ｕ_ｔと、移動体制御装置１０００により推定された時刻ｔのロボットＲｂｔ１の位置の２つの候補点Ｃ１_ｔ、Ｃ２_ｔとから、時刻ｔにおいて、ロボットＲｂｔ１が、図３の点Ｃ１_ｔ＋１、あるいは、点Ｃ２＿ＮＧ_ｔ＋１のいずれかに存在すると予測する（予測処理１）。

また、状態更新部４１は、時刻ｔ＋１の実際の観測より、時刻ｔ＋１において、ロボットＲｂｔ１とＬＭ１Ａとの距離は、ｄ１Ａ_ｔ＋１であり、ロボットＲｂｔ１とＬＭ２Ａとの距離は、ｄ２Ａ_ｔ＋１であるので、時刻ｔ＋１の実際の観測から求められる時刻ｔ＋１のロボットＲｂｔ１の位置は、候補点Ｃ１_ｔ＋１、あるいは、候補点Ｃ２_ｔ＋１であると予測する（予測処理２）。

つまり、状態更新部４１は、上記予測処理１、および、予測処理２の両方において、時刻ｔ＋１のロボットＲｂｔ１の位置の候補点であると予測された点Ｃ１_ｔ＋１を、時刻ｔ＋１のロボットＲｂｔ１の位置であると推定する。そして、状態更新部４１は、時刻ｔ＋１のロボットＲｂｔ１の位置についての内部状態データ（座標データ（ｘ０Ａ_ｔ＋１，ｙ０Ａ_ｔ＋１））を、点Ｃ１_ｔ＋１の座標情報により更新する。

以上のようにして、状態更新部４１により取得された時刻ｔ＋１のロボットＲｂｔ１の内部状態データＸ_ｔ＋１は、状態更新部４１から記憶部５に出力され、記憶部５で記憶される。

環境地図更新部４２は、ランドマーク検出部２から出力されるランドマーク検出信号ＬＭ_ｔから、検出されたランドマークについての情報を取得し、取得したランドマークの情報と、時刻ｔの環境地図データｍ_ｔと、現時刻ｔ＋１のロボットＲｂｔ１の内部状態データＸ_ｔ＋１とに基づいて、環境地図データを更新し、現時刻ｔ＋１の環境地図データｍ_ｔ＋１を取得する。

具体的には、時刻ｔ＋１におけるランドマークＬＭ１Ａのｘｙ平面上の位置座標を（ｘ１Ａ_ｔ＋１，ｙ１Ａ_ｔ＋１）とし、時刻ｔ＋１におけるランドマークＬＭ１Ｂのｘｙ平面上の位置座標を（ｘ１Ｂ_ｔ＋１，ｙ１Ｂ_ｔ＋１）とすると、環境地図更新部４２は、
ｘ１Ａ_ｔ＋１＝ｘ０_ｔ＋１＋ｄ１Ａ_ｔ＋１×ｃｏｓ（θ１Ａ_ｔ＋１）
ｙ１Ａ_ｔ＋１＝ｙ０_ｔ＋１＋ｄ１Ａ_ｔ＋１×ｓｉｎ（θ１Ａ_ｔ＋１）
ｘ１Ｂ_ｔ＋１＝ｘ０_ｔ＋１＋ｄ１Ｂ_ｔ＋１×ｃｏｓ（θ１Ｂ_ｔ＋１）
ｙ１Ｂ_ｔ＋１＝ｙ０_ｔ＋１＋ｄ１Ｂ_ｔ＋１×ｓｉｎ（θ１Ｂ_ｔ＋１）
により、時刻ｔにおけるランドマークＬＭ１Ａ、ＬＭ１Ｂのｘｙ平面上の位置座標（位置情報）を取得する。

そして、環境地図更新部４２は、上記により取得したデータにより、環境地図データｍ_ｔ＋１を更新する。

（時刻ｔ＋２以降の処理）：
時刻ｔ＋２以降において、上記と同様の処理が繰り返し実行される。

このように、移動体制御装置１０００では、２つのランドマークを１組とするランドマークセットを用いて、ロボットＲｂｔ１から２つのランドマークまでの距離を測定することで、ロボットＲｂｔ１の位置を少ない候補点（上記では２点）に絞り、高速かつ高精度に、ロボットＲｂｔ１の位置情報を含む内部状態データを推定することができる。例えば、上記で示した例では、移動体制御装置１０００において、時刻ｔ、時刻ｔ＋１の２回の推定処理を実行することで、ロボットＲｂｔ１の位置を１点に絞り込むことができる。

以上のように処理することで、移動体制御装置１０００では、ランドマークの情報から、移動体（ロボットＲｂｔ１）の位置を精度良く推定し、高精度な状態推定処理を高速かつ適切に実行することができる。その結果、移動体制御装置１０００では、移動体（ロボットＲｂｔ１）の制御を適切に行うことができる。

さらに、移動体制御装置１０００では、上記の通り、１つのランドマークセットに含まれる２つのランドマークと、ロボットＲｂｔ１との距離を取得するだけで、ロボットＲｂｔ１の位置を高速かつ高精度に推定することができる。このため、移動体制御装置１０００では、多くの内部状態変数を用いた複雑な処理をすることなく、少ない演算量で推定処理を実現することができる。

［第２実施形態］
次に、第２実施形態について、説明する。

第２実施形態において、第１実施形態と同様の部分については、同一符号を付し、詳細な説明を省略する。

図４は、第２実施形態に係る移動体制御装置２０００の概略構成図である。

第２実施形態の移動体制御装置２０００では、図４に示すように、第１実施形態の移動体制御装置１０００において、ランドマーク検出部２をランドマーク検出部２Ａに置換し、候補領域取得部３を候補領域取得部３Ａに置換した構成を有している。

ランドマーク検出部２Ａは、ランドマーク検出部２の機能に加えて、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）からランドマークを含む画像領域を抽出し、抽出した画像を画像Ｄ１＿ｉｍｇとして、候補領域取得部３に出力する。また、ランドマーク検出部２Ａは、ランドマーク検出信号ＬＭ_ｔを、候補領域取得部３Ａ、状態更新部４１、および、環境地図更新部４２に出力する。

候補領域取得部３Ａは、図４に示すように、ランドマーク検出部２Ａから出力されるランドマーク検出信号ＬＭ_ｔおよび画像Ｄ１＿ｉｍｇと、ランドマークパターン情報ＬＭ＿ｐｔｎとを入力する。ランドマークパターン情報ＬＭ＿ｐｔｎは、予め設定されている情報であり、ロボットＲｂｔ１が移動する環境に設置されているランドマークの表面に付されているパターン（色、模様等）についての情報である。候補領域取得部３は、ランドマークパターン情報ＬＭ＿ｐｔｎと、画像Ｄ１＿ｉｍｇとに基づいて、ロボットＲｂｔ１の現時刻の位置の候補位置を推定するための情報である自己位置候補情報Ｃｄｔ_ｔを取得する。そして、候補領域取得部３Ａは、取得した自己位置候補情報Ｃｄｔ_ｔを状態推定部４の状態更新部４１に出力する。

本実施形態において使用されるランドマーク（一例）は、細長い円筒形状であり、ランドマークの表面が、（１）第１パターンを付された第１パターン領域と、（２）第２パターンを付された第２パターン領域と、の２つの領域からなる。そして、ランドマークを設置した状態の平面視において、一方の半円に相当する円周上の表面が第１パターン領域であり、他方の半円に相当する円周上の表面が第２パターン領域であるものとする。

なお、ランドマークの形状および表面のパターンは、上記に限定されることはない。移動体制御装置２０００によりランドマークを発見したときに、当該ランドマークの形状、および／または、パターンを認識（特定）することで、移動体（ロボットＲｂｔ１）と、ランドマークとの位置関係（ランドマークがどの方向から観測されたかを知る手掛かりになる情報）を取得できるものであれば、ランドマークの形状および／またはパターンは、他のものであってもよい。

≪図５の場合≫
図５は、時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ１と、本実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ１とを模式的に示した図である。なお、画像Ｄ１＿ｉｍｇ１は、ランドマーク検出部２Ａから候補領域取得部３Ａに出力される画像（画像データ）である。

候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎに基づいて、図５に示す画像Ｄ１＿ｉｍｇ１を解析する。具体的には、候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎから、ランドマークＬＭ２の平面視において一方の半円に相当する円周上の表面が第１パターン領域（黒色一色のパターン）（図５において、ｐｔｎ１（ＬＭ２）と表記している領域）であり、ランドマークＬＭ２の平面視において他方の半円に相当する円周上の表面が第２パターン領域（白色一色のパターン）（図５において、ｐｔｎ２（ＬＭ２）と表記している領域）であることを認識する。

そして、候補領域取得部３Ａは、画像Ｄ１＿ｉｍｇ１上において、ランドマークの中央付近に、第１パターン領域（黒色一色のパターン）と第２パターン領域（白色一色のパターン）が存在し、かつ、左側に第１パターン領域（黒色一色のパターン）が存在し、右側に第２パターン領域（白色一色のパターン）が存在することを認識する。そして、候補領域取得部３Ａは、上記画像解析結果に基づいて、画像Ｄ１＿ｉｍｇ１が撮像された位置の候補領域ＡＲ＿Ｃ１を求める。つまり、図５に示すように、平面視において、ランドマークＬＭ２の位置（中心点）から、ランドマークＬＭ２の第１パターン領域ｐｔｎ１（ＬＭ２）と第２パターン領域ｐｔｎ２（ＬＭ２）との境界点を通る直線のｙ軸負方向に、距離ｄ２だけ離れた領域が、時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ１であると判定する。なお、候補領域取得部３Ａは、ランドマーク検出信号ＬＭ_ｔから、ランドマークＬＭ２とロボットＲｂｔ１との距離ｄ２を取得する。

候補領域取得部３Ａは、上記のようにして取得した時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ１についての情報を、自己位置候補情報Ｃｄｔ_ｔに含めて、状態更新部４１に出力する。

状態更新部４１は、自己位置候補情報Ｃｄｔ_ｔから、時刻ｔのロボットＲｂｔ１の位置が、候補領域ＡＲ＿Ｃ１である可能性が高いと判定し、ロボットＲｂｔ１の位置に関する内部状態データを更新する。

≪図６の場合≫
図６は、時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ２と、本実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ２とを模式的に示した図である。なお、画像Ｄ１＿ｉｍｇ２は、ランドマーク検出部２Ａから候補領域取得部３Ａに出力される画像（画像データ）である。

候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎに基づいて、図６に示す画像Ｄ１＿ｉｍｇ１を解析する。具体的には、候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎから、ランドマークＬＭ２の平面視において一方の半円に相当する円周上の表面が第１パターン領域（黒色一色のパターン）（図６において、ｐｔｎ１（ＬＭ２）と表記している領域）であり、ランドマークＬＭ２の平面視において他方の半円に相当する円周上の表面が第２パターン領域（白色一色のパターン）（図６において、ｐｔｎ２（ＬＭ２）と表記している領域）であることを認識する。

そして、候補領域取得部３Ａは、画像Ｄ１＿ｉｍｇ２上において、ランドマークの中央付近に、第１パターン領域（黒色一色のパターン）と第２パターン領域（白色一色のパターン）が存在し、かつ、右側に第１パターン領域（黒色一色のパターン）が存在し、左側に第２パターン領域（白色一色のパターン）が存在することを認識する。そして、候補領域取得部３Ａは、上記画像解析結果に基づいて、画像Ｄ１＿ｉｍｇ２が撮像された位置の候補領域ＡＲ＿Ｃ２を求める。つまり、図６に示すように、平面視において、ランドマークＬＭ２の位置（中心点）から、ランドマークＬＭ２の第１パターン領域ｐｔｎ１（ＬＭ２）と第２パターン領域ｐｔｎ２（ＬＭ２）との境界点を通る直線のｙ軸正方向に、距離ｄ２だけ離れた領域が、時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ２であると判定する。なお、候補領域取得部３Ａは、ランドマーク検出信号ＬＭ_ｔから、ランドマークＬＭ２とロボットＲｂｔ１との距離ｄ２を取得する。

候補領域取得部３Ａは、上記のようにして取得した時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ２についての情報を、自己位置候補情報Ｃｄｔ_ｔに含めて、状態更新部４１に出力する。

状態更新部４１は、自己位置候補情報Ｃｄｔ_ｔから、時刻ｔのロボットＲｂｔ１の位置が、候補領域ＡＲ＿Ｃ２である可能性が高いと判定し、ロボットＲｂｔ１の位置に関する内部状態データを更新する。

≪図７の場合≫
図７は、時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ３と、本実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ３とを模式的に示した図である。なお、画像Ｄ１＿ｉｍｇ３は、ランドマーク検出部２Ａから候補領域取得部３Ａに出力される画像（画像データ）である。

候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎに基づいて、図７に示す画像Ｄ１＿ｉｍｇ３を解析する。具体的には、候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎから、ランドマークＬＭ２の平面視において一方の半円に相当する円周上の表面が第１パターン領域（黒色一色のパターン）（図７において、ｐｔｎ１（ＬＭ２）と表記している領域）であり、ランドマークＬＭ２の平面視において他方の半円に相当する円周上の表面が第２パターン領域（白色一色のパターン）（図７において、ｐｔｎ２（ＬＭ２）と表記している領域）であることを認識する。

そして、候補領域取得部３Ａは、画像Ｄ１＿ｉｍｇ３上において、ランドマークＬＭ２の第２パターン領域（白色一色のパターン）のみが存在することを認識する。そして、候補領域取得部３Ａは、上記画像解析結果に基づいて、画像Ｄ１＿ｉｍｇ３が撮像された位置の候補領域ＡＲ＿Ｃ３を求める。つまり、図７に示すように、平面視において、ランドマークＬＭ２の位置（中心点）から、ランドマークＬＭ２の第２パターン領域ｐｔｎ２（ＬＭ２）の中央付近の点を通る直線のｘ軸正方向に、距離ｄ２だけ離れた領域が、時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ３であると判定する。なお、候補領域取得部３Ａは、ランドマーク検出信号ＬＭ_ｔから、ランドマークＬＭ２とロボットＲｂｔ１との距離ｄ２を取得する。

候補領域取得部３Ａは、上記のようにして取得した時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ３についての情報を、自己位置候補情報Ｃｄｔ_ｔに含めて、状態更新部４１に出力する。

状態更新部４１は、自己位置候補情報Ｃｄｔ_ｔから、時刻ｔのロボットＲｂｔ１の位置が、候補領域ＡＲ＿Ｃ３である可能性が高いと判定し、ロボットＲｂｔ１の位置に関する内部状態データを更新する。

≪図８の場合≫
図８は、時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ４と、本実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ４とを模式的に示した図である。なお、画像Ｄ１＿ｉｍｇ４は、ランドマーク検出部２Ａから候補領域取得部３Ａに出力される画像（画像データ）である。

候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎに基づいて、図８に示す画像Ｄ１＿ｉｍｇ４を解析する。具体的には、候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎから、ランドマークＬＭ２の平面視において一方の半円に相当する円周上の表面が第１パターン領域（黒色一色のパターン）（図８において、ｐｔｎ１（ＬＭ２）と表記している領域）であり、ランドマークＬＭ２の平面視において他方の半円に相当する円周上の表面が第２パターン領域（白色一色のパターン）（図８において、ｐｔｎ２（ＬＭ２）と表記している領域）であることを認識する。

そして、候補領域取得部３Ａは、画像Ｄ１＿ｉｍｇ４上において、ランドマークの中央付近に、第１パターン領域（黒色一色のパターン）のみが存在することを認識する。そして、候補領域取得部３Ａは、上記画像解析結果に基づいて、画像Ｄ１＿ｉｍｇ４が撮像された位置の候補領域ＡＲ＿Ｃ４を求める。つまり、図８に示すように、平面視において、ランドマークＬＭ２の位置（中心点）から、ランドマークＬＭ２の第２パターン領域ｐｔｎ２（ＬＭ２）の中央付近の点を通る直線のｘ軸負方向に、距離ｄ２だけ離れた領域が、時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ４であると判定する。なお、候補領域取得部３Ａは、ランドマーク検出信号ＬＭ_ｔから、ランドマークＬＭ２とロボットＲｂｔ１との距離ｄ２を取得する。

候補領域取得部３Ａは、上記のようにして取得した時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ４についての情報を、自己位置候補情報Ｃｄｔ_ｔに含めて、状態更新部４１に出力する。

状態更新部４１は、自己位置候補情報Ｃｄｔ_ｔから、時刻ｔのロボットＲｂｔ１の位置が、候補領域ＡＲ＿Ｃ４である可能性が高いと判定し、ロボットＲｂｔ１の位置に関する内部状態データを更新する。

≪図９の場合≫
図９は、は、時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ５と、本実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ５とを模式的に示した図である。なお、画像Ｄ１＿ｉｍｇ５は、ランドマーク検出部２Ａから候補領域取得部３Ａに出力される画像（画像データ）である。

候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎに基づいて、図９に示す画像Ｄ１＿ｉｍｇ５を解析する。具体的には、候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎから、ランドマークＬＭ２の平面視において一方の半円に相当する円周上の表面が第１パターン領域（黒色一色のパターン）（図９において、ｐｔｎ１（ＬＭ２）と表記している領域）であり、ランドマークＬＭ２の平面視において他方の半円に相当する円周上の表面が第２パターン領域（白色一色のパターン）（図９において、ｐｔｎ２（ＬＭ２）と表記している領域）であることを認識する。

そして、候補領域取得部３Ａは、画像Ｄ１＿ｉｍｇ５上において、ランドマークの左寄りの領域に、第１パターン領域（黒色一色のパターン）と第２パターン領域（白色一色のパターン）が存在し、かつ、左側に第１パターン領域（黒色一色のパターン）が存在し、右側に第２パターン領域（白色一色のパターン）が存在することを認識する。また、候補領域取得部３Ａは、画像上の水平方向において、第１パターン領域が占める割合と、第２パターン領域が占める割合とを求める。そして、候補領域取得部３Ａは、上記画像解析結果に基づいて、画像Ｄ１＿ｉｍｇ５が撮像された位置の候補領域ＡＲ＿Ｃ５を求める。つまり、図９に示すように、平面視において、ランドマークＬＭ２の位置（中心点）から、距離ｄ２だけ離れた点からランドマークを見たときに、画像Ｄ１＿ｉｍｇ５のランドマークの見え方と一致する点を求める。図９では、平面視において、ランドマークＬＭ２の中心点と、点Ａ１とを通る直線の図９の斜め下方向に距離ｄ２だけ離れた領域が、時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ５であると判定する。なお、候補領域取得部３Ａは、ランドマーク検出信号ＬＭ_ｔから、ランドマークＬＭ２とロボットＲｂｔ１との距離ｄ２を取得する。

候補領域取得部３Ａは、上記のようにして取得した時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ５についての情報を、自己位置候補情報Ｃｄｔ_ｔに含めて、状態更新部４１に出力する。

状態更新部４１は、自己位置候補情報Ｃｄｔ_ｔから、時刻ｔのロボットＲｂｔ１の位置が、候補領域ＡＲ＿Ｃ５である可能性が高いと判定し、ロボットＲｂｔ１の位置に関する内部状態データを更新する。

以上のように、移動体制御装置２０００では、所定のパターンを有するランドマークを撮像した画像を実際の観測データとして取得し、撮像画像上のランドマークの見え方を解析することで、移動体（ロボットＲｂｔ１）の位置を精度良く推定し、高精度な状態推定処理を高速かつ適切に実行することができる。その結果、移動体制御装置２０００では、移動体（ロボットＲｂｔ１）の制御を適切に行うことができる。

さらに、移動体制御装置２０００では、上記の通り、ランドマークと、ロボットＲｂｔ１との距離を取得するだけで、ロボットＲｂｔ１の位置を高速かつ高精度に推定することができる。このため、移動体制御装置２０００では、多くの内部状態変数を用いた複雑な処理をすることなく、少ない演算量で推定処理を実現することができる。

なお、本実施形態において、ランドマークは、第１パターン領域（黒色一色のパターン）と第２パターン領域（白色一色のパターン）とを有するものとして、説明したが、これに限定されることはなく、例えば、ランドマークは、図１０に示すランドマークＬＭ３のように、平面視において、第１パターン領域（図１０のパターンｐｔｎ１（ＬＭ３））、第２パターン領域（図１０のパターンｐｔｎ２（ＬＭ３））、および、（図１０のパターンｐｔｎ３（ＬＭ３））の３つのパターンを有するものであってもよい。さらに、ランドマークは、４パターン以上のパターンを有するものであってもよい。

［他の実施形態］
上記実施形態では、距離センサー付き撮像装置をロボットＲｂｔ１に搭載し、カラーの撮像画像と、距離画像を取得する場合について、説明したが、これに限定されることはない。例えば、３次元計測用のカメラをロボットＲｂｔ１に搭載し、３次元計測用のカメラから取得した２つの画像から、距離画像を取得するようにしてもよい。また、レンジファインダー等の距離センサーをロボットＲｂｔ１に搭載し、ランドマークまでの距離を測定するようにしてもよい。

さらに、複数のセンサーをロボットＲｂｔ１に搭載し、複数のセンサーから取得される信号から、実際の観測データを取得するようにしてもよい。

上記実施形態において、環境地図データを作成するために設定した座標は、図２、図３、および、図５〜図９に示すようにｘｙ座標であったが、これに限定されることはなく、例えば、極座標等を用いてもよい。また、上記実施形態では、絶対座標を前提として説明したが、これに限定されることはなく、上記実施形態の処理を実行するために、例えば、ロボットＲｂｔ１の位置を原点とする相対座標を用いてもよい。

また、上記実施形態では、移動体（ロボットＲｂｔ１）の位置、ランドマークの位置が、２次元データ（ｘ座標値、ｙ座標値）により特定される場合について説明したが、これに限定あれることはなく、移動体（ロボットＲｂｔ１）の位置、ランドマークの位置を、３次元データ（例えば、ｘ座標値、ｙ座標値、ｚ座標値）により特定するようにしてもよい。移動体（ロボットＲｂｔ１）の位置、ランドマークの位置を、３次元データにより特定する場合であって、例えば、ランドマークＬＭ１Ａが３次元空間内の１点に配置され、ランドマークＬＭ１Ｂが３次元空間内の他の１点に配置されている場合、ランドマークＬＭ１Ａを中心とする半径ｄ１Ａ_ｔの球と、ランドマークＬＭ１Ｂを中心とする半径ｄ１Ｂ_ｔの球とが交差してできる円上が、時刻ｔにおけるロボットＲｂｔ１の位置の候補となる。

上記実施形態の一部または全部を組み合わせて移動体制御装置を構成するようにしてもよい。

また、上記実施形態で説明した移動体制御装置において、各ブロックは、ＬＳＩなどの半導体装置により個別に１チップ化されても良いし、一部又は全部を含むように１チップ化されても良い。

なお、ここでは、ＬＳＩとしたが、集積度の違いにより、ＩＣ、システムＬＳＩ、スーパーＬＳＩ、ウルトラＬＳＩと呼称されることもある。

また、集積回路化の手法はＬＳＩに限るものではなく、専用回路又は汎用プロセサで実現してもよい。ＬＳＩ製造後に、プログラムすることが可能なＦＰＧＡ（Ｆｉｅｌｄ Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）や、ＬＳＩ内部の回路セルの接続や設定を再構成可能なリコンフィギュラブル・プロセッサーを利用しても良い。

また、上記各実施形態の各機能ブロックの処理の一部または全部は、プログラムにより実現されるものであってもよい。そして、上記各実施形態の各機能ブロックの処理の一部または全部は、コンピュータにおいて、中央演算装置（ＣＰＵ）、マイクロプロセッサ、プロセッサ等により行われる。また、それぞれの処理を行うためのプログラムは、ハードディスク、ＲＯＭなどの記憶装置に格納されており、ＲＯＭにおいて、あるいはＲＡＭに読み出されて実行される。

また、上記実施形態の各処理をハードウェアにより実現してもよいし、ソフトウェア（ＯＳ（オペレーティングシステム）、ミドルウェア、あるいは、所定のライブラリとともに実現される場合を含む。）により実現してもよい。さらに、ソフトウェアおよびハードウェアの混在処理により実現しても良い。

また、上記実施形態における処理方法の実行順序は、必ずしも、上記実施形態の記載に制限されるものではなく、発明の要旨を逸脱しない範囲で、実行順序を入れ替えることができるものである。

前述した方法をコンピュータに実行させるコンピュータプログラム及びそのプログラムを記録したコンピュータ読み取り可能な記録媒体は、本発明の範囲に含まれる。ここで、コンピュータ読み取り可能な記録媒体としては、例えば、フレキシブルディスク、ハードディスク、ＣＤ−ＲＯＭ、ＭＯ、ＤＶＤ、ＤＶＤ−ＲＯＭ、ＤＶＤ−ＲＡＭ、大容量ＤＶＤ、次世代ＤＶＤ、半導体メモリを挙げることができる。

上記コンピュータプログラムは、上記記録媒体に記録されたものに限られず、電気通信回線、無線又は有線通信回線、インターネットを代表とするネットワーク等を経由して伝送されるものであってもよい。

なお、本発明の具体的な構成は、前述の実施形態に限られるものではなく、発明の要旨を逸脱しない範囲で種々の変更および修正が可能である。

１０００、２０００ 移動体制御装置
１ 観測取得部
２、２Ａ ランドマーク検出部
３、３Ａ 候補領域取得部
４ 状態推定部
５ 記憶部

Claims (6)

1. ２個以上のランドマークを１組として含むランドマークの組であるランドマークセットが設置されている環境内を移動する移動体を制御するために、前記ランドマークの情報を用いて表現される環境地図の作成処理と、前記移動体の自己の内部状態の推定処理とを実行する移動体制御装置であって、
   観測可能な事象から得られる観測データを取得する観測取得部と、
   前記観測取得部により取得された前記観測データに基づいて、（１）前記移動体と前記ランドマークセットに含まれる２個以上のランドマークのそれぞれとの距離を、ランドマーク距離情報として取得し、（２）前記移動体と前記ランドマークセットに含まれる２個以上のランドマークのそれぞれとを結ぶ直線と所定の軸とのなす角度をランドマーク角度情報として取得するランドマーク検出部と、
   前記ランドマーク検出部により取得された前記ランドマーク距離情報に基づいて、前記移動体の自己位置の候補領域を決定し、決定した前記候補領域についての情報を候補領域情報として取得する候補領域取得部と、
   前記観測取得部により取得された前記観測データと、前記ランドマーク検出部により生成された前記ランドマーク距離情報および前記ランドマーク角度情報と、前記候補領域取得部により取得された前記候補領域情報とに基づいて、前記移動体の内部状態を推定することで、自己内部状態推定データを取得するとともに、前記候補領域情報と、前記ランドマーク距離情報と、前記ランドマーク角度情報とに基づいて、前記環境地図を推定することで、環境地図データを取得する状態推定部と、
   を備え、
   前記候補領域取得部は、
   前記ランドマーク検出部により時刻ｔにおいて取得された前記ランドマーク距離情報に基づいて、平面視において、前記２個以上のランドマークを中心とする、時刻ｔにおける、円の交点を求め、求めた時刻ｔの前記円の交点を含む領域を、前記移動体の自己位置の時刻ｔの候補領域に決定し、決定した時刻ｔの前記候補領域についての情報を時刻ｔの候補領域情報として取得し、
   時刻ｔの前記候補領域は、前記２個以上のランドマークのそれぞれを中心とする円であって、それぞれ対応するランドマークと前記移動体との間の時刻ｔでの距離を半径とする前記円の時刻ｔの複数の交点の情報を含み、
   さらに、前記候補領域取得部は、時刻ｔ＋１において、前記ランドマーク検出部により取得された前記ランドマーク距離情報に基づいて、前記２個以上のランドマークをそれぞれ中心とする円の、時刻ｔの次の時刻である時刻ｔ＋1の交点を求め、
   時刻ｔ＋１の交点を含む１個以上の領域を前記移動体の位置の候補領域に決定し、決定した当該候補領域を示す情報を時刻ｔ＋１の候補領域情報として取得し、
   時刻ｔ＋１の前記候補領域情報は、２個以上の前記ランドマークを中心とする円の時刻ｔ＋１の複数の交点についての情報を含んでおり、前記時刻ｔ＋１の２個以上の前記ランドマークを中心とする円は、それぞれ、前記移動体と各ランドマークとの間の時刻ｔ＋１の距離を半径としており、
   前記状態推定部は、（１）時刻ｔの前記候補領域情報と、（２）時刻ｔ＋１の前記候補領域情報と、（３）時刻ｔの制御入力データとに基づいて、前記移動体の内部状態および前記環境地図を推定することで、時刻ｔ＋１の前記移動体の位置を予測するものであり、
   時刻ｔの前記制御入力データは、時刻ｔの前記移動体の位置から前記移動体を所定の方向に移動させるための制御入力データであり、前記時刻ｔ＋１は、前記移動体が前記所定の方向に移動が完了したときの時刻である、
   移動体制御装置。
2. 円柱状のランドマークであって、
   前記ランドマークを設置した状態の平面視において、前記ランドマークの表面の一方の半円に相当する円周上の表面である第１表面領域は、第１パターンを有し、
   前記ランドマークを設置した状態の平面視において、前記ランドマークの表面の他方の半円に相当する円周上の表面である第２表面領域は、前記第１パターンとは異なる第２パターンを有している前記ランドマークが設置されている環境内を移動する移動体を制御するために、前記ランドマークの情報を用いて表現される環境地図の作成処理と、前記移動体の自己の内部状態の推定処理とを実行する移動体制御装置であって、
   観測可能な事象から得られる観測データを取得する観測取得部であって、前記ランドマークを撮像した画像をランドマーク撮像画像として、前記観測データに含めて取得する前記観測取得部と、
   前記観測取得部により取得された前記観測データに基づいて、（１）前記移動体と前記ランドマークとの距離を、ランドマーク距離情報として取得し、（２）前記移動体と前記ランドマークとを結ぶ直線と所定の軸とのなす角度をランドマーク角度情報として取得するランドマーク検出部と、
   前記ランドマーク検出部により取得された前記ランドマーク距離情報と、前記ランドマーク撮像画像上での前記ランドマークの前記第１パターンと前記第２パターンとの視認状態とに基づいて、前記移動体の自己位置の候補領域を決定し、決定した前記候補領域についての情報を候補領域情報として取得する候補領域取得部と、
   前記観測取得部により取得された前記観測データと、前記ランドマーク検出部により生成された前記ランドマーク距離情報および前記ランドマーク角度情報と、前記候補領域取得部により取得された前記候補領域情報とに基づいて、前記移動体の内部状態を推定することで、自己内部状態推定データを取得するとともに、前記候補領域情報と、前記ランドマーク距離情報と、前記ランドマーク角度情報とに基づいて、前記環境地図を推定することで、環境地図データを取得する状態推定部と、
   を備え、
   前記ランドマークの前記第１パターンと前記第２パターンとの境界線は、前記ランドマークの上面の中心点と底面の中心点とを結ぶ直線である前記ランドマークの中心軸と平行であり、
   前記候補領域取得部は、
   （１）前記ランドマーク検出部により取得された前記ランドマーク距離情報と、（２）前記ランドマーク撮像画像上の前記中心軸と直交する方向における、前記ランドマークの前記第１パターンと前記第２パターンの占有比率に基づいて、前記移動体の自己位置の候補領域を決定する、
   移動体制御装置。
3. 前記候補領域取得部は、
   前記ランドマーク距離情報と、前記ランドマーク撮像画像上での前記ランドマークの前記第１パターンと前記第２パターンとの占有比率とに基づいて、前記移動体の自己位置の候補領域を決定し、決定した前記候補領域についての情報を候補領域情報として取得する、
   請求項２に記載の移動体制御装置。
4. 請求項２または３に記載の移動体制御装置とともに用いられる３次元形状を有するランドマークであって、
   前記ランドマークの表面の一部の領域である第１表面領域は、第１パターンを有し、
   前記ランドマークの表面の一部の領域であって、前記第１表面領域以外の領域である第２表面領域は、前記第１パターンとは異なる第２パターンを有している、
   ランドマーク。
5. ２個以上のランドマークを１組として含むランドマークの組であるランドマークセットが設置されている環境内を移動する移動体を制御するために、前記ランドマークの情報を用いて表現される環境地図の作成処理と、前記移動体の自己の内部状態の推定処理とを実行する移動体制御方法をコンピュータで実行させるためのプログラムであって、
   観測可能な事象から得られる観測データを取得する観測取得ステップと、
   前記観測取得ステップにより取得された前記観測データに基づいて、（１）前記移動体と前記ランドマークセットに含まれる２個以上のランドマークのそれぞれとの距離を、ランドマーク距離情報として取得し、（２）前記移動体と前記ランドマークセットに含まれる２個以上のランドマークのそれぞれとを結ぶ直線と所定の軸とのなす角度をランドマーク角度情報として取得するランドマーク検出ステップと、
   前記ランドマーク検出ステップにより取得された前記ランドマーク距離情報に基づいて、前記移動体の自己位置の候補領域を決定し、決定した前記候補領域についての情報を候補領域情報として取得する候補領域取得ステップと、
   前記観測取得ステップにより取得された前記観測データと、前記ランドマーク検出ステップにより生成された前記ランドマーク距離情報および前記ランドマーク角度情報と、前記候補領域取得ステップにより取得された前記候補領域情報とに基づいて、前記移動体の内部状態を推定することで、自己内部状態推定データを取得するとともに、前記候補領域情報と、前記ランドマーク距離情報と、前記ランドマーク角度情報とに基づいて、前記環境地図を推定することで、環境地図データを取得する状態推定ステップと、
   を備え
   前記候補領域取得ステップでは、
   前記ランドマーク検出ステップにより時刻ｔにおいて取得された前記ランドマーク距離情報に基づいて、平面視において、前記２個以上のランドマークを中心とする、時刻ｔにおける、円の交点を求め、求めた時刻ｔの前記円の交点を含む領域を、前記移動体の自己位置の時刻ｔの候補領域に決定し、決定した時刻ｔの前記候補領域についての情報を時刻ｔの候補領域情報として取得し、
   時刻ｔの前記候補領域は、前記２個以上のランドマークのそれぞれを中心とする円であって、それぞれ対応するランドマークと前記移動体との間の時刻ｔでの距離を半径とする前記円の時刻ｔの複数の交点の情報を含み、
   さらに、前記候補領域取得ステップでは、時刻ｔ＋１において、前記ランドマーク検出ステップにより取得された前記ランドマーク距離情報に基づいて、前記２個以上のランドマークをそれぞれ中心とする円の、時刻ｔの次の時刻である時刻ｔ＋1の交点を求め、
   時刻ｔ＋１の交点を含む１個以上の領域を前記移動体の位置の候補領域に決定し、決定した当該候補領域を示す情報を時刻ｔ＋１の候補領域情報として取得し、
   時刻ｔ＋１の前記候補領域情報は、２個以上の前記ランドマークを中心とする円の時刻ｔ＋１の複数の交点についての情報を含んでおり、前記時刻ｔ＋１の２個以上の前記ランドマークを中心とする円は、それぞれ、前記移動体と各ランドマークとの間の時刻ｔ＋１の距離を半径としており、
   前記状態推定ステップは、（１）時刻ｔの前記候補領域情報と、（２）時刻ｔ＋１の前記候補領域情報と、（３）時刻ｔの制御入力データとに基づいて、前記移動体の内部状態および前記環境地図を推定することで、時刻ｔ＋１の前記移動体の位置を予測するものであり、
   時刻ｔの前記制御入力データは、時刻ｔの前記移動体の位置から前記移動体を所定の方向に移動させるための制御入力データであり、前記時刻ｔ＋１は、前記移動体が前記所定の方向に移動が完了したときの時刻である、
   移動体制御方法をコンピュータで実行させるためのプログラム。
6. 円柱状のランドマークであって、
   前記ランドマークを設置した状態の平面視において、前記ランドマークの表面の一方の半円に相当する円周上の表面である第１表面領域は、第１パターンを有し、
   前記ランドマークを設置した状態の平面視において、前記ランドマークの表面の他方の半円に相当する円周上の表面である第２表面領域は、前記第１パターンとは異なる第２パターンを有している前記ランドマークが設置されている環境内を移動する移動体を制御するために、前記ランドマークの情報を用いて表現される環境地図の作成処理と、前記移動体の自己の内部状態の推定処理とを実行する移動体制御方法をコンピュータで実行させるためのプログラムであって、
   観測可能な事象から得られる観測データを取得する観測取得ステップであって、前記ランドマークを撮像した画像をランドマーク撮像画像として、前記観測データに含めて取得する前記観測取得ステップと、
   前記観測取得ステップにより取得された前記観測データに基づいて、（１）前記移動体と前記ランドマークとの距離を、ランドマーク距離情報として取得し、（２）前記移動体と前記ランドマークとを結ぶ直線と所定の軸とのなす角度をランドマーク角度情報として取得するランドマーク検出ステップと、
   前記ランドマーク検出ステップにより取得された前記ランドマーク距離情報と、前記ランドマーク撮像画像上での前記ランドマークの前記第１パターンと前記第２パターンとの視認状態とに基づいて、前記移動体の自己位置の候補領域を決定し、決定した前記候補領域についての情報を候補領域情報として取得する候補領域取得ステップと、
   前記観測取得ステップにより取得された前記観測データと、前記ランドマーク検出ステップにより生成された前記ランドマーク距離情報および前記ランドマーク角度情報と、前記候補領域取得ステップにより取得された前記候補領域情報とに基づいて、前記移動体の内部状態を推定することで、自己内部状態推定データを取得するとともに、前記候補領域情報と、前記ランドマーク距離情報と、前記ランドマーク角度情報とに基づいて、前記環境地図を推定することで、環境地図データを取得する状態推定ステップと、
   を備え、
   前記ランドマークの前記第１パターンと前記第２パターンとの境界線は、前記ランドマークの上面の中心点と底面の中心点とを結ぶ直線である前記ランドマークの中心軸と平行であり、
   前記候補領域取得ステップでは、
   （１）前記ランドマーク検出部により取得された前記ランドマーク距離情報と、（２）前記ランドマーク撮像画像上の前記中心軸と直交する方向における、前記ランドマークの前記第１パターンと前記第２パターンの占有比率に基づいて、前記移動体の自己位置の候補領域を決定する、
   移動体制御方法をコンピュータで実行させるためのプログラム。
   
   

Images