自己位置推定装置および自己位置推定方法
【課題】高サンプリング周期で、高精度に自己位置を推定することが可能な自己位置推定装置を提供する。
【解決手段】本発明の自己位置推定装置は、移動機構を備え、所定の空間を移動可能な自律移動装置の移動機構による動作情報を検出する内界センサ、および所定の空間内に関する空間情報を検出する外界センサによる検出結果に基づいて、第１のサンプリング周期で自己位置を推定する第１の自己位置推定部と、第１の自己位置推定部による自己位置推定結果と、内界センサにより検出された自律移動装置の動作情報とに基づいて、第１のサンプリング周期よりも高いサンプリング周期で自己位置を推定する第２の自己位置推定部と、を備える。
【解決手段】本発明の自己位置推定装置は、移動機構を備え、所定の空間を移動可能な自律移動装置の移動機構による動作情報を検出する内界センサ、および所定の空間内に関する空間情報を検出する外界センサによる検出結果に基づいて、第１のサンプリング周期で自己位置を推定する第１の自己位置推定部と、第１の自己位置推定部による自己位置推定結果と、内界センサにより検出された自律移動装置の動作情報とに基づいて、第１のサンプリング周期よりも高いサンプリング周期で自己位置を推定する第２の自己位置推定部と、を備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自己位置推定装置および自己位置推定方法に関し、より詳細には、作業空間を自律的に移動可能な自律移動装置の自己位置推定装置および自己位置推定方法に関する。
【背景技術】
【０００２】
近年、自律的に移動し、様々な作業を行うロボットが実現されつつある。このような自律移動型ロボットには、例えば、原子力プラント等におけるメンテナンス作業や災害現場での救助作業、宇宙空間での作業等のように、人間の立ち入りが困難な環境での作業の遂行が期待されている。
【０００３】
一方で、自律移動ロボットを家庭内へ導入し、家庭内でユーザの介助を行わせることについても、期待が高まっている。例えば、高齢者や車椅子利用者の生活支援として、ユーザにとっては身体的に困難を伴う作業を自律移動型ロボットに代行させることで、ユーザの負荷を軽減することができる。
【０００４】
ロボットが部屋等の所定の空間内を自律的に移動できるためには、自己の存在する環境を認識して空間内における自己位置を推定し、自己の移動経路を特定できることが必要である。自己位置を推定するための手法としては、例えば、下記非特許文献１には、カメラの位置や姿勢とカメラの画像に映る特徴点の位置とを同時に推定可能なＳＬＡＭ（Simultaneous Localization And Mapping）とよばれる技術を応用して、実空間内に存在する物体の３次元位置を表現する環境マップを動的に生成する手法が記載されている。なお、単眼カメラを用いたＳＬＡＭ技術の基本的な原理は、下記非特許文献１において説明されている。
【０００５】
従来にも、上記のＳＬＡＭを用いた手法やその他の手法を用いて、高精度な自己位置推定を行う方法はあった。例えば、特許文献１には、複数の全方位カメラを用いて移動ロボットの位置を認識する方法が開示されている。また、特許文献２には、環境内に配置されたマーカを検出することでロボットの位置を認識することの可能な移動ロボットが開示されている。特許文献３には、姿勢回転偏差の時間的変化量を用いて自己位置を推定する方法が開示されている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２００８−２２９８３４号公報
【特許文献２】特開２００６−３４６７６７号公報
【特許文献３】特許第４２４６６９６号公報
【非特許文献】
【０００７】
【非特許文献１】Andrew J.Davison, “Real-Time Simultaneous Localization and Mapping with a Single Camera”, Proceedings of the 9th IEEE International Conference on Computer Vision Volume 2, 2003, pp.1403-1410
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかし、いずれの方法も、例えば特許文献１のようにセンサの特性上、低いサンプリング周期でしか情報を取得できなかったり、アルゴリズムの特性上、演算に多くの時間が必要とするものであったりした。このため、これらの方法により自己位置推定を行った場合、遅れを伴い、低いサンプリング周期でしか自己位置推定値を取得することができないという問題があった。一方、ロボットの移動機構から検出される駆動量を用いて、比較的高いサンプリング周期で自己位置推定値を取得することも可能である。かかる方法では、ロボットの初期位置からの差に基づき自己位置を推定するため、移動するにつれて初期位置からの誤差が増加し、高精度な自己位置推定をすることができない。また、移動機構のスリップ等によっても、検出値と実際の動きとの差が生じて、推定精度が低下する可能性がある。
【０００９】
また、ロボットの運動制御等のように、１ｍｓ、あるいはそれ以下の周期で演算を行うシステムの制御において、上記方法による自己位置推定値をそのまま用いると、不連続な値として入力されてしまい、制御精度を阻害する要因となってしまう。さらに、自己位置推定値をもとに、環境物体を認識する場合においては、自己位置推定値の更新が遅れると正確な環境物体の位置を取得することができなくなる。
【００１０】
そこで、本発明は、上記問題に鑑みてなされたものであり、本発明の目的とするところは、高サンプリング周期で、高精度に自己位置を推定することが可能な、新規かつ改良された自己位置推定装置および自己位置推定方法を提供することにある。
【課題を解決するための手段】
【００１１】
上記課題を解決するために、本発明のある観点によれば、移動機構を備え、所定の空間を移動可能な自律移動装置の移動機構による動作情報を検出する内界センサ、および所定の空間内に関する空間情報を検出する外界センサによる検出結果に基づいて、第１のサンプリング周期で自己位置を推定する第１の自己位置推定部と、第１の自己位置推定部による自己位置推定結果と、内界センサにより検出された自律移動装置の動作情報とに基づいて、第１のサンプリング周期よりも高いサンプリング周期で自己位置を推定する第２の自己位置推定部と、を備える、自己位置推定装置が提供される。
【００１２】
自己位置推定装置は、空間内の空間情報を環境マップとして記憶する記憶部をさらに備えることもできる。第１の自己位置推定部は、環境マップの空間情報、内界センサの検出値および外界センサの検出値に基づいて、自律移動装置の空間における自己位置を推定する位置推定部と、位置推定部により推定された自己位置と、外界センサの検出値とに基づいて、記憶部の環境マップを更新する更新部と、を備える。
【００１３】
外界センサは、自律移動装置から空間内に存在する物体までの距離を取得する距離センサであり、第１の自己位置推定部は、距離センサの位置姿勢と当該距離センサによる測距値に基づき認識された物体の位置とを同時に推定するＳＬＡＭを用いて、環境マップを動的に生成してもよい。
【００１４】
第２の自己位置推定部は、内界センサの検出値に基づく時間発展モデルと、第１の自己位置推定部により推定された自律移動装置の自己位置に基づく観測モデルとを備える拡張カルマンフィルタを用いて、自律移動装置の空間における自己位置を推定してもよい。
【００１５】
第２の自己位置推定部は、第１の自己位置推定部により推定された自己位置結果に内界センサの検出値を加算した加算値を用いて、観測モデルを生成してもよい。
【００１６】
内界センサは、自律移動装置の駆動機構の駆動量を検出する位置センサであってもよい。
【００１７】
第１のサンプリング周期は、外界センサの検出周期とし、第２のサンプリング周期は、内界センサの検出周期としてもよい。
【００１８】
また、上記課題を解決するために、本発明の別の観点によれば、移動機構を備え、所定の空間を移動可能な自律移動装置の移動機構による動作情報を検出する内界センサ、および所定の空間内に関する空間情報を検出する外界センサによる検出結果に基づいて、第１の自己位置推定部によって、第１のサンプリング周期で自己位置を推定するステップと、第２の自己位置推定部による自己位置推定結果と、内界センサにより検出された自律移動装置の動作情報とに基づいて、第２の自己位置推定部によって、第１のサンプリング周期よりも高いサンプリング周期で自己位置を推定するステップと、を含む、自己位置推定方法が提供される。
【発明の効果】
【００１９】
以上説明したように本発明によれば、高サンプリング周期で、高精度に自己位置を推定することが可能な、自己位置推定装置および自己位置推定方法を提供することができる。
【図面の簡単な説明】
【００２０】
【図１】本発明の実施形態に係る自己位置推定装置による自律移動ロボットの自己位置推定の概略を説明する説明図である。
【図２】同実施形態に係る自己位置推定装置の機能構成を示すブロック図である。
【図３】同実施形態に係る自己位置推定装置による自己位置推定方法を示すフローチャートである。
【図４】同実施形態に係る自己位置推定装置による自己位置推定方法を示すシーケンス図である。
【図５】ＥＫＦ自己位置推定部による自己位置推定値の算出手段を示す説明図である。
【図６】同実施形態に係る自己位置推定装置のハードウェア構成の一例を示すブロック図である。
【発明を実施するための形態】
【００２１】
以下に添付図面を参照しながら、本発明の好適な実施の形態について詳細に説明する。なお、本明細書及び図面において、実質的に同一の機能構成を有する構成要素については、同一の符号を付することにより重複説明を省略する。
【００２２】
なお、説明は以下の順序で行うものとする。
１．自己位置推定装置による自己位置推定の概要
２．自己位置推定装置の構成
３．自己位置推定方法
４．ハードウェア構成例
【００２３】
＜１．自己位置推定装置による自己位置推定の概要＞
まず、図１に基づいて、本実施形態に係る自己位置推定装置１００を用いた空間における自己位置推定の概略について説明する。なお、図１は、本実施形態に係る自己位置推定装置１００による自律移動ロボット１０の自己位置推定の概略を説明する説明図である。
【００２４】
本実施形態に係る自己位置推定装置１００は、自己の存在する空間において自己の位置を推定する装置である。例えば、図１に示すように、部屋に存在する自律移動型ロボット（以下、単に「ロボット」とも称する。）１０が、部屋のどの位置に存在するかを自己位置推定装置１００によって推定することができる。自律移動型ロボット１０は、移動機構（例えば、図２に示す車輪１４）と、空間を認識するための物体までの距離（測距値）を取得する距離センサ（例えば、図２に示すレーザレンジファインダ１２）とを備えている。ロボット１０は、移動機構の駆動量と取得した測距値とを用いて自己位置推定装置１００により自己位置を推定することで、部屋を自律的に移動することが可能となる。
【００２５】
また、自己位置推定装置１００は、距離センサにより認識された物体情報に基づいて、部屋の状況を表す環境マップを生成する。環境マップには、机１やタンス３等のような、部屋内で認識された物体の位置や形状、重量等の物体に関する情報が記憶されている。ロボット１０は、このような環境マップを参照することで、自己の存在する空間の状態を認識して、目的に沿った移動経路を決定することができる。
【００２６】
このとき、ロボット１０の動作の精度を高めるためには、自己の存在する空間における自己位置を正しく認識し、ロボット１０を動作させるための制御量を正確に算出する必要がある。ロボット１０の運動制御や自己位置推定値に基づく物体認識においては、高サンプリング周期で制御量を算出する必要があるため、自己位置推定値の更新も高サンプリング周期で行う必要がある。本実施形態に係る自己位置推定装置１００は、このようなロボット１０の動作制御にも適用可能な、高サンプリング周期で、高精度に自己位置推定値を算出できるものである。以下、図２〜図５に基づいて、本実施形態に係る自己位置推定装置１００およびこれによる自己位置推定方法について詳細に説明する。
【００２７】
＜２．自己位置推定装置の構成＞
まず、図２に基づいて、本実施形態に係る自己位置推定装置１００の機能構成について説明する。なお、図２は、本実施形態に係る自己位置推定装置１００の機能構成を示すブロック図である。
【００２８】
自己位置推定装置１００は、図２に示すように、車輪エンコーダ取得部１１０と、ＳＬＡＭ自己位置推定部１２０と、環境マップ記憶部１３０と、ＥＫＦ自己位置推定部１４０と、運動制御部１５０とを備える。
【００２９】
車輪エンコーダ取得部１１０は、自律移動型ロボット１０の移動機構による動作情報を検出する内界センサである。本実施形態に係る自律移動型ロボット１０は、移動機構として複数の車輪１４を備えている。各車輪１４には、車輪１４を回転駆動するアクチュエータの回転を検出するエンコーダが設けられている。車輪エンコーダ取得部１１０は、車輪１４に設けられたエンコーダの検出値（エンコーダ値）を取得する。そして、車輪エンコーダ取得部１１０は、検出値をＳＬＡＭ自己位置推定部１２０およびＥＫＦ自己位置推定部１４０へ出力する。
【００３０】
ＳＬＡＭ自己位置推定部１２０は、上述したＳＬＡＭを利用して、環境マップ、内界センサおよび外界センサの検出結果に基づき、自己位置推定値を算出する。ＳＬＡＭ自己位置推定部１２０は、自己位置推定を行う位置推定部１２２と、環境マップ記憶部１３０に記憶された環境マップを更新する環境マップ更新部１２４とからなる。
【００３１】
位置推定部１２２は、まず、内界センサである車輪１４に設けられたエンコーダの値を取得する車輪エンコーダ取得部１１０の取得値から車輪速度を算出し、解析的に自己位置を更新する。その後、位置推定部１２２は、外界センサであるレーザレンジファインダ１２により検出された測距値と、環境マップ記憶部１３０の環境マップとのマッチングを行い、ＳＬＡＭ自己位置推定部１２０における最終的な自己位置推定値を算出する。位置推定部１２２による自己位置推定は、低いサンプリング周期、例えば１００ｍｓで自己位置を推定する。位置推定部１２２により算出された自己位置推定値は、環境マップ更新部１２４およびＥＫＦ自己位置推定部１４０へ出力される。
【００３２】
環境マップ更新部１２４は、レーザレンジファインダ１２により検出された測距値と、位置推定部１２２により推定された自己位置推定値とに基づき、環境マップを更新する。環境マップ更新部１２４は、入力されたレーザレンジファインダ１２の測距値および自己位置推定値とから、空間内の物体の位置姿勢を算出し、環境マップ記憶部１３０の環境マップを更新する。
【００３３】
環境マップ記憶部１３０は、実空間内に存在する物体の３次元位置を表現する環境マップを記憶する。環境マップは、例えば、レーザレンジファインダ１２やカメラ（図示せず。）によって撮影された画像から認識された物体（例えば、机１やタンス３等）の情報を記憶する。物体の情報としては、例えば、三次元形状や位置、姿勢情報等がある。
【００３４】
ＥＫＦ自己位置推定部１４０は、ＳＬＡＭ自己位置推定部１２０により算出された自己位置推定値と、内界センサである車輪１４に設けられたエンコーダの値を取得する車輪エンコーダ取得部１１０の取得値とに基づいて、自己位置推定値を算出する。ＥＫＦ自己位置推定部１４０は、拡張カルマンフィルタ（Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ；ＥＫＦ）を用いて、高サンプリング周期で高精度な自己位置推定を行う。
【００３５】
拡張カルマンフィルタは、内界センサの検出値に基づく時間発展モデルと、ＳＬＡＭ自己位置推定部１２０において算出された自己位置推定値に基づく観測モデルとを有する。ＥＫＦ自己位置推定部１４０は、時間発展モデルと観測モデルとの同定を行うことにより、自己位置推定値を算出することができる。ＥＫＦ自己位置推定部１４０での自己位置推定は、車輪エンコーダ取得部１１０の取得タイミング（例えば、１ｍｓ周期）で行われる。なお、ＥＫＦ自己位置推定部１４０による自己位置推定処理の詳細については後述する。ＥＫＦ自己位置推定部１４０により算出された自己位置推定値は、運動制御部１５０に出力される。
【００３６】
運動制御部１５０は、ＥＫＦ自己位置推定部１４０により算出された自己位置推定値を用いて、自律移動型ロボット１０の運動制御を行う。運動制御部１５０は、ロボット１０に所望の動作を行わせるための運動制御値を算出し、当該運動制御値に基づいてロボット１０の車輪１４を回転するモータ等の駆動機構（図示せず。）を駆動させる。
【００３７】
＜３．自己位置推定方法＞
次に、図３〜図５に基づいて、本実施形態に係る自己位置推定装置１００による自己位置推定方法について説明する。図３は、本実施形態に係る自己位置推定装置１００による自己位置推定方法を示すフローチャートである。図４は、本実施形態に係る自己位置推定装置１００による自己位置推定方法を示すシーケンス図である。図５は、ＥＫＦ自己位置推定部１４０による自己位置推定値の算出手段を示す説明図である。
【００３８】
自己位置推定装置１００による自己位置推定方法では、図３に示すように、まず、車輪エンコーダ取得部１１０により、車輪１４の回転位置を測定するエンコーダの検出値（エンコーダ値）を取得する（ステップＳ１００）。車輪エンコーダ取得部１１０は、ロボット１０の移動機構である車輪１４の回転位置をエンコーダにより検出し、エンコーダ値としてＳＬＡＭ自己位置推定部１２０およびＥＫＦ自己位置推定部１４０へ出力する。
【００３９】
ここで、図４に示すように、車輪エンコーダ取得部１１０は、高サンプリング周期、例えば１ｍｓでエンコーダ値を取得することができる。車輪エンコーダ取得部１１０は、出力先の自己位置推定の処理周期に応じて、取得したエンコーダ値を出力する。すなわち、本実施形態においては、低サンプリング周期（例えば１００ｍｓ）で自己位置推定を行うＳＬＡＭ自己位置推定部１２０に対しては１００ｍｓごとに、車輪エンコーダ取得部１１０からＳＬＡＭ自己位置推定部１２０へエンコーダ値が出力される。一方、高サンプリング周期（例えば１ｍｓ）で自己位置推定を行うＥＫＦ自己位置推定部１４０に対しては１ｍｓごとに、車輪エンコーダ取得部１１０からＥＫＦ自己位置推定部１４０へエンコーダ値が出力される。
【００４０】
一方、レーザレンジファインダ１２は、ロボット１０から空間内に存在する物体までの距離を測距値として取得する（ステップＳ１１０）。レーザレンジファインダ１２は、エンコーダのように高いサンプリング周期で検出値を取得することができず、例えば図４に示すように、比較的低いサンプリング周期、例えば１００ｍｓ周期で測距値を取得する。レーザレンジファインダ１２は、取得した測距値を、ＳＬＡＭ自己位置推定部１２０へ出力する。
【００４１】
ＳＬＡＭ自己位置推定部１２０は、レーザレンジファインダ１２による測距値が入力されたタイミングで、自己位置推定処理を実行する（ステップＳ１２０）。ＳＬＡＭ自己位置推定部１２０は、図４に示すように、レーザレンジファインダ１２による測距値と、この測距値の測定時点における車輪エンコーダ取得部１１０のエンコーダ値とに基づいて、自己位置推定処理を行う。ＳＬＡＭによる自己位置推定処理は、例えば上記非特許文献１に記載の技術を用いて行うことができる。ＳＬＡＭ自己位置推定部１２０により算出された自己位置推定値は、ＥＫＦ自己位置推定部１４０へ出力される。
【００４２】
そして、ＥＫＦ自己位置推定部１４０は、拡張カルマンフィルタの原理に基づき、ＳＬＡＭ自己位置推定部１２０により算出された自己位置推定値と、車輪エンコーダ取得部１１０によるエンコーダ値とを用いて、自己位置推定値を算出する（ステップＳ１３０）。上述したように、エンコーダ値は高サンプリング周期で取得可能であるが、レーザレンジファインダ１２による測距値はこれより低いサンプリング周期でしか取得することができない。したがって、ＳＬＡＭを用いる場合、高精度に自己位置を推定することができる一方、ロボット１０の運動制御等、リアルタイムが要求される状況においては、より高いサンプリング周期で自己位置を推定できることが望ましい。そこで、ＥＫＦ自己位置推定部１４０は、高サンプリング周期で取得可能なエンコーダ値と、ＳＬＡＭ自己位置推定部１２０により高精度に推定された自己位置推定値とに基づき、高サンプリング周期で高精度な自己位置推定値を取得することを可能とする。
【００４３】
ＥＫＦ自己位置推定部１４０は、拡張カルマンフィルタを用いてロボット１０の自己位置推定値を算出するにあたり、まず、エンコーダ値に基づくオドメトリ出力値を算出する。オドメトリ出力値は、車輪エンコーダ取得部１１０から入力されたロボット１０の車輪１４の回転位置より算出される、車輪１４の速度や角速度等である。ＥＫＦ自己位置推定部１４０は、このオドメトリ出力値を、エンコーダのサンプリング周期で算出することが可能である。そして、ＥＫＦ自己位置推定部１４０は、オドメトリ出力値に基づき拡張カルマンフィルタの時間発展モデルを構築する。
【００４４】
また、ＥＫＦ自己位置推定部１４０は、オドメトリ出力値をＳＡＬＭ自己位置推定部１２０により算出されたＳＬＡＭによる自己位置推定値に加算する処理を行う。図４および図５に示すように、ＥＫＦ自己位置推定部１４０は、オドメトリ出力値とＳＬＡＭによる自己位置推定値とを融合し、自己位置推定値を算出するが、これらの値の取得タイミングは相違する。このため、ＳＬＡＭによる自己位置推定値をそのまま利用すると、かかる値が不連続な値として拡張カルマンフィルタに入力されることになる。
【００４５】
そこで、ＳＬＡＭ自己位置推定部１２０による自己位置推定値に、ＥＫＦ自己位置推定部１４０による自己位置推定処理に用いるエンコーダのオドメトリ出力値と同一の値を、ＳＬＡＭ自己位置推定部１２０による自己位置推定値に加算する。これにより、ＳＬＡＭ自己位置推定部１２０により推定された時点の自己位置推定値からのロボット１０の移動を含めた値が、エンコーダのサンプリング周期と同じ周期（例えば、１ｍｓ）で取得される。この取得値に基づき、観測モデルが構築される。したがって、ＥＫＦ自己位置推定部１４０は、同一タイミングで取得された値に基づくモデルより、自己位置推定値を算出することができる。
【００４６】
ＥＫＦ自己位置推定部１４０は、内界センサである車輪１４の検出値に基づく時間発展モデルと、ＳＬＡＭ自己位置推定部１２０において算出された自己位置推定値に基づく観測モデルとを有する拡張カルマンフィルタを用いて、自己位置推定値を算出する。拡張カルマンフィルタは、誤差のある観測値を用いて動的システムの状態を推定するためのオブザーバである。拡張カルマンフィルタは、前時刻の推定状態から現時刻の推定状態を算出する予測フェーズと現時刻の観測結果を用いて推定値を補正する更新フェーズとからなり、入力値ｘｔ−１、Ｐｔ−１、ｕｔ、ｙｔから出力値ｘｔ、Ｐｔを得る。予測フェーズおよび更新フェーズは、公知の下記数式１〜５によって表される。
【００４７】
【数１】
【００４８】
ここで、ｘは状態推定値、Ｐは誤差の共分散、ｕは制御値、ｙは観測値、ｇ（）は状態予測、ｈ（）は観測予測、Ａは時間発展モデルのヤコビ行列、Ｃは観測モデルのヤコビ行列、Ｒは時間発展モデルの共分散、Ｑは観測モデルの共分散、Ｋはカルマンゲインを示す。
【００４９】
具体的には、ＥＫＦ自己位置推定部１４０は、図５に示すように、車輪エンコーダ取得部１１０によるエンコーダ値から算出される車輪速度（オドメトリ出力値）に基づく時間発展モデルにより、オドメトリ自己位置推定を行う。時間発展モデルは、下記数式６により表される。
【００５０】
【数２】
【００５１】
ここで、ｘはｘ軸方向における位置、ｙはｙ軸方向における位置、θは旋回角度であり、ｘ＝（ｘ、ｙ、θ）Ｔである。また、ｖは並進速度、ｗは回転速度、εは分散を示す。すなわち、数式６では、オドメトリ出力値に基づき推定された時刻ｔにおける自己位置推定値を表している。
【００５２】
また、ＥＫＦ自己位置推定部１４０は、ＳＬＡＭ自己位置推定部１２０による自己位置推定値よりロボット１０の位置および方位を取得し、推定されたロボット１０の位置および方位、そして車輪速度に基づく観測モデルにより、ＳＬＡＭ自己位置推定を行う。観測モデルは、下記数式７により表される。
【００５３】
【数３】
【００５４】
ここで、ｕは直前のＳＬＡＭ自己位置推定時刻、δは分散を示す。すなわち、数式７では、ＳＬＡＭ自己位置推定値に基づき推定された時刻ｔにおける自己位置推定値を表している。ここで、数式７の右辺第１項括弧内の２項よりわかるように、時刻ｔ−ｕにおいて算出されたＳＬＡＭ自己位置推定値（左側の項）に、時刻ｔ−ｕ＋１から現時刻ｔまでの車輪１４による移動距離（右側の項）が加算されている（図５の積分器における加算処理）。このようにして、拡張カルマンフィルタの２つのモデルの同期を取り、推定精度を高めるようにしている。
【００５５】
ＥＫＦ自己位置推定部１４０は、数式６および７より表されるモデルを用いて、数式１〜５に基づきロボット１０の空間内における推定位置を算出する。このように、ＥＫＦ自己位置推定部１４０は、高精度のＳＬＡＭによる自己位置推定値と、高いサンプリング周期で取得されるオドメトリ出力値とを用いて、高精度かつ高サンプリング周期で自己位置を推定することができる。
【００５６】
その後、ＥＫＦ自己位置推定部１４０は、算出した自己位置推定値を運動制御部１５０へ出力する。運動制御部１５０は、ＥＫＦ自己位置推定部１４０により推定された自己位置に基づき、ロボット１０が移動・作業するための駆動機構の制御値を算出し、ロボット１０の制御を行う（ステップＳ１４０）。
【００５７】
以上、本実施形態に係る自己位置推定装置１００による自己位置推定方法について説明した。かかる方法によれば、低サンプリング周期でのＳＬＡＭを用いた自己位置推定結果と、高サンプリング周期で取得可能な車輪エンコーダ取得部１１０からのオドメトリ出力とを拡張カルマンフィルタによって融合する。これにより、低サンプリング周期で推定された自己位置推定結果を、より実位置に近いものに補正することができ、高サンプリング周期で高精度な自己位置推定を行うことが可能となる。
【００５８】
また、本実施形態の自己位置推定方法では、ＳＬＡＭによる自己位置推定結果をＥＫＦ自己位置推定部１４０に入力する前に、車輪エンコーダ取得部１１０からのオドメトリ出力を加算する。これにより、ＥＫＦ自己位置推定部１４０による自己位置推定結果が不連続な値とならず、この自己位置推定値を用いたロボット１０の運動制御等の制御を高精度に行うことが可能となる。
【００５９】
＜４．ハードウェア構成例＞
本実施形態にかかる自己位置推定装置１００による一部の処理は、ハードウェアにより実行させることもでき、ソフトウェアによって実行させることもできる。この場合、自己位置推定装置１００は、図６に示すようなコンピュータとして構成することもできる。以下、図６に基づいて、本実施形態に係る自己位置推定装置１００の一ハードウェア構成例について説明する。
【００６０】
本実施形態にかかる入力ユニットを構成する自己位置推定装置１００は、上述したように、コンピュータ等の情報処理装置により実現することができ、マニピュレータ１０を備える自律分散型ロボットに設けることができる。自己位置推定装置１００は、図６に示すように、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１０１と、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）１０２と、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）１０３と、ホストバス１０４ａとを備える。また、自己位置推定装置１００は、ブリッジ１０４と、外部バス１０４ｂと、インタフェース１０５と、入力装置１０６と、出力装置１０７と、ストレージ装置（ＨＤＤ）１０８と、ドライブ１０９と、接続ポート１１１と、通信装置１１３とを備える。
【００６１】
ＣＰＵ１０１は、演算処理装置および制御装置として機能し、各種プログラムに従って自己位置推定装置１００内の動作全般を制御する。また、ＣＰＵ１０１は、マイクロプロセッサであってもよい。ＲＯＭ１０２は、ＣＰＵ１０１が使用するプログラムや演算パラメータ等を記憶する。ＲＡＭ１０３は、ＣＰＵ１０１の実行において使用するプログラムや、その実行において適宜変化するパラメータ等を一時記憶する。これらはＣＰＵバスなどから構成されるホストバス１０４ａにより相互に接続されている。
【００６２】
ホストバス１０４ａは、ブリッジ１０４を介して、ＰＣＩ（Ｐｅｒｉｐｈｅｒａｌ Ｃｏｍｐｏｎｅｎｔ Ｉｎｔｅｒｃｏｎｎｅｃｔ／Ｉｎｔｅｒｆａｃｅ）バスなどの外部バス１０４ｂに接続されている。なお、必ずしもホストバス１０４ａ、ブリッジ１０４および外部バス１０４ｂを分離構成する必要はなく、一のバスにこれらの機能を実装してもよい。
【００６３】
入力装置１０６は、マウス、キーボード、タッチパネル、ボタン、マイク、スイッチおよびレバーなどユーザが情報を入力するための入力手段と、ユーザによる入力に基づいて入力信号を生成し、ＣＰＵ１０１に出力する入力制御回路などから構成されている。ユーザは、該入力装置１０６を操作することにより、ロボットの自己位置推定装置１００に対して各種のデータを入力したり処理動作を指示したりすることができる。
【００６４】
出力装置１０７は、例えば、液晶ディスプレイ（ＬＣＤ）装置、ＯＬＥＤ（Ｏｒｇａｎｉｃ Ｌｉｇｈｔ Ｅｍｉｔｔｉｎｇ Ｄｉｏｄｅ）装置およびランプなどの表示装置や、スピーカなどの音声出力装置を含む。
【００６５】
ストレージ装置１０８は、自己位置推定装置１００の記憶部の一例であり、データ格納用の装置である。ストレージ装置１０８は、記憶媒体、記憶媒体にデータを記録する記録装置、記憶媒体からデータを読み出す読出し装置および記憶媒体に記録されたデータを削除する削除装置などを含んでもよい。ストレージ装置１０８は、例えば、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）で構成される。このストレージ装置１０８は、ハードディスクを駆動し、ＣＰＵ１０１が実行するプログラムや各種データを格納する。
【００６６】
ドライブ１０９は、記憶媒体用リーダライタであり、自己位置推定装置１００に内蔵、あるいは外付けされる。ドライブ１０９は、装着されている磁気ディスク、光ディスク、光磁気ディスク、または半導体メモリ等のリムーバブル記録媒体に記録されている情報を読み出して、ＲＡＭ１０３に出力する。
【００６７】
接続ポート１１１は、外部機器と接続されるインタフェースであって、例えばＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）などによりデータ伝送可能な外部機器との接続口である。また、通信装置１１３は、例えば、通信網１５に接続するための通信デバイス等で構成された通信インタフェースである。また、通信装置１１２は、無線ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）対応通信装置であっても、ワイヤレスＵＳＢ対応通信装置であっても、有線による通信を行うワイヤー通信装置であってもよい。
【００６８】
なお、本実施形態に係る自己位置推定装置１００は、必ずしも図６に示すハードウェアを備える必要はない。例えば、入力装置１０６や表示装置１０７、ストレージ装置１０８、ドライブ１０９、接続ポート１１１、通信装置１１３等については、自己位置推定装置１００とは別の装置として、自己位置推定装置１００と接続して使用できるようにしてもよい。
【００６９】
以上、添付図面を参照しながら本発明の好適な実施形態について詳細に説明したが、本発明はかかる例に限定されない。本発明の属する技術の分野における通常の知識を有する者であれば、特許請求の範囲に記載された技術的思想の範疇内において、各種の変更例または修正例に想到し得ることは明らかであり、これらについても、当然に本発明の技術的範囲に属するものと了解される。
【００７０】
例えば、上記実施形態では、ＳＬＡＭによる自己位置推定値と内界センサによるオドメトリ出力値とを、拡張カルマンフィルタを用いて融合したが、本発明はかかる例に限定されない。例えば、拡張カルマンフィルタの代わりに、パーティクルフィルタ等を用いてもよい。なお、パーティクルフィルタを利用すると計算量が莫大に増加するため、拡張カルマンフィルタを用いるのが好適である。
【００７１】
また、上記実施形態では、外界センサとしてレーザレンジファインダ１２を用いたが、本発明はかかる例に限定されず、例えばステレオカメラ等を用いてもよい。
【００７２】
さらに、上記実施形態では、ＳＬＡＭ自己位置推定部１２０による自己位置推定処理を１００ｍｓ周期で行い、ＥＫＦ自己位置推定部１３０による自己位置推定処理を１ｍｓ周期で行ったが、本発明はかかる例に限定されない。ＥＫＦ自己位置推定部１４０のサンプリング周期がＳＬＡＭ自己位置推定部１２０のサンプリング周期よりも高い場合に、本発明は適用可能である。
【符号の説明】
【００７３】
１０ 自律移動型ロボット
１２ レーザレンジファインダ
１４ 車輪
１００ 自己位置推定装置
１１０ 車輪エンコーダ取得部
１２０ ＳＬＡＭ自己位置推定部
１２２ 位置推定部
１２４ 環境マップ更新部
１３０ 環境マップ記憶部
１４０ ＥＫＦ自己位置推定部
１５０ 運動制御部
【技術分野】
【０００１】
本発明は、自己位置推定装置および自己位置推定方法に関し、より詳細には、作業空間を自律的に移動可能な自律移動装置の自己位置推定装置および自己位置推定方法に関する。
【背景技術】
【０００２】
近年、自律的に移動し、様々な作業を行うロボットが実現されつつある。このような自律移動型ロボットには、例えば、原子力プラント等におけるメンテナンス作業や災害現場での救助作業、宇宙空間での作業等のように、人間の立ち入りが困難な環境での作業の遂行が期待されている。
【０００３】
一方で、自律移動ロボットを家庭内へ導入し、家庭内でユーザの介助を行わせることについても、期待が高まっている。例えば、高齢者や車椅子利用者の生活支援として、ユーザにとっては身体的に困難を伴う作業を自律移動型ロボットに代行させることで、ユーザの負荷を軽減することができる。
【０００４】
ロボットが部屋等の所定の空間内を自律的に移動できるためには、自己の存在する環境を認識して空間内における自己位置を推定し、自己の移動経路を特定できることが必要である。自己位置を推定するための手法としては、例えば、下記非特許文献１には、カメラの位置や姿勢とカメラの画像に映る特徴点の位置とを同時に推定可能なＳＬＡＭ（Simultaneous Localization And Mapping）とよばれる技術を応用して、実空間内に存在する物体の３次元位置を表現する環境マップを動的に生成する手法が記載されている。なお、単眼カメラを用いたＳＬＡＭ技術の基本的な原理は、下記非特許文献１において説明されている。
【０００５】
従来にも、上記のＳＬＡＭを用いた手法やその他の手法を用いて、高精度な自己位置推定を行う方法はあった。例えば、特許文献１には、複数の全方位カメラを用いて移動ロボットの位置を認識する方法が開示されている。また、特許文献２には、環境内に配置されたマーカを検出することでロボットの位置を認識することの可能な移動ロボットが開示されている。特許文献３には、姿勢回転偏差の時間的変化量を用いて自己位置を推定する方法が開示されている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２００８−２２９８３４号公報
【特許文献２】特開２００６−３４６７６７号公報
【特許文献３】特許第４２４６６９６号公報
【非特許文献】
【０００７】
【非特許文献１】Andrew J.Davison, “Real-Time Simultaneous Localization and Mapping with a Single Camera”, Proceedings of the 9th IEEE International Conference on Computer Vision Volume 2, 2003, pp.1403-1410
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかし、いずれの方法も、例えば特許文献１のようにセンサの特性上、低いサンプリング周期でしか情報を取得できなかったり、アルゴリズムの特性上、演算に多くの時間が必要とするものであったりした。このため、これらの方法により自己位置推定を行った場合、遅れを伴い、低いサンプリング周期でしか自己位置推定値を取得することができないという問題があった。一方、ロボットの移動機構から検出される駆動量を用いて、比較的高いサンプリング周期で自己位置推定値を取得することも可能である。かかる方法では、ロボットの初期位置からの差に基づき自己位置を推定するため、移動するにつれて初期位置からの誤差が増加し、高精度な自己位置推定をすることができない。また、移動機構のスリップ等によっても、検出値と実際の動きとの差が生じて、推定精度が低下する可能性がある。
【０００９】
また、ロボットの運動制御等のように、１ｍｓ、あるいはそれ以下の周期で演算を行うシステムの制御において、上記方法による自己位置推定値をそのまま用いると、不連続な値として入力されてしまい、制御精度を阻害する要因となってしまう。さらに、自己位置推定値をもとに、環境物体を認識する場合においては、自己位置推定値の更新が遅れると正確な環境物体の位置を取得することができなくなる。
【００１０】
そこで、本発明は、上記問題に鑑みてなされたものであり、本発明の目的とするところは、高サンプリング周期で、高精度に自己位置を推定することが可能な、新規かつ改良された自己位置推定装置および自己位置推定方法を提供することにある。
【課題を解決するための手段】
【００１１】
上記課題を解決するために、本発明のある観点によれば、移動機構を備え、所定の空間を移動可能な自律移動装置の移動機構による動作情報を検出する内界センサ、および所定の空間内に関する空間情報を検出する外界センサによる検出結果に基づいて、第１のサンプリング周期で自己位置を推定する第１の自己位置推定部と、第１の自己位置推定部による自己位置推定結果と、内界センサにより検出された自律移動装置の動作情報とに基づいて、第１のサンプリング周期よりも高いサンプリング周期で自己位置を推定する第２の自己位置推定部と、を備える、自己位置推定装置が提供される。
【００１２】
自己位置推定装置は、空間内の空間情報を環境マップとして記憶する記憶部をさらに備えることもできる。第１の自己位置推定部は、環境マップの空間情報、内界センサの検出値および外界センサの検出値に基づいて、自律移動装置の空間における自己位置を推定する位置推定部と、位置推定部により推定された自己位置と、外界センサの検出値とに基づいて、記憶部の環境マップを更新する更新部と、を備える。
【００１３】
外界センサは、自律移動装置から空間内に存在する物体までの距離を取得する距離センサであり、第１の自己位置推定部は、距離センサの位置姿勢と当該距離センサによる測距値に基づき認識された物体の位置とを同時に推定するＳＬＡＭを用いて、環境マップを動的に生成してもよい。
【００１４】
第２の自己位置推定部は、内界センサの検出値に基づく時間発展モデルと、第１の自己位置推定部により推定された自律移動装置の自己位置に基づく観測モデルとを備える拡張カルマンフィルタを用いて、自律移動装置の空間における自己位置を推定してもよい。
【００１５】
第２の自己位置推定部は、第１の自己位置推定部により推定された自己位置結果に内界センサの検出値を加算した加算値を用いて、観測モデルを生成してもよい。
【００１６】
内界センサは、自律移動装置の駆動機構の駆動量を検出する位置センサであってもよい。
【００１７】
第１のサンプリング周期は、外界センサの検出周期とし、第２のサンプリング周期は、内界センサの検出周期としてもよい。
【００１８】
また、上記課題を解決するために、本発明の別の観点によれば、移動機構を備え、所定の空間を移動可能な自律移動装置の移動機構による動作情報を検出する内界センサ、および所定の空間内に関する空間情報を検出する外界センサによる検出結果に基づいて、第１の自己位置推定部によって、第１のサンプリング周期で自己位置を推定するステップと、第２の自己位置推定部による自己位置推定結果と、内界センサにより検出された自律移動装置の動作情報とに基づいて、第２の自己位置推定部によって、第１のサンプリング周期よりも高いサンプリング周期で自己位置を推定するステップと、を含む、自己位置推定方法が提供される。
【発明の効果】
【００１９】
以上説明したように本発明によれば、高サンプリング周期で、高精度に自己位置を推定することが可能な、自己位置推定装置および自己位置推定方法を提供することができる。
【図面の簡単な説明】
【００２０】
【図１】本発明の実施形態に係る自己位置推定装置による自律移動ロボットの自己位置推定の概略を説明する説明図である。
【図２】同実施形態に係る自己位置推定装置の機能構成を示すブロック図である。
【図３】同実施形態に係る自己位置推定装置による自己位置推定方法を示すフローチャートである。
【図４】同実施形態に係る自己位置推定装置による自己位置推定方法を示すシーケンス図である。
【図５】ＥＫＦ自己位置推定部による自己位置推定値の算出手段を示す説明図である。
【図６】同実施形態に係る自己位置推定装置のハードウェア構成の一例を示すブロック図である。
【発明を実施するための形態】
【００２１】
以下に添付図面を参照しながら、本発明の好適な実施の形態について詳細に説明する。なお、本明細書及び図面において、実質的に同一の機能構成を有する構成要素については、同一の符号を付することにより重複説明を省略する。
【００２２】
なお、説明は以下の順序で行うものとする。
１．自己位置推定装置による自己位置推定の概要
２．自己位置推定装置の構成
３．自己位置推定方法
４．ハードウェア構成例
【００２３】
＜１．自己位置推定装置による自己位置推定の概要＞
まず、図１に基づいて、本実施形態に係る自己位置推定装置１００を用いた空間における自己位置推定の概略について説明する。なお、図１は、本実施形態に係る自己位置推定装置１００による自律移動ロボット１０の自己位置推定の概略を説明する説明図である。
【００２４】
本実施形態に係る自己位置推定装置１００は、自己の存在する空間において自己の位置を推定する装置である。例えば、図１に示すように、部屋に存在する自律移動型ロボット（以下、単に「ロボット」とも称する。）１０が、部屋のどの位置に存在するかを自己位置推定装置１００によって推定することができる。自律移動型ロボット１０は、移動機構（例えば、図２に示す車輪１４）と、空間を認識するための物体までの距離（測距値）を取得する距離センサ（例えば、図２に示すレーザレンジファインダ１２）とを備えている。ロボット１０は、移動機構の駆動量と取得した測距値とを用いて自己位置推定装置１００により自己位置を推定することで、部屋を自律的に移動することが可能となる。
【００２５】
また、自己位置推定装置１００は、距離センサにより認識された物体情報に基づいて、部屋の状況を表す環境マップを生成する。環境マップには、机１やタンス３等のような、部屋内で認識された物体の位置や形状、重量等の物体に関する情報が記憶されている。ロボット１０は、このような環境マップを参照することで、自己の存在する空間の状態を認識して、目的に沿った移動経路を決定することができる。
【００２６】
このとき、ロボット１０の動作の精度を高めるためには、自己の存在する空間における自己位置を正しく認識し、ロボット１０を動作させるための制御量を正確に算出する必要がある。ロボット１０の運動制御や自己位置推定値に基づく物体認識においては、高サンプリング周期で制御量を算出する必要があるため、自己位置推定値の更新も高サンプリング周期で行う必要がある。本実施形態に係る自己位置推定装置１００は、このようなロボット１０の動作制御にも適用可能な、高サンプリング周期で、高精度に自己位置推定値を算出できるものである。以下、図２〜図５に基づいて、本実施形態に係る自己位置推定装置１００およびこれによる自己位置推定方法について詳細に説明する。
【００２７】
＜２．自己位置推定装置の構成＞
まず、図２に基づいて、本実施形態に係る自己位置推定装置１００の機能構成について説明する。なお、図２は、本実施形態に係る自己位置推定装置１００の機能構成を示すブロック図である。
【００２８】
自己位置推定装置１００は、図２に示すように、車輪エンコーダ取得部１１０と、ＳＬＡＭ自己位置推定部１２０と、環境マップ記憶部１３０と、ＥＫＦ自己位置推定部１４０と、運動制御部１５０とを備える。
【００２９】
車輪エンコーダ取得部１１０は、自律移動型ロボット１０の移動機構による動作情報を検出する内界センサである。本実施形態に係る自律移動型ロボット１０は、移動機構として複数の車輪１４を備えている。各車輪１４には、車輪１４を回転駆動するアクチュエータの回転を検出するエンコーダが設けられている。車輪エンコーダ取得部１１０は、車輪１４に設けられたエンコーダの検出値（エンコーダ値）を取得する。そして、車輪エンコーダ取得部１１０は、検出値をＳＬＡＭ自己位置推定部１２０およびＥＫＦ自己位置推定部１４０へ出力する。
【００３０】
ＳＬＡＭ自己位置推定部１２０は、上述したＳＬＡＭを利用して、環境マップ、内界センサおよび外界センサの検出結果に基づき、自己位置推定値を算出する。ＳＬＡＭ自己位置推定部１２０は、自己位置推定を行う位置推定部１２２と、環境マップ記憶部１３０に記憶された環境マップを更新する環境マップ更新部１２４とからなる。
【００３１】
位置推定部１２２は、まず、内界センサである車輪１４に設けられたエンコーダの値を取得する車輪エンコーダ取得部１１０の取得値から車輪速度を算出し、解析的に自己位置を更新する。その後、位置推定部１２２は、外界センサであるレーザレンジファインダ１２により検出された測距値と、環境マップ記憶部１３０の環境マップとのマッチングを行い、ＳＬＡＭ自己位置推定部１２０における最終的な自己位置推定値を算出する。位置推定部１２２による自己位置推定は、低いサンプリング周期、例えば１００ｍｓで自己位置を推定する。位置推定部１２２により算出された自己位置推定値は、環境マップ更新部１２４およびＥＫＦ自己位置推定部１４０へ出力される。
【００３２】
環境マップ更新部１２４は、レーザレンジファインダ１２により検出された測距値と、位置推定部１２２により推定された自己位置推定値とに基づき、環境マップを更新する。環境マップ更新部１２４は、入力されたレーザレンジファインダ１２の測距値および自己位置推定値とから、空間内の物体の位置姿勢を算出し、環境マップ記憶部１３０の環境マップを更新する。
【００３３】
環境マップ記憶部１３０は、実空間内に存在する物体の３次元位置を表現する環境マップを記憶する。環境マップは、例えば、レーザレンジファインダ１２やカメラ（図示せず。）によって撮影された画像から認識された物体（例えば、机１やタンス３等）の情報を記憶する。物体の情報としては、例えば、三次元形状や位置、姿勢情報等がある。
【００３４】
ＥＫＦ自己位置推定部１４０は、ＳＬＡＭ自己位置推定部１２０により算出された自己位置推定値と、内界センサである車輪１４に設けられたエンコーダの値を取得する車輪エンコーダ取得部１１０の取得値とに基づいて、自己位置推定値を算出する。ＥＫＦ自己位置推定部１４０は、拡張カルマンフィルタ（Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ；ＥＫＦ）を用いて、高サンプリング周期で高精度な自己位置推定を行う。
【００３５】
拡張カルマンフィルタは、内界センサの検出値に基づく時間発展モデルと、ＳＬＡＭ自己位置推定部１２０において算出された自己位置推定値に基づく観測モデルとを有する。ＥＫＦ自己位置推定部１４０は、時間発展モデルと観測モデルとの同定を行うことにより、自己位置推定値を算出することができる。ＥＫＦ自己位置推定部１４０での自己位置推定は、車輪エンコーダ取得部１１０の取得タイミング（例えば、１ｍｓ周期）で行われる。なお、ＥＫＦ自己位置推定部１４０による自己位置推定処理の詳細については後述する。ＥＫＦ自己位置推定部１４０により算出された自己位置推定値は、運動制御部１５０に出力される。
【００３６】
運動制御部１５０は、ＥＫＦ自己位置推定部１４０により算出された自己位置推定値を用いて、自律移動型ロボット１０の運動制御を行う。運動制御部１５０は、ロボット１０に所望の動作を行わせるための運動制御値を算出し、当該運動制御値に基づいてロボット１０の車輪１４を回転するモータ等の駆動機構（図示せず。）を駆動させる。
【００３７】
＜３．自己位置推定方法＞
次に、図３〜図５に基づいて、本実施形態に係る自己位置推定装置１００による自己位置推定方法について説明する。図３は、本実施形態に係る自己位置推定装置１００による自己位置推定方法を示すフローチャートである。図４は、本実施形態に係る自己位置推定装置１００による自己位置推定方法を示すシーケンス図である。図５は、ＥＫＦ自己位置推定部１４０による自己位置推定値の算出手段を示す説明図である。
【００３８】
自己位置推定装置１００による自己位置推定方法では、図３に示すように、まず、車輪エンコーダ取得部１１０により、車輪１４の回転位置を測定するエンコーダの検出値（エンコーダ値）を取得する（ステップＳ１００）。車輪エンコーダ取得部１１０は、ロボット１０の移動機構である車輪１４の回転位置をエンコーダにより検出し、エンコーダ値としてＳＬＡＭ自己位置推定部１２０およびＥＫＦ自己位置推定部１４０へ出力する。
【００３９】
ここで、図４に示すように、車輪エンコーダ取得部１１０は、高サンプリング周期、例えば１ｍｓでエンコーダ値を取得することができる。車輪エンコーダ取得部１１０は、出力先の自己位置推定の処理周期に応じて、取得したエンコーダ値を出力する。すなわち、本実施形態においては、低サンプリング周期（例えば１００ｍｓ）で自己位置推定を行うＳＬＡＭ自己位置推定部１２０に対しては１００ｍｓごとに、車輪エンコーダ取得部１１０からＳＬＡＭ自己位置推定部１２０へエンコーダ値が出力される。一方、高サンプリング周期（例えば１ｍｓ）で自己位置推定を行うＥＫＦ自己位置推定部１４０に対しては１ｍｓごとに、車輪エンコーダ取得部１１０からＥＫＦ自己位置推定部１４０へエンコーダ値が出力される。
【００４０】
一方、レーザレンジファインダ１２は、ロボット１０から空間内に存在する物体までの距離を測距値として取得する（ステップＳ１１０）。レーザレンジファインダ１２は、エンコーダのように高いサンプリング周期で検出値を取得することができず、例えば図４に示すように、比較的低いサンプリング周期、例えば１００ｍｓ周期で測距値を取得する。レーザレンジファインダ１２は、取得した測距値を、ＳＬＡＭ自己位置推定部１２０へ出力する。
【００４１】
ＳＬＡＭ自己位置推定部１２０は、レーザレンジファインダ１２による測距値が入力されたタイミングで、自己位置推定処理を実行する（ステップＳ１２０）。ＳＬＡＭ自己位置推定部１２０は、図４に示すように、レーザレンジファインダ１２による測距値と、この測距値の測定時点における車輪エンコーダ取得部１１０のエンコーダ値とに基づいて、自己位置推定処理を行う。ＳＬＡＭによる自己位置推定処理は、例えば上記非特許文献１に記載の技術を用いて行うことができる。ＳＬＡＭ自己位置推定部１２０により算出された自己位置推定値は、ＥＫＦ自己位置推定部１４０へ出力される。
【００４２】
そして、ＥＫＦ自己位置推定部１４０は、拡張カルマンフィルタの原理に基づき、ＳＬＡＭ自己位置推定部１２０により算出された自己位置推定値と、車輪エンコーダ取得部１１０によるエンコーダ値とを用いて、自己位置推定値を算出する（ステップＳ１３０）。上述したように、エンコーダ値は高サンプリング周期で取得可能であるが、レーザレンジファインダ１２による測距値はこれより低いサンプリング周期でしか取得することができない。したがって、ＳＬＡＭを用いる場合、高精度に自己位置を推定することができる一方、ロボット１０の運動制御等、リアルタイムが要求される状況においては、より高いサンプリング周期で自己位置を推定できることが望ましい。そこで、ＥＫＦ自己位置推定部１４０は、高サンプリング周期で取得可能なエンコーダ値と、ＳＬＡＭ自己位置推定部１２０により高精度に推定された自己位置推定値とに基づき、高サンプリング周期で高精度な自己位置推定値を取得することを可能とする。
【００４３】
ＥＫＦ自己位置推定部１４０は、拡張カルマンフィルタを用いてロボット１０の自己位置推定値を算出するにあたり、まず、エンコーダ値に基づくオドメトリ出力値を算出する。オドメトリ出力値は、車輪エンコーダ取得部１１０から入力されたロボット１０の車輪１４の回転位置より算出される、車輪１４の速度や角速度等である。ＥＫＦ自己位置推定部１４０は、このオドメトリ出力値を、エンコーダのサンプリング周期で算出することが可能である。そして、ＥＫＦ自己位置推定部１４０は、オドメトリ出力値に基づき拡張カルマンフィルタの時間発展モデルを構築する。
【００４４】
また、ＥＫＦ自己位置推定部１４０は、オドメトリ出力値をＳＡＬＭ自己位置推定部１２０により算出されたＳＬＡＭによる自己位置推定値に加算する処理を行う。図４および図５に示すように、ＥＫＦ自己位置推定部１４０は、オドメトリ出力値とＳＬＡＭによる自己位置推定値とを融合し、自己位置推定値を算出するが、これらの値の取得タイミングは相違する。このため、ＳＬＡＭによる自己位置推定値をそのまま利用すると、かかる値が不連続な値として拡張カルマンフィルタに入力されることになる。
【００４５】
そこで、ＳＬＡＭ自己位置推定部１２０による自己位置推定値に、ＥＫＦ自己位置推定部１４０による自己位置推定処理に用いるエンコーダのオドメトリ出力値と同一の値を、ＳＬＡＭ自己位置推定部１２０による自己位置推定値に加算する。これにより、ＳＬＡＭ自己位置推定部１２０により推定された時点の自己位置推定値からのロボット１０の移動を含めた値が、エンコーダのサンプリング周期と同じ周期（例えば、１ｍｓ）で取得される。この取得値に基づき、観測モデルが構築される。したがって、ＥＫＦ自己位置推定部１４０は、同一タイミングで取得された値に基づくモデルより、自己位置推定値を算出することができる。
【００４６】
ＥＫＦ自己位置推定部１４０は、内界センサである車輪１４の検出値に基づく時間発展モデルと、ＳＬＡＭ自己位置推定部１２０において算出された自己位置推定値に基づく観測モデルとを有する拡張カルマンフィルタを用いて、自己位置推定値を算出する。拡張カルマンフィルタは、誤差のある観測値を用いて動的システムの状態を推定するためのオブザーバである。拡張カルマンフィルタは、前時刻の推定状態から現時刻の推定状態を算出する予測フェーズと現時刻の観測結果を用いて推定値を補正する更新フェーズとからなり、入力値ｘｔ−１、Ｐｔ−１、ｕｔ、ｙｔから出力値ｘｔ、Ｐｔを得る。予測フェーズおよび更新フェーズは、公知の下記数式１〜５によって表される。
【００４７】
【数１】
【００４８】
ここで、ｘは状態推定値、Ｐは誤差の共分散、ｕは制御値、ｙは観測値、ｇ（）は状態予測、ｈ（）は観測予測、Ａは時間発展モデルのヤコビ行列、Ｃは観測モデルのヤコビ行列、Ｒは時間発展モデルの共分散、Ｑは観測モデルの共分散、Ｋはカルマンゲインを示す。
【００４９】
具体的には、ＥＫＦ自己位置推定部１４０は、図５に示すように、車輪エンコーダ取得部１１０によるエンコーダ値から算出される車輪速度（オドメトリ出力値）に基づく時間発展モデルにより、オドメトリ自己位置推定を行う。時間発展モデルは、下記数式６により表される。
【００５０】
【数２】
【００５１】
ここで、ｘはｘ軸方向における位置、ｙはｙ軸方向における位置、θは旋回角度であり、ｘ＝（ｘ、ｙ、θ）Ｔである。また、ｖは並進速度、ｗは回転速度、εは分散を示す。すなわち、数式６では、オドメトリ出力値に基づき推定された時刻ｔにおける自己位置推定値を表している。
【００５２】
また、ＥＫＦ自己位置推定部１４０は、ＳＬＡＭ自己位置推定部１２０による自己位置推定値よりロボット１０の位置および方位を取得し、推定されたロボット１０の位置および方位、そして車輪速度に基づく観測モデルにより、ＳＬＡＭ自己位置推定を行う。観測モデルは、下記数式７により表される。
【００５３】
【数３】
【００５４】
ここで、ｕは直前のＳＬＡＭ自己位置推定時刻、δは分散を示す。すなわち、数式７では、ＳＬＡＭ自己位置推定値に基づき推定された時刻ｔにおける自己位置推定値を表している。ここで、数式７の右辺第１項括弧内の２項よりわかるように、時刻ｔ−ｕにおいて算出されたＳＬＡＭ自己位置推定値（左側の項）に、時刻ｔ−ｕ＋１から現時刻ｔまでの車輪１４による移動距離（右側の項）が加算されている（図５の積分器における加算処理）。このようにして、拡張カルマンフィルタの２つのモデルの同期を取り、推定精度を高めるようにしている。
【００５５】
ＥＫＦ自己位置推定部１４０は、数式６および７より表されるモデルを用いて、数式１〜５に基づきロボット１０の空間内における推定位置を算出する。このように、ＥＫＦ自己位置推定部１４０は、高精度のＳＬＡＭによる自己位置推定値と、高いサンプリング周期で取得されるオドメトリ出力値とを用いて、高精度かつ高サンプリング周期で自己位置を推定することができる。
【００５６】
その後、ＥＫＦ自己位置推定部１４０は、算出した自己位置推定値を運動制御部１５０へ出力する。運動制御部１５０は、ＥＫＦ自己位置推定部１４０により推定された自己位置に基づき、ロボット１０が移動・作業するための駆動機構の制御値を算出し、ロボット１０の制御を行う（ステップＳ１４０）。
【００５７】
以上、本実施形態に係る自己位置推定装置１００による自己位置推定方法について説明した。かかる方法によれば、低サンプリング周期でのＳＬＡＭを用いた自己位置推定結果と、高サンプリング周期で取得可能な車輪エンコーダ取得部１１０からのオドメトリ出力とを拡張カルマンフィルタによって融合する。これにより、低サンプリング周期で推定された自己位置推定結果を、より実位置に近いものに補正することができ、高サンプリング周期で高精度な自己位置推定を行うことが可能となる。
【００５８】
また、本実施形態の自己位置推定方法では、ＳＬＡＭによる自己位置推定結果をＥＫＦ自己位置推定部１４０に入力する前に、車輪エンコーダ取得部１１０からのオドメトリ出力を加算する。これにより、ＥＫＦ自己位置推定部１４０による自己位置推定結果が不連続な値とならず、この自己位置推定値を用いたロボット１０の運動制御等の制御を高精度に行うことが可能となる。
【００５９】
＜４．ハードウェア構成例＞
本実施形態にかかる自己位置推定装置１００による一部の処理は、ハードウェアにより実行させることもでき、ソフトウェアによって実行させることもできる。この場合、自己位置推定装置１００は、図６に示すようなコンピュータとして構成することもできる。以下、図６に基づいて、本実施形態に係る自己位置推定装置１００の一ハードウェア構成例について説明する。
【００６０】
本実施形態にかかる入力ユニットを構成する自己位置推定装置１００は、上述したように、コンピュータ等の情報処理装置により実現することができ、マニピュレータ１０を備える自律分散型ロボットに設けることができる。自己位置推定装置１００は、図６に示すように、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１０１と、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）１０２と、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）１０３と、ホストバス１０４ａとを備える。また、自己位置推定装置１００は、ブリッジ１０４と、外部バス１０４ｂと、インタフェース１０５と、入力装置１０６と、出力装置１０７と、ストレージ装置（ＨＤＤ）１０８と、ドライブ１０９と、接続ポート１１１と、通信装置１１３とを備える。
【００６１】
ＣＰＵ１０１は、演算処理装置および制御装置として機能し、各種プログラムに従って自己位置推定装置１００内の動作全般を制御する。また、ＣＰＵ１０１は、マイクロプロセッサであってもよい。ＲＯＭ１０２は、ＣＰＵ１０１が使用するプログラムや演算パラメータ等を記憶する。ＲＡＭ１０３は、ＣＰＵ１０１の実行において使用するプログラムや、その実行において適宜変化するパラメータ等を一時記憶する。これらはＣＰＵバスなどから構成されるホストバス１０４ａにより相互に接続されている。
【００６２】
ホストバス１０４ａは、ブリッジ１０４を介して、ＰＣＩ（Ｐｅｒｉｐｈｅｒａｌ Ｃｏｍｐｏｎｅｎｔ Ｉｎｔｅｒｃｏｎｎｅｃｔ／Ｉｎｔｅｒｆａｃｅ）バスなどの外部バス１０４ｂに接続されている。なお、必ずしもホストバス１０４ａ、ブリッジ１０４および外部バス１０４ｂを分離構成する必要はなく、一のバスにこれらの機能を実装してもよい。
【００６３】
入力装置１０６は、マウス、キーボード、タッチパネル、ボタン、マイク、スイッチおよびレバーなどユーザが情報を入力するための入力手段と、ユーザによる入力に基づいて入力信号を生成し、ＣＰＵ１０１に出力する入力制御回路などから構成されている。ユーザは、該入力装置１０６を操作することにより、ロボットの自己位置推定装置１００に対して各種のデータを入力したり処理動作を指示したりすることができる。
【００６４】
出力装置１０７は、例えば、液晶ディスプレイ（ＬＣＤ）装置、ＯＬＥＤ（Ｏｒｇａｎｉｃ Ｌｉｇｈｔ Ｅｍｉｔｔｉｎｇ Ｄｉｏｄｅ）装置およびランプなどの表示装置や、スピーカなどの音声出力装置を含む。
【００６５】
ストレージ装置１０８は、自己位置推定装置１００の記憶部の一例であり、データ格納用の装置である。ストレージ装置１０８は、記憶媒体、記憶媒体にデータを記録する記録装置、記憶媒体からデータを読み出す読出し装置および記憶媒体に記録されたデータを削除する削除装置などを含んでもよい。ストレージ装置１０８は、例えば、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）で構成される。このストレージ装置１０８は、ハードディスクを駆動し、ＣＰＵ１０１が実行するプログラムや各種データを格納する。
【００６６】
ドライブ１０９は、記憶媒体用リーダライタであり、自己位置推定装置１００に内蔵、あるいは外付けされる。ドライブ１０９は、装着されている磁気ディスク、光ディスク、光磁気ディスク、または半導体メモリ等のリムーバブル記録媒体に記録されている情報を読み出して、ＲＡＭ１０３に出力する。
【００６７】
接続ポート１１１は、外部機器と接続されるインタフェースであって、例えばＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）などによりデータ伝送可能な外部機器との接続口である。また、通信装置１１３は、例えば、通信網１５に接続するための通信デバイス等で構成された通信インタフェースである。また、通信装置１１２は、無線ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）対応通信装置であっても、ワイヤレスＵＳＢ対応通信装置であっても、有線による通信を行うワイヤー通信装置であってもよい。
【００６８】
なお、本実施形態に係る自己位置推定装置１００は、必ずしも図６に示すハードウェアを備える必要はない。例えば、入力装置１０６や表示装置１０７、ストレージ装置１０８、ドライブ１０９、接続ポート１１１、通信装置１１３等については、自己位置推定装置１００とは別の装置として、自己位置推定装置１００と接続して使用できるようにしてもよい。
【００６９】
以上、添付図面を参照しながら本発明の好適な実施形態について詳細に説明したが、本発明はかかる例に限定されない。本発明の属する技術の分野における通常の知識を有する者であれば、特許請求の範囲に記載された技術的思想の範疇内において、各種の変更例または修正例に想到し得ることは明らかであり、これらについても、当然に本発明の技術的範囲に属するものと了解される。
【００７０】
例えば、上記実施形態では、ＳＬＡＭによる自己位置推定値と内界センサによるオドメトリ出力値とを、拡張カルマンフィルタを用いて融合したが、本発明はかかる例に限定されない。例えば、拡張カルマンフィルタの代わりに、パーティクルフィルタ等を用いてもよい。なお、パーティクルフィルタを利用すると計算量が莫大に増加するため、拡張カルマンフィルタを用いるのが好適である。
【００７１】
また、上記実施形態では、外界センサとしてレーザレンジファインダ１２を用いたが、本発明はかかる例に限定されず、例えばステレオカメラ等を用いてもよい。
【００７２】
さらに、上記実施形態では、ＳＬＡＭ自己位置推定部１２０による自己位置推定処理を１００ｍｓ周期で行い、ＥＫＦ自己位置推定部１３０による自己位置推定処理を１ｍｓ周期で行ったが、本発明はかかる例に限定されない。ＥＫＦ自己位置推定部１４０のサンプリング周期がＳＬＡＭ自己位置推定部１２０のサンプリング周期よりも高い場合に、本発明は適用可能である。
【符号の説明】
【００７３】
１０ 自律移動型ロボット
１２ レーザレンジファインダ
１４ 車輪
１００ 自己位置推定装置
１１０ 車輪エンコーダ取得部
１２０ ＳＬＡＭ自己位置推定部
１２２ 位置推定部
１２４ 環境マップ更新部
１３０ 環境マップ記憶部
１４０ ＥＫＦ自己位置推定部
１５０ 運動制御部
【特許請求の範囲】
【請求項１】
移動機構を備え、所定の空間を移動可能な自律移動装置の前記移動機構による動作情報を検出する内界センサ、および前記所定の空間内に関する空間情報を検出する外界センサによる検出結果に基づいて、第１のサンプリング周期で自己位置を推定する第１の自己位置推定部と、
前記第１の自己位置推定部による自己位置推定結果と、前記内界センサにより検出された前記自律移動装置の動作情報とに基づいて、前記第１のサンプリング周期よりも高いサンプリング周期で自己位置を推定する第２の自己位置推定部と、
を備える、自己位置推定装置。
【請求項２】
前記空間内の空間情報を環境マップとして記憶する記憶部をさらに備え、
前記第１の自己位置推定部は、
前記環境マップの空間情報、前記内界センサの検出値および前記外界センサの検出値に基づいて、前記自律移動装置の前記空間における自己位置を推定する位置推定部と、
前記位置推定部により推定された自己位置と、前記外界センサの検出値とに基づいて、前記記憶部の環境マップを更新する更新部と、
を備える、請求項１に記載の自己位置推定装置。
【請求項３】
前記外界センサは、前記自律移動装置から前記空間内に存在する物体までの距離を取得する距離センサであり、
前記第１の自己位置推定部は、前記距離センサの位置姿勢と当該距離センサによる測距値に基づき認識された物体の位置とを同時に推定するＳＬＡＭを用いて、前記環境マップを動的に生成する、請求項２に記載の自己位置推定装置。
【請求項４】
前記第２の自己位置推定部は、前記内界センサの検出値に基づく時間発展モデルと、前記第１の自己位置推定部により推定された前記自律移動装置の自己位置に基づく観測モデルとを備える拡張カルマンフィルタを用いて、前記自律移動装置の前記空間における自己位置を推定する、請求項１〜３のいずれか１項に記載の自己位置推定装置。
【請求項５】
前記第２の自己位置推定部は、前記第１の自己位置推定部により推定された自己位置結果に前記内界センサの検出値を加算した加算値を用いて、前記観測モデルを生成する、請求項４に記載の自己位置推定装置。
【請求項６】
前記内界センサは、前記自律移動装置の駆動機構の駆動量を検出する位置センサである、請求項１に記載の自己位置推定装置。
【請求項７】
前記第１のサンプリング周期は、前記外界センサの検出周期であり、
前記第２のサンプリング周期は、前記内界センサの検出周期である、請求項１に記載の自己位置推定装置。
【請求項８】
移動機構を備え、所定の空間を移動可能な自律移動装置の前記移動機構による動作情報を検出する内界センサ、および前記所定の空間内に関する空間情報を検出する外界センサによる検出結果に基づいて、第１の自己位置推定部によって、第１のサンプリング周期で自己位置を推定するステップと、
前記前記第２の自己位置推定部による自己位置推定結果と、前記内界センサにより検出された前記自律移動装置の動作情報とに基づいて、第２の自己位置推定部によって、前記第１のサンプリング周期よりも高いサンプリング周期で自己位置を推定するステップと、
を含む、自己位置推定方法。
【請求項１】
移動機構を備え、所定の空間を移動可能な自律移動装置の前記移動機構による動作情報を検出する内界センサ、および前記所定の空間内に関する空間情報を検出する外界センサによる検出結果に基づいて、第１のサンプリング周期で自己位置を推定する第１の自己位置推定部と、
前記第１の自己位置推定部による自己位置推定結果と、前記内界センサにより検出された前記自律移動装置の動作情報とに基づいて、前記第１のサンプリング周期よりも高いサンプリング周期で自己位置を推定する第２の自己位置推定部と、
を備える、自己位置推定装置。
【請求項２】
前記空間内の空間情報を環境マップとして記憶する記憶部をさらに備え、
前記第１の自己位置推定部は、
前記環境マップの空間情報、前記内界センサの検出値および前記外界センサの検出値に基づいて、前記自律移動装置の前記空間における自己位置を推定する位置推定部と、
前記位置推定部により推定された自己位置と、前記外界センサの検出値とに基づいて、前記記憶部の環境マップを更新する更新部と、
を備える、請求項１に記載の自己位置推定装置。
【請求項３】
前記外界センサは、前記自律移動装置から前記空間内に存在する物体までの距離を取得する距離センサであり、
前記第１の自己位置推定部は、前記距離センサの位置姿勢と当該距離センサによる測距値に基づき認識された物体の位置とを同時に推定するＳＬＡＭを用いて、前記環境マップを動的に生成する、請求項２に記載の自己位置推定装置。
【請求項４】
前記第２の自己位置推定部は、前記内界センサの検出値に基づく時間発展モデルと、前記第１の自己位置推定部により推定された前記自律移動装置の自己位置に基づく観測モデルとを備える拡張カルマンフィルタを用いて、前記自律移動装置の前記空間における自己位置を推定する、請求項１〜３のいずれか１項に記載の自己位置推定装置。
【請求項５】
前記第２の自己位置推定部は、前記第１の自己位置推定部により推定された自己位置結果に前記内界センサの検出値を加算した加算値を用いて、前記観測モデルを生成する、請求項４に記載の自己位置推定装置。
【請求項６】
前記内界センサは、前記自律移動装置の駆動機構の駆動量を検出する位置センサである、請求項１に記載の自己位置推定装置。
【請求項７】
前記第１のサンプリング周期は、前記外界センサの検出周期であり、
前記第２のサンプリング周期は、前記内界センサの検出周期である、請求項１に記載の自己位置推定装置。
【請求項８】
移動機構を備え、所定の空間を移動可能な自律移動装置の前記移動機構による動作情報を検出する内界センサ、および前記所定の空間内に関する空間情報を検出する外界センサによる検出結果に基づいて、第１の自己位置推定部によって、第１のサンプリング周期で自己位置を推定するステップと、
前記前記第２の自己位置推定部による自己位置推定結果と、前記内界センサにより検出された前記自律移動装置の動作情報とに基づいて、第２の自己位置推定部によって、前記第１のサンプリング周期よりも高いサンプリング周期で自己位置を推定するステップと、
を含む、自己位置推定方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２０１１−２０９２０３（Ｐ２０１１−２０９２０３Ａ）
【公開日】平成２３年１０月２０日（２０１１．１０．２０）
【国際特許分類】
【出願番号】特願２０１０−７８８９１（Ｐ２０１０−７８８９１）
【出願日】平成２２年３月３０日（２０１０．３．３０）
【出願人】（０００００２１８５）ソニー株式会社 (34,172)
【Ｆターム（参考）】
【公開日】平成２３年１０月２０日（２０１１．１０．２０）
【国際特許分類】
【出願日】平成２２年３月３０日（２０１０．３．３０）
【出願人】（０００００２１８５）ソニー株式会社 (34,172)
【Ｆターム（参考）】
[ Back to top ]