JP2012064131A - 地図生成装置、地図生成方法、移動体の移動方法、及びロボット装置 - Google Patents

Abstract

【課題】たくさんの人が存在する混雑な環境下で地図構築を行う。
【解決手段】ロボットが移動する間に連続して撮影した連続画像を取得する連続画像取得部１１と、連続画像から各特徴点における局所特徴量を抽出する局所特徴量抽出部１２と、局所特徴量抽出部１２により抽出した局所特徴量について、連続する入力画像間でマッチングをとる特徴量マッチング１３と、特徴量マッチング部１３により所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める不変特徴量算出部１４と、連続画像を取得した各時刻におけるロボットの位置に基づき各不変特徴量に対応する距離情報を算出する距離情報取得部１５と、当該不変特徴量及び距離情報を有するハイブリッド地図として、局所メトリカル地図を生成する地図生成部１６とを有する。
【選択図】図２

Description

本発明は、複数の連続する連続画像から特徴量を抽出し、これに基づき地図を作成する地図生成装置及び方法、この地図に基づき移動する移動体の移動方法、並びにこの地図生成装置を搭載したロボット装置に関する。

近年のロボット産業の発展に伴い、警備ロボットや介護ロボットなどの、自律移動ロボットの導入が期待されている。一般に、移動ロボットが自律的にナビゲーションを行うためには、障害物の位置情報などを表した地図が必要となる。未知の環境で移動ロボットが自律的に地図構築・自己位置推定を行う問題はＳＬＡＭ(Simultaneously Localization And Mapping) と呼ばれており、従来から広く研究されてきた。

C.Bibby, I.Reid: "Simultaneous localisation and mapping in dynamic environments (SLAMIDE) with reversible data association", Proc. of Robotics: Scienceand Systems (RSS), 2007 W.Burgard, et al.:"Experiences with an interactive museum tour-guide robot", Arti_cial Intelligence, 114(1-2), 3-55, 1999 C.C.Wang: "Simultaneous Localization, Mapping and Moving Object Tracking", The International Journal of Robotics Research (IJRR), 2007 D.Wolf, G.S.Sukhatme: "Online Simultaneous Localization and Mapping in Dynamic Environments", Proc. of ICRA, 2004 F.T.Ramos, et al.:"Recognising and Modelling Landmarks to Close Loops in Outdoor SLAM", Proc. Of ICRA, 2007 P.Biber, T.Duckett: "Dynamic Maps for Long Term Operation of Mobile Service Robots", Proc. of Robotics: Science and Systems (RSS), 2005

しかしながら、従来のシステムでは、駅構内やショッピングモールのようにたくさんの人が入り乱れている環境下では、安定した地図構築・自己位置推定ができないという問題点がある。

一般的なＳＬＡＭの手法では、環境は静的であると仮定して処理を行う。そのため、それらの手法を混雑な環境下で用いた場合、誤って人間のような、動いている物をランドマークとして認識してしまうことによって、地図構築・自己位置推定に誤りが生じ、アルゴリズムが破綻してしまうという問題が起こる。このような問題に対応するためには、大きく分けて２種類の方法が考えられる。１つ目は、動いている物体を陽に扱う方法であり、２つ目は動いている物体を外れ値として取り除いて処理する方法である。

動いている物体を陽に扱う方法としては、例えば非特許文献１では、最小二乗法とＥＭアルゴリズムを用いることで過去の観測点に対するデータ対応付けのやり直しを可能とし、動的な物体と静的な物体とを同じ枠組みで扱う手法が提案されている。

また、動いている物を外れ値として取り除く方法としては、例えば非特許文献２ではエントロピーフィルタを用い、すでに知っている物と、人のように動いている障害物を分離することで、美術館という人が多い環境下で自己位置推定を行うツアーロボットが提案されたが、事前に正確な地図が与えられている必要があった。

また、非特許文献３、４では動く物体をトラッキングし、ＳＬＡＭの処理から除外することで、精度の向上を行っている。しかしこれらの研究においては、環境の表現方法や、動いている物と止まっている物の分離方法など、未解決の問題が残されている。

さらに、非特許文献５では、事前に画像データから木などの静的なものを学習しておき、それをランドマークとしてＳＬＡＭ に利用することで、人や車などの、事前に学習したランドマーク以外の物体による誤差を抑えるという手法がとられている。しかし、事前に画像からランドマークを学習しておく必要があるため、ロボットを違う環境に適応させる際、新しくランドマークを覚えなおす必要がある。

非特許文献６では、障害物の配置の変化など、長い時間尺度での環境の変化による新しい観測データと、過去に訪れた際の観測データとのトレードオフを考え、徐々に地図を修正する手法がとられている。しかし、動いている人のような、短い時間尺度での環境の変化に対するロバスト性については考慮されていなかった。

本発明は、このような問題点を解決するためになされたものであり、たくさんの人が存在する混雑な環境下でオンライン・追加的に地図構築・自己位置推定を行い、人間が教えた道順通りに自律的に移動するロボット装置、移動体の移動方法、当該ロボット装置が使用する地図を生成する地図生成装置及び地図生成方法を提供することを目的とする。

本発明に係る地図生成方法は、移動体が移動する間に連続して撮影した連続画像を取得する連続画像取得工程と、前記連続画像取得工程にて取得された連続画像から各特徴点における局所特徴量を抽出する局所特徴量抽出工程と、前記局所特徴量抽出工程にて抽出された前記局所特徴量について、前記連続する入力画像間でマッチングをとる特徴量マッチング工程と、前記特徴量マッチング工程にて所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める不変特徴量算出工程と、前記連続画像を取得した各時刻における前記移動体の位置に基づき前記各不変特徴量に対応する距離情報を算出する距離情報取得工程と、当該不変特徴量及び前記距離情報を有するハイブリッド地図として、局所メトリカル地図を生成する地図生成工程とを有するものである。

本発明においては、不変特徴量（ＰＩＲＦ）を算出し、連続画像を取得した各時刻における移動体の位置に基づき各ＰＩＲＦに対応する距離情報を算出し、ＰＩＲＦに距離情報を付加してハイブリッド地図である局所メトリカル地図を作成することができる。このハイブリッド地図の作成により、経路計画が容易となり、距離情報を有するため、最短経路の探索など、高度な経路計画を行うことが可能となる。

また、連続する複数の連続画像を取得した各時刻における前記移動体の移動に関する情報と、前記不変特徴量を有する特徴点の前記移動体からの計測方向を示す観測値とから、前記移動体の位置及び姿勢と、前記不変特徴量に対応する特徴点の位置とを求める位置情報取得工程とを有し、前記地図生成工程では、前記移動体の位置及び姿勢、並びに特徴点の位置及びその不変特徴量を有する前記ハイブリッド地図を生成することができる。移動に関する情報と観測値とから移動体の位置及び姿勢と特徴点の位置を求めることができ、これにより、より実用性の高いハイブリッド地図を生成することができる。

さらに、前記位置情報取得工程では、前記不変特徴量を求めた複数の連続画像を取得した各時刻における前記移動に関する情報の系列と、前記観測値の系列とから、確率的に尤もらしい前記移動体の位置及び姿勢、並びに前記特徴点の位置を算出することができ、より正確に移動体の位置及び姿勢と特徴点の位置を算出することができる。

更にまた、前記位置情報取得工程は、ｘ_ｉを前記移動体の位置及び姿勢としたとき、前記複数の連続画像間において、前記不変特徴量が各時刻において前記移動体からどの方向に見えるかを示す前記観測値ｚ_ｉｊ、及び前記移動体がｘ_ｉからｘ_ｉ−１まで移動するのに用いた距離情報を示す入力ｕ_ｉを取得し、前記観測値ｚ_ｉｊと前記入力ｕ_ｉとから下記式を最小化することで得られる、各時刻における前記移動体の位置及び姿勢、及び前記特徴点の位置を含む前記ハイブリッド地図を作成することができる。ＰＩＲＦに関する距離情報を求めるために、ＳＷＦ（Sliding Window Filter）を使用することができる。

