JP2016177749A - 移動体制御装置、プログラムおよび集積回路 - Google Patents

Abstract

【課題】効率良く環境地図を作成することで、高精度な状態推定処理を短い時間で実行し、移動体の制御を適切に行う移動体制御装置を実現する。
【解決手段】観測取得部１は、観測可能な事象から得られる観測データを取得する。ランドマーク予測部５は、現時刻よりも前の時刻の内部状態データおよび環境地図データに基づいて、現時刻におけるランドマークの情報を予測した情報を含むランドマーク予測信号を生成する。ランドマーク検出部２は、ランドマーク予測信号に基づいて、現時刻におけるランドマークの情報を検出し、検出結果を示すランドマーク検出信号を取得する。状態推定部３は、観測データと、ランドマーク検出信号とに基づいて、移動体の内部状態を推定することで、現時刻の自己内部状態推定データを取得するとともに、ランドマーク検出信号に基づいて、環境地図を推定することで、現時刻の推定環境地図データを取得する。
【選択図】図１

Description

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

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

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

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

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

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

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

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

しかしながら、上記技術により、ＲＧＢカメラで撮像した画像からランドマークの位置を取得するためには、画像全体を探索して、ランドマークを発見する探索処理を実行する必要がある。このため、上記技術では、ランドマークの位置を特定するまでの処理量が多くなり、処理時間が長くなる。このようにランドマークの位置を特定するまでの時間が長くなると、自律走行ロボットにおいて、推定結果の更新を行う時間間隔も長くなる。その結果、実際のロボットの姿勢と推定結果との間の誤差が大きくなり、ロボット（移動体）の安定した自律走行が困難になる。

そこで、本発明は、上記問題点に鑑み、効率良く環境地図を作成することで、高精度な状態推定処理を短い時間で実行し、移動体の制御を適切に行う移動体制御装置、プログラム、および、集積回路を実現することを目的とする。

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

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

ランドマーク予測部は、現時刻よりも前の時刻における移動体の内部状態を示す内部状態データおよび環境地図データに基づいて、現時刻におけるランドマークの情報を予測した情報を含むランドマーク予測信号を生成する。

ランドマーク検出部は、ランドマーク予測部により生成されたランドマーク予測信号に基づいて、現時刻におけるランドマークの情報を検出し、検出結果を示すランドマーク検出信号を取得する。

状態推定部は、観測取得部により取得された観測データと、ランドマーク検出信号とに基づいて、移動体の内部状態を推定することで、現時刻の自己内部状態推定データを取得するとともに、ランドマーク検出信号に基づいて、環境地図を推定することで、現時刻の推定環境地図データを取得する。

この移動体制御装置では、ランドマーク予測部が、（１）現時刻よりも前の時刻における移動体の内部状態データ、（２）現時刻よりも前の時刻における環境地図データに基づいて、現時刻の観測データに対して、ランドマークの情報を予測し、予測した情報を含むランドマーク予測信号を生成する。そして、ランドマーク検出部が、ランドマーク予測部により生成されたランドマーク予測信号に基づいて、現時刻におけるランドマークの情報を検出する。このため、この移動体制御装置では、ランドマークを探索する処理を効率良く行うことができる。これにより、この移動体制御装置では、環境地図データを作成（更新）するための処理時間を短くすることができる。つまり、この移動体制御装置では、上記処理を行うことにより、効率良く環境地図を作成することができ、高精度な状態推定処理を短い時間で実行することができる。その結果、この移動体制御装置により、移動体の制御を適切に行うことができる。

第２の発明は、第１の発明であって、観測データは、移動体から、移動体の周りの環境を撮像した画像データである。

ランドマーク予測部は、現時刻よりも前の時刻における移動体の内部状態を示す内部状態データおよび環境地図データに基づいて、現時刻において取得された画像データにより形成される画像上において、ランドマークが存在すると予測される画像領域である予測画像領域を算出し、算出した予測画像領域についての情報を含むランドマーク予測信号を生成する。

ランドマーク検出部は、ランドマーク予測部により算出された予測画像領域の中心領域に含まれる画像上の画素位置を探索の初期位置として、現時刻の画像データにより形成される画像の予測画像領域内を、初期位置からの画像上の距離が大きくなるように、探索することで、ランドマークに相当する画像領域を検出する処理であるローカル探索処理を実行する。

この移動体制御装置では、ランドマーク予測部が、（１）現時刻よりも前の時刻における移動体の内部状態データ、（２）現時刻よりも前の時刻における環境地図データに基づいて、現時刻の観測データである画像（画像データにより形成される画像）上において、ランドマークが存在する確率が高い画像領域を予測し、当該ランドマークの探索処理を行う。このため、この移動体制御装置では、ランドマークを探索するために、従来技術のように、画像上の全ての領域を探索する必要がない。その結果、この移動体制御装置１０００では、ランドマークを探索するための処理時間を著しく短縮することができる。これにより、この移動体制御装置における環境地図データを作成（更新）するための処理時間を短くすることができる。つまり、この移動体制御装置では、上記処理を行うことにより、効率良く環境地図を作成することができ、高精度な状態推定処理を短い時間で実行することができる。その結果、この移動体制御装置により、移動体の制御を適切に行うことができる。

第３の発明は、第２の発明であって、ランドマーク予測部は、現時刻よりも前の時刻において取得したランドマークの位置情報の推定精度の情報を、ランドマーク予測信号に含める。

ランドマーク検出部は、ランドマーク予測信号に基づいて、現時刻よりも前の時刻において取得したランドマークの位置情報の推定精度が所定の値よりも低いと判定した場合、現時刻の画像データにより形成される画像内を探索することで、ランドマークに相当する画像領域を検出する処理であるグローバル探索処理を実行する。

これにより、この移動体制御装置では、ランドマークの位置情報の推定精度が低い場合に、グローバル探索処理により、ランドマークを発見する確率を高めることができる。つまり、この移動体制御装置では、ランドマークの位置情報の推定精度が低い場合に、ローカル探索処理により、ランドマークが発見できず、かえって処理時間が長くなることを適切に防止することができる。

第４の発明は、第２または第３の発明であって、ランドマーク予測部は、現時刻よりも前の時刻において取得した移動体の内部状態データの推定精度の情報を、ランドマーク予測信号に含める。

ランドマーク検出部は、ランドマーク予測信号に基づいて、現時刻よりも前の時刻において取得した移動体の内部状態データの推定精度が所定の値よりも低いと判定した場合、現時刻の画像データにより形成される画像内を探索することで、ランドマークに相当する画像領域を検出する処理であるグローバル探索処理を実行する。

これにより、この移動体制御装置では、移動体の内部状態データの推定精度が低い場合に、グローバル探索処理により、ランドマークを発見する確率を高めることができる。つまり、この移動体制御装置では、移動体の内部状態データの推定精度が低い場合（例えば、移動体の位置を適切に推定できていない場合）に、ローカル探索処理により、ランドマークが発見できず、かえって処理時間が長くなることを適切に防止することができる。

第５の発明は、第２から第４のいずれかの発明であって、ランドマーク検出部は、所定の期間において、ランドマークの探索処理において、ランドマークを発見した回数であるランドマーク発見回数値を保持する検出状況取得部を備える。

ランドマーク検出部は、検出状況取得部に保持されているランドマーク発見回数値が所定の値よりも大きい場合、ローカル探索処理を実行する。

これにより、この移動体制御装置では、ランドマーク発見回数値が多い場合、すなわち、ランドマークの位置情報の推定結果の精度が高いと判断できる場合に、ローカル探索処理を実行し、処理時間を短縮することができる。

第６の発明は、第２から第４のいずれかの発明であって、ランドマーク検出部は、所定の期間において、ランドマークの探索処理において、ランドマークを発見されたか否かを示す情報であるランドマーク発見情報を保持する検出状況取得部を備える。

ランドマーク検出部は、検出状況取得部に保持されているランドマーク発見情報に基づいて、ランドマークが所定の期間発見されなかった場合、現時刻の画像データにより形成される画像内を探索することで、ランドマークに相当する画像領域を検出する処理であるグローバル探索処理を実行する。

これにより、この移動体制御装置では、一定時間、ランドマークが発見されない場合、グローバル探索処理を実行して、当該ランドマークを発見する可能性を高めることができる。

第７の発明は、ランドマークの情報を用いて表現される環境地図の作成処理と、自己の内部状態の推定処理とを実行しながら、移動する移動体を制御するための移動体制御方法をコンピュータに実行させるためのプログラムである。

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

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

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

ランドマーク予測ステップは、現時刻よりも前の時刻における移動体の内部状態を示す内部状態データおよび環境地図データに基づいて、現時刻におけるランドマークの情報を予測した情報を含むランドマーク予測信号を生成する。

ランドマーク検出ステップは、ランドマーク予測ステップにより生成されたランドマーク予測信号に基づいて、現時刻におけるランドマークの情報を検出し、検出結果を示すランドマーク検出信号を取得する。

