知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】6108818
(24)【登録日】2017年3月17日
(45)【発行日】2017年4月5日
(54)【発明の名称】移動装置及び移動装置の位置認識方法
(51)【国際特許分類】
B25J 19/02 20060101AFI20170327BHJP
G01B 21/00 20060101ALI20170327BHJP
G01B 11/00 20060101ALI20170327BHJP
G05D 1/02 20060101ALN20170327BHJP
【ＦＩ】
B25J19/02
G01B21/00 E
G01B11/00 H
G01B21/00 G
!G05D1/02 K
【請求項の数】18
【全頁数】28
(21)【出願番号】特願2012-280327(P2012-280327)
(22)【出願日】2012年12月21日
(65)【公開番号】特開2013-132748(P2013-132748A)
(43)【公開日】2013年7月8日
【審査請求日】2015年11月30日
(31)【優先権主張番号】10-2011-0141334
(32)【優先日】2011年12月23日
(33)【優先権主張国】KR
(73)【特許権者】
【識別番号】390019839
【氏名又は名称】三星電子株式会社
【氏名又は名称原語表記】Ｓａｍｓｕｎｇ Ｅｌｅｃｔｒｏｎｉｃｓ Ｃｏ．，Ｌｔｄ．
(74)【代理人】
【識別番号】100107766
【弁理士】
【氏名又は名称】伊東 忠重
(74)【代理人】
【識別番号】100070150
【弁理士】
【氏名又は名称】伊東 忠彦
(74)【代理人】
【識別番号】100091214
【弁理士】
【氏名又は名称】大貫 進介
(72)【発明者】
【氏名】安 成 桓
(72)【発明者】
【氏名】郭 魯 山
(72)【発明者】
【氏名】盧 慶 植
(72)【発明者】
【氏名】尹 碩 浚
(72)【発明者】
【氏名】▲ひょん▼ 乘 龍
【審査官】
臼井 卓巳
(56)【参考文献】
【文献】
特開２００８−０６５５６２（ＪＰ，Ａ）
【文献】
特開２０１１−１７５３９３（ＪＰ，Ａ）
【文献】
特開平０９−０１４９６２（ＪＰ，Ａ）
【文献】
米国特許出願公開第２０１０／００１０７４１（ＵＳ，Ａ１）
【文献】
特開２００４−０３０４４５（ＪＰ，Ａ）
【文献】
米国特許出願公開第２００５／０１８２５１８（ＵＳ，Ａ１）
【文献】
鏡 慎吾，センサフュージョン−センサネットワークの情報処理構造，電子情報通信学会論文誌，日本，電子情報通信学会，２００５年１２月 １日，No.12，1404-1412ページ
(58)【調査した分野】（Int.Cl.，ＤＢ名）
Ｂ２５Ｊ １９／０２−１９／０４
Ｇ０１Ｂ １１／００−２１／００
Ｇ０５Ｄ １／０２
Ｇ０６Ｆ １９／００
(57)【特許請求の範囲】
【請求項1】
少なくとも一つのセンサと、
前記少なくとも一つのセンサの検出値を用いて移動装置の現在の相対位置情報を生成する少なくとも一つの第１分散フィルタと、
前記少なくとも一つのセンサの検出値を用いて前記移動装置の現在の絶対位置情報を生成する少なくとも一つの第２分散フィルタと、
前記第１分散フィルタの前記相対位置情報と前記第２分散フィルタの前記絶対位置情報とを融合して位置認識を行う融合フィルタと、を含み、
前記融合フィルタは、更新された位置／姿勢情報を前記少なくとも一つの第１分散フィルタに伝送し、
前記少なくとも一つの第１分散フィルタは、前記移動装置の現在の相対位置情報を生成するとき前記融合フィルタからの更新された位置／姿勢情報を同化させる、
移動装置。
前記少なくとも一つのセンサの検出値を用いて移動装置の現在の相対位置情報を生成する少なくとも一つの第１分散フィルタと、
前記少なくとも一つのセンサの検出値を用いて前記移動装置の現在の絶対位置情報を生成する少なくとも一つの第２分散フィルタと、
前記第１分散フィルタの前記相対位置情報と前記第２分散フィルタの前記絶対位置情報とを融合して位置認識を行う融合フィルタと、を含み、
前記融合フィルタは、更新された位置／姿勢情報を前記少なくとも一つの第１分散フィルタに伝送し、
前記少なくとも一つの第１分散フィルタは、前記移動装置の現在の相対位置情報を生成するとき前記融合フィルタからの更新された位置／姿勢情報を同化させる、
移動装置。
【請求項2】
第２分散フィルタは、前記移動装置の移動空間上に予め設定されている少なくとも一つのノード検出を通じて前記絶対位置情報を生成する、請求項１に記載の移動装置。
【請求項3】
前記第２分散フィルタは、
前記移動装置の位置／姿勢情報を予測する予測部と、
前記少なくとも一つのセンサの検出値を用いて前記移動装置の前記予測された位置／姿勢情報を更新する更新部と、を含み、
前記更新された位置／姿勢情報を前記融合フィルタに提供し、前記融合フィルタの位置／姿勢情報の更新に反映するようにする、請求項２に記載の移動装置。
前記移動装置の位置／姿勢情報を予測する予測部と、
前記少なくとも一つのセンサの検出値を用いて前記移動装置の前記予測された位置／姿勢情報を更新する更新部と、を含み、
前記更新された位置／姿勢情報を前記融合フィルタに提供し、前記融合フィルタの位置／姿勢情報の更新に反映するようにする、請求項２に記載の移動装置。
【請求項4】
前記位置／姿勢情報の予測は、周期的に行い、
前記位置／姿勢情報の更新は、前記ノード検出ごとに行う、請求項３に記載の移動装置。
前記位置／姿勢情報の更新は、前記ノード検出ごとに行う、請求項３に記載の移動装置。
【請求項5】
前記第１分散フィルタは、前記相対位置情報を更新するとき、前記融合フィルタの前記融合された位置／姿勢情報を反映し、
前記第２分散フィルタは、前記絶対位置情報を更新するとき、前記融合された位置／姿勢情報を排除する、請求項３に記載の移動装置。
前記第２分散フィルタは、前記絶対位置情報を更新するとき、前記融合された位置／姿勢情報を排除する、請求項３に記載の移動装置。
【請求項6】
前記移動装置の移動空間上に予め設定されている少なくとも一つのノードを認識し、前記ノード認識結果と予め設けられたローカルマップとの比較を通じてノード検出の不確実性を除去したノード検出情報を生成して前記第２分散フィルタに提供する前処理部をさらに含む、請求項１に記載の移動装置。
【請求項7】
前記前処理部は、
前記ノード認識のためのノード認識部と、
前記ノード検出の不確実性を除去するためのノードマッチング部と、を含んでなる、請求項６に記載の移動装置。
前記ノード認識のためのノード認識部と、
前記ノード検出の不確実性を除去するためのノードマッチング部と、を含んでなる、請求項６に記載の移動装置。
【請求項8】
前記ローカルマップを格納するための格納部をさらに含む、請求項６または７に記載の移動装置。
【請求項9】
前記少なくとも一つのセンサは、前記移動装置の移動空間を撮影するためのカメラである、請求項１に記載の移動装置。
【請求項10】
少なくとも一つの第１分散フィルタが、少なくとも一つのセンサの検出値を用いて移動装置の現在の相対位置情報を生成し、
少なくとも一つの第２分散フィルタが、前記少なくとも一つのセンサの検出値を用いて前記移動装置の現在の絶対位置情報を生成し、
融合フィルタが、前記第１分散フィルタの前記相対位置情報と、前記第２分散フィルタの前記絶対位置情報とを融合して位置認識を行い、更新された位置／姿勢情報を前記少なくとも一つの第１分散フィルタに伝送し、
前記少なくとも一つの第１分散フィルタは、前記移動装置の現在の相対位置情報を生成するとき前記融合フィルタからの更新された位置／姿勢情報を同化させる、
移動装置の位置認識方法。
少なくとも一つの第２分散フィルタが、前記少なくとも一つのセンサの検出値を用いて前記移動装置の現在の絶対位置情報を生成し、
融合フィルタが、前記第１分散フィルタの前記相対位置情報と、前記第２分散フィルタの前記絶対位置情報とを融合して位置認識を行い、更新された位置／姿勢情報を前記少なくとも一つの第１分散フィルタに伝送し、
前記少なくとも一つの第１分散フィルタは、前記移動装置の現在の相対位置情報を生成するとき前記融合フィルタからの更新された位置／姿勢情報を同化させる、
移動装置の位置認識方法。
【請求項11】
前記第２分散フィルタは、前記移動装置の移動空間上に予め設定されている少なくとも一つのノード検出を通じて前記絶対位置情報を生成する、請求項１０に記載の移動装置の位置認識方法。
【請求項12】
前記第２分散フィルタは、
前記移動装置の位置／姿勢情報を予測する予測部と、
前記少なくとも一つのセンサの検出値を用いて前記移動装置の前記予測された位置／姿勢情報を更新する更新部と、を含み、
前記更新された位置／姿勢情報を前記融合フィルタに提供し、前記融合フィルタの位置／姿勢情報の更新に反映するようにする、請求項１１に記載の移動装置の位置認識方法。
前記移動装置の位置／姿勢情報を予測する予測部と、
前記少なくとも一つのセンサの検出値を用いて前記移動装置の前記予測された位置／姿勢情報を更新する更新部と、を含み、
前記更新された位置／姿勢情報を前記融合フィルタに提供し、前記融合フィルタの位置／姿勢情報の更新に反映するようにする、請求項１１に記載の移動装置の位置認識方法。
【請求項13】
前記位置／姿勢情報の予測は、周期的に行い、
前記位置／姿勢情報の更新は、前記ノード検出ごとに行う、請求項１２に記載の移動装置の位置認識方法。
前記位置／姿勢情報の更新は、前記ノード検出ごとに行う、請求項１２に記載の移動装置の位置認識方法。
【請求項14】
前記第１分散フィルタは、前記相対位置情報を更新するとき、前記融合フィルタの前記融合された位置／姿勢情報を反映し、
前記第２分散フィルタは、前記絶対位置情報を更新するとき、前記融合された位置／姿勢情報を排除する、請求項１２に記載の移動装置の位置認識方法。
前記第２分散フィルタは、前記絶対位置情報を更新するとき、前記融合された位置／姿勢情報を排除する、請求項１２に記載の移動装置の位置認識方法。
【請求項15】
前処理部が、前記移動装置の移動空間上に予め設定されている少なくとも一つのノードを認識し、前記ノード認識結果と予め設けられたローカルマップとの比較を通じてノード検出の不確実性を除去したノード検出情報を生成して前記第２分散フィルタに提供することをさらに含む、請求項１０に記載の移動装置の位置認識方法。
【請求項16】
前記前処理部は、
前記ノード認識のためのノード認識部と、
前記ノード検出の不確実性を除去するためのノードマッチング部と、を含んでなる、請求項１５に記載の移動装置の位置認識方法。
前記ノード認識のためのノード認識部と、
前記ノード検出の不確実性を除去するためのノードマッチング部と、を含んでなる、請求項１５に記載の移動装置の位置認識方法。
【請求項17】
前記ローカルマップを格納するための格納部をさらに含む、請求項１５または１６に記載の移動装置の位置認識方法。
【請求項18】
前記少なくとも一つのセンサは、前記移動装置の移動空間を撮影するためのカメラである、請求項１０に記載の移動装置の位置認識方法。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、移動装置に装着された複数のセンサを通じて検出された情報を用いて、移動装置自身の位置を推定する位置認識方法に関する。
【背景技術】
【0002】
位置認識は、移動装置が自ら空間知覚能力を有する技術であって、移動装置の自律移動機能の具現や拡張現実（Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ；ＡＲ）の具現などにおける核心技術である。ここで、移動装置は、人間が操縦せずに、人間の目に該当するセンサがコンピュータのような判断機能を揃えており、自立的に移動することができる移動ロボット（ロボット掃除機、歩行ロボットなど）や、自律移動機能を有していないが、手に持って操作できる程度の小さい大きさを有しており、人間が携帯し、移動しながら操作できるモバイル機器（携帯電話のような移動通信機器など）などを含む。
【0003】
ほとんどの位置認識技術は車輪を持つ移動ロボットの分野において発展してきた。このような技術は、カルマンフィルタ（Ｋａｌｍａｎ Ｆｉｌｔｅｒ）または粒子フィルタ（Ｐａｒｔｉｃｌｅ Ｆｉｌｔｅｒ）を核心アルゴリズムとして用いる単一フィルタ構造を有し、同時位置決め地図作成（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ；ＳＬＡＭ）という方式で具現される。予測（ｐｒｅｄｉｃｔｉｏｎ）ステップと更新（ｕｐｄａｔｅ）ステップを反復して行うことで、移動ロボットの位置を推定する。予測ステップでは、ロボット動きモデル（Ｍｏｔｉｏｎ Ｍｏｄｅｌ）を用いて次のステップでの位置を予測し、更新ステップでは、センサの情報を受け入れて、予測した移動ロボットの位置情報を更新する。
【0004】
位置認識技術は、ロボットの本体に映像センサ（カメラ）や距離センサ（レーザーセンサ、超音波センサなど）を装着し、それぞれのセンサから獲得した情報を同時に処理するセンサ融合に関連する研究が主になされている。ロボットは、カメラから獲得した映像情報から抽出した特徴点やレーザーセンサなどを通じて検出された距離情報から獲得したコーナー、壁、格子地図などを自然標識（ｎａｔｕｒａｌ ｌａｎｄｍａｒｋ）として用いて、自分と標識の位置情報を同時に推定する。
【0005】
一般に、多重センサ融合を通じた位置認識の具現時に、センサの数が増加すると位置推定の正確度が向上するという特性がある。単一フィルタ構造を有した従来の位置認識技術は、センサの数が増加するとフィルタの構造及び具現過程が複雑になり、演算される情報が一つのフィルタにのみ集中してフィルタの情報処理負荷が増加する。特に、更新ステップでは、装着されたセンサの数に比例して測定情報の量が増加するため、フィルタの演算量が増加し、動作速度が遅くなるという問題がある。
【0006】
また、単一フィルタ構造においては、フィルタが誤作動をしたり、誤ったセンサ情報が入力される場合にもこれを防止できる方案がないため、フィルタが外乱（ｄｉｓｔｕｒｂａｎｃｅ）に対して脆弱になるしかない。その結果、ロボットの位置推定の結果も発散しやすくなる。一般に、一回でもフィルタの推定値が発散すると、元の機能へ復旧しにくく、映像情報を用いてロボットの位置認識を行う場合には、Ｋｉｄｎａｐ Ｒｅｃｏｖｅｒｙという別途の技術を具現して復旧が可能であるが、計算及び具現過程が複雑であるという問題がある。
【発明の概要】
【発明が解決しようとする課題】
【0007】
それぞれ独立して動作する複数のローカルフィルタ（ｌｏｃａｌ ｆｉｌｔｅｒ）と、それぞれのローカルフィルタを通じて行われた位置認識の結果を融合する一つの融合フィルタ（ｆｕｓｉｏｎ ｆｉｌｔｅｒ）とからなる分散化されたフィルタシステムを用いて移動装置の位置認識動作を行い、分散化されたフィルタシステムに正確な位相学的絶対位置／姿勢情報をさらに適用することにより、広い空間での移動装置（走行ロボットと歩行ロボット、携帯可能な移動機器など）の位置認識性能（位置認識の正確性及び収束性）がより改善された移動装置及び移動装置の位置認識方法を提案する。
【課題を解決するための手段】
【0008】
本発明に係る移動装置は、少なくとも一つのセンサと、少なくとも一つのセンサの検出値を用いて現在の相対位置情報を生成する少なくとも一つの第１分散フィルタと、少なくとも一つのセンサの検出値を用いて現在の絶対位置情報を生成する少なくとも一つの第２分散フィルタと、第１分散フィルタの相対位置情報と第２分散フィルタの絶対位置情報とを融合して位置認識を行う融合フィルタと、を含む。
【0009】
また、第２分散フィルタは、移動装置の移動空間上に予め設定されている少なくとも一つのノード検出を通じて絶対位置情報を生成する。
【0010】
また、第２分散フィルタは、移動装置の位置／姿勢情報を予測する予測部と、少なくとも一つのセンサの検出値を用いて移動装置の予測された位置／姿勢情報を更新する更新部と、を含み、更新された位置／姿勢情報を融合フィルタに提供し、融合フィルタの位置／姿勢情報の更新に反映するようにする。
【0011】
また、位置／姿勢情報の予測は周期的に行い、位置／姿勢情報の更新はノードが検出されるごとに行う。また、第１分散フィルタは、相対的位置情報を更新するとき、融合フィルタの融合された位置／姿勢情報を反映し、第２分散フィルタは、絶対的位置情報を更新するとき、融合された位置／姿勢情報を排除する。
【0012】
また、移動装置の移動空間上に予め設定されている少なくとも一つのノードを認識し、ノード認識結果と予め設けられたローカルマップとの比較を通じてノード検出の不確実性を除去したノード検出情報を生成して第２分散フィルタに提供する前処理部をさらに含む。
【0013】
また、前処理部は、ノード認識のためのノード認識部と、ノード検出の不確実性を除去するためのノードマッチング部と、を含む。また、ローカルマップを格納するための格納部をさらに含む。また、少なくとも一つのセンサは、移動装置の移動空間を撮影するためのカメラである。
【0014】
本発明に係る移動装置の位置認識方法は、少なくとも一つの第１分散フィルタが少なくとも一つのセンサの検出値を用いて現在の相対位置情報を生成し、少なくとも一つの第２分散フィルタが少なくとも一つのセンサの検出値を用いて現在の絶対位置情報を生成し、融合フィルタが、第１分散フィルタの相対位置情報と第２分散フィルタの絶対位置情報とを融合して位置認識を行う。
【0015】
また、第２分散フィルタは、移動装置の移動空間上に予め設定されている少なくとも一つのノード検出を通じて絶対位置情報を生成する。また、第２分散フィルタは、移動装置の位置／姿勢情報を予測する予測部と、少なくとも一つのセンサの検出値を用いて移動装置の予測された位置／姿勢情報を更新する更新部と、を含み、更新された位置／姿勢情報を融合フィルタに提供し、融合フィルタの位置／姿勢情報の更新に反映するようにする。
【0016】
また、位置／姿勢情報の予測は周期的に行い、位置／姿勢情報の更新はノードが検出されるごとに行う。また、第１分散フィルタは、相対的位置情報を更新するとき、融合フィルタの融合された位置／姿勢情報を反映し、第２分散フィルタは、絶対的位置情報を更新するとき、融合された位置／姿勢情報を排除する。
【0017】
また、前処理部が、移動装置の移動空間上に予め設定されている少なくとも一つのノードを認識し、ノード認識結果と予め設けられたローカルマップとの比較を通じてノード検出の不確実性を除去したノード検出情報を生成して第２分散フィルタに提供することをさらに含む。
【0018】
また、前処理部は、ノード認識のためのノード認識部と、ノード検出の不確実性を除去するためのノードマッチング部と、を含む。また、ローカルマップを格納するための格納部をさらに含む。また、少なくとも一つのセンサは、移動装置の移動空間を撮影するためのカメラである。
【発明の効果】
【0019】
提案された移動装置及びその位置認識方法によれば、それぞれ独立して動作する複数のローカルフィルタと、それぞれのローカルフィルタを通じて行われた位置認識の結果を融合する一つの融合フィルタとからなる分散化されたフィルタシステムを用いて移動装置の位置認識動作を行い、分散化されたフィルタシステムに正確な位相学的絶対位置情報をさらに適用することにより、広い空間での移動装置の位置認識性能（位置認識の正確性及び収束性、速度など）を改善する。
【図面の簡単な説明】
【0020】
【図1】本発明の実施例に係る移動装置の一例である歩行ロボットの外観構成図である。
【図6】本発明の実施例に係るノード設定状態を示した図である。
【図13】本発明の実施例に係る移動装置の位置認識方法を示したフローチャートである。
【発明を実施するための形態】
【0021】
以下では、添付の図面を参照して、本発明の実施例を詳細に説明する。
【0022】
図１は、本発明の実施例に係る移動装置の一例である歩行ロボットの外観構成図である。図１に示されるように、本発明の実施例に係る移動装置の一例である歩行ロボット１０は、人間と同様に、二つの脚１６Ｌ、１６Ｒによって直立移動する二足歩行ロボットであって、頭１２、胴体１３、二つの腕１４Ｌ、１４Ｒからなる上体１１と、二つの脚１６Ｌ、１６Ｒからなる下体１５と、を有する。頭１２において人間の目の形状を有した機構部には、移動空間の周囲を撮影するためのカメラ１２０が装着される。
【0023】
参照符号において、数字の後ろの“Ｌ”と“Ｒ”は、それぞれロボット１０の左側（ｌｅｆｔ）と右側（ｒｉｇｈｔ）を示す。
【0024】
本発明の実施例では、移動装置として二足歩行ロボットを例に挙げて説明したが、その他にも家庭での掃除ロボット、公共場所用サービスロボット、生産現場での搬送ロボット、作業者支援ロボットなどの多様な形態の移動ロボット、及び自律移動機能を有してはいないが、人間が携帯し、移動しながら操作できるモバイル機器（携帯電話のような移動通信機器など）に対して本発明を適用することができる。
【0026】
頭１２は、首関節部１７を介して上体１１の胴体１３と連結される。胴体１３、二つの腕１４Ｌ、１４Ｒ及び二つの脚１６Ｌ、１６Ｒには、肩関節部１８、肘関節部１９、手首関節部２０、腰関節部２１、ヒップ関節部２２、膝関節部２３、足首関節部２４などの複数の関節部が設置される。各関節部１８，１９，２０，２１，２２，２３，２４には、自由度（ｄｅｇｒｅｅ ｏｆ ｆｒｅｅｄｏｍ；関節が動く方向の数）によって１個ないし３個の回転関節２５を含む。例えば、ヒップ関節部２２は、ヨー方向（ｙａｗ、Ｚ軸まわりの回転）の回転関節２５と、ピッチ方向（ｐｉｔｃｈ、Ｙ軸まわりの回転）の回転関節２５と、ロール方向（ｒｏｌｌ、Ｘ軸まわりの回転）の回転関節２５とを含んで３自由度を有する。各関節部１８，１９，２０，２１，２２，２３，２４は、リンク（Ｌ；図２で直線で示される構造）により連結される。
【0027】
上体１１の下側に設置される腰関節部２１には、上体１１を支持するための骨盤２６が連結される。骨盤２６は、ペルビスリンク２７を介してヒップ関節部２２に連結される。ペルビスリンク２７には、歩行ロボット１０の姿勢情報（角度情報）を検出するための慣性センサ１３０（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ；ＩＭＵ）が設置される。慣性センサ１３０は、重力方向と慣性系に対してペルビスリンク２７の相対的角度を検出して、ロール方向、ピッチ方向、ヨー方向の姿勢情報（角度情報）を発生させる。この慣性センサ１３０は、ペルビスリンク２７だけでなく胴体１３、頭１２などに設置することも可能である。
【0028】
図２に示されていないが、歩行ロボット１０の各関節部１８，１９，２０，２１，２２，２３，２４には、動力（電気、油圧）により各回転関節２５を駆動するモータなどのアクチュエーター（図３の“駆動部”に該当）と、各アクチュエーターの回転角度、すなわち、各回転関節２５の回転角度を検出するエンコーダ（図３の１１０）とが設置される。歩行ロボット１０の動作全般を制御する制御部（図３の２００）は、このようなアクチュエーターを適切に制御することによって歩行ロボット１０の多様な動作を具現できる。
【0029】
図３は、図１に示される歩行ロボットの制御系統を示したブロック図である。図３に示されるように、本発明の実施例に係る移動装置の一例である歩行ロボット１０の制御系統は、センサモジュール１００、制御部２００、格納部４００及び駆動部４５０を含む。
【0030】
センサモジュール１００は、歩行ロボット１０に関する情報及び移動空間に関する情報を検出するための複数のセンサ１１０，１２０，１３０を含んでなる。センサモジュール１００は、歩行ロボット１０の歩行に関連する回転関節の回転角度情報を獲得するためのエンコーダ１１０（ｅｎｃｏｄｅｒ）、歩行ロボット１０が歩行している移動空間の周辺を撮影するためのカメラ１２０、歩行ロボット１０の姿勢情報（角度情報）を検出するための慣性センサ１３０などの多様なセンサを含むことができる。
【0031】
センサモジュール１００において、エンコーダ１１０は、ヒップ関節部２２、膝関節部２３及び足首関節部２４に設けられた各回転関節２５を回転駆動するために設置されたアクチュエーター（駆動部）の回転角度を検出する。
【0032】
センサモジュール１００において、カメラ１２０は、被写体から反射される光を検出し、これをデジタル信号に変換して移動空間の周辺の映像情報を獲得する。カメラ１２０としては、ＣＣＤ（ｃｈａｒｇｅ−ｃｏｕｐｌｅｄ ｄｅｖｉｃｅ）カメラ、ＣＭＯＳ（ｃｏｍｐｌｅｍｅｎｔａｒｙ ｍｅｔａｌ ｏｘｉｄｅ）カメラ、ＴＯＦ（Ｔｉｍｅ ｏｆ Ｆｌｉｇｈｔ）カメラなどを用いることができ、その他にも、歩行ロボット１０が歩行する経路上に位置する物体に対する映像情報を獲得できる装置であれば、いかなる装置でも使用できることは勿論である。
【0033】
センサモジュール１００において、慣性センサ１３０は、歩行ロボット１０の加速度、速度、方向（角度）などの多様な航法関連情報を測定するためのもので、ペルビスリンク２７のロール方向、ピッチ方向、ヨー方向の傾き及び回転角度を測定する。慣性センサ１３０は、歩行ロボット１０の角度を測定する傾きセンサ、及び角速度を測定する角速度センサを含む。ここで、傾きセンサとしては加速度計が使用され、角速度センサとしてはレートジャイロスコープ（ｒａｔｅ−ｇｙｒｏｓｃｏｐｅ）が使用される。
【0034】
制御部２００は、歩行ロボット１０の動作全般を制御し、第１〜５前処理部２１０〜２５０及び位置認識部３００を含む。
【0035】
第１〜５前処理部２１０〜２５０は、エンコーダ１１０、カメラ１２０及び慣性センサ１３０から伝達される検出情報（回転角度情報、映像情報、慣性検出情報）に対して演算処理（ｐｒｅｐｒｏｃｅｓｓｉｎｇ）を行って、歩行ロボット１０の位置認識のために必要な測定情報（ｍｅａｓｕｒｅｄ ｄａｔａ）を算出し、算出された測定情報を位置認識部３００に提供する。
【0036】
位置認識部３００は、第１〜５前処理部２１０〜２５０から提供された各種情報を用いて、歩行ロボット１０自身の位置及び姿勢を推定する。位置認識部３００の構成については、後述する図４を参照して詳細に説明する。
【0037】
格納部４００は、歩行ロボット１０が位置認識を行うために必要とする事前情報、及び位置認識の結果を格納するためのもので、格納部４００には、リンク（関節部と関節部との間を連結する機構構造）の機構情報（長さ情報）、歩行ロボット１０の歩行過程で複数のアルゴリズム（複数のフィルタ）を用いて算出される歩行ロボット１０の位置認識結果（標識の位置情報、歩行ロボットの位置／姿勢情報）、及び複数のアルゴリズムのうち、特にＳＬＡＭアルゴリズムを用いて作成される移動空間（作業空間）に対するローカルマップなどが格納される。
【0038】
本発明の実施例では、位置認識を行うために必要とする事前情報及び位置認識の結果を格納するための格納部４００が別途に設けられる構成を例に挙げて説明したが、格納部４００の構成を採用せずに、制御部２００内の内部メモリに位置認識のための事前情報などを格納するように構成してもよい。
【0039】
駆動部４５０は、各関節部１８，１９，２０，２１，２２，２３，２４をなす各回転関節２５に電気又は油圧による動力を伝達するためのモータなどのアクチュエーターであって、制御部２００から伝達される制御信号に応じて各関節部１８，１９，２０，２１，２２，２３，２４の各回転関節２５を回転駆動する。
【0041】
図４に示されるように、位置認識部３００は、複数の第１〜５フィルタ３１０〜３５０からなる分散化されたフィルタシステム（ｄｉｓｔｒｉｂｕｔｅｄ ｆｉｌｔｅｒ ｓｙｓｔｅｍ）の構造を有する。図４において第１〜４ローカルフィルタ３１０，３２０，３３０，３４０は、“現在の相対位置情報を生成する少なくとも一つの第１分散フィルタ”に区分でき、第５ローカルフィルタ３５０は、“現在の絶対位置情報を生成する少なくとも一つの第２分散フィルタ”に区分することができ、融合フィルタ３６０は、このような第１分散フィルタ（すなわち、第１〜４ローカルフィルタ３１０，３２０，３３０，３４０）の相対位置情報と第２分散フィルタ（すなわち、第５ローカルフィルタ３５０）の絶対位置情報とを融合して、持続的で且つ効果的な位置認識を行う。
【0042】
本発明の実施例において、位置認識部３００は、第１〜５ローカルフィルタ３１０〜３５０及び一つの融合フィルタ３６０を含んでなる。第１〜５ローカルフィルタ３１０〜３５０及び一つの融合フィルタ３６０は独立して動作するので、歩行ロボット１０の位置認識を行うと、第１〜５ローカルフィルタ３１０〜３５０及び一つの融合フィルタ３６０のそれぞれから位置認識の結果を獲得できる。第１〜５ローカルフィルタ３１０〜３５０及び一つの融合フィルタ３６０のうち、第５ローカルフィルタ３５０を除外した残りの第１〜４ローカルフィルタ３１０，３２０，３３０，３４０及び融合フィルタ３６０は、それぞれ予測部３１２，３２２，３３２，３４２，３６２、更新部３１４，３２４，３３４，３４４，３６４及び同化部３１６，３２６，３３６，３４６，３６６を有し、歩行ロボット１０の移動が停止するまで反復して予測（ｐｒｅｄｉｃｔｉｏｎ）、更新（ｕｐｄａｔｅ）、同化（ａｓｓｉｍｉｌａｔｉｏｎ）を行う。第５ローカルフィルタ３５０は、予測部３５２と更新部３５４で構成され、第５ローカルフィルタ３５０は、同化部を有さない。
【0043】
第１〜５ローカルフィルタ３１０，３２０，３３０，３４０，３５０及び融合フィルタ３６０の各予測部３１２，３２２，３３２，３４２，３５２，３６２は、以前の更新ステップで推定（認識）された歩行ロボット１０の位置／姿勢情報から歩行ロボット１０の新しい位置／姿勢情報を予測する。
【0044】
第１〜５ローカルフィルタ３１０，３２０，３３０，３４０，３５０の各更新部３１４，３２４，３３４，３４４，３５４は、センサモジュール１００の検出情報を用いて予測ステップで予測された歩行ロボット１０の位置／姿勢情報を更新する。また、融合フィルタ３６０の更新部３６４は、第１〜５ローカルフィルタ３１０，３２０，３３０，３４０，３５０の各更新部３１４３２４，３３４，３４４，３５４から伝達される歩行ロボット１０の位置認識結果（更新された位置／姿勢情報）を融合し、融合された情報を用いて予測ステップで予測された歩行ロボット１０の位置／姿勢情報を更新する。
【0045】
第１〜４ローカルフィルタ３１０，３２０，３３０，３４０の各同化部３１６，３２６，３３６，３４６は、更新ステップで更新された歩行ロボット１０の位置／姿勢情報に、融合フィルタ３６０の更新部３６４から伝達された歩行ロボット１０の位置認識結果（更新された位置／姿勢情報）を反映する。ただし、第５ローカルフィルタ３５０は同化部を有さないので、同化過程を行わない。位相学的ノード情報を扱う第５ローカルフィルタ３５０において、予測過程は周期的に行われるが、更新過程はノードが検出された場合にのみ行われる。絶対的位置情報を用いる第５ローカルフィルタ３５０の場合、相対的位置情報を用いる残りの第１〜４ローカルフィルタ３１０，３２０，３３０，３４０よりも位置認識の正確度及び情報の質がさらに優れるので、他の位置／姿勢認識情報との同化過程は、むしろ第５ローカルフィルタ３５０の絶対位置／姿勢情報の一貫性（ｃｏｎｓｉｓｔｅｎｃｙ）を低下させる要因になりうるためである。融合フィルタ３６０の同化部３６６は、予測ステップで予測された歩行ロボット１０の位置／姿勢情報に、第１〜４ローカルフィルタ３１０，３２０，３３０，３４０の各更新部３１４，３２４，３３４，３４４から伝達される歩行ロボット１０の更新された相対位置／姿勢情報、及び第５ローカルフィルタ３５０の更新部３５４から伝達される歩行ロボット１０の更新された絶対位置／姿勢情報を融合する。
【0046】
すなわち、第１〜５ローカルフィルタ３１０，３２０，３３０，３４０，３５０は、各センサ１１０，１２０，１３０の検出情報を用いて更新された歩行ロボット１０の位置／姿勢情報を融合フィルタ３６０に伝達し、融合フィルタ３６０は、伝達された歩行ロボット１０の位置／姿勢情報を融合／更新して第１〜４ローカルフィルタ３１０，３２０，３３０，３４０にフィードバックする過程を行う。第５ローカルフィルタ３５０には、融合フィルタ３６０の更新された位置／姿勢情報がフィードバックされない。
【0047】
以下では、第５ローカルフィルタ３５０を除外した残りの第１〜４ローカルフィルタ３１０，３２０，３３０，３４０の共通した動作過程について説明する。本発明の実施例では、第１〜４ローカルフィルタ３１０，３２０，３３０，３４０がカルマンフィルタ（Ｋａｌｍａｎ ｆｉｌｔｅｒからなる場合を例に挙げて説明する。
【0048】
第１〜４ローカルフィルタ３１０，３２０，３３０，３４０は、融合フィルタ３６０とは構造的に独立した形態からなっており、使用するセンサ１１０，１２０，１３０の種類及びセンサ１１０，１２０，１３０を通じて検出された情報の処理方式が互いに異なってもよい。第１〜４ローカルフィルタ３１０，３２０，３３０，３４０は、歩行ロボット１０の位置／姿勢情報を推定するための状態変数ｘＬ（ｋ｜ｋ）として、ワールド座標系（ｗｏｒｌｄ ｃｏｏｒｄｉｎａｔｅ）に対する３次元位置ｒ、３次元姿勢（ｑｕａｔｅｒｎｉｏｎ）ｑ、３次元線速度ｖ、ボディー座標系（Ｂｏｄｙ ｃｏｏｒｄｉｎａｔｅ）に対する３次元角速度ｗを有する。
【0049】
共通に第１〜４ローカルフィルタ３１０，３２０，３３０，３４０の予測ステップでは、推定状態変数に基づいた等速度運動モデル（ｃｏｎｓｔａｎｔ ｖｅｌｏｃｉｔｙ ｍｏｄｅｌ）を用いて更新ステップでセンサ情報（より詳しくは測定情報）と融合（ｆｕｓｉｏｎ）する前に、現在の状態変数を下の数式（１）のようにモデリングする。
【0050】
【数1】
【0051】
第１〜４ローカルフィルタ３１０，３２０，３３０，３４０の更新ステップでは、各センサ１１０，１２０，１３０の検出情報に対する前処理を通じて獲得した測定情報ｚ（ｋ）、及び予測ステップで求めた状態変数ｘＬ（ｋ｜ｋ−１）を用いて算出した予測観測値ｈ（ｋ）を用いてカルマンフィルタ更新（Ｋａｌｍａｎ Ｆｉｌｔｅｒ Ｕｐｄａｔｅ）を行う。予測された歩行ロボット１０の位置／姿勢情報に対する更新が行われた後には、下の数式（２）及び数式（３）のように情報フィルタ（ｉｎｆｏｒｍａｔｉｏｎ ｆｉｌｔｅｒ）に対する入力形態で融合フィルタ３６０に必要な情報を伝達する。
【0052】
【数2】
【0053】
センサモジュール１００から検出情報が入力される毎に、基本的にこの二つの過程（予測と更新）が反復して行われ、融合フィルタ３６０から位置認識結果（更新された歩行ロボットの相対位置／姿勢情報）の入力がある場合、同化ステップでは、第１〜４ローカルフィルタ３１０，３２０，３３０，３４０の更新ステップで更新された歩行ロボット１０の相対位置／姿勢情報に、融合フィルタ３６０の位置認識結果（更新された歩行ロボットの位置／姿勢情報）を、更新ステップと類似したカルマンフィルタ更新（Ｋａｌｍａｎ Ｆｉｌｔｅｒ Ｕｐｄａｔｅ）を使用して反映する。これは、独立して駆動される第１〜４ローカルフィルタ３１０，３２０，３３０，３４０に異なるセンサの検出情報が反映されることにより、それぞれのセンサが他のセンサの検出情報を共有することを意味する。これによって、それぞれのセンサは、他のセンサの検出情報を通じて自分の検出情報を補完できるので、全体フィルタシステム３００の位置推定の正確度を大きく向上させることができる。
【0054】
図５は、図４に示した第５前処理部２５０の構成を示す図である。第５前処理部２５０は、カメラ１２０及び慣性センサ１３０から伝達される検出情報（回転角度情報、映像情報、慣性検出情報）に対して演算処理（ｐｒｅｐｒｏｃｅｓｓｉｎｇ）を行って、歩行ロボット１０の移動空間上に設定されているノード（Ｎｏｄｅ）（図６の説明参照）を検出する。ノードＡ〜Ｅの検出のために、第５前処理部２５０はノード認識部２５２とノードマッチング部２５４を含む。ノード認識部２５２は、カメラ１２０から伝達される検出情報（映像情報）を分析して歩行ロボット１０の移動空間上に予め設定されているノードを認識する。ノードマッチング部２５４は、ノード認識結果に含まれ得る不確実性を除去してノード認識率を高めるためのものである。
【0055】
図６を参照して、本発明の実施例に係るノード認識方法をより具体的に説明すると、次の通りである。図６は、本発明の実施例に係るノード設定状態を示した図である。ノード認識は、現在の映像から抽出した特徴点情報を用いて、格納部４００に格納されているローカルマップ上の特徴点情報と比較することを意味する。このとき、対応する特徴点の数が多いノードを決定して、歩行ロボット１０の現在の位置に隣接したノードを判断する。
【0056】
本発明の実施例では、融合フィルタ３６０に絶対位置認識情報を提供するために、図６のように歩行ロボット１０の移動空間上の一定地点をノードＡ〜Ｅに設定する。ノードＡ〜Ｅは、廊下の交差点やコーナーのような構造的特徴を有する地点や、多様な視覚情報を含む視覚的特徴を有する地点、または歩行ロボット１０の移動時に正確な位置情報を必要とする地点などに指定できる。与えられた建物の設計図あるいは平面図からワールド原点座標系に対するノード座標系の位置を定めることができ、これから絶対位置情報が含まれたノードで構成される位相学的地図を構築できる。決定されたそれぞれのノードＡ〜Ｅにおいて、ノード認識及びマッチングのために、視覚センサベースのＳＬＡＭアルゴリズムを用いて映像情報から抽出した特徴点の３次元位置と、歩行ロボット１０の３次元位置／姿勢情報とを同時に推定する。歩行ロボット１０の移動空間上のどの一点をノード原点Ｏ１とし、このノード原点Ｏ１を中心にカメラ１２０を移動させて、カメラ１２０を通じて確保される映像情報から位置認識に必要な特徴点を抽出する。抽出された特徴点情報は、第５ローカルフィルタ３５０の状態変数として登録され、確率的なフィルタ技法を用いて持続的に特徴点の位置及びカメラの位置／姿勢情報を推定する。このような視覚センサベースのＳＬＡＭアルゴリズムの結果物として得たノードＡ〜Ｅの周辺の特徴点で構成されたローカルマップは格納部４００に格納される。仮に３次元点群データ（３Ｄ Ｐｏｉｎｔ Ｃｌｏｕｄ Ｄａｔａ）を得ることができるカメラ（例えば、ステレオカメラ又はＴＯＦカメラなど）を使用する場合には、特徴点及び点群データで構成される３次元地図をノード情報として追加的に活用できる。
【0057】
ノード検出方法は、ノード認識（Ｎｏｄｅ Ｒｅｃｏｇｎｉｔｉｏｎ）とノードマッチング（Ｎｏｄｅ Ｍａｔｃｈｉｎｇ）の二つの過程からなる。ノード検出方法の１番目の過程であるノード認識は、カメラ１２０を通じて確保した映像から抽出した特徴点情報を、格納部４００に格納されている特徴点情報と比較することである。この比較を通じて、対応する特徴点の数が多いノードを認識できる。
【0058】
ノード検出方法の２番目の過程であるノードマッチングは、認識したノード座標系内でロボットの姿勢（位置及び方向）を決定する過程である。ノードを含むローカルマップ内の特徴点は、ノード座標系内で３次元位置と対応する不確実性情報（Ｃｏｖａｒｉａｎｃｅ）を有している。ノードマッチング過程で、それぞれのマッチングされる特徴点間の関係を用いると、ノード座標系内での歩行ロボット１０の３次元姿勢と対応する不確実性情報を計算することができ、そのために、下の数式（４）が使用される。
【0059】
【数3】
【0060】
このような過程を通じて求めたノード座標系に対する歩行ロボット１０の位置は、下の数式（５）を用いて、位相学的地図情報からワールド原点座標系に対する位置に変換可能であり、位相学的ノードの観測情報として融合フィルタ３６０に伝達される。
【0061】
【数4】
【0062】
第５ローカルフィルタ３５０で取り扱われる位相学的ノード情報は空間上に分布しているため、更新のためのノード認識情報の頻度数が少なく、一定周期ごとのサンプリングも不可能である。したがって、第１〜４ローカルフィルタ３１０，３２０，３３０，３４０とは異なり、推定をするための状態変数ＸＬ（ｋ｜ｋ）としてワールド原点座標系に対する３次元位置ｒ及び３次元姿勢（ｑｕａｔｅｒｎｉｏｎ）ｑのみを有する。
【0063】
推定状態変数からは速度成分を求めることができないため、推定状態変数に基づいた絶対位置モデル（Ｃｏｎｓｔａｎｔ Ｐｏｓｉｔｉｏｎ Ｍｏｄｅｌ）を使用して、更新過程でノード情報と融合（Ｆｕｓｉｏｎ）する前に、現在の状態変数を数式（６）のようにモデリングする。この過程は、第５ローカルフィルタ３５０の予測ステップで、ノード情報の更新と関係なく、一定時間ごとに周期的に行われる。
【0064】
【数5】
【0065】
第５ローカルフィルタの更新過程では、ノード検出を通じて得た測定情報ｚ（ｋ）（下の数式（７）参照）、及び予測過程で求めた状態変数ＸＬ（ｋ｜ｋ−１）を用いて得た予測観測情報ｈ（ｋ）（下の数式（８）参照）を用いてカルマンフィルタ更新（Ｋａｌｍａｎ Ｆｉｌｔｅｒ Ｕｐｄａｔｅ）を行う。
【0066】
【数6】
【0067】
【数7】
【0068】
位相学的ノード情報を扱うフィルタモジュールは、ワールド原点座標系に対する絶対位置及び方向値を用いて更新を行う。しかし、３次元姿勢（ｑｕａｔｅｒｎｉｏｎ）ｑの非線形性のため直接的なカルマンフィルタ更新の適用が難しいので、以前のステップで推定された状態変数値を基準として定め、その値に対する変化量を下の数式（９）及び数式（１０）のように求めて、ロドリゲスの回転公式（Ｒｏｄｒｉｇｕｅｓ’ ｒｏｔａｔｉｏｎ ｆｏｒｍｕｌａ）の形態に変換した後、更新過程を行う。
【0069】
【数8】
【0070】
更新が完了すると、第１〜４ローカルフィルタ３１０，３２０，３３０，３４０と同様に、前述した数式（２）及び数式（３）のように情報フィルタ（ｉｎｆｏｒｍａｔｉｏｎ ｆｉｌｔｅｒ）に対する入力形態で融合フィルタ３６０に必要な情報を伝達する。
【0072】
＜第１ローカルフィルタ３１０の動作＞図７は、図４に示される第１ローカルフィルタ３１０を用いた歩行ロボット１０の位置認識方法を示したフローチャートである。第１ローカルフィルタ３１０は、歩行ロボット１０の位置認識を行うためのセンサとしてエンコーダ１１０を使用し、歩行ロボット１０のオドメトリ（ｏｄｏｍｅｔｒｙ）情報を用いて予測された歩行ロボット１０の位置／姿勢情報を更新する。説明の便宜上、第１ローカルフィルタ３１０をオドメトリフィルタと定義する。
【0073】
本発明の実施例の動作説明のための初期条件として、格納部４００には、歩行ロボット１０が位置認識を行うための事前情報として歩行ロボット１０のリンク（関節部と関節部との間を連結する機構構造）の機構情報（長さ情報）が予め格納されていることを前提とする。また、時間的前後関係と関連して、以下で記述する以前のステップはｋ−１と、以前のステップの次のステップ（現在のステップと表示されてもよい）は、ｋと表示する。
【0074】
図７に示されるように、第１ローカルフィルタ３１０の予測部３１２は、以前の更新ステップｋ−１で推定（認識）された歩行ロボット１０の位置／姿勢情報に融合フィルタ３６０の位置認識結果が反映された新しい歩行ロボット１０の位置／姿勢情報から歩行ロボット１０の次のステップｋの位置／姿勢情報を予測する（７１０）。この過程は、位置認識アルゴリズムの予測（ｐｒｅｄｉｃｔｉｏｎ）ステップに該当する。
【0075】
次に、第１前処理部２１０は、エンコーダ１１０から、歩行ロボット１０の歩行に関連する関節部をなす回転関節の回転角度情報を獲得する（７２０）。
【0076】
その後、第１前処理部２１０は、格納部４００に予め格納されている各リンク（ｌｉｎｋ）の機構情報、及びエンコーダ１１０から入力された各回転関節の回転角度情報を用いてオドメトリ情報を算出する（７３０）。オドメトリ情報は、移動空間上の原点座標系（歩行ロボットが歩行動作を始めた地点を原点とする座標系）に対する歩行ロボット１０の位置（座標）情報及び姿勢（角度）情報を意味し、推測航法（ｄｅａｄ ｒｅｃｋｏｎｉｎｇ）を用いて累積する方式で算出する。
【0077】
次に、第１前処理部２１０は、ステップｋとステップｋ−１のオドメトリ情報を用いて、歩行ロボット１０の相対的な姿勢変化情報を算出する（７４０）。算出された歩行ロボット１０の相対的な姿勢変化情報が、前述した測定情報ｚ（ｋ）となる。
【0078】
オドメトリフィルタである第１ローカルフィルタ３１０において予測観測情報ｈ（ｋ）は、下の数式（１１）のように示すことができる。
【0079】
【数9】
【0080】
その後、同化部３１６は、融合フィルタ３６０の更新部３６４から歩行ロボット１０の位置認識結果（更新された位置／姿勢情報）が入力されるか否かを判断する（７６０）。融合フィルタ３６０の更新部３６４から歩行ロボット１０の位置認識結果が入力されると（７６０での‘はい’）、同化部３１６は、更新ステップで更新された歩行ロボット１０の位置／姿勢情報に融合フィルタ３６０の位置認識結果を反映し、融合フィルタ３６０の位置認識結果が反映された歩行ロボット１０の位置／姿勢情報を予測部３１２に伝送する（７７０）。この過程は、位置認識アルゴリズムの同化（ａｓｓｉｍｉｌａｔｉｏｎ）ステップに該当する。
【0081】
一方、融合フィルタ３６０の更新部３６４から歩行ロボット１０の位置認識結果が入力されない場合（７６０での‘いいえ’）、同化ステップは行わずに、動作７８０に進む。
【0082】
次に、制御部２００は、歩行ロボット１０の移動が停止されたか否かを判断する（７８０）。制御部２００は、入力部（図示せず）を通じて使用者から歩行ロボット１０の歩行停止命令が入力されたり、エンコーダ１１０からこれ以上の回転角度情報が入力されない場合、歩行ロボット１０の移動が停止したものと判断する。
【0083】
歩行ロボット１０の移動が停止されないと（７８０での‘いいえ’）、制御部２００は、動作７１０に戻り、歩行ロボット１０の位置認識動作を続けて行うように制御する。歩行ロボット１０の移動が停止された場合（７８０での‘はい’）、制御部２００は歩行ロボット１００の位置認識過程を終了する。
【0085】
第２ローカルフィルタ３２０は、歩行ロボット１０の位置認識を行うためのセンサとしてカメラ１２０を使用し、歩行ロボット１０の相対的な姿勢変化情報を用いて予測された歩行ロボット１０の位置／姿勢情報を更新する。説明の便宜上、第２ローカルフィルタ３２０を、視覚センサベースのオドメトリフィルタと定義する。
【0086】
図８に示されるように、第２ローカルフィルタ３２０内の予測部３２２は、以前の更新ステップｋ−１で推定（認識）された歩行ロボット１０の位置／姿勢情報に融合フィルタ３６０の位置認識結果が反映された新しい歩行ロボット１０の位置／姿勢情報から、歩行ロボット１０の次のステップｋの位置／姿勢情報を予測する（８１０）。この過程は、位置認識アルゴリズムの予測（ｐｒｅｄｉｃｔｉｏｎ）ステップに該当する。
【0087】
次に、第２前処理部２２０は、カメラ１２０から移動空間周辺の映像情報を獲得する（８２０）。
【0088】
その後、第２前処理部２２０は、以前のステップｋ−１で獲得した映像情報及び現在のステップｋで獲得した映像情報のそれぞれから視覚特徴点（ｆｅａｔｕｒｅ）を抽出し、抽出された特徴点をマッチング（ｍａｔｃｈｉｎｇ）する（８３０）。
【0089】
次に、第２前処理部２２０は、特徴点の抽出及びマッチングを通じて獲得した相関関係からオドメトリフィルタ３１０と同様に歩行ロボット１０の相対的な姿勢変化情報を算出する（８４０）。算出された歩行ロボット１０の相対的な姿勢変化情報が、前述した測定情報ｚ（ｋ）となる。
【0090】
視覚センサベースのフィルタである第２ローカルフィルタ３２０において予測観測情報ｈ（ｋ）は、下の数式（１２）のように示すことができる。
【0091】
【数10】
【0092】
その後、同化部３２６は、融合フィルタ３６０の更新部３６４から歩行ロボット１０の位置認識結果（更新された位置／姿勢情報）が入力されるか否かを判断する（８６０）。融合フィルタ３６０の更新部３６４から歩行ロボット１０の位置認識結果が入力されると（８６０での‘はい’）、同化部３２６は、更新ステップで更新された歩行ロボット１０の位置／姿勢情報に融合フィルタ３６０の位置認識結果を反映し、融合フィルタ３６０の位置認識結果が反映された歩行ロボット１０の位置／姿勢情報を予測部３２２に伝送する（８７０）。この過程は、位置認識アルゴリズムの同化（ａｓｓｉｍｉｌａｔｉｏｎ）ステップに該当する。
【0093】
一方、融合フィルタ３６０の更新部３６４から歩行ロボット１０の位置認識結果が入力されない場合（８６０での‘いいえ’）、同化ステップは行わずに、動作８８０に進む。
【0094】
次に、制御部２００は、歩行ロボット１０の移動が停止されたか否かを判断する（８８０）。制御部２００は、入力部（図示せず）を通じて使用者から歩行ロボット１０の歩行停止命令が入力されたり、エンコーダ１１０からこれ以上の回転角度情報が入力されない場合、歩行ロボット１０の移動が停止したものと判断する。
【0095】
歩行ロボット１０の移動が停止されないと（８８０での‘いいえ’）、制御部２００は、動作８１０に戻り、歩行ロボット１０の位置認識動作を続けて行うように制御する。歩行ロボット１０の移動が停止された場合（８８０での‘はい’）、制御部２００は歩行ロボット１００の位置認識過程を終了する。
【0097】
第３ローカルフィルタ３３０は、歩行ロボット１０の位置認識を行うためのセンサとしてカメラ１２０を使用し、映像情報から抽出した特徴点の３次元位置情報と歩行ロボット１０の３次元位置／姿勢情報とを同時に推定する。したがって、他のローカルフィルタ３１０，３２０，３４０，３５０とは異なり、特徴点の位置に対する状態変数ｙｉ（ｋ｜ｋ）をさらに有することになる。
【0098】
説明の便宜上、第３ローカルフィルタ３３０を視覚センサベースのＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）フィルタと定義する。視覚センサベースのＳＬＡＭフィルタは、ワールド座標系で歩行ロボット１０の位置／姿勢情報を推定できるという長所があるが、特徴点の位置情報を状態変数として維持しているため、移動距離が増加するにつれて計算時間が増加するという短所も存在する。
【0099】
図９に示されるように、第３ローカルフィルタ３３０内の予測部３３２は、以前の更新ステップｋ−１で推定（認識）された歩行ロボット１０の位置／姿勢情報に融合フィルタ３６０の位置認識結果が反映された新しい歩行ロボット１０の位置／姿勢情報から、歩行ロボット１０の次のステップｋの位置／姿勢情報を予測し、以前の更新ステップｋ−１で推定（認識）された特徴点の位置情報から歩行ロボット１０の次のステップｋの特徴点の位置情報を予測する（９１０）。この過程は、位置認識アルゴリズムの予測（ｐｒｅｄｉｃｔｉｏｎ）ステップに該当する。
【0100】
次に、第３前処理部２３０は、カメラ１２０から移動空間周辺の映像情報を獲得する（９２０）。その後、第３前処理部２３０は、格納部４００に格納されている既存の標識と現在獲得された映像情報から抽出された特徴点との間の同一性を判断する（９３０）。すなわち、第３前処理部２３０は、特徴点の追跡（ｔｒａｃｋｉｎｇ）及びマッチング（ｍａｔｃｈｉｎｇ）過程を通じて現在獲得された映像情報から抽出された特徴点が既存に標識として使用されていたのか、または新しい特徴点として登録しなければならないのかを判断する。
【0101】
次に、第３前処理部２３０は、既存に標識として登録された特徴点の位置情報と、現在獲得された映像情報から抽出されて既存の標識とマッチングされた特徴点の位置情報を算出する（９４０）。算出された既存の標識とマッチングされた特徴点の位置情報が、前述した測定情報ｚ（ｋ）となる。
【0102】
視覚センサベースのＳＬＡＭフィルタである第３ローカルフィルタ３３０において予測観測情報ｈ（ｋ）は、下の数式（１３）のように示すことができる。
【0103】
【数11】
【0104】
その後、同化部３３６は、融合フィルタ３６０の更新部３６４から歩行ロボット１０の位置認識結果（更新された位置／姿勢情報）が入力されるか否かを判断する（９６０）。融合フィルタ３６０の更新部３６４から歩行ロボット１０の位置認識結果が入力されると（９６０での‘はい’）、同化部３３６は、更新ステップで更新された歩行ロボット１０の位置／姿勢情報に融合フィルタ３６０の位置認識結果を反映し、融合フィルタ３６０の位置認識結果が反映された歩行ロボット１０の位置／姿勢情報を予測部３３２に伝送する（９７０）。この過程は、位置認識アルゴリズムの同化（ａｓｓｉｍｉｌａｔｉｏｎ）ステップに該当する。
【0105】
一方、融合フィルタ３６０の更新部３６４から歩行ロボット１０の位置認識結果が入力されない場合（９６０での‘いいえ’）、同化ステップは行わずに、動作９８０に進む。
【0106】
次に、制御部２００は、歩行ロボット１０の移動が停止されたか否かを判断する（９８０）。制御部２００は、入力部（図示せず）を通じて使用者から歩行ロボット１０の歩行停止命令が入力されたり、エンコーダ１１０からこれ以上の回転角度情報が入力されない場合、歩行ロボット１０の移動が停止したものと判断する。
【0107】
歩行ロボット１０の移動が停止されないと（９８０での‘いいえ’）、制御部２００は、動作９１０に戻り、歩行ロボット１０の位置認識動作を続けて行うように制御する。歩行ロボット１０の移動が停止された場合（９８０での‘はい’）、制御部２００は歩行ロボット１００の位置認識過程を終了する。
【0109】
第４ローカルフィルタ３４０は、歩行ロボット１０の位置認識を行うためのセンサとして慣性センサ１３０を使用し、３次元加速度情報及び３次元角速度情報を用いて予測された歩行ロボット１０の位置／姿勢情報を更新する。説明の便宜上、第４ローカルフィルタ３４０をＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）フィルタと定義する。
【0110】
図１０に示されるように、第４ローカルフィルタ３４０の予測部３４２は、以前の更新ステップｋ−１で推定（認識）された歩行ロボット１０の位置／姿勢情報に融合フィルタ３６０の位置認識結果が反映された新しい歩行ロボット１０の位置／姿勢情報から、歩行ロボット１０の次のステップｋの位置／姿勢情報を予測する（１０１０）。この過程は、位置認識アルゴリズムの予測（ｐｒｅｄｉｃｔｉｏｎ）ステップに該当する。
【0111】
次に、第４前処理部２４０は、慣性センサ１３０から慣性測定情報を獲得する（１０２０）。その後、第４前処理部２４０は、慣性センサ１３０を通じて検出された加速度情報及び角速度情報から３次元加速度情報及び３次元角速度情報を算出する（１０３０）。算出された３次元加速度情報及び３次元角速度情報が、前述した測定情報ｚ（ｋ）となる。
【0112】
ＩＭＵフィルタ３４０において予測観測情報ｈ（ｋ）は、下の数式（１４）のように示すことができる。
【0113】
【数12】
【0114】
次に、同化部３４６は、融合フィルタ３６０の更新部３６４から歩行ロボット１０の位置認識結果（更新された位置／姿勢情報）が入力されるか否かを判断する（１０５０）。融合フィルタ３６０の更新部３６４から歩行ロボット１０の位置認識結果が入力されると（１０５０での‘はい’）、同化部３４６は、更新ステップで更新された歩行ロボット１０の位置／姿勢情報に融合フィルタ３６０の位置認識結果を反映し、融合フィルタ３６０の位置認識結果が反映された歩行ロボット１０の位置／姿勢情報を予測部３４２に伝送する（１０６０）。この過程は、位置認識アルゴリズムの同化（ａｓｓｉｍｉｌａｔｉｏｎ）ステップに該当する。
【0115】
一方、融合フィルタ３６０の更新部３６４から歩行ロボット１０の位置認識結果が入力されない場合（１０５０での‘いいえ’）、同化ステップは行わずに、動作１０７０に進む。
【0116】
次に、制御部２００は、歩行ロボット１０の移動が停止されたか否かを判断する（１０７０）。制御部２００は、入力部（図示せず）を通じて使用者から歩行ロボット１０の歩行停止命令が入力されたり、エンコーダ１１０からこれ以上の回転角度情報が入力されない場合、歩行ロボット１０の移動が停止したものと判断する。
【0117】
歩行ロボット１０の移動が停止されないと（１０７０での‘いいえ’）、制御部２００は、動作１０１０に戻り、歩行ロボット１０の位置認識動作を続けて行うように制御する。歩行ロボット１０の移動が停止された場合（１０７０での‘はい’）、制御部２００は歩行ロボット１００の位置認識過程を終了する。
【0119】
第５ローカルフィルタ３５０は、歩行ロボット１０の位置認識を行うためのセンサとしてカメラ１２０を使用し、映像情報から抽出した特徴点の３次元位置情報を通じたノード認識と、歩行ロボット１０の３次元位置／姿勢情報とを同時に推定する。したがって、他のローカルフィルタ３１０，３２０，３３０，３４０とは異なり、推定のための状態変数ｘＬ（ｋ｜ｋ）としてワールド原点座標系に対する３次元位置ｒ及び３次元姿勢（ｑｕａｔｅｒｎｉｏｎ）ｑのみを有する。
【0120】
図１１に示されるように、第５ローカルフィルタ３５０の予測部３５２は、以前の更新ステップｋ−１で推定（認識）された歩行ロボット１０の位置／姿勢情報に新しいノード認識結果が反映された新しい歩行ロボット１０の位置／姿勢情報から、歩行ロボット１０の次のステップｋの位置／姿勢情報を予測し、以前の更新ステップｋ−１で推定（認識）されたノードの位置情報から歩行ロボット１０の次のステップｋのノードの位置情報を予測する（１１１０）。この過程は、位置認識アルゴリズムの予測（ｐｒｅｄｉｃｔｉｏｎ）過程に該当する。
【0121】
次に、第５前処理部２５０は、カメラ１２０から移動空間周辺の映像情報を獲得する（１１２０）。
【0122】
その後、第５前処理部２５０は、格納部４００に格納されているローカルマップ情報と、現在獲得された映像情報から抽出された特徴点との間の同一性の判断を通じてノードを検出する（１１３０）。すなわち、第５前処理部２５０は、特徴点の追跡（ｔｒａｃｋｉｎｇ）及びマッチング（ｍａｔｃｈｉｎｇ）過程を通じて現在獲得された映像情報から抽出された特徴点に対応するノードを検出する。
【0123】
次に、第５前処理部２５０は、検出されたノードの位置を基準としてローカルマップ上での歩行ロボット１０の絶対位置／姿勢情報ｚ（ｋ）（数式（７）参照）を算出する（１１４０）。
【0124】
ノード検出を通じて算出された絶対位置／姿勢情報ｚ（ｋ）及び予測観測情報ｈ（ｋ）（数式（８）参照）を用いてカルマンフィルタ更新（Ｋａｌｍａｎ Ｆｉｌｔｅｒ Ｕｐｄａｔｅ）を行う（１１５０）。すなわち、第５ローカルフィルタ３５０の更新部３５４は、算出された既存のノード位置情報を前述した数式（７）のｚ（ｋ）に代入し、数式（６）のヤコビアン行列を算出して数式（２）及び数式（３）のＨ（ｋ）に代入して、予測ステップで予測された歩行ロボット１０の位置／姿勢情報及び特徴点の位置情報を更新する。この過程は、位置認識アルゴリズムの更新（ｕｐｄａｔｅ）ステップに該当する。
【0125】
第５ローカルフィルタ３５０の更新部３５４は、更新された歩行ロボット１０の絶対位置／姿勢情報を融合フィルタ３６０の同化部３６６に提供する（１１６０）。第５ローカルフィルタ３５０から絶対位置／姿勢情報の提供を受けた融合フィルタ３６０の同化部３６６は、更新ステップで更新された歩行ロボット１０の位置／姿勢情報に第５ローカルフィルタ３５０の絶対位置／姿勢情報を反映して融合フィルタ３６０の予測部３６２に伝送することにより、融合フィルタ３６０での位置認識アルゴリズムの同化（ａｓｓｉｍｉｌａｔｉｏｎ）過程が行われる。
【0126】
次に、制御部２００は、歩行ロボット１０の移動が停止されたか否かを判断する（１１７０）。制御部２００は、入力部（図示せず）を通じて使用者から歩行ロボット１０の歩行停止命令が入力されたり、エンコーダ１１０からこれ以上の回転角度情報が入力されない場合、歩行ロボット１０の移動が停止したものと判断する。
【0127】
歩行ロボット１０の移動が停止されないと（１１７０の‘いいえ’）、制御部２００は、動作１１１０に戻り、歩行ロボット１０の位置認識動作を続けて行うように制御する。歩行ロボット１０の移動が停止された場合（１１７０での‘はい’）、制御部２００は歩行ロボット１００の位置認識過程を終了する。
【0128】
以下では、再び図４を参照して融合フィルタ３６０の動作過程について説明する。
【0129】
融合フィルタ３６０の基本的な動作原理は、第１〜４ローカルフィルタ３１０，３２０，３３０，３４０の動作原理と類似しているが、融合フィルタ３６０は、第１〜４ローカルフィルタ３１０，３２０，３３０，３４０の動作フィルタとして使用されるカルマンフィルタ（Ｋａｌｍａｎ ｆｉｌｔｅｒ）とデュアル（ｄｕａｌ）タイプである情報フィルタ（ｉｎｆｏｒｍａｔｉｏｎ ｆｉｌｔｅｒ）を動作フィルタとして使用する。情報フィルタは、歩行ロボット１０の位置及び姿勢を推定するために、状態変数（ｓｔａｔｅ）として、下の数式（１５）及び数式（１６）に示したように、情報状態ベクトルｙＧ（ｋ｜ｋ）と情報行列ＹＧ（ｋ｜ｋ）を有する。
【0130】
【数13】
【0131】
【数14】
【0132】
このような分散化されたフィルタシステム３００の構造は、第１〜５ローカルフィルタ３１０，３２０，３３０，３４０，３５０に問題が生じる場合、融合フィルタ３６０に伝達される情報を遮断することができ、第１〜５ローカルフィルタ３１０，３２０，３３０，３４０，３５０の問題のみ解決した後、再び位置認識動作を続けて行うことができるので、全体位置認識システムの強靭性を向上させることができる。
【0133】
＜融合フィルタ３６０の動作＞図１２は、図４に示される融合フィルタ３６０を用いた歩行ロボットの位置認識方法を示したフローチャートである。図１２に示されるように、融合フィルタ３６０の予測部３６２は、以前の更新ステップｋ−１で推定（認識）された歩行ロボット１０の位置／姿勢情報から歩行ロボット１０の次のステップｋの位置／姿勢情報を予測する（１２１０）。この過程は、位置認識アルゴリズムの予測（ｐｒｅｄｉｃｔｉｏｎ）ステップに該当する。
【0134】
次に、同化部３６６は、第１〜５ローカルフィルタ３１０，３２０，３３０，３４０，３５０から歩行ロボット１０の位置認識結果（更新された相対位置／姿勢情報及び絶対位置／姿勢情報）を獲得する（１２２０）。
【0135】
その後、同化部３６６は、第１〜５ローカルフィルタ３１０，３２０，３３０，３４０，３５０の位置認識結果を統合する（１２３０）。このとき、第５ローカルフィルタ３５０を通じて得た絶対位置／姿勢情報が他の第１〜４ローカルフィルタ３１０，３２０，３３０，３４０を通じて得た相対位置／姿勢情報に反映されることにより、第１〜４ローカルフィルタ３１０，３２０，３３０，３４０の相対位置／姿勢情報に含まれ得る誤差が改善されることができる。これによって、本発明の実施例に係る歩行ロボット１０は持続的にさらに強靭な位置認識を行うことができる。
【0136】
次に、更新部３６４は、統合された位置認識結果を用いて予測ステップで予測された歩行ロボット１０の位置／姿勢情報を更新する（１２４０）。この過程は、位置認識アルゴリズムの更新ステップに該当する。更新部３６４は、更新された歩行ロボット１０の位置／姿勢情報を第１〜４ローカルフィルタ３１０，３２０，３３０，３４０の同化部３１６，３２６，３３６，３４６に伝送する（１２５０）。第５ローカルフィルタ３５０は、融合フィルタ３６０から情報のフィードバックを受けない。
【0137】
その後、制御部２００は、歩行ロボット１０の移動が停止されたか否かを判断する（１２６０）。制御部２００は、入力部（図示せず）を通じて使用者から歩行ロボット１０の歩行停止命令が入力されたり、エンコーダ１１０からこれ以上の回転角度情報が入力されない場合、歩行ロボット１０の移動が停止したものと判断する。
【0138】
歩行ロボット１０の移動が停止されないと（１２６０の‘いいえ’）、制御部２００は、動作１２１０に戻り、歩行ロボット１０の位置認識動作を続けて行うように制御する。歩行ロボット１０の移動が停止された場合（１２６０の‘はい’）、制御部２００は歩行ロボット１００の位置認識過程を終了する。
【0139】
上記では、図１乃至図１２を参照して、多様な移動装置のうち歩行ロボット１０を例に挙げて、分散化されたフィルタシステムを用いて歩行ロボットの位置認識動作を行う場合を説明したが、歩行ロボットの他にも、多様な形態の移動ロボット、及び自律移動機能を有してはいないが人間が携帯し、移動しながら操作できる移動装置（携帯電話やタブレットなど）の場合にも、本発明で提示している分散化されたフィルタシステムを用いて位置認識動作を行うことが可能である。
【0140】
また、上記では、移動装置の位置認識を行うためのセンサとして、エンコーダ、カメラ、慣性センサを利用（総３個のセンサを利用）する場合を例に挙げて説明したが、その他にも、距離センサ、コンパスセンサ、衛星航法装置（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ；ＧＰＳ）など、移動装置の位置認識を行うのに利用され得るセンサ（または装置）であれば、いかなるセンサでも使用可能であることは勿論である。
【0142】
まず、各ローカルフィルタで移動装置の位置認識動作を行う（１３１０）。その後、各ローカルフィルタは、位置認識結果を融合フィルタに伝送する（１３２０）。次に、融合フィルタは、各ローカルフィルタから伝送された位置認識結果を融合する（１３３０）。
【0143】
その後、融合フィルタは、融合された位置認識結果を用いて移動装置の位置認識を行う（１３４０）。次に、融合フィルタは、位置認識結果を各ローカルフィルタに伝送する（１３５０）。その後、移動装置の移動が停止されたか否かを判断する（１３６０）。移動装置の移動が停止されないと（１３６０での‘いいえ’）、動作１３１０に戻り、移動装置の位置認識動作を続けて行う。
【0144】
一方、移動装置の移動が停止された場合（１３６０での‘はい’）、移動装置の位置認識過程を終了する。
【符号の説明】
【0145】
１０ 歩行ロボット１００ センサモジュール
１１０ エンコーダ
１２０ カメラ
１３０ 慣性センサ
２００ 制御部
２１０〜２５０ 第１〜５前処理部
３００ 位置認識部
３１０〜３４０ 第１〜４ローカルフィルタ（第１分散フィルタ）
３５０ 第５ローカルフィルタ（第２分散フィルタ）
３６０ 融合フィルタ
４００ 格納部
４５０ 駆動部