を示す。

また、前記不変特徴量と、前記移動体の位置及び姿勢とは、複数枚の連続画像から得られるものであって、前記地図生成工程は、前記移動体の位置及び姿勢及び前記不変特徴量を算出するために使用する連続画像が各時刻において同数となるよう、過去の連続画像を削除すると共に、当該連続画像を削除することで見えなくなる前記不変特徴量を削除する過去画像削除工程を有することができる。これにより、常に計算速度を一定に保つことができる。

さらに、現在の場所が過去に訪れたことがある場所か否かを判定する場所判定工程と、現在の場所が過去に訪れたことがある場所である場合に、前回の前記移動体の位置及び姿勢ｘ_Ｌｉ０と、今回の前記移動体の位置及び姿勢ｘ_Ｌｉとから、ｘ_Ｌｉ０及びｘ_Ｌｉを修正する軌跡修正工程とを有することができる。これにより、より正確な地図を作成することができる。

さらに、前記軌跡修正工程では、下記の式を最小化することで、前記ｘ_Ｌｉ０及びｘ_Ｌｉを修正することができる。

ここで、Ｐ_Ｌ、Ｑ_ｙは、マハラノビス距離を算出する際の共分散、ｘ_Ｌｉは、ｉ番目に過去に訪れた場所であることが検出された位置、ｘ_Ｌｉ０は、ｘ_Ｌｉと同じ場所であると認識された場所、Ｎ_Ｌは、過去に訪れた場所であることが検出された回数、Ｔは現在までの総ステップ数、ｖ_ｔは、ｘ_ｔ−１からみたｘ_ｔの相対座標、
x_(t-1)?v_tは、移動体の軌跡ｘ_ｔ−１の相対座標上でｖ_ｔ移動した場合に到達する位置を示す。

本発明にかかる地図生成装置は、移動体が移動する間に連続して撮影した連続画像を取得する連続画像取得手段と、前記連続画像取得手段により取得した連続画像から各特徴点における局所特徴量を抽出する局所特徴量抽出手段と、前記局所特徴量抽出手段により抽出した前記局所特徴量について、前記連続する入力画像間でマッチングをとる特徴量マッチング手段と、前記特徴量マッチング手段により所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める不変特徴量算出手段と、前記連続画像を取得した各時刻における前記移動体の位置に基づき前記各不変特徴量に対応する距離情報を算出する距離情報取得手段と、当該不変特徴量及び前記距離情報を有するハイブリッド地図として、局所メトリカル地図を生成する地図生成手段とを有するものである。

本発明にかかる移動体の移動方法は、予め作成された局所メトリカル地図に従って経路計画を行い移動する移動体の移動方法であって、移動体が移動する間に連続画像を取得する連続画像取得工程と、前記連続画像取得工程にて取得された連続画像から各特徴点における局所特徴量を抽出する局所特徴量抽出工程と、前記局所特徴量抽出工程にて抽出された前記局所特徴量について、前記連続する入力画像間でマッチングをとる特徴量マッチング工程と、前記特徴量マッチング工程にて所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める不変特徴量算出工程と、前記連続画像を取得した各時刻における前記移動体の位置に基づき前記各不変特徴量に対応する距離情報を算出する距離情報取得工程と、算出した不変特徴量及び距離情報と、前記局所メトリカル地図上に登録されている不変特徴量及び距離情報を比較し、現在の位置が登録されているか否かを判定し、登録されている場合は、当該局所メトリカル地図の情報に基づき現在位置の修正を行う軌跡修正工程とを有し、前記局所メトリカル地図は、前記移動体が前記連続画像を取得した各時刻における位置情報に基づき算出された前記各不変特徴量に対応する距離情報と、前記不変特徴量とを有するハイブリッド地図とすることができる。

本発明においては、不変特徴量と距離情報を有するハイブリッド地図によりナビゲーションを実施するため、例えば食堂などの複雑な環境下においても、安定した特徴量が抽出されているため、極めて正確なナビゲーションを実施することができる。

また、前記軌跡修正工程では、現在の場所が前記局所メトリカル地図上に登録されていた場合に、登録されている前記移動体の位置及び姿勢ｘ_{Ｌｅａｒｎｅｄｉ０}と、今回の前記移動体の位置及び姿勢ｘ_Ｌｉとから、ｘ_{Ｌｅａｒｎｅｄｉ０}及びｘ_Ｌｉを修正することができ、Ｌｏｏｐ−ｃｌｏｓｉｎｇにより、正確なナビゲーションを実施することができる。

さらに、前記軌跡修正工程では、下記の式を最小化することで、前記ｘ_{Ｌｅａｒｎｅｄｉ０}及びｘ_Ｌｉを修正することができる。

ここで、Ｐ_Ｌ、Ｑ_ｙは、マハラノビス距離を算出する際の共分散、ｘ_Ｌｉは、ｉ番目に地図に登録されている場所であることが検出された位置、ｘ_{Ｌｅａｒｎｅｄｉ０}は、ｘ_Ｌｉと同じ場所であると認識された場所、Ｎ_Ｌは、地図に登録された場所であることが検出された回数、Ｔは現在までの総ステップ数、ｖ_ｔは、ｘ_ｔ−１からみたｘ_ｔの相対座標、
x_(t-1)?v_tは、移動体の軌跡ｘ_ｔ−１の相対座標上でｖ_ｔ移動した場合に到達する位置を示す。

さらにまた、現在の位置が、地図に登録されている場所とどれだけ違うかを示す相対座標を算出する相対座標算出工程を更に有し、前記軌跡修正工程では、前記ｘ_Ｌｉとして前記相対座標を使用して、前記軌跡の修正を行うことができる。現在の位置と登録位置との相対座標を求めてＬｏｏｐ−Ｃｌｏｓｉｎｇすることにより、更に正確なナビゲーションを実施することができる。

本発明にかかるロボット装置は、予め作成された局所メトリカル地図に従って経路計画を行い移動するロボット装置であって、移動体が移動する間に連続画像を取得する連続画像取得手段と、前記連続画像取得手段により取得した連続画像から各特徴点における局所特徴量を抽出する局所特徴量抽出手段と、前記局所特徴量抽出手段により抽出した前記局所特徴量について、前記連続する入力画像間でマッチングをとる特徴量マッチング手段と、前記特徴量マッチング手段により所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める不変特徴量算出手段と、前記連続画像を取得した各時刻における前記移動体の位置に基づき前記各不変特徴量に対応する距離情報を算出する距離情報取得手段と、算出した不変特徴量及び距離情報と、前記局所メトリカル地図上に登録されている不変特徴量及び距離情報を比較し、現在の位置が登録されているか否かを判定し、登録されている場合は、当該局所メトリカル地図の情報に基づき現在位置の修正を行う軌跡修正手段とを有し、前記局所メトリカル地図は、前記移動体が前記連続画像を取得した各時刻における位置情報に基づき算出された前記各不変特徴量に対応する距離情報と、前記不変特徴量とを有するハイブリッド地図とすることができる。

本発明によれば、たくさんの人が存在する混雑な環境下で地図構築を行い、人間が教えた道順通りに自律的に移動するロボット装置、移動体の移動方法、当該ロボット装置が使用する地図を生成する地図生成装置及び地図生成方法を提供することができる。