状態推定ステップは、観測取得ステップにより取得された観測データと、ランドマーク検出信号とに基づいて、移動体の内部状態を推定することで、現時刻の自己内部状態推定データを取得するとともに、ランドマーク検出信号に基づいて、環境地図を推定することで、現時刻の推定環境地図データを取得する。

第８の発明は、ランドマークの情報を用いて表現される環境地図の作成処理と、自己の内部状態の推定処理とを実行しながら、移動する移動体を制御するための移動体制御装置に用いられる集積回路であって、観測取得部と、ランドマーク予測部と、ランドマーク検出部と、状態推定部と、を備える。

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

ランドマーク予測部は、現時刻よりも前の時刻における移動体の内部状態を示す内部状態データおよび環境地図データに基づいて、現時刻におけるランドマークの情報を予測した情報を含むランドマーク予測信号を生成する。

ランドマーク検出部は、ランドマーク予測部により生成されたランドマーク予測信号に基づいて、現時刻におけるランドマークの情報を検出し、検出結果を示すランドマーク検出信号を取得する。

状態推定部は、観測取得部により取得された観測データと、ランドマーク検出信号とに基づいて、移動体の内部状態を推定することで、現時刻の自己内部状態推定データを取得するとともに、ランドマーク検出信号に基づいて、環境地図を推定することで、現時刻の推定環境地図データを取得する。

これにより、第１の発明と同様の効果を奏する集積回路を実現することができる。

本発明によれば、効率良く環境地図を作成することで、高精度な状態推定処理を短い時間で実行し、移動体の制御を適切に行う移動体制御装置、プログラム、および、集積回路を実現することができる。

第１実施形態に係る移動体制御装置１０００の概略構成図。 時刻ｔ−１におけるロボットＲｂｔ１の推定位置と、ランドマークＬＭ１の推定位置と、ランドマークＬＭ２の推定位置との位置関係を示す図。 時刻ｔにおけるロボットＲｂｔ１の推定位置と、ランドマークＬＭ１の推定位置と、ランドマークＬＭ２の推定位置との位置関係を示す図。 ロボットＲｂｔ１の内部状態のうち、ロボットＲｂｔ１のｘｙ平面の位置を推定するためのパーティクルの分布（一例）、および、ランドマークＬＭ１、ＬＭ２のｘｙ平面の位置を推定するためのパーティクルの分布（一例）を模式的に示した図。 時刻ｔ−１の画像Ｄ１（画像Ｉｍｇ_ｔ−１）、および、時刻ｔの画像Ｄ１（画像Ｉｍｇ_ｔ）を模式的に示した図。 第２実施形態に係る移動体制御装置２０００の概略構成図。 第２実施形態のランドマーク予測部５Ａの概略構成図。 第２実施形態のランドマーク検出部２Ａの概略構成図。 第２実施形態の第１変形例に係る移動体制御装置２０００Ａの概略構成図。 第２実施形態の第１変形例のランドマーク予測部５Ｂの概略構成図。 第２実施形態の第１変形例のランドマーク検出部２Ｂの概略構成図。

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

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

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

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

なお、説明便宜のため、観測取得部１により取得される観測データＤ１は、移動体に搭載された撮像装置（不図示）により撮像された画像（画像データ）であるものとして、以下、説明する。

観測取得部１は、入力データＤｉｎを入力とする。観測取得部１は、入力データＤｉｎから観測データＤ１を取得し、取得した観測データＤ１をランドマーク検出部２と、状態推定部３の状態更新部３１とに出力する。

ランドマーク検出部２は、観測取得部１から出力される観測データＤ１と、ランドマーク予測部５から出力されるランドマーク予測信号Ｓｉｇ＿ｐｒｅとを入力する。ランドマーク検出部２は、ランドマーク予測信号Ｓｉｇ＿ｐｒｅに基づいて、観測データＤ１（画像データＤ１）により形成される画像上において、ランドマークに相当する画像領域を検出する処理を実行し、その検出結果を示す情報を含む信号を、ランドマーク検出信号ＬＭ_ｔとして、状態推定部３の状態更新部３１および環境地図更新部３２に出力する。

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

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

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

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

ランドマーク予測部５は、記憶部４に記憶されている、現時刻よりも前の時刻の内部状態データと、環境地図データとを読み出す。

なお、説明便宜のため、ランドマーク予測部５は、現時刻ｔの１つ前の時刻ｔ−１のデータを読み出すものとして、以下、説明する。

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

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

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

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

図２は、時刻ｔ−１におけるロボットＲｂｔ１の推定位置と、ランドマークＬＭ１の推定位置と、ランドマークＬＭ２の推定装置との位置関係を示す図（上方から見た図）である。なお、図２において、図２に示すようにｘ軸、ｙ軸をとるものとし、図２に「０」で示した位置を原点とする。

図３は、時刻ｔにおけるロボットＲｂｔ１の推定位置と、ランドマークＬＭ１の推定位置と、ランドマークＬＭ２の推定位置との位置関係を示す図（上方から見た図）である。なお、図３においても、図２と同様に、ｘ軸、ｙ軸、原点が設定されている。

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

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

（１．２．１：ロボットＲｂｔ１の内部状態データ）
移動体制御装置１０００では、ロボットＲｂｔ１の内部状態を示すデータＸ_ｔとして、
（１）時刻ｔにおけるロボットＲｂｔ１の位置および角度に関する内部状態データＸＰ_ｔと、
（２）時刻ｔにおけるロボットＲｂｔ１の分散共分散行列Ｖ＿ｍｔｘ_ｔ（Ｒｂｔ１）と、
が設定される。時刻ｔのロボットＲｂｔ１の内部状態データＸ_ｔを、
Ｘ_ｔ＝［ＸＰ_ｔ，Ｖ＿ｍｔｘ_ｔ（Ｒｂｔ１）］
と表記する。

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

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

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

図４は、ロボットＲｂｔ１の内部状態のうち、ロボットＲｂｔ１のｘｙ平面の位置を推定するためのパーティクルの分布（一例）を模式的に示した図である。

ロボットＲｂｔ１の内部状態を推定するために、Ｍ１個（Ｍ１：自然数）のパーティクルが用いられるものとする。

また、移動体制御装置１０００では、ロボットＲｂｔ１の内部状態を示すデータとして、時刻ｔにおけるロボットＲｂｔ１の分散共分散行列Ｖ＿ｍｔｘ_ｔ（Ｒｂｔ１）が設定される。具体的には、時刻ｔにおけるロボットＲｂｔ１の分散共分散行列Ｖ＿ｍｔｘ_ｔ（Ｒｂｔ１）は、以下の数式により表現される。

なお、ｘ０_ｔ ^（ｉ）は、ロボットＲｂｔ１の内部状態を推定するためのｉ番目（ｉ：自然数、１≦ｉ≦Ｍ１）のパーティクルのｘ座標位置である。ｙ０_ｔ ^（ｉ）は、ロボットＲｂｔ１の内部状態を推定するためのｉ番目（ｉ：自然数、１≦ｉ≦Ｍ１）のパーティクルのｙ座標位置である。

また、Ｅ［ａ］は、ａの期待値である。つまり、Ｅ［（ｘ０_ｔ ^（ｉ）−ｘ０_ｔ）（ｘ０_ｔ ^（ｉ）−ｘ０_ｔ）］は、ロボットＲｂｔ１の内部状態を推定するためのＭ１個（Ｍ１：自然数）のパーティクルのｘ座標値の分散値であり、Ｅ［（ｙ０_ｔ ^（ｉ）−ｙ０_ｔ）（ｙ０_ｔ ^（ｉ）−ｙ０_ｔ）］は、ロボットＲｂｔ１の内部状態を推定するためのＭ１個（Ｍ１：自然数）のパーティクルのｙ座標値の分散値である。

また、ｘ０_ｔは、Ｍ１個（Ｍ１：自然数）のパーティクルのｘ座標値の期待値（平均値）であり、ｙ０_ｔは、Ｍ１個（Ｍ１：自然数）のパーティクルのｙ座標値の期待値（平均値）である。

以上のように、移動体制御装置１０００では、ロボットＲｂｔ１の内部状態を示すデータＸ_ｔとして、（Ａ）時刻ｔにおけるロボットＲｂｔ１の位置および角度に関する内部状態データＸＰ_ｔ、および、（Ｂ）時刻ｔにおけるロボットＲｂｔ１の分散共分散行列Ｖ＿ｍｔｘ_ｔ（Ｒｂｔ１）が設定される。

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

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

時刻ｔにおける環境地図データｍ_ｔは、移動体制御装置１０００において、以下のように、設定される。
ｍ_ｔ＝［ＬＭ１Ｐ_ｔ，Ｖ＿ｍｔｘ_ｔ（ＬＭ１），ＬＭ２Ｐ_ｔ，Ｖ＿ｍｔｘ_ｔ（ＬＭ２），．．．．，ＬＭｋＰ_ｔ，Ｖ＿ｍｔｘ_ｔ（ＬＭｋ）］
ｋ：自然数
ＬＭｋＰ_ｔは、時刻ｔにおけるｋ番目のランドマークの位置に関する内部状態データであり、Ｖ＿ｍｔｘ_ｔ（ＬＭｋ）は、時刻ｔにおけるｋ番目のランドマークの分散共分散行列である。

