JP5793851B2 - 位置推定方法、位置推定装置及びプログラム - Google Patents

位置推定方法、位置推定装置及びプログラム Download PDF

Info

Publication number
JP5793851B2
JP5793851B2 JP2010250319A JP2010250319A JP5793851B2 JP 5793851 B2 JP5793851 B2 JP 5793851B2 JP 2010250319 A JP2010250319 A JP 2010250319A JP 2010250319 A JP2010250319 A JP 2010250319A JP 5793851 B2 JP5793851 B2 JP 5793851B2
Authority
JP
Japan
Prior art keywords
estimation
unit
data
electronic device
static map
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Expired - Fee Related
Application number
JP2010250319A
Other languages
English (en)
Other versions
JP2012103819A (ja
Inventor
彬 陳
彬 陳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Fujitsu Ltd
Original Assignee
Fujitsu Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Fujitsu Ltd filed Critical Fujitsu Ltd
Priority to JP2010250319A priority Critical patent/JP5793851B2/ja
Publication of JP2012103819A publication Critical patent/JP2012103819A/ja
Application granted granted Critical
Publication of JP5793851B2 publication Critical patent/JP5793851B2/ja
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Description

本発明は、位置推定方法、位置推定装置及びプログラムに関する。
近年、公共施設等で人にサービスを提供する自律移動型（又は、自律走行型）の知能ロボットが開発されている。このような知能ロボットは、サービスロボットとも呼ばれる。このようなサービスロボットの活用は、ロボット技術の進歩に伴い増えることが予想される。サービスロボットでは、予め決められたタスクを実行してサービスを提供するため、施設内で例えば指定されたサービスポイント間を自律移動することが求められる。ロボットを自律移動させるナビゲーションの分野では、ＳＬＡＭ（Simultaneous Localization And Mapping）等の技術を利用して、事前に作成され地図に基づいてサービスロボットが自己位置を推定する（例えば、非特許文献１）。
しかし、サービスロボットが同一施設内でサービスの質を低下させることなくサービスを提供するためには、環境変化により自己位置推定の精度が劣化することを防ぐことが望ましい。例えば、ショッピングセンタ内では、商品の種類の入れ替え、商品棚の撤去や増設、一時的に開催されるイベント等によって、環境地図が変化する。静止物体の配置が変化し得る環境は、非静的環境と呼ばれる（例えば、非特許文献２）。このような非静的環境において、サーボスロボットの導入時（即ち、サービスロボットが施設内に配備される時）に作成された地図のみに基づいた自己位置推定を行ったのでは、自己位置推定の精度が低下してしまい、自己位置推定が破綻する可能性もある。又、環境が変化する度にＳＬＡＭ等の技術を利用して地図を作成し直すのでは、環境変化に応じて地図を逐次更新する必要が生じ、地図更新処理によるロボットシステムへの負荷が増大し、ロボットシステムの維持費（即ち、メンテナンスコスト）も増加してしまう。
このように、サービスロボットの導入時に作成された環境全体の地図にも基づいて、非静的環境に適した自己位置推定を行うことが望まれている。又、事前に作成した環境の地図に基づいて自己位置推定を行う際に、環境の変化に拘わらず自己位置推定の精度と安定さを維持することも望まれている。
非静的環境下での自己位置推定方式は、例えば以下の３種類の方式に分類することができる。
第１の自己位置推定方式では、環境内の移動物体（即ち、移動している物や人）をセンサで検出してトラキングし、センサの計測データから移動物体を除去して自己位置推定や地図の作成を行う。
第２の自己位置推定方式では、環境全体を静的領域と非静的領域に分け、センサの計測データを静的領域データと非静的領域データに分類する。この場合、レーザレンジファインダ(ＬＲＦ) でスキャンしたレンジデータから可動物体の配置の変化を検出して、対応するレンジデータを計測データから除去して自己位置推定を行うことも提案されている（例えば、非特許文献２）。又、環境内での移動物体の移動頻度をパラメータ化し、移動尤度を設定してグリッドマップに書き込み、レイアウト確率マップを作成することも提案されている（例えば、非特許文献４）。この場合、ロボットが移動する際に、センサの計測データをレイアウト確率マップと照合して観測尤度を計算できるが、環境内の物体に関しての事前知識が必要であり、例えばショッピングセンタのような大規模な予測困難な環境での実用性は低い。更に、環境の地図を異なるタイミングで複数作成し、ＥＭ（Expectation Maximization）アルゴリズムに従って複数の地図間の差分処理と自己位置推定処理を繰り返して環境を動的領域と静的領域を分離することも提案されている（例えば、非特許文献７，８）。しかし、この場合は差分処理及び自己位置推定処理の２つの処理が互いに前回の処理結果を利用して次回の処理を行うため、いずれかの処理結果に比較的大きな誤差が発生すると、最終的には自己位置推定が破綻してしまう。
第３の自己位置推定方式では、環境の地図を異なるタイミングで複数作成し、複数の地図で環境レイアウトの変化を表現する。環境レイアウトとは、環境内の物や人の配置を指す。サービスロボットの自律走行時に、複数の地図から最適なものを選択して自己位置推定に利用する。環境レイアウトの変化の頻繁な局所領域は、複数のサブローカル地図を組み合わせることにより表現することが提案されている（例えば、非特許文献５）。これらのサブローカル地図は、夫々が対応する環境レイアウトを表し、学習によって獲得できる。サービスロボットの自律移動時に、最適なサブローカル地図を選択できれば自己位置推定の精度を向上できる。異なるタイミングで作成された複数の地図に、重要度を表すタイムスケールパラメータを付加しておき、重要度が比較的高い複数地図を照合することで自己位置推定を行うことも提案されている（例えば、非特許文献３）。しかし、あり得る全ての環境変化を事前に予測することが困難な場合には、自己位置推定の精度の向上は期待できない。
上記の自己位置推定方式の多くは、シミュレーションの環境で自己位置推定を行うものである。しかし、実環境の中では、環境レイアウトが変化する物体や領域の予測が困難であるため、上記の自己位置推定方式の多くは実用性が低い。このため、環境変換に関する事前の予測、或いは、事前の知識に依存せずに自己位置推定の精度を向上可能な自己位置推定技術が望まれている。
特開平７−７２９２４号公報 特開２００８−７１３５２号公報 特表２０１０−５１１９５７号公報 特開２００８−１６５２７５号公報
D.Austin and K. Kouzoubov, "Robust, Long Term Navigation of a Mobile Robot", Proc. of IARP/IEERAS Joint Workshop on Technical Challenges for Dependable Robots in Human Environments, October, LAS-CNRS, Toulouse, France, 2002 田中完爾, 木室義彦, 岡田伸廣, 近藤英二,「非静的環境における確率的アプローチによる変化検出と自己位置推定」, 電子情報通信学会論文誌 D-II, Vol.J88-D-II, No.5, pp.854-863, （社）電子情報通信学会 2005 P. Biber and T. Duckett, "Experimental Analysis of Sample-Based Maps for Long-Term SLAM", International Journal of Robotics Research, Vol.28, Issue 1, pp.20-33, January 2009 A. Ramirez-Serrano, H. Liu and G. C. Pettinaro, "Mobile robot localization in quasi-dynamic environments", Industrial Robot: An International Journal, Vol.35, No.3, pp.246-258, 2008 C. Stachniss and W. Burgard, "Mobile Robot Mapping and Localization in Non-Static Environments", Proc. of the National Conference on Artificial Intelligence (AAAI), Pittsburgh PA, USA, 2005 Denis Wolf and G. S. Sukhatme, "Online Simultaneous Localization and Mapping in Dynamic Environments", Proc. of the Intl. Conference on Robotics and Automation (ICRA), New Orleans, Louisiana, Apr. 2004 C.-C. Wang, C. Thorpe, and S. Thrun, "Online Simultaneous Localization and Mapping with Detection and Tracking of Moving Objects: Theory and Results from a Ground Vehicle in Crowded Urban Areas", Proc. of the International Conference on Robotics and Automation (ICRA), pp.842-849, 2003 D. Hahnel, R. Triebel, W. Burgard and S. Thrun, "Map Building with Mobile Robots in Dynamic Environments", Proc. of the International Conference on Robotics and Automation (ICRA), pp.1557-1563, 2003. A. I. Eliazar and R. Parr,"DP-SLAM2.0," IEEE 2004 International Conference on Robotics and Automation (ICRA), Vol.2, pp.1314-1320, 2004
従来の位置推定方法では、環境変化に関する事前の予測、或いは、事前の知識に依存せずに自己位置推定の精度を向上することは難しいという問題があった。
そこで、本発明は、環境変化に関する事前の予測、或いは、事前の知識に依存せずに自己位置推定の精度を向上可能な位置推定方法、位置推定装置及びプログラムを提供することを目的とする。
本発明の一観点によれば、コンピュータによる電子装置の位置推定方法であって、前記コンピュータが、内的センサの観測情報と、外的センサの観測情報と、前記電子装置が導入される環境の静的地図のデータとに基づいて推定した前記電子装置の位置と姿勢を含む移動履歴を取得して記憶部に一時的に格納する移動履歴取得工程と、前記コンピュータが、前記移動履歴及び前記外的センサの観測情報に基づき前記電子装置の移動中に実時間で作成された非静的地図のデータと、前記静的地図のデータとの照合に基づいて、前記電子装置の自己位置と前記電子装置の周囲の周辺地図を推定する推定工程とを含み、前記推定工程は、前記コンピュータに含まれる第１の推定部により前記コンピュータが、移動開始コマンドに応答して、前記外的センサの観測情報、前記静的地図のデータ、及び前記移動履歴を入力して逐時に前記電子装置の位置と姿勢を推定して更新する初期段階での自己位置推定処理のループを開始する初期化工程と、前記コンピュータに含まれ前記初期化工程では待機状態にある第２、第３、及び第４の推定部を起動する条件を判断し、前記条件が満たされると前記初期段階での前記自己位置推定処理を終了して、前記電子装置の位置のサンプルセットを含む推定結果を前記第２、第３、及び第４の推定部に同時に出力する切替工程とを実行し、前記第１の推定部により、前記コンピュータが、前記外的センサの観測情報、前記静的地図のデータ、及び前記移動履歴に基づいて前記電子装置の移動中に実時間で前記非静的地図のデータを作成し、前記第２の推定部により、前記コンピュータが、前記静的地図のデータ、前記移動履歴、及び前記非静的地図のデータを入力として、前記非静的地図のデータと前記外的センサの観測情報とを合わせた観測情報と、前記静的地図のデータとに基づいて前記電子装置の自己位置を推定し、前記第３の推定部により、前記コンピュータが、前記移動履歴、前記静的地図のデータ、及び前記非静的地図のデータを入力として、前記外的センサの観測情報と、前記静的地図のデータと前記非静的地図のデータとを合わせた地図のデータとに基づいて前記電子装置の自己位置を推定して前記非静的地図のデータを更新し、前記第４の推定部により、前記コンピュータが、前記第２の推定部から得られる推定された前記自己位置及び前記第３の推定部から得られる推定された前記自己位置に基づき、前記電子装置の現在位置と姿勢を統合した統合処理結果を出力し、前記第２及び第３の推定部の各々は、前記第４の推定部が出力した前記統合処理結果のフィードバックを受けると、前記統合処理結果に基づいて各自の自己位置の推定を更新する位置推定方法が提供される。
本発明の一観点によれば、外的センサの観測情報、電子装置が導入される環境の静的地図のデータ、及び前記電子装置の移動履歴に基づいて前記電子装置の移動中に実時間で非静的地図のデータを作成する第１の推定部と、前記移動履歴は、内的センサの観測情報と、前記外的センサの観測情報と、前記静的地図のデータとに基づいて推定した前記電子装置の位置と姿勢を含み、前記静的地図のデータ、前記移動履歴、及び前記非静的地図のデータを入力として、前記非静的地図のデータと前記外的センサの観測情報とを合わせた観測情報と、前記静的地図のデータとに基づいて前記電子装置の自己位置を推定する第２の推定部と、前記移動履歴、前記静的地図のデータ、及び前記非静的地図のデータを入力として、前記外的センサの観測情報と、前記静的地図のデータと前記非静的地図のデータとを合わせた地図のデータとに基づいて前記電子装置の自己位置を推定して前記非静的地図のデータを更新する第３の推定部と、前記第２の推定部から得られる推定された前記自己位置及び前記第３の推定部から得られる前記自己位置に基づき、前記電子装置の現在位置と姿勢を統合した統合処理結果を出力する前記第４の推定部とを備え、前記第１の推定部は、移動開始コマンドに応答して、前記外的センサの観測情報、前記静的地図のデータ、及び前記移動履歴を入力して逐時に前記電子装置の位置と姿勢を推定して更新する初期段階での自己位置推定処理のループを開始し、前記初期段階での前記自己位置推定処理のループの開始時には待機状態にある前記第２、第３、及び第４の推定部を起動する条件を判断し、前記条件が満たされると前記初期段階での前記自己位置推定処理を終了して、前記電子装置の位置のサンプルセットを含む推定結果を前記第２、第３、及び第４の推定部に同時に出力し、前記第２及び第３の推定部の各々は、前記第４の推定部が出力した前記統合処理結果のフィードバックを受けると、前記統合処理結果に基づいて各自の自己位置の推定を更新する位置推定装置が提供される。
本発明の一観点によれば、コンピュータに電子装置の位置を推定させるプログラムであって、内的センサの観測情報と、外的センサの観測情報と、前記電子装置が導入される環境の静的地図のデータとに基づいて推定した前記電子装置の位置と姿勢を含む移動履歴を取得して記憶部に一時的に格納する移動履歴取得手順と、前記移動履歴及び前記外的センサの観測情報に基づき前記電子装置の移動中に実時間で作成された非静的地図のデータと、前記静的地図のデータとの照合に基づいて、前記電子装置の自己位置と前記電子装置の周囲の周辺地図を推定する推定手順とを前記コンピュータに実行させ、前記推定手順は、前記プログラムで実現される第１の推定部により前記コンピュータが、移動開始コマンドに応答して、前記外的センサの観測情報、前記静的地図のデータ、及び前記移動履歴を入力して逐時に前記電子装置の位置と姿勢を推定して更新する初期段階での自己位置推定処理のループを開始する初期化手順と、前記プログラムで実現され前記初期化手順では待機状態にある第２、第３、及び第４の推定部を起動する条件を判断し、前記条件が満たされると前記初期段階での前記自己位置推定処理を終了して、前記電子装置の位置のサンプルセットを含む推定結果を前記第２、第３、及び第４の推定部に同時に出力する切替手順とを実行し、前記第１の推定部により、前記コンピュータが、前記外的センサの観測情報、前記静的地図のデータ、及び前記移動履歴に基づいて前記電子装置の移動中に実時間で前記非静的地図のデータを作成し、前記第２の推定部により、前記コンピュータが、前記静的地図のデータ、前記移動履歴、及び前記非静的地図のデータを入力として、前記非静的地図のデータと前記外的センサの観測情報とを合わせた観測情報と、前記静的地図のデータとに基づいて前記電子装置の自己位置を推定し、前記第３の推定部により、前記コンピュータが、前記移動履歴、前記静的地図のデータ、及び前記非静的地図のデータを入力として、前記外的センサの観測情報と、前記静的地図のデータと前記非静的地図のデータとを合わせた地図のデータとに基づいて前記電子装置の自己位置を推定して前記非静的地図のデータを更新し、前記第４の推定部により、前記コンピュータが、前記第２の推定部から得られる推定された前記自己位置及び前記第３の推定部から得られる推定された前記自己位置に基づき、前記電子装置の現在位置と姿勢を統合した統合処理結果を出力し、前記第２及び第３の推定部の各々は、前記第４の推定部が出力した前記統合処理結果のフィードバックを受けると、前記統合処理結果に基づいて各自の自己位置の推定を更新するプログラムが提供される。
開示の位置推定方法、位置推定装置及びプログラムによれば、環境変化に関する事前の予測、或いは、事前の知識に依存せずに自己位置推定の精度を向上することができる。
本発明の一実施例における自律走行型のロボットの構成の一例を示す図である。 走行系制御ＣＰＵとナビゲーションＣＰＵが実行する処理の一例を説明する図である。 移動履歴のデータの一例を説明する図である。 非静的地図の一例を説明する図である。 自己位置と地図の推定タスクが用いるアルゴリズムの一例を説明する図である。 自己位置と地図の推定タスクが用いるアルゴリズムの一例をより詳細に説明する図である。 自己位置推定の安定性を説明する図である。 従来の処理モジュールの一例を搭載した比較例のロボットと、図６に示す処理モジュールを搭載したロボット１により得られた自己位置推定が成功した頻度の分布を示す図である。 自己位置と地図の推定タスクが用いるアルゴリズムの他の例を詳細に説明する図である。 区間信念度を説明する図である。 ロボットの遠隔操作を説明する図である。
開示の位置推定方法、位置推定装置及びプログラムでは、電子装置の移動履歴を取得して一時的に格納する。又、電子装置の移動履歴と外的センサの観測情報と電子装置が導入される環境の静的地図のデータと、前記電子装置の移動中に実時間で作成された非静的地図のデータとに基づいて、電子装置の自己位置とその周囲の周辺地図を推定する。
更に、電子装置の自己位置とその周辺地図に基づいて、設定された経路に従って電子装置が目的地まで移動するための目標経路を生成しても良い。
以下に、開示の位置推定方法、位置推定装置及びプログラムの各実施例を図面と共に説明する。
先ず、本発明の一実施例における位置推定方法の一例について説明する。位置推定方法は、例えば自律移動型のロボットが自己位置推定を行うのに用いることができる。
一般的に、例えば自律移動型のロボットがある業務を行う（例えば、サービスを提供する）ために、新しい環境に導入される際には、先ず環境の地図を作成する。又、ロボットが業務を行うためのアプリケーションを開発する際には、作成した環境の地図の座標系を参照して、ロボットの作業場所、ロボットの移動先の目標地点、ロボットの移動経路等を座標形式で指定する。
しかし、環境レイアウトの変化に適応するために環境の地図を更新すると、新しく作成した更新後の地図と元の更新前の地図との間に差が生じる。地図の更新回数が多くなると、最新の地図が元の地図と大きく異なってしまう。このような場合、ロボットが業務を行うためのアプリケーションでの座標値を最新の地図に基づいて指定し直す必要が生じ、アプリケーションのメンテナンスが煩雑になり、メンテナンスに時間とコストがかかってしまう。
そこで、本実施例では、アプリケーションのメンテナンス作業を軽減するために、元の地図と推定された最新地図を同時に用いて自己位置推定を行う。つまり、環境レイアウトに変化をもたらす領域や物体等に関する情報を事前に作成して自己位置推定に用いることはない。自己位置推定は、以下の３つの処理モジュール（又は、処理部）を用いて行うことができる。
第１の推定モジュールは、静的地図に基づいて第１の自己位置推定手順を実行する。静的地図とは、ロボットを環境に導入する際に作成された、更新されない地図である。第１の自己位置推定手順で用いる観測情報は、後述する第２の推定モジュールで作成した仮想センサの観測（以下、仮想観測とも言う）と、実センサの観測（以下、実観測とも言う）を含む。仮想観測には、第２の推定モジュールで作成（即ち、推定）された最新地図が用いられる。このように、環境の静的地図の拘束によって最新地図を推定し、推定した最新地図を仮想観測として利用することで、最新の実観測と同時に、静的地図との照合によって自己位置推定を行う。第１の自己位置推定手順が行う自己位置推定は、矢印の左側を入力、矢印の右側を出力とすると、以下のような関係で表すことができる。
（仮想観測＋実観測）× 静的地図 → 自己位置推定
第２の推定モジュールは、非静的地図作成手順と第２の自己位置推定手順を実行する。第２の推定モジュールは、最新の実観測と地図情報に基づいて最新地図を作成（即ち、推定）しながら自己位置推定を行う。この場合の地図情報は、第１の推定モジュールで用いる静的地図と、最新の非静的地図との組み合わせである。非静的地図とは、ロボットが移動している間に実時間で作成される地図であり、最新の非静的地図が最新地図となる。第２の自己位置推定手順が行う自己位置推定は、矢印の左側を入力、矢印の右側を出力とすると、以下のような関係で表すことができる。
実観測 × （静的地図＋非静的地図） → 自己位置推定 ＋ 最新地図
統合モジュールは、上記第１及び第２の推定モジュールによって得られた推定結果を統合し、より高い精度で自己位置推定を行う。
上記第１及び第２の推定モジュール（又は、推定部）は、例えば２つの無香粒子フィルタ（又は、アンセンテッドパーティクルフィルタ（ＵＰＦ：Unscented Particle Filter））で形成可能である。この場合、２つのＵＰＦは、非静的地図の作成と、静的地図に基づく自己位置推定を並行に行う。非静的地図を作成するＵＰＦでは、与えられた静的地図を事前条件として、環境レイアウトの地図と現在位置を推定する。一方、静的地図に基づく自己位置推定を行うＵＰＦでは、レーザレンジファインダ（ＬＲＦ：Laser Range Finder）等の計測装置から得られる計測データと最新地図（即ち、最新の非静的地図）を観測データとして、静的地図と照合することによって各パーティクルを評価する。統合モジュール（又は、統合部）は、例えば無香カルマンフィルタ（又は、アンセンテッドカルマンフィルタ（ＵＫＦ：Unscented Kalman Filter））で形成可能である。この場合、ＵＫＦは、上記２つのＵＰＦからの推定結果を統合すると共に、統合結果を各ＵＰＦへ配分（又は、フィードバック）する。これにより、２つのＵＰＦは、統合結果を新たな観測情報として利用することでパーティクルの状態を更新できる。
図１は、本発明の一実施例における自律走行型のロボットの構成の一例を示す図である。ロボット１は、ナビゲーションＣＰＵ１１、走行制御ＣＰＵ１２、台車１３、センサ部１４、入出力部１５、及び記憶部１６を有する。入出力部１５は、利用者がロボット１に情報やコマンドを入力する入力部（図示せず）と、ロボット１から利用者へ情報を出力する出力部（図示せず）を含む。入力部は、例えばキーボード等の操作部、マイクロホン等を含む。一方、出力部は、表示部、スピーカ等を含む。ＣＰＵ１１，１２は、単一の計算機（又は、コンピュータ）を形成しても、別々の計算機（又は、コンピュータ）を形成しても良い。尚、ロボット１には、周知の構成を有し周知の動作を行うロボットアーム（図示せず）や、外部のサーバ（図示せず）等と通信するためのアンテナや送受信部を含む通信部（図示せず）を更に有しても良い。
記憶部１６は、ＣＰＵ１１，１２が実行するプログラムを含む各種プログラム、及びＣＰＵ１１，１２が実行する演算の中間データ、後述する静的地図及び非静的地図のデータ等を含む各種データを格納する。記憶部１６は、コンピュータ読み取り可能な記憶媒体により形成可能である。コンピュータ読み取り可能な記憶媒体は、一例として、磁気記録媒体、光記録媒体、光磁気記録媒体、ディスクを記録媒体として用いるディスク装置、ＲＯＭ（Read Only Memory）等を含む半導体記憶装置等を含む。ディスクを記録媒体として用いるディスク装置には、例えばＨＤＤ（Hard Disk Drive）が使用可能である。又、記憶部１６は、複数の記憶装置で形成されていても良く、この場合、アクセス速度の異なる記憶装置を含んでも良い。
台車１３は、ジャイロセンサ１３１、センサ・エンコーダ１３２、モータ１３３、及び車輪１３４を有する。ジャイロセンサ１３１は、車輪１３４の回転量を計測して走行制御ＣＰＵ１２に出力し、センサ・エンコーダ１３２は、車輪１３４の回転数を検出して走行制御ＣＰＵ１２に出力する。ジャイロセンサ１３１及びセンサ・エンコーダ１３２は、内的センサを形成する。モータ１３３は、走行制御ＣＰＵ１２からのコマンドに基づいて車輪１３４を直接、或いは、ギア機構（図示せず）を介して回転する。モータ１３３は、複数設けられていても良く、台車１３の移動方向を決めるステアリング部（図示せず）を駆動しても良い。モータ１３３、ギア機構、及びステアリング部等は、ロボット１の走行を制御する走行制御系を形成する。
走行制御ＣＰＵ１２は、台車１３の移動を制御して例えばナビゲーションＣＰＵ１１により指示された目標経路を追従させたり、台車１３内のジャイロセンサ１３１の出力情報及びセンサ・エンコーダ１３２の出力情報に基づいて台車１３、即ち、ロボット１の姿勢と現在位置を推定する。
センサ部１４は、カメラ１４１及び距離センサ１４２を有する。カメラ１４１は、例えば撮影画像から周知の方法で視覚的ランドマークを抽出してロボット１の３次元位置を計測するステレオカメラで形成可能である。距離センサ１４２は、周囲環境への距離を周知の方法で計測する例えばＬＲＦ等の計測装置で形成可能である。カメラ１４１及び距離センサ１４２は、外的センサを形成する。
ナビゲーションＣＰＵ１１は、内的センサ（ジャイロセンサ１３１、センサ・エンコーダ１３２）及び外的センサ（カメラ１４１及び距離センサ１４２）の出力情報に基づいて、ロボット１の現在位置を推定する。又、ナビゲーションＣＰＵ１１は、推定したロボット１の現在位置に基づいて、ロボット１の移動を計画する。
本実施例における位置推定装置は、図１に示す如きハードウェア構成を有するロボット１のナビゲーションＣＰＵ１１、即ち、ナビゲーション部の一部として搭載されていても良く、ロボット１が自律移動を行う際に自己位置推定を行う。
図２は、走行系制御ＣＰＵ１２とナビゲーションＣＰＵ１１が実行する処理の一例を説明する図である。走行制御ＣＰＵ１２は、内的センサの出力情報に基づいて台車１３、即ち、ロボット１の移動に関する情報を処理する。一方、ナビゲーションＣＰＵ１１は、外的センサの出力情報に基づいてロボット１の自己位置推定と移動を計画する。
走行系制御ＣＰＵ１２が実行するタスク（又は、処理）には、自己位置推定タスクＳＴ２１と経路追従タスクＳＴ２２が含まれる。自己位置推定タスクＳＴ２１は、内的センサを形成するジャイロセンサ１３１及びエンコーダ１３２の出力情報に基づいてロボット１の姿勢と現在位置を推定し、例えば記憶部１６に格納する。自己位置推定タスクＳＴ２１が行う推定は、外的センサの出力情報を用いないため、推定誤差はロボット１の移動距離に応じて拡大する。経路追従タスクＳＴ２２は、ナビゲーションＣＰＵ１１から指示された目標経路（又は、目標軌跡）を追従するようにモータ１３３を制御する。
ナビゲーションＣＰＵ１１が実行するタスク（又は、処理）には、移動履歴取得タスクＳＴ１１と、自己位置と地図の推定タスクＳＴ１２と、経路計画タスクＳＴ１３が含まれる。移動履歴取得タスクＳＴ１１は、自己位置と地図の推定タスクＳＴ１２の処理周期が走行制御ＣＰＵ１２の自己位置推定タスクＳＴ２１の処理周期より長い場合に自己位置と地図の推定処理を行う間にロボット１の移動履歴が失われてしまうことがないように、比較的速い処理周期で走行制御ＣＰＵ１２を介して記憶部１６をアクセスして、ロボット１の移動履歴を取得して例えば記憶部１６内に一時的に格納する。自己位置と地図の推定タスクＳＴ１２は、移動履歴取得タスクＳＴ１１が取得して記憶部１６に格納されたロボット１の移動履歴と、外的センサを形成するカメラ１４１及び距離センサ１４２の出力情報と、例えば記憶部１６に格納されたロボット１がサービスを提供する環境の全体的な静的地図のデータに基づいて、ロボット１の自己位置（即ち、現在位置）と周囲のローカル地図（又は、周辺地図）を推定する。経路計画タスクＳＴ１３は、例えば利用者が入出力部１５の入力部から設定した経路に従って、障害物等を回避しながらロボット１を目的地まで移動させるための目標経路をロボット１の自己位置と周囲のローカル地図に基づいて生成し、走行制御ＣＰＵ１２へ指示する。
図２に示す自己位置と地図の推定タスクＳＴ１２は、例えば図５に示すアルゴリズムに基づいて自己位置と地図の推定処理を行うことができる。図５は、自己位置と地図の推定タスクＳＴ１２が用いるアルゴリズムの一例を説明する図である。図５は、自己位置と地図の推定タスクＳＴ１２の処理を実行するＰＦ（Particle Filter）推定部２０、ＵＰＦ（Unscented Particle Filter）推定部２１、ＵＰＦ推定部２２、及びＵＫＦ（Unscented Kalman Filter）推定部２３を示す。ＰＦ推定部２０は、初期化、予測、更新、及び切替ステップを実行し、初期段階でのロボット１の自己位置推定及び各推定部２１〜２３の初期化のためのデータの準備を行う。ＵＰＦ推定部２１は、初期化、予測、及び更新ステップを実行し、静的地図を用いた自己位置推定を行う。ＵＰＦ推定部２２は、初期化、予測、及び更新ステップを実行し、非静的地図の作成と自己位置推定を行う。ＵＫＦ推定部２３は、初期化、予測、及び更新ステップを実行し、統合処理推定を行う。図５中、「Ｄ」が付されたポートはダイナミック情報入力ポート、「Ｍ」が付されたポートは地図データ入力ポート、「Ｏ」が付されたポートは観測情報入力ポート、左下がりのハッチングが付されたポートはその他の情報入力ポート、右下がりのハッチングが付されたポートは推定結果の出力ポートを夫々示す。又、括弧＜＞内は入出力データの内容を示す。
先ず、ＰＦ推定部２０の初期化ステップに移動開始コマンドが入力されると、初期段階での自己位置推定処理のループが開始する。この例では、外的センサ（例えば、距離センサ１４２）の出力情報（ＬＲＦデータ）、静的地図、及びロボット１の移動履歴である相対移動量を入力して、逐時にロボット１の位置と姿勢を予測（即ち、推定）して更新する。この時、他の推定部２１〜２３は待機状態にある。
次に、ＰＦ推定部２０の切替ステップで他の推定部２１〜２３を起動する条件を判断する。起動条件が満たされれば、初期段階での自己位置推定処理を終了して、ロボット１の位置のサンプルセットを処理結果として出力する。この処理結果は、静的地図を用いた自己位置推定を行うＵＰＦ推定部２１、非静的地図の作成と自己位置推定を行うＵＰＦ部２２、及び統合処理を行うＵＫＦ推定部２３に同時に供給され、各推定部２１〜２３の処理が開始される。
ＵＰＦ推定部２１は、静的地図及び相対移動量に加え、ＵＰＦ推定部２２の生成物である非静的地図を仮想センサの観測情報として入力し、更に、実センサの観測情報、即ち、外的センサ（カメラ１４１及び距離センサ１４２）の観測情報（ランドマークデータ及びＬＲＦデータ）と合わせて自己位置推定を行う。ただし、初期の段階では、非静的地図はまだ作成されていないため、実センサの観測情報のみを使用して自己位置推定を行う。又、ＵＫＦ推定部２３からのフィードバック情報がある場合には、このフィードバック情報を用いて自己位置推定を更新する。
ＵＰＦ推定部２２では、相対移動量に加え、静的地図と非静的地図を同時に利用して実センサの観測情報、即ち、外的センサ（距離センサ１４２）の観測情報（ＬＲＦデータ）に基づいて自己位置推定を行い、非静的地図を更新する。
ＵＫＦ推定部２３では、ＵＰＦ推定部２１，２２から得られる推定したロボット１の現在位置と姿勢を利用して統合処理を行う。統合処理の結果、即ち、ＵＫＦ推定部２３が推定したロボット１の現在位置及び姿勢は、ＵＰＦ推定部２１，２２にフィードバックする。各推定部２１，２２は、ＵＫＦ推定部２３からのフィードバック情報を受け取ると、各自の自己位置推定を更新する。
次に、図２に示す自己位置と地図の推定タスクＳＴ１２が用いるアルゴリズムを、図６と共により詳細に説明する。図６は、自己位置と地図の推定タスクＳＴ１２が用いるアルゴリズムの一例をより詳細に説明する図である。図６中、「観測」なるブロックは観測情報が格納される観測記憶領域、「地図」なるブロックは地図データが格納される地図記憶領域を示し、これらの記憶領域は例えば記憶部１６内に形成可能である。又、記憶部１６が複数の記憶装置で形成されている場合、各処理モジュール（又は、処理部）がアクセスする記憶領域は、各モジュール内に設けた記憶装置で形成しても良いことは言うまでもない。
図６は、自己位置と地図の推定タスクＳＴ１２を実行する７つの処理モジュール（又は、処理部）を示す。７つの処理モジュールには、移動履歴取得部４１、地図管理部４２、センサ管理部４３、ＰＦ推定部４４、ＵＰＦ推定部４５，４６、及びＵＫＦ推定部４７が含まれる。推定部４４〜４７は、図５に示す推定部２０〜２３に相当する。
移動履歴取得部４１は、走行制御ＣＰＵ１２にアクセスしてロボット１の移動履歴を取得して例えば記憶部１６に格納する。地図管理部４２は、ロボット１がサービスを提供する環境の全体的な静的地図、及びロボット１が移動する際に作成された最新の地図（即ち、非静的地図）を管理する。管理される静的地図及び非静的地図は、例えば記憶部１６に格納される。センサ管理部４３は、外的センサ（例えば、カメラ１４１、距離センサ１４２等）の計測データを管理する。管理される計測データは、例えば記憶部１６に格納される。ＰＦ推定部４４は、初期段階での自己位置推定を行い、自己位置推定の結果は他の推定部４５〜４７の状態の初期化に用いられる。
ＵＰＦ推定部４５は、静的地図に基づいて自己位置推定を行う。ＵＰＦ推定部４６は、静的地図に基づいて自己位置及び非静的地図を推定する。ＵＫＦ推定部４７は、ＵＰＦ推定部４５とＵＰＦ推定部４６の推定結果を統合して、より高い精度の自己位置推定を行う。更に、ＵＫＦ推定部４７は、推定結果を統合した結果を各ＵＰＦ推定部４５，４６にフィードバックして反映させることで、各ＵＰＦ推定部４５，４６の推定誤差を軽減する。
以下に、上記７つの処理部４１〜４７が行う処理の詳細を説明する。
移動履歴取得部４１：
ＵＰＦ推定部４５，４６及びＵＫＦ推定部４７の処理時間をtとすると、移動履歴取得部４１は推定部４５〜４７より速い周期で走行制御ＣＰＵ１２にアクセスして、時刻tから時刻t+1の間のロボット１の移動履歴（即ち、位置と姿勢の履歴、以下位置姿勢履歴とも言う）を記憶部１６から取得する。図３（ａ）に示すように、時刻tと時刻t+1に取得したロボット１の位置姿勢履歴のデータのインデックスを夫々kt, kt+1とし、位置姿勢履歴のデータを次式(1)で表すものとする。図３は、移動履歴のデータの一例を説明する図である。
便宜上、以下の説明では上記相対移動系列を簡略化してＵt+1と表記する。相対移動系列Ｕt+1は、図６に示す移動履歴情報保存メモリ４１１に格納される。この移動履歴情報保存メモリ４１１は、例えば記憶部１６により形成可能である。
ロボット１の相対移動量ukについての不確かさ（Uncertainty）Ｍkは、式(9)〜式(12)に従って計算できる。
時刻tから時刻t+1の間のロボット１の相対移動系列Ｕtとその不確かさＭtは、式(13)及び式(14) のようにまとめることができる。
ロボット１がある環境下である程度の期間に運用された場合、環境のレイアウトが導入前に比べて変化している可能性がある。このため、静的地図４２１のみの自己位置推定は不十分であり、ロボット１が移動している間に最新の地図を実時間で作成することが望ましい。非静的地図４２３は、このように実時間に作成された地図である。図４は、非静的地図４２３の一例を説明する図である。非静的地図４２３は、図４に示す如く非静的近辺地図４２３Ａ及び非静的周辺地図４２３Ｂを含む。非静的近辺地図４２３Ａ及び非静的執念地図４２３Ｂは、図６に示す近周辺地図マネージャ４２３Ｃにより管理される。
非静的近辺地図４２３Ａは、ロボット１を中心とした近辺エリアのレイアウト地図である。非静的近辺地図４２３Ａは、例えば記憶部１６の近辺地図記憶領域に格納される。非静的近辺地図４２３Ａは、推定部４５〜４７により比較的頻繁に利用されているため、記憶部１６を形成する比較的高速アクセス可能な記憶装置に格納しても良い。
非静的周辺地図４２３Ｂは、非静的地図４２３から非静的近辺地図４２３Ａを取り除いた地図である。非静的周辺地図４２３Ｂは、例えば記憶部１６の周辺地図記憶領域に格納される。非静的周辺地図４２３Ｂは、推定部４５〜４７が比較的頻繁に利用するものではないので、記憶部１６を形成する記憶装置のうち比較的アクセス速度の遅い記憶装置に格納しても良い。
近周辺地図マネージャ４２３Ｃは、ロボット１の移動により、非静的近辺地図４２３Ａであったデータが非静的周辺地図４２３Ｂのデータになったり、非静的周辺地図３２４Ｂであったデータが非静的近辺地図４２３Ａのデータになったりすることを判断して、非静的近辺地図４２３Ａになったデータを記憶部１６の周辺地図記憶領域から削除して近辺地図記憶領域にコピーする。又、非静的周辺地図４２３Ｂになったデータを記憶部１６の近辺地図記憶領域から削除して、周辺地図記憶領域に移す。
ＰＦ推定部４４：
ＰＦ推定部４４は、パーティクル初期化部４４１、タイマ更新部４４２、パーティクル状態予測部４４３、パーティクル状態評価部４４４、切替器４４５、コピー部４４６、及び現在位置推定値取得部４４７を有する。
ＰＦ推定部４４は、初期段階でのロボット１の自己位置推定及び各推定部４５〜４７の初期化のためのデータの準備を行う。
タイマ更新部４４２は、ＰＦ推定部４４の時刻tをインクリメントして、t=t+1に設定する。パーティクル状態予測部４４３は、移動履歴情報保存メモリ４１１からロボット１の相対移動系列Ｕt+1とその不確かさＭt+1を取得して、ＵＰＦ推定部４５，４６への移動制御系列とする。ロボット１の運動モデルと共に後述する方法により、パーティクル状態初期化部４４１で初期化したパーティクル状態（t=1の場合）、又は、後述するＰＦ推定部４４内のリサンプリングで得られたパーティクル状態（t>1の場合）を更新する。
パーティクル状態評価部４４４は、パーティクル状態の評価値を次式(15)に従って計算する。
切替器４４５は、ＰＦ推定部４４の処理を中断し、ＰＦ推定部４４の推定結果をＵＰＦ推定部４５、ＵＰＦ推定部４６、及びＵＫＦ 推定部３の初期化データとして、各推定部４５〜４７の処理を発動するためのスイッチの機能を有する。切替器４４５は、例えば現在の時刻tは既定の時刻Tth より大きいかを判断する処理を行う。t>Tthの場合は、ＵＰＦ推定部４５、ＵＰＦ推定部４６、及びＵＰＦ推定部３の処理に進む。t<=Tthの場合は、ＰＦ推定部４４のタイマ更新部４４２の処理に戻る。
現在位置とパーティクルセットのコピー部４４６は、ＵＰＦ推定部４５、ＵＰＦ推定部４６、及びＵＫＦ推定部４７に渡すためのデータとパーティクルセットをコピーして例えば記憶部１６に格納する。現在位置推定値取得部４４７は、式(16)〜式(18)に従って最大評価値を持っているパーティクルの状態をこのＰＦ推定部４４の推定結果とする。
ＵＰＦ推定部４５は、パーティクル状態初期化部４５１、タイマ更新部４５２、パーティクル状態予測部４５３、パーティクル状態評価部４５４、現在位置推定値取得部４５５、パーティクル状態更新部４５６、及びリサンプリング部４５７を有する。
ＵＰＦ推定部４５は、初期化、予測、及び更新ステップを実行し、静的地図を用いた自己位置推定を行う。つまり、ＵＰＦ推定部４５は、２種類の地図（静的地図及びランドマーク地図）と３種類のセンサ観測データ（視覚ランドマークデータ、最新のＬＲＦ計測データ、及び非静的近辺地図４５３Ａのデータ）を入力として、ロボット１の自己位置をＵＰＦを用いて推定する。
タイマ更新部４５２は、ＵＰＦ推定部４５の時刻tをインクリメントして、t=t+1に設定する。パーティクル状態予測部４５３は、移動履歴情報保存メモリ４１１からロボット１の相対移動系列Ｕt+1とその不確かさＭt+1を取得して、ＵＰＦ推定部４５への移動制御系列とする。パーティクルの予測更新と共に後述する方法により、パーティクル状態初期化部４５１で初期化したパーティクル状態（t=1の場合）、又は、後述するリサンプリング部４５７で得られたパーティクル状態（t>1の場合）を更新する。
パーティクル状態評価部４５４は、パーティクル状態の評価値を次式(19)に従って計算する。
現在位置推定値取得部４５５は、式(20)〜式(22)に従って最大評価値を持っているパーティクルの状態をこのＵＰＦ推定部４５の推定結果とする。ＵＰＦ推定部４５の推測結果は、ＵＫＦ推定部４７に入力される観測情報として出力される。
ＵＰＦ推定部４６は、パーティクル状態初期化部４６１、タイマ更新部４６２、パーティクル状態予測部４６３、パーティクル状態評価及び地図更新部４６４、現在位置推定値取得部４５５、パーティクル状態更新部４６６、及びリサンプリング部４６７を有する。
ＵＰＦ推定部４６は、初期化、予測、及び更新ステップを実行し、非静的地図の作成と自己位置推定を行う。つまり、ＵＰＦ推定部４５は、２種類の地図（静的地図４２１及びランドマーク地図４２２）と３種類のセンサ観測データ（視覚ランドマークデータ、最新のＬＲＦ計測データ、及び非静的近辺地図４２３Ａのデータ）を入力として、ロボット１の自己位置をＵＰＦを用いて推定する。
タイマ更新更新部４６２は、ＵＰＦ推定部４６の時刻tをt=t+1にインクリメントする。パーティクル状態予測部４６３は、移動履歴情報保存メモリ４１１からロボット１の相対移動系列Ｕt+1とその不確かさＭt+1を取得して、ＵＰＦ推定部４６への移動制御系列とする。パーティクルの予測更新と共に後述する方法により、パーティクル状態初期化部４６１で初期化したパーティクル状態（t=1の場合）、又は、後述するリサンプリング部４６７で得られたパーティクル状態（t>1の場合）を更新する。
パーティクル状態更新部４６５は、ＵＫＦ推定部４７からの推定結果をＵＰＦ推定部４６の観測情報とし、各パーティクルの状態をパーティクルの観測更新と共に後述する方法により更新する。
現在位置推定値取得部４６６は、式(25)〜式(28)に従って最大評価値を持っているパーティクルの状態をこのＵＰＦ推定部４６の推定結果とする。ＵＰＦ推定部４６の推測結果は、ＵＫＦ推定部４７に入力される観測情報として出力される。
ＵＫＦ推定部４７は、初期化部４７１、タイマ更新部４７２、状態予測部４７３、状態更新部４７４、統合後の位置情報取得部４７５、及び位置情報の配分器４７６を有する。
ＵＫＦ推定部４７は、初期化、予測、及び更新ステップを実行し、統合処理推定を行う。つまり、ＵＫＦ推定部４７は、ＵＰＦ推定部４５とＵＰＦ推定部４６の推定結果を観測情報として統合し、ＵＫＦを用いてロボット１の位置姿勢状態をより高い精度で推定する。更に、ＵＫＦ推定部４７は、その推定結果を各ＵＰＦ推定部４５，４６にフィードバックして反映させ、各ＵＰＦ推定部４５，４６のパーティクル状態を更新する。以下の説明では、ＵＫＦ推定部４７の状態を(3x0, 3Σ0)で表す。
初期化部４７１は、現在位置とパーティクルセットのコピー部４４６で記憶部１６に格納されたＰＦ推定部４４で推定したロボット１の位置姿勢及び共分散行列を用いてＵＫＦ推定部４７の状態(3x0, 3Σ0)を設定する。タイマ更新部４７２は、ＵＫＦ推定部４７の時刻tをt=t+1にインクリメントする。
状態予測部４７３は、式(29), (30)で表される、ＵＰＦ推定部４５とＵＰＦ推定部４６の推定結果(1xt+1, 1Σt+1)、(2xt+1, 2Σt+1)を観測情報として取得する。
又、状態予測部４７３は、式(31), (32)で表される拡張行列を形成する。
更に、状態予測部４７３は、式(31), (32)で表される拡張行列から式(33), (34)で表されるシグマポイント（sigma point）を生成する。シグマポイントの数はL3である。
状態予測部４７３は、シグマポイントを用いて式(35), (36)で表される状態を予測する。
状態更新部４７４は、式(37)〜式(39)に従ってシグマポイントを用いて観測情報を予測する。
又、状態更新部４７４は、式(40)〜式(43)に従って状態を更新する。
統合後の位置情報取得部４７５は、ＵＰＦ推定部４５，４６の推定結果を統合し、統合後の位置姿勢状態は次式(44)で表す平均値と共分散行列3Σt+1で表される。統合後の位置姿勢状態は、モータ１３３を含むロボット１の走行制御系に出力されて走行制御系を制御する。
位置情報の配分器４７６は、式(45), (46)に従ってＵＫＦ推定部４７の推定結果をＵＰＦ推定部４５，４６にフィードバックされる観測情報に変換する。
次に、ロボット１の運動モデルの計算式について説明する。時刻t+1にＵＰＦ推定部４５，４６に移動制御系列Ｕt+1を入力した場合、ロボット１の位置姿勢状態は式(47)〜式(49)で表せる。式(47)〜式(49)は、便宜上式(52)のようにまとめて表すことができる。
ＵＰＦ４５，４６におけるパーティクルの予測更新は、時刻tにおけるn番目のパーティクルの状態をnxt、不確かさをnΣtとして、以下のステップＳＴ１，ＳＴ２に従って行える。
ステップＳＴ１では、拡張行列の計算アンセンテッド（Unscented）変換処理中に、シグマポイントの計算を行う。この例では、パーティクルの状態は式(53)に従って計算でき、拡張行列は式(54)に従って計算できる。
ステップＳＴ２では、 シグマポイントによるロボット１の位置姿勢状態の予測拡張行列から式(55)に基づいてシグマポイントを求め、式(52)で表される運動モデルgに代入して、シグマポイントの状態を式(56)のように更新する。次に、各シグマポイントの重み付き平均と共分散行列を式(57),式(58)に基づいて求める。最後に、式(57),式(58) に基づいて得られた結果を平均と分散行列としたガウス分布からランダムサンプルを抽出する。抽出されたランダムサンプルは、パーティクルの新しい状態となる。
パーティクルの観測更新は、以下のように行うことができる。ＵＰＦ推定部５５，５６の観測情報をzt+1で表すと、zt+1=3xt+1となる。ここで、3xt+1は式(44)で表されるＵＫＦ推定部５７の推定結果である。又、観測の不確かさをQt+1で表すと、Qt+1=3Σt+1となる。そこで、ＵＰＦ推定部５５，５６に観測情報が入力された場合には、式(60)〜式(62)によってパーティクルの状態を更新する。
次に、図６に示す処理モジュール（又は、処理部）が実行するステップ（又は、手順）Ｓ１〜Ｓ７２について説明する。
ステップＳ１では、ＰＦ推定部４４のパーティクル状態初期化部４４１が開始コマンドに応答してパーティクル状態を初期化し、時間tをt=0に設定する。ステップＳ２では、ＰＦ推定部４４のタイマ更新部４４２がタイマの時間tをt=t+1に更新する。この場合のタイマ更新部４４２の入力データは、現在の時間t及びパーティクルセットの初期状態を含む。
ステップＳ３では、センサ管理部４３のＬＲＦデータ保持部４３１が距離センサ１４２から計測データ（以下、ＬＲＦデータとも言う）を読み込み、記憶部１６の計測データ記憶領域に格納する。ステップＳ４では、ＬＲＦデータ保持部４３１により記憶部１６の計測データ記憶領域に格納されたＬＲＦデータをＰＦ推定部４４が参照可能（又は、アクセス可能）な記憶部１６内の観測記憶領域（図６中、「観測」なるブロックで示す）にコピーする。ステップＳ５では、センサ管理部４３の画像データ保持部４３２がカメラ１４１から画像データを取得し、記憶部１６内の画像データ記憶領域に格納する。この場合の画像データ保持部４３２の入力データは、画像、ステレオビジョンで計測したレンジデータ、ランドマーク特徴点の位置と特徴ベクトル等の画像データを含む。ステップＳ６では、センサ管理部４３のランドマーク計測部４３３が画像データ保持部４３２が取得して記憶部１６の画像データ記憶領域に格納した画像データから１つ以上のランドマークを検出して、各ランドマークの３次元位置を計測する。この場合のランドマーク計測部４３３の入力データは、画像データ保持部４３２が取得した画像データを含む。
ステップＳ７では、ランドマーク計測部４３３が計測したランドマークの情報をＰＦ推定部４４が参照可能な記憶部１６内の観測記憶領域にコピーする。ステップＳ８では、地図管理部４２の静的地図４２１からロボット１の静的近辺地図データを抽出してＰＦ推定部４４が参照可能な記憶部１６内の地図記憶領域（図６中、「地図」なるブロックで示す）にコピーする。この場合の地図管理部４２の入力データは、静的近辺地図データ（又は、静的レイアウト地図データ）を含む。ステップＳ９では、地図管理部４２のランドマーク地図４２２からロボット１の静的近辺地図データを抽出してＰＦ推定部４４が参照可能な記憶部１６内の地図記憶領域にコピーする。この場合の地図管理部４２の入力データは、ランドマーク地図データを含む。
尚、観測記憶領域及び地図記憶領域は、夫々ＰＦ推定部４４内に設けられた記憶領域であっても良い。
ステップＳ１０では、移動履歴取得部４１の移動履歴情報保存メモリ４１１に保存されているロボット１の移動履歴情報をＰＦ推定部４４内のパーティクル状態予測部４４３に入力する。この場合のパーティクル状態予測部４４３の入力データは、ロボット１の移動履歴を示す移動軌跡（点列）を含む。ステップＳ１１では、パーティクル予測部４４３が時刻t+1におけるＰＦ推定部４４のパーティクル状態を予測する。この場合のパーティクル予測部４４３の入力データは、パーティクルの状態を含む。
ステップＳ１２では、ＰＦ推定部４４のパーティクル状態評価部４４４が記憶部１６内の地図記憶領域に格納された地図データを入力する。ステップＳ１３では、ＰＦ推定部４４のパーティクル情報評価部４４４が記憶部１６内の観測記憶領域に格納された観測データを入力する。この場合のパーティクル情報評価部４４４の入力データは、ＬＲＦデータ、ランドマークデータ等の観測データを含む。ステップＳ１４では、ＰＦ推定部４４のパーティクル状態評価部４４４が各パーティクルの状態を評価する。この場合のパーティクル状態評価部４４４の入力データは、パーティクル状態予測部４４３で予測されたパーティクルセットの状態を含む。
ステップＳ１５では、ＰＦ推定部４４の現在位置推定値取得部４４７が評価されたパーティクルの確率密度分布からＰＦ推定部４４の推定結果を記憶部１６の推定値記憶領域にコピーする。この場合の現在位置推定値取得部４４７の入力データは、パーティクル状態とその評価値、推定した現在の位置と姿勢の情報を含む。ステップＳ１６では、ＰＦ推定部４４の切替器４４５が現在の時刻tが既定時刻より前であるか否かを判定し、規定時刻より前であり判定結果がＹＥＳであると処理はステップＳ２へ戻る。この場合の切替器４４５の入力データは、パーティクル状態とその評価値を含む。又、ステップＳ１７では、切替器４４５が現在の時刻tが既定時刻以降であり判定結果がＮＯであると、処理はステップＳ１８へ進む。この場合の切替器４４５の入力データも、パーティクル状態とその評価値を含む。
ステップＳ１８では、ＵＰＦ推定部４５のパーティクル状態初期化部４５１がパーティクル状態を初期化して時間tをt=0に設定する。この場合のパーティクル状態初期化部４５１の入力データは、パーティクル状態とその評価値を含む。ステップＳ１９では、ＵＰＦ推定部４６のパーティクル状態初期化部４６１がパーティクル状態を初期化して時間tをt=0に設定する。この場合のパーティクル状態初期化部４６１の入力データは、パーティクル状態とその評価値を含む。ステップＳ２０では、ＵＫＦ推定部４７のパーティクル状態初期化部４７１がパーティクル状態を初期化し、時間tをt=0に設定する。この場合のパーティクル状態初期化部４７１の入力データは、パーティクル状態とその評価値を含む。
ステップＳ２１では、ＵＰＦ推定部４５のタイマ更新部４５２がタイマを更新してt=t+1とする。この場合のタイマ更新部４５２の入力データは、現在の時刻tを含む。ステップＳ２２では、ＵＰＦ推定部４６のタイマ更新部４６２がタイマを更新してt=t+1とする。この場合のタイマ更新部４６２の入力データは、現在の時刻tを含む。ステップＳ２３では、ＵＫＦ推定部４７のタイマ更新部４７２がタイマを更新してt=t+1とする。この場合のタイマ更新部４７２の入力データは、現在の時刻tを含む。
ステップＳ２４では、センサ管理部４３のＬＲＦデータ保持部４３１が距離センサ１４２からＬＲＦデータを読み込み、記憶部１６の計測データ記憶領域に格納する。ステップＳ２５では、ＬＲＦデータ保持部４３１により記憶部１６の計測データ記憶領域に格納されたＬＲＦデータをＵＰＦ推定部４５が参照可能な記憶部１６内の観測記憶領域（図６中、「観測」なるブロックで示す）にコピーする。ステップＳ２６では、ＬＲＦデータ保持部４３１により記憶部１６の計測データ記憶領域に格納されたＬＲＦデータをＵＰＦ推定部４６が参照可能な記憶部１６内の観測記憶領域（図６中、「観測」なるブロックで示す）にコピーする。
ステップＳ２７では、センサ管理部４３の画像データ保持部４３２がカメラ１４１から画像データを取得し、記憶部１６内の画像データ記憶領域に格納する。この場合の画像データ保持部４３２の入力データは、画像、ステレオビジョンで計測したレンジデータ、ランドマーク特徴点の位置と特徴ベクトル等の画像データを含む。ステップＳ２８では、センサ管理部４３のランドマーク計測部４３３が画像データ保持部４３２が取得して記憶部１６の画像データ記憶領域に格納した画像データから１つ以上のランドマークを検出して、各ランドマークの３次元位置を計測する。この場合のランドマーク計測部４３３の入力データは、画像データ保持部４３２が取得した画像データを含む。ステップＳ２９では、ランドマーク計測部４３３が計測したランドマークの情報をＵＰＦ推定部４５が参照可能な記憶部１６内の観測記憶領域にコピーする。この場合に観測記憶領域にコピーされるデータは、計測したランドマークの３次元位置情報を含む。ステップＳ３０では、地図管理部４２の静的地図４２１からロボット１の静的近辺地図データを抽出してＰＦ推定部４５が参照可能な記憶部１６内の地図記憶領域（図６中、「地図」なるブロックで示す）にコピーする。この場合に地図記憶領域にコピーされるデータは、静的近辺地図データ（又は、静的レイアウト地図データ）を含む。ステップＳ３１では、地図管理部４２の静的地図４２１からロボット１の静的近辺地図データを抽出してＰＦ推定部４６が参照可能な記憶部１６内の地図記憶領域（図６中、「地図」なるブロックで示す）にコピーする。この場合に地図記憶領域にコピーされるデータは、静的近辺地図データ（又は、静的レイアウト地図データ）を含む。
ステップＳ３２では、地図管理部４２のランドマーク地図４２２からロボット１の静的近辺地図データを抽出してＵＰＦ推定部４５が参照可能な記憶部１６内の地図記憶領域にコピーする。この場合に地図記憶領域にコピーされるデータは、ランドマーク地図データを含む。
ステップＳ３３では、地図管理部４２の近周辺地図マネージャ４２３Ｃから非静的周辺地図４２３Ｂにアクセスする。この場合の近周辺地図マネージャ４２３Ｃの入力データは、非静的地図４２３のレイアウト地図データを含む。ステップＳ３４では近周辺地図マネージャ４２３Ｃがロボット１の近辺エリアに関する非静的近辺地図４２３Ａを取得する。この場合の近周辺地図マネージャ４２３Ｃの入力データは、非静的地図４２３のレイアウト地図データを含む。
ステップＳ３５では、非静的近辺地図４２３をＵＰＦ推定部４５が参照可能な記憶部１６内の観測記憶領域（図６中、「観測」なるブロックで示す）にコピーする。この場合に観測記憶領域にコピーされるデータは、センサの計測データを含む。ステップＳ３６では、非静的近辺地図４２３をＵＰＦ推定部４６が参照可能な記憶部１６内の地図記憶領域（図６中、「地図」なるブロックで示す）にコピーする。この場合に地図記憶領域にコピーされるデータは、地図データを含む。
ステップＳ３７では、移動履歴情報保存メモリ４１１に保存されているロボット１の移動履歴情報をＵＰＦ推定部４５のパーティクル状態予測部４５３に入力する。この場合のパーティクル状態予測部４５３の入力データは、ロボット１の移動軌跡（離散時間で計測した点列の座標）を含む。ステップＳ３８では、移動履歴情報保存メモリ４１１に保存されているロボット１の移動履歴情報をＵＰＦ推定部４６のパーティクル状態予測部４６３に入力する。この場合のパーティクル状態予測部４６３の入力データは、ロボット１の移動軌跡（離散時間で計測した点列の座標）を含む。ステップＳ３９では、移動履歴情報保存メモリ４１１に保存されているロボット１の移動履歴情報をＵＫＦ推定部４７の状態予測部４７３に入力する。この場合の状態予測部４７３の入力データは、ロボット１の移動軌跡（離散時間で計測した点列の座標）を含む。
ステップＳ４０では、パーティクル状態予測部４５３が時刻t+1におけるＵＰＦ推定部４５のパーティクル状態を予測する。この場合のパーティクル状態予測部４５３の入力データは、ロボット１の移動軌跡（離散時間で計測した点列の座標）を含む。ステップＳ４１では、パーティクル状態評価部４５４が新しいパーティクルの状態をＵＰＦ推定部４５が参照可能な記憶部１６内のパーティクル状態記憶領域（図６中、「パーティクル状態」なるブロックで示す）に保存する。
ステップＳ４２では、ＵＰＦ推定部４５のパーティクル状態評価部４５４が記憶部１６内の地図記憶領域から地図データを入力する。ステップＳ４３では、ＵＰＦ推定部４５のパーティクル状態評価部４５４が記憶部１６内の観測記憶領域から観測データ情報を入力する。ステップＳ４４では、パーティクル状態評価部４５４がＵＰＦ推定部４５の各パーティクルの状態を評価する。ステップＳ４５では、現在位置推定値取得部４５５が評価されたパーティクルの確率密度分布からＵＰＦ推定部４５の推定結果を記憶部１６内の推定値記憶領域にコピーする。この場合に推定値記憶領域にコピーされるデータは、パーティクル状態とその評価値、推定した現在位置と姿勢の情報を含む。
ステップＳ４６では、パーティクル状態予測部４６３が時刻t+1におけるＵＰＦ推定部４６のパーティクル状態を予測する。ステップＳ４７では、パーティクル状態予測部４６３が新しいパーティクルの状態をＵＰＦ推定部４６が参照可能な記憶部１６内のパーティクル状態記憶領域（図６中、「パーティクル状態」なるブロックで示す）に保存する。
ステップＳ４８では、ＵＰＦ推定部４６のパーティクル状態評価及び地図更新部４６４が記憶部１６内の地図記憶領域から地図データを入力する。ステップＳ４９では、ＵＰＦ推定部４６のパーティクル状態評価及び地図更新部４６４が記憶部１６内の観測記憶領域から観測データを入力する。ステップＳ５０では、パーティクル状態評価及び地図更新部４６４がＵＰＦ推定部４６の各パーティクルの状態を評価する。
ステップＳ５１では、現在位置推定値取得部４６６が評価されたパーティクルの確率密度分布からＵＰＦ推定部４６の推定結果を記憶部１６内の推定値記憶領域にコピーする。この場合に推定値記憶領域にコピーされるデータは、パーティクル状態とその評価値、推定した現在の位置と姿勢の情報を含む。ステップＳ５２では、ＵＰＦ推定部４６のパーティクル状態評価及び地図更新部４６４で更新した地図データを地図管理部４２内の非静的近辺地図４２３Ａにコピーする。
ステップＳ５３では、近周辺地図マネージャ４２３Ｃが非静的近辺地図４２３Ａにアクセスする。ステップＳ５４では、アクセスした非静的近辺地図４２３Ａにロボット１の周辺エリア内となる地図データがあれば、近周辺地図マネージャ４２３Ｃがその地図データを非静的周辺地図４２３Ｂにアップロードする。
ステップＳ５５では、ＵＰＦ推定部４５の推定結果をＵＫＦ推定部４７が参照可能な記憶部１６内の観測記憶領域にコピーする。この場合に観測記憶領域にコピーされるデータは、推定したロボット１の現在位置と姿勢の情報を含む。ステップＳ５６では、ＵＰＦ推定部４６の推定結果をＵＫＦ推定部４７が参照可能な記憶部１６内の観測記憶領域にコピーする。この場合に観測記憶領域にコピーされるデータは、推定したロボット１の現在位置と姿勢の情報を含む。
ステップＳ５７では、ＵＫＦ推定部４７の状態予測部４７３が記憶部１６内の観測記憶領域から観測データを入力する。この場合の状態予測部４７３の入力データは、センサの計測データを含む。ステップＳ５８では、状態予測部４７３が運動モデルを用いてＵＫＦ推定部４７の状態を予測する。この場合の状態予測部４７３の入力データは、ロボット１の現在位置と姿勢に情報を含む。ステップＳ５９では、状態更新部４７４が観測モデルを用いてＵＫＦ推定部４７の状態を更新する。この場合の状態更新部４７４の入力データは、ロボット１の現在位置と姿勢の情報を含む。ステップＳ６０では、統合後の位置情報取得部４７５が更新したＵＫＦ推定部４７の状態を記憶部１６内の統合後記憶領域にコピーする。この場合の統合後の位置情報取得部４７５の入力データは、ロボット１の現在位置と姿勢の情報を含む。ステップＳ６１では、統合後の位置情報取得部４７５がＵＫＦ推定部４７の推定結果をモータ１３３を含む走行制御系に出力する。この場合のモータ１３３を含む走行制御系の入力データは、ロボット１の現在位置と姿勢の情報を含む。
ステップＳ６２では、統合後の位置情報取得部４７５がＵＫＦ推定部４７の推定結果を位置情報配分器４７６にコピーする。この場合の位置情報分配器４７６の入力データは、ロボット１の現在位置と姿勢の情報を含む。ステップＳ６３では、位置情報分配器４７６がＵＫＦ推定部４７の推定結果の情報をＵＦＫ推定部４７内のタイマ更新部４７３に配分する。この場合のタイマ更新部４７３の入力データは、ロボット１の現在位置と姿勢の情報を含む。
ステップＳ６４では、位置情報分配器４７６がＵＫＦ推定部４７の推定結果の情報をＵＰＦ推定部４５に配分するため、ＵＰＦ推定部４５が参照可能な記憶部１６内の観測記憶領域にコピーする。ステップＳ６５では、位置情報分配器４７６がＵＫＦ推定部４７の推定結果の情報をＵＰＦ推定部４６へ推定結果の情報を配分するため、ＵＰＦ推定部４６が参照可能な記憶部１６内の観測記憶領域にコピーする。
ステップＳ６６では、ＵＰＦ推定部４５のパーティクル状態更新部４５６が記憶部１６内の観測記憶領域から観測データを入力する。この場合のパーティクル状態更新部４５６の入力データは、ロボット１の現在位置と姿勢の情報を含む。ステップＳ６７では、パーティクル状態更新部４５６がＵＰＦ推定部４５の各パーティクルの状態を更新する。ステップＳ６８では、リサンプリング部４５７が状態が更新されたＵＰＦ推定部４５のパーティクルの離散的確率密度分布（Discrete Probability Density Distribution）からリサンプリングを行い、新しいパーティクルセットを生成する。
ステップＳ６９では、ＵＰＦ推定部４６のパーティクル状態更新部４６５が記憶部１６内の観測記憶領域から観測データを入力する。この場合のパーティクル状態更新部４６５の入力データは、ロボット１の現在位置と姿勢の情報を含む。ステップＳ７０では、パーティクル状態更新部４６５がＵＰＦ推定部４６の各パーティクルの状態を更新する。ステップＳ７１では、リサンプリング部４６７が状態が更新されたＵＰＦ推定部４６のパーティクルの離散的確率密度分布からリサンプリングを行い、新しいパーティクルセットを生成する。
ステップＳ７２では、リサンプリング部４５７によるリサンプリング後に処理をＳ２１へ戻し、タイマ更新部４５２によるタイマの更新により次の処理サイクルへ進む。同様に、ステップＳ７３では、リサンプリング部４６７によるリサンプリング後に処理をＳ２２へ戻し、タイマ更新部４６２によるタイマの更新により次の処理サイクルへ進む。
図６に示す例によれば、最新の地図を高速に作成することができる。静的地図が無い場合に、ある計算機とテストデータに基づいて地図を自動作成したところ、地図の自動作成時間は約１．６秒であった。一方、同じ計算機と同じテストデータに基づいて、図６に示す処理モジュールを用いた場合、地図の自動作成時間は約０．２秒であった。
又、図６に示す例によれば、自己位置推定の安定性を向上することができる。図７は、自己位置推定の安定性を説明する図である。図７中、レイアウト変更エリアＬＡではロボットを例えばショッピングセンタに導入後にレイアウトが変更されており、実線で示す推定移動軌跡ＭＬを経て現在位置ＰＰに到達したものとする。図７（ａ）は、非静的地図を成しない従来の処理モジュールの一例を搭載した比較例のロボットによる推定移動軌跡を示す。比較例のロボットの場合、レイアウト変更エリアＬＡの変更に対応できないため、エリアＥＡ内では比較例のロボットの移動軌跡が間違って推定されてしまうことがわかる。これに対し、図７（ｂ）は、図６に示す処理モジュールを搭載されたロボット１による推定移動軌跡を示す。図６に示す処理モジュールを搭載されたロボット１の場合、レイアウト変更エリアＬＡの変更に対応できるため、エリアＣＡ内のロボット１の移動軌跡が正しく推定されることが確認された。
更に、図６に示す例によれば、レイアウトが変化する環境の中でも安定して自己位置推定を行うことができる。図８は、環境レイアウトに変化をもたらす領域や物体等に関する情報を事前に作成せずに自己位置推定を行う従来の処理モジュールの一例を搭載した比較例のロボットと、同じく環境レイアウトに変化をもたらす領域や物体等に関する情報を事前に作成せずに自己位置推定を行うものの図６に示す処理モジュールを搭載したロボット１の両方をあるショッピングセンタ内で１ヶ月間仮運用し、その間に得られた自己位置推定が成功した頻度の分布を示す図である。図８中、縦軸は自己位置推定の成功率（％）を示し、横軸は日数（日）を示す。ロボット１をショッピングセンタに導入した時点Ｔ１以降の前段階では、新しい地図を使って自己位置推定を行うため、比較例のロボットの場合も図６に示す処理モジュールを搭載したロボット１の場合も、自己位置の成功率は略１００％であった。一方、レイアウトの変更時Ｔ２以後は、実際のレイアウトが局所的に事前地図と一致しないため、比較例のロボットによる自己位置推定の成功率が低下した。しかし、時点Ｔ３で上記時点Ｔ１以降の前段階の状態にあり図６に示す処理モジュールを搭載したロボット１をショッピングセンタに導入したところ、ロボット１による自己位置推定の成功率は略１００％まで回復することが確認できた。
又、推定部４４，４５，４６では、移動履歴取得部４１の移動履歴情報保存メモリ４１１に保存されているロボット１の移動履歴情報（移動軌跡（点列）を含む）をパーティクルの状態予測に用いるので、パーティクルの状態予測を高精度に行うことができる。
ＰＦ推定部４４は、静的地図との整合性をある程度取ったパーティクルのみを保存するので、最初に作成した非静的地図と静的地図の地図データが一致する度合いを比較的高く設定することができる。
ＵＰＦ推定部４４，４５は、実センサの計測データを時間的に累積処理して得られた非静的地図を観測情報として静的地図と照合するので、環境の局所的変化による自己位置推定への影響を抑制することができる。又、非静的地図と静的地図との照合により、自動的に作成した非静的地図と静的地図の地図データが一致する度合いを比較的高く設定することができる。
更に、ＵＫＦ推定部４７は、統合した推定結果を各ＵＰＦ推定部４５，４６へフィードバックするので、各ＵＰＦ推定部４５，４６の自己位置推定精度を向上させることができ、静的地図と非静的地図との整合性を取ってＵＫＦ推定部４７自体の自己位置推定精度を更に向上することができる。
次に、図２に示す自己位置と地図の推定タスクＳＴ１２が用いる他のアルゴリズムを、図９と共により詳細に説明する。図９は、自己位置と地図の推定タスクＳＴ１２が用いるアルゴリズムの他の例を詳細に説明する図である。図９中、図６と同一部分には同一符号を付し、その説明は省略する。
図９に示す例では、ＵＫＦを利用したＵＫＦ推定部４７の代わりに、信念度（Belief）の統合を利用した信念度統合部２４７が設けられている。信念度統合部２４７は、ＵＰＦ推定部４５とＵＰＦ推定部４６で推定したパーティクルの離散的な確率密度分布に基づいて計算した信念度のセットを統合して、推論によって真値を推定する。
ＵＰＦ推定部４５−１は、状態の離散的確率分布取得部４５５−１、期待値計算部４５８−１、及び確率分布修正部４５８−２を有する。サンプル区間信念度生成部１４５１、上下限確率分布関数生成部１４５２、上下限確率分布逆関数算出部１４５３、及び逆関数離散化部１４５４の少なくとも一部は、ＵＰＦ推定部４５−１内に設けられていても、信念度統合部２４７内に設けられていても良い。後述するように、ＵＰＦ推定部４５−１の期待値計算部４５８−１は、期待値をモータ１３３を含むロボット１の走行制御系に出力する。
ＵＰＦ推定部４６−１は、状態の離散的確率分布取得部４６６−１、期待値計算部４６８−１、及び確率分布修正部４６８−２を有する。サンプル区間信念度生成部１４６１、上下限確率分布関数生成部１４６２、上下限確率分布逆関数算出部１４６３、及び逆関数離散化部１４６４の少なくとも一部は、ＵＰＦ推定部４６−１内に設けられていても、信念度統合部２４７内に設けられていても良い。
信念度統合部２４７は、離散的上下限確率分布関数統合部２４７１、統合した上下限確率分布逆関数算出部２４７２、統合した上下限確率分布逆関数離散化部２４７３、及び送信データ抽出部２４７４を有する。
図９に示す処理モジュール（又は、処理部）が実行するステップ（又は、手順）Ｓ１０１〜Ｓ１８３について説明する。
図９において、ステップＳ１０１〜Ｓ１１９，Ｓ１２１，Ｓ１２２，Ｓ１２４〜Ｓ１３８，Ｓ１４０〜Ｓ１４４，Ｓ１４６〜Ｓ１５０は、図６に示すステップＳ１〜Ｓ１９，Ｓ２１，Ｓ２２，Ｓ２４〜Ｓ３８，Ｓ４０〜Ｓ４４，Ｓ４６〜Ｓ５０と同様である。図９に示す例では、図６に示すステップＳ２０，Ｓ２３，Ｓ３９が含まれない。
図９に示すステップＳ１４５では、ＵＰＦ推定部４５−１の状態の離散的確率分布取得部４５５−１が評価されたパーティクルの状態の離散的確率密度分布を記憶部１６内の確率分布記憶領域にコピーする。この場合に確率分布記憶領域にコピーされるデータは、パーティクル状態とその評価値と確率密度分布、推定した現在の位置と姿勢の情報を含む。又、ステップＳ１５１では、ＵＰＦ推定部４６−１の状態の離散的確率分布取得部４６６−１が評価されたパーティクルの状態の離散的確率密度分布を記憶部１６内の確率分布記憶領域にコピーする。この場合に確率分布記憶領域にコピーされるデータは、パーティクル状態とその評価値と確率密度分布、推定した現在の位置と姿勢の情報を含む。
ステップＳ１５２では、ＵＰＦ推定部４６のパーティクル状態評価及び地図更新部４６４で更新した地図データを地図管理部４２内の非静的近辺地図４２３Ａにコピーする。ステップＳ１５３では、近周辺地図マネージャ４２３Ｃが非静的近辺地図４２３Ａにアクセスする。ステップＳ１５４では、アクセスした非静的近辺地図４２３Ａにロボット１の周辺エリア内となる地図データがあれば、近周辺地図マネージャ４２３Ｃがその地図データを非静的周辺地図４２３Ｂにアップロードする。
ステップＳ１５５では、サンプル区間信念度生成部１４５１が記憶部１６内の確率分布領域に格納された離散的確率密度分布に基づいてサンプル区間信念度を生成する。パーティクルの状態区間の信念度（Belief）の形成について説明すると、パーティクルフィルタで推定を行う際、観測対象の状態の確率密度分布は一般的に図１０（ａ）のようなベイジアン（Bayesian）理論に従った方式で表せるので、尤度（Likelihood）をｗｉ、状態をｘｉで示すと、確率密度の信念度は｛ｘｉ；ｗｉ｝で表すことができる。このように、１つの状態量に対して１つの評価値である尤度が付与される。図１０は、区間信念度を説明する図である。
この例では、デンプスター・シェーファー（Dempster-Shafer）理論にもとづいて、ベイジアン方式の表現の信念度を状態量の区間の信念度に置き換える。即ち、１つの状態量の区間（以下、状態区間と言う）に対して１つの評価値である尤度を付与する。図１０（ｂ）は各パーティクルの状態区間の信念度の表現形式を説明する図であり、状態区間の信念度は、パーティクル状態の不確かさσを考慮して｛[ａｉ，ｂｉ]；ｗｉ｝で表される。ここで、ａｉ＝ｘｉ−σ，ｂｉ＝ｘｉ＋σである。パーティクルの状態値は状態空間上では１つのポイントのみを示しており、ポイントの近辺の状態は無視されている。区間信念度を導入することによって、特定のポイントだけを考慮するのではなく、そのポイントの近隣領域（又は、集合）をも同時に考慮することができる。
つまり、確率密度分布｛ｘｉ；ｗｉ｝は状態空間上の１つのポイント（ｘｉ）だけに対しての尤度を示しており、ポイントの近傍の状態は無視される。これに対し、区間信念度｛[ａｉ，ｂｉ]；ｗｉ｝を導入することにより、特定のポイントｘｉだけを考慮するのではなく、ポイントの近傍領域をも同時に考慮することができる。これにより、後述する上下限確率密度関数を利用して、他のパーティクルフィルタから得られた上下限確率密度関数との統合が可能となる。
ステップＳ１５７では、上下限確率分布逆関数生成部１４５３が上下限確率分布関数生成部１４５２で生成された上下限確率関数の逆関数を生成する。ステップＳ１５６で生成した上下限確率関数は離散的であるため、生成した逆関数も離散的である。
ステップＳ１５８では、逆関数離散化部１４５４が上下限確率関数の逆関数を以下のように離散化する。
ステップＳ１５９では、逆関数離散化部１４５４が送信データ｛Ｄ｝を信念度統合部２４７に送信する。
ステップＳ１７０では、ＵＰＦ推定部４５−１の確率分布修正部４５８−２が観測記憶領域にコピーされたデータを修正する。具体的には、先ず、パーティクルフィルタが推定した離散的確率密度分布｛ｘｉ；ｗｉ｝を取得して区間信念度｛[ａｉ，ｂｉ]；ｗｉ｝に書き直す。ただし、ａｉ＝ｘｉ−σ，ｂｉ＝ｘｉ＋σである。次に、以下に示す式に従って重みｗｉを修正し、修正後の離散確率密度分布を｛ｘｉ；πｉ｝とする。
ステップＳ１７１では、ＵＰＦ推定部４５−１の期待値計算部４５８−１が修正後の確率密度分布｛ｘｉ；πｉ｝を用いて、重み平均を計算して真値の推定値、即ち、期待値とする。ステップＳ１７２では、期待値計算部４５８−１が期待値をモータ１３３を含むロボット１の走行制御系に出力して走行制御系を制御する。
ステップＳ１７３では、期待値計算部４５８−１が期待値をパーティクル状態更新部４５６に出力する。ステップＳ１７４では、パーティクル状態更新部４５６が期待値に基づいてパーティクルの状態を新しいパーティクルの状態に更新する。ステップＳ１７５では、リサンプリング部４５７が新しいパーティクルの状態と重みπｉに基づいて新しいパーティクルのリサンプリングを行う。
ステップＳ１７６では、送信データ抽出部２４７４が送信記憶領域にコピーされたデータをＵＰＦ推定部４６−１がアクセス可能な観測記憶領域にコピーする。ステップＳ１７７では、送信データ抽出部２４７４が送信記憶領域にコピーされたデータをＵＰＦ推定部４５−１がアクセス可能な観測記憶領域にコピーする。
ステップＳ１７８では、ＵＰＦ推定部４６−１の確率分布修正部４６８−２が観測記憶領域にコピーされたデータを修正する。具体的には、先ず、パーティクルフィルタが推定した離散的確率密度分布｛ｘｉ；ｗｉ｝を取得して区間信念度｛[ａｉ，ｂｉ]；ｗｉ｝に書き直す。ただし、ａｉ＝ｘｉ−σ，ｂｉ＝ｘｉ＋σである。上記ステップＳ１７０と共に説明した式に従って重みｗｉを修正し、修正後の離散確率密度分布を｛ｘｉ；πｉ｝とする。ステップＳ１７９では、ＵＰＦ推定部４６−１の期待値計算部４６８−１が修正後の確率密度分布｛ｘｉ；πｉ｝を用いて、重み平均を計算して真値の推定値、即ち、期待値とする。
ステップＳ１７９では、期待値計算部４６８−１が期待値をパーティクル状態更新部４５６に出力する。ステップＳ１８０では、パーティクル状態更新部４６５が期待値に基づいてパーティクルの状態を新しいパーティクルの状態に更新する。ステップＳ１８１では、リサンプリング部４６７が新しいパーティクルの状態と重みπｉに基づいて新しいパーティクルのリサンプリングを行う。
ステップＳ１８２では、リサンプリング部４５７によるリサンプリング後に処理をＳ１２１へ戻し、ＵＰＦ推定部４５−１のタイマ更新部４５２によるタイマの更新により次の処理サイクルへ進む。同様に、ステップＳ１８３では、リサンプリング部４６７によるリサンプリング後に処理をＳ２２へ戻し、ＵＰＦ推定部４６−１のタイマ更新部４６２によるタイマの更新により次の処理サイクルへ進む。
図９に示す例によれば、図６に示す例の場合と同様の効果を得ることができる。又、信念度統合部２４７では、離散的な確率密度分布から生成した信念度を統合しているため、図６に示す例と比較すると、非ガウス分布の状態に対してもより精度の高い推定結果が得られる。
図１１は、ロボットの遠隔操作を説明する図である。ロボット１は、図１１に示すように、サーバ（又はセンタ）９０１と通信可能な構成を有し、サーバ９０１からサービスの提供タイミング等を遠隔操作により制御されるものであっても良い。サーバ９０１は、メモリ９０２、通信部９０３、及びＣＰＵ９０４を有する。図１１では、説明の便宜上、ロボット１内の通信部８０１以外の部分の図示は省略するが、通信部８０１は例えば図１に示すナビゲーションＣＰＵ１１及び走行制御ＣＰＵ１２の少なくとも一方に接続されている。
上記の各例では、ロボット１が自己位置推定に用いる各種データがロボット１内の記憶部１６に格納されているものとしたが、少なくともデータの一部をロボット１の制御及び管理を司るサーバ９０１内の記憶部９０２に格納しても良い。この場合、ロボット１の通信部８０１は、例えば無線ネットワーク９１１を介してサーバ９０１の通信部９０３と通信することで、自己位置推定に用いる各種データを取得すれば良い。サーバ９０１内の記憶部９０２に格納可能なデータには、観測記憶領域、地図記憶領域、近辺地図記憶領域、周辺地図記憶領域、計測データ記憶領域、画像データ記憶領域、推定値記憶領域、パーティクル状態記憶領域、統合後記憶領域、確率分布記憶領域、及び送信記憶領域等の記憶部１６に格納されるデータが含まれる。又、図６又は図９に示す地図管理部４３の機能の少なくとも一部をサーバ９０１側で実現するようにしても良い。自己位置推定に用いる各種データの少なくとも一部をサーバ９０１側に格納することで、ロボット１内で必要となる記憶容量を減らし、ロボット１内で必要となるデータ管理の負荷を低減可能となる。
開示の位置推定方法、位置推定装置及びプログラムの適用は、上記実施例の如き自律移動型のロボットに限定されるものではなく、各種自律移動型の装置や、携帯型の電子装置、例えば携帯電話、携帯端末、携帯型パーソナルコンピュータ等にも適用可能であることは言うまでもない。
以上の実施例を含む実施形態に関し、更に以下の付記を開示する。
（付記１）
コンピュータによる電子装置の位置推定方法であって、
前記コンピュータが、前記電子装置の移動履歴を取得して記憶部に一時的に格納する移動履歴取得工程と、
前記コンピュータが、前記電子装置の移動履歴と、外的センサの観測情報と、前記電子装置が導入される環境の静的地図のデータと、前記電子装置の移動中に実時間で作成された非静的地図のデータに基づいて、前記電子装置の自己位置と前記電子装置の周囲の周辺地図を推定する推定工程
を含むことを特徴とする、位置推定方法。
（付記２）
前記推定工程は、前記コンピュータに含まれる第１の推定部により前記コンピュータが、
移動開始コマンドに応答して、前記外的センサの観測情報、前記静的地図、及び前記電子装置の移動履歴を入力して逐時に前記電子装置の位置と姿勢を推定して更新する初期段階での自己位置推定処理のループを開始する初期化工程と、
前記コンピュータに含まれ前記初期化工程では待機状態にある第２〜第４の推定部を起動する条件を判断し、前記条件が満たされると前記初期段階での自己位置推定処理を終了して、前記電子装置の位置のサンプルセットを含む推定結果を前記第２〜第４の推定部に同時に出力する切替工程を含むことを特徴とする、付記１記載の位置推定方法。
（付記３）
第１の推定部では、前記コンピュータが、前記外的センサの観測情報及び前記静的地図に基づいて前記電子装置の移動中に実時間で非静的地図を作成し、
前記第２の推定部では、前記コンピュータが、前記静的地図、前記移動履歴、及び前記非静的地図に基づいて前記電子装置の自己位置を推定し、
前記第３の推定部では、前記コンピュータが、前記移動履歴、前記静的地図、及び前記非静的地図を入力して前記外的センサの観測情報に基づいて前記電子装置の自己位置を推定して前記非静的地図を更新し、
前記第４の推定部では、前記コンピュータが、前記第２及び第３の推定部から得られる推定された前記電子装置の現在位置と姿勢を統合した統合処理結果を出力することを特徴とする、付記２記載の位置推定方法。
（付記４）
前記第２及び第３の推定部の各々は、前記第４の推定部が出力した前記統合処理結果のフィードバックを受けると、前記統合処理結果に基づいて各自の自己位置の推定を更新することを特徴とする、付記３記載の位置推定方法。
（付記５）
前記第１の推定部はパーティクルフィルタ（ＰＦ）を用いた推定を行い、
前記第２及び第３の推定部は夫々無香粒子フィルタ（ＵＰＦ）を用いた推定を行い、
前記第４の推定部は無香カルマンフィルタ（ＵＫＦ）を用いた推定を行うことを特徴とする、付記２乃至４のいずれか１項記載の位置推定方法。
（付記６）
前記コンピュータが、前記電子装置の自己位置と前記周辺地図に基づいて、設定された経路に従って前記電子装置が目的地まで移動するための目標経路を生成して出力する経路計画工程
を更に含むことを特徴とする、付記１乃至５のいずれか１項記載の位置推定方法。
（付記７）
外的センサの観測情報及び電子装置が導入される環境の静的地図に基づいて前記電子装置の移動中に実時間で非静的地図を作成する第１の推定部と、
前記静的地図、前記電子装置の移動履歴、及び前記非静的地図に基づいて前記電子装置の自己位置を推定する第２の推定部と、
前記移動履歴、前記静的地図、及び前記非静的地図を入力して前記外的センサの観測情報に基づいて前記電子装置の自己位置を推定して前記非静的地図を更新する第３の推定部と、
前記第２及び第３の推定部から得られる推定された前記電子装置の現在位置と姿勢を統合した統合処理結果を出力する前記第４の推定部
を備えたことを特徴とする、位置推定装置。
（付記８）
前記第１の推定部は、
移動開始コマンドに応答して、前記外的センサの観測情報、前記静的地図、及び前記電子装置の移動履歴を入力して逐時に前記電子装置の位置と姿勢を推定して更新する初期段階での自己位置推定処理のループを開始し、
前記初期化工程では待機状態にある前記第２〜第４の推定部を起動する条件を判断し、前記条件が満たされると前記初期段階での自己位置推定処理を終了して、前記電子装置の位置のサンプルセットを含む推定結果を前記第２〜第４の推定部に同時に出力することを特徴とする、付記７記載の位置推定装置。
（付記９）
前記第２及び第３の推定部の各々は、前記第４の推定部が出力した前記統合処理結果のフィードバックを受けると、前記統合処理結果に基づいて各自の自己位置の推定を更新することを特徴とする、付記８記載の位置推定装置。
（付記１０）
前記第１の推定部はパーティクルフィルタ（ＰＦ）を有し、
前記第２及び第３の推定部は夫々無香粒子フィルタ（ＵＰＦ）を有し、
前記第４の推定部は無香カルマンフィルタ（ＵＫＦ）を有することを特徴とする、付記７乃至９のいずれか１項記載の位置推定装置。
（付記１１）
コンピュータに電子装置の位置を推定させるプログラムであって、
前記電子装置の移動履歴を取得して記憶部に一時的に格納する移動履歴取得手順と、
前記電子装置の移動履歴と、外的センサの観測情報と、前記電子装置が導入される環境の静的地図のデータと、前記電子装置の移動中に実時間で作成された非静的地図のデータに基づいて、前記電子装置の自己位置と前記電子装置の周囲の周辺地図を推定する推定手順
を前記コンピュータに実行させることを特徴とする、プログラム。
（付記１２）
前記推定手順は、前記プログラムで実現される第１の推定部により前記コンピュータが、
移動開始コマンドに応答して、前記外的センサの観測情報、前記静的地図、及び前記電子装置の移動履歴を入力して逐時に前記電子装置の位置と姿勢を推定して更新する初期段階での自己位置推定処理のループを開始する初期化手順と、
前記プログラムで実現され前記初期化手順では待機状態にある第２〜第４の推定部を起動する条件を判断し、前記条件が満たされると前記初期段階での自己位置推定処理を終了して、前記電子装置の位置のサンプルセットを含む推定結果を前記第２〜第４の推定部に同時に出力する切替手順
を実行することを特徴とする、付記１１記載のプログラム。
（付記１３）
第１の推定部により、前記コンピュータが、前記外的センサの観測情報及び前記静的地図に基づいて前記電子装置の移動中に実時間で非静的地図を作成し、
前記第２の推定部により、前記コンピュータが、前記静的地図、前記移動履歴、及び前記非静的地図に基づいて前記電子装置の自己位置を推定し、
前記第３の推定部により、前記コンピュータが、前記移動履歴、前記静的地図、及び前記非静的地図を入力して前記外的センサの観測情報に基づいて前記電子装置の自己位置を推定して前記非静的地図を更新し、
前記第４の推定部により、前記コンピュータが、前記第２及び第３の推定部から得られる推定された前記電子装置の現在位置と姿勢を統合した統合処理結果を出力することを特徴とする、付記１２記載のプログラム。
（付記１４）
前記第２及び第３の推定部の各々は、前記第４の推定部が出力した前記統合処理結果のフィードバックを受けると、前記統合処理結果に基づいて各自の自己位置の推定を更新することを特徴とする、付記１３記載のプログラム。
（付記１５）
前記第１の推定部はパーティクルフィルタ（ＰＦ）を用いた推定を行い、
前記第２及び第３の推定部は夫々無香粒子フィルタ（ＵＰＦ）を用いた推定を行い、
前記第４の推定部は無香カルマンフィルタ（ＵＫＦ）を用いた推定を行うことを特徴とする、付記１２乃至１４のいずれか１項記載のプログラム。
（付記１６）
前記電子装置の自己位置と前記周辺地図に基づいて、設定された経路に従って前記電子装置が目的地まで移動するための目標経路を生成して出力する経路計画手順
を更に前記コンピュータに実行させることを特徴とする、付記１１乃至１５のいずれか１項記載のプログラム。
以上、開示の位置推定方法、位置推定装置及びプログラムを実施例により説明したが、本発明は上記実施例に限定されるものではなく、本発明の範囲内で種々の変形及び改良が可能であることは言うまでもない。
１ ロボット
１１ ナビゲーションＣＰＵ
１２ 走行制御ＣＰＵ
１３ 台車
１４ センサ部
１５ 入出力部
１６ 記憶部
２０，４４ ＰＦ推定部
２１，２２，４５，４５−１，４６，４６−１ ＵＰＦ推定部
２３，４７ ＵＫＦ推定部
２４７ 信念度統合部

Claims (4)

  1. コンピュータに電子装置の位置を推定させるプログラムであって、
    内的センサの観測情報と、外的センサの観測情報と、前記電子装置が導入される環境の静的地図のデータとに基づいて推定した前記電子装置の位置と姿勢を含む移動履歴を取得して記憶部に一時的に格納する移動履歴取得手順と、
    前記移動履歴及び前記外的センサの観測情報に基づき前記電子装置の移動中に実時間で作成された非静的地図のデータと、前記静的地図のデータとの照合に基づいて、前記電子装置の自己位置と前記電子装置の周囲の周辺地図を推定する推定手順と
    を前記コンピュータに実行させ、
    前記推定手順は、前記プログラムで実現される第１の推定部により前記コンピュータが、
    移動開始コマンドに応答して、前記外的センサの観測情報、前記静的地図のデータ、及び前記移動履歴を入力して逐時に前記電子装置の位置と姿勢を推定して更新する初期段階での自己位置推定処理のループを開始する初期化手順と、
    前記プログラムで実現され前記初期化手順では待機状態にある第２、第３、及び第４の推定部を起動する条件を判断し、前記条件が満たされると前記初期段階での前記自己位置推定処理を終了して、前記電子装置の位置のサンプルセットを含む推定結果を前記第２、第３、及び第４の推定部に同時に出力する切替手順と
    を実行し、
    前記第１の推定部により、前記コンピュータが、前記外的センサの観測情報、前記静的地図のデータ、及び前記移動履歴に基づいて前記電子装置の移動中に実時間で前記非静的地図のデータを作成し、
    前記第２の推定部により、前記コンピュータが、前記静的地図のデータ、前記移動履歴、及び前記非静的地図のデータを入力として、前記非静的地図のデータと前記外的センサの観測情報とを合わせた観測情報と、前記静的地図のデータとに基づいて前記電子装置の自己位置を推定し、
    前記第３の推定部により、前記コンピュータが、前記移動履歴、前記静的地図のデータ、及び前記非静的地図のデータを入力として、前記外的センサの観測情報と、前記静的地図のデータと前記非静的地図のデータとを合わせた地図のデータとに基づいて前記電子装置の自己位置を推定して前記非静的地図のデータを更新し、
    前記第４の推定部により、前記コンピュータが、前記第２の推定部から得られる推定された前記自己位置及び前記第３の推定部から得られる推定された前記自己位置に基づき、前記電子装置の現在位置と姿勢を統合した統合処理結果を出力し、
    前記第２及び第３の推定部の各々は、前記第４の推定部が出力した前記統合処理結果のフィードバックを受けると、前記統合処理結果に基づいて各自の自己位置の推定を更新することを特徴とする、プログラム。
  2. 前記第１の推定部はパーティクルフィルタ（ＰＦ）を用いた推定を行い、
    前記第２及び第３の推定部は夫々無香粒子フィルタ（ＵＰＦ）を用いた推定を行い、
    前記第４の推定部は無香カルマンフィルタ（ＵＫＦ）を用いた推定を行うことを特徴とする、請求項１記載のプログラム。
  3. 外的センサの観測情報、電子装置が導入される環境の静的地図のデータ、及び前記電子装置の移動履歴に基づいて前記電子装置の移動中に実時間で非静的地図のデータを作成する第１の推定部と、
    前記移動履歴は、内的センサの観測情報と、前記外的センサの観測情報と、前記静的地図のデータとに基づいて推定した前記電子装置の位置と姿勢を含み、
    前記静的地図のデータ、前記移動履歴、及び前記非静的地図のデータを入力として、前記非静的地図のデータと前記外的センサの観測情報とを合わせた観測情報と、前記静的地図のデータとに基づいて前記電子装置の自己位置を推定する第２の推定部と、
    前記移動履歴、前記静的地図のデータ、及び前記非静的地図のデータを入力として、前記外的センサの観測情報と、前記静的地図のデータと前記非静的地図のデータとを合わせた地図のデータとに基づいて前記電子装置の自己位置を推定して前記非静的地図のデータを更新する第３の推定部と、
    前記第２の推定部から得られる推定された前記自己位置及び前記第３の推定部から得られる前記自己位置に基づき、前記電子装置の現在位置と姿勢を統合した統合処理結果を出力する前記第４の推定部と
    を備え、
    前記第１の推定部は、
    移動開始コマンドに応答して、前記外的センサの観測情報、前記静的地図のデータ、及び前記移動履歴を入力して逐時に前記電子装置の位置と姿勢を推定して更新する初期段階での自己位置推定処理のループを開始し、
    前記初期段階での前記自己位置推定処理のループの開始時には待機状態にある前記第２、第３、及び第４の推定部を起動する条件を判断し、前記条件が満たされると前記初期段階での前記自己位置推定処理を終了して、前記電子装置の位置のサンプルセットを含む推定結果を前記第２、第３、及び第４の推定部に同時に出力し、
    前記第２及び第３の推定部の各々は、前記第４の推定部が出力した前記統合処理結果のフィードバックを受けると、前記統合処理結果に基づいて各自の自己位置の推定を更新することを特徴とする、位置推定装置。
  4. コンピュータによる電子装置の位置推定方法であって、
    前記コンピュータが、内的センサの観測情報と、外的センサの観測情報と、前記電子装置が導入される環境の静的地図のデータとに基づいて推定した前記電子装置の位置と姿勢を含む移動履歴を取得して記憶部に一時的に格納する移動履歴取得工程と、
    前記コンピュータが、前記移動履歴及び前記外的センサの観測情報に基づき前記電子装置の移動中に実時間で作成された非静的地図のデータと、前記静的地図のデータとの照合に基づいて、前記電子装置の自己位置と前記電子装置の周囲の周辺地図を推定する推定工程と
    を含み、
    前記推定工程は、前記コンピュータに含まれる第１の推定部により前記コンピュータが、
    移動開始コマンドに応答して、前記外的センサの観測情報、前記静的地図のデータ、及び前記移動履歴を入力して逐時に前記電子装置の位置と姿勢を推定して更新する初期段階での自己位置推定処理のループを開始する初期化工程と、
    前記コンピュータに含まれ前記初期化工程では待機状態にある第２、第３、及び第４の推定部を起動する条件を判断し、前記条件が満たされると前記初期段階での前記自己位置推定処理を終了して、前記電子装置の位置のサンプルセットを含む推定結果を前記第２、第３、及び第４の推定部に同時に出力する切替工程と
    を実行し、
    前記第１の推定部により、前記コンピュータが、前記外的センサの観測情報、前記静的地図のデータ、及び前記移動履歴に基づいて前記電子装置の移動中に実時間で前記非静的地図のデータを作成し、
    前記第２の推定部により、前記コンピュータが、前記静的地図のデータ、前記移動履歴、及び前記非静的地図のデータを入力として、前記非静的地図のデータと前記外的センサの観測情報とを合わせた観測情報と、前記静的地図のデータとに基づいて前記電子装置の自己位置を推定し、
    前記第３の推定部により、前記コンピュータが、前記移動履歴、前記静的地図のデータ、及び前記非静的地図のデータを入力として、前記外的センサの観測情報と、前記静的地図のデータと前記非静的地図のデータとを合わせた地図のデータとに基づいて前記電子装置の自己位置を推定して前記非静的地図のデータを更新し、
    前記第４の推定部により、前記コンピュータが、前記第２の推定部から得られる推定された前記自己位置及び前記第３の推定部から得られる推定された前記自己位置に基づき、前記電子装置の現在位置と姿勢を統合した統合処理結果を出力し、
    前記第２及び第３の推定部の各々は、前記第４の推定部が出力した前記統合処理結果のフィードバックを受けると、前記統合処理結果に基づいて各自の自己位置の推定を更新することを特徴とする、位置推定方法。
JP2010250319A 2010-11-08 2010-11-08 位置推定方法、位置推定装置及びプログラム Expired - Fee Related JP5793851B2 (ja)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2010250319A JP5793851B2 (ja) 2010-11-08 2010-11-08 位置推定方法、位置推定装置及びプログラム

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2010250319A JP5793851B2 (ja) 2010-11-08 2010-11-08 位置推定方法、位置推定装置及びプログラム

Publications (2)

Publication Number Publication Date
JP2012103819A JP2012103819A (ja) 2012-05-31
JP5793851B2 true JP5793851B2 (ja) 2015-10-14

Family

ID=46394162

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2010250319A Expired - Fee Related JP5793851B2 (ja) 2010-11-08 2010-11-08 位置推定方法、位置推定装置及びプログラム

Country Status (1)

Country Link
JP (1) JP5793851B2 (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2019525342A (ja) * 2016-08-05 2019-09-05 ロブアート ゲーエムベーハーＲＯＢＡＲＴ ＧｍｂＨ 自律移動ロボッを制御する方法

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3734394A1 (en) * 2015-05-23 2020-11-04 SZ DJI Technology Co., Ltd. Sensor fusion using inertial and image sensors
JP6320542B2 (ja) 2015-05-23 2018-05-09 エスゼット ディージェイアイ テクノロジー カンパニー リミテッドＳｚ Ｄｊｉ Ｔｅｃｈｎｏｌｏｇｙ Ｃｏ．，Ｌｔｄ 初期構成を有する複数のセンサを有する可動物体に対する１または複数の外部パラメータを推定する方法、システム、及びプログラム
JP6775263B2 (ja) * 2016-12-02 2020-10-28 深▲せん▼前海達闥云端智能科技有限公司Ｃｌｏｕｄｍｉｎｄｓ （Ｓｈｅｎｚｈｅｎ） Ｒｏｂｏｔｉｃｓ Ｓｙｓｔｅｍｓ Ｃｏ．，Ｌｔｄ． 測位方法及び装置
WO2019098002A1 (ja) * 2017-11-20 2019-05-23 ソニー株式会社 情報処理装置、情報処理方法、プログラム、及び移動体
JP6800918B2 (ja) * 2018-07-12 2020-12-16 エスゼット ディージェイアイ テクノロジー カンパニー リミテッドＳｚ Ｄｊｉ Ｔｅｃｈｎｏｌｏｇｙ Ｃｏ．，Ｌｔｄ エラー回復を実行するための方法、システム、及びプログラム
JP2021056764A (ja) * 2019-09-30 2021-04-08 日本電産株式会社 移動体
WO2022186354A1 (ja) * 2021-03-03 2022-09-09 株式会社Preferred Networks 情報処理装置および自律移動ロボット
KR102478451B1 (ko) * 2021-05-04 2022-12-19 국방과학연구소 연산 장치 및 이를 이용한 시스템의 상태 추정 방법

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2009193240A (ja) * 2008-02-13 2009-08-27 Toyota Motor Corp 移動ロボット及び環境地図の生成方法
KR101503903B1 (ko) * 2008-09-16 2015-03-19 삼성전자 주식회사 이동 로봇의 지도 구성 장치 및 방법
JP5298741B2 (ja) * 2008-10-01 2013-09-25 村田機械株式会社 自律移動装置
JP2011209203A (ja) * 2010-03-30 2011-10-20 Sony Corp 自己位置推定装置および自己位置推定方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2019525342A (ja) * 2016-08-05 2019-09-05 ロブアート ゲーエムベーハーＲＯＢＡＲＴ ＧｍｂＨ 自律移動ロボッを制御する方法
JP7073336B2 (ja) 2016-08-05 2022-05-23 ロブアート ゲーエムベーハー 自律移動ロボットを制御する方法

Also Published As

Publication number Publication date
JP2012103819A (ja) 2012-05-31

Similar Documents

Publication Publication Date Title
JP5793851B2 (ja) 位置推定方法、位置推定装置及びプログラム
CN111771141B (zh) 自动驾驶车辆中使用3d cnn网络进行解决方案推断的lidar定位
CN111771135B (zh) 自动驾驶车辆中使用rnn和lstm进行时间平滑的lidar定位
CN111971574B (zh) 用于自动驾驶车辆的lidar定位的基于深度学习的特征提取
Wurm et al. Bridging the gap between feature-and grid-based SLAM
JP6855524B2 (ja) 低速特徴からのメトリック表現の教師なし学習
Xu et al. Novel hybrid of LS-SVM and Kalman filter for GPS/INS integration
JP2013534616A (ja) 画像センサおよび運動または位置センサから生じたデータを融合するための方法およびシステム
CN110118560A (zh) 一种基于lstm和多传感器融合的室内定位方法
JP2012064131A (ja) 地図生成装置、地図生成方法、移動体の移動方法、及びロボット装置
Malleswaran et al. A novel approach to the integration of GPS and INS using recurrent neural networks with evolutionary optimization techniques
KR20130073476A (ko) 이동 장치 및 이동 장치의 위치 인식 방법
KR102238522B1 (ko) 3차원 공간에 대응되는 맵을 생성하는 차량 및 방법
CN111145251A (zh) 一种机器人及其同步定位与建图方法和计算机存储设备
Guizilini et al. Dynamic hilbert maps: Real-time occupancy predictions in changing environments
Indelman Towards cooperative multi-robot belief space planning in unknown environments
Zhao et al. Improved Rao-Blackwellised particle filter based on randomly weighted particle swarm optimization
Garcia et al. Portable multi-hypothesis Monte Carlo localization for mobile robots
Grzonka et al. Look-ahead proposals for robust grid-based slam with rao-blackwellized particle filters
Revanth et al. Simultaneous localization and mapping of mobile robot using GMapping algorithm
Zhang et al. An adaptive artificial potential function approach for geometric sensing
CN113671942A (zh) 用于控制机器人的设备和方法
Hsu et al. Enhanced simultaneous localization and mapping (ESLAM) for mobile robots
Garzón et al. Pedestrian Trajectory Prediction in Large Infrastructures-A Long-term Approach based on Path Planning
Yoshimura et al. Highlighted map for mobile robot localization and its generation based on reinforcement learning

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20130805

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20140603

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20140730

A02 Decision of refusal

Free format text: JAPANESE INTERMEDIATE CODE: A02

Effective date: 20150203

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20150501

A911 Transfer to examiner for re-examination before appeal (zenchi)

Free format text: JAPANESE INTERMEDIATE CODE: A911

Effective date: 20150515

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20150714

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20150727

R150 Certificate of patent or registration of utility model

Ref document number: 5793851

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150

LAPS Cancellation because of no payment of annual fees