(57)【特許請求の範囲】
【請求項１】
２個以上のランドマークを１組として含むランドマークの組であるランドマークセットが設置されている環境内を移動する移動体を制御するために、前記ランドマークの情報を用いて表現される環境地図の作成処理と、前記移動体の自己の内部状態の推定処理とを実行する移動体制御装置であって、
観測可能な事象から得られる観測データを取得する観測取得部と、
前記観測取得部により取得された前記観測データに基づいて、（１）前記移動体と前記ランドマークセットに含まれる２個以上のランドマークのそれぞれとの距離を、ランドマーク距離情報として取得し、（２）前記移動体と前記ランドマークセットに含まれる２個以上のランドマークのそれぞれとを結ぶ直線と所定の軸とのなす角度をランドマーク角度情報として取得するランドマーク検出部と、
前記ランドマーク検出部により取得された前記ランドマーク距離情報に基づいて、前記移動体の自己位置の候補領域を決定し、決定した前記候補領域についての情報を候補領域情報として取得する候補領域取得部と、
前記観測取得部により取得された前記観測データと、前記ランドマーク検出部により生成された前記ランドマーク距離情報および前記ランドマーク角度情報と、前記候補領域取得部により取得された前記候補領域情報とに基づいて、前記移動体の内部状態を推定することで、自己内部状態推定データを取得するとともに、前記候補領域情報と、前記ランドマーク距離情報と、前記ランドマーク角度情報とに基づいて、前記環境地図を推定することで、環境地図データを取得する状態推定部と、
を備え、
前記候補領域取得部は、
前記ランドマーク検出部により時刻ｔにおいて取得された前記ランドマーク距離情報に基づいて、平面視において、前記２個以上のランドマークを中心とする、時刻ｔにおける、円の交点を求め、求めた時刻ｔの前記円の交点を含む領域を、前記移動体の自己位置の時刻ｔの候補領域に決定し、決定した時刻ｔの前記候補領域についての情報を時刻ｔの候補領域情報として取得し、
時刻ｔの前記候補領域は、前記２個以上のランドマークのそれぞれを中心とする円であって、それぞれ対応するランドマークと前記移動体との間の時刻ｔでの距離を半径とする前記円の時刻ｔの複数の交点の情報を含み、
さらに、前記候補領域取得部は、時刻ｔ＋１において、前記ランドマーク検出部により取得された前記ランドマーク距離情報に基づいて、前記２個以上のランドマークをそれぞれ中心とする円の、時刻ｔの次の時刻である時刻ｔ＋1の交点を求め、
時刻ｔ＋１の交点を含む１個以上の領域を前記移動体の位置の候補領域に決定し、決定した当該候補領域を示す情報を時刻ｔ＋１の候補領域情報として取得し、
時刻ｔ＋１の前記候補領域情報は、２個以上の前記ランドマークを中心とする円の時刻ｔ＋１の複数の交点についての情報を含んでおり、前記時刻ｔ＋１の２個以上の前記ランドマークを中心とする円は、それぞれ、前記移動体と各ランドマークとの間の時刻ｔ＋１の距離を半径としており、
前記状態推定部は、（１）時刻ｔの前記候補領域情報と、（２）時刻ｔ＋１の前記候補領域情報と、（３）時刻ｔの制御入力データとに基づいて、前記移動体の内部状態および前記環境地図を推定することで、時刻ｔ＋１の前記移動体の位置を予測するものであり、
時刻ｔの前記制御入力データは、時刻ｔの前記移動体の位置から前記移動体を所定の方向に移動させるための制御入力データであり、前記時刻ｔ＋１は、前記移動体が前記所定の方向に移動が完了したときの時刻である、
移動体制御装置。
【請求項２】
円柱状のランドマークであって、
前記ランドマークを設置した状態の平面視において、前記ランドマークの表面の一方の半円に相当する円周上の表面である第１表面領域は、第１パターンを有し、
前記ランドマークを設置した状態の平面視において、前記ランドマークの表面の他方の半円に相当する円周上の表面である第２表面領域は、前記第１パターンとは異なる第２パターンを有している前記ランドマークが設置されている環境内を移動する移動体を制御するために、前記ランドマークの情報を用いて表現される環境地図の作成処理と、前記移動体の自己の内部状態の推定処理とを実行する移動体制御装置であって、
観測可能な事象から得られる観測データを取得する観測取得部であって、前記ランドマークを撮像した画像をランドマーク撮像画像として、前記観測データに含めて取得する前記観測取得部と、
前記観測取得部により取得された前記観測データに基づいて、（１）前記移動体と前記ランドマークとの距離を、ランドマーク距離情報として取得し、（２）前記移動体と前記ランドマークとを結ぶ直線と所定の軸とのなす角度をランドマーク角度情報として取得するランドマーク検出部と、
前記ランドマーク検出部により取得された前記ランドマーク距離情報と、前記ランドマーク撮像画像上での前記ランドマークの前記第１パターンと前記第２パターンとの視認状態とに基づいて、前記移動体の自己位置の候補領域を決定し、決定した前記候補領域についての情報を候補領域情報として取得する候補領域取得部と、
前記観測取得部により取得された前記観測データと、前記ランドマーク検出部により生成された前記ランドマーク距離情報および前記ランドマーク角度情報と、前記候補領域取得部により取得された前記候補領域情報とに基づいて、前記移動体の内部状態を推定することで、自己内部状態推定データを取得するとともに、前記候補領域情報と、前記ランドマーク距離情報と、前記ランドマーク角度情報とに基づいて、前記環境地図を推定することで、環境地図データを取得する状態推定部と、
を備え、
前記ランドマークの前記第１パターンと前記第２パターンとの境界線は、前記ランドマークの上面の中心点と底面の中心点とを結ぶ直線である前記ランドマークの中心軸と平行であり、
前記候補領域取得部は、
（１）前記ランドマーク検出部により取得された前記ランドマーク距離情報と、（２）前記ランドマーク撮像画像上の前記中心軸と直交する方向における、前記ランドマークの前記第１パターンと前記第２パターンの占有比率に基づいて、前記移動体の自己位置の候補領域を決定する、
移動体制御装置。
【請求項３】
前記候補領域取得部は、
前記ランドマーク距離情報と、前記ランドマーク撮像画像上での前記ランドマークの前記第１パターンと前記第２パターンとの占有比率とに基づいて、前記移動体の自己位置の候補領域を決定し、決定した前記候補領域についての情報を候補領域情報として取得する、
請求項２に記載の移動体制御装置。
【請求項４】
請求項２または３に記載の移動体制御装置とともに用いられる３次元形状を有するランドマークであって、
前記ランドマークの表面の一部の領域である第１表面領域は、第１パターンを有し、
前記ランドマークの表面の一部の領域であって、前記第１表面領域以外の領域である第２表面領域は、前記第１パターンとは異なる第２パターンを有している、
ランドマーク。
【請求項５】
２個以上のランドマークを１組として含むランドマークの組であるランドマークセットが設置されている環境内を移動する移動体を制御するために、前記ランドマークの情報を用いて表現される環境地図の作成処理と、前記移動体の自己の内部状態の推定処理とを実行する移動体制御方法をコンピュータで実行させるためのプログラムであって、
観測可能な事象から得られる観測データを取得する観測取得ステップと、
前記観測取得ステップにより取得された前記観測データに基づいて、（１）前記移動体と前記ランドマークセットに含まれる２個以上のランドマークのそれぞれとの距離を、ランドマーク距離情報として取得し、（２）前記移動体と前記ランドマークセットに含まれる２個以上のランドマークのそれぞれとを結ぶ直線と所定の軸とのなす角度をランドマーク角度情報として取得するランドマーク検出ステップと、
前記ランドマーク検出ステップにより取得された前記ランドマーク距離情報に基づいて、前記移動体の自己位置の候補領域を決定し、決定した前記候補領域についての情報を候補領域情報として取得する候補領域取得ステップと、
前記観測取得ステップにより取得された前記観測データと、前記ランドマーク検出ステップにより生成された前記ランドマーク距離情報および前記ランドマーク角度情報と、前記候補領域取得ステップにより取得された前記候補領域情報とに基づいて、前記移動体の内部状態を推定することで、自己内部状態推定データを取得するとともに、前記候補領域情報と、前記ランドマーク距離情報と、前記ランドマーク角度情報とに基づいて、前記環境地図を推定することで、環境地図データを取得する状態推定ステップと、
を備え
前記候補領域取得ステップでは、
前記ランドマーク検出ステップにより時刻ｔにおいて取得された前記ランドマーク距離情報に基づいて、平面視において、前記２個以上のランドマークを中心とする、時刻ｔにおける、円の交点を求め、求めた時刻ｔの前記円の交点を含む領域を、前記移動体の自己位置の時刻ｔの候補領域に決定し、決定した時刻ｔの前記候補領域についての情報を時刻ｔの候補領域情報として取得し、
時刻ｔの前記候補領域は、前記２個以上のランドマークのそれぞれを中心とする円であって、それぞれ対応するランドマークと前記移動体との間の時刻ｔでの距離を半径とする前記円の時刻ｔの複数の交点の情報を含み、
さらに、前記候補領域取得ステップでは、時刻ｔ＋１において、前記ランドマーク検出ステップにより取得された前記ランドマーク距離情報に基づいて、前記２個以上のランドマークをそれぞれ中心とする円の、時刻ｔの次の時刻である時刻ｔ＋1の交点を求め、
時刻ｔ＋１の交点を含む１個以上の領域を前記移動体の位置の候補領域に決定し、決定した当該候補領域を示す情報を時刻ｔ＋１の候補領域情報として取得し、
時刻ｔ＋１の前記候補領域情報は、２個以上の前記ランドマークを中心とする円の時刻ｔ＋１の複数の交点についての情報を含んでおり、前記時刻ｔ＋１の２個以上の前記ランドマークを中心とする円は、それぞれ、前記移動体と各ランドマークとの間の時刻ｔ＋１の距離を半径としており、
前記状態推定ステップは、（１）時刻ｔの前記候補領域情報と、（２）時刻ｔ＋１の前記候補領域情報と、（３）時刻ｔの制御入力データとに基づいて、前記移動体の内部状態および前記環境地図を推定することで、時刻ｔ＋１の前記移動体の位置を予測するものであり、
時刻ｔの前記制御入力データは、時刻ｔの前記移動体の位置から前記移動体を所定の方向に移動させるための制御入力データであり、前記時刻ｔ＋１は、前記移動体が前記所定の方向に移動が完了したときの時刻である、
移動体制御方法をコンピュータで実行させるためのプログラム。
【請求項６】
円柱状のランドマークであって、
前記ランドマークを設置した状態の平面視において、前記ランドマークの表面の一方の半円に相当する円周上の表面である第１表面領域は、第１パターンを有し、
前記ランドマークを設置した状態の平面視において、前記ランドマークの表面の他方の半円に相当する円周上の表面である第２表面領域は、前記第１パターンとは異なる第２パターンを有している前記ランドマークが設置されている環境内を移動する移動体を制御するために、前記ランドマークの情報を用いて表現される環境地図の作成処理と、前記移動体の自己の内部状態の推定処理とを実行する移動体制御方法をコンピュータで実行させるためのプログラムであって、
観測可能な事象から得られる観測データを取得する観測取得ステップであって、前記ランドマークを撮像した画像をランドマーク撮像画像として、前記観測データに含めて取得する前記観測取得ステップと、
前記観測取得ステップにより取得された前記観測データに基づいて、（１）前記移動体と前記ランドマークとの距離を、ランドマーク距離情報として取得し、（２）前記移動体と前記ランドマークとを結ぶ直線と所定の軸とのなす角度をランドマーク角度情報として取得するランドマーク検出ステップと、
前記ランドマーク検出ステップにより取得された前記ランドマーク距離情報と、前記ランドマーク撮像画像上での前記ランドマークの前記第１パターンと前記第２パターンとの視認状態とに基づいて、前記移動体の自己位置の候補領域を決定し、決定した前記候補領域についての情報を候補領域情報として取得する候補領域取得ステップと、
前記観測取得ステップにより取得された前記観測データと、前記ランドマーク検出ステップにより生成された前記ランドマーク距離情報および前記ランドマーク角度情報と、前記候補領域取得ステップにより取得された前記候補領域情報とに基づいて、前記移動体の内部状態を推定することで、自己内部状態推定データを取得するとともに、前記候補領域情報と、前記ランドマーク距離情報と、前記ランドマーク角度情報とに基づいて、前記環境地図を推定することで、環境地図データを取得する状態推定ステップと、
を備え、
前記ランドマークの前記第１パターンと前記第２パターンとの境界線は、前記ランドマークの上面の中心点と底面の中心点とを結ぶ直線である前記ランドマークの中心軸と平行であり、
前記候補領域取得ステップでは、
（１）前記ランドマーク検出部により取得された前記ランドマーク距離情報と、（２）前記ランドマーク撮像画像上の前記中心軸と直交する方向における、前記ランドマークの前記第１パターンと前記第２パターンの占有比率に基づいて、前記移動体の自己位置の候補領域を決定する、
移動体制御方法をコンピュータで実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ＳＬＡＭ(Ｓｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｌｉｚａｔｉｏｎ  ａｎｄ  Ｍａｐｐｉｎｇ)技術を用いて移動体を制御する技術に関する。
【背景技術】
【０００２】
自律走行ロボット等の移動体が自律的に移動しながら、自己位置の推定と環境地図の作成とを同時に行うための技術として、ＳＬＡＭ(Ｓｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｌｉｚａｔｉｏｎ  ａｎｄ  Ｍａｐｐｉｎｇ)が知られている。
【０００３】
ＳＬＡＭ技術により自律走行するロボット（移動体）では、ロボットに搭載されたセンサーから得られる観測、および、ロボットに対する制御入力のみを使用して、ロボットの周囲の環境地図を作成しながら、作成した環境地図中でのロボットの自己の位置および姿勢を推定する。言い換えれば、この推定処理は、ロボットがシステムモデルｘｔ〜g（ｘｔ｜ｘｔ−１，Ｕｔ）（Ｕｔ:制御入力）に従い、観測が観測モデルＺｔ〜ｈ（Ｚｔ｜ｘｔ，ｍ）（ｍ：環境地図）に従う状態空間モデルに対して、制御入力系列Ｕ１：ｔ＝｛Ｕ１，Ｕ２，・・・，Ｕｔ｝、および、観測系列Ｚ１：ｔ＝｛Ｚ１，Ｚ２，・・・，Ｚｔ｝が与えられたときに、ロボットの自己位置および姿勢Ｘｔ（あるいは、Ｘ１：ｔ＝｛Ｘ１，Ｘ２，・・・，Ｘｔ｝）と、環境地図ｍとを推定する処理である。
【０００４】
この推定処理において、パーティクルフィルタを用いて、ロボットの姿勢を高精度で推定する技術が開発されている（例えば、特許文献１）。
【０００５】
観測を取得するためのセンサーとしては、可視光用イメージセンサーを用いたＲＧＢカメラや、赤外線やレーザーを使用して距離情報を取得するレンジファインダー等がよく使用される。また、観測を取得するために、２種類以上のセンサーを組み合わせて用いられることもある。
【０００６】
ＳＬＡＭ技術により自律走行するロボット（移動体）において、取得される環境地図は、幾何学的な地図ではなく、トポロジカルな地図として、取得されることが多い。環境地図は、例えば、ランドマークに基づく情報により、取得される。この場合、環境地図は、ランドマークの情報の集合として表現される。それぞれのランドマークのパラメータは、例えば、（１）ランドマークの位置情報、および、（２）ランドマークの位置情報の不確実性を表す共分散行列である。
【０００７】
ＳＬＡＭ技術により自律走行するロボットにおいて、ＲＧＢカメラをセンサーとして使用する場合、例えば、何らかの特徴を持つ点や線等、あるいは、判別可能なマーカーを貼付した物体等をランドマークとして設定し、ＲＧＢカメラにより撮像された画像中において、ランドマークに相当する画像領域を検出（特定）することで、ランドマークの情報を取得する。この場合、ロボットの実際の位置と、ランドマークの実際の位置との相対関係を、ＲＧＢカメラで取得した画像から特定し、特定した結果に基づいて、現時刻における観測が取得される。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特開２００８−１２６４０１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００９】
しかしながら、上記のように、ＲＧＢカメラで撮像した画像からランドマークの位置を取得する場合、ロボットの実際の位置と、ランドマークの実際の位置とを精度よく推定することが困難な場合がある。ＲＧＢカメラやレンジファインダーは指向性が高く観測できる範囲が制限されるため、ロボットの周囲に多くのランドマークが設置されている場合であっても、ＲＧＢカメラやレンジファインダーにより取得されたデータから、一度に発見することができるランドマークの数は少ない。そして、例えば、（１）ロボットから発見したランドマークまでの距離と、（２）ロボットが移動する環境を、例えば、上方から見たときのロボットから発見したランドマークまでの直線と所定の基準軸とのなす角度と、をランドマークの情報として取得する場合、取得したランドマークの情報からロボットの位置を精度良く推定することが困難である場合がある。これは、発見されたランドマークの位置を中心として、ロボットから発見したランドマークまでの距離を半径とする円周上の点の全てが、ロボットの位置の候補点となるためである。つまり、ロボットの位置の候補点が多いので、現時刻のロボットの位置を推定するための処理量が多くなり、ロボットの位置を精度良く推定することが難しい。このように、自律走行ロボットにおいて、ロボットの位置を精度良く推定できないと、実際のロボットの内部状態と推定結果との間の誤差が大きくなり、ロボット（移動体）の安定した自律走行が困難になる。
【００１０】
そこで、本発明は、上記問題点に鑑み、ランドマークの情報から、移動体（ロボット）の位置を精度良く推定し、高精度な状態推定処理を高速かつ適切に実行することで、移動体の制御を適切に行う移動体制御装置、プログラム、および、それらに用いられるランドマークを実現することを目的とする。
【課題を解決するための手段】
【００１１】
上記課題を解決するために、第１の発明は、２個以上のランドマークを１組として含むランドマークの組であるランドマークセットが設置されている環境内を移動する移動体を制御するために、ランドマークの情報を用いて表現される環境地図の作成処理と、移動体の自己の内部状態の推定処理とを実行する移動体制御装置であって、観測取得部と、ランドマーク検出部と、候補領域取得部と、状態推定部と、を備える。
【００１２】
観測取得部は、観測可能な事象から得られる観測データを取得する。
【００１３】
ランドマーク検出部は、観測取得部により取得された観測データに基づいて、（１）移動体とランドマークセットに含まれる２個以上のランドマークのそれぞれとの距離を、ランドマーク距離情報として取得し、（２）移動体とランドマークセットに含まれる２個以上のランドマークのそれぞれとを結ぶ直線と所定の軸とのなす角度をランドマーク角度情報として取得する。
【００１４】
候補領域取得部は、ランドマーク検出部により取得されたランドマーク距離情報に基づいて、移動体の自己位置の候補領域を決定し、決定した候補領域についての情報を候補領域情報として取得する。
【００１５】
状態推定部は、観測取得部により取得された観測データと、ランドマーク検出部により生成されたランドマーク距離情報およびランドマーク角度情報と、候補領域取得部により取得された候補領域情報とに基づいて、移動体の内部状態を推定することで、自己内部状態推定データを取得するとともに、候補領域情報と、ランドマーク距離情報と、ランドマーク角度情報とに基づいて、環境地図を推定することで、環境地図データを取得する。
【００１６】
この移動体制御装置では、２つ以上のランドマークを１組とするランドマークセットを用いて、移動体から２つ以上のランドマークまでの距離を測定することで、移動体の位置を少ない候補点に絞り、高速かつ高精度に、移動体の位置情報を含む内部状態データを推定することができる。つまり、この移動体制御装置では、ランドマークの情報から、移動体の位置を精度良く推定し、高精度な状態推定処理を高速かつ適切に実行することができる。その結果、この移動体制御装置では、移動体の制御を適切に行うことができる。
【００１７】
さらに、この移動体制御装置では、上記の通り、１つのランドマークセットに含まれる２つ以上のランドマークと、移動体との距離を取得するだけで、移動体の位置を高速かつ高精度に推定することができる。このため、この移動体制御装置では、多くの内部状態変数を用いた複雑な処理をすることなく、少ない演算量で推定処理を実現することができる。
【００１８】
第２の発明は、３次元形状を有するランドマークであって、ランドマークの表面の一部の領域である第１表面領域は、第１パターンを有し、ランドマークの表面の一部の領域であって、第１表面領域以外の領域である第２表面領域は、第１パターンとは異なる第２パターンを有しているランドマークが設置されている環境内を移動する移動体を制御するために、ランドマークの情報を用いて表現される環境地図の作成処理と、移動体の自己の内部状態の推定処理とを実行する移動体制御装置である。移動体制御装置は、観測取得部と、ランドマーク検出部と、候補領域取得部と、状態推定部と、を備える。
【００１９】
観測取得部は、観測可能な事象から得られる観測データを取得し、ランドマークを撮像した画像をランドマーク撮像画像として、観測データに含めて取得する。
【００２０】
ランドマーク検出部は、観測取得部により取得された観測データに基づいて、（１）移動体とランドマークとの距離を、ランドマーク距離情報として取得し、（２）移動体とランドマークとを結ぶ直線と所定の軸とのなす角度をランドマーク角度情報として取得する。
【００２１】
候補領域取得部は、ランドマーク検出部により取得されたランドマーク距離情報と、ランドマーク撮像画像上でのランドマークの第１パターンと第２パターンとの視認状態とに基づいて、移動体の自己位置の候補領域を決定し、決定した候補領域についての情報を候補領域情報として取得する。
【００２２】
状態推定部は、観測取得部により取得された観測データと、ランドマーク検出部により生成されたランドマーク距離情報およびランドマーク角度情報と、候補領域取得部により取得された候補領域情報とに基づいて、移動体の内部状態を推定することで、自己内部状態推定データを取得するとともに、候補領域情報と、ランドマーク距離情報と、ランドマーク角度情報とに基づいて、環境地図を推定することで、環境地図データを取得する。
【００２３】
この移動体制御装置では、所定のパターンを有するランドマークを撮像した画像を実際の観測データとして取得し、撮像画像上のランドマークの見え方を解析することで、移動体の位置を精度良く推定し、高精度な状態推定処理を高速かつ適切に実行することができる。その結果、この移動体制御装置では、移動体の制御を適切に行うことができる。
【００２４】
さらに、この移動体制御装置では、ランドマークと、移動体との距離を取得するだけで、移動体の位置を高速かつ高精度に推定することができる。このため、この移動体制御装置では、多くの内部状態変数を用いた複雑な処理をすることなく、少ない演算量で推定処理を実現することができる。
【００２５】
なお、ランドマークの第１パターンと第２パターンとの「視認状態」とは、例えば、撮像画像上における、ランドマークの第１パターンに相当する画像領域、および、第２パターンに相当する画像領域の（１）位置、（２）形状、（３）面積等で特定される状態のことをいう。
【００２６】
第３の発明は、第２の発明であって、候補領域取得部は、ランドマーク距離情報と、ランドマーク撮像画像上でのランドマークの第１パターンと第２パターンとの占有比率とに基づいて、移動体の自己位置の候補領域を決定し、決定した候補領域についての情報を候補領域情報として取得する。
【００２７】
これにより、この移動体制御装置では、ランドマーク撮像画像上でのランドマークの第１パターンと第２パターンとの占有比率とに基づいて、移動体の自己位置の候補領域を決定することができる。
【００２８】
第４の発明は、第２または第３の発明である移動体制御装置とともに用いられる３次元形状を有するランドマークである。ランドマークは、ランドマークの表面の一部の領域である第１表面領域は、第１パターンを有し、ランドマークの表面の一部の領域であって、第１表面領域以外の領域である第２表面領域は、第１パターンとは異なる第２パターンを有している。
【００２９】
これにより、第２または第３の発明である移動体制御装置により効率良く移動体の内部状態データの推定処理、環境地図の取得処理を実現するためのランドマークを実現することができる。
【００３０】
第５の発明は、２個以上のランドマークを１組として含むランドマークの組であるランドマークセットが設置されている環境内を移動する移動体を制御するために、ランドマークの情報を用いて表現される環境地図の作成処理と、移動体の自己の内部状態の推定処理とを実行する移動体制御方法をコンピュータで実行させるためのプログラムである。移動体制御方法は、観測取得ステップと、ランドマーク検出ステップと、候補領域取得ステップと、状態推定ステップと、を備える。
【００３１】
観測取得ステップは、観測可能な事象から得られる観測データを取得する。
【００３２】
ランドマーク検出ステップは、観測取得ステップにより取得された観測データに基づいて、（１）移動体とランドマークセットに含まれる２個以上のランドマークのそれぞれとの距離を、ランドマーク距離情報として取得し、（２）移動体とランドマークセットに含まれる２個以上のランドマークのそれぞれとを結ぶ直線と所定の軸とのなす角度をランドマーク角度情報として取得する。
【００３３】
候補領域取得ステップは、ランドマーク検出ステップにより取得されたランドマーク距離情報に基づいて、移動体の自己位置の候補領域を決定し、決定した候補領域についての情報を候補領域情報として取得する。
【００３４】
状態推定ステップは、観測取得ステップにより取得された観測データと、ランドマーク検出ステップにより生成されたランドマーク距離情報およびランドマーク角度情報と、候補領域取得ステップにより取得された候補領域情報とに基づいて、移動体の内部状態を推定することで、自己内部状態推定データを取得するとともに、候補領域情報と、ランドマーク距離情報と、ランドマーク角度情報とに基づいて、環境地図を推定することで、環境地図データを取得する。
【００３５】
これにより、第１の発明と同様の効果を奏する移動体制御方法をコンピュータで実行させるためのプログラムを実現することができる。
【００３６】
第６の発明は、３次元形状を有するランドマークであって、ランドマークの表面の一部の領域である第１表面領域は、第１パターンを有し、ランドマークの表面の一部の領域であって、第１表面領域以外の領域である第２表面領域は、第１パターンとは異なる第２パターンを有しているランドマークが設置されている環境内を移動する移動体を制御するために、ランドマークの情報を用いて表現される環境地図の作成処理と、移動体の自己の内部状態の推定処理とを実行する移動体制御方法をコンピュータで実行させるためのプログラムである。
【００３７】
移動体制御方法は、観測取得ステップと、ランドマーク検出ステップと、候補領域取得ステップと、状態推定ステップと、を備える。
【００３８】
観測取得ステップは、観測可能な事象から得られる観測データを取得する観測取得ステップであって、ランドマークを撮像した画像をランドマーク撮像画像として、観測データに含めて取得する。
【００３９】
ランドマーク検出ステップは、観測取得ステップにより取得された観測データに基づいて、（１）移動体とランドマークとの距離を、ランドマーク距離情報として取得し、（２）移動体とランドマークとを結ぶ直線と所定の軸とのなす角度をランドマーク角度情報として取得する。
【００４０】
候補領域取得ステップは、ランドマーク検出ステップにより取得されたランドマーク距離情報と、ランドマーク撮像画像上でのランドマークの第１パターンと第２パターンとの視認状態とに基づいて、移動体の自己位置の候補領域を決定し、決定した候補領域についての情報を候補領域情報として取得する。
【００４１】
状態推定ステップは、観測取得ステップにより取得された観測データと、ランドマーク検出ステップにより生成されたランドマーク距離情報およびランドマーク角度情報と、候補領域取得ステップにより取得された候補領域情報とに基づいて、移動体の内部状態を推定することで、自己内部状態推定データを取得するとともに、候補領域情報と、ランドマーク距離情報と、ランドマーク角度情報とに基づいて、環境地図を推定することで、環境地図データを取得する。
【００４２】
これにより、第２の発明と同様の効果を奏する移動体制御方法をコンピュータで実行させるためのプログラムを実現することができる。
【発明の効果】
【００４３】
本発明によれば、ランドマークの情報から、移動体（ロボット）の位置を精度良く推定し、高精度な状態推定処理を高速かつ適切に実行することで、移動体の制御を適切に行う移動体制御装置、プログラム、および、それらに用いられるランドマークを実現することができる。
【図面の簡単な説明】
【００４４】
【図１】第１実施形態に係る移動体制御装置１０００の概略構成図。
【図２】時刻ｔにおけるロボットＲｂｔ１の位置の候補となる点Ｃ１ｔ、Ｃ２ｔと、１組のランドマークＬＭ１ＡおよびランドマークＬＭ１Ｂとの位置関係を示す図（上方から見た図）。
【図３】時刻ｔ＋１におけるロボットＲｂｔ１の位置の候補となる点Ｃ１ｔ＋１、Ｃ２ｔ＋１と、１組のランドマークＬＭ１ＡおよびランドマークＬＭ１Ｂとの位置関係を示す図（上方から見た図）。
【図４】第２実施形態に係る移動体制御装置２０００の概略構成図。
【図５】時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ１と、第２実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ１とを模式的に示した図。
【図６】時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ２と、第２実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ２とを模式的に示した図。
【図７】時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ３と、第２実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ３とを模式的に示した図。
【図８】時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ４と、第２実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ４とを模式的に示した図。
【図９】は、時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ５と、第２実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ５とを模式的に示した図。
【図１０】ランドマークＬＭ３を模式的に示す図（上方から見た図）。
【発明を実施するための形態】
【００４５】
［第１実施形態］
第１実施形態について、図面を参照しながら、以下、説明する。
【００４６】
＜１．１：移動体制御装置の構成＞
図１は、第１実施形態に係る移動体制御装置１０００の概略構成図である。
【００４７】
移動体制御装置１０００は、移動体（例えば、自律走行ロボットＲｂｔ１）（不図示）に搭載される装置である。
【００４８】
移動体制御装置１０００は、図１に示すように、観測取得部１と、ランドマーク検出部２と、候補領域取得部３と、状態推定部４と、記憶部５とを備える。
【００４９】
なお、説明便宜のため、観測取得部１により取得される観測データＤ１は、移動体（ロボットＲｂｔ１）に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）と、距離センサー付き撮像装置により取得された距離画像（距離データ）であるものとして、以下、説明する。
【００５０】
観測取得部１は、移動体（ロボットＲｂｔ１）に搭載された距離センサー付き撮像装置（不図示）により取得されたデータを入力データＤｉｎとして入力とする。観測取得部１は、入力データＤｉｎから観測データＤ１、すなわち、画像（画像データ）Ｄ１＿ｉｍｇと距離画像（距離データ）Ｄ１＿ｄとを取得し、取得した観測データＤ１をランドマーク検出部２と、状態推定部４の状態更新部４１とに出力する。
ランドマーク検出部２は、観測取得部１から出力される観測データＤ１を入力する。ランドマーク検出部２は、観測データＤ１（画像データＤ１＿ｉｍｇおよび距離画像Ｄ１＿ｄ）から、ロボットＲｂｔ１からランドマークまでの距離および方向（角度）に関する情報を取得し、その取得した情報を含む信号を、ランドマーク検出信号ＬＭｔとして、候補領域取得部３と、状態推定部４の状態更新部４１と、環境地図更新部４２とに出力する。
【００５１】
候補領域取得部３は、ランドマーク検出部２から出力されるランドマーク検出信号ＬＭｔを入力する。候補領域取得部３は、ランドマーク検出信号ＬＭｔに基づいて、ロボットＲｂｔ１の現時刻の位置の候補位置を推定するための情報である自己位置候補情報Ｃｄｔｔを取得する。そして、候補領域取得部３は、取得した自己位置候補情報Ｃｄｔｔを状態推定部４の状態更新部４１に出力する。
【００５２】
状態推定部４は、図１に示すように、状態更新部４１と、環境地図更新部４２とを備える。
【００５３】
状態更新部４１は、観測取得部１から出力される観測データＤ１と、ランドマーク検出部２から出力されるランドマーク検出信号ＬＭｔと、候補領域取得部３から出力される自己位置候補情報Ｃｄｔｔと、移動体（ロボットＲｂｔ１）に対する制御入力データＵｔと、記憶部５から読み出した移動体（ロボットＲｂｔ１）の時刻ｔ−１の内部状態データＸｔ−１とを入力する。状態更新部４１は、観測データＤ１、ランドマーク検出信号ＬＭｔ、自己位置候補情報Ｃｄｔｔ、制御入力データＵｔ、および、ロボットＲｂｔ１の時刻ｔ−１の内部状態データＸｔ−１基づいて、ロボットＲｂｔ１の内部状態を推定し、推定結果を移動体の現時刻ｔの内部状態データＸｔとして、取得する（内部状態データを更新する）。そして、状態更新部４１は、取得した移動体の現時刻ｔの内部状態データＸｔを、状態推定部４から外部に出力するとともに、記憶部５に出力する。なお、状態推定部４から外部に出力された内部状態データＸｔは、移動体（自動走行ロボットＲｂｔ１）を制御するためのデータとして、例えば、移動体（自動走行ロボットＲｂｔ１）の全体を制御する機能部（全体制御部）（不図示）等により使用される。
【００５４】
また、状態更新部４１は、現時刻ｔのロボットＲｂｔ１の位置に関する内部状態データＸｔを、環境地図更新部４２に出力する。
【００５５】
また、状態更新部４１は、現時刻ｔのロボットＲｂｔ１に対する制御入力データＵｔを記憶部５に出力する。
【００５６】
環境地図更新部４２は、ランドマーク検出部２から出力されるランドマーク検出信号ＬＭｔと、記憶部５から読み出した時刻ｔ−１の環境地図データｍｔ−１と、状態更新部４１から出力される現時刻ｔのロボットＲｂｔ１の内部状態データＸｔとを入力する。環境地図更新部４２は、ランドマーク検出信号ＬＭｔから、検出されたランドマークについての情報を取得し、取得したランドマークの情報と、時刻ｔ−１の環境地図データｍｔ−１と、現時刻ｔのロボットＲｂｔ１の内部状態データＸｔとに基づいて、環境地図データを更新し、現時刻ｔの環境地図データｍｔを取得する。そして、環境地図更新部４２は、更新した環境地図データｍｔを状態推定部４から外部に出力するとともに、記憶部５に出力する。なお、状態推定部４から外部に出力された環境地図データｍｔは、移動体（自動走行ロボットＲｂｔ１）を制御するためのデータとして、例えば、移動体（自動走行ロボットＲｂｔ１）の全体を制御する機能部（全体制御部）（不図示）等により使用される。
【００５７】
記憶部５は、状態推定部４の状態更新部４１から出力される移動体の現時刻ｔの内部状態データＸｔと、環境地図データｍｔと、状態更新部４１から出力される制御入力データＵｔとを入力し、記憶する。記憶部５に記憶されたデータは、状態推定部４により、所定のタイミングで読み出される。なお、記憶部５は、複数の時刻の内部状態データと、環境地図データとを記憶することができる。
【００５８】
なお、現時刻ｔのデータとは、移動体制御装置１０００において、処理対象となっている観測データＤ１から取得されたデータ（移動体の内部状態データＸｔ、および、環境地図データｍｔ）のことをいい、現時刻ｔの１つ前の時刻ｔ−１のデータとは、現時刻ｔのデータを取得する１つ前に、移動体制御装置１０００において取得されたデータ（移動体の内部状態データＸｔ−１、および、環境地図データｍｔ−１）のことをいう。つまり、現時刻ｔのｋ個前の時刻ｔ−ｋ（ｋ：自然数）のデータとは、現時刻ｔのデータを取得するｋ個前に、移動体制御装置１０００において取得されたデータ（移動体の内部状態データＸｔ−ｋ、および、環境地図データｍｔ−ｋ）のことをいう。
【００５９】
また、上記データは、同期タイミングにより取得されるものであってもよいし、非同期タイミングにより取得されるものであってもよい。
【００６０】
＜１．２：移動体制御装置の動作＞
以上のように構成された移動体制御装置１０００の動作について、以下、説明する。
【００６１】
なお、以下では、自律走行する移動体をロボットＲｂｔ１とし、ロボットＲｂｔ１に搭載された可視光用イメージセンサーと距離センサーとを備える撮像装置（不図示）により撮像された画像（撮像画像）と、距離画像（距離データ）とを入力データＤｉｎとし、ランドマークの情報により環境地図が生成される場合について、説明する。
【００６２】
また、移動体制御装置１０００により環境地図を生成するために、２つのランドマークを１組のセットとして、ロボットＲｂｔ１が移動する環境に設置されているものとする。また、１組のセットを構成するランドマークは、他の組のランドマークと識別できるように、例えば、カラーコードや特定の模様が付されている。これにより、発見されたランドマークがどの組（どのランドマーク・セット）に属するランドマークであるかを特定することができる。
【００６３】
図２は、時刻ｔにおけるロボットＲｂｔ１の位置の候補となる点Ｃ１ｔ、Ｃ２ｔと、１組のランドマークＬＭ１ＡおよびランドマークＬＭ１Ｂとの位置関係を示す図（上方から見た図）である。なお、図２において、図２に示すようにｘ軸、ｙ軸をとるものとし、図２に「０」で示した位置を原点とする。なお、以下では、図２に示すように、説明便宜のため、２次元平面（ｘｙ平面）におけるロボットＲｂｔ１、ランドマークＬＭ１Ａ、ＬＭ１Ｂの位置関係に基づいて、移動体制御装置の処理について説明するが、これに限定されることはなく、移動体制御装置の処理については、３次元空間におけるロボットＲｂｔ１、ランドマークＬＭ１Ａ、ＬＭ１Ｂの位置関係に基づいて、実行されるものであってもよい。
【００６４】
図３は、時刻ｔ＋１におけるロボットＲｂｔ１の位置の候補となる点Ｃ１ｔ＋１、Ｃ２ｔ＋１と、１組のランドマークＬＭ１ＡおよびランドマークＬＭ１Ｂとの位置関係を示す図（上方から見た図）である。なお、図３においても、図２と同様に、図３に示すようにｘ軸、ｙ軸をとるものとし、図３に「０」で示した位置を原点とする。
【００６５】
以下では、時刻ｔにおいて、図２に点Ｃ１ｔで示す位置に存在するロボットＲｂｔ１に対して、図３に示す矢印Ｄｉｒ１方向に移動させるための制御データが入力され、時刻ｔ＋１において、図３に点Ｃ１ｔ＋１で示す位置にロボットＲｂｔ１が移動する場合を一例として、説明する。
【００６６】
ここで、移動体制御装置１０００において使用される、（１）ロボットＲｂｔ１の内部状態を示すデータ、および、（２）環境地図に関するデータについて、説明する。
【００６７】
（１．２．１：ロボットＲｂｔ１の内部状態データ）
移動体制御装置１０００では、ロボットＲｂｔ１の内部状態を示すデータＸｔとして、時刻ｔにおけるロボットＲｂｔ１の位置および角度に関する内部状態データＸｔが設定される。
【００６８】
時刻ｔにおけるロボットＲｂｔ１の位置および角度に関する内部状態データＸｔは、３次元の状態ベクトルであり、
Ｘｔ＝（ｘ０ｔ，ｙ０ｔ，θｔ）
ｘ０ｔ：時刻ｔにおけるロボットＲｂｔ１のｘ座標値の期待値
ｙ０ｔ：時刻ｔにおけるロボットＲｂｔ１のｙ座標値の期待値
θｔ：時刻ｔにおけるロボットＲｂｔ１のｘ軸正方向とのなす角度（ロボットＲｂｔ１の向き）の期待値
として表現される。
【００６９】
移動体制御装置１０００では、ロボットＲｂｔ１の内部状態を推定するために、パーティクルフィルタ（モンテカルロ近似）を用いるものとする。
【００７０】
つまり、パーティクルフィルタ処理では、時刻ｔ−１の状態ベクトルＸｔ−１の事後確率分布を現時刻ｔの状態ベクトルＸｔの事前確率分布とし、現時刻ｔの状態ベクトルＸｔの事前確率分布から予測処理により現時刻ｔの状態ベクトルＸｔの予測確率分布を取得する。そして、取得した予測確率分布と実際の観測データ（現時刻ｔの観測データ）とに基づいて、尤度を算出する。そして、算出した尤度に比例する割合で、粒子（パーティクル）を復元抽出する。このように処理することで、ロボットＲｂｔ１の内部状態を推定することができる。
【００７１】
以上のように、移動体制御装置１０００では、ロボットＲｂｔ１の内部状態を示すデータＸｔが設定される。
【００７２】
（１．２．２：環境地図に関するデータ）
移動体制御装置１０００では、環境地図に関するデータとして、時刻ｔにおける環境地図データｍｔが設定される。
【００７３】
環境地図データｍｔは、ランドマークの情報により構成される。ロボットＲｂｔ１によりランドマークが発見された場合、発見されたランドマークの情報は、環境地図データｍｔに追加される。
【００７４】
時刻ｔにおける環境地図データｍｔは、移動体制御装置１０００において、以下のように、設定される。
ｍｔ＝［ＬＭ１ＡＰｔ，ＬＭ１ＢＰｔ，．．．．，ＬＭｋＡＰｔ，ＬＭｋＢＰｔ］
ｋ：自然数
ＬＭｋＡＰｔは、時刻ｔにおけるｋ組目のランドマークセットを構成する一方のランドマークの位置に関する内部状態データであり、ＬＭｋＢＰｔは、時刻ｔにおけるｋ組目のランドマークセットを構成する他方のランドマークの位置に関する内部状態データである。なお、１組のランドマークセットは、２つのランドマークから構成される。
【００７５】
つまり、移動体制御装置１０００では、ｋ組のランドマークセットに含まれる２×ｋ個のランドマークについての内部状態データの集合が、時刻ｔにおける環境地図データｍｔとして、設定される。
【００７６】
図２の場合において、環境地図データｍｔは、１組のランドマークセットを構成する２つのランドマークＬＭ１Ａ、ＬＭ１Ｂの情報により構成される。図２の場合、環境地図データｍｔは、
ｍｔ＝［ＬＭ１ＡＰｔ，ＬＭ１ＢＰｔ］
である。
【００７７】
なお、ランドマークＬＭ１Ａ、ＬＭ１Ｂの情報（位置に関する内部状態データ）ＬＭ１ＡＰｔ、ＬＭ１ＢＰｔは、例えば、以下のように設定される。
ランドマークＬＭ１Ａ：
ＬＭ１ＡＰｔ＝（ｄ１Ａｔ，θ１Ａｔ）
ｄ１Ａｔ：ロボットＲｂｔ１からランドマークＬＭ１Ａまでの距離
θ１Ａｔ：平面視において、ロボットＲｂｔ１からランドマークＬＭ１Ａへ引いた直線とｘ軸とのなす角度
ランドマークＬＭ１Ｂ：
ＬＭ１ＢＰｔ＝（ｄ１Ｂｔ，θ１Ｂｔ）
ｄ１Ｂｔ：ロボットＲｂｔ１からランドマークＬＭ１Ｂまでの距離
θ１Ｂｔ：平面視において、ロボットＲｂｔ１からランドマークＬＭ１Ｂへ引いた直線とｘ軸とのなす角度
なお、以下では、説明便宜のため、図２、図３に示すように、移動体制御装置１０００を搭載したロボットＲｂｔ１が１つのランドマークセットに含まれる２つのランドマークＬＭＡ１、ＬＭＢ１により、ロボットＲｂｔ１の内部状態データ、および、環境地図データが取得される場合について、説明する。
【００７８】
（１．２．３：具体的処理）
次に、移動体制御装置１０００の具体的処理について、説明する。
【００７９】
（時刻ｔの処理）：  
時刻ｔにおいて、ロボットＲｂｔ１、ランドマークＬＭ１Ａ、ＬＭ１Ｂの状態は、図２に示す状態である。なお、移動体制御装置１０００に搭載された距離センサー付き撮像装置では、図２に示す領域ＡＲｃｐの撮像画像（画像データ）と、距離画像（距離データ）とが取得される。
【００８０】
観測取得部１は、移動体制御装置１０００に搭載された距離センサー付き撮像装置からの入力データＤｉｎから観測データＤ１、すなわち、領域ＡＲｃｐの画像（画像データ）Ｄ１＿ｉｍｇと距離画像（距離データ）Ｄ１＿ｄとを取得し、取得した観測データＤ１をランドマーク検出部２と、状態推定部４の状態更新部４１とに出力する。
【００８１】
ランドマーク検出部２では、観測データＤ１（画像データＤ１＿ｉｍｇおよび距離画像Ｄ１＿ｄ）から、ロボットＲｂｔ１からランドマークＬＭ１Ａ、ＬＭ１Ｂまでの距離および方向（角度）に関する情報が取得される。具体的には、以下のデータＬＭ１ＡＰｔ、ＬＭ１ＢＰｔが、ランドマーク検出部２により取得される。
ランドマークＬＭ１Ａ：
ＬＭ１ＡＰｔ＝（ｄ１Ａｔ，θ１Ａｔ）
ｄ１Ａｔ：ロボットＲｂｔ１からランドマークＬＭ１Ａまでの距離
θ１Ａｔ：平面視において、ロボットＲｂｔ１からランドマークＬＭ１Ａへ引いた直線とｘ軸とのなす角度
ランドマークＬＭ１Ｂ：
ＬＭ１ＢＰｔ＝（ｄ１Ｂｔ，θ１Ｂｔ）
ｄ１Ｂｔ：ロボットＲｂｔ１からランドマークＬＭ１Ｂまでの距離
θ１Ｂｔ：平面視において、ロボットＲｂｔ１からランドマークＬＭ１Ｂへ引いた直線とｘ軸とのなす角度
上記により取得されたランドマークＬＭ１Ａ、ＬＭ１ＢについてのデータＬＭ１ＡＰｔ、ＬＭ１ＢＰｔを含めた信号が、ランドマーク検出信号ＬＭｔとして、ランドマーク検出部２から、候補領域取得部３、状態推定部４の状態更新部４１、および、環境地図更新部４２に出力される。
【００８２】
候補領域取得部３では、ランドマーク検出信号ＬＭｔに基づいて、ロボットＲｂｔ１の現時刻ｔの位置の候補位置を推定するための情報が取得される。
【００８３】
具体的には、候補領域取得部３は、図２に示すように、（１）点ＬＭ１Ａを中心とし、半径をロボットＲｂｔ１からランドマークＬＭ１Ａまでの距離ｄ１Ａｔとする円と、（２）点ＬＭ１Ｂを中心とし、半径をロボットＲｂｔ１からランドマークＬＭ１Ｂまでの距離ｄ１Ｂｔとする円との２つの交点Ｃ１ｔ、Ｃ２ｔとを求める。そして、候補領域取得部３は、求めた２つの交点Ｃ１ｔ、Ｃ２ｔに関する情報（交点Ｃ１ｔ、Ｃ２ｔを特定するための情報）を自己位置候補情報Ｃｄｔｔとして、状態推定部４の状態更新部４１に出力する。
【００８４】
なお、候補領域取得部３での処理において、誤差やランドマーク位置の推定精度（ランドマークについての内部状態データの精度）を考慮して、候補領域取得部３は、求めた２つの交点Ｃ１ｔ、Ｃ２ｔを含む所定の面積を有する領域に関する情報を、自己位置候補情報Ｃｄｔｔとして、状態推定部４の状態更新部４１に出力するようにしてもよい。
【００８５】
状態推定部４の状態更新部４１は、観測データＤ１、ランドマーク検出信号ＬＭｔ、自己位置候補情報Ｃｄｔｔ、制御入力データＵｔ、および、ロボットＲｂｔ１の時刻ｔ−１の内部状態データＸｔ−１基づいて、ロボットＲｂｔ１の内部状態を推定し、推定結果をロボットＲｂｔ１の現時刻ｔの内部状態データＸｔとして、取得する（内部状態データを更新する）。
【００８６】
具体的には、自己位置候補情報Ｃｄｔｔから、ロボットＲｂｔ１の自己位置の２つの候補点である点Ｃ１ｔ、Ｃ２ｔのうちのどちらか１つの点を、時刻ｔのロボットＲｂｔ１の位置であると推定する。ここでは、状態更新部４１により、点Ｃ１ｔが、時刻ｔのロボットＲｂｔ１の位置であると推定されたものとして、以下、説明する。
【００８７】
状態更新部４１により取得された時刻ｔのロボットＲｂｔ１の内部状態データＸｔは、状態更新部４１から記憶部５に出力され、記憶部５で記憶される。
【００８８】
環境地図更新部４２は、ランドマーク検出部２から出力されるランドマーク検出信号ＬＭｔから、検出されたランドマークについての情報を取得し、取得したランドマークの情報と、時刻ｔ−１の環境地図データｍｔ−１と、現時刻ｔのロボットＲｂｔ１の内部状態データＸｔとに基づいて、環境地図データを更新し、現時刻ｔの環境地図データｍｔを取得する。
【００８９】
具体的には、時刻ｔにおけるランドマークＬＭ１Ａのｘｙ平面上の位置座標を（ｘ１Ａｔ，ｙ１Ａｔ）とし、時刻ｔにおけるランドマークＬＭ１Ｂのｘｙ平面上の位置座標を（ｘ１Ｂｔ，ｙ１Ｂｔ）とすると、環境地図更新部４２は、
ｘ１Ａｔ＝ｘ０ｔ＋ｄ１Ａｔ×ｃｏｓ（θ１Ａｔ）
ｙ１Ａｔ＝ｙ０ｔ＋ｄ１Ａｔ×ｓｉｎ（θ１Ａｔ）
ｘ１Ｂｔ＝ｘ０ｔ＋ｄ１Ｂｔ×ｃｏｓ（θ１Ｂｔ）
ｙ１Ｂｔ＝ｙ０ｔ＋ｄ１Ｂｔ×ｓｉｎ（θ１Ｂｔ）
により、時刻ｔにおけるランドマークＬＭ１Ａ、ＬＭ１Ｂのｘｙ平面上の位置座標（位置情報）を取得する。
【００９０】
そして、環境地図更新部４２は、上記により取得したデータにより、環境地図データｍｔを更新する。
【００９１】
また、時刻ｔにおいて、ロボットＲｂｔ１には、制御入力Ｕｔが入力される。なお、制御入力Ｕｔは、図３に矢印Ｄｉｒ１で示す方向にロボットＲｂｔ１を移動させるための制御入力データであるとして、以下、説明する。
【００９２】
（時刻ｔ＋１の処理）：
時刻ｔ＋１において、ロボットＲｂｔ１は、時刻ｔの制御入力データＵｔにより、図３に点Ｃ１ｔ＋１で示す位置、あるいは、図３に点Ｃ２＿ＮＧｔ＋１で示す位置に移動していると予測される。
【００９３】
観測取得部１は、移動体制御装置１０００に搭載された距離センサー付き撮像装置からの入力データＤｉｎから観測データＤ１、すなわち、領域ＡＲｃｐの画像（画像データ）Ｄ１＿ｉｍｇと距離画像（距離データ）Ｄ１＿ｄとを取得し、取得した観測データＤ１をランドマーク検出部２と、状態推定部４の状態更新部４１とに出力する。
【００９４】
ランドマーク検出部２では、観測データＤ１（画像データＤ１＿ｉｍｇおよび距離画像Ｄ１＿ｄ）から、ロボットＲｂｔ１からランドマークＬＭ１Ａ、ＬＭ１Ｂまでの距離および方向（角度）に関する情報が取得される。具体的には、以下のデータＬＭ１ＡＰｔ、ＬＭ１ＢＰｔが、ランドマーク検出部２により取得される。
ランドマークＬＭ１Ａ：
ＬＭ１ＡＰｔ＋１＝（ｄ１Ａｔ＋１，θ１Ａｔ＋１）
ｄ１Ａｔ＋１：ロボットＲｂｔ１からランドマークＬＭ１Ａまでの距離
θ１Ａｔ＋１：平面視において、ロボットＲｂｔ１からランドマークＬＭ１Ａへ引いた直線とｘ軸とのなす角度
ランドマークＬＭ１Ｂ：
ＬＭ１ＢＰｔ＋１＝（ｄ１Ｂｔ＋１，θ１Ｂｔ＋１）
ｄ１Ｂｔ＋１：ロボットＲｂｔ１からランドマークＬＭ１Ｂまでの距離
θ１Ｂｔ＋１：平面視において、ロボットＲｂｔ１からランドマークＬＭ１Ｂへ引いた直線とｘ軸とのなす角度
上記により取得されたランドマークＬＭ１Ａ、ＬＭ１ＢについてのデータＬＭ１ＡＰｔ＋１、ＬＭ１ＢＰｔ＋１を含めた信号が、ランドマーク検出信号ＬＭｔ＋１として、ランドマーク検出部２から、候補領域取得部３、状態推定部４の状態更新部４１、および、環境地図更新部４２に出力される。
【００９５】
候補領域取得部３では、ランドマーク検出信号ＬＭｔ＋１に基づいて、ロボットＲｂｔ１の現時刻ｔの位置の候補位置を推定するための情報が取得される。
【００９６】
具体的には、候補領域取得部３は、図３に示すように、（１）点ＬＭ１Ａを中心とし、半径をロボットＲｂｔ１からランドマークＬＭ１Ａまでの距離ｄ１Ａｔ＋１とする円と、（２）点ＬＭ１Ｂを中心とし、半径をロボットＲｂｔ１からランドマークＬＭ１Ｂまでの距離ｄ１Ｂｔ＋１とする円との２つの交点Ｃ１ｔ＋１、Ｃ２ｔ＋１とを求める。そして、候補領域取得部３は、求めた２つの交点Ｃ１ｔ＋１、Ｃ２ｔ＋１に関する情報（交点Ｃ１ｔ＋１、Ｃ２ｔ＋１を特定するための情報）を自己位置候補情報Ｃｄｔｔ＋１として、状態推定部４の状態更新部４１に出力する。
【００９７】
なお、候補領域取得部３での処理において、誤差やランドマーク位置の推定精度（ランドマークについての内部状態データの精度）を考慮して、候補領域取得部３は、求めた２つの交点Ｃ１ｔ＋１、Ｃ２ｔ＋１を含む所定の面積を有する領域に関する情報を、自己位置候補情報Ｃｄｔｔ＋１として、状態推定部４の状態更新部４１に出力するようにしてもよい。
【００９８】
状態推定部４の状態更新部４１は、観測データＤ１、ランドマーク検出信号ＬＭｔ＋１、自己位置候補情報Ｃｄｔｔ＋１、制御入力データＵｔ＋１、および、ロボットＲｂｔ１の時刻ｔの内部状態データＸｔ基づいて、ロボットＲｂｔ１の内部状態を推定し、推定結果をロボットＲｂｔ１の現時刻ｔ＋１の内部状態データＸｔ＋１として、取得する（内部状態データを更新する）。
【００９９】
具体的には、自己位置候補情報Ｃｄｔｔ＋１から、ロボットＲｂｔ１の自己位置の２つの候補点である点Ｃ１ｔ＋１、Ｃ２ｔ＋１のうちのどちらか１つの点を、時刻ｔのロボットＲｂｔ１の位置であると推定する。
【０１００】
移動体制御装置１０００は、（１）時刻ｔのロボットＲｂｔ１の位置の２つの候補点として、点Ｃ１ｔ、点Ｃ２ｔを取得し、（２）時刻ｔ＋１のロボットＲｂｔ１の位置の２つの候補点として、点Ｃ１ｔ＋１、点Ｃ２ｔ＋１を取得している。そして、時刻ｔのロボットＲｂｔ１への制御入力Ｕｔが、ロボットＲｂｔ１を図３の矢印Ｄｉｒ１への方向への移動をするためのデータである。
【０１０１】
状態更新部４１は、時刻ｔのロボットＲｂｔ１への制御入力Ｕｔと、移動体制御装置１０００により推定された時刻ｔのロボットＲｂｔ１の位置の２つの候補点Ｃ１ｔ、Ｃ２ｔとから、時刻ｔにおいて、ロボットＲｂｔ１が、図３の点Ｃ１ｔ＋１、あるいは、点Ｃ２＿ＮＧｔ＋１のいずれかに存在すると予測する（予測処理１）。
【０１０２】
また、状態更新部４１は、時刻ｔ＋１の実際の観測より、時刻ｔ＋１において、ロボットＲｂｔ１とＬＭ１Ａとの距離は、ｄ１Ａｔ＋１であり、ロボットＲｂｔ１とＬＭ２Ａとの距離は、ｄ２Ａｔ＋１であるので、時刻ｔ＋１の実際の観測から求められる時刻ｔ＋１のロボットＲｂｔ１の位置は、候補点Ｃ１ｔ＋１、あるいは、候補点Ｃ２ｔ＋１であると予測する（予測処理２）。
【０１０３】
つまり、状態更新部４１は、上記予測処理１、および、予測処理２の両方において、時刻ｔ＋１のロボットＲｂｔ１の位置の候補点であると予測された点Ｃ１ｔ＋１を、時刻ｔ＋１のロボットＲｂｔ１の位置であると推定する。そして、状態更新部４１は、時刻ｔ＋１のロボットＲｂｔ１の位置についての内部状態データ（座標データ（ｘ０Ａｔ＋１，ｙ０Ａｔ＋１））を、点Ｃ１ｔ＋１の座標情報により更新する。
【０１０４】
以上のようにして、状態更新部４１により取得された時刻ｔ＋１のロボットＲｂｔ１の内部状態データＸｔ＋１は、状態更新部４１から記憶部５に出力され、記憶部５で記憶される。
【０１０５】
環境地図更新部４２は、ランドマーク検出部２から出力されるランドマーク検出信号ＬＭｔから、検出されたランドマークについての情報を取得し、取得したランドマークの情報と、時刻ｔの環境地図データｍｔと、現時刻ｔ＋１のロボットＲｂｔ１の内部状態データＸｔ＋１とに基づいて、環境地図データを更新し、現時刻ｔ＋１の環境地図データｍｔ＋１を取得する。
【０１０６】
具体的には、時刻ｔ＋１におけるランドマークＬＭ１Ａのｘｙ平面上の位置座標を（ｘ１Ａｔ＋１，ｙ１Ａｔ＋１）とし、時刻ｔ＋１におけるランドマークＬＭ１Ｂのｘｙ平面上の位置座標を（ｘ１Ｂｔ＋１，ｙ１Ｂｔ＋１）とすると、環境地図更新部４２は、
ｘ１Ａｔ＋１＝ｘ０ｔ＋１＋ｄ１Ａｔ＋１×ｃｏｓ（θ１Ａｔ＋１）
ｙ１Ａｔ＋１＝ｙ０ｔ＋１＋ｄ１Ａｔ＋１×ｓｉｎ（θ１Ａｔ＋１）
ｘ１Ｂｔ＋１＝ｘ０ｔ＋１＋ｄ１Ｂｔ＋１×ｃｏｓ（θ１Ｂｔ＋１）
ｙ１Ｂｔ＋１＝ｙ０ｔ＋１＋ｄ１Ｂｔ＋１×ｓｉｎ（θ１Ｂｔ＋１）
により、時刻ｔにおけるランドマークＬＭ１Ａ、ＬＭ１Ｂのｘｙ平面上の位置座標（位置情報）を取得する。
【０１０７】
そして、環境地図更新部４２は、上記により取得したデータにより、環境地図データｍｔ＋１を更新する。
【０１０８】
（時刻ｔ＋２以降の処理）：
時刻ｔ＋２以降において、上記と同様の処理が繰り返し実行される。
【０１０９】
このように、移動体制御装置１０００では、２つのランドマークを１組とするランドマークセットを用いて、ロボットＲｂｔ１から２つのランドマークまでの距離を測定することで、ロボットＲｂｔ１の位置を少ない候補点（上記では２点）に絞り、高速かつ高精度に、ロボットＲｂｔ１の位置情報を含む内部状態データを推定することができる。例えば、上記で示した例では、移動体制御装置１０００において、時刻ｔ、時刻ｔ＋１の２回の推定処理を実行することで、ロボットＲｂｔ１の位置を１点に絞り込むことができる。
【０１１０】
以上のように処理することで、移動体制御装置１０００では、ランドマークの情報から、移動体（ロボットＲｂｔ１）の位置を精度良く推定し、高精度な状態推定処理を高速かつ適切に実行することができる。その結果、移動体制御装置１０００では、移動体（ロボットＲｂｔ１）の制御を適切に行うことができる。
【０１１１】
さらに、移動体制御装置１０００では、上記の通り、１つのランドマークセットに含まれる２つのランドマークと、ロボットＲｂｔ１との距離を取得するだけで、ロボットＲｂｔ１の位置を高速かつ高精度に推定することができる。このため、移動体制御装置１０００では、多くの内部状態変数を用いた複雑な処理をすることなく、少ない演算量で推定処理を実現することができる。
【０１１２】
［第２実施形態］
次に、第２実施形態について、説明する。
【０１１３】
第２実施形態において、第１実施形態と同様の部分については、同一符号を付し、詳細な説明を省略する。
【０１１４】
図４は、第２実施形態に係る移動体制御装置２０００の概略構成図である。
【０１１５】
第２実施形態の移動体制御装置２０００では、図４に示すように、第１実施形態の移動体制御装置１０００において、ランドマーク検出部２をランドマーク検出部２Ａに置換し、候補領域取得部３を候補領域取得部３Ａに置換した構成を有している。
【０１１６】
ランドマーク検出部２Ａは、ランドマーク検出部２の機能に加えて、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）からランドマークを含む画像領域を抽出し、抽出した画像を画像Ｄ１＿ｉｍｇとして、候補領域取得部３に出力する。また、ランドマーク検出部２Ａは、ランドマーク検出信号ＬＭｔを、候補領域取得部３Ａ、状態更新部４１、および、環境地図更新部４２に出力する。
【０１１７】
候補領域取得部３Ａは、図４に示すように、ランドマーク検出部２Ａから出力されるランドマーク検出信号ＬＭｔおよび画像Ｄ１＿ｉｍｇと、ランドマークパターン情報ＬＭ＿ｐｔｎとを入力する。ランドマークパターン情報ＬＭ＿ｐｔｎは、予め設定されている情報であり、ロボットＲｂｔ１が移動する環境に設置されているランドマークの表面に付されているパターン（色、模様等）についての情報である。候補領域取得部３は、ランドマークパターン情報ＬＭ＿ｐｔｎと、画像Ｄ１＿ｉｍｇとに基づいて、ロボットＲｂｔ１の現時刻の位置の候補位置を推定するための情報である自己位置候補情報Ｃｄｔｔを取得する。そして、候補領域取得部３Ａは、取得した自己位置候補情報Ｃｄｔｔを状態推定部４の状態更新部４１に出力する。
【０１１８】
本実施形態において使用されるランドマーク（一例）は、細長い円筒形状であり、ランドマークの表面が、（１）第１パターンを付された第１パターン領域と、（２）第２パターンを付された第２パターン領域と、の２つの領域からなる。そして、ランドマークを設置した状態の平面視において、一方の半円に相当する円周上の表面が第１パターン領域であり、他方の半円に相当する円周上の表面が第２パターン領域であるものとする。
【０１１９】
なお、ランドマークの形状および表面のパターンは、上記に限定されることはない。移動体制御装置２０００によりランドマークを発見したときに、当該ランドマークの形状、および／または、パターンを認識（特定）することで、移動体（ロボットＲｂｔ１）と、ランドマークとの位置関係（ランドマークがどの方向から観測されたかを知る手掛かりになる情報）を取得できるものであれば、ランドマークの形状および／またはパターンは、他のものであってもよい。
【０１２０】
≪図５の場合≫
図５は、時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ１と、本実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ１とを模式的に示した図である。なお、画像Ｄ１＿ｉｍｇ１は、ランドマーク検出部２Ａから候補領域取得部３Ａに出力される画像（画像データ）である。
【０１２１】
候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎに基づいて、図５に示す画像Ｄ１＿ｉｍｇ１を解析する。具体的には、候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎから、ランドマークＬＭ２の平面視において一方の半円に相当する円周上の表面が第１パターン領域（黒色一色のパターン）（図５において、ｐｔｎ１（ＬＭ２）と表記している領域）であり、ランドマークＬＭ２の平面視において他方の半円に相当する円周上の表面が第２パターン領域（白色一色のパターン）（図５において、ｐｔｎ２（ＬＭ２）と表記している領域）であることを認識する。
【０１２２】
そして、候補領域取得部３Ａは、画像Ｄ１＿ｉｍｇ１上において、ランドマークの中央付近に、第１パターン領域（黒色一色のパターン）と第２パターン領域（白色一色のパターン）が存在し、かつ、左側に第１パターン領域（黒色一色のパターン）が存在し、右側に第２パターン領域（白色一色のパターン）が存在することを認識する。そして、候補領域取得部３Ａは、上記画像解析結果に基づいて、画像Ｄ１＿ｉｍｇ１が撮像された位置の候補領域ＡＲ＿Ｃ１を求める。つまり、図５に示すように、平面視において、ランドマークＬＭ２の位置（中心点）から、ランドマークＬＭ２の第１パターン領域ｐｔｎ１（ＬＭ２）と第２パターン領域ｐｔｎ２（ＬＭ２）との境界点を通る直線のｙ軸負方向に、距離ｄ２だけ離れた領域が、時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ１であると判定する。なお、候補領域取得部３Ａは、ランドマーク検出信号ＬＭｔから、ランドマークＬＭ２とロボットＲｂｔ１との距離ｄ２を取得する。
【０１２３】
候補領域取得部３Ａは、上記のようにして取得した時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ１についての情報を、自己位置候補情報Ｃｄｔｔに含めて、状態更新部４１に出力する。
【０１２４】
状態更新部４１は、自己位置候補情報Ｃｄｔｔから、時刻ｔのロボットＲｂｔ１の位置が、候補領域ＡＲ＿Ｃ１である可能性が高いと判定し、ロボットＲｂｔ１の位置に関する内部状態データを更新する。
【０１２５】
≪図６の場合≫
図６は、時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ２と、本実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ２とを模式的に示した図である。なお、画像Ｄ１＿ｉｍｇ２は、ランドマーク検出部２Ａから候補領域取得部３Ａに出力される画像（画像データ）である。
【０１２６】
候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎに基づいて、図６に示す画像Ｄ１＿ｉｍｇ１を解析する。具体的には、候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎから、ランドマークＬＭ２の平面視において一方の半円に相当する円周上の表面が第１パターン領域（黒色一色のパターン）（図６において、ｐｔｎ１（ＬＭ２）と表記している領域）であり、ランドマークＬＭ２の平面視において他方の半円に相当する円周上の表面が第２パターン領域（白色一色のパターン）（図６において、ｐｔｎ２（ＬＭ２）と表記している領域）であることを認識する。
【０１２７】
そして、候補領域取得部３Ａは、画像Ｄ１＿ｉｍｇ２上において、ランドマークの中央付近に、第１パターン領域（黒色一色のパターン）と第２パターン領域（白色一色のパターン）が存在し、かつ、右側に第１パターン領域（黒色一色のパターン）が存在し、左側に第２パターン領域（白色一色のパターン）が存在することを認識する。そして、候補領域取得部３Ａは、上記画像解析結果に基づいて、画像Ｄ１＿ｉｍｇ２が撮像された位置の候補領域ＡＲ＿Ｃ２を求める。つまり、図６に示すように、平面視において、ランドマークＬＭ２の位置（中心点）から、ランドマークＬＭ２の第１パターン領域ｐｔｎ１（ＬＭ２）と第２パターン領域ｐｔｎ２（ＬＭ２）との境界点を通る直線のｙ軸正方向に、距離ｄ２だけ離れた領域が、時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ２であると判定する。なお、候補領域取得部３Ａは、ランドマーク検出信号ＬＭｔから、ランドマークＬＭ２とロボットＲｂｔ１との距離ｄ２を取得する。
【０１２８】
候補領域取得部３Ａは、上記のようにして取得した時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ２についての情報を、自己位置候補情報Ｃｄｔｔに含めて、状態更新部４１に出力する。
【０１２９】
状態更新部４１は、自己位置候補情報Ｃｄｔｔから、時刻ｔのロボットＲｂｔ１の位置が、候補領域ＡＲ＿Ｃ２である可能性が高いと判定し、ロボットＲｂｔ１の位置に関する内部状態データを更新する。
【０１３０】
≪図７の場合≫
図７は、時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ３と、本実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ３とを模式的に示した図である。なお、画像Ｄ１＿ｉｍｇ３は、ランドマーク検出部２Ａから候補領域取得部３Ａに出力される画像（画像データ）である。
【０１３１】
候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎに基づいて、図７に示す画像Ｄ１＿ｉｍｇ３を解析する。具体的には、候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎから、ランドマークＬＭ２の平面視において一方の半円に相当する円周上の表面が第１パターン領域（黒色一色のパターン）（図７において、ｐｔｎ１（ＬＭ２）と表記している領域）であり、ランドマークＬＭ２の平面視において他方の半円に相当する円周上の表面が第２パターン領域（白色一色のパターン）（図７において、ｐｔｎ２（ＬＭ２）と表記している領域）であることを認識する。
【０１３２】
そして、候補領域取得部３Ａは、画像Ｄ１＿ｉｍｇ３上において、ランドマークＬＭ２の第２パターン領域（白色一色のパターン）のみが存在することを認識する。そして、候補領域取得部３Ａは、上記画像解析結果に基づいて、画像Ｄ１＿ｉｍｇ３が撮像された位置の候補領域ＡＲ＿Ｃ３を求める。つまり、図７に示すように、平面視において、ランドマークＬＭ２の位置（中心点）から、ランドマークＬＭ２の第２パターン領域ｐｔｎ２（ＬＭ２）の中央付近の点を通る直線のｘ軸正方向に、距離ｄ２だけ離れた領域が、時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ３であると判定する。なお、候補領域取得部３Ａは、ランドマーク検出信号ＬＭｔから、ランドマークＬＭ２とロボットＲｂｔ１との距離ｄ２を取得する。
【０１３３】
候補領域取得部３Ａは、上記のようにして取得した時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ３についての情報を、自己位置候補情報Ｃｄｔｔに含めて、状態更新部４１に出力する。
【０１３４】
状態更新部４１は、自己位置候補情報Ｃｄｔｔから、時刻ｔのロボットＲｂｔ１の位置が、候補領域ＡＲ＿Ｃ３である可能性が高いと判定し、ロボットＲｂｔ１の位置に関する内部状態データを更新する。
【０１３５】
≪図８の場合≫
図８は、時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ４と、本実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ４とを模式的に示した図である。なお、画像Ｄ１＿ｉｍｇ４は、ランドマーク検出部２Ａから候補領域取得部３Ａに出力される画像（画像データ）である。
【０１３６】
候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎに基づいて、図８に示す画像Ｄ１＿ｉｍｇ４を解析する。具体的には、候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎから、ランドマークＬＭ２の平面視において一方の半円に相当する円周上の表面が第１パターン領域（黒色一色のパターン）（図８において、ｐｔｎ１（ＬＭ２）と表記している領域）であり、ランドマークＬＭ２の平面視において他方の半円に相当する円周上の表面が第２パターン領域（白色一色のパターン）（図８において、ｐｔｎ２（ＬＭ２）と表記している領域）であることを認識する。
【０１３７】
そして、候補領域取得部３Ａは、画像Ｄ１＿ｉｍｇ４上において、ランドマークの中央付近に、第１パターン領域（黒色一色のパターン）のみが存在することを認識する。そして、候補領域取得部３Ａは、上記画像解析結果に基づいて、画像Ｄ１＿ｉｍｇ４が撮像された位置の候補領域ＡＲ＿Ｃ４を求める。つまり、図８に示すように、平面視において、ランドマークＬＭ２の位置（中心点）から、ランドマークＬＭ２の第２パターン領域ｐｔｎ２（ＬＭ２）の中央付近の点を通る直線のｘ軸負方向に、距離ｄ２だけ離れた領域が、時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ４であると判定する。なお、候補領域取得部３Ａは、ランドマーク検出信号ＬＭｔから、ランドマークＬＭ２とロボットＲｂｔ１との距離ｄ２を取得する。
【０１３８】
候補領域取得部３Ａは、上記のようにして取得した時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ４についての情報を、自己位置候補情報Ｃｄｔｔに含めて、状態更新部４１に出力する。
【０１３９】
状態更新部４１は、自己位置候補情報Ｃｄｔｔから、時刻ｔのロボットＲｂｔ１の位置が、候補領域ＡＲ＿Ｃ４である可能性が高いと判定し、ロボットＲｂｔ１の位置に関する内部状態データを更新する。
【０１４０】
≪図９の場合≫
図９は、は、時刻ｔにおけるロボットＲｂｔ１の位置の候補となる領域ＡＲ＿Ｃ５と、本実施形態のランドマークＬＭ２との位置関係を示す図（上方から見た図）と、時刻ｔにおいて、ロボットＲｂｔ１に搭載された距離センサー付き撮像装置（不図示）により撮像された画像（画像データ）Ｄ１＿ｉｍｇ５とを模式的に示した図である。なお、画像Ｄ１＿ｉｍｇ５は、ランドマーク検出部２Ａから候補領域取得部３Ａに出力される画像（画像データ）である。
【０１４１】
候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎに基づいて、図９に示す画像Ｄ１＿ｉｍｇ５を解析する。具体的には、候補領域取得部３Ａは、ランドマークＬＭ２のランドマークパターン情報ＬＭ＿ｐｔｎから、ランドマークＬＭ２の平面視において一方の半円に相当する円周上の表面が第１パターン領域（黒色一色のパターン）（図９において、ｐｔｎ１（ＬＭ２）と表記している領域）であり、ランドマークＬＭ２の平面視において他方の半円に相当する円周上の表面が第２パターン領域（白色一色のパターン）（図９において、ｐｔｎ２（ＬＭ２）と表記している領域）であることを認識する。
【０１４２】
そして、候補領域取得部３Ａは、画像Ｄ１＿ｉｍｇ５上において、ランドマークの左寄りの領域に、第１パターン領域（黒色一色のパターン）と第２パターン領域（白色一色のパターン）が存在し、かつ、左側に第１パターン領域（黒色一色のパターン）が存在し、右側に第２パターン領域（白色一色のパターン）が存在することを認識する。また、候補領域取得部３Ａは、画像上の水平方向において、第１パターン領域が占める割合と、第２パターン領域が占める割合とを求める。そして、候補領域取得部３Ａは、上記画像解析結果に基づいて、画像Ｄ１＿ｉｍｇ５が撮像された位置の候補領域ＡＲ＿Ｃ５を求める。つまり、図９に示すように、平面視において、ランドマークＬＭ２の位置（中心点）から、距離ｄ２だけ離れた点からランドマークを見たときに、画像Ｄ１＿ｉｍｇ５のランドマークの見え方と一致する点を求める。図９では、平面視において、ランドマークＬＭ２の中心点と、点Ａ１とを通る直線の図９の斜め下方向に距離ｄ２だけ離れた領域が、時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ５であると判定する。なお、候補領域取得部３Ａは、ランドマーク検出信号ＬＭｔから、ランドマークＬＭ２とロボットＲｂｔ１との距離ｄ２を取得する。
【０１４３】
候補領域取得部３Ａは、上記のようにして取得した時刻ｔのロボットＲｂｔ１の位置の候補領域ＡＲ＿Ｃ５についての情報を、自己位置候補情報Ｃｄｔｔに含めて、状態更新部４１に出力する。
【０１４４】
状態更新部４１は、自己位置候補情報Ｃｄｔｔから、時刻ｔのロボットＲｂｔ１の位置が、候補領域ＡＲ＿Ｃ５である可能性が高いと判定し、ロボットＲｂｔ１の位置に関する内部状態データを更新する。
【０１４５】
以上のように、移動体制御装置２０００では、所定のパターンを有するランドマークを撮像した画像を実際の観測データとして取得し、撮像画像上のランドマークの見え方を解析することで、移動体（ロボットＲｂｔ１）の位置を精度良く推定し、高精度な状態推定処理を高速かつ適切に実行することができる。その結果、移動体制御装置２０００では、移動体（ロボットＲｂｔ１）の制御を適切に行うことができる。
【０１４６】
さらに、移動体制御装置２０００では、上記の通り、ランドマークと、ロボットＲｂｔ１との距離を取得するだけで、ロボットＲｂｔ１の位置を高速かつ高精度に推定することができる。このため、移動体制御装置２０００では、多くの内部状態変数を用いた複雑な処理をすることなく、少ない演算量で推定処理を実現することができる。
【０１４７】
なお、本実施形態において、ランドマークは、第１パターン領域（黒色一色のパターン）と第２パターン領域（白色一色のパターン）とを有するものとして、説明したが、これに限定されることはなく、例えば、ランドマークは、図１０に示すランドマークＬＭ３のように、平面視において、第１パターン領域（図１０のパターンｐｔｎ１（ＬＭ３））、第２パターン領域（図１０のパターンｐｔｎ２（ＬＭ３））、および、（図１０のパターンｐｔｎ３（ＬＭ３））の３つのパターンを有するものであってもよい。さらに、ランドマークは、４パターン以上のパターンを有するものであってもよい。
【０１４８】
［他の実施形態］
上記実施形態では、距離センサー付き撮像装置をロボットＲｂｔ１に搭載し、カラーの撮像画像と、距離画像を取得する場合について、説明したが、これに限定されることはない。例えば、３次元計測用のカメラをロボットＲｂｔ１に搭載し、３次元計測用のカメラから取得した２つの画像から、距離画像を取得するようにしてもよい。また、レンジファインダー等の距離センサーをロボットＲｂｔ１に搭載し、ランドマークまでの距離を測定するようにしてもよい。
【０１４９】
さらに、複数のセンサーをロボットＲｂｔ１に搭載し、複数のセンサーから取得される信号から、実際の観測データを取得するようにしてもよい。
【０１５０】
上記実施形態において、環境地図データを作成するために設定した座標は、図２、図３、および、図５〜図９に示すようにｘｙ座標であったが、これに限定されることはなく、例えば、極座標等を用いてもよい。また、上記実施形態では、絶対座標を前提として説明したが、これに限定されることはなく、上記実施形態の処理を実行するために、例えば、ロボットＲｂｔ１の位置を原点とする相対座標を用いてもよい。
【０１５１】
また、上記実施形態では、移動体（ロボットＲｂｔ１）の位置、ランドマークの位置が、２次元データ（ｘ座標値、ｙ座標値）により特定される場合について説明したが、これに限定あれることはなく、移動体（ロボットＲｂｔ１）の位置、ランドマークの位置を、３次元データ（例えば、ｘ座標値、ｙ座標値、ｚ座標値）により特定するようにしてもよい。移動体（ロボットＲｂｔ１）の位置、ランドマークの位置を、３次元データにより特定する場合であって、例えば、ランドマークＬＭ１Ａが３次元空間内の１点に配置され、ランドマークＬＭ１Ｂが３次元空間内の他の１点に配置されている場合、ランドマークＬＭ１Ａを中心とする半径ｄ１Ａｔの球と、ランドマークＬＭ１Ｂを中心とする半径ｄ１Ｂｔの球とが交差してできる円上が、時刻ｔにおけるロボットＲｂｔ１の位置の候補となる。
【０１５２】
上記実施形態の一部または全部を組み合わせて移動体制御装置を構成するようにしてもよい。
【０１５３】
また、上記実施形態で説明した移動体制御装置において、各ブロックは、ＬＳＩなどの半導体装置により個別に１チップ化されても良いし、一部又は全部を含むように１チップ化されても良い。
【０１５４】
なお、ここでは、ＬＳＩとしたが、集積度の違いにより、ＩＣ、システムＬＳＩ、スーパーＬＳＩ、ウルトラＬＳＩと呼称されることもある。
【０１５５】
また、集積回路化の手法はＬＳＩに限るものではなく、専用回路又は汎用プロセサで実現してもよい。ＬＳＩ製造後に、プログラムすることが可能なＦＰＧＡ（Ｆｉｅｌｄ  Ｐｒｏｇｒａｍｍａｂｌｅ  Ｇａｔｅ  Ａｒｒａｙ）や、ＬＳＩ内部の回路セルの接続や設定を再構成可能なリコンフィギュラブル・プロセッサーを利用しても良い。
【０１５６】
また、上記各実施形態の各機能ブロックの処理の一部または全部は、プログラムにより実現されるものであってもよい。そして、上記各実施形態の各機能ブロックの処理の一部または全部は、コンピュータにおいて、中央演算装置（ＣＰＵ）、マイクロプロセッサ、プロセッサ等により行われる。また、それぞれの処理を行うためのプログラムは、ハードディスク、ＲＯＭなどの記憶装置に格納されており、ＲＯＭにおいて、あるいはＲＡＭに読み出されて実行される。
【０１５７】
また、上記実施形態の各処理をハードウェアにより実現してもよいし、ソフトウェア（ＯＳ（オペレーティングシステム）、ミドルウェア、あるいは、所定のライブラリとともに実現される場合を含む。）により実現してもよい。さらに、ソフトウェアおよびハードウェアの混在処理により実現しても良い。
【０１５８】
また、上記実施形態における処理方法の実行順序は、必ずしも、上記実施形態の記載に制限されるものではなく、発明の要旨を逸脱しない範囲で、実行順序を入れ替えることができるものである。
【０１５９】
前述した方法をコンピュータに実行させるコンピュータプログラム及びそのプログラムを記録したコンピュータ読み取り可能な記録媒体は、本発明の範囲に含まれる。ここで、コンピュータ読み取り可能な記録媒体としては、例えば、フレキシブルディスク、ハードディスク、ＣＤ−ＲＯＭ、ＭＯ、ＤＶＤ、ＤＶＤ−ＲＯＭ、ＤＶＤ−ＲＡＭ、大容量ＤＶＤ、次世代ＤＶＤ、半導体メモリを挙げることができる。
【０１６０】
上記コンピュータプログラムは、上記記録媒体に記録されたものに限られず、電気通信回線、無線又は有線通信回線、インターネットを代表とするネットワーク等を経由して伝送されるものであってもよい。
【０１６１】
なお、本発明の具体的な構成は、前述の実施形態に限られるものではなく、発明の要旨を逸脱しない範囲で種々の変更および修正が可能である。
【符号の説明】
【０１６２】
１０００、２０００  移動体制御装置
１  観測取得部
２、２Ａ  ランドマーク検出部
３、３Ａ  候補領域取得部
４  状態推定部
５  記憶部
【図１】
0006594008.tif 000002
【図２】
0006594008.tif 000003
【図３】
0006594008.tif 000004
【図４】
0006594008.tif 000005
【図５】
0006594008.tif 000006
【図６】
0006594008.tif 000007
【図７】
0006594008.tif 000008
【図８】
0006594008.tif 000009
【図９】
0006594008.tif 000010
【図１０】
0006594008.tif 000011