つまり、時刻ｔにおいて、ｎ番目（ｎ：自然数、ｎ≦ｋ）のランドマークの情報として、（Ａ）内部状態データＬＭｎＰ_ｔ、および、（Ｂ）分散共分散行列Ｖ＿ｍｔｘ_ｔ（ＬＭｎ）が設定される。そして、ｋ個のランドマークについての上記（Ａ）（Ｂ）のデータの集合が、時刻ｔにおける環境地図データｍ_ｔとして、設定される。

図３の場合において、環境地図データｍ_ｔは、２つのランドマークＬＭ１、ＬＭ２の情報により構成される。図３の場合、環境地図データｍ_ｔは、
ｍ_ｔ＝［ＬＭ１Ｐ_ｔ，Ｖ＿ｍｔｘ_ｔ（ＬＭ１），ＬＭ２Ｐ_ｔ，Ｖ＿ｍｔｘ_ｔ（ＬＭ２）］
である。

つまり、図３の場合、環境地図データｍ_ｔは、
（１）ランドマークＬＭ１の位置に関する内部状態データＬＭ１Ｐ_ｔ、および、分散共分散行列Ｖ＿ｍｔｘ_ｔ（ＬＭ１）と、
（２）ランドマークＬＭ２の位置に関する内部状態データＬＭ２Ｐ_ｔ、および、分散共分散行列Ｖ＿ｍｔｘ_ｔ（ＬＭ２）と、
により構成される。

なお、以下では、説明便宜のため、図２、図３に示すように、２つのランドマークＬＭ１、ＬＭ２により、環境地図データが取得される場合について、説明する。

ランドマークＬＭ１の位置に関する内部状態データＬＭ１Ｐ_ｔは、２次元ベクトルであり、ランドマークＬＭ１のｘｙ平面上の位置の期待値を示す。つまり、ランドマークＬＭ１の位置に関する内部状態データＬＭ１Ｐ_ｔは、
ＬＭ１Ｐ_ｔ＝（ｘ１_ｔ，ｙ１_ｔ）
ｘ１_ｔ：時刻ｔにおけるランドマークＬＭ１のｘ座標値の期待値
ｙ１_ｔ：時刻ｔにおけるランドマークＬＭ１のｙ座標値の期待値
として表現される。

ランドマークＬＭ２の位置に関する内部状態データＬＭ２Ｐ_ｔは、２次元ベクトルであり、ランドマークＬＭ２のｘｙ平面上の位置の期待値を示す。つまり、ランドマークＬＭ２の位置に関する内部状態データＬＭ２Ｐ_ｔは、
ＬＭ２Ｐ_ｔ＝（ｘ２_ｔ，ｙ２_ｔ）
ｘ２_ｔ：時刻ｔにおけるランドマークＬＭ２のｘ座標値の期待値
ｙ２_ｔ：時刻ｔにおけるランドマークＬＭ２のｙ座標値の期待値
として表現される。

移動体制御装置１０００では、ランドマークの情報を推定するために、パーティクルフィルタ（モンテカルロ近似）を用いるものとする。

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

また、ランドマークＬＭ２の位置に関する内部状態データを推定するためのパーティクルフィルタ処理では、時刻ｔ−１のランドマークＬＭ２の状態ベクトルＬＭ２Ｐ_ｔ−１の事後確率分布を現時刻ｔのランドマークＬＭ２の状態ベクトルＬＭ２Ｐ_ｔの事前確率分布とし、現時刻ｔのランドマークＬＭ２の状態ベクトルＬＭ２Ｐ_ｔの事前確率分布から予測処理により現時刻ｔのランドマークＬＭ２の状態ベクトルＬＭ２Ｐ_ｔの予測確率分布を取得する。そして、取得した予測確率分布と実際の観測データ（現時刻ｔの観測データ）とに基づいて、尤度を算出する。そして、算出した尤度に比例する割合で、粒子（パーティクル）を復元抽出する。このように処理することで、ランドマークＬＭ２の情報（内部状態）を推定することができる。

図４は、ランドマークＬＭ１、ＬＭ２の情報（位置に関する内部状態データ）、つまり、ランドマークＬＭ１、ＬＭ２のｘｙ平面の位置を推定するためのパーティクルの分布（一例）を模式的に示した図である。

ランドマークＬＭ１、ＬＭ２の情報（ｘｙ平面の位置）を推定するために、ランドマークＬＭ１についてＭ１１個（Ｍ１１：自然数）のパーティクルが用いられ、ランドマークＬＭ２についてＭ１２個（Ｍ１２：自然数）のパーティクルが用いられるものとする。

また、ランドマークＬＭ１の情報（ランドマークＬＭ１の内部状態）を示すデータとして、時刻ｔにおけるランドマークＬＭ１の分散共分散行列Ｖ＿ｍｔｘ_ｔ（ＬＭ１）が設定される。具体的には、時刻ｔにおけるランドマークＬＭ１の分散共分散行列Ｖ＿ｍｔｘ_ｔ（ＬＭ１）は、以下の数式により表現される。

なお、ｘ１_ｔ ^（ｉ）は、ランドマークＬＭ１の内部状態を推定するためのｉ番目（ｉ：自然数、１≦ｉ≦Ｍ１１）のパーティクルのｘ座標位置である。ｙ１_ｔ ^（ｉ）は、ランドマークＬＭ１の内部状態を推定するためのｉ番目（ｉ：自然数、１≦ｉ≦Ｍ１１）のパーティクルのｙ座標位置である。

また、Ｅ［ａ］は、ａの期待値である。つまり、Ｅ［（ｘ１_ｔ ^（ｉ）−ｘ１_ｔ）（ｘ１_ｔ ^（ｉ）−ｘ１_ｔ）］は、ランドマークＬＭ１の内部状態を推定するためのＭ１１個（Ｍ１１：自然数）のパーティクルのｘ座標値の分散値であり、Ｅ［（ｙ１_ｔ ^（ｉ）−ｙ１_ｔ）（ｙ１_ｔ ^（ｉ）−ｙ１_ｔ）］は、ランドマークＬＭ１の内部状態を推定するためのＭ１１個（Ｍ１１：自然数）のパーティクルのｙ座標値の分散値である。

また、ｘ１_ｔは、ランドマークＬＭ１の内部状態を推定するためのＭ１１個（Ｍ１１：自然数）のパーティクルのｘ座標値の期待値（平均値）であり、ｙ１_ｔは、ランドマークＬＭ１の内部状態を推定するためのＭ１１個（Ｍ１１：自然数）のパーティクルのｙ座標値の期待値（平均値）である。

また、ランドマークＬＭ２の情報（ランドマークＬＭ２の内部状態）を示すデータとして、時刻ｔにおけるランドマークＬＭ２の分散共分散行列Ｖ＿ｍｔｘ_ｔ（ＬＭ２）が設定される。具体的には、時刻ｔにおけるランドマークＬＭ２の分散共分散行列Ｖ＿ｍｔｘ_ｔ（ＬＭ２）は、以下の数式により表現される。

なお、ｘ２_ｔ ^（ｉ）は、ランドマークＬＭ２の内部状態を推定するためのｉ番目（ｉ：自然数、１≦ｉ≦Ｍ１２）のパーティクルのｘ座標位置である。ｙ２_ｔ ^（ｉ）は、ランドマークＬＭ２の内部状態を推定するためのｉ番目（ｉ：自然数、１≦ｉ≦Ｍ１２）のパーティクルのｙ座標位置である。

また、Ｅ［ａ］は、ａの期待値である。つまり、Ｅ［（ｘ２_ｔ ^（ｉ）−ｘ２_ｔ）（ｘ２_ｔ ^（ｉ）−ｘ２_ｔ）］は、ランドマークＬＭ２の内部状態を推定するためのＭ１２個（Ｍ１２：自然数）のパーティクルのｘ座標値の分散値であり、Ｅ［（ｙ２_ｔ ^（ｉ）−ｙ２_ｔ）（ｙ２_ｔ ^（ｉ）−ｙ２_ｔ）］は、ランドマークＬＭ２の内部状態を推定するためのＭ１２個（Ｍ１２：自然数）のパーティクルのｙ座標値の分散値である。

また、ｘ２_ｔは、ランドマークＬＭ２の内部状態を推定するためのＭ１２個（Ｍ１２：自然数）のパーティクルのｘ座標値の期待値（平均値）であり、ｙ２_ｔは、ランドマークＬＭ２の内部状態を推定するためのＭ１２個（Ｍ１１：自然数）のパーティクルのｙ座標値の期待値（平均値）である。