本発明の実施の形態１にかかるハイブリッド地図を示す図である。 本発明の実施の形態１にかかる地図生成装置を示す図である。 本発明の実施の形態１にかかる特徴量ＰＩＲＦを抽出する方法を説明する図である。 本発明の実施の形態１におけるＰＩＲＦ及び３Ｄ−ＰＩＲＦの概念を示す図である。 本発明の実施の形態１におけるＳＷＦを示す図であって、３Ｄ−ＰＩＲＦを算出方法を説明する図である。 本発明の実施の形態１にかかる地図生成方法を示すフローチャートです。 本発明の実施の形態２にかかる本実施の形態にかかるナビゲーション装置を示すブロック図である。 本発明の実施の形態２における相対座標の算出方法を説明する図である。 本発明の実施の形態２にかかるロボットのナビゲーション方法を示す図である。 本発明の実施の形態２にかかるロボットを示すブロック図である。 本発明の実施例の屋内環境を示す図である。 本発明の実施例の環境の真値地図と、学習フェーズでロボットを移動させた道順を示す図である。 本発明の実施例において、カメラの情報を使わずオドメトリのみから計算したロボットの軌跡を示す図である。 本実施の形態の方法により、ロボットが学習した軌跡を示す図である。 ロボットが自律的に移動した軌跡を示す図である。

以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。この実施の形態は、本発明を、局所地図を用いたハイブリッド地図（局所メトリカル地図）を生成する地図生成装置、及びこの局所メトリカル地図を利用してナビゲーションするロボット装置に適用したものである。

本発明においては、たくさんの人が存在する混雑な環境下でオンライン・追加的に地図構築・自己位置推定を行い、人間が教えた道順通りに自律的に移動することを目的としている。

混雑な環境下でも安定な３次元上の特徴点の抽出画像を用いたＳＬＡＭにおいて多く用いられている特徴点として、ＳＩＦＴやコーナー点がある。一般にＳＩＦＴやコーナー点などの局所特徴点は対応付けが取りやすく、画像を用いたＳＬＡＭに適している。しかし、多くの人が入り乱れる混雑な環境下でロボットが取得した画像からＳＩＦＴを抽出すると、ロボットが少し移動しただけで消えてしまうような弱い特徴点や、動いている人物からの特徴点が多く抽出されてしまうという問題があった。そのような不安定な特徴点が多いと、無駄に処理時間が延びるだけでなく、地図構築に悪影響を及ぼすという問題が起こる。

これに対し、本件出願人は、特願２００９−２００７６４、特願２０１０−０８１０２７、及び特願２０１０−１１５３０７において、ＰＩＲＦ（Position Invariant Robust Feature）という特徴量を提案している。ＰＩＲＦは、後述するように、複数枚の連続画像からＳＩＦＴを抽出して連続的にマッチングを取り、全ての画像でマッチングが取れた特徴点のみを抽出する手法である。

ＰＩＲＦはロボットの移動に対して見た目の変化の小さい部分を特徴として抽出することができ、それにともなって、動いている人物などから抽出される見た目の変化の大きい部分の特徴点を除外できるという特徴がある。

ところで、ＳＬＡＭには大きく分けて、地図を連続的な空間配置で表すメトリカルな手法と、地図が離散的なグラフ構造で表されるトポロジカルな手法と、それらを組み合わせたハイブリッドな手法がある。ハイブリッドな手法では、各ノードが局所的なメトリカル地図の情報を持つようなトポロジカルな地図が構築される。一方、ハイブリッドな手法はグラフ構造を有するため経路計画が容易であり、距離情報も用いることが可能であるため、最短経路の探索など、より高度な経路計画を行うことが可能であるという特徴がある。

そこで、本願発明者等は鋭意実験研究し、上記ＰＩＲＦを利用して複数の画像データから特徴点を取り出すことで、たくさんの人が存在する混雑な環境下でも安定した特徴点を抽出し、これを３次元空間にマッピングすることで、ＰＩＲＦを利用したハイブリッド地図を構築する方法を見出した。

本来ＰＩＲＦは、画像から抽出される画像上の特徴点であり、直接ＳＬＡＭに適用することはできない。さらに、ＰＩＲＦは、１枚の画像からではなく、複数枚の連続する連続画像から抽出される特徴量である。したがって、単純には、この特徴量に距離情報を結びつけることができない。そこで、本願発明者等は、鋭意実験研究し、連続画像のＳＩＦＴにオドメトリの情報を組み合わせることで、ＰＩＲＦを３次元空間にマッピングする（３Ｄ−ＰＩＲＦ）手法を見出した。

本発明の実施の形態１．
図１は、本実施の形態にかかるハイブリッド地図を示す図である。ロボットの軌跡はノードｘ_ｔとエッジｖ_ｔによるグラフで表され、各ノードは３Ｄ−ＰＩＲＦの位置ｍ_ｘと分散Π_ｘの情報を有し、各エッジはノード間の相対位置ｖの情報を有する。なお、ｖ_ｔは、ｘ_ｔ−１から見たｘ_ｔの相対座標として計算することができる。

まず、本実施の形態にかかる地図生成装置について説明する。図２は、本実施の形態にかかる地図生成装置を示す図である。図２に示すように、本実施の形態にかかる地図生成装置１は、連続画像取得部１１、局所特徴量抽出部１２、特徴量マッチング部１３、不変特徴量算出部１４、距離情報取得部１５、及び地図生成部１６を有する。

連続画像取得部１１は、ロボット等の移動体が移動する間に連続して撮影した連続画像を取得する。局所特徴量抽出部１２は、連続画像取得部１１により取得した連続画像から各特徴点における局所特徴量を抽出する。特徴量マッチング部１３は、局所特徴量抽出部１２により抽出した局所特徴量について、連続する入力画像間でマッチングをとる。不変特徴量算出部１４は、特徴量マッチング部１３により所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める。距離情報取得部１５は、連続画像を取得した各時刻におけるロボットの位置（座標等）に基づき各不変特徴量に対応する距離情報を算出する。地図生成部１６は、この不変特徴量及び距離情報を有するハイブリッド地図として、局所メトリカル地図を生成する。

ここで、距離情報取得部１５又は地図生成部１６は、連続する複数の連続画像を取得した各時刻におけるロボットの移動に関する情報と、不変特徴量（ＰＩＲＦ）を有する特徴点のロボットからの計測方向を示す観測値とから、ロボットの位置及び姿勢と、ＰＩＲＦに対応する特徴点の位置とを求める位置情報取得部（不図示）を有している。これにより、地図生成部１６は、ロボットの位置及び姿勢、並びに特徴点の位置及びそのＰＩＲＦを有するハイブリッド地図を生成することができる。

また、位置情報取得部は、ＰＩＲＦを求めた複数の連続画像を取得した各時刻におけるロボットに関する情報の系列と、観測値の系列とから、確率的に最も尤もらしいロボットの位置及び姿勢、並びに特徴点の位置を算出する。このロボットの位置及び姿勢、特徴点の位置の求め方についての詳細は後述する。

以下、本実施の形態にかかる地図生成装置１について、詳細い説明する。先ず最初に、不変特徴量であるＰＩＲＦの算出方法について説明する。上述したように、本件出願人は、先に特願２００９−２００７６４、特願２０１０−０８１０２７、及び特願２０１０−１１５３０７により、ＰＩＲＦの算出方法について開示している。

ＰＩＲＦは、近くの対象については撮影位置や撮影時間帯の変化による見え方の差（特徴量変化）が大きいが、遠くの対象については変化が小さい（ランドマークの特徴量はあまり変化しない）ことに着目し、混雑下でも誤認識しにくい特徴量として考えられたものである。

簡単には、連続画像間で局所特徴のマッチングを行い、予め定めた枚数間で、連続してマッチングのとれている特徴を選択し、選択された特徴において、それとマッチングのとれている全特徴の平均を特徴量ＰＩＲＦとして抽出・記述するものである。

先ず、連続画像取得部１１は、連続画像を取得する。ＰＩＲＦは、複数枚の連続画像から抽出される特徴量であるため、入力としては、連続する画像、すなわちビデオ画像が必要となり、例えば全方位画像を使用することができる。ＰＩＲＦで要求される連続画像とは、ある画像セットであって、一定のフレームで、例えば１秒毎に２フレームなど、毎秒毎に連続的に撮影されたビデオ画像をいう。すなわち、ビデオからキャプチャされた画像は一般的に連続的であり、ＰＩＲＦにおける連続画像は、ビデオ画像を使用したものでなければならない。画像の取得率は、カメラの速度に応じて設定される。たとえば、カメラが車に搭載されていた場合、カメラの速度は１分間に約１０００ｍ／分であり、ビデオからキャプチャされる連続画像はおよそ５０乃至１００フレーム／秒となる。

