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