以上のように、移動体制御装置１０００では、時刻ｔにおける環境地図データｍ_ｔは、ランドマークの情報（各ランドマークの位置に関する内部状態データ、および、分散共分散行列）を用いて設定される。

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

（時刻ｔ−１の処理）：
時刻ｔ−１において、ロボットＲｂｔ１、ランドマークＬＭ１、ＬＭ２の状態は、図２に示す状態である。そして、時刻ｔ−１において、移動体制御装置１０００の状態推定部３により取得されているロボットＲｂｔ１の内部状態データＸ_ｔ−１、および、環境地図データｍ_ｔ−１は、以下の通りである。

≪ロボットＲｂｔ１の内部状態データＸ_ｔ−１≫
Ｘ_ｔ−１＝［ＸＰ_ｔ−１，Ｖ＿ｍｔｘ_ｔ−１（Ｒｂｔ１）］
ＸＰ_ｔ−１＝（ｘ０_ｔ−１，ｙ０_ｔ−１，θ_ｔ−１）
≪環境地図データｍ_ｔ−１≫
ｍ_ｔ−１＝［ＬＭ１Ｐ_ｔ−１，Ｖ＿ｍｔｘ_ｔ−１（ＬＭ１），ＬＭ２Ｐ_ｔ−１，Ｖ＿ｍｔｘ_ｔ−１（ＬＭ２）］
ＬＭ１Ｐ_ｔ−１＝（ｘ１_ｔ−１，ｙ１_ｔ−１）
ＬＭ２Ｐ_ｔ−１＝（ｘ２_ｔ−１，ｙ２_ｔ−１）
時刻ｔ−１において、移動体制御装置１０００の状態推定部３により取得された内部状態データＸ_ｔ−１、および、環境地図データｍ_ｔ−１は、記憶部４に記憶される。

また、時刻ｔ−１において、移動体制御装置１０００には、制御入力データＵ_ｔ−１が入力されている。なお、制御入力データＵ_ｔ−１は、ロボットＲｂｔ１を図２の矢印Ｄｉｒ１の方向に進行させるためのデータであるものとする。具体的には、制御入力データＵ_ｔ−１は、
Ｕ_ｔ−１＝（Ｖｘ_ｔ−１，Ｖｙ_ｔ−１）
θ_ｔ−１＝ｔａｎ^−１（−Ｖｙ_ｔ−１／Ｖｘ_ｔ−１）
Ｖｘ_ｔ−１：ｘ方向の速度
Ｖｙ_ｔ−１：ｙ方向の速度
である。この制御入力データＵ_ｔ−１により制御されることで、ロボットＲｂｔ１は、時刻ｔにおいて、図３に示す位置に移動すると予測される。なお、図２の点（ｘ０_ｔ−１，ｙ０_ｔ−１）と図３の点（ｘ０_ｔ，ｙ０_ｔ）との距離は、時刻ｔ−１から時刻ｔまでの時間、ロボットＲｂｔ１が、ｘ方向に速度Ｖｘ_ｔ−１、ｙ方向に速度Ｖｙ_ｔ−１で進んだときの距離（予測される距離）である。

また、ロボットＲｂｔ１に搭載されている撮像装置（不図示）は、その光学系の光軸が図２の矢印Ｄｉｒが示す方向と一致するように配置されており、図２に示す角度αにより、ロボットＲｂｔ１の周囲を撮像する。時刻ｔ−１において、ロボットＲｂｔ１の撮像装置により撮像される範囲は、図２の点線で示した領域ＡＲ＿ｃａｐ_ｔ−１である。

図５の左図に、時刻ｔ−１においてロボットＲｂｔ１の撮像装置により撮像された画像Ｉｍｇ_ｔ−１を模式的に示す。図５の左図に示すように、円形のランドマークＬＭ１が撮像されている。なお、図５の左図において、ランドマークＬＭ１の時刻ｔ−１における位置に関する内部状態データＬＭ１Ｐ_ｔ−１＝（ｘ１_ｔ−１，ｙ１_ｔ−１）に対応する点を黒丸で示している。

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

時刻ｔにおいて、ロボットＲｂｔ１の撮像装置により撮像された画像が、入力データＤｉｎとして、観測取得部１に入力される。

観測取得部１は、入力データＤｉｎに対して、カメラ信号処理（例えば、ゲイン調整処理、ガンマ補正処理、ホワイトバランス処理等）を実行し、処理後の画像信号Ｄ１（画像Ｄ１）を観測データとして、ランドマーク検出部２に出力する。

ランドマーク予測部５は、記憶部４から、時刻ｔ−１におけるデータＳＴ_ｔ−１を取得する。

なお、データＳＴ_ｔ−１には、（１）時刻ｔ−１のロボットＲｂｔ１の内部状態データＸ_ｔ−１、（２）時刻ｔ−１の環境地図データｍ_ｔ−１、および、（３）時刻ｔ−１のロボットＲｂｔ１の制御入力データＵ_ｔ−１が含まれる。

ランドマーク予測部５は、時刻ｔ−１におけるデータＳＴ_ｔ−１に基づいて、時刻ｔにおけるランドマークの情報を予測する。

具体的には、ランドマーク予測部５は、画像Ｄ１において、ランドマークが写っている可能性が高い位置を予測する。時刻ｔ−１のロボットＲｂｔ１の制御入力データＵ_ｔ−１から、時刻ｔにおいて、ロボットＲｂｔ１は、図３の位置に存在している可能性が高いと判断でき、時刻ｔにおいて、ロボットＲｂｔ１の撮像装置により撮像される範囲は、図３に点線で示した領域ＡＲ＿ｃａｐ_ｔであると予測できる。このとき、ロボットＲｂｔ１の撮像装置により取得される撮像画像（画像Ｄ１）には、ランドマークＬＭ１が写っている可能性が高いことが予測でき、画像Ｄ１上において、ランドマークＬＭ１が写っている可能性が高い画像領域を、（１）時刻ｔ−１のロボットＲｂｔ１の内部状態データＸ_ｔ−１、（２）時刻ｔ−１の環境地図データｍ_ｔ−１（時刻ｔ−１のランドマークＬＭ１の位置に関する情報）、および、（３）時刻ｔ−１のロボットＲｂｔ１の制御入力データＵ_ｔ−１に基づいて、算出することができる。

このようにして、算出されたランドマークＬＭ１が写っている可能性が高い画像領域ＡＲ１_ｔ（一例）を図５の右図に示す。

図５の右図は、画像Ｄ１（図５では、Ｉｍｇ_ｔとして示した画像）上において、ランドマークＬＭ１が写っている可能性が高い画像領域ＡＲ１_ｔを模式的に示した図である。

ランドマーク予測部５は、（１）時刻ｔにおいて、画像Ｄ１に写っている可能性が高いランドマークがランドマークＬＭ１のみであることを示す情報と、（２）上記処理により算出したランドマークＬＭ１が写っている可能性が高い画像領域ＡＲ１_ｔを特定するための情報と、を含めた信号を、ランドマーク予測信号Ｓｉｇ＿ｐｒｅとして、ランドマーク検出部２に出力する。

なお、説明便宜のため、画像領域ＡＲ１_ｔは、図５の右図に示すように、楕円の画像領域とする。そして、楕円の画像領域ＡＲ１_ｔの中心において、ランドマークＬＭ１が検出される可能性が高く、楕円の中心から離れるに従って、ランドマークＬＭ１が検出される可能性が低くなるものとする。

また、画像領域ＡＲ１_ｔの大きさは、例えば、時刻ｔ−１の環境地図データｍ_ｔ−１に含まれる、ランドマークＬＭ１の位置に関する情報、および／または、ランドマークＬＭ１の分散共分散行列に基づいて、設定される。なお、処理を簡便化するために、画像領域ＡＲ１_ｔの大きさを、所定の大きさに固定してもよい（ｋ番目のランドマークについての画像領域ＡＲｋ_ｔ（ｋ：自然数）の大きさについても同様）。

ランドマーク検出部２は、ランドマーク予測部５からのランドマーク予測信号Ｓｉｇ＿ｐｒｅに基づいて、時刻ｔにおいて、画像Ｄ１に写っている可能性が高いランドマークがランドマークＬＭ１のみであることを把握するとともに、ランドマークＬＭ１が写っている可能性が高い画像領域ＡＲ１_ｔを用いて、ランドマークＬＭ１を検出する処理を行う。

図５の右図に示すように、画像Ｄ１（画像Ｉｍｇ_ｔ）において、ランドマークＬＭ１は、図５の右図にＬＭ１で示した位置に写っている。ランドマーク検出部２は、ランドマークＬＭ１を検出処理の探索初期位置を画像領域ＡＲ１_ｔの中心点とし、その中心点からの距離が大きくなるように、順次、ランドマークＬＭ１の探索処理を実行する。図５の右図の場合、ランドマークＬＭ１は、画像領域ＡＲ１_ｔの中心点に近い位置に存在しているので、ランドマーク検出部２は、ランドマークＬＭ１の探索処理を、画像領域ＡＲ１_ｔの中心点から実行することで、即座に、ランドマークＬＭ１を検出することができる。なお、ランドマークを識別するための、ランドマークに特定のコードを示す模様や色や文字等のランドマーク識別情報（マーカー）を付してもよい。これにより、画像Ｄ１（画像Ｉｍｇ_ｔ）において、ランドマーク識別情報（マーカー）を検出することで、特定のランドマークを検出することができる（検出したランドマークがどのランドマークであるかを把握することができる）。