図３は、本実施の形態にかかる特徴量ＰＩＲＦを抽出する方法を説明する図である。局所特徴量抽出部１２は、既存の局所特徴量抽出方法を使用して局所特徴量を抽出する。局所特徴量抽出部１２は、例えば、ＳＩＦＴ（Scale Invariant Feature Transformation）、又はＳＵＲＦ（Speed Up Robustness Features）の特徴量を使用することができる。または、これらＳＩＦＴやＳＵＲＦに限らず、他の局所特徴量を使用することができることは勿論である。特に、スケール、回転の変動、又はノイズ等に対してロバストな他の局所特徴量を用いることが好ましい。これらの局所特徴量を用いることで、既存の特徴量が有する性能もそのまま引き継がれ、照明変化等にも頑健な特徴として抽出・記述することが可能となる。ここで、ｉ番目のエリアにおける全方位画像の数をｎ_ｉとし、その中でｊ番目の画像から抽出された局所特徴量の集合をＵ_ｊ ^ｉとする。ここでは、局所特徴量はｕ→で表す。

次に、特徴量マッチング部１３が、連続する２枚の画像間全てについて局所特徴量ｕ→を構成する各特徴量のマッチングを行う。すなわち、ｊ＝ｑ番目の画像の全局所特徴量について、ｊ＝ｑ＋１番目の画像の全局所特徴量に対してマッチングを行う。ここでは、それぞれマッチングのとれた特徴へのインデックスをマッチング結果ベクトルｍ_ｑ ^ｉ→として求める。

ここで、マッチング方法の一例について、ＳＩＦＴを例にとって説明する。画像Ｉ_ａから抽出される特徴をｖとする。この特徴ｖが次の画像Ｉ_ａ＋１の特徴ｖ'とマッチングするか否かを判定する。先ず、特徴ｖと画像Ｉ_ａ＋１から抽出した全ての特徴との間のドット積（dot product）を求める。そして、最も類似する特徴ｖ_{ｆｉｒｓｔ}と、２番目に類似する特徴ｖ_{ｓｅｃｏｎｄ}を求めるために、この結果をソートする。もし、
（ｖ_{ｆｉｒｓｔ}・ｖ）／（ｖ_{ｓｅｃｏｎｄ}・ｖ）＞θ
が成立する場合、マッチングのとれた特徴ｖ'＝ｖ_{ｆｉｒｓｔ}と判定する。ここで、閾値θはたとえば０．６とすることができる。上記が成立しない場合は、画像Ｉ_ａにおける特徴ｖにマッチングする特徴は、画像Ｉ_ａ＋１には存在しないと判定する。

図３に示すように、各入力画像から６つの局所特徴量が抽出された場合について説明する。これら６つの局所特徴量間でマッチングを取り、マッチングが取れた場合にのみ、マッチングが取れた特徴量へのインデックスを付す。例えば、ｍ_１ ^ｉ→の１番目の局所特徴量は、ｍ_２ ^ｉ→の３番目の局所特徴量とマッチングがとれていることを示し、ｍ_２ ^ｉ→の３番目の特徴量は、ｍ_３ ^ｉ→の６番目の特徴量とマッチングが取れていることを示す。

次に、連続特徴量選択部（不図示）は、連続特徴量を選択する。先ず、いくつのｍ_ｑ ^ｉ→を使用して連続特徴量を求めるかを決定する。このｍ_ｑ ^ｉ→の数を、本明細書においては、ウィンドウサイズｗともいう。また、このウィンドウサイズｗに含まれるｍ_ｑ ^ｉ→の集合を、サブプレイスという。ここで、ウィンドウサイズｗが大きいほどより頑健な、識別力の高い連続特徴量のみを抽出できるが、大きすぎると特徴数が極端に少なくなってしまう。また、小さすぎると、頑健ではない、識別力のないような特徴量も抽出してしまうので、目的等に応じて最適な大きさとする必要がある。

本実施の形態においては、ウィンドウサイズｗを３とする。したがって、連続する４枚の入力画像を使用して連続特徴量を求める。すなわち、図２に示すように、１番目のサブプレイスには、ｍ_１ ^ｉ→、ｍ_２ ^ｉ→、ｍ_３ ^ｉ→が含まれ、入力画像Ｉ_１、Ｉ_２、Ｉ_３、Ｉ_４が対応する。なお、インデックスの数が０の場合は、次にマッチングする特徴量がないことを示す。よって、図２の場合、１番目のサブプレイスには、３つの連続特徴量が含まれることになる。

連続特徴量選択部２３は、ウィンドウサイズｗを設定したらこのウィンドウｗをひとつずつずらしながら、そのウィンドウサイズに含まれる全方位画像４枚内で共通して出現する特徴を連続特徴量として抽出する。ウィンドウサイズｗを設定したら、連続特徴量を抽出するために用いる関数を以下のように定義する。ただし、ｂは注目するインデックスベクトルの番号とする。
そして、全てのマッチング結果ベクトルｍ_ｑ ^ｉ→について、ｆ（ｍ_ｘ,ｙ ^ｉ）を計算し、ｆ（ｍ_ｘ,ｙ ^ｉ）＞０となるときの局所特徴量ｕ_ｘ,ｙ ^ｉ→のみを抽出する。入力画像の数がｎ_ｉ、ウィンドウサイズがｗのとき、サブプレイスの数は、ｎ_ｉ−ｗ＋１となる。なお、このウィンドウサイズは、可変であってもよく、周りの状況等に合わせて、適宜変更してもよい。

不変特徴量算出部１４は、同一ウィンドウグループであるサブプレイス内においてマッチングがとれた局所特徴量の平均を求める。これらの平均値からなるベクトルによりＰＩＦＲ辞書が構成される。全サブプレイス（ｎ_ｉ−ｗ＋１）個から抽出された（ｎ_ｉ−ｗ＋１）個のサブプレイスＰＩＲＦ辞書（Ｄ_ｊ ^ｉ，ｊ≦ｎ_ｉ−ｗ＋１）をＰＩＲＦ辞書（Ｄ^ｉ）に登録する。このＰＩＲＦ辞書を構成する、マッチングがとれた局所特徴量の平均各がＰＩＲＦである。

このように抽出されたＰＩＲＦは各画像から抽出されたＳＩＦＴ特徴点のうち、すべての隣り合った画像間でマッチングがとれた特徴点として抽出されるため、見た目の変化の大きい部分の特徴点を除外することができる。

次に、このＰＩＲＦを使用して局所メトリカル地図を生成する方法について説明する。図１に戻って、先ず、連続画像とオドメトリから３Ｄ−ＰＩＲＦを算出する（Ｓ１０１）。ここで、３Ｄ−ＰＩＲＦを抽出するためには、特徴点の３次元位置を計算する必要があるが、３Ｄ−ＰＩＲＦは、過去数ステップの観測データを用いるため、ロボット等の移動体の現在の状態しか保持しないカルマンフィルタなどの手法と組み合わせることができない。

そこで、本実施の形態においては、ＳＷＦ（Sliding Window Filter）を用いる。ＳＷＦは、一定期間よりも古い姿勢・特徴点を消去していくことで、計算時間を一定に保つことができるという特徴がある。これを用いることで、３Ｄ−ＰＩＲＦの３次元位置を計算することができ、連続画像とオドメトリの情報から３Ｄ−ＰＩＲＦを抽出することが可能となる。この３Ｄ−ＰＩＲＦは複数のステップにおいて、取得された連続画像と、各ステップ間のオドメトリの情報から、３次元上の特徴点として抽出される。なお、本実施の形態においては、ＰＩＲＦと位置情報を結びつける手段としてＳＷＦの手法を用いるが、ＰＩＲＦを用いたハイブリッド地図を作成できればよく、その他の方法を用いてもよい。