ランドマーク検出部２は、上記処理により検出したランドマークＬＭ１の画像Ｄ１（画像Ｉｍｇ_ｔ）の位置、大きさと、画像Ｄ１（画像Ｉｍｇ_ｔ）が撮像されたときの撮影パラメータ（画角、焦点距離等）とに基づいて、ランドマークＬＭ１の環境地図上の位置（図２、図３に示したｘｙ平面上の座標位置）、大きさを取得する。そして、ランドマーク検出部２は、取得したランドマークＬＭ１の環境地図上の位置、大きさを特定するための情報を含めた信号を、ランドマーク検出信号ＬＭ_ｔとして、状態推定部３の状態更新部３１および環境地図更新部３２に出力する。

状態推定部３の状態更新部３１では、観測取得部１から出力される観測データＤ１と、ランドマーク検出部２から出力されるランドマーク検出信号ＬＭ_ｔと、移動体（ロボットＲｂｔ１）に対する制御入力データＵ_ｔとに基づいて、ロボットＲｂｔ１の内部状態を推定し、推定結果をロボットＲｂｔ１の現時刻ｔの内部状態データＸ_ｔとして、取得する（内部状態データを更新する）。

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

なお、現時刻ｔの状態ベクトルＸＰ_ｔを取得するために使用される実際の観測データは、例えば、（１）観測取得部１から出力される画像Ｄ１、（２）ランドマーク検出信号ＬＭ_ｔから取得したランドマーク（ランドマークＬＭｋ）の環境地図上の位置、大きさを特定するための情報、および、（３）その他のロボットＲｂｔ１の環境地図上の位置、大きさを特定するための情報（例えば、ロボットＲｂｔ１に取り付けられた位置センサーにより取得した情報）等、の少なくとも１つから導出されたデータである。

また、状態更新部３１は、ロボットＲｂｔ１の内部状態を示すデータとして、時刻ｔにおけるロボットＲｂｔ１の分散共分散行列Ｖ＿ｍｔｘ_ｔ（Ｒｂｔ１）を取得する。図２〜図４では、分散共分散行列の各要素の値、および／または、分散共分散行列の各要素の値から導出される値に基づいて、その形状、大きさ等が決定される楕円であって、ロボットＲｂｔ１の位置の内部状態を取得するためのパーティクルの大部分を含むように設定された楕円を模式的に示している。つまり、図２〜図４に示した楕円の大きさは、ロボットＲｂｔ１の位置の内部状態データの不確かさを示しており、楕円の大きさが小さい程、ロボットＲｂｔ１の位置が精度良く推定されていることを示している。

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

状態推定部３の環境地図更新部３２では、ランドマーク検出部２から出力されるランドマーク検出信号ＬＭ_ｔに基づいて、ランドマークの位置に関する内部状態を推定し、推定結果に基づいて、時刻ｔの環境地図データｍ_ｔを取得する。ここでは、ランドマークＬＭ１についての処理について説明する。

環境地図更新部３２は、パーティクルフィルタ処理により、ランドマークＬＭ１の現時刻ｔの位置に関する内部状態データＬＭ１Ｐ_ｔを取得（更新）する。具体的には、環境地図更新部３２は、時刻ｔ−１のランドマークＬＭ１の位置に関する内部状態データＬＭ１Ｐ_ｔ−１の事後確率分布を現時刻ｔのランドマークＬＭ１の位置に関する内部状態データＬＭ１Ｐ_ｔの事前確率分布とし、現時刻ｔのランドマークＬＭ１の位置に関する内部状態データＬＭ１Ｐ_ｔの事前確率分布から予測処理により現時刻ｔのランドマークＬＭ１の位置に関する内部状態データＬＭ１Ｐ_ｔの予測確率分布を取得する。そして、取得した予測確率分布と実際の観測データ（現時刻ｔの観測データ）とに基づいて、尤度を算出する。そして、算出した尤度に比例する割合で、粒子（パーティクル）を復元抽出する。このように処理することで、ランドマークＬＭ１の位置に関する内部状態を推定することができる。

なお、現時刻ｔのランドマークＬＭ１の位置に関する内部状態データＬＭ１Ｐ_ｔを取得するために使用される実際の観測データは、例えば、ランドマーク検出信号ＬＭ_ｔから取得したランドマークＬＭ１の環境地図上の位置、大きさを特定するための情報から導出されたデータである。

また、環境地図更新部３２は、ランドマークＬＭ１の位置に関する内部状態を示すデータとして、時刻ｔにおけるランドマークＬＭ１の分散共分散行列Ｖ＿ｍｔｘ_ｔ（ＬＭ１）を取得する。図２〜図４では、分散共分散行列の各要素の値、および／または、分散共分散行列の各要素の値から導出される値に基づいて、その形状、大きさ等が決定される楕円であって、ランドマークＬＭ１の位置の内部状態を取得するためのパーティクルの大部分を含むように設定された楕円を模式的に示している。つまり、図２〜図４に示した楕円の大きさは、ランドマークＬＭ１の位置の内部状態データの不確かさを示しており、楕円の大きさが小さい程、ランドマークＬＭ１の位置が精度良く推定されていることを示している。

環境地図更新部３２は、ランドマークＬＭ２についても、上記と同様の処理により、ランドマークＬＭ２の位置に関する内部状態データＬＭ２Ｐ_ｔ、および、分散共分散行列Ｖ＿ｍｔｘ_ｔ（ＬＭ２）を取得する。

環境地図更新部３２は、上記のように取得したデータにより、時刻ｔの環境地図データｍ_ｔを取得（更新）する。そして、環境地図更新部３２により取得された環境地図データｍ_ｔは、状態推定部３の外部、および、記憶部４に出力される。

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

このように処理することで、移動体制御装置１０００では、精度の高い、（１）環境地図データｍ_ｔ、および、（２）ロボットＲｂｔ１の内部状態データＸ_ｔを取得し続けることができる。そして、この高精度のデータを用いることで、ロボットＲｂｔ１を適切に制御することができる。

以上のように、移動体制御装置１０００では、ランドマーク予測部が、（１）時刻ｔ−１のロボットＲｂｔ１の内部状態データＸ_ｔ−１、（２）時刻ｔ−１の環境地図データｍ_ｔ−１、および、（３）時刻ｔ−１のロボットＲｂｔ１の制御入力データＵ_ｔ−１に基づいて、時刻ｔの観測データである画像Ｄ１上において、所定のランドマークが存在する確率が高い画像領域を予測し、当該ランドマークの探索処理を行う。このため、移動体制御装置１０００では、所定のランドマークを探索するために、従来技術のように、画像Ｄ１上の全ての領域を探索する必要がない。その結果、移動体制御装置１０００では、所定のランドマークを探索するための処理時間を著しく短縮することができる。これにより、移動体制御装置１０００における環境地図データを作成（更新）するための処理時間を短くすることができる。つまり、移動体制御装置１０００では、上記処理を行うことにより、効率良く環境地図を作成することができ、高精度な状態推定処理を短い時間で実行することができる。その結果、移動体制御装置１０００により、移動体の制御を適切に行うことができる。

なお、上記では、ランドマーク予測部が、現時刻ｔの１つ前の時刻ｔ−１のデータＳＴ_ｔ−１に基づいて、時刻ｔにおけるランドマークの情報を予測する場合について説明したが、これに限定されることはなく、現時刻ｔよりも前の過去の複数のデータ（例えば、ＳＴ_ｔ−１、ＳＴ_ｔ−２等）を用いて、時刻ｔにおけるランドマークの情報を予測するようにしてもよい。

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

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

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

図７は、第２実施形態のランドマーク予測部５Ａの概略構成図である。

図８は、第２実施形態のランドマーク検出部２Ａの概略構成図である。

第２実施形態の移動体制御装置２０００では、第１実施形態の移動体制御装置１０００において、ランドマーク検出部２をランドマーク検出部２Ａに置換し、ランドマーク予測部５をランドマーク予測部５Ａに置換した構成を有している。

ランドマーク検出部２Ａは、図７に示すように、ランドマーク予測信号生成部５１と、環境地図データ信頼度取得部５２とを備える。

ランドマーク予測信号生成部５１は、記憶部４から読み出された時刻ｔ−１におけるデータＳＴ_ｔ−１を入力する。ランドマーク予測信号生成部５１は、データＳＴ_ｔ−１からランドマーク予測信号Ｓｉｇ＿ｐｒｅを生成し、生成したランドマーク予測信号Ｓｉｇ＿ｐｒｅをランドマーク検出部２Ａに出力する。

環境地図データ信頼度取得部５２は、記憶部４から読み出された時刻ｔ−１におけるデータＳＴ_ｔ−１を入力する。環境地図データ信頼度取得部５２は、環境地図データｍ_ｔ−１から、各ランドマークの情報の信頼度Ｂｌｆ＿ｍを取得し、取得した各ランドマークの情報の信頼度Ｂｌｆ＿ｍをランドマーク検出部２Ａに出力する。なお、信頼度Ｂｌｆ＿ｍは、ランドマークごとに取得される。例えば、ランドマークＬＭ１の信頼度Ｂｌｆ＿ｍ（ＬＭ１）は、ランドマークＬＭ１の分散共分散行列Ｖ＿ｍｔｘ_ｔ−１（ＬＭ１）の対角成分の和、すなわち、ランドマークＬＭ１の時刻ｔ−１におけるｘ座標位置についての分散値およびランドマークＬＭ１の時刻ｔ−１におけるｙ座標位置についての分散値の和に対応した値に設定される。なお、ランドマークＬＭｋの信頼度Ｂｌｆ＿ｍ（ＬＭｋ）の算出方法は、上記に限定されることなく、ランドマークの位置の推定精度を表す値を取得する方法であれば、他の方法であってもよい。

ランドマーク検出部２Ａは、図８に示すように、ランドマーク初期位置取得部２１と、第１比較器２２と、ランドマーク探索部２３と、を備える。

ランドマーク初期位置取得部２１は、ランドマーク予測部５Ａから出力されるランドマーク予測信号Ｓｉｇ＿ｐｒｅを入力する。ランドマーク初期位置取得部２１は、ランドマーク予測信号Ｓｉｇ＿ｐｒｅから、画像Ｄ１上において、各ランドマークが存在する可能性が高い画像領域を設定するための情報を取得し、取得した情報に基づいて、画像Ｄ１上において、各ランドマークを探索するための（画像上の）初期位置を算出する。そして、ランドマーク初期位置取得部２１は、算出したランドマークごとの探索処理の初期位置についての情報Ｉｎｉｔ＿ｐｏｓをランドマーク探索部２３に出力する。

第１比較器２２は、ランドマーク予測部５Ａから出力される各ランドマークの情報の信頼度Ｂｌｆ＿ｍと、しきい値Ｔｈ１とを入力とし、両者の大小を比較し、比較結果を示す信号Ｄｅｔ１をランドマーク探索部２３に出力する。

ランドマーク探索部２３は、ランドマーク初期位置取得部２１から出力されるランドマークごとの探索処理の初期位置についての情報Ｉｎｉｔ＿ｐｏｓと、第１比較器２２から出力される比較結果を示す信号Ｄｅｔ１とを入力する。ランドマーク探索部２３は、比較結果を示す信号Ｄｅｔ１に基づいて、ランドマークごとに、（１）ローカル探索処理、あるいは、（２）グローバル探索処理を実行し、各ランドマークの環境地図上の位置（図２、図３に示したｘｙ平面上の座標位置）、大きさを取得する。そして、ランドマーク探索部２３は、取得したランドマークの環境地図上の位置、大きさを特定するための情報を含めた信号を、ランドマーク検出信号ＬＭ_ｔとして、状態推定部３の状態更新部３１および環境地図更新部３２に出力する。

＜２．２：移動体制御装置の動作＞
以上のように構成された移動体制御装置２０００の動作について、説明する。なお、第１実施形態と同様の部分については、説明を省略する。

ランドマーク予測信号生成部５１では、記憶部４から読み出されたデータＳＴ_ｔ−１からランドマーク予測信号Ｓｉｇ＿ｐｒｅが生成され、生成されたランドマーク予測信号Ｓｉｇ＿ｐｒｅは、ランドマーク予測信号生成部５１からランドマーク検出部２Ａのランドマーク初期位置取得部２１に出力される。

環境地図データ信頼度取得部５２では、記憶部４から読み出された時刻ｔ−１における環境地図データｍ_ｔ−１から、各ランドマークの情報の信頼度Ｂｌｆ＿ｍが取得され、取得された各ランドマークの情報の信頼度Ｂｌｆ＿ｍは、環境地図データ信頼度取得部５２からランドマーク検出部２Ａの第１比較器２２に出力される。

具体的には、環境地図データ信頼度取得部５２では、
（１）ランドマークＬＭ１の信頼度Ｂｌｆ＿ｍ（ＬＭ１）が、ランドマークＬＭ１の分散共分散行列Ｖ＿ｍｔｘ_ｔ−１（ＬＭ１）の対角成分の和に基づいて、取得され、
（２）ランドマークＬＭ２の信頼度Ｂｌｆ＿ｍ（ＬＭ２）が、ランドマークＬＭ１の分散共分散行列Ｖ＿ｍｔｘ_ｔ−１（ＬＭ２）の対角成分の和に基づいて、取得される。

なお、説明便宜のため、本実施形態においても、環境地図データが、ランドマークＬＭ１、ＬＭ２により構成されている場合について説明する。

ランドマーク検出部２Ａのランドマーク初期位置取得部２１では、ランドマーク予測部５Ａから出力されるランドマーク予測信号Ｓｉｇ＿ｐｒｅから、画像Ｄ１上において、各ランドマークが存在する可能性が高い画像領域を設定するための情報が取得され、取得された情報に基づいて、画像Ｄ１上において、各ランドマークを探索するための（画像上の）初期位置が算出される。そして、算出されたランドマークごとの探索処理の初期位置についての情報Ｉｎｉｔ＿ｐｏｓは、ランドマーク初期位置取得部２１からランドマーク探索部２３に出力される。

第１比較器２２では、ランドマーク予測部５Ａから出力される各ランドマークの情報の信頼度Ｂｌｆ＿ｍと、しきい値Ｔｈ１とが入力され、両者の大小が比較される。

具体的には、
（Ａ）ランドマークＬＭ１の信頼度Ｂｌｆ＿ｍ（ＬＭ１）とＴｈ１とが比較され、
（Ａ１）Ｂｌｆ＿ｍ（ＬＭ１）＜Ｔｈ１の場合、Ｄｅｔ１（ＬＭ１）＝１に設定され、
（Ａ２）Ｂｌｆ＿ｍ（ＬＭ１）≧Ｔｈ１の場合、Ｄｅｔ１（ＬＭ１）＝０に設定される。
（Ｂ）ランドマークＬＭ２の信頼度Ｂｌｆ＿ｍ（ＬＭ２）とＴｈ１とが比較され、
（Ａ１）Ｂｌｆ＿ｍ（ＬＭ２）＜Ｔｈ１の場合、Ｄｅｔ１（ＬＭ２）＝１に設定され、
（Ａ２）Ｂｌｆ＿ｍ（ＬＭ２）≧Ｔｈ１の場合、Ｄｅｔ１（ＬＭ２）＝０に設定される。

そして、上記により取得されたランドマークごとの検出結果を示す信号Ｄｅｔ１が、第１比較器２２からランドマーク探索部２３に出力される。

ランドマーク探索部２３では、ランドマーク初期位置取得部２１から出力されるランドマークごとの探索処理の初期位置についての情報Ｉｎｉｔ＿ｐｏｓと、第１比較器２２から出力される比較結果を示す信号Ｄｅｔ１とに基づいて、ランドマークごとに、（１）ローカル探索処理、あるいは、（２）グローバル探索処理を実行するかを決定する。

具体的には、ランドマーク探索部２３は、以下の（Ａ１）、（Ａ２）、（Ｂ１）、（Ｂ２）の処理を行う。
（Ａ１）Ｄｅｔ１（ＬＭ１）＝１の場合、ランドマークＬＭ１の推定結果の信頼度が高いと判断できるので、ランドマークＬＭ１については、ローカル探索処理を行う。ここで、「ローカル探索処理」とは、第１実施形態と同様に、ランドマークＬＭｋ（ｋ：自然数）が写っている可能性が高い画像領域ＡＲｋ_ｔを設定し、設定した画像領域ＡＲｋ_ｔを探索することで、ランドマークＬＭｋ（ｋ：自然数）を検出する処理のことをいう。
（Ａ２）Ｄｅｔ１（ＬＭ１）＝０の場合、ランドマークＬＭ１の推定結果の信頼度が高くないと判断できるので、ランドマークＬＭ１については、グローバル探索処理を行う。ここで、「グローバル探索処理」とは、画像Ｄ１上の全ての領域を探索領域に設定し、ランドマークＬＭｋ（ｋ：自然数）を検出する処理のことをいう。
（Ｂ１）Ｄｅｔ１（ＬＭ２）＝１の場合、ランドマークＬＭ２の推定結果の信頼度が高いと判断できるので、ランドマークＬＭ２については、ローカル探索処理を行う。
（Ｂ２）Ｄｅｔ１（ＬＭ２）＝０の場合、ランドマークＬＭ２の推定結果の信頼度が高くないと判断できるので、ランドマークＬＭ２については、グローバル探索処理を行う。