図４は、ＰＩＲＦ及び３Ｄ−ＰＩＲＦの概念を示す図である。各時刻における連続画像のＳＩＦＴからＰＩＲＦが算出されると共に、各時刻におけるロボットなどの移動体の位置（座標）及び姿勢（角度）、オドメトリ、各時刻における移動体からみた特徴点に観測値（移動体に対する特徴点の角度）から、３Ｄ−ＰＩＲＦが算出される。

図５は、ＳＷＦを示す図であって、３Ｄ−ＰＩＲＦを算出方法を説明する図である。ベクトルｘ＝[ｘ_０ ^Ｔ，・・・，ｘ_Ｍ ^Ｔ]^Ｔは、各時刻での移動体（ロボット）の位置及び姿勢を示す。ｚ_ｉｊは、ロボットの姿勢ｊから観測した特徴点ｉの観測値を示す。観測値とは、特徴点のロボットからの計測方向を示し、ロボットに対する特徴点の角度として表わされる。また、ｕ_ｉは、ロボットがある位置及び姿勢ｘ_ｉ−１から次の位置及び姿勢（次の時刻の位置及び姿勢）ｘ_ｉまで移動するのに用いた入力を示す。この入力とは、例えば、車輪型のロボットであれば、車輪に与えられたスピード及び回転数を示し、これらの値により、距離を算出することができる。

ＳＷＦは、入力の系列ｕ＝[ｕ１，・・・，ｕｍ−１]Ｔと、観測値ｚ＝[ｚ１１，・・・，ｚｎｍ]Ｔから、事後確率ｐ（ｘ，ｍ│ｕ，ｚ）を最大化する

まず、ｘ、ｍ、ｕ、ｚの同時確率は、以下のようになる。

ここで、ｐ（ｘ_０）は、ロボットの位置と姿勢に関する事前分布、ｐ（ｍ）は、地図に関する事前分布、ｐ（ｘ_ｔ│ｘ_ｔ−１，ｕ_ｔ）は、姿勢ｘ_ｔ−１で入力ｕ_ｔが加わった時のｘ_ｔの動作モデル、ｐ（ｚ_ｉ│ｘ_ｔ，ｍ_ｉ）は、ロボットの位置ｘ_ｔから特徴点ｍ_ｉを見たときの観測値ｚ_ｉの計測モデルである。

これらを、それぞれ以下のようなガウス分布と仮定する。

ｆは動作モデル、ｈは観測モデルを示す。

以上の設定の下、以下の（８）式を最小化することにより、最大事後確率となるｘ＾、ｍ＾を求めることができる。

この（８）式を最適化することにより、ｘ＾、ｍ＾と、それに対応する共分散（共分散行列Π＾）を求めることができる。

次に、これらの値を用いて、ロボットの状態と各特徴点の相対的な位置関係（局所地図）が登録されたハイブリッド地図１７を構築する。

先ず、特徴点の位置ｍ＾と、その共分散行列Π＾を、局所地図として保存する。特徴点の位置ｍ＾及び共分散行列Π＾を各ステップで保存していくことで、ハイブリッド地図のノード（局所地図）を構築することができる。

また、ハイブリッド地図のエッジが有する情報である、ノード間の相対位置を求める。この相対位置は、まず、（８）式を最小化することで、上記のようにロボットの位置・姿勢ｘ＾を求めることができる。それによって、各時刻でのロボットの位置及び姿勢ｘ_ｔが求められる。これにより、下記式（９）

によってｘ_ｔ−１から見たｘ_ｔの相対座標ｖ_ｔを求めることができる。式（９）は、ｘ_ｔ−１から見たｘ_ｔの相対座標を示している。この相対座標ｖ_ｔをエッジの情報として保存する。以上の処理により、ハイブリッド地図を構築することができる。

次に、本実施の形態においては、ロボットの位置及び姿勢及びＰＩＲＦを算出するために使用する連続画像が各時刻において同数となるよう、過去の連続画像を削除すると共に、当該連続画像を削除することで見えなくなるＰＩＲＦを削除する。

なお、ロボットの位置及び姿勢及びＰＩＲＦは、複数の連続画像から算出するが、この複数の連続画像の枚数は可変であってもよい。例えば、特徴量が特に多い場所では、連続画像の枚数を増やしたり、逆に、特徴量が少ない場所では、連続画像の枚数を減らしたりすることも可能である。

ここで、ある時刻でロボットの位置及び姿勢及びＰＩＲＦを算出し、次の時刻でロボットの位置及び姿勢及びＰＩＲＦを算出する際に、もっとも過去の連続画像を破棄することができる。これにより、メモリ容量を減らし、演算速度を一定に保つことができる。

これは、ＳＷＦにおいては、marginalizing outと呼ばれる手法である。marginalizing out は、過去のロボットの姿勢、その姿勢から見えていた特徴点を周辺化によって削除する処理であり、この処理によって、計算で扱われるロボットの姿勢数Ｍが常に一定となり、各ステップの計算時間を一定に保つことができる。

以上のように、過去数ステップ分の観測値を用いることができるＳＷＦを用いることで、特徴量としてＰＩＲＦを用いた地図を構築することが可能となる。なお、本実施の形態においては、ロボットは各ステップごとに、特徴点の位置ｍ＾をロボットから見た相対座標上に座標変換したものと、その共分散を、局所地図として保存することで、各ノードが局所地図を表すハイブリッド地図を構築する。このように３次元上の特徴点である３Ｄ−ＰＩＲＦを用いることで、後述するように、人の多い混雑な環境下でもロバストに、かつ特徴点の位置情報を用いたＳＬＡＭナビゲーションを行うことが可能となる。

次に、本実施の形態にかかる地図生成部１６は、現在の場所が過去に訪れたことがある場所か否かを判定し、現在の場所が過去に訪れたことがある場所である場合に、前回の移動体の位置及び姿勢ｘ_Ｌｉ０と、今回の移動体の位置及び姿勢ｘ_Ｌｉとから、ｘ_Ｌｉ０及びｘ_Ｌｉを修正する。これを、Ｌｏｏｐ−Ｃｌｏｓｉｎｇという（図１，Ｓ１０２）。

一般的にＳＬＡＭにおいては、移動距離が長くなるほど誤差が蓄積していくという問題がある。したがって、これを修正する必要がある。地図の誤差修正を行うためには、現在いる場所が過去に訪れたことのある場所かどうかを認識する必要がある。

過去に一度訪れたことがある場所であれば、局所メトリカル地図が構築されている。局所メトリカル地図には、その位置におけるＰＩＲＦが保存されている。ロボット装置は、連続画像を取り込む各タイミング（ノード）において、ＰＩＲＦを算出し、保存されている各ノードのＰＩＲＦと一致するか否かを判定する。例えば、９０％以上の特徴量が一致した場合、その位置は、過去に訪れたことがある位置であると判定することができる。なお、判定基準は、９０％に限らず、９０％以下でも以上であってもよい。

そして、過去に訪れた場所であると判定された場合は、下記数（１０）を最小化することで、ｘ_Ｌｉ０及びｘ_Ｌｉを修正する。

ここで、Ｐ_Ｌ、Ｑ_ｙは、マハラノビス距離を算出する際の共分散、ｘ_Ｌｉは、ｉ番目に過去に訪れた場所であることが検出された位置、ｘ_Ｌｉ０は、ｘ_Ｌｉと同じ場所であると認識された場所、Ｎ_Ｌは、過去に訪れた場所であることが検出された回数、Ｔは現在までの総ステップ数、ｖ_ｔは、ｘ_ｔ−１からみたｘ_ｔの相対座標、