以上の処理により取得した探索処理の結果に基づいて、第１実施形態と同様の処理により、ランドマークＬＭ１、ＬＭ２の環境地図上の位置（図２、図３に示したｘｙ平面上の座標位置）、大きさを取得する。そして、ランドマーク探索部２３は、取得したランドマークＬＭ１、ＬＭ２の環境地図上の位置、大きさを特定するための情報を含めた信号を、ランドマーク検出信号ＬＭ_ｔとして、状態推定部３の状態更新部３１および環境地図更新部３２に出力する。

以上のように、移動体制御装置２０００では、ランドマークの情報の信頼度に基づいて、ランドマークを検出するための処理を、ローカル探索処理、あるいは、グローバル探索処理にするかを切り替えることができる。そのため、移動体制御装置２０００では、ランドマークの情報の信頼度が高い場合は、高速にランドマークを検出することができ、ランドマークの情報の信頼度が高くない場合においても、グローバル探索処理により、ランドマークの発見確率を向上させることができる。

≪第１変形例≫
次に、第２実施形態の第１変形例について、説明する。

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

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

図１０は、第２実施形態の第１変形例のランドマーク予測部５Ｂの概略構成図である。

図１１は、第２実施形態の第１変形例のランドマーク検出部２Ｂの概略構成図である。

本変形例の移動体制御装置２０００Ｂでは、第２実施形態の移動体制御装置２０００において、ランドマーク検出部２Ａをランドマーク検出部２Ｂに置換し、ランドマーク予測部５Ａをランドマーク予測部５Ｂに置換した構成を有している。

ランドマーク予測部５Ｂは、図１０に示すように、ランドマーク予測部５Ａにおいて、自己位置信頼度取得部５３を追加した構成を有している。

自己位置信頼度取得部５３は、記憶部４から読み出された時刻ｔ−１におけるデータＳＴ_ｔ−１を入力する。自己位置信頼度取得部５３は、ロボットＲｂｔ１の内部状態データＸ_ｔ−１から、ロボットＲｂｔ１の内部状態データの信頼度Ｂｌｆ＿ｒｂｔを取得し、取得したロボットＲｂｔ１の内部状態データの信頼度Ｂｌｆ＿ｒｂｔをランドマーク検出部２Ａに出力する。なお、信頼度Ｂｌｆ＿ｒｂｔは、例えば、ロボットＲｂｔ１の分散共分散行列Ｖ＿ｍｔｘ_ｔ−１（Ｒｂｔ１）の対角成分の和、すなわち、ロボットＲｂｔ１の時刻ｔ−１におけるｘ座標位置についての分散値およびロボットＲｂｔ１の時刻ｔ−１におけるｙ座標位置についての分散値の和に対応した値に設定される。なお、ロボットＲｂｔ１の信頼度Ｂｌｆ＿ｒｂｔの算出方法は、上記に限定されることなく、ロボットＲｂｔ１の内部状態データの推定精度を表す値を取得する方法であれば、他の方法であってもよい。

ランドマーク検出部２Ｂは、図１１に示すように、ランドマーク検出部２Ａにおいて、ランドマーク探索部２３をランドマーク探索部２３Ａに置換し、ランドマーク検出部２において、第２比較器２４と、検出状況取得部２５とを追加した構成を有している。

第２比較器２４は、ランドマーク予測部５Ａの自己位置信頼度取得部５３から出力されるロボットＲｂｔ１の内部状態データの信頼度Ｂｌｆ＿ｒｂｔと、しきい値Ｔｈ２とを入力とし、両者の大小を比較し、比較結果を示す信号Ｄｅｔ２をランドマーク探索部２３Ａに出力する。

ランドマーク探索部２３Ａは、ランドマーク探索部２３において、さらに、第２比較器２４から出力される比較結果を示す信号Ｄｅｔ２と、検出状況取得部２５から出力される信号Ｄｅｔ３とを入力する構成を有している。ランドマーク探索部２３Ａは、ランドマーク探索部２３と同様の処理を実行し、さらに、信号Ｄｅｔ２および／または信号Ｄｅｔ３に基づいて、ランドマークを検出するための処理を、ローカル探索処理、および、グローバル探索処理のいずれにするかを決定する。

検出状況取得部２５は、ランドマーク探索部２３Ａから出力されるランドマーク検出信号ＬＭ_ｔを入力し、ランドマーク検出信号ＬＭ_ｔに基づいて、各ランドマークの検出状況を取得する。検出状況取得部２５は、例えば、過去の所定の期間において、各ランドマークの検出された回数や各ランドマークの位置に関する情報の推定精度を示す情報を含めた信号を検出信号Ｄｅｔ３として生成する。そして、検出状況取得部２５は、生成した検出信号Ｄｅｔ３をランドマーク探索部２３Ａに出力する。

以上のように構成された移動体制御装置２０００Ａの動作について、説明する。なお、移動体制御装置２０００と同様の部分については、説明を省略する。

自己位置信頼度取得部５３では、ロボットＲｂｔ１の内部状態データＸ_ｔ−１から、ロボットＲｂｔ１の内部状態データの信頼度Ｂｌｆ＿ｒｂｔが取得され、取得されたロボットＲｂｔ１の内部状態データの信頼度Ｂｌｆ＿ｒｂｔは、自己位置信頼度取得部５３からランドマーク検出部２Ｂの第２比較器２４に出力される。

ランドマーク検出部２Ｂの第２比較器２４では、ランドマーク予測部５Ｂの自己位置信頼度取得部５３から出力されるロボットＲｂｔ１の内部状態データの信頼度Ｂｌｆ＿ｒｂｔと、しきい値Ｔｈ２とが比較される。

具体的には、
（１）Ｂｌｆ＿ｒｂｔ＜Ｔｈ２の場合、Ｄｅｔ２＝１に設定され、
（２）Ｂｌｆ＿ｒｂｔ≧Ｔｈ２の場合、Ｄｅｔ２＝０に設定される。

そして、上記比較結果を示す信号Ｄｅｔ２が、第２比較器２４からランドマーク探索部２３Ａに出力される。

ランドマーク探索部２３Ａでは、第２実施形態のランドマーク探索部２３と同様の処理に加えて、信号Ｄｅｔ２に基づいて、ランドマークを検出するための処理を、ローカル探索処理、および、グローバル探索処理のいずれにするかを決定する。

具体的には、ランドマーク探索部２３Ａは、Ｄｅｔ２＝０である場合、ロボットＲｂｔ１の内部状態データの信頼度が低いと判断し、ランドマークを検出するための処理をグローバル探索処理とする。一方、Ｄｅｔ２＝１である場合、ランドマーク探索部２３Ａは、第２実施形態のランドマーク探索部２３と同様に、ランドマークごとに、信号Ｄｅｔ１に基づいて、ローカル探索処理、および、グローバル探索処理のいずれの処理を実行するか決定し、決定された処理によりランドマークの探索処理を実行する。

さらに、ランドマーク探索部２３Ａでは、検出信号Ｄｅｔ３に基づいて、ランドマークを検出するための処理を、ローカル探索処理、および、グローバル探索処理のいずれにするかを決定してもよい。

ランドマーク探索部２３Ａでは、検出状況取得部２５から出力される各ランドマークの検出状況を示す信号Ｄｅｔ３により、過去の所定の期間において、各ランドマークの検出された回数や各ランドマークの位置に関する情報の推定精度を把握することができる。したがって、検出信号Ｄｅｔ３により、所定のランドマークが一定期間以上、ランドマーク探索部２３Ａによる探索処理により発見（検出）されていない場合、ランドマーク探索部２３Ａは、当該ランドマークについての情報の信頼度が低いと判定し、当該ランドマークについて、グローバル探索処理を実行することを決定し、グローバル探索処理により当該ランドマークの探索処理を実行するようにしてもよい。

なお、本変形例の移動体制御装置２０００Ａにおいて、検出状況取得部２５を省略した構成としてもよい。

以上のように、移動体制御装置２０００Ａでは、ランドマークの情報の信頼度およびロボットＲｂｔ１の内部状態データの信頼度に基づいて、ランドマークを検出するための処理を、ローカル探索処理、あるいは、グローバル探索処理にするかを切り替えることができる。そのため、移動体制御装置２０００Ａでは、より高い精度で、ランドマークの検出処理を行うことができる。

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

また、上記実施形態（変形例を含む。）では、移動体（ロボットＲｂｔ１）の位置、ランドマークの位置が、２次元データ（ｘ座標値、ｙ座標値）により特定される場合について説明したが、これに限定されることはなく、移動体（ロボットＲｂｔ１）の位置、ランドマークの位置を、３次元データ（例えば、ｘ座標値、ｙ座標値、ｚ座標値）により特定するようにしてもよい。

また、上記実施形態（変形例を含む。）において、ランドマークＬＭｋ（ｋ：自然数）が写っている可能性が高い画像領域ＡＲｋ_ｔを楕円形の領域として説明したが、これに限定されることはない。ランドマークＬＭｋ（ｋ：自然数）が写っている可能性が高い画像領域ＡＲｋ_ｔは、他の形状の画像領域（例えば、矩形領域、円形領域等）であってもよい。

また、ランドマーク検出部は、ランドマークＬＭｋの探索処理において、ランドマークＬＭｋを発見したら、探索処理を終了するようにし、それ以上、探索領域として設定した画像領域ＡＲｋ_ｔを探索する処理を実行しないようにしてもよい。このようにすることで、探索処理の高速化を実現することができる。

また、ランドマーク検出部は、ランドマークＬＭｋの探索処理において、ランドマークＬＭｋを発見できなかった場合、探索領域として設定した画像領域ＡＲｋ_ｔ以外の画像領域をさらに探索するようにしてもよい。

また、上記実施形態（変形例を含む。）において、ロボットＲｂｔ１の位置の推定精度、ランドマークの位置の推定精度を、分散共分散行列を用いて取得する場合について説明したが、ロボットＲｂｔ１の位置の推定精度、ランドマークの位置の推定精度を、位置のばらつきを示す他の指標により取得するようにしてもよい。

また、上記実施形態（変形例を含む。）において、ロボットＲｂｔ１の位置の推定精度、ランドマークの位置の推定精度を、分散共分散行列の対角成分（ｘ座標値の分散値とｙ座標値の分散値に相当する成分）を用いて取得する場合について説明したが、これに限定されることはなく、例えば、分散共分散行列の一部または全ての要素を用いて、ロボットＲｂｔ１の位置の推定精度、ランドマークの位置の推定精度の指標となる値を算出するようにしてもよい。

また、上記実施形態（変形例を含む。）では、ロボットＲｂｔ１の位置の推定処理、および、ランドマークの位置の推定処理において、パーティクルフィルタを用いた場合を例に説明したが、これに限定されることはなく、カルマンフィルタ等の他の時系列フィルタを用いて処理を実行してもよい。また、パーティクルのリサンプリングにおいて、ランダムウォークのダイナミクスを仮定したガウシアンノイズ（ガウス分布に従うノイズ）を重畳させることで、予測処理後のパーティクルの集合を取得することを想定しているが、これに限定されることはなく、ガウス分布以外の分布に従うノイズを重畳させることで、予測処理後のパーティクルの集合を取得するようにしてもよい。

上記実施形態および変形例を組み合わせて移動体制御装置を構成するようにしてもよい。

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

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

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

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

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

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

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

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

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

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

Claims (8)

1. ランドマークの情報を用いて表現される環境地図の作成処理と、自己の内部状態の推定処理とを実行しながら、移動する移動体を制御するための移動体制御装置であって、
   観測可能な事象から得られる観測データを取得する観測取得部と、
   現時刻よりも前の時刻における前記移動体の内部状態を示す内部状態データおよび環境地図データに基づいて、現時刻におけるランドマークの情報を予測した情報を含むランドマーク予測信号を生成するランドマーク予測部と、
   前記ランドマーク予測部により生成された前記ランドマーク予測信号に基づいて、現時刻におけるランドマークの情報を検出し、検出結果を示すランドマーク検出信号を取得するランドマーク検出部と、
   前記観測取得部により取得された前記観測データと、前記ランドマーク検出信号とに基づいて、前記移動体の内部状態を推定することで、現時刻の自己内部状態推定データを取得するとともに、前記ランドマーク検出信号に基づいて、前記環境地図を推定することで、現時刻の推定環境地図データを取得する状態推定部と、
   を備える移動体制御装置。
2. 前記観測データは、前記移動体から、前記移動体の周りの環境を撮像した画像データであり、
   前記ランドマーク予測部は、
   現時刻よりも前の時刻における前記移動体の内部状態を示す内部状態データおよび環境地図データに基づいて、現時刻において取得された前記画像データにより形成される画像上において、前記ランドマークが存在すると予測される画像領域である予測画像領域を算出し、算出した予測画像領域についての情報を含む前記ランドマーク予測信号を生成し、
   前記ランドマーク検出部は、
   前記ランドマーク予測部により算出された前記予測画像領域の中心領域に含まれる画像上の画素位置を探索の初期位置として、現時刻の前記画像データにより形成される画像の前記予測画像領域内を、前記初期位置からの画像上の距離が大きくなるように、探索することで、前記ランドマークに相当する画像領域を検出する処理であるローカル探索処理を実行する、
   請求項１に記載の移動体制御装置。
3. ランドマーク予測部は、
   現時刻よりも前の時刻において取得した前記ランドマークの位置情報の推定精度の情報を、前記ランドマーク予測信号に含め、
   前記ランドマーク検出部は、
   前記ランドマーク予測信号に基づいて、現時刻よりも前の時刻において取得した前記ランドマークの位置情報の推定精度が所定の値よりも低いと判定した場合、現時刻の前記画像データにより形成される画像内を探索することで、前記ランドマークに相当する画像領域を検出する処理であるグローバル探索処理を実行する、
   請求項２に記載の移動体制御装置。
4. ランドマーク予測部は、
   現時刻よりも前の時刻において取得した前記移動体の内部状態データの推定精度の情報を、前記ランドマーク予測信号に含め、
   前記ランドマーク検出部は、
   前記ランドマーク予測信号に基づいて、現時刻よりも前の時刻において取得した前記移動体の内部状態データの推定精度が所定の値よりも低いと判定した場合、現時刻の前記画像データにより形成される画像内を探索することで、前記ランドマークに相当する画像領域を検出する処理であるグローバル探索処理を実行する、
   請求項２または３に記載の移動体制御装置。
5. 前記ランドマーク検出部は、
   所定の期間において、前記ランドマークの探索処理において、前記ランドマークを発見した回数であるランドマーク発見回数値を保持する検出状況取得部を備え、
   前記ランドマーク検出部は、
   前記検出状況取得部に保持されている前記ランドマーク発見回数値が所定の値よりも大きい場合、前記ローカル探索処理を実行する、
   請求項２から４のいずれかに記載の移動体制御装置。
6. 前記ランドマーク検出部は、
   所定の期間において、前記ランドマークの探索処理において、前記ランドマークを発見されたか否かを示す情報であるランドマーク発見情報を保持する検出状況取得部を備え、
   前記ランドマーク検出部は、
   前記検出状況取得部に保持されている前記ランドマーク発見情報に基づいて、前記ランドマークが所定の期間発見されなかった場合、現時刻の前記画像データにより形成される画像内を探索することで、前記ランドマークに相当する画像領域を検出する処理であるグローバル探索処理を実行する、
   請求項２から４のいずれかに記載の移動体制御装置。
7. ランドマークの情報を用いて表現される環境地図の作成処理と、自己の内部状態の推定処理とを実行しながら、移動する移動体を制御するための移動体制御方法をコンピュータに実行させるためのプログラムであって、
   観測可能な事象から得られる観測データを取得する観測取得ステップと、
   現時刻よりも前の時刻における前記移動体の内部状態を示す内部状態データおよび環境地図データに基づいて、現時刻におけるランドマークの情報を予測した情報を含むランドマーク予測信号を生成するランドマーク予測ステップと、
   前記ランドマーク予測ステップにより生成された前記ランドマーク予測信号に基づいて、現時刻におけるランドマークの情報を検出し、検出結果を示すランドマーク検出信号を取得するランドマーク検出ステップと、
   前記観測取得ステップにより取得された前記観測データと、前記ランドマーク検出信号とに基づいて、前記移動体の内部状態を推定することで、現時刻の自己内部状態推定データを取得するとともに、前記ランドマーク検出信号に基づいて、前記環境地図を推定することで、現時刻の推定環境地図データを取得する状態推定ステップと、
   を備える移動体制御方法をコンピュータに実行させるためのプログラム。
8. ランドマークの情報を用いて表現される環境地図の作成処理と、自己の内部状態の推定処理とを実行しながら、移動する移動体を制御するための移動体制御装置に用いられる集積回路であって、
   観測可能な事象から得られる観測データを取得する観測取得部と、
   現時刻よりも前の時刻における前記移動体の内部状態を示す内部状態データおよび環境地図データに基づいて、現時刻におけるランドマークの情報を予測した情報を含むランドマーク予測信号を生成するランドマーク予測部と、
   前記ランドマーク予測部により生成された前記ランドマーク予測信号に基づいて、現時刻におけるランドマークの情報を検出し、検出結果を示すランドマーク検出信号を取得するランドマーク検出部と、
   前記観測取得部により取得された前記観測データと、前記ランドマーク検出信号とに基づいて、前記移動体の内部状態を推定することで、現時刻の自己内部状態推定データを取得するとともに、前記ランドマーク検出信号に基づいて、前記環境地図を推定することで、現時刻の推定環境地図データを取得する状態推定部と、
   を備える集積回路。