は、移動体の軌跡ｘ_ｔ−１の相対座標上でｖ_ｔ移動した場合に到達する位置を示す。この（１０）式を最小化することにより、軌跡ｘ＝[ｘ_０ ^Ｔ，・・・，ｘ_Ｔ ^Ｔ]^Ｔの修正を行うことができる。また、Ｌｏｏｐ−Ｃｌｏｓｉｎｇが検出された位置では、地図の事前分布として、前に訪れた際に構築した局所地図の特徴点の位置を（８）式のｍの平均に代入し、共分散をΠに代入することで、過去の局所マップを考慮した地図構築を行うことができる。このように過去の局所地図と現在の観測データを組み合わせることで、局所地図の精度の向上を行うことが可能となる。

次に、本実施の形態にかかる地図生成方法について説明する。図６は、本実施の形態の地図生成方法を示すフローチャートです。地図は、例えば、連続画像を取得できる移動体が移動しながら連続画像を取得しつつ、順次構築することができる。ここでは、ロボットが連続画像を取得しながら、地図を構築する例について説明する。当該ロボットには、図２に示す地図生成装置が搭載されている。なお、ロボットの構成例については、後述する。

図６に示すように、先ず、連続画像を取得する(ステップＳ１)。次に、局所特徴量抽出部１２は、複数の連続画像のＳＩＦＴを計算する(ステップＳ２)。特徴量マッチング部１３が各連続画像間でＳＩＦＴのマッチングを取り、不変特徴量算出部１４が、マッチングが取れたＳＩＦＴの平均値をＰＩＲＦとして算出する。

これと並行して、距離情報取得部１５又は地図生成部１６の有する位置情報取得部は、上記（８）式を最小化し、最大事後確率ｘ＾、ｍ＾及びそれに対応する共分散を算出する(ステップＳ４)。

ここで、最大事後確率算出の際には、例えば、ロボットの位置ｘ_０、ｘ_１、ｘ_２の地点で撮影された３つの連続画像を使用するとする。この場合、次の最大事後確率及びＰＩＲＦの算出には、ロボットの位置ｘ_１、ｘ_２、ｘ_３の地点で撮影された３つの連続画像を使用し、地点ｘ_０の連続画像は使用しない。そして、このｘ_０の地点の連続画像が含まれないことで、認識されなくなったＰＩＲＦも削除される。いいかえれば、過去のロボットの姿勢、その姿勢から見えていた特徴点を周辺化によって削除する(ステップＳ４)。これにより、計算で扱われるロボットの姿勢数Ｍが常に一定となり、各ステップの計算時間を一定に保つ。

地図生成部１６は、ステップＳ３で得られた最大事後確率ｘ＾、ｍ＾及びそれに対応する共分散を用いて、ロボットの状態と各特徴点の相対的な位置関係(局所地図)を構築する(ステップＳ５)。

ここで、ロボットは、常に、現在の場所が過去に訪れたことがある場所か否かの判断を行っている(ステップＳ６)。ロボットは、連続画像を取得した各地点におけるＰＩＲＦの情報を有している。したがって、ロボットは移動しながらＰＩＲＦを算出するが、算出したＰＩＲＦが、保有しているＰＩＲＦと一致するか否かの判定を行っている。例えば９０％以上のＰＩＲＦが一致した場所を、過去に訪れたことがある場所と判定する。

過去に訪れたことがある場所と判定された場合は、上記（９）式を最小化することで、軌跡ｘの修正を行う(ステップＳ７)。移動距離が長くなると、誤差が蓄積してしまうが、このステップＳ７（Ｌｏｏｐ−ｃｌｏｓｉｎｇ）を実施することで、蓄積する誤差をリセットすることができる。そして、以上のステップＳ１乃至Ｓ７の処理を繰り返すことで、所望の地図を生成することができる。なお、ステップＳ７を実行するために、地図を作成する場合は、同一の経路を２回通る必要がある。これにより、より精度が高い地図を生成することができる。ただし、１回通ったのみで精度が高い地図が得られる場合は、ステップＳ７の処理を省略することも可能である。

本発明の実施の形態２．
次に、実施の形態２について説明する。本実施の形態においては、動いている物体を取り除いて処理する方法として、画像から混雑な環境下でも安定な３次元上の特徴点を抽出し、その特徴点を用いることで、ロバストにＳＬＡＭ・ナビゲーションを行う。その一例として本実施の形態においては、ロボットが、実施の形態１で作成した局所メトリカル地図を用いて、ナビゲーションする方法について説明する。

図７は、本実施の形態にかかるナビゲーション装置を示すブロック図である。図７に示すように、ナビゲーション装置は、連続画像取得部３１、局所特徴量抽出部３２、特徴量マッチング部３３、不変特徴量算出部３４、位置情報取得部３５、地図生成部３６、及び相対座標算出部３７を有し、局所メトリカル地図格納部４０に接続され、局所メトリカル地図が参照可能に構成されている。ここで、相対座標算出部３７以外の構成は、実施の形態１と同様であり、その動作も同様である。

このナビゲーション装置３０は、実施の形態１同様、ロボットに搭載される。ロボットは、始点と終点が与えられると、局所メトリカル地図格納部４０の局所メトリカル地図を参照し、経路計画を立てる。そして、その経路計画にしたがって、始点から終点まで移動する。その際、移動しながら、上述のステップＳ１乃至ステップＳ６までの処理を実施し、ナビゲーションフェーズにおいても、ハイブリッド地図１７を生成する。

なお、ステップＳ７までの処理を繰り返し、軌道を修正しながら、終点まで移動することも可能である。ここで、本実施の形態にかかるナビゲーション装置３０は、相対座標算出部３７を有している。この相対座標算出部３７は、ステップＳ６を実施し、現在の位置が過去に訪れた位置であると判定した場合、現在の位置が、過去における位置とどれだけずれているかを示す相対座標を算出する。

図８は、相対座標の算出方法を説明する図である。図８において、局所メトリカル地図を生成する際に通った位置をｘ_０、ｘ_１、ｘ_２とする。現在の位置をｘ_１'とする。この場合、現在の位置ｘ_１'から見た特徴点ｍ_１、ｍ_２の位置は、（８）式を最小化することで得られる。局所メトリカル地図には、位置ｘ_１からの特徴点ｍ_１、ｍ_２の位置の情報を含む。したがって、現在の位置ｘ_１'が、過去の位置ｘ_１からどれだけずれているかを算出することができる。

例えば、現在の位置ｘ_ｔ'＝ｘ_ｔ＋αとすると、相対位置αは、観測データから相対座標を計算できる任意のアルゴリズムを用いることで計算することができる。この任意のアルゴリズムとしては、５点アルゴリズム、８点アルゴリズム等がある（Richard I. Hartley: "In Defense of the Eight-Point Algorithm," IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol.19, No.6, pp.580-583(1997)）。

例えば、８点アルゴリズムは、図８において、ｘ_ｔとｘ_ｔ'の観測データ（特徴点の位置）を比較し、相対姿勢を求めるための手法である。８点アルゴリズムによって相対座標αが計算された場合には、以下の（１１）式を最小化することによって軌跡を修正することができる。

ここで、α_ｉは８点アルゴリズムによって計算されたｘ_Ｌｉと、ｘ_{Ｌｅａｒｎｅｄｉ０}との間の相対姿勢を示す。また、Ｐ_Ｌ、Ｑ_ｙは、マハラノビス距離を算出する際の共分散、ｘ_Ｌｉは、ｉ番目に地図に登録されている場所であることが検出された位置、ｘ_{Ｌｅａｒｎｅｄｉ０}は、ｘ_Ｌｉと同じ場所であると認識された場所であり、固定値である。Ｎ_Ｌは、地図に登録された場所であることが検出された回数、Ｔは現在までの総ステップ数、ｖ_ｔは、ｘ_ｔ−１からみたｘ_ｔの相対座標、

は、移動体の軌跡ｘ_ｔ−１の相対座標上でｖ_ｔ移動した場合に到達する位置を示す。

このように、本実施の形態においては、単に（１１）式を最小化して軌跡の修正を行うのではなく、現在の位置ｘ_１'と、局所メトリカル地図に登録されている過去の位置ｘ_１との相対座標を算出し、これを使用して、（１１）式を最小化することにより、より精度よく、ナビゲーションを行うことができる。

図９は、本実施の形態にかかるロボットのナビゲーション方法を示す図である。図９において、ステップＳ１１乃至Ｓ１６は、実施の形態１における図６のステップＳ１乃至Ｓ６と同様である。すなわち、実施の形態１と同様、３Ｄ−ＰＩＲＦを算出し、現在の位置が過去に訪れた位置であるかの判定を行う(ステップＳ１６)。そして、現在の位置が過去に訪れた位置である場合は、現在の位置と過去の位置の相対座標を算出する(ステップＳ１７)。そして、この相対座標を、（１１）式に代入し、これを最小化することで、軌跡ｘの修正を行う(ステップＳ１８)。その後は、ステップＳ１１からの処理を繰り返す。

ナビゲーションフェーズでは、学習フェーズで教えられた道順通りにロボットが自律的に移動する。移動ロボットは学習フェーズと同様にハイブリッド地図を構築しながら移動するが、ナビゲーションフェーズでは、教えられた道順通りに移動するために、学習フェーズで学習したハイブリッド地図と現在のハイブリッド地図それぞれの軌跡と局所マップを比較し、学習した軌跡の方向にロボットの位置を修正することで、学習した軌跡に追従するように行動計画を行う。

このようなナビゲーションを行うロボットの一例について説明しておく。図１０は、本実施の形態にかかるロボットを示すブロック図である。ロボット５０は、制御部５１、入出力部５２、駆動部５３、電源部５４、及び外部記憶部５５などを有している。

入出力部５２は、周囲の映像を取得するためのＣＣＤ（Charge Coupled Device）などからなるカメラ６１、周囲の音を集音するための１又は複数の内蔵マイク６２、音声を出力してユーザと対話等を行なうためのスピーカ６３、ユーザへの応答や感情等を表現するためのＬＥＤ６４、タッチセンサなどからなるセンサ部６５などを備える。

また、駆動部５３は、モー７１及びモータを駆動するドライバ７２などを有し、ユーザの指示などに従って脚部ユニットや腕部ユニットを動作させる。電源部５４は、バッテリ８１及びその放充電を制御するバッテリ制御部８２を有し、各部に電源を供給する。

外部記憶部５５は、着脱可能なＨＤＤ、光ディスク、光磁気ディスク等からなり、各種プログラムや制御パラメータなどを記憶し、そのプログラムやデータを必要に応じて制御部１０１内のメモリ（不図示）等に供給する。

制御部は、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、無線通信用のインターフェースなどを有し、ロボット５０の各種動作を制御する。そして、この制御部５１は、例えばＲＯＭに格納された制御プログラムに従って、カメラ６１により取得した映像を解析することによりハイブリッド地図を生成する地図生成モジュール４１、生成したハイブリッド地図に基づき、経路計画を行なう経路計画モジュール４２、始点と終点を与えられると経路計画に従ってナビゲーションするナビゲーションモジュール４３、音声認識を行なう音声認識モジュール４４などを有する。特に、本実施の形態においては、地図生成モジュール４１により、複雑な環境下でも安定な特徴点を抽出したハイブリッド地図が生成され、これに基づき経路計画を立て、行動を決定し、駆動部５３を制御するため、ロボット５０はより正確にナビゲーションを行うことができる。

実施例
２０ｍ×２０ｍ程度の屋内環境で地図生成を行った。図１１にその環境を示す。この環境は食堂であり、常に多くの人が出入りし、混雑している。このような常に人が出入りする環境の場合、従来の手法では動いている人から抽出される特徴点が悪影響を与え、ロバストにＳＬＡＭ・ナビゲーションを行うことができなかった。

一方、本実施例では、混雑な環境下でもロバストにＳＬＡＭ・ナビゲーションを行えるという特徴がある。まず学習フェーズでは、全方位カメラとエンコーダセンサを搭載したロボットが人間の操縦によって、上述の方法を用いて自己位置推定と地図構築を行いながら移動する。図１２は環境の真値地図と、学習フェーズでロボットを移動させた道順を表しており、まず左下の実線で示した小さいループＡ１を２周、次に右上に移動し、そこから点線で示した大きなループＡ２を２周、最後に右上の一点鎖線で示した小さいループＡ３を２周させた。

図１３は以上の経路に対して、カメラの情報を使わずオドメトリのみから計算したロボットの軌跡である。図１３において、Ｂ１は、スタート地点、Ｂ２は、ゴール地点を表しており、ルートはスタート地点から徐々に色が薄くなるように描かれている。この図から、オドメトリのみから計測した場合には移動距離が延びるほど自己位置推定の誤差が溜まり、正しい軌跡が学習できていないことがわかる。

一方、図１４は、本実施の形態の方法により、ロボットが学習した軌跡である。図１４において、丸印は過去に訪れたことがあると認識（Ｌｏｏｐ−Ｃｌｏｓｉｎｇ検出）された位置を表している。この図から、移動距離が延びても誤差が溜まっておらず、ルートを正しく学習出来ていることが分かる。

次にナビゲーションフェーズでは、学習フェーズで学習した軌跡に沿うように、ロボットが自律的に移動する。図１５は、ロボットが自律的に移動した軌跡である。今回は左下Ｃ１から右上Ｃ２まで移動するように経路計画を行った。図１５において、学習フェーズで学習した軌跡をＤ１で、ロボットが移動した軌跡をＤ２で示しており、学習フェーズで学習した軌跡に対してＬｏｏｐ−Ｃｌｏｓｉｎｇ検出がされた位置を丸で示している。この図から、ロボットは学習フェーズで学習した左下Ｃ１から右上Ｃ２に至る経路に沿うように自律的に移動し、ゴール地点まで到達できていることがわかる。

なお、本発明は上述した実施の形態のみに限定されるものではなく、本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。

例えば、上述の実施の形態では、ハードウェアの構成として説明したが、これに限定されるものではなく、任意の処理を、ＣＰＵ（Central Processing Unit）にコンピュータプログラムを実行させることにより実現することも可能である。この場合、コンピュータプログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１ 地図生成装置
１１ 連続画像取得部
１２ 局所特徴量抽出部
１３ 特徴量マッチング部
１４ 不変特徴量算出部
１５ 距離情報取得部
１６ 地図生成部
３０ ナビゲーション装置
３１ 連続画像取得部
３２ 局所特徴量抽出部
３３ 特徴量マッチング部
３４ 不変特徴量算出部
３５ 距離情報取得部
３６ 地図生成部
３７ 相対座標算出部
４０ 局所メトリカル地図格納部
５４１ 地図生成モジュール
５４２ 経路計画モジュール
５４３ ナビゲーションモジュール
５４４ 音声認識モジュール
５０ ロボット
５１ 制御部
５５２ 入出力部
５５３ 駆動部
５５４ 電源部
５５５ 外部記憶部
５６１ カメラ
５６２ 内蔵マイク
５６３ スピーカ
５６５ センサ部
５７１ モータ
５７２ ドライバ
５８１ バッテリ
５８２ バッテリ制御部

Claims (15)

1. 移動体が移動する間に連続して撮影した連続画像を取得する連続画像取得工程と、
   前記連続画像取得工程にて取得された連続画像から各特徴点における局所特徴量を抽出する局所特徴量抽出工程と、
   前記局所特徴量抽出工程にて抽出された前記局所特徴量について、前記連続する入力画像間でマッチングをとる特徴量マッチング工程と、
   前記特徴量マッチング工程にて所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める不変特徴量算出工程と、
   前記連続画像を取得した各時刻における前記移動体の位置に基づき前記各不変特徴量に対応する距離情報を算出する距離情報取得工程と、
   当該不変特徴量及び前記距離情報を有するハイブリッド地図として、局所メトリカル地図を生成する地図生成工程とを有する、地図生成方法。
2. 連続する複数の連続画像を取得した各時刻における前記移動体の移動に関する情報と、前記不変特徴量を有する特徴点の前記移動体からの計測方向を示す観測値とから、前記移動体の位置及び姿勢と、前記不変特徴量に対応する特徴点の位置とを求める位置情報取得工程とを有し、
   前記地図生成工程では、前記移動体の位置及び姿勢、並びに特徴点の位置及びその不変特徴量を有する前記ハイブリッド地図を生成する、請求項１記載の地図生成方法。
3. 前記位置情報取得工程では、前記不変特徴量を求めた複数の連続画像を取得した各時刻における前記移動に関する情報の系列と、前記観測値の系列とから、確率的に尤もらしい前記移動体の位置及び姿勢、並びに前記特徴点の位置を算出する、請求項２記載の地図生成方法。
4. 前記位置情報取得工程は、ｘ_ｉを前記移動体の位置及び姿勢としたとき、前記複数の連続画像間において、前記不変特徴量が各時刻において前記移動体からどの方向に見えるかを示す前記観測値ｚ_ｉｊ、及び前記移動体がｘ_ｉからｘ_ｉ−１まで移動するのに用いた距離情報を示す入力ｕ_ｉを取得し、前記観測値ｚ_ｉｊと前記入力ｕ_ｉとから下記式を最小化することで得られる、各時刻における前記移動体の位置及び姿勢、及び前記特徴点の位置を含む前記ハイブリッド地図を作成する、請求項２又は３記載の地図生成方法。
   を示す。
5. 前記不変特徴量と、前記移動体の位置及び姿勢とは、複数枚の連続画像から得られるものであって、
   前記地図生成工程は、前記移動体の位置及び姿勢及び前記不変特徴量を算出するために使用する連続画像が各時刻において同数となるよう、過去の連続画像を削除すると共に、当該連続画像を削除することで見えなくなる前記不変特徴量を削除する過去画像削除工程を有する、請求項２乃至４のいずれか１項記載の地図生成方法。
6. 現在の場所が過去に訪れたことがある場所か否かを判定する場所判定工程と、
   現在の場所が過去に訪れたことがある場所である場合に、前回の前記移動体の位置及び姿勢ｘ_Ｌｉ０と、今回の前記移動体の位置及び姿勢ｘ_Ｌｉとから、ｘ_Ｌｉ０及びｘ_Ｌｉを修正する軌跡修正工程とを有する、請求項２乃至４のいずれか１項記載の地図生成方法。
7. 前記軌跡修正工程では、下記式を最小化することで、前記ｘ_Ｌｉ０及びｘ_Ｌｉを修正する請求項６記載の地図生成方法。
   
8. 移動体が移動する間に連続して撮影した連続画像を取得する連続画像取得手段と、
   前記連続画像取得手段により取得した連続画像から各特徴点における局所特徴量を抽出する局所特徴量抽出手段と、
   前記局所特徴量抽出手段により抽出した前記局所特徴量について、前記連続する入力画像間でマッチングをとる特徴量マッチング手段と、
   前記特徴量マッチング手段により所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める不変特徴量算出手段と、
   前記連続画像を取得した各時刻における前記移動体の位置に基づき前記各不変特徴量に対応する距離情報を算出する距離情報取得手段と、
   当該不変特徴量及び前記距離情報を有するハイブリッド地図として、局所メトリカル地図を生成する地図生成手段とを有する、地図生成装置。
9. 連続する複数の連続画像を取得した各時刻における前記移動体の移動に関する情報と、前記不変特徴量を有する特徴点の前記移動体からの計測方向を示す観測値とから、前記移動体の位置及び姿勢と、前記不変特徴量に対応する特徴点の位置とを求める位置情報取得工程とを有し、
   前記地図生成手は、前記移動体の位置及び姿勢、並びに特徴点の位置及びその不変特徴量を有する前記ハイブリッド地図を生成する、、請求項８記載の地図生成装置。
10. 前記位置情報取得手段は、前記不変特徴量を求めた複数の連続画像を取得した各時刻における前記移動に関する情報の系列と、前記観測値の系列とから、確率的に最も尤もらしい前記移動体の位置及び姿勢、並びに前記特徴点の位置を算出する、請求項９記載の地図生成装置。
11. 予め作成された局所メトリカル地図に従って経路計画を行い移動する移動体の移動方法であって、
    移動体が移動する間に連続画像を取得する連続画像取得工程と、
    前記連続画像取得工程にて取得された連続画像から各特徴点における局所特徴量を抽出する局所特徴量抽出工程と、
    前記局所特徴量抽出工程にて抽出された前記局所特徴量について、前記連続する入力画像間でマッチングをとる特徴量マッチング工程と、
    前記特徴量マッチング工程にて所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める不変特徴量算出工程と、
    前記連続画像を取得した各時刻における前記移動体の位置に基づき前記各不変特徴量に対応する距離情報を算出する距離情報取得工程と、
    算出した不変特徴量及び距離情報と、前記局所メトリカル地図上に登録されている不変特徴量及び距離情報を比較し、現在の位置が登録されているか否かを判定し、登録されている場合は、当該局所メトリカル地図の情報に基づき現在位置の修正を行う軌跡修正工程とを有し、
    前記局所メトリカル地図は、前記移動体が前記連続画像を取得した各時刻における位置情報に基づき算出された前記各不変特徴量に対応する距離情報と、前記不変特徴量とを有するハイブリッド地図である、移動体の移動方法。
12. 前記軌跡修正工程では、現在の場所が前記局所メトリカル地図上に登録されていた場合に、登録されている前記移動体の位置及び姿勢ｘ_{Ｌｅａｒｎｅｄｉ０}と、今回の前記移動体の位置及び姿勢ｘ_Ｌｉとから、ｘ_{Ｌｅａｒｎｅｄｉ０}及びｘ_Ｌｉを修正する、請求項１１記載の移動体の移動方法。
13. 前記軌跡修正工程では、下記式を最小化することで、前記ｘ_{Ｌｅａｒｎｅｄｉ０}及びｘ_Ｌｉを修正する請求項１２記載の移動体の移動方法。
    
14. 現在の位置が、地図に登録されている場所とどれだけ違うかを示す相対座標を算出する相対座標算出工程を更に有し、
    前記軌跡修正工程では、前記ｘ_Ｌｉとして前記相対座標を使用して、前記軌跡の修正を行う、請求項１３記載の移動体の移動方法。
15. 予め作成された局所メトリカル地図に従って経路計画を行い移動するロボット装置であって、
    移動体が移動する間に連続画像を取得する連続画像取得手段と、
    前記連続画像取得手段により取得した連続画像から各特徴点における局所特徴量を抽出する局所特徴量抽出手段と、
    前記局所特徴量抽出手段により抽出した前記局所特徴量について、前記連続する入力画像間でマッチングをとる特徴量マッチング手段と、
    前記特徴量マッチング手段により所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める不変特徴量算出手段と、
    前記連続画像を取得した各時刻における前記移動体の位置に基づき前記各不変特徴量に対応する距離情報を算出する距離情報取得手段と、
    算出した不変特徴量及び距離情報と、前記局所メトリカル地図上に登録されている不変特徴量及び距離情報を比較し、現在の位置が登録されているか否かを判定し、登録されている場合は、当該局所メトリカル地図の情報に基づき現在位置の修正を行う軌跡修正手段とを有し、
    前記局所メトリカル地図は、前記移動体が前記連続画像を取得した各時刻における位置情報に基づき算出された前記各不変特徴量に対応する距離情報と、前記不変特徴量とを有するハイブリッド地図である、ロボット装置。